广告
FAST-LIO2: 快速直接的激光雷达-惯性里程计
2021-09-12 21:53:49· 来源：同济智能汽车研究所
编者按：SLAM技术广泛应用于室内或室外、城市或野外等不同的无人驾驶应用场景。激光雷达是最常使用的传感器之一，激光雷达可以提取环境中微小的细节特征。然而，
编者按：SLAM技术广泛应用于室内或室外、城市或野外等不同的无人驾驶应用场景。激光雷达是最常使用的传感器之一，激光雷达可以提取环境中微小的细节特征。然而，激光不适用于缺少结构特征的环境中，比如四壁光滑的通道，且纯激光算法在快速运动或复杂场景下，仅使用激光雷达测程容易出现问题。因此，激光雷达总是会与惯性测量单元(IMU)耦合,以提高系统的稳健性。
本文译自：《FAST-LIO2: Fast Direct LiDAR-inertial Odometry》
文章来源：arXiv preprint arXiv:2107.06829, 2021
作者：Wei Xu, Yixi Cai, Dongjiao He, Jiarong Lin, Fu Zhang
原文链接：https://arxiv.org/abs/2107.06829
摘要：本文介绍了FAST-LIO2：一种快速、稳健且通用的激光惯性里程计框架。FAST-LIO2建立在高效的紧耦合迭代卡尔曼滤波器的基础上，有两个关键的创新之处，可以实现快速、稳健和准确的激光雷达导航（和建图）。第一个是不提取特征直接将原始点配准到地图（并随后更新地图，即建图），而这使得环境中的细微特征能够被利用，从而提高匹配准确性，且取消提取特征模块能够适应有着不同扫描模式的新兴雷达；第二个主要新颖之处是通过增量k-d树（ikd-树）数据结构维护地图。ikd-tree支持增量更新（即点插入、删除）和动态平衡。与现有的动态数据结构（八叉树、R*-tree、nanoflann k-d 树）相比，ikd-树实现了优越的整体性能，同时自然地支持在树上的下采样。我们对来自各种开放激光雷达数据集的19个序列进行了详尽的基准比较。FAST-LIO2始终能保持更高的准确度，但是计算负载比其他最先进的激光惯性导航系统低得多。此外，文章也利用具有小视场角的固态激光雷达进行了各种真实世界的实验。总体而言，FAST-LIO2计算效率高（例如，在大型室外环境中高达100Hz的里程计和建图）、稳健（例如，在杂乱的室内环境中可靠的姿态估计，旋转速度高达1000度/秒），适用广泛（即，适用于多线旋转和固态激光雷达、无人机和手持平台，以及基于Intel和ARM的处理器），同时仍能实现比现有方法更高的精度。我们在Github上开源了FAST-LIO2和ikd-树的数据结构算法实现。
关键词：激光雷达惯性里程计，紧耦合迭代卡尔曼滤波，ikd-树
1 引言
构建未知区域的稠密三维（3D）地图环境的实时性，同时在地图中定位（即SLAM）对于自主机器人在未知环境中的安全导航至关重要。定位提供了机器人车载控制器的状态反馈，同时，稠密的3D地图提供了有关轨迹规划环境的必要信息（即自由空间freespace和障碍物）。基于视觉的SLAM[1]–[4]在定位方面非常精确，但是只维护一个稀疏的特征地图，并且会遭受照明变化和严重的运动模糊的影响。另一方面，仅利用机器人上搭载的计算资源，进行基于视觉传感器的实时密集、高分辨率、高精度的建图[5]–[8]，仍然是一个巨大的挑战。
由于能够提供直接、密集、活跃和精确的环境深度信息，Lidar（激光雷达）传感器已成为对于机器人另一种必要的传感器[9,10]。近十年来，激光雷达在许多自动机器人中发挥着越来越重要的作用，如自动驾驶汽车[11]和自动无人机[12,13]。在最近激光雷达技术发展中，已经实现了商业化、轻量批量生产、更具成本效益（成本范围类似于全球快门相机）、性能高（在数百米测量范围内拥有厘米精度）的固态激光雷达[14,15]引起了广泛的研究兴趣[16]–[20]。雷达成本、尺寸、重量、功率的显著降低具有使现有和新兴机器人应用领域受益的潜力。
采用基于激光雷达SLAM方法的核心要求是利用有限机载计算资源获得准确、低延迟的状态估计和密集的3D地图。然而，高效、准确的激光雷达里程计和地图绘制仍是有挑战性的问题：1）当前的激光雷达传感器每秒产生数十万到数百万的三维点，利用有限的车载计算资源处理如此大量的实时数据，要求具有较高的计算效率的激光雷达里程计方法。2）为了减少计算量，特征点，例如边缘点或平面点，通常是基于局部平滑度提取的。但是，特征提取模块的性能容易受环境影响。例如，在没有大量平面或长边的结构稀少的环境中，将导致提取很少的特征点。这种情况在新兴固态激光雷达典型的小视场角下会大大恶化 [16]。此外，特征提取也因激光雷达的不同而有所不同，具体取决于扫描模式（例如，旋转、基于prism[15]、基于MEMS[14]）和点密度。所以采用激光雷达里程计方法通常需要很多手动提取的工作;3)激光雷达点通常在连续运动按顺序采样，，这个过程会产生显著的运动失真，影响里程计和建图的性能，尤其是当运动很剧烈时。惯性测量单元(IMU)可以缓解这个问题，但引入了额外需要估计的状态量（例如，偏差，外参）；4) 在一次扫描中激光雷达通常有很长的测量范围（例如，数百米）但分辨率相当低。点云测量结果稀疏地分布在一个大型3D空间中，所以需要一个大而密集的地图来配准这些稀疏点。此外，该地图需要支持有效的搜索且同时实时更新新的测量数据。维持这样的地图是一项非常具有挑战性的任务，且不同于视觉测量的图像测量具有高分辨率，因此只需要稀疏特征图，在地图上特征点，只要它落在视场角中总能找到对应关系。
在这项工作中，我们通过两个关键新技术来解决这些问题：增量k-d树和直接配准点。更具体地说，我们的贡献如下：1）我们开发增量 k-d 树数据结构ikd-树，以有效表示大型密集点云图。除了高效的最近邻搜索，新的数据结构支持增量地图更新（即点插入、树上的下采样，点删除）和以最小的计算成本进行动态平衡。这些特性使 ikd-树非常适合应用于激光雷达里程计和建图，实现了100 Hz 里程计和在计算受限平台的建图，例如基于 Intel i7 的微型无人机板载计算机和甚至于 ARM 的处理器。Ikd-树数据结构工具箱已经在 Github上开源。2) 由于在ikd-树上计算效率的提高， 我们直接将原始点配准到地图上，这使得帧间配准即使是在剧烈的运动和非常混乱的环境中也准确可靠。我们称这种基于原始点的配准方法为直接法，类比于视觉SLAM[21]。去除手动特征提取使系统自然适用于不同的激光雷达传感器；3）我们将这两个关键技术整合到我们最近开发的一个完全紧耦合的激光雷达惯性里程计系统 FAST-LIO [22]。该系统通过使用 IMU的严格反向传播来修正每个点的运动并通过流形迭代卡尔曼滤波器估计系统的全部状态量。为了进一步加速计算，一种新的计算卡尔曼增益的数学等价公式被用于降低计算复杂度状态的维度（而非测量量）。新系统被称为 FAST-LIO2 并且在Github上是开源的；4）我们开展各种试验去评估 ikd-tree有效性、直接点配准法和整个系统。在18个不同大小的序列上的实验表明，ikd-Tree相对于现有的动态数据结构（八叉树、R*-tree、nanoflann k-d 树）在激光雷达里程计和地图的应用中实现了卓越的性能。在19个来自不同激光雷达数据集的序列上详尽的基准对比表明FAST-LIO2始终能保持更高的准确度，但具有比其他最先进激光惯性导航系统的计算负载低得多。我们最后展示FAST-LIO2在挑战由具有非常小的视场角的新兴固态激光雷达收集的真实世界数据，包括剧烈运动（如旋转速度 高达1000度/秒）和结构较少的环境中。
其余文章组织如下：在第Ⅱ章，我们讨论了相关背景的研究工作。我们分别在章节Ⅲ，Ⅳ和Ⅴ给出概述系统流程和每个关键部分的细节。第Ⅵ章中展示在开放数据集上的对比。在第Ⅶ章展示实际实验。最后是在第Ⅷ章的结论。
2 相关工作
A.激光雷达（-惯性系统）里程计
3D激光雷达SLAM的现有工作通常继承了在[23]中提出的LOAM结构。它由三个主要部分组成：特征提取、里程计和建图。为了减少计算量，新的一帧点云首先被基于局部平滑度的特征点（即边和平面）提取模块加工处理。然后，里程计模块（帧间）匹配来自连续两帧点云的特征点，获得粗略但实时（如10Hz）的激光雷达姿态里程计。通过里程计，多帧点云组合成一个扫描，然后该扫描被配准并合并到全局地图（即，建图）。在此过程中，地图点被用于构建一个k-d树，这使得一个非常有效的k近邻搜索(kNN搜索)成为可能。然后，LOAM通过迭代最近点(ICP)[24]–[26]方法实现点云配准，具体为：在其中每次迭代中，在目标点云的一些线或面上选取几个和原点云最近的点来实现ICP的配准。为了降低k-d树建设的时间成本，算法以规定的分辨率对地图点进行下采样。优化的建图过程通常以低得多的速率(1-2Hz)进行。
后续的激光雷达里程计工作保持了一个类似于LOAM的框架。例如，Lego-LOAM [27]引入了一种减少计算量的地面点分割方法以及减少长期漂移的回环模块。此外，LOAM-Livox[16]将LOAM适用于新兴的固态激光雷达。为了处理小视场角和非重复扫描的问题，即从连续两帧点云中提取的特征点几乎没有对应关系，LOAM-Livox通过直接将新的一帧点云直接配准到全局地图中来获得的里程计。这种直接将一帧点云配准到地图的方法提高了里程计的精度，但代价是在每一步构建中更新地图点的k-d树的计算量增加。
IMU可以通过提供ICP要求的良好的初始位姿来显著提高激光雷达里程计的精度和鲁棒性。此外，高频率的IMU测量数据可以有效地补偿激光点云帧中的运动失真。LION[28]是一种松耦合的激光雷达惯性SLAM方法，它保留了LOAM中的帧间配准方法（scan-to-scan registration），并在里程计中引入了可观测性检查以降低点数，从而节省计算量。更多的紧耦合的激光雷达惯性融合工作[17,29]–[31]在由固定数量的最近的激光点云帧（或关键帧）组成的在小尺寸局部地图中执行里程计。与帧间配准的方法相比，帧到局部地图的配准通常因使用更多最近的信息而更加准确。更具体地说，LIOM[29]提出了一种紧耦合激光雷达惯性融合方法，它在里程计中引入了IMU预积分。LILIOM[17]开发了一种新的特征提取方法对于非重复扫描的激光雷达，并在一个由20帧激光点云组成的小地图中执行帧间配准以获得里程计。LIO-SAM[30]的里程计需要一个9轴IMU来产生姿态测量，这个测量量是在一个小的局部地图中进行帧间配准的前提。LINS[31]将紧耦合迭代卡尔曼滤波器和机器人中心公式引入到里程计中的激光雷达姿态优化当中。因为上述工作为了获取实时的性能通常构建小的局部地图，所以里程计漂移地很快，需要进行低速率的建图过程，例如建图细化（LINS[31]），滑动窗口关节优化（LILI-OM[17]和LIOM[29]）和 因子图优化[32]（LIO-SAM[30]）。与上述方法相比，FAST-LIO[22]引入了一种形式化的反向传播，它精确考虑一帧点云中的每一个点的采样时间，并通过IMU测量值驱动的严格运动学模型对运动畸变进行补偿。此外，它还采用新的卡尔曼增益公式将计算复杂度从测量维度降低到状态维度。这个新公式在数学上被证明等价于传统的公式，但计算量减少了好几个数量级。计算的效率显著提高允许在里程计中进行直接并实时的一帧点云到地图的配准，并在每一步中更新地图（即建图）。它使用所有最近几帧点云中的点进行及时建图，这保证了里程计的精度。但是，为了防止构建地图的k-d树的时间越来越长，系统只能在小型环境中工作（例如，数百米）。
FAST-LIO2建立在FAST-LIO[22]的基础上，因此继承了紧耦合的融合框架，尤其是利用反向传播解决运动失真与利用快速卡尔曼增益计算提高效率。为了系统地解决计算量增长的问题，我们提出了一种新的数据结构ikd-树，它支持在每一步中更新增量地图和高效的kNN搜寻。受益于计算量的显著减少，里程计是通过将原始激光雷达点直接配准到地图上来执行的，从而提高了里程计和地图绘制的准确性和鲁棒性，尤其是当新的一帧点云中没有突出特征时（例如，由于小视场和/或无结构环境)。与上述均使用特征点的紧耦合激光雷达惯性方法相比，我们的方法更加轻量级，并可提高建图频率和里程计的精确性，并且无需为特征提取进行参数调整。
在我们的工作中直接注册原始点的想法已经在LION [28]中进行了探索，然而，如上所述，LION是一种松散耦合的方法。这个想法也很类似于[26]中提出的广义ICP（G-ICP），在其中，一个点被注册到地图中的小局部平面。这最终假设环境是平滑的，因此可以被视为局部平面。然而，广义ICP的计算量通常较大[33]。其他基于正态分布变换（NDT）[34]–[36]的工作也注册原始点，但NDT与ICP相比，有着更低的稳定性，在某些场景中可能会失效 [36]。
B.建图中的动态数据结构
为了实现实时建图，需要一个动态数据结构来支持增量更新和高效的kNN搜索。通常，kNN搜索问题可以通过为数据点建立空间索引来解决，具体可以分为两类：数据分割和空间分割。一个众所周知的数据分割的实例是 R-树 [37]，它基于空间中的数据接近度将数据聚类成潜在的重叠轴对齐立方体。各种R树按照线性、二次和指数复杂性分割节点，所有这些树都支持最近邻搜索和点式更新（插入，删除，和重新插入）。此外，R-树还支持在给定搜索区域或满足给定条件下进行搜索目标数据点。R-树的另一个版本是-树，它的性能优于原来的数[38]。-树按最小重叠标准进行插入，并对节点分割算法应用强制重插入原则。
八叉树[39]和k维树（k-d树）[40]是两种众所周知的数据结构，用于分割kNN搜索的空间。八叉树通过递归地将空间分成八个轴对齐的立方体来组织三维点云。当立方体为空或满足停止规则（例如，最小分辨率或最小点数量）时，立方体的分割停止。当进行进一步细分时，如果必要的话，新点将被插入到八叉树上的叶节点。八叉树支持kNN搜索和框式搜索，后者返回给定轴对齐长方体中的数据点。
k-d树是一种二叉树，其节点表示一个轴对齐的超平面，将空间分割为两部分。在标准构造规则中，分割节点被选择为沿着最长维度的中间点，以实现紧凑的空间分割[41]。在考虑建图中低维和存储在主存上的数据特性时，比较研究表明k-d树在kNN问题中取得了最好的性能[42, 43]。然而，在 k-d 树中插入新点和删除旧点会降低树的平衡性；因此，需要重建以重新平衡树。使用k-d 树库的建图方法，例如ANN [44]、libnabo[43]和FLANN [45]，完全重新构建 k-d 树以更新地图，但这会导致大量计算。虽然基于硬件的重建k-d树的方法已经在三维图形应用中得到彻底研究[46]–[49]，但所提出的方法严重依赖于计算资源，而这些资源通常局限于机器人应用的机载计算机。Galperin 等人没有完全重建树，而是提出了一种替罪羊 k-d 树，其中重新构建被部分应用于不平衡的子树，以保持整个树的松散平衡特性[50]。启用增量操作的另一种方法是以类似于 [51, 52]的对数方法维护一组k-d树，并重新构建一个精心选择的子集。Bkd-树在主存储器中维护了一个最大大小为M的k-d树，在外部存储器中维护了一组k-d树，其中第i棵树的大小为[53]。当树满时，算法从到提取点，并插入到第一个空树中。最先进的nanoflann k-d树利用对数结构进行增量更新，而惰性标签只标记删除的点，而不从树中删除它们(因此节省内存)[54]。
我们提出了一种基于替罪k-d树[50]的动态数据结构，命名为增量k-d树（ikd-树），以实现实时建图。我们的ikd-Tree支持点式插入和树上的下采样，这是建图中的常见要求，而在将新点插入其他动态数据结构之前，必须在外部进行下采样。当需要去除给定形状的规则区域（如长方体）中不必要的点时，现有的R-树和八叉树可以实现在给定空间内搜索点并逐个删除它们，而普通的k-d树使用半径搜索以获得点索引。与这种间接且低效的方法相比，ikd-Tree 通过维护范围信息和惰性标签直接删除给定轴对齐长方体中的点。标记为“已删除”的点将在重建过程中被删除。此外，尽管在应用部分重平衡方法(例如替罪羊k-d树[50]和纳米树k-d树[54])之后，增量更新是可用的，但是当重新构建大量点时，使用k-d树的建图方法受到间歇性延迟的影响。为了克服这一点，ikd-Tree通过并行重建避免了自身的显著延迟，同时保证了主线程的实时性和准确性。
广告
最新资讯
-
地平线发布征程6和高阶城区智驾样板间，以
2024-04-25 09:58
-
充电桩GB/T-2023设计要点
2024-04-25 09:49
-
中国汽车工程学会汽车大数据应用分会筹备会
2024-04-25 09:46
-
NVIDIA GPU 架构下的 FP8 训练与推理
2024-04-25 09:33
-
利用 NVIDIA Holoscan 1.0 开发生产就绪型
2024-04-25 09:21
广告
广告
