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

周義棚，楊威

（201620 上海市 上海工程技術大學 機械與汽車工程學院）

0 引言

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

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

1 車輛橫向動力學模型

本文采用考慮橫向運動和偏航運動的改進單軌（自行車）動力學模型設計路徑跟蹤控制器。車輛動力學示意圖如圖1 所示。

圖1 考慮路徑曲率的車輛橫向動力學模型Fig.1 Vehicle lateral dynamics model considering path curvature

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

車輛具體參數如表1 所示。

表1 車輛參數Tab.1 Vehicle parameters

2 MPC 橫向控制器設計

MPC 常用于路徑跟蹤控制。基于小角度假設的車輛動力學線性模型，狀態空間方程式（1）可以寫為

對于MPC 控制器，通過求解滿足目標函數以及各種約束的優化問題，在預測時域內求得最優控制序列，并將該控制序列的第1 個元素作為受控對象的實際控制量，重復上述的求解過程，實現對被控對象的持續控制。為了將模型應用于MPC 控制器的設計中，需要對式（2）進行離散化處理

在MPC 控制器中控制變量的變化速度對實際被控系統有較大影響，需要對控制量的增量進行約束限制，將式（3）變換為

可以得到新的狀態空間表達式

n——狀態量的維度；m——控制量的維度。

目標函數需要加入系統狀態量的偏差和控制量作為優化目標，以保證無人駕駛車輛快速且平穩地跟蹤參考路徑。目標函數可以采用以下形式

式中：Q，R——權重矩陣；Np，Nc——MPC 的預測時域和控制時域。

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

控制增量約束為

二次優化問題可用二次規劃求解器求解，計算出最優轉向角后，僅第1 個輸出作為系統下一次采樣時間的控制，優化問題在每個步驟內遞歸求解。

3 PID 縱向速度控制器

除了無人駕駛車輛的橫向跟蹤精度外，無人駕駛車輛速度控制的精度和平順性也是自動駕駛的關鍵性能指標。行駛舒適性要求輸出速度和加速度盡可能平穩，無需劇烈的制動/油門操作。速度控制器精度要求無人駕駛車輛的速度誤差盡可能小。PID 控制易于實現，不依賴精確的車輛模型，可以降低無人駕駛車輛路徑跟蹤過程的總體計算量。本文采用縱向位置誤差環和速度誤差環的雙PID 結構，設計無人駕駛車輛的縱向速度控制器，在滿足跟蹤精度的同時提高車輛行駛的平順性。

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

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

由圖2 可得

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

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

縱向位置誤差環中的誤差量es和縱向速度誤差環中的誤差量ev分別為

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

雙PID 縱向速度控制器的結構圖如圖3 所示。

圖3 縱向速度控制器的結構圖Fig.3 Block diagram of longitudinal speed controller

4 仿真實驗與驗證

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

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

圖4 參考路徑Fig.4 Reference path

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

表2 MPC 控制器參數Tab.2 MPC controller parameters

本文在車速為20 m/s 的極限工況下對雙移線路徑進行路徑跟蹤仿真，并和LQR 路徑跟蹤方法進行了對比。LQR 路徑跟蹤方法可參考Xu 等的研究[11]。無人駕駛車輛在20 m/s 速度下跟蹤參考路徑（圖4）的結果對比、仿真中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%。在縱向速度控制方面，本文的雙環PID 速度控制方法相較于普通PID 控制，在極限速度工況下速度跟蹤誤差從0.24 m/s 降低到0.16 m/s，速度跟蹤精度可提高33%。橫向控制精度提高是因為MPC 控制方法是在預測時域范圍進行迭代優化的過程，可以在每個控制時域中使車輛的橫向控制性能達到最優，而LQR 方法無法實現實時迭代優化。本文的縱向雙PID 速度控制器，在考慮縱向速度環的基礎上加入縱向位置誤差環，可以降低縱向速度跟蹤誤差。

5 結論

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

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