迎宾机器人的快速同时定位与地图生成(FastSLAM)：机器人定位和特征标志的位置估计

为了克服基于扩展卡尔曼滤波器 SLAM方法的缺点，Montemerlo等人提出了一种基 于 Rao -Blackwellized 粒子滤波器的快速 SLAM 算法，并称为 FastSLAM68][69] 。 FastSLAM 将 SLAM分解为机器人定位和特征标志的位置估计两个过程。粒子滤波器中 的每个粒子代表机器人的一条可能运动路径，利用观测信息计算每个粒子的权重，以评价 每条路径的好坏。对于每个粒子来说，机器人的运 动路径是确定的，因此特征标志之间相互d立，特 征标志的观测信息只与机器人的位姿有关，每个粒 子可以采用N 个卡尔曼滤波器分别估计地图中N 个特征的位置。假设需要k 个粒子实现 SLAM, FastSLAM总共有kn 个卡尔曼滤波器。 FastSLAM 的时间复杂度为0(kn), 通过利用树型的数据结构 进行优化，其时间复杂度可以达到0(klog₂n)。

FastSLAM方法的另一个主要优点是通过采用粒子滤波器估计机器人的位姿，可以很好地表示机器人的非线性、非高斯运动模型。图1.57给出了在 FastSLAM 实 验 中 生 成 的 地 图 。

由于粒子滤波器的采样实际上是从运动模型决定的提议分布中抽取的，如果提议分 布和实际的后验分布的形状相似，那么根据提议分布抽取的采样在利用权值进行补偿后 离散的采样能够很好地表示后验分布。但是，如果提议分布与实际后验分布相差加大，权 重函数将位于提议分布密度的尾端，这将会导致在权重取值较大的区域采样很少，此时离 散采样所表示的概率分布与实际的后验分布存在着较大的差别，从而导致粒子滤波器的 精度很低，需要大量的采样才能较好地表示后验分布。

粒子滤波器的另一个问题是早熟现象，即经过若干次迭代之后，大多数的采样权重都 趋于零，从而只有少数的采样真正对系统的状态估计起作用。虽然通过重新采样可以在 一定程度上避免这种现象，但是由于权重大的采样会被多次复制，权重小的采样会被忽 略，这样经过若干次迭代后采样会集中在某一个较小的区域。例如，在对称性较高的结构 化环境中，完成机器人的定位，需要长时间跟踪多个机器人的位姿假设，产生错误的定位 结果。

针对粒子滤波器存在的缺陷，一些研究者提出了改进的方法。为了使采样能够更好 地表示系统的后验分布，Thrun等人将似然函数也作为提议分布的一部分，提出了混合形 式的提议分布，按照这种提议分布抽取的采样能够融合当前的观测信息，因此能够更好地 表示系统的后验分布，但是这种方法使采样阶段的计算量大大增加。为了减少粒子滤波 器的计算量，提高效率，Fox 等人提出了自适应粒子滤波器，这种基于Kullback-Leibler

距离(KLD) 取样的粒子滤波器能够根据系统状态的不确定性自适应调整采样数的多 少。蒋正伟[70]等人采用连续窗口滤波法根据估计状态不确定性刷新采样粒子数，通过仿 真实验成功地解决了机器人定位非线性、非高斯分布的状态估计问题。Hahnel71 等人通 过扫描匹配算法修正了里程计的读数，改进了机器人的运动模型，获得了较好的提议分 布，有效地完成了大范围环境下的地图创建。Grisetti[72]等人根据激光测距传感器感知 环境时具有较高的峰值特点，假定感知模型和运动模型混合形成新的自适应提议分布仍 然为高斯分布，不仅融合了当前的观测值，而且减少粒子滤波器的计算量，采用了自适应 的运动模型来决定提议分布，根据提议分布和后验分布的差别，收缩或者扩张提议分布， 也较好地估计了后验分布。

SLAM算法的提出和应用得益于机器人硬件设备的推广。目前较为流行的 FastSLAM 同样离不开如激光、声纳等非视觉传感器获取的信息。从地图创建的角度考虑 FastSLAM,地图信息可能是记录的路标位置，也有可能是某一栅格存在障碍物的可能 值。在大规模未知环境尤其是拥挤的非结构化环境中，通常不存在人工路标供机器人提 取和匹配，由机器人提取自然特征作为地图路标是要的选择。从激光传感器信息中提 取的线段、曲线或拐角特征却难以区别于其他地点获取的同样特征，这就为FastSLAM 在 地图创建中的数据关联提出了难题。从实际应用的角度来看，地图创建的目的是为了定 位和路径规划，当机器人被放置于已知的大规模环境中而初始位姿未知时，根据激光传感 器获取的直线或拐角特征很难让机器人在地图中找到相应的坐标。所以，在FastSLAM 创建的地图中实现机器人的“诱拐恢复”是一个极富挑战性的问题。