程向紅 ，周月華

（1.微慣性儀表與先進(jìn)導(dǎo)航技術(shù)教育部重點(diǎn)實(shí)驗(yàn)室，南京210096；2.東南大學(xué)儀器科學(xué)與工程學(xué)院，南京210096）

在海洋應(yīng)用中大多數(shù)水下航行器依賴于慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）實(shí)現(xiàn)精確導(dǎo)航，但是慣性導(dǎo)航誤差隨時(shí)間累積，而且GPS在水下不能直接使用，地形輔助導(dǎo)航（Terrain Aided Navigation,TAN）技術(shù)以較高的隱蔽性、自主性和全天候性等特點(diǎn)被水下航行器廣泛應(yīng)用[1-3]。航行器在運(yùn)動(dòng)過程中利用水下傳感器實(shí)時(shí)獲得地形數(shù)據(jù)，根據(jù)相關(guān)匹配算法將實(shí)時(shí)圖與預(yù)先存儲(chǔ)的基準(zhǔn)圖進(jìn)行匹配計(jì)算，以抑制INS的誤差積累實(shí)現(xiàn)準(zhǔn)確導(dǎo)航。因此地形匹配算法是地形輔助導(dǎo)航的關(guān)鍵技術(shù)之一。

典型的地形匹配算法主要有三種：地形輪廓匹配（Terrain Contour Matching,TERCOM）算法、桑迪亞慣性地形輔助導(dǎo)航（Sandia Inertial TAN，SITAN）算法、最近等值線迭代（Iterative Closest Contour Point,ICCP）算法[4]。SITAN算法是一種遞推濾波方法，具有較高實(shí)時(shí)性，但存在濾波發(fā)散的問題，其應(yīng)用受到限制[5-7]；ICCP算法作為經(jīng)典的特征匹配方法之一，通過反復(fù)的剛性變換實(shí)現(xiàn)匹配定位，然而實(shí)時(shí)性較差[5,8-9]；TERCOM算法作為相關(guān)匹配算法的代表首先被水下航行器應(yīng)用，其操作簡單，實(shí)用性強(qiáng)，但計(jì)算復(fù)雜度高，輸出周期長[5,10]。

由于水下地形具有一些特殊性和復(fù)雜性，將傳統(tǒng)的地形匹配理論和算法直接應(yīng)用在地形輔助導(dǎo)航中容易出現(xiàn)定位精度大幅度降低和算法不穩(wěn)定等問題。地形匹配問題本質(zhì)上也屬于優(yōu)化問題，隨著智能計(jì)算的發(fā)展，粒子群優(yōu)化算法（Particle Swarm Optimization,PSO）作為智能群優(yōu)化方法的典型代表，應(yīng)用于海洋實(shí)測地磁場數(shù)據(jù)匹配中[11-12]，或優(yōu)化估計(jì)航路的改進(jìn)ICCP算法[11,13]。本文提出基于平均Hausdorff距離和改進(jìn)PSO的地形匹配算法，充分利用粒子群優(yōu)化算法的快速尋優(yōu)特性，克服傳統(tǒng)TERCOM算法計(jì)算量大、定位精度差的問題。

1 TERCOM匹配算法

地形輪廓匹配方法是一種批處理數(shù)據(jù)、定期匹配位置的方法[3]。TERCOM的基本工作原理為在存儲(chǔ)地形圖中找出一系列路徑，這些路徑平行于捷聯(lián)慣性導(dǎo)航系統(tǒng)（Strapdown INS,SINS）指示的路徑，接著根據(jù)一種性能指標(biāo)算法，檢驗(yàn)從地圖中提取的各地形剖面與實(shí)測地形剖面的相關(guān)程度，從中選取相關(guān)程度最高的一條作為最佳匹配剖面[10]。性能指標(biāo)通常采用均方差（Mean Square Difference,MSD）算法，定義為：

式中：L表示測量序列的長度（采樣點(diǎn)個(gè)數(shù)為偶數(shù)）；HA(i)為第i個(gè)采樣點(diǎn)的水深采樣值(i=1,2,…,L)；HS為數(shù)字地圖指示水深值；(x,y)為匹配基準(zhǔn)點(diǎn)位置；若水下航行器具有恒定速度，(τx,τy)為航行器在數(shù)字地圖兩個(gè)坐標(biāo)軸方向上的恒定距離分量。TERCOM匹配的過程就是通過相關(guān)計(jì)算使JMSD取最小值的過程，其算法流程描述如下：

步驟1：采樣。采樣L個(gè)點(diǎn)的純慣導(dǎo)指示位置坐標(biāo)、實(shí)時(shí)測量水深數(shù)據(jù)和數(shù)字地圖指示水深數(shù)據(jù)。

步驟 2：生成實(shí)時(shí)圖。根據(jù)慣導(dǎo)給出的位置坐標(biāo)和相應(yīng)的水深測量值生成實(shí)時(shí)圖。

步驟 3：生成基準(zhǔn)圖。根據(jù)實(shí)時(shí)圖、搜索位置和慣導(dǎo)導(dǎo)航誤差的標(biāo)準(zhǔn)差由數(shù)字地圖生成基準(zhǔn)圖。

步驟 4：計(jì)算相關(guān)值。通過計(jì)算每個(gè)基準(zhǔn)子圖與實(shí)時(shí)圖的JMSD，得出JMSD最小值。

步驟5：定位。根據(jù)JMSD最小值求得(x, y)，再根據(jù)數(shù)字地圖確定TERCOM位置。

2 改進(jìn)PSO算法

粒子群優(yōu)化算法是由Kennedy和Eberhart于1995年提出的一種智能優(yōu)化算法[14]。PSO的基本思想是：每個(gè)優(yōu)化問題的潛在解對應(yīng)于搜索空間中的“粒子”或“個(gè)體”，每個(gè)粒子都有自己的位置和速度，還有1個(gè)由被優(yōu)化函數(shù)決定的適應(yīng)值[12]。PSO初始化一群隨機(jī)粒子，各個(gè)粒子在解空間中記憶、追隨當(dāng)前的最優(yōu)粒子，通過迭代找到最優(yōu)解并跟蹤局部極值和全局極值更新自身[11-12]。

2.1 匹配搜索區(qū)域的確定

搜索區(qū)域較小時(shí)，將無法包含水下航行器的真實(shí)位置，容易導(dǎo)致匹配失??；搜索區(qū)域較大時(shí)，會(huì)導(dǎo)致計(jì)算量增大，影響地形輔助導(dǎo)航的匹配效率?？紤]初始位置點(diǎn)的累積誤差，根據(jù)慣性導(dǎo)航定位誤差橢圓確定搜索區(qū)域大小，假設(shè)慣性導(dǎo)航定位誤差服從標(biāo)準(zhǔn)正態(tài)分布，σx、σy、σxy分別為SINS系統(tǒng)的東向、北向位置標(biāo)準(zhǔn)差和協(xié)方差，則誤差橢圓計(jì)算公式為[13]：

式中：a和b分別為橢圓長和短半軸；φ為橢圓長半軸與正北向夾角；0?σ為擴(kuò)展因子。為計(jì)算方便，采用誤差橢圓的外切矩形計(jì)算公式[13]：

式中：xm和 ym分別為外切矩形的長和寬。根據(jù)“3σ”原則，當(dāng)0?σ=3.03時(shí)，誤差橢圓的置信度可以達(dá)到95%?？紤]到實(shí)際應(yīng)用過程中存在的各種不確定性誤差，本文仿真實(shí)驗(yàn)中取擴(kuò)展因子0?σ=5。

2.2 Hausdorff距離

適應(yīng)度函數(shù)選取的目的是評(píng)價(jià)待選解個(gè)體的質(zhì)量，是反映優(yōu)化進(jìn)程并引導(dǎo)算法向最優(yōu)解逼近的關(guān)鍵因素[12]。平均 Hausdorff距離（Mean Hausdorff Distance,MHD）對匹配對象存在的小誤差不敏感，抗干擾能力強(qiáng)，在一定程度上還可以反映空間內(nèi)實(shí)時(shí)測量曲線與水下地形圖水深曲線的方位關(guān)系，消除噪聲影響[15]。所以本文采用MHD作為實(shí)時(shí)水深測量序列與匹配序列之間的適應(yīng)度函數(shù)。

設(shè)水深測量序列真值為 S={s1, s2, …, sL}，其中， si= (sxi, syi, szi)為測量點(diǎn)的經(jīng)度、緯度和水深值，i=1,2,…,L)，L為測量序列長度。由于地形圖上每個(gè)點(diǎn)的水平坐標(biāo)與水深值可通過數(shù)字地圖映射，所以S在海圖上對應(yīng)的實(shí)時(shí)水深序列為：

MS ={ms1, ms2,…, msL}，

其中，msi= (msxi, msyi)為水平位置坐標(biāo)。

匹配序列確定方式如圖1所示，以SINS所給出的當(dāng)前指示位置為中心，根據(jù)式(2)和(3)建立xm×ym的搜索區(qū)域，在搜索窗口內(nèi)以每一個(gè)節(jié)點(diǎn)(粒子)為起點(diǎn)，以水深測量序列S為模板可形成一條匹配序列Tj。設(shè)匹配水深序列為 Tj={t1j, t2j, …, tLj}，其中 tij=(txij, tyij, tzij)(i=1, 2, …, L；j=1, 2, …, xm×ym)，對應(yīng)的水平位置坐標(biāo)序列為：

MTj={mt1j,mt2j,…,mtLj}，

其中，mtij=(mtxij, mtyij)。則適應(yīng)度函數(shù)定義為：

式中：||·||為歐氏距離即L2范數(shù)；函數(shù)HMHD(MS, MTj)為兩個(gè)點(diǎn)集之間的有向MHD距離，距離越大表示匹配程度越差。

圖1 匹配序列的確定Fig.1 Matching sequence

2.3 改進(jìn)PSO算法的思路

假設(shè)在一個(gè)D維的目標(biāo)搜索空間中，有N個(gè)粒子組成的群體以一定的速度運(yùn)動(dòng)，每個(gè)粒子都有速度和位置參數(shù)，其中第i個(gè)粒子在t時(shí)刻的狀態(tài)信息可用D維向量表示[12,14,16]：

位置為：

其中：1≤d≤D，1≤i≤N，xmin,d和 xmax,d分別為搜索空間的下限和上限，vmin,d和vmax,d分別為最小和最大速度。

在實(shí)際應(yīng)用過程中，粒子群優(yōu)化算法存在早熟收斂的問題，容易陷入局部極值點(diǎn)，從而導(dǎo)致不能收斂到全局極值點(diǎn)[15-16]，同時(shí)粒子的最大速度vmax決定當(dāng)前位置與最好位置之間的區(qū)域分辨率。在線性遞減權(quán)重粒子群優(yōu)化算法的基礎(chǔ)上，引入收斂因子用以對粒子的飛行速度進(jìn)行約束，則第i個(gè)粒子在(t+1)時(shí)刻的位置和速度更新公式為：

其中：c1和c2為學(xué)習(xí)因子，是加速度非負(fù)常數(shù)；r1和r2是[0,1]之間的隨機(jī)數(shù)；ω是慣性權(quán)重，設(shè)定為隨著迭代次數(shù)的增加，在[0.4,0.9]區(qū)間內(nèi)線性減少，即

式中，Tmax為最大迭代次數(shù)；γ是收斂因子，

3 基于改進(jìn)PSO的匹配方法設(shè)計(jì)

傳統(tǒng) TERCOM 匹配算法的搜索過程可以表示為實(shí)時(shí)水深測量序列通過平移產(chǎn)生待匹配序列，最終根據(jù) MSD判斷適應(yīng)度函數(shù)值最小的節(jié)點(diǎn)位置，獲得與實(shí)時(shí)水深測量序列最為相近的匹配序列。基于上述分析，基于改進(jìn)PSO的水下地形匹配算法描述如下：

步驟1：初始化。以SINS指示位置為搜索區(qū)域矩形中心，根據(jù)式(2)和(3)確定搜索區(qū)域大小xm×ym，隨機(jī)產(chǎn)生粒子群的初始位置和速度。

步驟 2：計(jì)算適應(yīng)度值。以當(dāng)前粒子為起點(diǎn)形成一條匹配序列，根據(jù)式(4)將改進(jìn)PSO搜索到的匹配序列與實(shí)時(shí)水深測量序列之間的平均 Hausdorff距離值作為當(dāng)前粒子的適應(yīng)度函數(shù)值。

步驟 3：評(píng)價(jià)粒子。對每個(gè)粒子，將它當(dāng)前適應(yīng)度值分別和個(gè)體最優(yōu)適應(yīng)度值、全局最優(yōu)適應(yīng)度值相比較，如果適應(yīng)度值較小，則對當(dāng)前個(gè)體最優(yōu)位置和當(dāng)前全局最優(yōu)位置進(jìn)行更新。

步驟 4：判斷終止條件。設(shè)置最大迭代次數(shù)，若不滿足，根據(jù)式(5)進(jìn)行位置和速度更新，返回“步驟2”進(jìn)行下一次迭代。

步驟 5：結(jié)果輸出。將全局最優(yōu)值及其所對應(yīng)的最佳匹配位置輸出。

綜上所述，基于改進(jìn)粒子群的地形匹配方法搜索流程圖如圖2所示。

圖2 改進(jìn)PSO匹配方法流程圖Fig.2 Flowchart of improved PSO matching method

4 仿真及分析

仿真實(shí)驗(yàn)在 Visual Studio2010 和 MATLAB8.0 平臺(tái)下進(jìn)行，仿真環(huán)境為 Pentium(R)Dual-Core(2.80GHz CPU,1.99G RAM)。慣性器件參數(shù)、TAN誤差參數(shù)和仿真初值如表1和表2所示。

表1 導(dǎo)航傳感器誤差Tab.1 Navigation sensor error

表2 SINS/TAN組合系統(tǒng)匹配仿真初值Tab.2 Matching simulation initial values of SINS/TAN

實(shí)驗(yàn)用數(shù)據(jù)選取我國某海域地形圖，水深范圍為東經(jīng) 111.777°～113.129°，北緯 15.893°～17.242°。圖 3 是對海圖水深數(shù)據(jù)進(jìn)行雙線性插值處理后得到的等深線圖。地形主要參數(shù)：格網(wǎng)數(shù)為 1250×1250，格網(wǎng)間距為 120 m，水深量程為-2 554～0 m，平均值為-1 009 m，標(biāo)準(zhǔn)差為 505.4 m。

水下航行器以8 kn勻速航行，下潛深度為700 m，SINS系統(tǒng)采樣間隔為1 s，導(dǎo)航時(shí)間為5 h，行駛過程中設(shè)船體繞航向軸、縱搖軸和橫搖軸三軸分別以4°、2°、2°作頻率為5 Hz的正弦規(guī)律搖擺。將航跡段分割成18個(gè)子條帶形成匹配序列，當(dāng)獲得第55個(gè)采樣點(diǎn)位置坐標(biāo)時(shí)開始匹配，水深值采樣周期為10 s，實(shí)測水深序列通過向真實(shí)水深值添加高斯白噪聲形成，水深序列長度為64個(gè)采樣點(diǎn)。

圖3 等深線地形圖Fig.3 Contour map

改進(jìn)粒子群優(yōu)化算法參數(shù)設(shè)置：粒子數(shù)為64，最大迭代次數(shù)為40，學(xué)習(xí)因子c1=2.05, c2=2.05，慣性權(quán)重ω在[0.4,0.9]線性遞減。通過設(shè)置初始位置誤差作為水下航行器在進(jìn)入匹配區(qū)域時(shí)SINS已經(jīng)累積的位置誤差。

當(dāng)水下航行器在不同初始航向狀態(tài)，并假設(shè)不同的初始位置誤差，仿真比較基于MSD的傳統(tǒng)TERCOM算法和改進(jìn)PSO匹配算法的匹配精度和匹配時(shí)間。設(shè)水下航行器的起始水平位置為(112.126°E,16.064°N)，北偏東45°勻速直航，當(dāng)SINS的初始位置誤差較小時(shí)，即東向、北向位置誤差均為0.3′，兩種匹配算法所形成的軌跡如圖4(a)所示，當(dāng)初始位置誤差較大時(shí)，即東向、北向位置誤差均為0.6′，其匹配結(jié)果如圖4(b)所示。

當(dāng)航行器的初始位置為(112.887°E,16.295°N)，北偏西 60°直航，在不同初始位置誤差的情況下兩種匹配算法所形成的軌跡分別如圖5(a)、5(b)所示。

根據(jù)上述兩種情況，通過計(jì)算匹配航跡上各水深測量點(diǎn)位置的匹配誤差平均值以及平均匹配時(shí)間來比較兩種匹配算法的匹配性能，如表3所示。

由圖4和圖5的仿真結(jié)果可知，針對水下航行器的兩種航行狀態(tài)，當(dāng)初始位置誤差較小時(shí)，相比于傳統(tǒng)TERCOM算法，改進(jìn)PSO算法匹配結(jié)果更接近于航行器的真實(shí)軌跡，當(dāng)初始位置誤差較大時(shí)，基于MSD的傳統(tǒng)TERCOM匹配算法陷入局部最優(yōu)，不能得到很好的匹配效果，甚至出現(xiàn)仿真發(fā)散，部分匹配點(diǎn)的誤差陡然增大，導(dǎo)致地形匹配失敗，而改進(jìn)PSO算法基本不受初始位置誤差影響，減小了誤匹配概率，匹配性能較好。

圖4 (a) 初始位置誤差較小時(shí)的PSO/TERCOM北偏東直航匹配結(jié)果Fig.4(a) PSO/TERCOM north-east simulation results for small initial position errors

圖4 (b) 初始位置誤差較大時(shí)的PSO/TERCOM北偏東直航匹配結(jié)果Fig.4(b) PSO/TERCOM north-east simulation results for large initial position errors

圖5 (a) 初始位置誤差較小時(shí)的PSO/TERCOM北偏西直航匹配結(jié)果Fig.5(a) PSO/TERCOM north-west simulation results for small initial position errors

圖5 (b) 初始位置誤差較大時(shí)的PSO/TERCOM北偏西直航匹配軌跡圖Fig.5(b) PSO/TERCOM north-west simulation results for large initial position errors

從表3可直觀看出：在水下航行器的不同航向狀態(tài)下，改進(jìn)PSO算法的定位精度和匹配速度均明顯優(yōu)于傳統(tǒng) TERCOM 算法；當(dāng)初始位置誤差較大時(shí)，改進(jìn)PSO方法的匹配精度優(yōu)于TERCOM算法近5倍，匹配時(shí)長降低了近10倍。因此改進(jìn)PSO匹配算法是有效的，在實(shí)際應(yīng)用中更具優(yōu)勢。

表3 兩種匹配算法的地形匹配結(jié)果比較Tab.3 Comparison on terrain matching results of two algorithms

5 結(jié) 論

本文針對水下地形輔助導(dǎo)航的特殊性，提出基于改進(jìn)粒子群優(yōu)化的水下地形匹配定位方法，結(jié)合Hausdorff距離強(qiáng)抗干擾能力和容錯(cuò)能力的特點(diǎn)，充分利用粒子群優(yōu)化算法的快速尋優(yōu)能力，大大降低誤匹配率，達(dá)到快速匹配和定位，提高了導(dǎo)航系統(tǒng)的綜合能力。仿真分析表明，在水下航行器經(jīng)過長時(shí)間航行累積了較大的初始誤差時(shí)，改進(jìn)粒子群優(yōu)化算法的匹配精度和收斂速度均明顯優(yōu)于傳統(tǒng) TERCOM 算法，具有效率高、魯棒性強(qiáng)的特點(diǎn)。此外，地形匹配結(jié)果可對 SINS進(jìn)行位置重置，再結(jié)合非線性濾波算法進(jìn)一步實(shí)現(xiàn)組合導(dǎo)航的實(shí)時(shí)定位。

（

）：

[1] Hagen O K,?nonsen K B,Skaugen A.Robust surface vessel navigation using terrain navigation[C]//OCEANS.IEEE,2013:1-7.

[2] ?nonsen K B,Hagen O K,?yvind Hegren?s,et al.The HUGIN AUV terrain navigation module[C]//2013OCEANS-San Diego.IEEE,2013:1-8.

[3] Hagen O K,?nonsen K B.Using terrain navigation to improve marine vessel navigation systems[J].Marine Technology Society Journal,2014,48(2):45-58.

[4] Shen J,Shi J,Xiong L.Modeling of underwater terrain aided navigation and terrain matching algorithm simu-lation[C]//Asian Simulation Conference.Springer Singa-pore,2016:424-432.

[5] Han Y,Wang B,Deng Z,et al.An improved TERCOM-based algorithm for gravity-aided navigation[J].IEEE Sensors Journal,2016,16(8):2537-2544.

[6] Hostetler L,Andreas R.Nonlinear Kalman filtering techniques for terrain-aided navigation[J].IEEE Tran-sactions on Automatic Control,1983,28(3):315-323.

[7] Melo J,Matos A.Survey on advances on terrain based navigation for autonomous underwater vehicles[J].Ocean Engineering,2017,139:250-264.

[8] Yuan G,Zhang H,Yuan K,et al.A Combinational Aided Navigation Algorithm Based on Terrain Variance Entropy and ICCP[C]//International Joint Conference on Compu-tational Sciences&Optimization.IEEE,2012:835-838.

[9] Liu M Q,Wang B,Deng Z H,et al.Improved ICCP algorithm and its application in gravity matching aided inertial navigation system[C]//Proceedings of Chinese Control Conference.Nanjing,2014:562-567.

[10] Zhao L,Gao N,Huang B Q,et al.A novel terrain-aided navigation algorithm combined with the TERCOM algori-thm and particle filter[J].IEEE Sensors Journal,2015,15(2):1124-1131.

[11] 何艷萍,劉新學(xué),蔡艷平,等.基于粒子群優(yōu)化的飛行器地形匹配新算法[J].紅外與激光工程,2016,45(s1):115-120.He Y P,Liu X X,Cai Y P,et al.New aircraft terrain matching algorithm based on particle swarm optimization[J].Infrared and Laser Engineering,2016,45(s1):115-120.

[12] 徐遵義,魏東,李璟,等.基于粒子群優(yōu)化的飛行器地磁相關(guān)匹配新算法仿真[J].艦船科學(xué)技術(shù),2011,33(11):3-6.Xu Z Y,Wei D,Li J,et al.Simulation research on the correlation matching algorithm based on particle swarm optimization for aircraft geomagnetic aid navigation[J].Ship Science and Technology,2011,33(11):3-6.

[13] 諶劍,張靜遠(yuǎn),查峰.一種改進(jìn) ICCP水下地形匹配算法[J].華中科技大學(xué)學(xué)報(bào)(自然科學(xué)版),2012,40(10):63-67.Shen J,Zhang J Y,Cha F.Improved ICCP algorithm for underwater terrain matching method[J].Journal of Hua-zhong University of Science&Technology,2012,40(10):63-67.

[14] Grandi R,Falconi R,Melchiorri C.A navigation strategy for multi-robot systems based on particle swarm optimi-zation techniques[J].IFAC Proceedings Volumes,2012,45(22):331-336.

[15] Xu X S,Yue Z Y,Zhang T,et al.Method on underwater assisted navigation system using two-dimensional terrain feature matching based on Hu moments[J].Journal of Chinese Inertial Technology,2015,23(3):363-368.

[16] 周玲,程向紅,等.基于約束粒子群優(yōu)化的海底地形輔助慣性導(dǎo)航定位方法[J].中國慣性技術(shù)學(xué)報(bào),2015,23(3):369-372.Zhou L,Cheng X H.Seabed terrain-aided SINS location based on constrained particle swarm optimization method[J].Journal of Chinese Inertial Technology,2015,23(3):369-372.