基于改進雙重無跡卡爾曼濾波算法的車輛狀態估計

費明哲,王健*,于金鵬,楊君,杜若飛,王云靖,鄧歡

1.山東交通學院汽車工程學院,山東 濟南 250357;2.青島大學自動化學院,山東 青島 266071

0 引言

車輛運行時,從反饋參數中獲取精確、實時的狀態信息是提高車輛主動安全控制能力的重要保證,隨無人駕駛技術的發展,狀態估計和參數估計越來越重要,無人駕駛的最終目標是實現零人為干預,要求車輛有一定的自我校準能力。開發車輛動力學控制系統旨在提高車輛、用戶及周圍其他要素的安全性,采用主動控制系統防止由地形、外部因素或駕駛員輸入過快而導致的不當行為,控制器性能主要取決于它們對車輛狀態和參數的估計程度[1]。車輛動力學控制系統在很大程度上依賴車輛各部分慣性矩、輪胎剛度等參數[2],這些參數隨時間變化,無法直接測量,如何得到更準確的車輛行駛狀態和參數極為重要。

車輛行駛狀態估計方法主要有卡爾曼濾波(Kalman filter,KF)算法[3]及其衍生的各種相關算法,如神經網絡[4-5]、狀態觀測器[6]、模糊邏輯[7]方法等。在簡單的線性動力學模型和線性平衡系統中經常采用KF算法,根據系統的輸入和輸出計算最優估計[8]。輸出數據受外界噪聲干擾,在某些情況下可把系統的估計過程看作是濾波過程。KF算法主要用于線性高斯系統,當車輛表現出復雜非線性時的估計精度較低[9]。標準擴展卡爾曼濾波(extended Kalman filter,EKF)算法以線性最小方差估計為依據,通過遞推算法對狀態變量進行濾波估計,是車輛狀態估計領域的經典方法,應用廣泛[10-11],若系統具有高非線性度時,EKF算法很容易在初始系統的一階線性分析中產生嚴重的線性誤差,且很難計算出所需的雅可比矩陣,同時該算法為無限增長的記憶濾波器,計算效率和估計精度會隨時間的推移而降低。文獻[12]提出了一種基于限定記憶隨機加權擴展卡爾曼濾波(limited memory random weighted extended Kalman flter,LMRWEKF)算法,響應速度較快,估計精度較高。

無跡卡爾曼濾波(unscented Kalman filter,UKF)算法未采用KF和EKF的方法,即將系統的非線性函數線性化,而是在卡爾曼估計結構中通過無跡變換(unscented transform,UT)方法,調整均值和方差的連續方程進行預測[13-14]。UKF算法無需運算雅可比矩陣,也無需忽略函數中的高階元素,減小了估計誤差,具有測量非線性分布的高精度計算能力[15]。文獻[16]在UKF算法中加入遺忘因子進行車輛參數估計,提高了算法對噪聲的抑制能力。文獻[17]提出一種改進的迭代無跡卡爾曼濾波(improved iterated unscented Kalman filter,IIUKF)算法,在系統方程處于非常嚴重的非線性狀態時,該算法具有較高的估計精度。文獻[18]提出了一種基于蟻群優化算法的UKF狀態估計自適應濾波算法,運用蟻群算法的尋優能力,提高了算法的魯棒性和估計精度。

本文基于車輛三自由度動力學模型,分別采用UKF算法、雙重無跡卡爾曼濾波算法(dual unscented Kalman filter,DUKF)和采用奇異值分解的改進雙重無跡卡爾曼濾波算法(singular value decomposition-dual unscented Kalman filter,SVD-DUKF)估計車輛行駛狀態及車輛參數,評估3種算法在車輛一定行駛工況下的狀態及參數估計能力。

1 車輛三自由度動力學模型

1.1 車輛模型

只考慮各輪胎的平面運動,忽略滾動和俯仰運動,建立包含縱向、側向、橫擺的車輛三自由度動力學非線性灰箱模型,如圖1所示。

a—質心到前軸的距離;b—質心到后軸的距離;Fxfl—左前輪的縱向力;Fxfr—右前輪的縱向力;Fxrl—左后輪的縱向力;Fxrr—右后輪的縱向力;Fyfl—左前輪的側向力;Fyfr—右前輪的側向力;δ—前輪轉角;ωr—橫擺角速度,vx—質心處的縱向速度;vy—質心處的側向速度。

不考慮空氣阻力,車輛三自由度動力學模型方程包括縱向、側向和橫擺方向的動力學方程。縱向的動力學方程為:

側向的動力學方程為:

橫擺方向的動力學方程為:

1.2 輪胎模型

輪胎支承車輛自重傳遞驅動和制動力矩,輪胎模型直接影響整車動力學模型的仿真結果[19]。輪胎模型為:

Fxf=Cxsf,Fxr=Cxsr,Fyf=Cyαf,Fyr=Cyαr,

式中:Fxf、Fxr分別為前軸輪胎、后軸輪胎的縱向力;Fyf、Fyr分別為前軸輪胎、后軸輪胎的側向力;Cx、Cy分別為輪胎的縱向、側向剛度,設輪胎的剛度相同;sf、sr分別為前軸輪胎、后軸輪胎的滑移率;αf、αr分別為前軸輪胎、后軸輪胎的滑移角,其中αf=δ-arctan[(vy+aωr)/vx],αr=-arctan[(vy-bωr)/vx]。

2 車輛狀態估計算法

2.1 UKF算法

UKF算法在卡爾曼估計結構中通過UT變換調整均值和方差的連續方程進行估計,估計精度較高,在強非線性系統中具有一定的魯棒性。

(1)

式中:Z(t)為考慮噪聲的系統觀測矩陣,f(·)為非線性狀態方程函數,h(·)為非線性觀測方程函數,W(t)為系統估計過程中的高斯白噪聲,V(t)為系統測量高斯白噪聲。

UKF算法通過UT變換得到X(t)的統計特征,即選取2n+1個采樣點,公式為:

(2)

式中:X(i)為第i個采樣點;P為x的協方差矩陣;λ為縮放比例系數,λ=d2(n+κ)-n,其中d為系數,d的選取控制采樣點的分布狀態,κ為待選參數,其取值沒有界限,但通常應確保矩陣(n+λ)P為半正定矩陣。

計算采樣點的權值,公式為:

(3)

UKF算法基本步驟有以下8步。

1)根據式(2)獲取1組采樣點(即Sigma點集)

2)計算Sigma點集的一步預測

X(i)(t+1|t)=f[t,X(i)(t|t)]。

(4)

(5)

式中Q為系統過程噪聲協方差矩陣。

4)對式(5)再次進行UT變換,產生新的Sigma點集

(6)

5)將式(6)代入式(1),得到預測的系統觀測量

Z(i)(t+1|t)=h(X(i)(t+1|t))。

(7)

式中R為測量噪聲協方差矩陣。

7)計算卡爾曼增益矩陣K(t+1)=PxtztPztzt-1。

2.2 DUKF算法

圖2 DUKF算法運行框架

2.3 SVD-DUKF算法

車輛動力學系統的非線性程度較高時,UKF算法的估計精度下降甚至發散。分析UKF算法的運行過程可知,UT變換過程中,生成Sigma點集時使用Cholesky分解運算誤差協方差矩陣,需保證其是非負定矩陣,但在車輛運行過程中,系統協方差矩陣易受噪聲影響,導致其成為奇異矩陣而無法處理,從而結束濾波過程,使濾波結果精度下降甚至發散。本文在UT變換中引入奇異值分解(singular value decomposition, SVD)替代Cholesky分解,使算法具有更強的魯棒性。

SVD算法在機器學習領域應用廣泛。假設任意矩陣Am×n的SVD過程可表示為:

Am×n=Um×mΣm×nVn×nT,

式中:U為正交矩陣,即滿足UTU=Im×m,且U∈Rm×m;Σ除主對角線外的元素都為0,主對角線上的每個元素稱為奇異值;V為正交矩陣,即滿足VTV=In×n,且V∈Rn×n。

在UKF算法中采用SVD替代Cholesky分解時,濾波過程的主體過程不變,僅改變產生Sigma點集的過程。對協方差矩陣Pt的SVD過程為:

Pt=UΣVT,

3 仿真結果對比

以MATLAB內置的高剛度輪胎模擬數據為仿真輸入數據,前輪的滑移輸入為具有恒定偏移的正弦曲線,ωr輸入也是正弦曲線,但振幅和頻率與滑移輸入不同。采用UKF算法估計車輛狀態時,因無法估計車輛參數,視正確參數為已知進行仿真。運行仿真時,設每秒迭代10次,仿真結果見圖3,誤差見圖4。

圖3 不同算法下車輛狀態估計結果

圖4 不同算法下車輛狀態估計誤差

為進一步分析仿真結果,用均方根誤差衡量不同算法的估計性能,均方根誤差

(8)

根據式(8),得到不同算法下車輛狀態估計的ERMS,如表1所示。由圖3、4及表1可知:3種算法的估計結果均較好,但UKF算法中將m等參數列為已知項,與實際情況不符;DUKF算法可同時估計車輛的狀態和車輛參數,但在UT變換進行Cholesky分解時,要求協方差為非負定矩陣,有一定限制;SVD-DUKF算法的魯棒性更強,更適合汽車強非線性系統。

表1 不同算法下車輛狀態估計的ERMS

在該車輛動力學模型中,已知m=1 700 kg,Cx=1.5×105N/rad,Cy=5.0×104N/rad。DUKF算法和SVD-DUKF算法對車輛參數估計結果如圖5所示。由圖5可知:估計m和Cy時,DUKF算法的波動較小,SVD-DUKF算法的誤差隨仿真進程逐漸減小,最后與DUKF算法均收斂至真實值附近;估計Cx時,SVD-DUKF算法能較快地收斂至真實值并在附近波動,SVD-DUKF算法比DUKF算法的適用性更高。

圖5 不同算法下車輛參數估計結果

4 結論

基于車輛三自由度動力學模型,分別采用UKF、DUKF、SVD-DUKF 3種算法估計車輛運行狀態及車輛參數,在同一工況下采用MATLAB仿真進行比較分析。UKF算法估計車輛狀態時需輸入車身質量等參數,且隨乘客及貨物的改變,在車輛實際行駛過程中很難獲取正確參數,難以單獨在實車中應用算法;DUKF算法通過UT變換獲取Sigma采樣點集時需進行Cholesky分解,此時需保證協方差為非負定矩陣,否則會導致估計結果不準確甚至發散;通過引入奇異值分解改進DUKF算法,在保證估計精度的同時,算法的適用范圍更廣,魯棒性更強。