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

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

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

0 引言

農(nóng)用機(jī)械自動導(dǎo)航控制是實(shí)現(xiàn)農(nóng)業(yè)現(xiàn)代化的一項(xiàng)關(guān)鍵技術(shù)[1-3]。避障是指移動裝備在跟隨給定路徑時，識別障礙物并完成避障動作[4]，它關(guān)系到農(nóng)用機(jī)械自主導(dǎo)航控制的安全性，因而具有十分重要的研究意義。實(shí)現(xiàn)避障功能需要兩項(xiàng)關(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ī)劃算法實(shí)現(xiàn)避障控制，另一種則基于模型預(yù)測控制(Model predictive control, MPC)方法建立避障控制器。常用于避障控制的路徑規(guī)劃算法有A*算法[5]、神經(jīng)網(wǎng)絡(luò)算法[6]、人工勢場法[7-8]等，由于路徑規(guī)劃算法通常將移動裝備視為質(zhì)點(diǎn)，未考慮執(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]，本文基于剛體運(yùn)動學(xué)和非完整約束條件建立用于避障控制的農(nóng)用拖掛車運(yùn)動學(xué)模型。在此基礎(chǔ)上，提出農(nóng)用拖掛車的狀態(tài)預(yù)測模型，并基于改進(jìn)的禁區(qū)懲罰函數(shù)設(shè)計(jì)優(yōu)化目標(biāo)函數(shù)，從而提出基于非線性模型預(yù)測控制(Non-linear model predictive control, NMPC)的農(nóng)用拖掛車避障控制方法。通過Matlab/Simulink對提出的避障控制方法進(jìn)行仿真驗(yàn)證。

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

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

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

(1)

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

θt——拖車航向角

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

vt——拖車縱向行駛速度

lt——拖車軸距

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

(2)

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

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

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

(3)

其中

γ=θt-θg

(4)

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

θg——掛車航向角

γ——鉸接角

lg——掛車車橋與鉸接點(diǎn)之間的距離

求解式(3)可得

(5)

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

(6)

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

求解式(6)可得

(7)

(8)

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

設(shè)計(jì)NMPC避障控制器包含建立預(yù)測模型和設(shè)計(jì)滾動優(yōu)化目標(biāo)函數(shù)兩部分。預(yù)測模型的作用是根據(jù)車輛當(dāng)前的位姿狀態(tài)和未來可能的控制輸入預(yù)測未來可能出現(xiàn)的位姿狀態(tài)，NMPC避障控制器的預(yù)測模型可以通過農(nóng)用拖掛車的運(yùn)動學(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ù)測時域?yàn)镹p，控制時域?yàn)镹c，則在預(yù)測時域內(nèi)拖掛車的位姿狀態(tài)為

(12)

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

(13)

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

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

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

xzj、yzj——第j個障礙物點(diǎn)的橫、縱坐標(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è)計(jì)如下適用于農(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]中的路徑跟蹤懲罰項(xiàng)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)證

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

3.1 對照仿真

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

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

圖2 實(shí)驗(yàn)組農(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)用拖掛車各橋端點(diǎn)軌跡與各障礙物中心的最小距離為0.550 5 m，減去障礙物半徑與安全裕度之和0.550 0 m后大于0 m。在拖車避障控制器的控制下，掛車車橋端點(diǎn)軌跡與障礙物中心(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)用拖掛車的拖車和掛車都順利地實(shí)現(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)用拖掛車各橋端點(diǎn)軌跡與障礙物中心的最小距離為0.550 0 m，減去障礙物半徑與安全裕度之和0.550 0 m等于0 m。

4 結(jié)論

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

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

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