点云配准文献之-Informed, Constrained, Aligned Degeneracy-aware Point Cloud Registration（2）

文献全名：Informed, Constrained, Aligned: A Field Analysis onDegeneracy-aware Point Cloud Registration in the Wild相关内容:点云配准文献之-Informed, Constrained, Aligned Degeneracy-aware Point Cloud Registration（1）本节将

花雨仙晨

1635人浏览 · 2025-12-08 16:56:59

花雨仙晨 · 2025-12-08 16:56:59 发布

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

二、文献详细内容

4 方法与实现(Methodology and implementation)

本节将介绍第 5 节中用于比较的各类方法的理论基础与实现细节。受第 1 节和第 2 节讨论的启发，这些方法被划分为两大类，即：
i) 主动退化缓解方法（Active degeneracy mitigation methods） 和
ii) 被动退化缓解方法（Passive degeneracy mitigation methods），如图 4 所示。
在这里插入图片描述

这两类方法的主要区别在于是否利用可用的退化检测信息。被动方法不使用这些信息，而主动方法会以不同方式利用这些信息来减轻优化退化带来的不利影响。

4.1 主动退化缓解

“主动退化缓解”类别包含那些利用检测到的退化信息来修改优化目标函数或优化变量的方法。这类方法通常需要精确的退化检测信息才能达到最佳效果。此外，这些方法会执行额外的计算，用于减轻退化带来的影响，或者通过避免执行配准来规避退化问题。此外，如图 4 所示，主动方法还可以根据其影响 ICP（迭代最近点）流程的阶段进一步分类。

4.1.1 等式约束

等式约束（Eq. Con.），如最近在 X-ICP [Tuna 等, 2023] 中讨论的，利用约束优化技术沿退化方向增加额外的等式约束，从而在迭代优化过程中防止沿这些方向更新解。在等式约束的公式中，将使用每个大小为 $\times 1$ 的平移和旋转退化特征向量 $v_j \in {V_t, V_r}$ 作为约束方向。这里， $V_{r,t} \in \mathbb{R}^{3\times3}$ 分别为旋转和平移部分的特征向量矩阵。给定这些特征向量，约束公式为：

在这里插入图片描述

其中 $t_0$ 和 $r_0$ 分别为平移和旋转的约束值。在本文中，等式约束（Eq. Con.）的约束值设置为 $t_0 = r_0 = 0$ ，以防止沿退化方向的运动更新。在实际应用中，这些约束值可以设置为任何预先确定的值，以引导优化收敛到正确的最小值，从而提高收敛性。约束值的确定是一个活跃的研究课题，本工作不予涉及。因此，约束值设置为 $0$ ，意味着优化仅限于沿退化方向 $v_j \in {V_t, V_r}$ 的优化变量 $x$ 的线性组合进行更新。

能够控制运动更新的方向和幅度是等式约束的一大特性，这直接有利于解决退化优化问题的效果。这些额外约束的潜在作用在第 3.2 节已被强调。

为了在方程 (3) 所示的无约束代价函数中使用这些约束，将方程与零向量进行增广，以应用于 6 自由度优化问题。得到的增广约束为：

在这里插入图片描述

然后将这些约束堆叠成矩阵形式 $C x = d$ 并集成到 ICP 问题的公式中：

在这里插入图片描述

其中等式约束的数量为 $m_t$ 个平移约束和 $m_r$ 个旋转约束，总约束数为 $c = m_t + m_r$ 。

此时，无约束 ICP 的代价函数可以重写为等式约束优化问题：

在这里插入图片描述

引入拉格朗日乘子可以将等式约束优化问题转化为无约束优化问题 [Tuna 等, 2023]。该最小化问题的拉格朗日函数定义为：

其中 $\lambda \in \mathbb{R}^c$ 为拉格朗日乘子， $c$ 为优化中的约束数量。有趣的是，可以通过拉格朗日乘子的大小推断约束的效果。约束作用越大，拉格朗日乘子的幅值越大；反之，如果乘子接近零，则说明约束对优化的影响很小。通过对比方程 (4) 和方程 (25)，可以看出约束的影响仅体现在增加的约束项上。

将拉格朗日函数重新整理为矩阵形式，得到熟悉的最小二乘形式：

$\min_{x' \in \mathbb{R}^{6+c}} \left| \underbrace{ \begin{bmatrix} 2A^\top A & C^\top \\ C & 0 \end{bmatrix}}_{A'} \underbrace{ \begin{bmatrix} x^* \\ \lambda^* \end{bmatrix}}_{x'}- \underbrace{ \begin{bmatrix} 2A^\top b \\ d \end{bmatrix}}_{b'} \right|,(26)$

其中 $\in \mathbb{R}^{c \times 6}$ 为约束矩阵， $\in \mathbb{R}^{c \times 1}$ 为约束向量。增广的优化向量 $x^{'}$ 由优化变量和拉格朗日乘子组成。该增广代价函数可使用 SVD 求解，与标准点到平面 ICP 方程 (3) 相似。从理论上，退化约束的激活情况可以通过拉格朗日乘子 $\lambda$ 的值来分析。乘子幅度越大，约束对优化的影响越大 [Bertsekas, 2014]。在本工作中，该方法为优化添加硬约束，通过生成拉格朗日函数（方程 25）影响优化的误差特性。约束也对 $b$ 和残差 $r$ 的相对误差进行了约束，同时限制了最小二乘优化问题对扰动的敏感性（方程 14 和方程 10）。

4.1.2 不等式约束

虽然等式约束提供了一种直接处理退化方向的方法，但系统的系统性误差（如特征向量计算误差、传感器噪声以及 ICP 的迭代性质）可能需要放松约束以逃离局部最小值。在这种情况下，不等式约束（Ineq. Con.）提供了一种简单而有效的替代方法，且具有更大的收敛区域：

$\min_{x \in \mathbb{R}^6} |Ax - b|^2, \\quad \text{s.t. } -\epsilon \le Lx \le \epsilon. (27)$

这里，约束矩阵 $\in \mathbb{R}^{6\times6}$ 是基于优化的退化方向按行生成的矩阵，每行表示一个退化方向。如第 3.4 节所述，X-ICP [Tuna 等, 2023] 的退化检测方法可检索退化方向。不等式约束的上下界可以不同，但为简化起见，这里两者取相同的幅值向量 $\epsilon \in \mathbb{R}^{6\times1}$ 。为考虑平移和旋转分量的尺度差异，旋转和位移分量的幅值分别设置为 $\epsilon/2$ 和 $\epsilon$ ，得到：

$\epsilon = \begin{bmatrix} \epsilon/2 \ ,\epsilon/2 \, \epsilon/2 \ ,\epsilon \ ,\epsilon \ ,\epsilon \end{bmatrix}.$

其中， $\epsilon$ 经过对 Ulmberg 隧道数据集的启发式调参得到，设置为 $\epsilon = 0.0014$ 。

不等式约束优化问题的求解并不像等式约束那样简单。为了求解，将其转化为二次规划（QP）问题：

$\min_{x \in \mathbb{R}^6} \frac{1}{2} x^\top F x + f^\top x, \quad \text{s.t. } -\epsilon \le Lx \le \epsilon,(28)$

其中， $2A^\top A$ ， $-2A^\top b$ 。该二次规划问题适用于 $F$ 可能病态的情况，并且可以处理不同类型的约束（等式和不等式同时存在）；但为了便于对比，本文仅使用不等式约束进行研究。开源 QP 库 QPmad [Schreppel 和 Brembeck, 2020] 用于求解该问题，内部采用 Goldfarb 和 Idnani 的双对偶方法 [Goldfarb 和 Idnani, 2006]。在每次迭代中，计算当前活动约束集下目标函数的最小值 [Schreppel 和 Brembeck, 2020]。如果约束不再活跃，则可以从活动约束集中移除，从而放松约束优化。

不等式约束的状态可通过其对偶变量（拉格朗日乘子）推断。如果约束未激活，则不等式约束的幅值 $\epsilon$ 大于该方向上的运动更新。与等式约束类似，该方法向优化中添加退化感知约束，为相对误差和残差 $r$ 提供界限，从而减小扰动对优化变量的影响，如第 3.2 节所述。

4.1.3 解重映射（Solution Remapping）

解重映射指的是 [Zhang 等, 2016] 提出的优化退化缓解方法，如图 4 所示，在本文中称为 Zhang 等方法。从现在起本文统一称为 Zhang 等方法。该方法通过对优化 Hessian 进行特征值分解，构造一个解投影矩阵，用于仅在约束充分的方向上投影退化优化解 $x$ 。

首先，该方法基于退化检测生成一个增广特征向量矩阵 $V_{\text{aug}}$ ，通过将 $V$ 中对应的退化特征向量置零实现。利用第 3 节引入的定义，重投影矩阵计算为：

$V_{\text{aug}}^{-1} V.(29)$

在最小化方程 (3) 后，将重投影矩阵应用到解 $x^*$ 上：

$x^*_{\text{aug}} = S x^*.(30)$

该算法的核心假设是优化仅在退化方向上退化，且通过识别 Hessian 的零空间即可将解重新投影到优化中约束充分的方向。在本文中，假设 Hessian 的零空间已知，因此只需要重建投影矩阵。在本文的背景下，Zhang 等方法不会干扰 $co n d (A)$ 或残差 $r$ ，而是将解 $x$ 投影到优化中约束充分的方向上。因此，在最小二乘优化过程中，输入噪声可能会被放大（见第 3.2 节）。欲了解更详细的推导和解释，请参考原文 [Zhang 等, 2016]。

4.1.4 截断奇异值分解（Truncated SVD, TSVD）

截断 SVD（TSVD）是一种处理高条件数病态矩阵的线性代数方法，通过有效降低矩阵秩来改善计算稳定性 [Hansen, 1990; Draisma 等, 2018]。对于对称半正定矩阵，TSVD 通过截断最小特征值，同时保留最大的特征值，从而产生矩阵的 $k$ 阶近似，其中 $k$ 表示保留的前 $k$ 个最大特征值。

TSVD 需要对 Hessian（方程 (3)）进行特征值分解：

$\Sigma V^\top.(31)$

然后，通过对角缩放矩阵 $\Sigma$ 进行截断，对角线上每个特征值 $\sigma_i$ （ $\in {1, \dots, 6}$ ）根据对应特征向量是否沿退化方向进行修改。如果特征向量沿退化方向，则设置 $\sigma_i^{\text{tr}} = 0$ ；否则，设置 $\sigma_i^{\text{tr}} = 1.0 / \sigma_i$ 。这里，截断后的对角线元素记作 $\sigma_i^{\text{tr}}, \forall i \in {1, \dots, 6}$ 。

设置 $\sigma_i^{\text{tr}} = 0$ 会产生秩亏，LAPACK 的 SVD 例程可以处理这种情况。截断后的 Hessian 可直接用于求解最小二乘解：

$x^* = A_{\text{tr}}^{-1} b, \quad A_{\text{tr}}^{-1} = V \Sigma_{\text{tr}} U^\top.(32)$

其中，截断后的对角缩放矩阵 $\Sigma_{\text{tr}}$ 定义为：

$\Sigma_{\text{tr}} = \begin{bmatrix} \sigma_1^{\text{tr}} & 0 & \cdots & 0 \\ 0 & \sigma_2^{\text{tr}} & \cdots & 0 \\ \vdots & \vdots & \ddots & \vdots \\ 0 & 0 & \cdots & \sigma_6^{\text{tr}} \end{bmatrix}.(33)$

由于 $\Sigma_{\text{tr}}$ 是通过截断特征值构造的，它从分解中忽略了约束不足的方向。通过截断特征值，直接将抑制沿退化方向运动的约束集成到 Hessian 中。按设计， $A_{\text{tr}}$ 是秩亏的；然而，由于 SVD 对所有矩阵均存在，方程 (15) 到方程 (18) 仍可用于求解。

TSVD 的有效性

Eckart-Young 定理 [Draisma 等, 2018] 提供了对 TSVD 方法的详细分析。根据该定理， $A$ 的最佳秩 $k$ 最小二乘近似为 $A_{\text{tr}}$ ，截断近似的 2-范数误差定义为被截断特征值的平方和：

$A_{\text{tr}}|*F = \sqrt{\sum_{i=k+1}^{6} \sigma_i^2}.(34)$

其中， $F$ 表示 Frobenius 范数。进一步观察方程 (32) 可以发现，对于小特征值， $b$ 中的噪声敏感性会增加。通过截断这些小特征值，TSVD 可以缓解噪声放大的问题。例如， $b$ 中的噪声可能来自表面法向量计算误差或对应点搜索误差。第 3.2 节讨论了扰动对最小二乘输出质量的影响；在这种情况下，通过截断近似奇异的特征值，TSVD 改变了 $co n d (A)$ 、 $b$ 中的噪声、残差 $r$ 以及相对误差，从而提高了数值稳定性。

4.1.5 Tikhonov 正则化线性最小二乘（Tikhonov Regularized Linear Least-Squares）

正则化（Regularization）在优化技术中，如 Levenberg-Marquardt（LM），是一种简单有效的方法，可以避免奇异性并改善优化特性。虽然 Tikhonov 正则化的性质在数值线性代数领域已有充分研究 [Golub 等, 1999]，但其在退化感知点云配准中的优势尚未被充分探索。基于这一思想，本文采用基于局部可定位性信息提供的零空间近似的线性最小二乘子空间优化，加入 Tikhonov 正则化（L-Reg.），如图 4 所示。

将之前定义的代价函数（方程 3）改写为正则化线性最小二乘问题：

$\min_x |Ax - b|_2^2 + \lambda |Lx|_2^2= \min_x \left| \begin{bmatrix} A \\ \sqrt{\lambda} L \end{bmatrix} x \begin{bmatrix} b \\ 0 \end{bmatrix} \right|_2^2.(34)$

其中，标量 $\lambda$ 为线性正则化参数， $\in \mathbb{R}^{c \times 6}$ 为约束矩阵， $c$ 为退化方向的数量。退化方向的数量由第 3.4 节讨论的可定位性检测模块提供。

类似于方程 (5) 的推导，正则化最小化的法方程为：

$(A^\top A + \lambda L^\top L) x = A^\top b.(36)$

此时， $\lambda$ 可以理解为对正则化项引入的附加代价矩阵 $L^\top L$ 的缩放参数。该项为解在零空间近似方向上的投影提供惩罚，从而改善问题条件数（参见第 3 节）。正则化参数 $\lambda > 0$ 在优化前未知，需要根据问题数据确定。在本文中， $\lambda$ 通过对 Ulmberg 隧道数据集的启发式调参选定，与不等式约束方法类似，设定为 $\lambda = 440$ 。约束矩阵 $L$ 设为按行排列的优化退化方向矩阵， $v_j \in {V_t, V_r} \in \mathbb{R}^{3\times3}$ 。方程 (36) 可通过 SVD 求解，如第 3 节所述。

理解 Tikhonov 正则化

理解正则化对优化的影响非常重要。对于一般情况 $L = I$ ，需要解 $(A, L)$ 的广义 SVD。为简化理解，以下解释假设 $L = I$ 。类似于方程 (18)，给定 $A$ 的 SVD，可通过以下步骤求解正则化解 $x_\lambda$ ：

在这里插入图片描述

将相似项合并重排，可揭示正则化如何影响 Hessian。设 $V^\top x_\lambda$ ，方程可简化为：

$(\Sigma^\top \Sigma + \lambda I) y = \Sigma^\top U^\top b.(38)$

由于 $V$ 是正交矩阵， $y$ 的 2-范数与 $x_\lambda$ 相同。通过二元分解，可得正则化问题的解：

其中 $z_i$ 为滤波系数，用于抑制 $A$ 的小特征值 $\sigma_i < \lambda$ 对应的贡献，从而抑制 $A^\top A$ 的小特征值 $\sigma_i^2 < \lambda$ 。因此，正则化主要影响退化方向，有效改变 Hessian 的特征值。最终，如方程 (39) 所示，该方法改变了优化 Hessian 的特征值，改善了 $co n d (A)$ ，从而提高最小二乘问题的敏感性特性（参见方程 14 和方程 10）。

4.1.6 带 Tikhonov 正则化的非线性优化（Non-Linear Optimization with Tikhonov Regularization）

本节介绍基于非线性优化的正则化技术——带 Tikhonov 正则化的非线性优化（NL-Reg.），如图 4 所示标记为 NL-Reg.。

先前的研究，如 [Fitzgibbon, 2003; Nashed 等, 2021]，利用非线性代价函数来改善 ICP 的性能。特别地， [Nashed 等, 2021] 提出了在因子图优化中，将优化的零空间作为非线性代价函数的正则化项。受到这一思路启发，本文在点云配准任务中，将 Tikhonov 正则化应用于非线性代价函数：

其中 $[r^\top, t^\top]^\top \in \mathbb{R}^6$ 为优化变量， $\lambda_D$ 为退化正则化参数， $L$ 为按行排列的退化方向约束矩阵。与之前的方法类似， $\lambda_D$ 在 Ulmberg 隧道数据集上通过启发式调参确定，设为 $\lambda_D = 675$ 。方程 (40) 的代价函数最小化使用著名的 Levenberg-Marquardt（LM）算法。

雅可比矩阵、梯度与 Hessian

代价函数 $E_{\text{NL}}(x)$ 相对于 $x$ 的雅可比矩阵、梯度和 Hessian 定义为：

$J_{\text{NL}} = \frac{\partial e}{\partial x}, \\\quad \nabla E_{\text{NL}}(x) = 2 J_{\text{NL}}^\top e_i + 2 \lambda_D L^\top L x, \\\quad H_{\text{NL}} = \frac{\partial^2 E_{\text{NL}}(x)}{\partial x^2} = 2 J_{\text{NL}}^\top J_{\text{NL}} + 2 \lambda_D L^\top L. (41)$

LM 算法更新规则

利用上述定义，LM 算法的迭代更新规则为：

$x_{\text{new}} = x - (H_{\text{NL}} + \lambda_s I)^{-1} \nabla E_{\text{NL}}(x),(42)$

其中， $\lambda_s$ 为 LM 算法的阻尼（平滑）参数，用于保证矩阵求逆的稳定性。

如 Ceres 文档中所述，状态修正的幅度可能导致算法不收敛，因此在求解中加入信赖域（trust region）约束。有关鲁棒非线性最小二乘的更多实现细节，请参考 Ceres 文档。为了清晰，修改后的优化终止容差见表 1。
在这里插入图片描述

由于该方法使用 LM 算法求解非线性代价函数（方程 40），第 3.2 节中的灵敏度分析不能直接应用。然而，NL-Reg. 方法通过加入退化感知正则化 $\lambda_D$ 改善了 Hessian $H_{\text{NL}}$ 的条件数。此外，由于 LM 算法本身通过阻尼参数 $\lambda$ 的正则化，对病态问题具有更强的鲁棒性。

4.1.7 外部先验（External Prior）

在这种方法中，一旦检测到优化问题出现病态（ill-conditioning），就直接使用初始估计 $T_{\text{ML,init}}$ 来进行点云配准。由此带来的结果是：通过完全跳过优化过程，使点云配准失去作用。

该方法在退化（degeneracy）情况下依赖外部里程计估计来传播位姿解，因此在图 4 中将其记为 Prior Only 并加以展示。由于在发生优化退化时该方法完全跳过点云配准步骤，点云配准中的最小二乘优化过程被整体省略。

由于没有来自点云配准的位姿更新，该方法的整体精度完全依赖于外部位姿先验的精度，尤其是在 LiDAR 发生退化的期间。

4.2 被动退化缓解（Passive Degeneracy Mitigation）

被动退化缓解指不主动利用检测到的退化信息去影响优化的方法。相反，这些方法被动使用退化信息，或者依赖于对扰动和退化较不敏感的算法。

4.2.1 全局最优点云配准（Globally Optimal Point Cloud Registration）

在 Quatro [Lim 等, 2022] 中，作者在全局最优点云配准的背景下将退化定义为匹配过程中缺乏内点对应关系。这一退化定义不同于本文使用的 LiDAR 退化定义（第 3 节），后者侧重于环境在特定方向上固有几何信息的缺失。

Quatro [Lim 等, 2022] 是一种鲁棒的全局最优点云配准方法，可在少于三个对应点的情况下估计 4 自由度（4-DoF）变换。Fast Global Registration（FGR）[Zhou 等, 2016] 是一种知名的全局配准方法，通过单阶段目标最小化求解两重叠表面间的变换。

第 5.1.2 节指出，这些全局最优点云配准框架无法补偿环境提供的几何约束不足；内点对应关系也无法约束沿对称和自相似运动方向的优化。

4.2.2 M-估计（M-Estimators）

为了分析鲁棒异常值剔除方法在点云配准中的有效性，本文研究了 Cauchy 鲁棒代价函数（M-Estimator）结合中位数绝对偏差（MAD）尺度估计器 [Babin 等, 2019] 的效果。本文中为简化，称该方法为 Cauchy，如图 4 所示。

非凸鲁棒代价函数可用于迭代加权最小二乘（IRLS）[Bergström & Edlund, 2014] 方法中，将点云配准代价函数（方程 3）改写为：

其中 $\rho(\cdot)$ 为 Cauchy 鲁棒代价函数：

$\rho(e) = \frac{\kappa^2}{2} \log \left( 1 + \left(\frac{e}{\kappa}\right)^2 \right), \quad w(e) = \frac{1}{1 + \left(\frac{e}{\kappa}\right)^2}.(44)$

这里， $e$ 为 ICP 迭代过程中每对对应点产生的误差， $\kappa$ 为调参参数。 $w(\cdot)$ 是非凸代价函数 $\rho(\cdot)$ 的分解权重。MAD 尺度估计器与 IRLS 求解细节请参见 [Babin 等, 2019]。本文采用 $\kappa = 1$ 。

使用方程 (44) 计算的权重后，代价函数（方程 3）可改写为：

$\min_{x \in \mathbb{R}^6} w(e) |A x - b|_2^2.(45)$

在本文中，Cauchy 方法通过降低 $b$ 和 $A$ 的噪声水平，提高了最小二乘问题的灵敏度，从而减轻了病态情况下噪声放大的不利影响，但并未改变 $A$ 的条件数。

4.2.3 几何稳定采样（Geometrically Stable Sampling）

作为使用采样改善 ICP 条件数的开创性工作之一，[Gelfand 等, 2003] 提出从对应点对 $(p, n)$ 进行成对信息采样。在该方法中，从对应点集合中采样点与表面法向对，直到收集到足够样本或满足条件数阈值为止。具体地，对所有特征向量 $v_k, k \in {1, \dots, 6}$ 顺序采样：

通过对信息的累加近似特征值，从而对 Hessian 条件数进行平滑调整。采样标准是重新采样对应点构造一个条件数尽量接近 1 的 Hessian。这种方法有效改善了 $co n d (A)$ ，避免了最小二乘解对扰动的过敏感（见第 3.2 节）。本文使用 libpointmatcher [Pomerleau 等, 2013] 的开源实现复现该方法。

4.2.4 冗余最小化采样（Redundancy Minimizing Sampling, RMS）

如第 2 节所述，[Petracek 等, 2024] 提出 RMS，用于最小化输入点云冗余，同时最大化数据熵以保留信息。具体地，该方法在所有优化方向上对输入点云采样，以最大化信息熵并保持 Hessian 条件良好。

作者提出一种启发式方法“梯度流”，用几何梯度流量量化点的独特性。每个点 $p$ 在半径为 $\lambda_p$ 的球形邻域中的梯度流定义为：

$N_p = { j \mid |j - p|*2 < \lambda_p, j \neq p, j \in P },\\ \quad \Delta_p = \frac{1}{|N_p|} \sum_{j \in N_p} (j - p).(47)$

基于梯度流的数据熵最大化定义为：

$\hat{P} = \arg\max_{\Omega \in {\Theta \mid \Theta \subset P_v, \Theta \neq \emptyset}} H_\Delta(\Omega),(48)$

其中 $P_v$ 为体素化点云集， $\Omega$ 为 $P_v$ 的子集。最大化过程以归一化相对熵率为条件以鼓励早期终止。虽然该方法未平衡旋转空间的可观测性，但作者指出仅在高旋转角度时相关。

与 Gelfand 等方法类似，RMS 提供了一种采样输入点云的方法，以最大化信息增益、改善 $co n d (A)$ 并最小化信息冗余，从而降低 $b$ 和 $A$ 的噪声。这种方法减轻了敏感最小二乘优化问题中的噪声放大效应（见第 3.2 节）。在本文中，RMS 被视为被动退化缓解方法，因为它改变的是输入点云，而非主动利用检测到的退化信息。该方法如图 4 所示标记为 Petracek 等，并使用原作的开源实现复现。

4.2.5 点到平面 ICP（Point-to-Plane ICP）

点到平面（Point-to-Plane）指使用点到平面代价函数 [Low, 2004] 的 ICP 公式，如第 3 节所述。本文其余部分将该方法称为 P2Plane。点到平面 ICP（P2Plane）是一种被动基线方法，在图 4 中以相同名称标示。

基线方法的结果显示，如果不采取退化感知措施来缓解优化病态问题，LiDAR 退化会对点云配准和 LiDAR 位姿估计产生不利影响。该方法的优化代价函数定义见方程 (3)。由于该方法不改变问题的敏感性属性，因此在评估中作为基线使用。

4.2.6 非线性点到平面 ICP（Non-linear Point-to-Plane ICP）

该基线方法称为非线性求解器（NL-Solver），用于分析求解器的影响。具体来说，该方法使用方程 (40) 和 (42) 中的代价函数，且 $\lambda_D = 0$ 。

类似于 NL-Reg. 方法，该基线方法使用 Ceres 实现代价函数，并使用带默认正则化的 LM 算法。优化终止条件由表 1 中定义的参数确定。在图 4 中标记为 NL-Solver。与 P2Plane 基线方法相比，NL-Solver 使用基于梯度的 LM 求解器。

最后，NL-Solver 不会通过退化感知正则化干预 $co n d (A)$ 的条件，但它利用阻尼参数 $\lambda$ 在 LM 算法更新步骤中提高稳定性。

4.3 实现细节（Implementation Details）

本节描述的所有方法均在开源点云配准库 libpointmatcher [Pomerleau 等, 2013] 中实现，并集成到 Open3d SLAM [Jelavic 等, 2022] 框架中，作为扫描到子图（scan-to-submap）配准模块。

Open3d SLAM 以 LiDAR 10 Hz 速率运行，使用外部里程计位姿估计 $T_{ML,init}$ 作为配准先验，实现扫描到子图的最近邻搜索以确定点对应关系。点的表面法向量通过 k-近邻搜索和主成分分析计算，邻域设置为 $k = 10$ 。LiDAR 扫描最大距离截断为 70 m，并在不下采样的情况下处理。此外，所有方法使用裁剪异常值滤波器以复现实际系统。点到平面 ICP 的最大迭代次数设为 30 次。如果位姿更新的平移范数低于 0.01 m 或角度更新小于 0.001 rad，则提前终止 ICP 算法。这些值基于 libpointmatcher 的默认设置。

如第 2.2.1 节所述，类似 X-ICP [Tuna 等, 2023] 的可定位性模块被用于所有需要可定位性信息的方法。具体地，X-ICP 简化为二值可定位性分类，即可定位与不可定位，以避免给更复杂的方法带来意外优势。该方法需要 3 个可定位性参数 $\kappa_1, \kappa_2, \kappa_3$ 和 1 个滤波参数 $\kappa_f$ 。可定位性参数基于所用 SLAM 框架和 ICP 算法的收敛域设置，并满足 $\kappa_1 \ge \kappa_2 > \kappa_3$ 。在本文的所有实验中，参数保持一致： $\kappa_1 = 300, \kappa_2 = 150, \kappa_3 = 45$ 。这些值仅根据 Open3d SLAM 的体素化和滤波特性设置一次。滤波参数在所有实验中设为 $\kappa_f = 80$ ，除 HEAP 挖掘机实验（第 5.4 节）外，该实验使用较旧且噪声较大的 LiDAR Ouster OS0 Rev-1， $\kappa_f = 60$ 。

依赖参数的方法 L-Reg.、NL-Reg. 和 Ineq. Con. 在 Ulmberg 自行车隧道实验（第 5.2 节）中根据性能调优。此外，[Petracek 等, 2024] 的 RMS 方法和 Gelfand 等方法各有一个可调参数。RMS 方法的可调参数 $\lambda$ 在第 5.4 和 5.2 节实验中设为 0.0042，其余实验设为 0.0036。这些值通过不同 $\lambda$ 测试并经验选择。对于 [Gelfand 等, 2003] 方法，当扫描点数超过 1000 时，最大采样点数设为总点数的 60%；扫描点数较少时，设为可用点数的 90%。

所有评估均在配备 Intel i9-13900K CPU 的桌面计算机上进行，为保证可重复性和确定性执行，所有操作均以单线程方式运行。

5 实验与评估

在本节中，我们通过仿真研究和真实世界外场实验对不同的退化缓解（degeneracy mitigation）方法进行了深入分析，以识别各方法的优势与不足。首先，在 第 5.1 节中开展了一项迭代退化点云配准的仿真实验，用于验证各方法的可行性并加深对其工作机理的理解。随后，在 第 5.1.2 节中讨论了一项全局点云配准的仿真研究，分析全局点云配准方法在缓解 LiDAR 退化方面的影响。最后，在 第 5.1.1 节中，将对比的方法嵌入到 Open3D-SLAM（Jelavic et al., 2022）的完整闭环中，并在仿真数据上进行测试。此外，如图 5 所示，这些方法还在多种真实机器人外场部署场景中进行了测试，包括：

自然开放环境（第 5.3 节），
城市隧道环境（第 5.2 节），
施工现场环境（第 5.4 节）。

真实世界实验采用了多种不同的机器人平台和传感器配置，具体包括：

配备 Velodyne VLP-16 LiDAR 的 ANYmal 四足机器人（Hutter et al., 2017，见第 5.3 节）；
配备 Ouster OS0-128 LiDAR 的 HEAP 挖掘机平台（Jud et al., 2021，见第 5.4 节）；
配备 Ouster OS1-128 LiDAR 的 ENWIDE 手持式传感器系统（Pfreundschuh et al., 2024，见第 5.2 节）。

每一组实验都引入了一组独特的真实世界挑战，用于检验现有点云配准框架的能力，从而对所对比的方法进行更为细致的分析。需要特别指出的是，其中一些挑战使得恢复环境的真实点云地图在实践中变得不可行。例如，在 Ulmberg 隧道实验（第 5.2 节）中，LiDAR 退化持续时间超过整个数据集的 80%，这使得仅依赖 LiDAR 的方法无法生成合理的地图。在这种场景下，系统性能主要取决于如何更有效地利用外部运动先验来缓解 LiDAR 退化带来的影响。

相比之下，第 5.3 节和第 5.4 节中的实验要么只存在较轻度的 LiDAR 退化（仍保留一定的几何信息），要么退化持续时间较短，扫描到子地图（scan-to-submap）配准仍然能够从退化状态中恢复。

结合上述挑战，本文在 表 2 中总结了不同实验场景下的退化程度以及所研究方法的预期行为表现。
在这里插入图片描述

5.1 仿真实验（Simulation Studies）

为评估各方法的适用性与有效性，进行了若干仿真实验。首先，在第 5.1.1 节中进行了迭代点云配准实验，以将点云配准步骤从完整建图框架中独立出来。该注册步骤包含两个已知扰动的点云及初始变换猜测。其次，第 5.1.2 节分析了在相同实验设置下全局最优点云配准方法的可行性。最后，进行了一个模拟行走的 ANYmal [Hutter 等, 2017] 机器人实验，结合 Open3d-SLAM 进行 LiDAR-SLAM 仿真。

5.1.1 静态仿真 — 迭代点云配准（Static Simulation - Iterative Point Cloud Registration）

在此实验中，每种方法都提供源点云和参考点云，如图 6 所示，采自模拟的完美圆柱形对称网格。参考点云通过已知变换扰动，并使用单位矩阵初始猜测将两点云关联。实验目的有两点：第一，分析各方法在仅旋转（绕 $Z$ 轴旋转）、单自由度退化设置下的性能；第二，观察 ICP 算法每次迭代的解演变。Prior Only 方法被省略，因为该实验关注配准任务，而 Prior Only 会跳过配准步骤。在这里插入图片描述

类似其他实验，各方法用于 ICP 配准任务，最大迭代次数为 30 次。结果见图 7。在这里插入图片描述

首先，如图 7 左上图所示，基于非线性优化的方法 NL-Reg. 和 NL-Solver 的计算成本高于其他方法。此外，基于采样的方法在开始阶段耗时较多，可能由于初始对应点较少。其他方法在计算复杂度上相当，适合实时部署。

其次，ICP 残差在初期尖峰后（除 Cauchy 方法外，由于快速增加的对应内点，见右上图），所有方法均被最小化到相似量级（中上图）。然而，该最小化代价不一定表示收敛到真实变换，只表明残差被成功最小化。尤其可以从 Frobenius 距离图看出，许多方法虽然降低了 ICP 残差，但实际上增加了与真实变换的 Frobenius 距离。这是因为沿退化旋转轴的每一次旋转都可能是优化的局部最小值。

然而，所有主动退化缓解方法正确利用退化信息，防止旋转误差进一步增加，同时减少平移误差（中中图）。有趣的是，通过比较退化方向（绕 $Z$ 旋转）总运动残差（中右图），可以观察主动退化缓解方法的内部动态差异。如预期，Eq. Con. 方法由于将退化约束作为等式约束，未产生运动残差。在软约束方法中，NL-Reg. 允许在迭代 10 前有一定运动，而 Ineq. Con. 允许线性缓慢运动，因为优化在每次注册步骤中都可沿退化方向移动。相比之下，L-Reg. 在退化方向上允许运动，可能由于较小的正则化参数。最后，TSVD 与 Zhang 等方法表现非常相似，因为这两种方法都通过特征值操作优化的特征向量（k 阶近似或解投影）。

5.1.2 静态仿真 — 全局点云配准（Static Simulation - Global Point Cloud Registration）

本实验使用第 4.2.1 节讨论的全局点云配准方法 Quatro [Lim 等, 2022] 和 FGR [Zhou 等, 2016]，任务与第 5.1.1 节类似。

与第 5.1.1 节相似，使用了单轴旋转退化点云（图 8），但对 $Y$ 和 $X$ 轴的旋转进行了不同扰动，因为 Quatro 仅能估计 4-DoF 变换。为保证公平比较，对两种方法进行了多种配置测试，取最佳配置生成图 8。
在这里插入图片描述

该实验旨在强调，退化点云配准问题不是对应点外点问题，而是对应点中几何信息缺失，即真实内点缺失问题。实验结果显示，在缺少内点情况下，即使全局最优点云配准方法也无法恢复真实变换。因此，退化检测与缓解非常重要。这是数据缺失问题，方法输出预期不是恢复真实变换，而是缓解退化的不利影响。

如图 8 所示，两种方法均很好地恢复了平移，但未能正确恢复退化轴（即 $Z$ 轴）旋转，点云颜色错位显示了这一点。由于退化轴的旋转不可观测，当所有良态方向收敛后终止优化被视为成功优化。该实验表明，全局配准方法不能直接解决退化点云配准问题，因为 LiDAR 点云退化本质上是病态的，优化解存在多个极小值。全局方法虽对外点鲁棒，并能在存在内点时恢复真实最小值，但退化点云配准中不存在内点，因此这些方法无法为第 4 节的方法提供额外优势。

5.1.3 ANYmal 仿真（ANYmal Simulation）

本实验模拟一台配备 Velodyne VLP-16 LiDAR 的 ANYmal 机器人，在三轴 LiDAR 退化环境中导航（图 9）。机器人在平面上绕矩形柱体移动，受到平面平移退化和地面法线旋转退化的影响。实验中，机器人机体速度达到 0.85 m/s 和 40°/s，增加了难度。

为利用模拟环境并分析算法稳健性，在原本完美的外部位姿先验上加入均匀噪声，作为所有方法点云配准的初始猜测。噪声从正态分布采样 $N(\mu_t, \sigma_t^2)$ 和 $N(\mu_r, \sigma_r^2)$ ，其中 $\mu_t = 0\ \text{cm}, \sigma_t = 0.05\ \text{m}, \mu_r = 0\ \text{rad}, \sigma_r = 0.01\ \text{rad}$ 。各方法的建图结果及柱体的真实点云见图 9。
在这里插入图片描述

为突出 LiDAR 退化导致的建图质量下降，对每种方法结果中的地面平面进行了裁剪。图 9-A 显示，由于 LiDAR 扫描稀疏且环境范围大，只有少量 LiDAR 扫描线落在柱体上，增加了难度。实验开始时，机器人位于 LiDAR 已退化位置，因此序列开始阶段的退化扫描到子图注册可视作退化扫描到扫描注册，这增加了优化对外点的敏感性，因为此阶段对应点数量有限。

图 9 的建图结果显示，Eq. Con. 和 Prior Only 方法表现良好。基线方法 P2Plane、NL-Solver 和 Cauchy 方法生成错位地图，这是由于实验设置中机器人在多方向 LiDAR 退化下开始和结束实验。采样方法 RMS [Petracek 等, 2024] 和 Gelfand 等方法也未能正确注册点云。RMS 方法在高转速下不鲁棒 [Petracek 等, 2024]，这是观察到的原因。此外，环境特征有限也影响了采样方法效果。

TSVD、L-Reg. 和 Ineq. Con. 方法生成的地图类似，旋转和平移有中等量漂移。L-Reg. 和 Ineq. Con. 的漂移与 Ulmberg 隧道实验调参有关，环境相关调参可能改善性能。TSVD 性能表明在优化过程中或优化前缓解退化的重要性，因为这是唯一在优化前进行主动退化缓解的方法。NL-Reg. 方法表现不如其他主动退化缓解方法，但优于被动方法，参数选择可能解释了其性能。

表 3 给出了动态 ANYmal 仿真实验的 APE 和 RPE（每 1 m 行驶距离）指标，最佳值加粗，次佳值下划线。通过 EVO 评估包计算相对和绝对误差 [Sturm 等, 2012]。结果显示，Eq. Con.、Prior Only 和 Zhang 等方法性能相当，Eq. Con. 略优。Prior Only 方法在退化存在时使用有噪声的位姿先验，误差来自注册与建图的数值误差及附加运动先验噪声。
在这里插入图片描述

有趣的是，允许约束松弛的可调方法 Ineq. Con.、L-Reg. 和 NL-Reg. 未能达到最佳方法性能。直觉上，约束松弛意味着更多依赖点云配准而非外部位姿先验，而实验中存在多个严重退化片段，因此依赖配准并不提升性能。相反，不允许约束松弛的方法表现更好。被动退化缓解方法和基线显示 LiDAR 滑移和错误注册地图。

5.2 Ulmberg 自行车隧道实验：单向退化（Ulmberg Bicycle Tunnel Experiment: One Directional Degeneracy）

LiDAR-SLAM 系统常见的退化情况之一是单向退化，这在走廊或隧道类结构中尤为典型。Ulmberg 自行车隧道数据集 [Pfreundschuh 等, 2023]（见图 5-D）即为此类环境的示例。本实验使用的手持传感器载荷（见图 5-C）配备 Ouster OS0-128 LiDAR。

值得注意的是，由于手持载荷没有可用于提供位姿先验的运动学测量，因此本实验使用 COIN-LIO [Pfreundschuh 等, 2023] 的基于强度的里程计估计来在优化退化期间传播前一次扫描到子图的注册位姿，适用于所有方法。COIN-LIO 能够在此类环境中成功定位，因为它利用环境的强度光谱视觉信息，而不依赖退化环境中的几何特征。

5.2.1 参数调优（Parameter Tuning）

Ineq. Con.、L-Reg. 和 NL-Reg. 方法对参数选择较为敏感。为解决该问题，本实验数据用于分析这些方法的性能。图 10c 展示了 Ineq. Con. 方法上下界选择对相对平移误差（RTE）和绝对平移误差（ATE）的影响。
在这里插入图片描述

RTE 衡量方法的局部一致性和稳定性；
ATE 衡量绝对误差，是长期一致性的指标。

当界限数值接近 0 时，RTE 误差增加，表明存在局部误差；但 ATE 下降表明约束加紧有助于防止大尺度漂移。因此，界限参数设置为 $\epsilon = 0.0014$ ，下界为 $-\epsilon$ ，上界为 $\epsilon$ 。该值选在鞍点区域，使平均误差指标较低。

类似地，L-Reg. 与 NL-Reg. 的正则化参数对 RTE 和 ATE 的影响分别如图 10b 和图 10a 所示。随着正则化权重增加，两方法的 RTE 达到最小区域，但进一步增加会提高 RTE；而 ATE 随权重增加而下降。基于此分析，L-Reg. 的 $\lambda = 440$ ，NL-Reg. 的 $\lambda_R = 675$ ，用于本文所有实验。

直观来看，正则化影响与权重幅度正相关。随着权重增加，这些方法的行为接近硬约束方法。由于正则化方法的优势源于退化感知正则化与点到平面代价函数的微妙平衡，因此选择第一个使 RTE 和 ATE 低的正则化参数。

5.2.2 全局点云配准（Global Point Cloud Registration）

为评估全局配准方法在真实 LiDAR 退化扫描中的可行性，进行了独立配准分析。注册设置由实验数据中提取的两个点云组成，保持约 70% 的重叠。

如图 11 上排所示，源点云经过变换后与参考点云一同输入到全局配准方法 Quatro [Lim 等, 2022] 和 FGR [Zhou 等, 2016]。相比第 5.1.2 节实验仅存在旋转退化，本实验仅存在平移方向退化。
在这里插入图片描述

如图 11 下排所示，在缺乏明显几何特征的情况下，两方法均无法找到真实全局最小值。但它们成功恢复了所有良约束轴的正确变换，显示方法使用正确。此外，在该问题设置中，扫描中的空缺位置充当伪全局最小值，方法无法区分真实最小值，可从图 11 右上角的屋顶光结构看出。该退化扫描到扫描的点云配准实验进一步说明，在缺乏可观测内点时，全局点云配准方法同样受 LiDAR 退化影响，可能无法带来额外优势。

5.2.3 循环 SLAM 配准（SLAM in the Loop Registration）

对于每种方法，返回时（重复观察）楼梯区域的点云地图如图 12 所示。此外，非退化区域的地面真实地图（使用视觉辅助 Leica BLK2GO 扫描仪收集 [Del Duca & Machado, 2023]）及环境实景图也在图 12 中提供。
在这里插入图片描述

建图结果显示主动退化缓解方法的有效性，这类方法均生成漂移极小或无漂移的点云地图。值得注意的是，Ineq. Con.、L-Reg. 和 NL-Reg. 已针对该环境调参，因此预期表现良好。

P2Plane 与 Prior Only 方法在隧道方向上出现地图重复；
Cauchy 方法漂移最大。

两种采样方法 RMS 与 Gelfand 等相较于 P2Plane 基线有所提升，但不如非线性基线 NL-Solver。原因在于本实验中退化严重且持续时间长，限制了采样方法性能。两基线方法的性能差异可能表明非线性求解器在缓解病态 Hessian 的不利影响方面的有效性。

每行走 1 m 的 RTE 变化和 ATE 如图 13b 与 13a 所示。图 13b 显示，主动退化缓解方法的误差变化低于 P2Plane 与 Cauchy 基线方法。Prior Only 方法在 $180\ \text{s}$ 出现较大估计峰值，足以生成错位地图。比较 Ineq. Con. 与 NL-Reg. 可见，尽管两方法表现良好，但 Ineq. Con. 的误差变化较大，导致轨迹末端地图重复。这提示，在大规模建图中，稳定性和鲁棒性优于局部精度。
在这里插入图片描述

图 13a 显示，所有方法在平台轨迹上均累积误差。误差来源于数据的随机不确定性和 Open3D-SLAM [Jelavic 等, 2022] 的认知不确定性，由于观测点入射角大，地图在轨迹上发生弯曲。

有趣的是， $40\ \text{s}$ 到 $80\ \text{s}$ ，Cauchy 方法优于其他方法，直到 $90\ \text{s}$ 开始累积误差，原因是扫描配准错误。这表明鲁棒范数在精度提升上有潜力，但仍受限于内点缺失。

此外，采样方法 RMS、Gelfand 等及非线性基线 NL-Solver 在 $120\sim150\ \text{s}$ 前均优于主动退化方法，但严重退化出现时效果下降。原因可能有二：i）主动方法依赖运动先验质量，ii）采样方法主动选择稀缺且独特的特征。

表 4 给出了这些指标的均值、标准差及 ICP 注册的计算成本。综合比较，Ineq. Con. 在 RTE 均值上最佳，Eq. Con. 变化最小。在未调参方法中，Zhang 等方法 RTE 均值最好，Eq. Con. ATE 最低。NL-Reg. ATE 最佳，Prior Only 变化最小。NL-Reg. 调参适合本环境，但计算成本较高，不适用于计算受限的平台。RMS 采样方法计算成本最稳定，得益于其减少点云冗余的采样策略。
在这里插入图片描述

5.3 ANYmal 森林实验：开阔场地退化（ANYmal Forest Experiment: Open Field Degeneracy）

在该实验中，ANYmal 机器人（见图 5-A）从森林旁开始，导航至开阔场地，进行快速旋转，然后返回之前观察过的通道，行走总距离为 107 m。在开阔场地（见图 5-B），优化预期存在三轴退化，即地面平面上的 XY 平移和绕该平面法线的旋转。然而，由于机器人靠近森林区域，树冠提供了非结构化但足够的几何信息，使优化能够收敛。因此，LiDAR 退化仅在 LiDAR 丢失对森林树冠的视线时发生。

在本实验中，ANYmal 的运动学腿部里程计估计器（TSIF）[Bloesch 等, 2017] 沿退化方向传播前一次扫描到子图的注册位姿。真实轨迹通过全球导航卫星系统（GNSS）获取，用于分析机器人平移的全局一致性，因为 GNSS 仅提供位置信息。此外，感兴趣区域的真实点云由手持 Leica BLK2GO [Del Duca & Machado, 2023] 扫描仪收集。

有趣的是，即使是商业扫描仪在退化的开阔场地也无法完成注册，因此仅保留重访区域附近的点用于误差指标计算。

实验的建图结果及重访区域的真实地图见图 1。从定性结果看，方法 NL-Reg.、Ineq. Con.、Eq. Con.、TSVD 和 NL-Solver 生成的地图良好，可通过观察环境中的人造长椅推断。所有方法（除 NL-Solver 外）均执行主动退化缓解，并在优化前（TSVD）或优化过程中（NL-Reg.、Ineq. Con.、Eq. Con.）处理退化。NL-Solver 在未感知退化的情况下也表现良好，这表明本实验中的退化较轻，可以恢复。

采样方法 RMS 与 Gelfand 等未能正确注册长椅，主要原因是大量叶子和植被存在，其非结构化特性妨碍了表面法线的正确计算。值得注意的是，NL-Reg. 与 Ineq. Con. 虽针对 Ulmberg 自行车隧道实验调参，但在本实验中也表现尚可。在无需调参的方法中，Eq. Con. 和 TSVD 表现较好，可能得益于优化前或优化中退化缓解。

P2Plane 和 Cauchy 方法未考虑 LiDAR 退化，生成的地图存在断裂与重复；Prior Only 方法依赖腿部里程计先验，但腿部里程计在软地面表现不佳，因此生成地图也错位。Zhang 等和 L-Reg. 方法虽然主动缓解退化，但生成地图仍存在 LiDAR 漂移。Zhang 等方法表现良好可能因为其在优化后处理退化，是唯一在优化后作用的退化缓解方法。该方法在 $180\ \text{s}$ 前表现较好，但此时机器人载荷阻挡了独特特征，导致 LiDAR 漂移。L-Reg. 表现受参数调节影响，依赖退化正则化参数。

每 2 m 的 RTE 和 ATE 指标及 ICP 计算成本见表 5。从量化指标看，RTE 与 ATE 平均误差最佳方法为 NL-Reg.，RTE 次优为 Ineq. Con.，ATE 次优为 Zhang 等。需要注意，Zhang 等方法在统计指标上表现良好，但图 1 中错位地图显示其旋转估计仍非最优。为深入理解各方法性能差异，图 14a 和图 14b 展示了整个轨迹上的 RTE 与 ATE 误差。两图中， $100\ \text{s}$ 时，多方法性能明显变化，该时刻对应三轴退化（包括绕地面法线旋转）。Cauchy 与 Zhang 等方法在退化前 ATE 表现优于其他方法，表明这些方法可能对退化严重性敏感。
在这里插入图片描述

在计算成本方面，除 NL-Reg. 与 NL-Solver 外，其他方法表现类似，Prior Only 方法领先，因为其跳过注册步骤。NL-Reg. 与 NL-Solver 计算成本约高 5 倍，因非线性优化求解时间较长，即使退化约束与其他方法相同。最快方法为 RMS [Petracek 等, 2024]，因其采样策略部分减少点云冗余。但 ATE 与 RTE 指标显示，该方法可能不适合快速旋转的非结构化自然环境。RTE 误差曲线亦显示在无 LiDAR 退化情况下表现不稳定，反映实验环境的挑战性。

5.4 HEAP 挖掘机实验：短时突发退化（HEAP Excavator Experiment: Short Burst Degeneracy）

在本实验中，瑞士 Oberglatt 的一处施工场地被 HEAP [Jud 等, 2021] 自动挖掘机行驶 170 m。正如 [Nubert 等, 2022a] 所示，该环境包含 GNSS 丢失的两栋建筑间窄通道。HEAP 及施工环境见图 5。

实验目的是分析方法在短时、严重 LiDAR 退化下的性能，同时通过 COIN-LIO [Pfreundschuh 等, 2023] 提供良好的注册初值。如图 5-F 所示，环境中仅建筑间段落存在 LiDAR 退化。但若不缓解退化，全局 LiDAR 地图一致性仍会受影响。建图结果见图 15。
在这里插入图片描述

结果表明，在提供良好初值的情况下，所有主动退化预防方法表现相当，无论退化严重性如何。注册初值的精度可从 Prior Only 方法的表现推断，其生成的地图与真实环境几何一致，表明 COIN-LIO 里程计准确。

相比之下，P2Plane、Cauchy 和 NL-Solver 方法在退化方向出现 LiDAR 滑移，因为它们未在注册过程中主动使用退化信息。RMS 与 Gelfand 等采样方法表现略优于其他方法，其精度受运动先验限制。这一改善源于环境中存在的直线和角点，采样方法可以主动识别并利用。

6 实际应用与经验总结（Practical Matters and Lessons Learned）

本节作者提供了在 LiDAR 退化环境下进行鲁棒点云注册的实践经验与启示。

野外机器人注册的最佳实践

在野外机器人应用中，通常需要多个功能模块相互协作。这些模块应根据彼此进行定制，以实现最佳性能，例如通信延迟、传感器标定与计算能力。对于这些模块而言，外部里程计模块（如视觉惯性里程计 [Pfreundschuh 等, 2023]、运动学惯性里程计 [Bloesch 等, 2017]、雷达里程计 [Nissov 等, 2024a] 等）的可用性至关重要。实验表明，可靠的外部里程计先验对于保证机器人位姿估计在随后的退化点云注册过程中具有鲁棒性非常关键，如 5.4 节实验所示，当提供良好的运动先验时，所有退化感知方法的表现相同。

多模态紧耦合退化感知方案可能减少对外部先验的依赖，但这是未来的研究方向。此外，硬件以及传感器部署配置也起关键作用。例如，LiDAR 视场受限可能导致某些环境出现退化，而不同的传感器布局在同一环境下能够成功定位。这在 5.3 节中得到体现，ANYmal 的 VLP-16 LiDAR 部分被遮挡。类似地，HEAP 挖掘机实验（5.4 节）显示，大视场 LiDAR（如 Ouster OS0-128）足以在两栋建筑间的自相似通道中生成退化感知的解决方案，这从采样方法 Gelfand 等与 RMS 的性能可推断。

软件配置的重要性

与机器人硬件不同，SLAM 后端和退化感知注册框架同样重要。仅靠退化缓解是无效的，必须结合正确的退化检测，从而实现退化感知点云注册。检测方法应对环境变化具有鲁棒性和泛化能力。

此外，精确可靠的空间特征提取（如表面法线）对于正确的退化检测是必要的。尽管本文未聚焦于选择 LiDAR 退化检测方法，但其对于缓解不良影响至关重要。使用稳健范数、统计滤波或语义滤波（如 RMS、Gelfand 等）有助于降低特征提取噪声。如 5.2 节图 13a 所示，在 LiDAR 退化导致误差积累达到一定范围前，Cauchy、RMS 与 Gelfand 等方法表现优于其他方法。

除数据与检测质量外，建图后端对鲁棒扫描到子图的点云注册至关重要，因为子图作为参考点云并体现系统对环境的理解。正如 5.2 节图 13a 所示，所有主动退化防护方法在全局尺度上均存在漂移，这是建图后端产生的共同认知不确定性所致。

最后，系统的计算复杂度必须能够实时运行，理想情况下与 LiDAR 传感器采样率匹配，且延迟最小，以保证对运动与导航等下游任务的可用性。5 节中多次提到 NL-Reg. 的计算成本，表明其虽然精确，但在计算受限的系统中可能需要跳过部分测量。

如何缓解 LiDAR 退化

在不同真实环境实验中，主动退化缓解方法在 LiDAR 退化存在时表现出更高的精度和可靠性。主动退化缓解方法中，Eq. Con.、TSVD 和 NL-Reg. 在所有实验中表现鲁棒（见表 6），并且建图表现良好，最终 ATE 误差最小（见 5 节结果）。这些方法的共性是约束或正则项在优化前或优化过程中加入，而非优化之后。

启发式参数依赖方法 L-Reg. 与 Ineq. Con. 对参数调节更敏感，相比之下 NL-Reg. 稳定性更好。例如，为 Ulmberg 隧道实验选择的最佳参数不一定在其他环境中表现同样优秀（表 6 所示）。应注意，这些可调方法的结论受经验参数选择影响，自动且最优的参数选择仍是未来研究方向。

在不同约束类型之间，很难指定某种方法绝对优越。Eq. Con.（等式约束）在各环境中表现更一致；Ineq. Con.（不等式约束）可能更精确，但估计一致性略低。TSVD 方法实现简单、计算开销小、跨环境表现稳定且误差最小，是良好的替代方案（表 6）。
在这里插入图片描述

值得注意的是，Prior Only 方法的整体表现表明，当运动先验良好（ANYmal 仿真实验）或退化持续时间短（HEAP 挖掘机实验）时，完全跳过点云注册也是可行的。Prior Only 方法可避免额外计算，同时消除 LiDAR 退化对最终解的不利影响。然而，跳过注册意味着在良约束方向也不做注册，从而丢失 LiDAR 精度。若机器人平台具有额外传感器（如相机、雷达），可使用第 2 节描述的传感器融合方法，这会使 Prior Only 更有效，但问题仍被转移到其他模态，而非根本解决优化不稳定性。

运动先验质量的重要性

运动先验精度是成功缓解 LiDAR 退化的关键因素。为突出其重要性，进行了 ANYmal 仿真实验的变体：

不再添加噪声到模拟机器人位姿，而使用完美位姿作为运动先验。结果（图 16）显示，所有主动方法表现相似，软约束方法（Ineq. Con.、NL-Reg.、L-Reg.）略差。这说明在严重退化场景下，退化点云注册可能会降低原本完美的先验估计精度。需注意，此处点云 voxel 化至 $0.1\ \text{m}$ ，因此点对点误差不可能完全为零。
为强调噪声运动先验的不利影响，相较 5.1.3 节增加了真实机器人位姿噪声。噪声采样自正态分布 $N(\mu_t, \sigma_t^2)$ 与 $N(\mu_r, \sigma_r^2)$ ，其中 $\mu_t=0.05\ \text{m},\ \sigma_t=0.05\ \text{m},\ \mu_r=0\ \text{rad},\ \sigma_r=0.02\ \text{rad}$ 。结果见图 17。Prior Only 方法显示噪声对注册质量影响严重。几乎所有方法（Eq. Con. 除外）均发生发散，可从红色墙体点云错位看出。Ineq. Con. 方法在注册墙体点云前也发生发散，因此未可视化。运动先验是下游任务（特征提取、对应点搜索、退化检测）的关键因素，需对其影响进行充分分析，以判断方法对运动先验变化的鲁棒性。

考虑因素与建议

除了 LiDAR 退化缓解方法的性能，计算复杂度与易用性同样重要。除 NL-Reg. 外，所有方法计算性能接近，可按 LiDAR 采样率实时优化。NL-Reg. 使用非线性优化，计算量大，可能落后于 LiDAR 采样率，因此需在提高精度与鲁棒性的同时放弃部分测量。

另一个考虑因素是参数调节需求。NL-Reg.、L-Reg. 与 Ineq. Con. 需参数调节（5.2.1 节）。在连续巡检或监控等需多次运行同一环境的应用中，针对环境进行调优可视为优势而非缺点。类似地，在生成度量语义数字孪生时，注册精度与鲁棒性可优先于易用性、参数调节或计算要求。

表 6 总结显示，Eq. Con.、TSVD 与 Ineq. Con. 在所有实验中均未失败；P2Plane 与 Cauchy 方法总是生成错误地图。NL-Reg. 尽管在 ANYmal 动态仿真中失败，但结果精确。Zhang 等方法在 ANYmal 仿真及森林实验中表现相当，但存在错位注册。

推荐策略：

若有充足计算资源与调参能力，可选 NL-Reg.。
在计算受限系统且可调参时，Ineq. Con. 潜力大（其 QP 形式可接受等式约束）。
若不可调参，Eq. Con. 在所有实验中表现可靠且精度较好，但依赖运动先验精度。
若希望优化自由度高，Zhang 等与 TSVD 方法可靠性较高，可在无硬约束情况下使用（无需 QP、Lagrangian、Ceres 或采样）。
RMS [Petracek 等, 2024] 作为采样方法，在几何特征丰富的环境中可加速框架，在城市场景表现最佳，但在其他实验中效果不佳。

本文结果与建议旨在提供退化点云注册问题的客观视角，同时需注意本文使用的系统（Open3D SLAM [Jelavic 等, 2022]），其采用 voxel 化建图后端、外部位姿先验及扫描到子图注册（见 4.3 节）。

7 结论与未来工作（Conclusion and Future Work）

本研究分析了退化缓解方法在多种真实环境与仿真场景中的有效性。本文提出了新方法，如 Ineq. Con.、TSVD 和 L-Reg.，并将其引入退化感知点云注册领域。此外，还对先前提出的方法进行了比较，包括 Eq. Con. [Tuna 等, 2023]、Solution Remapping [Zhang 等, 2016]、Cauchy 鲁棒范数 [Babin 等, 2019]、RMS [Petracek 等, 2024] 以及 [Gelfand 等, 2003] 的方法。

如第 5 节讨论的结果以及表 6 总结所示，主动退化缓解对于 LiDAR 退化问题至关重要且效果显著。此外，实验表明退化严重程度以及良好运动先验的可用性在选择合适方法时起关键作用。在主动退化缓解方法中，TSVD、Eq. Con. 与 Ineq. Con. 的表现更为一致，相比于 Zhang 等、L-Reg. 与 NL-Reg. 方法。

鲁棒范数方法 Cauchy 在 LiDAR 无退化情况下可提升 SLAM 系统性能；然而，当 LiDAR 退化存在时，其在所有数据集上均表现不佳。非线性优化的积极效果（以额外计算成本为代价）通过 P2Plane 与 NL-Solver 基线方法的对比得到验证。

此外，研究了全局最优点云注册方法在退化点云注册问题中的应用。如图 11 和 5.1.2 节所示，全局注册方法不能作为 LiDAR 退化的直接解决方案，因为假设存在信息充分的内点通常被违反。这些结果支持了这样一个观点：LiDAR 退化不是离群点移除问题，而是信息内点缺失问题。

在未来工作中，将对 L-Reg.、NL-Reg. 和 Ineq. Con. 方法的参数微调效果进行详细分析，并探索自动选择这些参数的方法。如果能够针对特定环境进行优化微调，这些方法具有进一步提升性能的潜力。

最后，作为本研究的延续，将研究主动退化缓解方法在传感器融合框架（如 Graph-MSF [Nubert 等, 2022a]）中的紧密集成方案。

三、算法优势 & 实验验证结果 / 论文结论

论文通过大量实验 (仿真 + 真实 field) 得到以下结论 / 优势：

Active degeneracy mitigation (hard constraints / subspace regularization) 是必要的，且在没有可靠外部估计 (odometry / prior) 的情况下尤为重要。对于纯几何 ICP / SLAM，依赖被动传感器融合并不能足够稳健。
Hard‑constraint (degeneracy‑aware) 在“野外 / 真实场景”表现优于事后加入约束 (post‑processing / sensor fusion)。也就是说，将约束融入优化过程 (pre / during) 更有效。
当使用 heuristic fine‑tuned 参数时，soft‑constraint / regularization 方法在复杂 / 几何极端劣化 (ill‑conditioned) 环境中，能取得与甚至优于硬约束的方法 —— 兼顾稳定性和灵活性。
相比只依赖外部传感器 / prior / odometry / sensor‑fusion，degeneracy‑aware ICP 提供了一种更通用、依赖更少 (less assumptions) 的解决方案 —— 更适合“in the wild, unknown, unstructured” 环境 (例如空旷场地、地下、密闭隧道、对称结构、多回环结构、无 GPS 等)。
该论文提供了可复现实现 (open‑source)，为后续研究者 / 系统工程实践者提供可借鉴、可扩展的 baseline。