基于改進雙重?zé)o跡卡爾曼濾波算法的車輛狀態(tài)估計

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

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

0 引言

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

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

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

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

1 車輛三自由度動力學(xué)模型

1.1 車輛模型

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

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

不考慮空氣阻力,車輛三自由度動力學(xué)模型方程包括縱向、側(cè)向和橫擺方向的動力學(xué)方程?？v向的動力學(xué)方程為:

側(cè)向的動力學(xué)方程為:

橫擺方向的動力學(xué)方程為:

1.2 輪胎模型

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

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

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

2 車輛狀態(tài)估計算法

2.1 UKF算法

UKF算法在卡爾曼估計結(jié)構(gòu)中通過UT變換調(diào)整均值和方差的連續(xù)方程進行估計,估計精度較高,在強非線性系統(tǒng)中具有一定的魯棒性。

(1)

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

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

(2)

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

計算采樣點的權(quán)值,公式為:

(3)

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

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

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

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

(4)

(5)

式中Q為系統(tǒng)過程噪聲協(xié)方差矩陣。

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

(6)

5)將式(6)代入式(1),得到預(yù)測的系統(tǒng)觀測量

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

(7)

式中R為測量噪聲協(xié)方差矩陣。

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

2.2 DUKF算法

圖2 DUKF算法運行框架

2.3 SVD-DUKF算法

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

SVD算法在機器學(xué)習(xí)領(lǐng)域應(yīng)用廣泛。假設(shè)任意矩陣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分解時,濾波過程的主體過程不變,僅改變產(chǎn)生Sigma點集的過程。對協(xié)方差矩陣Pt的SVD過程為:

Pt=UΣVT,

3 仿真結(jié)果對比

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

圖3 不同算法下車輛狀態(tài)估計結(jié)果

圖4 不同算法下車輛狀態(tài)估計誤差

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

(8)

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

表1 不同算法下車輛狀態(tài)估計的ERMS

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

圖5 不同算法下車輛參數(shù)估計結(jié)果

4 結(jié)論

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