利用TERCOM與ICCP進行聯合重力匹配導航

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

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

0 引言

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

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

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

1 現有重力匹配算法分析

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

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

1.1 TERCOM工作原理

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

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

均方差算法

(1)

平均絕對值算法

(2)

交叉相關算法

(3)

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

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

圖2 改進TERCOM搜索區域Fig.2 Improved TERCOM searching area

1.2 ICCP工作原理

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

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

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

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

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

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

2 ICCP輔助TERCOM修正算法

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

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

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

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

(4)

dTERi=

(5)

(6)

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

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

(7)

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

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

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

(8)

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

d1=

(9)

d2=

(10)

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

(11)

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

(12)

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

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

d3=|dMAi-dINSi|

(13)

d4=|dTERi-dINSi|

(14)

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

(15)

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

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

3 仿真實驗分析

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

實驗一匹配結果如圖7所示。

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

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

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

表1 三種匹配算法誤差

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

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

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

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

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

表2 三種匹配算法誤差

4 結論

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

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

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

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