基于CEP的重力自適應(yīng)并行EKF匹配算法

黃炎, 李姍姍, 范雕, 譚勖立, 馮進(jìn)凱, 呂明昊

1 軍事科學(xué)院國防科技創(chuàng)新研究院, 北京 100071 2 中國人民解放軍戰(zhàn)略支援部隊信息工程大學(xué)地理空間信息學(xué)院, 鄭州 450001 3 96941部隊, 北京 100080

0 引言

慣性/重力/重力梯度組合導(dǎo)航系統(tǒng)以其無源、自主、隱蔽等優(yōu)勢,成為水下潛航器進(jìn)行無源自主定位的主要導(dǎo)航系統(tǒng)和研究重點(diǎn)(趙偉,2014a,b).目前重力匹配導(dǎo)航算法通常分為兩大類:一類是基于相關(guān)分析原理的序列匹配算法(龍濤等,2021);另一類是基于遞推估計最優(yōu)濾波原理的單點(diǎn)迭代匹配算法(李釗偉等,2019).國內(nèi)外學(xué)者針對重力匹配算法展開了諸多研究,也取得了大量成果.歐陽明達(dá)和馬越原(2020)提出一種改進(jìn)A*算法用于重力匹配導(dǎo)航路徑規(guī)劃,實(shí)現(xiàn)了對規(guī)劃路徑的平滑優(yōu)化,減少了不必要的航向調(diào)整;鄒嘉盛等(2019)提出了一種基于約束條件的重力匹配導(dǎo)航算法,該算法利用慣導(dǎo)系統(tǒng)提供的軌跡方位與航距信息對待匹配干擾軌跡進(jìn)行剔除,有效提高了匹配效率和匹配精度;李釗偉等(2019)提出了一種基于先驗(yàn)遞推迭代最小二乘誤匹配校正方法,用來提高水下重力匹配導(dǎo)航可靠性;Dai等(2019)提出了一種基于仿射變換的人工蜂群算法,有效克服了重力異常匹配導(dǎo)航中初始誤差較大的問題;Liu和Chang(2015)提出一種基于邊緣魯棒無跡卡爾曼濾波的重力匹配輔助慣性導(dǎo)航技術(shù),該方法在減少sigma點(diǎn)使用數(shù)量的前提下提高了濾波的魯棒性.在諸多匹配算法中,序列相關(guān)匹配算法可靠性較好,但由于其一般以多個等值點(diǎn)的連續(xù)慣導(dǎo)輸出點(diǎn)為一組進(jìn)行搜索匹配,運(yùn)算量巨大,匹配速度較慢,影響導(dǎo)航的實(shí)時性.因此目前國外主要還是采用基于擴(kuò)展卡爾曼濾波(Extended Kalman Filter,EKF)的單點(diǎn)迭代匹配算法(李姍姍等,2011;王躍鋼等,2014;Li et al.,2020;Wei et al.,2017),其優(yōu)點(diǎn)是能滿足系統(tǒng)修正的實(shí)時性,且在低信噪比的條件下單點(diǎn)迭代匹配算法的精度較高.

由慣導(dǎo)誤差方程可知,平面位置誤差微分方程的狀態(tài)量應(yīng)有4個,分別為緯度和經(jīng)度誤差(δφ、δλ)以及北向和東向速度誤差(δvN、δvE).但是通過量測方程的可觀測分析,可知上述誤差中僅有平面位置誤差可觀(李勝全等,2012;汪鳳林等,2008;Han et al.,2018),但是僅使用平面位置誤差作為狀態(tài)量構(gòu)建Kalman濾波狀態(tài)方程時會存在一定的模型誤差,由此引起的函數(shù)模型不可靠會導(dǎo)致濾波結(jié)果不準(zhǔn)確甚至發(fā)散.并且,EKF匹配算法的量測值為水下潛航器搭載的重力儀測量得到的實(shí)測重力值與根據(jù)慣性導(dǎo)航系統(tǒng)指示位置在重力基準(zhǔn)圖上獲得的圖上重力值之差,而由于濾波函數(shù)模型不準(zhǔn)確以及慣性導(dǎo)航系統(tǒng)本身定位精度隨時間發(fā)散等原因,慣導(dǎo)指示位置的可信度隨之減小,導(dǎo)致EKF匹配算法的量測值偏差變大,從而進(jìn)一步造成濾波不準(zhǔn)確.

針對上述問題,首先利用自適應(yīng)因子對狀態(tài)方程預(yù)測信息的協(xié)方差矩陣進(jìn)行調(diào)整,削弱由狀態(tài)模型誤差造成的預(yù)測信息偏差對濾波結(jié)果的影響;同時使用圓概率誤差評定方法對慣性導(dǎo)航系統(tǒng)的定位誤差進(jìn)行分析,確定不同圓概率誤差半徑范圍慣性導(dǎo)航系統(tǒng)定位誤差的置信概率,然后基于此構(gòu)建CEP移動窗口模型,再利用濾波量測值與窗口坡度等信息對移動窗口范圍進(jìn)行約束,最后使用并行Kalman濾波器獲得最優(yōu)濾波結(jié)果,從而削弱狀態(tài)模型誤差與慣導(dǎo)指示位置誤差引起的濾波發(fā)散問題.

1 自適應(yīng)EKF匹配導(dǎo)航算法

由于水下潛器航行的隱蔽性需求,重力測量不需要對外界發(fā)出信號,具有較好的無源性,且地球重力場相對穩(wěn)定,不會產(chǎn)生較大的波動,可通過船載重力測量、衛(wèi)星測高、衛(wèi)星重力等方法進(jìn)行測量或計算得到(Shen et al., 2022; He et al., 2023; Liu et al. 2023),因此重力輔助慣性導(dǎo)航成為水下潛航器無源自主導(dǎo)航的重要研究內(nèi)容.重力EKF匹配算法的濾波量測值為重力異常和擾動重力梯度,首先根據(jù)慣導(dǎo)指示位置,通過存儲在計算機(jī)中的重力異常基準(zhǔn)圖、擾動重力梯度基準(zhǔn)圖,比對獲得慣導(dǎo)指示位置的重力異常值ΔgM(φi,λi)、擾動重力梯度值TM(φi,λi).同時利用搭載在水下潛航器上的重力儀、重力梯度儀,測量得到實(shí)際重力異常值Δgs、擾動重力梯度值Ts,將慣導(dǎo)指示位置重力值與測量值之差作為Kalman濾波輸入量(量測值),即可利用Kalman濾波方法完成對慣導(dǎo)位置的修正.

然而水下潛航器機(jī)動航行運(yùn)動狀態(tài)并非保持不變,因此在匹配過程中僅采用預(yù)設(shè)動力學(xué)模型難以實(shí)時準(zhǔn)確地反映潛航器的運(yùn)動狀態(tài).尤其是當(dāng)潛航器運(yùn)動狀態(tài)發(fā)生改變時,會與預(yù)設(shè)動力學(xué)模型出現(xiàn)較大差異,濾波效果會顯著降低甚至出現(xiàn)發(fā)散現(xiàn)象.因此,需要引入自適應(yīng)濾波算法通過自適應(yīng)因子對狀態(tài)預(yù)測信息進(jìn)行調(diào)整,即當(dāng)狀態(tài)方程出現(xiàn)較大的模型誤差時,利用自適應(yīng)因子對狀態(tài)方程預(yù)測信息的協(xié)方差矩陣進(jìn)行膨脹處理,從而削弱預(yù)測信息對濾波結(jié)果的影響,提高量測信息對濾波結(jié)果影響的占比.流程圖見圖1.

圖1 重力/重力梯度匹配自適應(yīng)EKF算法

1.1 EKF狀態(tài)方程

由于僅平面位置誤差可觀,因此在利用重力觀測量進(jìn)行匹配導(dǎo)航時,選用X=(δφ,δλ)T作為濾波狀態(tài)變量,式中δφ、δλ分別表示水下航行器的緯度和經(jīng)度誤差.

由慣性導(dǎo)航誤差方程和濾波狀態(tài)變量可知,慣性導(dǎo)航系統(tǒng)平面定位誤差微分方程表達(dá)式如下:

(1)

式中,ve表示東向速度,RN表示指示位置點(diǎn)卯酉圈曲率半徑,φ表示地心緯度,Wφ與Wλ表示由系統(tǒng)噪聲產(chǎn)生的緯向和經(jīng)向誤差.

即可將(1)式化簡為

(2)

若濾波周期為T,則將(2)式進(jìn)行離散化處理后可得

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

(3)

式中,Φk/k-1表示狀態(tài)轉(zhuǎn)移矩陣,其可由系統(tǒng)狀態(tài)矩陣F通過Laplace變換得到Φk/k-1=I+F*T.

1.2 EKF量測方程

水下潛航器在利用慣性導(dǎo)航系統(tǒng)進(jìn)行定位導(dǎo)航時,航行區(qū)域的重力場可以視為關(guān)于地理坐標(biāo)的函數(shù).故此,可以將慣導(dǎo)指示位置在基準(zhǔn)圖上對應(yīng)的重力值和水下潛器搭載的重力儀測量得到的實(shí)際重力值之差作為Kalman濾波量測值輸入濾波器,并據(jù)此構(gòu)建量測方程,從而實(shí)現(xiàn)對慣性導(dǎo)航系統(tǒng)位置誤差的修正.

由于地球重力場是一個與位置相關(guān)的連續(xù)物理場,因此,假設(shè)水下潛航器真實(shí)位置的重力異常和重力梯度(本文均以擾動重力梯度垂直張量為例)在慣導(dǎo)指示位置點(diǎn)(φi,λi)的鄰域內(nèi)存在一階導(dǎo)數(shù),利用泰勒級數(shù)將其展開可得:

(4)

式中,φt、λt分別表示潛航器真實(shí)位置點(diǎn)處的緯度和經(jīng)度;vgi與vTi表示泰勒級數(shù)展開過程中的截斷誤差;ΔgM(φi,λi)與TzzM(φi,λi)表示慣導(dǎo)指示位置在基準(zhǔn)圖上對應(yīng)的重力異常和擾動重力梯度垂直張量值;Δg(φt,λt)與Tzz(φt,λt)表示水下潛航器實(shí)際位置處的重力異常值和擾動重力梯度垂直張量真值.

由于水下潛航器自身搭載有重力儀以及重力梯度儀,因此,Δg(φt,λt)與Tzz(φt,λt)又可以有如下表示:

(5)

式中,vgs與vTs表示重力儀和重力梯度儀測量產(chǎn)生的觀測誤差,Δgs(φt,λt)與Tzzs(φt,λt)表示由重力儀和重力梯度儀測量并計算得到的重力異常和擾動重力梯度垂直張量觀測值.

聯(lián)合式(4)、(5)即可得到重力匹配導(dǎo)航的濾波量測方程分別為

(6)

隨機(jī)線性化過程可利用雙二次曲面法進(jìn)行實(shí)時快速計算(李姍姍等,2011).

根據(jù)(6)式可得到重力異常與擾動重力梯度垂直張量組合匹配導(dǎo)航濾波量測方程為

(7)

同理亦可得到重力異常與任意擾動重力梯度張量間的組合匹配濾波量測方程.

1.3 異常判斷與自適應(yīng)因子構(gòu)造

自適應(yīng)濾波主要由異常判斷與自適應(yīng)因子構(gòu)造兩部分組成.常用于異常判斷的統(tǒng)計量包括預(yù)測殘差統(tǒng)計量、方差分量比統(tǒng)計量以及狀態(tài)不符值統(tǒng)計量(楊元喜,2017;Wang et al.,2020).其中后兩種統(tǒng)計量需要通過多余觀測才能求得,而由式(1)與(7)可知,重力/重力梯度EKF匹配算法中,觀測值個數(shù)與狀態(tài)未知數(shù)個數(shù)均為2,不存在多余觀測,因此僅能選用預(yù)測殘差統(tǒng)計量作為異常判斷條件.設(shè)第k時刻的量測值向量為Lk,則該時刻的預(yù)測殘差向量可表示為

(8)

通過預(yù)測殘差可以構(gòu)造相應(yīng)的預(yù)測殘差統(tǒng)計量:

(9)

選定合適的異常判斷統(tǒng)計量后,需要針對狀態(tài)模型參數(shù)特點(diǎn),構(gòu)造合適的自適應(yīng)因子用以調(diào)節(jié)相應(yīng)的權(quán).常用的自適應(yīng)因子函數(shù)模型有指數(shù)函數(shù)、兩段函數(shù)以及三段函數(shù)(楊元喜,2017;Dai et al.,2019).

指數(shù)函數(shù)模型:

(10)

式中c為閾值常數(shù),根據(jù)經(jīng)驗(yàn)和水下潛航器運(yùn)動特點(diǎn)進(jìn)行取值,取值范圍為c=1.0～2.5.

兩段函數(shù)模型:

(11)

三段函數(shù)模型:

(12)

式中c0、c1為閾值常數(shù),取值范圍為c0=1～1.5、c1=1.0～8.5.其中,兩段函數(shù)與指數(shù)函數(shù)模型屬于非歸零函數(shù),三段函數(shù)模型屬于歸零函數(shù).由于三段函數(shù)存在等零區(qū)間,即該區(qū)間狀態(tài)預(yù)測信息對濾波結(jié)果的影響為零,但是實(shí)際重力量測信息與狀態(tài)參數(shù)相關(guān)性并不能支持其作為濾波解算的唯一信息,因此在重力匹配算法中,應(yīng)選取非歸零函數(shù)作為自適應(yīng)因子模型,鑒于匹配導(dǎo)航實(shí)時性需求,本文選取計算較為簡單的兩段函數(shù)構(gòu)造自適應(yīng)因子,在針對不同的參數(shù)值進(jìn)行試驗(yàn)后,選定解算結(jié)果相對穩(wěn)定的值作為閾值常數(shù),本文后續(xù)實(shí)驗(yàn)中閾值常數(shù)c均取1.2.

2 基于CEP的移動窗口并行濾波器

2.1 圓概率誤差分析

圓概率誤差(Circular Error Probable,CEP)在軍事、導(dǎo)航以及電子地圖領(lǐng)域有著廣泛的應(yīng)用,圓概率誤差評定是對水下潛航器慣性導(dǎo)航系統(tǒng)進(jìn)行誤差評定分析的重要方法之一,在慣導(dǎo)系統(tǒng)投入使用前進(jìn)行測試,可有效評價系統(tǒng)的導(dǎo)航定位精度(Zimmer and Turner,2017).根據(jù)《中華人民共和國國家軍用標(biāo)準(zhǔn)——慣性導(dǎo)航系統(tǒng)精度評定方法》(GJB 729-1989)中定義的圓概率誤差評定方法,用RPi表示第ti時刻置信概率為P的圓概率誤差半徑,則RPi可表示為

(13)

(14)

式中Δλji、Δφji分別表示第j次實(shí)驗(yàn)中第i個采樣點(diǎn)的經(jīng)度與緯度誤差,φji表示緯度.

計算得到所有時刻置信概率為P的圓概率誤差半徑RPi后,整個慣性導(dǎo)航系統(tǒng)的圓概率誤差半徑RP取RPi中的最大值,即RP=max(RPi).隨后需要對慣性導(dǎo)航系統(tǒng)圓概率誤差半徑的可信度進(jìn)行評價.若共有m個航跡作為實(shí)驗(yàn)對象,每個航跡中有l(wèi)個采樣點(diǎn),則置信概率為P的圓概率誤差半徑的可信度Pk可表示為

(15)

利用水下潛航器仿真模擬系統(tǒng)進(jìn)行慣性/重力/重力梯度組合導(dǎo)航實(shí)驗(yàn)前,可對該仿真系統(tǒng)中慣性導(dǎo)航系統(tǒng)的圓概率誤差半徑及其可信度進(jìn)行計算.分別取不同的置信概率P,計算其圓概率誤差半徑及可信度.經(jīng)查表可得對應(yīng)的Z′P見表1. 慣性導(dǎo)航系統(tǒng)慣性元器件誤差設(shè)置如表2所示.

表1 Z′P數(shù)值表

表2 慣性元器件誤差設(shè)置

以8條航行時間為24 h的航跡作為實(shí)驗(yàn)對象,慣導(dǎo)采樣率為100 Hz,每3 min取一個采樣點(diǎn),即m=8,l=479.由于慣性導(dǎo)航系統(tǒng)的定位誤差會隨著航行時間的增加而增加,因此,在對圓概率誤差半徑進(jìn)行統(tǒng)計時應(yīng)考慮不同航行時長的差異.基于此計算并統(tǒng)計不同置信概率P的圓概率誤差半徑及可信度見表3.

表3 圓概率誤差半徑及可信度

2.2 CEP移動窗口并行濾波器構(gòu)建

在上述實(shí)驗(yàn)基礎(chǔ)上,首先基于不同置信概率、不同航行時長的圓概率誤差半徑,劃分慣導(dǎo)指示位置“相關(guān)格網(wǎng)”(包括中心格網(wǎng)及其周圍相鄰格網(wǎng))對濾波結(jié)果的影響占比,從而構(gòu)建基于CEP的移動窗口模型;然后利用濾波量測值與窗口坡度信息,對移動窗口模型進(jìn)行約束,使其更好的反映慣性/重力/重力梯度組合導(dǎo)航系統(tǒng)實(shí)際圓概率誤差半徑范圍,最后設(shè)計可行、有效的并行Kalman濾波器,獲得最優(yōu)濾波解.

由于重力基準(zhǔn)圖是由離散格網(wǎng)組成,導(dǎo)致移動窗口覆蓋半徑不連續(xù),難以與圓概率誤差半徑完全吻合,因此首先需要對不同大小的移動窗口覆蓋半徑進(jìn)行統(tǒng)計,然后與圓概率誤差半徑進(jìn)行比對,尋求與之相對符合的移動窗口.六邊形格網(wǎng)相較于四邊形格網(wǎng)有著更大的角分辨率(Middleton and Sivaswamy,2005;Sahr,2011;Usman and Torsten,2008),在使用相同或相近分辨率基準(zhǔn)圖進(jìn)行重力匹配導(dǎo)航時,使用六邊形格網(wǎng)的匹配精度更高,因此選用六邊形格網(wǎng)構(gòu)建的重力異常和擾動重力梯度圖作為基準(zhǔn)圖.基準(zhǔn)圖采用12層級ISEA4H(Icosahedral Snyder Equal Area aperture 4 Hexagon)六邊形格網(wǎng)系統(tǒng)進(jìn)行構(gòu)建,相鄰格網(wǎng)中點(diǎn)距離1.884 km(約1.017nmile),格網(wǎng)平均面積3.040 km2.不同覆蓋范圍移動窗口及其半徑如圖2所示.

圖2 不同范圍移動窗口及半徑示意圖

如圖2所示,僅考慮中心格網(wǎng)時(圖2a),其覆蓋半徑約為0.509 n mile(1 n mile = 1.852 km);當(dāng)移動窗口覆蓋范圍擴(kuò)大至中心格網(wǎng)及其相鄰格網(wǎng)(1層相鄰格網(wǎng),綠色格網(wǎng),圖2b)時,共包含7個格網(wǎng),其覆蓋半徑約為1.526 n mile;當(dāng)移動窗口覆蓋范圍擴(kuò)大至圖2b及其相鄰格網(wǎng)(2層相鄰格網(wǎng),灰色格網(wǎng),圖2c)時,共包含19個格網(wǎng),其覆蓋半徑約為2.543 n mile;當(dāng)移動窗口覆蓋范圍擴(kuò)大至圖2c及其相鄰格網(wǎng)(3層相鄰格網(wǎng),棕色格網(wǎng),圖2d)時,共包含37個格網(wǎng),其覆蓋半徑約為3.560 n mile;當(dāng)移動窗口覆蓋范圍擴(kuò)大至圖2d及其相鄰格網(wǎng)(4層相鄰格網(wǎng),藍(lán)色格網(wǎng),圖2e)時,共包含61個格網(wǎng),其覆蓋半徑約為4.577 n mile.

由表3與圖2結(jié)果,根據(jù)相近原則對不同航行時間圓概率誤差范圍進(jìn)行分析,獲得不同范圍移動窗口格網(wǎng)影響占比分層模型如圖3所示.

由表3可知,不同置信概率P的圓概率誤差半徑可信度均達(dá)到了90%以上,因此可以將圓概率誤差半徑對應(yīng)的置信概率視為慣性導(dǎo)航系統(tǒng)定位點(diǎn)落在該半徑范圍內(nèi)的概率,進(jìn)一步可以將置信概率P視為該半徑范圍內(nèi)格網(wǎng)點(diǎn)作為濾波輸入計算所得結(jié)果對總體結(jié)果的影響占比.因此,依據(jù)圖3所示CEP移動窗口分層模型,水下潛器航行時間在0～4 h時為1層模型,中央格網(wǎng)影響占比80%,1層相鄰格網(wǎng)影響占比20%;航行時間在4～8 h時為2層模型,中央格網(wǎng)影響占比70%,1層相鄰格網(wǎng)影響占比30%;航行時間在8～12 h時為3層模型,中央格網(wǎng)影響占比60%,1層相鄰格網(wǎng)影響占比30%,2層相鄰格網(wǎng)影響占比10%;航行時間在12～16 h時為4層模型,中央格網(wǎng)影響占比50%,1層相鄰格網(wǎng)影響占比20%,2層相鄰格網(wǎng)影響占比20%,3層相鄰格網(wǎng)影響占比10%;航行時間在16～24 h時為5層模型,中央格網(wǎng)與1層相鄰格網(wǎng)共同影響占比60%,2層相鄰格網(wǎng)影響占比20%,3層相鄰格網(wǎng)影響占比10%,4層相鄰格網(wǎng)影響占比10%.

但是,重力/重力梯度EKF匹配算法是實(shí)時進(jìn)行解算的,濾波結(jié)果可以對慣性導(dǎo)航系統(tǒng)定位誤差進(jìn)行一定程度的修正,實(shí)際在進(jìn)行慣性/重力/重力梯度組合導(dǎo)航過程中,潛航器不同航行時間段的圓概率誤差半徑并不完全符合表3特征,理論上應(yīng)小于慣性導(dǎo)航產(chǎn)生的圓概率誤差半徑.因此還需通過重力濾波量測值Lk和基準(zhǔn)圖信息對圓概率誤差半徑進(jìn)行約束.由于濾波移動窗口范圍較小,最大的5層移動窗口模型覆蓋半徑也小于5′,因此移動窗口范圍內(nèi)的重力異常與擾動重力梯度的變化率可以近似認(rèn)為是線性的,即可以用空間平面f(x,y)對移動窗口范圍重力異常與擾動重力梯度場進(jìn)行近似擬合.分別用平面擬合重力異常與擾動重力梯度可得

(16)

式中kgx、kTx和kgy、kTx分別表示擬合窗口平面在X方向和Y方向的坡度,(φi,λi)表示慣導(dǎo)指示位置坐標(biāo).

依據(jù)圖3圓概率誤差移動窗口分層模型,將窗口內(nèi)格網(wǎng)點(diǎn)的坐標(biāo)值、重力異常和擾動重力梯度值帶入式(16)利用最小二乘方法求解擬合窗口平面坡度值.然后利用濾波量測值Lk與坡度值之比可以得到潛航器實(shí)際位置與慣導(dǎo)指示位置的近似距離Ds.比較Ds與該時刻移動窗口模型覆蓋半徑的大小,取其中的較小值作為實(shí)際圓概率誤差半徑范圍.若移動窗口模型半徑小,則直接依據(jù)圖3采用模型半徑進(jìn)行計算;若Ds小,則根據(jù)Ds大小對模型層級進(jìn)行約束,選擇能夠包含Ds的最近層級移動窗口模型作為實(shí)際圓概率誤差半徑范圍.由于1、2層移動窗口模型最大覆蓋半徑相同,因此增加約束條件Ds<0.509 n mile時,選用1層移動窗口模型.此外值得注意的是,當(dāng)潛航器實(shí)際位置與慣導(dǎo)指示位置位于同一等值線上時(考慮到實(shí)際測量誤差與基準(zhǔn)圖誤差等的影響,重力異常差值小于±4 mGal,擾動重力垂直梯度差值小于±9 E(1 E=10-9/s2)),則不對移動窗口進(jìn)行修正.確定移動窗口范圍流程圖如圖4.

圖4 確定移動窗口范圍流程圖

(17)

(18)

3 實(shí)驗(yàn)驗(yàn)證與分析

3.1 24 h航行仿真實(shí)驗(yàn)

使用慣性/重力/重力梯度組合導(dǎo)航仿真程序?qū)λ聺摵狡骱桔E與測量數(shù)據(jù)進(jìn)行仿真,仿真數(shù)據(jù)包含慣性元器件輸出(角增量與速度增量)、重力異常觀測數(shù)據(jù)、擾動重力梯度觀測數(shù)據(jù)以及水下潛航器真實(shí)航跡.當(dāng)前,海洋重力儀動態(tài)測量精度已達(dá)到1 mGal(如德國的KSS系列和美國的L&R系列重力儀),因此重力儀測量誤差一般取標(biāo)準(zhǔn)差為1 mGal的白噪聲,此外還需要增加一個誤差項(xiàng)用以表示由于厄特弗斯改正等因素造成的影響,根據(jù)經(jīng)驗(yàn)取為3 mGal (許大欣,2005).重力梯度儀的動態(tài)測量精度可達(dá)到7 E(如美國的HD-AGG和英國的EGG系列重力梯度儀),因此重力梯度儀測量誤差取標(biāo)準(zhǔn)差為7 E的白噪聲(胡平華等,2017).使用12層級ISEA4H六邊形格網(wǎng)系統(tǒng)構(gòu)建重力異常與擾動重力梯度基準(zhǔn)圖,選用丹麥科技大學(xué)(Technical University of Denmark,DTU)利用衛(wèi)星測高數(shù)據(jù)、實(shí)測重力異常數(shù)據(jù)等反演計算發(fā)布的DTU18全球重力異常模型(數(shù)據(jù)分辨率1′,中誤差2～3 mGal),利用內(nèi)插方法構(gòu)建六邊形海洋重力異常基準(zhǔn)圖.擾動重力梯度基準(zhǔn)圖則在EIGEN-6C4地球重力場模型與DTU18全球重力異常模型基礎(chǔ)上,使用Stokes移去-恢復(fù)法計算得到.基于Txx+Tyy+Tzz=0的內(nèi)符合約束條件,計算結(jié)果Txx+Tyy+Tzz絕對值最大值0.035 E,標(biāo)準(zhǔn)差0.0021 E,能夠一定程度上反映所構(gòu)建擾動重力梯度基準(zhǔn)圖的正確性.選擇重力異常Δg與擾動重力梯度垂直張量Tzz作為濾波觀測量,仿真試驗(yàn)區(qū)選在南海海域,區(qū)域大小為2.5°×3.5°(北緯7°—9.5°,東經(jīng)110°—113.5°),試驗(yàn)區(qū)重力異常與擾動重力梯度垂直張量基準(zhǔn)圖如圖5所示.

圖5 試驗(yàn)區(qū)基準(zhǔn)圖

實(shí)驗(yàn)航跡航行時長24 h,初始速度與姿態(tài)分別為(0 m·s-1,0 m·s-1,0 m·s-1)和(0°,0°,-10°),起始坐標(biāo)為(7°N,110°E,-100 m).潛航器參考航跡如圖黑色航跡所示,沿著北偏東45°方向先以0.0086 m·s-2的加速度,勻加速至速度達(dá)到10 n mile /h,然后勻速直線航行12 h后,右轉(zhuǎn)45°,再勻速航行12 h.潛航器慣導(dǎo)指示航跡如圖紅色航跡所示.

表4 24 h航行誤差統(tǒng)計

圖6 24 h航行誤差曲線圖

由表4與圖6可知,24 h航行過程中,慣性導(dǎo)航系統(tǒng)導(dǎo)航定位誤差最大達(dá)到5.005 n mile,平均值2.698 n mile.使用重力EKF匹配算法后,導(dǎo)航定位誤差最大值為1.973 n mile,相較于INS減少了60.6%,平均值0.643 n mile,減少了76.2%,定位精度提高了76.3%.在使用自適應(yīng)因子對濾波進(jìn)行優(yōu)化后,AEKF匹配算法導(dǎo)航定位誤差最大值為1.293 n mile,相較于INS減少了74.2%,平均值0.366 n mile,減少了86.4%,定位精度提高了87.7%.而基于CEP的自適應(yīng)并行濾波AEKF-CEP匹配算法導(dǎo)航定位誤差最大值為0.691 n mile,相較于INS減少了86.2%,平均值0.212 n mile,減少了92.1%,定位精度提高了93.8%.由實(shí)驗(yàn)結(jié)果可知,本文所提AEKF-CEP匹配算法相較于傳統(tǒng)EKF算法導(dǎo)航定位精度提高了74.0%,相較于AEKF提高了49.8%,能夠有效提高慣性/重力/重力梯度組合導(dǎo)航的定位精度.并由圖6c可知,AEKF-CEP能夠有效抑制由于慣性導(dǎo)航系統(tǒng)定位不準(zhǔn)確導(dǎo)致的濾波發(fā)散問題,潛航器24 h航行導(dǎo)航定位誤差均保持在0.7 n mile以內(nèi).

每3 min進(jìn)行一次濾波量測更新,潛航器24 h航行過程共進(jìn)行了479次量測更新,對歷次量測更新中使用的移動窗口模型層級進(jìn)行統(tǒng)計,各個層級模型使用情況如表5所示.

表5 CEP移動窗口模型使用情況統(tǒng)計表

由表5與可知,經(jīng)過近似距離Ds的約束后,CEP移動窗口模型最高使用至4層,且僅使用2次,使用占比0.42%;3層模型使用11次,占比2.29%;1、2層模型共使用466次,使用占比97.29%.結(jié)果表明,本文所提CEP移動窗口并行濾波器能夠較好的反映慣性/重力/重力梯度組合導(dǎo)航系統(tǒng)實(shí)際的圓概率誤差范圍,并且能夠?qū)Σ煌瑢蛹壪噜徃窬W(wǎng)中最優(yōu)濾波器進(jìn)行判定,在經(jīng)過重力AEKF-CEP濾波匹配修正后的組合導(dǎo)航系統(tǒng)定位誤差明顯減少,導(dǎo)航定位精度得到有效提升.

3.2 長時間航行仿真實(shí)驗(yàn)

為驗(yàn)證基于CEP的重力EKF匹配算法在潛航器長時間水下航行時的有效性,在4.1節(jié)的仿真實(shí)驗(yàn)基礎(chǔ)上,進(jìn)行潛航器長時間航行實(shí)驗(yàn).實(shí)驗(yàn)航跡航行時長240 h(10天),初始速度與姿態(tài)分別為(0 m·s-1,0 m·s-1,0 m·s-1)和(0°,0°,0°),起始坐標(biāo)為(46°S,76°E,-100 m).潛航器首先沿著正北方向以0.0086 m·s-2的加速度,勻加速至速度達(dá)到10 n mile /h,隨后勻速直線航行50 h;然后左轉(zhuǎn)90°,再勻速航行80 h;最后右轉(zhuǎn)90°,再勻速航行110 h.

使用基于CEP的自適應(yīng)并行EKF算法(AEKF-CEP)對航跡進(jìn)行匹配導(dǎo)航,自適應(yīng)因子閾值常數(shù)c取值為1.2.航跡與誤差曲線如圖7(航跡背景為航行區(qū)域擾動重力梯度基準(zhǔn)圖)所示,匹配導(dǎo)航誤差統(tǒng)計結(jié)果如表6所示.

表6 10天航行誤差統(tǒng)計

由表6與圖7可知,10天航行過程中,慣性導(dǎo)航系統(tǒng)導(dǎo)航定位誤差最大達(dá)到46.074 n mile,平均值16.739 n mile.使用AEKF-CEP算法后,導(dǎo)航定位誤差最大值降低為7.246 n mile,平均值1.030 n mile,定位精度提高了91.0%.由圖7b和圖7c可以看出,慣性導(dǎo)航緯向誤差呈周期性震蕩,經(jīng)向誤差則隨時間發(fā)散,AEKF-CEP重力匹配算法能夠減小慣性導(dǎo)航系統(tǒng)緯向誤差的震蕩幅度,并抑制經(jīng)向誤差的發(fā)散.

3.3 南海實(shí)測實(shí)驗(yàn)

為了更好地驗(yàn)證本文所提基于CEP的重力EKF匹配算法的有效性,利用南海某海域?qū)崪yINS、GPS和重力觀測數(shù)據(jù)進(jìn)行實(shí)驗(yàn).由于缺乏重力梯度儀,實(shí)驗(yàn)中僅采用重力異常作為量測值.本次實(shí)驗(yàn)數(shù)據(jù)是2013年8月16日在南海某海域采集,分別采用SystemⅡ海空重力儀采集重力數(shù)據(jù),內(nèi)符合精度1 mGal;采用光學(xué)捷聯(lián)慣導(dǎo)系統(tǒng)采集慣導(dǎo)數(shù)據(jù).

匹配導(dǎo)航測線航行時長3.7 h,慣導(dǎo)采樣率100 Hz,重力匹配間隔180 s.實(shí)驗(yàn)中還需將所得到的重力讀數(shù)通過添加厄特弗斯改正、空間改正和中間層改正等各項(xiàng)改正,轉(zhuǎn)換到平均海水面得到水面重力異常,然后再進(jìn)行重力異常匹配.使用AEKF-CEP算法對航跡進(jìn)行匹配導(dǎo)航,自適應(yīng)因子閾值常數(shù)c取值為1.2.誤差曲線如圖8所示,匹配導(dǎo)航誤差統(tǒng)計結(jié)果如表7所示.

表7 實(shí)測航行誤差統(tǒng)計

圖8 實(shí)測航行誤差曲線圖

由表7與圖8可知,測線航行過程中慣性導(dǎo)航系統(tǒng)導(dǎo)航定位誤差最大值為1.983 n mile,平均值1.086 n mile.使用AEKF-CEP算法后,導(dǎo)航定位誤差最大值降低為0.632 n mile,平均值0.306 n mile,定位精度提高了72.8%.由圖8c可知,當(dāng)慣導(dǎo)系統(tǒng)定位誤差小于重力基準(zhǔn)圖格網(wǎng)半徑時,匹配算法很難起到效果,且結(jié)合圖8a,在航行48 min時,匹配導(dǎo)航位置緯向誤差產(chǎn)生震蕩,匹配導(dǎo)航緯向誤差大于慣導(dǎo)誤差,其原因在于慣導(dǎo)誤差小于基準(zhǔn)圖分辨率,同時重力量測值可能存在粗差,從而導(dǎo)致匹配定位偏離實(shí)際位置.同時,本次實(shí)驗(yàn)也存在一定局限性,首先是測線航行時間較短,僅使用了一層模型,并不能很好的反映算法在長時間無源定位導(dǎo)航中的效果;其次,本次實(shí)驗(yàn)是對已有實(shí)測數(shù)據(jù)的的模擬實(shí)時處理,并非真正意義上的實(shí)時實(shí)測實(shí)驗(yàn).

4 結(jié)論與討論

針對現(xiàn)有慣性/重力/重力梯度組合導(dǎo)航EKF算法濾波狀態(tài)方程存在模型誤差以及慣性導(dǎo)航系統(tǒng)存在累積誤差而造成的濾波失準(zhǔn)乃至發(fā)散問題,提出了一種基于CEP的自適應(yīng)并行EKF匹配算法,該算法以濾波量測值和窗口坡度之比作為約束,設(shè)計了CEP移動窗口分層模型,然后依據(jù)分層窗口構(gòu)建并行濾波器,并通過SWRS值最小選擇最優(yōu)濾波器.經(jīng)實(shí)驗(yàn)計算分析表明,基于CEP的自適應(yīng)并行EKF匹配算法相較于傳統(tǒng)EKF算法和自適應(yīng)EKF算法的水下重力匹配導(dǎo)航定位精度分別提升了74.0%和49.8%.并根據(jù)水下潛航器長期水下作業(yè)航行特性,設(shè)計了一組長時間水下無源定位導(dǎo)航實(shí)驗(yàn),實(shí)驗(yàn)航行時長10天,重力AEKF-CEP匹配算法導(dǎo)航定位誤差最大值降低為8.783 n mile,導(dǎo)航定位精度提高了83.9%.在此基礎(chǔ)上,在南海海域進(jìn)行了實(shí)測數(shù)據(jù)實(shí)驗(yàn),重力AEKF-CEP匹配算法導(dǎo)航定位誤差最大值降低為0.632 n mile,導(dǎo)航定位精度提高了72.8%.通過仿真與實(shí)測數(shù)據(jù)實(shí)驗(yàn)表明,重力AEKF-CEP匹配算法能夠在一定程度上克服慣性導(dǎo)航系統(tǒng)由于時間推移誤差積累的缺陷,提高系統(tǒng)導(dǎo)航定位精度,增加匹配算法的魯棒性.

但是本文對于重力AEKF-CEP匹配算法的實(shí)測實(shí)驗(yàn)存在一定局限性,一是由于實(shí)測數(shù)據(jù)為船測水面數(shù)據(jù),與實(shí)際潛航器水下航行存在一定差異;二是實(shí)測數(shù)據(jù)實(shí)驗(yàn)測線較短,航行時間為3.7 h,僅使用了一層模型,不能較好地反映算法應(yīng)用于長時間無源定位導(dǎo)航中的效果;三是實(shí)測數(shù)據(jù)處理過程是對現(xiàn)有數(shù)據(jù)的模擬實(shí)時處理,盡管能夠在一定程度上反映算法對于實(shí)測數(shù)據(jù)的適用性,但依然與實(shí)時數(shù)據(jù)處理存在差異,后續(xù)將開展實(shí)時匹配航行實(shí)驗(yàn).同時,本文在實(shí)驗(yàn)過程中,僅分別使用單個Txx、Tyy和Tzz張量與重力異常Δg進(jìn)行組合構(gòu)建濾波量測方程,三種組合形式均可對慣導(dǎo)定位誤差進(jìn)行一定程度的修正,其中Tzz張量的組合形式匹配結(jié)果優(yōu)于Txx和Tyy,究其原因可能是Tzz的變化相較于Txx和Tyy更為劇烈.但是,尚未對多個擾動重力梯度張量最優(yōu)組合形式進(jìn)行探索與分析,后續(xù)將開展對多擾動重力梯度張量各類組合形式的研究.