北极冰下双自主水下机器人协同地形匹配导航

姜言清， 马东， 李智慧， 李晔， 马腾， 徐雪峰， 侯冬冬， 张文君， 武皓微

(1.哈尔滨工程大学 水下机器人技术重点实验室，黑龙江 哈尔滨 150001； 2.中国船舶航海保障技术实验室， 天津 300131； 3.河南省水下智能装备重点实验室，河南 郑州 450015; 4. 北京宇航系统工程研究所，北京 100076)

北极被冰层大面积覆盖，常规的导航手段如卫星导航、水下声学导航等由于冰层的限制，无法在北极安装使用，水下惯性导航由于存在随时间增加的累计误差，在无外界信息辅助条件下无法实现水下远距离导航[1-2]。自主水下机器人(autonomous underwater vehicles，AUVs)可以携带单波束声呐实现水下长时间、大潜深作业[3]。理论上，单波束地形匹配导航在北极维持长时间航行的同时，还能维持一定的定位精度[4-5]。由于北极地图分辨率及滤波算法等原因，导航结果出现不稳定情况，许多学者对其进行研究与改进。Zhou[6]在传感器受限情况下使用单波束声呐进行地形匹配导航，通过估计传感器误差，提高导航性能，并通过实验证明该方法在低成本AUV上的可行性；José[7]对低成本传感器上影响粒子滤波性能的因素进行研究，研究测量噪声、运动噪声和粒子数目等的影响，为算法优化提供方向；Liu等[8-9]对北极冰下导航算法进行研究，通过改进粒子滤波算法提高水下定位精度，为单波束声呐水下导航在北极的应用提供了解决方案；Georgios[10]对单波束地形匹配导航进行深入的研究，将单波束声呐应用于长航程导航中，分别采用粒子滤波、抖动粒子滤波和Rao-Blackwellized粒子滤波进行数据融合[11-13]，并研究导航参数对导航性能的影响，利用北极和大西洋地形数据进行仿真验证，均取得不错的仿真结果，为长航程水下导航提供未来发展方向。

上述方法虽能在一定范围内提升单波束声呐地形匹配导航的性能，但由于单波束声呐测量原理，导致在实际应用中存在较大定位误差，为此学者将单波束地形匹配导航引入协同导航中，采用多AUV协同作业，AUV之间进行信息交流，有效改善导航定位精度，提高作业效率[14]。目前基于单波束测深的多AUV协同地形匹配导航的研究较少，Georgios[15]研究一种水面无人艇辅助AUV的单波束地形匹配导航方法，取得不错的导航结果；Yew[16]提出一种3个AUV单波束声呐地形匹配导航方法，采用Rao-Blackwellized粒子滤波进行数据融合，实验表明该协同导航方法能用于水下远距离航行，具有较强的鲁棒性。文献[15-16]对单波束声呐地形匹配导航误差进行修正，但未将北极低分辨率先验地形图考虑到范围内，对北极低分辨率地形图无法取得较好的导航效果。

本文采用一种地形自适应双AUV协同地形匹配导航方法，在低分辨率地形图基础上，考虑海底地形置信度等因素，提高地形匹配定位精度。在粒子重采样阶段将地形置信度引入重采样方程中，避免粒子退化现象，提高算法鲁棒性。。

1 海底地形置信度分析

本文的先验地形图为图1所示北冰洋国际水深图[17]，利用地形高程标准差描述地形置信度，地形高程标准差为地形高程数据方差开方，能够表述地形表面复杂度。地形特征变化明显区域，连续2次采集的数据存在较大不同，较平坦区域具有更好的匹配效果。

地形高程方程为h=h(x,y)，对先验地形图离散处理，得到地形平均高程和标准差分别为：

(1)

(2)

式中：A为地形区域面积；i和j为地形区域中X轴和Y轴坐标位置。

图1 北冰洋国际水深Fig.1 International bathymetric chart of the arctic ocean

海底地形高程标准差与海底地形起伏度有关，地形起伏越快，地形高程标准差越大[18]。利用地形高程标准差对地形平坦度分类，当地形标准差位于[0,18)时，区域为平坦地形；当地形标准差位于区间[18,61]时，区域为中等区域；当地形标准差位于区间(61,+∞]时，区域为粗糙区域。

利用海底地形平坦度划分标准，建立海底地形置信度模型。海底地图栅格化处理，每个栅格化区域大小为20×20，网格分辨率为200 m，计算栅格区域内的地形置信度，结果如图2所示。

图2 地形置信度Fig.2 Terrain confidence map

2 双自主水下机器人地形匹配导航方法

单波束声呐每次测量获得一处水深数据，遇到地形平坦区域将导致地形匹配导航失效。本文提出一种双AUV协同地形匹配导航方法，AUV间利用水声通信，保证时间一致，水声通信周期内采用地形匹配导航修正自身位置，每隔通信周期T进行一次水声通信，获得AUV间距离、位置和地形置信度信息[19]，如图3所示。采用扩展卡尔曼滤波进行数据融合[20]，利用AUV中位置置信度高对位置置信度低进行修正，提高导航方法的鲁棒性。

图3 双AUV协同地形匹配导航示意Fig.3 Schematic diagram of dual AUVs cooperative terrain matching navigation

在AUV协同地形匹配导航中，未考虑粒子重采样带来的粒子贫化和地形平坦区域误匹配等问题。粒子多次迭代后，权重可能集中在小部分粒子上，导致粒子退化，造成地形匹配导航定位出现偏差[21]。粒子多次重采样后，会出现多数粒子来自同一粒子，造成粒子耗尽问题。为此对权值更新和重采样步骤进行改进，将地形置信度融入到重采样步骤中，抑制粒子在地形平坦区域的重采样，减少粒子贫化和误匹配问题，提升地形匹配导航定位的鲁棒性。

1)地形匹配导航。

(3)

式中ξ为权重调节系数。

权重归一化为：

(4)

AUV位置状态估计和方差估计分别为：

(5)

(6)

计算有效粒子数目为：

(7)

将有效粒子数目与粒子阈值比较。若小于粒子阈值，进行重采样，反之不操作。

对Xk+1位置进行估计:

Xk+1=Xk+uk+1+δk+1

(8)

2)协同地形匹配导航为：

xk+1=xk+Tvx(k)

(9)

yk+1=yk+Tvy(k)

(10)

式中：vx(k)和vy(k)为AUV的速度u在x轴和y轴上的投影，通信周期为T，每进行一次采样便进行一次信息交换。

该模型未考虑噪声影响，设定传感器噪声为高斯噪声，得到速度为：

umk=uk+δk

(11)

噪声协方差矩阵为：

(12)

AUV平面状态方程为：

Xk+1=f(Xk,uk,δk)=Xk+Γ(uk,δk)=

(13)

式中Γ(uk,δk)为非线性项。

在扩展卡尔曼滤波中，根据2条路径上AUV所在位置的地形置信度对扩展卡尔曼滤波参数进行调整，利用位置置信度高的对位置置信度低的进行修正，提高导航方法的鲁棒性。

通过扩展卡尔曼滤波量测方程得到系统的量测方程为：

(14)

式中wz为量测噪声。

根据协同导航非线性量测方程，得到扩展卡尔曼滤波量测雅可比矩阵为：

(15)

采用扩展卡尔曼滤波进行更新，更新方程为：

Xk+1,k=f(Xk,uk,δk)=Xk+Γ(uk,δk)

(16)

更新方程的协方差矩阵为：

(17)

在协同导航中考虑通信中断的影响，当通信中断时，AUV利用地形匹配导航进行位置修正，当通信连接时利用扩展卡尔曼滤波进行信息融合，对通讯中断时的偏差进行修正，提高定位精度。算法流程如图4所示。

图4 双AUV协同地形匹配导航流程Fig.4 Flowchart of dual AUVs cooperative terrain matching navigation

3 双自主水下机器人协同地形匹配导航模拟仿真

3.1 仿真参数设置

将地形自适应双AUV协同导航与单AUV地形匹配导航和基于扩展卡尔曼滤波的双AUV协同导航进行比较。仿真中考虑通信中断，存在一定水声信息丢失的情况，全程随机设置200个断点。在双AUV协同地形匹配导航中，先验地图分辨率为200 m，水声通信延迟时间约3 s，AUV 速度较低，航行距离远小于一个网格区域，不会对导航精度产生较大影响，通信延迟可忽略。当地形标准差小于18时，认为此时地形平坦。仿真参数设置如表1所示。

表1 AUV协同地形匹配导航参数设置

3.2 仿真结果

根据上述仿真参数进行模拟仿真，得到的仿真结果对比如表2所示。仿真结果对比图如图5所示。

表2 AUV仿真定位结果Table 2 AUV simulation positioning results

图5 匹配误差与匹配成功次数对比Fig.5 Comparison of matching errors and number of successful matches

4 结论

1)北极海面被冰层覆盖，水下长时间远距离精准导航成为水下探索的难点。本文提出的基于单波束声呐的双AUV协同地形匹配导航方法，能够在水下无源条件下实现长时间精准定位，提高AUV水下作业能力。

2)对北极稀疏先验地形图，将海底地形置信度融入协同地形匹配导航算法中，减少粒子退化现象，同时利用AUV中位置置信度高的对位置置信度低的进行修正，提高地形匹配成功次数，提升导航算法的鲁棒性。

3)对北极冰下远距离导航进行仿真验证，通过仿真结果证明方法的可行性与鲁棒性，为北极冰下资源探索与定位提供新的发展方向。