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

（1. 北京航天長征飛行器研究所，北京，100076；2. 試驗物理與計算數(shù)學(xué)國家重點實驗室，北京，100076）

0 引 言

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

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

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

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

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

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

式中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)或者勻速運動的狀態(tài)下，單獨利用加速度計并不能得到完整的3 個姿態(tài)角，需要利用磁強(qiáng)計的信息，才能得出全部的歐拉角。

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

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

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

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

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

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

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

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

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

2.1.2 加速度誤差方程

忽略加速度計一次項和二次項誤差，加速度誤差方程為

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

2.1.3 速度位置誤差方程

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

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

2.1.4 狀態(tài)方程

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2.2 觀測模型

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

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

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

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

令：

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

因此，最終的量測方程為

式中 Η 為量測矩陣，且：

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

卡爾曼濾波是一種線性最小方差估計。對卡爾曼濾波器估計的利用主要有輸出校正和反饋校正，反饋校正性能優(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

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

3 仿真與驗證

3.1 仿真條件

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

續(xù)表2

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

圖2 位置誤差對比 Fig.2 Comparison of Position Error

圖3 速度誤差對比 Fig.3 Comparison of Velocity Error

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

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

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

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

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

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

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

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

4 結(jié) 論

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