基于非線性模型的農(nóng)用車路徑跟蹤控制器設(shè)計與試驗

劉正鐸 張萬枝 呂釗欽 鄭文秀 穆桂脂

(1.山東農(nóng)業(yè)大學機械與電子工程學院，泰安 271018； 2.山東省園藝機械與裝備重點實驗室，泰安 271018)

0 引言

農(nóng)用車輛的路徑跟蹤是實現(xiàn)農(nóng)業(yè)機械自動化與智能化的關(guān)鍵技術(shù)，其路徑跟蹤的難點在于田間作業(yè)環(huán)境復(fù)雜，農(nóng)用車輛很難根據(jù)不同環(huán)境做出正確判斷，保證車輛的正常行駛。農(nóng)用車輛的路徑跟蹤性能依賴其控制器所用的控制算法，其中主要的控制算法包括LQR控制算法、Lyapunov法、Backstepping法和滑膜控制算法等[1-4]。

文獻[5]提出一種基于LQR算法的軌跡控制方法，在滾動時域的每一采樣時刻將參考軌跡上的軌跡跟蹤誤差線性化，從而得到線性模型并利用該模型設(shè)計了路徑跟蹤控制器。文獻[6-7]基于非切換解析模型預(yù)測控制理論，結(jié)合非線性模型預(yù)測控制，設(shè)計出一種船舶定位控制系統(tǒng)，使船舶可以快速移動到參考位置。文獻[8-9]通過推導(dǎo)無人駕駛車輛在轉(zhuǎn)彎時的速度、輪胎側(cè)偏角、橫擺角速度等參數(shù)，應(yīng)用滑膜控制方法在仿真條件下可以很好地控制車輛漂移時的行駛狀態(tài)。文獻[10]利用粒子群算法優(yōu)化模糊控制中的控制參數(shù)，該方法可以減小橫向誤差，當車速為0.8 m/s時曲線路徑跟蹤最大橫向偏差為5.9 cm。文獻[11]采用了基于觸須原理的局部路徑規(guī)劃，利用車輛在行進過程中多組不同半徑的圓弧段來描述路徑，在跟蹤直線路徑時得到了良好的效果，但是它忽略了車輛在轉(zhuǎn)彎時曲率變化的速度，從而造成在速度較高時無法成功跟蹤圓形路徑。

近年來模型預(yù)測控制越來越受到學者們的青睞，模型控制的優(yōu)點是預(yù)測與控制的效果良好，缺點是控制器的控制作用依賴于數(shù)學模型的精度，當模型精度低時直接影響到路徑跟蹤的精度。張純剛等[12]結(jié)合預(yù)測控制理論進行自動駕駛的局部路徑規(guī)劃，提出了基于滾動窗口的路徑跟蹤方法，該方法可以充分利用已知的局部信息，通過反饋進行局部路徑規(guī)劃，得到了很好的收斂性。文獻[13-15]采用線性模型預(yù)測進行農(nóng)用車的路徑跟蹤，通過求解帶約束的目標函數(shù)、在線滾動優(yōu)化、反饋校正、修正預(yù)測模型等使路徑跟蹤系統(tǒng)的誤差最小，但是由于建模過程涉及到線性化與離散化,所以在跟蹤圓形路徑時會造成跟蹤誤差較大。3 m/s時直線跟蹤最大橫向偏差為1.706 cm，圓形跟蹤最大橫向偏差為15.245 cm。本文從非線性模型預(yù)測控制算法角度出發(fā)，通過合理簡化模型提高建模精度，在降低計算量的同時保證路徑跟蹤的效果。

1 路徑跟蹤系統(tǒng)組成

本文設(shè)計的田間路徑跟蹤試驗小車如圖1所示，具體結(jié)構(gòu)參數(shù)如表1所示。

圖1 試驗小車Fig.1 Test car1.前輪轉(zhuǎn)角傳感器 2.速度傳感器 3.差速驅(qū)動裝置 4.前輪轉(zhuǎn)向驅(qū)動器 5.信號轉(zhuǎn)換裝置 6.電瓶 7.后輪驅(qū)動器 8.前輪轉(zhuǎn)向裝置

表1 試驗小車主要技術(shù)指標Tab.1 Main technical indexes of vehicle

路徑信息采集模塊：轉(zhuǎn)角傳感器為E6B2-CCWZ3E型編碼器，測量誤差為±0.3°；速度傳感器為JK5002D型霍爾接近開關(guān)，測速誤差±0.05 m/s?？刂茍?zhí)行模塊：轉(zhuǎn)向驅(qū)動器為ASMT-01series型大功率直線舵機控制器；轉(zhuǎn)向執(zhí)行裝置為42BYGH47型步進電動機驅(qū)動渦輪蝸桿式轉(zhuǎn)向裝置,其行程為80 mm,速度最快可達29 mm/s；后輪驅(qū)動器為CM2010型有刷電動機控制器；后輪驅(qū)動裝置為DMW86型無刷直流電動機，額定轉(zhuǎn)速為3 200 r/min，額定功率為1 000 W。路徑信息處理模塊：導(dǎo)航控制器下位機為STM32F103ZET6型單片機，其主要任務(wù)是采集試驗小車轉(zhuǎn)速傳感器與前輪轉(zhuǎn)角傳感器的模擬信號并轉(zhuǎn)換成數(shù)字信號，通過WiFi與USART通信傳輸給導(dǎo)航控制器上位機(DELL計算機)中的Matlab軟件，從而進行算法處理。路徑跟蹤系統(tǒng)工作流程圖如圖2所示。

圖2 路徑跟蹤系統(tǒng)工作流程圖Fig.2 Flow chart of path tracking system processing

2 路徑跟蹤控制器設(shè)計

2.1 農(nóng)用車運動學模型

對農(nóng)用車進行運動學分析，在平面坐標系下建立如圖3所示的模型。該模型前輪轉(zhuǎn)向，后輪驅(qū)動。在工作過程中，轉(zhuǎn)向輪與驅(qū)動輪通過調(diào)節(jié)電壓來調(diào)節(jié)轉(zhuǎn)角與轉(zhuǎn)速。在整個運動學分析過程中，將農(nóng)用車視為剛體，運行在水平面上。為確定農(nóng)用車在整個運行軌跡中的位置和姿態(tài)，建立導(dǎo)航坐標系。選取農(nóng)用車質(zhì)心點為參考點并定義農(nóng)用車的位姿信息為(x,y,φ)。在不考慮農(nóng)用車側(cè)傾、側(cè)滑、俯仰等情況下，可將低速行駛的農(nóng)用車輛簡化為二自由度運動學模型[16-19]。

圖3 農(nóng)用車的運動學模型Fig.3 Kinematics model of agricultural vehicle

農(nóng)用車運動學方程[20-21]為

(1)

式中l(wèi)——軸距

φ——航向角

δ——前輪偏角

x、y——質(zhì)心坐標值

v——前、后軸中心速度

將式(1)表示為狀態(tài)空間方程形式

(2)

其中

ξ=(x,y,φ)u=(v,δ)

式中u——系統(tǒng)控制量

f(·)——狀態(tài)方程

ξ——系統(tǒng)狀態(tài)量

η——系統(tǒng)輸出量

2.2 非線性模型預(yù)測控制器的設(shè)計

2.2.1控制器的預(yù)測模型

在非線性模型預(yù)測控制中，傳統(tǒng)算法存在計算量大、收斂速度慢等問題。本文利用歐拉公式對農(nóng)用車運動學模型進行離散化求解找到一個顯式的迭代方程[22-23]，減少了計算量，加快了系統(tǒng)的收斂速度。通過非線性模型、當前狀態(tài)量和控制時域內(nèi)的控制量對預(yù)測時域內(nèi)的控制量進行預(yù)測，得

(3)

式中k——離散變量

T——采樣周期

根據(jù)這一模型只需知道被控系統(tǒng)在某一時刻的初始位姿信息(x(k),y(k),φ(k))與兩個控制步長內(nèi)的控制輸入序列(v1,δ1)、(v2,δ2)便可以預(yù)測出系統(tǒng)在預(yù)測時域內(nèi)的控制輸出序列。將式(3)整理為輸出函數(shù)的形式

ηp(t+j|t)=Fj(ξ(t|t),u(t|t),…,u(t+Nc-1|t))(j=1,2,…,Np)

(4)

(5)

(6)

式中Np——預(yù)測時域

Nc——控制時域

為減少非線性模型控制的計算量、增強系統(tǒng)的實時性，在本文控制器中Nc=2，從而得到非線性模型預(yù)測控制器的預(yù)測模型為

(7)

2.2.2非線性模型的閉環(huán)校正

為了更好地描述系統(tǒng)及克服試驗時的不確定因素，在控制輸出的基礎(chǔ)上通過誤差預(yù)測和實時反饋實現(xiàn)對預(yù)測模型的閉環(huán)校正。

e(t+i-1|t)=Γr(t+i-1|t)-Γp(t+i-1|t)(i=1,2,…,Np)

(8)

其中

Γr(t+i-1|t)=

式中e(t+i-1|t)——t時刻系統(tǒng)在預(yù)測時域Np內(nèi)的預(yù)測誤差

Γr(t+i-1|t)——t時刻系統(tǒng)根據(jù)實時反饋推出的t+i-1時刻的系統(tǒng)輸出量

Γp(t+i-1|t)——t時刻系統(tǒng)根據(jù)運動學模型推出的t+i-1時刻的系統(tǒng)輸出量

Xr(t)、Yr(t)、φr(t)——系統(tǒng)通過實時反饋得到的農(nóng)用車實際位姿信息

vc、δc——控制時域內(nèi)的控制量

i=1時下角c為1，i>1時下角c為2。

設(shè)定f(0,0)=0為系統(tǒng)的一個穩(wěn)定點，同時也是系統(tǒng)的控制目標。對于任意預(yù)測時域Np,考慮如下形式的目標函數(shù)

(9)

其中

式中 ΔΓ——預(yù)測誤差序列

Δu——控制增量序列

Q——權(quán)重矩陣

R——權(quán)重系數(shù)

目標函數(shù)第1項表征了期望輸出的跟蹤能力，第2項表征終端約束。

通過設(shè)置狀態(tài)量與控制量的取值范圍將路徑跟蹤問題轉(zhuǎn)換為解決非線性二次型的約束問題。由于目標函數(shù)與約束函數(shù)在求解算法中的梯度都是連續(xù)的，所以目標函數(shù)可以根據(jù)所給的初始值進行梯度步長的收斂與求解。從而將式(9)轉(zhuǎn)換為求解基于遞推序列二次規(guī)劃法[24-25]的響應(yīng)問題，在此基礎(chǔ)上進行梯度計算解決非線性約束的邊界優(yōu)化

(10)

其中

Γmin≤Γ(i|t)≤Γmax

ΔΓmin≤ΔΓ(i|t)≤ΔΓmaxumin≤u(i|t)≤umax(i=0,1,…,Nc-1)Δumin≤Δu(i|t)≤Δumax(i=0,1,…,Nc-1)

式中Γmin、Γmax——狀態(tài)變量約束最小值、最大值

ΔΓmin、ΔΓmax——狀態(tài)變量增量約束最小值、最大值

umin、umax——控制量約束最小值、最大值

Δumin、Δumax——控制增量約束最小值、最大值

由式(10)可以看出，本文所述控制器是根據(jù)預(yù)測時域與控制時域內(nèi)的誤差信息對路徑跟蹤進行預(yù)測控制，在動態(tài)響應(yīng)階段，預(yù)測時域與控制時域內(nèi)的信息不完善，系統(tǒng)處于不穩(wěn)定狀態(tài)。由于此時控制系統(tǒng)啟動時間較短，各個參考速度下的農(nóng)用車實際速度相差不大，在同樣時間內(nèi)參考速度越大的運動學模型會預(yù)測出更多的路徑信息，使系統(tǒng)更快地趨于穩(wěn)定；當農(nóng)用車在行駛過程中(即系統(tǒng)穩(wěn)定階段),此時農(nóng)用車如果受到外界干擾,農(nóng)用車的速度越大，偏差就會越大，因為此時不同速度下的農(nóng)用車均已穩(wěn)定，控制器需要重新預(yù)測路徑，相同時間下，速度越快的農(nóng)用車，偏差越大。

2.2.3非線性模型的滾動優(yōu)化

本文采用內(nèi)點法[26-27]進行求解。將t時刻控制輸入量的第1個元素作為實際控制量作用于控制系統(tǒng)，在t+1時刻重復(fù)以上控制過程實現(xiàn)最優(yōu)控制。

3 控制器仿真與分析

3.1 仿真路徑

圖4 試驗參考路徑Fig.4 Experiment reference path

如圖4所示，農(nóng)用車輛從O點進入山東農(nóng)業(yè)大學試驗基地，K點為目標試驗田，其中O、A、B、C、D、E、F、G、H、J、K為路徑方程改變的節(jié)點，在一定程度上它們的各項參數(shù)反映出控制器對該段路徑的跟蹤性能。其中定義A點為動態(tài)響應(yīng)階段最大超調(diào)量坐標點?；贛atlab平臺進行模型預(yù)測控制器設(shè)計，從速度v、采樣周期T、預(yù)測時域Np3方面進行仿真分析。

3.2 速度對路徑跟蹤的影響

設(shè)定控制器參數(shù)為：Np=30、Nc=2、T=0.03 s，v分別為1、2、3 m/s時，在導(dǎo)航坐標系下路徑跟蹤結(jié)果如圖5所示。 其中，Δx表示橫向跟蹤偏差，Δy表示縱向跟蹤偏差。

圖5 速度對路徑跟蹤的影響Fig.5 Effect of speed on path tracking

圖6 采樣周期對路徑跟蹤的影響Fig.6 Effect of sampling period on path tracking

從圖5可看出，農(nóng)用車可以有效跟蹤參考路經(jīng)。從圖5b、5c可以看出，在動態(tài)響應(yīng)階段，v=1 m/s時超調(diào)量最大且調(diào)節(jié)時間最長,橫向最大跟蹤偏差為-9.24 cm,縱向最大跟蹤偏差為-10 cm,調(diào)節(jié)時間與超調(diào)量隨速度的增加而減小。圖5c在節(jié)點E、G處出現(xiàn)了較大的縱向跟蹤偏差，此時農(nóng)用車剛經(jīng)歷了AC段的直線路徑，隨后進入CE段圓形路徑，由于兩段曲線方程不一樣，致使控制器需要重新預(yù)測控制量以適應(yīng)新的參考路徑，從而造成較大偏差。圖5c、5d反映出路徑跟蹤時農(nóng)用車的行駛穩(wěn)定性，可以看出，在仿真條件下，速度的增大對跟蹤精度影響不大，但是對于動態(tài)響應(yīng)階段的穩(wěn)定性有較大影響。

3.3 采樣周期對路徑跟蹤的影響

設(shè)定控制器參數(shù)為：Np=30，Nc=2，v=3 m/s，T分別為0.01、0.02、0.03 s時，在導(dǎo)航坐標系下路徑跟蹤結(jié)果如圖6所示。

控制器的控制精度取決于預(yù)測模型的精度，而本文對于預(yù)測模型求解采用的是歐拉法，所以預(yù)測模型的精度與采樣周期T直接相關(guān)。由圖6可以看出，隨著采樣周期的變長，路徑跟蹤動態(tài)響應(yīng)階段的橫向跟蹤偏差變小，縱向跟蹤偏差變大；在路徑跟蹤過程中，跟蹤偏差則會隨著采樣周期的變長而變大。由此看出，在仿真條件下采樣時間的增大會降低路徑跟蹤的精度，同時會提高路徑跟蹤動態(tài)響應(yīng)階段的穩(wěn)定性。

3.4 預(yù)測時域?qū)β窂礁櫟挠绊?/h3>

設(shè)定控制器參數(shù)為：Nc=2，v=3 m/s，T=0.03 s，Np分別為15、30、45時，在導(dǎo)航坐標系下路徑跟蹤結(jié)果如圖7所示。

由圖7可以看出，預(yù)測時域越大橫向跟蹤偏差和前輪偏角變化幅度越小，這主要是因為預(yù)測時域較大時，控制器能夠很好地預(yù)測系統(tǒng)未來輸出，及時根據(jù)當前誤差修正系統(tǒng)控制信號，但同時會增加相應(yīng)的計算量。

圖7 預(yù)測時域?qū)β窂礁櫟挠绊慒ig.7 Effect of prediction horizon on path tracking

4 試驗與結(jié)果分析

4.1 非線性模型預(yù)測控制器路徑跟蹤試驗

試驗場地為山東農(nóng)業(yè)大學試驗基地，試驗現(xiàn)場如圖8所示。

圖9 試驗結(jié)果Fig.9 Experiment result

圖8 路徑跟蹤試驗現(xiàn)場Fig.8 Tracking experiment scene

試驗初始時，試驗小車的質(zhì)心位置與參考路徑起點重合。設(shè)定控制器參數(shù)為Np=30、Nc=2、T=0.03 s，試驗小車分別以1、2、3 m/s的速度沿參考路徑行駛。試驗分3組(每組進行1個速度的試驗)，每組進行3次試驗，取3次試驗的平均值進行繪圖。

由圖9可以看出，雖然路徑跟蹤的橫向偏差與縱向偏差有波動，但是試驗小車依然可以有效地跟蹤參考路徑。路徑跟蹤的具體數(shù)據(jù)如表2所示。

由試驗結(jié)果可以看出，試驗結(jié)果并沒有完全與仿真結(jié)果一致，偏差主要出現(xiàn)在路徑方程改變的位置與圓形路徑位置，如節(jié)點D、E、G、H處，由表2可以看出，其最大橫向偏差為-4.28 cm，最大縱向偏差為-6.61 cm。出現(xiàn)較大偏差的原因一方面是受到試驗條件的制約：小車各個機構(gòu)之間的鏈接間隙、轉(zhuǎn)向與速度的延遲、數(shù)據(jù)傳輸與處理的延遲、上位機(計算機)與下位機(STM32單片機) 之間的耦合等；另一方面是由于路徑方程改變的位置大都處于直線路徑的末端。此時小車在行駛過程中已經(jīng)有了較大的累計偏差，此時路徑方程的突然改變相當于把一個較大的橫向與縱向偏差作用到小車上從而致使控制器無法有效跟蹤參考路徑。從中同樣可以看出該控制器對于直線路段有著較好的控制作用，直線路段的最大橫向偏差為1.40 cm，最大縱向偏差為-3.05 cm。

表2 節(jié)點處的具體參數(shù)值Tab.2 Specific parameter values at node cm

4.2 與線性模型預(yù)測控制器的對比試驗

為了更好地驗證本文提出的路徑跟蹤算法，將本文方法與線性模型預(yù)測控制方法進行對比，分別進行直線與圓形路徑跟蹤試驗。設(shè)置主要控制器參數(shù)：Nc=2，T=0.01 s，Np=30，v為1、2、3 m/s時路徑跟蹤結(jié)果如圖10所示。

對比試驗表明，在速度不大于2 m/s的情況下，兩種控制器在跟蹤精度上沒有明顯差別；在v=3 m/s跟蹤圓形路徑時，線性模型預(yù)測控制器下的試驗小車出現(xiàn)了較大偏差，橫向跟蹤偏差均值為12.09 cm;縱向跟蹤偏差均值為38.08 cm。這是由于此時小車的非線性特性突出，控制器依然采用線性模型作為預(yù)測方程預(yù)測小車下一時刻運動就會導(dǎo)致較大跟蹤偏差。在本文所述控制器下小車橫向跟蹤偏差均值降低了36.8%，縱向跟蹤偏差均值降低了32.98%。由此可見，在速度較高時，本文所述控制器在跟蹤精度上有較大優(yōu)勢。

5 結(jié)論

(1)針對農(nóng)用車線性化建?？刂凭容^低，在系統(tǒng)非線性特性突出時預(yù)測效果較差等問題，提出非線性模型預(yù)測控制。利用歐拉法建立預(yù)測模型；通過反饋矯正、滾動優(yōu)化完成控制器的閉環(huán)設(shè)計。

圖10 對比試驗結(jié)果Fig.10 Contrast experiment results

并對其進行了仿真與試驗驗證，結(jié)果表明該方法具有計算簡單、精度高的特點。

(2)從仿真結(jié)果可以看出，在參考路徑的軌跡方程改變時，農(nóng)用車會產(chǎn)生一個不利于行駛穩(wěn)定性的較大偏差與波動；在行駛速度不大于3 m/s時，速度的增大對跟蹤精度影響不大,但是對于行駛穩(wěn)定性有較大影響。采樣時間的增大會降低路徑跟蹤的精度，同時會提高動態(tài)響應(yīng)階段的穩(wěn)定性；預(yù)測時域的增加可以提高跟蹤精度。

(3)在場地試驗中，試驗小車的最大橫向跟蹤偏差-4.28 cm發(fā)生在速度為2 m/s時，最大縱向跟蹤偏差-6.61 cm發(fā)生在速度為3 m/s時。從試驗結(jié)果中可以看出，該控制器無論是從路徑跟蹤的精度還是穩(wěn)定性上都可以較好地滿足農(nóng)用車輛路徑跟蹤的實際要求。