基于GPS/INS自主耕作拖拉機導航修正研究

趙書尚，李會彬，劉 斌

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

0 引言

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

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

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

1 導航參數修正

1.1 確定坐標系參數

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

圖1 拖拉機坐標系參數定義Fig.1 Definition of tractor coordinate system

1.2 拖拉機導航參數定義

拖拉機導航參數如圖2所示。將車體前進方向與大地坐標系x軸之間的夾角定義為拖拉機航向角，用Ψ表示，規定正方向為逆時針方向。將車體前進方向與地平面之間的夾角定義為拖拉機的俯仰角度，用θ表示，規定拖拉機前輪下傾為正，上傾為負。將車體橫軸與地平面的夾角定義為拖拉機的側傾角，用φ表示，規定沿車體前進方向右傾為正，左傾為負。

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

1.3 傳感器選擇、安裝

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

圖3 拖拉機試驗平臺及傳感器安裝圖Fig.3 Research platform and the fix of navigation sensors

1.4 GPS定位誤差分析

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

(1)

(2)

(3)

(4)

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

2 構建Kalman濾波器估計拖拉機導航參數

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

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濾波器的狀態變量為

其中

(9)

(10)

Xk+1=φkXk+ωk

(11)

其中

ωk為過程噪音，濾波器觀測變量為

Zk=[xck，yck]

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

(12)

(13)

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

Zk=hkXk+vk

(14)

其中

(15)

其中，hk為觀測矩陣，；vk為觀測噪音。假設觀測噪聲vk和過程噪聲ωk不相關，那么ωk、vk滿足

E(ωk)=0

(16)

E(vk)=0

(17)

(18)

(19)

其中，當i=k時，Δ(k-i)=1；i≠k時，Δ(k-i)=0。Δ(k-i)為Kronecherδ函數；Qk為過程噪聲協方差矩陣；Rk為觀測噪聲協方差矩陣。

通過對試驗數據分析，將Qk和Rk確定為

3 拖拉機路徑跟蹤控制策略

通過多次試驗，找到了一種簡單可行的控制方法，用校正后的GPS定位坐標值與預定的目標值進行對比，根據拖拉機當前坐標點與導航線之間的距離，判斷拖拉機的轉向，控制策略示意圖如圖4所示。其基本思想是：將預定導航路徑儲存在車載工控機中，由定位設備給出拖拉機的位置信息；計算拖拉機的實際位置和目標位置的差值，并將該差值作為控制量輸入PID控制器，進行轉向決策；同時，安裝在拖拉機前輪轉向軸處的角度傳感器將拖拉機當前的實際轉角反饋給控制系統，然后控制系統發出指令使轉向驅動機構完成拖拉機轉向，從而實現拖拉機的路徑跟蹤。控制原理如圖5所示。

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

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

設導航直線的方程為Ax+By+C=0，當前拖拉機的位置坐標為PC(x0，y0)，則拖拉機與目標直線的距離為

4 試驗與數據分析

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

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

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

5 結論

1)校正了因GPS傾斜帶來的定位誤差，減少了拖拉機運動軌跡的波動，提高了定位系統的抗干擾能力。

2)采用RTK-DGPS導航方式，運用Kalman濾波算法提高了拖拉機的定位精度， 使拖拉機在復雜地形下仍能精準作業。

3)GPS/INS組合導航系統，彌補了GPS定位信息不穩定的缺點，是一種較為理想的導航系統。

參考文獻：

[1] 吳曉鵬，趙祚喜.東方紅拖拉機自動轉向控制系統設計[J].農業機械學報,2009,40(6):15.

[2] 莊衛東.農機直線行走作業 DGPS 導航算法研究 [J].黑龍江八一農墾大學學報,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] 周俊，姬長英. 農業機器人視覺導航中多分辨率路徑識別[J].農業機械學報,2003,34(6)：120-123.

[6] 田光兆，姬長英. 智能化農業車輛導航系統關鍵技術研究[D].南京:南京農業大學,2013.

[7] 毛恩榮，田甜. 拖拉機自動駕駛系統GPS/DR組合導航定位方法的研究[D]. 北京：中國農業大學，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] 歐陽勁志.無人駕駛拖拉機電控系統研究[C]//鄭州:第十一屆河南省汽車工程科技學術研討會,2014: 3-6.

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

[12] 趙勻，武傳宇，胡旭東.農業機器人的研究進展及存在的問題[J].農業工程學報，2003(1):20-24.

[13] 張明.農業機器人路徑規劃與跟蹤方法研究[D]. 北京:北京工業大學,2010:5-8.

[14] 張立彬.農業機器人視覺傳感系統的實現與應用研究進展[J].農業工程學報,2002,18(4):18.

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

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

[17] 翟紅霞.拖拉機技術發展現狀及趨勢研究[J].農業科技與裝備,2014(2):61-63.

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

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