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

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

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

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

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

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

實際車輛行駛過程中，路面附著系數(shù)難以通過傳感器直接獲取，因此本文提出的橫向跟蹤控制算法結(jié)合實車裝配的四輪轉(zhuǎn)速及轉(zhuǎn)角傳感器，讀取狀態(tài)信息，通過具有算法穩(wěn)定性的擴(kuò)展卡爾曼濾波器進(jìn)行附著系數(shù)預(yù)測。再根據(jù)車輛當(dāng)前控制量與位姿狀態(tài)，對控制模型中的側(cè)偏角約束進(jìn)行調(diào)整，優(yōu)化車輛在未知路面下的橫向跟蹤控制效果。

1 軌跡跟蹤車輛動力學(xué)模型

1.1 車輛動力學(xué)模型

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

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

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

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

縱向運動方程為：

側(cè)向運動方程為：

橫擺方向運動方程為

車輛在慣性坐標(biāo)系OXY中的平面運動方程：

1.2 路面附著系數(shù)預(yù)測模型

根據(jù)Dugoff 模型[8]，對路面附著系數(shù)與車輪所受的縱向力、橫向力的關(guān)系進(jìn)行描述，建立歸一化橫縱向力的表達(dá)式，即：

其中：

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

2 變側(cè)偏角約束控制器設(shè)計

2.1 路面附著系數(shù)預(yù)估方法

1）建立系統(tǒng)狀態(tài)方程

2）建立測量方程

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

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

其中：

3）創(chuàng)建EKF 系統(tǒng)噪聲

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

4）設(shè)定系統(tǒng)誤差初值

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

6）更新校正狀態(tài)方程

誤差協(xié)方差矩陣為

7） 通過每一時刻的狀態(tài)，重復(fù)5）和6）進(jìn)行運算。

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

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

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

使用1 階差商法對式（18）進(jìn)行離散化，結(jié)合離散狀態(tài)量x(k) 和 控制量u(k)構(gòu)成離散的狀態(tài)空間表達(dá)式為：

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

2.3 側(cè)偏角約束設(shè)定

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

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

在調(diào)試過程中，以式（21）為理論依據(jù)，對不同路面附著系數(shù)下質(zhì)心側(cè)偏角進(jìn)行約束，當(dāng) μ分別取0.2、0.5、0.85 時，質(zhì)心側(cè)偏角 |β|軟約束取值分別設(shè)定為：2.2°、5.5°和9.4°。

為確保在控制車輛軌跡跟蹤過程中車輛的姿態(tài)穩(wěn)定性、跟蹤效果穩(wěn)定性、控制算法穩(wěn)定性，算法在求解階段對每次循環(huán)中的控制量進(jìn)行約束。

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

2.4 側(cè)偏角約束自適應(yīng)調(diào)節(jié)

為實現(xiàn)對路面附著系數(shù)的實時響應(yīng)及側(cè)偏角約束的實時調(diào)整。結(jié)合擴(kuò)展卡爾曼預(yù)測結(jié)果，選用擬合曲線方式進(jìn)行側(cè)偏角約束調(diào)節(jié)，如表2 所示。

表2 側(cè)偏角與路面附著系數(shù)對應(yīng)關(guān)系Tab.2 Lateral deviation angle and road adhesion coefficient correspondence

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

式（23）將實時變化的路面附著系數(shù)轉(zhuǎn)換為側(cè)偏角約束量，對橫向跟蹤控制算法進(jìn)行約束。

3 仿真分析

3.1 路面附著系數(shù)預(yù)測

聯(lián)合CarSim 和Simulink 分別對路面附著系數(shù)分別為0.20，0.50，0.85 的72 km/h 雙移線工況進(jìn)行仿真分析，結(jié)果如圖2 所示。

圖2 擴(kuò)展卡爾曼預(yù)測結(jié)果Fig.2 Extended Kalman prediction results of road adhesion coefficient

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

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

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

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

3.2 橫向跟蹤控制仿真

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

圖3 橫向跟蹤控制算法結(jié)構(gòu)Fig.3 Structure of lateral tracking control algorithm

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

1）創(chuàng)建雙移線軌跡并將預(yù)測控制模型初始化。

2）應(yīng)用線性時變的模型預(yù)測控制（LTV MPC）算法[13]，計算車輛前輪轉(zhuǎn)角，輸出至CarSim 車輛模型進(jìn)行目標(biāo)軌跡跟蹤。

3）讀取EKF 預(yù)測路面附著系數(shù)值[14]，調(diào)整LTV MPC 算法內(nèi)側(cè)偏角約束值，對車輛前輪轉(zhuǎn)角進(jìn)行預(yù)測控制。

4）重復(fù)過程2）和過程3），在每個程序循環(huán)中對未來時域的車輛狀態(tài)進(jìn)行預(yù)測，將最優(yōu)控制量輸出至車輛模型。

仿真結(jié)果表明：

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

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

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

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

3）當(dāng) μ= 0.85 時，常規(guī)算法與改進(jìn)算法差別較小，如圖6 所示）。觀測此時質(zhì)心側(cè)偏角約束發(fā)揮作用。常規(guī)控制算法控制下的車輛四輪側(cè)偏角最高7.8°，質(zhì)心側(cè)偏角最大值為3.8°，在4.8 s 時出現(xiàn)超調(diào)，導(dǎo)致車輛需要進(jìn)行再次回正調(diào)整。優(yōu)化算法所約束的四輪側(cè)偏角均控制在4.2°以內(nèi)，質(zhì)心側(cè)偏角保持在1°以內(nèi)，未出現(xiàn)回正超調(diào)現(xiàn)象，確保車輛姿態(tài)的穩(wěn)定。改進(jìn)算法在2.64 s 跟蹤參考軌跡達(dá)到縱向位移最大值，常規(guī)算法在2.7 s 跟蹤參考軌跡達(dá)到 縱向位移最大值，控制效果優(yōu)化2.2%。

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

因此證明在中低附著系數(shù)工況下，本文所使用的優(yōu)化算法可以對縱向速度為72 km/h 的車輛進(jìn)行更優(yōu)控制，在高附著系數(shù)雙移線的工況下，本文所使用的優(yōu)化算法提高了車輛姿態(tài)的穩(wěn)定性。

4 結(jié)論

1）由于道路受環(huán)境影響存在未知性，通過擴(kuò)展卡爾曼濾波實時預(yù)測路面附著系數(shù)，將車輛所行駛路面狀態(tài)融入控制算法，在橫向控制部分進(jìn)行應(yīng)用，可增強(qiáng)車輛在不同路面橫向軌跡跟蹤的魯棒性。

2）在低附著系數(shù)路面下，仿真實驗結(jié)果表明，常規(guī)算法出現(xiàn)過大漂移，無法完成仿真測試。優(yōu)化控制算法通過對附著系數(shù)預(yù)測后調(diào)整側(cè)偏角約束，完成對低附濕滑工況的橫向跟蹤，提高車輛橫向軌跡跟蹤效果。

3）在高附著系數(shù)路面下，對車輛四輪側(cè)偏角及質(zhì)心側(cè)偏角進(jìn)行軟約束，解決車輛在向右橫向位移后產(chǎn)生超調(diào)的問題，提高橫向跟蹤控制效果2.2%，同時也提高了車輛自身姿態(tài)的穩(wěn)定性。