基于改進粒子群優化的水下地形輔助導航方法

程向紅 ，周月華

（1.微慣性儀表與先進導航技術教育部重點實驗室，南京210096；2.東南大學儀器科學與工程學院，南京210096）

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

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

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

1 TERCOM匹配算法

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

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

步驟1：采樣。采樣L個點的純慣導指示位置坐標、實時測量水深數據和數字地圖指示水深數據。

步驟 2：生成實時圖。根據慣導給出的位置坐標和相應的水深測量值生成實時圖。

步驟 3：生成基準圖。根據實時圖、搜索位置和慣導導航誤差的標準差由數字地圖生成基準圖。

步驟 4：計算相關值。通過計算每個基準子圖與實時圖的JMSD，得出JMSD最小值。

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

2 改進PSO算法

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

2.1 匹配搜索區域的確定

搜索區域較小時，將無法包含水下航行器的真實位置，容易導致匹配失敗；搜索區域較大時，會導致計算量增大，影響地形輔助導航的匹配效率?？紤]初始位置點的累積誤差，根據慣性導航定位誤差橢圓確定搜索區域大小，假設慣性導航定位誤差服從標準正態分布，σx、σy、σxy分別為SINS系統的東向、北向位置標準差和協方差，則誤差橢圓計算公式為[13]：

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

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

2.2 Hausdorff距離

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

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

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

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

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

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

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

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

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

2.3 改進PSO算法的思路

假設在一個D維的目標搜索空間中，有N個粒子組成的群體以一定的速度運動，每個粒子都有速度和位置參數，其中第i個粒子在t時刻的狀態信息可用D維向量表示[12,14,16]：

位置為：

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

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

其中：c1和c2為學習因子，是加速度非負常數；r1和r2是[0,1]之間的隨機數；ω是慣性權重，設定為隨著迭代次數的增加，在[0.4,0.9]區間內線性減少，即

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

3 基于改進PSO的匹配方法設計

傳統 TERCOM 匹配算法的搜索過程可以表示為實時水深測量序列通過平移產生待匹配序列，最終根據 MSD判斷適應度函數值最小的節點位置，獲得與實時水深測量序列最為相近的匹配序列。基于上述分析，基于改進PSO的水下地形匹配算法描述如下：

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

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

步驟 3：評價粒子。對每個粒子，將它當前適應度值分別和個體最優適應度值、全局最優適應度值相比較，如果適應度值較小，則對當前個體最優位置和當前全局最優位置進行更新。

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

步驟 5：結果輸出。將全局最優值及其所對應的最佳匹配位置輸出。

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

圖2 改進PSO匹配方法流程圖Fig.2 Flowchart of improved PSO matching method

4 仿真及分析

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

表1 導航傳感器誤差Tab.1 Navigation sensor error

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

實驗用數據選取我國某海域地形圖，水深范圍為東經 111.777°～113.129°，北緯 15.893°～17.242°。圖 3 是對海圖水深數據進行雙線性插值處理后得到的等深線圖。地形主要參數：格網數為 1250×1250，格網間距為 120 m，水深量程為-2 554～0 m，平均值為-1 009 m，標準差為 505.4 m。

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

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

改進粒子群優化算法參數設置：粒子數為64，最大迭代次數為40，學習因子c1=2.05, c2=2.05，慣性權重ω在[0.4,0.9]線性遞減。通過設置初始位置誤差作為水下航行器在進入匹配區域時SINS已經累積的位置誤差。

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

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

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

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

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

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

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

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

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

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

5 結 論

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

（

）：

[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] 何艷萍,劉新學,蔡艷平,等.基于粒子群優化的飛行器地形匹配新算法[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] 徐遵義,魏東,李璟,等.基于粒子群優化的飛行器地磁相關匹配新算法仿真[J].艦船科學技術,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] 諶劍,張靜遠,查峰.一種改進 ICCP水下地形匹配算法[J].華中科技大學學報(自然科學版),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] 周玲,程向紅,等.基于約束粒子群優化的海底地形輔助慣性導航定位方法[J].中國慣性技術學報,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.