石志偉， 任師通， 魏民祥， 查曰珩

(1.山東理工大學(xué) 計(jì)算中心， 山東 淄博 255012；2.南京航空航天大學(xué) 能源與動(dòng)力學(xué)院， 江蘇 南京 210016；3.江蘇省文化館， 江蘇 南京 210016)

對(duì)汽車主動(dòng)安全性進(jìn)行分析時(shí)，獲取汽車行駛過(guò)程中的狀態(tài)參數(shù)尤為重要。用于汽車關(guān)鍵狀態(tài)參數(shù)估計(jì)的算法主要有卡爾曼濾波算法、粒子濾波算法、滑模觀測(cè)器算法、魯棒觀測(cè)器算法和龍貝格觀測(cè)器算法等。其中粒子濾波算法易出現(xiàn)因密度函數(shù)建立不準(zhǔn)確而導(dǎo)致粒子退化的現(xiàn)象；滑模觀測(cè)器算法依賴傳感器的精度和性能，否則易產(chǎn)生抖振現(xiàn)象；魯棒觀測(cè)器算法在有些情況下易產(chǎn)生低估估計(jì)偏差，從而引起算法發(fā)散；龍貝格觀測(cè)器算法的計(jì)算過(guò)程較復(fù)雜，不易滿足車輛估計(jì)算法的實(shí)時(shí)性要求；卡爾曼濾波算法(KF)計(jì)算簡(jiǎn)單、收斂快速，在汽車狀態(tài)參數(shù)估計(jì)中得到廣泛應(yīng)用。

汽車是一個(gè)強(qiáng)非線性系統(tǒng)，而卡爾曼濾波僅能對(duì)線性系統(tǒng)進(jìn)行處理，在標(biāo)準(zhǔn)卡爾曼濾波算法的基礎(chǔ)上開(kāi)發(fā)出擴(kuò)展卡爾曼濾波算法(EKF)和無(wú)跡卡爾曼濾波算法(UKF)來(lái)處理非線性系統(tǒng)。擴(kuò)展卡爾曼濾波算法通過(guò)將非線性系統(tǒng)的數(shù)學(xué)模型在最佳點(diǎn)進(jìn)行泰勒展開(kāi)，對(duì)非線性函數(shù)求解雅可比矩陣，從而將非線性系統(tǒng)線性化。采用該算法進(jìn)行非線性系統(tǒng)線性化時(shí)，只保留一階系統(tǒng)，對(duì)于二階或更高階的分量采用舍棄的方法，因而存在一定估計(jì)偏差，同時(shí)當(dāng)估計(jì)的目標(biāo)系統(tǒng)非線性較強(qiáng)時(shí)，其計(jì)算量過(guò)大，雅可比矩陣求解復(fù)雜，易產(chǎn)生發(fā)散現(xiàn)象。而無(wú)跡卡爾曼濾波算法摒棄了求解非線性函數(shù)的雅可比矩陣，采用構(gòu)造樣本點(diǎn)的方法將整個(gè)非線性系統(tǒng)線性化，進(jìn)而得到一些sigma點(diǎn)，減少了算法的計(jì)算量。對(duì)于得到的各個(gè)sigma點(diǎn)，保證其均值和方差與原始采集數(shù)據(jù)相同，并將其帶入非線性系統(tǒng)中進(jìn)行無(wú)跡變換，通過(guò)樣本加權(quán)求和使其接近高斯分布。基于高斯分布的特點(diǎn)，該算法可精確到三階均值和協(xié)方差，且運(yùn)算簡(jiǎn)單，得到的系統(tǒng)穩(wěn)定。

當(dāng)前的研究中，大多把觀測(cè)噪聲的協(xié)方差矩陣設(shè)定為固定值，然而車輛實(shí)際行駛過(guò)程中，過(guò)程噪聲和觀測(cè)噪聲是隨機(jī)產(chǎn)生的，并非固定不變，為更好地對(duì)行駛中車輛狀態(tài)參數(shù)進(jìn)行實(shí)時(shí)估計(jì)，該文將不同時(shí)間段的過(guò)程噪聲和觀測(cè)噪聲設(shè)定為不同值，提出自適應(yīng)無(wú)跡卡爾曼濾波(AUKF)算法。

1 三自由度車輛模型

為更好地表達(dá)車輛的真實(shí)狀態(tài)，建立具有橫向、縱向、橫擺3個(gè)自由度的車輛動(dòng)力學(xué)模型(見(jiàn)圖1)。

Fxf、Fyf分別為前輪在x、y軸方向受到的力；δf為前輪轉(zhuǎn)角；Fcf、Flf分別為前輪受到的縱向力和側(cè)向力；ω為橫擺角速度；Fxr、Fyr分別為后輪在x、y軸方向受到的力；Fcr、Flr分別為后輪受到的縱向力和側(cè)向力；a、b分別為質(zhì)心至前軸和后軸的距離

在x軸方向，車輛的動(dòng)力學(xué)方程為：

(1)

在y軸方向，車輛的動(dòng)力學(xué)方程為：

(2)

在繞z軸的橫擺方向，車輛的動(dòng)力學(xué)方程為：

(3)

式中：m表示汽車總質(zhì)量；Iz表示車輛繞z軸的轉(zhuǎn)動(dòng)慣量。

2 基于AUKF算法的汽車狀態(tài)參數(shù)估計(jì)

2.1 無(wú)跡卡爾曼原理

2.1.1 建立汽車狀態(tài)空間方程和量測(cè)方程

非線性汽車系統(tǒng)的狀態(tài)量為：

x=(ω,β,vx)T

(4)

式中：β為質(zhì)心側(cè)偏角；vx為車輛的縱向速度。

系統(tǒng)控制輸入量為：

u=(δf,ax)

(5)

系統(tǒng)觀測(cè)量為：

y=(ay)

(6)

根據(jù)建立的三自由度汽車模型，得汽車狀態(tài)空間方程如下：

(7)

量測(cè)方程為：

(8)

將過(guò)程噪聲和測(cè)量噪聲代入汽車狀態(tài)空間和量測(cè)方程，得：

xk+1=f(xk,uk)+Q

(9)

zk+1=h(xk+1,uk)+R

(10)

式中：xk+1為k+1時(shí)刻的狀態(tài)向量；f為前一時(shí)刻狀態(tài)量與后一時(shí)刻狀態(tài)量之間的映射關(guān)系；uk為系統(tǒng)的控制輸入量；Q為預(yù)測(cè)過(guò)程中的噪聲；zk+1為k+1時(shí)刻的觀測(cè)向量；h為狀態(tài)向量與觀測(cè)向量之間的映射關(guān)系；R為測(cè)量過(guò)程中的噪聲。

以上2種噪聲需滿足以下關(guān)系，否則易產(chǎn)生協(xié)方差矩陣的非正定現(xiàn)象：

E[Q]=0

(11)

E[R]=0

(12)

Cov[R,Q]=0

(13)

式中：E[*]表示*的均值；Cov[*]表示*的方差。

2.1.2 無(wú)跡變換

(1) 初始均值和方差的確定：

(14)

(15)

(2) 采集點(diǎn)與各權(quán)值計(jì)算。1) 采集點(diǎn)。構(gòu)造2n+1個(gè)樣本點(diǎn)[見(jiàn)式(16)]。2) 采樣點(diǎn)權(quán)值計(jì)算。由于存在采樣的非局部效應(yīng)，需對(duì)各采樣點(diǎn)的權(quán)值進(jìn)行一定比例修正，計(jì)算得到均值的權(quán)值見(jiàn)式(17)，方差的權(quán)值見(jiàn)式(18)。

(16)

(17)

(18)

式中：λ為調(diào)節(jié)參數(shù)。

2.1.3 Sigma點(diǎn)的獲取

根據(jù)式(16)和式(17)，可獲取一組Sigma點(diǎn)集，點(diǎn)集中包括2n+1個(gè)點(diǎn)，可由向量ζ表示：

(19)

式中：i=0,1,2,…,n。

2.1.4 預(yù)測(cè)更新過(guò)程

當(dāng)k大于1時(shí)，通過(guò)加權(quán)得到狀態(tài)的預(yù)測(cè)值：

(20)

預(yù)測(cè)更新后得到的均值為：

(21)

式中：q為過(guò)程噪聲的平均值。

預(yù)測(cè)更新后獲取的方差矩陣的預(yù)測(cè)值為：

(ζ(k+1|k)-x(k+1|k))T]+Q

(22)

根據(jù)測(cè)量方程對(duì)各個(gè)點(diǎn)進(jìn)行非線性變換：

ζ(k+1|k)=h(ζ(k+1|k))

(23)

得到模型預(yù)測(cè)更新后的觀測(cè)值：

(24)

式中：r為測(cè)量噪聲的平均值。

2.1.5 測(cè)量更新過(guò)程

系統(tǒng)的輸出方差矩陣為：

(ζ(k+1|k)-z(k+1|k)T)]+R

(25)

協(xié)方差矩陣為：

(ζ(k+1|k)-z(k+1|k)T)]+R

(26)

卡爾曼濾波增益矩陣為：

(27)

對(duì)狀態(tài)進(jìn)行更新后的濾波值為：

x(k+1|k+1)=x(k+1|k)+Ka(z(k+1)-

z(k+1|k))

(28)

后驗(yàn)方差矩陣為：

P(k+1|k+1)=P(k+1|k)-

KaPzk+1zk+1KaT

(29)

2.2 自適應(yīng)無(wú)跡卡爾曼濾波原理

在測(cè)量噪聲和過(guò)程噪聲為固定值的情況下，無(wú)跡卡爾曼濾波算法可完成車輛狀態(tài)參數(shù)估計(jì)。但實(shí)際上測(cè)量噪聲和過(guò)程噪聲具有不確定性。針對(duì)實(shí)際過(guò)程中噪聲的不確定性，提出基于Sage-Husa算法理論的自適應(yīng)無(wú)跡卡爾曼濾波算法，該算法可對(duì)無(wú)跡卡爾曼濾波中的測(cè)量噪聲協(xié)方差和過(guò)程噪聲協(xié)方差進(jìn)行實(shí)時(shí)修正。步驟如下：

(1) 按式(30)計(jì)算測(cè)量噪聲的估計(jì)平均值。

1|k)]

(30)

式中：bk+1=(1-d)/(1-dk+1)；d為遺忘因子，取0.9。

(2) 按式(31)計(jì)算測(cè)量噪聲的估計(jì)協(xié)方差。

(31)

式中：ek+1為誤差，ek+1=z(k+1)-z(k+1|k)。

(3) 按式(32)計(jì)算過(guò)程噪聲的估計(jì)平均值。

qqk+1=(1-bk+1)qqk+bk+1[xk+1-

(32)

(4) 按式(33)計(jì)算測(cè)量噪聲的估計(jì)協(xié)方差。

RRk+1=(1-bk+1)RRk+bk+1(kak+1ek+1·

(33)

3 仿真驗(yàn)證與分析

為驗(yàn)證文中所建模型與自適應(yīng)無(wú)跡卡爾曼濾波算法的正確性，建立Carsim和MATLAB/Simulink仿真平臺(tái)，聯(lián)合仿真結(jié)構(gòu)見(jiàn)圖2。

圖2 汽車狀態(tài)參數(shù)估計(jì)聯(lián)合仿真結(jié)構(gòu)圖

仿真環(huán)境選取雙移線典型工況、附著系數(shù)為0.85的水平瀝青路面，仿真車輛選擇Carsim中自帶車輛，車速設(shè)置為72 km/h，采樣時(shí)間設(shè)置為0.01 s，整車質(zhì)量m為1 500 kg，質(zhì)心距前軸的距離a為1.55 m，軸距l(xiāng)為3.1 m，汽車?yán)@z軸的轉(zhuǎn)動(dòng)慣量為4 607.4 kg·m2，前輪總側(cè)偏剛度為-264 570 N/rad，后輪總側(cè)偏剛度為-240 000 N/rad。仿真工況控制輸入量見(jiàn)圖3～5。

圖3 雙移線工況下車輛輸出側(cè)向加速度

圖4 雙移線工況下車輛輸出縱向加速度

圖5 雙移線工況下車輛輸出前輪轉(zhuǎn)角

把車輛輸出的控制量和觀測(cè)量輸入U(xiǎn)KF算法模型中，對(duì)3個(gè)狀態(tài)量進(jìn)行實(shí)時(shí)估計(jì)。以Carsim軟件中車輛輸出的狀態(tài)量結(jié)果作為虛擬試驗(yàn)值，將AUKF算法估計(jì)的狀態(tài)量結(jié)果與虛擬試驗(yàn)值進(jìn)行對(duì)比，驗(yàn)證AUKF算法的正確性。AUKF算法估計(jì)求解結(jié)果、UKF算法估計(jì)求解結(jié)果與虛擬試驗(yàn)值對(duì)比見(jiàn)圖6～8。

圖6 不同算法的橫擺角速度估計(jì)值對(duì)比

圖7 不同算法的質(zhì)心側(cè)偏角估計(jì)值對(duì)比

圖8 不同算法的縱向速度估計(jì)值對(duì)比

由圖6可知：采用UKF算法對(duì)橫擺角速度進(jìn)行估計(jì)時(shí)出現(xiàn)發(fā)散狀態(tài)，表明在應(yīng)對(duì)不同噪聲時(shí)，UKF算法不能準(zhǔn)確估計(jì)相應(yīng)的狀態(tài)參數(shù)；而AUKF算法對(duì)橫擺角速度的估計(jì)值與虛擬試驗(yàn)值較接近，兩者最大瞬態(tài)誤差為0.5 (°)/s，相對(duì)誤差僅5.101%，估計(jì)效果較理想。

由圖7可知：采用UKF算法對(duì)質(zhì)心側(cè)偏角進(jìn)行估計(jì)時(shí)出現(xiàn)發(fā)散狀態(tài)；而AUKF算法對(duì)質(zhì)心側(cè)偏角的估計(jì)值與虛擬試驗(yàn)值接近，最大瞬態(tài)誤差僅為0.04°，相對(duì)誤差為3.633%。

由圖8可知：采用UKF算法對(duì)縱向速度進(jìn)行估計(jì)時(shí)出現(xiàn)發(fā)散狀態(tài)；而AUKF算法對(duì)縱向速度的估計(jì)值與虛擬試驗(yàn)值接近，最大瞬態(tài)誤差為1.875 km/h，相對(duì)誤差僅為1.704%。

綜上，采用AUKF算法對(duì)汽車狀態(tài)參數(shù)進(jìn)行估計(jì)的效果較好。

4 結(jié)語(yǔ)

采用自適應(yīng)無(wú)跡卡爾曼濾波算法對(duì)汽車橫擺角速度、質(zhì)心側(cè)偏角和縱向速度進(jìn)行估計(jì)，結(jié)果表明該算法在車輛行駛環(huán)境中的噪聲發(fā)生變化時(shí)仍可對(duì)相應(yīng)狀態(tài)參數(shù)進(jìn)行估計(jì)，且估計(jì)精度較高。