景 亮 張亞飛 沈 躍 何思偉 劉 慧 崔業(yè)民

(1.江蘇大學(xué)電氣信息工程學(xué)院， 鎮(zhèn)江 212013； 2.南通廣益機(jī)電有限責(zé)任公司， 南通 226631)

0 引言

農(nóng)機(jī)自主導(dǎo)航技術(shù)是農(nóng)機(jī)自動化、智能化的關(guān)鍵技術(shù)之一，可顯著提高作業(yè)質(zhì)量和生產(chǎn)效率[1]。目前，農(nóng)機(jī)自主導(dǎo)航已廣泛應(yīng)用于耕作、播種、施肥、噴藥、收獲等農(nóng)業(yè)生產(chǎn)過程[2]。

農(nóng)機(jī)自動導(dǎo)航包括導(dǎo)航位姿信息獲取、導(dǎo)航路徑規(guī)劃和導(dǎo)航控制[3-4]，其中導(dǎo)航控制算法是導(dǎo)航系統(tǒng)的核心。而農(nóng)機(jī)自身大延遲、大慣性和高度非線性的特征要求導(dǎo)航?jīng)Q策控制算法具備一定的自適應(yīng)性和魯棒性[5-6]。目前國內(nèi)外常用的農(nóng)機(jī)導(dǎo)航控制方法包括 PID控制、模糊控制、純追蹤控制、滑模變結(jié)構(gòu)控制、最優(yōu)控制和模型預(yù)測等[7-10]。已有研究均假設(shè)農(nóng)機(jī)作業(yè)過程中滿足理想的“非完整約束條件”，即農(nóng)機(jī)在行駛過程中輪胎始終保持“純滾動無滑動”的狀態(tài)[11]，然而農(nóng)機(jī)的作業(yè)環(huán)境復(fù)雜多變，由于各種因素，如輪胎打滑、變形等，純滾動約束不可能得到嚴(yán)格滿足，特別是當(dāng)農(nóng)機(jī)需要在水田、坡地、濕滑草地等地面上行駛時(shí)，不可避免地發(fā)生側(cè)滑，進(jìn)而影響農(nóng)機(jī)自主導(dǎo)航的性能，甚至影響系統(tǒng)的穩(wěn)定性[12]。

目前對車輛側(cè)滑的相關(guān)研究較少。文獻(xiàn)[13]研究了車輪打滑情況下農(nóng)用車輛的軌跡跟蹤控制，同時(shí)也考慮了側(cè)滑角對導(dǎo)航精度的影響，結(jié)果表明農(nóng)機(jī)軌跡跟蹤的精度會受到車輪打滑的影響，該研究在模擬仿真環(huán)境中進(jìn)行了測試，并未在實(shí)際車輛上實(shí)現(xiàn)。文獻(xiàn)[14]提出了一種結(jié)合了高精度定位系統(tǒng)(RTK-GPS)的側(cè)滑角觀測器并將其集成到系統(tǒng)模型中，在試驗(yàn)中手動駕駛農(nóng)用拖拉機(jī)，并用(RTK-GPS)記錄路徑，然后估算模型的參數(shù), 估算結(jié)果較為準(zhǔn)確，為復(fù)雜環(huán)境下農(nóng)機(jī)自動導(dǎo)航研究提供了思路。文獻(xiàn)[15]針對果園自主導(dǎo)航車輛的軌跡跟蹤問題，提出了一種側(cè)滑估計(jì)器，采用高精度定位系統(tǒng)(RTK-GPS)估計(jì)車輛作業(yè)時(shí)的縱向和橫向滑移速度，提高了果園自主導(dǎo)航車輛的軌跡跟蹤性能，但該研究只考慮了車輛的橫向控制。本文研究工作場景主要為水田、草地的四輪獨(dú)立驅(qū)動(Four wheel independent drive，4WID)高地隙噴霧機(jī)的軌跡跟蹤問題。設(shè)計(jì)一種結(jié)合自適應(yīng)方法與反步控制方法的軌跡跟蹤控制算法。首先建立側(cè)滑情況下高地隙噴霧機(jī)運(yùn)動學(xué)模型，然后基于此模型設(shè)計(jì)自適應(yīng)控制器，通過參數(shù)自適應(yīng)來補(bǔ)償未知側(cè)滑效應(yīng)。最后在考慮側(cè)滑與不考慮側(cè)滑兩種情況下分別進(jìn)行仿真和水田試驗(yàn)。

1 四輪獨(dú)立電驅(qū)動運(yùn)動學(xué)建模

高地隙四輪獨(dú)立電驅(qū)動噴霧機(jī)的轉(zhuǎn)向結(jié)構(gòu)與傳統(tǒng)阿克曼以及差速轉(zhuǎn)向結(jié)構(gòu)不同，轉(zhuǎn)向結(jié)構(gòu)的3D模型如圖1所示，主要由車架和前、后轉(zhuǎn)向橋3部分組成，每個(gè)轉(zhuǎn)向橋通過平面軸承與車架前后轉(zhuǎn)向中心聯(lián)接，車輛動力來自4個(gè)獨(dú)立的大轉(zhuǎn)矩輪轂電機(jī)，每個(gè)電機(jī)的轉(zhuǎn)矩均可獨(dú)立控制，因此無需額外轉(zhuǎn)向動力就可以通過協(xié)同控制4個(gè)輪轂電機(jī)的轉(zhuǎn)速完成直行與轉(zhuǎn)向作業(yè)[16]。為保證同步轉(zhuǎn)向以及提高轉(zhuǎn)向橋的抗干擾性能，通過建模與計(jì)算在前后軸之間安裝了兩個(gè)輔助連桿[17]。

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

噴霧機(jī)運(yùn)動學(xué)模型如圖2所示，前后轉(zhuǎn)向橋通過連桿約束，確保前后轉(zhuǎn)向橋同步轉(zhuǎn)向。首先建立全局坐標(biāo)系XWY與車體坐標(biāo)系xoy，A、B分別為前后轉(zhuǎn)向中心，L為前后轉(zhuǎn)向中心距離，D為轉(zhuǎn)向橋長度，v為噴霧機(jī)線速度，δ為前后轉(zhuǎn)向橋轉(zhuǎn)角，θ為噴霧機(jī)在全局坐標(biāo)系XWY中的航向，C為轉(zhuǎn)向中心，定義逆時(shí)針旋轉(zhuǎn)為正。設(shè)噴霧機(jī)左前輪、右前輪、左后輪、右后輪速度分別為v1、v2、v3、v4。由三角函數(shù)關(guān)系可得轉(zhuǎn)向橋轉(zhuǎn)向半徑為

(1)

偏航角速度為

(2)

在噴霧機(jī)轉(zhuǎn)向的動態(tài)過程中，噴霧機(jī)每個(gè)車輪的速度由底盤繞轉(zhuǎn)向中心點(diǎn)C的速度與前后轉(zhuǎn)向橋繞各自轉(zhuǎn)向中心點(diǎn)A、B的速度合成，即

(3)

將式(2)代入式(3)得出四輪速度與噴霧機(jī)線速度、前后軸轉(zhuǎn)角的關(guān)系式為

(4)

為進(jìn)一步驗(yàn)證上述所建立數(shù)學(xué)模型的正確性，使用UG軟件繪制等比例噴霧機(jī)3D模型并進(jìn)行運(yùn)動學(xué)仿真，仿真時(shí)設(shè)置目標(biāo)車速和轉(zhuǎn)向角并通過式(4)計(jì)算得到的四輪輪速輸入到3D模型中，設(shè)置目標(biāo)速度為3.6 km/h，最大轉(zhuǎn)向角為25°，在UG軟件中噴霧機(jī)仿真結(jié)果如圖3所示，藍(lán)色虛線為其行走軌跡?？梢钥闯鲈撧D(zhuǎn)向結(jié)構(gòu)不僅轉(zhuǎn)向半徑小而且同側(cè)前后輪軌跡幾乎重合，在實(shí)際的農(nóng)田應(yīng)用場景中可減少對農(nóng)作物的損傷。

由于前后轉(zhuǎn)向軸之間安裝了輔助連桿，假設(shè)前后軸同步轉(zhuǎn)向且轉(zhuǎn)角一致，為簡化計(jì)算過程，基于文獻(xiàn)[18]中的運(yùn)動學(xué)模型簡化原理，將四輪模型轉(zhuǎn)化為二輪車模型，如圖4所示。

由圖4可以看出，二輪簡化模型的轉(zhuǎn)向中心和運(yùn)動規(guī)律與四輪模型一致，且二輪模型運(yùn)動軌跡與噴霧機(jī)質(zhì)心運(yùn)動軌跡重合。因此以噴霧機(jī)質(zhì)心為基準(zhǔn)點(diǎn)建立簡化的運(yùn)動學(xué)模型

(5)

式中P——噴霧機(jī)質(zhì)心在全局坐標(biāo)系下的位姿

1.2 位姿誤差模型

基于運(yùn)動學(xué)簡化模型建立如圖5所示的位姿誤差模型，o、or分別為噴霧機(jī)質(zhì)心與參考質(zhì)心，(xr,yr)為參考噴霧機(jī)質(zhì)心or在坐標(biāo)系XWY中的坐標(biāo)，(x,y)為噴霧機(jī)質(zhì)心o在坐標(biāo)系XWY中的坐標(biāo)，(xe,ye)為向量loor在坐標(biāo)系xoy中的坐標(biāo)，c(s)為目標(biāo)路徑的曲率，s為從初始位置沿著目標(biāo)路徑的點(diǎn)or的曲線坐標(biāo)，θ、θr分別為噴霧機(jī)中心線相對于慣性坐標(biāo)系的航向與參考航向，θe為航向誤差，v、vr分別為噴霧機(jī)相對于慣性坐標(biāo)系的速度與期望速度，vy為噴霧機(jī)橫向速度，δb為噴霧機(jī)轉(zhuǎn)向偏差。

軌跡跟蹤控制器的目標(biāo)即設(shè)計(jì)合適的控制律使得

(6)

利用坐標(biāo)變換將全局坐標(biāo)系下誤差轉(zhuǎn)換為噴霧機(jī)質(zhì)心坐標(biāo)系下誤差Pe，計(jì)算式為

(7)

(8)

噴霧機(jī)參考角速度為

(9)

將式(7)兩邊分別對時(shí)間求導(dǎo)得到理想位姿誤差模型為

(10)

此時(shí)噴霧機(jī)縱向速度與橫向速度滿足約束

(11)

角速度滿足約束

(12)

(13)

2 控制律設(shè)計(jì)

軌跡跟蹤控制系統(tǒng)結(jié)構(gòu)如圖6所示，來自RTK的實(shí)時(shí)位姿信息、目標(biāo)路徑信息和期望速度作為控制器的輸入以及側(cè)滑估計(jì)數(shù)據(jù)源，控制器通過融合側(cè)滑估計(jì)結(jié)果分別輸出噴霧機(jī)目標(biāo)速度vc以及目標(biāo)轉(zhuǎn)角δc實(shí)現(xiàn)軌跡跟蹤控制。為驗(yàn)證側(cè)滑估計(jì)的有效性，分別基于理想位姿誤差模型以及改進(jìn)位姿誤差模型設(shè)計(jì)控制律并對比分析。

2.1 基于理想位姿誤差模型的控制律設(shè)計(jì)

目前針對非線性系統(tǒng)大都采用級聯(lián)形式的控制方法，其中鏈?zhǔn)较到y(tǒng)法[19]以及反步控制方法[20-21]的應(yīng)用最為廣泛。其中反步控制方法可以通過反向設(shè)計(jì)使控制律的設(shè)計(jì)過程系統(tǒng)化、結(jié)構(gòu)化，還可以控制相對階為n的非線性系統(tǒng)，消除了經(jīng)典無源性設(shè)計(jì)中相對階為1的限制[22]。噴霧機(jī)位姿誤差模型是一種復(fù)雜的非線性系統(tǒng)，首先使用反步控制方法設(shè)計(jì)基于理想位姿誤差模型的控制律。

(14)

(15)

(16)

式中kx、ky、ku——正常數(shù)

2.2 基于改進(jìn)位姿誤差模型的控制律設(shè)計(jì)

(1)選取正定Lyapunov函數(shù)

(17)

?！獋?cè)向滑移速度估計(jì)誤差的控制參數(shù)

式(17)兩邊分別對時(shí)間求導(dǎo)并將式(13)代入可得

(18)

選取u1=sinθe作為第1步的虛擬輸入，為保證ye收斂，取u1目標(biāo)值計(jì)算式為

(19)

選取目標(biāo)車速

vc=vrcosθe+kxxe

(20)

選取橫向滑移加速度

(21)

式(19)與式(20)中ky、kx均為正常數(shù)，將式(19)～(21)代入式(18)可得

(22)

(2)定義u1與u1d之差

(23)

構(gòu)建擴(kuò)展Lyapunov函數(shù)

(24)

γ——轉(zhuǎn)向偏差估計(jì)誤差的控制參數(shù)

式(24)兩邊分別對時(shí)間求導(dǎo)可得

(25)

選取u2=tanδ作為第2步的輸入，將式(12)、(13)和式(23)代入式(25)并簡化

(26)

其中

α=c(s)vrcosθe+yevr+kysinθe

(27)

(28)

(29)

選取側(cè)滑參數(shù)微分值為

(30)

(31)

選取目標(biāo)轉(zhuǎn)角正切值u2為

(32)

式(32)中ku為任意正常數(shù)，將式(30)～(32)代入式(26)可得

(33)

(34)

運(yùn)用Lasalle定理[24]得，所有的解都收斂于

(35)

(36)

3 仿真驗(yàn)證

4 試驗(yàn)驗(yàn)證

高地隙噴霧機(jī)導(dǎo)航系統(tǒng)如圖9所示，主要由高地隙噴霧機(jī)、厘米級高精度定位系統(tǒng)RTK、英特爾微型主機(jī)和4G路由器等構(gòu)成。噴霧機(jī)上設(shè)有由28.5 kW汽油發(fā)電機(jī)和72 V蓄電池組成的油電混合的供電系統(tǒng)，在啟動汽油發(fā)動機(jī)的情況下，噴霧機(jī)可以連續(xù)工作12 h。運(yùn)輸狀態(tài)下噴霧機(jī)質(zhì)量、長度、寬度、高度和軸距分別為1 680 kg、3 200 mm、1 760 mm、2 400 mm和1 680 mm。離地間隙1 100 mm；工作狀態(tài)下噴霧機(jī)載藥500 L，噴桿可展開至11 200 mm。

為獲得更精準(zhǔn)的位置以及航向信息，GNSS雙天線的安裝位置基線與噴霧機(jī)前進(jìn)方向垂直，其中RTK用于獲取噴霧機(jī)位置信息，作為滑移參數(shù)估計(jì)數(shù)據(jù)；移動路由器形成的局域網(wǎng)絡(luò)實(shí)現(xiàn)遠(yuǎn)程端實(shí)時(shí)監(jiān)測導(dǎo)航作業(yè)；微型主機(jī)與整車控制單元(Vehicle control unit，VCU)之間的信息交互借助RS485總線實(shí)現(xiàn)；噴霧機(jī)車輪以及前后轉(zhuǎn)向橋中心分別安裝了霍爾傳感器和角度傳感器，VCU采集實(shí)時(shí)輪速和轉(zhuǎn)向角度信息作為反饋信息，電壓巡檢儀用來獲取噴霧機(jī)實(shí)時(shí)電壓。

微型主機(jī)裝載了Ubuntu以及第二代機(jī)器人操作系統(tǒng)(Robot operating system，ROS2)，其中ROS2是一個(gè)專門針對機(jī)器人軟件開發(fā)而設(shè)計(jì)的通信框架，能夠滿足導(dǎo)航控制系統(tǒng)的安全性、可擴(kuò)展性和容錯性要求[25]。使用C++語言編寫ROS2導(dǎo)航算法節(jié)點(diǎn)、噴霧機(jī)節(jié)點(diǎn)與上位機(jī)界面節(jié)點(diǎn)，導(dǎo)航作業(yè)時(shí)在遠(yuǎn)程端運(yùn)行上位機(jī)界面節(jié)點(diǎn)實(shí)時(shí)監(jiān)測噴霧機(jī)狀態(tài)并發(fā)布目標(biāo)路徑信息，再運(yùn)行導(dǎo)航算法節(jié)點(diǎn)訂閱來自傳感器的各種信息，并將計(jì)算后的控制指令發(fā)布出去；最后運(yùn)行噴霧機(jī)節(jié)點(diǎn)訂閱導(dǎo)航節(jié)點(diǎn)發(fā)布的控制指令實(shí)現(xiàn)導(dǎo)航作業(yè)。

為進(jìn)一步研究側(cè)滑對軌跡跟蹤性能的影響，驗(yàn)證本文軌跡跟蹤算法的有效性。在海安市1.3 hm2泥濘水田中，分別使用考慮與不考慮側(cè)滑的控制律進(jìn)行軌跡跟蹤試驗(yàn)。為保證試驗(yàn)數(shù)據(jù)的有效性，規(guī)劃目標(biāo)軌跡為兩段U形軌跡(由3條直線與2條曲線組成)。噴霧機(jī)展開噴桿后噴幅為12 m，因此設(shè)置地頭轉(zhuǎn)向半徑為6 m，根據(jù)地塊尺寸設(shè)置直線段長度為55 m。期望車速為3.6 km/h，由于本文控制器跟蹤的量為動態(tài)的位姿信號，在進(jìn)行參數(shù)調(diào)節(jié)時(shí)先在校內(nèi)水泥地平整路面設(shè)定一個(gè)目標(biāo)點(diǎn)讓噴霧機(jī)單獨(dú)去跟蹤并記錄控制器輸出信號與軌跡跟蹤誤差信號，在進(jìn)行參數(shù)調(diào)節(jié)后跟蹤效果良好的情況下，將該參數(shù)應(yīng)用到水田U形路徑的跟蹤控制中，再根據(jù)噴霧機(jī)跟蹤效果進(jìn)行微調(diào)，最終選擇一組跟蹤效果較好的控制參數(shù)kx=0.1，ky=0.6，ku=0.05，Γ=0.1，γ=0.05。試驗(yàn)現(xiàn)場如圖10所示。

校正前后的軌跡跟蹤如圖11所示。由圖11a可以看出，當(dāng)噴霧機(jī)跟蹤的目標(biāo)路徑在由直線變?yōu)榍€的開始與結(jié)束時(shí)側(cè)滑效應(yīng)較為明顯，而直行時(shí)側(cè)滑效應(yīng)影響較小，但仍會引起小范圍的震蕩。由于考慮了側(cè)滑效應(yīng)，圖11b中的軌跡跟蹤效果更好。

校正前后橫向、縱向以及航向誤差如圖13所示。圖中分別標(biāo)注了直線部分與曲線部分的試驗(yàn)結(jié)果，可以看出試驗(yàn)結(jié)果與仿真結(jié)果基本一致。

校正前后橫向、縱向以及航向誤差結(jié)果如表1所示。由表1可知，側(cè)滑效應(yīng)對橫向跟蹤精度以及航向跟蹤精度的影響較為明顯，對縱向控制精度幾乎沒有影響。在不考慮側(cè)滑的情況下，噴霧機(jī)最大橫向誤差為0.287 m，平均絕對橫向誤差為0.114 m，標(biāo)準(zhǔn)差為0.13 m；最大縱向誤差為0.069 m，平均絕對縱向誤差為0.019 m，標(biāo)準(zhǔn)差為0.019 m；最大航向誤差為14.56°，平均絕對航向誤差為4.55°，標(biāo)準(zhǔn)差為5.54°?？紤]側(cè)滑后，側(cè)滑效應(yīng)的影響減弱。噴霧機(jī)最大橫向誤差減小為0.167 m，平均絕對橫向誤差減小為0.041 m，標(biāo)準(zhǔn)差減小為0.059 m；噴霧機(jī)縱向控制誤差幾乎沒變化；噴霧機(jī)最大航向誤差減小為11.97°，平均絕對航向誤差減小為2.56°，標(biāo)準(zhǔn)差減小為3.57°。

校正前后控制轉(zhuǎn)角如圖14所示。校正前的控制器未考慮側(cè)滑進(jìn)而導(dǎo)致軌跡跟蹤時(shí)累計(jì)誤差較大，為跟蹤到目標(biāo)軌跡，控制器所輸出的控制轉(zhuǎn)角信號波動較大，而校正后的控制器由于考慮了側(cè)滑，所輸出的控制轉(zhuǎn)角信號更為平滑，對轉(zhuǎn)向機(jī)構(gòu)更為友好。

5 結(jié)論

(1)針對4WID高地隙噴霧機(jī)特殊的轉(zhuǎn)向模型，建立了該噴霧機(jī)的運(yùn)動學(xué)模型并在UG軟件中仿真驗(yàn)證所建立模型的準(zhǔn)確性。

(2)建立了噴霧機(jī)軌跡跟蹤的理想位姿誤差模型以及考慮側(cè)滑因素的改進(jìn)位姿誤差模型，并分別設(shè)計(jì)軌跡跟蹤控制器。以典型U形路徑為例，對兩種控制器進(jìn)行了仿真驗(yàn)證。結(jié)果表明，當(dāng)模型中未加入側(cè)滑影響時(shí)，兩種控制器均可以保證噴霧機(jī)快速跟蹤到目標(biāo)軌跡，跟蹤精度較高；當(dāng)考慮側(cè)滑影響時(shí)，自適應(yīng)控制律能夠?qū)崟r(shí)估計(jì)并補(bǔ)償側(cè)滑參數(shù)，跟蹤軌跡誤差更小，控制效果更好。

(3)田間試驗(yàn)結(jié)果表明，考慮側(cè)滑后的軌跡跟蹤最大橫向誤差減小為0.167 m，平均絕對橫向誤差減小為0.041 m，標(biāo)準(zhǔn)差減小為0.059 m；最大航向誤差減小為11.97°，平均絕對航向誤差減小為2.56°，標(biāo)準(zhǔn)差減小為3.57°。因此，在水田等復(fù)雜的作業(yè)環(huán)境下，加入側(cè)滑參數(shù)的模型更加貼合農(nóng)機(jī)實(shí)際的運(yùn)動規(guī)律，基于該模型設(shè)計(jì)軌跡跟蹤控制律能夠有效提高農(nóng)機(jī)導(dǎo)航精度和自動駕駛的穩(wěn)定性。