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

吳睿盈, 王 燕, 孫 勇

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

0 引言

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

慣性/重力匹配導(dǎo)航系統(tǒng)由慣性導(dǎo)航系統(tǒng)、 海洋重力場(chǎng)基準(zhǔn)圖、 重力測(cè)量系統(tǒng)和重力匹配導(dǎo)航系統(tǒng)四部分構(gòu)成, 由航行器自身搭載的重力儀實(shí)時(shí)測(cè)量所處位置的重力場(chǎng)特征信息, 再將實(shí)時(shí)測(cè)量值與預(yù)先構(gòu)建好的海洋重力場(chǎng)背景圖進(jìn)行匹配,通過(guò)重力匹配算法估計(jì)航行器的位置信息。 慣性/重力匹配導(dǎo)航系統(tǒng)基本工作原理如圖1 所示。 重力匹配算法是慣性/重力匹配導(dǎo)航的核心技術(shù), 由目前發(fā)展更為成熟的地形匹配算法發(fā)展而來(lái), 主要分為單點(diǎn)迭代和序列迭代算法。 單點(diǎn)迭代算法以桑迪亞慣性地形輔助導(dǎo)航（Sandia Inertial Terrain Aided Navigation, SITAN）算法為代表, SITAN 算法借助卡爾曼濾波技術(shù)可以進(jìn)行實(shí)時(shí)重力匹配。 序列迭代算法主要包括地形輪廓匹配（Terrain Contour Matching, TERCOM）算法和最近等值線迭代（Iterated Closest Contour Point, ICCP）算法, 對(duì)達(dá)到一定長(zhǎng)度的觀測(cè)采樣序列進(jìn)行匹配。 相比于TERCOM算法, ICCP 算法對(duì)初始誤差和重力觀測(cè)誤差較為敏感, 對(duì)重力背景圖的分辨率依賴性更強(qiáng)［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 算法的精度, 針對(duì)TERCOM 算法僅依靠平移不能完全修正INS 產(chǎn)生誤差的問(wèn)題, Han 等［9］提出了一種基于最短 路 徑的TERCOM 算法, 在進(jìn)行相關(guān)分析時(shí), 額外引入了線段的角度和距離作為最短路徑權(quán)重計(jì)算的參數(shù),減小了較大的航向誤差。 趙建虎等［10］提出了一種基于自適應(yīng)旋轉(zhuǎn)變換的TERCOM 算法, 在TERCOM 算法的基礎(chǔ)上增加了旋轉(zhuǎn)變換并能夠自適應(yīng)地確定最佳旋轉(zhuǎn)角。 魏二虎等［11］提出了一種帶有旋轉(zhuǎn)和尺度變換功能的改進(jìn)TERCOM 算法, 提高了定位精度。 蔡體菁等［12］提出了一種兩步式改進(jìn)TERCOM 算法, 首先使用TERCOM 算法進(jìn)行剛性變換粗匹配, 然后進(jìn)行精匹配, 在粗匹配軌跡附近篩選重力等值點(diǎn)組成精匹配重力序列, 利用貪心算法解算出最佳匹配軌跡, 既提高了匹配精度也保證了匹配效率。

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

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

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

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

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

交叉相關(guān)算法

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

圖6 改進(jìn)算法流程圖Fig.6 Flowchart of improved algorithm

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

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

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

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

4）自適應(yīng)仿射參數(shù)修正: 當(dāng)ed＞300 時(shí), 說(shuō)明粗匹配結(jié)果不穩(wěn)定, 需重復(fù)進(jìn)行精匹配過(guò)程直到結(jié)果穩(wěn)定。 重復(fù)精匹配過(guò)程中, 需重新確定仿射變換系數(shù):, 搜索間隔設(shè)為,θ′為上一次匹配所得軌跡對(duì)應(yīng)的旋轉(zhuǎn)系數(shù); 伸縮系數(shù)計(jì)算公式更改為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)的一半; 同時(shí), 若edx＜500, 經(jīng)度方向平移變換參數(shù)ΔXJ減小為原先的一半, 若edy＜300, 緯度方向平移變換參數(shù)ΔYJ減小為原先的一半, 搜索間隔均不改變。

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

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

3 仿真實(shí)驗(yàn)分析

3.1 仿真條件

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

圖7 仿真實(shí)驗(yàn)用重力基準(zhǔn)圖Fig.7 Gravity reference diagram for simulation experiment

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

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

3.2 適配區(qū)實(shí)驗(yàn)結(jié)果

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

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

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

3.3 非適配區(qū)實(shí)驗(yàn)結(jié)果

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

（1）粗匹配成功

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

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

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

（2）粗匹配失敗

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

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

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

4 結(jié)論

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