基于卡爾曼濾波算法的彈箭飛行姿態(tài)測(cè)試方法

王勇貞

(中北大學(xué) 電子測(cè)試技術(shù)國(guó)家重點(diǎn)實(shí)驗(yàn)室，山西 太原 030051)

0 引言

獲取彈箭的飛行姿態(tài)信息對(duì)分析其飛行動(dòng)力學(xué)特性具有重要意義。在現(xiàn)代武器試驗(yàn)中，常用遙測(cè)系統(tǒng)獲取各系統(tǒng)的工作狀態(tài)參數(shù)，為評(píng)定導(dǎo)彈的性能及為進(jìn)行導(dǎo)彈故障分析提供依據(jù)。目前國(guó)內(nèi)外對(duì)于彈體飛行姿態(tài)參數(shù)，常用的捷聯(lián)慣導(dǎo)系統(tǒng)是采用加速度計(jì)法進(jìn)行測(cè)量，但由于傳統(tǒng)的加速度計(jì)存在安裝復(fù)雜、安裝精度要求高、解算復(fù)雜等問題，文中舍棄了加速度計(jì)作為慣性器件，而是選用MEMS 陀螺作為慣性器件。MEMS 陀螺是根據(jù)牛頓慣性定律的原理進(jìn)行測(cè)量的，此方法受外界因素影響較小，在過(guò)載不太大時(shí)應(yīng)用較方便，但是MEMS 陀螺誤差隨時(shí)間積累［1，2］。文中將地磁傳感器引入慣性測(cè)量系統(tǒng)進(jìn)行組合設(shè)計(jì)，抑制系統(tǒng)輸出誤差，并在此基礎(chǔ)上設(shè)計(jì)了卡爾曼濾波器進(jìn)行信息融合用來(lái)修正捷聯(lián)慣導(dǎo)系統(tǒng)的累積誤差。

1 基于地磁/MEMS 陀螺的卡爾曼濾波算法

基于地磁/MEMS 陀螺組合測(cè)量彈箭飛行姿態(tài)的多傳感信息融合算法，采用這種組合可以提供兩種具有高度互補(bǔ)特性的獨(dú)立信息源，MEMS 陀螺提供時(shí)間長(zhǎng)、穩(wěn)定性高的數(shù)據(jù)，地磁傳感器提供時(shí)間短、精度高的數(shù)據(jù)，最終通過(guò)Kalman 信息融合算法將這兩種數(shù)據(jù)處理并獲取相對(duì)準(zhǔn)確的彈箭飛行姿態(tài)信息?？柭鼮V波算法［3］是一個(gè)高效的最優(yōu)化自回歸數(shù)據(jù)處理算法，它實(shí)質(zhì)上是通過(guò)系統(tǒng)的量測(cè)值重新構(gòu)造系統(tǒng)的狀態(tài)向量，并以“預(yù)測(cè)—實(shí)測(cè)—修正”的模式順序遞推，它利用測(cè)量系統(tǒng)的系統(tǒng)噪聲和量測(cè)噪聲的統(tǒng)計(jì)特性，把系統(tǒng)的觀測(cè)量作為濾波器的輸入，把所要求取的參數(shù)估計(jì)值作為濾波器的輸出，通過(guò)空間狀態(tài)更新和觀測(cè)更新算法有效地組織在一起，最后根據(jù)系統(tǒng)的狀態(tài)方程和觀測(cè)方程計(jì)算出我們所需要的參數(shù)。

使用三軸陀螺角速度傳感器測(cè)量姿態(tài)角的方法很多，比如有:四元數(shù)法、歐拉角法、卡爾丹角法、等效旋轉(zhuǎn)矢量法等［4］。由于四元數(shù)法的計(jì)算量相對(duì)比較小、算法簡(jiǎn)單，因此在基于地磁/MEMS 陀螺組合測(cè)量系統(tǒng)中采用四元數(shù)姿態(tài)更新算法。基于地磁/MEMS 陀螺的卡爾曼濾波算法如下所述，當(dāng)彈箭飛行體的初始位置確定(即彈箭飛行體的初始姿態(tài)角已知)以后，可推導(dǎo)求出四元數(shù)的初始值，四元數(shù)的初值計(jì)算公式如下:

根據(jù)MEMS 陀螺的三軸輸出數(shù)據(jù)求出采樣時(shí)間間隔的角增量，依據(jù)四元數(shù)微分方程的畢卡求解法就可以求出各個(gè)時(shí)刻的四元數(shù)，但是由于MEMS 陀螺存在累積誤差，因此上面求出的四元數(shù)存在一定的誤差，需要利用地磁傳感器的輸出信息對(duì)它做出修正，該修正過(guò)程是通過(guò)卡爾曼濾波算法將陀螺數(shù)據(jù)求出的四元數(shù)與地磁傳感器的輸出信息進(jìn)行信息融合求出最優(yōu)的四元數(shù)，最后經(jīng)最優(yōu)的四元數(shù)計(jì)算出彈箭飛行體的飛行姿態(tài)角信息。在卡爾曼濾波算法中采用畢卡求解法的四階近似算法作為濾波系統(tǒng)的狀態(tài)方程，如下所示:

式中，Hb1、Hbm、Hbn為系統(tǒng)的觀測(cè)量。假設(shè)系統(tǒng)的噪聲驅(qū)動(dòng)矩陣Γk，k－1為單位矩陣，{Wk}和{Vk}是互不相關(guān)的零均值白噪聲序列。

2 軟件仿真條件

下面對(duì)所建立的基于地磁/MEMS 陀螺的卡爾曼濾波算法進(jìn)行軟件的仿真和算法分析，仿真軟件采用MATLAB 軟件。實(shí)驗(yàn)中的地磁信息采用太原地區(qū)的地磁參數(shù)，如下式所示:

由于該課題主要用來(lái)解決近程高自旋的小型彈箭的飛行姿態(tài)測(cè)試，所以假設(shè)彈箭的姿態(tài)角變化規(guī)律如下式所示:

式中，t為時(shí)間，采樣頻率為10k/s ，采樣總時(shí)間為10s 。有了式(6)所假設(shè)的運(yùn)動(dòng)規(guī)律我們可求出陀螺三個(gè)軸的輸出，求解公式如下:

現(xiàn)在，根據(jù)前面所構(gòu)造的卡爾曼濾波算法模型即可進(jìn)行編程和軟件仿真。

3 軟件仿真結(jié)果與分析

在仿真條件下，采用卡爾曼濾波信息融合算法對(duì)基于地磁/MEMS 陀螺組合測(cè)量彈箭飛行姿態(tài)的仿真結(jié)果如圖1所示:

圖1 彈箭飛行姿態(tài)角仿真結(jié)果曲線

仿真條件下，彈箭飛行姿態(tài)角誤差曲線如圖2、圖3和圖4所示:

圖2 滾轉(zhuǎn)角誤差曲線圖

圖3 俯仰角誤差曲線圖

圖4 偏航角誤差曲線圖

從仿真結(jié)果圖2 到圖4所示，滾轉(zhuǎn)角誤差在仿真時(shí)間內(nèi)最大幅值為3.8×10－4弧度，俯仰角誤差在仿真時(shí)間內(nèi)的最大幅值為11.3×10－4弧度，偏航角誤差在仿真時(shí)間內(nèi)的最大幅值為4×10－4弧度，這些誤差隨著時(shí)間的推移整體上有放大的趨勢(shì)，通過(guò)分析可知這些誤差主要來(lái)自模型的不準(zhǔn)確，而誤差隨時(shí)間的推移有放大趨勢(shì)則可能是由于算法的遞推特性引起的，同樣模型不準(zhǔn)確也可能引起誤差隨時(shí)間的推移而放大，但從仿真結(jié)果看它也足以滿足小型彈箭的飛行姿態(tài)測(cè)試要求了，從這些仿真圖的結(jié)果看，基于地磁/MEMS 陀螺的卡爾曼濾波算法理論上是完全可以滿足對(duì)小型彈箭飛行姿態(tài)測(cè)試技術(shù)要求的。

另外通過(guò)大量的實(shí)驗(yàn)發(fā)現(xiàn)如果飛行體的各個(gè)姿態(tài)角變化頻率和幅度越大該算法受誤差的影響越小。因此該算法比較適用于高動(dòng)態(tài)短近程的小型彈箭的姿態(tài)測(cè)量。

4 結(jié)論

文中重點(diǎn)介紹了基于地磁、陀螺的卡爾曼濾波估計(jì)算法，通過(guò)軟件分析可知用卡爾曼濾波優(yōu)化姿態(tài)角的算法較好地克服了由單陀螺解算姿態(tài)角時(shí)存在的零漂問題，提高了測(cè)試精度，有進(jìn)一步研究的價(jià)值和必要性。

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

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

［3］劉建業(yè)，曾慶化，趙偉，等.導(dǎo)航系統(tǒng)理論與應(yīng)用［M］.西安:西北工業(yè)大學(xué)出版社，2010.

［4］鄧正隆.慣性技術(shù)［M］.哈爾濱:哈爾濱工業(yè)大學(xué)出版社，2006.

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

［6］曹詠弘，祖靜，林祖森，等.基于加速度計(jì)與磁強(qiáng)計(jì)組合的子彈姿態(tài)虛擬測(cè)試［J］.測(cè)試技術(shù)學(xué)報(bào)，2004，18(S5):42－45.