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

費(fèi)明哲,王健*,于金鵬,楊君,杜若飛,王云靖,鄧歡

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

0 引言

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

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

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

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

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

1.1 車輛模型

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

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

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

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

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

1.2 輪胎模型

輪胎支承車輛自重傳遞驅(qū)動(dòng)和制動(dòng)力矩,輪胎模型直接影響整車動(dòng)力學(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)估計(jì)算法

2.1 UKF算法

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

(1)

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

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

(2)

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

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

(3)

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

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

2)計(jì)算Sigma點(diǎn)集的一步預(yù)測(cè)

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

(4)

(5)

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

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

(6)

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

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

(7)

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

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

2.2 DUKF算法

圖2 DUKF算法運(yùn)行框架

2.3 SVD-DUKF算法

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

SVD算法在機(jī)器學(xué)習(xí)領(lǐng)域應(yīng)用廣泛。假設(shè)任意矩陣Am×n的SVD過(guò)程可表示為:

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

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

在UKF算法中采用SVD替代Cholesky分解時(shí),濾波過(guò)程的主體過(guò)程不變,僅改變產(chǎn)生Sigma點(diǎn)集的過(guò)程。對(duì)協(xié)方差矩陣Pt的SVD過(guò)程為:

Pt=UΣVT,

3 仿真結(jié)果對(duì)比

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

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

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

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

(8)

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

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

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

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

4 結(jié)論

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