變速條件下農(nóng)業(yè)機(jī)械路徑跟蹤穩(wěn)定控制方法

段賢強(qiáng) 陶建峰 覃程錦 蔡道清 李彥明 劉成良

(上海交通大學(xué)機(jī)械與動力工程學(xué)院， 上海 200240)

0 引言

農(nóng)業(yè)機(jī)械(簡稱農(nóng)機(jī))自動導(dǎo)航是精準(zhǔn)農(nóng)業(yè)技術(shù)體系的核心技術(shù)之一，廣泛應(yīng)用于耕作、播種、施肥、噴藥、收獲等農(nóng)業(yè)生產(chǎn)過程[1]。近年來，控制技術(shù)、傳感器技術(shù)、信息技術(shù)以及微處理器技術(shù)的高速發(fā)展極大地推動了農(nóng)機(jī)裝備智能化的進(jìn)展，使得農(nóng)業(yè)機(jī)械的自動導(dǎo)航控制技術(shù)受到了國內(nèi)外學(xué)者廣泛的關(guān)注[2-3]。

目前，農(nóng)機(jī)自動導(dǎo)航跟蹤控制方法主要有PID控制[4-6]、純追蹤模型[7-11]、智能控制[12-14]和模型預(yù)測控制[15-19]等。PID控制是一種對誤差量進(jìn)行線性組合反饋控制的方法，應(yīng)用較為廣泛。例如，文獻(xiàn)[4]采用PID控制方法，在速度恒定時，農(nóng)機(jī)直線跟蹤精度較高，但在速度改變時，需要重新整定PID參數(shù)?；诩冏粉櫮Ｐ偷目刂品椒ㄊ且环N幾何方法，其優(yōu)點是無需對系統(tǒng)進(jìn)行建模。文獻(xiàn)[7]采用基于Kalman濾波和純追蹤模型的方法進(jìn)行農(nóng)機(jī)的自動導(dǎo)航，在固定前視距離和行駛速度的前提下，具有比較高的跟蹤精度。然而，前視距離是影響控制精度與穩(wěn)定性的主要參數(shù)，固定的前視距離的純追蹤模型控制方法對于農(nóng)機(jī)速度變化非常敏感，在速度變化時，可能出現(xiàn)控制不穩(wěn)定的情況。文獻(xiàn)[11]采用模糊控制自適應(yīng)調(diào)整前視距離參數(shù)，在兩種不同速度下實現(xiàn)了農(nóng)機(jī)的穩(wěn)定控制。模糊控制方法不依賴于被控對象的物理模型。文獻(xiàn)[12]建立模糊控制器對拖拉機(jī)轉(zhuǎn)向進(jìn)行控制，實現(xiàn)了對直線路徑的跟蹤。文獻(xiàn)[13]采用基于遺傳算法的自適應(yīng)模糊控制方法，提高了系統(tǒng)的跟蹤精度，但對于不同速度下的穩(wěn)定性未進(jìn)行研究。模型預(yù)測控制方法是基于農(nóng)機(jī)的運(yùn)動學(xué)或者動力學(xué)模型，預(yù)測農(nóng)機(jī)未來一段時域的運(yùn)動，根據(jù)評價函數(shù)，最優(yōu)地選擇控制量，其預(yù)測與控制效果依賴于數(shù)學(xué)模型的精度，即模型的精度與控制的實時性之間存在矛盾。文獻(xiàn)[16]將非線性模型預(yù)測控制應(yīng)用于拖拉機(jī)的自主導(dǎo)航，實現(xiàn)了泥濘田地中復(fù)雜路徑的跟蹤，但跟蹤精度還有待提高。文獻(xiàn)[20]提出了一種與行駛速度無關(guān)的農(nóng)機(jī)路徑跟蹤方法，對農(nóng)機(jī)的速度變化具有一定的魯棒性，但在轉(zhuǎn)向系統(tǒng)時間常數(shù)較大時可能存在振蕩現(xiàn)象。

分析上述文獻(xiàn)發(fā)現(xiàn)，當(dāng)前農(nóng)機(jī)路徑跟蹤控制在不同速度條件下仍存在穩(wěn)定性和魯棒性較差的問題。因此，為提高農(nóng)機(jī)路徑跟蹤控制在不同速度條件下的穩(wěn)定性和魯棒性，本文提出基于鏈?zhǔn)较到y(tǒng)模型和小范圍穩(wěn)定性分析優(yōu)化的路徑跟蹤控制方法。以水稻穴直播機(jī)為實驗平臺進(jìn)行直線路徑跟蹤對比實驗和農(nóng)機(jī)作業(yè)實驗，以驗證所提控制方法控制效果。

1 系統(tǒng)建模

1.1 農(nóng)機(jī)運(yùn)動學(xué)建模

自動導(dǎo)航控制車輛的物理模型一般為運(yùn)動學(xué)模型和動力學(xué)模型。在低速時，車輛平臺運(yùn)動學(xué)約束影響較大，而隨著速度的增加，動力學(xué)特性對運(yùn)動規(guī)劃與控制的影響就越明顯[21]。由于農(nóng)機(jī)一般為低速行駛，所以本文建立運(yùn)動學(xué)模型。以待跟蹤目標(biāo)直線AB為X軸，垂直目標(biāo)直線為Y軸建立田地XOY直角坐標(biāo)系(圖1)，以農(nóng)機(jī)后輪軸線中點C為車身坐標(biāo)點，根據(jù)運(yùn)動學(xué)關(guān)系，建立運(yùn)動學(xué)模型為

(1)

式中x、y——車身坐標(biāo)點C橫、縱坐標(biāo)，m

θ——航向角，逆時針為正,(°)

δ——前輪轉(zhuǎn)角，逆時針為正,(°)

v——車輛速度,m/s

l——車輛軸距,m

圖1 田地坐標(biāo)系Fig.1 Coordinate of field

1.2 前輪轉(zhuǎn)向系統(tǒng)建模

農(nóng)機(jī)前輪自動轉(zhuǎn)向系統(tǒng)采用步進(jìn)電機(jī)驅(qū)動，其電機(jī)轉(zhuǎn)速與控制器輸出成正比，故可以將前輪自動轉(zhuǎn)向系統(tǒng)看作一階慣性系統(tǒng)，為

(2)

式中δt——目標(biāo)前輪轉(zhuǎn)角,(°)

T——一階慣性系統(tǒng)時間常數(shù)

轉(zhuǎn)向曲率γ與前輪轉(zhuǎn)角δ的關(guān)系為

(3)

當(dāng)前輪轉(zhuǎn)角δ較小時，則式(3)可以簡化為

γ=δ/l

(4)

由式(2)與式(4)可得農(nóng)機(jī)轉(zhuǎn)向曲率速率為

(5)

式中γt——期望轉(zhuǎn)向曲率

根據(jù)式(1)、(3)、(5)，可得前輪轉(zhuǎn)角較小時農(nóng)機(jī)運(yùn)動學(xué)模型為

(6)

2 路徑跟蹤控制

2.1 轉(zhuǎn)向控制方法

由式(1)可知，農(nóng)機(jī)運(yùn)動學(xué)模型是一個非線性模型，根據(jù)文獻(xiàn)[22]所提的鏈?zhǔn)较到y(tǒng)，將非線性的農(nóng)機(jī)運(yùn)動學(xué)模型轉(zhuǎn)換為線性的鏈?zhǔn)较到y(tǒng)，從而得到前輪轉(zhuǎn)向控制方法。鏈?zhǔn)较到y(tǒng)的一般表達(dá)式為

(7)

式中xn——狀態(tài)量u1、u2——系數(shù)

為使鏈?zhǔn)较到y(tǒng)為線性，將狀態(tài)量對時間的導(dǎo)數(shù)用各個狀態(tài)量對x1的導(dǎo)數(shù)替代，記為

(8)

則式(7)可改寫為

(9)

農(nóng)機(jī)運(yùn)動學(xué)模型式(1)為三階系統(tǒng)，鏈?zhǔn)较到y(tǒng)和線性的鏈?zhǔn)较到y(tǒng)的三階式為

(10)

(11)

令x1=x，x2=y，根據(jù)式(1)和式(10)可得

(12)

(13)

由式(12)、(13)，可得

x3=tanθ

(14)

再根據(jù)式(8)和式(10)，可得u2和u3為

(15)

(16)

式(11)表示的鏈?zhǔn)较到y(tǒng)為線性系統(tǒng)，將誤差項進(jìn)行線性組合，可得到控制律為

u3=-kyx2-kθx3

(17)

式中ky、kθ——位置偏差與航向偏差的比例系數(shù)

根據(jù)式(14)、(16)、(17)，可以得到期望前輪轉(zhuǎn)角為

δt=arctan(lcos3θ(-kyy-kθtanθ))

(18)

2.2 小范圍穩(wěn)定性分析

在小范圍速度條件下，將前輪轉(zhuǎn)角為小角度的農(nóng)機(jī)運(yùn)動學(xué)模型式(6)在平衡位置y=0,θ=0處進(jìn)行泰勒展開，保留一階項，可得

(19)

式中φy、φθ、φγ——γt關(guān)于y、θ、γ在平衡位置y=0,θ=0處的導(dǎo)數(shù)

其中，農(nóng)機(jī)行駛速度v由機(jī)器速度限制與工作要求確定。

由式(3)與式(18)可得

(20)

對式(20)分別求對y、θ、γ的導(dǎo)數(shù),并代入平衡位置y=0,θ=0，可得

(21)

將式(21)代入式(19)，可得系數(shù)矩陣

(22)

由式(22)可得系統(tǒng)多項式為

(23)

式中I——單位矩陣

化簡得

(24)

由Routh穩(wěn)定判據(jù)得

(25)

式中a0、a1、a2、a3、A1——Routh判據(jù)系數(shù)

農(nóng)機(jī)在平衡位置小范圍穩(wěn)定的條件為

(26)

2.3 轉(zhuǎn)向控制優(yōu)化

由穩(wěn)定性條件式(26)知，當(dāng)參數(shù)一定時，該方法在農(nóng)機(jī)行駛速度小于固定值時才能夠穩(wěn)定，但在實際應(yīng)用中，農(nóng)機(jī)行駛速度是根據(jù)工作要求變化的，若采用上述方法，可能出現(xiàn)不滿足該穩(wěn)定性條件的情況，為提高農(nóng)機(jī)在平衡位置小范圍的穩(wěn)定性，將線性鏈?zhǔn)较到y(tǒng)轉(zhuǎn)向控制律式(17)進(jìn)行優(yōu)化，優(yōu)化后的控制方法為

(27)

(28)

式中v0——整定參數(shù)時的行駛速度

速度為v0時，穩(wěn)定性條件式(26)成立，有

(29)

則優(yōu)化后的期望前輪轉(zhuǎn)角為

(30)

同樣，按照Routh穩(wěn)定判據(jù)可知，優(yōu)化后農(nóng)機(jī)在平衡位置小范圍穩(wěn)定的條件為

(31)

由式(31)穩(wěn)定條件可知，在v≤v0時，由式(29)可得

(32)

則穩(wěn)定性條件成立。

在v≥v0且v0≥1時，知穩(wěn)定性條件

(33)

成立。

綜上，優(yōu)化后的控制方法滿足小范圍穩(wěn)定性條件，農(nóng)機(jī)在平衡位置小范圍穩(wěn)定條件與農(nóng)機(jī)行駛速度無關(guān)。

2.4 系統(tǒng)仿真

為驗證上述轉(zhuǎn)向控制方法優(yōu)化策略的有效性，在Matlab/Simulink仿真環(huán)境下，建立農(nóng)機(jī)運(yùn)動學(xué)與前輪轉(zhuǎn)向系統(tǒng)模型，進(jìn)行直線跟蹤實驗，對優(yōu)化前后的農(nóng)機(jī)控制方法進(jìn)行仿真實驗。

仿真初始條件設(shè)置如下：初始橫向偏差為0.2 m，航向角偏差為0°，采樣時間為100 ms，速度v=1.0 m/s，分別調(diào)整優(yōu)化前后轉(zhuǎn)向控制方法的參數(shù)ky、kθ，使得優(yōu)化前后的轉(zhuǎn)向控制方法在控制的快速性、穩(wěn)定性和超調(diào)量等方面均表現(xiàn)良好。在速度v=0.5 m/s和v=1.5 m/s條件下，分別進(jìn)行優(yōu)化前和優(yōu)化后控制方法的仿真實驗，得到優(yōu)化前后的轉(zhuǎn)向控制方法的橫向偏差與前進(jìn)距離之間的關(guān)系。

在不同速度下，優(yōu)化前控制方法的農(nóng)機(jī)行駛路徑橫向偏差與前進(jìn)距離之間的關(guān)系如圖2所示，在速度v=0.5 m/s和v=1.0 m/s時，優(yōu)化前控制方法具有收斂速度快、沒有超調(diào)量和穩(wěn)定等優(yōu)點，但當(dāng)速度增加至v=1.5 m/s時，優(yōu)化前控制方法不再收斂。

圖2 優(yōu)化前不同速度下橫向偏差Fig.2 Lateral deviation at different speeds before optimizing

優(yōu)化后控制方法的仿真結(jié)果如圖3所示，在不同行駛速度下，優(yōu)化后的控制方法均能快速地收斂至平衡位置，并保持穩(wěn)定。該仿真實驗結(jié)果與理論分析結(jié)果一致，本文所提出的轉(zhuǎn)向控制方法的優(yōu)化方法能在速度增大時保持控制的穩(wěn)定性。

圖3 優(yōu)化后不同速度下橫向偏差Fig.3 Lateral deviation at different speeds after optimizing

3 實驗與結(jié)果分析

3.1 實驗平臺

圖4 實驗平臺Fig.4 Experiment platform

為驗證所提出的控制方法，將洋馬VP6E型水稻穴直播機(jī)(圖4)進(jìn)行自動化升級改造后作為實驗平臺，在原有結(jié)構(gòu)基礎(chǔ)上，增加自動轉(zhuǎn)向系統(tǒng)、速度控制系統(tǒng)、高精度北斗定位系統(tǒng)、前輪轉(zhuǎn)角測量傳感器和車身姿態(tài)傳感器等。

圖5 不同速度橫向偏差曲線Fig.5 Lateral deviation curves at different speeds

定位系統(tǒng)采用北斗高精度差分定位系統(tǒng)(RTK-GPS)，能以最高10 Hz的頻率更新定位信息，其定位精度可達(dá)2 cm，航向精度可達(dá)0.1°。車身姿態(tài)傳感器采用BW-VG525型動態(tài)傾角傳感器，該傳感器是一款高性能的慣性測量設(shè)備，其動態(tài)測量精度為0.1°，靜態(tài)測量精度可達(dá)0.01°，最大輸出頻率為100 Hz，該傳感器用于測量車身的橫滾角與俯仰角，完成對車身定位數(shù)據(jù)的修正；前輪轉(zhuǎn)角傳感器采用MCGJ485B型絕對值角度傳感器，測量精度為0.2°，主要對前輪轉(zhuǎn)角數(shù)據(jù)進(jìn)行測量。速度控制系統(tǒng)采用在現(xiàn)有油門結(jié)構(gòu)基礎(chǔ)上增加電動推桿-拉線的方式實現(xiàn)。通過以上自動化升級改造，搭建了基于ARM控制器的農(nóng)機(jī)位置、航向、前輪轉(zhuǎn)角信息的獲取和方向盤與油門的自動控制導(dǎo)航平臺。

3.2 實驗結(jié)果與分析

采用上述的農(nóng)機(jī)實驗平臺進(jìn)行路徑跟蹤實驗，驗證所提出的控制方法。根據(jù)實驗平臺RTK-GPS實時采集的定位數(shù)據(jù)，得到期望路徑與實際位置之間的誤差，作為路徑跟蹤實驗的跟蹤誤差。

3.2.1直線跟蹤對比實驗

實驗平臺在較為平整的水田中分別以3種不同速度(v=0.8 m/s、v=1.0 m/s和v=1.2 m/s)進(jìn)行本文控制方法和PID控制方法直線路徑跟蹤對比實驗，由于無需播種作業(yè)，則播種機(jī)具處于提升狀態(tài)。根據(jù)PID控制方法可以得到期望前輪轉(zhuǎn)角為

(34)

在速度v=1.0 m/s時，根據(jù)經(jīng)驗法，整定本文控制方法和PID控制方法,本文控制方法參數(shù)設(shè)定為ky=2，kθ=4，PID參數(shù)為kp=5,kd=1,ki=0.03，實驗時為防止積分項絕對值過大，設(shè)定其最大為200，并且僅在其絕對值小于0.15 m時進(jìn)行累加。實驗時，通過高精度差分定位系統(tǒng)設(shè)定AB點，確定目標(biāo)跟蹤直線，農(nóng)機(jī)行駛至目標(biāo)直線附近，使得農(nóng)機(jī)初始橫向偏差為0.5 m，開啟導(dǎo)航控制系統(tǒng)，農(nóng)機(jī)沿著目標(biāo)直線行駛至目標(biāo)位置后關(guān)閉導(dǎo)航控制系統(tǒng)。分別采用本文控制方法和傳統(tǒng)PID控制方法在上述3個速度下重復(fù)該實驗。實驗結(jié)果如表1所示。

表1 不同速度下橫向偏差對比Tab.1 Comparison of lateral deviation at different speeds

在不同速度下的橫向偏差與行駛距離的關(guān)系如圖5所示。結(jié)果表明，在速度為0.8 m/s和1.0 m/s時，本文控制方法與PID控制方法均能保證控制結(jié)果收斂，且本文控制方法的橫向偏差的平均絕對誤差小于PID算法的平均絕對誤差；在速度為1.2 m/s時，本文控制方法的控制結(jié)果收斂，平均絕對誤差和最大絕對誤差無明顯增大，而PID控制不穩(wěn)定。

3.2.2農(nóng)機(jī)作業(yè)實驗

農(nóng)機(jī)在實際工作環(huán)境下不僅需要簡單的點到點的直線跟蹤，而且還需要按照規(guī)劃路徑遍歷農(nóng)田，根據(jù)水稻穴直播機(jī)按行播種的作業(yè)特點，采用梭行法遍歷農(nóng)田，得到農(nóng)機(jī)作業(yè)規(guī)劃路徑,如圖6所示。

圖6 農(nóng)機(jī)作業(yè)規(guī)劃路徑Fig.6 Agricultural machinery operation planning path

在作業(yè)時，設(shè)定農(nóng)田的四頂點確定待作業(yè)農(nóng)田，根據(jù)該實驗平臺的轉(zhuǎn)彎半徑，設(shè)置地頭寬度，采用梭行法進(jìn)行農(nóng)機(jī)行駛路徑的規(guī)劃，農(nóng)機(jī)行駛至目標(biāo)路徑起點，使播種機(jī)具處于下降狀態(tài)，啟動自動導(dǎo)航控制，農(nóng)機(jī)自動完成目標(biāo)路徑的跟蹤。農(nóng)機(jī)作業(yè)跟蹤路徑如圖7所示，實際作業(yè)路徑如圖8所示。

圖7 農(nóng)機(jī)作業(yè)路徑Fig.7 Agricultural machinery operation path

圖8 農(nóng)機(jī)實際作業(yè)路徑Fig.8 Agricultural machinery actual operation path

本文控制方法在各行的直線跟蹤控制效果如表2所示，速度和橫向偏差隨時間變化關(guān)系如圖9和圖10所示。其中，橫向偏差在每次換行轉(zhuǎn)彎時發(fā)生突變；上線距離是指開始直線跟蹤位置與農(nóng)機(jī)橫向偏差絕對值第1次小于6 cm時位置的距離；表2中實驗數(shù)據(jù)均是上線后的統(tǒng)計結(jié)果。對比表1和表2可知，農(nóng)機(jī)作業(yè)實驗結(jié)果在平均絕對誤差方面較直線跟蹤實驗結(jié)果增大較多，經(jīng)過分析發(fā)現(xiàn)，主要有3個原因：

表2 各行直線跟蹤結(jié)果Tab.2 Line tracking results

圖9 速度變化曲線Fig.9 Velocity variation curve

圖10 橫向偏差變化曲線Fig.10 Lateral deviation variation curve

(1)直線跟蹤對比實驗和農(nóng)機(jī)作業(yè)實驗的初始誤差不同。直線跟蹤對比實驗時，初始誤差均為50 cm，而農(nóng)機(jī)作業(yè)實驗時，每次地頭轉(zhuǎn)彎初始誤差均大于50 cm。

(2)直線跟蹤對比實驗和農(nóng)機(jī)作業(yè)實驗的實驗環(huán)境不同。直線對比實驗在水田中進(jìn)行實驗時，為保證本文控制方法與PID控制的實驗環(huán)境條件相同，選擇在較為平整的水田中重復(fù)多次實驗；而田間作業(yè)實驗則需要遍歷整個田地。

(3)直線跟蹤對比實驗和田間作業(yè)實驗時播種機(jī)具的狀態(tài)不同。直線對比實驗時，播種機(jī)具由于無需播種，則處于空閑狀態(tài)，即提升狀態(tài)；田間作業(yè)實驗時播種機(jī)具處于工作狀態(tài)，即下降狀態(tài)，二者狀態(tài)的不同導(dǎo)致機(jī)器負(fù)載變化也是影響作業(yè)精度的因素之一。

實驗結(jié)果表明，在農(nóng)機(jī)實際工作環(huán)境下，行駛速度0.4～2.0 m/s范圍變化時，本文控制方法依然具有穩(wěn)定性，平均絕對誤差均值為0.047 m，最大絕對誤差為0.128 m。進(jìn)一步驗證了本文控制方法對速度變化具有魯棒性，并且具有較高的控制精度，適用于農(nóng)機(jī)復(fù)雜的實際工作環(huán)境。

4 結(jié)論

(1)針對農(nóng)機(jī)路徑跟蹤控制方法在不同速度下的穩(wěn)定性問題，提出了一種基于鏈?zhǔn)较到y(tǒng)模型和小范圍穩(wěn)定性分析的路徑跟蹤控制方法。通過理論分析和仿真實驗，驗證了其在不同速度條件下均能保持穩(wěn)定性。

(2)采用水稻穴直播機(jī)在水田中開展了本文方法與PID控制方法的直線跟蹤對比實驗和農(nóng)機(jī)作業(yè)實驗。驗證了本文控制方法在3種不同速度下穩(wěn)定性優(yōu)于常用的PID控制方法；農(nóng)機(jī)行駛速度在0.4～2.0 m/s范圍內(nèi)變化時，本文方法能夠保持穩(wěn)定，平均絕對誤差均值為0.047 m，最大絕對誤差為0.128 m，適用于復(fù)雜的農(nóng)機(jī)作業(yè)環(huán)境。