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

崔 振，翟陸陽，趙志強

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

崔 振，翟陸陽，趙志強

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

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

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

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

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

1 車輛運動學建模

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

圖1 運動學模型

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

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

此時車輛的運動學模型為

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

1.1 預測模型

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

參考軌跡可以表示為

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

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

式（7）減去式（6）得

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

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

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

經過推導，得到系統(tǒng)的預測輸出表達式：

=()+Δ（12）

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

1.2 目標函數

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

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

1.3 約束條件

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

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

=0,1,?,c-1

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

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

將式（13）中的目標函數轉為標準二次型形式，得到如下帶有約束條件的公式：

s.t.

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

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

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

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

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

2 車輛運動學模型驗證

為了驗證式（4）所得到的運動學模型的有效性，在Simulink里搭建車輛的運動學模型，在相同的速度和前輪轉角輸入下，與Carsim中的車輛模型輸出結果進行比較。

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

圖2 仿真結果

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

3 MPC算法仿真驗證

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

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

3.1 直線軌跡跟蹤

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

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

圖3 參考軌跡與實際軌跡

3.2 圓形軌跡跟蹤

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

圖4 參考軌跡與實際軌跡

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

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

4 結論

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

[1] 呂文杰,馬戎,李歲勞,等.基于純追蹤模型的路徑跟蹤改進算法[J].測控技術,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] 高興泉,李洪濤,王瑞雪,等.輪式移動機器人的滾動LQR軌跡跟蹤控制[J].吉林化工學院學報,2008(2): 38-41.

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

[6] 席裕庚.預測控制[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] 梁忠超,張歡,趙晶,等.基于自適應MPC的無人駕駛車輛軌跡跟蹤控制[J].東北大學學報(自然科學版), 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—），男，工程師，研究方向為智能網聯汽車，E-mail: zhen_cui@126.com。