机器人 / 系统课时
里程计约束如何连接相邻关键位姿
里程计约束如何连接相邻关键位姿
机器人移动时,里程计能够估计相邻时刻的运动,却会逐渐积累误差。位姿图 SLAM 将关键时刻的机器人位姿作为节点,把相邻关键帧之间的运动估计作为约束边。本篇通过二维位姿计算和可运行示例,理解里程计边如何建立,以及局部误差为什么会演变成全局漂移。
学习目标
- 理解位姿图节点、里程计边和相对位姿测量之间的关系。
- 能从相邻关键位姿计算二维相对变换,并理解约束的不确定性。
- 通过数值实验观察误差累积,解释里程计约束与回环约束的不同作用。
里程计边连接的是什么
在二维位姿图中,每个关键帧对应一个节点,表示机器人在某个时刻的位姿:
其中 、 是位置, 是朝向。相邻关键帧 和 之间的里程计边包含一个相对运动测量:
这里的 、 通常表示在前一个关键帧局部坐标系中的平移,而不是世界坐标系中的直接坐标差。 表示两个关键位姿之间的航向变化。
例如机器人从关键帧 K0 向前行驶 1 m,随后到达 K1,就可以建立 K0 到 K1 的里程计边。继续行驶后,再建立 K1 到 K2 的边。这样的相邻约束串联起来,便形成位姿图的基本骨架。

一条里程计边通常还需要保存起点节点编号、终点节点编号和测量不确定性。边的方向不能随意交换:如果将 K0 到 K1 的边反过来使用,必须对整个相对变换求逆,而不是简单交换节点编号。
从相邻位姿计算相对变换
假设机器人在 ROS 2 中发布 /odom 消息,消息类型为 nav_msgs/msg/Odometry,其位姿通常相对于 odom 坐标系表示。也可以通过 TF 获取 base_link 在 odom 坐标系下的位姿。
建立一条相邻关键帧约束时,按以下顺序处理:
- 在关键帧时间戳 和 获取机器人位姿,记作 和 。
- 用前一帧位姿的逆变换乘以后帧位姿,得到局部相对运动。
- 将相对平移和旋转转换为里程计边测量,并附加协方差或信息矩阵。
二维齐次变换的相对位姿关系为:
注意乘法顺序。如果直接使用两个全局位置相减,得到的是参考坐标系中的位移,不一定是机器人局部坐标系中的前向和侧向位移。
例如前一关键帧位姿为 ,后一关键帧位姿为 。虽然在 odom 坐标系中机器人沿 方向移动了 1 m,但由于机器人朝向为 ,在前一关键帧的局部坐标系中,这段运动应写为 ,即前进 1 m、侧向位移 0 m、航向变化 0。
真实的里程计边还需要描述测量可靠程度。二维情况下,协方差矩阵通常是 ,分别对应局部平移的两个分量和航向角。假设三个分量的标准差分别为 0.03 m、0.05 m、0.02 rad,且暂时忽略相关性,可以使用如下对角协方差:
对应的信息矩阵是 。信息矩阵的数值越大,表示优化器越信任相应维度的测量。上述对角矩阵仅是教学示例;如果传感器估计存在明显的平移与旋转相关性,应保留协方差中的非对角项。
用数值实验观察累计漂移
假设真实机器人沿直线连续前进五次,每次 1 m,真实航向不变。但由于传感器存在系统偏差,里程计每次都错误地报告额外旋转 0.02 rad。
下面使用 Python 3 标准库,将五条局部里程计边依次累积到初始位姿。示例不依赖 ROS 2,模拟的是已经从传感器数据中提取出的相对运动测量。
import math
# 五条相邻里程计边:局部前进1米,每条都有0.02弧度航向偏差
edges = [(1.0, 0.0, 0.02)] * 5
poses = [(0.0, 0.0, 0.0)]
for dx, dy, dtheta in edges:
x, y, yaw = poses[-1]
# 局部平移必须根据前一关键位姿的朝向转换到固定坐标系
world_dx = math.cos(yaw) * dx - math.sin(yaw) * dy
world_dy = math.sin(yaw) * dx + math.cos(yaw) * dy
# 将角度规范化到[-pi, pi)区间
new_yaw = (yaw + dtheta + math.pi) % (2 * math.pi) - math.pi
poses.append((x + world_dx, y + world_dy, new_yaw))
last = poses[-1]
print(f"关键位姿: {len(poses)}; 里程计边: {len(edges)}")
print(f"终点: x={last[0]:.3f} m, y={last[1]:.3f} m, yaw={math.degrees(last[2]):.1f}°")
print(f"相对理想直线的横向偏差: {last[1]:.3f} m")预期输出:
关键位姿: 6; 里程计边: 5
终点: x=4.994 m, y=0.200 m, yaw=5.7°
相对理想直线的横向偏差: 0.200 m理想终点应该是 ,但累计结果出现约 0.20 m 的横向偏差和 5.7° 的航向偏差。原因是每次新的局部平移都会受到已经累积的航向误差影响,使后续运动投影到错误的全局方向。
这个例子也说明,相邻运动测量即使只有很小的系统误差,连续积分后仍会形成明显漂移。随机噪声和系统偏差的累积规律并不相同,实际漂移大小还取决于轨迹形状、运动距离和传感器误差模型。
图优化如何使用里程计约束
在位姿图优化中,节点 和 是需要估计的变量,而里程计测量 是已知的约束数据。优化器先根据当前两个节点的估计位姿计算预测相对运动:
然后比较预测运动与实际测量,构造约束残差。对二维刚体变换,可以写成:
这里的 是将二维刚体变换误差映射为三维误差向量的李群对数映射,不是普通的逐元素对数。优化器使用信息矩阵加权残差,最小化所有约束的总误差:
实际构图时,需要注意三个边界条件:
- 固定参考位姿:只包含相对约束的位姿图存在整体平移和旋转自由度,通常固定 K0 或添加合理先验,避免优化问题出现规范自由度。
- 保持测量方向一致: 表示从节点 到节点 的相对变换,残差计算、协方差所在坐标系和边的方向必须一致。
- 正确设置置信度:不能把所有里程计边都赋予极大的信息矩阵权重,否则错误的里程计测量可能压制后续更可信的回环约束。
建边时怎样检查数据是否正确
将这一机制用于 ROS 2 激光 SLAM 时,可以针对每条新增边进行三项检查。
- 检查时间戳:相对运动应对应两个关键帧的实际采样时刻。TF 查询需要使用正确时间戳,不能随意使用消息接收时间代替采样时间。
- 检查局部坐标方向:机器人朝向世界坐标系正 轴时,向前走 1 m,局部里程计边仍应接近 。若变成 ,可能错误地使用了全局坐标差。
- 检查旋转和不确定性:角度差应处理跨越 与 的情况;协方差需与误差向量顺序一致,供优化使用的信息矩阵应满足对称正定等必要条件。
还可以做一次简单的回放验证:沿无明显障碍的直线行驶约 5 m,记录连续关键帧的相对平移、航向变化及累计轨迹。如果机器人实际保持直行,但里程计边的航向增量持续偏向同一侧,应优先检查轮径、轮距、里程计标定及时间同步,而不是直接提高图优化迭代次数。
里程计边负责保留相邻关键帧之间的局部运动关系;后续加入回环边后,优化器才有机会利用不同路径之间的几何一致性重新分配误差,修正整条轨迹的累计漂移。