利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器_

1、第13卷第2期 中國慣性技術(shù)學(xué)報(bào) 2005年4月 文章編號：1005-6734(200502-0027-04利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器黃 旭，王常虹，伊國興，王玉峰（哈爾濱工業(yè)大學(xué)空間控制與慣性技術(shù)研究中心，哈爾濱 150001）摘要：提出了一種用于慣性測量組合(IMU姿態(tài)估計(jì)的擴(kuò)展卡爾曼濾波器。此濾波器的狀態(tài)轉(zhuǎn)移陣?yán)没谒脑獢?shù)的等效旋轉(zhuǎn)矢量來計(jì)算姿態(tài)。等效旋轉(zhuǎn)矢量法可以避免歐拉角的奇異問題，并能有效抑制不可交換誤差。測量四元數(shù)是利用高斯-牛頓迭代法解算以加速度計(jì)和磁強(qiáng)計(jì)輸出數(shù)據(jù)為變量的方程組得出的。用從實(shí)際IMU 采集的數(shù)據(jù)對濾波器進(jìn)行測試，并比較了帶有濾波

2、器和沒有濾波器的結(jié)果。關(guān) 鍵 詞：卡爾曼濾波器；姿態(tài)；磁強(qiáng)計(jì)；加速度計(jì)；陀螺 中圖分類號：U666.1 文獻(xiàn)標(biāo)識碼：AExtended Kalman Filter for IMU Attitude Estimation Using Magnetometer, MEMS Accelerometer and GyroscopeHUANG Xu, WANG Chang-hong, YI Guo-xing, WANG Yu-feng(Space Control and Inertial Technology Research Center, Harbin Institute of Technology

3、, Harbin 150001, ChinaAbstract: This paper presents an extended Kalman filter for IMU attitude estimation. The transition matrix of this filter uses equivalent rotation vector based on quaternions to calculate the attitude, which can avoid the singularity problem of Euler angles and suppress the non

4、commutativity error. The measurement quaternion is derived from accelerometer and magnetometer data using Gauss-Newton iteration algorithm. The data collected from the real sensor are utilized to test the filter, and results are presented and compared for using and without using the filter. Key word

5、s: Kalman filter; attitude; magnetometer; accelerometer; gyroscope 0 引 言陀螺在許多應(yīng)用場合用來估計(jì)姿態(tài)，例如：航天器、衛(wèi)星、導(dǎo)彈等。但是機(jī)械陀螺、光纖陀螺和激光陀螺極高價(jià)格和大體積限制了陀螺在某些場合的應(yīng)用。近年來，在汽車工業(yè)的需求推動(dòng)下，微機(jī)械陀螺獲得了很大發(fā)展。微機(jī)械陀螺與其它陀螺相比，具有體積小、成本低、易于批量生產(chǎn)等優(yōu)點(diǎn)，從而擴(kuò)展了陀螺的應(yīng)用領(lǐng)域；但是它的精度較低，并且陀螺漂移會(huì)在短時(shí)間內(nèi)引起較大的姿態(tài)誤差。解決上述問題的常用方法是采用卡爾曼濾波器15?？柭鼮V波器利用來自GPS 、磁強(qiáng)計(jì)、加速度計(jì)和傾斜計(jì)等傳感器

6、的數(shù)據(jù)來補(bǔ)償陀螺偏差引起的姿態(tài)誤差。文獻(xiàn)1描述了一個(gè)利用GPS 和磁強(qiáng)計(jì)來收稿日期：2005-01-08作者簡介：黃旭（1973），男，哈爾濱工業(yè)大學(xué)博士生，從事組合導(dǎo)航研究。DOI:10.13695/ki.12-1222/o3.2005.02.00728 中國慣性技術(shù)學(xué)報(bào) 2005年4月補(bǔ)償姿態(tài)誤差的卡爾曼濾波器，但是未給出詳細(xì)信息。文獻(xiàn)2和文獻(xiàn)3分別給出了一個(gè)用于跟蹤問題的卡爾曼濾波器。本文提出一個(gè)用于估算IMU 姿態(tài)的擴(kuò)展卡爾曼濾波器，該IMU 由九個(gè)傳感器組成：三個(gè)加速度計(jì)、三個(gè)陀螺和三個(gè)磁強(qiáng)計(jì)。本文中的濾波器與Marins J. L.在文獻(xiàn)4中給出的濾波器相似，例如，所選擇的狀態(tài)變量

7、，以及采用高斯-牛頓迭代法來計(jì)算測得的四元數(shù)。但是至少在以下兩點(diǎn)是不同的：此濾波器采用了不同的狀態(tài)轉(zhuǎn)移陣，而且不需要地磁場模型來計(jì)算四元數(shù)測量值。1 四元數(shù)姿態(tài)就是聯(lián)系動(dòng)坐標(biāo)系和參考坐標(biāo)系角位置的參數(shù)。歐拉角和四元數(shù)是常用的表示姿態(tài)的兩種方式。四元數(shù)由于可以避免歐拉角的奇異問題而應(yīng)用得更加廣泛。由Hamilton 引入的四元數(shù)可寫為：0123q q q q =+n i j k (1其中，2221, , , =i j k ij k jk i ki j ik 。四元數(shù)乘法定義如下：01230123( (p p p p q q q q =+r i j j i j k =321001231032230

8、13210q q q q p p p p p p p p p p p p p p p p (2 四元數(shù)可用來表示矢量旋轉(zhuǎn)，其矢量部分定義了旋轉(zhuǎn)軸，標(biāo)量部分定義了旋轉(zhuǎn)角度。如果旋轉(zhuǎn)軸由參考系的方向余弦來定義，并且四元數(shù)范數(shù)限制為單位范數(shù)，則可以方便地表示旋轉(zhuǎn)。假設(shè)方向余弦角為、和，旋轉(zhuǎn)角度為，則旋轉(zhuǎn)四元數(shù)可表示為：TTcos cos sin cos sin cos sin cos sin =q n (3式中，n 為沿旋轉(zhuǎn)軸的單位矢量，T coscos cos =n 。矢量u 繞n 軸旋轉(zhuǎn)角度后的矢量為：1=v q u q (4式中，1q 是四元數(shù) q 的逆，10123q q q q =qi j

9、k 。方程(4也可以寫成矩陣乘法的形式：1=v q u q Tu (5其中，2222012312031302222212030123230122221302230101232(2( 2(2( 2( 2(q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q q +=+T 。矩陣T 的方向余弦形式為： cos cos cos sin sin sin sin cos cos sin sin sin sin cos cos cos sin sin cos cos sin sin sin cos sin sin cos

10、 cos cos =+T 。 2 等效旋轉(zhuǎn)矢量法本文采用的等效旋轉(zhuǎn)矢量法由Miller 6提出。該算法為：分別計(jì)算時(shí)刻h h h t , 3/2, 3/1=時(shí)的增量角1、2和3；按照132310.45( 0.675( =+計(jì)算角度；按如下方式計(jì)算增量四元數(shù)( h q ：( h =r q C S ， 0cos(/2 =C ， 00(1/sin(/2 =S ， T 1/20( =r (6利用( ( ( T h T h +=Q Q q 計(jì)算更新后的四元數(shù)( T h +Q ，其中表示四元數(shù)乘法。第2期 黃旭等：利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器設(shè)計(jì) 293 利用加速度計(jì)和磁強(qiáng)計(jì)

11、計(jì)算測量四元數(shù)選擇參考坐標(biāo)系為NED 坐標(biāo)系，并忽略磁北極和真北之間的差別。當(dāng)載體在參考坐標(biāo)系中靜止，并且體坐標(biāo)系和參考坐標(biāo)系重合時(shí)，加速度計(jì)和磁強(qiáng)計(jì)的測量輸出為：00r g =A ， 0r n d m m =m (7其中，g 是重力加速度，n m 是地磁場在NED 坐標(biāo)系中的北向分量，d m 是地磁場在NED 坐標(biāo)系中的垂直分量。當(dāng)載體處于任意姿態(tài)時(shí)，假定加速度計(jì)和磁強(qiáng)計(jì)的輸出是：b xy z a a a =A ， b x y z m m m =m (8參考坐標(biāo)系和體坐標(biāo)系的關(guān)系可按方程(5表示：b r =A TA ，b r =m Tm ，可以得到如下方程：(22031q q q q g a

12、 x =， (21032q q q q g a y +=， (23222120q q q q g a z += (90 (2 ( (21032232221203021=+q q q q m q q q q m q q q q m z y x (10由此可見，利用高斯-牛頓迭代法解算上述方程，可得到相應(yīng)的轉(zhuǎn)動(dòng)四元數(shù)。4 卡爾曼濾波器本文采用文獻(xiàn)7中的偽線性卡爾曼濾波器?？紤]如下系統(tǒng)：1k k k k +=+x Ax Bu w ， k k k =+y Cx v (11其中，x 是狀態(tài)矢量，A 是狀態(tài)轉(zhuǎn)移矩陣，B 是控制矩陣，u 是系統(tǒng)輸入矢量，w 是協(xié)方差陣為k Q 的過程噪聲矢量；Y 是量測矢量

13、，C 是系統(tǒng)觀測矩陣，v 是協(xié)方差陣為k R 的量測噪聲矢量?？柭鼮V波器是計(jì)算狀態(tài)x 的最優(yōu)估計(jì)x的遞歸算法?？柭鼮V波器在利用幾個(gè)不同噪聲測量量來估計(jì)無法直接測量的變量方面非常有效。因此，盡管陀螺是通過對角速率信號積分來獲得姿態(tài)，加速度計(jì)和磁強(qiáng)計(jì)卻能提供不同噪聲、無漂移的姿態(tài)?？柭鼮V波器能夠適當(dāng)權(quán)衡兩方面信息，充分利用從各個(gè)傳感器得到的數(shù)據(jù)。下面給出了我們所采用的卡爾曼濾波器的算法7。 時(shí)間傳播狀態(tài)變量的時(shí)間傳播根據(jù)：1/k k k k k k +=+xA x Bu (12 協(xié)方差陣的傳播根據(jù)：T 1/k k k k k k k +=+P A P A Q (13測量更新計(jì)算卡爾曼增益矩陣

14、：T T 111/1/1k k k k k k +=+K P C CP C R (14更新狀態(tài)估計(jì)：1/11/111/k k k k k k k k +=+xx K y Cx (15 更新協(xié)方差陣：T T 1/111/1111k k k k k k k k k +=+P I K C P I K C K R K (165 卡爾曼濾波器設(shè)計(jì)實(shí)際上，方程(7可看作差分方程，濾波器的狀態(tài)矢量在方程(7的四元數(shù)基礎(chǔ)上，增加了角速度：30 中國慣性技術(shù)學(xué)報(bào) 2005年4月 0123x y z q q q q =x (17其中，x 、y 和z 是陀螺輸出的角速率信號，0q 、1q 、2q 和3q 是由第2節(jié)

15、所述方法得出的四元數(shù)的 各個(gè)分量。輸入矢量是：0000( ( ( ( ( (x x y y z z T T T T T T T T T =+u (18因此，可得到狀態(tài)轉(zhuǎn)移矩陣A 和控制矩陣B ：0123103223013210000000000000000010000000100001h h h h h h h h h h h h h h h h q q q q q q q q q q q q q q q q =A ，0000000000100010001=B (19 式(19中，0h q 、1h q 、2h q 和3h q 是式 (9中( h q 的各個(gè)分量。 量測矢量和狀態(tài)變量相同。但是，

16、量測四元數(shù)由方程（9）（10）得出。量測矩陣7=C I ，其中7I 是77單位矩陣。姿態(tài)估計(jì)的流程圖如圖1所示。6 濾波器測試結(jié)果采用我們正在開發(fā)的IMU 的實(shí)測數(shù)據(jù)對濾波器進(jìn)行了測試。IMU 安裝在單軸轉(zhuǎn)臺上，臺面調(diào)水平，采樣率分別為200 Hz和50 Hz，數(shù)據(jù)長度為10 000點(diǎn)。通過兩種方式采集數(shù)據(jù)：轉(zhuǎn)臺靜止和轉(zhuǎn)臺勻速轉(zhuǎn)動(dòng)。轉(zhuǎn)臺靜止時(shí)采集的數(shù)據(jù)用來計(jì)算噪聲陣Q 和R 。由于單軸轉(zhuǎn)臺保持水平，IMU 的橫滾角和俯仰角一直為0，只能利用偏航角來評價(jià)濾波器；同時(shí)還要把轉(zhuǎn)臺位置的反饋信號記錄下來，作為準(zhǔn)確的偏航角，與通過計(jì)算IMU 傳感器數(shù)據(jù)所得的偏航角進(jìn)行比較。圖2和圖3是未采用濾波器，只利

17、用陀螺輸出計(jì)算出的偏航角和轉(zhuǎn)臺位置反饋，偏航角明顯開始發(fā)散。圖4和圖5是利用濾波器計(jì)算所得的偏航角和位置反饋。測試結(jié)束時(shí)，采樣率為200 Hz ，采用濾波器時(shí)偏航角誤差為0.36，采樣率為50 Hz時(shí)偏航角誤差為4.56。通過比較圖2圖5可以清楚看出，該濾波器能有效補(bǔ)償陀螺漂移引起的姿態(tài)誤差。 （下轉(zhuǎn)第34頁）圖1 姿態(tài)估計(jì)流程圖圖4 采樣率50 Hz時(shí)有濾波器偏航角和位置反饋?zhàn)兓€圖5 采樣率200 Hz時(shí)有濾波器偏航角和位置反饋?zhàn)兓€圖3 采樣率200 Hz時(shí)無濾波器偏航角和位置反饋?zhàn)兓€圖2 采樣率50 Hz時(shí)無濾波器偏航角和位置反饋?zhàn)兓€34 中國慣性技術(shù)學(xué)報(bào) 2005年4月

18、波器自身也有局限性，實(shí)際濾波結(jié)果與實(shí)際幅值并不完全吻合的。但在各 自允許的動(dòng)態(tài)范圍內(nèi)對濾波結(jié)果進(jìn)行數(shù)據(jù)處理后，兩路均可以實(shí)現(xiàn)線性化輸出(如圖4，分別是大轉(zhuǎn)速和小轉(zhuǎn)速輸入時(shí)檢測系統(tǒng)的輸出。在DSP 內(nèi)部采用軟件分檔的處理方案。對輸出結(jié)果根據(jù)最小二乘擬合，采用分段線性化進(jìn)行線性化修正，可以實(shí)現(xiàn)全量程的線性化數(shù)字量輸出。實(shí)驗(yàn)證明這樣可以取得很好的效果，如圖5。 實(shí)驗(yàn)用光纖陀螺的動(dòng)態(tài)范圍為100 ( /s，零偏穩(wěn)定性達(dá)到10 ( /s，標(biāo)度因數(shù)非線性度小于10-3的量級。4 結(jié) 論 結(jié)合分檔方法的全數(shù)字處理方案有效地解決了開環(huán)方案動(dòng)態(tài)范圍局限性帶來的難題，并克服了PZT 溫度敏感性給開環(huán)系統(tǒng)引入的測量

19、誤差。目前實(shí)驗(yàn)陀螺還存在零偏較大、零漂有待減小的問題，需進(jìn)一步改善信號處理電路，如對低噪聲放大電路、電路抗干擾等進(jìn)行優(yōu)化。對于數(shù)字信號的處理也在做進(jìn)一步研究，解決好這些問題將進(jìn)一步提高陀螺的性能。 參考文獻(xiàn)：1 Bohm K, Petermann K. Signal processing schemes for the fiber-optic gyroscopeA. FiberOptic Gyros, Proc. SPIEC, 1986, Vol.719: 36-44. 2 常勝利. 實(shí)用中低精度光纖陀螺的信號處理設(shè)計(jì)J. 激光雜志，2002，（3）. 3 孫圣和. 光纖陀螺全數(shù)字信號處理技術(shù)

20、研究J. 儀器儀表學(xué)報(bào)，2000，（6）.4 楊遠(yuǎn)洪. 開環(huán)FOG 中PZT 調(diào)制器的在線研究J. 北京航空航天大學(xué)學(xué)報(bào)，1999，（6）.（上接第30頁）7 結(jié) 論本文描述了一個(gè)用于慣測組合姿態(tài)估計(jì)的擴(kuò)展卡爾曼濾波器。該濾波器利用加速度計(jì)和磁強(qiáng)計(jì)的數(shù)據(jù)有效補(bǔ)償了陀螺漂移引起的姿態(tài)誤差。需要指出的是該濾波器只適用于載體沒有線加速度的情況。因此在IMU 應(yīng)用中，需要先根據(jù)加速度計(jì)的輸出，判斷載體的狀態(tài)。如果載體是靜止或者沒有線加速度，則使用此濾波器計(jì)算姿態(tài)，否則直接使用陀螺輸出數(shù)據(jù)計(jì)算姿態(tài)，而不使用此濾波器。參考文獻(xiàn)：1 Rios J A, White E. Fusion filter algo

21、rithm enhancements for a MEMS GPS/IMUA. ION Meeting ProceedingsC, San Diego, California, January 2002.2 Gebre-Egziabher D, Hayward R C, Powell J D. A low cost GPS/inertial attitude heading reference systemA. Proceedings of PLANS 98C, Rancho Mirage, C A, April 1998.3 Foxlin E. Inertial head-tracker sensor fusion by a complementary separate-bias Kalman filterA. Proceedings of VRAIS96, IEEEC, 1996: