基于磁強(qiáng)計(jì)/陀螺的卡爾曼濾波定姿算法

張曉霞,曹詠弘

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

0 引言

在組合導(dǎo)航系統(tǒng)中,姿態(tài)解算是一個(gè)重要的研究課題。陀螺是最常用的姿態(tài)傳感器,但是高性能陀螺的體積大、價(jià)格高,限制了陀螺在某些領(lǐng)域的應(yīng)用。近年來,MEMS陀螺由于其體積小、成本低、易于批量生產(chǎn),因而在各大領(lǐng)域受到青睞。

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

本文針對以上算法的不足和限制,提出了一種磁強(qiáng)計(jì)和MEMS陀螺組合確定姿態(tài)的四元數(shù)卡爾曼濾波方法。

1 方向余弦四元數(shù)

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

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

2 磁強(qiáng)計(jì)與陀螺組合的卡爾曼濾波定姿算法

2.1 卡爾曼濾波定姿算法的狀態(tài)方程

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

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

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

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

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

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

2.3 卡爾曼濾波定姿算法

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

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

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

增益矩陣為:

預(yù)測、估計(jì)狀態(tài)分別為:

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

3 仿真實(shí)驗(yàn)

以工程實(shí)際問題為仿真背景,對由陀螺和磁強(qiáng)計(jì)構(gòu)成的組合測試系統(tǒng)進(jìn)行數(shù)值仿真。仿真參數(shù)如下:仿真時(shí)間為20 s,算法采樣周期為0.01 s。取太原為實(shí)驗(yàn)地,該地區(qū)磁傾角I=55°,磁偏角α=4°,垂直分量為:0.43×10-4T,水平分量為:0.31×10-4T。

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

其對應(yīng)的均方誤差陣為:

仿真實(shí)驗(yàn)結(jié)果如圖1—圖3所示。

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

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

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

圖3 滾轉(zhuǎn)角誤差曲線圖Fig.3 Error curve of roll angle

4 結(jié)論

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

[1]黃旭,王常虹,伊國興,等.利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器[J].中國慣性技術(shù)學(xué)報(bào),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]崔敏,馬鐵華,段精婧,等.基于磁強(qiáng)計(jì)和陀螺的彈箭飛行姿態(tài)測試方法[J].彈箭與制導(dǎo)學(xué)報(bào),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]鮑亞琪,陳國光,吳坤,等.基于磁強(qiáng)計(jì)和 MEMS陀螺的彈箭全姿態(tài)探測[J].兵工學(xué)報(bào),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]劉建業(yè),曾慶化,趙偉,等.導(dǎo)航系統(tǒng)理論與應(yīng)用[M].西安:西北工業(yè)大學(xué)出版社,2010.

[5]鄧正隆.慣性技術(shù)[M].哈爾濱:哈爾濱工業(yè)大學(xué)出版社,2006.

[6]王志賢.最優(yōu)狀態(tài)估計(jì)與系統(tǒng)辨識[M].西安:西北工業(yè)大學(xué)出版社,2004.

[7]曹詠弘,祖靜,林祖森,等.基于加速度計(jì)與磁強(qiáng)計(jì)組合的子彈姿態(tài)虛擬測試[J].測試技術(shù)學(xué)報(bào),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.