鲁棒位姿图优化

标准的位姿图优化有一个危险的特性：它信任每一条边。里程计边通常是可靠的,但回环边来自场景识别,而场景识别有时会出错——感知混淆(两条看起来一模一样的走廊)会产生错误的回环。将一条错误的回环边输入最小二乘,优化器会欣然把你的地图折叠到自身上,因为二次代价函数会让一个坏的约束主导一切。鲁棒位姿图优化就是让后端在存在离群约束的情况下依然存活的一系列技术。

路线图中提到的三种经典方法：

• 可切换约束(Switchable Constraints)(Sunderhauf与Protzel,2012)。每条回环边 $(i,j)$ 都获得一个额外的优化变量——一个开关 $s_{ij} \in [0, 1]$,用于缩放该边的信息矩阵。优化器可以”关闭”与图中其余部分不一致的边,同时付出一个惩罚项,以阻止无谓地关闭边：

  $$\min_{\{T_i\}, \{s_{ij}\}} \sum_{\text{odom}} \|\mathbf{e}_{ij}\|^2_{\Sigma^{-1}} + \sum_{\text{loops}} s_{ij}^2\, \|\mathbf{e}_{ij}\|^2_{\Sigma^{-1}} + \lambda (1 - s_{ij})^2$$

• 动态协方差缩放(Dynamic Covariance Scaling, DCS)(Agarwal等,2013)。它观察到,最优的开关值可以由当前残差以闭式形式计算出来,从而消去了额外的变量。每条回环边的协方差都会实时缩放——残差小则完全信任,残差大则膨胀协方差。DCS的行为类似于一个鲁棒的M估计核函数,收敛速度比可切换约束更快。

• 成对一致性最大化(Pairwise Consistency Maximization, PCM)(Mangelson等,2018)。PCM不是在优化器内部软化边,而是在优化之前过滤回环：如果两个回环所隐含的相对运动(与它们之间的里程计相结合)在其协方差范围内相互一致,则称这两个回环是成对一致的。构建一个一致性图并求其最大团,即可得到最大的相互一致的回环子集;其余的回环则被拒绝。PCM已成为多机器人SLAM(DOOR-SLAM、Kimera-Multi)中标准的守门机制,在这类场景中,机器人间的回环尤其不可靠。

这些思想与更广泛的鲁棒估计工具箱相连：M估计核函数(Huber、Cauchy)对中等程度的离群点降权,而渐进非凸化(Graduated Non-Convexity, GNC)可以在不需要初始化的情况下求解强非凸的鲁棒代价函数。在实践中,系统会分层设防：前端的几何验证、PCM式的一致性检查,以及优化器中的鲁棒核函数或DCS作为最后一道防线。

M估计量的视角

所有这些软方法本质上都是同一种操作的变体：用一个鲁棒核函数 $\rho$ 替代二次代价,

$$\min_{\{T_i\}} \sum_{(i,j)} \rho\left(\|\mathbf{e}_{ij}\|_{\Sigma_{ij}^{-1}}\right),$$

选择这个核函数,使得代价的影响函数(其导数)在残差较大时停止增长——或者反而收缩。在二次函数下,一条残差为 $10\sigma$ 的边所施加的拉力是 $1\sigma$ 边的100倍;在Huber核下它的拉力只是线性增长;在Cauchy或Geman-McClure核下,其拉力会随残差增大而减弱。在实现层面,这就变成了迭代重加权最小二乘：每次高斯-牛顿迭代都会用一个由当前残差计算出的权重 $w(\|\mathbf{e}\|)$ 乘以每条边的信息矩阵——这恰恰就是DCS用一个特定的闭式权重所做的事,也是可切换约束用显式开关变量所实现的效果。其中的权衡是：重新下降型核函数(Cauchy及更强的核函数)可以彻底拒绝严重的离群点,但它们会使代价函数变为非凸,因此不良的初始化可能会把好的边也”锁在门外”——这正是GNC所要解决的问题,它从一个凸的替代函数开始,逐步锐化。

前端一侧的一个补充性的硬检查是卡方门限检验：在一条边被接受之前,检验其残差在当前估计下是否统计上合理,即 $\mathbf{e}^T \Sigma^{-1} \mathbf{e} < \chi^2_{d,\,0.95}$(对于 $SE(3)$ 边为6个自由度)。门限检验能以低成本捕捉明显的离群点,但它信任当前的估计——在严重漂移的情况下,一个正确的回环也可能看起来不合理,而这恰恰是PCM的成对一致性逻辑能更好处理的情形。

常见陷阱

• 对错误的边施加鲁棒化 ——里程计边很少是离群点;将其包裹在激进的核函数中会让优化器”开脱”真实的漂移,并可能把轨迹分裂成互不相连的孤岛。应对回环边做鲁棒化,让里程计边保持(接近)二次的代价。
• 误以为鲁棒性能替代验证 ——核函数和开关只能应对一部分离群点;如果前端产生40%的错误回环,地图仍然会被破坏。需要分层设防。
• 调参上的盲目性 ——可切换约束中的 $\lambda$、DCS的 $\Phi$ 参数,以及核函数宽度,都编码了”残差要多大才能怀疑这条边”这一判断;它们必须相对于真实的边协方差来设定,而这些协方差本身往往也是靠猜测得来的。
• 重新下降型核函数与不良初始化叠加 ——在大漂移情况下,正确的回环一开始就会有巨大的残差,并立即被关闭;GNC式的延续策略或PCM预过滤可以避免这一失败。
• 感知混淆在结构化环境中会产生一致的谎言 ——来自外观相同的走廊段的多个错误匹配可能相互吻合;一致性最大化提高了门槛,但并非万无一失的证明。

对SLAM的意义

一次错误的回环就可能毁掉一张耗费一小时才建成的地图,因此在已部署的系统中,后端的鲁棒性绝非可选项。这一主题也是通往多机器人SLAM的入口,在多机器人SLAM中,机器人必须决定是否信任其他机器人产生的约束——这正是PCM所针对的场景。

动手实践

• Kimera-RPGO 动手实践

相关条目

• 位姿图优化
• GNC
• DOOR-SLAM
• 机器人间回环检测
• 视觉场景识别(VPR)