基于非線性模型的農用車路徑跟蹤控制器設計與試驗

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

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

0 引言

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

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

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

1 路徑跟蹤系統組成

本文設計的田間路徑跟蹤試驗小車如圖1所示，具體結構參數如表1所示。

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

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

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

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

2 路徑跟蹤控制器設計

2.1 農用車運動學模型

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

圖3 農用車的運動學模型Fig.3 Kinematics model of agricultural vehicle

農用車運動學方程[20-21]為

(1)

式中l——軸距

φ——航向角

δ——前輪偏角

x、y——質心坐標值

v——前、后軸中心速度

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

(2)

其中

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

式中u——系統控制量

f(·)——狀態方程

ξ——系統狀態量

η——系統輸出量

2.2 非線性模型預測控制器的設計

2.2.1控制器的預測模型

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

(3)

式中k——離散變量

T——采樣周期

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

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

(4)

(5)

(6)

式中Np——預測時域

Nc——控制時域

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

(7)

2.2.2非線性模型的閉環校正

為了更好地描述系統及克服試驗時的不確定因素，在控制輸出的基礎上通過誤差預測和實時反饋實現對預測模型的閉環校正。

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時刻系統在預測時域Np內的預測誤差

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

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

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

vc、δc——控制時域內的控制量

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

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

(9)

其中

式中 ΔΓ——預測誤差序列

Δu——控制增量序列

Q——權重矩陣

R——權重系數

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

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

(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——狀態變量約束最小值、最大值

ΔΓmin、ΔΓmax——狀態變量增量約束最小值、最大值

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

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

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

2.2.3非線性模型的滾動優化

本文采用內點法[26-27]進行求解。將t時刻控制輸入量的第1個元素作為實際控制量作用于控制系統，在t+1時刻重復以上控制過程實現最優控制。

3 控制器仿真與分析

3.1 仿真路徑

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

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

3.2 速度對路徑跟蹤的影響

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

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

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

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

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

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

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

3.4 預測時域對路徑跟蹤的影響

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

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

圖7 預測時域對路徑跟蹤的影響Fig.7 Effect of prediction horizon on path tracking

4 試驗與結果分析

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

試驗場地為山東農業大學試驗基地，試驗現場如圖8所示。

圖9 試驗結果Fig.9 Experiment result

圖8 路徑跟蹤試驗現場Fig.8 Tracking experiment scene

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

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

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

表2 節點處的具體參數值Tab.2 Specific parameter values at node cm

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

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

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

5 結論

(1)針對農用車線性化建模控制精度較低，在系統非線性特性突出時預測效果較差等問題，提出非線性模型預測控制。利用歐拉法建立預測模型；通過反饋矯正、滾動優化完成控制器的閉環設計。

圖10 對比試驗結果Fig.10 Contrast experiment results

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

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

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