基于非線性模型預(yù)測控制的農(nóng)用拖掛車避障控制器研究

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

(北京科技大學(xué)機(jī)械工程學(xué)院， 北京 100083)

0 引言

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

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

考慮到農(nóng)用拖掛車結(jié)構(gòu)[15-18]不同于文獻(xiàn)[9-14]中的乘用車和無人機(jī)，而不同的結(jié)構(gòu)產(chǎn)生的內(nèi)輪差也不相同[19]，本文基于剛體運動學(xué)和非完整約束條件建立用于避障控制的農(nóng)用拖掛車運動學(xué)模型。在此基礎(chǔ)上，提出農(nóng)用拖掛車的狀態(tài)預(yù)測模型，并基于改進(jìn)的禁區(qū)懲罰函數(shù)設(shè)計優(yōu)化目標(biāo)函數(shù)，從而提出基于非線性模型預(yù)測控制(Non-linear model predictive control, NMPC)的農(nóng)用拖掛車避障控制方法。通過Matlab/Simulink對提出的避障控制方法進(jìn)行仿真驗證。

1 運動學(xué)模型建立

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

由于拖車的結(jié)構(gòu)與乘用車的結(jié)構(gòu)基本相同，因此拖車的運動學(xué)模型可參照文獻(xiàn)[13，22]中的乘用車運動學(xué)模型

(1)

式中xh、yh——拖車后橋中點的橫、縱坐標(biāo)值

θt——拖車航向角

δ——拖車轉(zhuǎn)向角

vt——拖車縱向行駛速度

lt——拖車軸距

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

(2)

式中xq、yq——拖車前橋中點的橫、縱坐標(biāo)值

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

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

(3)

其中

γ=θt-θg

(4)

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

θg——掛車航向角

γ——鉸接角

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

求解式(3)可得

(5)

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

(6)

式中xg、yg——掛車車橋中點橫、縱坐標(biāo)值

求解式(6)可得

(7)

(8)

2 NMPC避障控制器設(shè)計

設(shè)計NMPC避障控制器包含建立預(yù)測模型和設(shè)計滾動優(yōu)化目標(biāo)函數(shù)兩部分。預(yù)測模型的作用是根據(jù)車輛當(dāng)前的位姿狀態(tài)和未來可能的控制輸入預(yù)測未來可能出現(xiàn)的位姿狀態(tài)，NMPC避障控制器的預(yù)測模型可以通過農(nóng)用拖掛車的運動學(xué)模型推導(dǎo)獲得。

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

(9)

其中

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

(10)

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

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

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

(11)

假設(shè)預(yù)測時域為Np，控制時域為Nc，則在預(yù)測時域內(nèi)拖掛車的位姿狀態(tài)為

(12)

若農(nóng)用拖掛車行駛區(qū)域內(nèi)共有Nz個障礙物點，則可以定義拖車前橋中點、拖車后橋中點和掛車車橋中點在預(yù)測時域內(nèi)的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個障礙物點的橫、縱坐標(biāo)值

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

NMPC避障控制器另一個關(guān)鍵部分是滾動優(yōu)化，其優(yōu)化核心是目標(biāo)函數(shù)。對比文獻(xiàn)[12]提出的禁區(qū)懲罰函數(shù)、文獻(xiàn)[13]中提出的反比例懲罰函數(shù)和文獻(xiàn)[14]中提出的指數(shù)懲罰函數(shù)可知，禁區(qū)懲罰函數(shù)具有障礙物邊界劃分較為清晰等優(yōu)勢，因此本文參照文獻(xiàn)[12]提出的禁區(qū)懲罰函數(shù)，設(shè)計如下適用于農(nóng)用拖掛車避障控制的優(yōu)化目標(biāo)函數(shù)，若同時滿足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——足夠大的正數(shù)

此外，為避免求得局部最優(yōu)解，將式(15)改進(jìn)為

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

(16)

加入文獻(xiàn)[13]中的路徑跟蹤懲罰項Jl，即可得拖掛車避障控制優(yōu)化目標(biāo)函數(shù)為

(17)

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

(18)

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

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

(19)

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

3 仿真驗證

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

3.1 對照仿真

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

表1 農(nóng)用拖掛車及NMPC控制器參數(shù)Tab.1 Parameters of agricultural tractor-trailer and parameters of NMPC controller

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

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

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

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

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

表2 對照仿真中農(nóng)用拖掛車的關(guān)鍵位置與

3.2 復(fù)雜工況仿真

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

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

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

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

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

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

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

4 結(jié)論

(1)為實現(xiàn)農(nóng)用拖掛車的避障控制，考慮到農(nóng)用拖掛車與乘用車不同的車身結(jié)構(gòu)，基于剛體運動學(xué)和非完整約束條件推導(dǎo)了農(nóng)用拖掛車運動學(xué)模型，該模型能夠同時表達(dá)拖車和掛車的位姿狀態(tài)。

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

(2)基于運動學(xué)模型推導(dǎo)了農(nóng)用拖掛車位姿狀態(tài)預(yù)測模型，基于改進(jìn)的禁區(qū)懲罰函數(shù)設(shè)計了優(yōu)化目標(biāo)函數(shù)，完成了基于非線性模型預(yù)測控制的農(nóng)用拖掛車避障控制器的設(shè)計。仿真結(jié)果表明，該控制器能夠在較復(fù)雜的工況下實現(xiàn)農(nóng)用拖掛車的避障控制，農(nóng)用拖掛車各車橋端點軌跡與障礙物中心的最小距離減去障礙物半徑與安全裕度之和的結(jié)果均大于或等于0 m。

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

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