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

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

0 引言

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

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

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

1.1 雙軸磁傳感器測量滾轉(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)系下的磁場向量Mg為(mxg,myg,mzg)，雙軸磁強(qiáng)計測得彈體坐標(biāo)系下磁場向量Mb為(x,myb,mzb)，其中x與解算滾轉(zhuǎn)角無關(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)計信息解出滾轉(zhuǎn)角。

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

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

(3)

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

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

(4)

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

1.3 互補(bǔ)濾波理論

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

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

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

(5)

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

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

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

互補(bǔ)濾波的關(guān)鍵是設(shè)置合適的濾波器參數(shù)K。針對引信轉(zhuǎn)速高，干擾特征變化明顯的問題，本文利用磁測數(shù)據(jù)γmag計算出角速度ωm，然后計算ω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)速自動調(diào)整濾波器系數(shù)K從而達(dá)到自動適應(yīng)各種場景的目的。當(dāng)引信轉(zhuǎn)速降低，陀螺的輸出誤差隨之減??；當(dāng)引信進(jìn)入修正階段，磁強(qiáng)計受到舵機(jī)的干擾，誤差隨之增大。自適應(yīng)系統(tǒng)將根據(jù)式(8)自動分辨當(dāng)前的干擾情況，采取更恰當(dāng)?shù)腒值，從而提高解算精度。

(8)

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

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

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

3 仿真驗證

為了驗證算法在高轉(zhuǎn)速、轉(zhuǎn)速減小、低轉(zhuǎn)速彈道修正等環(huán)境下的測量精度和穩(wěn)定性，分別在三個場景采用K取1，0.5，0.1等定值的傳統(tǒng)互補(bǔ)濾波算法和自適應(yīng)互補(bǔ)濾波算法進(jìn)行仿真對比。

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

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

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

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

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

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

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

4 結(jié)論

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