机器人 / 系统课时
SLAM 为什么需要同时估计位姿和地图
SLAM 为什么需要同时估计位姿和地图
机器人进入没有现成地图的走廊时,激光雷达只能测量障碍物相对自身的距离,里程计也只能估计机器人移动了多少。要生成空间位置一致的地图,就必须同时推断机器人在哪里、周围环境是什么样。本篇通过走廊测距算例解释两者为何相互依赖,以及 ROS 2 中的输入与输出。
学习目标
- 区分机器人位姿、环境地图和传感器观测,解释它们的相互依赖关系。
- 用具体测距数据说明里程计误差如何影响建图,以及联合估计如何修正误差。
- 识别 ROS 2 SLAM 的主要输入、输出、坐标系和典型失效现象。
位姿与地图为什么相互依赖
对于平面移动机器人,位姿通常用三个量表示:位置 x、y 和朝向角 yaw。地图则描述墙壁、障碍物或环境特征在统一坐标系中的位置。激光雷达测得的是传感器到障碍物的相对距离,而不是障碍物的全局坐标。
假设机器人正对一面墙,激光测得距离为 3.0 m。如果机器人位于 x=0 m,墙可能位于 x=3.0 m;如果机器人位于 x=2.0 m,墙就可能位于 x=5.0 m。只看这一条测距数据,无法确定机器人和墙壁各自的位置。
反过来,如果已经知道墙的位置,就能利用测距推断机器人位姿。这形成了两个相互依赖的问题:
- 定位依赖地图:必须知道观测到的墙壁在哪里,才能判断机器人在哪里。
- 建图依赖定位:必须知道每次扫描时机器人在哪里,才能把不同时间的障碍物观测放进同一张地图。
轮式里程计能够提供相邻时刻的运动估计,但车轮打滑、编码器误差和转向误差会不断累积。如果只用里程计拼接扫描,走得越远,墙壁轮廓通常越容易发生错位。
SLAM(Simultaneous Localization and Mapping,同时定位与建图)将机器人轨迹和地图作为相关的未知量,利用运动约束与环境观测共同估计它们。初始位姿通常被选作坐标参考;没有外部定位基准时,地图的绝对位置和方向本身无法唯一确定。
用走廊测距理解联合估计
考虑一辆沿 x 轴前进的机器人。为了简化问题,假设机器人始终朝向正 x 方向,前方只有一面静止的墙,且第一次观测时定义机器人位于 x=0 m。
| 测量项目 | 数值 | 含义 |
|---|---|---|
| 第一次激光测距 | 3.00 m | 初始位置到墙壁的距离 |
| 里程计位移 | 1.10 m | 机器人估计自己前进的距离 |
| 第二次激光测距 | 2.00 m | 移动后到同一面墙的距离 |
如果完全相信第一次测距,墙壁位于 x=3.00 m。若再完全相信里程计,第二次位姿就是 x=1.10 m,预测墙距应为 1.90 m,与第二次实际观测的 2.00 m 相差 0.10 m。

这 0.10 m 的差异说明三项测量不能同时被视为完全准确。SLAM 需要根据传感器的不确定性分配修正量,而不是永远相信里程计或某一次激光观测。
例如,假设里程计位移的标准差为 0.10 m,两次独立测距的标准差均为 0.05 m。采用独立高斯误差模型和按方差加权的最小二乘估计,可得到:
- 第二次机器人位置约为 x=1.033 m。
- 墙壁位置约为 x=3.017 m。
- 修正后的位姿和地图共同解释三项带噪声观测,而不强制每项残差都为零。
这个一维算例展示了联合估计的本质。真实二维 SLAM 还需要处理朝向角、多个障碍物、数据关联和传感器噪声,并持续维护整条轨迹与地图的一致性。
ROS 2 中的输入和输出是什么
以二维激光 SLAM 为例,ROS 2 系统通常需要激光扫描、里程计和正确的坐标变换。不同机器人可以采用其他传感器组合,例如相机、惯性测量单元或视觉里程计,但都需要建立运动与环境观测之间的约束。
| 数据 | 常见 ROS 2 接口 | 作用 |
|---|---|---|
| 激光扫描 | /scan,sensor_msgs/msg/LaserScan |
提供各方向的距离观测 |
| 里程计 | /odom,nav_msgs/msg/Odometry |
提供局部位姿与速度估计 |
| 坐标变换 | /tf、/tf_static |
关联机器人、传感器和地图坐标系 |
| 二维地图 | /map,nav_msgs/msg/OccupancyGrid |
输出栅格化的环境占据信息 |
这些话题名是常见约定,实际名称可能因机器人配置和命名空间而不同。SLAM 算法还会估计机器人在地图坐标系中的位姿,有些实现也维护关键帧轨迹或位姿图。
在典型 ROS 2 导航系统中,坐标关系包括 map、odom 和 base_link:
map:全局地图参考坐标系,允许定位修正改变机器人在地图中的估计位置。odom:连续的局部里程计坐标系,短时间运动通常平滑,但允许长期漂移。base_link:机器人本体参考坐标系,激光雷达还需要通过外参变换关联到机器人本体。
通常由里程计系统提供 odom 到 base_link 的变换,由 SLAM 或全局定位系统提供 map 到 odom 的变换。这种分工允许全局位姿被修正,同时保留局部运动估计的连续性。详见 Nav2 坐标系说明。
对于二维占据栅格,OccupancyGrid 使用 -1 表示未知,占据概率值使用 0~100 表示。常见二值化地图用 0 表示空闲、100 表示占用。未知区域不等于已确认可通行区域,不能因为还没有观测到障碍物,就把它当作安全空间。
回到原处为什么能够修正轨迹
假设机器人沿走廊行驶一圈后回到出发位置,但由于累计运动误差,里程计估计的位置与起点相差 0.40 m。如果再次看到起点附近相同的墙角或环境特征,就可能建立一条连接当前观测与历史位置的约束。
这一过程称为回环检测。在采用位姿图优化的 SLAM 系统中,回环约束可以与原有里程计约束、扫描匹配约束一起优化,从而调整历史位姿,并据此修正地图。回环并非把最后一个位置直接强制改成起点,而是根据约束和不确定性分配整段轨迹的误差。
但回环检测也存在风险。两段形状相似的走廊可能被误认为同一个位置,错误约束会把原本不同的区域拉到一起。因此,回环候选通常需要通过几何一致性等条件验证;环境缺少可区分特征时,也可能始终无法可靠闭环。
如何判断 SLAM 结果是否可信
首次查看二维 SLAM 结果时,可以采用以下检查顺序:
- 检查坐标关系:确认地图、里程计、机器人本体和激光雷达之间存在有效的坐标变换,时间戳能够对应。
- 检查扫描与地图的一致性:机器人静止时,稳定墙面不应反复漂移;移动时,扫描点应大致落在地图对应的墙体附近。
- 检查重复经过的区域:再次经过同一面直墙时,不应持续生成两条明显分离的墙壁轮廓。例如稳定出现相距约 0.20 m 的双墙,就需要检查定位漂移、扫描匹配或传感器外参。
- 检查建图覆盖范围:已扫描区域应出现合理的空闲和占据栅格,未探索区域应保持未知,不能把整张地图都视为已完成。
SLAM 的直接成果是相互一致的机器人位姿估计与环境地图,而不是一张单独绘制的障碍物图片。只有运动轨迹、传感器观测和地图能够相互解释,后续导航与路径规划才有可信的空间基础。