机器人 / 系统课时

位姿图中的节点和边分别表示什么

位姿图中的节点和边分别表示什么

机器人依靠里程计不断估计位置时,微小误差会沿轨迹累积。位姿图把关键时刻的机器人位姿表示为节点,把里程计测量和回环匹配表示为边,使不同时间的观测能够共同修正轨迹。本篇通过一个三节点算例,解释图的结构、约束残差和信息矩阵如何影响优化结果。

学习目标

  • 区分位姿图中的节点、里程计边、回环边和参考位姿约束。
  • 理解相对位姿测量的坐标系,并计算一条边的约束残差。
  • 根据测量协方差构造信息矩阵,判断不同约束对优化结果的影响。

节点记录待估计的机器人位姿

二维 SLAM 中,一个位姿节点通常包含三个变量:世界坐标系中的位置 xx、yy 和航向角 θ\theta。例如节点 x0=(0,0,0)\mathbf{x}_0=(0,0,0) 表示机器人位于地图原点、朝向世界坐标系的正 X 方向。

实际系统通常以关键帧为单位建立节点,而不是让每束激光或每个扫描点都成为节点。关键帧可以根据移动距离、旋转角度或扫描匹配质量选取。例如,机器人每移动约 0.5 m 或旋转约 10° 时,可以考虑生成新的关键位姿;这些数值只是教学示例,实际阈值需要结合场景和算法调整。

考虑三个关键位姿,位置单位为米,角度单位为度:

节点 x y 航向角 含义
x0\mathbf{x}_0 0 0 0° 起始位姿
x1\mathbf{x}_1 2 0 0° 第一个关键位姿
x2\mathbf{x}_2 2 2 90° 转弯后的关键位姿

这里的三个节点是待估计变量,其坐标可以在优化时调整,并非始终固定的真值。通常需要将第一个位姿固定,或为它添加先验约束,否则整张图一起平移或旋转也不会改变节点之间的相对关系,优化问题便缺少唯一的全局参考。

边表示两个位姿之间的测量约束

位姿图中的边不是实际画在地面上的运动轨迹,而是传感器提供的相对位姿测量。一条从节点 ii 指向节点 jj 的边,可以写成 zij=(Δx,Δy,Δθ)\mathbf{z}_{ij}=(\Delta x,\Delta y,\Delta\theta),其中平移量在节点 ii 的局部坐标系中表达。

常见的边有两种:

  • 里程计边:连接时间相邻的关键位姿,测量来自轮式里程计、扫描匹配或融合里程计。本例中 x0\mathbf{x}_0 到 x1\mathbf{x}_1 的测量为 (2,0,0°)(2,0,0°),x1\mathbf{x}_1 到 x2\mathbf{x}_2 的测量为 (0,2,90°)(0,2,90°)。
  • 回环边:连接时间上不相邻、但被判断存在空间对应关系的关键位姿。它通常由激光扫描匹配或其他地点识别与几何验证过程产生,用于限制长期累计漂移。

二维 SLAM 位姿图中的节点、里程计边和回环边

为了理解回环边的方向,假设在节点 x2\mathbf{x}_2 处重新观测到与起始节点相关的静态环境,并通过扫描匹配获得从节点 2 指向节点 0 的相对位姿测量:

z20=(−2,2,−90∘)\mathbf{z}_{20}=(-2,2,-90^\circ)

机器人在节点 2 朝向世界坐标系的正 Y 方向。此时,起点位于它的右侧 2 m、前方反方向 2 m,因此在节点 2 的局部坐标中,测量平移为 (−2,2)(-2,2)。这里的局部 X 轴指向机器人前方,局部 Y 轴指向左方;向世界坐标系负 X、负 Y 方向的位移转换后,恰好得到上述结果。

[!info] 在实际位姿图中,回环边的测量值应来自独立的观测匹配及几何验证,不能直接把当前估计的两个节点相减,再当作一条新的独立回环测量。否则只是重复使用已有估计,无法提供真正的新约束。

约束残差衡量预测与观测的冲突

给定两个当前估计位姿,可以计算它们之间的预测相对变换。设 TiT_i、TjT_j 是二维刚体位姿变换,则:

T^ij=Ti−1Tj\hat{T}_{ij}=T_i^{-1}T_j

这个公式先将世界坐标中的位置关系转换到节点 ii 的坐标系,再得到预测相对平移和相对旋转。不能直接对世界坐标的 xx、yy 分量做减法后,就认为得到了边的局部平移测量。

教学中可以把边的残差理解为三个分量的预测值与测量值之差:

eij=[Δx^−ΔxΔy^−Δywrap⁡(Δθ^−Δθ)]\mathbf{e}_{ij}=\begin{bmatrix}\hat{\Delta x}-\Delta x\\\hat{\Delta y}-\Delta y\\\operatorname{wrap}(\hat{\Delta\theta}-\Delta\theta)\end{bmatrix}

其中 wrap⁡\operatorname{wrap} 将角度差归一化到 [−π,π)[-\pi,\pi),避免 179° 与 -179° 被错误地当成相差 358°。这是常见的二维分量残差形式;使用严格的 SE(2) 李群误差定义时,还可以通过 Log⁡(Zij−1Ti−1Tj)\operatorname{Log}(Z_{ij}^{-1}T_i^{-1}T_j) 构造残差,二者应与求解器采用的扰动模型保持一致。

假设累计漂移使节点 2 的当前估计变为 (2.2,2,90°)(2.2,2,90°),而节点 0 仍固定在原点。由这两个位姿计算出的预测相对位姿为 (−2,2.2,−90°)(-2,2.2,-90°),与回环观测 (−2,2,−90°)(-2,2,-90°) 相比,第二个平移分量多出 0.2 m。

这条边产生非零残差,说明当前轨迹估计与回环测量不完全一致。图优化会调整可变节点,使所有边的加权误差共同减小,而不是仅让某一条边完全满足。

信息矩阵决定约束的可信权重

不同来源的边并不具有相同的精度。信息矩阵用来表达测量误差在各方向上的权重,通常定义为协方差矩阵的逆:

Ωij=Σij−1\Omega_{ij}=\Sigma_{ij}^{-1}

对于假设三个误差分量互相独立的二维位姿测量,若平移 X、Y 方向的标准差都是 0.1 m,角度标准差为 5°,那么使用弧度作为角度计算单位后,信息矩阵约为:

Ω=diag⁡(100,100,131.31)\Omega=\operatorname{diag}(100,100,131.31)

对角元素是对应标准差平方的倒数。标准差越小,信息矩阵对应的权重越大,优化器越不愿意违背该约束。真实扫描匹配产生的误差可能存在方向相关性,此时协方差矩阵也可能包含非零的非对角元素。

位姿图优化通常最小化所有边的加权平方误差:

E=∑(i,j)eijTΩijeijE=\sum_{(i,j)}\mathbf{e}_{ij}^{\mathsf{T}}\Omega_{ij}\mathbf{e}_{ij}

本例的平移残差大小为 0.2 m,两个平移方向标准差均为 0.1 m,角度残差为零,因此这条边的加权平方误差是 (0.2/0.1)2=4(0.2/0.1)^2=4。这并不是 4 m,而是按照测量不确定性归一化后的无量纲误差。

用 Python 计算一条回环边的误差

下面使用 Python 3 标准库验证前面的几何计算,不依赖 ROS 2 或第三方优化库。将代码保存为 pose_graph_demo.py,使用 python pose_graph_demo.py 运行。它计算一条回环边的残差与加权误差,不执行整张图的非线性优化。

python
import math


def wrap(angle):
    return (angle + math.pi) % (2 * math.pi) - math.pi


def relative(a, b):
    # 把世界坐标中的位移旋转到起始节点 a 的局部坐标系
    dx, dy = b[0] - a[0], b[1] - a[1]
    c, s = math.cos(a[2]), math.sin(a[2])
    return (
        c * dx + s * dy,
        -s * dx + c * dy,
        wrap(b[2] - a[2]),
    )


x0 = (0.0, 0.0, 0.0)
x2 = (2.2, 2.0, math.radians(90))
z20 = (-2.0, 2.0, math.radians(-90))

pred = relative(x2, x0)
res = (
    pred[0] - z20[0],
    pred[1] - z20[1],
    wrap(pred[2] - z20[2]),
)

# 独立噪声假设下,用各分量标准差归一化残差
sigmas = (0.1, 0.1, math.radians(5))
cost = sum((v / sigma) ** 2 for v, sigma in zip(res, sigmas))

print(f"预测位移: ({pred[0]:.2f}, {pred[1]:.2f}) m")
print(f"平移残差: {math.hypot(res[0], res[1]):.2f} m")
print(f"加权平方误差: {cost:.2f}")

预期输出:

text
预测位移: (-2.00, 2.20) m
平移残差: 0.20 m
加权平方误差: 4.00

可以将代码中的 x2 改为 (2.0, 2.0, math.radians(90)) 再运行。此时预测相对位姿与测量一致,平移残差和加权平方误差都会变为 0.00。这个检查说明残差反映的是当前节点估计与边测量之间的冲突,而不是节点到世界原点的距离。

为什么错误的回环边会破坏整张图

里程计边主要约束局部连续运动,回环边则可以把相隔很久的两个关键位姿重新联系起来。但优化器并不知道某条回环测量是否匹配到了错误的走廊或重复结构;如果错误边被赋予很大的信息权重,它也可能强行拉动整张轨迹。

例如,在当前 0.2 m 残差下,若把回环平移标准差从 0.1 m 错误地缩小为 0.02 m,其他条件不变,则加权平方误差会从 4 增加到 100。这意味着优化器对同一个误差施加了更强的惩罚,并不代表回环测量变得更准确。

添加回环约束前,应检查扫描重叠情况、匹配残差和位姿几何一致性,并依据实际测量质量设置协方差。遇到可能的异常约束,还可以在图优化中引入鲁棒损失函数,例如 Huber 损失,降低大残差边的影响。

如果使用 GTSAM,节点之间的相对位姿约束可以由 BetweenFactorPose2 表示,起点先验可以由 PriorFactorPose2 表示。对应的 Pose2 SLAM 官方示例 展示了里程计约束、回环约束和优化结果之间的关系。

一个能够有效修正累计漂移的位姿图,需要同时满足三个条件:节点表达合理的待估计位姿,边携带真实且坐标系正确的相对观测,信息矩阵反映测量的不确定性。只有这些条件成立,回环约束才能帮助优化器修正整条轨迹,而不是引入新的全局变形。

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

目录与阅读设置

阅读设置

17

记录一个想法

位姿图中的节点和边分别表示什么

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

登录哪都通