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

付林威 趙東明 付 林

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

重力匹配慣性導(dǎo)航是實(shí)現(xiàn)水下運(yù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)算法、迭代最近等值線點(diǎn)(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ù),具有實(shí)時性好、可操作性強(qiáng)等特點(diǎn),在量測值不含粗差且量測誤差服從正態(tài)分布時能夠求得最優(yōu)無偏估值,但在實(shí)際情況下可能出現(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運(yùn)動學(xué)模型誤差問題,計算自適應(yīng)因子調(diào)節(jié)狀態(tài)預(yù)測協(xié)方差矩陣;在此基礎(chǔ)上,針對自適應(yīng)因子的失效問題,通過求解抗差因子剔除粗差;最后利用重力異常模型數(shù)據(jù)開展仿真實(shí)驗。

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

SITAN算法首先根據(jù)慣導(dǎo)指示位置,在重力異常模型中得到該處的重力異常值;然后利用搭載的水下重力傳感器測量得到運(yùn)載體航行位置處的真實(shí)重力異常值,將二者的差值作為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)量需要取平面位置誤差?；诖?本文取水下運(yùn)載體的平面位置誤差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ī)線性化。在量測更新時刻,對水下運(yùn)載體航行位置(φ,λ)處的真實(shí)重力異常在慣導(dǎo)指示位置(φi,λi)處進(jìn)行一階泰勒級數(shù)展開:

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

(6)

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

水下運(yùn)載體真實(shí)位置處的重力異?？杀硎緸閷?shí)測重力異常與量測噪聲之和:

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

(7)

式中,Δgs(φt,λt)為水下運(yùn)載體實(shí)際觀測重力異常,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ǔ)。但在實(shí)際的水下組合導(dǎo)航應(yīng)用中,濾波數(shù)學(xué)模型與真實(shí)模型存在偏差,且觀測值會受到外界環(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運(yùn)動學(xué)推算誤差。

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

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

3 實(shí)驗驗證與分析

3.1 仿真實(shí)驗設(shè)計

為驗證匹配算法的有效性,選取南海一塊重力異常變化豐富的海域開展仿真實(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的高斯白噪聲,可以得到水下運(yùn)載體的重力異常觀測數(shù)據(jù)。圖2為實(shí)驗海域的重力異常三維模型圖。

圖2 實(shí)驗海域重力異常三維模型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°)。本次實(shí)驗水下運(yùn)載體真實(shí)航跡運(yùn)動過程如下:首先以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,基于生成的真實(shí)航跡,利用捷聯(lián)慣導(dǎo)反演算法生成航跡上各采樣點(diǎn)處的慣性器件輸出,即角增量和速度增量。將生成的慣性器件輸出表1所示的誤差參數(shù),模擬真實(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算法)展開重力匹配仿真實(shí)驗。為對比2種算法的抗差效果,在第160～240次量測更新之間,每隔5次在原始觀測值上加入30 mGal的粗差,對應(yīng)濾波時段為8 ～12 h。

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

真實(shí)航跡、慣導(dǎo)航跡以及EKF算法匹配航跡見圖3。利用EKF算法處理帶有粗差觀測值的匹配航跡(EKF-Outlier)、慣導(dǎo)航跡(INS)與EKF算法匹配航跡的位置誤差見圖4。由圖3和圖4可知,在觀測值未加入粗差的情況下,EKF算法能夠有效修正慣導(dǎo)整體航跡,其匹配結(jié)果定位精度優(yōu)于INS精度;在觀測值加入粗差的情況下,濾波時長小于8 h,匹配航跡能收斂于真實(shí)航跡,觀測值加入粗差后,濾波在緯度和經(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)因子和抗差因子實(shí)現(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%,濾波器運(yùn)行平穩(wěn)。由此可見,RAEKF算法能有效抑制觀測粗差的影響。

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

4 結(jié) 語

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