基于非線性模型預測控制的農用拖掛車避障控制器研究

白國星 劉 立 孟 宇 羅維東 顧 青 梁 晨

(北京科技大學機械工程學院， 北京 100083)

0 引言

農用機械自動導航控制是實現農業現代化的一項關鍵技術[1-3]。避障是指移動裝備在跟隨給定路徑時，識別障礙物并完成避障動作[4]，它關系到農用機械自主導航控制的安全性，因而具有十分重要的研究意義。實現避障功能需要兩項關鍵技術，即障礙物檢測技術和避障控制技術。關于障礙物檢測技術的研究成果頗豐，文獻[4]詳細介紹了該技術在國內外的研究現狀和發展趨勢，而關于農用拖掛車等農用機械避障控制的研究正在發展。

農用拖掛車與無人機、乘用車輛等同為移動裝備，所以可以參考無人機等領域的避障控制方法。目前在無人機領域存在兩種解決方案，其中一種通過在線路徑規劃算法實現避障控制，另一種則基于模型預測控制(Model predictive control, MPC)方法建立避障控制器。常用于避障控制的路徑規劃算法有A*算法[5]、神經網絡算法[6]、人工勢場法[7-8]等，由于路徑規劃算法通常將移動裝備視為質點，未考慮執行器飽和等系統約束，因此移動裝備可能無法以較小的誤差跟蹤規劃得到的路徑。MPC方法最顯著的優勢是可以顯式地考慮系統的狀態約束，基于MPC方法的避障控制器在乘用車、無人機等移動裝備的自主導航中已有較為廣泛的應用[9-14]。

考慮到農用拖掛車結構[15-18]不同于文獻[9-14]中的乘用車和無人機，而不同的結構產生的內輪差也不相同[19]，本文基于剛體運動學和非完整約束條件建立用于避障控制的農用拖掛車運動學模型。在此基礎上，提出農用拖掛車的狀態預測模型，并基于改進的禁區懲罰函數設計優化目標函數，從而提出基于非線性模型預測控制(Non-linear model predictive control, NMPC)的農用拖掛車避障控制方法。通過Matlab/Simulink對提出的避障控制方法進行仿真驗證。

1 運動學模型建立

運動學模型在乘用車等移動裝備運動控制中的應用非常廣泛[13,20-23]，其理論基礎是剛體運動學，假設條件為車體不發生側向滑動，即車體滿足非完整約束條件。農用拖掛車的運動學模型如圖1所示，包括拖車和掛車兩部分，車身之間通過鉸接點連接。

由于拖車的結構與乘用車的結構基本相同，因此拖車的運動學模型可參照文獻[13，22]中的乘用車運動學模型

(1)

式中xh、yh——拖車后橋中點的橫、縱坐標值

θt——拖車航向角

δ——拖車轉向角

vt——拖車縱向行駛速度

lt——拖車軸距

避障控制不僅需要考慮拖車后橋中點的位置和姿態，還需考慮其前橋，參照文獻[13]，由式(1)推導得到拖車前橋中點的位置和姿態

(2)

式中xq、yq——拖車前橋中點的橫、縱坐標值

式(1)和式(2)即用于乘用車避障控制的運動學模型，即拖車運動學模型。

對于掛車的運動學模型，由于拖車與掛車的鉸接點距離拖車的后橋中點較近，因此可視為掛車在鉸接點處的速度與拖車后橋中點的速度相同，即

(3)

其中

γ=θt-θg

(4)

式中vg——掛車縱向行駛速度

θg——掛車航向角

γ——鉸接角

lg——掛車車橋與鉸接點之間的距離

求解式(3)可得

(5)

由于掛車也符合非完整約束

(6)

式中xg、yg——掛車車橋中點橫、縱坐標值

求解式(6)可得

(7)

(8)

2 NMPC避障控制器設計

設計NMPC避障控制器包含建立預測模型和設計滾動優化目標函數兩部分。預測模型的作用是根據車輛當前的位姿狀態和未來可能的控制輸入預測未來可能出現的位姿狀態，NMPC避障控制器的預測模型可以通過農用拖掛車的運動學模型推導獲得。

為了便于表達，式(8)抽象為

(9)

其中

控制器的采樣間隔很小，所以可以采用歐拉法(Euler method)離散化式(9)得

(10)

式中T——控制器的采樣間隔

將式(9)代入式(10)可得

X(t+1|t)=X(t|t)+Tf(X(t|t),U(t|t))

(11)

假設預測時域為Np，控制時域為Nc，則在預測時域內拖掛車的位姿狀態為

(12)

若農用拖掛車行駛區域內共有Nz個障礙物點，則可以定義拖車前橋中點、拖車后橋中點和掛車車橋中點在預測時域內的t+i時刻與第j個障礙物點之間的距離為

(13)

式中dqj(t+i|t)——t+i時刻拖車前橋中點與第j個障礙物點之間距離

dhj(t+i|t)——t+i時刻拖車后橋中點與第j個障礙物點之間距離

dgj(t+i|t)——t+i時刻掛車車橋中點與第j個障礙物點之間距離

xzj、yzj——第j個障礙物點的橫、縱坐標值

式(13)即為NMPC避障控制器中農用拖掛車與障礙物間距離的預測模型。

NMPC避障控制器另一個關鍵部分是滾動優化，其優化核心是目標函數。對比文獻[12]提出的禁區懲罰函數、文獻[13]中提出的反比例懲罰函數和文獻[14]中提出的指數懲罰函數可知，禁區懲罰函數具有障礙物邊界劃分較為清晰等優勢，因此本文參照文獻[12]提出的禁區懲罰函數，設計如下適用于農用拖掛車避障控制的優化目標函數，若同時滿足dqj(t+i|t)>Rq+Rz+Ra，dhj(t+i|t)>Rh+Rz+Ra和dgj(t+i|t)>Rg+Rz+Ra，則

Jj(t+i|t)=0

(14)

否則

Jj(t+i|t)=M

(15)

式中Rq、Rh、Rg——臨界半徑，其值等于車橋中心到輪胎外沿的距離

Rz——障礙物半徑

Ra——安全裕度

M——足夠大的正數

此外，為避免求得局部最優解，將式(15)改進為

Jj(t+i|t)=M-dqj(t+i|t)-dhj(t+i|t)-dgj(t+i|t)

(16)

加入文獻[13]中的路徑跟蹤懲罰項Jl，即可得拖掛車避障控制優化目標函數為

(17)

參照文獻[13]乘用車的約束條件，在約束條件下求解

(18)

即可獲得農用拖掛車避障控制所需的控制變量序列

U*=(U(1),U(2),…,U(Np-1))

(19)

其中第一個元素U(1)即農用拖掛車避障控制器在下一時刻輸出的控制變量。

3 仿真驗證

本文提出的基于非線性模型預測控制的農用拖掛車避障控制器通過Matlab/Simulink進行仿真驗證。仿真系統中包含避障控制器和農用拖掛車的計算機模型。由于計算機模型可以直接給出農用拖掛車的位姿狀態信息，所以仿真系統中未包含定位系統和傳感系統。仿真中農用拖掛車和NMPC控制器的參數如表1所示。仿真分為對照仿真和復雜工況仿真。

3.1 對照仿真

在這組仿真中，實驗組為本文提出的農用拖掛車避障控制器，以下簡稱拖掛車避障控制器，對照組控制器則為采用乘用車運動學模型作為預測模型的避障控制器，以下簡稱為拖車避障控制器。在拖車避障控制器中，掛車的位姿狀態未考慮。圖2顯示了實驗組農用拖掛車避障控制器的避障控制結果，圖3則顯示了對照組拖車避障控制器的避障控制結果。圖2a和圖3a為仿真結果的整體展示，由圖可知，拖掛車避障控制器和拖車避障控制器都對障礙物做出了避讓控制，且避障結束后，被控的農用拖掛車又回到了給定行駛路徑。圖2b和圖3b則為仿真結果的局部放大，由圖可知，在本文提出的拖掛車避障控制器的控制下，農用拖掛車的拖車和掛車都實現了避障；而在拖車避障控制器的控制下，農用拖掛車的拖車避開了障礙物，掛車則與障礙物發生了碰撞。

表1 農用拖掛車及NMPC控制器參數Tab.1 Parameters of agricultural tractor-trailer and parameters of NMPC controller

圖2 實驗組農用拖掛車避障控制器的避障控制仿真結果Fig.2 Simulation results of obstacle avoidance control based on tractor-trailer obstacle avoidance controller of experimental group

圖3 對照組拖車避障控制器的避障控制仿真結果Fig.3 Simulation results of obstacle avoidance control based on tractor obstacle avoidance controller of control group

圖4為避障控制過程中的控制變量變化曲線。由圖4可知，在仿真過程中，拖掛車避障控制器和拖車避障控制器的控制變量變化都較為平緩，符合農用拖掛車的系統約束。此外，由圖4b可知，由于未考慮掛車的位姿狀態，拖車控制器在農用拖掛車的拖車駛過障礙物(41,19.2)m后，即控制拖掛車向給定行駛路徑靠攏，因此掛車與拖車的內輪差導致掛車與障礙物發生了碰撞。

表2為農用拖掛車的關鍵位置與障礙物中心的最小距離，在拖掛車避障控制器的控制下，農用拖掛車各橋端點軌跡與各障礙物中心的最小距離為0.550 5 m，減去障礙物半徑與安全裕度之和0.550 0 m后大于0 m。在拖車避障控制器的控制下，掛車車橋端點軌跡與障礙物中心(41,19.2)m的最小距離為0.490 5 m，減去障礙物半徑0.500 0 m后小于0 m，掛車車橋右端與障礙物發生了碰撞。

圖4 對照仿真的控制變量Fig.4 Control variable of comparison simulation

表2 對照仿真中農用拖掛車的關鍵位置與

3.2 復雜工況仿真

為了進一步測試本文提出的拖掛車避障控制器，在農用拖掛車的給定行駛路徑上布置了更多障礙物。由圖5可知，在拖掛車避障控制器的控制下，農用拖掛車的拖車和掛車都順利地實現了避障，并且在完成避障后回到了給定行駛路徑。

由圖6可知，在避障控制中，控制變量符合農用拖掛車的系統約束條件。

圖7為農用拖掛車的拖車后橋與給定形式路徑之間的誤差曲線，此誤差用于表征拖掛車避障控制器在避障后將農用拖掛車與給定行駛路徑之間的誤差收斂能力。由圖7a可知，在完成避障后，農用拖掛車與給定行駛路徑的橫向誤差超調量較小，收斂速度較快，最終收斂為0 m。由圖7b可知，航向誤差的超調量較小，收斂速度較快，最終收斂為0 rad。

圖5 農用拖掛車避障控制仿真結果Fig.5 Simulation results of obstacle avoidance control for agricultural tractor-trailers

圖6 復雜工況仿真的控制變量Fig.6 Control variable of simulation of complex condition

圖7 拖車后橋中心與給定行駛路徑之間的誤差Fig.7 Errors between center of rear axle of tractor and given path

表3中，農用拖掛車各橋端點軌跡與障礙物中心的最小距離為0.550 0 m，減去障礙物半徑與安全裕度之和0.550 0 m等于0 m。

4 結論

(1)為實現農用拖掛車的避障控制，考慮到農用拖掛車與乘用車不同的車身結構，基于剛體運動學和非完整約束條件推導了農用拖掛車運動學模型，該模型能夠同時表達拖車和掛車的位姿狀態。

表3 復雜工況仿真中農用拖掛車的關鍵位置與 障礙物中心的最小距離Tab.3 Minimum distance between key position of agricultural tractor-trailer and center of obstacle in simulation of complex condition m

(2)基于運動學模型推導了農用拖掛車位姿狀態預測模型，基于改進的禁區懲罰函數設計了優化目標函數，完成了基于非線性模型預測控制的農用拖掛車避障控制器的設計。仿真結果表明，該控制器能夠在較復雜的工況下實現農用拖掛車的避障控制，農用拖掛車各車橋端點軌跡與障礙物中心的最小距離減去障礙物半徑與安全裕度之和的結果均大于或等于0 m。

(3)對照仿真結果表明，不考慮掛車位姿狀態的拖車避障控制器控制下的農用拖掛車與障礙物發生了碰撞，掛車車橋端點軌跡與障礙物中心的最小距離減去障礙物半徑與安全裕度之和的結果小于0 m。通過對比，證明了本文提出的農用拖掛車避障控制器的有效性。

(4)完成避障控制后，本文提出的農用拖掛車避障控制器對路徑跟蹤性能的影響較小。仿真結果中，農用拖掛車與給定行駛路徑之間的橫向誤差和航向誤差均收斂為零，且收斂速度較快。