机器人 / 系统课时
非线性最小二乘怎样优化位姿图
非线性最小二乘怎样优化位姿图
里程计沿轨迹累积的小误差,会让机器人回到旧位置时仍在地图上错位。这里把相邻里程计和回环检测变成统一的残差目标,用非线性最小二乘同时修正多个关键位姿,并通过一个可运行的二维闭环算例检验修正效果。
学习目标
- 写出二维位姿图的平移、旋转残差与带权目标函数。
- 解释迭代线性化、固定参考位姿和回环边的作用。
- 运行四节点示例,量化优化前后的回环误差。
位姿图的边怎样产生残差
二维关键位姿记为 。一条边 携带在节点 局部坐标系内测得的相对平移 和旋转 。优化变量是各节点的全局位姿,不是直接修改测量值。
对于一条边,先把两个全局位置之差旋转回节点 的坐标系,再与边的测量值相减:
这里 , 是二维旋转矩阵,wrap 把角度差归一到约 至 。例如边 量到前进 m、转向 ;若当前位姿预测仅前进 m,该边就有约 m 的前向残差。
带权求和并固定参考位姿
对每条边设置测量协方差 ,信息矩阵为 ,再最小化所有边的加权平方残差:
可给普通里程计边设置平移标准差 m、角度标准差 ,给经验证的回环边设置 m 和 。标准差越小,约束的权重越高;权重应来自传感器不确定性,而非为了让轨迹“更好看”随意调整。
如果全部节点一起平移或旋转,相对约束的误差不变;二维图因此有三个全局坐标自由度。固定 ,只优化其余节点,才能确定唯一的全局参考系,并避免由这一自由度引起的奇异性。

线性化为什么需要反复迭代
由于局部坐标变换中存在 、,残差随位姿非线性变化。每轮在当前估计 附近近似 ,其中 是残差对待优化位姿的雅可比矩阵。
Gauss–Newton 将近似目标化为线性最小二乘,求出增量 后更新各位姿;下一轮必须重新计算旋转、残差与雅可比。信赖域方法还会限制不可靠的大步长。停止条件可取目标下降很小、变量更新很小或达到迭代上限。角度残差不做 wrap 会使跨越 的小转动被误判为接近 的大错误。
运行四节点闭环算例
下面设置三条带小误差的相邻边,再添加从节点 3 返回节点 0 的回环边。使用 Python 3、NumPy 与 SciPy;将代码保存为 pose_graph_demo.py 后执行 python pose_graph_demo.py。least_squares 负责数值求导与迭代,首个位姿始终固定为零。
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 版本略有差异):
加权残差范数: 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"),或在图优化库中为回环边添加稳健核。不要通过极度提高回环权重来强行闭合地图,否则一条错误边就可能拉歪整条轨迹。