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

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

0 引言

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

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

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

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

1 3 自由度車輛模型

1.1 模型構(gòu)建

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

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

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

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

1.2 模型轉(zhuǎn)換

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

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

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

表1 車輛參數(shù)

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

2.1 傳統(tǒng)EKF 算法

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

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

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

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

2.2 性能匹配分析

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

式中:Rk表示真實(shí)的量測(cè)噪聲協(xié)方差;Qk|k-1表示真實(shí)的過(guò)程噪聲協(xié)方差;上標(biāo)f 表示實(shí)際應(yīng)用假設(shè)值;ΔRk和ΔQk|k-1表示實(shí)際應(yīng)用假設(shè)值和真實(shí)值的偏差。

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

其中

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

其中

對(duì)比式(19)和式(21)可以發(fā)現(xiàn)，當(dāng)噪聲協(xié)方差存在誤差時(shí)，仍然使用不精準(zhǔn)的噪聲協(xié)方差來(lái)設(shè)計(jì)Kalman 濾波器，將會(huì)導(dǎo)致估計(jì)誤差協(xié)方差與真實(shí)的TMSE 存在偏差，Kalman 濾波器的精度降低，因此有必要設(shè)計(jì)一種噪聲自適應(yīng)濾波算法解決噪聲協(xié)方差不精準(zhǔn)的問(wèn)題。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 P-NAEKF 算法

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

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

表2 P-NAEKF 算法流程

5 仿真分析

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

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

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

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

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

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

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

圖6 方向盤(pán)轉(zhuǎn)角輸入信號(hào)

5.2 參數(shù)估計(jì)

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

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

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

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

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

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

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

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

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

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

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

算法2:本文提出的P-NAEKF 算法，對(duì)應(yīng)第4章提出的算法，其中已知不精準(zhǔn)的噪聲協(xié)方差與算法1 設(shè)置一樣，實(shí)際算法采用的噪聲協(xié)方差通過(guò)5.1 節(jié)的方法2 估計(jì)得到。

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

圖9 縱向車速曲線

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

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

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

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

圖11 橫擺角速度曲線

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

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

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

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

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

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

6 結(jié)論

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

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

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