SLAM文献之-Degeneracy-Aware Factors with Applications to Underwater SLAM

论文概述

由 Hinduja, Ho 和 Kaess（2019，IROS） 提出，旨在处理水下 SLAM（主要基于声纳测距）的退化问题，核心包括两个阶段：

1. Degeneracy-Aware ICP：在配准阶段识别并避开退化方向。
2. Partial Loop Closure Factor：将退化感知融入图优化因子，只对稳定方向施加约束。
3. 

---

1. Degeneracy-Aware ICP（退化感知 ICP 与解重映射）：算法原理和流程

a. 问题背景

常规 point-to-plane ICP 在欠约束环境下（如光滑海底）容易产生退化，导致配准不可靠。

b. 解 remapping 技术

借鉴 Zhang 等人的解 remapping 方法，通过分析 Hessian 矩阵的特征值和特征向量：

• 对 Hessian 矩阵 $\mathbf{H}={\mathbf{A}}^{\top }\mathbf{A}$ 进行谱分解。
• 识别低特征值（或条件不良）的方向，将更新投影到其正交子空间。
• 在每次迭代前动态更新检测结果（基于最新点云数据），避免长期误判。

由于退化问题，点到面 ICP 可能会产生错误的配准结果。这种错误会在加入闭环约束构建位姿图时导致非常差的轨迹估计结果。为了解决这些问题，我们需要检测状态空间中退化（即约束较弱）的方向，并在 ICP 的优化过程中仅使用受良好约束的方向。

Zhang 等人 [22] 展示了一种方法，通过仅在良好约束方向上进行更新，迭代求解非线性系统。在从线性化系统中获得矩阵 $A$ 的特征值 ${\lambda }_i$ 和特征向量 $v_i$ 后，该方法构建三个矩阵：$V_d$、$V_f$ 和 $V$。其中：

• $V_d$ 包含退化方向对应的特征向量，
• $V_f$ 包含良好约束方向的特征向量，
• $V$ 包含全部的特征向量。

这种划分是基于将每个特征值 ${\lambda }_i$ 与一个统一阈值 ${\lambda }_t$ 进行比较得出的。

线性化系统通常的解为：

$$x_f=(A^{T}A{)}^{-1}{A}^{T}b$$

然后，将该标准解投影到良好约束方向的子空间中。该增量更新被加入到当前的状态估计中，并在新的线性化点重复此过程。

在退出的退化感知 ICP 系统中，仅使用良好条件方向来更新优化解，而忽略被判定为退化的方向。退化感知 ICP 算法的伪代码描述见算法 1。

通过扩展 Point Cloud Library (PCL) 的点到面 ICP 方法来实现退化感知 ICP。线性化点到面误差项以获得 $A^{T}A$ 和 $A^{T}b$ 的方法，采用了文献 [10] 中的实现方式。
提出的方法与 Zhang 等人 [22] 的“解重映射（solution remapping）”方法之间的本质区别在于：在每一次迭代中都会基于当前的协方差矩阵 $A^{T}A$ 重新计算良好条件方向。这反映了一个事实：每次以更新后的 $T_{\text{icp}}$ 为线性化点时，退化方向可能会发生变化。

---

c. 算法流程（Algorithm 1）

1. 初始化 $T_{\text{ICP}}$。

2. 迭代收敛直至误差收敛：

   • 构建线性系统 $\mathbf{A}\Delta x=\mathbf{b}$。
   • 计算 $\mathbf{H}={\mathbf{A}}^{\top }\mathbf{A}$ 及 $\mathbf{H}\Delta x={\mathbf{A}}^{\top }\mathbf{b}$。
   • 谱分解 $\mathbf{H}=\mathbf{U}\Lambda {\mathbf{U}}^{\top }$。
   • 根据 $\Lambda$ 中较小特征值，构建 well-conditioned 子空间 ${\mathbf{V}}_f$。
   • 将 $\Delta x$ 投影到非退化子空间，更新 $T_{\text{ICP}}$。

该方法确保仅在可靠方向上施加配准更新，有效抑制退化带来的误差。

算法 1：退化感知的 ICP

输入：来自里程计的初始相对位姿变换 $T_{\text{odom}}$
输出：退化感知后的 ICP 相对变换 $T_{\text{icp}}$

1:  T_icp ← T_odom             // 将初始位姿赋值为当前配准变换
2:  while 非线性优化未收敛 do
3:      在当前 T_icp 处对优化问题线性化，得到矩阵 A^T A
4:      计算 A^T A 的特征值 λ_i 和特征向量 v_i, 其中 i = 1 到 6
5:      设定特征值阈值 λ_t（用于判定是否退化）
6:      构造包含所有特征向量的矩阵 V
7:      构造仅包含“良好条件”（即非退化）方向的矩阵 V_f（满足 λ_i > λ_t）
8:      在完整空间中计算优化更新量：Δx_f ← (A^T A)^(-1) A^T b
9:      将更新量投影到非退化方向：T_icp ← T_icp + V^(-1) V_f Δx_f
10: end while
11: return T_icp

---

注释说明：

• A^T A 是 point-to-plane ICP 误差的 Hessian 近似，用于识别退化方向；
• Vf 是筛选出来的“可信方向”的正交基；
• 更新时仅使用 Vf 子空间的分量，从而避免在退化方向上的无效或错误更新；
• 最终得到的 $T_{\text{icp}}$ 是一个经过退化感知调整后的配准变换。

2. 阈值设定

为了判定哪些方向是退化的，需要设置一个阈值 ${\lambda }_t$，用于与退化感知 ICP 中得到的特征值进行比较。

Zhang 等人 [22] 使用了一个包含已知退化与非退化区段的数据集，并基于最小特征值分布的中点来确定阈值。但这种方式并不适用于实时系统。

Cho 等人 [3] 比较各方向特征值之间的比值，并依据传感器属性设定固定阈值排序。而我们的方法类似于 [13]，采用条件数（即最大与最小特征值之比）作为每次迭代中的特征值阈值。设：

$$\text{条件数}=\frac{{\lambda }_{\max }}{{\lambda }_{\min }}$$

这种阈值设定在高度退化的场景中尤为有效——在这些情况下，${\lambda }_{\min }$ 可能非常小，甚至多个方向都极小，从而导致 ${\lambda }_t$ 显著增大，自动拒绝这些不可靠的约束。

---

3. Partial Loop Closure Factor（在 pose graph 中应用）

a. 问题

SLAM 闭环检测中引入的 ICP 匹配若受退化影响，则可能向图优化引入错误约束。

b. 部分因子设计

• 使用 Degeneracy-Aware ICP 获得的 well-conditioned 子空间 ${\mathbf{V}}_f$。
• 在图优化中定义新的 loop closure 因子，仅对 ${\mathbf{V}}_f$ 中方向施加约束：

C. 部分闭环约束因子细节

在通过退化感知 ICP 获得完全受约束方向的特征向量矩阵 $V_f$ 后，将这一结果融入位姿图（pose graph）优化中，提出了 部分闭环约束因子（partial loop closure factor）。

给定一个闭环观测模型 $g(x_i,x_k)$，结合在提出的最小二乘 SLAM 优化方法，可以利用矩阵 $V_f$ 来构造如下优化目标：

$$x^{\ast }=\arg \underset{x}{\min }\sum _{(i,k)\in \mathcal{L}}{\Vert V_f\left(g(x_i,x_k)-l_{ik}\right)\Vert }_{V_f{\Lambda }_{ik}{V}_f^T}^2\text{\hspace{1em}(9)}$$

其中：

• $\mathcal{L}$ 表示所有考虑加入闭环约束的节点对 $(i,k)$ 的集合，
• $l_{ik}$ 是节点 $x_i$ 与 $x_k$ 间的观测值（例如 ICP 配准估计的相对位姿），
• ${\Lambda }_{ik}$ 是该观测的协方差矩阵，
• $V_f$ 是通过退化分析提取出的良好约束方向的特征向量矩阵。

通过该测量模型的重构方式，可以仅在 良好约束方向 上生成约束因子，从而实现对配准结果中可信信息的利用，忽略不可靠或退化的方向。

在每次迭代中，闭环约束的协方差 ${\Lambda }_{ik}$ 会被更新为：

$${\Lambda }_{ik}^{\prime }=V_f{\Lambda }_{ik}{V}_f^T$$

从而仅保留在受良好约束方向上的信息贡献。

在将这种“部分因子”引入到因子图中后，便可在构建的位姿图上进行最大后验推断（MAP inference），从而估计未知节点 $x_i$ 的最优值，使其在考虑测量不确定性的前提下，最大程度地符合观测信息。

这样，退化方向不会贡献误差项，从而避免将虚假配准对整体优化造成污染。

---

主要创新点

1. 动态退化检测：相比固定阈值，基于当前 Hessian 判断退化方向，使配准结果更稳健。

   • 在线性空间中只约束 well-conditioned 方向；
   • 可集成进通用因子图框架；
   • 显著减少虚假 loop closure 对轨迹和地图的误影响([cs.cmu.edu][1])。

---

总流程

以下是简化后的流程步骤：

for each new scan S:1. 与地图执行 Degeneracy-Aware ICP：-> 得到配准增量 Δx 和 well-conditioned 子空间 Vf2. 将作为 loop closure：-> 构建 Partial Loop Closure 因子，只对 Vf 方向约束-> 将因子加入 pose graph3. 优化 pose graph（GN/LM）更新所有 pose

---

总结

• 核心思路：仅施加可靠方向上的约束，忽略/屏蔽退化方向。
• 实现方式：结合谱分解、解 remapping 技术，在 ICP 和图优化中同步应用。
• 适用场景：水下 SLAM、隧道环境，以及任何 geometry-poor 场景。

该方法通过“Online + Partial Factor + 一致约束”机制，实现了鲁棒高效的退化处理，在实际水下 SLAM 任务中表现优异。

---