王偉東，李 革，劉佳琪，劉 鑫，馬 凱

（1. 北京航天長(zhǎng)征飛行器研究所，北京，100076；2. 試驗(yàn)物理與計(jì)算數(shù)學(xué)國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京，100076）

0 引 言

在飛行器導(dǎo)航中，最早應(yīng)用的導(dǎo)航方法是慣性導(dǎo)航，基本原理是利用慣性器件測(cè)量載體運(yùn)動(dòng)參數(shù)，通過(guò)數(shù)據(jù)處理獲取飛行器當(dāng)前的位置、姿態(tài)等信息。而慣導(dǎo)系統(tǒng)由于慣性器件本身帶有的測(cè)量漂移誤差，其誤差會(huì)隨時(shí)間累積，不適合長(zhǎng)時(shí)間導(dǎo)航任務(wù)。為實(shí)現(xiàn)空間在軌飛行器長(zhǎng)時(shí)間精確導(dǎo)航，通常采用高精度傳感器，如星敏感器和光纖陀螺儀等[1]，但其體積大、功耗高、成本高，難以在低成本飛行器上得到廣泛應(yīng)用。地球磁場(chǎng)穩(wěn)定性高、變化較慢、精度適中[2]，慣性/地磁組合導(dǎo)航已應(yīng)用于船舶、潛艇、巡航導(dǎo)彈等領(lǐng)域?qū)Ш絒3～8]。但由于地磁導(dǎo)航系統(tǒng)（Geomagnetic Navigation System，GNS）的精度主要取決于慣性器件、地磁匹配圖、地磁測(cè)量設(shè)備等的精度和計(jì)算機(jī)解算速度因素，目前尚未達(dá)到軌道飛行器高速動(dòng)態(tài)導(dǎo)航要求[9]。

本文針對(duì)短時(shí)間在軌小型飛行器，提出了一種基于低成本慣導(dǎo)/地磁敏感器件的信息融合組合導(dǎo)航方法，通過(guò)利用微機(jī)電系統(tǒng)（Micro-Electro-Mechanical System，MEMS）慣性組合和磁強(qiáng)計(jì)輸出參數(shù)共同構(gòu)建狀態(tài)方程和量測(cè)方程，并采用基于反饋校正的卡爾曼濾波算法，分析了高、中、低精度的慣導(dǎo)組合和MEMS 三軸磁強(qiáng)計(jì)不同測(cè)量精度條件下的導(dǎo)航精度偏差，結(jié)果表明采用慣導(dǎo)/地磁信息融合導(dǎo)航，可降低對(duì)慣性器件的精度要求，從而降低了軌道飛行器導(dǎo)航的經(jīng)濟(jì)、質(zhì)量成本和對(duì)安裝空間的需求。同時(shí)繼承了慣性組合僅依靠自身傳感器，不與外界發(fā)生信息交換的優(yōu)點(diǎn)，具有廣泛的應(yīng)用前景。

1 MEMS 三軸磁強(qiáng)計(jì)與加速度計(jì)組合測(cè)量姿態(tài)原理

在飛行過(guò)程中，飛行器姿態(tài)變化導(dǎo)致三軸微磁傳感器感應(yīng)到的地球磁場(chǎng)矢量在其各軸上的投影分量發(fā)生變化；信號(hào)采集與處理模塊進(jìn)行信號(hào)變送和調(diào)理，傳輸給微處理器中解算出姿態(tài)信息。

定義載體坐標(biāo)系，縱軸為X 軸，向右為Y 軸，Z 軸為符合右手定則的方向。定義地磁坐標(biāo)系，磁北為X 軸，重力方向?yàn)閆 軸，Y 軸為符合右手定則的方向。將三軸磁強(qiáng)計(jì)和三軸加速度計(jì)沿載體坐標(biāo)系安裝，重合時(shí)，如果載體處于靜止或者勻速運(yùn)動(dòng)狀態(tài)，則加速度計(jì)的測(cè)量輸出ta 為

式中 g 為重力加速度。如果載體處于任意姿態(tài)，則 加速度計(jì)的測(cè)量輸出為

式中xa ，ya ，za 為ta 在X 軸、Y 軸、Z 軸的分量。

由歐拉角的旋轉(zhuǎn)關(guān)系可以得到如下轉(zhuǎn)換（3-2-1 的旋轉(zhuǎn)順序）：

式中 θ ，ψ ，γ 分別為俯仰角、偏航角和滾轉(zhuǎn)角。

從式（3）中可以得到俯仰角和滾轉(zhuǎn)角：

因此，在載體靜態(tài)或者勻速運(yùn)動(dòng)的狀態(tài)下，單獨(dú)利用加速度計(jì)并不能得到完整的3 個(gè)姿態(tài)角，需要利用磁強(qiáng)計(jì)的信息，才能得出全部的歐拉角。

假設(shè)已知當(dāng)?shù)氐卮艌?chǎng)強(qiáng)度為M ，磁傾角為η，這樣的信息可以事先得到，三軸磁強(qiáng)計(jì)的輸出為

由式（5）得到的航向角為

微磁傳感器通過(guò)敏感地磁場(chǎng)，利用已知的地球磁場(chǎng)空間分布可實(shí)現(xiàn)載體的姿態(tài)測(cè)量。此方案在無(wú)磁干擾環(huán)境中有很好的應(yīng)用效果，難點(diǎn)在于存在強(qiáng)磁背景環(huán)境或變化軟磁環(huán)境下，由磁信息解算姿態(tài)存在較大誤差。對(duì)于空間應(yīng)用來(lái)說(shuō)，其磁背景環(huán)境干凈，干擾較小。

2 姿態(tài)測(cè)量與數(shù)據(jù)融合方法

本文中以發(fā)射點(diǎn)慣性系l 系為導(dǎo)航坐標(biāo)系，建立捷聯(lián)慣導(dǎo)（Strapdown Interial Navigation System，SINS）/地磁組合導(dǎo)航系統(tǒng)的狀態(tài)方程和量測(cè)方程。

2.1 組合導(dǎo)航系統(tǒng)狀態(tài)方程

2.1.1 數(shù)學(xué)平臺(tái)失準(zhǔn)角誤差方程

忽略陀螺儀的一次項(xiàng)和二次項(xiàng)誤差，數(shù)學(xué)平臺(tái)失準(zhǔn)角誤差方程為

式中φ為數(shù)學(xué)平臺(tái)失準(zhǔn)角；Cbl為載體坐標(biāo)系b 系到l系的變換矩陣；ε 為陀螺儀常值漂移；Wε為陀螺儀模型高斯白噪聲。

2.1.2 加速度誤差方程

忽略加速度計(jì)一次項(xiàng)和二次項(xiàng)誤差，加速度誤差方程為

式中δa為加速度誤差；[ a ×]為加速度計(jì)敏感的比力組成的反對(duì)稱矩陣；?為載體坐標(biāo)系b 系下加速度計(jì)的等效偏置。

2.1.3 速度位置誤差方程

在發(fā)射點(diǎn)慣性坐標(biāo)系中，簡(jiǎn)化引力場(chǎng)模型為球形有心力場(chǎng)，目標(biāo)速度位置誤差方程為

式中 系數(shù)f14, f15, f16, f24, f25, f26, f34, f35, f36為引力加速度對(duì)位置坐標(biāo)的偏導(dǎo)數(shù)。

2.1.4 狀態(tài)方程

綜合數(shù)學(xué)平臺(tái)失準(zhǔn)角、加速度誤差、速度誤差和位置誤差方程，可得如下系統(tǒng)狀態(tài)方程：

式中X(t) 為狀態(tài)向量，各分量分別為3 個(gè)數(shù)學(xué)平臺(tái)失準(zhǔn)角、速度誤差、位置誤差、陀螺常值漂移和加速度計(jì)常值偏置；F(t) 為狀態(tài)轉(zhuǎn)移矩陣。

式中Fa為由式（12）元素組成的矩陣：

系統(tǒng)噪聲驅(qū)動(dòng)陣 G(t) 為

系統(tǒng)噪聲 W(t) 為

式中Wεx,Wεy,Wεz分別為陀螺誤差；W?x,W?y,W?z分別為加速度計(jì)誤差。兩者均為零均值的高斯白噪聲誤差。

2.2 組合導(dǎo)航系統(tǒng)量測(cè)方程

2.2.1 姿態(tài)誤差角與平臺(tái)失準(zhǔn)角的關(guān)系

在捷聯(lián)慣導(dǎo)系統(tǒng)中，載體姿態(tài)角是通過(guò)姿態(tài)矩陣（數(shù)學(xué)平臺(tái)）計(jì)算出來(lái)的。理想情況下，導(dǎo)航計(jì)算機(jī)計(jì)算的導(dǎo)航坐標(biāo)系（ l? 系）應(yīng)和理想的導(dǎo)航坐標(biāo)系（l 系）一致，然而，由于系統(tǒng)存在測(cè)量誤差、計(jì)算誤差和干擾誤差等情況，計(jì)算的導(dǎo)航系與理想的導(dǎo)航系之間將產(chǎn)生偏差，對(duì)應(yīng)的誤差角為平臺(tái)失準(zhǔn)角，用角矢量表示。忽略二階小量，l 系與l? 系之間滿足如下關(guān)系：

式中[φ × ]為平臺(tái)失準(zhǔn)角 φ的元素組成的反對(duì)稱矩陣。

定義姿態(tài)誤差角矢量η 為

由姿態(tài)誤差角的定義可知：

真實(shí)的姿態(tài)矩陣Clb可表示為

式中Cl?b為捷聯(lián)解算的姿態(tài)矩陣。將式（10）和式（12）代入式（18），忽略二階小量，可得l 系下姿態(tài)誤差角與平臺(tái)失準(zhǔn)角關(guān)系：

式中M 為誤差角轉(zhuǎn)換矩陣：

2.2.2 觀測(cè)模型

三軸磁強(qiáng)計(jì)測(cè)量值mB 可表示為

式中Be， Bb分別為真實(shí)的地磁場(chǎng)矢量在地球坐標(biāo)系e 系和本體系b 系中的表示，Bb=ClbCilC eiBe；Cil為地心慣性系i 系到l 系的轉(zhuǎn)移矩陣；Cei為e 系到i 系的轉(zhuǎn)移矩陣； εm為量測(cè)噪聲。

磁強(qiáng)計(jì)在載體預(yù)估狀態(tài)下根據(jù)地磁場(chǎng)模型計(jì)算的地磁場(chǎng)強(qiáng)度值Bc可表示為

定義上述磁強(qiáng)計(jì)測(cè)量值Bm與根據(jù)地磁場(chǎng)模型計(jì)算的地磁場(chǎng)強(qiáng)度值Bc之差為量測(cè) ? B：

令：

式中Ηl為磁強(qiáng)計(jì)的測(cè)量誤差對(duì)l 系下載體位置的偏導(dǎo)數(shù)； xl為載體在l 系下的位置誤差。

因此，最終的量測(cè)方程為

式中 Η 為量測(cè)矩陣，且：

2.3 基于反饋校正的卡爾曼濾波

卡爾曼濾波是一種線性最小方差估計(jì)。對(duì)卡爾曼濾波器估計(jì)的利用主要有輸出校正和反饋校正，反饋校正性能優(yōu)于輸出校正。圖1 為捷聯(lián)慣性/地磁組合導(dǎo)航系統(tǒng)反饋校正解算框圖。

圖1 捷聯(lián)慣性/地磁組合導(dǎo)航系統(tǒng)解算框圖 Fig. 1 The Strapdown Inertial and Geomagnetic Integrated Navigation System Feedback Correction Forward Solution Algorithm Diagram

使用卡爾曼濾波器時(shí)，若采用誤差作為狀態(tài)信息，引入狀態(tài)反饋的目標(biāo)是使輸出信息盡可能趨于零。因此，濾波器估計(jì)值是校正后的導(dǎo)航參數(shù)差，即狀態(tài)向量中包含的不再是陀螺常值漂移和加速度計(jì)常值偏置，而是陀螺常值漂移估計(jì)誤差和加速度計(jì)常值漂移估計(jì)誤差。反饋校正的濾波方程與標(biāo)準(zhǔn)卡爾曼濾波方程有所不同，詳見(jiàn)文獻(xiàn)[10]。

3 仿真與驗(yàn)證

3.1 仿真條件

a）飛行器發(fā)射點(diǎn)設(shè)定為：北緯40°，東經(jīng)40°，發(fā)射方位角40°，初始偏航角0°，俯仰角90°，滾轉(zhuǎn)角0°。

b）飛行器主動(dòng)段飛行時(shí)間設(shè)定為200 s，發(fā)動(dòng)機(jī)推力加速度設(shè)定為40 m/s2，方向沿飛行器體軸，其中，0～10 s 為垂直上升段，10～30 s 為主動(dòng)轉(zhuǎn)彎段，俯仰角從90°變?yōu)?0°，30～200 s 為加速飛行段，俯仰角保持20°不變。

c）主動(dòng)段仿真僅為提供本文的仿真初始參數(shù)，因此暫忽略其他因素影響。

d）仿真時(shí)間200～1728 s，為飛行器在軌自由飛行段，僅考慮飛行器重力影響。

在慣性/地磁組合中，捷聯(lián)慣導(dǎo)的初始時(shí)刻姿態(tài)失準(zhǔn)角分別為10″、6′和10″，磁強(qiáng)計(jì)工作開(kāi)始時(shí)間為10 s。根據(jù)采樣周期、慣組器件和磁強(qiáng)計(jì)的參數(shù)變化，設(shè)計(jì)6 種情形的典型仿真實(shí)例，如表1 所示。

表1 仿真情形設(shè)計(jì) Tab.1 Simulation Case Design

情形1：作為基準(zhǔn)參照組，加速度計(jì)、陀螺儀、磁強(qiáng)計(jì)精度最高，采樣周期時(shí)間最短。

情形2：降低加速度計(jì)的精度，通過(guò)濾波獲得位置、速度和姿態(tài)誤差結(jié)果。

情形3：降低陀螺儀的精度，通過(guò)濾波獲得位置、速度和姿態(tài)誤差結(jié)果。

情形4：降低磁強(qiáng)計(jì)的測(cè)量精度，通過(guò)濾波獲得位置、速度和姿態(tài)誤差結(jié)果。

情形5：增加磁強(qiáng)計(jì)的采樣周期，通過(guò)濾波獲得位置、速度和姿態(tài)誤差結(jié)果。

情形6：同時(shí)降低加速度計(jì)、陀螺儀、磁強(qiáng)計(jì)精度，并增加磁強(qiáng)計(jì)的采樣周期，通過(guò)濾波獲得位置、速度和姿態(tài)誤差結(jié)果。

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

對(duì)情形1 至情形6 的位置、速度和3 個(gè)姿態(tài)角誤差仿真結(jié)果進(jìn)行統(tǒng)計(jì)分析，結(jié)果列于表2。

表2 仿真結(jié)果統(tǒng)計(jì)表 Tab.2 Statistics of Simulation Results

續(xù)表2

典型誤差曲線如圖2～6 所示。

圖2 位置誤差對(duì)比 Fig.2 Comparison of Position Error

圖3 速度誤差對(duì)比 Fig.3 Comparison of Velocity Error

圖4 橫滾角誤差對(duì)比 Fig.4 Comparison of Roll Angle Error

圖5 航向角誤差對(duì)比 Fig.5 Comparison of Yaw Angle Error

圖6 俯仰角誤差對(duì)比 Fig.6 Comparison of Pitch Angle Error

通過(guò)分析仿真結(jié)果，可得：

a）當(dāng)僅僅依靠捷聯(lián)慣導(dǎo)時(shí)，因加速度計(jì)常值偏置與陀螺儀常值漂移的影響導(dǎo)致位置姿態(tài)誤差的發(fā)散。即便采用高精度加速度計(jì)和零漂穩(wěn)定性達(dá)0.01（°）/h的陀螺儀，經(jīng)過(guò)0.5 h 的飛行，因漂移而造成的位置、速度及姿態(tài)角的誤差量級(jí)很大，如果沒(méi)有其它導(dǎo)航方式的輔助融合，單獨(dú)的捷聯(lián)慣導(dǎo)應(yīng)用于空間目標(biāo)導(dǎo)航不可行。

b）情形2、3 與情形1 對(duì)照分析表明，當(dāng)磁強(qiáng)計(jì)精度和采樣率均較高時(shí)，慣性組合的精度降低，對(duì)捷聯(lián)慣導(dǎo)/地磁組合導(dǎo)航精度影響不顯著。

c）情形4、5 與情形1 對(duì)照分析表明，磁場(chǎng)強(qiáng)度的測(cè)量精度和采樣率，均對(duì)捷聯(lián)慣導(dǎo)/地磁組合導(dǎo)航系統(tǒng)精度具有影響，其中磁場(chǎng)強(qiáng)度的測(cè)量精度對(duì)其影響更顯著。

d）情形6 與情形1 對(duì)照分析表明，當(dāng)磁強(qiáng)計(jì)精度和采樣率均較低時(shí)，慣性組合的精度降低，對(duì)捷聯(lián)慣導(dǎo)/地磁組合導(dǎo)航精度影響顯著。

4 結(jié) 論

捷聯(lián)慣組與地磁進(jìn)行組合導(dǎo)航，通過(guò)提高地磁測(cè)量的精度和采樣率，在保持同等導(dǎo)航精度條件下，可顯著降低對(duì)捷聯(lián)慣組的精度要求；利用高精度MEMS磁強(qiáng)計(jì)可滿足導(dǎo)航精度需求，降低軌道飛行器導(dǎo)航的經(jīng)濟(jì)、質(zhì)量成本，減少安裝空間。