基于MPC 和PID 的無人駕駛車輛路徑跟蹤控制

周義棚，楊威

（201620 上海市 上海工程技術(shù)大學(xué) 機(jī)械與汽車工程學(xué)院）

0 引言

無人駕駛車輛和車輛部件的電氣化是汽車行業(yè)的熱門話題，許多車輛部件已被電動(dòng)裝置取代，無人駕駛技術(shù)有利于提高車輛的通行效率和行駛安全性[1]?？刂茻o人駕駛車輛沿期望路徑行駛是最重要的問題，已有多種路徑控制方法得以應(yīng)用，如視覺伺服控制、滑?？刂?、模糊控制、線性二次調(diào)節(jié)器（LQR）和模型預(yù)測控制（MPC）用于路徑跟蹤[2-6]。相關(guān)研究中，冀杰等[7]構(gòu)建了多約束模型預(yù)測控制（MMPC）系統(tǒng)用于車道變換路徑跟蹤，可以保證智能車輛在車道變換過程中的路徑跟蹤性能及操縱穩(wěn)定性能；章仁燮等[8]提出一種基于條件積分算法的無人駕駛車輛軌跡跟蹤魯棒控制方法，根據(jù)車輛相對軌跡的非線性運(yùn)動(dòng)學(xué)關(guān)系，得到有界的車輛期望橫擺角速度實(shí)現(xiàn)車輛相對軌跡的側(cè)向位移誤差的全局漸進(jìn)穩(wěn)定；辛喆等[9]針對極限輪胎-路面條件，提出約束車輛的前后輪側(cè)偏角的模型預(yù)測控制方法，保證極限工況下無人駕駛車輛的行駛穩(wěn)定性；張栩源等[10]提出了一種考慮前饋控制和轉(zhuǎn)角補(bǔ)償?shù)木€性二次調(diào)節(jié)器（LQR）和PID 的橫縱向協(xié)同控制方法，在泊車和換道工況下驗(yàn)證了該方法的有效性。

本文考慮無人駕駛車輛在變曲率條件下的路徑跟蹤精度和穩(wěn)定性，提出一種基于道路曲率車輛模型的MPC 橫向控制器和雙環(huán)PID 縱向速度控制器。MPC 橫向控制器得到最優(yōu)前輪轉(zhuǎn)向角輸入到無人駕駛車輛；雙環(huán)PID 縱向速度控制器根據(jù)縱向速度誤差環(huán)和縱向位置誤差環(huán)控制無人駕駛車輛穩(wěn)定快速地達(dá)到期望速度。

1 車輛橫向動(dòng)力學(xué)模型

本文采用考慮橫向運(yùn)動(dòng)和偏航運(yùn)動(dòng)的改進(jìn)單軌（自行車）動(dòng)力學(xué)模型設(shè)計(jì)路徑跟蹤控制器。車輛動(dòng)力學(xué)示意圖如圖1 所示。

圖1 考慮路徑曲率的車輛橫向動(dòng)力學(xué)模型Fig.1 Vehicle lateral dynamics model considering path curvature

圖1 中：ey——橫向誤差；eφ——偏航角誤差；δf——前輪轉(zhuǎn)向角；θf、θr——前后輪側(cè)偏角；lf、lr——質(zhì)心到前后輪的距離；φ——橫擺角；V——質(zhì)心速度。

車輛具體參數(shù)如表1 所示。

表1 車輛參數(shù)Tab.1 Vehicle parameters

2 MPC 橫向控制器設(shè)計(jì)

MPC 常用于路徑跟蹤控制?；谛〗嵌燃僭O(shè)的車輛動(dòng)力學(xué)線性模型，狀態(tài)空間方程式（1）可以寫為

對于MPC 控制器，通過求解滿足目標(biāo)函數(shù)以及各種約束的優(yōu)化問題，在預(yù)測時(shí)域內(nèi)求得最優(yōu)控制序列，并將該控制序列的第1 個(gè)元素作為受控對象的實(shí)際控制量，重復(fù)上述的求解過程，實(shí)現(xiàn)對被控對象的持續(xù)控制。為了將模型應(yīng)用于MPC 控制器的設(shè)計(jì)中，需要對式（2）進(jìn)行離散化處理

在MPC 控制器中控制變量的變化速度對實(shí)際被控系統(tǒng)有較大影響，需要對控制量的增量進(jìn)行約束限制，將式（3）變換為

可以得到新的狀態(tài)空間表達(dá)式

n——狀態(tài)量的維度；m——控制量的維度。

目標(biāo)函數(shù)需要加入系統(tǒng)狀態(tài)量的偏差和控制量作為優(yōu)化目標(biāo)，以保證無人駕駛車輛快速且平穩(wěn)地跟蹤參考路徑。目標(biāo)函數(shù)可以采用以下形式

式中：Q，R——權(quán)重矩陣；Np，Nc——MPC 的預(yù)測時(shí)域和控制時(shí)域。

本文主要考慮控制過程中的控制量極限約束和控制增量約束，控制量極限約束為

控制增量約束為

二次優(yōu)化問題可用二次規(guī)劃求解器求解，計(jì)算出最優(yōu)轉(zhuǎn)向角后，僅第1 個(gè)輸出作為系統(tǒng)下一次采樣時(shí)間的控制，優(yōu)化問題在每個(gè)步驟內(nèi)遞歸求解。

3 PID 縱向速度控制器

除了無人駕駛車輛的橫向跟蹤精度外，無人駕駛車輛速度控制的精度和平順性也是自動(dòng)駕駛的關(guān)鍵性能指標(biāo)。行駛舒適性要求輸出速度和加速度盡可能平穩(wěn)，無需劇烈的制動(dòng)/油門操作。速度控制器精度要求無人駕駛車輛的速度誤差盡可能小。PID 控制易于實(shí)現(xiàn)，不依賴精確的車輛模型，可以降低無人駕駛車輛路徑跟蹤過程的總體計(jì)算量。本文采用縱向位置誤差環(huán)和速度誤差環(huán)的雙PID 結(jié)構(gòu)，設(shè)計(jì)無人駕駛車輛的縱向速度控制器，在滿足跟蹤精度的同時(shí)提高車輛行駛的平順性。

本文在參考路徑上建立Frenet 坐標(biāo)系，如圖2所示。車輛的實(shí)際位置A投影到Frenet 坐標(biāo)系B上，本文假設(shè)離投影點(diǎn)B最近的離散坐標(biāo)點(diǎn)C為無人駕駛車輛路徑跟蹤的參考路徑點(diǎn)。

圖2 Frenet 坐標(biāo)系下的車輛誤差模型Fig.2 Vehicle error model in Frenet coordinate system

由圖2 可得

式中：τm——參考點(diǎn)C的切線單位向量；ηm——參考點(diǎn)C的法線單位向量。

式中：X——車輛實(shí)際位置A 的二維向量；Xm——最近參考點(diǎn)C的二維向量；d——車輛實(shí)際位置A到路徑參考點(diǎn)C的向量之差。

縱向位置誤差環(huán)中的誤差量es和縱向速度誤差環(huán)中的誤差量ev分別為

式中：vref——無人駕駛車輛跟蹤的目標(biāo)速度；vdes——車輛投影到Frenet 坐標(biāo)系上的速度。

雙PID 縱向速度控制器的結(jié)構(gòu)圖如圖3 所示。

圖3 縱向速度控制器的結(jié)構(gòu)圖Fig.3 Block diagram of longitudinal speed controller

4 仿真實(shí)驗(yàn)與驗(yàn)證

為了驗(yàn)證所提MPC 橫向控制器和雙PID 縱向速度控制器的有效性和可行性，在CarSim-Simulink 聯(lián)合仿真平臺進(jìn)行試驗(yàn)仿真和驗(yàn)證。采用雙移線方程為參考路徑，驗(yàn)證本文的控制方法在變曲率道路的路徑跟蹤效果，表達(dá)式為

圖4（a）為雙移線的路徑位置，其路徑點(diǎn)由式（15）求得；圖4（b）為路徑的參考航向角由式（16）求得。

圖4 參考路徑Fig.4 Reference path

MPC 橫向控制器的參數(shù)如表1 所示，車輛的基本參數(shù)如表2 所示。

表2 MPC 控制器參數(shù)Tab.2 MPC controller parameters

本文在車速為20 m/s 的極限工況下對雙移線路徑進(jìn)行路徑跟蹤仿真，并和LQR 路徑跟蹤方法進(jìn)行了對比。LQR 路徑跟蹤方法可參考Xu 等的研究[11]。無人駕駛車輛在20 m/s 速度下跟蹤參考路徑（圖4）的結(jié)果對比、仿真中MPC+PID 和LQR+PID 算法的路徑跟蹤效果對比如圖5 所示，。

圖5 20 m/s 工況下跟蹤對比效果Fig.5 Tracking comparison effect under working condition 20 m/s

由圖5（b）可知，在20 m/s 的極限速度工況下，無人駕駛車輛采用本文的MPC+雙PID 控制方法的橫向誤差最大為0.085 m，而LQR 方法的最大誤差是0.181 m。本文MPC+雙PID 比LQR 方法在極限速度工況的橫向控制精度可提高53%。在縱向速度控制方面，本文的雙環(huán)PID 速度控制方法相較于普通PID 控制，在極限速度工況下速度跟蹤誤差從0.24 m/s 降低到0.16 m/s，速度跟蹤精度可提高33%。橫向控制精度提高是因?yàn)镸PC 控制方法是在預(yù)測時(shí)域范圍進(jìn)行迭代優(yōu)化的過程，可以在每個(gè)控制時(shí)域中使車輛的橫向控制性能達(dá)到最優(yōu)，而LQR 方法無法實(shí)現(xiàn)實(shí)時(shí)迭代優(yōu)化。本文的縱向雙PID 速度控制器，在考慮縱向速度環(huán)的基礎(chǔ)上加入縱向位置誤差環(huán)，可以降低縱向速度跟蹤誤差。

5 結(jié)論

本文針對無人駕駛車輛在變曲率道路環(huán)境的跟蹤問題，設(shè)計(jì)了基于曲率車輛模型的橫向MPC控制器，相較于傳統(tǒng)LQR 方法可以提高無人駕駛車輛在橫向控制上的性能，使橫向誤差減少。另外，基于PID 原理，在速度誤差環(huán)的基礎(chǔ)上加入縱向位置誤差環(huán)可以明顯提高縱向速度控制性能。

仿真結(jié)果表明，本文的MPC+雙PID 方法可以保證無人駕駛車輛有較高的路徑跟蹤精度。從橫向誤差、航向誤差和速度跟蹤誤差上可以看出，本文的方法在無人駕駛車輛路徑跟蹤控制上能保證較好的穩(wěn)定性和準(zhǔn)確性，在縱向速度控制上能提高車輛行駛的舒適性。