基于P-NAEKF 算法的汽車行駛狀態(tài)估計

戴凌宇，馮張棋，馬成宇，胡子添

(1.南通大學 電氣工程學院，江蘇 南通 216000;2.江蘇大學 汽車與交通工程學院，江蘇 鎮(zhèn)江 212013;3.江蘇大學 汽車工程研究院，江蘇 鎮(zhèn)江 212013)

0 引言

近年來，自動駕駛技術(shù)飛速發(fā)展，其安全性被社會廣泛關(guān)注。隨著自動駕駛技術(shù)離開實驗室走向社會，將面臨更加復雜的交通狀況，為了確保車輛自身的安全性，它需要準確感知面臨的環(huán)境并做出合理決策［1］。目前很多高精度的傳感器因為產(chǎn)品價格問題不能普及到所有的汽車，有些關(guān)鍵狀態(tài)參數(shù)難以直接量測，所以需要使用一些較為廉價的傳感器先獲取一部分狀態(tài)信息，利用這些信息對不宜直接測量的關(guān)鍵車輛狀態(tài)進行估計，有效保障汽車安全自動駕駛［2］。

目前針對汽車的狀態(tài)估計的方法主要有:擴展卡爾曼濾波、無跡卡爾曼濾波、容積卡爾曼濾波、粒子濾波、滑模觀測器和神經(jīng)網(wǎng)絡的方法［3-8］。趙強等［9］考慮車輛側(cè)向運動、橫擺運動和側(cè)傾運動，建立非線性模型，設計了擴展Kalman濾波器來過濾噪聲追蹤實際值;方賢生等［10］將模糊控制引入到無跡卡爾曼濾波算法中，提出了一種模糊自適應無跡卡爾曼濾波算法完成車輛狀態(tài)參數(shù)估計;邢德鑫等［11］針對車輛狀態(tài)估計中的強非線性、噪聲非高斯問題設計了一種自適應容積粒子濾波狀態(tài)估計器。以Kalman 濾波理論為基礎的車輛狀態(tài)估計方法是當前研究的主流方向［12］。

郝亮等［13］基于3 自由度的車輛模型采用標準的擴展卡爾曼濾波(EKF)方法對橫擺角速度和質(zhì)心側(cè)偏角進行了估計，仿真驗證了其有效性，但其噪聲特性是通過試驗和經(jīng)驗得到的，難以驗證其準確性，影響了濾波效果;Li 等［14］提出了基于多種傳感器信息融合的EKF 汽車狀態(tài)估計方法，雖然考慮了多種噪聲特性集合，但本質(zhì)上還是假設噪聲統(tǒng)計特性已知，適用范圍有限;李靜等［15］提出了一種自適應容積卡爾曼濾波，它是將標準的容積卡爾曼濾波(cubature kalman filter，CKF)與Sage-Husa 次優(yōu)估計算法結(jié)合在一起，對部分車輛參數(shù)實時在線估計，雖然該方法解決了噪聲協(xié)方差的估計問題，但是Sage-Husa 次優(yōu)估計方法容易使噪聲協(xié)方差失去正定性而導致濾波發(fā)散。

根據(jù)上述討論，以往對于EKF 在車輛狀態(tài)估計的研究中，在噪聲協(xié)方差的不準確性問題上還存在一些不足，為了改善這一問題，本文分析了噪聲協(xié)方差不準確給狀態(tài)估計結(jié)果帶來的影響，給出了2 種噪聲協(xié)方差的估計方法:基于Sage-Husa的次優(yōu)估計方法和基于粒子群的噪聲協(xié)方差優(yōu)化估計方法，分析了Sage-Husa 次優(yōu)估計算法容易發(fā)散和過于依賴統(tǒng)計特性的不足，選擇粒子群算法對不精準的噪聲協(xié)方差進行優(yōu)化估計，并將這一部分作為參數(shù)估計模塊與傳統(tǒng)EKF 算法相結(jié)合，提出噪聲自適應的EKF 算法(P-NAEKF)，該算法能在噪聲協(xié)方差不準確時對其進行估計修正，大大提高了濾波精度，經(jīng)過仿真驗證，這種算法能夠完成車輛行駛狀態(tài)的準確估計。

1 3 自由度車輛模型

1.1 模型構(gòu)建

構(gòu)建基于縱向車速，橫擺角速度和質(zhì)心側(cè)偏角的3 自由度模型［13］如圖1 所示。

圖1 3 自由度非線性模型示意圖

圖1 中，XOY 是以車輛質(zhì)心為原點的坐標系，其中:X 軸與車輛的縱向?qū)ΨQ軸重疊，并且選擇向右為正方向;Y 軸通過車輛的質(zhì)心，并選擇向上為正向。選擇質(zhì)心側(cè)偏角，橫擺角速度和縱向速度為狀態(tài)量，選擇縱向加速度為量測量，選擇橫向加速度和方向盤轉(zhuǎn)角為控制量構(gòu)建如下的3 自由度非線性汽車狀態(tài)方程:

式中:β 為車輛質(zhì)心側(cè)偏角;ω 為車輛橫擺角速度;u 為車輛縱向速度;ax為車輛縱向加速度;ay為車輛側(cè)向加速度;δ 是車輛的前輪轉(zhuǎn)角。

1.2 模型轉(zhuǎn)換

使用擴展Kalman 濾波算法對式(1)進行狀態(tài)估計，假設u 在采樣周期T=0.01 s 內(nèi)不變，將汽車狀態(tài)方程離散化:

將式(1)與式(3)聯(lián)立方程組并移項可得

式中，相關(guān)參數(shù)值如表1 所示。

表1 車輛參數(shù)

2 傳統(tǒng)EKF 算法及問題描述

2.1 傳統(tǒng)EKF 算法

基于式(3)和式(4)可知狀態(tài)方程和量測方程可以表示為:

式中，狀態(tài)變量x=［β，ω，u］T，輸出變量z=［ay］T，控制變量u=［δ，ax］，wk-1與vk為互不相關(guān)的高斯白噪聲，且服從正態(tài)分布。

傳統(tǒng)的EKF 用于汽車狀態(tài)估計時，噪聲協(xié)方差R 和過程噪聲協(xié)方差Q 往往是通過不斷調(diào)試或是經(jīng)驗得到的［16］，此時可以得到EKF 的迭代方程:

式中:A 和H 是通過求取狀態(tài)x 的雅可比矩陣得到的，將式(4)和式(5)展開可得式(13)和式(14)，再依據(jù)式(15)和式(16)求解得到A 和H。

2.2 性能匹配分析

在實際汽車狀態(tài)估計時，復雜的路況環(huán)境是千變?nèi)f化的，傳感器受到干擾往往會導致噪聲協(xié)方差存在誤差，對濾波性能的干擾是極其巨大的［16］。當系統(tǒng)存在不匹配的噪聲方差，可以得到:

式中:Rk表示真實的量測噪聲協(xié)方差;Qk|k-1表示真實的過程噪聲協(xié)方差;上標f 表示實際應用假設值;ΔRk和ΔQk|k-1表示實際應用假設值和真實值的偏差。

在濾波過程中，通常用估計誤差協(xié)方差來表示濾波估計的均方誤差(mean squared error，MSE)，根據(jù)不準確的噪聲方差和乘性噪聲相關(guān)系數(shù)，得到不準確的均方誤差(FMSE)為:

其中

式中，上標f 表示濾波器實際的計算值，但是這個計算值與真實值存在一定的偏差，所以并不是真實的MSE。真實的均方誤差(TMSE)為:

其中

對比式(19)和式(21)可以發(fā)現(xiàn)，當噪聲協(xié)方差存在誤差時，仍然使用不精準的噪聲協(xié)方差來設計Kalman 濾波器，將會導致估計誤差協(xié)方差與真實的TMSE 存在偏差，Kalman 濾波器的精度降低，因此有必要設計一種噪聲自適應濾波算法解決噪聲協(xié)方差不精準的問題。

3 2 種參數(shù)優(yōu)化估計方法

3.1 基于Sage-Husa 的噪聲協(xié)方差估計方法

Sage-Husa 算法［17］是一種利用協(xié)方差特性估計噪聲協(xié)方差的方法，該方法已得到證明，現(xiàn)作簡要描述:

式中，dk=(1-c)/(1-ck+1)，0＜c＜1 為遺忘因子。

但是直接使用這種算法估計噪聲協(xié)方差存在一些問題:

1)式(23)和式(24)中運用了減法運算，雖然能夠滿足無偏估計的要求，但容易使得噪聲協(xié)方差失去正定性導致濾波發(fā)散。

2)過于依賴噪聲統(tǒng)計特性，估計效果取決于信息的好壞。

3.2 基于粒子群算法的噪聲協(xié)方差估計方法

3.1 節(jié)中介紹了基于Sage-Husa 的噪聲協(xié)方差估計方法，但存在一些問題，于是本小節(jié)以汽車狀態(tài)估計擴展Kalman 濾波器為研究對象，針對過程噪聲協(xié)方差和量測噪聲協(xié)方差不精準的情況，提出一種新的估計方法即粒子群算法估計噪聲協(xié)方差。

新息協(xié)方差的計算值為:

新息協(xié)方差的實際值為:

根據(jù)式(23)式(24)可以得到:

為了保證ΔRk和ΔQk|k-1盡可能小，設置適應度函數(shù):

在每次迭代后，產(chǎn)生一組最優(yōu)值，即個體最優(yōu)值(gbest)和全局最優(yōu)值(pbest)，粒子通過最優(yōu)值更新位置和速度，即:

通過迭代尋優(yōu)得到適應度最小的粒子位置b=(b1，b2，…，bD)T，即得到噪聲協(xié)方差的估計值:

該算法與3.1 節(jié)提出的傳統(tǒng)Sage-Husa 方法相比，保證了正定，提高了算法的收斂性。

參數(shù)估計算法流程圖2 所示。

圖2 噪聲協(xié)方差估計算法流程圖

4 P-NAEKF 算法

基于3.2 節(jié)的參數(shù)估計方法，可以得到較為準確的噪聲協(xié)方差的值，將這種參數(shù)估計方法嵌入傳統(tǒng)的EKF 算法中，可以得到P-NAEKF 算法。

P-NAEKF 算法流程如表2 所示。

表2 P-NAEKF 算法流程

5 仿真分析

5.1 Carsim 與Matlab/Simulink 聯(lián)合仿真平臺

在本文中，首先在Carsim 中搭建車輛模型，并設定工況［18］，將其與Matlab/Simulink 中建立的傳統(tǒng)的EKF 算法模塊與本文的P-NAEKF 算法模塊聯(lián)合仿真，對汽車的縱向車速、橫擺角速度和質(zhì)心側(cè)偏角等相關(guān)參數(shù)進行實時估計，并對比估計結(jié)果，驗證P-NAEKF 算法的優(yōu)越性。

聯(lián)合仿真平臺如圖3 所示。

圖3 Carsim 與Matlab/Simulink 聯(lián)合仿真平臺框圖

工況設定:選取Carsim 中自帶的雙移線工況，路面附著系數(shù)為0.855，其他車輛參數(shù)見表1 所示，準確的噪聲參數(shù)為:

將傳感器測量的縱向加速度、側(cè)向加速度以及方向盤轉(zhuǎn)角信號作為估計模塊輸入信號，3 個輸入信號如圖4—6 所示。

圖4 側(cè)向加速度輸入信號

圖6 方向盤轉(zhuǎn)角輸入信號

5.2 參數(shù)估計

本章節(jié)使用2 種參數(shù)估計方法對噪聲協(xié)方差進行估計，進行50 次蒙特卡洛實驗，并對估計效果進行比較，結(jié)果如圖7 所示。

方法1:傳統(tǒng)的Sage-Husa 算法，對應文中3.1節(jié)提出的方法;

方法2:基于粒子群算法的參數(shù)優(yōu)化估計方法，對應文中3.2 節(jié)提出的方法。

這一部分對過程噪聲協(xié)方差進行了估計，比較2 種不同的估計方法，觀察圖7 可以清晰地發(fā)現(xiàn)，方法2 的估計效果明顯好于方法1，且50 次的結(jié)果均穩(wěn)定在真實值附近，誤差在0～±2.5 之間，仿真表明方法2 能夠較好地估計過程噪聲協(xié)方差。

圖7 過程噪聲協(xié)方差估計結(jié)果

這一部分對量測噪聲協(xié)方差進行了估計，使用了方法1 和方法2 并做了比較，從圖8 可以清晰地發(fā)現(xiàn)，方法2 的估計效果明顯好于方法1，且50次的估計結(jié)果穩(wěn)定在真實值附近，誤差在0～0.02之間，仿真表明方法2 能夠較好地估計量測噪聲協(xié)方差。

圖8 量測噪聲協(xié)方差估計結(jié)果

綜上所述，本文提出的參數(shù)優(yōu)化估計方法可以在噪聲協(xié)方差不準確情況上估計出準確的噪聲協(xié)方差，為車輛狀態(tài)估計提供保障。

5.3 汽車狀態(tài)估計

為了便于描述，現(xiàn)對狀態(tài)估計算法做如下簡化定義:

算法1:傳統(tǒng)的EKF 算法即2.1 節(jié)介紹的算法，其中噪聲協(xié)方差使用不準確的固定值:

算法2:本文提出的P-NAEKF 算法，對應第4章提出的算法，其中已知不精準的噪聲協(xié)方差與算法1 設置一樣，實際算法采用的噪聲協(xié)方差通過5.1 節(jié)的方法2 估計得到。

通過傳統(tǒng)EKF 算法估計的縱向車速以及本文提出的P-NAEKF 算法估計的縱向車速與Carsim輸出參數(shù)值進行對比分析?？v向車速的狀態(tài)估計仿真結(jié)果如圖9 和圖10 所示。

圖9 縱向車速曲線

圖10 縱向車速濾波偏差曲線

從圖9 和圖10 可以發(fā)現(xiàn)，P-NAEKF 算法的估計效果明顯優(yōu)于EKF，最大誤差僅為0.2 m/s，數(shù)值波動能夠處于穩(wěn)定狀態(tài)，且與實際數(shù)值接近;而EKF 的最大誤差達到了0.5 m/s。

通過傳統(tǒng)EKF 算法估計的橫擺角速度以及本文提出的P-NAEKF 算法估計的橫擺角速度與Carsim 輸出參數(shù)值進行對比分析。橫擺角速度的狀態(tài)估計仿真結(jié)果如圖11 和圖12 所示。

從圖11 和圖12 可以發(fā)現(xiàn)，P-NAEKF 算法的估計效果明顯優(yōu)于EKF，其估計值一直處于真實值附近，誤差很小，而傳統(tǒng)的EKF 在某些時刻存在發(fā)散現(xiàn)象。

圖11 橫擺角速度曲線

圖12 橫擺角速度濾波偏差曲線

通過傳統(tǒng)EKF 算法估計的質(zhì)心側(cè)偏角以及本文提出的P-NAEKF 算法估計的質(zhì)心側(cè)偏角與Carsim 輸出參數(shù)值進行對比分析。質(zhì)心側(cè)偏角狀態(tài)估計仿真如圖13 和圖14 所示。

從圖13 和圖14 可以發(fā)現(xiàn)，P-NAEKF 的估計效果大大優(yōu)于EKF，傳統(tǒng)EKF 在10 s 的時間里存在不止一次的發(fā)散現(xiàn)象，而P-NAEKF 算法保持較好的收斂性，其估計值位于真實值附近，最大誤差僅為0.2°左右，數(shù)值波動能夠處于穩(wěn)定狀態(tài)。

圖13 質(zhì)心側(cè)偏角曲線

圖14 質(zhì)心側(cè)偏角濾波偏差曲線

根據(jù)仿真實驗，可以得到:在噪聲協(xié)方差獲取數(shù)值不精準的情況下，P-NAEKF 可以通過基于粒子群的參數(shù)優(yōu)化估計方法對噪聲協(xié)方差進行矯正，因此P-NAEKF 具有優(yōu)異的估計精度和良好的收斂性，能夠較好地解決噪聲協(xié)方差獲取不準確的情況下的車輛行駛狀態(tài)估計問題。

6 結(jié)論

1)構(gòu)建了基于縱向車速，橫擺角速度和質(zhì)心側(cè)偏角的3 自由度模型，考慮了縱向車速和傳統(tǒng)汽車狀態(tài)參數(shù)之間的非線性關(guān)系，更嚴謹?shù)亟鉀Q汽車狀態(tài)估計問題。

2)提出了基于粒子群算法的參數(shù)優(yōu)化估計方法，并通過Matlab 仿真與傳統(tǒng)的Sage-Husa 算法進行對比，驗證了本文提出的參數(shù)估計方法的優(yōu)越性。

3)提出了基于NAEKF 算法的汽車行駛狀態(tài)估計方法，通過聯(lián)合仿真驗證了該算法在噪聲協(xié)方差不準確的環(huán)境下可以提供更加準確的車輛狀態(tài)估計。