基于约束粒子群优化的海底地形辅助惯性导航定位方法

周 玲，程向红

（1. 东南大学 仪器科学与工程学院 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 运城学院 物理与电子工程系，运城 044000）

基于约束粒子群优化的海底地形辅助惯性导航定位方法

周 玲1,2，程向红1

（1. 东南大学 仪器科学与工程学院 微惯性仪表与先进导航技术教育部重点实验室，南京 210096；2. 运城学院 物理与电子工程系，运城 044000）

针对水下自主航行器长时间航行后惯性导航系统位置误差积累的问题，提出一种关于约束粒子群优化的海底地形辅助惯性导航定位方法。通过在等值域内进行粒子群初始化，改善粒子“早熟”问题。采用惯性导航和水深测量序列计算相邻测量点水平距离、航迹水平转向角和水深变化量三个匹配参数，同时利用待匹配航迹上的粒子群计算以上三个参数，将航迹水平转向角和水深变化量转换为各自等效的水平相对距离，最后采用对应于相同匹配参数的等效水平相对距离的平均绝对差之和构建适应度函数，采用约束粒子群优化进行地形匹配，辅助惯导系统准确定位。通过某海图内海底地形匹配进行仿真实验，结果表明当航行器初始位置误差较大时：在地形变化明显区，ICCP的定位精度为200 m，约束粒子群优化的定位精度提高到20 m以内；在地形变化平坦区，ICCP无法有效定位的情况下，约束粒子群优化的定位精度约为250 m。

地形匹配；约束粒子群优化；地形辅助导航；水下地形导航；水下自主航行器

长航时、高精度、自主性、隐蔽性等性能是水下自主航行器(AUV)对导航系统提出的基本要求。近年来，随着海洋测绘技术的发展，海底地形辅助惯性导航逐渐成为研究热点[1]。海洋水深的分布情况反映了海底地形的变化程度，水深与水平地理位置存在着对应关系，因此通过AUV装备的深度计和测深仪测量水深值，与预先存储的海图水深数据库进行匹配，可以实现地形辅助惯性导航系统的定位。

地形匹配定位算法可以分为批处理和滤波处理两大类[2]。批处理方法将一系列水深测量数据累积起来处理，根据水深测量序列和待匹配航迹上的海图水深序列的相关度，确定匹配航迹。最早的批处理算法有地形轮廓匹配算法(TERCOM)[3]，其主要用于巡航导弹和飞机。由于水下环境和AUV的特殊性，在批处理算法中，基于等值线图形匹配(ICCP)算法得到广泛研究。但ICCP适于初始位置误差较小的情况，当初始位置误差较大时，存在计算量大，易陷入局部最优的缺点。用于海洋地球物理匹配导航的ICCP改进算法中，采用滑动窗口技术[4]减少最近点计算量，采用混沌优化算法[5]、三角形约束模型[6]、BP神经网络[7]、粒子群优化[8,9]、基于误差平方和价值函数最优的等值线匹配算法[10]等改善算法局部收敛性和实时性。当初始位置误差较大时，采用TERCOM粗匹配辅助ICCP精匹配的方式[11]。虽然上述方法从多个方面改进了ICCP算法，并取得一定效果，但是在克服ICCP易陷入局部最优的缺陷方面仍然有限。

粒子群优化(PSO)是智能群优化方法的典型代表，本文在以上算法研究的基础上，充分利用粒子群优化快速的搜索特性，结合地形特征和惯导短时间导航精度高的特点，解决水下地形辅助惯性导航系统快速、准确定位问题。

1 约束粒子群优化

实际应用中，PSO存在粒子“早熟”的问题，即搜索过程中有陷入局部最小值的可能。针对这一问题，采用一种基于约束粒子群优化(CPSO)的搜索方法。粒子群的位置参数通过等值域进行约束，即要求粒子群的水平位置(x,y)满足以下约束条件：

式中：(xc, yc)为双线性插值法获取的水深数据等值线上插值点的水平位置；ζ是服从均值为零的高斯白噪声，其方差根据初始粒子群位于水深等值域内的原则确定。

假设一个由N个粒子组成的群体在D维的搜索空间以一定的速度运动，粒子i在t时刻的状态属性设置如下：

其中，1≤d≤D，1≤i≤N。

则粒子在t+1时刻的位置通过下式更新获得：

式中：惯性权重ω=ωstart-(ωstart-ωend)ttmax，tmax为最终时刻（或称为最大迭代次数），ωstart、ωend分别为初始惯性权重和终止惯性权重；r1、r2为均值分布在(0,1)区间的随机数；c1、c2称为学习因子，通常取c1=c2=2。

地形匹配是一个数据相关问题，可用相关性准则来定义适应度函数。算法中选用平均绝对差(MAD)准则，即定义

采用SINS和水深测量序列S计算相邻测量点的航迹水平转向角和水深变化量两个匹配参数，同时利用待匹配航迹T上水深等值域内粒子群计算这两个参数，并将匹配参数转换为各自等效的水平相对距离。然后，类似于公式(4)，得到航迹水平转向角和水深变化量对应的水平相对距离的MAD，分别记为E2和E3。最后，将Ei作为CPSO算法的适应度函数，当适应值最小时对应搜索的最优解。

2 基于CPSO的海底地形匹配方法设计

基于CPSO的地形匹配方法流程图如图1所示。具体的地形匹配方法的设计如下：

① 在水深测量点的等值域约束下初始化一群二维粒子，分别对应初始水平位置；

② 计算待匹配航迹上的粒子群与水深测量序列之间的适应度函数值；

③ 对每个粒子，将它的适应值和它经历过的个体最优适应值作比较，如果较小，则将其对应的位置作为当前个体最优位置；

④ 对每个粒子，将它的适应值和全局粒子所经历过的最优适应值作比较，如果较小，则将其对应的位置作为当前全局最优位置；

⑤ 根据式(2)和(3)改变粒子的速度和位置；

⑥ 设置最大迭代次数，判断是否达到预设的最大迭代次数，如未达到，回到步骤②，否则，输出全局最优值。航迹均从A到B，两种方法定位结果基本相同，都较好地实现了地形匹配。

图1 基于CPSO的地形匹配方法流程图Fig.1 Flowchart of terrain matching method based on CPSO

当惯性导航系统初始位置误差较大时，东向位置误差为800 m，北向位置误差为10 m，如图3所示，其他条件的设置同图2。仿真结果显示：CPSO时长8.69 s，航迹从A到B；ICCP运行时长28.62 s，航迹从C到D。与图2相比较，ICCP算法受初始位置误差的影响较大，定位速度和精度均变差，而CPSO基本不受初始位置误差的影响，定位效率和效果均明显优于ICCP算法。

在地形平坦区航行时，如图4所示。仿真的真实航迹为起始水平位置 (2220 m, 110 m)，北偏西60°直

图2 在初始位置误差较小情况下地形变化明显区CPSO/ICCP地形匹配结果Fig.2 CPSO/ICCP simulation results under the condition of less initial position errors in a rough terrain

3 仿真及分析

仿真实验在MATLAB7.0平台下进行，环境Inter(R) Core(TM) （3.3 GHz CPU, RAM 2 GB）。仿真产生海底地形，东向位置范围为0～3000 m，北向位置范围为0～1120 m，海图分辨率约为5 m×5 m，水深量程范围为-200～0 m。AUV以4 kn匀速航行，速度误差0.1 m/s，SINS陀螺随机常值为0.05 (°)/h，加速度计零偏为0.1 mg，初始航向误差为0.3°。此外，通过设置初始位置误差以仿真AUV在进入地形匹配区域时已经累积的位置误差。

仿真实验中，测量两种不同地形的水深序列。CPSO参数设置为：待匹配航迹上各水深值的粒子个数为50，学习因子c1=2.1，c2=2.0，惯性权重ω在0.9～0.4范围线性递减，最大迭代次数设为20；水深测量间隔为2 min。

图3 在初始位置误差较大情况下地形变化明显区CPSO/ICCP地形匹配结果Fig.3 CPSO/ICCP simulation results under the condition of larger initial position errors in a rough terrain

当惯性导航系统初始位置误差较小时，如图2所示。仿真的真实航迹为起始水平位置(555 m, 20 m)，北偏东40°直航，东向位置误差为100 m，北向位置误差为10 m。地形熵为0.54，为地形起伏较明显区。仿真结果显示ICCP运行时长7.7 s ，CPSO时长8.9 s。航，地形熵为0.77。惯性导航系统初始东向位置误差500 m，北向位置误差10 m。仿真结果显示：CPSO时长13.98 s，航迹从A到B；ICCP运行时长18.8 s，航迹从C到D。与地形变化明显区的图2和图3比较，两种方法在地形平坦区的定位精度均下降，需要更长时间和更多水深测量进行定位，但两种方法相比较，ICCP更易陷入局部最优。

图4 地形变化平坦区CPSO/ICCP地形匹配结果Fig.4 CPSO/ICCP simulation results in flat area

当AUV在水下作拐弯运动，如图5所示。起始水平位置为（555 m, 110 m），初始航向北偏东60°，地形熵为0.68。惯性导航系统初始东向位置误差800 m，北向位置误差100 m。仿真结果显示：CPSO时长10.48 s，航迹从A到B；ICCP运行时长16.68 s，航迹从C到D。两种方法比较，ICCP受初始位置误差的影响较大，且易陷入局部最优；CPSO适用范围更广，且定位精度明显优于ICCP。

图5 拐弯运动下CPSO/ICCP地形匹配结果Fig.5 CPSO/ICCP simulation results in turning movement

基于CPSO中粒子群的随机分布，对两种不同地形特征下的两条水深序列进行50次仿真实验，分析待匹配航迹上各水深测量点位置的平均圆概率误差（CEP 50%），如表1所示。表1中，地形特征明显时，定位时间短，定位精度高；地形平坦时，定位时间长，定位精度低，这与仿真图中的结果相一致。

从以上仿真情况可以看出：当地形变化明显时，两种方法所需的水深测量次数较少，匹配精度较高；当地形变化平坦时，待匹配的水深测量次数较多，匹配精度较低，匹配时间较长。与CPSO相比，ICCP算法更容易陷入局部最优。为提高匹配效率和优化匹配效果，可根据地形特征，如地形熵和地形方差，从测量数据中选取待匹配的水深值进行匹配。最终，将地形匹配获取的当前时刻的位置信息对惯导系统进行位置重置以实现SINS位置误差的修正。

表1 两种地形下各水深测量值地形匹配的CEP 50%值比较Tab.1 CEP 50% comparison of each sample point for terrain matching on two kinds of terrains

4 结 论

基于CPSO的海底地形辅助惯性导航定位方法，通过在水深测量值的等值域内进行粒子群初始化，极大的提高了CPSO的搜索效率，改善了粒子“早熟”问题。结合地形特征和惯导短时间导航精度高的特点，辅助惯导系统准确定位。通过某海图内海底地形匹配仿真实验表明，在AUV经过长时间航行累积了较大位置误差后，进入海图覆盖区域，CPSO地形匹配结果明显优于ICCP算法，具有效率高、鲁棒性强的特点，地形匹配的结果可作为AUV长航时后惯性导航系统的“位置初始对准”。此外，再结合非线性滤波算法可实现地形辅助惯性导航的实时定位。

[1] 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.

[2] Donovan G T. Position error correction for an autonomous underwater vehicle inertial navigation system (INS) using a particle filter[J]. IEEE Journal of Oceanic Engineering， 2012， 37(3): 431-445.

[3] Golden J. Terrain contour matching (TERCOM): A cruise missile guidance aid[J]. Image Processing for Missile Guidance， 1980， 238: 10-18.

[4] 童余德， 边少锋， 蒋东方， 等. 实时ICCP算法重力匹配仿真[J]. 中国惯性技术学报， 2011， 19(3): 340-343. Tong Yu-de， Bian Shao-feng， Jiang Dong-fang， et al. Gravity matching simulation of real-time ICCP algorithm[J]. Journal of Chinese Inertial Technology， 2011， 19(3): 340-343.

[5] 张涛， 徐晓苏， 李佩娟. 混沌优化水下地形匹配算法研究[J]. 中国惯性技术学报， 2009， 17(2): 156-158.

Zhang Tao， Xu Xiao-su， LI Pei-juan. Underwater terrain matching algorithm based on chaotic optimization[J]. Journal of Chinese Inertial Technology， 2009， 17(2): 156-158.

[6] Yang Z L， Zhu Z S， Zhao W G， et al. A triangle matching algorithm for gravity-aided navigation for underwater vehicles[J]. Journal of Navigation， 2014， 67(2): 227-247.

[7] Zhang Tao， Xu Xiao-su. The application of chaotic BP neural network in underwater terrain matching navigation [C]//21st Chinese Control and Decision Conference. 2009: 695-698.

[8] 谌剑， 张静远， 查峰， 等. 一种改进ICCP水下地形匹配算法[J]. 华中科技大学学报: 自然科学版， 2012，40(10): 63-67.

Shen Jian， Zhang Jing-yuan， Zha Feng. Improved ICCP al gorithm for underwater terrain matching method[J]. Journal of Huazhong University of Science and Technology (Nature Science Edition)， 2012， 40(10): 63-67.

[9] Yuan Gan-nan， Tan Jia-lin， Liu Li-qiang， et al. Seabed terrain match algorithm based on hausdorff distance and particle swarm optimization[C]//Proceeding of the 2009 International Joint Conference on Computational Science and Optimization. 2009: 236-239.

[10] 孙枫， 王文晶， 高伟， 等. 用于无源重力导航的等值线匹配算法[J]. 仪器仪表学报， 2009， 30(4): 817-822.

Sun Feng， Wang Wen-jing， Gao Wei， et al. Contour matching algorithm for passive gravity navigation[J]. Chinese Journal of Scientific Instrument， 2009， 30(4): 817-822.

[11] 徐晓苏， 吴剑飞， 徐胜保， 等. 基于仿射修正技术的水下地形ICCP匹配算法[J]. 中国惯性技术学报， 2014，22(3): 362-367.

Xu Xiao-su， Wu Jian-fei， Xu Sheng-bao， et al. ICCP algorithm for underwater terrain matching navigation based on affine correction[J]. Journal of Chinese Inertial Technology， 2014， 22(3): 362-367.

Seabed terrain-aided SINS location based on constrained particle swarm optimization method

ZHOU Ling1,2, CHENG Xiang-hong1
(1. School of Instrument Science & Engineering, Key Laboratory of Micro-inertial Instrument and Advanced Navigation Technology, Ministry of Education, Southeast University, Nanjing 210096, China; 2. Department of Physics and Electronic Engineering, Yuncheng University, Yuncheng 044000, China)

In view of the problem of position error accumulation when an autonomous underwater vehicle (AUV) has sailed for a long time, a constrained particle swarm optimization (CPSO) method is presented for seabed terrain-aided SINS location. By improving the premature convergence of particle swarm optimization, the initialization of particle swarm is constrained in the depth contour neighborhood of each measurement. Three matching parameters, i.e. horizontal distance, horizontal turn angle and depth difference between each measurement, are calculated by the SINS and the depth measurement sequence. Meanwhile, the three parameters are calculated by the corresponding particle swarms. Thus the horizontal turn angle and the depth difference are converted into the equivalent relative horizontal distances, respectively. In the end, the sum of mean absolute differences of the relative horizontal distances corresponding to the same parameters is taken as the fitness function. The CPSO is applied to terrain matching for improving the AUV positioning accuracy. The simulation experiments of terrain-aided SINS in a certain sea chart show that the CPSO has advanced positioning accuracy of 20 m in comparison with the ICCP’s positioning accuracy of 200 m in a rough terrain, and when the positioning accuracy of the CPSO reaches 250 m, the ICCP fails to locate effectively in flat areas.

terrain matching; constrained particle swarm optimization; terrain-aided navigation; underwater terrain navigation; autonomous underwater vehicle

U666.1

A

1005-6734(2015)03-0369-04

10.13695/j.cnki.12-1222/o3.2015.03.016

2015-01-17；

2015-05-05

国家自然科学基金资助项目（61374215）

周玲（1981—），女，博士研究生，专业为仪器科学与技术。E-mail：s03031212@126.com

联 系 人：程向红（1963—），女，教授，博士生导师。E-mail：xhcheng@seu.edu.cn