机器人 / 系统课时

非线性最小二乘怎样优化位姿图

非线性最小二乘怎样优化位姿图

里程计沿轨迹累积的小误差,会让机器人回到旧位置时仍在地图上错位。这里把相邻里程计和回环检测变成统一的残差目标,用非线性最小二乘同时修正多个关键位姿,并通过一个可运行的二维闭环算例检验修正效果。

学习目标

  • 写出二维位姿图的平移、旋转残差与带权目标函数。
  • 解释迭代线性化、固定参考位姿和回环边的作用。
  • 运行四节点示例,量化优化前后的回环误差。

位姿图的边怎样产生残差

二维关键位姿记为 Ti=(xi,yi,θi)T_i=(x_i,y_i,\theta_i)。一条边 (i,j)(i,j) 携带在节点 ii 局部坐标系内测得的相对平移 d^ij\hat{\mathbf d}_{ij} 和旋转 ϕ^ij\hat\phi_{ij}。优化变量是各节点的全局位姿,不是直接修改测量值。

对于一条边,先把两个全局位置之差旋转回节点 ii 的坐标系,再与边的测量值相减:

rij=[R(θi)T(pj−pi)−d^ijwrap⁡(θj−θi−ϕ^ij)]\mathbf r_{ij}= \begin{bmatrix} R(\theta_i)^{\mathsf T}(\mathbf p_j-\mathbf p_i)-\hat{\mathbf d}_{ij}\\ \operatorname{wrap}(\theta_j-\theta_i-\hat\phi_{ij}) \end{bmatrix}

这里 pi=(xi,yi)T\mathbf p_i=(x_i,y_i)^{\mathsf T},RR 是二维旋转矩阵,wrap 把角度差归一到约 −π-\pi 至 π\pi。例如边 0→10\to1 量到前进 2.102.10 m、转向 92∘92^\circ;若当前位姿预测仅前进 2.002.00 m,该边就有约 0.100.10 m 的前向残差。

带权求和并固定参考位姿

对每条边设置测量协方差 Σij\Sigma_{ij},信息矩阵为 Ωij=Σij−1\Omega_{ij}=\Sigma_{ij}^{-1},再最小化所有边的加权平方残差:

min⁡{Ti}∑(i,j)∈ErijTΩijrij\min_{\{T_i\}}\sum_{(i,j)\in E}\mathbf r_{ij}^{\mathsf T}\Omega_{ij}\mathbf r_{ij}

可给普通里程计边设置平移标准差 0.100.10 m、角度标准差 3∘3^\circ,给经验证的回环边设置 0.050.05 m 和 2∘2^\circ。标准差越小,约束的权重越高;权重应来自传感器不确定性,而非为了让轨迹“更好看”随意调整。

如果全部节点一起平移或旋转,相对约束的误差不变;二维图因此有三个全局坐标自由度。固定 T0=(0,0,0)T_0=(0,0,0),只优化其余节点,才能确定唯一的全局参考系,并避免由这一自由度引起的奇异性。

四节点位姿图中的里程计漂移、回环边和优化后的共同修正

线性化为什么需要反复迭代

由于局部坐标变换中存在 sin⁡θ\sin\theta、cos⁡θ\cos\theta,残差随位姿非线性变化。每轮在当前估计 x\mathbf x 附近近似 r(x+δ)≈r(x)+Jδ\mathbf r(\mathbf x+\delta)\approx\mathbf r(\mathbf x)+J\delta,其中 JJ 是残差对待优化位姿的雅可比矩阵。

Gauss–Newton 将近似目标化为线性最小二乘,求出增量 δ\delta 后更新各位姿;下一轮必须重新计算旋转、残差与雅可比。信赖域方法还会限制不可靠的大步长。停止条件可取目标下降很小、变量更新很小或达到迭代上限。角度残差不做 wrap 会使跨越 180∘180^\circ 的小转动被误判为接近 360∘360^\circ 的大错误。

运行四节点闭环算例

下面设置三条带小误差的相邻边,再添加从节点 3 返回节点 0 的回环边。使用 Python 3、NumPy 与 SciPy;将代码保存为 pose_graph_demo.py 后执行 python pose_graph_demo.py。least_squares 负责数值求导与迭代,首个位姿始终固定为零。

python
import numpy as np
from scipy.optimize import least_squares

def wrap(a):
    return np.arctan2(np.sin(a), np.cos(a))

def relative(a, b):
    dx, dy = b[:2] - a[:2]
    c, s = np.cos(a[2]), np.sin(a[2])
    return np.array([c * dx + s * dy, -s * dx + c * dy,
                     wrap(b[2] - a[2])])

def compose(a, z):
    c, s = np.cos(a[2]), np.sin(a[2])
    return np.array([a[0] + c*z[0] - s*z[1],
                     a[1] + s*z[0] + c*z[1], wrap(a[2] + z[2])])

d = np.deg2rad
# (起点, 终点, 局部测量, 各分量标准差)
edges = [
    (0, 1, np.array([2.10, 0, d(92)]), np.array([.10, .10, d(3)])),
    (1, 2, np.array([2.05, 0, d(93)]), np.array([.10, .10, d(3)])),
    (2, 3, np.array([2.00, 0, d(91)]), np.array([.10, .10, d(3)])),
    (3, 0, np.array([2.00, 0, d(90)]), np.array([.05, .05, d(2)])),
]

poses = [np.zeros(3)]
for i, j, z, sigma in edges[:3]:
    poses.append(compose(poses[i], z))
x0 = np.array(poses[1:]).ravel()

def poses_of(x):
    # P0 不是优化变量:消除二维位姿图的三维规范自由度
    return np.vstack([np.zeros(3), x.reshape(3, 3)])

def residuals(x):
    p, result = poses_of(x), []
    for i, j, z, sigma in edges:
        e = relative(p[i], p[j]) - z
        e[2] = wrap(e[2])
        result.extend(e / sigma)  # 用标准差白化三维残差
    return np.array(result)

def loop_error(x):
    p = poses_of(x)
    e = relative(p[3], p[0]) - edges[-1][2]
    return np.linalg.norm(e[:2]), abs(np.rad2deg(wrap(e[2])))

solution = least_squares(residuals, x0, method="trf", max_nfev=200)
a, b = loop_error(x0), loop_error(solution.x)
print(f"加权残差范数: {np.linalg.norm(residuals(x0)):.3f} -> "
      f"{np.linalg.norm(residuals(solution.x)):.3f}")
print(f"回环平移残差: {a[0]:.3f} m -> {b[0]:.3f} m")
print(f"回环角度残差: {a[1]:.2f}° -> {b[1]:.2f}°")
p3 = poses_of(solution.x)[3]
print(f"优化后 P3: x={p3[0]:.3f} m, y={p3[1]:.3f} m")

预期输出(末位可能随 SciPy 版本略有差异):

text
加权残差范数: 6.188 -> 1.224
回环平移残差: 0.271 m -> 0.007 m
回环角度残差: 6.00° -> 0.53°
优化后 P3: x=-0.011 m, y=1.999 m

优化后的回环残差不必严格为零,因为里程计边与回环边的测量相互冲突,求解器要按权重在所有边之间分配修正量。

识别错误回环与不可信结果

检查时先看求解器 solution.success 是否为 True,再比较优化前后的总加权残差,并逐条检查边残差。总残差下降只能证明对当前约束拟合得更好,不证明回环匹配一定正确。

在实际二维激光 SLAM 中,长走廊或重复门洞可能制造错误回环。应先检查扫描匹配质量和相对位姿的一致性;若仍存在少数异常约束,可在求解器中使用稳健损失(例如 SciPy 的 loss="soft_l1"),或在图优化库中为回环边添加稳健核。不要通过极度提高回环权重来强行闭合地图,否则一条错误边就可能拉歪整条轨迹。

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

目录与阅读设置

阅读设置

17

记录一个想法

非线性最小二乘怎样优化位姿图

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

登录哪都通