当前位置：首页 > ops >正文

论文Review 激光SLAM VoxelMap | RAL 2022 港大MARS出品！| 经典平面特征体素激光SLAM

ops 2025/8/15 17:24:56

基本信息

题目：Efficient and Probabilistic Adaptive Voxel Mapping for Accurate Online LiDAR Odometry

来源：IEEE ROBOTICS AND AUTOMATION LETTERS 2022

学校：HKU-MARS

是否开源：https://github.com/hku-mars/VoxelMap

摘要：体素激光SLAM

本文提出了一种高效的、概率自适应的LiDAR里程计体素映射方法。映射是体素的集合；每个特征包含一个平面特征，能够实现环境的概率表示和新的LiDAR扫描的精确配准。我们进一步分析了由粗到精的体素映射的需求，然后使用一种由哈希表和八叉树组织的新型体素映射来高效地构建和更新映射。我们将所提出的体素图应用于迭代扩展卡尔曼滤波器，并构造了一个用于位姿估计的最大后验概率问题。在公开的KITTI数据集上的实验表明，与其他先进方法相比，我们的方法具有较高的准确率和效率。利用固态激光雷达和非重复扫描激光雷达在室内和非结构化室外环境下的实验进一步验证了我们的建图方法对不同环境和激光雷达扫描模式。

该方法提出了一种概率体素地图，用于LiDAR里程计，核心创新是：

概率平面表示：显式建模点云和平面不确定性（测量+位姿误差），使用协方差传播，提高匹配鲁棒性。
粗到细地图构建：自适应体素（Hash+octree），从稀疏点建粗地图，渐细化；高效更新（不确定性收敛后丢历史点，避免计算爆炸）。
点到平面匹配：基于3σ不确定性阈值，过滤虚假匹配。
状态估计：EKF融合先验和观测，考虑所有不确定性。优势：适用于稀疏/稠密点云，低计算成本，鲁棒于噪声。适用于户外环境，支持LiDAR-only或LiDAR-inertial。

Introduction

固态激光雷达广泛应用在自动驾驶车辆，无人机和三维移动测绘设备。

LiDAR里程计的一个基本要求是对环境(地图)的正确表示，并且能够有效地维护和注册新的点。当注册一个新的扫描时，扫描中的每个点通过迭代最近点( ICP ) [ 11 ]或其变体(如广义ICP )注册到地图中由几个最近点拟合的小平面上。本工作发展于fastlio2.

激光雷达测量的不确定性导致地图的一致性受到影响。ICP方法中，平面是由最近邻的点云拟合而来，并且认为新的点云也会落在该平面，但是该拟合平面并非真实平面。虽然在拟合平面(以及后续的点配准)中包含这些噪声是可能的，但它是不太可靠的(因为为了避免伪平面,最近点通常较少)和耗时的(因为在配准过程中,如果最近点发生变化,则需要重新计算不确定度)。

考虑地图的不确定性需要对环境中的显著特征(例如,平面)进行明确的参数化，在不同的激光雷达扫描中跟踪这些特征，并估计这些特征参数及其不确定性。这些任务具有挑战性，因为各种可能的环境包含不同大小的特征。由于LiDAR测量中普遍存在由粗到精的现象，使得问题变得更加复杂。例如，由于LiDAR分辨率、扫描类型的不同，以及空间中点的分布极不均匀，扫描或地图中的点在空间中的密度往往有很大的差异。由于LiDAR传感器的顺序扫描特性，点密度也随时间变化。

为了解决上述挑战，本文提出了一种新颖的在线自适应体素建图方法，通过构造不同大小的体素来适应环境结构和点密度的变化。一个体素追踪一个显著特征(我们在这篇信中使用了平面)。根据接收到的点在体素内重复估计特征参数和不确定性。参数化的特征使得我们可以准确地考虑当前点和地图的不确定性。贡献如下：

提出了一种自适应尺寸和由粗到精的体素构造方法，该方法能够适应结构多变的环境，并且对LiDAR点云的稀疏性和不规则性具有鲁棒性。自适应体素图以八叉树哈希数据结构组织，以提高体素构建、更新和查询的效率。
真实的概率地图表示，其中包含在体素地图中的每个特征(即,平面)准确地考虑了由点测量噪声和位姿估计误差引起的不确定性。
在LiDAR ( -inertial )里程计系统中对提出的建图方法进行开源实现，并在多种环境的真实数据集(结构化和非结构化)和LiDARs (多旋激光雷达和非传统固态激光雷达)上对设计进行充分验证。特别地，在KITTI数据集上展示了本文方法相对于其他先进方法的优越性。

Related Works

一方面，我们的体素图中使用的平面特征与Suma和Suma++ 中使用的surfel最为相似，它们也通过优化点到平面的残差来注册新的点，并显式地维护和估计图中的surfel。Suma中的surfel是一个非常小的平面特征，这也是我们的平面特征在杂乱环境中的情况。然而，由于我们的体素图具有自适应的大小，在结构化环境中，我们的平面特征可能非常大。这种自适应策略使得我们的方法适用于结构多变的环境，同时保证了较高的计算效率。我们的平面特征与Suma系列中的面元的另一个区别在于表面/平面的关联。在Suma中，地图中的面元和新的LiDAR点都被投影到一个虚拟的图像平面上进行点到平面的关联。这样的surfel渲染过程非常耗时，并且对于在线应用通常需要GPU加速。相比之下，我们的平面关联是通过高效的哈希表查询进行的。

如MRSLaserMap所示，将surfel映射建模为高斯混合模型，并通过期望最大化进行surfel关联，可以缓解surfel渲染中的计算问题。同时该工作也采用了多分辨率的surfel map，其中有效的面元被从最细分辨率合并到较粗分辨率。这种由细到粗的地图方法起源于MRSMap，该方法受启发于LiDAR扫描仪的径向特性，通常对LiDAR点云的稀疏性和不规则性敏感。相比之下，我们的方法以由粗到精的方式构建体素图，对LiDAR点云的不规则性更加鲁棒，对不同环境的适应性更强。

之前的工作将LiDAR测量噪声引入到面元中，而不是确定性的面元，这导致了类似于我们概率平面表示的概率点到面元配准。然而，他们仅考虑了LiDAR测量噪声，忽略了LiDAR位姿估计中误差带来的不确定性。相比之下，我们的平面不确定性同时考虑了这两种不确定性。需要注意的是，这里使用位姿估计对测得的LiDAR点从局部的body系到world系的表面/平面估计，因此位姿估计误差将导致表面/平面的不确定性。

另一方面，我们的体素地图类似于NDT，该地图由许多体素(不同大小的)组成。在每个体素中，从位于该体素中的地图点拟合一个3D高斯分布，并通过最大化该点从该分布中被取走的可能性来注册一个新的点。LiTAMIN2通过将ICP与NDT结合，进一步提高了NDT的准确性。与这些NDT方法相比，我们的方法显式地参数化和估计一个体素中的平面。这种显式的平面参数化允许进行直接的点到平面配准，这比NDT更健全，而且往往是准确的，因为在NDT中，三维高斯分布通过惩罚所有三个方向的(而不仅仅是平面法线)来约束新的点。显式的平面参数化还允许在LiDAR点测量和位姿估计中引入噪声，从而提供了进一步的精度改进。

在数据结构方面，我们提出的八叉树数据结构和由粗到精的映射方法与OGN和HSP类似。OGN在深度卷积解码器中引入八叉树表示，实现了体三维输出的计算和存储高效生成。HSP使用分层表面上由粗到精的预测来进行更精确的三维物体重建。在OGN和HSP 的三维物体输出和物体重建取得良好效果的同时，我们将高效的八叉树数据结构和由粗到精的平面表示融合成一种新型地图，并将该地图应用于LiDAR里程计问题中，提高了LiDAR里程计的适应性、效率和精度。

Method

A. Probabilistic Plane Representation

这一节介绍了体素地图中每个体素包含一个概率特征（probabilistic feature），这里以平面特征（plane features）为例，因为平面在环境中广泛存在。作者首先分析点云噪声（在世界坐标系下），然后推导这些噪声如何影响平面不确定性。体素地图在世界坐标系（world frame）中表示，因此点噪声来源包括：原始LiDAR测量噪声（相对LiDAR本地坐标系）和LiDAR位姿估计误差（将本地点投影到世界坐标系）。

Uncertainty of Point $W p_i$

LiDAR点的噪声分析基于参考文献[27]。LiDAR点在本地坐标系（LiDAR frame）中的不确定性分为两部分：测距不确定性（ranging uncertainty）和方位不确定性（bearing direction uncertainty），如图1(a)所示。

[27] C. Yuan, X. Liu, X. Hong, and F. Zhang, “Pixel-level extrinsic self calibration of high resolution LiDAR and camera in targetless environments,” IEEE Robot. Automat. Lett., vol. 6, no. 4, pp. 7517–7524, Oct. 2021.

噪声和协方差如下所示：

关键洞察：近距离点噪声由测距主导，远距离由方位主导。这为平面不确定性奠基。

Plane Uncertainty Modelling

B. Coarse-to-Fine and Efficient Voxel Map Construction

这一节解释了粗到细（coarse-to-fine）体素地图构建的动机和方法，以高效处理LiDAR点云。

1) Motivation

LiDAR点云是顺序采样的，从稀疏到稠密（如图3）。传统surfel-based方法在稀疏时只能提取少量平面，适用于高分辨率LiDAR和低扫描率（需运动补偿）。作者提出粗到细体素地图：在稀疏时建粗地图，稠密时细化。

2) Voxel Map Construction

使用Hash表+八叉树（octree）的自适应体素地图。

空间切分为粗分辨率体素。
第一帧LiDAR定义世界坐标系，将点分配到体素。
填充体素加入Hash表。
对于每个体素：如果点共面（点协方差矩阵最小特征值 < 阈值），存储点、计算平面参数(5)和不确定性(8)；否则，分成8子体素，递归直到最大层数。
体素大小不同，每个含一个平面。

3) Voxel Map Update

新扫描点通过估计位姿(Section III-D)注册到地图。

新点在空体素：新建。
在现有体素：更新平面参数和不确定性。
效率问题：不确定性快速收敛（如图4，50点后收敛，噪声方差0.1 m²）。
收敛后：丢弃历史点，保留(n, q, Σ_{n,q})。
新点来时：保留最新10点，计算新法向量；若与收敛法向量差异大，假设地图变化，重构(Section III-B2)。

C. Point-to-Plane Match

匹配新LiDAR点到体素地图平面，构建位姿估计约束。

D. State Estimation

实验

硬件平台

所有实验在Intel i7-10700 @ 2.9 GHz、16 GB RAM、Nvidia GeForce GTX 730（2 GB RAM）的桌面电脑上运行

数据集

KITTI Vision Benchmark

Benchmark

与SOTA比较：比较对象包括：

FAST-LIO2 ：基于点云地图的LiDAR-惯性里程计（LiDAR-inertial odometry），使用常速度模型作为运动先验（因为KITTI无IMU数据）。
MULLS ：多特征LiDAR里程计系统。
LiTAMIN2 ：改进的ICP-NDT混合方法。
Suma ：基于surfel的LiDAR SLAM。
Lego-Loam ：地面优化的LiDAR里程计和映射系统。

结论：

运行时间：

室内测试：

使用固态LiDAR Intel L515在室内环境测试，使用常速度模型作为运动先验。手持设备采集三组数据（实验室和仓库），轨迹有意起始结束相同。

与SOTA比较：比较SSL_SLAM [33]（专为该LiDAR设计的SLAM）。参数统一：最大体素0.5 m，最大八叉树层2。SSL_SLAM使用开源默认参数。

结果：

表IV显示端到端误差和每扫描平均计算时间：Ours准确性和效率均优于SSL_SLAM。
在SSL_SLAM原数据集[33]上测试：Ours 0.12 m / 5.1 ms，SSL_SLAM 0.15 m / 25.1 ms。小改进因原数据慢速稳定（UGV采集）；本实验手持快速运动（尤其旋转），Ours更鲁棒。
补充材料[34]图1显示仓库彩色点云地图：Ours更精确，SSL_SLAM模糊（尤其返回路径）。

优势：避免特征提取，考虑LiDAR点和平面不确定性。

C. Unstructured Environment Test

验证在非结构化环境（公园、山地）的适应性，使用非重复扫描固态LiDAR Livox Avia（内置IMU，10 Hz LiDAR + 200 Hz IMU）。IMU补偿运动畸变并提供先验（类似FAST-LIO2）。参数同A节（最大体素3 m，最大层3），尽管扫描模式和环境不同。

与SOTA比较：比较Faster-LIO 和FAST-LIO2 （均用IKFoM和平面特征，但地图结构不同：Ours概率自适应体素，Faster-LIO Hash-indexed iVoxel，FAST-LIO2增量kd-tree）。关闭下采样，使用所有原始点；其他默认。结果平均5次运行。

1) Park Environment：

手持采集三轨迹：两组485 m（公园外围），一组815 m（外围+内部）。起始结束相同以显示漂移。

2) Mountain Environment：

UAV携带LiDAR向下采集山地数据（更少结构，更挑战）。公路大平面低不确定性，植被小平面高不确定性。

3) Result Analysis：

表V显示端到端误差和每扫描平均时间：Ours准确性远高于Faster-LIO和FAST-LIO2（因精确平面不确定性，而其他假设确定性地图）。
效率：Ours和Faster-LIO（均用Hash表）优于FAST-LIO2（kd-tree）。Ours额外计算概率分布（更多时间），但可剔除异常值（III-C），总体时间相当。

[33] H. Wang, C. Wang, and L. Xie, “Lightweight 3-D localization and mapping for solid-state LiDAR,” IEEE Robot. Automat. Lett., vol. 6, no. 2, pp. 1801–1807, Apr. 2021.