基于抗差自適應(yīng)SITAN算法的水下重力匹配導(dǎo)航方法

付林威 趙東明 付 林

1 信息工程大學(xué)地理空間信息學(xué)院,鄭州市科學(xué)大道62號,450001

重力匹配慣性導(dǎo)航是實現(xiàn)水下運載體長時間高精度導(dǎo)航定位的重要手段之一,能有效減弱慣性導(dǎo)航系統(tǒng)(inertial navigation system,INS)隨時間推移產(chǎn)生的誤差累積[1]。水下重力匹配慣性導(dǎo)航精度依賴于INS精度、水下重力傳感器測量精度、重力異常基準(zhǔn)圖精度以及匹配算法等要素,其中,匹配算法是核心,主要包括基于相關(guān)分析技術(shù)的地形輪廓匹配(terrain contour matching,TERCOM)算法、迭代最近等值線點(iterated closest contour point,ICCP)算法和基于遞推濾波技術(shù)的桑迪亞慣性地形輔助導(dǎo)航(Sandia inertial terrain aided navigation,SITAN)算法[2]等。近年來,有學(xué)者提出矢量匹配算法[3]、粒子濾波算法[4]、具有約束條件的匹配算法[5]和三角形約束算法[6]等。SITAN算法綜合了卡爾曼濾波(Kalman filter,KF)技術(shù)和地形隨機(jī)線性化技術(shù),具有實時性好、可操作性強(qiáng)等特點,在量測值不含粗差且量測誤差服從正態(tài)分布時能夠求得最優(yōu)無偏估值,但在實際情況下可能出現(xiàn)異常觀測值和動力學(xué)模型誤差等問題,嚴(yán)重時甚至?xí)?dǎo)致濾波發(fā)散。為此,學(xué)者們提出多種針對動力學(xué)模型誤差[7-10]和觀測粗差[11-12]的算法,取得了較好的效果。

本文在SITAN算法的基礎(chǔ)上,提出一種基于抗差自適應(yīng)擴(kuò)展卡爾曼濾波(robust adaptive extended Kalman filter,RAEKF)的SITAN算法,針對IMU運動學(xué)模型誤差問題,計算自適應(yīng)因子調(diào)節(jié)狀態(tài)預(yù)測協(xié)方差矩陣;在此基礎(chǔ)上,針對自適應(yīng)因子的失效問題,通過求解抗差因子剔除粗差;最后利用重力異常模型數(shù)據(jù)開展仿真實驗。

1 SITAN算法數(shù)學(xué)模型

SITAN算法首先根據(jù)慣導(dǎo)指示位置,在重力異常模型中得到該處的重力異常值;然后利用搭載的水下重力傳感器測量得到運載體航行位置處的真實重力異常值,將二者的差值作為Kalman濾波的量測輸入;最后結(jié)合慣導(dǎo)誤差方程和重力異常隨機(jī)線性化技術(shù),構(gòu)建狀態(tài)方程和量測方程。

1.1 狀態(tài)方程

汪鳳林等[13]對重力匹配慣性導(dǎo)航系統(tǒng)的可觀測性進(jìn)行研究,結(jié)果表明,系統(tǒng)狀態(tài)量需要取平面位置誤差。基于此,本文取水下運載體的平面位置誤差X=(δφ,δλ)T作為濾波狀態(tài)量,δφ、δλ分別為緯度、經(jīng)度誤差。

INS位置誤差微分方程為:

(1)

式中,δvN、δvE分別為N、E向的速度誤差,RM、RN分別為子午圈、卯酉圈曲率半徑,δφ、δλ和δh分別為緯度、經(jīng)度和高度誤差。

若不考慮速度誤差和高度誤差的影響,式(1)可簡化為:

(2)

令

(3)

則式(2)可表示為:

(4)

對式(3)進(jìn)行離散化,得到SITAN算法的濾波狀態(tài)方程:

Xk=Φk/k-1Xk-1+Wk-1

(5)

式中,Φk/k-1為一步轉(zhuǎn)移矩陣,Φk/k-1=I+F(tk-1)Ts,Ts為離散化時間間隔,Xk、Xk-1分別為k、(k-1)時刻的狀態(tài)向量,Wk-1為(k-1)時刻的狀態(tài)噪聲。

1.2 量測方程

采用SITAN算法進(jìn)行匹配時,首先要建立重力異常與狀態(tài)位置的線性關(guān)系,即重力異常隨機(jī)線性化。在量測更新時刻,對水下運載體航行位置(φ,λ)處的真實重力異常在慣導(dǎo)指示位置(φi,λi)處進(jìn)行一階泰勒級數(shù)展開:

Δg(φ,λ)=ΔgM(φi,λi)+

(6)

式中,Δg(φ,λ)為水下運載體航行位置處的真實重力異常,ΔgM(φi,λi)為根據(jù)慣導(dǎo)指示位置在重力異常基準(zhǔn)圖上讀取的重力異常,?ΔgM/?φi為鄰域內(nèi)重力異常緯向梯度,?ΔgM/?λi為鄰域內(nèi)重力異常徑向梯度,vi為泰勒級數(shù)展開過程中的截斷誤差和重力異常基準(zhǔn)圖誤差。

水下運載體真實位置處的重力異常可表示為實測重力異常與量測噪聲之和:

Δg(φ,λ)=Δgs(φt,λt)+vs

(7)

式中,Δgs(φt,λt)為水下運載體實際觀測重力異常,vs為水下重力傳感器量測噪聲。

由式(6)和式(7)可得:

ΔgM(φi,λi)-Δgs(φt,λt)=

(8)

令

(9)

整理式(9)可得SITAN算法的濾波量測方程為:

Zk=HkXk+Vk

(10)

2 基于RAEKF的SITAN算法

SITAN算法的濾波遞推方程如下:

(11)

精確的濾波模型和誤差統(tǒng)計特性是卡爾曼濾波最優(yōu)估計的基礎(chǔ)。但在實際的水下組合導(dǎo)航應(yīng)用中,濾波數(shù)學(xué)模型與真實模型存在偏差,且觀測值會受到外界環(huán)境的影響,使得異常觀測值的存在不可避免,若直接進(jìn)行解算將會導(dǎo)致導(dǎo)航解次優(yōu)甚至濾波發(fā)散[14]。為此,本文采用基于RAEKF的SITAN算法,其原理是利用自適應(yīng)因子和抗差因子調(diào)節(jié)預(yù)測信息和觀測信息對濾波結(jié)果貢獻(xiàn)的權(quán)重[15]:

(12)

(13)

(14)

(15)

抗差因子選擇IGGⅢ函數(shù)模型:

(16)

(17)

預(yù)測殘差只有在觀測值不包含粗差的情況下才能反映狀態(tài)預(yù)測信息的異常,即在量測信息可靠的情況下自適應(yīng)因子才有效。因此,在計算自適應(yīng)因子之前,應(yīng)判別觀測值是否包含粗差,若包含粗差,則直接選擇抗差濾波進(jìn)行解算;若不包含粗差,則選擇自適應(yīng)濾波進(jìn)行解算。當(dāng)預(yù)測殘差統(tǒng)計量大于閾值時,采用自適應(yīng)因子抑制IMU運動學(xué)推算誤差。

基于RAEKF的SITAN算法流程見圖1。

圖1 算法流程Fig.1 Flow chart of algorithm

3 實驗驗證與分析

3.1 仿真實驗設(shè)計

為驗證匹配算法的有效性,選取南海一塊重力異常變化豐富的海域開展仿真實驗。重力異常基準(zhǔn)圖采用DTU17重力異常模型,分辨率為1′×1′,中誤差為2～3 mGal。初始狀態(tài)量為X0=(0,0)T,P0=10-5I,Qk=0.03I,Rk=0.25。通過在重力異常基準(zhǔn)圖數(shù)據(jù)上添加中誤差為3 mGal的高斯白噪聲,可以得到水下運載體的重力異常觀測數(shù)據(jù)。圖2為實驗海域的重力異常三維模型圖。

圖2 實驗海域重力異常三維模型Fig.2 3D model of gravity anomaly in the test area

利用航跡仿真算法生成參考航跡,時長為24 h,初始位置為(7°N,113°E,-100 m),初始速度為(0 m/s, 0 m/s,0 m/s),初始姿態(tài)為(0°,0°,45°)。本次實驗水下運載體真實航跡運動過程如下:首先以0.008 6 m/s2的加速度加速600 s,速度達(dá)到10 kn后勻速行駛28 110 s;然后左轉(zhuǎn)45°,勻速行駛28 800 s;最后右轉(zhuǎn)45°,勻速行駛28 890 s。設(shè)置慣導(dǎo)采樣頻率為100 Hz,基于生成的真實航跡,利用捷聯(lián)慣導(dǎo)反演算法生成航跡上各采樣點處的慣性器件輸出,即角增量和速度增量。將生成的慣性器件輸出表1所示的誤差參數(shù),模擬真實環(huán)境下的慣導(dǎo)元器件輸出,利用雙子樣算法進(jìn)行慣導(dǎo)解算,生成慣導(dǎo)航跡。

表1 慣性器件誤差參數(shù)Tab.1 Inertial device error parameters

結(jié)合載體航行速度和重力異常基準(zhǔn)圖分辨率,設(shè)置量測更新間隔為180 s,分別利用常規(guī)SITAN算法(EKF算法)和抗差自適應(yīng)SITAN算法(RAEKF算法)展開重力匹配仿真實驗。為對比2種算法的抗差效果,在第160～240次量測更新之間,每隔5次在原始觀測值上加入30 mGal的粗差,對應(yīng)濾波時段為8 ～12 h。

3.2 常規(guī)SITAN算法仿真

真實航跡、慣導(dǎo)航跡以及EKF算法匹配航跡見圖3。利用EKF算法處理帶有粗差觀測值的匹配航跡(EKF-Outlier)、慣導(dǎo)航跡(INS)與EKF算法匹配航跡的位置誤差見圖4。由圖3和圖4可知,在觀測值未加入粗差的情況下,EKF算法能夠有效修正慣導(dǎo)整體航跡,其匹配結(jié)果定位精度優(yōu)于INS精度;在觀測值加入粗差的情況下,濾波時長小于8 h,匹配航跡能收斂于真實航跡,觀測值加入粗差后,濾波在緯度和經(jīng)度方向上逐漸發(fā)散,導(dǎo)航定位精度與INS精度相當(dāng),濾波器失效。

圖3 常規(guī)SITAN算法匹配航跡Fig.3 The matching tracks of ordinary SITAN algorithm

圖4 航跡位置誤差Fig.4 Position error of tracks

對EKF算法匹配結(jié)果的位置誤差進(jìn)行統(tǒng)計,并與INS的位置誤差進(jìn)行對比,結(jié)果見表2(單位n mile)。由表2可知,觀測值存在粗差的情況下,EKF算法導(dǎo)航定位誤差最大值為9.93 n mile,RMS為5.01 n mile;INS導(dǎo)航定位誤差最大值為10.24 n mile,RMS為6.16 n mile,濾波結(jié)果失真。由此可知,EKF算法無法抵抗異常觀測值的影響。因此,需要構(gòu)建自適應(yīng)因子和抗差因子實現(xiàn)狀態(tài)預(yù)測信息的自適應(yīng)控制以及粗差剔除。

表2 誤差統(tǒng)計Tab.2 Error statistics

3.3 基于RAEKF的SITAN算法仿真

RAEKF算法匹配航跡以及航跡位置誤差見圖5和圖6,其中,RAEKF-Outlier為利用RAEKF算法處理帶有粗差觀測值的匹配航跡。由圖5和圖6可知,在觀測值未加入粗差的情況下,RAEKF算法能顯著降低慣導(dǎo)累積誤差的影響,修正慣導(dǎo)整體軌跡,且RAEKF算法的匹配精度遠(yuǎn)優(yōu)于INS。相較于EKF算法,RAEKF算法的導(dǎo)航定位精度也有提升。在觀測值加入粗差的情況下,RAEKF算法的效果更加顯著。由此可見,在自適應(yīng)因子和抗差因子的作用下,系統(tǒng)的穩(wěn)健性和精度均有提高。

圖5 基于RAEKF的SITAN算法匹配航跡Fig.5 The matching tracks of SITAN algorithm based on RAEKF

圖6 航跡位置誤差Fig.6 Position error of tracks

RAEKF算法匹配結(jié)果位置誤差統(tǒng)計見表3(單位n mile)。由表2和表3可知,在觀測值未加入粗差的情況下,RAEKF算法相較于EKF算法的導(dǎo)航定位精度提升了1.6%;在觀測值加入粗差的情況下,RAEKF算法相較于EKF算法在緯向和經(jīng)向上的定位精度分別提高了49.5%和81.6%,整體導(dǎo)航定位精度提高了73.9%,濾波器運行平穩(wěn)。由此可見,RAEKF算法能有效抑制觀測粗差的影響。

表3 誤差統(tǒng)計Tab.3 Error statistics

4 結(jié) 語

針對水下重力匹配導(dǎo)航中存在觀測粗差和動力學(xué)模型誤差的問題,提出一種抗差自適應(yīng)SITAN算法,并利用重力異常模型數(shù)據(jù)開展水下重力匹配仿真實驗。結(jié)果表明,在觀測值存在粗差的情況下,抗差自適應(yīng)SITAN算法的定位精度可達(dá)到1.31 n mile,較常規(guī)SITAN算法的精度提升73.9%。