黎永鍵，趙祚喜，高俊文，吳曉鵬，關(guān)偉

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

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

黎永鍵1，趙祚喜2*，高俊文1，吳曉鵬2，關(guān)偉1

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

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

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

黎永鍵， 趙祚喜， 高俊文， 吳曉鵬， 關(guān)偉. 基于DGPS定位與雙閉環(huán)轉(zhuǎn)向控制的農(nóng)業(yè)自動(dòng)導(dǎo)航系統(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è)自動(dòng)導(dǎo)航需要解決準(zhǔn)確定位和自動(dòng)轉(zhuǎn)向控制的問(wèn)題［1］。為提高農(nóng)業(yè)導(dǎo)航定位精度，農(nóng)業(yè)智能車輛田間導(dǎo)航定位需要測(cè)量載體位置坐標(biāo)、航向角以及姿態(tài)角等導(dǎo)航參數(shù)，其測(cè)量值應(yīng)盡量準(zhǔn)確［2］。大量研究工作表明，Kalman濾波能有效解決慣性導(dǎo)航系統(tǒng)誤差隨時(shí)間積累的問(wèn)題［3］。趙祚喜［4］等設(shè)計(jì)了基于GPS的組合慣性導(dǎo)航系統(tǒng)，利用“位置—速度—姿態(tài)”運(yùn)動(dòng)學(xué)模型建立了Kalman濾波器，使導(dǎo)航定位精度提高到0.05 m以內(nèi)。但是，試驗(yàn)證明Kalman濾波的使用有以下局限性：研究物體的運(yùn)動(dòng)過(guò)程能夠線性化表示，模型誤差和傳感器測(cè)量誤差能夠用高斯白噪聲模型化；不滿足以上條件，則不能提供最優(yōu)估計(jì)［5］。因此，實(shí)現(xiàn)農(nóng)業(yè)機(jī)械自動(dòng)導(dǎo)航不僅需要Kalman濾波提供準(zhǔn)確姿態(tài)信息，還需要對(duì)非線性的轉(zhuǎn)向過(guò)程進(jìn)行控制。

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

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

1 自動(dòng)導(dǎo)航控制系統(tǒng)結(jié)構(gòu)

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

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

1.2 轉(zhuǎn)向控制系統(tǒng)結(jié)構(gòu)

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

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

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

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

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

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

1）KMA199。KMA199是一種非接觸式的磁阻效應(yīng)傳感器，其基本原理：當(dāng)有磁場(chǎng)作用于傳感器時(shí)，磁阻就會(huì)發(fā)生改變，根據(jù)磁阻值的變化由數(shù)學(xué)關(guān)系即可推算出轉(zhuǎn)角大?。?4］。

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

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

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

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

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

2.1 GPS與AHRS組合的導(dǎo)航定位校正方法

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

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

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

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

2.2 Kalman濾波器設(shè)計(jì)

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

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

將式（3）、式（4）進(jìn)行三角函數(shù)展開(kāi)，則：

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

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

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

2）觀測(cè)方程為：

式中：Zk= ［xckyck］，vk是觀測(cè)噪聲，Hk是觀測(cè)矩陣，由下式計(jì)算：

3）將線性離散Kalman濾波器的遞歸差分構(gòu)成預(yù)測(cè)方程：

4）濾波方程為：

增益方程為：

濾波誤差方差方程為：

Qk為輸入噪聲協(xié)方差矩陣，Rk為觀測(cè)噪聲協(xié)方差矩陣，通過(guò)對(duì)試驗(yàn)數(shù)據(jù)進(jìn)行多元統(tǒng)計(jì)獲得。經(jīng)過(guò)多次反復(fù)試驗(yàn)，Qk和Rk分別?。?/p>

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

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

圖4 油缸運(yùn)動(dòng)學(xué)分析Fig. 4 Kinematics analysis of cylinder

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

轉(zhuǎn)向輪轉(zhuǎn)角（δ）與油缸活塞位移（y）關(guān)系為：

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

比例閥開(kāi)度（x）與輸入電流大小（u）關(guān)系為：

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

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

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

2.4 轉(zhuǎn)向控制器的設(shè)計(jì)

考慮到轉(zhuǎn)向系統(tǒng)的非線性特點(diǎn)，對(duì)傳統(tǒng)PID算法進(jìn)行改進(jìn)，設(shè)計(jì)了基于雙閉環(huán)控制方法的轉(zhuǎn)向控制器（圖5）?；舅悸肥牵阂赞D(zhuǎn)向角度控制為外環(huán)基礎(chǔ)上，加入一個(gè)角速度控制的內(nèi)環(huán)。外環(huán)是通過(guò)角度傳感器測(cè)量轉(zhuǎn)向輪實(shí)時(shí)轉(zhuǎn)角（θR）與目標(biāo)角度（θT）進(jìn)行對(duì)比，由控制器1實(shí)現(xiàn)角度控制；內(nèi)環(huán)是通過(guò)陀螺儀測(cè)量實(shí)時(shí)角速度（ωR）與目標(biāo)角速度（ωT）進(jìn)行對(duì)比，由控制器2實(shí)現(xiàn)速度控制。加入內(nèi)環(huán)控制的目的是使得轉(zhuǎn)向車輪能較平穩(wěn)地以較低速度到達(dá)目標(biāo)位置［23］。

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

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

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

2）內(nèi)環(huán)角速度控制。系統(tǒng)輸出量是由速度內(nèi)環(huán)決策，即電控比例閥的輸入電流量OI，ω為當(dāng)前車輪轉(zhuǎn)向的角速度。設(shè)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)采樣時(shí)間。

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

3）參數(shù)估計(jì)方法。以Simulink為平臺(tái)建立仿真模型，經(jīng)過(guò)仿真得到系統(tǒng)傳遞函數(shù)的未知參數(shù)［24］。根據(jù)傳遞函數(shù)式，使用一個(gè)二階慣性環(huán)節(jié)加上一個(gè)系統(tǒng)的延時(shí)，分別建立系統(tǒng)外環(huán)、內(nèi)環(huán)的仿真模型。主要應(yīng)用模塊為：采用波形信號(hào)發(fā)生器產(chǎn)生關(guān)于角速率的目標(biāo)方波信號(hào)；MATLAB Function在此次實(shí)現(xiàn)PID控制算法的實(shí)現(xiàn)；系統(tǒng)延時(shí)模塊Transport Delay，延時(shí)時(shí)間常數(shù)設(shè)置τ=0.2 s。經(jīng)過(guò)反復(fù)參數(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 驗(yàn)證試驗(yàn)與結(jié)果分析

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

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

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

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

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

圖6 方波信號(hào)響應(yīng)與跟蹤誤差Fig. 6 Square signal response and tracking error

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

3.2 田間驗(yàn)證試驗(yàn)

試驗(yàn)?zāi)康模候?yàn)證本文設(shè)計(jì)Kalman濾波器與雙閉環(huán)轉(zhuǎn)向控制系統(tǒng)在拖拉機(jī)實(shí)際工作過(guò)程中能有效提高導(dǎo)航控制精度，拖拉機(jī)能按照預(yù)先規(guī)劃的路徑自動(dòng)轉(zhuǎn)向和行駛。

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

1）首先長(zhǎng)方形的試驗(yàn)田四個(gè)頂點(diǎn)作為定點(diǎn)，分別記為A、B、C、D，使用RTK-DGPS測(cè)定每個(gè)點(diǎn)的坐標(biāo)。

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

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

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

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

圖7 拖拉機(jī)田間試驗(yàn)結(jié)果Fig.7 Result of the field test on tractor

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

表1 試驗(yàn)跟蹤誤差統(tǒng)計(jì)結(jié)果Table 1 Results of the experimental tracking error

4 結(jié)論

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

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

［1］ 蔣浩然， 陳軍， 王虎， 等. 移動(dòng)機(jī)器人自動(dòng)導(dǎo)航技術(shù)研究進(jìn)展［J］.西北農(nóng)林科技大學(xué)學(xué)報(bào)（自然科學(xué)版）， 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ī)設(shè)計(jì)與試驗(yàn)［J］.農(nóng)業(yè)機(jī)械學(xué)報(bào)， 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］ 秦永元. 慣性導(dǎo)航［M］ . 北京： 科學(xué)出版社， 2014： 243-259. Qin Y Y. Inertial Navigation［M］. Beijing： Science Press， 2014：243-259.

［4］ 黎永鍵， 趙祚喜， 高俊文. 基于GPS/SINS組合的農(nóng)業(yè)導(dǎo)航定位系統(tǒng)設(shè)計(jì)與研究［J］. 農(nóng)機(jī)化研究， 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］ 鄧自立， 王欣， 高媛. 建模與估計(jì)［M］. 北京： 科學(xué)出版社，2008： 126-135. Deng Z L， Wang X， Gao Y. Modeling and Estimation［M］. Beijing：Science Press， 2008： 126-135.

［6］ 劉豹， 唐萬(wàn)生. 現(xiàn)代控制理論［M］. 北京： 機(jī)械工業(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è)機(jī)械學(xué)報(bào)， 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拖拉機(jī)的DGPS自動(dòng)導(dǎo)航控制系統(tǒng)［J］. 農(nóng)業(yè)工程學(xué)報(bào)， 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］ 吳曉鵬. 東方紅拖拉機(jī)轉(zhuǎn)向伺服控制系統(tǒng)設(shè)計(jì)［D］. 廣州： 華南農(nóng)業(yè)大學(xué)， 2011. Wu X P. Steering control system design for Dongfanghong X-804 tractor［D］. Guangzhou： South China Agricultural University，2011.

［10］ 吳曉鵬， 趙祚喜. 東方紅拖拉機(jī)自動(dòng)轉(zhuǎn)向系統(tǒng)設(shè)計(jì)［J］. 農(nóng)業(yè)機(jī)械學(xué)報(bào)， 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］ 張智剛. 插秧機(jī)的DGPS 自動(dòng)導(dǎo)航控制系統(tǒng)研究［D］. 廣州： 華南農(nóng)業(yè)大學(xué)， 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拖拉機(jī)自動(dòng)轉(zhuǎn)向控制系統(tǒng)研究與設(shè)計(jì)［D］.廣州： 華南農(nóng)業(yè)大學(xué)， 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］ 趙汝祺， 趙祚喜， 趙汝準(zhǔn). 基于卡爾曼濾波器的車輛定位試驗(yàn)［J］. 農(nóng)業(yè)工程學(xué)報(bào)， 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)慣導(dǎo)系統(tǒng)中四元素法求解姿態(tài)角仿真模擬［J］. 彈箭與制導(dǎo)學(xué)報(bào)， 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.

（責(zé)任編輯：童成立）

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

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

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

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

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