SuperLoc：通过预测匹配风险实现稳健激光雷达和惯导定位

文章：SuperLoc: The Key to Robust LiDAR-Inertial Localization Lies in Predicting Alignment Risks

作者：Shibo Zhao , Honghao Zhu , Yuanjun Gao , Beomsoo Kim , Yuheng Qiu, Aaron M. Johnson , and Sebastian Scherer

编辑：点云PCL

主页：https://superodometry.com/superloc

欢迎各位加入知识星球，获取PDF论文，欢迎转发朋友圈。文章仅做学术分享，如有侵权联系删文。

从技术角度观察行业发展，努力跟上时代的步伐。公众号致力于点云处理，SLAM，三维视觉，具身智能，自动驾驶等领域相关内容的干货分享，欢迎各位加入，有兴趣的可联系dianyunpcl@163.com。文章未申请原创，未经过本人允许请勿转载，有意转载联系微信920177957。

摘要

基于地图的激光雷达定位技术虽在自主系统中广泛应用，但在特征缺失的退化环境中面临严峻挑战。本文提出SuperLoc—一种鲁棒激光雷达定位方案，它能有效解决现有方法的关键局限性。该方案采用创新的预测式匹配风险评估技术，可在优化前实现潜在故障的早期检测与规避，从而在长廊、隧道、洞穴等挑战性场景中显著提升性能。与依赖后优化分析和启发式阈值的现有退化缓解算法不同，SuperLoc能够直接评估原始传感器数据的可定位性。实验结果表明，该方案在多种退化环境中的性能显著优于现有先进方法，定位精度提升54%且具有更优的鲁棒性。为促进相关研究，我们公开了实现代码以及来自八种挑战性场景的数据集。

主要贡献

基于地图的激光雷达定位是自动驾驶、无人机巡检、搜救任务等自主系统中广泛使用的技术。迭代最近点算法通过匹配对应点或特征、计算相对位姿变换、构建详细场景地图，成为在三维先验地图中估计机器人位姿的主流方法。尽管基于激光雷达的定位与建图算法已取得一定成功，但实现鲁棒定位仍面临以下关键挑战：

退化场景中的可靠性不足：当前先进的激光雷达定位系统虽能实现长距离低漂移，但在走廊、隧道、洞穴、平面区域等退化场景中容易失效。这些环境因几何特征缺失导致优化过程约束不足。

延迟响应导致的配准失败：现有退化缓解算法基于海森矩阵进行解重映射，仅能沿约束良好方向更新位姿。但由于退化期间约束不足，这类方法往往难以及时阻止即将发生的失败，且存在数值不稳定性。

针对这些局限，本文提出SuperLoc——一种鲁棒激光雷达定位方案，通过将先前里程计系统扩展至基于地图的定位场景。本方法具有三大创新：

1. 预测式匹配风险评估：本工作核心贡献在于提出一种既能预测点云间对齐失败风险，又能在机器人运行期间评估当前扫描置信度的方法，且有效规避退化时的数值不稳定性
2. 主动式传感器融合：通过在前端估计可观测性并将其融入传感器融合，改变了传统被动融合模式。这使得系统能在最佳时机主动引入来自备选里程计的位姿先验正则化，该主动退化缓解策略将鲁棒性与准确度显著提升54%
3. 开源贡献：公开了源码及包含8种挑战性环境的数据集（含真实地图），以推动极端场景下定位能力的突破 最终在发现退化问题的本质并非异常值干扰，而是源于约束不足。在发生失败前提供额外约束可显著提升定位算法的鲁棒性，这揭示了在优化之前（而非优化过程中或完成后）预测对齐风险或退化现象的重要性。

图1：SuperLoc作为开源激光雷达-惯性定位系统，不仅能预测对齐风险、评估扫描数据的可观测性，更能在故障发生前主动融合其他里程计源的位姿先验。整个流程无需依赖启发式阈值调整即可检测退化情况，并已在洞穴、长廊、平坦开阔区域、楼梯等多种挑战性环境中完成验证。

主要内容

概述

多数传感器融合方法通过海森矩阵的特征值来量化状态估计不确定性，其假设即使存在退化情况，估计结果仍服从高斯分布。然而当某些状态或路标点变得不可观测时，该假设即告失效。此类方法存在四个关键局限：

（一）特征值分析位于优化流程末端，无法提供及时修正；

（二）需针对不同环境设置差异化阈值，削弱了普适性；

（三）基于海森矩阵的分析可能引发数值不稳定，影响可靠性；

（四）难以平衡多模态数据间的权重关系。本文重点阐释预测对齐风险、可观测性估计及主动传感器融合模块，系统框架如图2所示。

图2：系统架构图。红色框线标注了本研究的核心创新模块，该流程始于激光雷达与IMU测量数据，从中提取点、平面及线特征。通过点-平面对应关系进行主成分分析（PCA），确定主方向与法向量方向，进而进行可观测性分析。各状态方向的置信度值用于生成可观测性扫描图，将位姿先验融入传感器融合系统以预防退化。

预测对齐风险

为了克服现有方法只能在优化失败后才进行补救的局限性，SuperLoc的核心创新在于能够在里程计发生故障之前，就提前探测到对齐失败的风险。

其基本思路是：在尝试将两个点云进行配准（对齐）时，我们使用“点-平面”对应关系。这意味着，对于第一个点云中的每一个点，我们在第二个点云中找到一个对应的点，并且这个对应点带有一个法向量（即它所属局部平面的垂直方向）。

我们的目标是找到一个最佳的刚体变换（包括旋转和平移），使得第一个点云中的点在经过变换后，能尽可能地贴近第二个点云中对应点所在的平面。这个“贴近”的程度，就是所谓的对齐误差。

关键在于，我们无需等到完整的迭代优化计算完成，才能知道结果的好坏。通过深入分析这些“点-平面”约束关系的数学本质，我们发现了一个重要规律：仅凭原始点云数据（点的位置和对应平面的法向量），我们就可以在优化开始之前，快速预判出在哪个运动方向（例如X、Y、Z轴的移动或旋转）上，约束是充分或不足的。

状态估计的可观测性评估

为了解决不同环境需要不同阈值以及基于海森矩阵的方法存在数值不稳定性这两个问题，需要为状态估计构建一个更可靠的协方差矩阵。核心思路是：一个条件良好的问题，其约束会均匀地分布在所有运动方向上；而一个病态问题，则在某些特定的“退化方向”上缺乏足够的约束。因此能识别出这些约束的方向，就能通过统计方法计算出每个方向上的约束数量，并由此预测出哪些方向是不可观测的。具体而言，利用从点云数据中推导出的全局约束信息。对于点云中的每一个“点-平面对”，我们通过分析，将其归类到对其约束最强的那个运动维度上（即X、Y、Z平移，或横滚、俯仰、偏航旋转）。这就像为每一个数据点赋予了一个“可观测性标签”。接着对一整帧扫描数据中所有点的这些标签进行统计分析。通过统计在每个运动维度上分别有多少个“标签”，我们就能了解约束在不同方向上的分布情况。基于这个分布，我们计算出平移和旋转各自由度的归一化置信度指标。这些指标量化了在每一个特定运动方向上，约束的相对强度。

这些置信度指标随后被用来构建一个对角协方差矩阵。这个矩阵的关键优势在于：它评估的是某个方向上的约束数量相对于总约束数量的比例。 这是一种相对比较的方法，其基本假设是：在结构良好的环境中，可观测性本应均匀分布在所有方向上。因此，我们的方法不再需要为不同场景手动调整启发式阈值，因为其输出的是一个范围在[0,1]之间的相对值。经过在各种环境中的实验验证，我们发现当这个协方差矩阵中的任何一个元素值低于0.2时，该方向就极有可能发生了退化。重要的是，这个0.2的阈值在绝大多数场景中都是通用的，通常无需根据具体环境进行调整。

主动传感器融合

传统的传感器融合方法通常被动地融合所有测量数据，并依赖鲁棒核函数来减轻不良影响。为了克服难以平衡多模态数据权重（局限iv）的难题，我们提出了一种更具前瞻性的策略：退化感知的传感器融合。该方法能主动引入具有适当权重的额外约束，尤其是在已识别出的退化方向上。我们的系统不仅能识别退化方向，还能利用前面提出的协方差矩阵 Σcov 来精确决定在这些方向上需要施加的约束权重。

实验结果

本文所提出方法的可行性及其在真实场景中的性能表现。主要包含三个阶段：

• 对齐风险预测：我们在可能失效的扫描场景（洞穴、隧道、走廊）中评估本方法，并通过协方差矩阵验证对齐风险预测能力
• 主动传感器融合：基于协方差分析，我们融合先验里程计以缓解退化问题，并证明其能提升地图精度与鲁棒性
• 里程计精度评估：我们在挑战性环境中全面评估本方法的精度与鲁棒性

对齐风险预测

为评估方法有效性，在洞穴、多层楼宇和长廊三类挑战性环境中进行了分析。针对每种场景，我们聚合约10秒的点云数据，并将置信度指标投影至三维空间，生成可观测性扫描图。在洞穴与长廊环境中，平台的前向运动显示出X方向（前进方向）的置信值明显低于其他维度。这一现象在可观测性扫描图中得到直观呈现（图3），图中红色点的稀缺表明X方向对应关系不足。可观测性扫描图清晰揭示了特定方向的退化情况：洞穴与长廊环境中红色点（X方向）稀缺，而多层楼宇环境中蓝色点（Z方向）稀少。

图3：实时对齐风险分析。从上至下分别为：洞穴、多层楼宇和长廊环境

主动传感器融合

本节展示如何及时利用置信度估计融合辅助里程计，以增强挑战性环境中的定位鲁棒性。我们通过手持设备、腿式机器人和地面车辆在三类退化环境中开展实地实验：

i) 狭窄自相似的洞穴环境；

ii) 密闭多层楼宇环境；

iii) 含平坦开阔区域的长廊。

为评估定位算法精度，我们采用FARO扫描仪建立精度误差小于2毫米的地面实况地图。每次实验均通过地图分析量化定位生成地图相对于地面实况的局内点与异常点比例，并以10厘米阈值界定局内点与异常点。各场景中均将本算法与HDL定位和FASTLIO定位进行对比，所有定位算法均获得统一提供的地面实况地图与初始位姿。

图4：洞穴环境中的定位表现。本方法展现出卓越性能，异常值率显著降低至0.50%，较次优方法提升达6倍。底部粉色高亮区域显示X方向置信度较低，此时我们的方法会主动融合X方向的备用里程计数据。

2) 多层楼宇实验：使用腿式机器人在废弃医院中评估算法性能。该场景因楼梯等狭窄空间限制传感器视野而带来特殊挑战。此外，如图1所示，即使微小的高程估计误差也会导致楼层间严重错位。如表II所示，本方法在此挑战性环境中表现优异，在270米行程中异常点率仅8.03%，较次优方法（58.49%）提升7.2倍。图5凸显了本方法的关键特性：底部蓝色虚线框标示Z方向低置信度区域，在此区域本方法主动融合Z维度的辅助里程计数据。

图5：多层楼宇环境中的定位表现。本方法的异常值数量明显减少，异常值率为8.03%，较次优方法提升达7.2倍。底部蓝色虚线框标示区域显示Z方向置信度较低，此时我们的方法会主动融合Z方向的备用里程计数据。

3) 长廊实验：我们采用遥控车进行长廊实验，车辆以最高5米/秒线速度与57°/秒角速度在室内走廊与室外开阔区域行进。长廊因重复对称结构及明显几何特征缺失，对LiDAR系统构成重大挑战（图6）。尽管如此，本算法仍实现较次优方法7.8倍的提升（表II）。这一改进源于对齐风险预测、退化方向识别以及在传感器融合中分配合适置信度权重的能力。值得注意的是，当环境变化时，本方法无需调整阈值即可检测点云退化。

里程计精度评估

为进步验证位姿估计精度，我们在SubT-MRS数据集上使用自研里程计系统进行绝对轨迹误差分析。该数据集涵盖传感器退化、剧烈运动与极端天气等挑战性环境。数据集八个序列分为两组测试：几何退化（仅点云退化）与混合退化（点云与视觉同时退化）。对比系统的ATE结果取自公开SLAM挑战赛。参赛团队被允许使用闭环检测技术进行后处理精度提升。尽管如此，本方法仍以0.271的平均ATE（未使用闭环检测）超越次优方法54%（表III）。

里程计鲁棒性指标评估

理想评估指标应兼顾精度与完整性。虽然ATE能有效评估轨迹精度，但在完整性考量上存在不足。为弥补此缺陷，我们采用鲁棒性指标来综合考量两方面因素。表IV展示了各场景与算法的综合指标报告。图7展示了所有方法的鲁棒性指标汇总，本里程计方法对应的蓝线曲线在几乎所有范围内均保持最高水平。

图7：鲁棒性指标对比。

运行时效性能

本方法在AMD Ryzen 7 3700X 8核CPU上实现每秒22帧的处理速率，单帧处理耗时45毫秒。

总结

本研究提出了一种基于地图的激光雷达定位方案，该方案能在优化过程开始前预判并规避定位失败风险，同时无需针对不同环境进行启发式阈值调整即可提供可靠的可观测性评估。与现有方法相比，本方案以显著优势超越次优算法，实现54%的精度提升。在后续工作中，我们计划将可观测性评估融入全局扫描配准框架，以更好地解决绑架场景下的重定位问题。