基于改进粒子群优化的水下地形辅助导航方法

程向红 ，周月华

（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.