基于自適應(yīng)互補(bǔ)濾波的滾轉(zhuǎn)角測(cè)量算法

陳 督，霍鵬飛，陳 超，雷瀧杰

(西安機(jī)電信息技術(shù)研究所，陜西 西安 710065)

0 引言

彈體滾轉(zhuǎn)姿態(tài)角是制導(dǎo)彈藥判定實(shí)際飛行彈道的基本原始信息之一，為了提高測(cè)量精度和抗干擾能力，近年來(lái)普遍采用多傳感器方案進(jìn)行姿態(tài)測(cè)量，其姿態(tài)解算的關(guān)鍵技術(shù)為數(shù)據(jù)融合技術(shù)[1]。基于MEMS的傳感器具有可靠性高、小體積、低成本和高集成度等特點(diǎn)[2]，隨著MEMS技術(shù)的迅速發(fā)展，MEMS傳感器的精度不斷提高，其在多傳感器組合方面的應(yīng)用也隨之不斷增加。文獻(xiàn)[3]對(duì)擴(kuò)展卡爾曼濾波器(EKF)和互補(bǔ)濾波器進(jìn)行了比較驗(yàn)證，表明了互補(bǔ)濾波器相對(duì)于EKF具有較快的收斂速度，但是由于只限于兩種傳感器，其精度還是有限。文獻(xiàn)[4]利用三軸陀螺儀提出四元數(shù)擴(kuò)展卡爾曼濾波器，但是未對(duì)地磁矢量加以考慮，磁場(chǎng)的干擾仍未得到有效的解決。國(guó)內(nèi)外在互補(bǔ)濾波器的研究方面，對(duì)于兩種不同傳感器數(shù)據(jù)融合頗為注重[5]，致使多傳感器數(shù)據(jù)資源沒有得到充分的利用，此外，還多用陀螺儀輔助磁羅盤完成故障檢測(cè)[6]，對(duì)陀螺儀的精度要求較高。本文針對(duì)上述問題，提出了基于自適應(yīng)互補(bǔ)濾波的滾轉(zhuǎn)角測(cè)量算法。

1 滾轉(zhuǎn)角測(cè)量數(shù)學(xué)模型和互補(bǔ)濾波理論

基于互補(bǔ)濾波器的多傳感器姿態(tài)信息融合，首先在傳感器級(jí)別利用慣性傳感器進(jìn)行姿態(tài)解算，得到各個(gè)傳感器解算的結(jié)果，隨后利用互補(bǔ)濾波器進(jìn)行數(shù)據(jù)融合得到載體的姿態(tài)信息。

1.1 雙軸磁傳感器測(cè)量滾轉(zhuǎn)角數(shù)學(xué)模型

彈體的速度矢量如圖1所示，地磁要素如圖2所示。

圖1 地面坐標(biāo)系下的速度矢量Fig.1 Velocity vector in ground coordinate system

圖2 地磁要素圖Fig.2 Geomagnetic feature map

設(shè)地面坐標(biāo)系下的磁場(chǎng)向量Mg為(mxg,myg,mzg)，雙軸磁強(qiáng)計(jì)測(cè)得彈體坐標(biāo)系下磁場(chǎng)向量Mb為(x,myb,mzb)，其中x與解算滾轉(zhuǎn)角無(wú)關(guān)。由文獻(xiàn)[7]知Mg與Mb關(guān)系如下：

(1)

令:

k1=-sinθcosψmxg+cosθmyg+sinθsinψmzg

k2=sinψmxg+cosψmzg

k3=sinψmxg+cosψmzg

k4=sinθcosψmxg-cosθmyg-sinθsinψmzg

由速度矢量可以解出θ和ψ，則由式(1)可解出：

(2)

由式(2)可根據(jù)磁強(qiáng)計(jì)信息解出滾轉(zhuǎn)角。

1.2 單軸陀螺測(cè)量滾轉(zhuǎn)角數(shù)學(xué)模型

設(shè)ωx是陀螺儀的輸出，ω0是陀螺輸出的零偏。通過下式可以由陀螺信息解算滾轉(zhuǎn)角。

(3)

當(dāng)采樣率足夠高時(shí)，式(3)可以近似為:

γn=(ωx-ω0)ΔT+γn-1

(4)

通過算法確定當(dāng)前零偏量和滾轉(zhuǎn)角初始值即可計(jì)算當(dāng)前的滾轉(zhuǎn)角度。

1.3 互補(bǔ)濾波理論

假如我們對(duì)一個(gè)物理量有兩種觀測(cè)手段，一種容易受到高頻干擾，另一種容易受到低頻干擾，那么我們就可以設(shè)計(jì)兩個(gè)互補(bǔ)的濾波器F1和F2。F1是低通濾波器,F2是高通濾波器，且F1+F2≡1[8]。

互補(bǔ)濾波算法是將干擾信號(hào)在頻率上具有互補(bǔ)特性兩個(gè)信號(hào)通過上述的一組互補(bǔ)濾波器，然后加權(quán)求和從而提取有用信息的方法。基于互補(bǔ)濾波的滾轉(zhuǎn)角測(cè)量公式如下：

γ=K·γmag+(1-K)·γL

(5)

式(5)中，γ是當(dāng)前滾轉(zhuǎn)角，γmag是F1濾波器輸出的測(cè)量結(jié)果，γL是濾波器F2輸出的測(cè)量結(jié)果，K是互補(bǔ)濾波算法調(diào)節(jié)參數(shù)，它的取值直接影響到濾波精度。

2 基于自適應(yīng)互補(bǔ)濾波的滾轉(zhuǎn)角測(cè)量

互補(bǔ)濾波器是針對(duì)同一個(gè)信號(hào)的不同傳感器測(cè)量得到的噪聲特性,從頻率域分辨和消除噪聲。相比于時(shí)域的濾波器,并不需要對(duì)信號(hào)進(jìn)行精確建模,而且操作簡(jiǎn)單易實(shí)現(xiàn)[9]。

互補(bǔ)濾波的關(guān)鍵是設(shè)置合適的濾波器參數(shù)K。針對(duì)引信轉(zhuǎn)速高，干擾特征變化明顯的問題，本文利用磁測(cè)數(shù)據(jù)γmag計(jì)算出角速度ωm，然后計(jì)算ωm與陀螺儀輸出ω的差值eω，最后通過eω由式(6)—式(8)自適應(yīng)調(diào)整互補(bǔ)濾波的系數(shù)K，從而改善在不同干擾下滾轉(zhuǎn)角算法的適應(yīng)性。

(6)

eω=ωm-ω

(7)

算法需要根據(jù)陀螺角速度誤差和當(dāng)前轉(zhuǎn)速自動(dòng)調(diào)整濾波器系數(shù)K從而達(dá)到自動(dòng)適應(yīng)各種場(chǎng)景的目的。當(dāng)引信轉(zhuǎn)速降低，陀螺的輸出誤差隨之減小；當(dāng)引信進(jìn)入修正階段，磁強(qiáng)計(jì)受到舵機(jī)的干擾，誤差隨之增大。自適應(yīng)系統(tǒng)將根據(jù)式(8)自動(dòng)分辨當(dāng)前的干擾情況，采取更恰當(dāng)?shù)腒值，從而提高解算精度。

(8)

同時(shí)由于設(shè)計(jì)的濾波器在通帶內(nèi)存在t秒的群延時(shí)，產(chǎn)生的延時(shí)誤差wt在高轉(zhuǎn)速情況下會(huì)更加顯著。因此在高轉(zhuǎn)速且eω較小的情況下對(duì)濾波器輸出進(jìn)行延時(shí)補(bǔ)償可以顯著降低延時(shí)帶來(lái)的誤差。

自適應(yīng)互補(bǔ)濾波系統(tǒng)框圖如圖3所示。

圖3 系統(tǒng)框圖Fig.3 System block diagram

3 仿真驗(yàn)證

為了驗(yàn)證算法在高轉(zhuǎn)速、轉(zhuǎn)速減小、低轉(zhuǎn)速?gòu)椀佬拚拳h(huán)境下的測(cè)量精度和穩(wěn)定性，分別在三個(gè)場(chǎng)景采用K取1，0.5，0.1等定值的傳統(tǒng)互補(bǔ)濾波算法和自適應(yīng)互補(bǔ)濾波算法進(jìn)行仿真對(duì)比。

考慮到高速旋轉(zhuǎn)以及減速旋轉(zhuǎn)時(shí)舵機(jī)處于靜止?fàn)顟B(tài)，噪聲主要是高斯白噪聲，彈道修正階段，舵機(jī)噪聲強(qiáng)且同時(shí)存在高斯白噪聲，設(shè)計(jì)以下三個(gè)場(chǎng)景進(jìn)行仿真驗(yàn)證。

在高速旋轉(zhuǎn)場(chǎng)景下使轉(zhuǎn)速維持在300 r/s，陀螺儀引入31.4 rad/s的零偏，雙軸磁強(qiáng)計(jì)和陀螺儀各引入1 dBm/Hz的高斯噪聲；減速環(huán)境下，轉(zhuǎn)速100 r/s減到0 r/s，陀螺儀引入31.4 rad/s零偏，各傳感器各加1 dBm/Hz的高斯噪聲；彈道修正階段，陀螺加上0.3 rad/s的零偏，傳感器加上從實(shí)驗(yàn)樣機(jī)提取的實(shí)際舵機(jī)噪聲。圖4—圖6為上述各場(chǎng)景下的濾波誤差對(duì)比。

圖4 高轉(zhuǎn)速下的幾種情況誤差對(duì)比Fig.4 Comparison of several algorithm errors at high speeds

圖5 減旋階段的誤差對(duì)比Fig.5 Comparison of errors in the derotation phase

圖6 修正階段的誤差對(duì)比Fig.6 Comparison of errors during attitude correction

由圖4可見，傳統(tǒng)互補(bǔ)濾波器在高轉(zhuǎn)速時(shí)濾波器延時(shí)帶來(lái)的誤差十分顯著。圖5顯示，在轉(zhuǎn)速減小的場(chǎng)景下傳統(tǒng)互補(bǔ)濾波器誤差隨著轉(zhuǎn)速減小而減小。圖4、圖5中的自適應(yīng)互補(bǔ)濾波誤差均小于0.12 rad，顯著優(yōu)于傳統(tǒng)互補(bǔ)濾波器。

圖6表明傳統(tǒng)互補(bǔ)濾波器在低轉(zhuǎn)速下由濾波器延時(shí)帶來(lái)的誤差已經(jīng)可以忽略不記，且隨著K值減小，誤差有所減小，而自適應(yīng)互補(bǔ)濾波誤差小于0.03 rad，精度比傳統(tǒng)互補(bǔ)濾波提高了2～3倍。可見在全彈道環(huán)境下，自適應(yīng)互補(bǔ)濾波算法均具有穩(wěn)定的測(cè)量效果。

4 結(jié)論

本文提出了基于自適應(yīng)互補(bǔ)濾波的滾轉(zhuǎn)角測(cè)量算法。該方法采用互補(bǔ)濾波器對(duì)速率陀螺儀、磁強(qiáng)計(jì)信息進(jìn)行了融合，針對(duì)引信在出炮口時(shí)刻高速旋轉(zhuǎn)，衛(wèi)星對(duì)準(zhǔn)時(shí)刻的滾轉(zhuǎn)減旋以及彈道修正時(shí)刻的滾轉(zhuǎn)角穩(wěn)定等不同彈道特征點(diǎn)處的干擾特性，設(shè)計(jì)了參數(shù)自適應(yīng)調(diào)整的互補(bǔ)濾波器，能夠有效克服傳統(tǒng)互補(bǔ)濾波器無(wú)法適應(yīng)引信的全部工作場(chǎng)景的困難。理論分析及仿真結(jié)果表明，該算法能夠有效補(bǔ)償高速旋轉(zhuǎn)情況下濾波器延時(shí)帶來(lái)的延時(shí)誤差、陀螺零偏漂移、舵機(jī)高速運(yùn)動(dòng)引入的磁場(chǎng)干擾等，能夠在干擾存在的情況下較傳統(tǒng)固定系數(shù)互補(bǔ)濾波器滾轉(zhuǎn)角測(cè)量精度提升2～3倍，具有更強(qiáng)的環(huán)境適應(yīng)性和工程可實(shí)現(xiàn)性。