基于磁強計/陀螺的卡爾曼濾波定姿算法

張曉霞,曹詠弘

(中北大學理學院,山西太原 030051)

0 引言

在組合導航系統中,姿態解算是一個重要的研究課題。陀螺是最常用的姿態傳感器,但是高性能陀螺的體積大、價格高,限制了陀螺在某些領域的應用。近年來,MEMS陀螺由于其體積小、成本低、易于批量生產,因而在各大領域受到青睞。

對陀螺測得的三軸姿態角速率積分可以確定姿態,但其測量精度易受陀螺角速率誤差的影響,會在短時間內引起較大的誤差,解決該問題常用的方法是采用卡爾曼濾波器[1-3]。卡爾曼濾波器利用來自加速度計、磁強計、傾斜計等傳感器的數據來補償陀螺引起的誤差。隨著低成本固態慣性器件和磁傳感器件的發展,利用MEMS陀螺和磁強計進行姿態探測是近年來測量系統研究的熱點之一。文獻[1]設計了一個利用加速度計和磁強計來補償誤差的擴展卡爾曼濾波器。結合對重力場和地磁場的測量,使用高斯-牛頓迭代法求解非線性方程組,從而獲得四元數。該算法不僅使用了對重力場的測量,而且要求載體處于靜止或不存在線加速度。因此,該算法具有局限性。文獻[2]采用磁強計和陀螺來測量姿態角。該算法是先通過磁阻傳感器來確定滾轉角,再利用公式計算另外兩個姿態角,從而獲得姿態四元數。但在磁阻傳感器測姿態角一節中,計算滾轉角的公式只描述了一種特殊情形,因而具有片面性。文獻[3]提出了一種采用磁強計和MEMS陀螺構建低成本姿態探測系統的方案。該算法利用“先驗彈道信息”輔助磁強計的測量,獲得滾轉姿態角,隨后利用遞推算法獲取另外兩個姿態角,從而獲得姿態四元數。該算法必須事先獲得彈道的先驗信息,再結合磁強計的數據對陀螺誤差進行補償,因而限制了該算法的運用。

本文針對以上算法的不足和限制,提出了一種磁強計和MEMS陀螺組合確定姿態的四元數卡爾曼濾波方法。

1 方向余弦四元數

確定彈丸的姿態就是確定載體坐標系(b)相對于慣性坐標系(n)的位置,為避免出現奇異,彈丸的姿態通常由四元數形式的方向余弦矩陣確定。式

(1)中,四元數 q0、q1、q2、q3約束方程為(t)+(t)+q22(t)+q23(t)=1,方向余弦矩陣在姿態確定中起著重要作用,由此方向余弦矩陣還可以求出姿態角。

2 磁強計與陀螺組合的卡爾曼濾波定姿算法

2.1 卡爾曼濾波定姿算法的狀態方程

公式(2)為描述四元數變化規律的四元數微分方程[4]:

式(2)中,ωx,ωy,ωz為安裝在彈軸上的陀螺測量的角速度分量。

把式(2)當做狀態方程,并取q(t)=[q0(t)q1(t)q2(t)q3(t)]T為狀態向量,由于該方程是連續的,應用比卡逼近法[5]將其離散化,得到:

公式(3)就是離散化的狀態方程。其中:

2.2 卡爾曼濾波定姿算法的觀測方程

公式(4)即為觀測方程,該方程的特點是H(k)矩陣中含有狀態變量q(k)。其中:

2.3 卡爾曼濾波定姿算法

由于實際問題都是含有噪聲的,所以反映實際問題的狀態方程和觀測方程如下:

式中,W為均值為零方差為Q的系統白噪聲,V為均值為零方差為R的量測白噪聲,且W與V不相關。T為4階單位陣。

下面給出卡爾曼濾波器遞推算法[6]:預測、估計誤差方差陣分別為:

增益矩陣為:

預測、估計狀態分別為:

卡爾曼濾波是一種遞推算法,該算法充分利用陀螺輸出的角速率信號和磁強計輸出的地磁信號,適當權衡兩方面的信息,得到對狀態q(k)的最優估計,且狀態每一次更新估計都是由前一次估計和新的輸入數據計算得到,因此,只需儲存前一次的估計,即可實現實時處理。

3 仿真實驗

以工程實際問題為仿真背景,對由陀螺和磁強計構成的組合測試系統進行數值仿真。仿真參數如下:仿真時間為20 s,算法采樣周期為0.01 s。取太原為實驗地,該地區磁傾角I=55°,磁偏角α=4°,垂直分量為:0.43×10-4T,水平分量為:0.31×10-4T。

仿真中:假定陀螺為均值為零,方差為0.1(°)/s的白噪聲,磁強計為均值為零方差為0.000 1T的白噪聲,卡爾曼濾波器初值分別為:

其對應的均方誤差陣為:

仿真實驗結果如圖1—圖3所示。

從仿真實驗結果可知:運用卡爾曼濾波算法進行最優估計姿態角誤差不隨時間積累,被控制在一定范圍之內。偏航角誤差保持在±0.5°之內,俯仰角誤差也在±0.48°,而滾轉角誤差也被控制在±0.5°。因此,上述模型解算姿態角算法簡單,易于操作,從一定程度上抑制了誤差的積累,解算精度有了明顯提高。

圖1 偏航角誤差曲線圖Fig.1 Error curve of yaw angle

圖2 俯仰角誤差曲線圖Fig.2 Error curve of pitch attitude

圖3 滾轉角誤差曲線圖Fig.3 Error curve of roll angle

4 結論

本文提出了一種基于磁強計與陀螺組合確定姿態的卡爾曼濾波算法。該算法利用陀螺測量的姿態角速率,結合磁強計輸出的地磁分量,運用卡爾曼濾波算法對三個姿態角同時進行最優估計,而不需要先驗假設。仿真結果表明:估計誤差較小且不隨時間積累,即算法可行。應該指出的是:影響陀螺磁強計組合姿態測量精度的因素有很多,在這里僅考慮了白噪聲對系統的影響,其他因素的影響有待于進一步的研究。

[1]黃旭,王常虹,伊國興,等.利用磁強計及微機械加速度計和陀螺的姿態估計擴展卡爾曼濾波器[J].中國慣性技術學報,2005,13(2):27-30.HUAN Xu,WANG Changhong,YI Guoxing,et al.Extended Kalman Filter for IMU attitude estimation using magnetometer,MEMS accelerometer and gyroscope[J].Journal of Chinese Inertial Technology,2005,13(2):27-30.

[2]崔敏,馬鐵華,段精婧,等.基于磁強計和陀螺的彈箭飛行姿態測試方法[J].彈箭與制導學報,2010,30(6):85-87.CUI Min,MA Tiehua,DUAN Jingjing,et al.Missile/rocket flight attitude test method based on magnetometer and gyroscope[J].Journal of Projectiles,Rockets,Missiles and Guidance,2010,30(6):85-87.

[3]鮑亞琪,陳國光,吳坤,等.基于磁強計和 MEMS陀螺的彈箭全姿態探測[J].兵工學報,2008,29(10):1 227-1 231.BAO Yaqi,CHEN Guoguang,WU Kun,et al.Research on attitude determination using magnetometers and MEMS inertial sensors[J].Acta Armamentarii,2008,29(10):1 227-1 231.

[4]劉建業,曾慶化,趙偉,等.導航系統理論與應用[M].西安:西北工業大學出版社,2010.

[5]鄧正隆.慣性技術[M].哈爾濱:哈爾濱工業大學出版社,2006.

[6]王志賢.最優狀態估計與系統辨識[M].西安:西北工業大學出版社,2004.

[7]曹詠弘,祖靜,林祖森,等.基于加速度計與磁強計組合的子彈姿態虛擬測試[J].測試技術學報,2004,18(S5):42-45.CAO Yonghong,ZU Jing,LIN Zusen,et al.Virtual test of attitude of bullet based on the accelerometer and magnetometers combination[J].Journal of Test and Measurement Technology,2004,18(S5):42-45.