做LiDAR SLAM的朋友应该都有这种经历车开进一条长走廊或者经过一片大草坪地图肉眼可见地开始歪轨迹像喝醉了一样。问题不在算法不够努力而是你正在做一次退化LiDAR配准——环境提供的信息不足配准问题本身就没有唯一解。最近读到的DCReg论文核心思路就是把这件事拆成三步先检测再诊断最后只在能解的方向上做求解。这篇博文是我对照论文标题、结合自己实际调LiDAR里程计经验做的读后梳理会尽量把每一步背后“为什么这么设计”讲清楚也会补一些工程落地时容易踩的坑。1. 先搞清楚一件事退化不是精度问题而是约束矩阵缺秩1.1 配准在数学上到底在求什么先把底层逻辑捋一下。点云配准最常见的做法是ICP、GICP或者NDT变体本质都是找一个位姿 $T$把当前帧点云变换到目标点云坐标系下让对应点残差最小。以点到面残差为例$$e_i n_i^T (Rp_i t - q_j)$$这里 $R$ 是旋转矩阵$t$ 是平移向量$n_i$ 是目标点所在局部平面的法向量。把所有残差堆起来目标函数就是$$\min_T \frac{1}{2} \sum_i |e_i|^2$$迭代求解时高斯牛顿法会在当前位姿附近对残差做线性化得到形如 $H\Delta x g$ 的方程。这里的 $HJ^TJ$也就是近似的Hessian矩阵也常被称为信息矩阵$gJ^Tr$ 是梯度。$H$ 的行列式大小、特征值分布直接决定了这个最小二乘问题在当前位置附近是否“有约束”。如果环境在某个方向上有明显的几何起伏比如墙角、树干、栏杆那么残差在这个方向上会随位姿变化产生明显响应$H$ 对应的特征值就大。反过来如果环境在某一个方向上几乎一模一样比如一条笔直的隧道、一片平整的雪地那么你无论沿那个方向平移多少最近邻对应关系都几乎不变残差大小也不变。反映到矩阵上就是 $H$ 在这个方向上有一个接近0的特征值矩阵接近奇异。这就是“退化”的数学本质不是算法性能差不是参数没调好而是观测数据本身不足以约束位姿的全部自由度。说得更直白一点就像你在完全平整的冰面上闭着眼睛走路你只知道自己的朝向大概没变但根本不知道自己往前滑了多少米。配准一旦在退化方向上强行求一个解解出来的那个数值基本就是噪声噪声再经过迭代放大就成了轨迹漂移。1.2 一个常见误区加正则项不等于解决退化很多人在工程里遇到 $H$ 奇异第一反应是给对角线加一个小量比如常见的Levenberg-Marquardt阻尼项 $\lambda I$。加了之后方程确实能求逆位姿更新也能算出来于是看起来“跑起来了”。但问题是这个操作只是让求解器不再报错并没有告诉求解器“哪个方向的信息是缺失的”。你在退化方向上加的那个 $\lambda$相当于凭空捏造了一个约束这个约束的强度来自你的参数而不是来自环境。结果就是每一次迭代退化方向都会产生一个带有随机性的小更新累积起来就成了漂移。很多时候你去看轨迹误差不是一下子跳飞出去的而是慢慢“游走”走的这其实就是正则项在退化方向上的副作用。DCReg这类工作的出发点就是不再把退化当成一个需要靠调参硬压下去的bug而是把它当成一个可以被识别、被量化、被单独处理的条件。先承认“这条路没有信息”再决定这个方向上该听谁的。2. 第一步退化检测——先知道“病”是否发生2.1 用信息矩阵的特征值谱做体检DCReg的第一步是在每次配准收敛后对 $H$ 做一次特征值分解。为什么要用特征值而不是直接看匹配分数因为匹配分数只能告诉你“整体对得好不好”而特征值谱能告诉你“每个方向上约束强不强”。把这六个特征值从大到小排一下大致对应六个自由度方向上的约束强度。如果所有特征值都明显大于0说明当前帧在六个自由度上都被环境约束住了配准结果可信。如果最小特征值接近0说明至少有一个方向缺约束。常用判断指标有这么几个条件数 $\text{cond}(H)\lambda_{\max}/\lambda_{\min}$条件数越大病态程度越高最小特征值与最大特征值的比值越接近0越危险有效约束数量即大于某个阈值的特征值个数。实际做的时候我一般会把特征值列表直接打印到日志里而不是只看一个综合分数。因为六个特征值本身就能告诉你很多信息是只有一个方向退化还是好几个方向同时退化这些信息对后续处理至关重要。只用一个“是否退化”的布尔量其实白白丢掉了大量有用的诊断信息。一个容易被忽略的点是旋转和平移的量纲不一样。如果你直接对6x6的 $H$ 做特征值分解平移对应的特征值单位是“距离的平方”旋转对应的特征值单位是“角度的平方”两者数值上天然差了好几个量级。比如同一个走廊场景平移方向特征值可能是100量级旋转方向可能是0.01量级你拿同一个绝对阈值去判断结论必然跑偏。所以要么分块处理要么在构建雅可比时对旋转部分乘一个特征长度做归一化。这个细节看起来小但不处理好退化方向分解出来的结果基本是错的。2.2 阈值应该怎么标定别迷信固定阈值退化检测的阈值到底设多少不是一劳永逸的事。不同传感器、不同扫描线数、不同角分辨率$H$ 的数值量级完全不同。64线激光雷达在同一个环境里得到的约束强度大概率比16线雷达高一个量级。如果固守一个绝对阈值可能在某台设备上“全在退化”换一台设备又“永远不退化”。我自己的做法是分两步标定。第一步采集几段已知健康的环境数据比如园区路面、有建筑的街区统计正常场景下最小特征值和条件数的分布区间第二步采集几段已知退化的场景数据比如走廊、隧道、开阔操场看两类场景的分布有没有明显分界。然后选一个能最大限度拉开两类场景的阈值而不是拍脑袋给个常数。另外退化不是一个非黑即白的标签而是连续变化的。我更推荐输出一个0到1之间的退化得分或者直接输出每个自由度的“可观性分数”阈值只是用来做后续策略切换。这样在临界场景下系统可以做到平滑过渡而不是频繁地在“正常”和“退化”之间跳变把后端优化搞得一愣一愣的。3. 第二步退化解耦——把“退化”从笼统标签变成具体方向3.1 为什么只说“退化程度”不够很多系统的退化处理是检测到退化后直接放弃这一帧的配准结果或者把整帧的协方差放大。这样做最大的问题是太粗暴它把“退化”当成一个整体事件实际退化的往往只是某个方向甚至是某几个方向的组合。举两个典型的例子。车辆在长直隧道里前行两侧墙壁和地面都在侧向位移、俯仰、翻滚通常还有约束真正缺失的约束大概率是沿隧道走向的前进方向平移。这时候如果整帧放弃等于同时丢掉了三个本来很可靠的约束方向地图在侧向也会跟着摆动。另一种情况是在开阔雪地或水面上地面是平的但四周没有任何垂直结构。这时候水平方向的位置和偏航角缺乏约束而竖直方向、翻滚、俯仰反而可以被重力平面约束住。两个场景都叫“退化”但需要补充的信息源完全不一样。“可诊断”的核心就在这里与其笼统地说“这帧退化我不信它”不如明确告诉系统“这帧的 $x$ 方向不可观其他方向照常使用$x$ 方向交给轮速里程计”。这样既保留了有效信息又避免在缺失方向上乱猜。3.2 退化子空间怎么提取方向级的退化信息同样来自 $H$ 的特征分解。特征值接近0对应的特征向量就是退化子空间的基。把这些特征向量挑出来组成一个子空间 $U$剩余特征值较大对应的特征向量组成可解子空间 $S$。之后所有的位姿增量都可以投影到这两个子空间上分别处理。这里要稍微注意特征向量给出的是“方向组合”不一定刚好等于某个坐标轴。比如在走廊里退化方向可能既包含沿走廊的平移也轻微耦合了绕某个轴的旋转因为传感器的视场和非对称扫描模式会导致自由度之间互相耦合。你要做的是把特征向量当成一个整体方向看待而不是强行解释成“就是x方向退化”。我在可视化时一般会把这几个退化特征向量投影到点云坐标系里画成箭头一眼就能看出当前缺的是哪个方向上的约束。另外退化子空间的判断阈值不能只看特征值大小还要结合当前运动状态。比如车辆静止时所有方向都可能“退化”因为点云没有变化但这不代表环境缺约束车辆高速前进时某个方向短暂退化可能只是这一帧的视场恰好被遮挡。所以DCReg这类框架在实际使用中通常会结合运动模型或IMU预测来区分“环境退化”和“观测退化”。4. 第三步分量级求解——在能解的方向用配准不能解的方向交给先验4.1 不是修改目标函数而是修改搜索空间传统配准是在整个 $SE(3)$ 流形上搜索位姿退化时也要硬着头皮搜索于是产生伪更新。DCReg第三步的思路则是把搜索空间拆开在可观测子空间内继续使用点云残差提供的信息求解在不可观测子空间内不再依赖点云残差而是改用外部先验来约束。实现层面可以这样理解def dc_registration(source, target, T_init, prior, threshold): T T_init for _ in range(max_iter): J, r build_linearization(source, target, T) H J.T J g J.T r eigval, eigvec np.linalg.eigh(H) observable eigval threshold unobservable ~observable delta_obs solve_subspace(H, g, eigvec, observable) delta_unobs project_prior_to_subspace(prior, eigvec[:, unobservable]) T exp_se3(delta_obs delta_unobs) T return T这里的solve_subspace并不是真的要新写一个求解器而是在全量Hessian里把不可观测方向对应的自由度冻结掉或者加上一个强先验让信息矩阵变成非奇异。具体做法可以选择固定增量投影、置零、或者加一个大权重先验项。效果都一样点云配准只负责它擅长回答的问题不能回答的问题明确交给别的信息来源。一个关键理念是在退化方向上“完全不动”往往比“乱动”要好。如果你对当前位移完全没有把握那最优策略就是保持上一时刻的估计同时把这个方向的不确定度传递给后端。后续如果有回环检测、地图匹配或新的观测进来再把这个方向上的位姿修正过来。这里面体现的其实是一种“知道自己不知道什么”的机器人学思维。4.2 先验怎么选IMU不是万能的第三步的效果高度依赖先验质量。很多人下意识会觉得退化方向缺约束那就融合IMU呗。但IMU在不同方向上的可靠性差别很大。竖直方向、翻滚角、俯仰角IMU因为有重力观测长期很稳可以放心作为先验偏航角低成本IMU的零偏不稳定长期会漂只能做短期约束不能当长期锚点水平位置IMU完全没有直接观测只能靠积分误差快速累积。所以在开阔平地这种主要缺水平位置和偏航角的场景单纯叠加IMU可能越救越乱。更稳妥的方案是结合轮速里程计、车辆运动学模型或者地面标志物而不是把宝全押在IMU上。反过来在隧道里如果主要退化方向是前进方向轮速里程计通常是比较靠谱的先验IMU反而不是必须的。这里顺便说一个和“lidar-imu标定”相关的经验。标定LiDAR和IMU外参时很多人会遇到标定结果在某个分量上反复横跳怎么优化都不收敛。其实很多时候不是算法bug而是你用来标定的数据里某个方向本身就不被激励。比如车一直在平直道路上匀速开那么外参中与水平位置相关的分量几乎没有可观性优化器自然瞎猜。DCReg这套检测诊断的思路完全可以迁移到标定问题上先分析运动激励和场景结构把不可观的外参分量剔掉或者固定住只优化有信息的分量标定结果会稳定很多。4.3 和地形点云配准结合的思路这几年地形点云配准需求越来越多尤其在农机、矿山、巡检机器人场景。这类点云有个明显特点以平整地面为主法向量高度集中在z轴附近。纯地面点云在x、y水平平移和偏航角方向上几乎没有约束配准很容易原地打转。用地形点云做配准时DCReg的思想可以帮助做一件很实际的事先把地面连续性作为强先验约束z轴和横滚、俯仰然后把水平方向和偏航角交给点云中的非地面点、地形起伏特征或者外部定位源。如果当前地形真的非常平坦、又没有植被遮挡那系统就应该如实输出“水平方向退化”而不是强行算一个位姿让轨迹看起来平滑。很多时候轨迹误差反而不如一个诚实的置信度有用因为有了置信度你才能决定要不要触发回环、要不要降低速度、要不要切换定位源。5. DCReg这组三步比单纯“加权重”强在哪5.1 可解释性才是工程落地的关键做机器人定位系统的人都有体会系统跑崩了不可怕可怕的是崩了你不知道为什么。传统配准给不出任何中间信息你只能看到一堆残差和耗时。DCReg这套框架最大的工程价值不是新奇的非线性优化技巧而是给整个配准过程提供了一个可解释的接口。每次配准结束你手里会有特征值列表、退化方向向量、每个维度的置信度。把这些东西写进日志出问题时就能回放某一秒轨迹开始漂你打开日志看那一帧的最小特征值是不是突然掉下去了退化方向是不是突然指向某个轴。是环境问题还是阈值设错了还是动态物体干扰了特征值分解一眼就能分辨。这个能力在实际维护长期运行的机器人系统时比多一个百分点的精度更值钱。下面是一个我常用的场景对照表可以帮助判断当前场景的退化方向和推荐先验典型场景常见退化方向推荐先验或辅助手段长直走廊沿走廊方向的前进平移轮速里程计、预先地图匹配开阔水面/雪地水平位置、偏航角更高层路径约束、多传感器组合平整草地/农田水平位置、偏航角轮速里程计、稀疏特征点隧道内前进方向平移、部分姿态分量里程计辅助、放慢速度地下矿山巷道沿巷道走向的平移轮速、惯性辅助、布设反光标记这个表不是死的具体取决于你的激光雷达线数、安装角度、视场范围。比如你把雷达斜装原本退化的方向可能就变得可观了这也是工程上常见的破解退化手段之一。5.2 动态物体和阈值标定是最大的坑特征值分解本身很便宜6x6矩阵的分解一次也就微秒级对实时性几乎不构成压力。真正的坑在数据质量上。最大的坑是动态物体。如果当前帧里混入大量行人、车辆或者其他移动物体这些点在对应匹配时会产生错误的残差它们的雅可比会“伪造”出虚假的大特征值让原本退化的方向看起来有约束。比如在空旷广场上本来水平方向没有约束但刚好有几辆车从旁边开过特征值分解就会误判“这个方向有信息”配准结果被动态物体带着走。所以退化检测之前一定先做动态点剔除或者至少用鲁棒核压低异常残差的权重否则整个DCReg的三步流程都是建立在脏数据上的。另一个坑就是前面提到的阈值标定。我建议一步到位把特征值、条件数、每个自由度的置信度全部做成可配置参数并且在系统启动阶段做短暂的自适应采集用初始几十帧的正常数据来动态确定基线。不要图省事写死一个全局常数尤其是同一套代码要部署到不同车型、不同雷达上时固定阈值早晚会在某个你没测过的场景里坑你一次。5.3 处理结果的策略要平滑最后提醒一点DCReg检测到退化后处理策略不要做得太“硬”。比如一旦检测到某个方向退化就把该方向的增量置零这样简单但会产生地图上的台阶感。更好的做法是给退化方向一个连续的可信度权重让该方向的更新在“完全信任点云”和“完全信任先验”之间平滑过渡。这个权重可以直接由特征值比值映射得到比如把 $\lambda_{\min}/\lambda_{\max}$ 经过一个sigmoid函数转成0到1之间的置信度再用来混合点云增量与先验增量。另外位姿置信度还应该传递给下游比如局部地图更新、关键帧判定、回环检测触发条件。如果某个关键帧的某个方向置信度很低你就不应该让它作为地图优化的强锚点否则它会拖着整个地图一起漂。把这些信息串起来整个SLAM系统的鲁棒性才会真正提升而不是只让单帧配准的结果好看。6. 读完之后我建议你在自己的数据上做三件小事DCReg的理念并不复杂理解了“检测、诊断、求解”三步之后更重要的是把它落到自己的系统里验证。我建议你打开自己之前的失败数据做三件很小但很有用的事。第一把你的配准Hessian特征值打出来看看。不管你现在用的是ICP、GICP还是NDT都能拿到类似的近似信息矩阵。找一段走廊数据打印每一帧的最小特征值你会直观地看到数值是怎么掉到接近0的也会对“退化”建立真正的手感而不是只在论文里看到这个概念。第二把退化特征向量可视化出来。把特征值最小的一两个特征向量按位姿投影到点云中心画成带方向的箭头叠加在点云上。你会发现箭头指向的方向往往就是你轨迹漂移最严重的方向这种可视化比任何理论解释都更能说服团队重视退化问题。第三给轨迹加一个“可观性得分”字段。不需要改太多代码只要在记录位姿时同时记录最小特征值和条件数离线评估时把ATE误差曲线和退化得分曲线画在同一张图里。我自己的经验是两者往往高度相关。看到误差尖峰和特征值低谷同时出现的那一刻你就知道这套诊断逻辑到底有多值钱了。我自己实际用过类似思路做过一版室外履带机器人的定位系统最大的感受是处理退化不应该靠“祈祷环境别太差”而应该靠“知道哪里差了然后想办法补哪里”。DCReg把这件事拆成三步等于给了工程师一个非常清晰的行动框架。最后再分享一个小技巧以后遇到配准漂移先别急着调迭代次数和收敛阈值先去看看特征值谱那个东西会告诉你问题出在环境还是出在代码。
