基于同时定位与地图构建的AUV导航定位方法综述*

聂永芳 潘 爽 孙东平 施建礼

1. 海军潜艇学院战略导弹与水中兵器系，青岛 266071 2. 清华大学自动化系，北京 100084

自主式水下机器人(Autonomous Underwater Vehicle，AUV)在过去20年中得到迅速发展，逐步进入主流的海洋探测工具行列，由于海洋环境的特殊性，常用于陆地和空中的一些传感器(例如GPS)不能在水中使用；海洋环境本身的不定性和非结构化，使声纳等距离传感器的分辨率较低，所以，实现AUV的精确导航仍是一项艰难的任务。近年来，研究者们开始对AUV运用自身携带的扫描成像声纳,配合平台传感器(惯性测量器件(Inertial Measurement Units，IMU)、罗盘等)和多普勒测速仪(Doppler Velocity Log，DVL)等在航行过程中自主地探索、识别周边环境同时在线地实现自身定位等问题进行研究。上述导航定位方法就是机器人领域研究热点—同时定位与地图构建方法(Simultaneous Localization and Mapping，SLAM)。SLAM理论被研究者们一致认为是移动机器人在未知环境中实现真正自主导航的主要途径。目前，SLAM理论大多应用在室内、室外和空中环境，应用于水下环境的并不多。

以往对水下潜器导航技术进行综述的文献[1-3]中，大多包括了对现有的水下导航技术(船位推算导航、惯性导航、水下声学导航、视觉导航和地球物理导航方法)的全部综述，SLAM技术只作为其中的一小部分，叙述简略。文献[4-5]则是对SLAM理论进行了总体论述，并没有指明具体的应用环境和应用对象。文献[6]虽然是针对水下SLAM的综述，但只介绍了AUV-SLAM的几种常用方法，其他方面并没涉及，到目前为止，尚没有文献对AUV-SLAM问题进行全面综述。近几年随着水下传感器技术的进步，应用于水下SLAM问题的研究也有了新进展，因此，本文主要对SLAM导航方法在AUV上的应用进行综述，主要内容包括国内外的研究现状、水下SLAM的主要实现方法和存在的难点，并探讨了AUV-SLAM未来的发展趋势。

1 AUV-SLAM的研究现状

水下机器人的导航问题是许多国家的研究热点，但研究用SLAM方法进行AUV导航定位研究的国家并不多，国外的研究机构主要集中在美国、西班牙、澳大利亚及英国等国家。

美国的MIT学院是以John Leonard为首的海洋机器人(Marine Robotics Group)研究团队，他们利用第三代Odyssey AUVs，并与MIT的水声学团队和海格兰特学院合作，主要侧重于水下SLAM的应用研究。他们提出了唯距离SLAM[7]，常时间SLAM[8]和递增平滑与构图(Increamental Smooth and Mapping)[9]和视觉SLAM[10-11]等众多理论。Leonard首次将EKF-SLAM运用到基于特征的水下目标识别中，另外，该团队也注重SLAM理论在多水下机器人协作定位和船舱检测方面的研究。

澳大利亚悉尼大学的菲尔德机器人研究中心是世界上著名的水下SLAM研究机构。Hugh Durrant-Whyte是SLAM理论的奠基人，他的团队利用Oberon AUV，主要开展对经典的EKF-SLAM理论的研究。在包含人造路标(位置已知)的游泳池内测试了基于几何投影滤波器(Geometrical Projection Filter，GPF)的SLAM算法[12]。

西班牙的Girona大学计算机视觉和机器人组主要侧重于对水下结构性环境中SLAM方法的研究。西班牙Zaragoza大学的José Neira教授，是联合相容分支定界算法(Joint Compatibility Branch and Bound，JCBB)的首次提出者[13]。他的学生David Ribas用Ictineu AUV在人造结构性环境中使用机械扫描成像声纳(Mechanical Scanned Imaging Sonar，MSIS)获取数据进行导航定位研究，还使用立体照相机用EKF-SLAM算法对海底地形进行了实地勘探[14]。David Ribas给出了著名的西班牙废弃码头的数据集，为后来许多研究水下结构性SLAM算法的学者提供了非常大的帮助。他们合作出版了水下SLAM方面的著作[15]。

中国开展AUV-SLAM研究的高校和研究所也寥寥无几，主要有哈尔滨工程大学、华中科技大学和中国海洋大学等。

表1列出了国内外的主要研究机构、主要研究方向和所用的AUV。

表1 研究AUV-SLAM技术的主要国家和组织机构

2 应用在AUV导航上的主要SLAM方法

SLAM方法大致可分为2类：1)基于贝叶斯估计理论，基础是贝叶斯法则和马尔科夫假设的概率SLAM。目前应用于AUV上的这类SLAM主要是基于扩展卡尔曼滤波器(Extended Kalman Filter，EKF)的EKF-SLAM和基于粒子滤波器(Particle Filter，PF)的FastSLAM；2)应用在AUV上的是非概率SLAM，主要是graph-SLAM。

另一种分类是根据对环境地图的描述[1]，分为基于特征的SLAM和基于视觉的SLAM。在基于特征的SLAM中，特征由外部传感器测得，AUV每运动一步，都会把新的位姿和新测得的特征加入到状态向量中。非结构的自然水下环境不适合基于特征的SLAM，海底的山峰和底谷这些特征在测量时对视角比较敏感，较难辨识和可靠的建模。而基于视觉的SLAM，AUV每运动一步，并不提取任何特征，只是把当前看到的视图和上一次看到的视图作比较。状态向量中可以包括一步或多步的位姿。图1(b)中，在位姿P3时，用V3和V2对比来发现视角的变化。

图1 a) 基于特征的SLAM； b)基于视觉的SLAM

2.1 EKF-SLAM

由于AUV的运动模型与观测模型是非线性的，因此常采用基于EKF的EKF-SLAM。EKF-SLAM的计算分为4个过程[16]：预测、观测更新、数据关联和状态扩维，其具体的运算过程不在本文赘述，有兴趣的读者可参阅文献[16]。

文献[15]论述了EKF-SLAM理论在水下人造环境(港口、船坞、海事平台及大坝等)的应用，依靠机械扫描成像声纳数据和Hough变换理论来提取线特征，产生了一幅基于特征的2D地图。数据相关使用的是典型的最近邻法则，并引入了局部地图的概念。文献[17]论述了EKF-SLAM理论在非结构的浅水中利用前向声纳提取人造点特征，使用GPS提供真实数据，使用DVL和FOG进行航位推算。在浅水区域，图像声纳会受到由地面折射、水面折射和反射引起的噪声的严重影响，这些影响会使计算能力有限的AUV得不到鲁棒的图像特征和数据相关。当AUV提取到鲁棒的图像特征时，EKF-SLAM可以有效地纠正由航位推算带来的误差。文献[18]给出了一种选择性子地图连接SLAM，论述了EKF-SLAM理论在非结构环境中利用侧扫声纳提取3D的点特征，2个子地图之间的数据相关使用了JCBB算法，并用来自REMUS-100 AUV上获得的真实数据进行实验证明，选择性的子地图SLAM可以降低地图的计算代价并保证在大尺度下的构图一致性，且每个子地图所包括的路标数在10～15个之间时所需的计算代价最少，当2个子地图之间共享的路标数为子地图路标数的50%时，可将2个子地图进行合并。

表2列出了3种比较典型的用不同传感器测量路标时使用EKF-SLAM为AUV导航定位的情况。

表2 典型的EKF-SLAM导航在AUV中的应用

由表2可以看出，EKF-SLAM通常用在结构性的环境中。在非结构的环境中，必须有显著的点特征或人工加入点特征才能使用。EKF-SLAM继承了EKF算法的优缺点，算法原理简单，易于实现，但由于系统非线性化引起的误差可能会使算法不收敛，且计算代价大。

2.2 基于粒子滤波器的FastSLAM

FastSLAM的基础是PF[19]，PF的核心思想是利用一系列随机抽取的样本和样本的权重表示状态的后验概率分布，当样本的个数足够多时，通过这样的随机抽样，可以很好地近似真实的后验分布。

FastSLAM算法把联合SLAM状态分成运动部分和条件地图部分以缩小采样空间，机器人的位姿用不同权值的粒子表示，地图用独立的高斯分布解析表示，机器人位姿状态的递归估计采用PF方法，地图状态的递归估计采用EKF方法。而且在给定机器人位姿的条件下，路标之间是相互独立的。

FastSLAM算法的一般包括4个步骤： 1)对每一个粒子，从提议分布中采样； 2)计算重要性权重； 3)进行将权重低的粒子由权重高的粒子来代替的重采样过程； 4)对每一个粒子，在已知机器人位姿的条件下通过测量模型对观测的路标进行EKF更新。

FastSLAM1.0算法和2.0算法的区别在于所选的提议分布模型不同。前者用运动模型当作提议分布进行采样，而后者的提议分布包括了当前的观测。

2.0算法的重采样过程会带来“粒子耗尽”问题。文献[20]对2.0算法进行了改进，采用单个粒子最大似然算法和改进的“反面证据”相结合的数据关联方法；采用基于等级的重采样方法克服粒子退化问题。利用C-Range AUV真实数据验证了改进FastSLAM算法比标准FastSLAM算法有着更好的准确性和一致性。文献[21]利用AUV DEPTHX在墨西哥的萨卡通灰岩洞进行了试验，验证了FastSLAM方法的有效性。

FastSLAM算法用树结构表示路标位置的不确定性，并在此基础上完成采样集合的更新处理，最终算法复杂度相比于EKF-SLAM显著降低。其缺点是存在着粒子“蜕化”问题，而使用重采样又不可避免地带来样本枯竭问题，同时用于生成粒子的建议分布函数即重要性密度函数难以找到一个通用的选择标准。

2.3 Graph-SLAM

Graph-SLAM处理数据的方式和滤波的方法不同，它不是在线纠正位姿，而是把所有数据记下来，进行离线处理。在Graph-SLAM中，机器人的位姿是一个节点或顶点，位姿之间的关系构成边。所以Graph-SLAM问题分解成2个任务[22]：

1)构建图

机器人位姿当作顶点，位姿间关系当作边，这一步常常被称为前端，往往是传感器信息的堆积。

图2 基于图优化的 SLAM 框架

2)优化图

调整机器人位姿顶点尽量满足边的约束，这一步称为后端。

图3 GraphSLAM的模型

Graph-SLAM的目的就是建立好图以后，采用非线性最优算法求解机器人位姿，使得误差平方函数最小。

文献[23]提出了一种新的NAPS(Negative and Positive Scoring)数据算法，实验证明该算法比JCBB匹配性能好。在使用Graph-SLAM时为避免不断增加的图形带来的计算复杂度，每隔一定的时间周期对前面产生的节点进行边缘化处理，只保留1个节点。如图4所示，当AUV往前运动时，节点和观测的特征在每一次迭代后加入到图形中，形成稠密的路径，这种稠密的路径很大。因此，边缘化一部分节点最终形成绿色的综合观测作为Graph-SLAM的输入。时间周期与AUV的运动速度和观测到的特征之间的距离有关。文献[24]使用前视的双频辨识声纳提取图像数据，并利用NDT(Normal Distribution Transform,NDT)算法对数据进行成对注册，该算法可以给出紧凑的扫描图像而无需获得点与点之间的对应关系。

图4 Graph-SLAM计算时的数据综合测量

基于图优化的方法被认为太耗费时间，无法满足SLAM的实时性要求，但随着高效求解方法的出现，基于图优化的方法重新得到重视．在经历一个复兴后，其成为当前SLAM研究的一个热点。

上述3种算法，根据能处理的路标数量、可达区域、计算需求和灵活度等特点[6]，总结在表3中。表中m表示特征数量，N表示采样的粒子数。

表3 3种AUV-SLAM算法对比

3 AUV-SLAM存在的难点问题

目前，理论上比较成熟的水下SLAM方法主要限定在结构化的人工环境或小尺度的浅海中，若要扩大航程或实现更大尺度自然水下环境中的实时导航定位，首先要解决的是随着特征数量的增加造成的地图过大、计算复杂问题；其次，要提高SLAM运行时的鲁棒性，观测特征和地图中的已有特征必须要有正确的数据关联。这2个问题也是普遍存在于整个SLAM研究领域中的2大难点。

3.1 计算复杂度

子地图方法是目前机器人领域学者们解决大尺度SLAM计算复杂度问题的主要思路[25]。分离随机地图构建(Decoupled Stochastic Mapping，DSM)方法是一种高效的大尺度SLAM解决方法[26]，它将环境分成很多个有重叠区域的子地图，降低传统随机地图构建算法的计算压力。DSM方法虽然可以极大降低SLAM算法的计算复杂度，却切断了地图间的关联关系，因此获得的地图都有可能不一致。Williams等人提出的约束局部子地图滤波器[27]提出构建相对于局部参考坐标系的随机地图，并且确保局部地图在统计上是完全独立的。由于大部分更新运算都在各局部子地图内进行，因此能改善计算复杂度问题，但不能保证全局一致性。Atlas[28]在计算时间上有所改善，但在本质上是一种逼近方法，通过牺牲估计精度换取计算复杂度的降低。条件独立局部地图方法[29]通过分享连续子地图间环境特征及机器人状态等信息建立最终全局地图，而且最终地图中不会引入不必要的近似计算。但该方法需要根据先验知识对每一幅新的子地图进行初始化。

3.2 数据关联

在基于特征的SLAM算法中，数据关联是一个最为关键也最为困难的问题，特别是在水下复杂多变的环境中，可靠的数据关联显得尤为重要。数据关联是对2个环境特征的检验进行匹配，确定它们是否对应环境中的同一个特征。水下SLAM问题中，常用的数据关联算法有独立兼容最近邻算法(Individual Compatibility Nearest Neighbor，ICNN)、联合相容分支定界算法(Joint Compatibility Branch and Bound，JCBB)和多假设跟踪算法(Multi-Hypothesis Tracker，MHT)。

3.2.1 ICNN算法

ICNN算法[30]是一种具有固定记忆并能工作于多回波环境的关联方法，该方法将特征预测位置和有效回波间的统计距离即马氏距离作为度量对象，在落入检验门之内的观测值中“唯一性”选取与被观测目标的预测位置间统计距离最小的观测值作为关联对象。

ICNN算法概念简单，计算复杂度低，许多文献中都采用该方法。当AUV位姿不确定性相对于特征间距足够小或是存在较少杂波时，ICNN算法可以表现出较好的工作性能，AUV位姿估计的精度也会较高，在工程上有广泛的应用。但是当AUV处在复杂多变的水下环境中，ICNN的性能急剧下降，不正确的特征匹配会使整个SLAM算法失败。所以ICNN仅适用于特征稀疏和结构性的环境中。

3.2.2 JCBB算法

Neira等人[13]在NN数据关联的基础上，将单个观测、马氏距离和检验门限结合起来，提出了能够解决一组观测数据的关联问题的联合相容性检验(Joint Compatibility Test，JCT)数据关联方法。其基本思想是对同一时刻获得的一帧观测中所有观测值和全部环境特征间关联假设的联合相容性进行检验，从中选取相容性最大的关联假设作为关联结果。由于算法在最优关联假设的搜索过程中使用了分支界定(Branch and Bound，BB)法，所以也被称为联合相容性分支界定关联。

JCBB算法通过纵向和横向遍历搜索测量—地图特征空间组成的解释树寻找最大的联合关联数据集，是一种约束性更强的数据关联算法，关联结果正确率很高。但一方面在噪声或杂波对观测影响较大时，特征的实际观测值和估计值间可能存在较大偏差，过为严格的关联假设判断条件可能会丢弃正确的关联假设；另一方面其计算复杂度随环境特征个数呈指数型增长，因此不适用于大尺度环境。许多学者一直在ICNN和JCBB的基础上进行改进，以得到更合适的水下SLAM数据关联方法。

3.2.3 MHT算法

MHT算法[31]通过一个有限长度的时间滑窗建立多个候选关联假设，通过假设产生、假设概率计算和假设管理技术实现多目标的跟踪。MHT的鲁棒性强，特别适于大尺度的复杂环境[5]。但由于该算法中可行关联假设的个数随着路标个数和测量个数的增加呈指数增长，由此带来存储量和计算量的迅速增长。所以在大规模关联问题中，该方法的使用受到了极大的限制，一般用于稀疏和局部子地图中。

4 发展趋势

AUV-SLAM的发展已经历了十多年，但目前的研究结果基本局限于人工结构、小尺度的浅海和深海的局部区域的水下环境，且大多数为理论成果，工程实现的很少。将来，AUV-SLAM主要以大尺度、自然环境的实用化智能化发展为重点，具体表现在：

1)随着AUV可利用能源的不断发展进步，结合图像处理和模式识别的最新进展，使用双目立体视觉传感器获得更多更准确3D环境特征的水下Visual-SLAM将成为该领域实现水下精确导航的重点研究方向[25,32]；

2)基于随机有限集理论(Random Finite Set,RFS)的RFS-SLAM[33]，将各时刻机器人位姿、传感器观测信息以及环境地图表示为随机有限集，能更好地描述观测噪声、视场、检测概率及虚警等多种传感器信息，避免了数据关联过程，在模型建立上更加准确，能实现机器人位姿和地图状态的同时估计。对复杂多变、噪声未知的水下环境非常适用，具有较大的发展潜力，将成为未来AUV-SLAM问题的一个研究热点；

3)多个AUV进行的协同导航(Cooperative SLAM，C-SLAM)是未来解决水下中间层区域AUV导航定位的重要方法。随着水下通信技术的飞速发展，多个AUV之间将通过共享传感器信息和各自的位姿信息，利用SLAM进行协同导航。与单AUV相比，通过AUV之间的相互协调与合作以及信息共享，C-SLAM可以提高地图创建的效率，提高定位与地图的精度，使多个AUV都具有误差有界能力。2015年，MIT的J.J.Leonard团队首次实现了在低带宽通信条件下的多AUV C-SLAM算法[34]。针对特征稀疏的大尺度水下环境的开发和利用，协同的AUV C-SLAM必将是各国重点发展的水下导航技术；

4)扩展AUV-SLAM的应用环境一直是水下机器人导航的研究热点。将目前局限于2D静态水下结构性环境中的研究与应用扩展到与自然非结构环境契合的动态环境，结合平滑子地图技术构建复杂水下环境的精确3D地图，解决水下检测任务中的避碰和精确对接任务[35]。

参 考 文 献

[1] Liam Paull, Sajad Saeedi, Mae Seto et al. AUV Navigation and Localization-a Review[J]. IEEE Journal of Oceanic Engineering,2014,39(1):131-149.

[2] P.Miller, J.Farrell, Y.Zhao et al. Autonomous Underwater Vehicle Navigation[J]. IEEE Journal of Oceanic Engineering,2010,35(3):663-678.

[3] L.Stutters,H.liu,C.Tiltman et al. Navigation Technologies for Autonomous Underwater Vehicle[J]. System, man, Cybernetics, Part C:Application and Reviews, IEEE Transactions on,2008，38(4): 581-589.

[4] H.Durrant-Whyte, T.Bailey. Simultaneous Localization and Mapping (SLAM):Part I The Essential Algorithms [J]. IEEE Robotics & Automation Magazine,2006,13(2): 99-110.

[5] T.Baile, H.Durrant-Whyte. Simultaneous Localization and Mapping(SLAM):Part II State of the art[J]. IEEE Robotics & Automation Magazine, 2006,13(3):108-117.

[6] F.Hidalgo， T. Braunl. Review of Underwater SLAM Techniques[C]// Proceedings of the 6th International Conference on Automation, Robotics and Applications. Queenstown, New Zealand,2015:306-311.

[7] P.Newman, J.J.Leonard. Pure Range-only Sub-sea SLAM[C]// IEEE International Conference on Robotics and Automation (ICRA), Taipei, Taiwan ,2003:1921-1926.

[8] P.M.Newman, J.J.Leonard, R.J.Rikoski. Towards Constant-time SLAM on an Autonomous Underwater Vehicle Using Synthetic Aperture Sonar[C]// Proceedings of the 11th International Symposium on Robotics Research, Sienna, Italy , 2003:409-420.

[9] M.Kaess, H.Johannsson, R.Roberts et al. iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree[J]. International Journal of Robotics Research,2012,31(2), 216-235.

[10] R.M.Eustice, H.Singh, J.J.Leonard, et al. Visually Mapping the RMS Titanic: Conservative Covariance Estimates for SLAM Information Filters[J]. International Journal of Robotics Research, 2006,25(12), 1223-1242.

[11] O.Pizarro, R.M.Eustice, H.Singh. Large area 3d Reconstructions from Underwater Optical Surveys[J]. IEEE Journal of Oceanic Engineering, 2009,34(20), 150-169.

[12] P.Newman. On the Structure and Solution of the Simultaneous Localization and Map Building Problem[D]. Australian, University of Sydney, 1999.

[13] J.Neira, J.D.Tardos. Data Association in Stochastic Mapping Using the Joint Compatibility Test[J]. IEEE Transactions on Robotics and Automation, 2001:17(6):890-897.

[14] D.Ribas, P.Ridao, J.D.Tardos et a1．Visual SLAM for 3D Large-Scale Seabed Acquisition Employing Underwater Vehicles[C]//IEEE/RSJ Intemational Conference on Intelligent Robots and Systems，Nice，France,2008：1011-1016.

[15] D. Ribas， P. Ridao，J. Neira. Underwater SLAM for Structured Environments.Using an Imaging Sonar[M]. Springer Tracts in Advanced Robotics, B. Siciliano, O. Khatib, and F. Groen, Eds. Heidelberg, Alemania: Springer Verlag, August 2010, no. 65.

[16] I.Tena. Enhanced Concurrent Mapping and Localization Using Forward-looking Sonar[D].England, Heriot-Watt University, 2001.

[17] ACT.Koh, WS.Wijesoma, SL. Pua et al. Shallow waters SLAM Experiments on Meredith AUV Using Forward Looking Sonar[C]//.IEEE Oceans,Biloxi,MS,2009:1-6.

[18] J.Aulinas, X.Lladó, X.Lladó et al. Selective Submap Joining for Underwater Large Scale 6-DOF SLAM[C]//IEEE International Conference on Intelligent Robots & System，2010,6219(1):2552-2557.

[19] M.Montemerlo, S.Thrun, D.Roller et al．FastSLAM 2.0:An Improved Particle Filtering Algorithm for Simultaneous Localization and Mapping that Provably Converges[C]//Proceedings of the International Conference on Artificial Intelligence. Califomia, CA,USA, 2003:115l-1156.

[20] Bo He, Yan Liang, Xiao Feng et al. AUV SLAM and Experiments Using a Mechanical Scanning Forward-Looking Sonar[J].Sensors,2012,12(7):9386-9410.

[21] N.Fairfield, G.Kantor, D.Wettergreen. Towards Particle Filter SLAM with Three Dimensional Evidence Grids in a Flooded Subterranean Environment[C]//IEEE International Conference on Robotics and Automatio. Orlando, Florida 2006:3575-3580.

[22] G.Grisetti, R.Kummerle, C.Stachniss et al. A Tutorial on Graph-based SLAM[J]．IEEE Intelligent Transportation Systems Magazine, 2010,2(4):31-43.

[23] M.F.Fallon, J.Folkesson, H.McClelland et al. Relocating Underwater Features Autonomously Using Sonar-based SLAM[J]. IEEE Journal of Oceanic Engineering, 2013,38(3):500-513.

[24] H. Johannsson, M. Kaess, B. Englot et al. Imaging Sonar-aided Navigation for Autonomous Underwater Harbor Surveillance[C]//IEEE/RSJ International Conference on Intelligent Robots and Systems,2013:4396-4403.

[25] 张书景.大尺度环境中自主式水下机器人同时定位与地图构建算法研究[D].青岛，中国海洋大学，2014.(Zhang Shujing, Simultaneous Localization and Mapping for AUVs in Large Scale Environments [D].Qingdao,Chinese Marine University.)

[26] J.J.Leonard, H.J.S.Feder. Decoupled Stochastic Mapping for Mobile Robot & Auv Navigation[J]. IEEE Journal of Oceanic Engineering, 2001,26(4): 561-571.

[27] S.B.Williams, G.Dissanayake, H.Durrant-Whyte. An Efficient Approach to the Simultaneous Localization and Mapping Problem[C]//IEEE International Conference on Robotics and Automation (ICRA), Washington, DC, USA 2002:406-411.

[28] M.Bosse, P.Newman, J.J.Leonard, et al. Simultaneous Localization and Map Building in Large-scale Cyclic Environments Using the Atlas Framework[J]// International Journal of Robotics Research, 2004,23(12):1113-1139.

[29] P.Piniés，J.D.Tardós. Large-scale Slam Building Conditionally Independent Local Maps: Application to Monocular Vision[J]. IEEE Transactions on Robotics,2008, 24(5):1094-1106.

[30] Y.Bar-Shalom, T.E.Fortmann. Tracking and Data Association[M].Boston, MA: Academic, 1988.

[31] I.J.Cox, J.J.Leonard. Modeling a Dynamic Environment Using a Bayesian Multiple Hypothesis Approach[J]. Artificial Intelligence, 1994,66(2):311-344.

[32] F. Bonin-Font, A. Cosic, P. Lluis Negre et al. Stereo SLAM for Robust Dense 3D Reconstruction of Underwater Environments[C]// IEEE Oceans, Genoa,2015:1-6.

[33] J.Mullane, BN.Vo, M.D.Adams et al. A Random-Finite-Set Approach to Bayesian SLAM[J]. IEEE Transactions on Robotics,2011,27(2):268-282.

[34] L.Paull, Guoquan Huang, M.Seto et al. Communication-constrained Multi-AUV Cooperative SLAM[C]// IEEE International Conference on Robotics and Automation (ICRA),Seattle, Washington, 2015:26-30.

[35] M.VanMiddlesworth, M.Kaess, F.Hover et al. Mapping 3D Underwater Environments with Smoothed Submaps[J]. Springer Tracts in Advanced Robotics,2015,105:17-30.