基于改進(jìn)純追蹤模型的農(nóng)機(jī)路徑跟蹤算法研究

張華強(qiáng) 王國(guó)棟 呂云飛 秦昌禮 劉 林 宮金良

(1.山東理工大學(xué)機(jī)械工程學(xué)院， 淄博 255049； 2.山東理工大學(xué)生態(tài)無(wú)人農(nóng)場(chǎng)研究院， 淄博 255049)

0 引言

農(nóng)業(yè)機(jī)械(以下簡(jiǎn)稱(chēng)農(nóng)機(jī))自動(dòng)導(dǎo)航控制技術(shù)已經(jīng)成為解放生產(chǎn)力、實(shí)現(xiàn)農(nóng)業(yè)自動(dòng)化的重要技術(shù)。2017年英國(guó)哈珀亞當(dāng)斯大學(xué)建立了全球首家應(yīng)用自動(dòng)拖拉機(jī)進(jìn)行播種、噴灑的無(wú)人農(nóng)場(chǎng)，美國(guó)愛(ài)荷華州超級(jí)農(nóng)場(chǎng)的智能拖拉機(jī)已經(jīng)能夠完成種植、噴灑農(nóng)藥的全部工作。我國(guó)正處在傳統(tǒng)農(nóng)業(yè)向現(xiàn)代農(nóng)業(yè)轉(zhuǎn)型的重要時(shí)期，農(nóng)業(yè)科技對(duì)我國(guó)農(nóng)業(yè)未來(lái)發(fā)展起著至關(guān)重要的作用[1]。農(nóng)機(jī)路徑跟蹤控制是農(nóng)業(yè)裝備自動(dòng)導(dǎo)航技術(shù)的關(guān)鍵，我國(guó)研究人員對(duì)路徑跟蹤方法進(jìn)行了研究，主要方法包括PID控制方法、模糊控制方法、A*控制方法、純追蹤模型等[2]。文獻(xiàn)[3]采用RTK-DGPS定位技術(shù)設(shè)計(jì)了直線跟蹤的PID導(dǎo)航控制系統(tǒng)，在農(nóng)機(jī)行進(jìn)速度為0.8 m/s時(shí)，最大橫向誤差小于0.15 m，平均誤差小于0.03 m。文獻(xiàn)[4]采用模糊控制方法，路徑跟蹤最大誤差小于0.1 m，最小誤差小于0.05 m。PID控制方法的缺點(diǎn)是參數(shù)整定困難，需要進(jìn)行大量的試驗(yàn)；模糊控制方法進(jìn)行直線行駛的精度在很大程度上依賴于專(zhuān)家的經(jīng)驗(yàn)和知識(shí)以及操作員的操作模式，因而精度有限。

純追蹤模型是一種模擬人駕駛行為的幾何追蹤模型，通過(guò)計(jì)算曲率將車(chē)輛從初始位置移動(dòng)到目標(biāo)位置，純追蹤模型的核心是確定一個(gè)合適的前視距離。文獻(xiàn)[5]通過(guò)試驗(yàn)證明，取輪距為前視距離時(shí)能得到較好的跟蹤效果，但得到的精度有限。文獻(xiàn)[6]通過(guò)BP神經(jīng)網(wǎng)絡(luò)動(dòng)態(tài)調(diào)整能夠獲得更加合適的前視距離，且具有較高的魯棒性，但此方法需要大量的優(yōu)質(zhì)樣本，并且容易陷入局部最優(yōu)。

為了提升農(nóng)機(jī)作業(yè)時(shí)直線行駛的精度，本文提出一種通過(guò)PSO算法動(dòng)態(tài)確定純追蹤模型前視距離的路徑跟蹤算法，進(jìn)而得到更高精度的跟蹤軌跡。以東方紅1104-C型拖拉機(jī)為試驗(yàn)平臺(tái)構(gòu)建農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)，進(jìn)行農(nóng)田播種試驗(yàn)，以驗(yàn)證算法的有效性。

1 直線行駛路徑跟蹤方法設(shè)計(jì)

在自動(dòng)導(dǎo)航控制系統(tǒng)中，為了對(duì)農(nóng)機(jī)直線行駛路徑進(jìn)行精確跟蹤，構(gòu)建了路徑跟蹤控制器，其由卡爾曼濾波模塊、PSO模塊、純追蹤模型模塊組成，如圖1所示。圖中ξd為經(jīng)過(guò)卡爾曼濾波模塊的橫向誤差、ξθ為經(jīng)過(guò)卡爾曼濾波模塊的航向角誤差；Ld為PSO算法自適應(yīng)實(shí)時(shí)確定的前視距離。

圖1 路徑跟蹤系統(tǒng)原理圖Fig.1 Path following system

1.1 運(yùn)動(dòng)學(xué)建模

農(nóng)機(jī)在路徑跟蹤的作業(yè)過(guò)程中，符合低速行駛、小轉(zhuǎn)向角度微調(diào)行駛路徑特點(diǎn)，假設(shè)輪胎與地面不產(chǎn)生橫向力，輪胎不產(chǎn)生橫偏角，則車(chē)輛遵循阿克曼轉(zhuǎn)向模型[7]。阿克曼轉(zhuǎn)向幾何示意圖如圖2所示，圖中δ1為外輪轉(zhuǎn)向角、δ2為內(nèi)輪轉(zhuǎn)向角、L為軸距、r為轉(zhuǎn)向半徑、b為輪距、δ為前輪平均轉(zhuǎn)向角。

圖2 阿克曼轉(zhuǎn)向幾何示意圖Fig.2 Ackerman steering geometry diagram1.前輪外輪 2.前輪內(nèi)輪

對(duì)圖2中幾何關(guān)系分析，內(nèi)外輪轉(zhuǎn)向角關(guān)系為

cotδ2-cotδ1=b/L

(1)

前輪平均轉(zhuǎn)向角為

δ≈L/r

(2)

農(nóng)機(jī)通常以二輪模型進(jìn)行運(yùn)動(dòng)學(xué)分析，即假設(shè)農(nóng)機(jī)在平滑路面行駛，輪胎與地面只產(chǎn)生縱向壓力，可將農(nóng)機(jī)模型簡(jiǎn)化為二輪模型。輪胎為剛性輪，不產(chǎn)生側(cè)向滑動(dòng)，二輪模型實(shí)際是簡(jiǎn)化后的阿克曼模型。二輪模型示意圖如圖3所示。圖中，A(xs,ys)表示前輪軸心坐標(biāo)，B(xd,yd)表示后輪軸心坐標(biāo)，α為農(nóng)機(jī)航向角，δs為前輪轉(zhuǎn)向角，vs和vd分別表示前、后輪速度。

圖3 二輪模型示意圖Fig.3 Schematic of two-round model

由二輪模型可得前輪轉(zhuǎn)向角為

δs=arctan(L/R)

(3)

根據(jù)轉(zhuǎn)向角與后輪線性速度關(guān)系可得

(4)

根據(jù)前后輪幾何約束可得農(nóng)機(jī)運(yùn)動(dòng)學(xué)模型為

(5)

1.2 改進(jìn)純追蹤模型構(gòu)建

1.2.1卡爾曼濾波器設(shè)計(jì)

由于農(nóng)田地面凹凸不平、定位系統(tǒng)自身精度不足等原因，系統(tǒng)的定位誤差精度必然會(huì)受到影響。為得到精度更高的農(nóng)機(jī)橫向誤差和航向誤差，本文設(shè)計(jì)了卡爾曼濾波對(duì)定位誤差進(jìn)行平滑處理。

農(nóng)機(jī)在作業(yè)過(guò)程中通常以低速行進(jìn)，設(shè)農(nóng)機(jī)行駛過(guò)程中恒速且加速度為零。卡爾曼濾波器的狀態(tài)向量為Xt=(xt,yt,vt)，其中xt和yt為t時(shí)刻農(nóng)機(jī)的平面位置，vt為農(nóng)機(jī)行駛速度[8]。

對(duì)連續(xù)系統(tǒng)進(jìn)行離散處理后狀態(tài)向量為

Xt+1=φtXt+wt

(6)

式中φt——系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣

wt——離散時(shí)間白噪聲

設(shè)θt為t時(shí)刻農(nóng)機(jī)航向角、Δθt為t時(shí)刻農(nóng)機(jī)航向偏移誤差、T為采樣周期，則卡爾曼濾波器[9]的狀態(tài)轉(zhuǎn)移矩陣為

(7)

離散處理后的觀測(cè)方程為

Zt=HtXt+Vt

(8)

式中Zt——t時(shí)刻系統(tǒng)的觀測(cè)向量

Ht——觀測(cè)矩陣

Vt——高斯白噪聲

通過(guò)前期仿真試驗(yàn)，并將數(shù)據(jù)進(jìn)行多元統(tǒng)計(jì)分析，觀測(cè)噪聲協(xié)方差Rt[10]確定為0.05，過(guò)程激勵(lì)噪聲協(xié)方差Qt確定為0.01。設(shè)計(jì)的卡爾曼濾波器能夠最大化地降低其他噪聲信號(hào)對(duì)真實(shí)信號(hào)的干擾[11]，保證了農(nóng)機(jī)當(dāng)前位姿信息的可靠性。

1.2.2純追蹤算法建模

純追蹤模型是一種幾何追蹤模型，按照人的駕駛習(xí)慣，通過(guò)計(jì)算跟蹤農(nóng)機(jī)當(dāng)前位置到目標(biāo)位置的曲率進(jìn)行工作，并能夠依據(jù)目標(biāo)點(diǎn)距離對(duì)當(dāng)前誤差進(jìn)行動(dòng)態(tài)調(diào)整[12-14]。純追蹤模型示意圖如圖4所示。圖中，C(xg,yg)表示目標(biāo)點(diǎn)坐標(biāo)，2α為到達(dá)目標(biāo)點(diǎn)所轉(zhuǎn)過(guò)的航向角，k為農(nóng)機(jī)轉(zhuǎn)彎曲率，d為當(dāng)前位置距目標(biāo)路徑的橫向誤差，θ為航向誤差，m為預(yù)設(shè)路徑。

圖4 純追蹤模型示意圖Fig.4 Schematic of pure tracking model

根據(jù)圖4，由正弦定理得

Ld/sinα=2R

(9)

根據(jù)目標(biāo)點(diǎn)橫縱坐標(biāo)表達(dá)式及勾股定理得

(10)

由圖4目標(biāo)點(diǎn)橫坐標(biāo)與橫向誤差幾何關(guān)系得

(11)

將式(3)、(9)～(11)聯(lián)立求得前輪轉(zhuǎn)向角關(guān)系式為

(12)

式(12)中，橫向誤差d和航向誤差θ由DGPS定位信息計(jì)算后通過(guò)卡爾曼濾波平滑處理后得到。只有前視距離Ld未知，下面將設(shè)計(jì)一種改進(jìn)的PSO算法，根據(jù)農(nóng)機(jī)實(shí)時(shí)誤差信息對(duì)前視距離Ld進(jìn)行動(dòng)態(tài)調(diào)整[15-16]。

2 前視距離確定方法

由式(12)可知，當(dāng)前視距離Ld較大時(shí)，農(nóng)機(jī)以較小曲率k向預(yù)定軌跡跟蹤路徑，但系統(tǒng)響應(yīng)較慢；當(dāng)前視距離Ld較小時(shí)，農(nóng)機(jī)以較大曲率k向預(yù)定軌跡跟蹤路徑，但容易產(chǎn)生欠阻尼響應(yīng)，出現(xiàn)控制振蕩。

PSO算法[17]是一種群體智能算法，假設(shè)每個(gè)微粒在n維空間中是沒(méi)有質(zhì)量和體積的個(gè)體，并以一定的速度在約束空間下飛行。粒子群在n維空間進(jìn)行飛行搜索時(shí)，Xi=(xi1,xi2,…,xin)為微粒個(gè)體i的當(dāng)前位置，Vi=(vi1,vi2,…,vin)為微粒個(gè)體i當(dāng)前的速度。

PSO算法的基本進(jìn)化公式為

vij(s+1)=ωvij(s)+c1r1j(s)(pij(s)-xij(s))+
c2r2j(s)(pgj(s)-xij(s))

(13)

xij(s+1)=xij(s)+vij(s+1)

(14)

式中j——微粒個(gè)體的維數(shù)

s——迭代次數(shù)

ω——慣性權(quán)重系數(shù)

pi——微粒個(gè)體所經(jīng)歷的最好位置

pg——所有微粒群體所經(jīng)歷的最好位置

c1、c2——步長(zhǎng)

r1、r2——(0,1)之間的隨機(jī)數(shù)

慣性權(quán)重系數(shù)ω具有平衡粒子群全局和局部搜索的能力，能夠減小粒子群算法對(duì)最大速度的需要。當(dāng)慣性權(quán)重系數(shù)ω較大時(shí)，粒子群全局搜索能力較強(qiáng)[18]；當(dāng)慣性權(quán)重系數(shù)ω較小時(shí)，粒子群局部搜索能力較強(qiáng)。目前粒子群搜索方法主要采用慣性權(quán)重系數(shù)ω隨著進(jìn)化而線性減小的方法，但由于粒子群算法早期收斂的速度較快[19]，所以應(yīng)盡快使粒子群進(jìn)行局部搜索，因此慣性權(quán)重系數(shù)ω線性減小的方法求解效率較差。

2.1 PSO算法慣性權(quán)重系數(shù)的改進(jìn)

為盡快使粒子群進(jìn)行局部搜索，應(yīng)加速慣性權(quán)重系數(shù)ω的遞減速度，故本文設(shè)計(jì)了一種慣性權(quán)重系數(shù)ω從ωmax到ωmin以凹函數(shù)遞減的方法

ω=e-s/smax

(15)

式中smax——最大迭代次數(shù)

根據(jù)式(15)得到不同迭代次數(shù)的凹函數(shù)和線性函數(shù)的變化對(duì)比，如圖5所示。

圖5 不同線性曲線權(quán)重系數(shù)ω變化對(duì)比Fig.5 Comparison of weight coefficients ω of different linear curves

由圖5可知，當(dāng)?shù)螖?shù)s增大時(shí)，凹函數(shù)比線性函數(shù)的慣性權(quán)重系數(shù)降低更快，能夠使PSO算法更快地進(jìn)入局部搜索，使粒子群具有更好的局部搜索能力，且能夠獲得更好的求解效率。

通過(guò)多次對(duì)比試驗(yàn)，當(dāng)慣性權(quán)重系數(shù)ω采用凹函數(shù)遞減時(shí)，在迭代260次左右，粒子群便能獲得最優(yōu)解；而當(dāng)慣性權(quán)重系數(shù)采用線性函數(shù)遞減時(shí)，當(dāng)?shù)?00次左右時(shí)，粒子群才能獲得最優(yōu)解。因此凹函數(shù)遞減方法能夠較大幅度提高粒子群算法計(jì)算的效率，減少計(jì)算時(shí)間。

2.2 適應(yīng)度函數(shù)設(shè)計(jì)

適應(yīng)度函數(shù)[20]是粒子群算法的關(guān)鍵，合適的適應(yīng)度函數(shù)能夠使粒子快速找到最優(yōu)解。均方根誤差是衡量實(shí)際值與真值之間誤差的重要依據(jù)，本文采用橫向誤差d與航向誤差θ的均方根之和作為粒子群尋優(yōu)的主要參考。

橫向誤差d的評(píng)價(jià)參數(shù)為

(16)

航向誤差θ的評(píng)價(jià)參數(shù)為

(17)

由式(16)、(17)為基礎(chǔ)設(shè)計(jì)適應(yīng)度函數(shù)

F=λF1+(1-λ)F2

(18)

式中N——工作時(shí)長(zhǎng)M——采樣時(shí)刻

λ——權(quán)重

d(t)、θ(t)——t時(shí)刻的橫向誤差和航向誤差

所設(shè)計(jì)的適應(yīng)度函數(shù)F能夠根據(jù)橫向誤差d，調(diào)整橫向誤差d和航向誤差θ的決策權(quán)重，具有良好的適應(yīng)性。

2.3 權(quán)重函數(shù)設(shè)計(jì)

在基于PSO算法的純追蹤模型中，當(dāng)橫向誤差d較大時(shí)，前視距離Ld主要通過(guò)橫向誤差d進(jìn)行決策；當(dāng)橫向誤差d較小時(shí)，前視距離Ld主要通過(guò)航向誤差θ進(jìn)行決策，故設(shè)計(jì)權(quán)重為[21]

λ=loga((d+1)/U) (U≥(dmax+1)/a)

(19)

式中a——權(quán)重函數(shù)的底數(shù)，a>1

U——調(diào)整參數(shù)

dmax——最大橫向誤差

根據(jù)式(19)得到權(quán)重示意圖如圖6所示。

圖6 權(quán)重示意圖Fig.6 Schematic of weight coefficient

由圖6可知，當(dāng)橫向誤差d增大時(shí)，權(quán)重加快增大，即橫向誤差d在適應(yīng)度函數(shù)F中比重快速增大，航向誤差θ比重減小，當(dāng)橫向誤差增大到0.25 m時(shí)，權(quán)重λ為0.65，即適應(yīng)度函數(shù)為F=0.65F1+0.35F2，適應(yīng)度函數(shù)F主要通過(guò)橫向誤差d進(jìn)行決策。

2.4 PSO算法參數(shù)設(shè)置及流程

在PSO算法中，最大迭代次數(shù)t為300，粒子群個(gè)數(shù)為50，步長(zhǎng)c1、c2均為1，隨機(jī)數(shù)r1、r2均為0.9，權(quán)重系數(shù)最大值ωmax為1、最小值ωmin為1/e，前視距離Ld范圍為[1 m,7 m]，粒子速度v范圍為[0.7 m/s,1.5 m/s]。PSO算法流程如圖7所示。

圖7 PSO算法流程圖Fig.7 PSO algorithm flow chart

將PSO算法中慣性權(quán)重系數(shù)進(jìn)行改進(jìn)，設(shè)計(jì)了凹函數(shù)式(15)使粒子群能夠盡快進(jìn)行局部搜索，提高了PSO算法的求解效率；設(shè)計(jì)了具有權(quán)重調(diào)整的適應(yīng)度函數(shù)式(18)，能夠使粒子群根據(jù)橫向誤差和航向誤差調(diào)節(jié)決策權(quán)重[22]，以便能夠獲得更加合適的前視距離。

本文通過(guò)上述改進(jìn)PSO算法確定前視距離，步驟如下：

(2)計(jì)算每個(gè)粒子的適應(yīng)度。根據(jù)農(nóng)機(jī)的實(shí)時(shí)位姿信息，通過(guò)所設(shè)計(jì)的適應(yīng)度函數(shù)式(18)確定每個(gè)粒子，即每個(gè)前視距離的適應(yīng)度。

(3)比較每個(gè)粒子與個(gè)體最優(yōu)位置pi適應(yīng)度，若更好，則將該粒子作為個(gè)體最優(yōu)位置。

(4)比較每個(gè)粒子與全局最優(yōu)位置pg適應(yīng)度，若更好，則將該粒子作為全局最優(yōu)位置。

(5)根據(jù)PSO算法的基本進(jìn)化公式(13)、(14)，以及本文所設(shè)計(jì)的凹函數(shù)式(15)對(duì)粒子的位置和速度進(jìn)行進(jìn)化。

(6)在達(dá)到最大迭代次數(shù)前，重復(fù)步驟(2)～(5)；達(dá)到最大迭代次數(shù)后，停止運(yùn)算，輸出全局最優(yōu)位置，即最優(yōu)前視距離。

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

3.1 試驗(yàn)平臺(tái)搭建

通過(guò)前期仿真試驗(yàn)，本文所設(shè)計(jì)的基于改進(jìn)純追蹤模型的農(nóng)機(jī)路徑跟蹤算法具有良好的適應(yīng)性，能夠以較高精度進(jìn)行直線行駛。為了進(jìn)一步驗(yàn)證該方法有效性，以東方紅1104-C型拖拉機(jī)為試驗(yàn)平臺(tái)構(gòu)建農(nóng)機(jī)試驗(yàn)系統(tǒng)，如圖8所示，并于山東省淄博市朱臺(tái)鎮(zhèn)試驗(yàn)田進(jìn)行了實(shí)地驗(yàn)證。

圖8 播種試驗(yàn)系統(tǒng)平臺(tái)Fig.8 Seeding test system platform1.GNSS天線 2.電動(dòng)方向盤(pán) 3.自動(dòng)駕駛控制器 4.角度傳感器 5.播種機(jī)

東方紅1104-C型拖拉機(jī)參數(shù)見(jiàn)表1。

表1 東方紅1104-C型拖拉機(jī)參數(shù)Tab.1 Dongfanghong 1104-C tractor parameters

試驗(yàn)平臺(tái)系統(tǒng)組成如圖9所示，主要由平臺(tái)端、自動(dòng)駕駛端、控制執(zhí)行端構(gòu)成[23]。其中自動(dòng)駕駛端的核心是基于改進(jìn)純追蹤模型的路徑跟蹤控制器[24]。

圖9 試驗(yàn)平臺(tái)系統(tǒng)組成圖Fig.9 Composition chart of test platform system

圖10 試驗(yàn)平臺(tái)系統(tǒng)結(jié)構(gòu)示意圖Fig.10 Schematic of test platform system

路徑跟蹤過(guò)程首先由平臺(tái)端(RTK-DGPS)采集邊界信息數(shù)據(jù)生成路徑，隨后指令下發(fā)給自動(dòng)駕駛端[25]，自動(dòng)駕駛端解析路徑文件及控制文件，根據(jù)位置判斷下發(fā)控制指令，通過(guò)CAN總線控制執(zhí)行端，執(zhí)行端完成啟動(dòng)、停止、加速、轉(zhuǎn)彎、農(nóng)具升降。試驗(yàn)平臺(tái)系統(tǒng)結(jié)構(gòu)如圖10所示。

3.2 前視距離對(duì)比試驗(yàn)

設(shè)置起始點(diǎn)A坐標(biāo)為(20 m,13 m)，農(nóng)機(jī)行駛速度為0.7 m/s，試驗(yàn)行駛長(zhǎng)度為70 m，行距為2.5 m，采用魚(yú)尾型調(diào)頭方式，設(shè)定當(dāng)農(nóng)機(jī)位于預(yù)設(shè)軌跡北側(cè)時(shí)，橫向誤差d>0；位于南側(cè)時(shí)，橫向誤差d<0。試驗(yàn)路徑規(guī)劃如圖11所示。

圖11 試驗(yàn)路徑規(guī)劃Fig.11 Test path planning

在相同的試驗(yàn)路徑下，對(duì)比固定前視距離和基于改進(jìn)的純追蹤模型動(dòng)態(tài)調(diào)整前視距離的方法進(jìn)行對(duì)比，設(shè)定初始固定前視距離為輪距2.0 m。

圖12 固定前視距離的行駛軌跡Fig.12 Driving trajectory with fixed-looking distance

圖13 固定前視距離的橫向誤差Fig.13 Lateral deviation with fixed-looking distance

圖14 動(dòng)態(tài)調(diào)整前視距離的行駛軌跡Fig.14 Dynamic adjustment of driving trajectory of forward-looking distance

圖15 動(dòng)態(tài)調(diào)整前視距離的橫向誤差Fig.15 Dynamic adjustment of lateral deviation of forward-looking distance

圖12、13表示在固定前視距離下的行駛軌跡和橫向誤差變化；圖14、15表示動(dòng)態(tài)前視距離下的行駛軌跡和橫向誤差變化。對(duì)比試驗(yàn)結(jié)果，在相同規(guī)劃路徑下，農(nóng)機(jī)上線后，固定前視距離的行駛軌跡橫向誤差波動(dòng)較大，最大橫向誤差為0.15 m，行駛距離大于20 m以后，最大橫向誤差為0.05 m；而動(dòng)態(tài)前視距離行駛軌跡橫向誤差變化較小，最大橫向誤差為0.09 m，當(dāng)行駛距離大于20 m以后，最大橫向誤差為0.02 m。動(dòng)態(tài)前視距離變化如圖16所示。

圖16 動(dòng)態(tài)前視距離變化曲線Fig.16 Dynamic forward-looking distance change

通過(guò)對(duì)比試驗(yàn)可以確定：基于改進(jìn)的純追蹤模型動(dòng)態(tài)調(diào)整前視距離的方法，在直線跟蹤的效果以及初始調(diào)整的距離上明顯優(yōu)于固定前視距離的方法。

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

在改進(jìn)純追蹤模型的農(nóng)機(jī)路徑跟蹤算法下，以農(nóng)機(jī)試驗(yàn)平臺(tái)進(jìn)行播種試驗(yàn)，對(duì)農(nóng)機(jī)行駛的6條軌跡的橫向誤差進(jìn)行統(tǒng)計(jì)分析，統(tǒng)計(jì)結(jié)果如表2所示。

表2 橫向跟蹤誤差統(tǒng)計(jì)結(jié)果Tab.2 Statistical results of horizontal tracking error m

根據(jù)統(tǒng)計(jì)結(jié)果，除第1、3行外，其他行最大橫向誤差不大于0.02 m，這說(shuō)明本文所設(shè)計(jì)系統(tǒng)方法具有良好的穩(wěn)定性和精度。第1行最大誤差0.09 m，是由于農(nóng)機(jī)在前5 m時(shí)初始對(duì)準(zhǔn)產(chǎn)生的；第3行最大誤差-0.025 m，超過(guò)0.02 m，是因?yàn)樘镩g作業(yè)不確定因素較多，有的軌跡路線上會(huì)出現(xiàn)凹坑、碎石等。由于第1行初始校準(zhǔn)誤差較大，統(tǒng)計(jì)分析意義不大，所以只分析第2行及以后各行。不同軌跡橫向誤差示意圖如圖17所示。

圖17 不同軌跡橫向誤差變化曲線Fig.17 Lateral deviation of different trajectories

由圖17可知，除部分軌跡因田間不可避免因素產(chǎn)生較大誤差，其余軌跡均能保證在預(yù)定軌跡0.02 m范圍內(nèi)波動(dòng)，動(dòng)態(tài)調(diào)整前視距離能夠使橫向誤差快速消除，精度大幅提高。

4 結(jié)論

(1)針對(duì)農(nóng)機(jī)直線行駛的精度問(wèn)題，提出了一種改進(jìn)純追蹤模型的農(nóng)機(jī)路徑跟蹤算法，基于PSO粒子群算法改進(jìn)純追蹤模型，根據(jù)農(nóng)機(jī)實(shí)時(shí)位姿信息動(dòng)態(tài)調(diào)整前視距離。采用該方法的試驗(yàn)結(jié)果表明：當(dāng)行駛距離超過(guò)5 m、前進(jìn)速度為0.7 m/s時(shí)，農(nóng)機(jī)行駛的最大橫向誤差為0.02 m，與常規(guī)方法相比，基于PSO算法的改進(jìn)純追蹤模型能夠提高直線行駛的精度。

(2)設(shè)計(jì)了以東方紅1104-C型拖拉機(jī)為試驗(yàn)平臺(tái)的農(nóng)機(jī)自動(dòng)導(dǎo)航系統(tǒng)，并進(jìn)行農(nóng)田播種試驗(yàn)。針對(duì)信號(hào)噪聲影響GPS導(dǎo)航精度問(wèn)題，設(shè)計(jì)了卡爾曼濾波器，保證了農(nóng)機(jī)位姿信息的精度。

(3)改進(jìn)的PSO算法參數(shù)能夠使粒子群盡快進(jìn)行局部搜索，減少運(yùn)算時(shí)間，優(yōu)于權(quán)重系數(shù)ω線性減小的方法。