馬志凱，種 坤，趙曉順，趙建國(guó)，趙樹(shù)朋，于合龍

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

吉林 長(zhǎng)春 130118）

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

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

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

1 拖拉機(jī)動(dòng)力學(xué)模型

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

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

前后輪運(yùn)動(dòng)學(xué)約束表示為：

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

根據(jù)前后輪幾何關(guān)系可得：

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

同時(shí)拖拉機(jī)轉(zhuǎn)彎的幾何關(guān)系可得：

根據(jù)機(jī)具與后軸幾何關(guān)系可得：

因此拖拉機(jī)運(yùn)動(dòng)學(xué)模型為：

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

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

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

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

圖1 拖拉機(jī)路徑跟蹤圖Fig.1 Tractor path tracking

2 模型預(yù)測(cè)控制問(wèn)題構(gòu)建

按照拖拉機(jī)位置及農(nóng)機(jī)具與期望路徑誤差最小，將拖拉機(jī)路徑跟蹤問(wèn)題構(gòu)建為模型預(yù)測(cè)控制問(wèn)題，由（11）式描述，其中T為測(cè)時(shí)域，p(x)為拖拉機(jī)參考路徑。

2.1 代價(jià)函數(shù)

式（11）表示了模型預(yù)測(cè)控制問(wèn)題的評(píng)價(jià)指標(biāo)，設(shè)定為代價(jià)函數(shù)J，如式（15）所示。

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

2.2 約束條件

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

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

其中vt表示設(shè)定的固定車(chē)速，ωmax為設(shè)定的橫擺角速度上限。

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

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

由式（10）可得：

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

3 算法可行性驗(yàn)證

3.1 試驗(yàn)構(gòu)建

為驗(yàn)證該算法路徑跟蹤保持的可行性，搭建實(shí)物進(jìn)行試驗(yàn)。

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

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

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

圖2 試驗(yàn)拖拉機(jī)雷沃950Fig.2 Experimental tractor Revo 950

3.2 算法可行性試驗(yàn)

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

起步入軌是指拖拉機(jī)后軸中心點(diǎn)偏離跟蹤軌跡一定距離起步跟蹤預(yù)定軌跡行駛。倒車(chē)入軌是指拖拉機(jī)在軌跡上倒車(chē)起步跟蹤預(yù)定軌跡行駛。試驗(yàn)測(cè)得拖拉機(jī)后軸中心點(diǎn)橫向誤差、速度及拖拉機(jī)航向誤差如圖3 所示。

圖3 入軌試驗(yàn)Fig3 Test of enter an orbit

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

單次換擋是指拖拉機(jī)行駛過(guò)程中換入相鄰擋位。多次換擋是指拖拉機(jī)在行駛過(guò)程中多次換入相鄰擋位。試驗(yàn)驗(yàn)測(cè)的拖拉機(jī)后軸中心點(diǎn)橫向誤差、速度及拖拉機(jī)航向誤差如圖4 所示。

圖4 換擋試驗(yàn)Fig.4 Experiment of shift gears

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

起步入軌，倒車(chē)行駛、單次換擋、多次換檔試驗(yàn)表面，本算法能有效實(shí)現(xiàn)拖拉機(jī)直線路徑跟隨，并適應(yīng)拖拉機(jī)不同復(fù)雜工況的使用要求。

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

拖拉機(jī)帶農(nóng)機(jī)具作業(yè)時(shí)農(nóng)機(jī)具受三點(diǎn)懸掛結(jié)構(gòu)及作業(yè)環(huán)境的影響，農(nóng)機(jī)具行駛軌跡與拖拉機(jī)的行駛軌跡并不重合。為驗(yàn)證此算法對(duì)農(nóng)機(jī)具直線跟蹤的影響，進(jìn)行拖拉機(jī)帶機(jī)具作業(yè)試驗(yàn)。

4.1 拖拉機(jī)帶機(jī)具作業(yè)試驗(yàn)

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

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

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

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

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

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

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

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

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

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

5 結(jié)論

1.拖拉機(jī)模型控制預(yù)測(cè)算法對(duì)拖拉機(jī)直線路徑跟蹤具有良好的可行性。

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