-
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建模数学机理
-
因子图核心思想是根据里程计（惯导）及不同点的量测信息对机器人及其周围环境进行最大后验概率估计，即
-
式中：是机器人的输入控制；为所探测到得测量信息；为平台的姿态；路标点位置。假设系统过程模型和观测模型服从高斯噪声分布，即
-
考虑到比例因子，有：
-
其最大后验概率估计可转化为
-
针对地形近似所导致的错误回环问题，需考虑在图优化过程中引入开关变量通过非线性迭代，实现优化过程中自适应关闭由相似场景所导致的错误回环，提高并完善同时定位与地图构建算法的容错性。如图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实验要求。
-
图3 实验平台
-
Fig.3 Experimental platform
-
在真实环境中，多波束声呐的数据会有很多杂波和野值，直接进行建图效果比较差，需要根据环境的不同进行合适的滤波，以使数据能够更好地反应出环境特征，如图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再次对本文的算法进行验证，以便获得更好的定位与建图效果。
-
参考文献
-
[1] 蒋新松.水下机器人[M].沈阳：辽宁科学技术出版社，2000.
-
[2] 付梦印，刘飞，袁书明，等.水下惯性/重力匹配自主导航综述[J].水下无人系统学报，2017，25(2)：35-47.
-
[3] 赵俊波，葛锡云，冯雪磊，等.水下 SINS/DVL 组合导航技术综述[J].鱼雷技术，2018，26(1)：2-9.
-
[4] LEE J G，PARK C G，PARK H W.Multiposition alignment of strapdown inertial navigation system[J].IEEE Transactions on Aerospace and Electronic systems，1993，29(4)：1323-1328.
-
[5] 宗文鹏，李广云，李明磊，等.激光扫描匹配方法研究综述[J].中国光学，2018，11（6）：914-930.
-
[6] 朱磊，樊继壮，赵杰.未知环境下的移动机器人SLAM方法[J].华中科技大学学报：自然科学版，2011，21（7）：9-13.
-
[7] 邸凯昌，万文辉，赵红颖，等.视觉SLAM技术的进展与应用[J].测绘学报，2018，47(6)：770-779.
-
[8] KLIPPENSTEIN J，ZHANG H.Quantitative evaluation of feature extractors for visual SLAM[C]//Conference on Computer & Robot Vision.US：IEEE，2007.
-
[9] ENGELHARD N，ENDRES F，HESS J，et al.Real-time 3D visual SLAM with a hand-held RGB-D camera[C]//Proc.of the RGB-D Workshop on 3D Perception in Robotics at the European Robotics Forum.Vasteras：Association for Computing Machinery，2011.
-
[10] 易柯敏，沈艳霞.激光SLAM导航移动机器人定位算法研究综述[J].机器人技术与应用，2019(5)：25-28.
-
[11] 李晨曦，张军，靳欣宇，等.激光雷达SLAM技术及其在无人车中的应用研究进展[J].北京联合大学学报，2017，31(4)：67-75.
-
[12] HESS W，KOHLER D，RAPP H，et al.Real-time loop closure in 2D LIDAR SLAM[C]//2016 IEEE International Conference on Robotics and Automation(ICRA).US：IEEE，2016.
-
[13] HENING S，IPPOLITO C A，KRISHNAKUMAR K S，et al.3D LIDAR SLAM integration with GPS/INS for UAVs in urban GPS-degraded environments[C]//AIAA Information Systems-AIAA Infotech @ Aerospace.US：AIAA，2017.
-
[14] MORENO L，GARRIDO S，BLANCO D，et al.Differential evolution solution to the SLAM problem[J].Robotics and Autonomous Systems，2009，57（4）：441-450.
-
[15] 陈质二，俞建成，张艾群.面向海洋观测的长续航力移动自主观测平台发展现状与展望[J].海洋技术学报，2016，35（1）：122-130.
-
[16] 李岳明，李晔，盛明伟，等.AUV 搭载多波束声纳进行地形测量的现状及展望[J].海洋测绘，2016，36（4）：7-11.
-
[17] XIAO G D，WANG B，DENG Z H.An acoustic communication time delays compensation method for master-slave AUV cooperative navigation[J].IEEE Sensors Journal，2016，12（99）：1-1.
-
[18] KALMAN R E.A new approach to linear filtering and prediction problems[J].Journal of Basic Engineering Transactions，1960，82（1）：35-46.
-
[19] AHN S H，CHOI J W，DOH N L，et al.A practical approach for EKF-SLAM in an indoor environment：fusing ultrasonic sensors and stereo camera[J].Autonomous Robots，2008，24（3）：315-335.
-
[20] CHEN L R.Sequential Monte Carlo methods for dynamic systems[J].Journal of the American Statistical Association，1998，93（443）：1032-1044.
-
[21] KITAGAWA G.Monte Carlo filter and smoother for non-Gaussian nonlinear state space models[J].Journal of Computational & Graphical Statistics，1996，5(1)：1-25.
-
[22] THRUN S，KOLLER D，GHAHRAMANI Z，et al.Simultaneous mapping and localization with sparse extended information filters：theory and initial results[J].Algorithmic of Robotics V，2004，7（2）：363-380.
-
[23] JULIER S，UHLMANN J，DURRANT-WHYTE H F.A new method for the nonlinear transformation of means and covariances in filters and estimators[J].IEEE Trans Automatic Control，2000，14（5）：51-59.
-
[24] LOURAKIS M I A.SBA：A software package for generic sparse bundle adjustment[J].Acm Trans.math.softw，2009，36（1）：2-7.
-
[25] CARLEVARIS B N，KAESS M，EUSTICE R M.Generic node removal for factor-graph SLAM[J].IEEE Transactions on Robotics，2014，30(6)：1371-1385.
-
[26] DELLAERT F.Factor graphs，Bayes trees and preconditioning for SLAM and SFM[J].Fisheries Management & Ecology，2003，10(10)：377-384.
-
摘要
对自主水下航行器定位与建图进行了研究。分析了SLAM的算法原理，采用BlueROV2和声呐、DVL、IMU搭建了实验平台，并选择合适的算法框架完成了完整的算法设计。使用声呐为主要感知单元完成自主水下航行器的SLAM实验，并对声呐数据的杂波、野值、虚警等进行了滤波处理。最后，在水池、湖泊试验中对所研究的SLAM算法的定位与建图进行了验证，取得了良好的试验结果。
Abstract
This paper studies the positioning and mapping of autonomous underwater vehicles. The principle of SLAM algorithm is analyzed，the experimental platform is built with BlueROV2，sonar，DVL and IMU，and the appropriate algorithm framework is selected to achieve the complete algorithm design. Sonar，as the main sensing unit，is used to complete the SLAM experiment of autonomous underwater vehicles，and the clutter，outliers and false alarms of sonar data are filtered. In the pool and lake test，the positioning and mapping algorithm of the studied SLAM algorithm are verified，and good test results are obtained.