基于卡爾曼濾波算法的汽車運動參數(shù)測試方法研究

1、第9卷第3期中國慣性技術學報2001年9月文章編號:10056734(2001 03001905基于卡爾曼濾波算法的汽車運動參數(shù)測試方法研究倪江生(東南大學, 江蘇南京210096摘要:提出了一種采用多維組合慣性測量元件進行汽車運動參數(shù)測試的方法, 并利用卡爾曼濾波算法提高了測試系統(tǒng)的精度。仿真結果表明:卡爾曼濾波算法對汽車姿態(tài)、速度等的解算具有較好的效果。關鍵詞:慣性系統(tǒng); 運動參數(shù); 卡爾曼濾波中圖分類號: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引言汽車的整車運動性能, 如操縱穩(wěn)定性、制動性等, 是道路行駛的重要技術性能, 關系到汽車的行駛安全。隨著高速公路及高速車輛的迅速發(fā)展, 道路交通安全日益重要, 相應地對汽車性能測試的要求也越來越高

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

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

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

7、濾波器的設計3. 1系統(tǒng)狀態(tài)方程測試系統(tǒng)的狀態(tài)向量設為: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)轉移矩陣可表示為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期倪江生:基于卡爾曼濾波算法的汽車運動參數(shù)測試方法研究213. 2系統(tǒng)量測方程根據(jù)文獻4,車體相對地球的加速度與慣性傳感器輸出之間的關系如下式所示:V ep =f -(2ie +ep ×V ep +g , (5其中, V ep 為車體平臺相對地球的加速度, f

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

11、中, p b 為車體相對車體平臺的姿態(tài)速率向量, ib 為車體相對慣性系的姿態(tài)速率向量, 陀螺儀可測出其大小。對于低精度陀螺儀表, 地球自轉等角速度難于敏感, 同時考慮到汽車測試的要求和特點, 將方程(7 簡化為p b =ib根據(jù)上式, 可得速率陀螺的量測方程為 x y =根據(jù)方程(6 , 可得加速度計的量測方程為: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除上述量測變量以外, 光電速度計的輸出為:22(12上式中, 為方便量測方程求偏導數(shù), 取速度信號的平方作為輸出。另外, 取垂直方向的速度作為量測變量, 如下式所示:V z =z ,(132因此, 系統(tǒng)量測向量為Y =x y z f x f y f z V V z ,(14 (15量測方程為Y (k +1 =h X (k +1 +V (k +1 。3. 3擴展卡爾曼濾波算法 22中國慣性技術學報2001年上述系統(tǒng)的擴展卡爾曼濾波算法如下: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測試系統(tǒng)的仿真, 。在短時, 且采用低精度測量元件, 系統(tǒng)的積累誤差如何是一個決定性的問題。另外, 通過慣性傳感器的測量信息直接求解運動參數(shù)與綜合采用慣

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

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

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

17、 4. 9 。不管是姿態(tài)角誤差(圖4 還是速度誤差(圖4 、位置誤差(圖4 , 通過卡爾曼濾波后都有顯著減小 。, , 為航向、俯仰、傾斜角誤差(rad ; , , 為x , y , z 方向的速度誤差(m s ; , , 為x , y , z 方向的位置誤差(m 。圖4測試誤差的仿真曲線5討論從仿真結果看:即使是短時慣性測量系統(tǒng), 如果慣性測量元件的零位偏置誤差較大, 則依靠慣性傳感器直接解算所測得的姿態(tài)、速度、位置等也會有較大的誤差, 而且誤差增長速度非常快。這種情況下, 慣性測量系統(tǒng)只適用于時間非常短的汽車測試項目, 如制動試驗等(只需要幾秒鐘 , 而不太適合時間較長的道路試驗項目。為了提高廉價慣性測量系統(tǒng)的精度, 滿足汽車測試的要求, 可以采用卡爾曼濾波算法。仿真表明:對于汽車姿態(tài)、速度等運動參數(shù), 使用卡爾曼濾波后精度顯著提高。參考文獻: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