景云鵬 劉 剛 金志坤

(1.中國農(nóng)業(yè)大學(xué)現(xiàn)代精細(xì)農(nóng)業(yè)系統(tǒng)集成研究教育部重點實驗室， 北京 100083； 2.中國農(nóng)業(yè)大學(xué)農(nóng)業(yè)農(nóng)村部農(nóng)業(yè)信息獲取技術(shù)重點實驗室, 北京 100083)

0 引言

在農(nóng)業(yè)領(lǐng)域自動導(dǎo)航技術(shù)多用于農(nóng)田生產(chǎn)過程[1]，通過對拖拉機(jī)和平地鏟的自動控制，實現(xiàn)農(nóng)田平地機(jī)的自動導(dǎo)航，可大大降低操作者的工作強(qiáng)度，具有較高的研究開發(fā)價值。

國內(nèi)外對農(nóng)田平地機(jī)導(dǎo)航控制的研究較少，一些國外公司例如Trimble和John Deere研發(fā)了用于各種農(nóng)業(yè)生產(chǎn)作業(yè)的導(dǎo)航系統(tǒng)，但缺少針對農(nóng)田平地機(jī)導(dǎo)航系統(tǒng)的開發(fā)。國內(nèi)學(xué)者多側(cè)重于對平地導(dǎo)航路徑進(jìn)行規(guī)劃設(shè)計。文獻(xiàn)[2-3]根據(jù)地勢分布，對平地導(dǎo)航的全局作業(yè)路徑進(jìn)行合理規(guī)劃。文獻(xiàn)[4]通過拉力傳感器實時獲取平地鏟的負(fù)載，引導(dǎo)駕駛員輔助導(dǎo)航平地作業(yè)。與自動插秧機(jī)[5-6]、施肥機(jī)[7]和直播機(jī)等不同，農(nóng)田平地機(jī)的自動導(dǎo)航不僅需要關(guān)注其導(dǎo)航行駛過程中平地鏟的位姿變化，還應(yīng)關(guān)注平地鏟能否對事先規(guī)劃好的農(nóng)田平整路徑進(jìn)行有效追蹤。目前，農(nóng)田平地機(jī)的拖拉機(jī)與平地鏟的連接模式主要分為三點懸掛式[8]和銷軸連接的牽引式[9]2種。牽引式農(nóng)田平地機(jī)平地面積更大，鏟車擺向靈活。

對于牽引式農(nóng)機(jī)的導(dǎo)航系統(tǒng)，文獻(xiàn)[10-11]通過建立農(nóng)機(jī)具-拖拉機(jī)組合系統(tǒng)的動力學(xué)模型，提出相應(yīng)的非線性導(dǎo)航控制器，同時也考慮了側(cè)滑角對導(dǎo)航精度的影響。為了保證控制精度，這些研究在車輛和農(nóng)機(jī)具連接處添加了橫向控制器，增加了控制模型的參數(shù)。文獻(xiàn)[12]對鉸接擺桿式大功率拖拉機(jī)進(jìn)行了轉(zhuǎn)向仿真試驗。文獻(xiàn)[13-16]對鉸接式無人駕駛車輛的系統(tǒng)建模、路徑追蹤算法、導(dǎo)航精度優(yōu)化和避障進(jìn)行了深入研究，為牽引式農(nóng)田平地機(jī)自動導(dǎo)航研究提供了思路。農(nóng)田平地機(jī)由拖拉機(jī)提供牽引動力，導(dǎo)航過程中通過控制拖拉機(jī)前輪轉(zhuǎn)角和后車輪行駛速度的變化引導(dǎo)鏟車駛?cè)胫付肪€，這需要建立不同數(shù)學(xué)模型來描述平地機(jī)的運(yùn)動狀態(tài)。同時，在農(nóng)田環(huán)境下，車輪側(cè)滑產(chǎn)生的側(cè)滑角會影響導(dǎo)航控制精度，因此必須考慮側(cè)滑角的影響。針對以上問題，本文提出一種帶有側(cè)滑估計的自適應(yīng)PID導(dǎo)航控制方法，旨在實現(xiàn)基于GNSS的農(nóng)田平地機(jī)的自動導(dǎo)航，提高平地作業(yè)的路徑跟蹤精度。

1 牽引式農(nóng)田平地機(jī)數(shù)學(xué)模型

1.1 路徑追蹤狀態(tài)描述

牽引式農(nóng)田平地機(jī)在平地作業(yè)過程中，通過控制拖拉機(jī)的行駛狀態(tài)實現(xiàn)平地鏟對規(guī)劃路徑的追蹤，車輪與農(nóng)田表面之間由于摩擦力的影響產(chǎn)生側(cè)滑角。如圖1所示，拖拉機(jī)與平地鏟之間通過銷軸進(jìn)行連接，待追蹤路徑為曲線C。以后輪軸的中心位置為研究點，得到拖拉機(jī)與追蹤曲線的位置誤差為dTe，平地鏟與追蹤曲線的位置誤差為dFe。拖拉機(jī)的前輪轉(zhuǎn)向改變農(nóng)田平地機(jī)的航向，前輪轉(zhuǎn)角為δT，轉(zhuǎn)向過程中產(chǎn)生的側(cè)滑角為β1。拖拉機(jī)后輪為驅(qū)動輪，行駛速度為VT，路徑追蹤時產(chǎn)生的側(cè)滑角為β2。行駛過程中，平地鏟與拖拉機(jī)連接處的夾角為φ，受到牽引力的作用，平地鏟的行駛速度為VF，車輪產(chǎn)生的側(cè)滑角為β3。

圖1 農(nóng)田平地機(jī)路徑追蹤狀態(tài)Fig.1 Path tracking of farmland leveler

1.2 運(yùn)動學(xué)模型

對拖拉機(jī)和平地鏟簡化后的組合模型進(jìn)行運(yùn)動學(xué)分析，如圖2所示。當(dāng)農(nóng)田平地機(jī)追蹤至規(guī)劃路徑C時，拖拉機(jī)和平地鏟的追蹤航向角分別為θTd和θFd，追蹤夾角為φd。結(jié)合圖1可知，追蹤點T和F處的曲率半徑分別為1/C(sT)和1/C(sF)，農(nóng)田平地機(jī)的路徑追蹤目的在于實時地調(diào)整拖拉機(jī)的前輪轉(zhuǎn)角δT和速度VT，使平地鏟與曲線C的橫向誤差dFe趨于0，航向誤差θFe趨于0，拖拉機(jī)與平地鏟的夾角趨于φd。結(jié)合文獻(xiàn)[17]，拖拉機(jī)的追蹤誤差方程可改進(jìn)為

(1)

圖2 運(yùn)動學(xué)模型Fig.2 Kinematic model

其中

θTe=θTd-θT

式中sT——T點沿曲線C的坐標(biāo)

θTe——拖拉機(jī)航向追蹤誤差

C(sT)——T點曲率

L1——拖拉機(jī)車輪軸距

θT——拖拉機(jī)航向角

假設(shè)平地鏟與拖拉機(jī)的連接處存在一個虛擬轉(zhuǎn)向輪，平地鏟行駛過程中不斷地調(diào)整轉(zhuǎn)向角δF，同理可得平地鏟的追蹤誤差方程為

(2)

其中

θFe=θFd-θF

式中sF——F點沿曲線C的坐標(biāo)

C(sF)——F點曲率

L2——平地鏟車輪軸與連接處的距離

δF——平地鏟虛擬前輪的期望轉(zhuǎn)角

θF——平地鏟航向角

夾角φ與拖拉機(jī)航向角θT及平地鏟航向角θF的變化有關(guān)，由空間位置關(guān)系可知

(3)

(4)

式中a1、a2、a3——狀態(tài)變量對時間的導(dǎo)數(shù)

控制變量的線性形式為

(5)

式中m1、m2——控制變量對時間的導(dǎo)數(shù)

將式(4)中的a2、a3對曲線坐標(biāo)a1進(jìn)行求導(dǎo)，可求得追蹤誤差(式(2))的鏈?zhǔn)奖磉_(dá)[18-19]

(6)

鏈?zhǔn)奖磉_(dá)式(6)是線性的，可以將控制參數(shù)m3表達(dá)為

m3=-Kda3-Kθa2(Kd、Kθ∈R+)

(7)

式中Kd——橫向誤差de的控制系數(shù)

Kθ——航向誤差θe的控制系數(shù)

結(jié)合式(6)、(7)，可以得到

a″2+Kda′2+Kθa2=0

(8)

由式(4)、(8)可知，平地鏟的橫向誤差dFe和航向誤差θFe都收斂于0。并將式(8)代入式(5)、(6)，可以求得平地鏟虛擬轉(zhuǎn)向輪的控制率為

(9)

其中

γ=θFe+β3

η=-KddFe-Kθεtanγ+C(sF)εtan2γ
ε=1-C(sF)dFe

將式(9)和式(3)聯(lián)立，可得

(10)

由式(10)可知，夾角φ作為路徑追蹤時關(guān)聯(lián)拖拉機(jī)和平地鏟的變量，其由平地鏟的狀態(tài)變量(sF，dFe，θFe)和拖拉機(jī)的控制變量(VT，δT)共同決定。因此，聯(lián)立式(10)、(1)可最終求得路徑追蹤下牽引式農(nóng)田平地機(jī)的運(yùn)動學(xué)模型

(11)

2 導(dǎo)航側(cè)滑估計器

2.1 側(cè)滑角計算

(12)

考慮極限情況下側(cè)滑角減小為0，即農(nóng)田平地機(jī)不發(fā)生側(cè)滑，則式(12)可在控制變量(β2，β1，β3)趨于0時進(jìn)行線性化

(13)

控制變量的預(yù)測矩陣D是由函數(shù)f(B,E)對控制變量E求偏導(dǎo)后得到

(14)

其中

式(14)中，當(dāng)橫向偏差θTe≠π/2，dFe≠1/C(sF)且λ1+λ2+λ3≠0，則矩陣D可逆。側(cè)滑估計器的目的是使估計變量B與測量變量X之間的誤差R為0(R=B-X)，誤差的運(yùn)動學(xué)方程可表示為

(15)

偏差增益矩陣G為Hurwitz矩陣，定義了所估計的狀態(tài)變量B向測量變量E的收斂時間[21]。測量變量X的值是等間隔的(取決于采集設(shè)備)，因此可得

(16)

其中T為采樣周期。最終，通過狀態(tài)變量和測量變量的不斷收斂計算，可求得輸入變量E(即農(nóng)田平地機(jī)的側(cè)滑角(β1，β2，β3))

(17)

圖3 平地機(jī)導(dǎo)航的控制結(jié)構(gòu)圖Fig.3 Navigation control structure of land leveler

2.2 導(dǎo)航控制率

農(nóng)田平地機(jī)路徑追蹤時拖拉機(jī)的行駛狀態(tài)(θTe,dTe,VT)和平地鏟的行駛狀態(tài)(θFe,dFe,VF)作為輸入變量，是可以通過測量和計算獲得的。結(jié)合運(yùn)動學(xué)模型，拖拉機(jī)的前輪期望轉(zhuǎn)角δT作為平地機(jī)導(dǎo)航的控制輸入，通過夾角φ建立拖拉機(jī)與平地鏟的聯(lián)系。可將運(yùn)動過程中平地鏟相對于拖拉機(jī)橫擺的曲率看作一階慣性環(huán)節(jié)[22]

(18)

將式(18)、(11)聯(lián)立，可得到農(nóng)田平地機(jī)的前輪期望轉(zhuǎn)角

(19)

其中采樣周期T取決于RTK-GNSS數(shù)據(jù)接收設(shè)備。φd為追蹤至期望路徑時平地鏟與拖拉機(jī)之間的理想夾角，φd=θFd-θTd。

3 非線性自適應(yīng)PID控制

3.1 非線性參數(shù)整定

農(nóng)田平地機(jī)路徑追蹤時，拖拉機(jī)的行駛方向由電機(jī)轉(zhuǎn)向執(zhí)行機(jī)構(gòu)帶動方向盤實現(xiàn)自動控制。非線性自適應(yīng)控制結(jié)構(gòu)如圖3所示，PID控制器接收到誤差輸入信號，控制電機(jī)轉(zhuǎn)向機(jī)構(gòu)帶動方向盤轉(zhuǎn)動，調(diào)整拖拉機(jī)追蹤時的航向。角度傳感器實時地測量前輪轉(zhuǎn)角的角度并與轉(zhuǎn)角控制器求得的控制率δT比較，得到誤差再輸入至PID控制器，形成一個閉環(huán)控制系統(tǒng)。

PID比例增益參數(shù)KP、積分增益參數(shù)KI和微分參數(shù)KD的選擇會影響控制器適應(yīng)性和魯棒性，本文使用一種改進(jìn)的非線性整定方法[23]，以轉(zhuǎn)角誤差eδ(t)為輸入量，3個增益參數(shù)為輸出量，構(gòu)造非線性整定函數(shù)

(20)

式中aP、bP、cP——比例增益范圍系數(shù)

aI、cI——積分增益范圍系數(shù)

aD、bD、cD、dD——微分增益范圍系數(shù)

3.2 自適應(yīng)控制方法

如式(20)所示，范圍系數(shù)(正實數(shù))決定著增益的取值區(qū)間。當(dāng)誤差eδ(t)→∞時，比例增益系數(shù)KP取最大值aP+bP。當(dāng)eδ(t)=0時，KP取最小值aP。bP為KP的變化區(qū)間，調(diào)整cP可調(diào)整KP的變化速率；積分增益參數(shù)KI的取值范圍為(0,aI)，當(dāng)eδ(t)=0時，KI取最大值，cI決定著KI變化速率；對于微分增益，aD為KD的最小值，aD+bD為KD的最大值。當(dāng)eδ(t)=0時，KD=aD+bD/(1+cD)，調(diào)整dD可調(diào)整KD的變化速率。

隨著轉(zhuǎn)角誤差eδ(t)的實時計算，非線性自適應(yīng)PID控制器的控制輸入變量為

(21)

式中ew(t)——轉(zhuǎn)角誤差的變化速率

4 田間試驗驗證

4.1 試驗平臺與參數(shù)設(shè)定

農(nóng)田平地機(jī)以John Deere 5-904型拖拉機(jī)為牽引動力，使用北京盛恒天寶公司生產(chǎn)的平地鏟，兩者通過銷軸式半剛性連接，試驗現(xiàn)場如圖4所示。

圖4 農(nóng)田平地機(jī)試驗現(xiàn)場Fig.4 Experimental site of land leveler in field

使用上海華測導(dǎo)航公司的GNSS雙天線導(dǎo)航定位系統(tǒng)測量農(nóng)田平地機(jī)的運(yùn)動狀態(tài)，配置1個中繪i70型基站。移動端的GNSS雙天線和接收機(jī)有2套：1套GNSS定向、定位天線分別放置于拖拉機(jī)前后車軸的中心位置，測量拖拉機(jī)運(yùn)動過程中的位置與航向信息；另1套GNSS定向天線放置于牽引連接處，GNSS定位天線放置于平地鏟后輪軸的中心位置，測量平地鏟運(yùn)動過程中的位置與航向信息。2個定位測向接收機(jī)的型號都為P3-DT，其平面定位精度為0.8 cm±1 mm，航向測量精度為0.1°。用于接收GNSS天線傳輸?shù)臄?shù)據(jù)，接收頻率設(shè)置為5 Hz。采用北京通磁偉業(yè)公司生產(chǎn)的WYT-3I3D型無觸點角度傳感器測量拖拉機(jī)的前輪轉(zhuǎn)角，測量精度為0.1°。配合自行設(shè)計的步進(jìn)電機(jī)轉(zhuǎn)向平臺和導(dǎo)航控制器，實現(xiàn)平地路徑的導(dǎo)航追蹤控制。

農(nóng)田平地機(jī)的拖拉機(jī)前后軸距為2.3 m，拖拉機(jī)前輪軸長為1.6 m，拖拉機(jī)的后輪軸長為1.7 m，平地鏟的寬度為2.5 m，平地鏟前后軸距為2.7 m。通過前期的Matlab/Simulink仿真平臺的調(diào)試，結(jié)合平地導(dǎo)航所使用的步進(jìn)電機(jī)轉(zhuǎn)向控制系統(tǒng)的特性[24-25]，本文的電機(jī)導(dǎo)航系統(tǒng)響應(yīng)追蹤達(dá)到穩(wěn)定時，非線性整定函數(shù)的比例增益范圍系數(shù)aP、bP、cP分別為1.2、2.5、0.2；積分增益范圍系數(shù)aI、cI分別為2、0.5；微分增益范圍系數(shù)aD、bD、cD、dD分別為0.5、1.5、4.5、0.3。再由手工試湊法，得出橫向偏差系數(shù)Kd為1，航向偏差系數(shù)Kθ為2時導(dǎo)航控制系統(tǒng)的振蕩最小。同時可得偏差增益矩陣為

(22)

4.2 田間平地機(jī)的導(dǎo)航試驗

試驗地點位于中國農(nóng)業(yè)大學(xué)上莊實驗站，選取1塊收獲玉米后旋耕過的農(nóng)田，設(shè)定的追蹤路徑由3條直線與2條曲線組成。分別進(jìn)行3次路徑追蹤試驗：試驗1導(dǎo)航控制方法帶有側(cè)滑估計器，但PID參數(shù)固定；試驗2導(dǎo)航控制方法既有側(cè)滑估計器，又進(jìn)行非線性參數(shù)整定；試驗3導(dǎo)航控制方法無側(cè)滑估計器，且PID參數(shù)固定。

圖5 平地機(jī)路徑追蹤試驗結(jié)果Fig.5 Path tracking experiments results for farmland leveler

農(nóng)田平地機(jī)3次試驗的路徑追蹤效果如圖5所示，為了符合農(nóng)田平整時的作業(yè)條件，農(nóng)田平地機(jī)的追蹤起始點距離設(shè)定路線大于1 m，拖拉機(jī)的行駛速度控制為1.05 m/s。分析圖6a的路徑追蹤偏差變化，可知拖拉機(jī)與平地鏟的路徑追蹤時間隨著起始距離的變大而增加。當(dāng)追蹤路線由直線至曲線或曲線至直線過渡時，拖拉機(jī)實時地調(diào)整自身位置，牽引平地鏟向規(guī)劃路徑行駛。使平地鏟路徑過渡時的偏差峰值小于拖拉機(jī)的峰值，平地鏟的路徑追蹤誤差快速下降。

圖6 平地機(jī)路徑追蹤偏差與參數(shù)變化Fig.6 Changes of path tracking deviation and gain parameters for farmland leveler

農(nóng)田路徑追蹤偏差結(jié)果如表1所示，可知試驗3在不考慮側(cè)滑角和參數(shù)自適應(yīng)控制的情況下，拖拉機(jī)最大偏差達(dá)1.976 8 m，標(biāo)準(zhǔn)偏差為0.448 2 m，平地鏟標(biāo)準(zhǔn)偏差為0.326 4 m。這是由于拖拉機(jī)在追蹤路線變換時發(fā)生側(cè)滑，為牽引平地鏟駛向規(guī)劃路線不斷調(diào)整自身航向，導(dǎo)致追蹤偏差變大。試驗1使用了本文設(shè)計的側(cè)滑估計器，農(nóng)田平地機(jī)在對第2條曲線至第3條直線路徑進(jìn)行追蹤時，側(cè)滑現(xiàn)象的影響減弱。拖拉機(jī)的標(biāo)準(zhǔn)偏差減小至0.358 1 m，

表1 農(nóng)田路徑追蹤偏差結(jié)果Tab.1 Deviation results of farmland path tracking m

平地鏟路徑追蹤的標(biāo)準(zhǔn)偏差減小至0.293 0 m。試驗2的路徑追蹤效果最為顯著，拖拉機(jī)和平地鏟路徑追蹤的標(biāo)準(zhǔn)偏差分別為0.343 1 m和0.225 4 m，相比于試驗3，試驗2中平地鏟路徑追蹤的平均絕對偏差減小了6.78 cm，標(biāo)準(zhǔn)偏差減小了10.1 cm。試驗2中增益參數(shù)KP、KI、KD的變化如圖6b所示，表明本文提出的非線性自適應(yīng)控制方法可以實時地根據(jù)輸入轉(zhuǎn)角誤差，調(diào)整PID的3個增益數(shù)值。

路徑追蹤時的角度變化如圖7所示，結(jié)合圖7a，在試驗2的路徑追蹤過程中，導(dǎo)航控制器控制轉(zhuǎn)向執(zhí)行機(jī)構(gòu)。使前輪轉(zhuǎn)角快速地追蹤至轉(zhuǎn)角控制器計算的理論轉(zhuǎn)角，表明本文所使用的非線性自適應(yīng)控制方法可以很好地調(diào)整增益系數(shù)，增強(qiáng)了PID控制器對農(nóng)田平地機(jī)導(dǎo)航控制系統(tǒng)的適應(yīng)性和魯棒性。同時從圖7b可以看出，側(cè)滑角峰值變化的時間區(qū)間與路徑追蹤誤差峰值變化的時間基本一致。說明農(nóng)田平地機(jī)在路徑追蹤的過程中，當(dāng)追蹤路徑由直線向曲線路徑變化時，平地機(jī)整體的航向角出現(xiàn)較大變化，此時側(cè)滑的影響最為嚴(yán)重，證明本文提出的側(cè)滑估計方法可以合理有效地預(yù)測平地機(jī)在田間路徑追蹤時的側(cè)滑角。

圖7 試驗2路徑追蹤時的角度變化曲線Fig.7 Angles changes during path tracking in experiment 2

5 結(jié)論

(1)建立了牽引式農(nóng)田平地機(jī)的運(yùn)動學(xué)模型，以橫向偏差、航向偏差、車輛機(jī)具之間的夾角為輸入量，提出了一種側(cè)滑估計器，實時計算農(nóng)田平地機(jī)進(jìn)行路徑追蹤時產(chǎn)生的側(cè)滑角，提高了導(dǎo)航控制率的計算精度。

(2)采用改進(jìn)的非線性參數(shù)整定自適應(yīng)PID控制方法，根據(jù)轉(zhuǎn)角輸入誤差在線調(diào)整PID控制器的增益參數(shù)，使導(dǎo)航控制系統(tǒng)的適應(yīng)能力得到提高。

(3)田間導(dǎo)航試驗表明，相比于無側(cè)滑估計與參數(shù)自適應(yīng)的導(dǎo)航控制，帶側(cè)滑估計的自適應(yīng)控制方法可以削弱側(cè)滑對農(nóng)田平地機(jī)路徑跟蹤的影響，增強(qiáng)了導(dǎo)航控制器對農(nóng)田平地機(jī)系統(tǒng)的適應(yīng)性和魯棒性。當(dāng)行駛速度為1.05 m/s時，平地鏟路徑追蹤的平均絕對偏差減小了6.78 cm，標(biāo)準(zhǔn)偏差減小了10.1 cm。該方法有效解決了農(nóng)田平地機(jī)路徑追蹤時平地鏟對規(guī)劃路徑的準(zhǔn)確跟蹤問題，適用于無人駕駛的農(nóng)田平整作業(yè)。