在未知环境中，机器人如何定位、建图与移动？

SLAMTEC

1.前言：

随着计算机技术的迅速发展，机器人研究的深入以及人们对机器人需求的扩大，能自主导航与智能移动的机器人成为研究的热点和重点。

对于已知环境中的机器人自主定位和已知机器人位置的地图创建已经有了一些实用的解决方法。然而在很多环境中，机器人不能利用全局定位系统进行定位，而且事先获取机器人工作环境的地图很困难，甚至是不可能的。这时机器人需要在自身位置不确定的条件下，在完全未知环境中创建地图,同时利用地图进行自主定位和导航。而SLAM（同步定位与地图构建），就被认为是实现真正全自主移动机器人的关键。

机器人在未知环境中从一个未知位置开始移动，在移动过程中根据位置估计和传感器数据进行自身定位，同时逐步完善和构建完整地图，这就是一个SLAM的过程。在SLAM中，机器人利用自身携带的传感器识别未知环境中的特征标志，然后根据机器人与特征标志之间的相对位置和里程计的读数估计机器人和特征标志的全局坐标。这种在线的定位与地图创建需要保持机器人与特征标志之间的详细信息。近几年来，SLAM的研究取得了很大的进展，并已应用于各种不同的环境，如：机器人、AR、VR、无人机、自动驾驶等。

2.SLAM的关键性问题

2.1地图的表示方式

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

(1)栅格地图

栅格地图表示法即将整个环境分为若干相同大小的栅格，对于每个栅格各指出其中是否存在障碍物。这种方法最早由Elfes和Moravec提出，而后Elfes进行了进一步的研究。

优点：

创建和维护容易，尽量的保留了整个环境的各种信息；

同时借助于该地图，可以方便地进行自定位和路径规划。

缺点：当栅格数量增大时(在大规模环境或对环境划分比较详细时)，对地图的维护行为将变得困难，同时定位过程中搜索空间很大，如果没有较好的简化算法，实现实时应用比较困难。

（2）几何信息地图

几何信息地图表示法是指机器人收集对环境的感知信息，从中提取更为抽象的几何特征，例如线段或曲线，使用这些几何信息描述环境。

优点：

更为紧凑，且便于位置估计和目标识别；

几何方法利用卡尔曼滤波在局部区域内可获得较高精度，且计算量小，但在广域环境中却难以维持精确的坐标信息；

缺点：

几何信息的提取需要对感知信息作额外处理，且需要一定数量的感知数据才能得到结果。

（3）拓扑地图

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

优点：

有利于进一步的路径和任务规划；

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

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

缺点：

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

2.2不确定信息的描述

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

如上图所示，感知信息的不确定性必然导致所构建的环境模型也不可能是完全精确的，同样，当依靠模型和感知进行决策时也带有不确定性，即不确定性具有传递性。

对不确定性进行度量的方法主要有概率度量、信任度量、可能性度量、模糊度量和证据理论等。目前，在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 KalmanFilter)。

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）研究更好的将视觉处理与其他传感器结合，提高环境特征提取的精度，减少误差，提高定位和构图的精确性。