移动机器人同时定位和地图创建(SLAM)原理：机器人的位姿和地图信息

机器人通过外部传感器信息获得 周围环境相对位置信息来确定自身位姿的过程。在全局定位中，由于机器人只能观测到 环境中的一些局部信息，因而环境的对称性可能会使机器人在很多不同的位置获得完全 相同的观测信息，产生多个可能的机器人位姿假设。全局定位中机器人位姿的后验概率 分布是一个多峰分布，基于高斯分布的不确定性表示方法不适用于全局定位。在全局定 位过程中如果在移动机器人里程计没有记录的情况下，将机器人从一个地方搬到另外一 个地方会产生机器人“诱拐”(Kidnapped Robot)现象[148]。

当将机器人从A点搬到B 点时，机器人里程计无法记录实际位姿变 化。根据传感器信息，机器人也不能立刻确定自己在环境中的位置，需要根据多次观测信 息重新定位。所以很多研究者认为机器人“诱拐”问题也是一个机器人的全局定位问 题。与全局定位不同的是，机器人可能不知道自己什么时候发生了“诱拐”,该方法常用 来测试定位方法的鲁棒性。在全局定位过程中，存在很多不确定性因素。先是机器人 本身的不确定性，如轮子打滑所造成的里程计误差累积，传感器噪声所造成的读数不可 信。其次是机器人所处的环境也是不可预知的，如人的走动和物体的移动所造成的环境 变化。这些不确定性会使定位变得更加困难。因此，近来越来越多的研究者一方面采用 可靠的传感器，如采用能感知丰富环境信息的视觉传感器和测距精度较高的激光测距器； 另一方面把概率理论应用到移动机器人定位中，试图采用概率定位方法解决不确定性问 题。

SLAM的基本原理是利用已经创建的地图修正基于运动模型的机器人位姿估计误差，提高定位精度；同时根据可靠的机器人位姿，创建出精度更高的地图[149]。

未知环境下的SLAM方法的核心都是围绕和针对传感器信息和环境中存在的不确定 性而展开的。关于传感器的不确定，以Z常用的里程计为例，其典型的误差积累如图1.61 所示。其中，左图是d立利用里程计定位、d立利用激光传感器感知环境所创建的地图， 由于没有进行里程计误差补偿，几次创建的地图差异很大，与实际环境也不符；右图是采用SLAM 创建的地图，基于SLAM 可以利用已创建的地图修正里程计的误差。这样机器 人的位姿误差就不会随着机器人的运动距离的增大而无限制增长，因此可以创建精度更 高的地图，也同时解决了未知环境中的机器人定位问题。

在SLAM中，系统的状态由机器人的位姿和地 图信息(包含各特征标志的位置信息)组成。假设 机器人在t 时刻观测到了特征m₁, 如图1 . 62所 示。根据观测信息只能获得特征m₁ 在机器人坐标 系R 中的坐标，机器人需要估计机器人自己本身在 世界坐标系W中的位姿，然后通过坐标变换才能 计算特征的世界坐标。可见，在地图创建的过程 中，需要计算机器人的位姿，也就是进行机器人的 定位。然而，根据里程计获得的机器人位置信息很 不准确，显然错误的位置信息将会导致地图的不准 确。

在初始时刻，地图中没有任何特征。当机器人在t 时刻观测到特征m 时，可以根据机 器人的位姿(x,y:,θ.), 以及观测到的特征m₁ 在机器人坐标系下的坐标计算出特征的世 界坐标(x¹,y!), 并且将特征m₁ 加入到地图中。当机器人运动一步之后，根据里程计信息 可以预测到机器人的位姿将变为(x+1,Y:+1,0,+1)。根据特征m₁ 的世界坐标(x¹,y) 可 以 计算出当机器人位姿为(x:+1,y:+1,0.+1) 时 m₁ 在机器人坐标系下的坐标。然而，机器人同 时也会再次观测到特征m, 而且获得m₁ 在新的机器人坐标系下的坐标(x+1,y+1) 。 实 际 观测到的特征坐标与计算获得的坐标(x^+1,y+1) 将存在差别。这种差别是由两种原因引 起的，其一是因为机器人的预测位姿(x:+1,y:+1,0+1)不准确，其二是因为根据以前的观测 信息计算获得的特征的世界坐标(x¹,y¹) 不准确。在SLAM中，根据这种差别重新计算特 征的世界坐标(也就是地图创建),同时重新估计机器人的位姿(也就是机器人的定位)。 当机器人继续运动时，它将观测到更多的特征，根据同样的方法，机器人会把它们加入到 地图中，并且根据观测到的信息更新机器人的位姿以及它们的世界坐标。简单地说， SLAM利用观测到的特征计算它们的世界坐标以实现地图创建，同时更新机器人的位姿 以实现机器人的定位，如图1.63所示。

SLAM 的概率描述为：p(81,MI z1:,uo:,k1)=p(x,I z1:,u₀:-1,k1:),其中，s1:=81, S2, … ,s, 和z1: 分别表示机器人从1到t 时刻的运动路径和感知信息，u₁-1表 示t-1 到 t 时 刻的运动控制信息，z, 表示机器人的当前感知信息。当机器人穿过一个未知环境时，设t 时刻机器人位姿s,=[x₁,y:,θ,], 已经观测到的地图为M, 其中，m 表示第k 个路标，K表 示已经观测的路标数，k,∈{1,…,N} 表示t 时刻感知到的路标索引号。

系统的完整状态 可以表示为x,=[s1,M]T,SLAM 的图形模式如图1.64所示，机器人从位姿s。开始通过控 制命令序列uo,u₁,…,u1-1 移动，随着机器人的移动，附近的路标被感知到，时刻t=1, 感 知 到路标m₁, 并获得测量数据z₁ (包括距离和方向),时刻t=2, 感知到路标m₂, 并在时刻t=3,重新感知到路标m₁, 现在已经形成的地图为：M={m₁,m₂,m 。SLAM 的输入信息是， 路标观测信息z1:, 以及运动控制信息u₀:-1 。SLAM 的目的是，根据输入信息估计机器人 运动路径s1 以及地图M。