具有噪聲魯棒性的車輛質心側偏角估計

孫堅添，葉貽財，郭聰聰

（212013江蘇省 鎮江市 江蘇大學 汽車與交通工程學院）

0 引言

汽車高級輔助駕駛系統和主動安全系統性能的充分發揮需要依賴準確的車輛運動狀態信息[1]。由于汽車生產成本與安裝條件的制約[2]，狀態估計（又稱信息軟測量）方法利用低成本傳感器間接獲取系統需要的關鍵信息，成為獲取車輛運動狀態信息的一種有效途徑。目前，卡爾曼濾波器是廣泛使用的軟測量算法，但當量測噪聲統計特性發生變化時，該算法估計性能會降低甚至出現濾波發散的情況。針對這個問題，研究人員提出了不同種類的自適應卡爾曼濾波（Adaptive Kalman Filter,AKF）算法。李剛[3]等人利用基于最大后驗準則的Sage-Husa-AKF算法遞歸地估計噪聲統計特性。然而，該算法不能保證右向噪聲協方差矩陣的收斂性，有可能導致結果發散[4]；多模型AKF（The Multiple Model AKF，MMAKF）是貝葉斯方法的一種近似，它并行操作一組具有不同數學模型的卡爾曼濾波器來處理模型的不確定性問題[5]，但其計算復雜性較大；有學者則提出了一種自適應無跡卡爾曼濾波器（Adaptive Unscented Kalman Filter，AUKF）用于處理過程噪聲協方差與量測噪聲協方差的關系[6]。近年來，為了減少傳感器時變噪聲協方差對估計結果的影響，一類基于變分貝葉斯（Variational Bayesian，VB）理論的算法[7]因其良好的魯棒性和較高的精度受到科研人員的關注。

本文基于雙軌車輛模型，通過變分貝葉斯自適應容積卡爾曼濾波（Variational Bayesian-based Adaptive Cubature Kalman Filter，VBACKF）算法對車輛運動狀態進行估計。CarSim與MATLAB/Simulink聯合仿真平臺的實驗結果表明：在時變量測噪聲協方差條件下，VBACKF算法能夠適應時變噪聲協方差且具有較高的估計精度。

1 雙軌車輛模型

1.1 車輛模型

為描述車輛運動，建立了非線性雙軌車輛模型如圖1所示。假設車輛左、右前輪的轉向角均為δ。方向盤轉角δ0和前輪轉向角δ的關系近似為δ=Kδ0，其中K為車輛轉向系統的角傳動比。

圖1 非線性雙軌車輛模型Fig.1 Nonlinear dual-track model

根據達朗貝爾原理，車輛的縱向、側向及橫擺方向的運動描述如下：

其中：vx——車輛縱向速度；vy——車輛側向速度；r——車輛橫擺角速度；Iz——車輛繞z軸的慣性矩。車輛的縱向加速度ax，側向加速度ay和橫擺力矩Mz通過式（2）—式（4）計算：

式中：m——車輛質量；g——重力加速度；Fxi，Fyi——縱向和側向輪胎力；下標i（i=1，2，3，4）——左前輪、右前輪、左后輪和右后輪；lF，lR——由車輛質心到車輛前、后軸的距離；bF，bR——車輛前、后軸的寬度。

輪胎垂直動載荷Fz表示為

式中：h——車輛質心高度。車輪滑移率σ為

式中：ω——車輪轉速；Rt——車輪半徑。車輪輪心處的速度vt為

車輪側傾角αt為

1.2 Dugoff輪胎模型

Dugoff輪胎模型具有較高的解算精度且適合實時在線計算，因此，采用該模型表征縱向和側向輪胎力Fxi和Fyi：

式中：Cσ，Cα——輪胎的縱向和側向剛度。輪胎模型參數λ為

式中：μ——道路附著系數。

2 噪聲魯棒車輛運動狀態估計器

2.1 變分貝葉斯自適應容積卡爾曼濾波

VBACKF基于VB理論，將系統狀態與噪聲一起作為待估計向量，采用定點迭代的方式得到局部最優解。考慮如下狀態空間

式中：k——時間點標志；xk——狀態向量；uk——輸入向量；zk——量測向量；wk，vk——零均值、協方差矩陣為Qk和Rk的高斯過程和量測噪聲。

卡爾曼濾波框架下，一步預測的概率密度函數p（xk| z1:k ：1）和似然概率密度函數p（zk| xk）符合高斯分布，即

式中：N（·；μ，∑）——高斯分布，其均值為μ、協方差矩陣為∑。

VB理論假設量測噪聲協方差Rk的先驗分布p（Rk| z1:k-1）服從逆威沙特（Inverse Wishart，IW）分布，即

式中：IW（·；μk，∑k）——自由度參數為μk和逆尺度矩陣為∑k的IW分布。由于量測噪聲在實際應用中變化非常緩慢，噪聲先驗參數的近似后驗值可通過一個系數ρ傳播

式中：ρ∈（0 1]——調整因子，矩陣B有0＜ |B|≤1，通常取。

為同時獲取Rk與xk的后驗概率密度函數，采用Rk與xk的近似后驗概率密度函數表示聯合概率密度函數p(xk,Rk| z1:k)

式中：q（·）——p（·）的近似后驗概率密度函數。通過最小化Kullback-Leibler散度（Kullback-Leibler Divergence,KLD）求解q(xk)和q(Rk)

式（16）的最優解滿足式（17）[8]：

通過定點迭代的方式逐步逼近式（17）的局部最優解，并以最后一次迭代值作為最終的估計結果。

2.2 VBACKF估計器設計

基于非線性雙軌車輛模型，選取估計器的狀態向量x、輸入向量u和量測向量z為

根據卡爾曼濾波原理，VBACKF估計器可分為時間預測和量測變分迭代更新兩步。

（1）時間預測

預測車輛運動狀態與系統狀態協方差：

量測噪聲參數預測，見式（14）。

（2）量測變分迭代更新

估計器容積點的選取與傳播：

估計器變分迭代初值的設定：

當i ≤ N時，進入以下變分迭代循環。N為迭代次數，i為當前次數

更新量測噪聲協方差；

更新車輛運動狀態及協方差：

更新量測噪聲參數：

當i ＞ N時，退出以上循環。并取第N次的迭代值為估計結果

為減少計算量，在獲得車輛運動狀態后，車輛質心側偏角通過式（26）計算：

3 仿真實驗驗證

基于CarSim-MATLAB/Simulink聯合仿真平臺，在雙移線工況與角階躍工況下進行仿真實驗，聯合仿真模型如圖2所示。

圖2 CarSim-MATLAB/Simulink聯合仿真模型Fig.2 CarSim-MATLAB/Simulink joint model

選用CarSim中的D-Class Sedan作為實驗車，車輛相關參數如表1。路面附著系數μ=0.85，采樣時間τ=0.005 s；VBACKF算法中調整因子ρ=0.97，迭代次數N=2。

表1 D-Class Sedan車輛參數Tab.1 Vehicle parameters of D-Class Sedan

雙移線工況的車輛初始速度為70 km/h。量測量(側向加速度ay和橫擺角速度r)添加的噪聲情況如圖3所示，其均值為0噪聲協方差隨時間階躍變化。

圖3 量測量時變噪聲設置Fig.3 Time-variant noise settings for measurement

以CarSim輸出的信號作為參考值，將VBACKF與CKF的估計結果作比較，如圖4所示。兩算法的絕對誤差曲線，如圖4（b）。對比估計誤差曲線，4 s之前，2種算法的估計誤差大致相同，而4 s之后，兩者的誤差出現差異，但VBACKF算法的誤差明顯較小。這是由于量測噪聲在4 s時發生變化，CKF算法不能對噪聲進行自適應，導致誤差較大。這種對比有力證明了VBACKF算法在時變量測噪聲協方差情況下的噪聲魯棒性。

圖4 雙移線工況仿真實驗結果Fig.4 Experimental result under the DLC condition

角階躍工況下車輛初始速度為30 km/h，在10 s時將方向盤轉角調整為60°。量測噪聲協方差變化情況如圖5。

圖5 量測量時變噪聲設置Fig.5 Time-variant noise settings for measurement

車輛質心側偏角的實驗結果如圖6所示。在10 s之前的勻速行駛工況下，VBACKF的估計誤差小于CKF的估計誤差，但相差不大。10 s之后，方向盤轉角變為60 °且量測噪聲協方差發生變化。此時，CKF算法的誤差出現明顯的波動與增加(見圖6（b）)。但VBACKF算法的誤差在經過一個短暫的跳動后，迅速收斂并穩定在一個較小的范圍內。以上實驗結果說明，VBACKF算法不僅在車輛保持勻速直線行駛時具有較高的精度，在量測噪聲協方差發生變化的角階躍工況下也具有更好的適應性和噪聲魯棒性。

圖6 角階躍工況仿真實驗結果Fig.6 Estimation result under angle step condition

4 結束語

本文基于雙軌車輛模型，采用VBACKF算法研究了時變量測噪聲協方差條件下的車輛質心側偏角估計問題。仿真實驗對比了本文提出的VBACKF算法與傳統CKF算法的估計效果。雙移線工況及角階躍工況的結果直接表明：VBACKF算法的車輛質心側偏角估計精度高于CKF算法。在量測噪聲協方差發生變化時，VBACKF的估計誤差明顯小于CKF算法的估計誤差，且估計誤差的波動較小、分布更穩定。結果證明了基于VBACKF算法的車輛運動狀態估計器估計精度高且具有噪聲魯棒性。