基于卡爾曼濾波算法的汽車運(yùn)動(dòng)參數(shù)測(cè)試方法研究

1、第9卷第3期中國(guó)慣性技術(shù)學(xué)報(bào)2001年9月文章編號(hào):10056734(2001 03001905基于卡爾曼濾波算法的汽車運(yùn)動(dòng)參數(shù)測(cè)試方法研究倪江生(東南大學(xué), 江蘇南京210096摘要:提出了一種采用多維組合慣性測(cè)量元件進(jìn)行汽車運(yùn)動(dòng)參數(shù)測(cè)試的方法, 并利用卡爾曼濾波算法提高了測(cè)試系統(tǒng)的精度。仿真結(jié)果表明:卡爾曼濾波算法對(duì)汽車姿態(tài)、速度等的解算具有較好的效果。關(guān)鍵詞:慣性系統(tǒng); 運(yùn)動(dòng)參數(shù); 卡爾曼濾波中圖分類號(hào):666. 123; 467. 11:AM M otion Param eterson Kal man F ilterN I J iangsheng(Sou theast U n iver

2、sity , N an jing 210096, Ch ina Abstract :A new m easu rem en t m ethod u sing m u lti dem en si on in tegrated inertial sen so r fo r au tom ob ile m o ti on p aram eters is p ropo sed in th is pap er , and the m easu rem en t p recisi on is i m 2p roved based on the Kal m an filter . T he si m u l

3、ati on resu lts show that the Kal m an filter algo 2rithm is effective in calcu lating the attitude and velocity of au tom ob ile . Key words :inertial system ; m o ti on param eter ; Kal m an filter 1引言汽車的整車運(yùn)動(dòng)性能, 如操縱穩(wěn)定性、制動(dòng)性等, 是道路行駛的重要技術(shù)性能, 關(guān)系到汽車的行駛安全。隨著高速公路及高速車輛的迅速發(fā)展, 道路交通安全日益重要, 相應(yīng)地對(duì)汽車性能測(cè)試的要求也越來越高

4、。汽車運(yùn)動(dòng)性能的道路試驗(yàn)離不開運(yùn)動(dòng)參數(shù)的測(cè)試, 如汽車的運(yùn)動(dòng)速度、加速度、運(yùn)動(dòng)軌跡、轉(zhuǎn)向角速度、姿態(tài)角等的測(cè)試。目前, 這些參數(shù)都依靠各個(gè)單獨(dú)的儀器進(jìn)行測(cè)試, 主要利用五輪儀、非接觸式車速儀以及方位陀螺儀、垂直陀螺儀等儀器。這樣構(gòu)成的測(cè)試系統(tǒng)信息量少、測(cè)試參數(shù)不夠完整, 而且使用不方便。因此, 研究一種體積小、功能強(qiáng)、使用方便的便攜式路試系統(tǒng)具有重要的實(shí)際意義和經(jīng)濟(jì)價(jià)值。近年來, 各種微小型廉價(jià)慣性元件發(fā)展迅速1-2, 面向汽車用的廉價(jià)產(chǎn)品更日益受到關(guān)注。這些慣性元件一方面用于車輛的輔助導(dǎo)航, 另一方面用于汽車的電子控制, 如轉(zhuǎn)向控制、AB S 等。本文針對(duì)慣性元件在汽車上的應(yīng)用, 提出一種車

5、輛道路試驗(yàn)的測(cè)試方法, 采用廉價(jià)的集成多微慣性傳感器測(cè)量車體的運(yùn)動(dòng)信息, 并基于擴(kuò)展卡爾曼濾波算法計(jì)算汽車的運(yùn)動(dòng)參數(shù)。仿真結(jié)果表明:這種方法是完全可行的。2測(cè)試系統(tǒng)的構(gòu)成及基本原理本系統(tǒng)的核心測(cè)量元件采用六維壓電式慣性傳感器, 即三向加速度計(jì)及三向速率陀螺。從理論上講, 空間六維信息可以確定一個(gè)剛體在空間的位置及其運(yùn)動(dòng)參數(shù)。測(cè)試系統(tǒng)的構(gòu)成如圖1所示?；痦?xiàng)目:江蘇省汽車工程重點(diǎn)實(shí)驗(yàn)室開放基金項(xiàng)目(K 99043 收稿日期:20010602作者簡(jiǎn)介:倪江生(1962- , 男, 東南大學(xué)儀器科學(xué)與工程系副教授, 從事精密儀器及測(cè)試技術(shù)專業(yè)。20中國(guó)慣性技術(shù)學(xué)報(bào)2001年如圖1所示, 加速度計(jì)及陀

6、螺儀分別用于測(cè)量車體沿車體坐標(biāo)系X , Y , Z 三個(gè)方向的加速度和旋轉(zhuǎn)速率, 這種壓電式慣性元件的優(yōu)點(diǎn)是體積小, 使用方便, 很適合汽車的道路試驗(yàn)。系統(tǒng)還采用了汽車測(cè)試中常用的非接觸式光電測(cè)速傳感器, 雖然六維慣性元件已經(jīng)可以測(cè)量所需信息, 但由于低價(jià)元件性能不高, 解算的積累誤差增長(zhǎng)較快, 完全依賴慣性測(cè)量信息有時(shí)不能滿足要求, 因此采用非接觸式速度傳感器有利于提高系統(tǒng)測(cè)試精度, 增加數(shù)據(jù)處理的靈活性。除上述傳感器以外, 汽車性能測(cè)試還需踏板力計(jì)及扭力方向盤等。傳感器的信號(hào)經(jīng)必要的接口及調(diào)理電路進(jìn)行外置式AD 轉(zhuǎn)換器, 1式計(jì)算機(jī)的并行口, , 將捷聯(lián)式慣性導(dǎo)航的原理應(yīng), 。3擴(kuò)展卡爾曼

7、濾波器的設(shè)計(jì)3. 1系統(tǒng)狀態(tài)方程測(cè)試系統(tǒng)的狀態(tài)向量設(shè)為:X GXX GX (k X A X (k X (k =X GY (k X A Y (k X GZ (k X A Z (k X A X(k =(k (k (k (k , (k =x (k x (k x (k x (k ,¨¨¨, X GY (k =(k (k (k (k , X A Y (k =y (k y (k y (k y (k , X GZ (k =(k (k (k (k ,¨¨(1, X A Z (k =z (k z (k z (k z (k ¨其中, 上式中, , , 分

8、別為車體的俯仰、傾斜、航向角。狀態(tài)方程如下:X (k +1 =FX (k +W (k ,其中, 狀態(tài)轉(zhuǎn)移矩陣可表示為F GX(20F A X00 F GY000F A Y0000F GZ00000F A 0F =1 , F GX =F ZX =F GY =F A Y =F GZ =F A Z =000T s2T s 2T s3T s 62T s , 2T s0000000010000000101(3式中, T s 為采樣間隔。方程(2 中, W (k 為過程噪聲, 其協(xié)方差矩陣為3:Q G0Q A00Q G000Q A0000Q G00000Q A72526212167252153042142

9、4 3217252622226725225304224 2432222。(4 22Q =00000000000, Q G =7252042183216221, Q A =725204228322620030421218321213221212213042222832222322222T s 22124622462T s 21第3期倪江生:基于卡爾曼濾波算法的汽車運(yùn)動(dòng)參數(shù)測(cè)試方法研究213. 2系統(tǒng)量測(cè)方程根據(jù)文獻(xiàn)4,車體相對(duì)地球的加速度與慣性傳感器輸出之間的關(guān)系如下式所示:V ep =f -(2ie +ep ×V ep +g , (5其中, V ep 為車體平臺(tái)相對(duì)地球的加速度, f

10、 為加速度計(jì)測(cè)得的比力向量, ie , ep 分別為地球相對(duì)慣性系、車體平臺(tái)相對(duì)地球的轉(zhuǎn)動(dòng)角速度向量, g 為重力加速度向量。從方程式(9 可以看出:加速度計(jì)的輸出信號(hào)中除了要測(cè)量的車體運(yùn)動(dòng)外, 還包含了另外兩項(xiàng), 一項(xiàng)是重力加速度, 另一項(xiàng)是由地球自轉(zhuǎn)和車體繞地球轉(zhuǎn)動(dòng)而產(chǎn)生的加速度。對(duì)于汽車性能測(cè)試而言, 其特點(diǎn)是車速低, 測(cè)試時(shí)間短, 地球運(yùn)動(dòng)引起的哥氏加速度和離心加速度很小, 而且其積累誤差也較小。另一方面, 對(duì)于民用測(cè)試系統(tǒng)來說, 一般選用低價(jià)的慣性元件, , 因此對(duì)微弱的哥氏、離心加速度難以檢測(cè)。根據(jù)文獻(xiàn)5的分析, (9 , V ep(6 (7:p b =ib -(ie +ep ,其

11、中, p b 為車體相對(duì)車體平臺(tái)的姿態(tài)速率向量, ib 為車體相對(duì)慣性系的姿態(tài)速率向量, 陀螺儀可測(cè)出其大小。對(duì)于低精度陀螺儀表, 地球自轉(zhuǎn)等角速度難于敏感, 同時(shí)考慮到汽車測(cè)試的要求和特點(diǎn), 將方程(7 簡(jiǎn)化為p b =ib根據(jù)上式, 可得速率陀螺的量測(cè)方程為 x y =根據(jù)方程(6 , 可得加速度計(jì)的量測(cè)方程為:f f fx(8-s c c s c c 01。(9 s x¨¨¨00 ,gs c y=T b +p (yz(10z其中, g 為重力加速度, T b p 為捷聯(lián)矩陣,c c -T p =bs s s c s +s s c -c s c c s c +

12、c s s s s -c s c V =x +y ,2s 。c c (11除上述量測(cè)變量以外, 光電速度計(jì)的輸出為:22(12上式中, 為方便量測(cè)方程求偏導(dǎo)數(shù), 取速度信號(hào)的平方作為輸出。另外, 取垂直方向的速度作為量測(cè)變量, 如下式所示:V z =z ,(132因此, 系統(tǒng)量測(cè)向量為Y =x y z f x f y f z V V z ,(14 (15量測(cè)方程為Y (k +1 =h X (k +1 +V (k +1 。3. 3擴(kuò)展卡爾曼濾波算法 22中國(guó)慣性技術(shù)學(xué)報(bào)2001年上述系統(tǒng)的擴(kuò)展卡爾曼濾波算法如下:X (k +1 =F (k +1 k X (k +K g (k +1 Y (k +1

13、 -h X (k +1 k ,-1K g (k +1 =P g (k +1 k H g (k +1 H g (k +1 P g (k +1 k H g (k +1 +R (k +1 ,P g (k +1 k =F P g (k F +Q (k , P g (k +1 =(I m x -K g (k +1 H g (k +1 P g (k +1 k 。(16上式中,H g (k +1 =X k +1X (k +1 =X (k +1 k 。(174測(cè)試系統(tǒng)的仿真, 。在短時(shí), 且采用低精度測(cè)量元件, 系統(tǒng)的積累誤差如何是一個(gè)決定性的問題。另外, 通過慣性傳感器的測(cè)量信息直接求解運(yùn)動(dòng)參數(shù)與綜合采用慣

14、性傳感器和速度傳感器并基于卡爾曼濾波算法解算運(yùn)動(dòng)參數(shù)兩種方法相比較在測(cè)試誤差方面有何不同, 下面通過計(jì)算機(jī)仿真進(jìn)行進(jìn)一步的分析。仿真采用的行駛路徑應(yīng)能充分反映汽車的直線加減速運(yùn)動(dòng)和轉(zhuǎn)向運(yùn)動(dòng)。設(shè)運(yùn)動(dòng)軌跡如圖3中實(shí)線所示, 汽車從A 點(diǎn)開始沿Y 向加速度運(yùn)動(dòng), 加速度為1m2s , 前進(jìn)30m 后繞半徑為15m, , 為x , y , z 速率陀螺; , , 為x , y , z 加速度計(jì)輸出圖2 慣性測(cè)量單元輸出信號(hào)的仿真的圓周作勻速運(yùn)動(dòng), 環(huán)繞1圈4后沿-X 方向作減速運(yùn)動(dòng), 減速2度為1m s 最終到達(dá)B 點(diǎn)停止。設(shè)系統(tǒng)采用低精度的慣性元件, 三向加速度計(jì)和三向速率陀螺都具有較大的零位偏置和

15、隨機(jī)圖3設(shè)定軌跡和測(cè)試仿真軌跡誤差。設(shè)三向加速度計(jì)的零位偏置為10m g (g 為重力加速度單位 , 隨機(jī)干擾為10m g , 三向速率陀螺的零位偏置為0. 1(°s 。隨機(jī)干擾為0. 2(°s 。另外設(shè)光電測(cè)速儀的隨機(jī)誤差為1km h 。各隨機(jī)誤差都為均方差。仿真時(shí)將慣性元件的理論輸出值加上零位偏置, 再加上由程序產(chǎn)生的均勻分布白噪聲, 然后分別用直接求解法和卡爾曼濾波進(jìn)行解算, 比較兩者的結(jié)果。直接求解時(shí)采用四階龍格庫(kù)塔法解方程(9 。仿真結(jié)果如圖2、3、4所示。圖2表示的是帶有零位偏置和隨機(jī)噪聲的量測(cè)信號(hào)。圖3表示了設(shè)定軌跡和測(cè)試仿真軌跡, 其中實(shí)線為設(shè)定軌跡, 短實(shí)

16、線為慣性測(cè)量信號(hào)直接求解結(jié)果, 長(zhǎng)實(shí)線為卡爾曼濾波的計(jì)算軌跡。圖4為測(cè)試誤差的仿真曲線, 即測(cè)試軌跡與設(shè)定軌跡之差, 實(shí)線為直接求解法的誤差曲線, 虛線為卡爾曼濾波的誤差曲線。第3期倪江生:基于卡爾曼濾波算法的汽車運(yùn)動(dòng)參數(shù)測(cè)試方法研究23從圖3可以看到:由于慣性元件的零位偏置較大, 單獨(dú)依靠六維慣性傳感器的解算軌跡具有很大誤差(圖3短虛線 。采用卡爾曼濾波算法, 解算軌跡與原設(shè)定軌跡比較接近(圖3長(zhǎng)實(shí)線 。從圖4可以看到:在慣性傳感器直接解算的情況下, 誤差隨時(shí)間快速增長(zhǎng)(圖4實(shí)線 , 而采用卡爾曼濾波后, 誤差的增長(zhǎng)要緩慢得多(圖4虛線 , 在Z 方向上, 速度及位置誤差均為零(圖4. 6,

17、 4. 9 。不管是姿態(tài)角誤差(圖4 還是速度誤差(圖4 、位置誤差(圖4 , 通過卡爾曼濾波后都有顯著減小 。, , 為航向、俯仰、傾斜角誤差(rad ; , , 為x , y , z 方向的速度誤差(m s ; , , 為x , y , z 方向的位置誤差(m 。圖4測(cè)試誤差的仿真曲線5討論從仿真結(jié)果看:即使是短時(shí)慣性測(cè)量系統(tǒng), 如果慣性測(cè)量元件的零位偏置誤差較大, 則依靠慣性傳感器直接解算所測(cè)得的姿態(tài)、速度、位置等也會(huì)有較大的誤差, 而且誤差增長(zhǎng)速度非?？?。這種情況下, 慣性測(cè)量系統(tǒng)只適用于時(shí)間非常短的汽車測(cè)試項(xiàng)目, 如制動(dòng)試驗(yàn)等(只需要幾秒鐘 , 而不太適合時(shí)間較長(zhǎng)的道路試驗(yàn)項(xiàng)目。為了提高廉價(jià)慣性測(cè)量系統(tǒng)的精度, 滿足汽車測(cè)試的要求, 可以采用卡爾曼濾波算法。仿真表明:對(duì)于汽車姿態(tài)、速度等運(yùn)動(dòng)參數(shù), 使用卡爾曼濾波后精度顯著提高。參考文獻(xiàn):1H iro sh i K O . Op tical fiber gyro scope fo r automobile J . Op tronics . 1994, (3 :6168(in Japanese .2N obo ru W . Gyro scope fo r automobile app licati on J