基于DGPS定位與雙閉環(huán)轉向控制的農(nóng)業(yè)自動導航系統(tǒng)

黎永鍵，趙祚喜，高俊文，吳曉鵬，關偉

（1. 廣東農(nóng)工商職業(yè)技術學院，廣東 廣州 510507； 2. 華南農(nóng)業(yè)大學工程學院，廣東 廣州 510642）

基于DGPS定位與雙閉環(huán)轉向控制的農(nóng)業(yè)自動導航系統(tǒng)

黎永鍵1，趙祚喜2*，高俊文1，吳曉鵬2，關偉1

（1. 廣東農(nóng)工商職業(yè)技術學院，廣東 廣州 510507； 2. 華南農(nóng)業(yè)大學工程學院，廣東 廣州 510642）

以東方紅X-804拖拉機為平臺，開發(fā)了一種基于RTK-DGPS定位和雙閉環(huán)轉向控制相組合的農(nóng)業(yè)自動導航系統(tǒng)。系統(tǒng)主要包括RTK-DGPS接收機、姿態(tài)航向參考系統(tǒng)（AHRS）、轉向控制器、電控液壓轉向裝置和轉向角檢測傳感器。設計了Kalman濾波器對定位數(shù)據(jù)進行平滑處理，同時實現(xiàn)航向角的校正。為實現(xiàn)自動轉向，在拖拉機原有手動控制系統(tǒng)基礎上加上電控比例液壓閥，并設計電控單元。然后，推導了轉向系統(tǒng)的數(shù)學模型，通過Matlab仿真工具箱得到傳遞函數(shù)的參數(shù)，設計了雙閉環(huán)轉向控制算法。最后，進行了算法驗證試驗和田間試驗，結果表明，雙閉環(huán)控制方法較好抑制了穩(wěn)態(tài)時的震蕩現(xiàn)象，方波信號的角度跟蹤穩(wěn)態(tài)時最大誤差0.60°，平均誤差0.40°，平均延時為0.20 s；設計的Kalman濾波器有助于提高定位系統(tǒng)的精度，橫向跟蹤誤差不超過0.09 m，轉向角度平均跟蹤誤差為0.43°，延時0.25 s。

農(nóng)業(yè)自動導航；載波相位差分全球定位系統(tǒng)；姿態(tài)航向參考系統(tǒng)；卡爾曼濾波；雙閉環(huán)控制

黎永鍵， 趙祚喜， 高俊文， 吳曉鵬， 關偉. 基于DGPS定位與雙閉環(huán)轉向控制的農(nóng)業(yè)自動導航系統(tǒng)［J］. 農(nóng)業(yè)現(xiàn)代化研究，2016， 37（2）： 387-394.

Li Y J， Zhao Z X， Gao J W， Wu X P， Guan W. Agricultural automatic navigation system based on DGPS positioning and double closed-loop steering control［J］.

Research of Agricultural Modernization， 2016， 37（2）： 387-394.

農(nóng)業(yè)自動導航需要解決準確定位和自動轉向控制的問題［1］。為提高農(nóng)業(yè)導航定位精度，農(nóng)業(yè)智能車輛田間導航定位需要測量載體位置坐標、航向角以及姿態(tài)角等導航參數(shù)，其測量值應盡量準確［2］。大量研究工作表明，Kalman濾波能有效解決慣性導航系統(tǒng)誤差隨時間積累的問題［3］。趙祚喜［4］等設計了基于GPS的組合慣性導航系統(tǒng)，利用“位置—速度—姿態(tài)”運動學模型建立了Kalman濾波器，使導航定位精度提高到0.05 m以內(nèi)。但是，試驗證明Kalman濾波的使用有以下局限性：研究物體的運動過程能夠線性化表示，模型誤差和傳感器測量誤差能夠用高斯白噪聲模型化；不滿足以上條件，則不能提供最優(yōu)估計［5］。因此，實現(xiàn)農(nóng)業(yè)機械自動導航不僅需要Kalman濾波提供準確姿態(tài)信息，還需要對非線性的轉向過程進行控制。

農(nóng)業(yè)導航系統(tǒng)采用的控制方法一般為線性模型控制法、模糊控制法和最優(yōu)控制等方法［6］。周建軍等［7］提出了一種基于遺傳算法的自適應模糊控制方法，將拖拉機自動導航的跟蹤精度控制在10 cm以內(nèi)。羅錫文等［8］設計了基于PID的農(nóng)業(yè)自動導航控制器，采用跨行地頭轉向控制方式，直線跟蹤的最大誤差小于0.15 m。以上研究主要使用了PID控制算法，存在的問題是控制算法單一，通常只用位置傳感器或者角速度傳感器作為反饋信號，容易出現(xiàn)控制超調(diào)現(xiàn)象。近年來，雙閉環(huán)控制方法在農(nóng)業(yè)自動導航得到了應用。吳曉鵬［9］設計了雙閉環(huán)PID控制算法，實現(xiàn)拖拉機自動轉向控制，其信號跟蹤誤差最大不超過1.1°。

本文在東方紅X-804拖拉機上開發(fā)了基于載波相位差分GPS（RTK-DGPS）與雙閉環(huán)轉向控制的自動導航系統(tǒng)。設計了Kalman濾波器對定位數(shù)據(jù)進行平滑，從而消除航向角偏差。對原有機械系統(tǒng)進行了電控液壓系統(tǒng)改造，將拖拉機運動與轉向模型相結合，設計雙閉環(huán)轉向控制系統(tǒng)實現(xiàn)拖拉機田間地頭自動轉彎。通過以上方法提高農(nóng)業(yè)車輛田間導航控制精度。

1 自動導航控制系統(tǒng)結構

1.1 GPS與AHRS組合的導航定位系統(tǒng)

自動導航控制系統(tǒng)在東方紅X-804拖拉機上進行設計，其主要組成部分包括全球定位系統(tǒng)（GPS）、姿態(tài)航向參考系統(tǒng)（AHRS）、轉向控制系統(tǒng)、轉向執(zhí)行機構等。吳曉鵬和趙祚喜［10］介紹了拖拉機基本參數(shù)。GPS 設備是Trimble 公司的Trimble5700 型，包括GPS 天線與流動站接收機，支持RTK-DGPS定位，動態(tài)測量水平定位精度為±10 mm +1 ppm RMS，垂直定位精度可達±20 mm +1 ppm RMS［11］。AHRS使用由美國Crossbow公司生產(chǎn)高精度姿態(tài)航向參考系統(tǒng)AHRS500GA，其基本參數(shù)為：26位數(shù)字輸出，測量信號包括姿態(tài)角、三軸角速度、三軸角速度、溫度等，測量精度分別為：航向0.2° RMS，俯仰與橫滾0.03° RMS［12］。

1.2 轉向控制系統(tǒng)結構

為了實現(xiàn)東方紅X-804田間自動導航作業(yè)，陳斌［13］對其轉向系統(tǒng)進行了改造。在原機械式轉向系統(tǒng)基礎上并聯(lián)一臺由步進電機直接驅(qū)動的全液壓轉向器，執(zhí)行機構包括電控液壓閥、換擋電磁閥和溢流閥（圖1）。系統(tǒng)工作過程為：當選擇自動控制模式時，系統(tǒng)切換閥油路，由手動控制油路切換為自動控制油路。使用電控液壓閥控制流量以調(diào)整轉向速度，換向電磁閥控制油路切換以控制前輪轉向，溢流閥作用是防止油壓過載。

圖1 東方紅X-804電液控轉向系統(tǒng)Fig. 1 Electro-hydraulic controlled steering system of Dongfanghong X-804 tractor

東方紅拖拉機轉向控制由安裝在轉向橋上的轉向控制器實現(xiàn)，采用ADIS16300（美國ADI公司生成的MEMS慣性測量集成模塊）和KMA199 （NXP公司生成的非接觸磁阻效應傳感器）聯(lián)合測量。圖2是轉向控制系統(tǒng)的傳感器部分安裝示意圖，傳感器安裝方法為：以轉向輪轉向軸為軸心，ADIS16300安裝在與KMA199同軸的平面上，分別測量以轉向軸為軸心的轉向角速度和轉向角。

圖2 轉向控制系統(tǒng)安裝示意圖Fig. 2 Layout of steering control system on tractor

1.3 自動導航控制系統(tǒng)的硬件實現(xiàn)

本文設計的導航系統(tǒng)硬件主要部分包括電源電路、傳感器、換向閥驅(qū)動電路、比例閥驅(qū)動電路、SD卡存儲電路等，上位機使用導航控制器AT91SAM9261（圖3）。

1）KMA199。KMA199是一種非接觸式的磁阻效應傳感器，其基本原理：當有磁場作用于傳感器時，磁阻就會發(fā)生改變，根據(jù)磁阻值的變化由數(shù)學關系即可推算出轉角大小［14］。

圖3 導航控制系統(tǒng)硬件框圖Fig. 3 Structural diagram of the navigation control system

2）ADIS16300。ADIS16300是美國ADI公司生產(chǎn)的四自由度慣性檢測系統(tǒng)，其內(nèi)部集成了數(shù)字角速度傳感器、三軸加速度傳感器以及溫度傳感器，上電后以采樣頻率819.2 SPS測量載體角速度和三軸加速度，同時提供橫滾角和俯仰角計算值［15］。

3）CAN總線控制器。CAN即控制總路線區(qū)域網(wǎng)絡，具有結構簡單、傳輸距離短、傳輸效率快、各節(jié)點實現(xiàn)自由通信的特點［16］。考慮到組合導航系統(tǒng)設計要求和拖拉機工作過程，選用CAN通訊網(wǎng)絡作為導航控制網(wǎng)絡。

4）主控芯片LMS8962。LMS8962是ARM7v7架構的Cortex-M3內(nèi)核微處理器，具有運算能力強大、低功耗的特點，最高運算速度可達 70 MIPS［17］。從性能上看，LMS8962可作為本文轉向控制系統(tǒng)的主控芯片。

2 Kalman濾波與雙閉環(huán)轉向控制算法

2.1 GPS與AHRS組合的導航定位校正方法

研究表明，農(nóng)業(yè)車輛在凹凸不平的田地里行駛過程中，由于車體發(fā)生傾斜，GPS天線端與車體中心不重合，導致GPS定位出現(xiàn)較大偏差［18］。本文使用RTK-DGPS為主要導航手段，AHRS500GA輔助修正的方法對農(nóng)業(yè)拖拉機導航參數(shù)進行測量。

對于坐標系，為表示方便，在三維位置矢量的右上方用符號表示該坐標系屬性，其中大地坐標系（n），載體坐標系（b）。根據(jù)坐標轉換關系，有以下式子成立：

式中：Xa、Ya、Za為真實的質(zhì)心位置，Xg、Yg、Zg為GPS測得的天線位置。在大地坐標系中，（1）式右端第一項由GPS測得；在載體坐標系下，（1）式右端第二項由車體幾何關系，測量獲得。Cbn是方向余弦矩陣，作用是將定位誤差投影到高斯平面坐標系下，該矩陣計算方法為［19］：

式中：φ、θ、ψ分別為農(nóng)用車輛的橫滾角、俯仰角和航向角。如果準確測得φ、θ、ψ的值，代入（2）式求出方向余弦矩陣，再代入式（1）即可車輛真實質(zhì)心位置。

2.2 Kalman濾波器設計

在組合導航系統(tǒng)里，拖拉機的鐵質(zhì)材料對于AHRS500GA磁強計測量產(chǎn)生一定的影響，從而導致航向角測量值的偏移誤差。設計Kalman濾波器可解決以上問題［20］。

Guo［21］介紹了應用于導航控制的拖拉機運動學模型。假設vk為車體速度，δk是時間τ內(nèi)的轉向輪偏轉角，xk、yk表示K時刻位置坐標，T為采樣周期，△ψk為航向偏移誤差，由航位推算方法為：

將式（3）、式（4）進行三角函數(shù)展開，則：

由此設定車體前進速度為vk，vkx和vky分別是△ψk和△ψk+90°方向上投影，即vkx=vkcos△ψk，vky=vksin△k。Kalman濾波器的狀態(tài)變量為：

1）設wk為輸入噪聲，系統(tǒng)狀態(tài)方程為：

式中：wk是輸入噪聲，φk是系統(tǒng)矩陣，由下式計算：

2）觀測方程為：

式中：Zk= ［xckyck］，vk是觀測噪聲，Hk是觀測矩陣，由下式計算：

3）將線性離散Kalman濾波器的遞歸差分構成預測方程：

4）濾波方程為：

增益方程為：

濾波誤差方差方程為：

Qk為輸入噪聲協(xié)方差矩陣，Rk為觀測噪聲協(xié)方差矩陣，通過對試驗數(shù)據(jù)進行多元統(tǒng)計獲得。經(jīng)過多次反復試驗，Qk和Rk分別取：

2.3 轉向系統(tǒng)模型的建立

根據(jù)轉向系統(tǒng)工作原理，將系統(tǒng)進行簡化，主要部分包括油管、轉向油缸、油箱和轉向輪等（圖4）。

圖4 油缸運動學分析Fig. 4 Kinematics analysis of cylinder

對油缸運動進行動態(tài)分析，轉向時液壓油油管進入轉向油缸，通過活塞驅(qū)動轉向輪轉動，轉向過程中作用在轉向輪上的外力為Ft。以活塞為研究對象，設A為油缸活塞橫截面積，pL為負載壓降，B、m分別為活塞和負載的粘性阻尼系數(shù)、質(zhì)量，Q是流入轉向油缸的液壓油量，y是轉向油缸活塞移動距離，則根據(jù)牛頓第二定律，平衡時上述變量滿足關系式：

轉向輪轉角（δ）與油缸活塞位移（y）關系為：

轉向輪轉向角速度（ω）與油缸活塞位移（y）一階導數(shù)關系為：

比例閥開度（x）與輸入電流大小（u）關系為：

式中：kδ、kω、ku為對應式子比例系數(shù)。

在不考慮Ft情況下，對式（12）-（15）進行拉氏變換并聯(lián)解得到系統(tǒng)傳遞函數(shù)，結果為二階函數(shù)［22］。考慮到電控液壓閥存在動作延時，并且轉向輪等處存在摩擦，系統(tǒng)的傳遞函數(shù)改為轉向角速度ω（s）與系統(tǒng)輸入U（s）之間的傳遞函數(shù)Gω（s）、轉向角度Φ（s）與系統(tǒng)輸入U（s）之間的傳遞函數(shù)Gφ（s）。使用一個二階慣性環(huán)節(jié)和一個延時環(huán)節(jié)表示為：

式中：Kt、Kb、Kc表示傳遞函數(shù)的未知參數(shù)，其確定方法為：首先進行開環(huán)階躍響應，輸入比例閥控制電流的階躍信號，輸出為轉向角速率。然后通過Matlab系統(tǒng)辨識工具箱對轉向系統(tǒng)進行辨識，得：Kt=0.405 8；Kb=6.945 8；Kc=3.788 1。

2.4 轉向控制器的設計

考慮到轉向系統(tǒng)的非線性特點，對傳統(tǒng)PID算法進行改進，設計了基于雙閉環(huán)控制方法的轉向控制器（圖5）。基本思路是：以轉向角度控制為外環(huán)基礎上，加入一個角速度控制的內(nèi)環(huán)。外環(huán)是通過角度傳感器測量轉向輪實時轉角（θR）與目標角度（θT）進行對比，由控制器1實現(xiàn)角度控制；內(nèi)環(huán)是通過陀螺儀測量實時角速度（ωR）與目標角速度（ωT）進行對比，由控制器2實現(xiàn)速度控制。加入內(nèi)環(huán)控制的目的是使得轉向車輪能較平穩(wěn)地以較低速度到達目標位置［23］。

1）外環(huán)轉向角度控制。設eθ為轉角誤差，由PID算法可得到下式：

式中：KθP、KθI、KθD是外環(huán)角度PID控制的比例、積分和微分部分的系數(shù)，eθ_b為上一次的角度誤差，eiθ為角度誤差的積分，eiθ_b為上一次的角度誤差積分，edθ為角度誤差的微分，tS為采樣時間。

圖5 雙閉環(huán)控制結構框圖Fig. 5 Block diagram of double closed-loop control

2）內(nèi)環(huán)角速度控制。系統(tǒng)輸出量是由速度內(nèi)環(huán)決策，即電控比例閥的輸入電流量OI，ω為當前車輪轉向的角速度。設eω為角速度誤差，由PID算法可得：

式中：KωP、KωI、KωD是內(nèi)環(huán)角速率PID控制的比例、積分和微分部分的系數(shù)，eω_b為上一次的角速率誤差，eiω為角速率誤差的積分，eiω_b為上一次的角速率誤差積分，edω為角速率誤差的微分，tS為內(nèi)環(huán)采樣時間。

聯(lián)立式（18）-（23），即可得到輸出量OI。在本文控制系統(tǒng)中，轉向輪的角度和角速度分別通過角度傳感器KMA199和ADIS16300陀螺儀測量得到。

3）參數(shù)估計方法。以Simulink為平臺建立仿真模型，經(jīng)過仿真得到系統(tǒng)傳遞函數(shù)的未知參數(shù)［24］。根據(jù)傳遞函數(shù)式，使用一個二階慣性環(huán)節(jié)加上一個系統(tǒng)的延時，分別建立系統(tǒng)外環(huán)、內(nèi)環(huán)的仿真模型。主要應用模塊為：采用波形信號發(fā)生器產(chǎn)生關于角速率的目標方波信號；MATLAB Function在此次實現(xiàn)PID控制算法的實現(xiàn)；系統(tǒng)延時模塊Transport Delay，延時時間常數(shù)設置τ=0.2 s。經(jīng)過反復參數(shù)調(diào)整，得到角度外環(huán)的PID控制參數(shù)為：KθP=1.01，KθI=0.020，KθD=0.012。角速度內(nèi)環(huán)的PID控制參數(shù)為：KωP=103.50，KωI=8.29，KωD=1.75。

3 驗證試驗與結果分析

3.1 雙閉環(huán)轉向控制算法試驗

試驗目的：驗證本文設計雙閉環(huán)PID控制算法能有效地使轉向執(zhí)行機構按控制系統(tǒng)指令達到目標角度。

試驗方法：上位機以10 Hz頻率發(fā)送控制指令方波信號，該信號為5o或10o階躍信號，通過CAN通訊網(wǎng)絡向轉向控制器傳輸控制指令，下位機的執(zhí)行頻率同樣10 Hz。運行本文設計的雙閉環(huán)PID控制算法，考慮到之前的試驗結果有較大的震蕩超調(diào)現(xiàn)象，加入微分控制環(huán)節(jié)，觀測控制信號追蹤結果。圖6為一次典型方波跟蹤試驗結果。

1）從圖6（a）可以看出，對于5o或10o階躍信號，跟蹤角度與方波信號有良好一致性，較好抑制了超調(diào)振蕩現(xiàn)象。圖6（b）是方波信號跟蹤誤差，可知角度跟蹤值未達到穩(wěn)態(tài)時，出現(xiàn)類似于三角波的較大誤差曲線，說明有明顯震蕩；當跟蹤值達到穩(wěn)態(tài)時，誤差在0o附近波動，平均誤差值為0.40o，最大誤差值0.60o。最大跟蹤時間1.6 s，最小跟蹤時間1.0 s，平均跟蹤時間為1.3 s，平均延時0.2 s。可見，穩(wěn)態(tài)的振蕩有較好的抑制。

2）圖6（c）是圖6（a）在方波信號跟蹤時的角速率跟蹤情況。由圖6（d）可以看出，跟蹤角速率與目標角速率曲線有良好一致性，統(tǒng)計數(shù)據(jù)表明穩(wěn)態(tài)時平均誤差1.25 °/s，延時時間平均值為0.2 s。振蕩較大時存在角速率跟蹤超調(diào)現(xiàn)象，最大誤差達到5 °/s。

圖6 方波信號響應與跟蹤誤差Fig. 6 Square signal response and tracking error

3）綜上所述，當轉向角度與目標角度相差較大時，應控制車速為較大值；當轉向角度接近目標角度時，速度應降低，以一個較小的速度平緩達到目標角度。但是在換向和進入穩(wěn)態(tài)前角速率控制出現(xiàn)了較大的超調(diào)現(xiàn)象，角速率控制尚存不足。

3.2 田間驗證試驗

試驗目的：驗證本文設計Kalman濾波器與雙閉環(huán)轉向控制系統(tǒng)在拖拉機實際工作過程中能有效提高導航控制精度，拖拉機能按照預先規(guī)劃的路徑自動轉向和行駛。

試驗方法：試驗在華南農(nóng)業(yè)大學的一塊試驗田中進行，實驗的內(nèi)容包括無人駕駛拖拉機直線行駛和地頭轉向。按以下步驟進行路徑規(guī)劃：

1）首先長方形的試驗田四個頂點作為定點，分別記為A、B、C、D，使用RTK-DGPS測定每個點的坐標。

2）以AD構成的邊為基準線，劃定總數(shù)15條平行作業(yè)線，行與行之間的距離相同，由拖拉機作業(yè)間距決定。

3）按照地頭轉向的設計要求，編制作業(yè)順序。直線跟蹤過程中，拖拉機的機械速度設定為0.5 m/s；地頭轉向過程中，拖拉機機械速度設定為0.2 m/s。RTK-DGPS與AHRS500GA組合系統(tǒng)實時提供拖拉機的位置和姿態(tài)信息，經(jīng)過Kalman濾波獲得高精度的導航定位數(shù)據(jù)。拖拉機的直線行走和地頭轉向使用了雙閉環(huán)PID控制，以盡量按預定軌跡行駛。在田間作業(yè)過程中，由工控機記錄下GPS以及轉向系統(tǒng)的數(shù)據(jù)，CAN總線實現(xiàn)數(shù)據(jù)通信。按此方法拖拉機進行多次田間行走，啟實時記錄并儲存數(shù)據(jù)。

將拖拉機行走實時GPS定位坐標點到當前作業(yè)行坐標的距離定義為橫向跟蹤誤差（LTE）。在獲取LTE參數(shù)的基礎上，對LTE進行統(tǒng)計分析，以LTE平均值LTEavg、最大值LTEmax以及標準差σLTE作為路徑跟蹤效果的評價指標，計算方法為：

試驗結果分析：圖7是本文設計方法下東方紅拖拉機在田間作業(yè)時的定位追蹤與轉向角跟蹤情況。圖7（a）反映了目標轉向角度（Target angle）與轉向輪實際轉角（KMA Angle）關系，其中Target angle控制指令由上位機發(fā)送，KMA Angle則是KMA199傳感器測量結果。從圖中可知，轉向系統(tǒng)在-5°至25°轉角范圍內(nèi)轉向動作執(zhí)行情況良好，平均跟蹤誤差為0.43o，誤差值控制在合理范圍內(nèi)。

圖7 拖拉機田間試驗結果Fig.7 Result of the field test on tractor

圖7（b）是拖拉機田間行駛實際軌跡與規(guī)劃路徑比較。對實驗數(shù)據(jù)進行統(tǒng)計分析（表1），路徑跟蹤的精度指標和不采用Kalman濾波處理相比，直線跟蹤時，分別由0.044 m、0.136 m和0.056 m減小到0.020 m、0.090 m和0.020 m；地頭轉向跟蹤時，m、0.082 m和0.115 m。可知，Kalman濾波算法和分別由0.041 m、0.088 m和0.019 m減小到0.018 雙閉環(huán)PID轉向控制方法能有效提高導航控制精度和穩(wěn)定性。但仍存在有問題，如田頭轉彎過程出現(xiàn)一定的控制延滯，平均延時為0.25 s；在轉向角度較大的區(qū)域，跟蹤誤差較大。

表1 試驗跟蹤誤差統(tǒng)計結果Table 1 Results of the experimental tracking error

4 結論

本文提出基于RTK-DGPS定位與雙閉環(huán)轉向控制的農(nóng)業(yè)自動導航系統(tǒng)，設計了適用于農(nóng)業(yè)機械田間作業(yè)的Kalman濾波方法以及轉向控制器。測試結果表明，轉向輪能按照轉向控制系統(tǒng)發(fā)出的指令達到目標角度和角速度，穩(wěn)態(tài)跟蹤誤差不超過1°。拖拉機能按照預先規(guī)劃的路徑自動轉向和行駛，直線行駛的橫向跟蹤誤差不超過0.09 m，地頭轉向的橫向跟蹤誤差不超過0.088 m。有效提高GPS定位精度。

采用基于ARM7v7架構的Cortex-M3內(nèi)核微處理器LMS8962實現(xiàn)了以上濾波算法與控制算法，研制了導航控制器，在精細農(nóng)業(yè)的實踐中有良好的推廣前景。

［1］ 蔣浩然， 陳軍， 王虎， 等. 移動機器人自動導航技術研究進展［J］.西北農(nóng)林科技大學學報（自然科學版）， 2011， 39（12）： 207-209. Jiang H R， Chen J， Wang H， et al. Research progress of automatic navigation technology for orchard mobile robot［J］. Journal of Northwest A & F University （Natural Science Edition）， 2011，39（12）： 207-209.

［2］ 胡煉， 羅錫文， 張霖， 等. 1PJ-4.0型激光平地機設計與試驗［J］.農(nóng)業(yè)機械學報， 2014， 45（4）： 146-150. Hu L， Luo X W， Zhang L， et al. Development of 1PJ-4.0 laser leveler installed on a wheeled tractor for paddy field［J］. Transactions of the Chinese Society for Agricultural Machinery，2014， 45（4）： 146-150.

［3］ 秦永元. 慣性導航［M］ . 北京： 科學出版社， 2014： 243-259. Qin Y Y. Inertial Navigation［M］. Beijing： Science Press， 2014：243-259.

［4］ 黎永鍵， 趙祚喜， 高俊文. 基于GPS/SINS組合的農(nóng)業(yè)導航定位系統(tǒng)設計與研究［J］. 農(nóng)機化研究， 2014， 36（3）： 16-22. Li Y J， Zhao Z X， Gao J W. Design and research of agricultural navigation positioning system based on GPS and SINS［J］. Journal of Agricultural Mechanization Research， 2014， 36（3）： 16-22.

［5］ 鄧自立， 王欣， 高媛. 建模與估計［M］. 北京： 科學出版社，2008： 126-135. Deng Z L， Wang X， Gao Y. Modeling and Estimation［M］. Beijing：Science Press， 2008： 126-135.

［6］ 劉豹， 唐萬生. 現(xiàn)代控制理論［M］. 北京： 機械工業(yè)出版社，2013： 188-193. Liu B， Tang W S. Modern Control Theory［M］. Beijing： China Machine Press， 2013： 188-193.

［7］ 周建軍，張漫，劉剛， 等. 基于模糊控制的農(nóng)用車輛路線跟蹤［J］. 農(nóng)業(yè)機械學報， 2009， 40（4）： 151-155. Zhou J J， Zhang M， Liu G， et al. Path tracking for agricultural vehicle based on fuzzy control［J］. Transactions of the Chinese Society for Agricultural Machinery， 2009， 40（4）： 151-155.

［8］ 羅錫文， 張智剛， 趙祚喜， 等. 東方紅X-804拖拉機的DGPS自動導航控制系統(tǒng)［J］. 農(nóng)業(yè)工程學報， 2009， 25（11）： 139-143. Luo X W， Zhang Z G， Zhao Z X， et al. Design of DGPS navigation control for Dongfanghong X-804 tractor［J］. Transactions of the Chinese Society of Agricultural Engineering， 2009， 25（11）： 139-143.

［9］ 吳曉鵬. 東方紅拖拉機轉向伺服控制系統(tǒng)設計［D］. 廣州： 華南農(nóng)業(yè)大學， 2011. Wu X P. Steering control system design for Dongfanghong X-804 tractor［D］. Guangzhou： South China Agricultural University，2011.

［10］ 吳曉鵬， 趙祚喜. 東方紅拖拉機自動轉向系統(tǒng)設計［J］. 農(nóng)業(yè)機械學報， 2009（S1）： 1-5. Wu X P，Zhao Z X. Development of automatic steering control system based on Dongfanghong tractor［J］. Transactions of the Chinese Society for Agricultural Machinery， 2009（S1）： 1-5.

［11］ 張智剛. 插秧機的DGPS 自動導航控制系統(tǒng)研究［D］. 廣州： 華南農(nóng)業(yè)大學， 2006. Zhang Z G. Research of DGPS navigation control system for rice transplanter［D］. Guangzhou： South China Agricultural University， 2006.

［12］ Crossbow Technology， Inc. AHRS500GA-Series User's Manual：Revision B［M］. New York： McGraw Hill， 2007： 21-25.

［13］ 陳斌. 東方紅X-804拖拉機自動轉向控制系統(tǒng)研究與設計［D］.廣州： 華南農(nóng)業(yè)大學， 2009. Chen B. Research and design of steering control system for Dongfanghong X-804 tractor［D］. Guangzhou： South China Agricultural University， 2009.

［14］ NXP， Inc. KMA199E product data sheet［DB/OL］. （2014-11-01）［2015-04-01］. http：//www.nxp.com/acrobat_download/datasheets/ KMA199E_1.pdf.

［15］ Analog Devices， Inc. ADIS16300 Datasheet［DB/OL］. （2009-04-01）［2015-01-28］. http：//www.analog.com/static/imported-files/ data_sheets/ADIS16300.pdf.

［16］ Bosch， Inc. Bosch CAN user's guide［EB/OL］. （2013-09-01）［2015-02-01］. http：//www.xhl.com.cn/sjsc/sjscdetail.asp？sid=25.

［17］ Luminary Micro， Inc. LM3S8962 microcontroller datasheet［DB/ OL］. （2010-01-19）［2015-01-22］. http：//www.luminarymicro.com.

［18］ 趙汝祺， 趙祚喜， 趙汝準. 基于卡爾曼濾波器的車輛定位試驗［J］. 農(nóng)業(yè)工程學報， 2012， 28（18）： 49-54. Zhao Y Q， Zhao Z X， Zhao Y Z. Research and experiment on vehicle localization based on polynomial Kalman filter［J］. Transactions of the Chinese Society of Agricultural Engineering，2012， 28（18）： 49-54.

［19］ 孫冬梅， 田曾山， 韓寧軍. 捷聯(lián)慣導系統(tǒng)中四元素法求解姿態(tài)角仿真模擬［J］. 彈箭與制導學報， 2009， 29（1）： 51-60. Sun D M， Tian Z S， Han L J. Simulation on quaternion calculate attitude angle of the strapdown inertial navigation system［J］. Journal of Projectiles， Rockets， Missiles and Guidance， 2009，29（1）： 51-60.

［20］ Lu J J， Zhou J， Lu X D， et al. The measurement correction of MEMS gyroscope based interlaced Kalman filter［C］. Advanced Materials Research， 2011： 1083-1088.

［21］ Guo L S. Development of a low-cost navigation system for autonomous off-road vehicle［D］. Urbana-Champaign： University of Illinois， 2003.

［22］ Gordon M. Robot Builder's Sourcebook［M］. New York： McGraw Hill eBook， 2003： 577-581.

［23］ Sakai A， Kuroda Y. Discriminatively trained unscented Kalman filter for mobile robot localization［J］. Journal of Advanced Research in Mechanical Engineering， 2010， 1（3）： 153-161.

［24］ Jay A F. Aided Navigation： GPS with High Rate Sensors［M］. New York： McGraw Hill， 2006： 35-60.

（責任編輯：童成立）

Agricultural automatic navigation system based on DGPS positioning and double closed-loop steering control

LI Yong-jian1， Zhao Zuo-xi2， GAO Jun-wen1， WU Xiao-peng2， GUAN Wei1

（1. Guangdong AIB Polytechnic College， Guangzhou， Guangdong 510507， China； 2. College of Engineering， South China Agricultural University， Guangzhou， Guangdong 510642， China）

An agricultural automatic navigation system based on real time kinematic differential global positioning system （RTK-DGPS） and double closed-loop steering control was developed on Dongfanghong X-804 tractor. The system includes RTK-DGPS receiver， attitude and heading reference system （AHRS）， steering controller， electrohydraulic steering actuator and encoder sensor for steering angle testing. One Kalman filter was designed to smooth the collected GPS positioning data and evaluate the bias error of the heading angle. In order to realize automatic steering， an electro-hydraulic proportional valve was added to the original manually operated steering system and the electronic control unit was designed. Then the mathematical model of steering system was derived， using Matlab System Identification Toolbox to estimate transfer function parameters， and a double closed-loop control algorithm for steering was designed. Finally， experimental results of square signal tracking test and field test are presented， showing that the double closed-loop control solved the control overshoot well and the maximum error is 0.60°， average error 0.40°， average delay 0.20 s. The designed Kalman filter is helpful to improve the accuracy of positioning system and the maximum lateral tracking error is less than 0.09 m， average steering angle tracking error 0.43°， average delay 0.25 s.

agricultural automatic navigation； real time kinematic differential global positioning system （RTK-DGPS）；attitude and heading reference system （AHRS）； Kalman filter； double closed-loop control

National Natural Science Foundation of China （61175081）； 948 Key Project of Ministry of Agriculture （2011-G32）； Doctoral Project （20114404110003）.

ZHAO Zuo-xi， E-mail： zhao_zuoxi@scau.edu.cn.

26 May， 2015； Accepted 20 October， 2015

S219.02

A

1000-0275（2016）02-0387-08

10.13872/j.1000-0275.2015.0186

國家自然科學基金項目（61175081）；農(nóng)業(yè)部948計劃重點項目（2011-G32）；博士點基金項目（20114404110003）。

黎永鍵（1983-），男，廣東清遠人，碩士，講師，主要從事農(nóng)業(yè)電氣化與自動化研究，E-mail： liyj@gdaib.edu.cn；

趙祚喜（1968-），男，湖南慈利人，博士，教授，主要從事農(nóng)業(yè)機械導航技術研究，E-mail： zhao_zuoxi@scau.edu.cn。

2015-05-26，接受日期：2015-10-20