一種彈道導彈捷聯慣導/地磁組合導航方法

李 鎮，王海涌，靳宇航，秦天沐，李晶津，高自謙

（北京航空航天大學 宇航學院，北京 100191）

一種彈道導彈捷聯慣導/地磁組合導航方法

李 鎮，王海涌，靳宇航，秦天沐，李晶津，高自謙

（北京航空航天大學 宇航學院，北京 100191）

基于慣性器件和磁強計的測量信息，提出一種彈道導彈捷聯慣導/地磁組合導航方法。以捷聯慣導誤差方程為基礎建立系統的狀態模型，以磁強計測量值與根據地磁場模型計算的地磁場強度值之差作為量測，只用一個觀測表達式即同時包含載體的位置及姿態信息。引入狀態反饋，利用混合校正的卡爾曼濾波得到系統導航信息的最優估計。仿真結果表明，該算法能有效抑制捷聯解算誤差的發散，磁強計精度為100 nT時，定位精度2.68 km，姿態精度優于5′。該導航方法完全自主，精度較高，具有一定工程應用價值。

組合導航；捷聯慣性導航；地磁場；磁強計；卡爾曼濾波

慣性/星光/GPS三者或兩兩之間的組合導航是目前組合導航的主要研究范疇[1-3]，但仍存在一定的局限性：雖然星敏感器是目前姿態測量精度最高的姿態確定傳感器，但星敏感器體積、重量和功耗較大，價格昂貴，某些型號還難以應用；GPS導航在實戰中可能受到干擾，嚴格地說并不是真正的自主導航。近年來地磁導航研究方向越來越引起業內關注，它具有自主性、無積累誤差、成本低、結構簡單等優點。因此，某些型號的自主導航，轉向慣性和地磁的組合尋求技術解決方案[4-5]。

慣性/地磁組合導航主要有地磁匹配輔助慣性導航和地磁濾波輔助慣性導航兩種方式：前者是將地磁匹配定位算法得到的載體位置估計作為量測輸入，再利用濾波技術估計慣導系統的誤差，該算法須預先存儲大量地磁數據，并且累積一段路徑才能完成導航，實時性較差；后者直接以磁強計測量值作為量測輸入，需要的存儲空間較小，同時利用遞推濾波技術，實時性較好，衛星和導彈通常采用后者[6]，因此本文的研究主要針對地磁濾波輔助慣性導航。地磁濾波輔助慣性導航的基本方法是將慣導解算位置在地磁基準圖上讀取的地磁場強度與磁強計測量值之差作為量測，推導并建立觀測模型[7-8]，然而該量測僅與載體的位置誤差直接相關，姿態信息只能通過濾波技術間接獲得。文獻[9]利用加速度計敏感重力場，結合磁強計測量值得到載體的三軸姿態，但該算法局限于載體處于靜止或不存在線加速度的情況，并且沒有進行位置解算。

基于慣性器件和磁強計的測量信息，本文提出一種彈道導彈捷聯慣性/地磁組合導航方法。該方法采用地磁濾波輔助慣性導航的組合方式，利用地磁場信息，以三軸磁強計測量值與地磁場矢量計算值之差作為量測，只用一個觀測表達式即同時包含載體的位置及姿態信息，通過混合校正的卡爾曼濾波即可得到系統導航信息的最優估計。

1 地磁場矢量模型

地磁場是一個矢量場，它是位置的函數。如果有精確的地磁場分布模型，那么根據地磁場矢量的測量值，經過一定的濾波技術，就能獲得載體的位置信息。國際地磁學與高空大氣物理學協會每隔5年公布一次IGRF(International Geomagnetic Reference Field)，它具有較為完善的數學模型[10]。地磁場勢函數V表示為

由圖1可知，地磁場強度B在東北天地理系f中的矢量Bf與其在地球坐標系e中的矢量Be有如下關系：

圖1 地磁場球坐標示意圖Fig.1 Magnetic spherical coordinates

2 捷聯慣性/地磁組合系統模型

本文以彈道導彈常用的發射點慣性系li系為導航坐標系，建立捷聯慣性/地磁組合導航系統的狀態方程和量測方程。

2.1 組合導航系統狀態方程

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

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

② 加速度誤差方程

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

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

③ 速度位置誤差方程

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

④ 狀態方程

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

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

2.2 量測方程

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

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

式中，[φ ×]為平臺失準角φ的元素組成的反對稱矩陣。定義姿態誤差角矢量η為

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

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

2.2.2 觀測模型

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

根據微分運算法則：

由式(24)可知，磁強計測量誤差主要由兩部分組成。右邊第一項是由于姿態誤差產生的測量誤差，第二項是由于位置誤差產生的測量誤差。由于量測Z同時包含了姿態和位置信息，因此可以同時進行定姿和定位。式(24)右邊第一項可以簡寫為

值得注意的是，由于組合系統狀態變量中代表姿態信息的是平臺失準角φ，而式(35)中的量測是用姿態誤差角η表示的，2.2.1節已對他們之間的關系進行了推導，這里只須將式(16)代入，得：

因此，最終的量測方程為

量測矩陣H：

2.3 信息融合

卡爾曼濾波是一種線性最小方差估計。對卡爾曼濾波器估計的利用主要有輸出校正，反饋校正和混合校正三種方式，混合校正是前兩種校正方式的結合，在不增加計算量的同時，性能優于單獨的輸出校正和反饋校正。圖2為組合導航系統混合校正原理圖。

圖2 組合導航系統原理圖Fig.2 Principle figure of integrated navigation system

3 仿真分析

3.1 仿真條件

① 飛行器發射點北緯39.98°，東經116.34°，垂直向東發射；落點北緯31.59°，東經161.40°；主動段飛行時間160 s，全程飛行時間1100 s，射高761 km，射程4125 km。

② 初始航向角0°，俯仰角90°，滾轉角0°；俯仰角誤差3′，偏航角誤差6′，橫滾角誤差3′；

③ 采樣周期：陀螺及加速度計為0.01 s，磁強計為0.1 s，卡爾曼濾波周期0.1 s；

④ 慣性元件參數：陀螺常值漂移為0.003 (°)/s，陀螺測量白噪聲標準差為0.0003 (°)/s；加速度計常值偏置為1 mg，加速度計白噪聲標準差為0.1 mg；

⑤ 磁強計測量誤差100 nT。

3.2 仿真結果及分析

圖3為彈道導彈在發射點慣性系下的軌跡。捷聯解算及組合導航相應的位置誤差、速度誤差、姿態角誤差見圖4～圖6。經過1100 s的飛行，純捷聯解算的位置、速度和姿態誤差均發散，位置誤差49.18 km，速度誤差54.48 m/s，俯仰角誤差3.59°，偏航角誤差3.44°，橫滾角誤差 3.31°。利用磁強計信息進行組合導航后，上述誤差的發散均得到有效抑制，位置誤差2.68 km，速度誤差5.23 m/s，俯仰角誤差4.74′，偏航角誤差3.90′，橫滾角誤差3.06′。

算法對陀螺常值漂移的估計見圖 7。由于觀測值同時包含有姿態和位置信息，因此理論上算法也能對加速度計的常值偏置做出估計。當加速度計常值偏置為0.01g時，仿真結果如圖8所示。

圖3 彈道導彈軌跡Fig.3 Trajectory of ballistic missile

圖4 位置誤差Fig.4 Position errors

圖5 速度誤差Fig.5 Velocity errors

圖6 姿態角誤差Fig.6 Attitude errors

圖7 陀螺常值漂移Fig.7 Gyro constant drifts

由圖8可知，算法對加速度計在X和Y軸方向的常值偏置估計較好，Z軸方向估計較差，這是由于Z軸方向為與發射面垂直的方向，所受激勵較小。需要說明的是，當加速度計精度較高時，導彈的位置及速度誤差將主要來源于陀螺漂移引起的姿態解算誤差，而算法對陀螺常值漂移的估計將很好的補償掉這部分誤差。

實際發射任務前，應結合異磁場對地磁場模型進行時變修正。為了驗證組合導航系統在磁干擾環境中的工作性能，分別選取磁干擾強度為100 nT，300 nT，500 nT，1000 nT，飛行全程的導航參數均方根誤差統計見表1。由表1可知，隨著磁干擾強度的增加，組合系統的位置、速度和姿態角精度有所降低。當磁干擾強度為1000 nT時，組合系統的定位精度8.78 km，姿態角精度優于40′，與捷聯解算的結果相比，仍存在明顯的優勢。

圖8 加速度計常值偏置Fig.8 Accelerometer constant biases

表1 不同磁干擾強度對組合系統影響Tab.1 Integrated system influence of different magnetic interferences

4 結 論

基于慣性器件和磁強計的測量信息，提出一種彈道導彈捷聯慣性/地磁組合導航方法。以捷聯慣導誤差方程為基礎建立系統的狀態模型，以磁強計測量值與根據地磁場模型計算的地磁場強度值之差作為量測，只用一個觀測表達式即同時包含載體的位置及姿態信息。引入狀態反饋，通過混合校正的卡爾曼濾波進行信息融合，得到系統導航信息的最優估計。仿真結果表明，該算法能有效抑制捷聯解算誤差的發散，磁強計精度為100 nT時，經過1100 s的飛行，系統的定位精度2.68 km，姿態精度優于5′。需要特別指出的是，當結合異磁場對主磁場模型進行修正時，文中量測模型的推導依然成立。在磁干擾強度為1000 nT時，系統的定位精度8.78 km，姿態精度優于40′。該導航方法完全自主，精度較高，具有一定工程應用價值。

（References）：

[1] 王濤, 王雪梅. 一種新的SINS/GPS組合導航建模與濾波[J]. 彈箭與制導學報, 2012, 32(2): 25-28. Wang Tao, Wang Xue-mei. A new modeling method and filter algorithm of SINS/GPS integrated navigation[J]. Journal of Projectiles, Rockets, Missiles and Guidance, 2012, 32(2): 25-28.

[2] 王宏力, 陳聰, 崔祥祥, 等. 適用于彈道導彈的導航星選取方法[J]. 北京航空航天大學學報, 2013, 39(2): 143-147. Wang Hong-li, Chen Cong, Cui Xiang-xiang, et al. Guide star selection method applied to ballistic missile[J]. Journal of Beijing University of Aeronautics and Astronautics, 2013, 39(2): 143-147.

[3] Qian Hua-ming, Sun Long, Cai Jia-nan, et al. A novel navigation method used in a ballistic missile[J]. Measurement Science and Technology, 2013, 24(10): 105011.

[4] 朱占龍, 單友東, 楊翼, 等. 基于信息正交性自適應濾波的慣性/地磁組合導航方法[J]. 中國慣性技術學報, 2015, 23(1): 66-70. Zhu Zhan-long, Shan You-dong, Yang Yi, et al. INS/GNS integrated method based on innovation orthogonality adaptive kalman filter[J]. Journal of Chinese Inertial Technology, 2015, 23(1): 66-70.

[5] Guo Cai-fa, Cai Hong, Hu Zheng-dong. Nonlinear filtering techniques for geomagnetic navigation[C]//Proceedings of the Institution of Mechanical Engineers, Part G: Journal of Aerospace Engineering, 2014, 223(2): 305-320.

[6] 趙國榮, 黃婧麗, 胡正高. 地磁輔助慣性組合導航技術分析[J]. 系統仿真技術, 2013, 9(2): 119-123. Zhao Guo-rong, Huang Jing-li, Hu Zheng-gao. Geomagnetic aided inertial navigation technical analysis[J]. System Simulation Technology, 2013, 9(2): 119-123.

[7] Ma Xiao-jiao, Liu Hong-wei, Xiao Di, et al. Key technologies of geomagnetic aided inertial navigation system[C] //2009 IEEE Intelligent Vehicles Symposium. Xi'an, China, 2009: 464-469.

[8] 劉睿, 董漢成, 王常紅. 基于模糊自適應強跟蹤濾波的慣性/地磁組合導航方法[J]. 中國慣性技術學報, 2011, 19(3): 329-334. Liu Rui, Dong Han-cheng, Wang Chang-hong. Inertial/ Geomagnetic navigation system based on fuzzy adaptive strong tracking kalman filter[J]. Journal of Chinese Inertial Technology, 2011, 19(3): 329-334.

[9] Zhu Rong, Sun Dong, Zhou Zhao-ying, et al. A linear fusion algorithm for attitude determination using low cost MEMS-based sensors[J]. Measurement, 2007, 40(3): 322-328.

[10] Finlay C C, Maus S, Beggan C D, et al. International geomagnetic reference field: the eleventh generation[J]. International Journal of Geophysical, 2010, 183(3): 1216-1230.

[11] 王仕成, 劉元元, 孫淵, 等. 基于 Legendre 函數的超高階次地磁場建模方法[J]. 中國慣性技術學報, 2012, 20(3): 333-338. Wang Shi-cheng, Liu Yuan-yuan, Sun Yuan, et al. Method for ultra high-order magnetic model based on Legendre function[J]. Journal of Chinese Inertial Technology, 2012, 20(3): 333-338.

Strapdown inertial/geomagnetic integrated navigation method for ballistic missile

LI Zhen, WANG Hai-yong, JIN Yu-hang, QIN Tian-mu, LI Jing-jin, GAO Zi-qian
(School of Astronautics, Beijing University of Aeronautics and Astronautics, Beijing 100191, China)

Based on the measurement information of inertial instrument and magnetometer, a strapdown inertial navigation/geomagnetic integrated navigation method for ballistic missile was proposed. The state model was established according to the error equations of strapdown inertial navigation system (SINS), and the observation model was constructed by using the difference between magnetometer output and the geomagnetic field intensity calculated by the geomagnetic reference field model, which included carrier position and attitude parameters. With the introduction of state feedback, the optimal estimations of navigation parameters were obtained by the mixedly corrected Kalman filter. Simulation results show that the proposed method can effectively suppress the divergence of the errors. The positioning accuracy is 3.214 km and the attitude accuracy is better than 5 arcmin when the magnetometer’s accuracy is 100 nT. This method has such advantages as fully autonomous navigation and high accuracy that it is suitable for engineering application.

integrated navigation; SINS; geomagnetic field; magnetometer; Kalman filter

V249.3

：A

2015-06-16；

：2015-09-21

航空科學基金（20130151004）

李鎮（1988—），男，碩士研究生，從事組合導航研究。E-mail：lizhen@buaa.edu.cn

聯 系 人：王海涌（1969—），男，博士，講師。E-mail：why@buaa.edu.cn

1005-6734(2015)05-0636-06

10.13695/j.cnki.12-1222/o3.2015.05.014