从粒子到特征：面向鲁棒SLAM的不确定性引导几何推理

《ISPRS JOURNAL OF PHOTOGRAMMETRY AND REMOTE SENSING》：From particles to features: Uncertainty-guided geometric reasoning for robust SLAM

　　

摘要

同时定位与建图（SLAM）系统必须在不确定性下对特征匹配和回环闭合决策做出承诺，然而错误的承诺会级联为灾难性的建图失败。传统粒子滤波器解决了状态不确定性问题，但特征级处理仍是确定性的，这种不对称性限制了鲁棒性。我们提出了粒子引导特征融合（Particle-Guided Feature Fusion, PGFF），该框架颠覆了粒子的经典角色：粒子不再用于采样机器人位姿，而是通过特征空间传播以计算重要性权重，为几何证据积累提供信息。这一范式转变实现了三项创新：（1）通过局部特征值分析和信息论散度识别信息量丰富的特征的几何意外先验；（2）粒子根据空间和几何一致性对匹配可靠性进行投票的自适应特征加权；（3）推迟决策直至证据充分积累的多假设回环闭合管理。我们将PGFF集成到紧耦合的激光雷达-惯性里程计系统中，并在NCLT长期自主导航基准和VBR数据集上进行了评估，两个数据集均包含动态物体、几何退化性和感知歧义性。与FAST-LIO2相比，PGFF将绝对轨迹误差降低了23.5%；与LIO-SAM相比降低了29.8%，相对于最新的退化感知方法也有显著改进。将PGFF与渐进非凸优化（Graduated Non-Convexity, GNC）结合可带来进一步的加性改进，证实了匹配前和匹配后的鲁棒性针对的是互补的失效模式。PGFF使迭代扩展卡尔曼滤波（IEKF）收敛速度快10.1%，仅增加0.7%的总运行时间开销。多假设回环闭合将多个地点识别前端的误报率降低了58%，同时保持94%的召回率。这些结果表明，由不确定性引导的特征选择和证据积累，而不仅仅是状态估计，对可靠的长期自主导航至关重要，这提示我们应该从根本上重新审视滤波与感知之间的边界。我们的实现代码可在https://github.com/wencabrel/PGFF-SLAM获取。

引言

一辆配送机器人在仓库走廊中行驶，遭遇了一种常见却灾难性的场景：两侧对称的墙壁、重复的结构元素，以及一个与三个不同位置同样匹配的远距离回环闭合候选目标。机器人的SLAM系统被迫立即做出决策，选择了错误的匹配。几分钟内，累积的漂移就使地图变得不可用。这种失效模式——在几何歧义下过早承诺——仍然是可靠长期自主导航的根本障碍（Cadena等人，2016）。

其根本原因在于几乎所有现代SLAM系统中存在的一种架构性不对称。状态估计管线，从扩展卡尔曼滤波到因子图（Dellaert和Kaess，2017），都通过概率分布、协方差矩阵和假设空间显式地传播不确定性。紧耦合的激光雷达-惯性里程计系统，如FAST-LIO2（Xu等人，2022）和LIO-SAM（Shan等人，2020），通过将IMU预积分（Forster等人，2017）与迭代状态估计融合，实现了卓越的精度。然而，为这些估计器提供输入的特征提取和匹配阶段却是确定性的：每个激光雷达点都受到同等对待，每个回环闭合候选目标都要求立即接受或拒绝，几何推理过程无法访问流经系统其他部分的不确定性信息。这种不对称性意味着状态估计器仔细地推理机器人可能在哪里，却不推理机器人应该注意什么。

传统方法通过离群值剔除机制——随机采样一致（RANSAC，Fischler和Bolles，1981）、M估计器（Huber，1964）和启发式距离阈值——作为特征提取后的后处理步骤来解决这一问题。尽管对随机噪声有效，但这些方法在离群值具有几何一致性的结构化环境中会系统性失效：以编队方式移动的动态物体、对称的建筑元素，或在退化走廊中所有点看起来同样（不）有信息量。如Scan Context（Kim和Kim，2018）等地点识别描述符能够实现回环闭合检测，但仍要求立即做出承诺决策。根本限制在于，这些机制无法访问下游状态估计中发生的概率推理。

我们提出了粒子引导特征融合（PGFF），该框架通过颠覆粒子在SLAM中的经典角色来解决这种不对称性。不再使用粒子来采样机器人位姿——这是由FastSLAM（Montemerlo等人，2002）及其Rao-Blackwell化变体（Grisetti等人，2007）确立的方法——PGFF通过特征空间传播粒子以计算重要性权重，为几何证据积累提供信息。这种颠覆使得不确定性信息能够双向流动：从状态估计回流到特征选择，形成闭环，使系统对其状态的置信度能够指导它应该信任哪些特征。图1说明了这一根本区别：传统管线将特征提取视为确定性预处理步骤，而PGFF在状态不确定性和特征加权之间建立了双向信息流。

我们在本文中使用的"几何推理"一词指代特定类别的操作：对竞争性几何假设的评估、跨观测的证据积累，以及推迟决策直至后验置信度超过阈值。这一用法比语言模型中的思维链推理范围更窄，并不声称具有符号推理或组合推理能力。单独来看，PGFF的特征加权组件（第3.2-3.3节）是基于信息论和粒子共识的合理加权机制。将框架提升到超越单纯加权的是多假设回环闭合模块（第3.4节），该模块维持竞争性位姿假设，逐帧积累贝叶斯证据（公式(22)），并推迟决策直至后验概率比满足阈值。这一证据积累-决策循环，结合状态不确定性与特征选择之间的双向反馈，就是我们在全文中所称的"几何推理"。

我们承认，此处描述的操作——多假设评估、贝叶斯证据积累和基于后验的推迟决策——在概率机器人领域已有成熟基础，更准确地说是数学意义上的贝叶斯推理。PGFF是该范式的一种实例化，而非新的推理形式。我们的贡献不是推理框架本身，而是一种特定的耦合设计：状态不确定性与匹配前特征选择之间的双向反馈，结合基于激光雷达几何残差的描述符无关多假设回环闭合模块。我们在标题和章节标题中保留"几何推理"一词，作为这种耦合的简洁描述符，但在技术章节中使用更精确的贝叶斯推理和证据积累语言。

这一范式转变实现了三项技术贡献：

1. 几何意外先验。我们基于局部特征值分析（Weinmann等人，2015）推导了一种信息论特征重要性度量。其观测几何偏离邻域预期的点——通过Kullback-Leibler散度量化——获得更高权重，自动强调角点、边缘和结构不连续区域，同时抑制冗余的平面区域。

2. 基于粒子的自适应加权。分布在特征空间中的粒子对匹配可靠性进行投票。粒子间高一致性表明匹配可靠；分歧则表明几何歧义需要谨慎处理。该机制提供了有理有据的离群值处理，无需手动调参阈值，是对现有鲁棒估计技术的补充而非替代。

3. 多假设回环闭合。PGFF不立即对回环闭合候选做出承诺，而是维持竞争性假设并使用贝叶斯更新跨帧积累证据。仅当后验概率比超过决策阈值时才做出承诺，从而防止在感知歧义环境中因过早闭合而产生的灾难性失效（Lowry等人，2016）。

我们将PGFF集成到基于迭代扩展卡尔曼滤波（IEKF）公式（Xu和Zhang，2021）构建的紧耦合激光雷达-惯性里程计系统中。在NCLT长期自主导航基准（Carlevaris-Bianco等人，2016）和一个具有挑战性的43GB城市校园数据集上的评估表明，PGFF将绝对轨迹误差降低了23.5%（与最先进基准FAST-LIO2和LIO-SAM相比），同时状态估计速度快10.1%。消融研究证实每个组件都对整体性能有实质性贡献。

除实验改进外，我们的结果还带来了更广泛的启示：滤波与特征提取之间的边界——长期以来被视为SLAM管线中固定的架构决策（Stachniss等人，2016）——应该被重新审视。PGFF表明，具有不确定性感知能力的感知，而不仅仅是具有不确定性感知能力的估计，对于鲁棒的自主导航至关重要。

章节摘要

相关工作

我们将PGFF定位在三个交叉研究领域：激光雷达-惯性里程计与状态估计、特征选择与鲁棒匹配、以及感知歧义下的回环闭合。

方法论

本节介绍粒子引导特征融合（PGFF）的技术细节。我们首先概述系统架构和问题表述（第3.1节），然后描述基于信息论特征分析的几何意外先验（第3.2节）、基于粒子共识的自适应加权机制（第3.3节）、多假设回环闭合策略（第3.4节）、与迭代扩展卡尔曼滤波的集成（第3.5节），以及实现细节。

实验评估

我们通过大规模真实数据集上的全面实验评估所提出的粒子引导特征融合（PGFF）框架。我们的评估旨在回答四个研究问题：（RQ1）PGFF是否相比最先进方法改进了里程计精度，特别是在几何挑战性环境中？（RQ2）多假设回环闭合是否能在保持高召回率的同时减少感知歧义场景中的误报？（RQ3）PGFF的各个组件如何...

讨论

实验结果表明PGFF相比最先进方法实现了持续改进，但仍有若干方面值得深入探讨。

结论

我们提出了粒子引导特征融合（PGFF），该框架通过颠覆粒子滤波器的经典角色——将粒子通过特征空间而非位姿空间传播——在激光雷达-惯性里程计中建立了状态估计与特征选择之间的双向信息流。三项贡献支撑了这一成果：基于KL散度的几何意外先验、基于粒子共识投票的匹配可靠性评估、以及多假设回环闭合机制...

CRediT作者贡献声明

Cabrel Wouladje：写作–原创草稿，可视化，验证，软件，方法论，形式分析，概念构思。Golden Tendekai Mumanikidzwa：写作–原创草稿，可视化，验证，方法论，调查研究。Huiying Xu：写作–审阅与编辑，监督，资源，资金获取。Hongbo Li：写作–审阅与编辑，监督，资源，资金获取。Mantang Liu：写作–审阅与编辑，调查研究，数据整理。Wenjie Wang：

利益冲突声明

作者声明他们不存在已知的竞争经济利益或个人关系，这些可能影响本文所报告的工作。

致谢

本工作得到了国家自然科学基金（62376252）和浙江省领军人才计划（2025C02025）的资助。

Cabrel Wouladje | Golden Tendekai Mumanikidzwa | Huiying Xu | Hongbo Li | Mantang Liu | Wenjie Wang | Zhendong Chen | Wenzhe Tan | Chao Chen | Balong Wang | Zhenglong Wang | Xinzhong Zhu

浙江师范大学智能教育及应用浙江省重点实验室，浙江金华 321004，中国