自主移动机器人同时定位与地图创建（SLAM）方法

1.引言：

机器人的研究越来越多的得到关注和投入，随着计算机技术和人工智能的发展，智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。移动机器人的定位和地图创建是自主移动机器人领域的热点研究问题。对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。然而在很多环境中机器人不能利用全局定位系统进行定位，而且事先获取机器人工作环境的地图很困难，甚至是不可能的。这时机器人需要在自身位置不确定的条件下，在完全未知环境中创建地图,同时利用地图进行自主定位和导航。这就是移动机器人的 同时定位与地图创建 (SLAM) 问题，最先是由SmithSelf 和Cheeseman在1988年提出来的，被认为是实现真正全自主移动机器人的关键。

SLAM问题可以描述为： 机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和传感器数据进行自身定位，同时建造增量式地图。 在SLAM中，机器人利用自身携带的传感器识别未知环境中的特征标志，然后根据机器人与特征标志之间的相对位置和里程计的读数估计机器人和特征标志的全局坐标。这种在线的定位与地图创建需要保持机器人与特征标志之间的详细信息。近几年来，SLAM的研究取得了很大的进展，并已应用于各种不同的环境，如：室内环境、水下、室外环境。

2.SLAM的关键性问题

2.1地图的表示方式

目前各国研究者已经提出了多种表示法，大致可分为三类： 栅格表示 、 几何信息表示 和 拓扑图 表示，每种方法都有自己的优缺点。

栅格地图 表示法即将整个环境分为若干相同大小的栅格，对于每个栅格各指出其中是否存在障碍物。这种方法最早由Elfes和Moravec提出，而后Elfes进行了进一步的研究。它的优点在于 创建和维护容易 ，尽量的保留了 整个环境的各种信息 ，同时借助于该地图，可以方便地进行 自定位和路径规划 。缺点在于：当栅格数量增大时(在大规模环境或对环境划分比较详细时)，对地图的维护行为将变得困难，同时定位过程中搜索空间很大，如果没有较好的简化算法，实现实时应用比较困难。

几何信息地图 表示法是指机器人收集对环境的感知信息，从中提取更为抽象的几何特征，例如 线段或曲线 ，使用这些几何信息描述环境。该方法更为 紧凑 ，且 便于位置估计和目标识别 。几何方法利用卡尔曼滤波在 局部区域内可获得较高精度，且计算量小 ，但在 广域环境中却难以维持精确的坐标信息 。但几何信息的提取需要对感知信息作额外处理，且需要一定数量的感知数据才能得到结果。

拓扑地图 抽象度高，特别在环境大而简单时。这种方法将环境表示为一张拓扑意义中的图(graph)，图中的节点对应于环境中的一个特征状态、地点。如果节点间存在直接连接的路径则相当于图中连接节点的弧。其优点是：

(1)有利于进一步的路径和任务规划，

(2)存储和搜索空间都比较小，计算效率高，

(3)可以使用很多现有成熟、高效的搜索和推理算法。

缺点在于对拓扑图的使用是建立在对拓扑节点的识别匹配基础上的，如当环境中存在两个很相似的地方时，拓扑图方法将很难确定这是否为同一点。

2.2不确定信息的描述

在完全未知环境中由机器人依靠其自身携带的传感器所提供的信息建立环境模型是移动机器人进行自主定位和导航的前提之一。所谓完全未知环境是指机器人对环境一无所知不存在任何先验信息，如环境形状、障碍物位置、人为设定的参照物等。在这种环境下，移动机器人必须依赖传感器所获得的信息，如 里程计 、 声纳 、 激光测距仪 、 视觉 等。由于传感器自身的限制， 感知信息存在不同程度的不确定性 ，例如 激光测距仪 的不确定性主要来自 距离的测量误差 以及 反光镜旋转 和 激光散射 引起的测量角误差。如图1-1所示，感知信息的不确定性必然导致所构建的环境模型也不可能是完全精确的，同样，当依靠模型和感知进行决策时也带有不确定性，即不确定性具有传递性。

对不确定性进行度量的方法主要有 概率度量、信任度量、可能性度量、模糊度量和证据理论 等。目前，在AMR地图构建中使用较多的是概率度量和模糊度量。概率度量主要存在两种形式：

(1)以均值、方差和协方差等概率特征来描述不确定信息。这种度量方法的优点是 均值等概率特征 具有明确的几何意义，缺点是 概率特征的离散计算公式 还没有确定的形式；

(2)以概率模型来描述不确定信息，主要采用 Bayes 法则 与 Markov 假设 。这种度量方法的优点是以随机概率模型描述机器人的位姿和环境信息，鲁棒性非常好，缺点是概率模型的计算量非常大而且必须事先知道模型的先验概率，给实际应用造成了困难。

2.3定位与环境特征提取

移动机器人自定位与环境建模问题是紧密相关的。环境模型的准确性依赖于定位精度，而定位的实现又离不开环境模型。在未知环境中，机器人没有什么参照物，只能依靠自己并不十分准确的传感器来获取外界信息，如同一个盲人在一个陌生环境中摸索的情况。这种情况下，定位是比较困难的。有地图的定位和有定位的地图创建都是容易解决的，但无地图的定位和未解决定位的地图创建如同"鸡--蛋"问题，无从下手。已有的研究中对这类问题的解决方法可分为两类：

1） 利用自身携带的多种 内部传感器 （包括里程仪、罗盘、加速度计等），通过 多种传感信息的融合 减少定位的误差，使用的融合算法多为基于 卡尔曼滤波 的方法。这类方法由于没有参考外部信息，在 长时间的漫游后误差的积累 会比较大。

2） 在依靠内部传感器估计自身运动的同时 ，使用 外部传感器 （如激光测距仪、视觉等）感知环境，对获得的信息进行 分析提取环境特征 并 保存 ，在下一步通过 对环境特征的比较对自身位置进行校正 。但这种方法依赖于能够取得环境特征。环境特征提取的方法有：

（1）       Hough transform是一类基于灰度图检测直线和其他曲线的方法。该方法需要一簇能被搜索的预先准备的特定曲线，并根据显示的灰度图中一簇曲线产生曲线参数。

（2）       Clustering分析是一种数据探测工具，对于未分类样例是有效的，同时，它的目标就是把所针对对象分组成自然类别或基于相似性或距离的簇类。在被提取对象类别未知的情况中，簇技术是一类比HoughTransform更有效的技术。簇类应是以“凝聚”为中心，而不是支离破碎的、不相交的。而环境特征有时是很难提取出的，例如：环境特征不够明显时或者传感器信息比较少，难以从一次感知信息中获得环境特征。

2.4数据关联

数据关联是 对两个特征标志进行匹配 ，确定它们是否对应环境中的同一物体。SLAM中的数据关联主要需要完成三个任务：1）新特征标志的检测2）特征标志的匹配3）地图之间的匹配。虽然在目标跟踪、传感融合等领域，数据关联已经得到较好的解决，但是这些方法的计算量大，不能满足SLAM的实时性要求。实现 m 个标志 与 拥有 n 个标志的地图 之间的数据关联的 复杂度 与m之间 呈指数关系 ，假设每个观测到的标志i有 个可能的匹配，那么对于m个标志需要在指数空间 =  中搜索正确的匹配。数据关联的搜索空间与环境的复杂程度以及机器人的定位误差有关，环境的复杂程度的增加会使m增大，而误差的增大会使Ni 增大。

2.5累积误差

SLAM中的误差主要来自三个方面：1） 观测 误差2） 里程计的误差 3）错误的数据关联带来的误差。当机器人在已知地图的环境中进行定位时，机器人可以通过观测位置已知的特征标志对里程计的误差进行补偿，每一次观测使机器人的位置误差趋向于观测误差与特征标志的位置误差之和。然而在SLAM中，由于机器人的位置和环境中的特征标志的位置都是未知的，观测信息不能有效纠正里程计的误差，机器人的位置误差随着机器人的运动距离而增大。而机器人的位置误差的增大将导致错误的数据关联，从而增大特征标志的位置误差：反过来，特征标志的误差又将增大机器人的位置误差。因此，机器人的位置误差与特征标志的位置误差密切相关。它们之间的相互影响使机器人和特征标志的位置估计产生累计误差，难以保证地图的一致性。

3.SLAM的实现方法

目前SLAM方法大致可分为两类：1）基于概率模型的方法：基于卡尔曼滤波的完全SLAM、压缩滤波、FastSLAM等2）非概率模型方法：SM-SLAM、扫描匹配、数据融合(dataassociation)、基于模糊逻辑等。

3.1基于卡尔曼滤波器的实现方法

从统计学的观点看，SLAM是一个滤波问题，也就是根据系统的 初始状态和从 0 到 t 时刻 的 观测信息 与 控制信息 （ 里程计的读数 ） 估计 系统的 当前状态 。在SLAM中，系统的状态 = ，由机器人的 位姿 r 和地 图信息 m 组成（包含各特征标志的位置信息）。假设系统的运动模型和观测模型是带高斯噪声的线性模型，系统的状态 服从高斯分布，那SLAM可以采用卡尔曼滤波器来实现。基于卡尔曼滤波器的SLAM 包括系统 状态预测 和 更新 两步，同时还需要进行 地图信息的管理 ，如： 新特征标志的加入 与 特征标志的删除 等。

卡尔曼滤波器假设系统是线性系统，但是实际中机器人的运动模型与观测模型是非线性的。因此通常采用 扩展卡尔曼滤波器 （Extended Kalman Filter），扩展卡尔曼滤波器通过一阶泰勒展开来近似表示非线性模型。另一种适用于 非线性模型 的卡尔曼滤波器是 UKF （ Unscented Kalman Filter ） ，UKF采用 条件高斯分布 来近似 后验概率分布 ，与EKF相比，UKF的线性化精度更高，而且不需要计算雅可比矩阵。

卡尔曼滤波器已经成为实现SLAM的基本方法。其协方差矩阵包含了 机器人的位置 和 地图的不确定信息 。当机器人连续地观测环境中的特征标志时，协方差矩阵的任何子矩阵的行列式呈单调递减。从理论上讲，当观测次数趋向于无穷时， 每个特征标志的协方差 只与机器人 起始位置的协方差有 关。卡尔曼滤波器的时间复杂度是O( )，由于每一时刻机器人只能观测到少数的几个特征标志，基于卡尔曼滤波器的SLAM的时间复杂度可以优化为 O( ) ，n表示地图中的特征标志数。

3.2局部子地图法

局部子地图法 从空间的角度将SLAM分解为一些较小的子问题。子地图法中主要需要考虑以下几个问题：1）如何划分子地图2）如何表示子地图间的相互关系3）如何将子地图的信息传递给全局地图以及能否保证全局地图的一致性。

最简单局部子地图方法是不考虑各子地图之间的相互关系，将全局地图划分为包括 固定特征标志数的独立子地图 ，在各子地图中分别实现SLAM，这种方法的时间复杂度为O(1)。但是，由于丢失了表示不同子地图之间相关关系的有用信息，这种方法不能保证地图的 全局一致性 。

对此，Leonard 等人提出了DSM（DecoupledStochastic Mapping）方法，DSM中各子地图分别保存自己的机器人位置估计，当机器人从一个子地图A进入另一个子地图B时，采用基于EKF的方法来将子地图A中的信息传送给子地图B；B.Williams等人提出了一种基于 CLSF （ConstrainedLocal Submap Filter）的SLAM方法，CLSF 在地图中创建全局坐标已知的子地图，机器人前进过程中只利用观测信息更新机器人和局部子地图中的特征标志的位置，并且按一定的时间间隔把局部子地图信息传送给全局地图。 虽然实验表明这两种算法具有很好的性能，但是没有从理论上证明它们能够保持地图的一致性。J.Guivant等人提出了一种没有任何信息丢失的SLAM优化算法CEKF(CompressedExtended Kalman Filter)。CEKF将已经观测到的特征标志分为A与B部分，A表示与机器人当前位置相邻的区域，被称为 活动子地图 。当机器人在活动子地图A中运动时，利用观测信息实时更新机器人的位置与子地图A，并采用 递归 的方法记录观测信息对子地图B的影响；当机器人离开活动子地图A时，将观测信息无损失地传送给子地图B，一次性地实现子地图B的更新，同时创建新的活动子地图。该方法的计算时间由两部分组成：活动子地图中的SLAM，其时间复杂度为O( )， 是活动子地图A中特征标志的数目；子地图B的更新，其时间复杂度为O( ) ， 是地图B中特征标志的数目。当子地图合并的时间间隔较大时，CEKF能有效减少SLAM的计算量。

3.3去相关法

降低SLAM复杂度的另一种方法是将表示相关关系的协方差矩阵中一些 取值较小的元素忽略掉 ，使其变为一个 稀疏矩阵 。然而这也会因信息的丢失而使地图失去一致性。但是，如果能改变协方差矩阵的表示方式，使其中的很多的元素接近于零或等于零，那么就可以将其安全地忽略了。基于扩展信息滤波器EIF（ExtendedInformation Filter）的SLAM就是出于这一思想。EIF EKF的基于信息的表达形式，它们的区别在于表示信息的形式不一样。EIF采用协方差矩阵的逆矩阵来表征SLAM中的不确定信息，并称之为信息矩阵。两个不相关的信息矩阵的融合可以简单地表示为两个矩阵相加。信息矩阵中每个非对角线上的元素表示机器人与特征标志之间或特征标志与特征标志之间的一种约束关系，这些约束关系可以通过系统状态的信关系进行局部更新。这种局部更新使得信息矩阵近似于稀疏矩阵，对其进行稀疏化产生的误差很小。根据这一点，S.Thrun等人提出了一种基于 稀疏信息滤波器 SEIF(Sparse Extended InformationFilter)的SLAM方法，并证明利用稀疏的信息矩阵实现SLAM的时间复杂度是O(1)。虽然EIF可以有效降低SLAM的时间复杂度，但是在地图信息的表示和管理方面还存在一些问题。首先，在常数时间内只能近似算得系统状态的均值；其次，在基于EIF 的SLAM 方法中，特征标志的增删不方便。

3.4分解法（FastSLAM）

M.Montemerlo 等人提出了一种基于粒子滤波器(ParticleFilter) FastSLAM 方法。FastSLAM 将SLAM分解为 机器人定位 和 特征标志 的位置估计两个过程。 粒子滤波器中的每个粒子代表机器人的一条可能运动路径，利用观测信息计算每个粒子的权重，以评价每条路径的好坏。对于每个粒子来说，机器人的运动路径是确定的，因此特征标志之间相互独立，特征标志的观测信息只与机器人的位姿有关，每个粒子可以采用n个卡尔曼滤波器分别估计地图中n个特征的位置。假设需要k个粒子实现SLAM、FastSLAM，总共有kn个卡尔曼滤波器。FastSLAM的时间复杂度为O(kn)，通过利用树型的数据结构进行优化，其时间复杂度可以达到O(klog n)。Fast2SLAM方法的另一个主要优点是通过采用粒子滤波器估计机器人的位姿，可以很好地表示机器人的非线性、非高斯运动模型。

3.5基于多机器人协作的SLAM

一些研究者对基于多机器人协作的同时定位与地图创建CSLAM（CooperativeSimultaneous Localizationand Mapping）进行了探讨和研究。 与单机器人相比，通过机器人之间的相互协调与合作以及信息共享，CSLAM可以提高地图创建的效率和提高定位与地图的精度。CSLAM按照地图的存储与处理方式的不同可以分为两大类型：集中式CSLAM和分布式CSLAM。在集中式CSLAM中，存在一个中央处理模块，每个机器人分别在自己所在的局部地图中进行定位与地图创建，然后利用无线通信装置将在局部地图中获得的信息传送给中央模块。这种方法通过子地图的匹配，可以充分利用子地图间的冗余信息提高定位与地图创建的精度。但是，当机器人数量增加时中央模块的计算量会显著增大，而且集中式的信息传递需要很大的带宽；系统的可靠性也比较低，一旦中央模块出现故障,整个系统都会陷入瘫痪状态。在分布式CSLAM中，不存在中央模块，每个机器人都拥有自己的全局地图，在每一时刻机器人把来自其他相邻机器人的信息和自己的观测信息融合到自己的全局地图中，然后以点对点的方式将新的信息传送给其他机器人。每个机器人只能获得与其相邻的机器人的位置信息，不知道整个系统的拓扑结构。这种方法与分布式的信息融合十分相似，可以利用信息滤波器来实现。由于两个不相关信息矩阵的信息融合可以通过两个矩阵的相加而实现，所以利用信息滤波器实现分布式CSLAM可以避免复杂的计算。

4.研究方向与发展趋势

综上所述，近几年来机器人领域的研究者对SLAM进行了大量的研究，特别是在降低计算复杂度、提高鲁棒性等方面取得了很大的进展。随着研究的深入，以下的几方面成为了当前SLAM的研究热点方向。

1.    扩展SLAM的应用环境：将目前局限二维静态环境中的研究与应用扩展到与现实中的环境切合的动态的三维环境；

2.    深入研究基于多机器人协作的SLAM，提高其应用水平；

3.    研究更有效的SLAM实现方法，将人工智能、智能控制等领域的方法引入到SLAM中，开发更有效的SLAM算法.；

4.    研究更好的地图表达方式，特别是复杂地形和大环境中的地图表达方式；

5.    研究更好的将视觉处理与其他传感器结合，提高环境特征提取的精度，减少误差，提高定位和构图的精确性。

自主移动机器人同时定位与地图创建（SLAM）方法1.引言：机器人的研究越来越多的得到关注和投入，随着计算机技术和人工智能的发展，智能自主移动机器人成为机器人领域的一个重要研究方向和研究热点。移动机器人的定位和地图创建是自主移动机器人领域的热点研究问题。对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。然而在很多环境中机器人不能利用全局定位系统进行定位，而且 那么如何生成这种拓扑 地图 呢？ 本文主要目的是生成一个简单的拓扑 地图 ，并在g2o_viewer 中显示。1.拓扑 地图 的生成1.1安装g2o_viewer参考我的上一篇博客 http://blog.csdn.net/ktigerhero3/article/details/754574321.2生成拓扑 地图 本文参考g2o 正如图中所示，机器人自主 定位 导航技术中包括： 定位 和 地图 创建 （ SLAM ）与路径规划和运动控制两个部分，而 SLAM 本身只是完成机器人的 定位 和 地图 创建 ，二者有所区别。 首先明确一下自主导航的两大解决方案： 1.传统方案（稍微成熟一点）： SLAM +路径规划+运动控制； 2.深度学习（还不完善）： 这一两年，深度学习成为人工智 Gmapping功能包 一种粒子滤波算法，将 定位 与 建 图分离，先进行 定位 、再进行 建 图，适合构 建 小场景环境下的 地图 信息。 需要 机器人提供里程计信息作为先验知识。 虽然可以降低对激光雷达频率的要求，但是也对机器人硬件提出了需求 Ros对gampping做了很好的封装，通过功能包提供的接口进行使用。要使用gmapping， 需要 机器人能够发布深度信息（雷达传感器为主，也可以使用三维相机转换成二维雷达信息，LaserScan消息结构的数据）、IMU信息（惯性测量单元获取的加速度信息，可有可无）、里