S-修正的自適應(yīng)卡爾曼濾波與模糊卡爾曼濾波相結(jié)合的汽車狀態(tài)估計(jì)算法

黃 超 林 棻

南京航空航天大學(xué)，南京，210016

0 引言

準(zhǔn)確而實(shí)時(shí)地獲取汽車行駛過程中的狀態(tài)信息是汽車動(dòng)態(tài)控制系統(tǒng)研究的關(guān)鍵，由此衍生出的汽車狀態(tài)估計(jì)器的設(shè)計(jì)逐漸成為近年來研究的熱點(diǎn)。目前汽車狀態(tài)估計(jì)器設(shè)計(jì)所采用的方法主要有線性卡爾曼濾波（Kalman filter，KF）、擴(kuò)展卡爾曼濾波［1-3］、Unscented卡爾曼濾波［4-5］、神經(jīng)網(wǎng)絡(luò)［6］、狀態(tài)觀測器［7］、模糊邏輯［8］等，這些方法都是對(duì)汽車控制系統(tǒng)中的關(guān)鍵控制變量（包括質(zhì)心側(cè)偏角、側(cè)向速度、橫擺角速度等）進(jìn)行估計(jì)。

上述算法中廣泛應(yīng)用的是基于KF的改進(jìn)算法，而在常規(guī)KF算法中，量測噪聲的統(tǒng)計(jì)特性在濾波過程中預(yù)先設(shè)為定值，若噪聲水平改變，將會(huì)使得估計(jì)精度降低甚至導(dǎo)致濾波發(fā)散。為了提高基于KF汽車狀態(tài)估計(jì)算法的魯棒性與估計(jì)精度，本文提出一種改進(jìn)的自適應(yīng)模糊卡爾曼濾波算法，該算法是S-修正的自適應(yīng)卡爾曼濾波（以下簡稱S-AKF）算法與模糊卡爾曼濾波（FKF）算法的結(jié)合，其中的S-AKF算法基于數(shù)學(xué)理論推導(dǎo)直接對(duì)估計(jì)誤差協(xié)方差矩陣進(jìn)行加權(quán)處理，而模糊卡爾曼濾波基于模糊邏輯推理，根據(jù)實(shí)時(shí)得到的量測信息的實(shí)際方差與理論方差的比值，由設(shè)計(jì)的模糊系統(tǒng)在線實(shí)時(shí)調(diào)整量測噪聲矩陣。這使濾波算法不需要得到準(zhǔn)確的量測噪聲矩陣的先驗(yàn)知識(shí)，且對(duì)于時(shí)變的量測噪聲也能得到準(zhǔn)確的估計(jì)值。

由于FKF算法是對(duì)量測噪聲的協(xié)方差矩陣進(jìn)行自適應(yīng)估計(jì)，狀態(tài)量的估計(jì)精度會(huì)受到不準(zhǔn)確的過程噪聲的統(tǒng)計(jì)特性影響，而S-AKF恰可以彌補(bǔ)這個(gè)缺陷，故本文將S-AKF與FKF相結(jié)合，可以在很大程度上提高算法的估計(jì)精度及魯棒性。

1 汽車動(dòng)力學(xué)模型

僅考慮汽車沿y軸的側(cè)向運(yùn)動(dòng)與繞z軸的橫擺運(yùn)動(dòng)這兩個(gè)自由度，另外限定汽車的側(cè)向加速度在0.4g以下，輪胎側(cè)偏特性處于線性范圍，因此建立線性二自由度汽車模型如圖1所示，模型描述［9］如下：

式中，m為整車質(zhì)量；Iz為整車對(duì)z軸轉(zhuǎn)動(dòng)慣量；a為質(zhì)心到前軸的距離；b為質(zhì)心到后軸的距離；k1為前輪側(cè)偏剛度；k2為后輪側(cè)偏剛度；vx為質(zhì)心處縱向速度；vy為質(zhì)心處側(cè)向速度；δ為前輪轉(zhuǎn)角；ax為縱向加速度；ay為側(cè)向加速度；β為質(zhì)心側(cè)偏角；ωr為橫擺角速度。

由運(yùn)動(dòng)學(xué)的基本關(guān)系并結(jié)合圖1，可得tanβ＝vy／vx，由于β 較小，則可得

將式（1）～ 式（4）整理后得狀態(tài)方程和觀測方程如下：

圖1 汽車二自由度模型示意圖

2 S-AKF算法

對(duì)于線性常規(guī)Kalman濾波算法，系統(tǒng)的狀態(tài)空間模型為

式中，Xk為狀態(tài)向量；Φk／k-1為狀態(tài)轉(zhuǎn)移矩陣；Hk為量測矩陣；Zk為觀測向量；W、V分別為過程噪聲向量和量測噪聲向量。

濾波的預(yù)測與更新過程如下：

狀態(tài)預(yù)測公式為

方差預(yù)測公式為

濾波增益公式為

方差更新公式為

狀態(tài)更新公式為

式中，P為誤差協(xié)方差矩陣；Q、R分別為過程噪聲協(xié)方差矩陣和量測噪聲協(xié)方差矩陣；K為濾波增益矩陣；I為單位矩陣。

Sk值的選擇過程如下：

判斷濾波發(fā)散性的條件是

又有

最后可得

具體的推導(dǎo)過程參考文獻(xiàn)［10］。

3 FKF算法及整體程序流程

常規(guī)KF算法假定觀測噪聲為零均值的高斯白噪聲，且已知方差陣R。對(duì)于汽車行駛的實(shí)際過程，觀測噪聲的統(tǒng)計(jì)特性是不確定的，它易受外界環(huán)境的影響，通過引入模糊控制器可以在線估計(jì)R值［11］。定義k時(shí)刻觀測噪聲方差的估計(jì)值為＾Rk，且有

式中，α為自適應(yīng)調(diào)整因子。

定義k時(shí)刻信息的實(shí)際方差與理論方差的比值為

式中，Ak為新息的實(shí)際方差；Tk為新息的理論方差。

3.1 模糊規(guī)則的設(shè)計(jì)

本文中模糊控制器以信息的實(shí)際方差與理論方差的比值為輸入，以自適應(yīng)調(diào)整因子α為輸出。最優(yōu)情況下，信息的理論方差應(yīng)近似等于實(shí)際方差值，如果理論方差長期偏離實(shí)際方差，說明觀測噪聲統(tǒng)計(jì)特性發(fā)生了變化，需要對(duì)理論方差進(jìn)行調(diào)整，以使其回到實(shí)際方差附近。根據(jù)上述思想，設(shè)計(jì)模糊控制器。上述設(shè)計(jì)過程借助MATLAB中的FUZZY工具箱完成，然后利用Simulink模糊控制器將上述模糊工具箱封裝起來，形成Fuzzy＿R模糊控制器，基本步驟如圖2所示。

圖2 Fuzzy＿R模糊控制器

定義輸入輸出的模糊子集如下：L1表示小于1，E1表示約等于1，M1表示大于1。本文采用的輸入輸出隸屬度函數(shù)為三角形隸屬度函數(shù)，具體如圖3和圖4所示。

模糊邏輯規(guī)則如下：

解模糊化方法有最大隸屬度法、重心法、加權(quán)平均法等，本文采用目前應(yīng)用較多的重心法進(jìn)行求解。

圖3 輸入的隸屬度函數(shù)

圖4 輸出的隸屬度函數(shù)

3.2 M文件與Simulink混合編程流程

Simulink是基于MATLAB的框圖式仿真環(huán)境，具有結(jié)構(gòu)清晰、建模迅速等優(yōu)勢，但實(shí)現(xiàn)循環(huán)等操作復(fù)雜，執(zhí)行效率低；M文件編程環(huán)境代碼執(zhí)行效率高，但程序結(jié)構(gòu)復(fù)雜，建模較困難。本文結(jié)合兩者的優(yōu)勢，程序主體結(jié)構(gòu)采用M文件來編寫，模糊控制器采用Simulink框圖來搭建，并供主程序調(diào)用。從圖5可看出，濾波循環(huán)過程在KF的基礎(chǔ)上依次順序加入了式（8）以及圖2的Fuzzy＿R模糊控制器，這樣在每一次循環(huán)中都可以利用FKF對(duì)量測噪聲協(xié)方差矩陣進(jìn)行在線估計(jì)，并利用S-AKF對(duì)估計(jì)誤差協(xié)方差矩陣直接加權(quán)，給濾波過程加入了“雙保險(xiǎn)”，使得算法更加有效。

圖5 程序流程框圖

4 虛擬試驗(yàn)驗(yàn)證

為了檢驗(yàn)算法的正確性，在虛擬樣機(jī)軟件ADAMS／CAR中建立整車模型，為了模擬極限工況下的汽車操縱響應(yīng)，在仿真中采用驅(qū)動(dòng)樣機(jī)產(chǎn)生汽車的行駛路徑，如圖6所示。進(jìn)行雙移線工況仿真，整車的參數(shù)如下：m＝1528kg；Iz＝2440kg· m2；a ＝ 1.48m；b ＝ 1.08m；k1＝-226180N／rad；k2＝-294 390N／rad；試驗(yàn)車速為80km／h，整個(gè)過程歷時(shí)10s；Qk＝I3×3，R0＝1。為充分檢驗(yàn)算法的魯棒性與估計(jì)精度，給觀測量ay加入時(shí)變的高斯白噪聲，如圖7所示，具體如表1所示。

圖6 汽車的行駛路徑

圖7 帶有時(shí)變噪聲的側(cè)向加速度

表1 量測值時(shí)變噪聲方差值

從圖8、圖9可以看出，由于噪聲時(shí)變的影響，使得橫擺角速度與質(zhì)心側(cè)偏角的估計(jì)值偏離虛擬試驗(yàn)值，估計(jì)誤差增大。

圖8 未加入自適應(yīng)算法的橫擺角速度估計(jì)值

圖9 未加入自適應(yīng)算法的質(zhì)心側(cè)偏角估計(jì)值

綜合圖10、圖11可以看出，單純采用FKF算法時(shí)，質(zhì)心側(cè)偏角的估計(jì)值可以平穩(wěn)地跟蹤虛擬試驗(yàn)值的結(jié)果，但橫擺角速度的估計(jì)值仍然出現(xiàn)一定的誤差。這是因?yàn)闋顟B(tài)量的估計(jì)精度不僅與量測噪聲有關(guān)，而且還與過程噪聲有關(guān)，且仿真中將Qk設(shè)為定值，不準(zhǔn)確的過程噪聲協(xié)方差矩陣導(dǎo)致了橫擺角速度的估計(jì)誤差。

圖10 不同自適應(yīng)算法的橫擺角速度估計(jì)值

圖11 不同自適應(yīng)算法的質(zhì)心側(cè)偏角估計(jì)值

當(dāng)算法中加入S-AKF之后，F(xiàn)KF＋SAKF算法的估計(jì)結(jié)果較之前的算法有了很大改善，橫擺角速度與質(zhì)心側(cè)偏角的估計(jì)值均能較好地跟蹤虛擬試驗(yàn)值的結(jié)果，說明S-AKF的加入可以彌補(bǔ)FKF算法的不足，從而可以獲得更高的魯棒性與估計(jì)精度。

基于個(gè)人電腦和MATLAB環(huán)境的程序累積運(yùn)算時(shí)間為8.9s。在時(shí)間歷程為10s的虛擬試驗(yàn)下具有較好的實(shí)時(shí)性。

為了定量的比較兩種算法的估計(jì)精度，給出了估計(jì)值相對(duì)于實(shí)際值的平均絕對(duì)誤差（MAE）和均方根誤差（RSME），如表2、表3所示。

表3 3種算法的RSME指標(biāo)

從表2和表3可以看出，隨著算法的改進(jìn)，橫擺角速度的MAE和RSME指標(biāo)明顯逐步變好；質(zhì)心側(cè)偏角的MAE和RSME指標(biāo)呈現(xiàn)下降趨勢，雖然FKF＋S-AKF指標(biāo)相對(duì)于FKF時(shí)略微反彈，但差別不大。從算法的總體效果來看，F(xiàn)KF＋S-AKF算法優(yōu)于其他的算法。

3.1 節(jié)中設(shè)計(jì)的模糊控制器對(duì)濾波過程中的每一次遞推循環(huán)均進(jìn)行噪聲協(xié)方差的自適應(yīng)調(diào)整，與表1進(jìn)行對(duì)應(yīng)可得具體的調(diào)整效果，如圖12所示。

圖12 量測噪聲協(xié)方差真實(shí)值與估計(jì)值

從圖12可以看出，模糊控制器可以較好地跟蹤量測噪聲協(xié)方差矩陣的變化，所以本文的算法要比非自適應(yīng)算法具有更高的估計(jì)精度。

5 結(jié)論

（1）提出將S-AKF與FKF相結(jié)合的汽車狀態(tài)估計(jì)算法，F(xiàn)KF可以對(duì)時(shí)變噪聲的協(xié)方差矩陣進(jìn)行在線估計(jì)，實(shí)現(xiàn)其自適應(yīng)；S-AKF算法是一種實(shí)際不發(fā)散的濾波算法，有較好的濾波精度，F(xiàn)KF＋S-AKF算法進(jìn)一步提高算法的魯棒性與估計(jì)精度。

（2）通過虛擬試驗(yàn)驗(yàn)證表明，F(xiàn)KF＋S-AKF算法的估計(jì)結(jié)果可以很好地跟蹤虛擬試驗(yàn)值，因而優(yōu)于其他算法。

（3）后續(xù)研究將致力于選擇更優(yōu)的模糊控制器，并應(yīng)用到非線性汽車模型中，為汽車穩(wěn)定性控制系統(tǒng)的研究提供理論指導(dǎo)。

［1］林棻，趙又群.基于雙重?cái)U(kuò)展自適應(yīng)卡爾曼濾波的汽車狀態(tài)和參數(shù)估計(jì)［J］.中國機(jī)械工程，2009，20（6）：750-755.Lin Fen，Zhao Youqun.Vehicle State and Parameter Estimation Based on Dual Extended Adaptive Kalman Filter［J］.China Mechanical Engineering，2009，20（6）：750-755.

［2］郭孔輝，付皓，丁海濤.基于擴(kuò)展卡爾曼濾波的汽車質(zhì)心側(cè)偏角估計(jì)［J］.汽車技術(shù)，2009（4）：1-3.Guo Konghui，F(xiàn)u Hao，Ding Haitao.Estimation of CG Sideslip Angle Based on Extended Kalman Filter［J］.Automobile Technology，2009（4）：1-3.

［3］Chen B C，Hsieh F C.Sideslip Angle Estimation Using Extended Kalman Filter［J］.Vehicle System Dynamics，2008，46，353-364.

［4］趙又群，林棻.基于UKF算法的汽車狀態(tài)估計(jì)［J］.中國機(jī)械工程，2010，21（5）：615-619.Zhao Youqun，Lin Fen.Vehicle State Estimation Based on Unscented Kalman Filter Algorithm［J］.China Mechanical Engineering，2010，21（5）：615-619.

［5］Antonov S，F(xiàn)ehn A，Kugi A.Unscented Kalman Filter for Vehicle State Estimation［J］.Vehicle System Dynamics，2011，49（9）：1497-1520.

［6］Melzi S，Sabbioni E.On the Vehicle Sideslip Angle Estimation through Neural Networks：Numerical and Experimental Results［J］.Mechanical Systems and Signal Processing，2011，25（6）：2005-2019.

［7］Cheli F，Melzi S，Sabbioni E.An Adaptive Observer for Sideslip Angle Estimation：Comparison with Experimental Results［C］／／ASME Conference Proceedings.Las Vegas，2007：1193-1199.

［8］施樹明，Lupker H，Bremmer P，等.基于模糊邏輯的車輛側(cè)偏角估計(jì)方法［J］.汽車工程，2005（4）：426-430.Shi Shuming，Lupker H，Bremmer P，et al.Estimation of Vehicle Side Slip Angle Based on Fuzzy Logic［J］.Automotive Engineering，2005，27（4）：426-430.

［9］余志生.汽車?yán)碚摚跰］.北京：機(jī)械工業(yè)出版社，2009.

［10］宋文堯，張牙.卡爾曼濾波［M］.北京：科學(xué)出版社，1991.

［11］肖志濤，趙培培，李士心.基于INS／GPS組合導(dǎo)航的自適應(yīng)模糊卡爾曼濾波［J］.中國慣性技術(shù)學(xué)報(bào)，2010（2）：195-198.Xiao Zhitao，Zhao Peipei，Li Shixin.Adaptive Fuzzy Kalman Filter Based on INS／GPS Integrated Navigation System ［J］.Journal of Chinese Inertial Technology，2010（2）：195-198.