一種基于慣導/地磁信息融合的 低成本組合導航方法

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

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

0 引 言

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

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

1 MEMS 三軸磁強計與加速度計組合測量姿態原理

在飛行過程中，飛行器姿態變化導致三軸微磁傳感器感應到的地球磁場矢量在其各軸上的投影分量發生變化；信號采集與處理模塊進行信號變送和調理，傳輸給微處理器中解算出姿態信息。

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

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

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

由歐拉角的旋轉關系可以得到如下轉換（3-2-1 的旋轉順序）：

式中 θ ，ψ ，γ 分別為俯仰角、偏航角和滾轉角。

從式（3）中可以得到俯仰角和滾轉角：

因此，在載體靜態或者勻速運動的狀態下，單獨利用加速度計并不能得到完整的3 個姿態角，需要利用磁強計的信息，才能得出全部的歐拉角。

假設已知當地地磁場強度為M ，磁傾角為η，這樣的信息可以事先得到，三軸磁強計的輸出為

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

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

2 姿態測量與數據融合方法

本文中以發射點慣性系l 系為導航坐標系，建立捷聯慣導（Strapdown Interial Navigation System，SINS）/地磁組合導航系統的狀態方程和量測方程。

2.1 組合導航系統狀態方程

2.1.1 數學平臺失準角誤差方程

忽略陀螺儀的一次項和二次項誤差，數學平臺失準角誤差方程為

式中φ為數學平臺失準角；Cbl為載體坐標系b 系到l系的變換矩陣；ε 為陀螺儀常值漂移；Wε為陀螺儀模型高斯白噪聲。

2.1.2 加速度誤差方程

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

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

2.1.3 速度位置誤差方程

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

式中 系數f14, f15, f16, f24, f25, f26, f34, f35, f36為引力加速度對位置坐標的偏導數。

2.1.4 狀態方程

綜合數學平臺失準角、加速度誤差、速度誤差和位置誤差方程，可得如下系統狀態方程：

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

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

系統噪聲驅動陣 G(t) 為

系統噪聲 W(t) 為

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

2.2 組合導航系統量測方程

2.2.1 姿態誤差角與平臺失準角的關系

在捷聯慣導系統中，載體姿態角是通過姿態矩陣（數學平臺）計算出來的。理想情況下，導航計算機計算的導航坐標系（ l? 系）應和理想的導航坐標系（l 系）一致，然而，由于系統存在測量誤差、計算誤差和干擾誤差等情況，計算的導航系與理想的導航系之間將產生偏差，對應的誤差角為平臺失準角，用角矢量表示。忽略二階小量，l 系與l? 系之間滿足如下關系：

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

定義姿態誤差角矢量η 為

由姿態誤差角的定義可知：

真實的姿態矩陣Clb可表示為

式中Cl?b為捷聯解算的姿態矩陣。將式（10）和式（12）代入式（18），忽略二階小量，可得l 系下姿態誤差角與平臺失準角關系：

式中M 為誤差角轉換矩陣：

2.2.2 觀測模型

三軸磁強計測量值mB 可表示為

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

磁強計在載體預估狀態下根據地磁場模型計算的地磁場強度值Bc可表示為

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

令：

式中Ηl為磁強計的測量誤差對l 系下載體位置的偏導數； xl為載體在l 系下的位置誤差。

因此，最終的量測方程為

式中 Η 為量測矩陣，且：

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

卡爾曼濾波是一種線性最小方差估計。對卡爾曼濾波器估計的利用主要有輸出校正和反饋校正，反饋校正性能優于輸出校正。圖1 為捷聯慣性/地磁組合導航系統反饋校正解算框圖。

圖1 捷聯慣性/地磁組合導航系統解算框圖 Fig. 1 The Strapdown Inertial and Geomagnetic Integrated Navigation System Feedback Correction Forward Solution Algorithm Diagram

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

3 仿真與驗證

3.1 仿真條件

a）飛行器發射點設定為：北緯40°，東經40°，發射方位角40°，初始偏航角0°，俯仰角90°，滾轉角0°。

b）飛行器主動段飛行時間設定為200 s，發動機推力加速度設定為40 m/s2，方向沿飛行器體軸，其中，0～10 s 為垂直上升段，10～30 s 為主動轉彎段，俯仰角從90°變為20°，30～200 s 為加速飛行段，俯仰角保持20°不變。

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

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

在慣性/地磁組合中，捷聯慣導的初始時刻姿態失準角分別為10″、6′和10″，磁強計工作開始時間為10 s。根據采樣周期、慣組器件和磁強計的參數變化，設計6 種情形的典型仿真實例，如表1 所示。

表1 仿真情形設計 Tab.1 Simulation Case Design

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

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

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

情形4：降低磁強計的測量精度，通過濾波獲得位置、速度和姿態誤差結果。

情形5：增加磁強計的采樣周期，通過濾波獲得位置、速度和姿態誤差結果。

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

3.2 仿真結果及分析

對情形1 至情形6 的位置、速度和3 個姿態角誤差仿真結果進行統計分析，結果列于表2。

表2 仿真結果統計表 Tab.2 Statistics of Simulation Results

續表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

通過分析仿真結果，可得：

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

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

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

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

4 結 論

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