文献全名：Informed, Constrained, Aligned: A Field Analysis on
Degeneracy-aware Point Cloud Registration in the Wild

一、文献总结概述

1. 简介

• 这篇论文研究的是 LiDAR 点云配准（registration）中，当环境几何结构不良（geometry‑poor／degenerate）的情况下，传统配准方法（如 Iterative Closest Point, ICP）经常“失灵”：不收敛、或收敛到错误对齐／错误 pose。
• 作者系统地调查并比较了多种“退化 (degeneracy) 缓解 (mitigation)” 方法 —— 包括已有方法与新的尝试 —— 目的在于提升基于 LiDAR 的定位 / SLAM 系统在“真实世界、复杂 / 极端 /几何不良 (ill‑conditioned)”环境中的稳健性和可靠性。
• 论文在真实野外 (field) 与仿真 (simulated) 场景下进行了大规模实验 (first time at this scale) 来验证这些方法的效果。
  论文名“Informed, Constrained, Aligned (ICA)”也反映其方法论核心 (见下)。

---

2. 算法 / 研究 背景

背景：为何 “degeneracy” 是问题

• 点云配准 (registration) — 尤其 ICP — 假设环境具有足够丰富、多样和非对称几何结构 (planes, edges, corners …) 使得优化 (least‑squares) 有良好约束。
• 但在很多现实场景 (例如隧道、空旷走廊、重复结构、对称走廊、墙面、无人区、几何单调 / 平坦 / 对称环境) 中，这种假设破裂 —— 导致几何信息不足 (degenerate / ill‑conditioned)，ICP 的 Hessian / 信息矩阵变病态 (ill‑conditioned)，优化变得不可靠、收敛性 / 精度很差。
• 现有实践通常通过额外信息 (例如 IMU / odometry / GPS / loop‑closure / prior) 或后处理 (sensor fusion) 来“补偿”几何退化方向。但这种依赖外部信息的方法，不总是可行 (比如户外、未知环境、无 GPS、无 prior map、传感器失败等情况下)。

为什么需要系统研究

• 虽然社区中已有一些“degeneracy‑aware”方法 (例如最近的 X-ICP) 提出约束 / localizability‑aware registration，但缺乏系统、广泛 (多方法 + 多场景) 的对比和分析。
• 作者希望通过统一框架，对比许多策略 (active vs passive, hard vs soft constraints, regularization, global registration, etc.)，分析它们在真实场景下 (wild) 的表现 (not just synthetic toy cases)。

---

3. 文章贡献（创新点）

主要贡献可以总结为：

1. 首次大规模、系统性地对 degeneracy‑mitigation 方法进行对比分析，覆盖多种方法、数据集、真实战场 (field) 与仿真环境。
2. 引入多种 degeneracy‑aware 方法到 ICP / 点云配准中，并对它们进行分类 / 定性＋定量评估：包括硬约束 (hard constraints)、软约束 (soft constraints / regularization)、降维 / 子空间 (TSVD)、以及 global registration 等。
3. 敏感性分析 (sensitivity analysis) —— 对 least‑squares 优化在 ill‑conditioned 情况下的行为进行剖析，展示不同方法对 Hessian / 优化结构的影响，以及各方法在不同 degenerate 情况下的适用性 / 性能分布。
4. 公开实现 (open‑source) + 实际 field experiments + 仿真，为社区提供可复现、可比较的 baseline；增强方法的可用性。

总体来说，这篇 paper 为点云配准 / LiDAR‑SLAM 社区 系统化、严谨地处理 degeneracy 问题奠定了基础。

---

4. 需解决的关键问题 / 挑战

论文针对以下关键问题 (及挑战)：

• 如何检测 / 判断退化 (degeneracy)? —— 也就是在优化 / 配准过程中，哪些“方向 / DoF (degrees of freedom)”是缺乏几何约束 (under‑constrained) 的？
• 如何设计有效的缓解机制 (mitigation)？ —— 是依赖额外传感器 / prior (passive)，还是修改优化 (active)？哪种机制更稳健 / 通用？
• 不同场景 (环境几何条件) 下，各种方法的通用性 / 稳健性如何？ —— 不同退化类型 (平面、对称、重复结构、噪声、部分观测、低重叠) 是否都能被同一方法覆盖？
• 合理权衡精度 vs 稳健性 vs 计算开销 —— 硬约束 / 正则 / 全局注册 / 降维 /子空间 等方法，在现实 SLAM 中是否可实时 / 实用。
• 方法的可推广性 / 可复现性 —— 不仅在 controlled 仿真实验，而是在真实机器人 + LiDAR SLAM 流水线 (scan-to-map) 中是否可靠。

---

5. 论文提出的方法 (流程概览 / ICA 框架)

论文总体采用的方法框架可以概括为 ICA (Informed, Constrained, Aligned) —— “知情 / 约束 / 对齐”。 具体流程 (high-level)：

1. 检测 / 识别 degeneracy (degeneracy detection)

   • 在每次 ICP / 点云配准前 (或迭代中) 通过分析优化的 Hessian (或信息矩阵) / 特征值 (eigenvalues) 来判断是否存在 degenerate directions (ill‑conditioned directions) 。

2. 选择合适的缓解 (mitigation) 方法 —— 方法体系包括：

   • 硬约束 (hard‑constraint)：对检测出的 degenerate directions 引入 equality / inequality constraints，禁止或限制在这些方向上的过大更新 (drift)。
   • 软约束 / 正则化 (soft‑constraint / regularization)：通过子空间截断 (e.g. Truncated Singular Value Decomposition, TSVD)、Tikhonov 正则化 (线性/非线性) 等方式，抑制 ill‑conditioned 小特征值对优化的影响。
   • 全局注册 (global registration)：在严重几何退化 / 大变换 / 初值不佳 / 重叠较低时，使用 global registration 方法 (非迭代局部优化) 作为 fallback，以避免 local minima / drift。
   • 混合 / heuristic 参数调优：对于 soft constraints，通过启发式 (heuristic) 调整权重 / 参数，使其在复杂 / 退化环境中表现良好。

3. 实验验证 (simulation + real‑world field test)

   • 在仿真环境 (controlled degenerate scenarios) 和真实机器人 + LiDAR SLAM pipeline (scan-to-map) 中测试各方法。
   • 使用标准评价指标 (translational error, rotational error, convergence rate, robustness) 来比较方法性能。

---

二、文献详细内容

摘要（Abstract ）

ICP 配准算法在近十年来一直是基于 LiDAR 的机器人定位的首选方法。然而，即使在现代 SLAM 方案中，ICP 在几何退化 (geometrically ill-conditioned) 环境下仍可能退化并变得不可靠。现有的解决方案主要集中于利用额外的信息来源，例如外部里程计 (odometry)，以替代优化解中的退化方向，或在传感器融合设置中事后添加额外约束。

针对这一问题，本研究首次在文献中大规模地调查并比较了新颖及已有的退化缓解 (degeneracy mitigation) 方法，以实现稳健的基于 LiDAR 的定位，并分析这些方法在退化环境中的有效性。具体来说，本研究探讨了：

1. 对于 LiDAR 退化环境中 ill-conditioned ICP 问题，使用 主动 (active) 或 被动 (passive) 退化缓解方法的效果；
2. 首次评估 TSVD (Truncated Singular Value Decomposition)、不等式约束 (inequality constraints) 以及线性 / 非线性 Tikhonov 正则化在退化点云配准中的应用。

此外，本研究对 ICP 问题的最小二乘 (least-squares) 优化步骤进行了敏感性分析，以更好地理解每种方法对优化的影响及其预期表现。

通过多次真实机器人实地 (field) 和仿真实验验证了分析结果。结果表明，对于缺乏可靠外部估计辅助的 LiDAR-SLAM，主动优化退化缓解方法 (active optimization degeneracy mitigation) 是必要且有利的；而在复杂、 ill-conditioned 场景中，经过启发式参数微调的 软约束方法 (soft-constrained methods) 可以提供更优的结果。

---

1 引言（Introduction）

光探测与测距 (LiDAR, Light Detection and Ranging) 传感器能够以高频率提供精确的距离测量，使其非常适合在大规模环境中进行可靠的机器人位姿估计和精确建图。同步定位与建图 (SLAM, Simultaneous Localization and Mapping) 是在复杂和退化环境中操作机器人的关键工具 [Ebadi et al., 2024]。尽管 SLAM 和机器人定位也可以通过其他传感方式实现 [Kazerouni et al., 2022; Khattak et al., 2020b; Nissov et al., 2024b]，但野外机器人研究领域主要关注改进基于 LiDAR 的 SLAM 技术，以在大规模环境中提高精度和鲁棒性 [Bavle et al., 2023; Cadena et al., 2016; Ebadi et al., 2024; Nair et al., 2024]。

大多数被广泛接受的 LiDAR-SLAM 框架利用点云配准的高效性来生成准确的环境地图。这些算法通过寻找六自由度 (6 Degrees-of-Freedom, DoF) 的刚体变换来对具有足够重叠的点云进行配准。一种成熟的点云配准方法是迭代最近点 (ICP, Iterative Closest Point) 算法，广泛应用于计算机视觉、三维位姿估计和机器人领域 [Li et al., 2023]。在典型的 LiDAR-SLAM 框架中 [Khattak et al., 2020a; Jelavic et al., 2022]，执行 LiDAR 扫描到地图的配准，这对于将新信息与已生成的环境地图对齐至关重要。

算法局限性

尽管 ICP 成功且应用广泛，但仍存在一些限制，包括：

1. 收敛到局部最小值的风险；
2. 对初始变换估计的不准确敏感；
3. 易受逐点特征提取噪声影响；
4. 在优化问题缺乏几何约束时鲁棒性不足 [Censi, 2007a; Brossard et al., 2020]。

值得注意的是，前三种误差源可以通过修改 LiDAR-SLAM 框架直接解决，但优化问题本身的病态条件 (ill-conditioning) 直接源于环境中沿特定方向缺乏信息，因此难以克服。

LiDAR 退化的定义

根据文献 [Tuna et al., 2023; Petracek et al., 2024; Gelfand et al., 2003; Hatleskog and Alexis, 2024]，LiDAR 退化指的是 ICP 算法优化过程的病态问题。如果 LiDAR 点云信息不足，例如在自相似环境中几何约束不足，优化状态的一些线性组合可能变得不可观测。在存在这些不可观测优化状态的情况下，ICP 优化被认为是病态、退化或不可定位的。

退化点云配准

尽管机器人社区已经开发出在大多数环境中稳健运行的基于 LiDAR 的建图框架 [Vizzo et al., 2023; Xu et al., 2022; Jelavic et al., 2022]，但在退化环境（缺乏足够明显特征）下的点云配准及随后建图仍然是一个挑战，因为数据中缺乏必要的信息。针对这些场景的最新方法 [Tuna et al., 2023; Zhang et al., 2016; Westman and Kaess, 2019] 通常将问题分为两个步骤：

1. 分析与检测退化方向；
2. 缓解退化对优化的不利影响。

尤其是第一点受到了研究社区的广泛关注 [Nobili et al., 2018; Ramezani et al., 2020; Tagliabue et al., 2020; Liu et al., 2021; Nubert et al., 2022b; Zhen et al., 2017; Zhen and Scherer, 2019; Tuna et al., 2023; Zhang et al., 2016]，而针对已检测退化方向尽可能利用信息的工作则较少 [Nashed et al., 2021; Tuna et al., 2023; Zhang et al., 2016]。后者方法试图缓解退化对优化的影响，并能够充分利用位姿更新，至少在约束良好的优化方向上，而前者方法则完全跳过优化，依赖外部里程计传播 ICP 位姿估计。如 [Tuna et al., 2023] 所讨论的，完全跳过优化在 ICP 病态条件下仅在短时间内是可行策略，尤其是在初始配准估计不可靠且进一步微调点云配准有益的情况下。

野外 LiDAR 退化缓解

随着来自野外实验的 LiDAR 退化机器人数据集逐渐可用 [Chen et al., 2024b; Nissov et al., 2024c; Tranzatto et al., 2022; Pfreundschuh et al., 2024]，机器人社区对在复杂退化环境中操作机器人越来越关注。尽管如此，一旦检测到 LiDAR 退化，关于退化缓解方法有效性的研究仍存在基本空白。已有许多方法利用外部信息缓解 LiDAR 退化，例如基于因子图优化的传感器融合 [Jin and Jiang, 2024; Ebadi et al., 2021] 和滤波器 [Chen et al., 2024a]。然而，针对 ICP 基点云配准的优化步骤进行处理 [Tuna et al., 2023] 或在 ICP 方法内缓解 LiDAR 退化仍然是一个未解决的问题。

本工作聚焦于在 ICP 公式中缓解 LiDAR 退化的方法，以缩小上述研究空白。ICP 中的退化缓解方法被分为两类：被动 (Passive) 和主动 (Active) 退化缓解。

• 主动退化缓解 (Active degeneracy mitigation)： 如果方法输出的任何显式 LiDAR 检测结果（例如 2.2.1 节）用于在 ICP 代价函数优化中采取行动缓解退化，则该方法被归类为主动退化缓解。
• 被动退化缓解 (Passive degeneracy mitigation)： 与前者相反，如果 LiDAR 退化信息未在 ICP 配准中显式使用以缓解退化，则称其为被动退化缓解。

每类方法的相关工作将在 2.2.2 节中介绍。

本文贡献

针对上述研究空白，本工作通过多种仿真和真实世界实验（如图 1 所示）研究并比较了新旧退化缓解方法，以分析复杂退化环境下点云配准的有效性。除了研究结果外，本工作还提出了三种约束优化问题的方法，以实现稳健的点云配准。最后，将研究结果与理论推导及最先进方法的局限性结合，并通过多次机器人实地实验和仿真示例进行验证。本文主要贡献如下：

1. 针对机器人在几何退化的真实环境（如开阔自然场、城市隧道和复杂施工现场）中的操作，对不同约束类型在退化点云配准任务中的有效性进行了系统研究；
2. 首次在文献中引入、研究并讨论了子空间 Tikhonov 正则化 (sub-space Tikhonov regularization)、截断奇异值分解 (Truncated Singular Value Decomposition, TSVD) 和不等式约束 ICP 方法在退化点云配准领域的应用；
3. 通过仿真示例和多次详细的机器人实地实验（使用步行机器人、行走式挖掘机以及手持不同 LiDAR 的实验）对所讨论方法进行公平、独立的比较；
4. 整合开源方法与所有研究方法，并计划统一开源，以促进未来开发面向退化感知 (degeneracy-aware) 的 LiDAR-SLAM 系统研究。
   

---

2 相关工作（Related work）

2.1 （鲁棒）点云配准

通过点云配准实现快速且精准的位姿估计，是机器人感知领域数十年来的核心研究课题之一[Pomerleau et al., 2015]。在这一领域中，人们探索了多种方法族，例如：

• ICP 及其变体
  [Segal et al., 2009; Babin et al., 2021; Censi, 2008; Rusinkiewicz, 2019]
• 基于特征的方法
  [Zhang and Singh, 2014; Behley and Stachniss, 2018]
• 概率模型方法
  [Biber and Straßer, 2003; Myronenko and Song, 2010; Maken et al., 2020]
• 深度学习方法
  [De Maio and Lacroix, 2022; Bai et al., 2021a; Nubert et al., 2021]
• 鲁棒的全局配准方法
  [Zhou et al., 2016; Lim et al., 2022]

尽管已有大量进展，但由于其简洁性与实用性，较为简单的 ICP 变体仍然是最广泛使用的基于 LiDAR 的点云配准算法，例如：

• 点到点 ICP [Besl and McKay, 1992]
• 点到面 ICP [Low, 2004]
• 对称点到面 ICP [Rusinkiewicz, 2019]
• G-ICP [Segal et al., 2009]

这些 ICP 方法的共同点是：通过反复寻找两幅点云之间具有足够重叠的对应点对，再迭代地最小化预定义的对齐度量，从而求解点云间的六自由度刚体变换[Pomerleau, 2013; Li et al., 2023]。

---

另一方面，由于 ICP 方法容易陷入局部最优，研究者开始关注**全局最优且可验证（certifiable）**的稳健点云配准方法
[Yang et al., 2020b; Yang and Carlone, 2019; Carlone et al., 2015; Li et al., 2023; Lim et al., 2022, 2024]。这些算法能够在异常值占多数的情况下，依然保证估计结果为全局最优解。
同时，鲁棒 M-估计器也被提出用于处理噪声主导的点云配准问题。这些估计器通过降低异常值权重来减轻其对代价函数的干扰。常见的 M-估计器包括：

• Welsch
• Tukey
• Cauchy
• Huber
• Geman-McClure

已广泛用于机器人领域[Vizzo et al., 2023; Zhang et al., 2021; Babin et al., 2019]。最近，Babin 等人在 [2019] 中分析了不同鲁棒范数在非退化配准问题中的有效性，并得出结论：如果参数调整得当，大多数鲁棒范数在提高优化问题的信噪比方面表现相似。
为减少手动调参的需求，研究者提出：

• 自适应 M-估计器 [Chebrolu et al., 2021]，以应对不同输入分布

• 自适应非凸提升（Graduated Non-Convexity, GNC）
  [Yang et al., 2020a, 2020b; Briales and Gonzalez-Jimenez, 2017; Lim et al., 2022, 2024]
  以提高对大扰动的鲁棒性。
  此外，受 ICP 在实际应用中成功的推动，研究者还从多个角度增强其鲁棒性，例如：

• 改进特征采样
  [Kwok, 2018; Kwok and Tang, 2016; Petracek et al., 2024]

• 自适应匹配技术
  [Vizzo et al., 2023]

• 优化加速方案
  [Zhang et al., 2021]

---

然而，这些方法都没有特别解决当点云本身缺乏足够信息（例如环境结构对称或自相似）时的退化问题。这种情况在实际环境中十分常见，例如：

• 隧道
• 开阔平地
• 狭窄走廊
  [Tuna et al., 2023; Nubert et al., 2022b; Pfreundschuh et al., 2023]
  这些环境因缺乏足够的几何约束而导致 ICP 优化退化。

---

2.2 优化退化的检测与缓解

2.2.1 优化退化检测

配准不确定性

之前已有方法通过估计位姿估计过程的协方差来建模点云配准的不确定性，并捕获优化问题中的退化性 [Censi, 2007a; Brossard et al., 2020]。虽然这可能是一个良好的病态指标，但所得度量往往过于乐观 [Bonnabel et al., 2016]。近期，[Talbot et al., 2023] 的作者进一步深入研究如何精确建模 LiDAR 点云配准信息的不确定性，以作为松耦合测量用于多模态传感器融合框架。更具体地，作者在 LiDAR SLAM 的背景下评估了 [Censi, 2007b] 中的 ICP 不确定性建模，并为降低计算量而简化了跨协方差项。

类似地，在 LOG-LIO2 [Huang et al., 2024] 中，作者提供了一种增量式方法，以高效计算 LiDAR 测量的不确定性。他们提出的不确定性模型同时考虑了距离与方位角的不确定性，以及来自 LiDAR 激光束入射角与表面粗糙度的不确定性。随后，该不确定性模型被整合进 LiDAR 惯性里程计框架中，以展示其在实际部署中的效益。作为最新的工作之一，[Hatleskog and Alexis, 2024] 提出了一种新的概率方法，通过考虑测量噪声和表面法向估计噪声，来估计优化方向退化的概率。

基于学习的方法

近期研究探索了数据驱动的方法来识别 LiDAR 信息的退化。[Gao et al., 2020] 通过融合协方差估计与环境信息量，引入了一种基于深度学习的熵指标。为避免机器人运行时复杂的蒙特卡洛采样，[Nubert et al., 2022b] 建议使用合成数据进行训练，并仅依赖当前 LiDAR 扫描来计算 6 自由度的可定位性指标。尽管数据驱动方法能够通过建模高阶动态来超越传统方法，但其可解释性仍然存在挑战。

基于优化稳定性的方法

其他工作 [Cho et al., 2018; Tagliabue et al., 2020; Ebadi et al., 2021] 提议使用优化条件数作为整个 6 自由度位姿估计的综合退化指标。类似地，CompSLAM [Khattak et al., 2020a] 使用 D-optimality 准则 [Arévalo, 2018] 来检测多模态传感器融合框架中的优化退化。基于类似的建模，[Hinduja et al., 2019] 采用优化的相对条件数作为退化检测阈值，而不是最小特征值。

作为早期开创性方法之一，[Zhang et al., 2016] 提出了一个称为退化因子的检测指标。该指标通过分析优化 Hessian 矩阵的最小特征值来识别优化中的退化方向。类似地，DAMS-LIO [Han et al., 2023] 继承了 [Zhang et al., 2016] 的思想，通过对优化 Hessian 的特征值进行阈值处理来获得退化方向，以用于基于滤波的状态估计方法。

此外，[Bai et al., 2021b] 指出仅使用 Hessian 的最小特征值在实际中并不可靠，因为需人为调节阈值。为解决这一问题，作者提出同时利用最小特征值与条件数。近期，[Jin and Jiang, 2024] 使用最小特征值和部分条件数提出了一个称为“退化度（degenerate degree）”的指标。

考虑到为最小特征值设定阈值的挑战，[Lee et al., 2024] 提议使用卡方检验来计算归一化特征值的非启发式（non-heuristic）阈值，随后用这些归一化特征值检测 LiDAR 与视觉优化问题中的退化方向。

在最新的工作中，[Ji et al., 2024] 提出一种点到分布（point-to-distribution）的 ICP 退化检测方法。利用其自适应体素化方法得到的概率分布，作者证明该指标本质上与优化 Hessian 的特征值相关。

细粒度方法

类似地，[Zhen and Scherer, 2019] 提出直接利用 Hessian 特征空间估计环境的可定位性，通过度量点与法向约束对优化状态的敏感度。基于该公式，[Zou et al., 2023] 从 LiDAR 测量生成信息矩阵来识别优化中的退化方向，并将其传播至因子图中用于精确的机器人位姿估计。

为了更细粒度的退化处理，X-ICP [Tuna et al., 2023] 提出一种基于对应关系的退化检测方法，用于约束点云配准的优化问题。该方法独立考虑每个点到平面对应项的可观性贡献。沿着 X-ICP 的方向，[Chen et al., 2024a] 在配准步骤中提出利用点云对应关系来检测 LiDAR 退化。

---

2.2.2 机器人中的退化缓解

在复杂与非结构化环境中的机器人操作中，已有大量方法被提出以缓解 LiDAR 位姿估计方法中优化病态的问题。这些方法可根据其缓解退化的方式分为 被动(Passive) 或 主动(Active)。更明确地说，这取决于方法是否改变底层优化代价函数，或基于检测到的退化信息来改变点云配准产生的相对 LiDAR 位姿估计。技术细节在第 4 节介绍。

被动退化缓解

大多数被动方法使用补充传感器模态进行传感器融合 [Nissov et al., 2024c; Wen et al., 2024; Bai et al., 2021b]，或依赖外部里程计来源 [Khattak et al., 2020a; Pfreundschuh et al., 2023; Ebadi et al., 2021; Tagliabue et al., 2020]，以便在 LiDAR 退化时继续传播机器人位姿。

值得注意的是，[Lim et al., 2023] 使用了一种自适应参数设置策略来缓解 LiDAR 退化的影响。类似地，[Ferrari et al., 2024] 通过改进 ICP 的对应搜索来提高 ICP 质量，从而间接增强退化鲁棒性。

基于对优化变量的分析，[Gelfand et al., 2003] 研究了点到平面 ICP 的几何稳定性，并提出了一种基于采样的方法来提取最具信息的扫描点，以改善优化问题的条件性。[Petracek et al., 2024] 提出减少输入点云中的信息冗余。为此，作者采用了一种精细且退化感知的采样策略，从而减轻 LiDAR 退化的负面影响。

主动退化缓解

[Floery, 2010] 是最早尝试主动退化缓解的工作之一，通过为点云配准添加额外约束来减少错误配准。类似地，[Olsson and Eriksson, 2008] 通过引入非线性等式约束来降低旋转变量线性化带来的误差。

近期，[Zhang et al., 2016] 提出了 Solution Remapping 方法，将点云配准所得的相对位姿更新投影到约束良好的方向上。X-ICP [Tuna et al., 2023] 中，作者提出沿退化方向加入硬约束来减少位姿估计误差。

[Nashed et al., 2021] 在非线性最小二乘问题中使用额外的正则化项以缓解优化退化。此外，据作者所知，[Nashed et al., 2021] 是唯一对不同退化缓解方法进行比较的工作。使用 X-ICP 的约束优化，[Chen et al., 2024a] 提出在其约束型误差状态 IEKF 中使用检测到的退化信息，以防止在退化方向使用 LiDAR 测量更新。然而，他们的比较有限，并未推广至 SLAM 或点云配准的实际应用。此外，[Nashed et al., 2021] 并未进行真实场地实验，并且使用 RGBD 摄像头而非 3D LiDAR。

尽管已有大量关于 LiDAR 退化的研究，但文献仍缺乏关于如何有效利用 LiDAR 退化信息的全面分析。本工作在仿真与真实世界场景中深入分析并比较不同方法，以填补这一空白。

---

2.2.3 多传感器融合与多模态退化缓解

除 LiDAR 中心的方法外，其他方法也关注利用补充的感知模态来缓解 LiDAR 退化带来的不利影响。

COIN-LIO [Pfreundschuh et al., 2023] 利用 LiDAR 强度图像的视觉特征来补充 LiDAR 惯性框架中信息不足的方向。类似地，[Jin and Jiang, 2024] 使用超宽带定位的约束补充 ICP 退化方向。此外，[Nissov et al., 2024c] 使用 LiDAR 与雷达模态构建因子图滑动窗口平滑器，并为每个模态提供显式退化信息。

近期，[Lee et al., 2024] 将 LiDAR、IMU 与视觉信息结合在一个位姿图优化框架中，并在不同模态之间主动切换。作者分别检测 LiDAR 与相机的优化退化，而不是依赖通用的协方差估计方法，如动态协方差缩放（DCS）[Agarwal et al., 2013] 或 fallback 图 [Nubert et al., 2022a]。

在 DARPA SubT 挑战 [Ebadi et al., 2024] 中，多项方法强调了退化感知 LiDAR-SLAM 的重要性。例如，CompSLAM [Khattak et al., 2020a] 使用退化检测来切换传感器模态，而 DARE-SLAM [Ebadi et al., 2021] 则通过在退化情况下阻止添加 LiDAR 因子来防止退化传播。另一个方法 LION [Tagliabue et al., 2020] 通过测量多路复用器估计 LiDAR 因子的测量不确定性，以便在滑窗因子图中稳健融合 IMU 数据。

为减少系统对单一模态的依赖，[Zhao et al., 2021] 提出运行三个并行位姿图：一个用于 LiDAR-IMU，一个用于视觉-IMU，以及一个用于融合的中央 IMU 因子图——这种依赖精确测量不确定性的方式展示了因子图方法的高效性。在扩展该框架的基础上，[Zhao et al., 2024] 提出预测 LiDAR 扫描的可观性并将其作为协方差矩阵融入系统。

尽管这些方法提高了整个系统的鲁棒性，但它们未从根本上解决 LiDAR 退化问题，而是依赖于外部传感器融合来增强系统层级的鲁棒性。本工作专注于研究如何在退化点云配准框架内提升 LiDAR 位姿估计的质量与鲁棒性。

---

3 问题分析与预备知识

本工作研究了不同优化退化缓解方法的优缺点。具体来说，本文考虑了在几何上具有挑战性的环境下，LiDAR-SLAM 中点云配准的退化问题。本节将介绍点云配准和退化检测的预备知识。

3.1 点云配准与 ICP

点云配准问题旨在对齐两个点云，并获得它们之间的相对刚性变换，如图 2 所示。为了实现这一目标，已经提出了各种技术和度量方法，如第 2 节所讨论。在本工作中，采用了带点到平面（point-to-plane）代价函数的 ICP 算法 [Low, 2004]，因为它被机器人学界和许多点云配准框架广泛采用 [Jelavic et al., 2022; Tagliabue et al., 2020; Khattak et al., 2020a; Dellenbach et al., 2022; Pomerleau et al., 2013; Xu et al., 2022]。

具体来说，点云配准侧重于估计源点云与参考点云之间的齐次变换 $T_L^M\in SE(3)$，源点云和参考点云的点数分别为 $N_p$ 和 $N_q$，给定初始猜测 $T_{L,\text{init}}^M$。

源点云 $L_P\in {\mathbb{R}}^{3\times N_p}$ 表示在 LiDAR 坐标系 $L$ 中，而参考点云 $M_Q\in {\mathbb{R}}^{3\times N_q}$ 表示在（子）地图坐标系 $M$ 中。齐次变换 $T_{ML}=\left[\begin{array}{c}{R}_{ML}|t_{ML}\end{array}\right]$ 包含旋转矩阵 $R\in SO(3)$ 和平移向量 $t\in {\mathbb{R}}^3$，其中 $t=[t_x,t_y,t_z{]}^{\top }$。

对于源点云中的每个点 $L_{p_i}\in {\mathbb{R}}^3$，在参考点云中通过 K 最近邻搜索找到最近的参考点 $M_{q_i}\in {\mathbb{R}}^3$，搜索过程中使用初始变换 $T_{L,\text{init}}^M$，如图 2 中的匹配模块所示。通常，将该变换作为初始猜测应用于 $L_P$ 中的所有点，以获得 $M_{P,\text{init}}$，从而改进匹配过程和优化收敛特性 [Brossard et al., 2020]，通过 ICP 的迭代过程实现。在本节其余部分，所有变量均以（子）地图坐标系 $M$ 表示，为简洁起见，省略坐标系符号。

对应关系搜索的结果可表示为一对多匹配 $p_i,Q(p_i)$，其中 $i\in 1,\dots ,N$，$p_i$ 与 $q_i:=Q(p_i)$ 分别为源点云和参考点云匹配的点对，且 $N\le N_p$。

此外，$n_i\in {\mathbb{R}}^3$, $|n_i|=1$ 表示参考点 $q_j$ 的表面法向量，这是点到平面 ICP 代价函数所需的。匹配点的数量 $N$ 表示优化问题的规模。点到平面 ICP 最小化问题的代价函数定义为：

公式 (1) 所示的问题可以按照 [Pomerleau et al., 2015] 中的推导，在引入标量三重积和旋转矩阵线性化后，重写为二次代价最小化问题：

其中，$x=[r^{\top },t^{\top }{]}^{\top }\in {\mathbb{R}}^6$ 为优化变量，其中平移部分 $t\in {\mathbb{R}}^3$。旋转向量 $r\in {\mathbb{R}}^3$ 通过近似 $R\approx r^{\wedge }+I$ 表示，符号 $\wedge$（hat operator）将向量转换为其对应的反对称矩阵 $r^{\wedge }\in so(3)$。这里，$so(3)$ 是 SO(3) 群的李代数，由 3×3 的反对称矩阵组成。

将 $A$ 和 $b$ 代入公式 (2) 后，可得到常规的线性最小二乘优化形式：

其中 $E(x)$ 表示优化的残差误差，对该代价函数求最小值即可得到 Frobenius 范数（2-范数）解。此处，$A\in {\mathbb{R}}^{6\times 6}$ 表示优化问题的 Hessian 矩阵，$b\in {\mathbb{R}}^6$ 包含配准施加的约束。在每个线性化点上，都可以计算最优平移向量 $t$ 和旋转向量 $r$。由于线性化误差和对应点匹配误差，ICP 的匹配和最小化操作需要迭代进行，直到收敛。

最小化问题公式 (1) 可以通过非线性求解器求解，如 高斯-牛顿法 (Gauss-Newton) 或 Levenberg-Marquardt (LM) 优化 [Agarwal et al., 2024]，也可以使用闭式解求解器，如 奇异值分解 (SVD) [Wall et al., 2003] 或 上下三角分解 (LU decomposition) 来求解公式 (3)。该操作对应于图 2 中的退化缓解模块。除第 4.1.6 节和第 4.2.1 节外，本工作采用 SVD 来求解公式 (3)，原因将在下文说明。

公式 (3) 的最小值出现在代价函数对 $x$ 的梯度在所有方向上为零时。根据之前的定义：

重新排列项后得到正规方程：

利用公式 (5) 可求解最小二乘问题，其几何解释为 $b$ 在矩阵 $A$ 的列空间上的投影，如图 3 所示。优化残差可视为 $b$ 到 $A$ 列空间的正交距离。

---

3.2 最小二乘问题的敏感性

本节将前文介绍的方法与最小二乘最优化问题的敏感性和条件性联系起来。一个问题对输入的小变化的敏感性被称为该问题的条件性。如果输入的小变化会导致输出发生很大变化，则称问题是病态（ill-conditioned），否则称为良态（well-conditioned） [Heath, 2018]。

根据推导 [Golub and Van Loan, 2013; Heath, 2018]，矩阵 $A$ 的欧几里得条件数可以定义为：

其中，$A^{+}$ 是 $A$ 的伪逆。伪逆可以通过 $A$ 的 SVD 计算，如第 3.3 节和公式 (15) 所述。此外，${\sigma }_{\max }$ 和 ${\sigma }_{\min }$ 分别表示 $A$ 的最大和最小特征值。同样，特征值也可以通过 SVD 获得，如公式 (15) 所示。

此前，作者 [Zhang et al., 2016] 对其退化检测因子及方阵线性系统 $Ax=b$ 的敏感性分析提供了详细技术细节。然而实际上，ICP 的公式并不使用该等式，而是求解最小二乘问题，如第 3 节和公式 (5) 所述。最小二乘问题 $Ax\approx b$ 的条件性不仅取决于矩阵 $A$，还取决于右端向量 $b$，因此仅用 $\text{cond}(A)$ 并不足以刻画敏感性。如图 3 所示，当约束向量 $b$ 接近 $\text{span}(A)$ 时，将产生较小的残差，因此 $b$ 的扰动对 $x$ 的影响较小。可以推断，当 $\theta \to \pi /2$ 时，最小二乘解将变得奇异。该行为可用

$$\cos \theta =\frac{|Ax|}{|b|}$$

来刻画。

对 $b$ 的扰动

分析 $x$ 对右端向量 $b$ 改变的敏感性。为简洁起见，以下使用 2-范数表示为 $|\cdot |$。设扰动为 $\Delta b$，由此产生 $x$ 的变化为 $\Delta x$，则正规方程公式 (5) 可重写为：

抵消原正规方程的项后得到：

取两边的 2-范数得到：

为了得到相对变化幅度，对两边除以 $|x|$，并结合 Cauchy–Schwarz 不等式 $\text{cond}(A)=|A||A^{+}|\ge 1$ 以及 $|A^{+}|\approx 1/{\sigma }_{\min }$，得到：

公式 (10) 表明，$\Delta x$ 的敏感性不仅受 $\text{cond}(A)$ 的影响，还取决于 $b$ 是否能被 $Ax$ 良好近似。当 $\cos \theta \approx 1$ 时，有效条件数约为 $\text{cond}(A)$，而当 $\cos \theta \approx 0$ 时，有效条件数会远大于 $\text{cond}(A)$。这说明，即使矩阵良态，如果 $b$ 有噪声且残差较大，最小二乘优化也可能不良定义。

对 $A$ 的扰动

为了完整理解敏感性，也应考虑 $A$ 的扰动。类似 $b$ 的扰动，加入 $\Delta A$ 后正规方程为：

展开并忽略高阶项，抵消原正规方程的项，得到：

于是：

应用 2-范数并整理变量，得到相对变化的不等式：

公式 (14) 清楚地展示了最小二乘问题对 $A$ 扰动的敏感性。可以看到，有两项影响扰动在 $x$ 上的传播：一是矩阵 $A$ 的条件数，二是与残差 $r$ 成比例并被 $\text{cond}(A{)}^2$ 缩放的项。第二项尤其重要，因为任何残留的 $r$ 都会被 $\text{cond}(A{)}^2$ 放大，对于病态问题至关重要。因此，最小二乘优化问题的有效条件性与矩阵 $A$ 的条件性不同。这说明在优化时或优化之前缓解退化非常重要，以确保残差项不会干扰最小化的质量。

---

3.3 优化求解

在上一节中，分析了正规方程（公式 (5)）对 $A$ 和 $b$ 扰动的敏感性；然而，对于如何获取最优优化变量并未讨论。因此，本节介绍如何利用 SVD 计算能够最小化公式 (3) 的优化变量。

公式 (5) 可以通过高效的分解方法求解，例如 Cholesky 分解，前提是 $A^{\top }A$ 不病态（即不接近奇异）。然而，由于本工作关注 LiDAR 的退化问题，因此优化问题可能是病态的，使用 Cholesky 分解并不具优势。相反，采用 奇异值分解 (SVD)，该方法在对角化原矩阵的同时生成两个正交基，用于求解该方程，尤其适用于病态优化问题，因为 SVD 解对所有（非方阵）矩阵均存在。

对于一个 $m\times n$ 的矩阵 $A$，SVD 分解定义为：

其中，$U$ 是 $m\times m$ 的旋转矩阵，$\Sigma$ 是 $m\times n$ 的对角缩放与投影矩阵，包含矩阵 $A^{\top }A$ 的奇异值，$V$ 是 $n\times n$ 的第二旋转矩阵。此外，$A^{+}$ 是 $A$ 的伪逆，在 $A$ 满秩的情况下，$A^{+}\approx A^{-1}$。对于对称且正（半）定矩阵 $A$，SVD 与特征值分解相同：

其中 ${\Sigma }^2$ 是一个平方对角矩阵，其对角线元素为 $A$ 的奇异值平方。矩阵 $A$ 的右奇异向量 $V$ 等价于 $A^{\top }A$ 的特征向量，而 $A$ 的奇异值是 $A^{\top }A$ 特征值的平方根。

完成 SVD 分解后，得到的正交基与对角缩放矩阵可以用于伪逆计算：

将公式 (17) 与公式 (15) 代入公式 (5) 可得最优解：

由于 SVD 基于公式 (17) 中的伪逆操作，无论 $A$ 的条件性如何，都可以求得最优 $x^{\ast }$。然而，在优化病态方向上，该 $x^{\ast }$ 并非真正最优，因为这些方向仍然不可观测。

类似地，SVD 对病态或接近秩亏的优化问题尤其有效，因为分解允许忽略接近零的奇异值。因此，最小二乘解对数据扰动的敏感性较低 [Heath, 2018]。这一特性被方法之一 Truncated SVD 所利用，详见第 4.1.4 节。

---

3.4 LiDAR 退化检测

当机器人在几何特征稀缺且环境复杂的场景中部署时，点云配准可能变得病态（ill-conditioned）。因此，需要一种方法来检测优化中的退化方向。由于本工作关注退化缓解，所有讨论的方法均采用 X-ICP [Tuna et al., 2023] 中描述的退化检测方法，以保证公平比较（对于所有需要位姿可定位性信息的方法）。该可定位性检测过程在图 2 中显示为 localizability detection 模块。

根据 [Tuna et al., 2023] 的推导，优化 Hessian 矩阵被划分为子矩阵，以识别对应于优化变量 $x$ 的旋转部分 $r$ 和平移部分 $t$ 的特征向量：

分解后的 Hessian 矩阵 $A_{rr}\in {\mathbb{R}}^{3\times 3}$ 和 $A_{tt}\in {\mathbb{R}}^{3\times 3}$ 分别对应旋转信息和平移信息。此外，由于平移和旋转在尺度和物理意义上存在差异，直接联合处理 $t$ 和 $r$ 并不简单，因此 $A_{rr}$ 和 $A_{tt}$ 被用于退化检测。对于对应的子矩阵，其特征值分解为：

其中，$V^t\in {\mathbb{R}}^{3\times 3}$ 和 $V^r\in {\mathbb{R}}^{3\times 3}$ 是矩阵形式的特征向量，${\Sigma }^t=\text{diag}({\sigma }_{tt})$ 和 ${\Sigma }^r=\text{diag}({\sigma }_{rr})$ 分别是 $A_{tt}$ 和 $A_{rr}$ 的缩放矩阵。${\sigma }_{tt}$ 和 ${\sigma }_{rr}$ 是 $A_{tt}$ 和 $A_{rr}$ 的特征值。

在本工作的其余部分，通过结合 ${\Sigma }^t$ 和 ${\Sigma }^r$ 的特征值，假设 $V^t$ 和 $V^r$ 的零空间近似是已知的。如果一个特征向量 $v_j\in V^t,V^r$ 位于 $V^t$ 或 $V^r$ 的零空间中，则被标记为退化（不可定位），否则为非退化（可定位）。

此外，还考虑了映射关系 $V\leftrightarrow V^r,V^t$，因为第 4 节中描述的一些方法期望退化特征向量在 ${\mathbb{R}}^{3\times 3}$ 中，而其他方法期望在 ${\mathbb{R}}^{6\times 6}$ 中。此外，分别检测平移和旋转退化可以更精确地考虑 Hessian 矩阵 $A$ 条目的尺度差异。基于这一考虑，交叉项 $A_{tr}$ 和 $A_{rt}$ 被省略。然而，由于所有需要退化检测信息的方法都可以访问相同的退化信息，因此这种省略的影响被减轻。

图 4：
对所有研究方法的概览图，展示了这些方法在点云配准流程的哪一个阶段发挥作用。字母 P 和 A 分别表示被动（Passive）与主动（Active）退化缓解方法。符号 ⋆ 表示该方法在本研究中首次被用于退化点云配准的场景。
其中，Zhang et al. 指代文献 [Zhang et al., 2016]，Petracek et al. 指代文献 [Petracek et al., 2024]，Gelfand et al. 指代文献 [Gelfand et al., 2003]。

---