基于MPC的無人駕駛車輛軌跡跟蹤算法

崔 振，翟陸陽，趙志強(qiáng)

基于MPC的無人駕駛車輛軌跡跟蹤算法

崔 振，翟陸陽，趙志強(qiáng)

（徐州徐工汽車制造有限公司 技術(shù)中心，江蘇 徐州 221000）

基于三自由度車輛運(yùn)動學(xué)模型，為前輪轉(zhuǎn)向的無人駕駛車輛設(shè)計(jì)了一種軌跡跟蹤控制算法，建立了車輛軌跡跟蹤狀態(tài)方程，采用模型預(yù)測控制算法，通過滾動優(yōu)化和反饋校正對無人駕駛車輛實(shí)現(xiàn)了最優(yōu)控制。為了驗(yàn)證運(yùn)動學(xué)模型和該算法的有效性，對運(yùn)動學(xué)模型進(jìn)行仿真分析，并建立Carsim-Simulink聯(lián)合仿真平臺，分別對直線軌跡和圓形軌跡進(jìn)行仿真實(shí)驗(yàn)。仿真結(jié)果表明，該運(yùn)動學(xué)模型和算法能夠有效進(jìn)行無人駕駛的軌跡跟蹤。

無人駕駛車輛；運(yùn)動學(xué)模型；模型預(yù)測控制；軌跡跟蹤；滾動優(yōu)化；反饋校正；Carsim- Simulink聯(lián)合仿真平臺

隨著人們對汽車要求的不斷提高，智能化、網(wǎng)聯(lián)化成為目前汽車發(fā)展的重要方向，代表未來的智能網(wǎng)聯(lián)汽車能夠提高駕駛安全性和避免交通擁堵。

軌跡跟蹤控制是無人駕駛研究領(lǐng)域的關(guān)鍵技術(shù)之一，控制算法主要包括純跟蹤算法PID（Pro- portion, Integration, Differentiation）控制、線性二次型調(diào)節(jié)器（Linear Quadratic Regulator, LQR）、模型預(yù)測控制（Model Predictive Control, MPC）等。純跟蹤算法因?yàn)楹唵我子?，已?jīng)用于無人駕駛車輛多年[1]，但其前視距離的最優(yōu)值難以獲得。早期的學(xué)者在傳統(tǒng)PID的基礎(chǔ)上做了改進(jìn)[2-3]，改進(jìn)的PID控制器雖然可以較好地控制車輛跟蹤期望軌跡，但其控制參數(shù)的獲取需要通過大量實(shí)驗(yàn)進(jìn)行修正，而且不能實(shí)現(xiàn)最優(yōu)控制。LQR通過將非線性控制系統(tǒng)線性化，并利用該線性模型求解LQR問題得到最優(yōu)解[4]，但LQR無法處理多約束問題，限制了其使用。因此，本文基于模型預(yù)測控制算法，對無人駕駛車輛在給定軌跡情況下的循跡效果進(jìn)行研究。

1 車輛運(yùn)動學(xué)建模

無人駕駛車輛在低速行駛時(shí)，輪胎的側(cè)偏角較小，不考慮動力學(xué)問題，此時(shí)可以使用三自由度的單車模型作為無人駕駛汽車的模型[5]。

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

如圖1所示，坐標(biāo)系--為固定在地面的慣性坐標(biāo)系，(,)為車輛后軸中心坐標(biāo)，為前輪轉(zhuǎn)角；為橫擺角；為車輛后軸中心速度；為車輛軸距。

所以可以得到車輛的橫擺角速度為

此時(shí)車輛的運(yùn)動學(xué)模型為

模型預(yù)測控制算法具有預(yù)測模型、滾動優(yōu)化和反饋校正幾個(gè)特征[6]，首先建立能夠預(yù)測系統(tǒng)未來狀態(tài)的預(yù)測模型；其次是建立適當(dāng)?shù)哪繕?biāo)函數(shù)，并綜合考慮車輛行駛的穩(wěn)定和迅速的要求，建立合適的約束條件，在每個(gè)采樣時(shí)刻計(jì)算出有限時(shí)段的最優(yōu)控制序列，實(shí)現(xiàn)系統(tǒng)的滾動優(yōu)化；最后將最優(yōu)控制序列中的第一個(gè)控制量作為輸入，實(shí)現(xiàn)反饋校正功能。

1.1 預(yù)測模型

由式（4）可知，系統(tǒng)的狀態(tài)量為=[,,]T，控制量為=[,]T，因此，連續(xù)狀態(tài)方程為

參考軌跡可以表示為

式中，r=[rrr]T，r=[rr]T。

將式（5）在參考軌跡點(diǎn)泰勒展開，只保一階項(xiàng)并忽略高階項(xiàng)，得到線性化的無人駕駛車輛誤差模型：

式（7）減去式（6）得

對式（8）進(jìn)行離散化，得到離散形式的狀態(tài)空間表達(dá)式：

在控制器設(shè)計(jì)過程中，為防止系統(tǒng)的控制量發(fā)生突變，對式（9）做如下轉(zhuǎn)換，將其中的控制量轉(zhuǎn)化為控制增量形式：

得到一個(gè)新的狀態(tài)空間表達(dá)式：

經(jīng)過推導(dǎo)，得到系統(tǒng)的預(yù)測輸出表達(dá)式：

=()+Δ（12）

由此，可以通過當(dāng)前的狀態(tài)量()和控制增量Δ，預(yù)測系統(tǒng)未來的狀態(tài)變量和輸出變量。

1.2 目標(biāo)函數(shù)

系統(tǒng)的控制增量無法直接測量得到，因此，需要建立優(yōu)化目標(biāo)函數(shù)進(jìn)行求解，參考文獻(xiàn)[7]中的方法使用下面的目標(biāo)函數(shù)：

式中，p為系統(tǒng)的預(yù)測時(shí)域；c為系統(tǒng)的控制時(shí)域；實(shí)時(shí)變化的系統(tǒng)為了避免目標(biāo)函數(shù)無解，設(shè)置了松弛因子，同時(shí)加入權(quán)重系數(shù)對目標(biāo)函數(shù)進(jìn)行軟化處理[8]。

1.3 約束條件

在控制過程中，需要滿足控制量和控制增量的約束，表達(dá)形式為

min(+)≤(+)≤max(+),

=0,1,?,c-1

Δmin(+)≤Δ(+)≤Δmax(+),

=0,1,?,c-1 （14）

將式（13）中的目標(biāo)函數(shù)轉(zhuǎn)為標(biāo)準(zhǔn)二次型形式，得到如下帶有約束條件的公式：

s.t.

至此，連續(xù)系統(tǒng)的模型預(yù)測控制問題轉(zhuǎn)化為最優(yōu)規(guī)劃問題。

在每個(gè)控制周期對式（15）完成最優(yōu)求解后，得到控制時(shí)域內(nèi)的一系列控制輸入增量：

將計(jì)算得到的第一個(gè)控制增量作用于系統(tǒng)：

()=(-1)+Δt*（17）

系統(tǒng)根據(jù)狀態(tài)信息得到新的控制增量序列，通過對控制過程進(jìn)行周期性重復(fù)，完成對期望軌跡的跟蹤。

2 車輛運(yùn)動學(xué)模型驗(yàn)證

為了驗(yàn)證式（4）所得到的運(yùn)動學(xué)模型的有效性，在Simulink里搭建車輛的運(yùn)動學(xué)模型，在相同的速度和前輪轉(zhuǎn)角輸入下，與Carsim中的車輛模型輸出結(jié)果進(jìn)行比較。

Simulink和Carsim中的運(yùn)動學(xué)模型的速度輸入恒為10 m/s，車輛的初始位置和航向角均為0，車輛的前輪轉(zhuǎn)角輸入如圖2(a)所示，得到的車輛位置和航向角輸出如圖2(b)和圖2(c)所示。

圖2 仿真結(jié)果

從圖2可以看出，在相同的車速和前輪轉(zhuǎn)角輸入下，Simulink中搭建的運(yùn)動學(xué)模型輸出與Carsim的輸出結(jié)果十分接近，證明所建立的運(yùn)動學(xué)模型與車輛的實(shí)際情況非常接近。

3 MPC算法仿真驗(yàn)證

為了驗(yàn)證模型預(yù)測控制算法，在 Carsim和Simulink中進(jìn)行聯(lián)合仿真，其過程為在Carsim里對車輛參數(shù)進(jìn)行設(shè)置，得到精確的車輛模型，并將其發(fā)送到Simulink里，最后根據(jù)上文設(shè)計(jì)的控制器建立Simulink模型。

為了驗(yàn)證設(shè)計(jì)的跟蹤控制器在低車速下對參考軌跡的跟蹤能力，選取直線和圓形兩種工況對其進(jìn)行仿真測試。

3.1 直線軌跡跟蹤

直線軌跡的參考車速設(shè)置為5 m/s和10 m/s，初始位置都為（0,0），航向角為0°，初始時(shí)間為0 s，采樣時(shí)間=0.05 s，參考軌跡為=5的直線。

仿真結(jié)果如圖3所示，可以看出，在車輛具有初始偏差的初始條件下，以兩種不同的速度行駛均能迅速跟蹤直線軌跡。

圖3 參考軌跡與實(shí)際軌跡

3.2 圓形軌跡跟蹤

仿真過程依然選取速度分別為5 m/s和10 m/s，車輛的初始前輪轉(zhuǎn)角設(shè)置為5.96°，參考軌跡為半徑為25 m的圓形，如圖4所示。

圖4 參考軌跡與實(shí)際軌跡

從圖4可以看出，在具有初始偏差的情況下，兩種不同參考速度下，控制器都能夠很好地控制車輛對圓形軌跡進(jìn)行跟蹤。

聯(lián)合仿真結(jié)果表明，本文所研究的MPC算法是有效的，在低速時(shí)具有良好的魯棒性和跟蹤期望軌跡的能力。

4 結(jié)論

本文首先建立了三自由度的車輛運(yùn)動學(xué)模型，并在Carsim和Simulink中對其進(jìn)行仿真分析，驗(yàn)證了所建立的車輛運(yùn)動學(xué)模型的準(zhǔn)確性。對車輛的非線性模型進(jìn)行線性化和離散化處理，推導(dǎo)出線性時(shí)變模型，將設(shè)計(jì)的控制器在Carsim和Simulink中對直線和圓形軌跡進(jìn)行仿真驗(yàn)證，結(jié)果說明，設(shè)計(jì)的MPC控制器能夠較好地跟蹤期望軌跡。仿真結(jié)果顯示，設(shè)計(jì)的MPC控制器能夠在低速下迅速、穩(wěn)定地對期望軌跡進(jìn)行跟蹤，對于無人駕駛車輛軌跡跟蹤控制的研究具有一定的參考價(jià)值。

[1] 呂文杰,馬戎,李歲勞,等.基于純追蹤模型的路徑跟蹤改進(jìn)算法[J].測控技術(shù),2011,30(7):93-96,100.

[2] 趙熙俊,陳慧巖.智能車輛路徑跟蹤橫向控制方法的研究[J].汽車工程,2011,33(5):382-387.

[3] DONG E,GUO S,LIN X,et al.A Neural Network- based Self-tuning PID Controller of an Autonomous Underwater Vehicle[C]// International Conference on Mechatronics and Automation.Piscataway:IEEE, 2012: 898-903.

[4] 高興泉,李洪濤,王瑞雪,等.輪式移動機(jī)器人的滾動LQR軌跡跟蹤控制[J].吉林化工學(xué)院學(xué)報(bào),2008(2): 38-41.

[5] 龔建偉,姜巖,徐威.無人駕駛車輛模型預(yù)測控制[M]. 北京:北京理工大學(xué)出版社,2014.

[6] 席裕庚.預(yù)測控制[M].北京:國防工業(yè)出版社,2013.

[7] KüHNE F,LAGES W.F,SILVA J M.Model Predivtive Control of a Mobile Robot Using Linearization [J]. Prodeedings of Mechatronics and Robototics,2004(4): 525-530.

[8] 梁忠超,張歡,趙晶,等.基于自適應(yīng)MPC的無人駕駛車輛軌跡跟蹤控制[J].東北大學(xué)學(xué)報(bào)(自然科學(xué)版), 2020,41(6):835-840.

Driverless Vehicle Trajectory Tracking Algorithm Based on MPC

CUI Zhen, ZHAI Luyang, ZHAO Zhiqiang

( Technical Center, Xuzhou XCMG Automobile Manufacturing Company Limited, Xuzhou 221000, China )

Based on the kinematics model of three-degree-of-freedom vehicle, a trajectory tracking control algorithm was designed for the driverless vehicle with front-wheel steering, and the vehicle trajectory tracking state equations were established. The model predictive control algorithm was used to achieve optimal control of the driverless vehicle through roll optimization and feedback correction. In order to verify the effectiveness of the kinematics model and the algorithm, the kinematics model was simulated and analyzed, and a Carsim-Simulink co-simulation platform was established to conduct simulation experiments on linear trajectories and circular trajectories respectively. The simulation results showed that the kinematics model and the algorithm can effectively track the trajectory of the driverless vehicle.

Driverless vehicle; Kinematics model; Model predictive control; Trajectory tracking;Roll optimization; Feed back correction; Carsim-Simulink co-simulation platform

U461.1

A

1671-7988(2022)21-43-04

U461.1

A

1671-7988(2022)21-43-04

10.16638/j.cnki.1671-7988.2022.021.008

崔振（1988—），男，工程師，研究方向?yàn)橹悄芫W(wǎng)聯(lián)汽車，E-mail: zhen_cui@126.com。