一種仿射變換兩步式TERCOM 重力匹配算法

吳睿盈, 王 燕, 孫 勇

（北京航天控制儀器研究所, 北京 100039）

0 引言

水下航行器長航時、 高精度的自主導航不僅是執行水下任務、 發展海洋經濟及維護領海安全的前提基礎, 也是建設 “海洋強國” 的重要保障［1-2］。 慣 性 導 航 系 統（Inertial Navigation System,INS）因其同時具備自主性、 隱蔽性和抗干擾性等諸多優點被廣泛應用于各類自主導航平臺［3］。 但是因為慣性器件的誤差隨時間累積, 現階段水下航行器自主導航多采用以慣性導航為主, 以基于重力、地形、 地磁等地球物理特征的匹配導航為輔的組合導航方式。 其中, 重力匹配導航獲取重力信息時不會對外輻射能量, 可以保證航行器的隱蔽性,且海洋重力場特征較為穩定, 匹配定位精度高,利用重力匹配輔助慣性導航對于提高水下航行器的自主導航能力具有重要意義和軍事價值［4-5］。

慣性/重力匹配導航系統由慣性導航系統、 海洋重力場基準圖、 重力測量系統和重力匹配導航系統四部分構成, 由航行器自身搭載的重力儀實時測量所處位置的重力場特征信息, 再將實時測量值與預先構建好的海洋重力場背景圖進行匹配,通過重力匹配算法估計航行器的位置信息。 慣性/重力匹配導航系統基本工作原理如圖1 所示。 重力匹配算法是慣性/重力匹配導航的核心技術, 由目前發展更為成熟的地形匹配算法發展而來, 主要分為單點迭代和序列迭代算法。 單點迭代算法以桑迪亞慣性地形輔助導航（Sandia Inertial Terrain Aided Navigation, SITAN）算法為代表, SITAN 算法借助卡爾曼濾波技術可以進行實時重力匹配。 序列迭代算法主要包括地形輪廓匹配（Terrain Contour Matching, TERCOM）算法和最近等值線迭代（Iterated Closest Contour Point, ICCP）算法, 對達到一定長度的觀測采樣序列進行匹配。 相比于TERCOM算法, ICCP 算法對初始誤差和重力觀測誤差較為敏感, 對重力背景圖的分辨率依賴性更強［6-7］。TERCOM 算法是目前應用最廣泛的算法, 但只利用平移變換修正導航誤差, 精度和穩定性較差［8］。

圖1 慣性/重力匹配導航系統基本工作原理Fig.1 Basic working principle of INS/gravity matching navigation system

為了提高TERCOM 算法的精度, 針對TERCOM 算法僅依靠平移不能完全修正INS 產生誤差的問題, Han 等［9］提出了一種基于最短 路 徑的TERCOM 算法, 在進行相關分析時, 額外引入了線段的角度和距離作為最短路徑權重計算的參數,減小了較大的航向誤差。 趙建虎等［10］提出了一種基于自適應旋轉變換的TERCOM 算法, 在TERCOM 算法的基礎上增加了旋轉變換并能夠自適應地確定最佳旋轉角。 魏二虎等［11］提出了一種帶有旋轉和尺度變換功能的改進TERCOM 算法, 提高了定位精度。 蔡體菁等［12］提出了一種兩步式改進TERCOM 算法, 首先使用TERCOM 算法進行剛性變換粗匹配, 然后進行精匹配, 在粗匹配軌跡附近篩選重力等值點組成精匹配重力序列, 利用貪心算法解算出最佳匹配軌跡, 既提高了匹配精度也保證了匹配效率。

為了更好解決TERCOM 算法僅依靠平移修正位置誤差、 精度和穩定性較低的缺陷, 本文設計了一種基于仿射變換的兩步式TERCOM 算法, 意圖在提高匹配精度和穩定性的同時縮短匹配時間。考慮對待匹配軌跡實施仿射變換尋找最佳匹配軌跡, 給出了仿射變換模型及仿射參數的確定方法,并介紹了實施改進算法的具體流程。 通過仿真實驗驗證, 改進的TERCOM 算法精度和穩定性明顯優于傳統TERCOM 算法。

1 TERCOM 算法的工作原理及分析

由于地球重力場具有連續、 隨機、 等值（多個地理點的重力異常值相等）的特性, 基于單個重力異常觀測量無法唯一確定水下航行器的位置, 因而要求重力異常采樣序列達到一定長度再進行匹配。 TERCOM 算法的實施過程是在一定搜索范圍內按等間隔平移慣導指示軌跡, 并按照平移后的位置在重力基準圖中提取對應的重力異常, 生成待匹配軌跡的重力序列, 遍歷搜索范圍內的格網,得到多組待匹配軌跡的重力序列, 通過相關分析算法計算選取滿足最優化準則的待匹配重力序列,將該最優序列的坐標軌跡當做載體真實軌跡, 慣導指示軌跡與該最優序列的平移量即為慣性導航的修正量［13-15］, 如圖2 所示。 搜索范圍可設為正方形, 由于慣導誤差有累積特性, 搜索范圍邊長由重力異常觀測序列中最后一點的漂移范圍a確定,慣導指示軌跡在（INSx±a,INSy±a）范圍內平移,形成一個以慣導指示軌跡為中心軸邊長為2a的菱形柱體, 如圖3 所示。 常用的相關分析算法有三種, 包括交叉相關（Cross Correlation, COR） 算法、平均絕對差相關（Mean Absolute Deviation, MAD）算法以及平均平方差相關（Mean Square Deviation,MSD）算法, 假定在時間序列ti、ti+1、 …、ti+N時刻共采樣得到N個重力異常觀測量, 記為, 平移后得到的第j條待匹配軌跡的重力序列為, 則三種算法的計算模型如下所示:

圖2 TERCOM 算法的工作原理示意圖Fig.2 Working principle of TERCOM algorithm

圖3 搜索范圍示意圖Fig.3 Diagram of search scope

交叉相關算法

平均絕對差相關算法

平均平方差相關算法

最優化的匹配準則就是選取滿足JCOR取最大值或者JMAD、JMSD取最小值的待匹配重力序列, 文中的重力匹配使用的相關分析算法均為MSD 算法。

傳統TERCOM 算法通過平移慣導軌跡在重力基準圖中搜索最佳匹配序列, 原理簡單, 對初始誤差不敏感, 應用最為廣泛。 但實際應用中, 慣導在不同方向上具有不同的誤差特性, 即使是同一方向的誤差, 因為各種因素的影響, 也在不斷的變化［11］, 僅依靠平移修正軌跡并不能完全消除位置誤差, 必然導致匹配結果的精度與穩定性均比較差［10］。

2 基于自適應仿射修正的改進TERCOM 算法

根據慣性器件誤差特性, 陀螺和加速度計的常值漂移、 隨機游走以及零偏會激勵導航定位參數產生舒勒和地球兩種周期振蕩, 并產生速度、位置和航向上的常值誤差分量, 且在經度上產生隨時間增長積累的誤差, 所以考慮慣性導航會產生的三種誤差: 航向誤差、 速度誤差和位置誤差。

航向誤差使慣性導航指示軌跡與真實軌跡之間存在旋轉變換關系, 速度誤差使慣性導航指示軌跡與真實軌跡之間存在伸縮變換關系, 位置誤差則對應著平移變換關系。 由上述分析可以看出,慣性導航指示軌跡與真實軌跡之間的關系可以利用仿射變換描述, 仿射關系如圖4 所示, 曲線Ri為慣性導航指示軌跡, 曲線Rt為真實軌跡, 曲線R′i與Ri平行, 曲線R′i旋轉θ角度得到R″i, 同Rt航向一致。

圖4 仿射變換示意圖Fig.4 Schematic diagram of affine transformation

建立慣性導航指示軌跡與真實軌跡之間的仿射變換模型

式（4）中,xt、yt分別為載體的真實經度和緯度坐標位置;k1、k2分別為經度和緯度方向上伸縮變換的伸縮系數;θ為旋轉變換的旋轉角度參數;x、y分別為慣性導航指示軌跡的經度和緯度坐標位置;x0、y0分別為重力匹配導航開始時慣性導航指示軌跡的經緯度坐標, 即重力匹配導航初始坐標; ΔX、 ΔY為平移變換參數, 由載體進行重力匹配導航前累積的初始位置誤差δx、δy和重力匹配導航過程中累計的慣導位置誤差Δx、 Δy組成

伸縮系數、 旋轉系數的確定方式如圖5 所示,P1為重力匹配導航第一個匹配點的慣性指示坐標,Pn為重力匹配導航最后一個匹配點的慣性指示坐標。

圖5 仿射變換參數確定方式示意圖Fig.5 Schematic diagram of affine transformation parameters determination method

圖5 中標識出的角度存在以下關系

式（6）中, Δφ、 Δλ為行駛到Pn時累積的緯度誤差和經度誤差, 一般通過慣性器件的參數進行估算, 數值與式（5）中的ΔX、 ΔY相同, 旋轉系數。 將Δφ、 Δλ造成的位置誤差投影到P1Pn延長線上得到ΔL, 即

由ΔL可以確定伸縮系數k1、k2的范圍, 一般來說, 慣性器件的漂移會造成匹配點之間的間距變大, 所以伸縮系數的上限設為1。

本文提出的改進TERCOM 算法分為粗匹配和精匹配兩部分: 首先估計載體在經度和緯度方向上的位置漂移, 利用傳統TERCOM 算法在漂移范圍內尋找到一條粗匹配軌跡, 精匹配在粗匹配軌跡附近的小范圍區域內進行, 即縮小平移變換參數取值范圍, 以此減小精匹配的搜索范圍, 減少精匹配時間; 精匹配過程是一個循環迭代的過程, 循環包括自適應仿射參數修正及重力匹配兩個步驟, 每次循環的結果相互比較、 印證, 以此逐步縮小重力序列搜索范圍, 得到準確且穩定匹配結果的同時, 減少多次匹配造成的計算量增加。 算法流程如圖6 所示, 具體實施步驟如下:

圖6 改進算法流程圖Fig.6 Flowchart of improved algorithm

1）粗匹配: 根據式（5）計算位置誤差ΔX、 ΔY,根據位置誤差設置重力序列搜索范圍, 依據MSD匹配準則, 利用傳統TERCOM 算法在搜索范圍內尋找到一條粗匹配軌跡L1。

2）仿射參數初始設置: 精匹配以粗匹配軌跡L1為中心, 按照式（4）給出的仿射模型進行仿射變換, 在搜索范圍內按照搜索間隔提取待匹配重力序列: 利用式（6）計算旋轉系數, 搜索間隔設為; 利用式（8）計算伸縮系數k1,k2∈, 搜索間隔設為, 因為伸縮系數范圍通常很小, 所以搜索間隔設置大一些, 對于匹配精度影響不大; 由于經度誤差具有累計特性, 數值較大, 精匹配時, 經度方向平移變換參數的初始范圍設為, 搜索間隔為, 緯度方向平移變換參數的初始范圍設為, 搜索間隔為, 可取ΔXJ=2ΔYJ。

3）重力匹配: 依據MSD 匹配準則, 對步驟2提取出的待匹配重力序列進行計算, 得到一條最優軌跡作為精匹配軌跡L2。 計算L1和L2的平均距離誤差, 公式如下

式（9）中,ed為平均距離誤差,di為L1中第i點與L2中對應點之間的距離,n為重力序列中采樣點總數。 同時計算L1和L2的平均經度誤差edx和平均緯度誤差edy, 單位均為m。

4）自適應仿射參數修正: 當ed＞300 時, 說明粗匹配結果不穩定, 需重復進行精匹配過程直到結果穩定。 重復精匹配過程中, 需重新確定仿射變換系數:, 搜索間隔設為,θ′為上一次匹配所得軌跡對應的旋轉系數; 伸縮系數計算公式更改為k1∈［k′1-kr,k′1+kr］,k2∈［k′2-kr,k′2+kr］,k′1、k′2為上一次匹配所得軌跡的伸縮系數,kr為設定的搜索范圍, 搜索間隔為, 每次循環kr都減小為上次循環的一半; 同時, 若edx＜500, 經度方向平移變換參數ΔXJ減小為原先的一半, 若edy＜300, 緯度方向平移變換參數ΔYJ減小為原先的一半, 搜索間隔均不改變。

5）再次重力匹配: 將上一次循環得到的精匹配軌跡L2重新記為L1, 再次進行步驟3, 得到一條新的精匹配軌跡L2, 并計算L1和L2的平均距離誤差ed、 平均經度誤差edx和平均緯度誤差edy, 若ed＞300, 則轉到步驟4。

6）迭代終止條件: 重復進行重力匹配及仿射參數修正過程, 直到ed≤300, 循環結束, 最后一次匹配結果即為算法最終匹配結果。

3 仿真實驗分析

3.1 仿真條件

重力基準圖由圣地亞哥大學發布的衛星測高全球重力異常數據制作, 實驗所選取的導航區域為: 北緯4.7° ～6.4°, 東經120° ～120.5°, 重力異常單位為mGal, 分辨率為1′ ×1′, 重力基準圖如圖7 所示。

圖7 仿真實驗用重力基準圖Fig.7 Gravity reference diagram for simulation experiment

仿真所用的真實軌跡由慣導軌跡生成器解算,慣導軌跡生成器的作用為設置陀螺儀和加速度計的常值零偏、 隨機游走值和采樣頻率以及載體的航向角和航行速度, 輸出陀螺儀和加速度計的對應角增量和速度增量, 再利用慣性導航原理計算仿真軌跡。 本實驗設定陀螺儀的常值零偏為0.01（°） /h,隨機游走為0.005（°） /h1/2; 加速度計零偏為5μg, 隨機游走為2.5μg/Hz1/2。 設載體行駛3h 后, 開始進行重力匹配導航。 仿真得到的慣導軌跡與真實軌跡誤差如圖8 所示, 表1 給出了慣導軌跡誤差的具體數值。

圖8 慣導軌跡誤差圖Fig.8 Diagram of inertial navigation trajectory error

表1 慣導軌跡誤差數據Table 1 Data of inertial navigation trajectory error

本文在實驗區選擇了兩條航線: 一條載體初始位置為（5.2°, 120.2°）, 其航線位于重力信息較為豐富的適配區; 另一條載體初始位置為（5.6°,120.1°）, 其航線位于重力信息稀疏的非適配區。每條航線沿經線和緯線的航速均為5nmile/h（約2.6m/s）, 慣導軌跡初始航向誤差約為3°, 重力儀觀測值的仿真采用基準圖重力異常數據加入測量噪聲的方法。 現今重力儀的動態精度已經達到1mGal 量級, 因此測量噪聲取方差為1mGal 的白噪聲。 重力匹配時長為1h, 6min 采樣1 次, 每次匹配共取10 個采樣點。 根據所用慣導器件誤差特性,估算粗匹配開始時緯度漂移范圍為1.4nmile（約2593m）, 經度漂移范圍為3nmile（約5679m）。

3.2 適配區實驗結果

在（5.2°, 120.2°）位置附近區域, 重力信息較為豐富, 重力匹配導航的效果較好, 傳統TERCOM 算法也基本可以保證全程定位精度在千米以內, 平均定位精度小于800m。 由圖9（a）和圖9（b）對比可知, 改進TERCOM 算法的匹配效果明顯優于傳統算法; 由圖9（c）和圖9（d）對比可知, 改進算法X方向、Y方向和位置的平均誤差均降低到了傳統算法的一半, 其中位置平均誤差為355.5m。表2 對比了傳統TERCOM 算法和改進TERCOM 算法的匹配誤差, 可以看出改進算法的每一項均優于傳統算法。

表2 適配區兩種算法匹配位置誤差統計Table 2 Matching position error statistics of two algorithms in adaptation area

圖9 適配區匹配效果Fig.9 Matching results in adaptation area

3.3 非適配區實驗結果

非適配區利用TERCOM 算法進行粗匹配時存在匹配成功但匹配效果不佳和匹配失敗兩種情況。

（1）粗匹配成功

在（5.6°, 120.1°）位置處, 重力信息稀疏, 匹配效果不佳。 由圖10（a）可知, 傳統重力匹配軌跡和真實軌跡相差很遠; 由圖10（c）可知, 傳統算法的位置誤差全部在1500m 以上, 誤差最大點處甚至達到了3000m; 而由圖10（b）可知, 改進算法的匹配軌跡則明顯更加貼近真實軌跡; 由圖10（d）可知, 改進算法的定位精度均保持在千米以內, 計算得到匹配軌跡的平均位置精度小于800m,X方向、Y方向以及總位置的定位誤差約為傳統算法的1/4, 改進算法在傳統算法匹配不佳的條件下依然可以取得良好的匹配效果。 表3 列出了兩種算法的匹配結果數據。

圖10 非適配區粗匹配成功時的匹配效果Fig.10 Matching results when coarse matching succeeds in non-adaptive area

表3 非適配區粗匹配成功時兩種算法匹配位置誤差統計Table 3 Matching position error statistics of two algorithms when coarse matching succeeds in non-adaptive area

（2）粗匹配失敗

在（5.6°, 120.1°） 位置處重復進行匹配實驗,傳統TERCOM 算法會有小概率出現誤匹配情況。如圖11（a）所示, 匹配軌跡偏離真實軌跡的距離很遠, 位置誤差甚至遠超純慣性導航的誤差。 因為改進算法的粗匹配過程使用傳統TERCOM 算法,且精匹配過程是在粗匹配的基礎上進行的, 所以粗匹配的結果對于改進算法的匹配效果影響很大。由圖11（d）可知, 改進算法的匹配效果不佳, 多個匹配點的位置誤差超過了2000m。 但由圖11（a）和圖11（b）對比可知, 改進算法的匹配軌跡比慣性指示軌跡接近真實軌跡, 大大減少了匹配位置誤差,很大程度上提升了重力匹配算法的可靠性。 表4 列出了兩種算法的匹配結果數據。 雖然改進算法的匹配精度并不高, 平均誤差約在2000m 左右, 但粗匹配和精匹配后的匹配結果可以相互印證, 如果相差很大, 可以起到警示匹配有誤或不穩定的作用。 精匹配不斷縮小收縮范圍, 且設置迭代終止條件為與上一次精匹配結果位置誤差小于閾值,就是為了追求較為穩定的結果。 相比于傳統算法只對匹配軌跡進行一次搜索, 改進算法提高了其可靠性。

圖11 非適配區粗匹配失敗時的匹配效果Fig.11 Matching results when coarse matching fails in non-adaptive area

表4 非適配區粗匹配失敗時兩種算法匹配位置誤差統計Table 4 Matching position error statistics of two algorithms when coarse matching fails in non-adaptive area

4 結論

傳統TERCOM 算法僅依靠平移修正慣導軌跡,忽略了由于慣性器件的誤差特性對軌跡造成的旋轉變換和伸縮變換, 精度和穩定性較低。 針對此問題, 本文提出了一種基于仿射變換的兩步式改進TERCOM 重力匹配算法。 通過仿射變換修正慣導軌跡, 提高了匹配精度和穩定性; 通過粗匹配、精匹配結合減少待匹配軌跡數量, 保證了匹配速度。 仿真實驗表明: 在傳統TERCOM 算法的重力適配區域, 改進的TERCOM 算法可以明顯提高導航精度, 精度提升約50%; 在傳統TERCOM 算法匹配效果并不十分理想時, 改進的TERCOM 算法仍具有一定適應性, 能夠大幅提高導航精度, 使得重力匹配導航更具有穩定性; 在傳統TERCOM算法出現誤匹配、 失去導航能力時, 說明匹配結果不穩定, 會使得改進的TERCOM 算法的粗匹配、精匹配結果相差巨大, 起到對誤匹配情況的警示作用。