基于DGPS與雙閉環控制的拖拉機自動導航系統

黎永鍵 趙祚喜 黃培奎 關 偉 吳曉鵬

(1.廣東農工商職業技術學院機電系， 廣州 510507；2.華南農業大學南方農業機械與裝備關鍵技術教育部重點實驗室， 廣州 510642)

基于DGPS與雙閉環控制的拖拉機自動導航系統

黎永鍵1趙祚喜2黃培奎2關 偉1吳曉鵬2

(1.廣東農工商職業技術學院機電系， 廣州 510507；2.華南農業大學南方農業機械與裝備關鍵技術教育部重點實驗室， 廣州 510642)

以東方紅X-804型拖拉機為平臺，設計了一種基于RTK-DGPS定位和雙閉環轉向控制相結合的自動導航系統，研究提高農業機械導航控制精度的方法。闡述了導航系統整體設計方案，以RTK-DGPS和AHRS500GA分別提供位置信息和輔助修正信息實現準確定位，以電控液壓轉向系統實現轉向控制。分析了整體控制的策略，建立了路徑跟蹤的傳遞函數模型，闡述了雙閉環轉向控制算法的建立過程，以及控制器的硬件實現。試驗結果表明：GPS定位數據經過校正后，平均偏差降低至0.031 m；雙閉環控制算法提高了自動轉向系統性能，穩態時方波信號以及正弦波信號的跟蹤誤差平均值為0.40°；在拖拉機田間作業跟蹤過程中，路徑跟蹤誤差平均值不超過0.019 m，轉向輪偏角跟蹤誤差平均值為0.43°，標準差不超過0.041 m。

拖拉機； 自動導航； 轉向系統； 跟蹤誤差； 載波相位差分全球定位系統； 雙閉環控制

引言

農業機械自動導航技術是現代農業機械裝備的重要支持技術之一。實現農業機械的智能化導航，可以有效降低勞動強度，提高田間作業精準度[1]。近年來，國內外對于農用車輛路徑跟蹤控制技術進行了大量研究，主要采用全球定位系統(Global positioning system，GPS)技術與慣性導航技術相結合的方法[2]。ZHANG等[3]利用載波相位差分全球定位系統(Real time kinematic differential global positioning system，RTK-DGPS)、磁羅盤傳感器和慣性傳感器，將Kalman濾波融合處理后的拖拉機定位精度控制在0.1 m內。羅錫文等[4]采用RTK-DGPS定位技術，研究東方紅X-804型拖拉機的自動導航系統，設計了直線跟蹤的比例-微分控制(Proportion-differentiation，PD)導航控制器,試驗結果表明：在拖拉機行進速度為0.8 m/s時，直線跟蹤最大誤差控制在0.15 m以內，平均跟蹤誤差控制在0.03 m以內。該系統具有良好的適應性，但是存在以下問題：①田間作業環境復雜，系統誤差隨時間積累，導致GPS定位準確度下降。②電控液壓系統具有非線性特征，轉向控制采用單一的位置傳感器或者角度傳感器測量值作為控制反饋量，容易出現控制超調現象。

本文在東方紅X-804型拖拉機上開發基于RTK-DGPS與雙閉環轉向控制相結合的自動導航系統，實現東方紅拖拉機較高精度的速度控制和轉向控制，減少控制超調，以期提高農業車輛田間導航控制精度。

1 自動導航系統整體設計

自動導航系統在東方紅X-804型拖拉機上進行設計，拖拉機基本參數如表1所示[5]。

1.1 自動導航系統整體設計方案

根據研究現狀，表2首先列出了原有控制方案。對比前人的研究，本文選擇的方案有以下改進：①建立RTK-GPS與姿態航向參考系統(Attitude and heading reference system，AHRS)組合的定位系統。②轉向控制系統的傳感部分采用無接觸磁阻傳感器KMA199和慣性傳感器ADIS16300組合。③改進控制電路，控制芯片采用AT91SAM9261。 ④雙閉環控制方法。

表1 東方紅X-804型拖拉機參數Tab.1 Main parameters of Dongfanghong X-804 tractor

表2 自動導航系統設計方案Tab.2 Design of automatic navigation system

1.2 組合定位方法

農業車輛在田間作業行駛時，由于地面凹凸不平導致車體傾斜而存在較大的橫滾角、俯仰角，車載GPS天線端與車體中心不重合。因此，定位數據實際上是天線的位置，而不是車體中心的位置，定位偏差較大[6]。基于以上考慮，必須通過姿態角校正原始接收數據的方法以提高定位準確度。

本文設計了Trimble5700型RTK-DGPS(Trimble 公司)與AHRS500GA型AHRS(Crossbow公司)組合的定位方法。Trimble5700 型RTK-DGPS的動態測量水平定位精度為±10 mm+10-6RMS，垂直定位精度可達±20 mm+10-6RMS[7]。AHRS500GA測量精度為：俯仰與橫滾0.03°RMS，航向0.2°RMS[8]。在定位算法中，RTK-DGPS為主要導航方式，用于測量拖拉機的位置和速度信息；AHRS500GA提供車體姿態和航向等信息以修正位置信息。坐標轉換關系為[9]

(1)

其中

(2)

由式(1)、(2)可知，利用GPS測得大地坐標系下的位置信息，通過AHRS500GA準確測得φ、θ、ψ的值，代入式(2)求出方向余弦矩陣，再代入式(1)即可計算出車體中心的真實GPS位置信息。

1.3 電液控轉向系統

為實現自動轉向，設計并搭建了電液控轉向系統，結構如圖1所示。在東方紅拖拉機原機械轉向系統的基礎上并聯1臺由步進電動機驅動的液壓轉向器，并安裝換向電磁閥、比例閥、溢流閥作為執行元件。工作過程如下：當駕駛員選擇自動模式，轉向控制器驅動換向電磁閥工作從而實現油路后切換，以控制前輪轉向；以脈寬控制的方式通過電控比例閥控制流量以調整轉向速度，使前輪平穩地轉向目標角度[10]。溢流閥用于防止油壓過載。

圖1 東方紅X-804型電液控轉向控制系統Fig.1 Electro-hydraulic steering system of Dongfanghong X-804 tractor1.油箱 2.手動切換閥 3.轉向器 4.方向盤 5.油管 6.轉向油缸 7.轉向前輪 8.手動切換閥 9.三位四通電磁閥 10.比例閥 11.溢流閥

圖2為轉向系統的運動學分析。轉向時液壓油從油管進入轉向油缸，推動活塞使轉向輪動作。相關物理量的表示規定如下：Fw是轉向過程中作用在轉向輪的外力，SA是活塞橫截面積，PQ是負載壓降，BQ是活塞的粘性阻尼系數，m是負載質量，QL是進入油缸的液壓油量，yh是活塞移動距離。

圖2 油缸運動學分析Fig.2 Kinematics analysis of cylinder1.油管 2.轉向油缸 3.油箱 4.轉向前輪 5.活塞橫截面

以活塞為研究對象，根據牛頓第二定律建立力的平衡方程為

(3)

轉向輪轉角δ與活塞移動距離yh關系為[11]

δ=kδyh

(4)

轉向輪的角速率ω與油缸活塞移動距離yh屬于一階導數的關系[12]

(5)

比例閥的開度φp與輸入電流OI的關系為

φp=kφOI

(6)

式中kδ、kω、kφ——比例系數

文獻[13]對液壓轉向系統的傳遞函數進行了計算，結果為二階函數，但前提是Fw約為0。在實際的轉向過程中，存在電控液壓閥的執行動作延時，并且轉向輪與地面有較大的摩擦力。因此，本文使用1個二階慣性環節和1個延時環節表示，并建立相應的傳遞函數Gω(s)和傳遞函數Gδ(s)

(7)

(8)

式中U(s)——系統控制輸入δ(s)——轉向角度控制輸出ω(s)——角速度控制輸出s——拉氏變換的復數τ——時間函數的延遲Ka、Kb、Kc——系統傳遞函數參數

以比例閥電流為輸入信號，轉向角速率為輸出，利用Matlab系統辨識工具箱進行參數估計，可得：Ka=0.422 8，Kb=6.952 4，Kc=3.790 2。

1.4 CAN通信系統

以控制局域網(Controller area net，CAN)作為導航系統的通信網絡，設計主控制器節點和功能節點實現拖拉機自動導航[14]。主控制器負責處理RTK-GPS位置信息、AHRS500GA姿態信息，以及根據當前的轉向角、轉向角速度和拖拉機機具升降信息，發出控制指令，按照預先設定的路線規劃行駛及轉向。功能節點包括轉向控制節點、油門控制節點、制動控制節點、角速度測量以及機具升降控制節點。

2 導航控制器的設計

自動導航系統的控制內容包括路徑跟蹤及轉向控制，系統整體控制策略如圖3所示。由GPS測量拖拉機當前位置的坐標，由AHRS500GA測量航向角。以橫向偏差和航向角測量值為輸入量1，由控制器1決策出目標航向角，并且計算出航向角的偏差。以航向角偏差和當前轉向輪轉角為輸入量2，由控制器2決策出目標轉向角。

圖3 整體控制策略框圖Fig.3 Control block diagram of whole system

2.1 路徑跟蹤的控制學模型

路徑跟蹤的主要功能是實現拖拉機按照預先規劃的路徑行駛，將誤差控制在盡可能小的范圍內。將拖拉機行走實時GPS定位坐標點到當前作業橫坐標的距離定義為橫向跟蹤誤差。本文設計的路徑跟蹤方法如下：首先計算出拖拉機實時位置的橫向偏差量并測量實時航向角，利用控制器決策目標航向角，然后通過轉向控制系統決策目標轉向角，最后通過轉向執行機構控制拖拉機行駛路線，從而實現路徑跟蹤功能。

文獻[15]研究了拖拉機的運動學模型，方程為

傳遞函數為

(9)

式中x、y——位置坐標L——拖拉機軸距Vx——拖拉機橫向行駛速度Vy——拖拉機縱向行駛速度

2.2 雙閉環轉向控制算法設計

結合運動學模型與轉向系統模型，可得出導航系統的傳遞函數

(10)

在控制系統中，為消除結構不穩定，引入PID控制算法。PID控制由比例、積分、微分3個環節構成，其傳遞函數表達式為[16]

式中KP、KI、KD——比例系數、積分系數、微分系數

TI——積分時間

TD——微分時間

比例環節作用是成比例反映控制系統的偏差信號，以減少偏差；積分環節的作用是消除靜差，提高系統的無差度；微分環節作用是反映偏差信號的變化趨勢，引入一個有效的早期修正信號。

由于電液控轉向系統的非線性特性[17]，在自動轉向過程中存在較大的控制超調量，導致轉向輪的轉角無法準確達到目標值，從而產生震蕩現象。因此，必須設計相應的控制算法抑制超調量。相關研究表明，雙閉環控制方法較好地解決了控制超調大的問題[18-20]。本文設計了基于轉向輪的轉角和角速率的雙閉環控制系統：以轉向角度控制為外環，通過角度傳感器測量轉向輪實時轉角δR與目標角度δT進行對比，由控制算法決策出目標角速率；以角速率控制為內環，通過陀螺儀測量實時角速率ωR與目標角速率ωT進行對比，由控制算法決策出比例閥輸入電流的目標值。

(1)角度控制

設eδ為轉角誤差，則有

eδ=δT-δR

(11)

控制目標的角速率是ωT，由PID算法可得到

ωT=KδPeδ+KδIeiδ+KδDedδ

(12)

其中

eiδ=eiδ_b+eδts

(13)

edδ=(eδ-eδ_b)/ts

(14)

式中KδP、KδI、KδD——角度控制算法的系數eδ_b——上一時刻的角度誤差eiδ——角度誤差的積分部分eiδ_b——上一時刻的角度誤差積分部分edδ——角度誤差的微分部分ts——采樣時間

(2)角速率控制

設eω為角速率誤差，控制目標的比例閥電流是OT，由PID算法可得

OT=KωPeω+KωIeiω+KωDedω

(15)

其中

eiω=eiω_b+eωts

(16)

edω=(eω-eω_b)/ts

(17)

式中KωP、KωI、KωD——角速率控制算法系數eω_b——上一時刻的角速率誤差eiω——角速率誤差的積分部分eiω_b——上一時刻的角速率誤差積分部分edω——角速率誤差的微分部分

2.3 基于Simulink控制參數估計

以Simulink為平臺建立仿真模型，通過仿真得到系統傳遞函數的未知參數。根據傳遞函數式，使用1個二階慣性環節加上1個系統延時，分別建立系統外環、內環的仿真模型。運行仿真程序，圖4是典型的仿真結果。

圖4 三角波信號的跟蹤仿真結果Fig.4 Triangle wave signal tracking simulation results

圖4a是角度跟蹤結果，輸入信號為周期10 s的三角波信號，輸出信號準確完成了目標角度跟蹤，延時為0.1 s。圖4b是對應的角速度跟蹤仿真結果，三角波信號對應的角速率跟蹤信號也很好地跟蹤上目標角速率。以上跟蹤效果達到要求，其中重要的原因是跟蹤目標信號的變化率不大，表明：穩態下的雙閉環控制效果理想。經過反復參數調整，得到角度控制外環的參數為：KδP=1.05，KδI=0.023，KδD=0.015；角速度控制內環參數為：KωP=103.70，KωI=8.35，KωD=1.80。

2.4 地頭轉向的路徑規劃方法

文獻[4]通過試驗證明，由于東方紅X-804型拖拉機轉向半徑為5.3 m，在地頭轉向的過程中占空間較大，導致對行行駛困難。因此，本文采用跨行地頭轉向的方法進行轉彎時的路徑規劃，如圖5所示。當前作業行記為i，當拖拉機行駛至該行終點時，不進入i+1行，而進入n+j行(j>1，本文取j=7，其計算見3.3節)。采用跨行地頭轉向的依據是：拖拉機可以在較大的運動空間內通過兩次接近90°的轉向動作和一段短距離的直線行走即可完成地頭轉向，以解決對行困難的問題。

圖5 跨行地頭轉向的路徑規劃Fig.5 Path planning of headland turning through spanning line

在地頭轉向控制過程中，航向角是關鍵參數之一。AHRS500GA提供實際的航向角信息，控制系統將航向角測量值與目標航向角進行對比。目標航向角ψT計算式為[15]

ψT=arctan((yk-yk-N)/(xk-xk-N))×180/π

(18)

式中N為常數，其大小取決于拖拉機行駛的速度，通過大量的田間測試，本文的N取10。

地頭轉向的控制過程如下：設定距離閾值Ep和航向角度閾值Eθ。當拖拉機沿直線行駛至每行地頭轉向點附近時，車體質心距離該行節點小于Ep，主控制器發送轉向行走速度指令和機具提升指令，執行地頭轉向動作。轉向控制系統使前輪以最大轉向角向目標航向轉向；當實時航向角與目標航向之間的差值小于Eθ，即停止轉向動作并切換回直線跟蹤模式。按照相同的方法，拖拉機進入第i+7作業行。如此循環，完成16作業行的行駛。Ep和Eθ的取值由拖拉機行駛速度、轉向控制參數決定。通過反復的田間試驗調節，本文的Ep取1.5 m，Eθ取25°。

2.5 導航控制系統的硬件實現

圖6 導航控制系統硬件框圖及安裝示意圖Fig.6 Structural diagram and installation sketch of navigation system1.轉向輪 2.KMA199 3.ADIS16300 4.轉向控制系統 5.轉向橋 6.轉向節 7.轉向中心軸

設計的系統硬件框圖如圖6a所示，主要部分包括：導航控制器AT91SAM9261、電源電路、傳感器部分、轉向控制系統等。IO是上位機控制指令，IF是角度反饋信息，IL是LMS8962控制信號，ax是載體繞X軸轉動的加速度，ADC是模擬/數字轉換器，DAC是數字/模擬轉換器，SPI是串行外設接口控制器，I/O是輸入輸出口。圖6b是傳感器安裝示意圖。

2.5.1 傳感器部分

(1)角度傳感器KMA199

KMA199是一種非接觸式的磁阻效應傳感器，其測量原理：安裝該傳感器的載體運動時產生旋轉的磁場，作用在傳感器上引起磁阻的改變，磁阻值的變化量正比于轉角大小，利用數學關系即可換算得到角度值[21]。將KMA199傳感器安裝在拖拉機轉向節上以測量前輪轉向角。

(2)慣性測量單元ADIS16300

ADIS16300是4自由度慣性測量模塊，內部集成1個數字陀螺儀和3軸式加速度計，數字控制采樣速率可達819.2 SPS[22]。在慣性導航領域，ADIS16300主要用于檢測運動載體的姿態角、角速率和加速度等信息。本文以該傳感器測量轉向輪的角速率。

2.5.2 控制器選擇

(1)導航控制器：采用ARM9E 32位嵌入式微處理器AT91SAM9261作為導航控制器(上位機)。該處理器運算速度可達200 MIPS，并集成了64 MB SDRAM[23]。基于AT91SAM9261的導航控制器可完成GPS、AHRS、各傳感器的信號采集與處理，并控制執行機構工作。

(2)轉向控制芯片：采用ARMv7架構的Cortex-M3內核微處理器LMS8962作為轉向控制器芯片。該芯片內外設主要包括數模轉換模塊ADC、CAN總線控制器、 SPI總線控制器，運算能力強大且功耗低[24]。

3 試驗與結果分析

3.1 GPS定位校正試驗

為驗證GPS傾斜校正的設計效果，進行拖拉機定位信息校正試驗。

在平整路面上，首先用直尺劃出1條20 m長的直線，然后用厚度為10 cm、邊長為35 cm方磚按照直線的軌跡鋪成道路，方磚之間保持8 cm的距離。試驗過程中，駕駛東方紅X-804型拖拉機沿著直線軌跡通過方磚，其中左側車輪壓在方磚上。由于磚塊之間的空隙，車體行駛過程中有劇烈的抖動。同時啟動本文設計的組合導航系統，收集RTK-DGPS與AHRS500GA信號，并利用式(1)、(2)進行校正，數據保存在SD卡內。

圖7是1次典型試驗結果。圖7校正前由于車輛抖動導致GPS天線傾斜，其表現為定位數據偏離實際的中心直線，統計數據表明校正前最大偏差為20 cm,平均偏差為7.2 cm；圖7校正后GPS數據與規劃直線表現出較好的一致性，最大偏差為12 cm，平均偏差為3.1 cm。可知校正公式對定位數據起到平滑作用，能更準確反映車體質心位置。誤差來源包括人工駕駛技術、鋪設路面測量誤差，以及車身振動等[25]。

圖7 不平路面的直線行走試驗結果Fig.7 Straight line driving test result on uneven road

3.2 雙閉環轉向控制算法試驗

為驗證設計的雙閉環PID控制算法能有效地使轉向執行機構按控制系統指令達到目標角度，進行轉向試驗。

上位機以10 Hz頻率發送控制指令信號，通過CAN通信網絡向轉向控制器傳輸控制指令，下位機的執行頻率同樣為10 Hz。運行本文設計的雙閉環控制算法，控制信號追蹤結果如圖8所示。

(1)從圖8a可以看出，對于方波信號，跟蹤角度與目標信號有良好一致性。圖8b是跟蹤誤差曲線，可知角度變化較大時，出現較大的震蕩；穩態時，誤差基本在0°附近。統計結果表明：平均誤差為0.40°，最大誤差為0.60°。最大跟蹤時間為1.6 s，平均跟蹤時間為1.3 s。以上分析可知，轉向系統的超調量得到較好抑制。

(2)從圖8c、8d可知，正弦信號的跟蹤值與目標值呈現較好的一致性。統計表明：跟蹤誤差平均值為0.40°，平均跟蹤時間為0.15 s。因此，跟蹤過程比較平穩，超調控制較好。

圖8 轉向信號響應與對應誤差Fig.8 Steering signal tracking and error

圖9 田間試驗測試結果Fig.9 Results of field test on tractor

3.3 田間試驗

為驗證拖拉機自動導航系統的實際工作精度，進行田間試驗。

試驗在華南農業大學的一塊試驗田中進行，規劃出如圖9所示的行駛路徑。路徑設定方法如下：①首先確定長方形試驗田(測量寬度為40 m，長度為75 m)的4個頂點為地頭節點，分別記為A、B、C、D，使用GPS裝置測量各頂點的位置坐標。②以AD構成的邊為基準線，劃定總數若干平行作業線，行與行之間的距離均為2.5 m。將第1作業行定位于北端，拖拉機在該行的行駛方向為自東向西，在該行的終點處拖拉機跨7行進入下一作業行并按照自西向東行駛。以同樣的方法循環作業，規劃的作業行數目為16行。③計算所有平行作業線與邊界的交點，作為每行的節點，跨行數目j=(16-1)/2，取j=7。④按照文中2.3節所介紹的地頭轉向規劃方法，編制作業順序。將相應作業線節點存儲進2維數組，供導航控制軟件調用。

駕駛員首先將拖拉機駕駛至首行，車頭對準首行路徑方向，啟動自動導航系統，拖拉機處于自動行駛狀態。拖拉機從第一作業行起點開始行駛，導航控制系統實時控制拖拉機按照規劃路徑行駛至終點，在每行的終點執行地頭轉向。在行駛過程中，由上位機記錄下RTK-GPS、AHRS500GA以及轉向系統的數據，導航控制系統工作并儲存數據。田間作業結束后，從控制系統導出以下主要參數：GPS定位數據、轉向輪偏角期望值和測量值、姿態信息,在PC平臺上進行試驗結果分析。

圖9是拖拉機在田間作業時的定位追蹤與轉向角跟蹤情況。圖9a給出了目標轉向角度與轉向輪實際轉角關系。由導航控制器決策并發出目標轉向角度的控制指令，由 KMA199測量轉向角度的實際值。從圖可知，在-5°～35°范圍內轉向執行機構按照控制指令動作，跟蹤誤差平均值為0.43°，控制在合理范圍內。

圖9b是拖拉機田間行駛實際軌跡與規劃路徑比較。在獲取試驗數據的基礎上，對誤差進行統計分析，以橫向跟蹤誤差(記為W)平均值Wavg、最大值Wmax以及標準差σW作為路徑跟蹤效果的評價指標，計算式為[26]

對試驗數據進行統計分析(表3)，結果表明：所有行的平均跟蹤誤差均不大于0.020 m，標準差不超過0.041 m。除第7、12、14、16行之外，其他所有行的最大跟蹤誤差不超過0.088 m。與文獻[4]試驗結果進行對比，平均誤差、各作業行的最大跟蹤誤差均減少。可知，雙閉環PID轉向控制方法能有效提高導航控制精度和穩定性。觀察采樣數據發現，所有行的最大跟蹤誤差出現在起始位置，其原因是：從地頭轉向結束，開始直線行駛的過渡期間，由于田間路面顛簸、車體振動而產生相對較大的誤差。因此，將來的工作需改進地頭轉向與直線行駛的銜接問題。

表3 試驗跟蹤誤差統計結果Tab.3 Statistical results of experimental offset error m

4 結論

(1) GPS傾斜校正試驗表明，基于RTK-DGPS與AHRS組合可有效平滑GPS定位數據，更準確反映車體實際位置。

(2)轉向控制算法試驗表明，轉向輪能按照轉向控制系統發出的指令達到目標角度和角速度，穩態跟蹤誤差不超過0.6°，試驗效果良好。

(3)田間行走試驗證明，拖拉機能按照預先規劃的路徑自動轉向和行駛。路徑跟蹤誤差平均值不超過0.019 m，標準差不超過0.041 m，角度跟蹤誤差平均值為0.43°。本文設計的導航系統能滿足拖拉機田間導航控制的要求。

1 REID J F, ZHANG Q, NOGUCHI N, et al. Agricultural automatic guidance research in North America[J].Computers and Electronics in Agriculture,2000, 25(1-2):155-167.

2 張智剛. 插秧機的DGPS自動導航控制系統研究[D]. 廣州：華南農業大學，2006. ZHANG Zhigang. Research of DGPS navigation control system for rice transplanter[D]. Guangzhou: South China Agricultural University, 2006. (in Chinese)

3 ZHANG Q, REID J F, NOBORU N. Agricultural vehicle navigation using multiple guidance sensors [C]∥Proceedings of the International Conference on Field and Service Robotics, 1999:293-298.

4 羅錫文，張智剛，趙祚喜,等.東方紅X-804拖拉機的DGPS自動導航控制系統[J].農業工程學報，2009，25(11)：139-143. LUO Xiwen, ZHANG Zhigang, ZHAO Zuoxi, et al. Design of DGPS navigation control for Dongfanghong X-804 tractor[J]. Transactions of the CSAE, 2009, 25(11): 139-143.(in Chinese)

5 吳曉鵬，趙祚喜，張智剛，等.東方紅拖拉機自動轉向系統設計[J].農業機械學報，2009，40(增刊)：1-5. WU Xiaopeng, ZHAO Zuoxi, ZHANG Zhigang, et al. Development of automatic steering control system based on Dongfanghong tractor[J].Transactions of the Chinese Society for Agricultural Machinery, 2009,40(Supp.):1-5. (in Chinese)

6 籍穎，劉兆祥，劉剛，等.基于卡爾曼濾波農用車輛導航定位方法[J].農業機械學報，2009，40(9)：13-17. JI Ying, LIU Zhaoxiang, LIU Gang, et al. Positions research of vehicle navigation system based on kalman filter[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009，40(9): 13-17. (in Chinese)

7 張智剛，羅錫文，趙祚喜，等.基于Kalman濾波與純追蹤模型的農業機械導航控制[J]．農業機械學報，2009，40(增刊)：6-12． ZHANG Zhigang, LUO Xiwen, ZHAO Zuoxi, et al. Trajectory tracking control method based on Kalman filter and pure pursuit model for agricultural vehicle[J]. Transactions of the Chinese Society for Agricultural Machinery, 2009, 40(Supp.): 6-12. (in Chinese)

8 Crossbow Technology, Inc．AHRS500GA-Series User's Manual：Revision B[M]. New York: McGraw Hill, 2007:21-25．

9 秦永元.慣性導航[M]．北京:科學出版社，2014：243-259．

10 陳斌. 東方紅X-804拖拉機自動轉向控制系統研究與設計[D].廣州：華南農業大學，2009. CHEN Bin. Research and design of steering control system for Dongfanghong X-804 tractor [D]. Guangzhou: South China Agricultural University, 2009. (in Chinese)

11 NOGUCHI N, WILL J, REID J, et al. Development of a master-slave robot system for farm operation[J].Computer and Electronics in Agriculture，2004，44(1)：1-19.

12 吳曉鵬.東方紅拖拉機轉向伺服控制系統設計[D].廣州：華南農業大學，2011. WU Xiaopeng. Steering control system design for Dongfanghong X-804 tractor[D]. Guangzhou: South China Agricultural University, 2011. (in Chinese)

13 GUO Linsong, HE Yong, ZHANG Qin, et al. Real-time tractor position estimation system using Kalman filter[J]. Transactions of the CSAE,2002,18(5)：96-101.

14 Bosch CAN User's Guide [EB/OL]. http:∥www.xhl.com.cn/sjsc/sjscdetail.asp?sid=25,2007.

15 HAN S, ZHANG Q, NOBORU N. Kalman filtering of DGPS position for a parallel tracking application [J].Transactions of the ASAE, 2002, 45(3):553-559.

16 GREWAL M S, WEILL L R, ANDREWS A P. Global positioning systems, inertial navigation, and integration[M]. 2nd ed. New York: John Wiley & Sons，Inc. , 2007: 316-381．

17 KURODA S K. Discriminatively trained unscented Kalman filter for mobile robot localization[J]. Journal of Advanced Research in Mechanical Engineering, 2010, 1(3):153-161.

18 李勇，于韶輝.電液比例閥的雙閉環控制技術[J].微特電機，2005，33(6)：35-36. LI Yong, YU Shaohui. Double feedback control of a proportional valve[J]. Small & Special Electrical Machines, 2005, 33(6):35-36. (in Chinese)

19 魏萬迎，殷國富，田大慶.電動助力轉向系統雙閉環模糊PID控制系統分析[J].制造業自動化，2010,32(14):41-42. WEI Wanying, YIN Guofu, TIAN Daqing. Analysis on eps machitrical fuzzy dual closed-loop control system [J]. Manufacturing Automation, 2010, 32(14):41-42. (in Chinese)

20 胡煉，羅錫文，張霖，等.1PJ-4.0型激光平地機設計與試驗[J/OL].農業機械學報，2014，45(4):146-150. http:∥www.j-csam.org/jcsam/ch/reader/view_abstract.aspx?flag=1&file_no=20140423&journal_id=jcsam. DOI:10.6041/j.issn.1000-1298.2014.04.023. HU Lian, LUO Xiwen, ZHANG Lin, et al. Development of 1PJ-4.0 laser leveler installed on a wheeled tractor for paddy field[J/OL].Transactions of the Chinese Society for Agricultural Machinery, 2014, 45(4): 146-150. (in Chinese)

21 NXP INC. KMA199E product data sheet [DB/OL]. (2014-05-15)[2016-02-28]. http:∥www.nxp.com/acrobat_download/datasheets/ KMA199E_1.pdf, 2008.

22 Analog Devices Inc.ADIS16300 Datasheet [DB/OL]. (2013-04)[2016-01-28]. http:∥ www.analog.com/static/imported-files/data_sheets/ADIS16300.pdf.

23 FARRELL J A. Aided navigation：GPS with high rate sensors[M].New York: McGraw Hill，2006:35-60.

24 Luminary Micro, Inc. LM3S8962 microcontroller data sheet[DB/OL]. [EB/OL]. (2012-1-19) [2016-1-22]. http:∥www.luminarymicro.com.

25 GUO Linsong. Development of a low-cost navigation system for autonomous off-road vehicle [D].Urbana-Champaign: University of Illions, 2003.

26 周俊，張鵬，宋百華.農業機械導航中的GPS定位誤差分析與建模[J].農業機械學報，2010，41(4)：189-192. ZHOU Jun, ZHANG Peng, SONG Baihua. Analysis and modeling of GPS positioning error for navigation of agricultural machinery[J]. Transactions of the Chinese Society for Agricultural Machinery,2010，41(4)：189-192. (in Chinese)

Automatic Navigation System of Tractor Based on DGPS and Double Closed-loop Steering Control

LI Yongjian1ZHAO Zuoxi2HUANG Peikui2GUAN Wei1WU Xiaopeng2

(1.DepartmentofMechanicalandElectricalEngineering,GuangdongAIBPolytechnicCollege,Guangzhou510507,China2.KeyLaboratoryofKeyTechnologyonAgriculturalMachineandEquipment,MinistryofEducation,SouthChinaAgriculturalUniversity,Guangzhou510642,China)

An agricultural automatic navigation system was designed on Dongfanghong X-804 tractor to improve the navigation control of agricultural machinery by using RTK-DGPS and double closed-loop steering control. The make-up of the whole navigation system and working principle were presented, among which their main features were: RTK-DGPS could offer positioning data, including heading, roll and pitch parameters, which were acquired from AHRS500GA, and the electro-hydraulic steering controller was developed for automatic steering control. Then, the system control strategy was analyzed and the control transfer function model was developed for trajectory tracking, with a double closed-loop control algorithm for steering system designed according to characteristic of the system nonlinear. The implementation description on an ARM9E-based embedded control system was provided in terms of electronics hardware design. Tests were conducted to examine the navigation system, including a straight line driving test on uneven road, which was to verify the effectiveness of the correction model. The test results showed that the proposed positioning and orientation evaluation algorithm could eliminate the effects of uneven field condition on GPS positioning and the average error of GPS positioning was reduced to 0.43°. Then a test of steering control system was carried out to verify the performance of double closed-loop control algorithm. Test results showed that the steering control system solved the control overshoot well and the average error was 0.40°. Finally, the field test results showed that the performance of automatic navigation system was improved, with average route tacking error was less than 0.019 m, average steering angle tracking error was 0.43° and standard deviation was less than 0.041 m. The field test results indicated that the proposed positioning evaluation algorithm and double closed-loop steering control algorithm on uneven field were appropriated to Dongfanghong X-804 tractor.

tractor; automatic navigation; steering system; tracking error; real time kinematic differential global positioning system; double closed-loop control

10.6041/j.issn.1000-1298.2017.02.002

2016-02-25

2016-09-01

國家自然科學基金項目(61175081)、國家重點研發計劃項目(2016YFD0700101)和農業部948計劃重點項目(2011-G32)

黎永鍵(1983—)，男，講師，主要從事農業電氣化與自動化研究，E-mail： leeeyong@qq.com

趙祚喜(1968—)，男，教授，博士生導師，主要從事農業機械與裝備自動控制設計研究，E-mail： zhao_zuoxi@hotmail.com

S11+2

A

1000-1298(2017)02-0011-09