吳睿盈, 王 燕, 孫 勇

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

0 引言

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

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

圖1 慣性/重力匹配導(dǎo)航系統(tǒng)基本工作原理Fig.1 Basic working principle of INS/gravity matching navigation system

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

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

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

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

交叉相關(guān)算法

平均絕對差相關(guān)算法

平均平方差相關(guān)算法

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

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

2 基于自適應(yīng)仿射修正的改進TERCOM 算法

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

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

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

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

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

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

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

圖5 中標識出的角度存在以下關(guān)系

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

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

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

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

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

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

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

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

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

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

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

3 仿真實驗分析

3.1 仿真條件

重力基準圖由圣地亞哥大學(xué)發(fā)布的衛(wèi)星測高全球重力異常數(shù)據(jù)制作, 實驗所選取的導(dǎo)航區(qū)域為: 北緯4.7° ～6.4°, 東經(jīng)120° ～120.5°, 重力異常單位為mGal, 分辨率為1′ ×1′, 重力基準圖如圖7 所示。

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

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

圖8 慣導(dǎo)軌跡誤差圖Fig.8 Diagram of inertial navigation trajectory error

表1 慣導(dǎo)軌跡誤差數(shù)據(jù)Table 1 Data of inertial navigation trajectory error

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

3.2 適配區(qū)實驗結(jié)果

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

表2 適配區(qū)兩種算法匹配位置誤差統(tǒng)計Table 2 Matching position error statistics of two algorithms in adaptation area

圖9 適配區(qū)匹配效果Fig.9 Matching results in adaptation area

3.3 非適配區(qū)實驗結(jié)果

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

（1）粗匹配成功

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

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

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

（2）粗匹配失敗

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

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

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

4 結(jié)論

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