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

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

0 引言

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

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

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

1 四輪獨立電驅動運動學建模

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

1.1 運動學模型

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

(1)

偏航角速度為

(2)

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

(3)

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

(4)

為進一步驗證上述所建立數學模型的正確性，使用UG軟件繪制等比例噴霧機3D模型并進行運動學仿真，仿真時設置目標車速和轉向角并通過式(4)計算得到的四輪輪速輸入到3D模型中，設置目標速度為3.6 km/h，最大轉向角為25°，在UG軟件中噴霧機仿真結果如圖3所示，藍色虛線為其行走軌跡。可以看出該轉向結構不僅轉向半徑小而且同側前后輪軌跡幾乎重合，在實際的農田應用場景中可減少對農作物的損傷。

由于前后轉向軸之間安裝了輔助連桿，假設前后軸同步轉向且轉角一致，為簡化計算過程，基于文獻[18]中的運動學模型簡化原理，將四輪模型轉化為二輪車模型，如圖4所示。

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

(5)

式中P——噴霧機質心在全局坐標系下的位姿

1.2 位姿誤差模型

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

軌跡跟蹤控制器的目標即設計合適的控制律使得

(6)

利用坐標變換將全局坐標系下誤差轉換為噴霧機質心坐標系下誤差Pe，計算式為

(7)

(8)

噴霧機參考角速度為

(9)

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

(10)

此時噴霧機縱向速度與橫向速度滿足約束

(11)

角速度滿足約束

(12)

(13)

2 控制律設計

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

2.1 基于理想位姿誤差模型的控制律設計

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

(14)

(15)

(16)

式中kx、ky、ku——正常數

2.2 基于改進位姿誤差模型的控制律設計

(1)選取正定Lyapunov函數

(17)

Γ——側向滑移速度估計誤差的控制參數

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

(18)

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

(19)

選取目標車速

vc=vrcosθe+kxxe

(20)

選取橫向滑移加速度

(21)

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

(22)

(2)定義u1與u1d之差

(23)

構建擴展Lyapunov函數

(24)

γ——轉向偏差估計誤差的控制參數

式(24)兩邊分別對時間求導可得

(25)

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

(26)

其中

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

(27)

(28)

(29)

選取側滑參數微分值為

(30)

(31)

選取目標轉角正切值u2為

(32)

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

(33)

(34)

運用Lasalle定理[24]得，所有的解都收斂于

(35)

(36)

3 仿真驗證

4 試驗驗證

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

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

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

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

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

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

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

校正前后控制轉角如圖14所示。校正前的控制器未考慮側滑進而導致軌跡跟蹤時累計誤差較大，為跟蹤到目標軌跡，控制器所輸出的控制轉角信號波動較大，而校正后的控制器由于考慮了側滑，所輸出的控制轉角信號更為平滑，對轉向機構更為友好。

5 結論

(1)針對4WID高地隙噴霧機特殊的轉向模型，建立了該噴霧機的運動學模型并在UG軟件中仿真驗證所建立模型的準確性。

(2)建立了噴霧機軌跡跟蹤的理想位姿誤差模型以及考慮側滑因素的改進位姿誤差模型，并分別設計軌跡跟蹤控制器。以典型U形路徑為例，對兩種控制器進行了仿真驗證。結果表明，當模型中未加入側滑影響時，兩種控制器均可以保證噴霧機快速跟蹤到目標軌跡，跟蹤精度較高；當考慮側滑影響時，自適應控制律能夠實時估計并補償側滑參數，跟蹤軌跡誤差更小，控制效果更好。

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