馬志凱，種 坤，趙曉順，趙建國，趙樹朋，于合龍

（1.河北農業(yè)大學 機電工程學院，河北 保定 071001；2.吉林農業(yè)大學 智慧農業(yè)研究院,

吉林 長春 130118）

中國制造2025 計劃將智能農業(yè)裝備列為十大發(fā)展領域之一［1］，拖拉機自動駕駛系統(tǒng)是當前智能農業(yè)裝備領域不可或缺的一部分［2-4］。隨著高精度差分定位技術的推廣，自動駕駛技術在農業(yè)作業(yè)方面得到了廣泛的應用［5-7］。拖拉機的自動導航不僅能提高勞動生產率，解決農村勞動力不足的問題，還有利于精細農業(yè)、規(guī)范生產等技術的推廣［8-10］。拖拉機自動導航是精準農業(yè)技術體系中重要基礎技術之一。

拖拉機田間作業(yè)時作業(yè)環(huán)境復雜，且拖拉機車況差別大，農機具的作業(yè)精度受拖拉機導航精度影響也較大。華南農業(yè)大學的趙祚喜教授對拖拉機衛(wèi)星導航作業(yè)精度檢驗方法進行了研究［9］。華南農業(yè)大學的朱金光研究員對水稻插秧機作業(yè)效果進行了研究［11］。江蘇大學劉繼展研究員對農業(yè)機械無人駕駛導航參數進行了研究［12］。拖拉機通過三點懸掛拖帶農機具作業(yè)時，受三點懸掛的影響作業(yè)精度會被放大，此時農機具跟蹤誤差要大于拖拉機本身的跟蹤誤差，而拖拉機拖帶農機具作業(yè)的目標是獲得農機具按目標路徑進行作業(yè)，而不是獲得拖拉機本身按目標路徑進行行駛。因此，研發(fā)以農機具作業(yè)誤差最小為控制目標的導航控制算法尤為重要。

拖拉機的導航控制問題即為拖拉機的路徑跟蹤問題［11-12］。目前研究拖拉機路徑跟蹤的諸多問題中常以拖拉機后軸中心的路徑跟蹤誤差最小值為優(yōu)化目標進行優(yōu)化控制，未考慮農機具的路徑跟蹤誤差，導致拖拉機帶農機具作業(yè)時農機具的跟蹤誤差要大于拖拉機后軸的跟蹤誤差。針對上述問題本文建立以農機具跟蹤誤差與拖拉機跟蹤誤差之和最小為優(yōu)化目標的拖拉機模型預測控制算法，并通過試驗驗證了此算法能有效提高農機具的作業(yè)精度。

1 拖拉機動力學模型

具有阿克曼轉向的輪式車輛在無障礙物的X-Y歐式空間中可以到達任意的位置和姿態(tài)，意味著該類車輛擁有二自由度［14-15］。但是實際中該拖拉機在車輛坐標系XV-OV-YV中，由于其機械結構的限制，不可側向移動。所以該類車輛的運動自由度數大于其動作控制量個數。如圖1（A）所示，記大地坐標系下拖拉機后軸中心坐標為(xr，yr)，拖拉機前軸中心坐標為(xf，yf)，拖拉機前輪轉角為α，拖拉機航向為φ，后軸中心速度為vr，拖拉機軸距為D坐標為(xj，yj)，機具中心與后軸中心距離為L，拖拉機橫擺角速度為ω。

后軸中心處的速度表示為：

前后輪運動學約束表示為：

由式（1）和式（2）可得：

根據前后輪幾何關系可得：

將式（3）和式（4）帶入式（2）可得：

同時拖拉機轉彎的幾何關系可得：

根據機具與后軸幾何關系可得：

因此拖拉機運動學模型為：

其中狀態(tài)量為X=［xr ,yr ,φ］T，控制量為u=［vr ,w］T。

式（8）所示為包含農機具的拖拉機系統(tǒng)運動學模型，在拖拉機路徑跟蹤問題中，應考慮道路因素的影響，因此需要根據拖拉機位置與參考路徑計算路徑跟蹤誤差。

目前拖拉機路徑導航主要以直線路徑跟隨為主，因此需構建直線形式的離散路點參考路徑。式y(tǒng)=p(x)

在拖拉機坐標系下，定義路徑跟蹤誤差為拖拉機后軸中心位置與參考線之間的橫向誤差e1兩后軸中心位置與參考線之間的航向誤差e2置中心位置與參考線之間的橫向誤差e3，見圖1（B）所示。則路徑跟蹤誤差由式（10）表示：

圖1 拖拉機路徑跟蹤圖Fig.1 Tractor path tracking

2 模型預測控制問題構建

按照拖拉機位置及農機具與期望路徑誤差最小，將拖拉機路徑跟蹤問題構建為模型預測控制問題，由（11）式描述，其中T為測時域，p(x)為拖拉機參考路徑。

2.1 代價函數

式（11）表示了模型預測控制問題的評價指標，設定為代價函數J，如式（15）所示。

其中，w1至w6為系數，e1(T)，e2(T)，e3(T)分別表示預測時域末端時刻的路徑跟蹤誤差，這部分代價函數的主要目的是為了防止模型預測控制問題在求解時發(fā)散，期望車輛在預測時域時刻保持在路徑上。代價函數中的積分項代表了拖拉機行駛過程中，路徑跟蹤位置誤差與航向誤差，同時w6所表示的指標期望拖拉機以盡可能穩(wěn)定的橫擺角速度行進。

2.2 約束條件

式（12）所示為拖拉機系統(tǒng)狀態(tài)約束條件，由式（8）表示。式（13）為拖拉機狀態(tài)量約束，由于拖拉機位置與航向在路徑跟蹤問題中為相互獨立的狀態(tài)量，不受環(huán)境條件約束，因此沒有約束上下限，即：

式（14）為拖拉機控制量約束，拖拉機在作業(yè)時，通常以固定車速行進，同時設定拖拉機橫擺角速度上下限，即：

其中vt表示設定的固定車速，ωmax為設定的橫擺角速度上限。

2.3 時變系統(tǒng)離散化

本文使用非線性求解器IPOPT 對模型預測控制問題進行求解，IPOPT 使用內點法求解非線性問題的局部最優(yōu)解。該方法將等式與不等式約束分解為一系列等式約束，然后用迭代的方法求解只含有等式約束的非線性問題。為保證求解結果的準確性，使用精確的雅可比矩陣與海森矩陣求解。在使用IPOPT 非線性求解器時，需要將時變系統(tǒng)進行離散化，由式（8）可得：

由式（10）可得：

式（18）和式（19）所示為拖拉機路徑跟蹤系統(tǒng)的離散表達形式。

3 算法可行性驗證

3.1 試驗構建

為驗證該算法路徑跟蹤保持的可行性，搭建實物進行試驗。

定位系統(tǒng)采用基于北斗的ublox 模塊自行開發(fā)的雙天線RTK 高精定位系統(tǒng)。該定位系統(tǒng)通過連接千尋定位服務器獲取RTK 定位信號，絕對定位精度可以達到±2 cm。主控制器為基于ARM Cortex?-M 核的32 位處理器M481LIDAE 開發(fā)的嵌入式控制器。在該控制器中實現軌跡跟蹤，控制周期為100 ms。

轉向電機采用滿足控制要求北京濤峰科技有限公司生產的直驅電機，該電機最大扭矩可達10 N·m，額定工作轉速100 r/min，位置控制精度0.1°。

選取磨損比較嚴重，轉向系統(tǒng)左右兩側空行程較大，且不對稱的雷沃歐豹950 為試驗車。試驗搭建車輛如圖2 所示。

圖2 試驗拖拉機雷沃950Fig.2 Experimental tractor Revo 950

3.2 算法可行性試驗

為驗證此算法對拖拉機直線跟蹤保持的可行性，選取代價函數的系數w1=0.02，w2=0.02，w3=0.02，w4=1，w5=0，w6=0.4 即算法對拖拉機路徑跟蹤控制的可行性驗證。試驗包括拖拉機起步入軌，倒車入軌、單次換擋、多次換檔試驗。試驗的數據采樣周期為0.05 s。

起步入軌是指拖拉機后軸中心點偏離跟蹤軌跡一定距離起步跟蹤預定軌跡行駛。倒車入軌是指拖拉機在軌跡上倒車起步跟蹤預定軌跡行駛。試驗測得拖拉機后軸中心點橫向誤差、速度及拖拉機航向誤差如圖3 所示。

圖3 入軌試驗Fig3 Test of enter an orbit

圖3（A）中知拖拉機偏離預定直線軌跡0.5 m處起步入軌，圖3（C）知車輛行駛4.5 s，行駛約為5 m，以1.3 m/s 左右的速度進入預設軌道，圖3（A）拖拉機偏離預定直線軌跡0.025 m 起步倒車跟蹤預定軌跡行駛，圖3（C）知車輛行駛2.5 s，行駛約為3 m，以1.02 m/s 左右的速度進入預設軌道。上述2種工況拖拉機入軌后，能穩(wěn)定地跟蹤直線路徑行駛，同時橫向最大偏差小于5 cm，航向角最大偏差小于1°。上述試驗表明此算法能使拖拉機有效跟隨直線路徑。

單次換擋是指拖拉機行駛過程中換入相鄰擋位。多次換擋是指拖拉機在行駛過程中多次換入相鄰擋位。試驗驗測的拖拉機后軸中心點橫向誤差、速度及拖拉機航向誤差如圖4 所示。

圖4 換擋試驗Fig.4 Experiment of shift gears

圖4（A）知換擋過程橫向誤差變化較小，受工況、速度影響多次換擋的橫向偏差優(yōu)于單次換擋，受速度影響多次換擋的后期橫向偏差變差。圖4（A）知單次換擋航向角在換擋時受速度波動影響出現較大航向角偏差，但仍保持了較高的跟隨精度。圖4（C）知單次換擋過程中由于動力中斷時間過長，導致拖拉機速度出現大幅度下降，直至速度降為0 m/s，同時在換擋過程中23 s 處由于操作失誤導致1 次速度抖動，引起了航向角偏差增大，隨后仍能保持較高跟隨效果。多次換擋中停車后再次掛擋起步且經過1 次高檔切換，橫向位置偏差變化較小，控制精度未受到影響。從上述表述知此算法能適應不同狀態(tài)的換擋過程。

起步入軌，倒車行駛、單次換擋、多次換檔試驗表面，本算法能有效實現拖拉機直線路徑跟隨，并適應拖拉機不同復雜工況的使用要求。

4 拖拉機帶機具作業(yè)試驗與分析

拖拉機帶農機具作業(yè)時農機具受三點懸掛結構及作業(yè)環(huán)境的影響，農機具行駛軌跡與拖拉機的行駛軌跡并不重合。為驗證此算法對農機具直線跟蹤的影響，進行拖拉機帶機具作業(yè)試驗。

4.1 拖拉機帶機具作業(yè)試驗

使用上述拖拉機拖帶河北雙天農機有限公司生產的1.5 m 旋耕機作業(yè)測試，其中旋耕深度是130 mm。選取w1=0.02，w2=0.02，w3=0.02，w4=1，w5=0，w6=0.4算法不帶機具誤差測得農機具中心點的橫向誤差，速度及拖拉機航向誤差數據如圖5 所示。取w1=0.02，w2=0.02，w3=0.02，w4=1，w5=1，w6=0.4 算法帶機具誤差測得農機具中心點的橫向誤差，速度及拖拉機航向誤差數據如圖6 所示。圖5、6 中低速為0.4 m/s，中速為1.5 m/s，高速3.3 m/s。

圖5 算法不帶機具誤差的作業(yè)試驗Fig.5 Work test without considering agricultural machinery errors

圖6 算法帶機具誤差的作業(yè)試驗Fig.6 Work test with considering agricultural machinery errors

4.2 試驗結果分析

由圖5 知，該算法在不考慮機具誤差進行拖帶旋耕機低速作業(yè)時，橫向偏差最大值在25～26 s 之間為0.048 6 m，航向角偏差小于1°；中速作業(yè)時，最大橫向偏差在47～48 s 之間為0.045 8 m，最大航向角偏差在56 s 處為1.55°，高速作業(yè)時，最大橫向偏差在4～5 s 之間為0.052 6 m，此時航向角偏差達到了負向最大值1.488 5°，10 s 以后橫向偏差、航向角偏差都趨于穩(wěn)定。此時，拖拉機以中速與高速、低速作業(yè)行時橫向偏差前者優(yōu)于后兩者，以低速行駛時橫向位置偏差最大。

由圖6 知，該算法考慮機具誤差進行拖帶旋耕機低速作業(yè)時，橫向誤差基本上±0.02 m 之間，在23～25 s 之間的最大偏差為0.023 m，橫向角偏差變化幅度較小，基本上處于±0.8°之間，中速作業(yè)時，最大橫向偏差在36 ～38 s 之間為0.024 6 m，航向角偏差受工況影響在42～44 s 之間，出現了大于1°的航向波動，波動時間非常短且只出現了1次，可視為偶然誤差。高速作業(yè)時，最大橫向偏差在47～48 s 之間為0.032 m，橫向偏差波動具有隨機性，航向偏差波動較穩(wěn)定。此時拖拉機高速、中速、低速作業(yè)行時橫向偏差，航向偏差有所差別，但相差較小，速度對其影響不明顯。

由圖5、6 知，從改善程度看，橫向位置偏差比航向角偏差更為明顯。不同作業(yè)速度下旋耕機的橫向偏差基本在0.02 m 以內，直線跟隨能力得到了大幅度提高。具體差異如表1 所示。

表1 旋耕機作業(yè)精度對比表Table 1 Rotary cultivator operation precision comparison table

上述表中可知，該算法考慮農機具誤差后，直線保持性能得到提高，最大橫向偏差變小，橫向最大偏差分別降低了54.11%、45.74%、38.93%，低速與中速航向角最大偏差分別提高了8.06%、9.11%，高速航向角最大偏差降低了35.51%。低速與中速航向角最大偏差角有所增大，但超過1°的時間分別降低了85.06%、82.57%。同時從其標準差看，橫向偏差標準差分別降低了36.06%、38.82%、47.85%，航向角偏差標準差分別降低了36.97%、23.26%、32.89%。

從以上數據知，該算法考慮農機具的誤差時，農機具的橫向偏差得到了極大地改善，且機具航向角的標準差得到了大幅度提高。綜上所述，拖拉機拖帶農機具作業(yè)時采用拖拉機與農機具誤差之和最小值為優(yōu)化目標的模型預測控制算法能夠減小農機具的跟蹤誤差。

5 結論

1.拖拉機模型控制預測算法對拖拉機直線路徑跟蹤具有良好的可行性。

2.考慮機具誤差的拖拉機模型控制預測算法能有效提高拖拉機帶農機具作業(yè)時農機具的作業(yè)精度。試驗表明，拖拉機在低、中、高3 種速度帶農機具作業(yè)時，橫向偏差平均降低了42.36 %，橫向偏差標準差平均降低了40.91 %，航向角最大偏差平均降低了6.11 %，航向角偏差標準差平均降低了31.04 %。對帶機具作業(yè)的拖拉機進行直線跟蹤作業(yè)控制時，在代價函數中增設農機具誤差項的預測控制算法能有效改善農機具的作業(yè)精度。