雙軸地磁輔助的慣性/衛(wèi)星組合導(dǎo)航方法

賴玉敏, 宋嘉鈺, 韓志強(qiáng), 劉洪政, 游金川

（四川航天系統(tǒng)工程研究所, 成都 610199）

0 引言

旋轉(zhuǎn)制導(dǎo)導(dǎo)彈是一種在飛行中繞自身縱軸連續(xù)旋轉(zhuǎn)的導(dǎo)彈。 傳統(tǒng)上, 旋轉(zhuǎn)體制的導(dǎo)彈主要采用單通道控制, 利用舵面偏轉(zhuǎn)產(chǎn)生的周期平均作用力控制導(dǎo)彈飛向目標(biāo)［1］。 隨著戰(zhàn)爭環(huán)境的惡化,攻防對抗更激烈, 對導(dǎo)彈高機(jī)動性能的要求不斷加強(qiáng), 具有更高控制效率的控制方案越來越多地應(yīng)用在旋轉(zhuǎn)彈上, 控制系統(tǒng)對彈體的姿態(tài)精度也提出了更高的要求。

慣性/衛(wèi)星組合導(dǎo)航一般適用于傾斜穩(wěn)定的導(dǎo)彈, 能夠較為準(zhǔn)確地提供彈體的姿態(tài)、 速度和位置信息。 而對于旋轉(zhuǎn)彈, 由于其繞自身縱軸旋轉(zhuǎn),直接利用陀螺儀測量滾轉(zhuǎn)角速率將引入很大的誤差（尤其對于低成本的MEMS 陀螺儀）［2］, 因此常利用地磁傳感器測量旋轉(zhuǎn)彈的滾轉(zhuǎn)姿態(tài)。 文獻(xiàn)［3］利用旋轉(zhuǎn)彈體本身動力學(xué)模型與安裝在彈體橫截面的雙軸地磁傳感器輸出分別構(gòu)建狀態(tài)方程和量測方程, 通過擴(kuò)展Kalman 濾波（EKF）實現(xiàn)了精度約10°的彈體滾轉(zhuǎn)角估計; 文獻(xiàn)［4］推導(dǎo)出了彈體滾轉(zhuǎn)角速率與磁阻滾轉(zhuǎn)角速率的關(guān)系式, 指出彈體滾轉(zhuǎn)角速率的精度受俯仰角、 偏航角以及射線等因素的影響; 文獻(xiàn)［5］根據(jù)坐標(biāo)變換關(guān)系建立了聯(lián)立方程式, 推導(dǎo)了彈體滾轉(zhuǎn)角解算公式, 在射向大于15°時, 滾轉(zhuǎn)角誤差高達(dá)20°。 上述方案表明,僅靠地磁傳感器測量滾轉(zhuǎn)角存在較大的測量誤差,因此也有學(xué)者研究地磁傳感器與慣組的組合測量濾波算法［6-8］, 但由于地磁傳感器僅提供姿態(tài)信息,慣性導(dǎo)航計算所得的速度、 位置無法得到校正。

本文提出了一種適用于旋轉(zhuǎn)彈的雙軸地磁輔助慣性/衛(wèi)星組合導(dǎo)航方法, 能夠同時精確計算出導(dǎo)彈的姿態(tài)、 速度以及位置。 利用安裝在彈體橫截面的雙軸地磁傳感器輸出的兩個地磁分量作為姿態(tài)觀測量, 與衛(wèi)星導(dǎo)航提供的位置、 速度觀測量共同構(gòu)建濾波器觀測方程。 為解決衛(wèi)星導(dǎo)航接收機(jī)與地磁傳感器信息輸出周期不一致的問題,本文采用序貫Kalman 濾波實現(xiàn)兩個不同信息源的信息融合。 由于方案中, 地磁傳感器為安裝在彈體橫截面的雙軸地磁傳感器, 因此也具有地磁標(biāo)定簡單、 對彈道要求低的優(yōu)點。

1 地磁場模型

在靠近地面空間內(nèi)的無源區(qū), 主磁場可以表示為標(biāo)量磁位V, 將其展開成如下的球諧函數(shù)表達(dá)式［9］

式（1）中,a為參考球半徑;r為離開地心的徑向距離,r=a+h;h為海拔高度;θ為地理余緯;λ為地理經(jīng)度;pmn（cosθ）為n階m次的締合Legendre 函數(shù)（施密特準(zhǔn)歸一化的n階m次Legendre 函數(shù)）。 利用IGRF 提供的地磁球諧項系數(shù)可計算北-天-東地理系下的地磁分量, 如下所示

對于近程導(dǎo)彈, 地磁場在地理坐標(biāo)系與發(fā)射坐標(biāo)系中的轉(zhuǎn)換關(guān)系如圖1 所示。

圖1 地磁場在不同坐標(biāo)系下的投影Fig.1 Projection of geomagnetic field in different coordinate systems

式（5）中,A0為發(fā)射方位角, 定義為發(fā)射方向與真北方向的夾角。

2 雙軸地磁傳感器姿態(tài)測量方案

雙軸地磁傳感器安裝在過彈體質(zhì)心的橫截面內(nèi), 可敏感彈體法向和橫向的地磁分量與, 如圖2 所示。 在無磁干擾的環(huán)境下, 傳感器鉛直磁場中滾轉(zhuǎn)一周, 以Z軸輸出為橫軸, 以Y軸輸出為縱軸, 理論上輸出曲線應(yīng)該是以原點（0,0）為圓心的一個圓。

圖2 雙軸地磁傳感器安裝示意圖Fig.2 Installation diagram of two-axis magnetometer

由于地磁傳感器受鐵磁性物質(zhì)或者其他設(shè)備引起的磁場畸變、 緯度漂移、 強(qiáng)磁干擾等影響,其輸出曲線變?yōu)榻频臋E圓, 且圓心相對原點偏移到了（ZOFF,YOFF）。 同時, 因為軟磁干擾的不對稱, 使得輸出的橢圓繞中心旋轉(zhuǎn)了角度θ, 因此使用前需進(jìn)行標(biāo)定和補(bǔ)償。 對于雙軸地磁傳感器的標(biāo)定補(bǔ)償可采用常見的橢圓擬合法［10］, 由于地磁標(biāo)定方法非本文重點, 因此橢圓擬合法在此不再贅述。

3 雙軸地磁輔助慣性/衛(wèi)星組合導(dǎo)航算法

3.1 建立導(dǎo)航系統(tǒng)狀態(tài)量及狀態(tài)方程

本文選擇導(dǎo)彈發(fā)射坐標(biāo)系為導(dǎo)航系, 以慣性導(dǎo)航系統(tǒng)的3 個位置誤差、 3 個速度誤差、 3 個姿態(tài)誤差以及加速度計的零偏和陀螺儀的零漂作為狀態(tài)量, 表示如下

式（6）中,δx、δy、δz為發(fā)射系下的位置誤差,δvx、δvy、δvz為發(fā)射系下的速度誤差,φx、φy、φz為姿態(tài)失準(zhǔn)角,εx、εy、εz為陀螺儀常值漂移,為加速度計零位偏置。

根據(jù)捷聯(lián)慣導(dǎo)系統(tǒng)誤差模型, 可構(gòu)建系統(tǒng)狀態(tài)方程如下

F（t）滿足

式（8）中,f代表發(fā)射坐標(biāo)系,gr為地球引力加速度在地心矢徑方向的投影,r為彈體與地心連線的距離,為地球自轉(zhuǎn)角速度的反對稱矩陣,為比力的反對稱矩陣,為發(fā)射系相對于慣性系旋轉(zhuǎn)角速度的反對稱矩陣,為彈體姿態(tài)矩陣。

將式（7）進(jìn)行離散化處理, 可得

式（9） 中,Φk,k-1為狀態(tài)轉(zhuǎn)移矩陣, 具體計算如下

式（10）中,I15×15為一個15 ×15 的單位陣,Ts為慣導(dǎo)更新周期。

系統(tǒng)激勵噪聲為高斯型白色隨機(jī)向量序列,滿足如下條件

3.2 建立包含位置、 速度及雙軸地磁分量的量測方程

（1）建立雙軸地磁分量的量測方程

雙軸地磁分量的量測為計算得到的地磁矢量在本體系下Y、Z兩軸的分量與安裝于彈體橫截面Y、Z兩軸的地磁傳感器測量得到的地磁分量之差

由此, 可將雙軸地磁分量的量測方程寫為

式（16）中,V1（k） 表示測量噪聲, 為高斯白色隨機(jī)向量序列, 且滿足如下條件

量測矩陣H1（k）的計算如下

式（19）中,A2×3滿足

式（20）中,ci,j為矩陣Cb f的第i行第j列, 由慣導(dǎo)解算得出。

（2）建立包含位置、 速度的量測方程

由此, 可將包含位置、 速度的量測方程寫為

式（23）中,V2（k） 為衛(wèi)星導(dǎo)航測量噪聲, 建模為高斯白色隨機(jī)向量序列, 且滿足如下條件

量測矩陣H2（k）的計算如下

3.3 序貫Kalman 濾波算法步驟

本文采用序貫Kalman 濾波算法依次對多個測量數(shù)據(jù)進(jìn)行量測更新, 從而實現(xiàn)不同周期多源信息的融合, 具體步驟如下［11-12］:

1）初始化狀態(tài)量X（0）和協(xié)方差陣P（0）;

2）求取狀態(tài)轉(zhuǎn)移矩陣Φk,k-1, 并進(jìn)行狀態(tài)預(yù)測

3）求解協(xié)方差預(yù)測矩陣

4）依次對2 個測量數(shù)據(jù)進(jìn)行量測更新

式（29） ～式（31）中,i=1, 2, 且令。

5）融合中心最終的估計為

4 算法仿真驗證

4.1 仿真條件

采用某旋轉(zhuǎn)彈為仿真對象, 利用Matlab 仿真生成外彈道飛行數(shù)據(jù)以及各彈載姿態(tài)傳感器測量輸出的理想數(shù)據(jù)。 按表1 所示的彈載傳感器誤差參數(shù)進(jìn)行設(shè)置, 仿真生成彈載傳感器的實際測量輸出數(shù)據(jù), 在此基礎(chǔ)上進(jìn)行彈體飛行姿態(tài)估計算法的仿真驗證。

表1 傳感器誤差參數(shù)設(shè)置Table 1 Setting of sensor error parameters

導(dǎo)彈全程飛行時間約為65s, 彈體轉(zhuǎn)速如圖3所示。 導(dǎo)航解算初始俯仰角誤差為10′, 初始滾轉(zhuǎn)角誤差為10′, 初始偏航角誤差為18′。 捷聯(lián)慣導(dǎo)解算周期為2ms, 雙軸地磁傳感器量測輸出周期為10ms, 衛(wèi)星導(dǎo)航量測輸出周期為100ms。

圖3 彈體轉(zhuǎn)速Fig.3 Roll rate of the projectile

4.2 仿真結(jié)果及分析

根據(jù)上述仿真設(shè)置的條件, 采用前述序貫Kalman 濾波完成對旋轉(zhuǎn)彈飛行狀態(tài)的估計, 其位置、速度以及姿態(tài)估計誤差如圖4 ～圖6 所示。

圖4 位置誤差Fig.4 Curves of position error

圖5 速度誤差Fig.5 Curves of velocity error

圖6 姿態(tài)角誤差Fig.6 Curves of attitude angle error

由圖4、 圖5 可知, 序貫Kalman 濾波將衛(wèi)星導(dǎo)航測量噪聲濾除, 從而獲得較高精度的位置、速度估計。 濾波器穩(wěn)定后, 三軸位置誤差在2m 以內(nèi), 三軸速度誤差在0.5m/s 以內(nèi)。 經(jīng)統(tǒng)計, 得到位置誤差、 速度誤差分別為1.46m、 0.15m/s（1σ）。

由圖6 可知, 本文所述雙軸地磁輔助的慣性/衛(wèi)星組合導(dǎo)航算法與常規(guī)慣性/衛(wèi)星組合導(dǎo)航算法在偏航角以及俯仰角的姿態(tài)估計上精度相當(dāng)。 而對于滾轉(zhuǎn)角的估計, 本文所述算法由于引入了反映滾轉(zhuǎn)角信息的高頻率雙軸地磁傳感器輸出, 滾轉(zhuǎn)角估計精度更優(yōu), 且在很短的時間可得到很好的收斂。 而采用常規(guī)的慣性/衛(wèi)星組合導(dǎo)航算法,對于滾轉(zhuǎn)角的估計需要近20s 才能收斂。 在濾波穩(wěn)定后, 經(jīng)統(tǒng)計利用本文算法獲得的彈體滾轉(zhuǎn)角、偏航角和俯仰角姿態(tài)估計誤差分別為0.46°、0.47°和0.28°（1σ）。

5 結(jié)論

基于雙軸地磁傳感器、 MEMS 慣組和衛(wèi)星導(dǎo)航的測量信息, 本文提出了一種適用于旋轉(zhuǎn)彈的雙軸地磁傳感器輔助慣性/衛(wèi)星組合導(dǎo)航方法。 以捷聯(lián)慣導(dǎo)誤差方程為基礎(chǔ)建立了系統(tǒng)狀態(tài)模型, 以雙軸地磁傳感器測量值與衛(wèi)星導(dǎo)航接收機(jī)測量值分別建立了量測模型, 利用序貫Kalman 濾波實現(xiàn)了信息融合。 仿真結(jié)果表明, 該方法實現(xiàn)了較快的收斂速度和較高的精度, 但對于實際應(yīng)用還有待驗證。