基于自適應(yīng)模型預(yù)測(cè)的智能汽車軌跡跟蹤控制研究

路宏廣 聶小芮 顧凱峰

（重慶交通大學(xué) 機(jī)電與車輛工程學(xué)院，重慶400074）

主題詞：軌跡跟蹤控制 自適應(yīng)控制 模型預(yù)測(cè)控制 智能駕駛

縮略語(yǔ)

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 前言

智能車輛的縱、橫向跟蹤控制技術(shù)是自動(dòng)駕駛技術(shù)的基礎(chǔ)，也是自動(dòng)駕駛領(lǐng)域的重點(diǎn)和難點(diǎn)，跟蹤控制技術(shù)的好壞直接決定智能車輛的性能[1-3]。

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

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

2 軌跡跟蹤車輛動(dòng)力學(xué)模型

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

圖1 軌跡跟蹤誤差示意

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

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

可以得到縱向跟蹤誤差的表達(dá)式：

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

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

式中，

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

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

式中，

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

其中，cf、cr分別為前后輪的側(cè)偏角剛度；m車身質(zhì)量；lr、lf分別為前后軸到質(zhì)心的距離；Iz為車身繞z軸旋轉(zhuǎn)的轉(zhuǎn)動(dòng)慣量。

3 軌跡跟蹤控制器設(shè)計(jì)

3.1 預(yù)測(cè)模型

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

從調(diào)度參數(shù)取值范圍中選取有限個(gè)點(diǎn)組成等間距的隊(duì)列：

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

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

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

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

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

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

式中，k代表當(dāng)前時(shí)刻；

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

3.2 滾動(dòng)優(yōu)化

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

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

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

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

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

式中，

Q、R、RΔ、E、L、M由式（9）、（11）推導(dǎo)得出。

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

4 仿真驗(yàn)證

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

4.1 車道跟隨

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

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

圖2 S形道路

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

圖3 縱向跟蹤結(jié)果

圖4 縱向跟蹤誤差像

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

圖5 橫向跟蹤結(jié)果

圖6 橫向跟蹤誤差

4.2 換道軌跡跟蹤

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

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

圖7 規(guī)劃軌跡

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

圖8 縱向跟蹤結(jié)果

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

圖9 橫向跟蹤結(jié)果

圖10 橫向跟蹤誤差

5 結(jié)論

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

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

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