开头

前阵子调试一套多线激光雷达SLAM，在一条两侧全是重复立柱的隧道里，配准结果直接偏了快一米。换了阈值、调了迭代次数都没用，后来看到DCReg这篇论文才意识到，问题根本不在ICP迭代本身，而是我们压根没搞清楚“哪里退化”了。所谓退化LiDAR配准，就是环境中某个方向缺少几何变化，导致算法没法在该方向给出唯一解。DCReg最大的贡献，是把这种原本让人抓狂的退化配准拆成了三个可诊断、可求解的步骤：先量化退化方向和程度，再把退化子空间从优化问题中解耦，最后用IMU、地形点云配准这类外部约束去补全剩余自由度。对跑无人车、机器人、做测绘的朋友来说，这套思路很有参考价值，这篇笔记就是把它掰开揉碎讲清楚。

1. 退化配准是什么，为什么它一直让SLAM头疼

1.1 先给一个生活化类比

配准的本质，是把两帧点云通过旋转和平移对齐。正常情况下，房间的墙角、路沿、树干、电线杆，都能给算法提供“这里对齐了没”的反馈，就像拼图图案里的色块和纹理，块与块之间有唯一对应关系。但在长走廊、隧道、地下车库、草原雪地这类场景里，几何结构变成了一排排重复的立柱、笔直的墙面、均匀的地面。拼图图案全是同一条条纹，你左右挪一格、前后推一点，视觉上看起来都一样。于是算法陷入“鬼打墙”——沿某个方向怎么移动，残差都不变，这就是退化。

如果只是慢一点还好，实际处理起来更麻烦。退化的方向上一旦积累了误差，移动机器人会认为自己在沿走廊直线前进，实际上可能已经蹭到了侧墙，或者在一个十字路口转了弯自己还不知道。后续建图、路径规划全都会被带偏，而且很难通过“多迭代几次”补救，因为问题不在迭代次数，而在信息不足。

1.2 数学上到底发生了什么

配准问题通常被建模成最小化点云残差：

T* = argmin_T Σ || p_i - (R q_i + t) ||²

这里的T是6自由度位姿，包含三维平移和三维旋转。正常环境下，目标函数在6个维度上都有曲率，像一个碗，只有一个碗底，求解器可以稳定滑到最低点。退化环境下，某个方向上的残差曲面变得越来越平，甚至变成一条直线。用专业一点的说法，就是Hessian矩阵（或者叫信息矩阵）在对应方向上的特征值趋于零。

我整理了几类典型退化场景和它们对应的不可观自由度，供对照：

理解了这个数学本质，就能理解DCReg为何强调“先诊断”。因为连退化方向都没识别出来，后面一切优化都是盲调。

2. DCReg三步骤框架：先诊断、再解耦、后补全

2.1 第一步：把退化量化，而不是只喊“匹配不好”

DCReg在设计上非常强调“可诊断性”。传统做法是算一个配准得分或者协方差矩阵，得分低就说“匹配不好”，但到底是什么方向不好，完全不知道。DCReg第一步做的是构造一个信息矩阵，来源于每一对匹配点的表面法向量外积。简单记忆：每个点提供的约束方向就是它的法向量方向，把所有点的法向量加权累加，就能看出当前帧在哪些方向上有“支撑”。

然后对该信息矩阵做特征值分解，得到λ1 ≥ λ2 ≥ λ3三个特征值，以及对应的特征向量v1、v2、v3。如果某个特征值很小，说明沿对应特征向量方向几乎没有几何约束，这就是退化轴。论文里通常用λ_min / λ_max来定义退化程度，也可以把三个特征值分别投影到6自由度位姿增量上，判断平移和旋转各自的退化强度。

这一步的输出，是一个明确的“诊断报告”，比如“本帧在平移方向沿走廊轴向退化，退化强度为0.02”。有了这份报告，后两步就可以针对性地处理，而不是对所有方向一刀切。

2.2 第二步：把优化分解成非退化子空间和退化子空间

拿到退化方向后，DCReg并不急着把整个6自由度问题交给优化器。它做的是把状态增量分成两块：一块由非退化方向组成，这些方向几何约束强，可以放心用ICP或者NDT去精配；另一块是退化子空间，这些方向上点云残差基本不提供信息，如果继续优化，求解器只会沿着退化轴“顺坡滑”。

具体做法是把位姿增量投影到可观测的子空间，然后求解一个带掩码的优化问题。粗略说，就是求增量δT时只让它在非退化方向上有非零分量，退化方向的增量设为零或者使用弱先验。这样避免了优化器在退化方向上瞎跑，也让非退化方向的解能保持高精度。我实测下来的感觉是：在长走廊里，先用DCReg的退化诊断锁定x方向，再强制x方向不参与点云优化，侧向y和航向角反而收敛得非常好，整体轨迹也没有明显漂移。

这个步骤还解决了“一个坏方向污染所有方向”的问题。普通配准在退化时会把误差均匀摊到6个自由度上，即便好的方向也被带偏。DCReg的解耦思路很像在解一个病态方程组时，先把条件好的变量解出来，剩下的方程再单独处理，效果远好于对全矩阵求伪逆。

2.3 第三步：用IMU和地形约束补全剩余自由度

退化子空间靠点云解不出来，那就必须引入其他信息源。DCReg第三步的做法比较务实：把IMU预积分、地形点云配准、先验地图这样的约束写成正则项，加进优化框架里，并且权重和退化程度挂钩。

先说IMU。IMU能提供角速度和加速度的短时积分，尤其在车辆加速、转弯的时候，能直接约束平移方向和姿态变化。但IMU有漂移，尤其加速度积分两次之后误差会爆炸，所以不能全程用强约束，只在退化方向上给一个“短时锚定”最合理。对于lidar imu标定，这一步非常敏感。如果外参标定不准，IMU的积分结果在退化方向上的投影本身就是错的，那约束越多越糟糕。我自己的经验是，在使用DCReg这类方法前，一定要花时间把雷达和IMU的外参标定到毫米级和零点几度以内。

再说地形点云配准。这里的“地形”指的是地面曲面或道路平面。在越野、颠簸场景中，虽然水平方向上有坑洼、石头可以提供约束，但垂直方向和俯仰滚转往往不够，此时地面拟合平面可以提供一个明确的“大地水平”约束。具体做法是把当前帧扫到的地面点提取出来，拟合一个平面，或者在局部建一个高程栅格，然后约束点到平面的距离。对平坦道路，它能稳定z、roll、pitch；对斜坡地形，它可以结合车辆运动学模型提供高度变化约束。

整个第三阶段的目标函数可以写成：

E_total = E_cloud + λ1 · E_imu + λ2 · E_terrain

其中λ1和λ2不是固定值，而是根据第一步诊断出的退化强度自适应。退化严重的方向，外部约束权重调大；退化不严重的方向，点云残差权重正常。DCReg的巧妙之处在于，它没有把外部约束“平均分配”到所有自由度，而是精准投送到真正需要补全的维度上，这样既保精度，又不会让IMU漂移污染本来很好的点云解。

2.4 三步之间的关系，为什么这个顺序管用

这三个步骤不是简单的流水线，而是有因果关系的。先诊断，才能知道哪些方向要靠外部约束，避免盲目调权重。再解耦，才能保证点云自身在非退化方向的精度，这是整个系统的基础。最后补全，是利用额外的传感器或地图信息，把退化方向“填起来”。

如果反过来，直接一开始就加IMU约束、地形约束，你会发现权重很难调：权重小了，退化照旧漂移；权重大了，非退化方向反而被拉弯。DCReg把这个问题拆解成三个独立环节后，每个环节都可以单独测试和调参，这在工程落地上是巨大的优势。读论文时我最大的感受是，它整篇文章在讲述“如何诚实地面对信息缺失”，而不是硬凑一个看似漂亮的数学公式。

3. 实操视角：在真机上实现DCReg的几个关键点

3.1 退化检测指标怎么算，才算准

实现第一步的退化检测时，最核心的代码是计算信息矩阵和特征值分解。我建议不要直接用配准雅可比矩阵，因为不同传感器线数、不同距离下，点的密度差异会严重影响特征值。更稳妥的做法是做一次法向量估计，然后用加权的法向量外积来构造信息矩阵。

流程大致是：

1. 对当前帧点云，按最近邻搜索每个点的邻域，用PCA求法向量。
2. 知道每个匹配点对应的残差，计算残差关于位姿的雅可比矩阵J。
3. 信息矩阵I = Σ w_i · J_iᵀJ_i。
4. 把I做特征值分解，得到特征值降序排列。
5. 用最小特征值与最大特征值的比值衡量退化强度。

这里有几个容易踩的坑。第一，邻域半径不能直接用一个固定值，要考虑到点云是近密远疏的，建议按扫描线深度自适应，比如在近处取0.2米，远处取0.5米。第二，法向量估计时，如果邻域内只有共面的点（比如纯平面），求出的法向量会不稳定，需要在PCA后检查第二和第三特征值，如果它们相近且都很大，说明这个邻域不是理想的平面结构，要降权或者丢弃。第三，不要对所有点一视同仁，动态物体上的点（行人、车辆）会污染法向量方向，最好先滤除。

我在自己项目里用的阈值经验是：λ_min / λ_max < 0.01，判定为严重退化；0.01到0.1之间为轻度退化；大于0.1则视为几何充裕。这个阈值和应用场景关系很大，如果你用的是64线雷达，点云密集，阈值可以放宽到0.05；如果是16线雷达，空旷环境下比值容易偏小，建议先用bag日志统计正常场景的分布再定，不要照搬论文里的数。

3.2 子空间解耦在代码里怎么落地

很多同学拿到论文后最懵的是第二步的落地。其实代码思路不复杂，核心是构造一个掩码矩阵M。假设状态量δT是一个6维向量，对应(t_x, t_y, t_z, roll, pitch, yaw)。经过第一步特征值分解后，你会得到哪些维度退化，比如只有t_x退化，那么掩码M就是对角矩阵，对应t_x位置为0，其余位置为1。

求解时，先计算原始正规方程：

(JᵀJ) δT = Jᵀr

然后让退化方向的增量置零，等价于解带约束的方程。实际操作可以把掩码矩阵乘到正规方程两侧，再去求解。我习惯这样写：

# 状态量顺序: [tx, ty, tz, roll, pitch, yaw] # degenerate_dims 是第一步诊断出的退化维度列表，如 [0] import numpy as np H = J.T @ J b = J.T @ r mask = np.ones(6) mask[degenerate_dims] = 0.0 M = np.diag(mask) # 对H做掩码，将退化方向的行列置0（对角设为1避免奇异） H_masked = M @ H @ M H_masked[degenerate_dims, degenerate_dims] = 1.0 b_masked = M @ b b_masked[degenerate_dims] = 0.0 delta_T = np.linalg.solve(H_masked + 1e-8*np.eye(6), b_masked)

这段代码展示了一个简化思路。关键点是，在退化维度上不要设0矩阵的求逆，而是用1做对角占位，保证数值稳定。然后外部传感器约束的加入，是在b_masked上再叠加一项，比如IMU的残差投影到退化方向。

如果你用的是因子图优化，比如GTSAM，那么做法是把退化方向设置成低精度先验因子，也就是给退化方向的协方差一个非常大的值（表示不信任点云），同时给非退化方向一个很小协方差（表示信任点云）。这样在因子图优化里也能自然表达DCReg的第二步思想。

3.3 IMU约束要松耦合还是紧耦合

根据我试过的几种方式，松耦合和紧耦合各有取舍。松耦合的做法是：先用IMU积分得到帧间相对位姿，然后把相对位姿作为一个先验约束放进点云配准后的优化。DCReg更适合松耦合，因为它需要IMU约束精准作用在退化方向上，而不是参与全状态的紧耦合修正。紧耦合的优势是所有因子统一建模，精度理论上更高，但对外参标定和内参噪声更敏感，调起来费时。

实际工程里，我建议先做松耦合验证退化方向识别是否正确，再决定要不要上紧耦合。如果你发现IMU约束加进去之后，退化方向的漂移确实被拉住，而非退化方向的精度也没有明显下降，那说明诊断和解耦都正常。如果加完IMU后反而整体精度下降，先不要急着调权重，回头检查lidar imu标定外参的旋转偏差，通常能省掉半天时间。

还有一个容易忽略的点：IMU的bias随机游走随时间增长，退化场景往往持续几十秒甚至几分钟（比如长隧道），单纯依赖IMU预积分会在后期产生明显漂移。此时最好利用地面约束或车道线约束做定期修正，否则就是用一个漂移补偿另一个漂移，得不偿失。

3.4 地形点云配准能补哪些方向，怎么构造约束

“地形点云配准”这个词最近很多人提，本质就是把地面、路沿、地形起伏当成一种特殊的点云约束。DCReg里第三步提到的地形约束，我理解是一种弱平面约束，而不是把地面完整地当“结构化地图”。

最稳的做法是提取地面点。对机械式LiDAR，可以按扫描线拟合地面，或者使用简单的RANSAC平面拟合。拟合出地面平面方程ax+by+cz+d=0后，残差就是点到平面的距离。这样能给z、roll、pitch三个方向提供非常强的约束，尤其在开阔广场这种四野无物、地面平整的场景里，地面约束几乎是唯一靠谱的稳定器。

但要注意，这里的地面拟合必须做范围限制。只取车辆前后3到10米内的地面点，太远了地面可能不平，而且点云稀疏，拟合误差大。另外，如果地面是湿滑水面或者松软沙地，拟合出的平面会有噪声，建议设置距离阈值，比如超过0.1米的点不参与拟合。在草地上，草叶会产生随机点，也会干扰平面拟合，可以先做体素降采样，再用高度范围滤除噪声。

如果你有现成的先验DEM（数字高程模型）或者高精地图，也可以把当前地面点和DEM栅格做高度配准，这本质上还是地形点云配准，不过是与先验地图配，对长期项目非常实用。

4. 常见问题与排查技巧实录

4.1 为什么我的场景一直在报退化，明明看起来结构很丰富

这是最容易误导人的地方。我曾经在园区里测试，周围有楼和树木，但DCReg一直报退化。查了半天发现是法向量估计的邻域半径太大，把细小结构全平滑掉了，点云看起来“像”退化。邻域半径要按传感器特性去设，16线雷达在30米外的点云很稀，如果拿固定0.5米邻域去估计，基本每个点都是孤立点，法向量全是噪声。我的建议是：先做一次点云降采样到0.1米分辨率，再用KD树找半径内邻居，并且对邻居数量做下限判断，少于5个点的直接丢弃。

另一类常见原因是运动畸变。如果雷达没有去畸变，旋转过程中车又跑得快，一帧点云里的墙是斜的，法向量也被扭歪，特征值比值看起来退化。解决办法是在配准前用IMU做帧内畸变校正，这也是很多系统不稳定的原因。

还有就是匹配对应关系本身太差。DCReg诊断用的信息矩阵依赖正确匹配，如果初始位姿误差很大，匹配点全是错配，信息矩阵就算出来一堆零。所以强烈建议先做一轮粗配，比如使用里程计预测初始位姿，再做退化诊断，顺序不要反。

4.2 特征值阈值到底怎么定，调参有什么技巧

阈值没有通用值，因为不同点云密度、不同距离范围，特征值尺度差异很大。不要直接看绝对值，要用比值λ_min/λ_max。但我发现，这个比值在旋转和平移上最好分开看。平移对应的特征值和旋转对应的特征值尺度不一样，混合在一起容易掩盖问题。

我的经验是，把特征值分解后的结果分别投影到平移和旋转子空间。然后记录几段正常场景和退化场景bag的比值分布，画直方图，找到明显的分界点。比如我在某台16线雷达上测出来，正常场景比值在0.2到0.5之间，退化场景小于0.05，于是把0.1作为中度退化阈值。如果你已经有先验地图，还可以用配准后的一致性亮度来验证阈值设置是否合理。

另外，阈值最好设成滞回模式：进入退化状态用0.1，退出退化状态用0.15。这样避免在阈值附近反复横跳，否则IMU约束权重忽大忽小，轨迹上会出现抖动。

4.3 退化方向处理不当会带来哪些“隐蔽”问题

最大的坑是“以为处理了，其实没处理彻底”。比如只把退化维度的增量置零，但更新旋转时用的是李代数更新，退化维度在投影的时候没有对应到正确的状态方向上，导致优化后位姿仍然沿退化轴漂移。我的做法是把退化诊断从笛卡尔坐标转换到当前帧的世界坐标下再做投影，确保“退化方向”和“状态增量方向”一致。

另一个隐蔽问题是，发布里程计时如果不带上不确定性，下游融合模块会把退化方向的错误估计当成高置信度信息。DCReg第二步如果想发挥最大价值，就应该输出一个包含退化方向信息的协方差矩阵，退化方向的方差设大，非退化方向方差设小。这样后端直接吃掉这个先验，不会把一个不可观方向当成可观方向去融合。我在实际集成中明显感觉到，有协方差输出之后，整个系统的刹车、避障判断都稳定了，因为规划模块知道哪个方向“没把握”。

4.4 强退化场景直接放弃点云，还是保留弱约束

有些同学认为既然退化了，就应该完全放弃点云约束，只用IMU。我的体会是不要极端。长走廊方向虽然平移不可观，但侧向和姿态还是可观的，放弃点云会白白损失精度。正确做法是像DCReg那样，保留非退化方向的点云约束，只对退化方向做弱化。这样即使IMU质量一般，系统还能维持一个不差的姿态估计，等回到有特征的环境时，也能快速拉回来。

如果完全没有IMU，也不想用地形约束，另一个备选方案是使用先验地图。只要地图中对应位置的几何结构能约束退化方向，就可以把它抽出来单独建一个“约束源”。这和地形点云配准是同一思路，只是约束源更丰富，不再限于地面。

最后说点个人体会

我把DCReg读完并落地实践之后，最大的转变是：面对配准退化，我开始先回答“退在哪里”，而不是急着改参数。这个诊断成本很低，但带来的收益远超我预期。以前在隧道里走偏了，只能反复调ICP阈值，最后可能是把传感器架歪导致的，也没发现。现在跑SLAM之前，我会先做一段退化检测镜像测试，把各种场景的特征值分布记录下来，部署到真机后直接用这套诊断逻辑，省心很多。

如果让我给一个实用建议，就是从第一步退化检测开始做起，别一口气上完整套。先把“退化方向识别”做成一个独立模块，让它实时输出方向向量和退化强度，配合录制的bag回放看效果。诊断稳定了，再去接IMU和地形约束。这样既不容易翻车，也方便将来在其他配准框架里复用。DCReg整体不复杂，但它把“退化”这个问题从玄学变成了工程上可以标定、可以测试、可以修复的东西，这本身就很值钱。