基于改進(jìn)TERCOM的地形輔助導(dǎo)航算法

王 丹，劉利強(qiáng)，奔粵陽，戴平安，王健成

（哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，哈爾濱 150001）

自主水下航行器（Autonomous Underwater Vehicle,AUV）是海洋資源開發(fā)和探測的重要工具。隨著技術(shù)的發(fā)展對AUV 自主性、隱蔽性和長時性的要求越來越高，但慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）誤差會隨著時間增加而發(fā)散，影響AUV 隱蔽性。為降低INS 位置誤差可通過地形輔助導(dǎo)航方法進(jìn)行修正[1]，地形輔助導(dǎo)航系統(tǒng)具有很好的隱蔽性和抗干擾能力，能很好地幫助INS 解決位置發(fā)散問題，匹配算法是實現(xiàn)該技術(shù)的關(guān)鍵。地形輪廓匹配（Terrain Contour Matching,TERCOM）算法是地形輔助導(dǎo)航算法中最經(jīng)典的算法之一，該算法是根據(jù)INS 提供航跡進(jìn)行平移得到最終匹配序列，對INS 提供的航跡在角度上有較高要求，具有易實現(xiàn)、實用性強(qiáng)、隱蔽性高、性能可靠等優(yōu)點，但實時性差且對水下航行器的機(jī)動性和INS 航向誤差較敏感[2]。

許多學(xué)者對TERCOM 算法進(jìn)行了研究，韓等人[3]采用最短路徑算法改進(jìn)TERCOM 算法，將基于受限空間順序約束算法的誤匹配判斷方法與TERCOM 算法結(jié)合，提高了算法的實時性和定位精度。程等人[4]提出采用兩階段聯(lián)合算法，提高TERCOM 的匹配精度，但匹配時間變長。為提高TERCOM 算法的性能，趙等人[5]和王等人[6]提出通過降低航向角度誤差提高TERCOM 算法的匹配精度，但匹配速度較慢。袁等人[7]提出將TERCOM 與ICCP 算法聯(lián)合使用，利用不同算法的性能差異實現(xiàn)互補(bǔ)，改善了TERCOM 算法的航向問題，提高了匹配精度，但由于兩種算法都屬于批處理算法，匹配時間變長。因粒子群算法具有結(jié)構(gòu)簡單、運(yùn)行速度快等優(yōu)點，許等人[8]提出采用粒子群優(yōu)化（Particle Swarm Optimization,PSO）算法提高地磁匹配成功率。季等人[9]提出基于模擬退火優(yōu)化PSO（Particle Swarm Optimization based on Simulated Annealing,SAPSO）算法提高匹配精度。

TERCOM 算法可與PSO 算法結(jié)合來提高地形匹配效果。本文提出在傳統(tǒng)TERCOM 算法基礎(chǔ)上增加旋轉(zhuǎn)角度機(jī)制，首先依據(jù)INS 航跡進(jìn)行兩次旋轉(zhuǎn)平移并采用TERCOM 算法進(jìn)行粗匹配來降低航向誤差，其次采用改進(jìn)PSO 算法進(jìn)行精匹配，從而得到最優(yōu)匹配序列。匹配過程由粗到精，提高了傳統(tǒng)TERCOM算法的匹配效果。

1 TERCOM 算法

TERCOM 算法是序列相關(guān)匹配方法之一，匹配時需采集多個點數(shù)，匹配周期較長。TERCOM 算法工作原理為：首先以INS 位置為中心，按一定搜索范圍將數(shù)據(jù)格網(wǎng)化，其次遍歷搜索范圍內(nèi)每個格網(wǎng)，得到多組與INS 航跡平行的序列，最后將每組序列中各點深度值與實測深度值進(jìn)行相關(guān)分析[10]。通常采用均方差算法（Mean Square Difference,MSD）作為匹配方法的判定標(biāo)準(zhǔn)[11]：

式中，n為實測匹配序列長度；HA(i)為實測第i個采樣點水深值(i= 1,2…n)；HS為數(shù)字地圖指示水深值；(x,y)為慣導(dǎo)參考點位置；(τx,τy)為航行器在數(shù)字地圖兩個坐標(biāo)軸方向的恒定距離分量。JMSD取值越小，匹配精度越高。

根據(jù)TERCOM 算法的原理分析，尋找最優(yōu)匹配序列的核心是INS 提供的航跡位置和航向誤差。如圖1所示，當(dāng)INS 沒有航向偏差時TERCOM 算法的匹配航跡是與INS 航跡相平行的一組高程剖面。TERCOM算法在實際應(yīng)用中隱含了一個條件，即假設(shè)匹配時INS 的航向偏差為0°或很小的角度（＜0.5°）。但在實際航行中INS 的航向偏差不可能為0°，當(dāng)航向偏差較大時（＞0.5°），INS 航跡偏離了真實航跡，與INS 航跡平行的最優(yōu)匹配序列也偏離了真實航跡。為更好地接近真實航跡，本文提出在傳統(tǒng)TERCOM 算法基礎(chǔ)上增加旋轉(zhuǎn)角度機(jī)制，降低航向偏差，提高TERCOM算法的匹配效果。

圖1 TERCOM 算法匹配示意圖Fig.1 TERCOM algorithm matching diagram

2 改進(jìn)粒子群算法

PSO 算法是一種模擬鳥群覓食過程的典型群智能優(yōu)化算法，在PSO 算法中粒子位置是根據(jù)自身經(jīng)驗和整個群體經(jīng)驗進(jìn)行更新。算法簡單速度快，但易陷入局部最優(yōu)。在D維解空間中，初始種群數(shù)量為N，用表示第i個粒子位置，表示第i個粒子在第j維空間的位置；表示粒子搜索個體最優(yōu)解，表示第i個粒子在第j維空間的個體最優(yōu)解；表示整個種群搜索的全局最優(yōu)解，表示在第i維空間的全局最優(yōu)解。標(biāo)準(zhǔn)粒子群速度和位置更新公式如下[12-14]：

式中，ω為慣性權(quán)重；為第i個粒子在t時刻的飛行速度，i= 1,2…N；t為當(dāng)前迭代次數(shù)；為第i個粒子在t時刻的位置；和為學(xué)習(xí)因子；和為在[0,1]間隨機(jī)數(shù)。

PSO 算法的速度由三部分組成，分別對應(yīng)式(2)的慣性部分、認(rèn)知部分和社會部分。認(rèn)知和社會部分表示粒子種群中的信息交換,使粒子能夠通過優(yōu)良信息不斷修正前進(jìn)的速度和方向，進(jìn)而使算法找到最優(yōu)解。慣性部分可擴(kuò)大搜索范圍，提高全局搜索能力，從而使粒子跳出局部最優(yōu)。其中慣性權(quán)重ω具備調(diào)節(jié)PSO 算法全局與局部尋優(yōu)能力，在迭代初期使全局搜索能力變強(qiáng)，不斷搜索未知區(qū)域，在迭代后期局部開發(fā)能力變強(qiáng)，在最優(yōu)解范圍內(nèi)進(jìn)行開發(fā)，所以參數(shù)ω隨收斂過程逐漸減小[15-19]。慣性部分對算法在地形匹配的影響較大。

參數(shù)ω的選擇直接影響粒子群算法的性能，本文基于ω參數(shù)的特點設(shè)計四種不同模型。模型1 將ω設(shè)為線性形式，如式(4)所示，ω在迭代初期具有最大值，全局搜索能力較強(qiáng)，但是在迭代后期ω變化較快，說明在迭代后期尋找局部最優(yōu)能力較差；為彌補(bǔ)線性形式的不足，模型2 將ω設(shè)為指數(shù)形式，如式(5)所示，ω值緩慢降低提高了迭代后期局部搜索能力，但與線性形式相比全局搜索能力稍有降低；為提高迭代前期的全局搜索能力，在模型2 基礎(chǔ)上增加一個參數(shù)m構(gòu)造成模型3，如式(6)所示，其中m是正整數(shù)，τ是小于等于1 的數(shù)，τ的m次冪可有效提高ω的值，從而提高算法的全局尋優(yōu)能力。但在PSO 算法中需要更好地協(xié)調(diào)全局尋優(yōu)能力和局部尋優(yōu)能力，所以在模型3基礎(chǔ)上增加一個正弦函數(shù)構(gòu)造成模型4，如式(7)所示，正弦函數(shù)是一個周期函數(shù)，能夠更好地協(xié)調(diào)全局搜索和局部搜索，而且因正弦函數(shù)圖像的特點，可以很好地保持模型4 的優(yōu)勢。

式中，ωmax為慣性權(quán)重最大值；τ=t/T，t為當(dāng)前迭代次數(shù)，T為最大迭代次數(shù)；m為正整數(shù)；λ和k為非負(fù)整數(shù)。

為驗證ω參數(shù)四種模型的特點，設(shè)當(dāng)ωmax=1、λ=10、m=3、k=6、T=3000 次時，四種模型曲線如圖2所示。模型1 線性形式的收斂曲線在迭代前期具有很高的ω值，說明迭代前期全局搜索能力強(qiáng)，但在迭代后期下降很快，導(dǎo)致算法在迭代后期局部搜索能力變差；模型2 指數(shù)形式與模型1 相比，在迭代后期ω值變化緩慢，說明局部搜索能力變強(qiáng)，但在迭代初期ω值較小，說明全局搜索能力相對減弱；與模型2 相比，模型3 不僅保留了局部尋優(yōu)的能力，同時增加的m參數(shù)提高了全局搜索能力；模型4 中ω值呈周期性變化，在迭代初期很快取得最大ω值，若能在迭代初期找到全局最優(yōu)解，可更快進(jìn)行收斂，且在迭代后期ω值逐漸變小，說明局部搜索能力較強(qiáng)。因τ是離散值，且(k+0.5)(1 -τ)不是純整數(shù)，所以模型4 曲線是無限趨近與0。對比其他三種模型，模型4 能更好地協(xié)調(diào)全局性和局部性，應(yīng)用到地形匹配中可在全局搜索和局部搜索中不斷尋找最優(yōu)匹配序列，最終找到最優(yōu)解，并且模型4 是周期函數(shù)，若在前期找到匹配最優(yōu)解即可進(jìn)行收斂，提高了匹配速度，縮短了匹配時間。

圖2 四種模型慣性權(quán)重曲線圖Fig.2 Inertial weight curves of the four models

將模型4 的慣性權(quán)重改變形式引入到式(2)的標(biāo)準(zhǔn)PSO 算法中，改進(jìn)后的PSO 算法記作改進(jìn)慣性權(quán)重粒子群算法（Weight Particle Swarm Optimization,WPSO），WPSO 流程圖如圖3所示。

圖3 WPSO 算法流程圖Fig.3 Flow chart of WPSO algorithm

3 增加旋轉(zhuǎn)角度機(jī)制TERCOM 算法

增加旋轉(zhuǎn)角度機(jī)制TERCOM 算法的設(shè)計思路是先通過TERCOM 算法粗匹配后，用WPSO 算法進(jìn)行精匹配。首先根據(jù)INS 提供的航跡，確定整個匹配航跡的重心坐標(biāo)：

式中，(xi,yi)為INS 提供的航跡位置坐標(biāo)；D為實測深度值序列與INS 航跡序列間距離，采用馬氏距離進(jìn)行計算，馬氏距離可有效降低噪聲的影響[20]；為對應(yīng)點在數(shù)字地圖中的數(shù)值與實際測量深度值的差；n為匹配序列長度；C為協(xié)方差矩陣；為第i個實測采樣點水深值；為第i個點在數(shù)字地圖中的數(shù)值；為平均值。

其次以匹配序列重心為原點，對INS 提供航跡增加旋轉(zhuǎn)角度公式如下：

式中，(xp,yp)為增加旋轉(zhuǎn)角α后的航跡坐標(biāo)；f(α)為由α組成的轉(zhuǎn)向矩陣。

最后將[xp,yp]作為初始匹配序列進(jìn)行TERCOM匹配，實現(xiàn)匹配定位。

增加旋轉(zhuǎn)角度機(jī)制TERCOM 算法過程為：

（1）根據(jù)INS 的航跡位置，確定航跡的重心坐標(biāo)(xh,yh)。

（2）初始匹配時，旋轉(zhuǎn)角α從0°開始，以s=0.2°為步長對原始INS 航跡進(jìn)行旋轉(zhuǎn)。每旋轉(zhuǎn)一次，遍歷搜索區(qū)內(nèi)每一個格網(wǎng)，依據(jù)旋轉(zhuǎn)后的航跡平移并進(jìn)行傳統(tǒng)TERCOM 匹配，直到旋轉(zhuǎn)角大于2 倍INS 角度誤差時停止旋轉(zhuǎn)，k表示2 倍INS 角度誤差。

（3）根據(jù)MSD 準(zhǔn)則，通過TERCOM 匹配后，找到最小MSD 值及對應(yīng)的粗旋轉(zhuǎn)最優(yōu)角αi。

（4）基于粗旋轉(zhuǎn)結(jié)果，以αi為中心，在[αi-2s,αi+2s]范圍內(nèi)以s/10為步長進(jìn)行精旋轉(zhuǎn)，并采用TERCOM 平移匹配，重復(fù)步驟（3）。當(dāng)相鄰兩次匹配結(jié)果差值滿足限差時（限差為50 m），則停止迭代，從而得到精旋轉(zhuǎn)角度αj及兩次旋轉(zhuǎn)后的匹配序列Pj。

（5）在兩次旋轉(zhuǎn)后的匹配序列Pj基礎(chǔ)上采用WPSO 算法進(jìn)行精匹配。精匹配的過程：①初始化粒子群各參數(shù)；②評價每個粒子適應(yīng)度值，適應(yīng)度函數(shù)采用MSD 算法；③更新Pbest和Gbest；④判定是否找到全局最優(yōu)解；⑤判斷是否滿足終止條件。

（6）當(dāng)滿足終止條件時，停止迭代，得到最優(yōu)匹配序列。若不滿足終止條件返回步驟（5）。終止條件是WPSO 算法達(dá)到最大迭代次數(shù)。

增加旋轉(zhuǎn)角度機(jī)制TERCOM 算法的匹配流程圖如圖4所示。

圖4 增加旋轉(zhuǎn)角度機(jī)制TERCOM 算法流程圖Fig.4 Flow chart of adding rotation angle mechanism TERCOM algorithm

增加旋轉(zhuǎn)角度機(jī)制TERCOM 算法可更好地向真實航跡靠攏。由粗旋轉(zhuǎn)到精旋轉(zhuǎn)的過程可有效降低航向偏差，采用WPSO 算法進(jìn)一步優(yōu)化旋轉(zhuǎn)后的匹配序列，從而提高匹配精度。增加旋轉(zhuǎn)角度機(jī)制的TERCOM 算法記作TERCOM-A。

4 仿真及分析

海底深度值位于東經(jīng)122.0568 °～122.1263 °、北緯37.5704 °～37.6078 °，插值后數(shù)據(jù)有1391×750 個網(wǎng)格點。多波束測深系統(tǒng)觀測誤差為5 m，慣性導(dǎo)航系統(tǒng)陀螺漂移為0.01 °/h，加速度計常值偏置0.01 mg。水下航行器采樣間隔1 s，采樣周期30 s，以8 kn 速度勻速航行，速度誤差為 0.1 m/s，水深測量范圍-65 m-0 m。通過設(shè)置初始位置誤差作為水下航行器進(jìn)入匹配區(qū)域時INS 已累積的位置誤差。

仿真實驗中，WPSO算法參數(shù)設(shè)置：粒子數(shù)為200，最大迭代次數(shù)為3000 次，學(xué)習(xí)因子c1=c2=1.35，慣性權(quán)重ω在(0,1]隨著迭代次數(shù)的增加而逐漸遞減。

為驗證算法的有效性，分別在2 次不同初始誤差情況下，比較基于 MSD 傳統(tǒng) TERCOM 算法和TERCOM-A 算法的匹配效果。為保證數(shù)據(jù)準(zhǔn)確性，算法進(jìn)行30 次后求平均值。當(dāng)AUV 在水下做轉(zhuǎn)彎運(yùn)動時，水下航行器初始位置均在（122.086°E,37.577°N），初始航向均為北偏東150 °，初始航向誤差均設(shè)置為3 °，匹配序列均為14，搜索范圍是以INS 指示位置為中心，以6σ為邊長的矩形區(qū)域。其中，σ為INS 位置誤差的標(biāo)準(zhǔn)差，INS 在東向和北向誤差的標(biāo)準(zhǔn)差均為30 m。地形熵為0.267，水深標(biāo)準(zhǔn)差為7.73 m。當(dāng)INS初始位置誤差較小時，仿真設(shè)置為經(jīng)度誤差0.3′，緯度誤差0.3′；當(dāng)INS 初始位置誤差較大時，仿真設(shè)置為經(jīng)度誤差1′，緯度誤差1′。兩種匹配算法的軌跡如圖5-6所示。

圖5 INS 初始位置誤差較小時兩種算法匹配圖Fig.5 Matching graph of two algorithms under the condition of INS small initial position errors

從圖5 和圖6 可以看出，TERCOM-A 算法的匹配精度明顯優(yōu)于TERCOM 算法。為進(jìn)一步對比兩種算法的匹配性能，分別針對上述兩種情況，計算匹配航跡上各水深測量點位置的匹配時間、匹配誤差、匹配誤差標(biāo)準(zhǔn)差、經(jīng)緯度誤差和航向誤差，兩種匹配算法的地形匹配結(jié)果如表1所示。

圖6 INS 初始位置誤差較大時兩種算法匹配圖Fig.6 Matching graph of two algorithms under the condition of INS large initial position errors

表1 兩種匹配算法的地形匹配結(jié)果Tab.1 Terrain matching results of two matching algorithms

從表1 可知，當(dāng)初始位置誤差較小時，與傳統(tǒng)TERCOM 算法相比，TERCOM-A 算法的匹配誤差可降低4 倍，經(jīng)度誤差可降低近5 倍，緯度誤差可降低3 倍多，航向誤差降低了89.3%；當(dāng)初始位置誤差較大時，與傳統(tǒng) TERCOM 算法相比，TERCOM-A 算法的匹配誤差可降低6 倍多，經(jīng)度誤差可降低10 倍多，緯度誤差可降低近2 倍，航向誤差降低了84%。且在兩種不同狀態(tài)下，TERCOM-A算法的匹配誤差標(biāo)準(zhǔn)差較小，說明匹配誤差變化小且穩(wěn)定。

改進(jìn)后的TERCOM 算法的匹配精度明顯優(yōu)于傳統(tǒng)TERCOM算法，主要是因為改進(jìn)后的TERCOM算法，經(jīng)過由粗匹配到精匹配的過程，有效降低了INS 航跡與真實航跡的航向角度偏差，提高了傳統(tǒng)TERCOM 算法的匹配精度。同時在匹配時間方面，增加旋轉(zhuǎn)角度機(jī)制的TERCOM 算法并沒有增加太多時間，也充分說明了在精匹配時采用改進(jìn)粒子群算法在快速尋優(yōu)方面的優(yōu)勢。因此改進(jìn)的TERCOM算法有效解決了傳統(tǒng)TERCOM 算法的航向敏感問題，提高了匹配效果。

4 結(jié)論

增加旋轉(zhuǎn)角度機(jī)制TERCOM 算法實現(xiàn)了由粗匹配到精匹配的過程，匹配精度更高，更好地抑制航向誤差，限制累積誤差增長，從而提高了匹配效果。同時改進(jìn)的PSO 算法可有效提高PSO 的全局尋優(yōu)能力，經(jīng)WPSO 精匹配后的TERCOM 算法可實現(xiàn)快速、精準(zhǔn)定位，提高匹配效率。改進(jìn)的TERCOM算法可抑制INS 位置誤差發(fā)散，提高AUV 長時水下自主航行隱蔽性，具有一定工程應(yīng)用價值。