基于DGPS與雙閉環(huán)控制的拖拉機自動導航系統(tǒng)

黎永鍵 趙祚喜 黃培奎 關(guān) 偉 吳曉鵬

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

基于DGPS與雙閉環(huán)控制的拖拉機自動導航系統(tǒng)

黎永鍵1趙祚喜2黃培奎2關(guān) 偉1吳曉鵬2

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

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

拖拉機； 自動導航； 轉(zhuǎn)向系統(tǒng)； 跟蹤誤差； 載波相位差分全球定位系統(tǒng)； 雙閉環(huán)控制

引言

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

本文在東方紅X-804型拖拉機上開發(fā)基于RTK-DGPS與雙閉環(huán)轉(zhuǎn)向控制相結(jié)合的自動導航系統(tǒng)，實現(xiàn)東方紅拖拉機較高精度的速度控制和轉(zhuǎn)向控制，減少控制超調(diào)，以期提高農(nóng)業(yè)車輛田間導航控制精度。

1 自動導航系統(tǒng)整體設(shè)計

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

1.1 自動導航系統(tǒng)整體設(shè)計方案

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

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

表2 自動導航系統(tǒng)設(shè)計方案Tab.2 Design of automatic navigation system

1.2 組合定位方法

農(nóng)業(yè)車輛在田間作業(yè)行駛時，由于地面凹凸不平導致車體傾斜而存在較大的橫滾角、俯仰角，車載GPS天線端與車體中心不重合。因此，定位數(shù)據(jù)實際上是天線的位置，而不是車體中心的位置，定位偏差較大[6]?；谝陨峡紤]，必須通過姿態(tài)角校正原始接收數(shù)據(jù)的方法以提高定位準確度。

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

(1)

其中

(2)

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

1.3 電液控轉(zhuǎn)向系統(tǒng)

為實現(xiàn)自動轉(zhuǎn)向，設(shè)計并搭建了電液控轉(zhuǎn)向系統(tǒng)，結(jié)構(gòu)如圖1所示。在東方紅拖拉機原機械轉(zhuǎn)向系統(tǒng)的基礎(chǔ)上并聯(lián)1臺由步進電動機驅(qū)動的液壓轉(zhuǎn)向器，并安裝換向電磁閥、比例閥、溢流閥作為執(zhí)行元件。工作過程如下：當駕駛員選擇自動模式，轉(zhuǎn)向控制器驅(qū)動換向電磁閥工作從而實現(xiàn)油路后切換，以控制前輪轉(zhuǎn)向；以脈寬控制的方式通過電控比例閥控制流量以調(diào)整轉(zhuǎn)向速度，使前輪平穩(wěn)地轉(zhuǎn)向目標角度[10]。溢流閥用于防止油壓過載。

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

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

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

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

(3)

轉(zhuǎn)向輪轉(zhuǎn)角δ與活塞移動距離yh關(guān)系為[11]

δ=kδyh

(4)

轉(zhuǎn)向輪的角速率ω與油缸活塞移動距離yh屬于一階導數(shù)的關(guān)系[12]

(5)

比例閥的開度φp與輸入電流OI的關(guān)系為

φp=kφOI

(6)

式中kδ、kω、kφ——比例系數(shù)

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

(7)

(8)

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

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

1.4 CAN通信系統(tǒng)

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

2 導航控制器的設(shè)計

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

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

2.1 路徑跟蹤的控制學模型

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

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

傳遞函數(shù)為

(9)

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

2.2 雙閉環(huán)轉(zhuǎn)向控制算法設(shè)計

結(jié)合運動學模型與轉(zhuǎn)向系統(tǒng)模型，可得出導航系統(tǒng)的傳遞函數(shù)

(10)

在控制系統(tǒng)中，為消除結(jié)構(gòu)不穩(wěn)定，引入PID控制算法。PID控制由比例、積分、微分3個環(huán)節(jié)構(gòu)成，其傳遞函數(shù)表達式為[16]

式中KP、KI、KD——比例系數(shù)、積分系數(shù)、微分系數(shù)

TI——積分時間

TD——微分時間

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

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

(1)角度控制

設(shè)eδ為轉(zhuǎn)角誤差，則有

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——角度控制算法的系數(shù)eδ_b——上一時刻的角度誤差eiδ——角度誤差的積分部分eiδ_b——上一時刻的角度誤差積分部分edδ——角度誤差的微分部分ts——采樣時間

(2)角速率控制

設(shè)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——角速率控制算法系數(shù)eω_b——上一時刻的角速率誤差eiω——角速率誤差的積分部分eiω_b——上一時刻的角速率誤差積分部分edω——角速率誤差的微分部分

2.3 基于Simulink控制參數(shù)估計

以Simulink為平臺建立仿真模型，通過仿真得到系統(tǒng)傳遞函數(shù)的未知參數(shù)。根據(jù)傳遞函數(shù)式，使用1個二階慣性環(huán)節(jié)加上1個系統(tǒng)延時，分別建立系統(tǒng)外環(huán)、內(nèi)環(huán)的仿真模型。運行仿真程序，圖4是典型的仿真結(jié)果。

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

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

2.4 地頭轉(zhuǎn)向的路徑規(guī)劃方法

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

圖5 跨行地頭轉(zhuǎn)向的路徑規(guī)劃Fig.5 Path planning of headland turning through spanning line

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

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

(18)

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

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

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

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

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

2.5.1 傳感器部分

(1)角度傳感器KMA199

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

(2)慣性測量單元ADIS16300

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

2.5.2 控制器選擇

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

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

3 試驗與結(jié)果分析

3.1 GPS定位校正試驗

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

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

圖7是1次典型試驗結(jié)果。圖7校正前由于車輛抖動導致GPS天線傾斜，其表現(xiàn)為定位數(shù)據(jù)偏離實際的中心直線，統(tǒng)計數(shù)據(jù)表明校正前最大偏差為20 cm,平均偏差為7.2 cm；圖7校正后GPS數(shù)據(jù)與規(guī)劃直線表現(xiàn)出較好的一致性，最大偏差為12 cm，平均偏差為3.1 cm?？芍Ｕ綄Χㄎ粩?shù)據(jù)起到平滑作用，能更準確反映車體質(zhì)心位置。誤差來源包括人工駕駛技術(shù)、鋪設(shè)路面測量誤差，以及車身振動等[25]。

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

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

為驗證設(shè)計的雙閉環(huán)PID控制算法能有效地使轉(zhuǎn)向執(zhí)行機構(gòu)按控制系統(tǒng)指令達到目標角度，進行轉(zhuǎn)向試驗。

上位機以10 Hz頻率發(fā)送控制指令信號，通過CAN通信網(wǎng)絡(luò)向轉(zhuǎn)向控制器傳輸控制指令，下位機的執(zhí)行頻率同樣為10 Hz。運行本文設(shè)計的雙閉環(huán)控制算法，控制信號追蹤結(jié)果如圖8所示。

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

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

圖8 轉(zhuǎn)向信號響應與對應誤差Fig.8 Steering signal tracking and error

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

3.3 田間試驗

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

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

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

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

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

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

表3 試驗跟蹤誤差統(tǒng)計結(jié)果Tab.3 Statistical results of experimental offset error m

4 結(jié)論

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

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

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

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自動導航控制系統(tǒng)研究[D]. 廣州：華南農(nóng)業(yè)大學，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自動導航控制系統(tǒng)[J].農(nóng)業(yè)工程學報，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 吳曉鵬，趙祚喜，張智剛，等.東方紅拖拉機自動轉(zhuǎn)向系統(tǒng)設(shè)計[J].農(nóng)業(yè)機械學報，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 籍穎，劉兆祥，劉剛，等.基于卡爾曼濾波農(nóng)用車輛導航定位方法[J].農(nóng)業(yè)機械學報，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濾波與純追蹤模型的農(nóng)業(yè)機械導航控制[J]．農(nóng)業(yè)機械學報，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拖拉機自動轉(zhuǎn)向控制系統(tǒng)研究與設(shè)計[D].廣州：華南農(nóng)業(yè)大學，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 吳曉鵬.東方紅拖拉機轉(zhuǎn)向伺服控制系統(tǒng)設(shè)計[D].廣州：華南農(nóng)業(yè)大學，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 李勇，于韶輝.電液比例閥的雙閉環(huán)控制技術(shù)[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 魏萬迎，殷國富，田大慶.電動助力轉(zhuǎn)向系統(tǒng)雙閉環(huán)模糊PID控制系統(tǒng)分析[J].制造業(yè)自動化，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型激光平地機設(shè)計與試驗[J/OL].農(nóng)業(yè)機械學報，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 周俊，張鵬，宋百華.農(nóng)業(yè)機械導航中的GPS定位誤差分析與建模[J].農(nóng)業(yè)機械學報，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)、國家重點研發(fā)計劃項目(2016YFD0700101)和農(nóng)業(yè)部948計劃重點項目(2011-G32)

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

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

S11+2

A

1000-1298(2017)02-0011-09