基于多波束声呐的同时定位与地图构建

张飞虎，称晨升，王 璨

（西北工业大学 航海学院，陕西 西安 710072）

0 引言

随着科技的进步，智能水下机器人（Autonomous Underwater Vehicle，AUV）已成为海洋调查、海洋资源开发、海洋考古救援以及海洋测绘的重要载体。AUV在水下环境中发挥重要的作用，主要在于其能在无人控制的情况下独立完成复杂任务。由于AUV的自主能力主要受水下导航技术制约，因此，突破GPS受限场景下的高精度导航技术瓶颈，对实现AUV智能化具有重要的意义[1]。

通常，水下导航技术主要包括：1）使用惯性导航系统[2-4]。通过将陀螺系统与多普勒测速仪结合使用，实现小尺度定位与导航功能。然而，其面临误差累计的影响，需要经过一定周期利用 GPS信号重新标定。2）使用地磁导航系统。与惯性导航相反，地磁导航系统成本相对较低，且能提供全局航向基准，但面临地磁异常区域所导致的航向偏离以及无法准确定位的问题。综上所述，无论是惯性导航还是地磁导航，在深远海领域中所面临的迫切问题可概括为2点︰1）对其自身位置难以进行大尺度精确估计；2）对所处环境难以进行高精度有效测绘。

令人欣慰的是，同时定位与地图构建（Simultaneous Localization and Mapping，SLAM）技术可以很好地解决上述难题[5]。当前，视觉 SLAM[6-9]以及激光SLAM[10-14]都已取得了很大进展，并通过成果转化实现商业部署，如无人驾驶/扫地机器人领域。相较于陆地，水下同时定位与地图构建主要面临着“看不清”和“看不准”等技术瓶颈：看不清是因为在深水中没有光线，电磁信号在水下大幅衰减，很多在地面上可正常使用的传感器（如GPS、激光雷达、视觉传感器等）在水下无法使用[15-16]；看不准是由于受地质结构制约，水下传感器所获的观测数据面临在不同区域难以有效区分，易导致将不同处相似场景识别为同一场景，进而影响导航系统整体性能。因此，寻求一种具有容错特性的 SLAM方法，使其能够针对水下复杂场景进行全局优化，很有必要。

本文深入研究水下相似场景的导航方法：首先，建立基于因子图的全局优化 SLAM 模型，将多波束声呐与多普勒测速仪进行耦合解算，构建多约束方程确保导航边界性条件。其次，围绕导航过程中存在地形相似等场景，采用自适应变结构拓扑模型，通过优化迭代保障SLAM的容错性。最后，通过搭建软硬件平台，对所提方案可靠性和鲁棒性进行了初步验证。

1 因子图SLAM概述

借鉴美国国防部高级研究计划局DARPA举办多届的无人驾驶挑战赛经验，由于所有参赛车辆都必须在雨雾天中无 GPS信号的情况下行走，因此同时定位与地图构建方法当之无愧的成为 GPS受限场景下导航的核心算法[17]。

SLAM最初被提出是用于研究陆地机器人的，刚开始都是基于卡尔曼滤波原理[18]来解决 SLAM问题，之后在卡尔曼滤波的基础上发展出了扩展卡尔曼滤波（Extended Kalman Filter，EKF），其具有原理简单且易于操作的特点，逐渐成为解决SLAM问题的经典方法[19]。随后，研究者们又陆续提出了粒子滤波器 PF（Particle Filter）方法[20-21]、EIF（Extended Information Filter）方法[22]、无损卡尔曼滤波的 UKF（Unscented Kalman Filter）方法[23]等。2009年，研究者们提出了图优化理论，比较有代表性的是 Lourakis等人提出的基于图优化的SLAM解决方案[24]。

1.1 因子图SLAM建模拓扑描述

因子图 SLAM[25-26]是通过图优化模型来实现同时定位与地图构建算法技术方案。如图1所示，因子图由节点和边沿2种元素构造而成。其中，x节点表示机器人的自身状态信息，在估计过程中通过相应的运动模型来构建条件概率密度从而进行非线性优化。黑色边沿代表里程计约束，在这里指机器人自身的运动模型。

图1 基于图优化模型的SLAM表示方法Fig. 1 SLAM representation method based on graph optimization model

因此，基于因子图的 SLAM 包含关联和估计这两个既相互独立、又交叉影响的过程：1）关联，传感器获得的量测和机器人自身位置、地图路标进行匹配；2）估计，针对匹配结果在贝叶斯框架下进行状态估计。

1.2 因子图SLAM建模数学机理

因子图核心思想是根据里程计（惯导）及不同点的量测信息对机器人及其周围环境进行最大后验概率估计，即

式中：U是机器人的输入控制；Z为所探测到得测量信息；X为平台的姿态；M路标点位置。假设系统过程模型和观测模型服从高斯噪声分布，即

针对地形近似所导致的错误回环问题，需考虑在图优化过程中引入开关变量通过非线性迭代，实现优化过程中自适应关闭由相似场景所导致的错误回环，提高并完善同时定位与地图构建算法的容错性。如图2所示，其中s表示由相似地形所导致的错误回环。

图2 自适应变结构因子图Fig. 2 Factor diagram of adaptive variable structure

根据变结构因子图拓扑描述，其最大后验概率估计可描述为

基于式（7），图优化 SLAM 算法可通过开关因子的区间估计来自适应调整回环检测约束的权值，从而具有在相似场景下错误闭环的容错性能。

2 实验

基于上述的理论支撑，我们结合实验平台BlueROV2的结构和携带的传感器特性，选择了合适的SLAM算法框架，并完成完整SLAM算法的设计和测试。在实验中，我们使用声呐作为感知单元，IMU和DVL作为定向和定位单元，把这些感知数据输入到 SLAM 算法中，完成受限场景下的定位与建图。

2.1 平台搭建及预处理

本实验中使用的是水下机器人平台BlueROV2（如图3（a）所示）来完成水下航行器的SLAM实验。BlueROV2是一款小型的商用ROV，在原有设备基础上，我们另外配备了 Oculus多波束图像声呐、PathfinderDVL多普勒测速仪等。当前改装完成的 BlueROV2（如图 3（b）所示），能很好地满足水下SLAM实验要求。

在真实环境中，多波束声呐的数据会有很多杂波和野值，直接进行建图效果比较差，需要根据环境的不同进行合适的滤波，以使数据能够更好地反应出环境特征，如图4所示。在本实验中，对声呐的原始数据进行了处理，分别为阈值分割、数据转换、基于距离约束的滤波。

图4 声呐数据处理Fig. 4 Sonar data processing

2.2 室内水池实验

首先，在无人水下运载技术重点实验室多功能水池进行了测试。水池长度为70 m，宽度为44 m，浅水区10 m，深水区15 m，足够支撑我们完成算法的初步验证。

实验水池环境如图5所示。在实验中，我们控制装载多波束声呐的BlueROV沿着水池的墙壁进行扫描，绘制出水池的轮廓，并未在水池中间持续扫描，所以水池中部显示未探测区域。

定位与建图效果如图 6所示。图中，红色部分代表航行器依靠惯导进行航位推算的轨迹，白色部分代表探测的可行驶区域，黑色部分代表SLAM 所构建的障碍物地图，灰色部分代表未探测区域。

图5 实验水池环境Fig. 5 Environment of experimental pool

图6 实验水池建图效果Fig. 6 Construction effect of experimental pool

2.3 野外湖上实验

在湖上实验过程中，乘坐小艇接近目标环境，投放BlueROV2，控制航行器扫描目标环境，进行定位与建图实验。在小艇上有 GPS定位天线，在BlueROV2扫描环境的过程中，小艇紧紧跟随，以获取BlueROV2附近的GPS坐标，为定位与建图实验的定量分析提供真实数据。图7-9分别是3个实验场景的卫星图和实验效果。

由实验结果可以看出，建图效果与卫星图大部分是一致的。对于卫星图与 SLAM 建立的地图形状存在偏差的地方，有以下3方面原因：1）由于水位的变化可能会导致卫星图与实际建立的地图存在偏差；2）由于岛屿和湾的周围分布有暗礁，使得声呐扫描到暗礁，而不是我们从卫星图上看到的岛屿部分；3）岸边一些凹进去的地方里面有树枝和暗礁，航行器不能靠的太近，导致建立的地图有些断开的部分。

图7 场景1卫星图与SLAM建图效果Fig. 7 Satellite image and SLAM construction effect in scene 1

图8 场景2卫星图与SLAM建图效果Fig. 8 Satellite image and SLAM construction effect in scene 2

图9 场景3卫星图与SLAM建图效果Fig. 9 Satellite image and SLAM construction effect in scene 3

针对湖上实验过程中野外极端场景下，相似回环约束对算法的影响可通过图10展现。将外场实验中某段运行轨迹放大，可以看到红色的轨迹是惯导的航位推算结果，相对比较平滑。受湖上相似场景导致的错误回环影响（暗礁以及树枝在水下严重影响了ROV的探测和匹配性能），导致SLAM性能严重下降。其中，白色区域为所建立的地图效果，可以看到，呈现出跳变及不收敛的现象。综上所述，所提容错性SLAM技术方案是可靠的。

图10 相似场景所导致的错误闭环效果Fig. 10 Error closed-loop effect caused by similar scenarios

为了定量分析 SLAM 的定位效果，即地图的位置准确度，我们分别对真值坐标（GPS测量值）、惯导的推算坐标和 SLAM 算法的优化值进行对比分析。

位置比较的结果如图11-13所示，横坐标代表时间轴，纵坐标代表x、y的值随时间的变化，绿色线条表示惯导位置信息，蓝色线条表示 SLAM算法优化过的位置信息，红色线条表示 GPS测量的真值。

由对比结果分析：1）惯导的位置，SLAM 优化值和真值基本上保持一致，而随着时间的推移，惯导的位置信息与真值的曲线变化规律还基本一致，但是它们之间的差值却越来越大；2）SLAM优化后的位置信息变化比较平稳，数值上更加接近于真值，而在对环境扫描完成1个周期时，SLAM优化的位置信息与真值非常接近。

图11 场景1中3种位置坐标对比图Fig. 11 Comparison of three position coordinates in scene 1

图12 场景2中3种位置坐标对比图Fig. 12 Comparison of three position coordinates in scene 2

图13 场景3中3种位置坐标对比图Fig. 13 Comparison of three position coordinates in scene 3

3 结论与先进性

将水池实验以及湖上实验反馈的结果进行总结分析，多波束声呐SLAM实验在建图效果和定位精度方面都有较好的效果，证明了本实验所用多波束声呐 SLAM 在水下环境中的可行性和准确性。可以肯定，基于多波束声呐的定位精度和建图效果突破了水下导航技术中的看不清和看不准2项瓶颈。

在此基础上，其先进性还包括3点：1）如果没有回环场景，SLAM误差增长速率小于惯导误差增长速率；2）如果存在回环场景，SLAM 误差收敛在稳定范围；3）如果存在由相似场景所导致的错误回环，SLAM能具有一定容错性，使得误差继续收敛在稳定范围。

4 结束语

本文主要研究了基于声呐的自主水下航行器的定位与建图，且自主水下航行器的定位与建图实验在实验水池和野外湖泊都取得了不错的效果。这其中也存在一些不足之处，在未来的工作中需要继续努力完成。

1）本文中主要使用声呐图像数据转换为激光雷达数据格式来进行 SLAM 试验，这其中涉及到声呐的滤波和转换过程的准确匹配度。当前还是存在一些问题，并不能很好地把想要的环境特征完全提取出来，且转换后的激光雷达数据与真正的激光雷达数据在描述环境特征方面存在一些差异，导致基于点云匹配的算法不能很好地使用，容易出现误匹配。对于这个问题，未来的工作中会继续试验测试，研究更好的滤波方式，从而提高数据转换准确率。

2）本论文中对自主水下航行器的定位与建图的实验验证平台为BlueROV2，因为本文不考虑控制，所以用其来验证 SLAM 算法的可行性和效果与使用AUV实质上并无太大差别。但BlueROV2质量轻且带缆，在野外湖泊中进行试验很容易受到风浪的影响，导致对其位姿的估计出现较大偏差，建图效果就会比较差。在未来的工作中，我们将使用AUV再次对本文的算法进行验证，以便获得更好的定位与建图效果。