一種基于LTVMPC 改進的無人駕駛汽車路徑跟蹤控制算法

張維剛，張朋，韋昊，熊覺振

（湖南大學汽車車身先進設計制造國家重點實驗室，湖南長沙 410082）

路徑跟蹤是無人駕駛汽車自主行駛過程中的基本任務和關鍵環節，其主要目標是使車輛自動沿著規劃路徑安全穩定地行駛[1].當車輛行駛在低附著路面上時，由于此時車輛極易處于失穩狀態，路徑跟蹤精確性和車輛行駛穩定性就面臨著很大的挑戰.

為了增強路徑跟蹤控制效果，相關研究人員提出了許多算法.模型預測控制（MPC）由于能夠有效處理系統約束、方便建立多輸入多輸出控制系統且具備前饋加反饋控制的優點，已經成為其中最有效的方法之一[2].MPC 首先利用已有模型來預測系統未來的動態，并將系統當前的狀態作為初始狀態，以某項性能指標達到最優為目標，形成一個有限時域開環最優化問題，通過在線求解該問題而獲得最優控制量[3-4].由于在每個采樣時刻都求解二次規劃或非線性規劃問題，當預測模型過于復雜或考慮過多的非線性約束時，計算量會過于龐大而限制其在車輛控制系統中的實際應用[5-6].為提高算法的實時性，線性時變模型預測控制（LTVMPC）往往作為一種次優的選擇[7].

LTVMPC 在每個時間步長內，將非線性車輛動力學模型在當前工作點連續線性化，且一般會將最優化問題轉化為二次規劃問題進行求解.動力學預測模型是LTVMPC 的核心，模型的精度和復雜度直接決定了路徑跟蹤的精確性和實時性[8].文獻[9]使用三種不同精度的車輛模型來建立LTVMPC 路徑跟蹤控制器，指出精度高的車輛模型可以有效提高跟蹤性能，但復雜的模型會相應增加計算負擔，當模型的維度達到一定階次后，性能的提升幅度有限.文獻[10] 基于簡化的三自由度車輛動力學模型建立了典型的LTVMPC 軌跡跟蹤控制器，但由于未考慮左右輪胎受力的差異及低附著工況下質心側偏角和橫擺角速度對車輛穩定性的影響，路徑跟蹤精確性和車輛行駛穩定性的問題依然存在.

文獻[11]研究了質心側偏角對路徑跟蹤效果的影響，指出把參考橫擺角定義為期望路徑航向角，橫向誤差和橫擺角誤差將難以同時收斂趨于0，而利用質心側偏角對參考橫擺角進行補償能提高路徑跟蹤的精確性.文獻[12-13]在LTVMPC 性能指標中加入橫擺角速度誤差，指出該項的加入能夠提高路徑跟蹤性能，但未對其影響作具體的闡述.質心側偏角與地面對輪胎的橫擺力矩和側向力變化范圍直接相關，橫擺角速度表征了車輛的轉彎能力和動態行為，通過對兩者實施相應的動力學約束能夠提升車輛的橫擺穩定性[14].因此，在LTVMPC 路徑跟蹤控制算法中，應綜合考慮質心側偏角和橫擺角速度的影響，以提高路徑跟蹤的精確性和車輛行駛的穩定性.

基于以上分析，本文針對低附著路面情況下，基于LTVMPC 的無人駕駛汽車路徑跟蹤控制精確性和穩定性問題，在精確建立四輪輪胎側偏角、滑移率和車輛狀態參數非線性關系的基礎上，考慮預測模型維度對算法實時性的影響，簡化雅可比矩陣的求解，建立改進的三自由度車輛模型.在二次規劃性能指標中加入橫擺角速度誤差項，利用質心側偏角修正參考橫擺角，并對橫擺角速度和質心側偏角施加穩定性約束，建立改進的LTVMPC.在Carsim-Simulink聯合仿真平臺進行低附著路面情況下的雙移線跟蹤仿真，驗證改進措施的有效性.

1 汽車動力學預測模型

假設車輛在水平路面上行駛，忽略空氣阻力，建立主要考慮車輛縱向、橫向以及橫擺動力學特性的平面四輪車輛模型.如圖1 所示，oxyz 為車輛坐標系，固定于車輛質心，OXYZ 為慣性坐標系.

圖1 車輛動力學預測模型Fig.1 Vehicle dynamics prediction model

車輛橫向、縱向及橫擺運動的動力學方程可以分別表示為：

式中：Fx*，·，Fy*，·表示車輛的縱向力和橫向力，下標*∈{f，r}表示前后軸，·∈{l，r}表示左右輪.a，b 分別為前后軸到質心的距離，c 為前后軸輪距（在此認為相等）；m 為整車質量，I 為車輛繞z 軸的轉動慣量，y和分別為車輛沿著y 軸和x 軸的橫向速度和縱向速度，為車輛橫擺角速度.

輪胎在x 軸和y 軸方向上的受力分別為：

式中：F1*，·、Fc*，·分別為輪胎所受到的縱向力和側向力.

基于Dugoff 輪胎模型，輪胎的縱向力和側向力可以表示為車輛狀態參數和路面附著系數的復雜函數：

式中：α 為輪胎側偏角，s 為滑移率，Fz為輪胎受到的垂向載荷，μ 為路面附著系數.

考慮到控制算法要顯式表征線性化車輛動力學預測模型的狀態轉移矩陣，公式（6）（7）的復雜函數關系會大大增加矩陣元素的復雜度，從而影響模型預測控制的實時性，因此對輪胎的縱向力和側向力進行簡化處理，分別表示為：

式中：C1*，·，Cc*，·分別為輪胎縱向剛度和側偏剛度，s*，·為滑移率，α*，·為輪胎側偏角.四輪側偏角可以分別表示為車輛狀態量和前輪轉角的非線性函數：

式中：δ 為前輪轉角.

滑移率可以表示為：

式中：r 為車輪半徑，ω 為車輪旋轉角速度，v1*，·表示車輪中心縱向速度（如圖1 所示）并可分別表示為：

忽略小角度假設，且認為車輛僅通過前輪轉向，從而得到七自由度平面四輪車輛模型的狀態方程：

該模型的維度較高且呈高度非線性，考慮到無人駕駛汽車對控制器的實時性要求，對式（15）線性化、離散化處理.

在預測時域Np內，將輪速作為非狀態量，對系統狀態方程（13）在ξ（t）處進行一階泰勒展開.在MATLAB 環境中，采用Jacobian 函數分別求解對ξ（t）和u（t）的雅可比矩陣，從而得到新的狀態方程：

經過上述處理后，在預測時域Np內，對車輛狀態進行預測時，車輛狀態參數降為3 個，控制器的實時性得以提高.

2 路徑跟蹤目標協調與優化

雙移線工況是能表征汽車操縱穩定性的典型行駛工況，同時能在一定程度上反映車輛的超車和避障能力.期望雙移線路徑由橫向坐標Yref和橫擺角ψref組成，兩者均為縱向坐標的非線性函數，具體可表達為如下形式：

圖2 顯示了車輛跟蹤參考路徑的運動過程，圖中，β 為車輛質心側偏角，φ 為車輛橫擺角，ψref為參考路徑橫擺角，φref為修正后的參考橫擺角，e 為車輛質心與參考路徑的直線距離.

圖2 路徑跟蹤示意圖Fig.2 Path tracking schematic diagram

從實際應用的角度來說，要使車輛精確地沿著期望路徑行進，橫向誤差收斂到0 應為主要目標.從汽車動力學的角度看，這意味著車輛質心速度方向要與車輛參考路徑切線方向一致.否則，車輛在參考路徑法向上就會有速度分量，車輛便有位置誤差增大的運動趨勢.為實現以上要求，將參考橫擺角的修正值取為：

相應地，參考橫擺角速度為：

式中：ρ 為參考路徑的曲率.

將參考橫擺角速度作為跟蹤目標之一且考慮質心側偏角的影響，通過對兩者添加動力學約束可以提高車輛在跟蹤參考路徑時的穩定性.

3 改進LTVMPC 控制器設計

在每個采樣時刻，LTVMPC 控制器通過建立最小化性能指標和添加相關約束，并采用二次規劃算法求解這樣一個有限時域最優化問題.在新的采樣時刻，利用被控對象的實際輸出信息，更新上述優化控制問題，實現滾動優化的過程，其控制原理如圖3所示：

圖3 LTVMPC 原理框圖Fig.3 LTVMPC schematic diagram

為了能對控制量變化做出有效的約束，實現平穩的路徑跟蹤過程，針對狀態方程（14），以前輪轉角變化量為新的控制量，得到新的狀態空間表達式：

控制器的主要目標是使車輛精確穩定地跟蹤參考路徑，性能指標應能兼顧跟蹤精度和平穩性.根據上節對跟蹤誤差的分析結果，總優化目標定義為跟蹤誤差和控制增量的加權求和：

式中：Hp和Hc分別為預測時域和控制時域，η=（φ，Y，）為系統輸出，ηref為相應的參考輸出，Q，R，ρ為相應優化目標的權重矩陣，ε是為了確保在規定時間內存在最優解而加入的松弛因子，ΔUt為控制時域內的控制增量序列.

為了使得輪胎處于線性范圍內，以保證建立動力學預測模型時的線性化處理對模型精度影響較小，對側偏角施加軟約束.輪胎側偏角為控制量和狀態量的非線性函數，式（8）-（11）可以表示為：

采用和上述系統方程相似的線性化離散化方法：

寫成以控制增量為變量的形式為：

車輛在實際行進過程中，作為控制輸出的前輪轉角范圍會受到轉向機構的限制，為了使得前輪轉角變化比較平穩且易于實現，分別對前輪轉角及其變化量取值范圍做出約束.

車輛在低附著路面上行進時，極易處于失穩狀態.考慮到質心側偏角對車輛穩定性的影響以及路面附著條件對輪胎力的限制，對兩者做出約束：

式中：μ 為路面附著系數，β 由于不是直接輸出量，采用與輪胎側偏角類似的方式進行約束，并且為了保證車輛在低附著路面上行駛時的穩定性而不加松弛因子.為直接輸出量，可以直接對其進行限制.

綜合考慮上述目標函數及約束，最優化問題可以規整為：

式中：yout表示控制器的直接輸出量，包括車輛橫擺角、橫向位置及橫擺角速度.

將式（25）的最優化問題轉化為二次規劃問題，在MATLAB 環境中，使用Quadprog 函數并采用有效集法進行求解，將解的第一個元素作為實際控制量.

4 仿真實驗結果及分析

根據我國道路交通安全法，車輛在一般道路上的行駛速度不超過70 km/h.在Carsim-Simulink 聯合仿真平臺上進行仿真實驗，為了驗證控制器在低附著路面不同車速情況下的路徑跟蹤效果，設置路面附著系數μ=0.3，初始車速為30 km/h 和70 km/h 兩種情況.

為了驗證改進模型的有效性，以基于二自由度車輛模型的LTVMPC 為比較對象，不加入質心側偏角修正且不考慮橫擺角速度跟蹤誤差，除預測模型不同外，控制器的其他參數均相同且如下所示：

● 仿真步長T=0.05 s，預測時域Hp=20，控制時域Hc=5

● 優化目標權重：

● 相關約束：Δumin=-0.85°，Δumax=0.85°，umin=-10°，umax=10°，ymin=（-0.5 -2）T，ymax=（0.5 4）T.

圖4、圖5 分別顯示了30 km/h 和70 km/h 初始車速情況下改進模型前后的路徑跟蹤仿真結果：

圖4 v=30 km/h 時路徑跟蹤仿真結果Fig.4 Simulation results of path tracking when v=30 km/h

圖5 v=70 km/h 時路徑跟蹤仿真結果Fig.5 Simulation results of path tracking when v=70 km/h

使用均方根跟蹤誤差（RMSE）表示改進模型前后的跟蹤精度變化，如表1 所示，YRMS表示改進模型前后的均方根誤差.

表1 跟蹤精度對比Tab.1 Comparison of tracking accuracy

表1 表明，相比基于二自由度模型的LTVMPC，改進模型后的控制器在初始車速為30 km/h、70 km/h情況下的跟蹤誤差分別減小了74.92%和13.83%，說明改進后的預測模型能有效提高LTVMPC 的路徑跟蹤精度.

動力學預測模型很大程度上決定了每個仿真步長內控制器的求解時間，改進模型前后控制器的實時性如表2 所示，其中，tave、tmax分別表示控制器在每個仿真步長內計算時間的平均值和最大值.

表2 實時性對比Tab.2 Comparison of real-time performance

表2 說明，改進模型前后，控制器的實時性沒有出現量級上的變化，最大計算時間不超過35 ms，基于改進模型的LTVMPC 具有較好的實時性.

為了表示在性能指標中加入橫擺角速度跟蹤誤差并利用質心側偏角修正參考橫擺角和橫擺角速度的路徑跟蹤效果，基于改進的動力學預測模型，對比了目標優化前后的跟蹤性能.考慮到車輛在低附著路面上行駛時的穩定性，在以上約束的基礎上加入側偏角約束和橫擺角速度約束，并將橫擺角速度的權重取為=10，α 絕對值上限取為4°.

車速為30 km/h 情況下，目標優化前后的仿真結果如圖6 所示：

圖6 v=30 km/h 時目標優化前后仿真結果Fig.6 Simulation results before and after objective optimization when v=30 km/h

圖6（a）顯示了路徑跟蹤過程中車輛橫向位置跟蹤誤差變化情況，使用質心側偏角和橫擺角速度對跟蹤目標修正和優化后，橫向位置最大誤差不超過23 cm，目標優化前后跟蹤精度提高較小，這是因為車輛以低速行駛時，質心側偏角和橫擺角速度變化較小，其對跟蹤目標的優化作用較小.圖6（b）顯示了經過目標優化后橫擺角速度的跟蹤效果，跟蹤誤差均方根為1.2°/s，表明車輛能較好地跟蹤參考值.

車速為70 km/h 情況下，目標優化前后的仿真結果如圖7 所示：

圖7 v=70 km/h 時目標優化前后仿真結果Fig.7 Simulation results before and after objective optimization when v=70 km/h

圖7（a）顯示了對參考軌跡的跟蹤情況，從圖中可以看出，在高速低附工況下，由于考慮了橫擺角速度的跟蹤情況，經過目標優化后的控制器相比優化前對最大橫向位置的跟蹤誤差減小了16.8 cm，但為跟蹤參考橫擺角速度，車輛由于慣性而使得車輛的實際運動路徑稍微滯后.車輛在最后一個轉角處誤差收斂的速度下降且出現略微的超調，這是因為將橫擺角速度作為跟蹤目標之一，相當于增加了汽車的動態響應需求，跟蹤誤差的收斂性相對下降.圖7（b）、7（c）表明了穩定性約束對路徑跟蹤效果的影響，從圖中可以看出，相比優化前，橫擺角速度由于受到約束而處于穩定性范圍內，且質心側偏角-質心側偏角速度相平面具有更小的變化區間，說明橫擺角速度和質心側偏角的綜合約束使得車輛具有良好的穩定性.

5 結論

針對低附著路面情況下基于LTVMPC 的無人駕駛汽車路徑跟蹤控制精確性和穩定性問題，提出相應的改進措施，包括：1）建立兼顧路徑跟蹤精確性和實時性的改進三自由度車輛模型；2）將橫擺角速度跟蹤誤差加入二次規劃性能指標，利用車輛質心側偏角修正跟蹤目標，建立改進的LTVMPC.

搭建Carsim-Simulink 聯合仿真平臺，在低附著路面情況下，車輛以30 km/h 和70 km/h 兩種初速度跟蹤雙移線的仿真表明：改進后的車輛模型能提高路徑跟蹤精度且不影響實時性；將質心側偏角和橫擺角速度加入跟蹤目標，能提高控制器的路徑跟蹤能力.優化后的LTVMPC 路徑跟蹤控制器能更精確地跟蹤參考軌跡，且能提高車輛在低附著路面情況下的行駛穩定性.