周 瀟，楊功流，王 晶，史 俊，劉元元，李 晶

（1. 北京航空航天大學 儀器科學與光電工程學院，北京 100191；2. 慣性技術國防重點實驗室，北京 100191）

基于Kalman濾波原理對慣導中重力擾動的估計及補償方法

周 瀟，楊功流，王 晶，史 俊，劉元元，李 晶

（1. 北京航空航天大學 儀器科學與光電工程學院，北京 100191；2. 慣性技術國防重點實驗室，北京 100191）

重力擾動(空間同一點實際重力與正常重力之差，包括垂線偏差和重力異常兩部分) 一直是慣性導航系統(tǒng)的重要誤差源之一 。隨著慣性器件精度的逐步提高，重力擾動所引起的導航誤差已成為高精度長航時慣導系統(tǒng)的主要誤差之一，不能被忽略，必須對其進行補償。針對重力擾動誤差精確補償問題，只考慮重力異常的情況，推導并建立了考慮重力異常的慣導誤差方程。對誤差傳播進行了分析，利用Kalman濾波原理設計了帶有重力異常狀態(tài)量的狀態(tài)方程，并利用GPS和慣導的速度誤差作為觀測量對重力異常進行濾波并對濾波后得到的重力異常值對慣導重力異常項補償。進行了2 h的仿真，結果表明：Kalman濾波后可得到當?shù)氐闹亓Ξ惓Ｖ?，重力異常補償后，速度誤差精度可以提高約0.3 m/s，姿態(tài)精度提高約0.3′，位置精度提高約150 m。

捷聯(lián)慣導系統(tǒng)；重力擾動補償；慣導誤差方程；Kalman濾波

慣性導航系統(tǒng)[1]不需要任何外來信息，也不會向外輻射任何信息，僅靠慣性導航系統(tǒng)本身就能在全天侯條件下，在全球范圍內(nèi)和任何介質環(huán)境里自主地、隱蔽地進行連續(xù)的三維定位和三維定向，具有諸多其他導航系統(tǒng)無法比擬的優(yōu)點。在慣性導航系統(tǒng)中[2-3]，加速度計組件測量的是比力，比力是載體運動加速度和重力加速度的矢量和。運動加速度是進行導航解算需要知道的加速度，但是，根據(jù) Einstein等價性原理，加速度計是無法區(qū)分運動加速度和重力加速度的，需要通過其他手段將比力中的重力加速度扣除，此稱為重力補償。在傳統(tǒng)的慣導解算過程中，通常采用通過地球重力場模型得到的正常重力矢量對慣導系統(tǒng)進行重力補償。正常重力矢量和實際的重力矢量很多情況下是不相等的，二者之差為重力擾動矢量。重力擾動[4]包括正切分量垂直偏差和正交分量重力異常。對于慣性測量傳感器而言，一方面它不能辨別重力擾動的垂直偏差和載體實際的水平加速度，使慣性導航系統(tǒng)產(chǎn)生水平速度和位置誤差；另一方面，利用包含重力異常的重力數(shù)據(jù)直接進行慣性導航解算，也將直接引起位置和速度誤差。隨著慣性器件精度的逐步提高以及高精度捷聯(lián)慣性導航系統(tǒng)應用需求的提出，重力擾動已經(jīng)成為高精度捷聯(lián)慣性導航系統(tǒng)最主要的剩余誤差源之一。

本文在只考慮重力異常而不考慮垂線偏差的情況下，對高精度慣性元件的慣性導航系統(tǒng)進行誤差分析，推導了帶有重力異常作為狀態(tài)量的Kalman濾波方程[5-6]并進行補償仿真，分析重力異常引起的系統(tǒng)誤差和重力異常補償后對系統(tǒng)精度的改進效果。

1 考慮重力異常的慣導誤差方程

1.1 慣導系統(tǒng)原理圖及坐標系定義

慣性導航系統(tǒng)原理圖[7]如圖1所示。在慣性導航系統(tǒng)中，陀螺儀和加速度計通過穩(wěn)定平臺安裝在載體上，分別測量載體相對慣性空間的角運動信息和線運動信息，在給定相關初值和其他必要信息（如重力信息）的情況下，經(jīng)過一系列運算和控制可以得到載體相對于導航坐標系的位置、速度和姿態(tài)等信息。圖1中的虛線框表示重力異常對慣性導航系統(tǒng)的補償，在精度要求不高的傳統(tǒng)系統(tǒng)中，這一過程可以忽略。然而，隨著慣性器件精度的不斷提高和對高精度慣導的需求，討論和研究慣導的重力擾動補償越來越具有實際意義。

圖1 慣性導航系統(tǒng)原理圖Fig.1 Principle of inertial navigation system

坐標系定義：b系—載體坐標系，n系—導航坐標系（在本文中取為東-北-天當?shù)厮街副弊鴺讼担?，e系—地固坐標系，i系—慣性坐標系。

1.2 考慮重力擾動的Kalman濾波方程的推導及建模

捷聯(lián)慣性導航誤差方程[8-9]：姿態(tài)誤差方程：

速度誤差方程：

相對位置誤差方程：

所以有：

假設陀螺測量誤差[10]中包含陀螺標度因數(shù)誤差δGF、安裝誤差δGI和陀螺漂移 εb，則有：

同理，假設加速度計測量誤差中包含加速度計標度因數(shù)誤差δAF，安裝誤差δAI，重力異常 δgn，因為加速度計為高精度加速度計故忽略加速度計零偏▽b，則有：

如果記陀螺誤差矩陣δKG為 δKG=δGF+δGI，加速度計誤差矩陣δKA為 δKA=δAF+δAI，則有：

順便指出，陀螺誤差矩陣δKG和加速度計誤差矩陣δKA可具體展開如下式所示：

至此，姿態(tài)誤差方程式(1)經(jīng)整理改寫成：

速度誤差方程式(2)經(jīng)整理改寫成：

位置誤差方程式(3)經(jīng)整理改寫成：

假設捷聯(lián)慣組經(jīng)過嚴格標定以后陀螺誤差矩陣δKG和加速度計誤差矩陣δKA均很小，可以忽略它們的影響，建立如下定義15維狀態(tài)向量[11-12]，其中 φT為姿態(tài)角， δvnT為導航坐標系下速度誤差， δp為位置誤差， εbT載體坐標系下陀螺漂移， δgnT導航坐標系下重力異常。

則捷聯(lián)慣導系統(tǒng)誤差方程可表示為

本文中選擇GPS和慣導的速度誤差作為觀測量，有：

2 仿真實驗與分析

首先設計飛機飛行軌跡，然后利用飛機運動軌跡發(fā)生器模擬飛機運動并產(chǎn)生IMU數(shù)據(jù)和GPS數(shù)據(jù)，在所產(chǎn)生的 IMU數(shù)據(jù)中疊加慣性器件常值漂移。最后，利用本文中所提出的 Kalman濾波方法對軌跡發(fā)生器所得出數(shù)據(jù)進行濾波并將濾波所得重力異常補償仿真。

2.1 仿真條件

本文在慣導軌跡生成器中引入重力異常，包括重力異常東向分量Δgx、北向分量Δgy、垂直天向分量Δ gz。利用軌跡生成器生成的慣導數(shù)據(jù)以及GPS數(shù)據(jù)對系統(tǒng)進行仿真計算。

仿真條件如下：重力擾動東向分量Δgx=10 mGal，北向分量Δgy=20 mGal，垂直天向分量Δgz=30 mGal。三個陀螺的常值漂移均為5×10-5(°)/h，三個加速度計的零偏均為5×10-7g，忽略慣性組件的標度因數(shù)誤差以及安裝誤差角。方位初始誤差角30′，初始緯度北緯34°，初始經(jīng)度東經(jīng)108°，飛機巡航速度50 m/s，巡航時間4 h，先向北飛2 h，轉90°再向東飛2 h。仿真步長20 ms。

2.2 仿真結果與分析

使用上述仿真方案的仿真結果如圖2至圖8所示。

圖2 重力異常估計曲線Fig.2 Estimation curve of gravity anomaly

由圖2中(a)(b)(c)三圖分別可以看出經(jīng)Kalman濾波后估計出的重力異常東向、北向、天向三個方位的分量值分別為10 mGal、20 mGal、30 mGal。濾波仿真結果與設定值相符，從而驗證了Kalman濾波模型的正確性。

對比圖3和圖4中的姿態(tài)誤差曲線可以看出在經(jīng)過重力異常補償后的慣導系統(tǒng)姿態(tài)精度方面有明顯提高。經(jīng)過2 h仿真計算，俯仰角精度能夠提高約0.3′，橫滾角精度能夠提高約0.25′，航向角精度能夠提高約0.15′。

對比圖5和圖6中的速度誤差曲線可以看出在經(jīng)過重力異常補償后的慣導系統(tǒng)速度精度方面有明顯提高。經(jīng)過2 h的仿真計算，慣導東向速度誤差能夠提高約0.25 m/s，北向速度誤差能夠提高約0.06 m/s。

圖3 重力異常補償前姿態(tài)誤差曲線Fig.3 Error curves of attitude without gravity anomaly compensation

圖4 重力異常補償后姿態(tài)誤差曲線Fig.4 Error curves of attitude with gravity anomaly compensation

圖5 重力異常補償前速度誤差曲線Fig.5 Error curves of velocity without gravity anomaly compensation

圖6 重力異常補償后速度誤差曲線Fig.6 Error curves of velocity with gravity anomaly compensation

圖7 重力異常補償前位置誤差曲線Fig.7 Error curves of position without gravity anomaly compensation

圖8 重力異常補償后位置誤差曲線Fig.8 Error curves of position with gravity anomaly compensation

對比圖7和圖8中的位置誤差曲線可以看出在經(jīng)過重力異常補償后的慣導系統(tǒng)位置精度方面有明顯提高。經(jīng)過 2 h的仿真計算，慣導緯度誤差能夠減小約150 m，經(jīng)度誤差能夠減小約50 m。

3 結 論

本文基于 Kalman濾波原理對重力擾動關于慣導的誤差方程進行推導并建模，用仿真數(shù)據(jù)對重力異常進行濾波估計并補償。由上述仿真分析可以形成以下結論：

① 本文所推導設計出的考慮重力異常的 Kalman濾波模型是正確可靠的，經(jīng)仿真驗證可得到實驗預期的重力異常值。

② 重力異常補償后，速度誤差精度可以提高約0.3 m/s，姿態(tài)精度提高約0.3′，位置精度提高約150 m。

（References）：

[1] 吳太旗, 歐陽永忠, 陸秀平, 等. 重力匹配導航的影響模式分析[J]. 中國慣性技術學報, 2011, 19(5): 559-564. Wu Tai-qi, Ouyang Yong-zhong, Lu Xiu-ping, et al. Analysis on effecting mode of several essential factors to gravity aided navigation[J]. Journal of Chinese Inertial Technology, 2011, 19(5): 559-564.

[2] 盧鑫, 練軍想, 吳美平. 高精度艦載慣性導航系統(tǒng)的重力影響研究[J]. 導航與控制, 2010, 9(4): 15-21. Lu Xin, Lian Jun-xiang, Wu Mei-ping. Research on the influence of gravity error compensation on marine inertial navigation system[J]. Navigation and Control, 2010, 9(4): 15-21.

[3] DeGregoria A. Gravity gradiometry and map matching an aid to aircraft inertial navigation systems[D]. Ohio: Air Force Institute of Technology, 2010.

[4] Rummel R, Yi W, Stummer C. GOCE gravitational gradiometry[J]. Journal of Geodesy, 2011, 85: 777-790.

[5] Senobari M. New results in airborne vector gravimetry using strapdown INS/DGPS[J]. Journal of Geodesy, 2010, 84: 277-291.

[6] Li X. Strapdown INS/DGPS airborne gravimetry tests in the Gulf of Mexico[J]. Journal of Geodesy, 2011, 85: 597-605.

[7] 常國賓, 許江寧, 李安, 等. 載體運動對雙軸連續(xù)旋轉調(diào)制式慣導方案誤差的影響[J]. 中國慣性技術學報, 2011, 19(2): 175-179. Chang Guo-bin, Xu Jiang-ning, Li An, et al. Influence of body dynamics on the error of the scheme of INS with dual-axis continuous autocompensation technique [J]. Journal of Chinese Inertial Technology, 2011, 19(2): 175-179.

[8] Senobari M. New results in airborne vector gravimetry using strapdown INS/DGPS[J]. Journal of Geodesy, 2010, 84: 277-291.

[9] Wang Hu-biao, Wang Yong, Fang Jian. Simulation research on a minimum root-mean-square error rotation-fitting algorithm for gravity matching navigation[J]. Science China: Earth Sciences, 2012, 42(7): 1055-1062.王虎彪, 王勇, 方劍. “最小均方差旋轉擬合法”重力輔助導航仿真研究[J]. 中國科學?地球科學, 2012, 42(7): 1055-1062.

[10] Li Shan-shan, Wu Xiao-ping, Ma Biao. Correlative extremum matching algorithm using underwater gravity anomalies[J]. Acta Geodaetica et Cartograpghica Sinica, 2011, 40(4): 464-470.李珊珊, 吳曉平, 馬彪. 水下重力異常相關極值匹配算法[J]. 測繪學報, 2011, 40(4): 464-470.

[11] Yuang, Zhang H W, Yuan K F. A Combinational underwater aided navigation algorithm based on TERCOMM/ ICCP and Kalman filter[J]. IEEE Journal of Sciences and Optimization, 2011, 23(1): 952-955.

[12] Man X, Fang J C H, Ning X L. An overview of autonomous navigation for a gravity-assist interplanetary spacecraft[J]. Progress in Aerospace Sciences, 2013, 63: 56-66.

Estimation and compensation for gravity disturbance based on Kalman filtering in inertial navigation

ZHOU Xiao, YANG Gong-liu, WANG Jing, SHI Jun, LIU Yuan-yuan, LI Jing
(1. School of Instrumentation Science and Opto-electronics Engineering, Beihang University, Beijing 100191, China; 2. Science and Technology on Inertial Laboratory, Beijing 100191, China)

The gravity disturbance in inertial navigation system is the difference between the actual gravity and normal gravity on the same point, which includes two parts: deviation of plumb line and gravity anomaly. With the increase of inertial components’ precision, the gravity disturbance becomes one of the major influence factors for the precision of long-time and high-precision inertial navigation system. To compensate this gravity disturbance, we derived and established the inertial navigation error equations by taking into account the gravity anomaly. The propagation of the errors was analyzed, and the state equation considering gravity anomaly is designed based on Kalman filtering. The gravity anomaly of inertial navigation system is compensated by using the velocity difference between GPS and inertial navigation system as view measurement. The simulation is carried out for 2 h, which shows that the gravity anomaly can be correctly calculated by Kalman filtering, and after the compensation, the accuracy of velocity, attitude and position can be improved by about 0.3 m/s and 0.3′ and 150 m, respectively.

SINS; gravity disturbance compensation; inertial navigation error equation; Kalman filter

U666.1

：A

2015-09-05；

：2015-11-29

國家自然科學基金（61340044，11202010）

周瀟（1987—），男，博士研究生，從事慣性技術研究。E-mail: Jackzhou002@hotmail.com

聯(lián) 系 人：楊功流（1967—），男，教授，博士生導師。E-mail: bhu17-yang@139.com

1005-6734(2015)06-0721-06

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