机器人 / 系统课时

里程计约束如何连接相邻关键位姿

里程计约束如何连接相邻关键位姿

机器人移动时,里程计能够估计相邻时刻的运动,却会逐渐积累误差。位姿图 SLAM 将关键时刻的机器人位姿作为节点,把相邻关键帧之间的运动估计作为约束边。本篇通过二维位姿计算和可运行示例,理解里程计边如何建立,以及局部误差为什么会演变成全局漂移。

学习目标

  • 理解位姿图节点、里程计边和相对位姿测量之间的关系。
  • 能从相邻关键位姿计算二维相对变换,并理解约束的不确定性。
  • 通过数值实验观察误差累积,解释里程计约束与回环约束的不同作用。

里程计边连接的是什么

在二维位姿图中,每个关键帧对应一个节点,表示机器人在某个时刻的位姿:

Xi=(xi,yi,θi)X_i=(x_i,y_i,\theta_i)

其中 xix_i、yiy_i 是位置,θi\theta_i 是朝向。相邻关键帧 ii 和 jj 之间的里程计边包含一个相对运动测量:

Zij=(Δx,Δy,Δθ)Z_{ij}=(\Delta x,\Delta y,\Delta\theta)

这里的 Δx\Delta x、Δy\Delta y 通常表示在前一个关键帧局部坐标系中的平移,而不是世界坐标系中的直接坐标差。Δθ\Delta\theta 表示两个关键位姿之间的航向变化。

例如机器人从关键帧 K0 向前行驶 1 m,随后到达 K1,就可以建立 K0 到 K1 的里程计边。继续行驶后,再建立 K1 到 K2 的边。这样的相邻约束串联起来,便形成位姿图的基本骨架。

相邻关键位姿、局部里程计边及累计漂移关系

一条里程计边通常还需要保存起点节点编号、终点节点编号和测量不确定性。边的方向不能随意交换:如果将 K0 到 K1 的边反过来使用,必须对整个相对变换求逆,而不是简单交换节点编号。

从相邻位姿计算相对变换

假设机器人在 ROS 2 中发布 /odom 消息,消息类型为 nav_msgs/msg/Odometry,其位姿通常相对于 odom 坐标系表示。也可以通过 TF 获取 base_link 在 odom 坐标系下的位姿。

建立一条相邻关键帧约束时,按以下顺序处理:

  1. 在关键帧时间戳 tit_i 和 tjt_j 获取机器人位姿,记作 TiodomT_i^{odom} 和 TjodomT_j^{odom}。
  2. 用前一帧位姿的逆变换乘以后帧位姿,得到局部相对运动。
  3. 将相对平移和旋转转换为里程计边测量,并附加协方差或信息矩阵。

二维齐次变换的相对位姿关系为:

Zij=(Tiodom)−1TjodomZ_{ij}=(T_i^{odom})^{-1}T_j^{odom}

注意乘法顺序。如果直接使用两个全局位置相减,得到的是参考坐标系中的位移,不一定是机器人局部坐标系中的前向和侧向位移。

例如前一关键帧位姿为 (1,2,90∘)(1,2,90^\circ),后一关键帧位姿为 (1,3,90∘)(1,3,90^\circ)。虽然在 odom 坐标系中机器人沿 yy 方向移动了 1 m,但由于机器人朝向为 90∘90^\circ,在前一关键帧的局部坐标系中,这段运动应写为 (1,0,0)(1,0,0),即前进 1 m、侧向位移 0 m、航向变化 0。

真实的里程计边还需要描述测量可靠程度。二维情况下,协方差矩阵通常是 3×33\times3,分别对应局部平移的两个分量和航向角。假设三个分量的标准差分别为 0.03 m、0.05 m、0.02 rad,且暂时忽略相关性,可以使用如下对角协方差:

Σij=diag⁡(0.0009,0.0025,0.0004)\Sigma_{ij}=\operatorname{diag}(0.0009,0.0025,0.0004)

对应的信息矩阵是 Ωij=Σij−1\Omega_{ij}=\Sigma_{ij}^{-1}。信息矩阵的数值越大,表示优化器越信任相应维度的测量。上述对角矩阵仅是教学示例;如果传感器估计存在明显的平移与旋转相关性,应保留协方差中的非对角项。

用数值实验观察累计漂移

假设真实机器人沿直线连续前进五次,每次 1 m,真实航向不变。但由于传感器存在系统偏差,里程计每次都错误地报告额外旋转 0.02 rad。

下面使用 Python 3 标准库,将五条局部里程计边依次累积到初始位姿。示例不依赖 ROS 2,模拟的是已经从传感器数据中提取出的相对运动测量。

python
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")

预期输出:

text
关键位姿: 6; 里程计边: 5
终点: x=4.994 m, y=0.200 m, yaw=5.7°
相对理想直线的横向偏差: 0.200 m

理想终点应该是 (5,0,0)(5,0,0),但累计结果出现约 0.20 m 的横向偏差和 5.7° 的航向偏差。原因是每次新的局部平移都会受到已经累积的航向误差影响,使后续运动投影到错误的全局方向。

这个例子也说明,相邻运动测量即使只有很小的系统误差,连续积分后仍会形成明显漂移。随机噪声和系统偏差的累积规律并不相同,实际漂移大小还取决于轨迹形状、运动距离和传感器误差模型。

图优化如何使用里程计约束

在位姿图优化中,节点 XiX_i 和 XjX_j 是需要估计的变量,而里程计测量 ZijZ_{ij} 是已知的约束数据。优化器先根据当前两个节点的估计位姿计算预测相对运动:

Z^ij=Xi−1Xj\hat Z_{ij}=X_i^{-1}X_j

然后比较预测运动与实际测量,构造约束残差。对二维刚体变换,可以写成:

eij=Log⁡(Zij−1Z^ij)e_{ij}=\operatorname{Log}(Z_{ij}^{-1}\hat Z_{ij})

这里的 Log⁡\operatorname{Log} 是将二维刚体变换误差映射为三维误差向量的李群对数映射,不是普通的逐元素对数。优化器使用信息矩阵加权残差,最小化所有约束的总误差:

E=∑(i,j)eijTΩijeijE=\sum_{(i,j)}e_{ij}^{T}\Omega_{ij}e_{ij}

实际构图时,需要注意三个边界条件:

  • 固定参考位姿:只包含相对约束的位姿图存在整体平移和旋转自由度,通常固定 K0 或添加合理先验,避免优化问题出现规范自由度。
  • 保持测量方向一致:ZijZ_{ij} 表示从节点 ii 到节点 jj 的相对变换,残差计算、协方差所在坐标系和边的方向必须一致。
  • 正确设置置信度:不能把所有里程计边都赋予极大的信息矩阵权重,否则错误的里程计测量可能压制后续更可信的回环约束。

建边时怎样检查数据是否正确

将这一机制用于 ROS 2 激光 SLAM 时,可以针对每条新增边进行三项检查。

  1. 检查时间戳:相对运动应对应两个关键帧的实际采样时刻。TF 查询需要使用正确时间戳,不能随意使用消息接收时间代替采样时间。
  2. 检查局部坐标方向:机器人朝向世界坐标系正 yy 轴时,向前走 1 m,局部里程计边仍应接近 (1,0,0)(1,0,0)。若变成 (0,1,0)(0,1,0),可能错误地使用了全局坐标差。
  3. 检查旋转和不确定性:角度差应处理跨越 −π-\pi 与 π\pi 的情况;协方差需与误差向量顺序一致,供优化使用的信息矩阵应满足对称正定等必要条件。

还可以做一次简单的回放验证:沿无明显障碍的直线行驶约 5 m,记录连续关键帧的相对平移、航向变化及累计轨迹。如果机器人实际保持直行,但里程计边的航向增量持续偏向同一侧,应优先检查轮径、轮距、里程计标定及时间同步,而不是直接提高图优化迭代次数。

里程计边负责保留相邻关键帧之间的局部运动关系;后续加入回环边后,优化器才有机会利用不同路径之间的几何一致性重新分配误差,修正整条轨迹的累计漂移。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

里程计约束如何连接相邻关键位姿

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通