基于卡爾曼濾波的汽車縱向速度濾波算法及應(yīng)用

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

(1.中車時代電動汽車股份有限公司，湖南 株洲 412000； 2.長沙中車智馭新能源科技有限公司，長沙 410000)

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

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

1 卡爾曼濾波的理論簡介

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

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

由于要實現(xiàn)計算機代碼計算，需要將系統(tǒng)方程離散化，假設(shè)隨機線性離散系統(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)的觀測向量；H(n)為系統(tǒng)的觀測矩陣；V(n)為系統(tǒng)的觀測噪聲或測量噪聲。

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é)方差矩陣。

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

1)使用前一時刻濾波器的輸出進行當前時刻的系統(tǒng)狀態(tài)的預(yù)測(時間更新)。

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

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

(4)

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

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

(5)

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

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

增益矩陣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)估計：

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

(7)

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

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

(8)

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

圖1 卡爾曼濾波流程圖

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

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

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

S(n)=S(n-1)

(9)

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

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

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

(10)

式中：S(n)為n時刻車速估計值；s(n)為n時刻原始車速信號；s(n-i)為n時刻的前i時刻的原始車速信號。

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

2.2 仿真分析

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

(a) 仿真信號

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

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

2.3 實車試驗

將以上3種算法自動生成C代碼，燒寫進整車控制器進行實車試驗，結(jié)果如圖3所示。

(a) 實車試驗結(jié)果

(b) 實車試驗結(jié)果部分放大顯示

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

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

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

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

3 結(jié)束語

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