利用TERCOM與ICCP進行聯(lián)合重力匹配導(dǎo)航

鄒嘉盛， 肖 云， 孫愛斌， 韋建成, 孟 寧， 艾尚校

(1.地理信息工程國家重點實驗室，西安 710054; 2.長安大學(xué)地質(zhì)工程與測繪學(xué)院，西安 710054; 3.西安測繪研究所，西安 710054; 4.中國電子科技集團公司第二十研究所，西安 710068)

0 引言

目前，對于海洋科學(xué)探測和海洋開發(fā)利用的水下自主航行器而言，水下導(dǎo)航與定位技術(shù)能夠提供滿足需求的高精度速度、姿態(tài)、位置等信息。而潛艇作為軍事力量的重要手段之一，科學(xué)實驗航行器具備的水下導(dǎo)航精度無法滿足潛艇長航時、長航距、高隱蔽性的要求。目前，水下導(dǎo)航定位方式主要包括慣性導(dǎo)航、聲學(xué)定位以及導(dǎo)航和海洋地球物理定位與導(dǎo)航等。其中，慣性導(dǎo)航是最為主要的自主導(dǎo)航方式之一，因為慣性導(dǎo)航可以輸出全面的導(dǎo)航信息，其主要原理是通過內(nèi)部慣性敏感元件對水下載體相對于慣性空間的線運動和角運動參數(shù)進行測量，在給定的運動初始條件下，如經(jīng)緯度和速度信息，解算出水下載體的姿態(tài)、方位和位置等信息。慣性導(dǎo)航具有精度高、可靠性強、基本不受外界干擾等優(yōu)勢，但在軌跡解算和定位解算時，系統(tǒng)誤差隨著時間積累不斷擴大，這就需要一些輔助匹配定位技術(shù)對慣性導(dǎo)航的激烈誤差進行修正。由于海洋重力場特征穩(wěn)定且匹配定位精度較高[1-3]，利用地球重力場輔助水下載體慣性導(dǎo)航是水下自主導(dǎo)航的重要研究趨勢[4-6]。

童余德等提出了基于局部重力圖逼近的組合匹配算法，利用二維高斯基函數(shù)對局部離散格網(wǎng)重力異常圖進行逼近以獲取其解析表達式，利用地形輪廓匹配(Terrain Contour Matching, TERCOM)選擇最優(yōu)軌跡，最后利用迭代更新軌跡，控制了誤差范圍，縮小了誤匹配的最大距離[7]；蔣東方等提出了在具有統(tǒng)一解析式連續(xù)背景場的基礎(chǔ)上實現(xiàn)迭代最近等值線 (Iterated Closest Contour Point, ICCP)的匹配算法，建立了局部連續(xù)背景場的最近點搜索模型，采用BFGS(Broyden-Fletcher-Gold-farb-Shanno)擬牛頓方法實現(xiàn)了置信范圍內(nèi)最近等值點的精確定位[8]；王志剛等提出了將匹配位置誤差作為觀測量，利用Kalman濾波對慣導(dǎo)系統(tǒng)誤差進行最優(yōu)估計，以驗證ICCP算法位置是否可用于修正慣導(dǎo)誤差[9]；蔡龍飛等采用固定初始序列長度的方式對算法結(jié)構(gòu)進行改善，并推導(dǎo)出單點迭代公式，同時采用滑動窗搜索方式縮小搜索范圍以提高搜索速度[10]；王勝平等提出了聯(lián)合TERCOM與ICCP的水下地磁匹配導(dǎo)航新方法，對粗匹配和精匹配進行分析，通過改進ICCP算法，解決了傳統(tǒng)匹配算法速度、精度以及完備性不能兼得的問題[11]。以上文獻對傳統(tǒng)TERCOM以及ICCP算法做出了多項理論改進，但忽略了慣導(dǎo)在短時間、短航距上可提供高精度導(dǎo)航的特性。

ICCP算法較好地保持了匹配軌跡空間位置關(guān)系的整體性，TERCOM算法則更加注重軌跡重力異常值的序列一致性。本文通過對比分析慣性導(dǎo)航系統(tǒng)(Inertial Navigation System, INS)采樣間隔航距以及兩種匹配算法的匹配航距，有效找出改進型TERCOM匹配點存在的明顯匹配異樣點，并用ICCP匹配點進行替換，從而聯(lián)合兩種算法的匹配結(jié)果點進行擇優(yōu)錄取。實驗結(jié)果表明：在ICCP自身匹配誤差不大的前提下，以均方差匹配作為主要匹配結(jié)果，根據(jù)一系列判斷方式，擇優(yōu)選取兩種匹配算法點生成一條由均方差匹配點與ICCP迭代點組合而成的新匹配軌跡，有效提高了匹配精度。

1 現(xiàn)有重力匹配算法分析

重力匹配INS是利用地球重力場特性，在高分辨率的重力異常圖組成的基本特性信息數(shù)據(jù)庫中[12]，結(jié)合INS提供的實時輸出位置以及海洋重力儀提供的實測值，按照一定的匹配算法對慣導(dǎo)位置進行修正，以實現(xiàn)高精度水下定位的一種導(dǎo)航技術(shù)。系統(tǒng)原理結(jié)構(gòu)圖[13]如圖1所示。

圖1 重力匹配算法原理圖Fig.1 Gravity matching algorithm schematic

1.1 TERCOM工作原理

在地球陸地表面上不同位置的重力值通常是不同的，對于海面上任意位置的地理坐標，都可以根據(jù)其周圍重力場的等值線或者重力場分布單值進行確定[14]。在匹配區(qū)域中，測量若干重力值，由慣導(dǎo)系統(tǒng)的位置信息和誤差信息在已有的數(shù)字重力圖上確定搜索區(qū)域，將重力值序列和相關(guān)區(qū)域按一定的算法進行相關(guān)分析，所得的相關(guān)極值點對應(yīng)的位置就是潛艇的航行位置，進而修正主導(dǎo)航系統(tǒng)的導(dǎo)航參數(shù)[15]。

常用的TERCOM算法有均方差算法、平均絕對差算法以及交叉相關(guān)算法。

均方差算法

(1)

平均絕對值算法

(2)

交叉相關(guān)算法

(3)

其中，gk表示第k條匹配軌跡的重力值；X(i)表示重力儀的實測值。三種算法對于最優(yōu)匹配結(jié)果，一般要求平均絕對算法和均方差算法的計算結(jié)果應(yīng)該為最小，而交叉互相關(guān)算法所得結(jié)果最大。

為了更加精確地搜索每個待匹配點的真實坐標，本文對傳統(tǒng)的TERCOM匹配算法進行改進，利用滑動窗口搜索模式，根據(jù)慣性導(dǎo)航誤差隨時間積累的情況，分別對不同采樣時間的采樣點的搜索范圍進行重新定義，其搜索區(qū)域隨航行距離的增加而增加，并在相應(yīng)搜索范圍內(nèi)尋找實測重力等值點，隨后進行按序列的隨機連線，生成N1×N2×…×Ni條待匹配軌跡，將初始匹配點的誤差控制在較小范圍內(nèi)，如圖2所示，利用上述相關(guān)性分析求取最佳匹配軌跡。該改進算法有效保證了匹配初期的匹配精度，充分考慮了單點匹配情況。

圖2 改進TERCOM搜索區(qū)域Fig.2 Improved TERCOM searching area

1.2 ICCP工作原理

ICCP算法是一種基于剛性變換(旋轉(zhuǎn)和平移)的反復(fù)接近最近等值線點的算法，通過將重力測量點變換到與之最近的等值線點，并利用這些點構(gòu)成估計航跡，不需要事先確定對應(yīng)點，而是利用算法不斷重復(fù)運動變換—確定對應(yīng)關(guān)系—求運動變換的過程，逐步改進運動的估計，進而校正慣導(dǎo)誤差。如圖3所示,算法在重力圖上缺乏明顯圖形特征的情況下比較適用，而重力儀的最小感量、重力圖的網(wǎng)格間距和數(shù)據(jù)密度對導(dǎo)航定位精度具有較大影響[16-17]。算法具體步驟如下[18]：

1)對每一個指示航跡點Pi在測量重力值對應(yīng)的重力等值線Ci上尋找最近點，記為Yi；

圖3 ICCP匹配算法基本原理Fig.3 Basic principles of ICCP matching algorithm

3)將集合P變換到TP，將新的集合TP作為起始集合進行下一步迭代，直至收斂即T停止顯著變化；

4)經(jīng)過收斂迭代后獲得的集合TP即為最終的匹配航跡。

ICCP算法有很多終止標準，如：1)旋轉(zhuǎn)和平移的增量都小于門限值；2)旋轉(zhuǎn)和平移的絕對值都小于門限值；3)殘差變化量小于門限值[19]。

2 ICCP輔助TERCOM修正算法

TERCOM算法充分利用潛器攜帶的重力儀實測數(shù)據(jù)，與所在區(qū)域的重力底圖進行配準，采用軌跡多點采樣，根據(jù)底圖柵格數(shù)據(jù)點尋找與實測軌跡重力異常數(shù)據(jù)序列的最佳匹配序列，反算其最近格網(wǎng)對應(yīng)坐標，以得到最終航跡坐標。TERCOM算法包括均方差匹配、平均絕對差匹配以及交叉相關(guān)匹配。本文采用滑動窗口搜索模型，分別以每個采樣點為搜索半徑設(shè)定搜索范圍，并搜索范圍內(nèi)的等值點，依次進行隨機連線，生成N條待匹配軌跡。對于最佳匹配結(jié)果，一般要求平均絕對算法和均方差算法的計算結(jié)果應(yīng)該為最小，而交叉相關(guān)算法所得結(jié)果最大。三種算法重點考慮了重力異常值的對應(yīng)關(guān)系，但忽略了潛艇運行軌跡帶來的空間幾何位置關(guān)系，在實際匹配過程中，由于重力儀自身存在的儀器測量誤差，且重力底圖數(shù)據(jù)也存在誤差，同時還存在因柵格分辨率導(dǎo)致的精度誤差，因此經(jīng)常出現(xiàn)較為明顯的誤匹配現(xiàn)象，如圖4所示，其中2個采樣點出現(xiàn)了較為明顯的匹配誤差，不可作為有效匹配點。本文涉及的軌跡匹配圖均采用東北天坐標系，其中X軸指向為東，Y軸指向為北。

圖4 均方差匹配Fig.4 Mean variance matching

相比于TERCOM匹配在空間位置關(guān)系上的不足，ICCP算法則在這方面具有較強的優(yōu)勢，僅在每次迭代初期利用重力異常值尋找底圖相應(yīng)等值線，隨后利用空間位置關(guān)系進行旋轉(zhuǎn)和平移，從而有效地保障了整條軌跡的一致性和整體性。但ICCP匹配對慣導(dǎo)精度要求較高，受影響因素較多，在多數(shù)情況下匹配效果低于TERCOM算法。因此，本文結(jié)合TERCOM算法在整體匹配中的高精度特點和ICCP算法在匹配軌跡中的整體結(jié)構(gòu)性特點，以TERCOM匹配結(jié)果為主，ICCP匹配結(jié)果為輔，綜合兩者的匹配結(jié)果。首先利用慣導(dǎo)軌跡提供的相鄰采樣點間的航跡的，與對應(yīng)航跡的TERCOM匹配結(jié)果進行對比，通過分析采樣點兩側(cè)航距，尋找出可疑錯誤匹配點，以及均方差匹配結(jié)果明顯異常點，用ICCP匹配結(jié)果點進行替換，經(jīng)過多次循環(huán)對比、分析和替換，從而生成一條新的、基于兩種匹配算法聯(lián)合的軌跡點。詳情如下：

1)計算INS軌跡中每個采樣間隔的航距，同時計算均方差匹配和ICCP匹配中每個采樣間隔的航距。

(4)

dTERi=

(5)

(6)

2)對均方差匹配結(jié)果進行分析。設(shè)置限差，分析從第2個采樣間隔點到第N-1個采樣間隔點之間的航距，選取采樣間隔點在上一時刻航距異常、下一時刻航距正常的點，用ICCP對應(yīng)采樣點進行賦值替換，否則保留均方差匹配結(jié)果點。

MA(Xk,Yk)=ICCP(Xk,Yk)

(7)

3)重新計算由步驟2)結(jié)果更新后的新軌跡的每段采樣間隔航距。

4)對新軌跡進行分析。設(shè)置限差，分析從第2個采樣間隔點到第N-1個采樣間隔點之間的航距，選取采樣間隔點在上一時刻航距異常、下一時刻航距也異常的點，用ICCP賦值對應(yīng)采樣點進行賦值替換，否則保留均方差匹配結(jié)果點。

MA(Xt,Yt)=ICCP(Xt,Yt)

(8)

5)分別計算均方差和ICCP匹配算法的最后一個采樣點與步驟4)的第N-1個采樣點之間的距離，將2個距離結(jié)果與INS最后采樣航距段進行對比，選擇更接近慣導(dǎo)航距的點作為改正替換點。

d1=

(9)

d2=

(10)

MA(XN,YN)=ICCP(XN,YN)(d1

(11)

MA(XN,YN)=TER(XN,YN)(d1>d2)

(12)

6)重新計算由步驟5)結(jié)果更新后的新軌跡的每段采樣間隔航距。

7)計算步驟6)所得軌跡采樣間隔航距與慣導(dǎo)軌跡對應(yīng)航距的差，同時獲取TERCOM航距與慣導(dǎo)軌跡對應(yīng)航距的差，選擇更為接近慣導(dǎo)航距的點作為更新后軌跡點。

d3=|dMAi-dINSi|

(13)

d4=|dTERi-dINSi|

(14)

MA(Xi,Yi)=TER(Xi,Yi)(d3>d4)

(15)

8)重復(fù)步驟4)得到最終軌跡。流程圖如圖5所示。

圖5 本文新匹配算法流程圖Fig.5 New matching algorithm flow charts

3 仿真實驗分析

本文采用MATLAB進行編程仿真實驗，重力異常分辨率為1′×1′,假定水下潛艇以10m/s的速度航行，實驗一海域位于某海域，如圖6所示,采樣間隔為3min一個采樣點，單次匹配以10個采樣間隔點航跡為一次匹配。匹配實驗分別采用TERCOM均方差匹配和ICCP算法匹配，其中ICCP算法迭代限值條件為迭代次數(shù)超過100次或相鄰迭代坐標平均變化不超過10m。隨后利用本文提及的綜合算法，限差為二分之一的底圖分辨率，綜合均方差匹配以及ICCP算法匹配結(jié)果點中誤差較小的點生成一條新的匹配軌跡。

實驗一匹配結(jié)果如圖7所示。

圖6 實驗一重力異常背景圖Fig.6 Gravity anomaly background of experiment one

圖7 實驗一匹配結(jié)果Fig.7 Matching results of experiment one

如圖7所示，均方差匹配總體結(jié)果較好，但在第4和第7個采樣點出現(xiàn)明顯匹配誤差，超過7km，導(dǎo)致平均誤差范圍達到2015m。誤差原因是由對應(yīng)慣導(dǎo)點為搜索半徑的區(qū)域內(nèi)出現(xiàn)與重力儀實測數(shù)據(jù)相同點，導(dǎo)致誤差點符合均方差匹配算法，但不符合實際潛艇運動軌跡在限定速度與限定時間情況下的理論航距。利用本文所述組合定位方法，實現(xiàn)了X、Y方向平均誤差由1個格網(wǎng)精度降至三分之一個格網(wǎng)精度，如表1所示，最小值均實現(xiàn)了最優(yōu)選擇，誤差均方差由km級降至百m級。因此，借助ICCP匹配結(jié)果點，有效修正了TERCOM均方差匹配誤差，從而提高了匹配精度，降低了匹配誤差，使得平均誤差降至872m。

表1 三種匹配算法誤差

實驗二選擇重力異常變化較緩區(qū)域，海域重力場背景圖如圖8所示，潛艇航行于重力異常起伏變化緩慢區(qū)域，同樣采用采樣時間間隔3min，航速10m/s的軌跡線，每10個采樣點進行一次匹配修正。

圖8 實驗二重力異常背景圖Fig.8 Gravity anomaly background of experiment two

實驗二匹配結(jié)果如圖9所示，由于潛艇航行于重力異常變化不明顯區(qū)域，導(dǎo)致TERCOM匹配結(jié)果出現(xiàn)多處明顯錯誤。從軌跡誤匹配的航距角度分析，已不再與實驗一中出現(xiàn)的情況類似，由簡單的單點誤匹配變成了多點連續(xù)誤匹配。

如圖9所示，由于軌跡處于重力異常圖特征性標準差較小區(qū)域，所以均方差匹配效果不佳，平均誤差達到了2200m。第2、第6和第7個軌跡點出現(xiàn)了較大匹配誤差，但ICCP算法匹配效果整體較好，平均誤差僅為797m，遠低于均方差誤差，其中的第1，第3～第5個點匹配誤差略低于均方差匹配，其他點均優(yōu)于均方差匹配，如表2所示。通過本文提出的綜合判斷方式，擇優(yōu)最佳點，分別選取了X、Y方向的最小值，同時最大值均不超過傳統(tǒng)TERCOM和ICCP最大值，X、Y方向平均誤差較TERCOM有顯著下降，分別由X向的1900m下降至540m,Y向的770m下降至160m，實驗說明綜合判斷方式提高了匹配精度，實現(xiàn)了由km級降至百m級的誤差效果，綜合誤差為624m。

圖9 實驗二匹配結(jié)果Fig.9 Matching results of experiment two

表2 三種匹配算法誤差

4 結(jié)論

基于TERCOM算法在平均匹配精度上的優(yōu)點以及ICCP算法在匹配軌跡整體一致性的特點，本文采用以均方差匹配結(jié)果點為主要匹配點，ICCP匹配結(jié)果點為修正匹配點的方式，通過計算兩種匹配方法在每段采樣間隔的航距，與慣性導(dǎo)航的對應(yīng)航距進行比較，找出在均方差匹配結(jié)果中常出現(xiàn)的誤匹配現(xiàn)象點，并用ICCP對應(yīng)匹配結(jié)果點進行替換的聯(lián)合匹配算法。

1)該算法能有效辨別出TERCOM 匹配中出現(xiàn)的較為明顯的錯誤匹配點，并通過替換的方式減小匹配誤差，降低了整體匹配軌跡的平均誤差，由km級降至百m級。

2)目前主流的水下重力匹配導(dǎo)航算法多以TERCOM、SITAN、ICCP三種為基礎(chǔ)，依舊缺乏可同時吸取多種算法優(yōu)點，降低各種算法自身缺陷的新算法。

3)本文雖在一定程度上有效降低了TERCOM匹配誤差，但對于匹配誤差實現(xiàn)數(shù)量級程度的修正仍然不夠，同時對TERCOM初始匹配以及ICCP整體匹配精度較高，需要在今后的工作中繼續(xù)加大研究。