熊琦瑋，彭再武，石 魏，羅宏亮，王澍龍

(1.中車(chē)時(shí)代電動(dòng)汽車(chē)股份有限公司，湖南 株洲 412000； 2.長(zhǎng)沙中車(chē)智馭新能源科技有限公司，長(zhǎng)沙 410000)

獲取準(zhǔn)確的實(shí)時(shí)車(chē)輛縱向速度是進(jìn)行相關(guān)精確控制的基礎(chǔ)，車(chē)輛縱向速度現(xiàn)有的計(jì)算方法較為粗糙，其精度不高且波動(dòng)大，難以作為控制器的控制輸入。隨著對(duì)汽車(chē)控制系統(tǒng)性能要求的不斷提高以及智能駕駛汽車(chē)的出現(xiàn)，準(zhǔn)確的縱向加速度信號(hào)在汽車(chē)上的應(yīng)用越來(lái)越廣泛[1]。由于加速度傳感器價(jià)格從幾千元到上萬(wàn)元不等，如果加裝加速度傳感器，則會(huì)大大增加車(chē)輛成本，而對(duì)車(chē)輛縱向速度進(jìn)行差分可方便地得到車(chē)輛加速度[2]。因此，如何對(duì)傳感器信號(hào)進(jìn)行處理并獲取準(zhǔn)確的車(chē)速顯得尤為重要。

本文設(shè)計(jì)了基于卡爾曼濾波的車(chē)輛速度估計(jì)，仿真與實(shí)車(chē)試驗(yàn)表明，與其他濾波方法相比，該方法具有良好的實(shí)時(shí)性與估計(jì)精度。

1 卡爾曼濾波的理論簡(jiǎn)介

濾波的方法最初用于信號(hào)處理領(lǐng)域，其作用是消除各類(lèi)傳感器獲取信號(hào)中的隨機(jī)誤差，提高信號(hào)精度和準(zhǔn)確度。因?yàn)殡S機(jī)信號(hào)的功率譜特性是確定并可得到相關(guān)參數(shù)的，所以要對(duì)得到的信號(hào)進(jìn)行濾波，可根據(jù)有用信號(hào)和需要去除的干擾信號(hào)的功率譜設(shè)計(jì)相應(yīng)的濾波器及其參數(shù)。

R.E.Kalman于1960年發(fā)表論文提出了卡爾曼濾波器，它是一種從系統(tǒng)狀態(tài)有關(guān)的觀測(cè)量中估計(jì)出所需信號(hào)精確值的最優(yōu)遞推濾波算法[3-4]。由于卡爾曼濾波是僅在時(shí)域內(nèi)設(shè)計(jì)的濾波器，而且存儲(chǔ)的數(shù)據(jù)很少，所以具有很強(qiáng)的實(shí)時(shí)性，十分容易以計(jì)算機(jī)代碼的方式實(shí)現(xiàn)[5-7]。在許多工程領(lǐng)域，如制導(dǎo)、雷達(dá)和通訊等工程中，卡爾曼濾波已得到了較好的應(yīng)用，并衍生出了擴(kuò)展卡爾曼濾波、無(wú)跡卡爾曼濾波等算法[8-12]。

由于要實(shí)現(xiàn)計(jì)算機(jī)代碼計(jì)算，需要將系統(tǒng)方程離散化，假設(shè)隨機(jī)線性離散系統(tǒng)的方程如式(1)：

(1)

式中：X(n)為系統(tǒng)的狀態(tài)向量；G(n-1)為系統(tǒng)狀態(tài)的轉(zhuǎn)移矩陣；W(n-1)為系統(tǒng)噪聲；Z(n)為系統(tǒng)的觀測(cè)向量；H(n)為系統(tǒng)的觀測(cè)矩陣；V(n)為系統(tǒng)的觀測(cè)噪聲或測(cè)量噪聲。

W(n-1)和V(n)為概率密度函數(shù)服從正態(tài)分布的高斯噪聲，且滿足式(2)和式(3)：

Cov[W(i),W(j)]=Q(i)δ(ij)

(2)

Cov[V(i),V(j)]=R(i)δ(ij)

(3)

式中：Q和R分別為W和V的協(xié)方差矩陣。

建立隨機(jī)線性離散系統(tǒng)的卡爾曼濾波方程可歸納為兩步：

1)使用前一時(shí)刻濾波器的輸出進(jìn)行當(dāng)前時(shí)刻的系統(tǒng)狀態(tài)的預(yù)測(cè)(時(shí)間更新)。

系統(tǒng)狀態(tài)的一步預(yù)測(cè)：

X(n|n-1)=G(n-1)X(n-1)

(4)

系統(tǒng)狀態(tài)的一步預(yù)測(cè)誤差方差矩陣：

P(n|n-1)=G(n-1)P(n-1|n-1)G(n-1)T+Q

(5)

2)在得到當(dāng)前時(shí)刻狀態(tài)的測(cè)量值(信號(hào)原始數(shù)據(jù))后對(duì)時(shí)間更新中得到的當(dāng)前狀態(tài)預(yù)測(cè)值進(jìn)行修正，得到系統(tǒng)當(dāng)前時(shí)刻的狀態(tài)估計(jì)值(觀測(cè)更新)。

只需給出初值X(0|0)和P(0|0)，即可根據(jù)當(dāng)前時(shí)刻的系統(tǒng)觀測(cè)值和前一時(shí)刻的預(yù)測(cè)值得到當(dāng)前時(shí)刻的系統(tǒng)狀態(tài)估計(jì)值。

增益矩陣Kg：

Kg(n)=P(n|n-1)H(n)T[H(n)P(n|n-1)
H(n)T+R]-1

(6)

系統(tǒng)狀態(tài)估計(jì)：

X(n|n)=
X(n|n-1)+Kg(n)[Z(n)-H(n)X(n|n-1)]

(7)

系統(tǒng)狀態(tài)估計(jì)誤差方差矩陣：

P(n|n)=[1-Kg(n)H(n)]P(n|n-1)

(8)

因?yàn)槭?4)～式(8)是在時(shí)域內(nèi)的遞推，所以非常容易以計(jì)算機(jī)代碼的方式實(shí)現(xiàn)，流程如圖1所示。

圖1 卡爾曼濾波流程圖

2 車(chē)輛縱向速度的卡爾曼濾波應(yīng)用

2.1 濾波器設(shè)計(jì)方案

在Matlab-Simulink環(huán)境中利用Kalman Filter工具箱方便地設(shè)計(jì)卡爾曼濾波器。在本文卡爾曼濾波器設(shè)計(jì)中，式(1)中的狀態(tài)向量X、觀測(cè)向量Z均為車(chē)速S，因此系統(tǒng)觀測(cè)矩陣H為1。由于實(shí)際采樣頻率為100 Hz，采樣周期較短，可以近似認(rèn)為車(chē)速在此時(shí)間內(nèi)保持不變，即：

S(n)=S(n-1)

(9)

式中：S(n)為n時(shí)刻車(chē)速值；S(n-1)為n時(shí)刻的前一時(shí)刻的車(chē)速值。

同時(shí)將式(1)中的系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣G取1，并取系統(tǒng)噪聲W為0.01，系統(tǒng)觀測(cè)噪聲V為1。為了驗(yàn)證設(shè)計(jì)的卡爾曼濾波器的性能，與下列兩種濾波算法進(jìn)行比較：

1) 加權(quán)平均法。

(10)

式中：S(n)為n時(shí)刻車(chē)速估計(jì)值；s(n)為n時(shí)刻原始車(chē)速信號(hào)；s(n-i)為n時(shí)刻的前i時(shí)刻的原始車(chē)速信號(hào)。

2) 巴特沃斯IIR低通濾波。在Matlab-Simulink中利用FDA Tool工具箱設(shè)計(jì)4階巴特沃斯IIR低通濾波器，取截止頻率為1 Hz。

2.2 仿真分析

在Simulink環(huán)境中生成幅值為10無(wú)量綱的正弦信號(hào)，為模擬真實(shí)信號(hào)，加入方差為1的隨機(jī)值作為白噪聲，如圖2(a)所示。不同的濾波方法對(duì)加入噪聲的信號(hào)濾波仿真結(jié)果如圖2(b)所示。

(a) 仿真信號(hào)

(b) 濾波結(jié)果對(duì)比

由仿真結(jié)果看出，采用“加權(quán)平均法”處理后的結(jié)果仍有較大誤差，精度不高；“巴特沃斯低通濾波”和“卡爾曼濾波”處理后的結(jié)果曲線較為平滑，但“巴特沃斯低通濾波”處理后的結(jié)果出現(xiàn)了時(shí)域上較大的延遲。

2.3 實(shí)車(chē)試驗(yàn)

將以上3種算法自動(dòng)生成C代碼，燒寫(xiě)進(jìn)整車(chē)控制器進(jìn)行實(shí)車(chē)試驗(yàn)，結(jié)果如圖3所示。

(a) 實(shí)車(chē)試驗(yàn)結(jié)果

(b) 實(shí)車(chē)試驗(yàn)結(jié)果部分放大顯示

實(shí)車(chē)試驗(yàn)結(jié)果表明，“巴特沃斯低通濾波”的時(shí)域延遲仍較大，“加權(quán)平均法”處理后的結(jié)果仍有較大波動(dòng)，“卡爾曼濾波器”處理后的結(jié)果時(shí)域延遲較低且比較平滑。

繼續(xù)通過(guò)快速傅里葉變換對(duì)數(shù)據(jù)進(jìn)行頻譜分析，結(jié)果如圖4所示。

圖4 濾波結(jié)果頻域分析

由圖4可知，“加權(quán)平均法”對(duì)40 Hz以上信號(hào)濾波不明顯，“巴特沃斯低通濾波”優(yōu)于“加權(quán)平均法”，但其信號(hào)幅值仍大于“卡爾曼濾波”。綜合時(shí)域和頻域表現(xiàn)，“卡爾曼濾波”在3種算法中性能最優(yōu)。

3 結(jié)束語(yǔ)

通過(guò)對(duì)原始車(chē)速值進(jìn)行不同算法的濾波處理，得到了車(chē)輛速度估計(jì)值。仿真和實(shí)車(chē)試驗(yàn)表明，卡爾曼濾波方法具有良好的實(shí)時(shí)性與較高的精度，為提高控制系統(tǒng)的準(zhǔn)確度提供了可靠的保證。