基于GPS/INS自主耕作拖拉機(jī)導(dǎo)航修正研究

趙書尚，李會(huì)彬，劉 斌

(1.河南科技大學(xué) 機(jī)電工程學(xué)院，河南 洛陽 471003；2.河南林業(yè)職業(yè)學(xué)院，河南 洛陽 471003)

0 引言

自主耕作拖拉機(jī)作為一種智能化、信息化的現(xiàn)代農(nóng)業(yè)裝備，導(dǎo)航控制是其顯著的特征之一。實(shí)現(xiàn)拖拉機(jī)的自動(dòng)化能有效降低駕駛員的勞動(dòng)強(qiáng)度，顯著提高拖拉機(jī)的耕作精度。目前，我國(guó)拖拉機(jī)的智能化水平只是相當(dāng)于20世紀(jì)80年代的國(guó)際水平，國(guó)內(nèi)產(chǎn)品與國(guó)外產(chǎn)品還存在著一定的差距，主要體現(xiàn)在自動(dòng)控制精度及可靠性等方面。吳曉鵬[1]改造了東方紅-X804拖拉機(jī)原有油路，使用電液比例閥實(shí)現(xiàn)了拖拉機(jī)自動(dòng)轉(zhuǎn)向控制。莊衛(wèi)東[2]提出偏航距離計(jì)算方法用于農(nóng)機(jī)導(dǎo)航控制。目前，國(guó)內(nèi)對(duì)于拖拉機(jī)導(dǎo)航誤差的研究較少，因此基于GPS/INS自主耕作拖拉機(jī)導(dǎo)航修正的研究具有重要意義。

為了使拖拉機(jī)更加精準(zhǔn)的作業(yè)，需優(yōu)化其導(dǎo)航參數(shù)，包括定位坐標(biāo)、側(cè)傾角度及航行角度等，常選用卡爾曼濾波進(jìn)行處理。Noguchiyt[3]采用擴(kuò)展卡爾曼濾波和基于兩維概率密度函數(shù)的靜態(tài)方法將傳感器數(shù)據(jù)進(jìn)行融合，開發(fā)了基于RTK-GPS和機(jī)器視覺的自主導(dǎo)航系統(tǒng)。Dale[4]使用DGPS和FOG對(duì)農(nóng)業(yè)車輛進(jìn)行導(dǎo)航，使DGPS的定位精度提高至25cm以內(nèi)，車輛偏轉(zhuǎn)角預(yù)測(cè)誤差從10%降低到1%。田光兆[5]選用分散式卡爾曼濾波器將定位數(shù)據(jù)數(shù)據(jù)融合，利用電子羅盤獲得角度參數(shù)，建立拖拉機(jī)運(yùn)動(dòng)模型提高拖拉機(jī)導(dǎo)航精度。周俊[6]融合各種傳感器信息建立非線性隨機(jī)數(shù)學(xué)模型，利用卡爾曼濾波算法預(yù)測(cè)跟路徑。

本文選用載波相位差分GPS和慣性導(dǎo)航系統(tǒng)組合導(dǎo)航方式，融合導(dǎo)航參數(shù)，主要針對(duì)因地形傾斜和土質(zhì)硬度不均引起的GPS定位誤差進(jìn)行校正，旨在提高自主耕作拖拉機(jī)的導(dǎo)航控制精度。

1 導(dǎo)航參數(shù)修正

1.1 確定坐標(biāo)系參數(shù)

坐標(biāo)系定義示意圖如圖1所示。將坐標(biāo)系oxy定義為大地坐標(biāo)系，即高斯平面坐標(biāo)系，o'x'y'定義為車體坐標(biāo)系，坐標(biāo)系原點(diǎn)o'與質(zhì)心重合，縱軸y'與車體重合并指向車體前進(jìn)方向，橫軸x'指向車體前進(jìn)方向右側(cè)并與縱軸垂直。

圖1 拖拉機(jī)坐標(biāo)系參數(shù)定義Fig.1 Definition of tractor coordinate system

1.2 拖拉機(jī)導(dǎo)航參數(shù)定義

拖拉機(jī)導(dǎo)航參數(shù)如圖2所示。將車體前進(jìn)方向與大地坐標(biāo)系x軸之間的夾角定義為拖拉機(jī)航向角，用Ψ表示，規(guī)定正方向?yàn)槟鏁r(shí)針方向。將車體前進(jìn)方向與地平面之間的夾角定義為拖拉機(jī)的俯仰角度，用θ表示，規(guī)定拖拉機(jī)前輪下傾為正，上傾為負(fù)。將車體橫軸與地平面的夾角定義為拖拉機(jī)的側(cè)傾角，用φ表示，規(guī)定沿車體前進(jìn)方向右傾為正，左傾為負(fù)。

圖2 拖拉機(jī)右傾、俯仰情況下GPS傾斜示意圖Fig.2 GPS positioning correction based on the roll or pitch of tractor

1.3 傳感器選擇、安裝

由于GPS信號(hào)易被屏蔽和干擾，因此選用GPS/INS組合導(dǎo)航方式獲取拖拉機(jī)的導(dǎo)航參數(shù)，導(dǎo)航精度可得到顯著提高，最高可以達(dá)到1.5cm，輸出頻率可以達(dá)到20Hz。導(dǎo)航設(shè)備主要包括GPS天線、基站、移動(dòng)站及RF無線數(shù)傳模塊等。GPS天線安裝在拖拉機(jī)前蓋中間部位，距地面高度約1.5cm。慣導(dǎo)系統(tǒng)安裝在拖拉機(jī)質(zhì)心部位，用于獲取拖拉機(jī)的航向角度、側(cè)傾角度、俯仰角度、三軸角速度和三軸加速度等參數(shù)。其中，航向角量程為360°，測(cè)試精度為0.8°，橫滾角度和俯仰角度量程為±90°，靜態(tài)精度為0.05°。慣導(dǎo)系統(tǒng)的x軸與車體橫軸重合、y軸與車體縱軸重合，z軸與車體坐標(biāo)系平面垂直，向下為正。設(shè)備現(xiàn)場(chǎng)安裝如圖3所示。

圖3 拖拉機(jī)試驗(yàn)平臺(tái)及傳感器安裝圖Fig.3 Research platform and the fix of navigation sensors

1.4 GPS定位誤差分析

拖拉機(jī)在農(nóng)田耕作過程中，由于地面起伏或土質(zhì)硬度不均，某一車輪下限導(dǎo)致車體發(fā)生傾斜，從而引起GPS天線發(fā)生傾斜，這將嚴(yán)重影響GPS的定位精度。試驗(yàn)中，GPS的天線高度約為1.5m，當(dāng)側(cè)傾角φ達(dá)到10°時(shí)，GPS定位的橫坐標(biāo)將會(huì)產(chǎn)生26cm的誤差；若側(cè)傾角達(dá)到20°時(shí)，誤差將達(dá)到51cm。為了提高自主耕作拖拉機(jī)的導(dǎo)航精度，必須消除該因素引起的誤差。式(1)、式(2)表示因GPS傾斜造成的橫向誤差和縱向誤差，利用該誤差對(duì)GPS的定位參數(shù)進(jìn)行修正，則

(1)

(2)

(3)

(4)

其中，ex、ey分別為GPS傾斜后在橫軸、縱軸方向上產(chǎn)生的偏差，利用此偏差修正GPS原始定位參數(shù)，提高導(dǎo)航精度。

2 構(gòu)建Kalman濾波器估計(jì)拖拉機(jī)導(dǎo)航參數(shù)

假設(shè)拖拉機(jī)勻速前進(jìn)，速度為vk，ψk為k時(shí)刻拖拉機(jī)的航向角，Δψk為k時(shí)刻拖拉機(jī)航向偏移誤差，(xk，yk)為拖拉機(jī)k時(shí)刻的位置坐標(biāo)，(xk+1，yk+1)為拖拉機(jī)機(jī)k+1時(shí)刻的位置坐標(biāo)，T為采樣周期。取采樣周期為0.05s，在一個(gè)采樣周期內(nèi)拖拉機(jī)航向角度變化非常小，可以忽略不計(jì)，則拖拉機(jī)位置坐標(biāo)關(guān)系式可表示為

xk+1=xk+Tvkcos(ψk+Δψk)

(5)

yk+1=yk+Tvksin(ψk+Δψk)

(6)

將式(5)、式(6)兩式展開得

xk+1=xk+TvkcosψkcosΔψk-TvksinψksinΔψk

(7)

yk+1=yk+TvksinψkcosΔψk+TvkcosψksinΔψk

(8)

Kalman濾波器的狀態(tài)變量為

其中

(9)

(10)

Xk+1=φkXk+ωk

(11)

其中

ωk為過程噪音，濾波器觀測(cè)變量為

Zk=[xck，yck]

由式(3)、式(4)可獲得校正后的GPS參數(shù)為

(12)

(13)

其中，xok、yok為原始GPS定位參數(shù)，濾波器的觀測(cè)方程可以表示為

Zk=hkXk+vk

(14)

其中

(15)

其中，hk為觀測(cè)矩陣，；vk為觀測(cè)噪音。假設(shè)觀測(cè)噪聲vk和過程噪聲ωk不相關(guān)，那么ωk、vk滿足

E(ωk)=0

(16)

E(vk)=0

(17)

(18)

(19)

其中，當(dāng)i=k時(shí)，Δ(k-i)=1；i≠k時(shí)，Δ(k-i)=0。Δ(k-i)為Kronecherδ函數(shù)；Qk為過程噪聲協(xié)方差矩陣；Rk為觀測(cè)噪聲協(xié)方差矩陣。

通過對(duì)試驗(yàn)數(shù)據(jù)分析，將Qk和Rk確定為

3 拖拉機(jī)路徑跟蹤控制策略

通過多次試驗(yàn)，找到了一種簡(jiǎn)單可行的控制方法，用校正后的GPS定位坐標(biāo)值與預(yù)定的目標(biāo)值進(jìn)行對(duì)比，根據(jù)拖拉機(jī)當(dāng)前坐標(biāo)點(diǎn)與導(dǎo)航線之間的距離，判斷拖拉機(jī)的轉(zhuǎn)向，控制策略示意圖如圖4所示。其基本思想是：將預(yù)定導(dǎo)航路徑儲(chǔ)存在車載工控機(jī)中，由定位設(shè)備給出拖拉機(jī)的位置信息；計(jì)算拖拉機(jī)的實(shí)際位置和目標(biāo)位置的差值，并將該差值作為控制量輸入PID控制器，進(jìn)行轉(zhuǎn)向決策；同時(shí)，安裝在拖拉機(jī)前輪轉(zhuǎn)向軸處的角度傳感器將拖拉機(jī)當(dāng)前的實(shí)際轉(zhuǎn)角反饋給控制系統(tǒng)，然后控制系統(tǒng)發(fā)出指令使轉(zhuǎn)向驅(qū)動(dòng)機(jī)構(gòu)完成拖拉機(jī)轉(zhuǎn)向，從而實(shí)現(xiàn)拖拉機(jī)的路徑跟蹤?？刂圃砣鐖D5所示。

圖4 拖拉機(jī)路徑跟蹤控制策略示意圖Fig.4 Tractor path tracking control strategy

圖5 PID控制原理圖Fig. 5 PID control system

設(shè)導(dǎo)航直線的方程為Ax+By+C=0，當(dāng)前拖拉機(jī)的位置坐標(biāo)為PC(x0，y0)，則拖拉機(jī)與目標(biāo)直線的距離為

4 試驗(yàn)與數(shù)據(jù)分析

試驗(yàn)在中國(guó)一拖農(nóng)田試驗(yàn)基地進(jìn)行，車載工控機(jī)的采樣頻率為20Hz，同步采集RTK-DGPS原始定位數(shù)據(jù)和慣性導(dǎo)航系統(tǒng)的狀態(tài)參數(shù)。圖6中，實(shí)線為拖拉機(jī)實(shí)際行駛軌跡(由于圖中坐標(biāo)軸是以m為單位，實(shí)線和虛線區(qū)分不太明顯，圖7為A區(qū)域處的局部放大圖，可以明顯區(qū)分實(shí)線和虛線)。用MatLab進(jìn)行分析，比較濾波前后的效果，圖中虛線為修正后拖拉機(jī)的運(yùn)動(dòng)軌跡。取圖中A點(diǎn)區(qū)域進(jìn)行放大，從局部放大圖可以看出，拖拉機(jī)原始軌跡在某一時(shí)刻有較大波動(dòng)，主要是由于田間地面起伏及土質(zhì)硬度不均造成的，濾波后的拖拉機(jī)運(yùn)動(dòng)軌跡較原始拖拉機(jī)運(yùn)動(dòng)軌跡平穩(wěn)了許多，有效校正了GPS定位傾斜誤差，提高了定位系統(tǒng)抗干擾能力，使拖拉機(jī)在復(fù)雜地形仍然能夠按照預(yù)定路徑行駛。

圖6 拖拉機(jī)原始軌跡和校正后軌跡效果圖Fig.6 Effect of tractor original and updated track

圖7 拖拉機(jī)原始軌跡和校正后軌跡局部放大效果圖Fig.7 Local amplification effect of tractor original and updated track

5 結(jié)論

1)校正了因GPS傾斜帶來的定位誤差，減少了拖拉機(jī)運(yùn)動(dòng)軌跡的波動(dòng)，提高了定位系統(tǒng)的抗干擾能力。

2)采用RTK-DGPS導(dǎo)航方式，運(yùn)用Kalman濾波算法提高了拖拉機(jī)的定位精度， 使拖拉機(jī)在復(fù)雜地形下仍能精準(zhǔn)作業(yè)。

3)GPS/INS組合導(dǎo)航系統(tǒng)，彌補(bǔ)了GPS定位信息不穩(wěn)定的缺點(diǎn)，是一種較為理想的導(dǎo)航系統(tǒng)。

參考文獻(xiàn)：

[1] 吳曉鵬，趙祚喜.東方紅拖拉機(jī)自動(dòng)轉(zhuǎn)向控制系統(tǒng)設(shè)計(jì)[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2009,40(6):15.

[2] 莊衛(wèi)東.農(nóng)機(jī)直線行走作業(yè) DGPS 導(dǎo)航算法研究 [J].黑龍江八一農(nóng)墾大學(xué)學(xué)報(bào),2006,18(6)15.

[3] Noguchi N, Reid J F, WiII J,et al. Vehicleautomation system based on multi-sensor integration[C]//Annual International ASAE Meeting, MI,1998.

[4] Dale W J. Sensor fusion for field robot localization[D].University of Illinois at Urbana-Champaign,2001.

[5] 周俊，姬長(zhǎng)英. 農(nóng)業(yè)機(jī)器人視覺導(dǎo)航中多分辨率路徑識(shí)別[J].農(nóng)業(yè)機(jī)械學(xué)報(bào),2003,34(6)：120-123.

[6] 田光兆，姬長(zhǎng)英. 智能化農(nóng)業(yè)車輛導(dǎo)航系統(tǒng)關(guān)鍵技術(shù)研究[D].南京:南京農(nóng)業(yè)大學(xué),2013.

[7] 毛恩榮，田甜. 拖拉機(jī)自動(dòng)駕駛系統(tǒng)GPS/DR組合導(dǎo)航定位方法的研究[D]. 北京：中國(guó)農(nóng)業(yè)大學(xué)，2006.

[8] Kevin Murrant. Attitude and Heading Reference System for Small Unmanned Aircraft Collision Avoidance Maneuver-s[D].Memorial University of Newfoundland,2012：22-46.

[9] Christopher J Fisher.Using an Accelerometerfor Inclination Sensing[EB/OL].2017-01-20.http://www.analog.com/media/cn/technical-documentation/application-notes/AN-1057_cn.pdf：1-8.

[10] 歐陽勁志.無人駕駛拖拉機(jī)電控系統(tǒng)研究[C]//鄭州:第十一屆河南省汽車工程科技學(xué)術(shù)研討會(huì),2014: 3-6.

[11] 羅成.無人駕駛拖拉機(jī)[J].南方農(nóng)機(jī),2009(4):56-70.

[12] 趙勻，武傳宇，胡旭東.農(nóng)業(yè)機(jī)器人的研究進(jìn)展及存在的問題[J].農(nóng)業(yè)工程學(xué)報(bào)，2003(1):20-24.

[13] 張明.農(nóng)業(yè)機(jī)器人路徑規(guī)劃與跟蹤方法研究[D]. 北京:北京工業(yè)大學(xué),2010:5-8.

[14] 張立彬.農(nóng)業(yè)機(jī)器人視覺傳感系統(tǒng)的實(shí)現(xiàn)與應(yīng)用研究進(jìn)展[J].農(nóng)業(yè)工程學(xué)報(bào),2002,18(4):18.

[15] 李鳳英,高田,羊彥. 磁羅差姿態(tài)解算及誤差補(bǔ)償[J].計(jì)算機(jī)測(cè)量與控制, 2011,19(7):170-173.

[16] 朱思洪.全架式大功率輪式拖拉機(jī)的研制[J].拖拉機(jī)與農(nóng)用運(yùn)輸車,2009,36(3):105-108.

[17] 翟紅霞.拖拉機(jī)技術(shù)發(fā)展現(xiàn)狀及趨勢(shì)研究[J].農(nóng)業(yè)科技與裝備,2014(2):61-63.

[18] 黃才志.拖拉機(jī)無線遙控裝置推廣的探究[J].廣西農(nóng)業(yè)機(jī)械化,2010(5):35-37.

AbstractID:1003-188X(2018)05-0058-EA