基于自適應模型預測的智能汽車軌跡跟蹤控制研究

路宏廣 聶小芮 顧凱峰

（重慶交通大學 機電與車輛工程學院，重慶400074）

主題詞：軌跡跟蹤控制 自適應控制 模型預測控制 智能駕駛

縮略語

MPC Model Predictive Control

ACC Adaptive Cruise Control

NMPC Nonlinear Model Predictive Control

LPV Linear Parameter-Varying

LTI Linear and Time-invariant System

LKA Local Key-Account

1 前言

智能車輛的縱、橫向跟蹤控制技術是自動駕駛技術的基礎，也是自動駕駛領域的重點和難點，跟蹤控制技術的好壞直接決定智能車輛的性能[1-3]。

國內外學者對此進行了廣泛且深入的研究。文獻[3-5]針對縱向跟蹤問題，采用模型預測控制理論（Model Predictive Control，MPC）設計了綜合考慮車輛多性能指標的自適應巡航控制算法（Adaptive Cruise Control,ACC），并研究了解決控制算法低魯棒性和非可行解的問題，提出了改進方法。文獻[6-7]針對新能源車輛的縱向跟蹤問題，提出了一種改進的ACC控制算法，運用非線性模型預測控制理論(Nonlinear Model Predictive Control,NMPC)實現(xiàn)了車速的優(yōu)化控制，利用仿真及硬件在環(huán)驗證了控制器的時效性。文獻[8-9]針對橫向跟蹤問題，考慮跟蹤精確性和行駛穩(wěn)定性約束，提出了具有良好穩(wěn)定性能的路徑跟蹤控制器。文獻[10]針對路徑跟蹤預瞄控制方法的普適性問題，采用粒子群優(yōu)化算法（Particle Swarm Optimization,PSO）對預瞄距離進行自適應尋優(yōu)，仿真和硬件在環(huán)測試表明自適應預瞄距離能夠滿足不同工況下的控制要求。文獻[11]針對路徑跟蹤控制問題，綜合純跟蹤算法與多點預瞄模型，將預瞄偏差轉化為前輪轉角控制量，并通過試驗驗證了控制算法的有效性。

上述研究多為分散式控制，即通過對縱、橫向運動進行解耦，把軌跡跟蹤問題分解為縱向運動控制和橫向路徑跟蹤問題。由于車輛的縱、橫向狀態(tài)存在強非線性和強耦合性，當極限工況下車輛的非線性特性和動力學耦合增強時，分散式控制難以保證良好的跟蹤性能。本文從系統(tǒng)整體角度出發(fā)解決軌跡跟蹤控制問題，充分地考慮縱、橫向運動控制間的耦合特性，設計了基于自適應模型預測控制理論的軌跡跟蹤控制器，通過主動控制前輪轉角和加速度實現(xiàn)車輛的橫向和縱向控制。考慮了車輛運動學耦合特性的影響，以縱向速度作為調度參數(shù)，引入車輛線性變參數(shù)系統(tǒng)模型(Linear Parameter-Varying,LPV)，通過對控制器內部預測模型參數(shù)化處理，更新控制器的狀態(tài)來適應車輛行駛環(huán)境變化。分析預測時域內的約束條件和代價函數(shù)，將跟蹤控制問題轉化為了預測時域內的二次規(guī)劃問題，獲取控制器的輸入，從而實現(xiàn)車輛的軌跡跟蹤。

2 軌跡跟蹤車輛動力學模型

針對彎曲道路上對前車的軌跡跟蹤控制問題，考慮對橫向位姿跟蹤和縱向速度和距離跟蹤。如圖1所示，前車沿彎曲道路中心線行駛，曲線o2o3為前車的行駛軌跡，ve、vl為自車和前車車輛的質心速度，srea、sdes、Δs為frenet 坐標系so2d 下的車間實際距離、期望距離和距離誤差；Δy、Δψ、為車身坐標系xo1y下的橫向位置誤差和航向角誤差。

圖1 軌跡跟蹤誤差示意

縱向跟蹤分為對車間期望距離的跟蹤和設定速度的跟蹤，車間期望距離模型如下：

式中，τe為車間時距；s0為車輛靜止時的距離；ve為自車質心速度。

可以得到縱向跟蹤誤差的表達式：

式中，vr為引導車速度vl或設定速度vset；srea為車間實際距離；sdes為車間期望距離。當未檢測到引導車輛時，即對實時規(guī)劃軌跡跟蹤時，srea=sdes。

縱向跟蹤的狀態(tài)空間表示如下：

式中，

其中，Δs為距離跟蹤誤差；Δv為速度跟蹤誤差；ae為自車加速度；ul為控制輸入（自車期望加速度）；ωl為可測干擾（前車加速度）。

橫向跟蹤包括橫向位置和方向角的跟蹤，對于已知的期望路徑，橫向跟蹤的狀態(tài)空間表示如下：

式中，

其中，Δy為橫向位置誤差；Δψ為航向角誤差；ur為控制輸入（前輪轉角δ）；ωr為可測干擾（期望橫擺角速度ψdes=ρve，ρ為期望軌跡曲率）。

其中，cf、cr分別為前后輪的側偏角剛度；m車身質量；lr、lf分別為前后軸到質心的距離；Iz為車身繞z軸旋轉的轉動慣量。

3 軌跡跟蹤控制器設計

3.1 預測模型

式(4)為線性時不變系統(tǒng)(LTI)，未能考慮縱、橫向耦合特性，因此為考慮縱向速度變化，采用線性變參數(shù)系統(tǒng)(LPV)對控制問題進行描述，并把縱向速度變化引入預測模型中，不斷更新狀態(tài)工作點，對系統(tǒng)模型進行線性化。把縱向速度作為調度參數(shù)(Scheduling parameters)，LTI模型中的常數(shù)狀態(tài)空間矩陣Ar、Gr轉化為參數(shù)化狀態(tài)空間矩陣Ar(ve)、Gr(ve)，即a22、a24、a42、a44、g21、g21會跟隨調度參數(shù)的變化而變化。

從調度參數(shù)取值范圍中選取有限個點組成等間距的隊列：

式中，vemin、vemax分別為車輛速度的下、上限，n為調度空間中選取的點的數(shù)量，在選取的每一個節(jié)點上，建立對應的LTI模型：

式(6)代表了n 個LTI 模型，用以表示在節(jié)點附近局部范圍內的狀態(tài)變化，并通過相鄰點處的線性插值來獲得調度參數(shù)節(jié)點之間的調度位置狀態(tài)矩陣，以此獲得模型的狀態(tài)變化。例如，當k時刻的調度參數(shù)滿足如下條件：

k時刻狀態(tài)矩陣Ar即為：

綜上，即可用式(6)的LTI 模型的插值隊列來表示橫向跟蹤過程中速度發(fā)生變化時的LPV 模型。在線性控制過程中，調度參數(shù)ve可以由實時的控制輸入和被控對象狀態(tài)得到：

式中，Ts為仿真步長。根據(jù)調度參數(shù)的實時反饋，實現(xiàn)狀態(tài)空間矩陣Ar(ve)、Gr(ve)的自適應變化。

聯(lián)合式(3)、式(6)建立軌跡跟蹤模型，并以Ts作為離散步長得：

式中，k代表當前時刻；

控制目標為y=[0 0 al0 0 0 0]T，控制輸入為ae和δ，和al為干擾輸入。

3.2 滾動優(yōu)化

在軌跡跟蹤過程中，模型的狀態(tài)量、控制量及其變化率需要滿足如下的約束條件:

式中，vmax為最高車速；amax、amin為車輛加速度的上、下限；W為車道寬度，lw為輪距；u為路面附著系數(shù)；δmax為前輪最大轉向角；m、n分別為預測時域內狀態(tài)量和控制量受約束的時域長度，以p作為預測時域，q 作為控制時域，預測模型可由離散化狀態(tài)空間模型式(7)得到。為了保證約束效果同時提高運算速度，m、n的取值應滿足：

m、n 取值越小，運算的實時性越好，用于每個循環(huán)預測時域內優(yōu)化計算的代價函數(shù)包括3個部分：跟蹤誤差代價Je、控制量代價Ju和控制量的變化率代價JΔ。

式中，U=[u ( k )…u( k+1)]T，為控制序列；ny、nu分別為輸出量和控制量的維度；wyj、wuj、wΔj分別為每一項的權值；

綜上，即把軌跡跟蹤控制問題轉化為如下的二次規(guī)劃問題：

式中，

Q、R、RΔ、E、L、M由式（9）、（11）推導得出。

在線控制時，對當前預測時域內進行式(13)的優(yōu)化求解，得到最優(yōu)控制序列，運用序列的第一項進行控制，然后向前推進一步，對下一步重復此操作。式(13)同時考慮了橫向跟蹤和縱向跟蹤控制問題，由于縱、橫向狀態(tài)的強耦合關系，引入了LPV模型，把縱向速度對橫向跟蹤的影響轉化為LPV 模型中調度參數(shù)的變化，把縱、橫向跟蹤的高度非線性動力學模型轉化為LTI 模型的插值隊列，這樣既可讓預測模型自適應調度參數(shù)的改變，又可在保證跟蹤效果的同時有效降低計算量。

4 仿真驗證

為驗證軌跡跟蹤控制器的性能，基于MATLAB/Simulink 和MATLAB/Automated Driving Toolbox 搭 建了仿真驗證平臺。分別針對車道跟隨(LKA+ACC)工況和已知換道軌跡跟蹤工況進行仿真驗證。車輛參數(shù)如表1所示。

4.1 車道跟隨

圖2 所示為一條S 形道路，自車跟隨引導車輛從左向右行駛，跟蹤過程中，引導車輛沿道路中心線行駛，同時速度呈正弦曲線變化（圖3），仿真結果如下。

表1 車輛主要參數(shù)

圖2 S形道路

從圖3、圖4 可以看出，對于縱向的速度距離跟蹤，控制器能夠很好的適應，速度誤差控制在±1.5 m/s,縱向跟蹤誤差控制在±1.5 m，既確保了跟蹤性能也保證了行駛安全。

圖3 縱向跟蹤結果

圖4 縱向跟蹤誤差像

從圖5、圖6 可以看出，對于橫向跟蹤，自車的橫向偏移和航向角偏移均控制在很小的范圍內，且控制器能夠很好的自適應自車速度的變化，保證了橫向跟蹤的精確性。

圖5 橫向跟蹤結果

圖6 橫向跟蹤誤差

4.2 換道軌跡跟蹤

參考一條已規(guī)劃好的換道軌跡：

式中，x、y為橫縱坐標；t為時間，已規(guī)劃的總換道時間為4.5 s。

圖7 規(guī)劃軌跡

由式(14)得到如圖8 的一條換道軌跡，其不僅包含幾何路徑信息還包含時間（速度）信息。對其進行跟蹤仿真，結果如如圖8 所示，其中藍色虛線為式(7)規(guī)劃軌跡中包含的期望速度信息，從結果中可以看出控制器能夠基本保證速度跟隨，但存在一定的偏差，是因為在高速狀態(tài)下?lián)Q道時，為了確保安全，控制器會重點考慮橫向跟蹤，同時適當?shù)亟档蛙囁佟?/p>

圖8 縱向跟蹤結果

圖9 、圖10所示為橫向跟蹤結果和誤差，其中橫向軌跡跟蹤誤差被控制在較小的范圍內，表明控制器能夠很好的完成高速狀態(tài)下的軌跡跟蹤控制，確保行駛安全。

圖9 橫向跟蹤結果

圖10 橫向跟蹤誤差

5 結論

（1）本文針對智能車輛的縱、橫向跟蹤控制問題，設計了自適應模型預測軌跡跟蹤控制器。首先為解決縱、橫向控制的強非線性問題，設計了軌跡跟蹤LPV 模型，把縱向速度作為調度參數(shù)，使預測模型的狀態(tài)空間矩陣能夠自適應地改變。

（2）考慮跟蹤過程的跟蹤性能和安全性因素，建立了預測時域內的代價函數(shù)和約束條件，將跟蹤控制問題轉化為每個預測時域內的二次規(guī)劃問題。

（3）仿真驗證了控制器在車道跟蹤和軌跡跟蹤中的有效性，在保持跟蹤精度的同時還有效提高了行駛穩(wěn)定性。