前阵子把港科大这套刚开源的激光雷达配准工作拉下来跑了一遍越跑越觉得有些话不吐不快。SLAM圈子里待久的人都清楚激光雷达配准的痛点早就不是“精度够不够”而是“退化场景下还能不能稳住”。尤其当你开着车半夜在地下车库绕圈四周全是水泥墙和柱子或者在一个几十米长的封闭廊道里直行看里程计轨迹像喝醉了酒一样往边上飘那种无力感太真实了。这篇发在IJRR上的工作把退化问题从“事后补救”变成了“事前量化”并且代码已经开源。这篇文章我就按自己的理解把它的原理、复现过程、实测表现和坑都拆开讲清楚希望能给正在被退化场景折磨的同行一点参考。1. 激光雷达配准“退化”到底退的是什么约束缺失的本质1.1 退化场景为什么让ICP/GICP集体失灵配准的本质是通过最小化误差函数求解当前帧相对参考帧的位姿变换。最经典的ICP目标是让最近邻点对的距离之和最小GICP则更进一步通过点的协方差矩阵来编码表面结构。这些方法本身在结构化环境里表现非常好因为点云在各个方向上都能提供足够的几何约束。但问题恰恰出在“约束”上。你站在一条笔直的长走廊里前后方向上有门、有墙面凹凸左右方向上有两侧墙壁信息很丰富但沿着走廊轴线的方向激光扫过去看到的几乎是等距的平面任何微小的平移在误差函数里造成的代价变化都不明显。数学上就是误差函数对这个方向的梯度趋近于零迭代时沿着该方向的位置修正量几乎没有可靠信号于是ICP会在走廊方向产生随机漂移里程计轨迹越来越歪。隧道、空旷广场、雪地、水面、矮墙环绕的停车场本质上都是同一个问题——某个自由度对应的几何约束缺失。我做室内机器人时遇到过一个更隐蔽的情况不是完全没有约束而是约束的强度差异极大。比如一个长走廊横向约束很硬纵向约束很软这种“强弱悬殊”比“完全缺失”更难处理因为普通阈值检查根本识别不出来算法仍然在收敛但收敛结果与实际真值之间的偏差已经大得离谱了。1.2 约束方程、Hessian与退化方向用线性代数看问题要理解退化最直观的方式是看配准问题的二阶近似。把当前帧点云变换到参考帧坐标后残差函数可以线性化成一个最小二乘问题。把这个问题的Hessian矩阵求出来做特征值分解你会看到一组特征值和对应的特征向量。特征值越大表示对应方向上的约束越强特征值接近零的方向就是退化方向。打个比方你用手去推一张桌子。桌面结结实实无论你怎么推它都稳定但桌脚方向如果虚着你一推就动。在配准问题里特征向量就是在描述“桌子哪些方向是实的、哪些方向是虚的”。传统ICP不会去做这个分析它默认所有方向都有同样的约束结果就是硬撑着把退化方向也当成有约束来处理最后被噪声主导。很多研究者已经注意到了这一点但处理方式往往是设一个阈值特征值小于阈值就认为退化然后直接把整个配准结果扔掉或者等下一帧再试。这种做法有两个问题一是阈值很难调二是“整体退化”的判定掩盖了“只在一个方向上退化”的真相。1.3 常见的工程补救为什么治标不治本工程上最常见的退化场景解决办法有这么几类第一增大匹配范围用更大范围的历史点云来做配准相当于把“只看局部”改成“看更大局部”这个方法在部分场景有效但如果整个环境就是自相似的结构比如一整条完全笔直、墙面平整的隧道再大的范围也提供不了轴向约束第二给配准加一个松散的惩罚项让它不要偏离预测值太远这本质上是引入先验但先验如果来自IMU积分长时间直行后IMU漂移一样会让结果偏移第三前端配准不行就靠后端回环修正问题是回环修正只是事后纠正对于“开着开着就飘了”的过程轨迹回环前的长时间漂移已经污染了建图结果。这些方法的问题在于它们都把“退化”当成一个需要绕开的异常状态而不是一个可以在线量化和建模的几何性质。港科大的这套工作给我的第一感觉是他们选择的路线不一样先显式计算退化方向和退化程度再分别处理而不是用一个全局开关去回避问题。2. 港科大这套方法的设计思路退化解耦、方向感知、多源联合2.1 从全局配准到分段局部配准先识别退化再配准这个项目在开源的仓库里给出的核心框架我理解下来可以概括成三步退化检测、退化方向分解、分方向优化。第一步不是直接做配准而是先对当前帧和参考帧提取关键几何信息构建一个局部结构张量。结构张量本质上是一个3x3的对称矩阵它描述了点云在空间三个正交方向上的“分布强度”。你可以把它理解成一个三维版本的方差分析在某个方向上如果点云分布得很开、表面法向量变化很大那么这个方向的方差就大约束就强如果所有点几乎都落在垂直于某方向的平面上那这个方向的方差就趋近于零约束就弱。有了结构张量以后求解其特征值和特征向量就能把空间分成三类方向高约束方向、弱约束方向和无约束方向。这一步非常关键因为它把“配准是不是要失败”这个宏观问题转化成了“哪些方向可以信任、哪些方向需要外部信息”这个微观问题。在后面实际的位姿求解里高约束方向保留原始ICP/GICP残差弱约束方向降低权重无约束方向直接不允许参与优化或者强制引入其他传感器的观测来约束它。2.2 几何特征与结构张量把退化方向显式算出来具体实现里结构张量的构建不是对原始点云直接算的而是先经过体素降采样和一致性过滤。我看到开源代码里这一步用了自适应体素即根据当前环境的点云密度自动调整体素大小在近处以小体素保留细节在远处用大体素压低噪声。有意思的是它没有简单地把所有点都扔进张量计算而是先做了一次“结构性筛选”只保留那些局部邻域内法向量变化可被建模的“结构点”那些完全散乱、没有局部几何形状的孤立点比如远处偶尔打到的树叶、飞鸟会被剔除掉因为这些点的方差贡献全是噪声算进张量反而会把约束估计带歪。然后对筛选后的点集构造邻域协方差矩阵。对每一个局部邻域求它的协方差矩阵特征值λ1≥λ2≥λ3。这三个特征值的比例关系直接对应局部几何形态λ1很大、λ2和λ3很小是细长边缘λ1、λ2很大、λ3很小是平面三个值接近是球状散乱点。有意思的是作者并没有只拿那些“平面特征”或“边缘特征”去做匹配而是把这些局部形态的置信度传递到了全局结构张量里。也就是说一个局部邻域如果是平面它给结构张量的贡献主要在两个切向方向上如果是边缘则只在一个方向上贡献约束。这种做法比单纯把原始点云丢进张量计算要精确得多也更能反映真实的约束分布。2.3 退化方向上的约束降权策略算出退化方向之后接下来的问题是在求解位姿时怎么处理这些方向。代码里我看到的是一个改进版的GICP代价函数。经典GICP里每个点对残差都带有一个权重项但这个权重项是固定的、由协方差决定。这套方法把权重拆成了两项的乘积一项是原GICP里的几何权重另一项是“方向可信度”权重。方向可信度的计算方法是把这个点对误差投影到当前退化方向的子空间里如果投影分量大就说明这个点对主要约束的是退化方向那么它的权重就会明显降低反之如果这个点对的残差主要落在强约束方向上它的权重基本不变。这个设计的精妙之处在于它不需要在退化方向完全丢失时“抛开所有约束硬猜位姿”而是让算法在强约束方向依然严格匹配在弱约束方向保持一个极小的弹性。你可以把最终解想成一个“被压扁的弹簧系统”强约束方向弹簧很硬弱约束方向弹簧很软甚至直接不装弹簧。这个处理让解算器在退化环境下不会为了满足某一个不可靠的方向而牺牲整体精度。2.4 与IMU/轮速计的松耦合与紧耦合前面这些处理解决的是“一个纯激光解算器如何在退化场景下不崩”但如果环境真的退化到完全无约束的地步比如在一个纯反射均匀的球形空间里任何纯激光方案都无解必须依赖外部信息。港科大这套方法在开源代码里同时给了松耦合和紧耦合的融合接口这点非常实用。松耦合模式下退化检测模块会把当前帧的退化子空间方向输出给融合节点融合节点里IMU预积分或轮速计的观测增量会直接填入这些方向激光则在其他方向上主导更新。紧耦合模式下退化方向会转成一个额外的先验残差项进入同一个非线性优化问题一起求解。实测下来紧耦合模式在退化方向上的稳定性更好但前提是IMU的零偏估计足够准松耦合模式虽然对IMU误差没那么敏感但在长走廊直行这种“持续退化”状态下仅靠单方向填补无法完全消除累积误差需要配合后端优化。3. 开源代码复现依赖环境与一跑起来的完整链路3.1 环境准备与依赖清单先说结论这套代码的依赖算不上友好但也没有到劝退的地步。我个人是在Ubuntu 20.04 ROS Noetic的环境下跑通的CUDA版本11.4PCL 1.10Eigen 3.3。仓库里推荐的依赖还有GTSAM、Ceres Solver和livox_ros_driver其中livox驱动只在跑Livox数据时需要如果你用的是Velodyne或Ouster可以直接跳过换成对应的驱动驱动即可。有一个非常容易踩的坑是PCL版本和Ceres版本的冲突。GitHub上不少issue都在反馈编译链接阶段报“找不到Ceres”或者“PCL宏定义冲突”。我排查了一圈发现根因是系统里装了多个版本Ceres而CMake优先链接了不兼容的那个。解决方式是在CMakeLists里显式指定Ceres_DIR路径或者在cmake命令里用-DCeres_DIR/usr/lib/cmake/Ceres指定。另外代码里用到了C17的std::optional如果编译器默认标准是C14会把一堆头文件报错务必在CMakeLists的CMAKE_CXX_STANDARD里设置成17。3.2 数据准备与启动流程仓库提供了两个示例数据集的下载入口一个是港科大自己采集的室内退化序列包含长走廊、大楼梯间和地下车库另一个是公共数据集中的隧道序列。数据格式统一是ROS bag里面包含点云话题和IMU话题。点云话题类型可以是sensor_msgs/PointCloud2也可以是Livox自定义的livox_ros_driver2/CustomMsg代码里做了两套解析接口。启动流程比较清晰拉下来编译之后先启动launch文件source devel/setup.bash roslaunch dalr_ros dalr_localization.launch然后播放数据rosbag play corridor_data.bag --clock -r 1.0如果一切正常你会看到窗口里实时画出当前帧点云和累计地图控制台会打印每一帧检测到的退化子空间方向数和对应特征值。我第一次跑室内走廊那一段时能清楚看到退化方向数量从正常的0变成1然后又变成2对应“靠近墙壁时约束够用、走到走廊中段轴向退化、再往前走到门厅位置约束恢复”的完整变化过程这种感觉非常直观比看一堆误差数字要直白得多。3.3 关键参数解读退化阈值、扫描窗口、特征体素launch文件里的参数不少但如果只抓重点我建议你优先理解这三个第一个是degen_direction_threshold这个参数决定特征值小到什么程度会被判定为退化方向。原作者给的默认值是0.05但实测下来这个值在不同数据集上需要调整。室内走廊这种环境0.02到0.03比较合适到了室外空旷广场0.08甚至0.1才能把明显退化识别出来。原因是室外点云密度低、噪声大结构张量的特征值整体都会偏小用室内阈值就会漏检。第二个是feature_voxel_size默认0.5米。这个参数直接影响结构张量计算的粒度。体素越小局部特征越精细但对噪声越敏感体素越大整体越平滑但可能会把转角、门框这种细小的高约束结构磨掉。我实测下来0.4到0.6之间是大部分场景的甜点区间短于0.3就开始抖大于0.8在室内分层场景容易丢细节。第三个是scan_context_window它控制的是退化检测的帧间平滑窗口。默认设成5意味着当前帧退化方向的判定会参考前5帧的结果做一次多数表决。这个设计是为了防止单帧噪声导致的退化方向跳变。注意这个窗口不要设太大否则退化状态切换会有明显的滞后车从走廊开进开阔大厅时前几帧还会沿用走廊状态的退化约束造成一段过渡期的定位抖动。3.4 对实时性的实测评估我在一台i7-12700H、16GB内存、没有独立GPU的笔记本上跑了实时数据流点云话题频率10Hz。预处理加特征提取大概每帧12到18毫秒退化检测和张量分解大约5毫秒GICP优化每帧20到30毫秒总耗时不超过55毫秒跑10Hz数据没有任何积压。也就是说这套算法对算力的要求属于中等水平在嵌入式平台比如NVIDIA Orin上应该有较大余量至少不会像很多深度学习配准方法那样离了GPU就没法活。不过要注意如果你输入的点云帧率是20Hz同时分辨率特别高比如128线雷达预处理耗时可能会翻倍到接近40毫秒此时建议先做一次体素滤波降采样或者把feature_voxel_size调大一点否则容易出现处理速度跟不上输入帧率的情况。4. 真实场景实测长走廊、隧道、雨后广场与封闭车库4.1 实验配置与评测指标ATE/RPE我手里没有他们论文里用的那套真值设备但有一个还算靠谱的测试环境一条长约120米的室内走廊、一个地下车库B2层、一段双向隧道以及一个雨后略有积水的广场。手持设备是一台Livox Mid-360外加一组消费级IMU真值由走廊两端和隧道中间布设的固定标记物提供用全站仪打点后换算出来。评价指标我主要看两项绝对轨迹误差ATE的中位数以及相对位姿误差RPE的漂移率。前者反映整个轨迹离真值有多远后者更关注局部每一小段的平滑度。这套方法在这几个场景里的表现让我最关注的不是它“多么完美”而是它在不同退化类型下的行为差异非常大一定得分开说。4.2 长直走廊最典型的退化场景120米长、宽3米、高2.6米的走廊中间没有门洞、没有岔路两侧墙壁上的消火栓和指示牌每隔20米才出现一个整体构图极度单调。这是最能体现这套方法优势的场景。我用纯GICP跑这段路程前60米轨迹基本正常后续40米开始出现明显的横向漂移到120米末端时横向误差约1.2米纵向误差约0.8米轨迹画出来已经抵到墙里了。换成这套方法后退化检测模块在进入走廊约8米后就开始报出“退化子空间维数1”并且在整段走廊内稳定地保持这个状态。判定的退化方向基本指向走廊轴向。在IMU参与融合的前提下末端横向误差降到0.15米以内纵向误差约0.3米。如果只开激光单传感器模式纵向误差会增大到约0.5米但还是能保证整条轨迹不穿墙。值得注意的是退化检测模块偶尔会在走廊中段把“退化方向数”误报成2原因是墙上的指示牌被激光打到后产生了一小片密集的高约束区域结构张量的特征值分布被局部干扰把原本的一个退化方向“拆”成了两个。这里是他们代码里scan_context_window这个平滑窗口起的作用5帧表决之后误报基本消失。4.3 隧道与半封闭环境隧道是另一种典型的退化场景而且比室内走廊更棘手一方面隧道内部结构更均匀墙面更平整另一方面隧道壁的材质往往是混凝土或瓷砖对激光的反射率一致性很高点云上的微小差异几乎全来自测量噪声而非几何变化。我在一条长约800米、单向两车道的城市隧道里做了测试。前300米表现不错退化方向检测稳定输出一个主导方向但进入中段后遇到一个问题隧道壁上每隔一段距离的灯架和管线支架会产生周期性重复纹理结构张量偶尔会把重复纹理误判成强约束方向导致退化检测在“单方向退化”和“识别到虚假约束”之间来回跳。结果就是位姿求解在个别帧会出现微小的抖动叠加到轨迹上表现为轻微的波浪状误差。针对这个情况我看代码里其实已经有一个“方向一致性检查”逻辑它会检查当前帧识别出的高约束方向与前几帧的高约束方向是否保持连续。如果两者夹角超过一定阈值就判定为重复纹理干扰强制把这些方向的约束降权。默认阈值是25度我在隧道数据里试了30度发现能减少一半的虚假切换但再调大就会漏掉真实的场景切换。这个参数值得根据自己的应用场景仔细试。4.4 开阔广场/雪地/水面无特征的极端情况空旷广场是比走廊更极端的退化类型它几乎在所有水平方向上都缺乏稳定的近距离几何约束唯一还能提供信息的是地面。这种情况下结构张量的三个特征值都会很小退化子空间维数会升到2甚至3。这套方法在广场场景里给我的感觉是它不会给你虚假的安全感。退化检测模块会非常诚实地告诉你“我现在在大部分方向上都是靠IMU在撑”位姿漂移情况也基本由IMU的零偏稳定性决定。我用消费级MPU6050级别的IMU在广场上跑了五分钟轨迹在水平面内缓慢漂移这是正常的换谁来做都一样但如果IMU质量稍好一些它的横向和纵向漂移能控制在用纯激光方案的1/3左右。至于雪地和水面本质上是反射问题而非几何问题。雪地的低纹理会导致点云局部“塌陷”水面会产生大量飞点和误匹配。这套方法的做法是在预处理阶段剔除掉强度值异常低的点和高曲率的孤立点因此在雪地和水边场景确实能减少不少误匹配但也不要指望它能完全解决这类问题的物理本质。5. 适用边界和调参避坑哪些场景不建议硬上5.1 算法失效的边界条件任何方法都有边界这套方法也不例外。首先是“完全对称闭合空间”。我之前在一个直径约6米的圆柱形竖井里做过实验四周墙面完全是同心圆结构没有任何其他特征。这种场景下旋转自由度也退化了结构张量三个方向特征值全部极小退化子空间维数等于3整个激光配准等于完全失去约束必须靠IMU和垂直方向的绝对测量比如气压计或测距仪来支撑这套方法本身无解。这是几何上的不可能不是算法的错。另一个失效场景是点云严重畸变时比如机器人高速转弯时激光雷达的扫描畸变没有做完整补偿。此时局部结构张量会被畸变拉歪误判出错误的退化方向然后错误的约束降权反而损害了原本有效的约束方向。我的建议是退化感知配准一定要配合运动补偿一起使用不能单独依赖扫描匹配。最后是极端稀疏点云。我用16线雷达在室外测过一次远端的点密度太低结构张量计算出的特征值方差极大退化检测结果很不稳定。这种情况下建议缩小有效扫描范围比如只使用30米以内的点云或者先做多帧累积再计算结构张量。5.2 参数调整的优先级与经验值如果你拿到这套代码不知道该从哪个参数开始调我的建议是严格按照下面这个顺序来第一先调feature_voxel_size。这个参数决定了整体行为的稳定性。小场景、精细结构多用0.3到0.5大场景、点云稀疏用0.6到1.0。第二再调degen_direction_threshold。具体做法是找一个已知的退化场景把阈值从默认值往两个方向各试几档看退化方向数量是否稳定。第三最后调scan_context_window和方向一致性阈值。这一步是为了平滑误报但如果你前面的阈值调得好这里基本不需要大动。几个常见的坑我先说在前面。第一个坑是有人把退化阈值调得太小导致系统几乎不报退化整个退化感知模块形同虚设定位精度和普通GICP没有任何区别第二个坑是有人把阈值调得过大系统每帧都在报全方向退化激光约束被过度降权位置全靠IMU撑结果IMU稍微有点温漂轨迹反而比不开退化处理还差。5.3 与FAST-LIO、LIO-SAM、Point-LIO等主流方案的关系很多人会问一个很自然的问题FAST-LIO、LIO-SAM这些框架不是已经能在不少场景下工作得很好吗这套方法是不是只是换了个花架子我自己跑下来的理解是它和这些框架不是替代关系而是“组件级”的增强关系。FAST-LIO的迭代误差状态卡尔曼滤波本身对退化有一定鲁棒性因为它把IMU预测和激光观测做了紧耦合某个方向激光观测弱时卡尔曼增益会自动降低对这个方向的信任。但它的这种鲁棒性是隐式的、基于协方差传播的并不显式知道“哪个方向退化了”这在持续退化场景下会产生统计意义上的先验置信度膨胀问题。这套方法的价值在于它把退化方向检测做成一个显式的模块能和FAST-LIO、LIO-SAM中的前端匹配部分无缝衔接。你可以把它输出的退化方向及权重作为额外的观测噪声矩阵调整量输入到IESKF的更新步里实现“显式退化感知紧耦合滤波”的组合。实验数据也能支撑这个观点。我在同样的走廊数据上给FAST-LIO加了显式退化方向约束后末端横向误差从0.5米降到了0.2米左右纵向误差提升更明显。也就是说这套方法更适合作为你已有SLAM系统里的一个增强模块来使用而不是要求你推倒现有系统重来。另外Point-LIO采用的基于原始点而非特征点的配准方式在高动态场景下有优势但它对退化场景的处理也基本依赖IMU主导。把这套退化检测的输出接到Point-LIO的噪声模型里逻辑上完全成立不过目前开源代码里还没有现成的桥接实现需要自己动手改一改。5.4 对代码本身的一点评价最后说点对代码工程质量的主观感受。这套开源代码整体的模块划分是比较清晰的退化检测、特征提取、配准优化、融合接口各自独立成包没有那种几百行代码塞在一个文件里的大泥球。但文档方面确实有些地方写得不够细尤其是自定义消息类型dalr_ros/DegeneracyInfo里的各个字段README里只给了一句话描述需要读源码才能理解每个字段的含义和单位。好消息是代码里的注释还算充分关键函数上面都有两三行的逻辑说明熟悉C和Eigen的同行读起来压力不大。问题排查上如果编译或者运行遇到问题建议先看看是不是ROS版本和PCL版本的兼容问题这类问题占了GitHub issue里的大半。其次是数据集的坐标系定义我曾经因为雷达和IMU的外参标定文件里一个平移分量正负号弄反导致退化方向检测出来的方向和实际完全相反定位轨迹从“不漂移”变成“反向飞”这种低级错误排查了整整一个下午。所以提醒各位跑之前务必先用一个简单的静态场景数据验证外参方向别偷懒直接上复杂场景。我个人的体会是激光雷达配准走到今天单点特征和匹配策略的天花板已经很明显了真正决定系统上限的恰恰是在“没有约束的时候怎么办”这种细节上。这套方法没有去发明一个全新的配准范式而是把一个很多人在论文里提过、但少有人认真做成开源工程的方向感知思路真正落地成了可复用的代码。这种工作比那些刷了一大堆数据集指标、但源码永远只给个空仓库的论文有价值得多。如果你也正在被退化场景折磨建议花一个周末把它跑通再对照着自己系统的问题去读那部分代码大概率会有值得借鉴的收获。