R-VoxelMap：通过递归平面拟合实现高精度体素建图的在线lidar里程计方法

文章：R-VoxelMap: Accurate Voxel Mapping with Recursive Plane Fitting for Online LiDAR Odometry

作者：Haobo Xi, Shiyong Zhang*, Qianli Dong, Yunze Tong, Songyang Wu, Jing Yuan, Xuebo Zhang

编辑：dianyunPCL

摘要

本文提出了一种新的体素建图方法 R-VoxelMap，通过一种几何驱动的递归平面拟合策略构建高精度体素地图，以提升在线 LiDAR 里程计的定位精度。传统的 VoxelMap 及其变体通常在单个体素内使用所有点进行平面拟合与验证，这种方式容易受到离群点影响，导致平面参数偏移、大尺度平面的过度分割，以及不同物理平面之间的错误合并。

为解决上述问题，R-VoxelMap 采用了一种基于离群点检测与复用（outlier detect-and-reuse）流程的几何驱动递归构建策略。具体而言，对于每一个体素，首先利用**随机采样一致性算法（RANSAC）**在分离离群点的同时拟合高精度平面；随后，将剩余的离群点传递到更深层的八叉树节点中进行递归处理，从而实现对环境的精细表示。此外本文设计了一种基于点分布的平面有效性检测算法，以防止不同物理平面之间的错误合并。大量在多种开源 LiDAR（及 LiDAR–惯性）同步定位与建图（SLAM）数据集上的实验结果表明，与当前最先进的方法相比，所提出的方法在保持相当计算效率和内存开销的同时，实现了更高的定位精度。相关代码将开源于https://github.com/NKU-MobFly-Robotics/R-VoxelMap.git

图 1. VoxelMap 的平面特征存在对离群点敏感的问题（c，中黄色标注的平面因周围离群点影响而发生错误偏移）、平面过度分割（d，白色框标出的平面特征被过度切分，导致较大的不确定性），以及错误的平面合并（d，红色标注区域显示了不同物理平面被错误合并，在非真实平面区域生成了大尺度平面结构）。这些问题都会降低定位精度。相比之下，本文提出的方法能够有效解决上述局限性。

主要贡献

本文提出 R-VoxelMap：一种用于高精度体素建图的几何驱动递归平面拟合策略。与其他变体方法不同，R-VoxelMap 并未采用传统的分裂–合并（split-and-merge）策略，而是在由粗到细的平面特征提取过程中，直接引入所提出的离群点检测与复用（outlier detect-and-reuse）流程，从而有效抑制离群点的影响。该方法在同时保证平面特征提取精度与多尺度几何信息保留的前提下，显著提升了里程计估计精度。本文的主要贡献总结如下：

1） 提出了一种新的平面特征体素地图结构及其构建方法 R-VoxelMap，该方法基于几何驱动的递归平面拟合进行构建。通过在每一层递归迭代中执行离群点检测与复用流程，R-VoxelMap 能够有效抑制离群点的干扰，实现更加精确的地图表示，并减少平面的过度分割问题。

2） 设计了一种基于点分布的平面有效性检测算法，通过将点云投影并聚类到 RANSAC 拟合的平面上，有效防止不同物理平面之间的错误合并。

3） 在多个开源 LiDAR（及 LiDAR–惯性）SLAM 数据集上进行了大量对比实验，实验结果表明，所提出的方法在状态估计精度方面优于现有的多种最先进方法。

主要内容

系统概述

R-VoxelMap 由一组体素构成，每个体素都关联一个概率平面模型。该方法被嵌入到迭代扩展卡尔曼滤波器（Iterated Extended Kalman Filter，IEKF）框架中用于位姿估计，整体系统架构如图 2 所示。在没有惯性测量单元（IMU）的情况下，所提出的方法也可以在恒速度模型下作为一个独立的 LiDAR 里程计系统运行。

图 2. 基于 R-VoxelMap 的 LiDAR 里程计系统整体框架。红色高亮模块表示 R-VoxelMap 的核心部分，即一种基于离群点检测与复用流程的几何驱动递归地图构建算法；蓝色模块则用于支持 R-VoxelMap 的基础功能。

R-VoxelMap 的底层数据结构是一个以位置为键的哈希表，其中每一个键对应一个表示低分辨率体素的八叉树。根据点云分布情况，每个体素会被自适应地细分为不同分辨率的子体素。与以往的 VoxelMap 方法不同，R-VoxelMap 允许八叉树中的任意节点（不仅限于叶子节点）存储点云数据和概率平面模型。每个体素内部的点通过链表结构进行存储，以支持高效的点划分与复用。

当接收到新的点云数据时，R-VoxelMap 会针对新观测到的体素构建新的八叉树，并通过基于 RANSAC 的高精度平面提取来降低离群点的影响、提升环境表示的准确性。检测出的离群点会在八叉树中被进一步细分，并进行递归处理。为防止不同物理平面的错误合并，系统引入了一种基于点分布的平面有效性检测方法，仅保留几何上一致的平面。对于已有的八叉树结构，R-VoxelMap 还支持基于链表点组织方式的高效八叉树更新与重构。

图3. R-VoxelMap构建（下半部分）与VoxelMap构建（上半部分）的对比。VoxelMap的平面参数在宽松阈值下对离群值敏感（左上），而严格阈值则会导致过分割问题（右上）。我们的递归方法采用离群值检测与再利用流程及平面有效性验证，有效缓解了这些问题

R-VoxelMap 的构建方法

为了解决传统 VoxelMap 在使用体素内所有点通过特征值分解进行平面拟合时，容易出现对离群点敏感以及平面过度分割的问题，R-VoxelMap 提出了一种基于离群点检测与复用流程的递归地图构建方法。

与 VoxelMap 类似，R-VoxelMap 使用世界坐标系下的点云来构建概率平面特征，并同时考虑测量不确定性和状态估计不确定性。以 LiDAR 里程计为例：对于一个在 LiDAR 坐标系下的激光点，其本身带有测量协方差；同时，里程计估计得到的传感器位姿也存在旋转和平移的不确定性。通过位姿变换，该点被转换到世界坐标系中，其对应的不确定性由原始点测量误差和位姿估计误差共同传播得到。

在地图结构构建阶段，R-VoxelMap 首先通过哈希表确定每一个世界坐标系下的点所属的低分辨率体素。对于每一个低分辨率体素，当其内部点数满足初始化条件时，系统会通过所提出的递归构建流程生成一个八叉树结构（见算法 1）。

平面有效性检测

尽管 RANSAC 能够有效剔除离群点，但在某些情况下仍可能错误地将来自不同平面的点合并到同一个平面中，尤其是在多个平面法向量相近但并不完全一致时。这种错误合并主要会引发两类问题：

• 第一，来自其他平面的点会对平面参数估计产生偏置，导致平面参数不准确，其对应的不确定性估计也不可靠；
• 第二，在匹配阶段，里程计系统可能会错误地假设在实际为空的空间中存在平面（例如图 4(a) 中两个点簇之间的空隙），从而产生错误的点到平面匹配。

图 4. 基于点分布的平面有效性检测示意图。（a）从 RANSAC 的结果中获取最佳候选平面；（b）以固定分辨率将该平面离散为二维网格，将内点投影到平面上，并统计每个网格中的点数；（c）对被占据的网格进行聚类，并计算每个聚类中包含的点的总数量；（d）选择最优聚类，并基于该聚类内的点重新拟合平面。

为了解决上述问题，R-VoxelMap 在 RANSAC 之后引入了平面有效性检测步骤，对内点集合进行进一步筛选，以剔除潜在的错误内点。

地图更新与八叉树重构

由于八叉树的构建需要访问体素中的所有点，因此在每接收一个新点时都重新构建八叉树是低效的。为此，R-VoxelMap 采用了一种增量式地图更新策略。

当一个新点被分配到某个已有的八叉树中时，系统会在该八叉树内选择距离最近的平面作为匹配候选平面，并以增量方式更新平面参数。具体做法是：基于当前平面所包含的点数，对平面中心点的位置进行加权更新，同时将新点的信息累积到用于描述点分布的统计量中。随后，系统根据更新后的点分布重新评估该平面的几何特性。如果更新后得到的平面仍然满足预设的平面质量判定条件，则该新点被接受，并用于更新当前平面的参数；否则，该点会被拒绝作为平面内点，并被存储到当前叶子节点的非平面点集合中，以便在后续递归或重构过程中再次利用。然而，仅依赖初始八叉树构建后的增量更新，仍可能引入误差。这是因为在系统运行早期，点云通常较为稀疏，难以充分表达环境结构，从而导致八叉树结构和平面参数的初始化不够理想。此外，随着传感器视角的变化，已经建图的体素可能会获得新的环境信息，而这些信息并不会立即反映在当前的八叉树结构中。

为了解决上述问题，R-VoxelMap 会在累计足够多的新点之后，周期性地对八叉树进行重构。得益于其基于链表的数据存储方式，系统可以通过拼接链表的方式高效访问八叉树中的所有点，避免了传统基于向量存储方式所带来的数据拷贝时间和额外内存开销。在大规模环境中，为防止地图占用过多内存，R-VoxelMap 还引入了一种基于最近最少使用（LRU）策略的缓存机制。该缓存通过哈希表和链表实现，并以低分辨率体素作为基本管理单元。每次地图更新后，被访问或更新的体素会被移动到缓存队列的前端，而超过缓存容量限制的体素则会被逐出，从而有效控制整体内存使用。

用于状态估计的点到平面匹配

在扫描到地图的匹配过程中，系统会将当前点所属低分辨率体素中的所有平面作为候选匹配平面，并按照 VoxelMap 的方式构建点到平面的距离约束。

对于每一个点与候选平面的组合，系统会计算该点到平面的有符号距离，并同时考虑平面参数不确定性和点位置不确定性。通过一阶近似的方式，将该距离建模为一个零均值的随机变量，其方差由平面参数的不确定性和点位姿不确定性共同决定。基于该距离及其不确定性，系统进一步计算该点属于当前平面的概率。在所有满足距离阈值约束的候选平面中，具有最高匹配概率的平面将被选为该点的最终匹配平面。所有最终的点到平面匹配结果都会与先验信息进行融合，并通过最大后验估计（MAP）推断的方式完成系统状态（如位姿）的估计。

实验结果

为验证所提出方法的有效性，我们在多个数据集上进行了实验，并将该方法与当前多种最先进算法进行了对比。这些数据集的主要信息汇总于下表中。

对比方法包括 Fast-LIO2、Faster-LIO 、Fast-GICP/IG-LIO、KISS-ICP、VoxelMap以及 C3P-VoxelMap。这些 LiDAR–惯性里程计（LIO）系统在精度和鲁棒性方面表现优异，已在大量研究中得到广泛应用。

其中Fast-LIO2 和 Faster-LIO 分别采用 ik-d tree 与 基于体素的地图结构 进行点云建图；GICP 通过高斯分布建模局部点云几何结构，并利用马氏距离进行分布到分布的匹配；IG-LIO 将 GICP 融合进 IEKF 框架，将环境离散为体素并在体素内使用高斯模型描述点分布；KISS-ICP 通过自适应数据关联阈值实现精确且鲁棒的配准；VoxelMap 及其变体则使用概率平面表示环境，并通过点到平面的残差进行匹配。

为保证对比公平性，所有里程计方法均采用相同的点云预处理流程，并使用各自的默认参数。对于统一参数设置（如体素降采样分辨率与最大迭代次数），在所有方法中保持一致：

• 一般数据集：体素分辨率 0.5 m，最大迭代次数 3；
• 退化走廊序列：体素分辨率 0.1 m，最大迭代次数 5。

对于 VoxelMap 及其所有变体（包括本文方法），统一设置体素相关参数：一般数据集的最大体素尺寸为 3 m，退化走廊数据集为 0.5 m，所有数据集的最大八叉树层级均设为 4。所有实验均在一台配备 Intel i9-13900HX CPU 与 32GB 内存 的笔记本电脑上完成。

（1）精度显著提升：KITTI 上平均 ATE 提升超过 20%（表 II），在多条序列上取得最优结果

（2）退化环境优势明显：在走廊退化场景和非结构化户外环境中（表 III、IV，图 5），相比 VoxelMap 更稳定、漂移更小。

图 5.(a) 与 (b) 为在 M3DGR 数据集（corridor2 序列）上，本文提出的方法与 VoxelMap 的建图结果对比。(c) 与 (d) 为在 AVIA 数据集（avia1 序列）上，本文提出的方法与 VoxelMap 的端到端误差对比。结果表明，所提出的方法在建图质量和定位精度方面均优于 VoxelMap。

（3）效率与内存友好：在精度提升的同时，运行时间和内存占用不增反降（表 V、VI）

（4）鲁棒性强：在不同随机种子下结果稳定（表 VII），验证了方法的工程可靠性

总结

本文提出了一种新的高精度 LiDAR 里程计体素建图方法 R-VoxelMap。与 VoxelMap 及其变体不同，R-VoxelMap 在平面提取阶段并不直接使用体素内的所有点进行平面拟合，而是采用一种以几何约束为核心的递归式地图构建框架，结合离群点检测与复用流程以及基于点分布的平面有效性判定方法。该方法能够有效减弱离群点对平面拟合的影响，并避免将不同物理平面错误地合并在一起。在多种数据集上的实验结果表明，在同时包含结构化和非结构化传感环境的情况下，与当前先进方法相比，R-VoxelMap 在不引入额外计算时间或内存开销的前提下，显著提升了定位精度。

尽管所提出的方法在抑制离群点影响等方面取得了良好效果，但基于 VoxelMap 的整体框架仍然对多个参数较为敏感。未来的工作将进一步研究降低系统参数敏感性的策略，并提升其在多样化环境中的定位精度。

以上内容如有错误请留言评论，欢迎指正交流。如有侵权，请联系删除