無人車輛橫向跟蹤控制研究

李伯雄，王立勇，孫鵬，季文龍

（1.北京信息科技大學 現代測控技術教育部重點實驗室，北京 100192；2.中國人民解放軍 63966 部隊，北京 100072）

無人車輛的軌跡跟蹤控制算法在實際車輛控制系統中將決策層與硬件層相結合，對車輛軌跡跟蹤起到重要作用。通過提高無人車輛在未知路面對決策軌跡的跟蹤效果，可提高車輛追蹤決策軌跡的準確性及高效性。軌跡跟蹤控制算法的研究主要使用模型預測控制器（MPC）[1]進行搭建。

MPC 通過引入滾動優化及反饋校正，實現對系統狀態的預測，進一步提高車輛運動軌跡的控制穩定性。李想等[2]提出一種變權重的模型預測控制算法，針對不同車速進行權值再分配，從而實現兼顧不同目標跟蹤速度下的高精度控制。何德峰等[3]提出通過控制增量分層的策略以降低系統運算量，在保證追蹤性能的前提下加快系統運算速度。

車輛行駛過程中的狀態變化與路面條件關系緊密，因此諸多學者借助車載傳感器對路面信息進行準確辨識。張航星等[4]使用擴展卡爾曼濾波對車輛的輪胎力進行觀測，從而預測路面附著系數。劉志強等[5]使用自適應衰減卡爾曼濾波（AFUKF）對四輪路面附著系數進行估測，并使用Sage-Husa 噪聲估計器提高了預測模型的準確性，但未對轉向工況進行分析。另一些研究者則著重提高在未知路面條件下車輛的控制性能及安全性能。李緒龍等[6]提出一種雙重無跡卡爾曼算法，將預測得到的路面附著系數反映到車輛縱向控制算法中，修正剎車距離與安全跟車距離，降低汽車發生碰撞的可能性。未對彎道工況進行控制優化。楊秀建等[7]針對低附著系數工況，使用最小二乘法對路面附著系數進行實時預估，通過預測結果的反饋改變安全距離約束，確保車輛控制的可靠性。但對于高附著系數工況的路面估計存在較大偏差。

實際車輛行駛過程中，路面附著系數難以通過傳感器直接獲取，因此本文提出的橫向跟蹤控制算法結合實車裝配的四輪轉速及轉角傳感器，讀取狀態信息，通過具有算法穩定性的擴展卡爾曼濾波器進行附著系數預測。再根據車輛當前控制量與位姿狀態，對控制模型中的側偏角約束進行調整，優化車輛在未知路面下的橫向跟蹤控制效果。

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

1.1 車輛動力學模型

Carsim 中無人車輛模型主要參數見表1。

本文研究的軌跡跟蹤控制模型是基于四自由度動力學模型構建的。該模型可以在車輛轉向時分析四輪橫縱向受力、轉速及車輛橫擺角速度等運動性能，如圖1 所示。

圖1 四自由度車輛動力學模型Fig.1 Four degree of freedom vehicle's dynamics model

車輛在路面行駛過程中，沿x、y及z軸（橫擺）方向受力平衡，由此得出車輛縱向、側向及橫擺方向微分方程。

縱向運動方程為：

側向運動方程為：

橫擺方向運動方程為

車輛在慣性坐標系OXY中的平面運動方程：

1.2 路面附著系數預測模型

根據Dugoff 模型[8]，對路面附著系數與車輪所受的縱向力、橫向力的關系進行描述，建立歸一化橫縱向力的表達式，即：

其中：

聯立式（8）和式（9）及式（2），式（4）和式（5）得到整車四輪的非線性動力學微分方程，即：

2 變側偏角約束控制器設計

2.1 路面附著系數預估方法

1）建立系統狀態方程

2）建立測量方程

式 中：4 個 輪 胎 路 面 附 著 數xk= [μ1,μ2,μ3,μ4]T；A=I4?4； 過 程 噪 聲wk= 0.01I4?4； 測 量 變 量yk=[ax,ay,ω˙]T；控制輸入量uk=[δ] ；觀測噪聲vk=I4?4。

Hk為測量方程yk對xk求偏導數，將模型線性化得到的雅各比矩陣。

其中：

3）創建EKF 系統噪聲

式中：Q為過程噪聲協方差矩陣；x-0為參數變量初始數值；R為觀測噪聲協方差矩陣；P-(t0)為初始協方差矩陣。

4）設定系統誤差初值

5）定義狀態預測量x?kˉ=f(xk-1,0)及狀態協方差Pkˉ=Pk-1+Q。

6）更新校正狀態方程

誤差協方差矩陣為

7） 通過每一時刻的狀態，重復5）和6）進行運算。

2.2 建立變約束線性時變模型

本文重點研究提高軌跡跟蹤效果，不涉及具有不確定性的突發情況，因此預測控制模型選用線性時變模型預測控制器[9]。首先對式（1）～ 式（5）進行線性化處理，得到線性時變方程，即

式中：系統狀態量 ξ=[x˙,y˙,ω,ω˙,X,Y]T；系統控制量u(t)=[δ]；A(t)=?f(ξ)/?ξ，B(t)=?f(ξ)/?u。

使用1 階差商法對式（18）進行離散化，結合離散狀態量x(k) 和 控制量u(k)構成離散的狀態空間表達式為：

式中：A?(k)=I+TA(t) ，B?(k)=T B(t)，T為采樣周期。

2.3 側偏角約束設定

通過對質心側偏角添加軟約束，實現未知路面附著系數工況下，車輛穩定性的進一步提升。通過文獻[10]所描述，使用相平面法分析得出：隨路面附著系數減小，能夠支持車輛穩定行駛的最大質心側偏角隨之減小。當在車輛處于不同路面附著系數工況行駛時，質心側偏角穩定區域的邊界條件為

式中： μ為路面附著系數；g為萬有引力常數。

在調試過程中，以式（21）為理論依據，對不同路面附著系數下質心側偏角進行約束，當 μ分別取0.2、0.5、0.85 時，質心側偏角 |β|軟約束取值分別設定為：2.2°、5.5°和9.4°。

為確保在控制車輛軌跡跟蹤過程中車輛的姿態穩定性、跟蹤效果穩定性、控制算法穩定性，算法在求解階段對每次循環中的控制量進行約束。

式中:J(ξ(t)) 為 輸出量和期待輸出量的差值；u(t-1)為上一時刻控制量； ?U(t)為上一控制時域與當前控制時域控制量的差值；yh為系統輸出硬約束，包括最大前輪轉角、最大車速、最大加速度；ys為系統輸出軟約束，包括橫向加速度約束、側偏角約束。

2.4 側偏角約束自適應調節

為實現對路面附著系數的實時響應及側偏角約束的實時調整。結合擴展卡爾曼預測結果，選用擬合曲線方式進行側偏角約束調節，如表2 所示。

表2 側偏角與路面附著系數對應關系Tab.2 Lateral deviation angle and road adhesion coefficient correspondence

借助MATLAB/Simulink 中Cftool 工具對實際曲線進行擬合獲得

式（23）將實時變化的路面附著系數轉換為側偏角約束量，對橫向跟蹤控制算法進行約束。

3 仿真分析

3.1 路面附著系數預測

聯合CarSim 和Simulink 分別對路面附著系數分別為0.20，0.50，0.85 的72 km/h 雙移線工況進行仿真分析，結果如圖2 所示。

圖2 擴展卡爾曼預測結果Fig.2 Extended Kalman prediction results of road adhesion coefficient

在路面附著系數 μ= 0.2 的工況下，0.8 s 時預測值初次達到實際路面附著系數值，在完成當前工況行駛的9 s 內， μ平均值為0.193，均方根誤差值為0.019，相對平均偏差7.59%。

在 μ=0.5 工況下，1.2 s 時預測值初次達到實際值，在完成當前工況行駛的9 s 內， μ平均值為0.499，均方根誤差值為0.024，相對平均偏差3.46%。

在 μ=0.85 工況下，1.4 s 時預測值初次達到實際值，在完成當前工況行駛的9 s 內， μ平均值為0.821，均方根誤差值為0.065，相對平均偏差6.07%。

3 種路面工況下平均值誤差在3.5%以內，均可以為控制算法提供可靠的路面附著系數值。

3.2 橫向跟蹤控制仿真

本文仿真車輛以72 km/h 的縱向速度，在μ=0.2、 μ=0.5、 μ=0.85 的雙移線工況[11]下行駛。聯合Carsim 與Simulink 進行仿真，Carsim 選用2020版本MATLAB/Simulink 選用R2020a 版本。Carsim提供車輛的實時位姿及狀態信息，Simulink 環境下搭建路面附著系數預測算法及模型預測控制器。橫向跟蹤控制算法結構如圖3 所示。

圖3 橫向跟蹤控制算法結構Fig.3 Structure of lateral tracking control algorithm

通過對比橫向追蹤參考值與仿真值對控制效果進行評價[12]，過程如下：

1）創建雙移線軌跡并將預測控制模型初始化。

2）應用線性時變的模型預測控制（LTV MPC）算法[13]，計算車輛前輪轉角，輸出至CarSim 車輛模型進行目標軌跡跟蹤。

3）讀取EKF 預測路面附著系數值[14]，調整LTV MPC 算法內側偏角約束值，對車輛前輪轉角進行預測控制。

4）重復過程2）和過程3），在每個程序循環中對未來時域的車輛狀態進行預測，將最優控制量輸出至車輛模型。

仿真結果表明：

1）當 μ=0.2 時，車輛經過第一個彎道后，常規算法控制的車輛由于缺少質心側偏角軟約束，此時車輛質心側偏角達到11°，在方向盤轉角達到最大時仍無法控制車輛回正，導致車輛處于失控狀態[15]，如圖4a)所示。同一工況下，改進控制算法通過路面附著系數反饋，提前對側偏角軟約束進行修正，將四輪側偏角控制在0.8°以內。質心側偏角在0.19°以內。在1.8 s 時控制車輛行駛回到初始橫向位置，如圖4b)和圖4c)所示。

圖4 附著系數μ = 0.20 的控制效果Fig.4 Control effect when the adhesion coefficient μ = 0.2

2）當 μ=0.5 時，常規算法控制車輛經過第一個彎道回正時出現橫向位移偏差，在4.3 s 時回到初始橫向位置，此時車輛位姿處于超調狀態。第6 s 時，車輛處于縱向位移– 4.675 m 位置，此時質心側偏角達到–14.6°，隨后無法控制車輛回到規定軌跡上，車輛屬于完全失控狀態，如圖5a)所示。經過改進的控制算法在減小橫向位移偏差的同時，通過添加質心側偏角軟約束在回正過程中對轉角進行調節，將四輪側偏角約束在5°以內，使車輛在6.9 s 回到規定軌跡，如圖5b)和圖5c)所示。

圖5 附著系數μ = 0.50 的控制效果Fig.5 Control effect when the adhesion coefficient μ = 0.50

3）當 μ= 0.85 時，常規算法與改進算法差別較小，如圖6 所示）。觀測此時質心側偏角約束發揮作用。常規控制算法控制下的車輛四輪側偏角最高7.8°，質心側偏角最大值為3.8°，在4.8 s 時出現超調，導致車輛需要進行再次回正調整。優化算法所約束的四輪側偏角均控制在4.2°以內，質心側偏角保持在1°以內，未出現回正超調現象，確保車輛姿態的穩定。改進算法在2.64 s 跟蹤參考軌跡達到縱向位移最大值，常規算法在2.7 s 跟蹤參考軌跡達到 縱向位移最大值，控制效果優化2.2%。

圖6 附著系數μ = 0.85 的控制效果Fig.6 Control effect when the adhesion coefficient μ=0.85

因此證明在中低附著系數工況下，本文所使用的優化算法可以對縱向速度為72 km/h 的車輛進行更優控制，在高附著系數雙移線的工況下，本文所使用的優化算法提高了車輛姿態的穩定性。

4 結論

1）由于道路受環境影響存在未知性，通過擴展卡爾曼濾波實時預測路面附著系數，將車輛所行駛路面狀態融入控制算法，在橫向控制部分進行應用，可增強車輛在不同路面橫向軌跡跟蹤的魯棒性。

2）在低附著系數路面下，仿真實驗結果表明，常規算法出現過大漂移，無法完成仿真測試。優化控制算法通過對附著系數預測后調整側偏角約束，完成對低附濕滑工況的橫向跟蹤，提高車輛橫向軌跡跟蹤效果。

3）在高附著系數路面下，對車輛四輪側偏角及質心側偏角進行軟約束，解決車輛在向右橫向位移后產生超調的問題，提高橫向跟蹤控制效果2.2%，同時也提高了車輛自身姿態的穩定性。