雙軸地磁輔助的慣性/衛(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)爭(zhēng)環(huán)境的惡化,攻防對(duì)抗更激烈, 對(duì)導(dǎo)彈高機(jī)動(dòng)性能的要求不斷加強(qiáng), 具有更高控制效率的控制方案越來越多地應(yīng)用在旋轉(zhuǎn)彈上, 控制系統(tǒng)對(duì)彈體的姿態(tài)精度也提出了更高的要求。

慣性/衛(wèi)星組合導(dǎo)航一般適用于傾斜穩(wěn)定的導(dǎo)彈, 能夠較為準(zhǔn)確地提供彈體的姿態(tài)、 速度和位置信息。 而對(duì)于旋轉(zhuǎn)彈, 由于其繞自身縱軸旋轉(zhuǎn),直接利用陀螺儀測(cè)量滾轉(zhuǎn)角速率將引入很大的誤差（尤其對(duì)于低成本的MEMS 陀螺儀）［2］, 因此常利用地磁傳感器測(cè)量旋轉(zhuǎn)彈的滾轉(zhuǎn)姿態(tài)。 文獻(xiàn)［3］利用旋轉(zhuǎn)彈體本身動(dòng)力學(xué)模型與安裝在彈體橫截面的雙軸地磁傳感器輸出分別構(gòu)建狀態(tài)方程和量測(cè)方程, 通過擴(kuò)展Kalman 濾波（EKF）實(shí)現(xiàn)了精度約10°的彈體滾轉(zhuǎn)角估計(jì); 文獻(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°時(shí), 滾轉(zhuǎn)角誤差高達(dá)20°。 上述方案表明,僅靠地磁傳感器測(cè)量滾轉(zhuǎn)角存在較大的測(cè)量誤差,因此也有學(xué)者研究地磁傳感器與慣組的組合測(cè)量濾波算法［6-8］, 但由于地磁傳感器僅提供姿態(tài)信息,慣性導(dǎo)航計(jì)算所得的速度、 位置無法得到校正。

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

1 地磁場(chǎng)模型

在靠近地面空間內(nèi)的無源區(qū), 主磁場(chǎng)可以表示為標(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 提供的地磁球諧項(xiàng)系數(shù)可計(jì)算北-天-東地理系下的地磁分量, 如下所示

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

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

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

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

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

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

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

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 個(gè)位置誤差、 3 個(gè)速度誤差、 3 個(gè)姿態(tài)誤差以及加速度計(jì)的零偏和陀螺儀的零漂作為狀態(tài)量, 表示如下

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

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

F（t）滿足

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

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

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

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

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

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

（1）建立雙軸地磁分量的量測(cè)方程

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

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

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

量測(cè)矩陣H1（k）的計(jì)算如下

式（19）中,A2×3滿足

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

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

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

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

量測(cè)矩陣H2（k）的計(jì)算如下

3.3 序貫Kalman 濾波算法步驟

本文采用序貫Kalman 濾波算法依次對(duì)多個(gè)測(cè)量數(shù)據(jù)進(jìn)行量測(cè)更新, 從而實(shí)現(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ù)測(cè)

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

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

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

5）融合中心最終的估計(jì)為

4 算法仿真驗(yàn)證

4.1 仿真條件

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

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

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

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

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

根據(jù)上述仿真設(shè)置的條件, 采用前述序貫Kalman 濾波完成對(duì)旋轉(zhuǎn)彈飛行狀態(tài)的估計(jì), 其位置、速度以及姿態(tài)估計(jì)誤差如圖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)航測(cè)量噪聲濾除, 從而獲得較高精度的位置、速度估計(jì)。 濾波器穩(wěn)定后, 三軸位置誤差在2m 以內(nèi), 三軸速度誤差在0.5m/s 以內(nèi)。 經(jīng)統(tǒng)計(jì), 得到位置誤差、 速度誤差分別為1.46m、 0.15m/s（1σ）。

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

5 結(jié)論

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