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

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

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

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

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

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

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

引言

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

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

1 自動(dòng)導(dǎo)航系統(tǒng)整體設(shè)計(jì)

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

1.1 自動(dòng)導(dǎo)航系統(tǒng)整體設(shè)計(jì)方案

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

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

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

1.2 組合定位方法

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

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

(1)

其中

(2)

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

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

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

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

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

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

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

(3)

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

δ=kδyh

(4)

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

(5)

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

φp=kφOI

(6)

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

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

(7)

(8)

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

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

1.4 CAN通信系統(tǒng)

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

2 導(dǎo)航控制器的設(shè)計(jì)

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

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

2.1 路徑跟蹤的控制學(xué)模型

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

文獻(xiàn)[15]研究了拖拉機(jī)的運(yùn)動(dòng)學(xué)模型，方程為

傳遞函數(shù)為

(9)

式中x、y——位置坐標(biāo)L——拖拉機(jī)軸距Vx——拖拉機(jī)橫向行駛速度Vy——拖拉機(jī)縱向行駛速度

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

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

(10)

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

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

TI——積分時(shí)間

TD——微分時(shí)間

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

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

(1)角度控制

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

eδ=δT-δR

(11)

控制目標(biāo)的角速率是ω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——上一時(shí)刻的角度誤差eiδ——角度誤差的積分部分eiδ_b——上一時(shí)刻的角度誤差積分部分edδ——角度誤差的微分部分ts——采樣時(shí)間

(2)角速率控制

設(shè)eω為角速率誤差，控制目標(biāo)的比例閥電流是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——上一時(shí)刻的角速率誤差eiω——角速率誤差的積分部分eiω_b——上一時(shí)刻的角速率誤差積分部分edω——角速率誤差的微分部分

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

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

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

圖4a是角度跟蹤結(jié)果，輸入信號(hào)為周期10 s的三角波信號(hào)，輸出信號(hào)準(zhǔn)確完成了目標(biāo)角度跟蹤，延時(shí)為0.1 s。圖4b是對(duì)應(yīng)的角速度跟蹤仿真結(jié)果，三角波信號(hào)對(duì)應(yīng)的角速率跟蹤信號(hào)也很好地跟蹤上目標(biāo)角速率。以上跟蹤效果達(dá)到要求，其中重要的原因是跟蹤目標(biāo)信號(hào)的變化率不大，表明：穩(wěn)態(tài)下的雙閉環(huán)控制效果理想。經(jīng)過(guò)反復(fù)參數(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ī)劃方法

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

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

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

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

(18)

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

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

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

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

2.5.1 傳感器部分

(1)角度傳感器KMA199

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

(2)慣性測(cè)量單元ADIS16300

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

2.5.2 控制器選擇

(1)導(dǎo)航控制器：采用ARM9E 32位嵌入式微處理器AT91SAM9261作為導(dǎo)航控制器(上位機(jī))。該處理器運(yùn)算速度可達(dá)200 MIPS，并集成了64 MB SDRAM[23]。基于AT91SAM9261的導(dǎo)航控制器可完成GPS、AHRS、各傳感器的信號(hào)采集與處理，并控制執(zhí)行機(jī)構(gòu)工作。

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

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

3.1 GPS定位校正試驗(yàn)

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

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

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

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

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

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

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

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

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

圖8 轉(zhuǎn)向信號(hào)響應(yīng)與對(duì)應(yīng)誤差Fig.8 Steering signal tracking and error

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

3.3 田間試驗(yàn)

為驗(yàn)證拖拉機(jī)自動(dòng)導(dǎo)航系統(tǒng)的實(shí)際工作精度，進(jìn)行田間試驗(yàn)。

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

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

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

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

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

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

4 結(jié)論

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

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

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

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 張智剛. 插秧機(jī)的DGPS自動(dòng)導(dǎo)航控制系統(tǒng)研究[D]. 廣州：華南農(nóng)業(yè)大學(xué)，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拖拉機(jī)的DGPS自動(dòng)導(dǎo)航控制系統(tǒng)[J].農(nóng)業(yè)工程學(xué)報(bào)，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ī)自動(dòng)轉(zhuǎn)向系統(tǒng)設(shè)計(jì)[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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)用車輛導(dǎo)航定位方法[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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ī)械導(dǎo)航控制[J]．農(nóng)業(yè)機(jī)械學(xué)報(bào)，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 秦永元.慣性導(dǎo)航[M]．北京:科學(xué)出版社，2014：243-259．

10 陳斌. 東方紅X-804拖拉機(jī)自動(dòng)轉(zhuǎn)向控制系統(tǒng)研究與設(shè)計(jì)[D].廣州：華南農(nóng)業(yè)大學(xué)，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 吳曉鵬.東方紅拖拉機(jī)轉(zhuǎn)向伺服控制系統(tǒng)設(shè)計(jì)[D].廣州：華南農(nóng)業(yè)大學(xué)，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].微特電機(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 魏萬(wàn)迎，殷國(guó)富，田大慶.電動(dòng)助力轉(zhuǎn)向系統(tǒng)雙閉環(huán)模糊PID控制系統(tǒng)分析[J].制造業(yè)自動(dòng)化，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ī)設(shè)計(jì)與試驗(yàn)[J/OL].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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è)機(jī)械導(dǎo)航中的GPS定位誤差分析與建模[J].農(nóng)業(yè)機(jī)械學(xué)報(bào)，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

國(guó)家自然科學(xué)基金項(xiàng)目(61175081)、國(guó)家重點(diǎn)研發(fā)計(jì)劃項(xiàng)目(2016YFD0700101)和農(nóng)業(yè)部948計(jì)劃重點(diǎn)項(xiàng)目(2011-G32)

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

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

S11+2

A

1000-1298(2017)02-0011-09