基于自適應濾波算法的磁強計/陀螺組合誤差修正*

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

(中北大學機電工程學院,太原 030051)

基于自適應濾波算法的磁強計/陀螺組合誤差修正*

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

(中北大學機電工程學院,太原 030051)

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

磁強計;陀螺;自適應卡爾曼濾波;組合姿態測量

0 引言

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

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

1 姿態解算和誤差模型

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

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

(1)

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

(2)

考慮姿態解算誤差,由真實姿態到解算姿態的轉換矩陣為:

(3)

φ=(φxφyφz)T為姿態誤差角。兩姿態之間的相對轉動角速率之差為:

(4)

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

ξ=ξg+wg

(5)

(6)

Tg為相關時間常數,wg與wξ均為白噪聲。

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

(7)

設Bn為地磁矢量在參考坐標系下的矢量,Bc為磁強計輸出值,考慮其誤差,有:

(8)

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

(9)

(10)

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

(11)

2 對姿態誤差的修正

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

將得出的系統誤差狀態方程和觀測方程離散化:

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

Zk=HkXk+Vk

(12)

其中:

該自適應濾波算法的具體原理是根據觀測量Zk估計出相關函數序列Ck,再由Ck推算出最優穩態增益矩陣K,使增益K不斷與實際測量數據Zk相適應[6]。

假設觀測量Zk具有各態歷經性,則其相關函數序列的估計為:

(13)

又因為相關函數滿足關系式:

(14)

可得:

(15)

增益矩陣是以卡爾曼濾波的最優增益矩陣為基礎的,可表示為:

(16)

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

(17)

最后有估計方程:

(18)

3 對修正算法的仿真驗證

仿真實驗以太原為仿真背景地點,以某型國產MEMS陀螺和HMC1043三軸磁強計的參數指標對陀螺和磁強計信號進行模擬。該地區磁場強度的地理坐標系分量為:Bn=10-4×[-0.02160.309-0.43]T,陀螺漂移1°/s,分辨率0.05°/s,時間常數Tg=360 s,白噪聲方差0.1°/s,磁強計分辨率1.2×10-8T,白噪聲方差10-4T。

為在計算機中模擬陀螺角速率輸出,這里假設彈體姿態參數的變化規律為:俯仰角?=π/4-0.1sin 0.2t,偏航角ψ=π/4,滾轉角γ=πt,則陀螺的理想輸出為:

(19)

實際輸出根據上述噪聲參數加入隨機漂移量和白噪聲。磁強計參考輸出為Bn在彈體坐標上的投影,并加入白噪聲。姿態解算步長0.005 ms,濾波器步長0.01 ms,仿真時間14 s,仿真曲線圖如圖1～圖3。

圖1 俯仰角仿真結果曲線

圖2 滾轉角仿真結果曲線

圖3 偏航角仿真對比曲線

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

4 結論

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

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

[2] 鮑亞琪, 陳國光, 吳坤, 等. 基于磁強計和MEMS陀螺的彈箭全姿態探測 [J]. 兵工學報, 2008, 29(10): 1227-1231.

[3] 黃旭, 王常虹, 伊國興, 等. 利用磁強計及微機械加速度計和陀螺的姿態估計擴展卡爾曼濾波器 [J]. 中國慣性技術學報, 2005, 13(2): 27-34.

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

[5] 吳錦杰, 劉昆, 石實, 等. 基于磁強計和光纖陀螺的小衛星姿態確定非線性濾波算法 [J]. 國防科技大學學報, 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