王嘉雨,曹紅松,白 松,張憲國

(中北大學(xué)機(jī)電工程學(xué)院,太原 030051)

基于自適應(yīng)濾波算法的磁強(qiáng)計(jì)/陀螺組合誤差修正*

王嘉雨,曹紅松,白 松,張憲國

(中北大學(xué)機(jī)電工程學(xué)院,太原 030051)

針對彈箭姿態(tài)測量中低精度陀螺姿態(tài)解算誤差因漂移迅速增大的問題,文中采用三軸磁強(qiáng)計(jì)和陀螺組合測姿方案,對姿態(tài)角的輸出進(jìn)行修正。通過建立基于陀螺姿態(tài)解算誤差角的狀態(tài)方程和磁強(qiáng)計(jì)輸出的誤差觀測方程,采用自適應(yīng)卡爾曼濾波方法以抑制濾波發(fā)散并對姿態(tài)誤差角進(jìn)行估計(jì)。仿真結(jié)果表明:該方法能有效利用磁強(qiáng)計(jì)的輸出抑制了陀螺漂移帶來的誤差,提高解算精度,滿足長時(shí)間姿態(tài)測量的要求。

磁強(qiáng)計(jì);陀螺;自適應(yīng)卡爾曼濾波;組合姿態(tài)測量

0 引言

磁強(qiáng)計(jì)、微機(jī)械陀螺等低成本高集成度的傳感器元件在武器彈藥精確化的過程中有著明顯的優(yōu)勢和潛力,所以采用敏感元件的彈箭姿態(tài)測量方法成為近年來研究較多的課題[1]。但是微機(jī)械陀螺精度低,單一使用難以滿足長時(shí)間姿態(tài)測量要求,而單獨(dú)使用三軸磁強(qiáng)計(jì)無法完成載體相對參考坐標(biāo)系的姿態(tài)測量[2]。

利用磁強(qiáng)計(jì)與微機(jī)械陀螺形成組合彈箭姿態(tài)測量系統(tǒng)是一個(gè)合適的選擇,它能夠綜合陀螺短時(shí)間的高精度和磁強(qiáng)計(jì)的無漂移特性[3]。由此文中提出一種基于三軸微機(jī)械陀螺與磁強(qiáng)計(jì)的組合彈箭姿態(tài)測量方法,該方法利用微機(jī)械陀螺的輸出實(shí)時(shí)解算姿態(tài)信息,然后依據(jù)建立的誤差角狀態(tài)方程,以磁強(qiáng)計(jì)的輸出作為姿態(tài)誤差角的觀測量,使用自適應(yīng)卡爾曼濾波方法對姿態(tài)誤差角進(jìn)行估計(jì),并對陀螺解算的姿態(tài)進(jìn)行修正,該方法使用靈活,能夠有效抑制陀螺漂移,具有較高的精度。

1 姿態(tài)解算和誤差模型

文中姿態(tài)解算使用的參考坐標(biāo)系為地理坐標(biāo)系(O-xnynzn),即常用的東-北-天坐標(biāo)系。

彈體姿態(tài)解算方法采用解姿態(tài)矩陣微分方程的方法,其公式如下:

(1)

采用四階畢-卡逼近求解之:

(2)

考慮姿態(tài)解算誤差,由真實(shí)姿態(tài)到解算姿態(tài)的轉(zhuǎn)換矩陣為:

(3)

φ=(φxφyφz)T為姿態(tài)誤差角。兩姿態(tài)之間的相對轉(zhuǎn)動(dòng)角速率之差為:

(4)

其中:ωc為陀螺輸出值;ωa為載體相對參考坐標(biāo)的真實(shí)轉(zhuǎn)動(dòng)角速率。設(shè)ωc與ωa之間的差值為角速率測量誤差ξ,這里將其視為白噪聲和一階馬爾科夫過程的疊加:

ξ=ξg+wg

(5)

(6)

Tg為相關(guān)時(shí)間常數(shù),wg與wξ均為白噪聲。

若以φ和ξg作為姿態(tài)誤差方程的狀態(tài)變量,則由式(4)～式(6)可以得出系統(tǒng)誤差方程為:

(7)

設(shè)Bn為地磁矢量在參考坐標(biāo)系下的矢量,Bc為磁強(qiáng)計(jì)輸出值,考慮其誤差,有:

(8)

測量誤差v視為均值為零的白噪聲。

(9)

(10)

則根據(jù)式(8)～式(10)可得觀測方程:

(11)

2 對姿態(tài)誤差的修正

考慮到陀螺及磁強(qiáng)計(jì)輸出中包含的噪聲并不為嚴(yán)格的白噪聲,且其統(tǒng)計(jì)特性無法精確獲得。為避免由此帶來的濾波器易發(fā)散的問題,文中使用一種能夠在線估計(jì)噪聲統(tǒng)計(jì)特性的自適應(yīng)卡爾曼濾波算法,它通過觀測值間接推算出自適應(yīng)濾波增益,使得濾波具有良好的收斂性質(zhì)。

將得出的系統(tǒng)誤差狀態(tài)方程和觀測方程離散化:

Xk=φk,k-1Xk-1+Wk-1

Zk=HkXk+Vk

(12)

其中:

該自適應(yīng)濾波算法的具體原理是根據(jù)觀測量Zk估計(jì)出相關(guān)函數(shù)序列Ck,再由Ck推算出最優(yōu)穩(wěn)態(tài)增益矩陣K,使增益K不斷與實(shí)際測量數(shù)據(jù)Zk相適應(yīng)[6]。

假設(shè)觀測量Zk具有各態(tài)歷經(jīng)性,則其相關(guān)函數(shù)序列的估計(jì)為:

(13)

又因?yàn)橄嚓P(guān)函數(shù)滿足關(guān)系式:

(14)

可得:

(15)

增益矩陣是以卡爾曼濾波的最優(yōu)增益矩陣為基礎(chǔ)的,可表示為:

(16)

式中只有Sk為未知量,由于:

(17)

最后有估計(jì)方程:

(18)

3 對修正算法的仿真驗(yàn)證

仿真實(shí)驗(yàn)以太原為仿真背景地點(diǎn),以某型國產(chǎn)MEMS陀螺和HMC1043三軸磁強(qiáng)計(jì)的參數(shù)指標(biāo)對陀螺和磁強(qiáng)計(jì)信號進(jìn)行模擬。該地區(qū)磁場強(qiáng)度的地理坐標(biāo)系分量為:Bn=10-4×[-0.02160.309-0.43]T,陀螺漂移1°/s,分辨率0.05°/s,時(shí)間常數(shù)Tg=360 s,白噪聲方差0.1°/s,磁強(qiáng)計(jì)分辨率1.2×10-8T,白噪聲方差10-4T。

為在計(jì)算機(jī)中模擬陀螺角速率輸出,這里假設(shè)彈體姿態(tài)參數(shù)的變化規(guī)律為:俯仰角?=π/4-0.1sin 0.2t,偏航角ψ=π/4,滾轉(zhuǎn)角γ=πt,則陀螺的理想輸出為:

(19)

實(shí)際輸出根據(jù)上述噪聲參數(shù)加入隨機(jī)漂移量和白噪聲。磁強(qiáng)計(jì)參考輸出為Bn在彈體坐標(biāo)上的投影,并加入白噪聲。姿態(tài)解算步長0.005 ms,濾波器步長0.01 ms,仿真時(shí)間14 s,仿真曲線圖如圖1～圖3。

圖1 俯仰角仿真結(jié)果曲線

圖2 滾轉(zhuǎn)角仿真結(jié)果曲線

圖3 偏航角仿真對比曲線

圖1～圖3為未經(jīng)修正的陀螺單獨(dú)解算姿態(tài)角、地磁+陀螺組合解算的姿態(tài)角和彈體實(shí)際姿態(tài)的仿真曲線圖,以及相應(yīng)的誤差比較曲線。從圖中可以看出,隨著時(shí)間增長,陀螺單獨(dú)解算的姿態(tài)誤差迅速積累,誤差角呈發(fā)散趨勢或存在較大振蕩;而經(jīng)過地磁傳感器校正的地磁+陀螺組合解算的結(jié)果,解算的姿態(tài)角的誤差量沒有出現(xiàn)明顯的增大趨勢,始終較好的貼合了實(shí)際姿態(tài)曲線,濾波收斂度良好,最大誤差角均不超過2°,滿足彈箭姿態(tài)測量的精度指標(biāo),說明濾波算法有效抑制了陀螺隨機(jī)漂移帶來的姿態(tài)角解算發(fā)散,提高了姿態(tài)解算精度。

4 結(jié)論

文中提出的方法是以陀螺輸出量對彈體姿態(tài)進(jìn)行解算,通過建立陀螺解算的姿態(tài)誤差角狀態(tài)方程和磁強(qiáng)計(jì)輸出為觀測量的觀測方程,使用自適應(yīng)卡爾曼濾波方法對誤差角進(jìn)行估計(jì),并對解算結(jié)果進(jìn)行修正。從仿真結(jié)果看:采用陀螺與磁強(qiáng)計(jì)的組合測姿方法相對陀螺單獨(dú)解算的姿態(tài)精度有了明顯提高,而且無需借助其他輔助信息,誤差量穩(wěn)定,說明了算法的有效性和方案的簡便性。但該方法只經(jīng)過數(shù)值仿真的驗(yàn)證,實(shí)際影響解算精度的因素則要多很多,如陀螺建模的誤差,磁場干擾等等,因此還需要大量的實(shí)測數(shù)據(jù)支持和進(jìn)一步的分析研究。

[1] 曹紅松, 馮順山, 趙捍東, 等. 地磁陀螺組合彈藥姿態(tài)探測技術(shù)研究 [J]. 彈箭與制導(dǎo)學(xué)報(bào), 2006, 26(3): 142-145.

[2] 鮑亞琪, 陳國光, 吳坤, 等. 基于磁強(qiáng)計(jì)和MEMS陀螺的彈箭全姿態(tài)探測 [J]. 兵工學(xué)報(bào), 2008, 29(10): 1227-1231.

[3] 黃旭, 王常虹, 伊國興, 等. 利用磁強(qiáng)計(jì)及微機(jī)械加速度計(jì)和陀螺的姿態(tài)估計(jì)擴(kuò)展卡爾曼濾波器 [J]. 中國慣性技術(shù)學(xué)報(bào), 2005, 13(2): 27-34.

[4] 晏登洋, 任建新, 宋永軍. 慣性/地磁組合導(dǎo)航技術(shù)研究 [J]. 機(jī)械與電子, 2007(1): 19-22.

[5] 吳錦杰, 劉昆, 石實(shí), 等. 基于磁強(qiáng)計(jì)和光纖陀螺的小衛(wèi)星姿態(tài)確定非線性濾波算法 [J]. 國防科技大學(xué)學(xué)報(bào), 2013, 35(1): 1-6.

[6] Haykin S. Adaptive filter theroy [M]. Beijing: Publishing House of Electronics Industry, 2002.

[7] Crassidis J L, Markley F L. Unscented filtering for spacecraft attitude estimation [J]. Journal of Guidance, Control and Dynamics, 2003, 26(4): 536-545.

Research on Integrated Error Correction of Magnetometers and Gyroscope Using Adaptive Filtering

WANG Jiayu, CAO Hongsong, BAI Song, ZHANG Xianguo

(School of Mechatronics Engineering, North University of China, Taiyuan 030051, China)

As for attitude measurement of missiles, low-precision gyroscope can not get satisfactory accuracy. For this reason, an integrated attitude measurement method was used to correct output of attitude angle, the state equation of attitude error angle and the error observation linear equations that based on the outputs of three-axis magnetometer were built, an adaptive filtering method was used to estimate error angle optimally and correct the output of attitude angle. The simulation results show that this method suppresses the error caused by gyroscope drift effectively by using the output of magnetometer and improves the accuracy of the attitude solver, satisfying the requirement of longtime attitude measurement.

magnetometer; gyroscope; adaptive Kalman filtering; integrated attitude measurement

2014-04-04

王嘉雨(1991-),女,陜西富平人,碩士研究生,研究方向:智能控制。

TN397

A