FAST-LIO2: 快速直接的激光雷达-惯性里程计

来源 | 同济智能汽车研究所

编者按：SLAM技术广泛应用于室内或室外、城市或野外等不同的无人驾驶应用场景。激光雷达是最常使用的传感器之一，激光雷达可以提取环境中微小的细节特征。然而，激光不适用于缺少结构特征的环境中，比如四壁光滑的通道，且纯激光算法在快速运动或复杂场景下，仅使用激光雷达测程容易出现问题。因此，激光雷达总是会与惯性测量单元(IMU)耦合,以提高系统的稳健性。

---

---

摘要：本文介绍了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通过并行重建避免了自身的显著延迟，同时保证了主线程的实时性和准确性。

3 系统总览

FAST-LIO2的流程图如图1所示。顺序采样的激光雷达原始点首先在10ms（对于100Hz更新）和100ms（对于10Hz更新）之间的时间段内累积。累积的点云称为一帧点云。为了执行状态估计，新的一帧点云中的点和地图点配准（即里程计），其中地图点通过紧耦合的迭代卡尔曼滤波器框架（红色大虚线块，参见第四章）保存在一个大型局部地图中。大型局部地图中的全局地图点由增量k-d树结构ikd-Tree（蓝色大虚线块，参见第五章）组织。如果当前激光雷达视场角范围越过地图边界，则距离激光雷达姿态最远的地图区域中的历史点将从 ikd-Tree中被删除。因此，ikd-Tree以一定长度(本文中称为“地图大小”)跟踪大立方体区域中的所有地图点，并用于计算状态估计模块中的残差。优化后的位姿最终将新的一帧点云中的点配准到全局坐标系中，并把通过以里程计的速率插入ikd-Tree的方式，将新的一帧点云中的点合并到地图中（即建图）。

图1 FAST-LIO2系统的总览

4 状态估计

FAST-LIO2的状态估计继承自 FAST-LIO [22] 的紧耦合迭代卡尔曼滤波器，但进一步结合了 LiDAR-IMU 外部参数的在线校准。在这里，我们简要解释了过滤器的基本公式和工作流程，并请读者参阅 [22]以了解更多详细信息。

A.运动学模型

我们首先推导出系统模型，它由一个状态转换模型和测量模型成。

1) 状态转移模型

取第一个IMU坐标系（记为I）作为全局坐标系（表示为G）并记

为激光雷达和IMU之间未知的外参，运动模型是：

其中表示IMU在全局坐标系中的位置和姿态，G_g是全局坐标系中的重力矢量，a_m和ω_m是IMU测量值，n_a和n_ω表示a_m和ω_m的测量噪声，ba和bw是建模为由n_ba和n_bw驱动的随机游走过程的IMU偏差，符号表示向量的斜对称叉积矩阵。

i表示IMU测量值的索引。基于[22]中定义的操作，连续运动学模型（1）可在IMU采样周期∆t进行离散化[55]：

其中函数 f、状态 x、输入 u 和噪声 w 定义如下：

2）测量模型：激光雷达通常逐个地采样点。因此，当激光雷达进行连续运动时，会在不同的姿态下采样得到点。为了纠正这种扫描中的运动，我们采用了[22]中提出的反向传播，该反向传播基于惯性测量单元的测量，估计一帧点云中每个点相对于一帧点云结束时姿态的激光雷达姿态。估计的相对位姿使我们能够根据一帧点云中每个单独点的精确采样时间，将所有点投影到扫描结束时间。因此，可以将一帧点云中的点视为在扫描结束时间时同时采样得到的。

k表示激光雷达扫描的索引，j=1,... ,m} 表示在扫描结束时间时，在局部激光雷达坐标系L中第k次扫描采到的点。由于激光雷达测量噪声，每个测量点通常都会受到由测距和波束定向噪声组成的噪声的污染。去除此噪声可得到在局部激光雷达坐标系中的真实点位置

在使用相应的激光雷达姿态和外参将真实点投影到全局坐标系之后，这个真实点应该正好位于地图中的一个局部小平面块上，即：

其中是对应平面的法向量，是平面上的一个点（见图2）。需要注意的是，和都包含在状态向量xk中。因此，第 j个点测量值所贡献的测量值可以从（4）总结为更紧凑的形式，如下所示

这定义了状态向量的隐式测量模型.

图2 测量模型

B.迭代卡尔曼滤波

基于在流形上制定的状态模型(2)和测量模型(5)，我们采用迭代卡尔曼滤波器，按照[55]和[22]中的过程直接在流形M上操作。它由两个关键步骤组成：每此IMU测量值的传播和每此激光雷达扫描的迭代更新，这两个步骤都自然地估计流形上的状态，从而避免任何重整化。由于IMU测量的频率通常高于激光雷达扫描的频率（例如，IMU测量为200Hz，激光雷达扫描为10Hz~100Hz），因此滤波器在更新之前通常会执行多个传播步骤。

1）传播：假设在融合最后一次（即第k-1次）激光雷达扫描后的最佳状态估计是为，协方差矩阵为。前向传播是在IMU测量值到达时执行的。更具体地说，通过将过程噪声wi设置为零，状态和协方差按照(2)进行传播：

其中Qi是噪声wi的协方差，矩阵和的计算如下（参见[55]中更抽象的推导和[22]中更具体的推导）：

前向传播一直持续到新的（即第k次）扫描结束，新扫描中的传播状态和协方差表示为。

2）残差计算：假设在当前迭代更新时状态xk的估计值（见第IV-B3节）是，根据（6）中的传播预测出的状态，当k=0时（即在第一次迭代前），。然后，我们将每个测量的激光雷达点投影到全局坐标系，并在由ikd-Tree表示的地图中搜索其最近的5个点（参见第V-A节）。然后使用找到的最近相邻点来拟合局部小平面块，该小平面块具有测量模型中使用的法向量和质心（参见（4）和（5））。此外，通过在处进行的一阶近似来近似测量方程(5)，得到：

其中，是相对于的雅可比矩阵,在零处计算，是由于原始测量噪声引起的，而被称为残差：

3）迭代更新：来自第IV-B1节的传播状态和协方差对未知状态xk施加先验高斯分布。更具体地说，表示以下误差状态的协方差：

其中是相对于评估为零的的偏微分，在零处计算：

其中在[22,25]中被定义，和分别是IMU姿态状态和旋转外参的误差。对于第一次迭代，，

除了先验分布，我们还有一个由测量方程（8）引起的状态分布：

 将 (10) 中的先验分布与 (12) 中的测量模型相结合，得到由等价表示的状态xk的后验分布，并且的最大后验估计为：

其中。这个MAP问题可以通过迭代卡尔曼滤波器解决，如下所示（为了简化符号，让，P=，）：

请注意，卡尔曼增益K计算需要反转状态维度的矩阵，而不是以前工作中使用的测量维度。

重复上述过程直到收敛（即）。收敛后，最优状态和协方差估计为:

随着状态更新，第k次扫描中的每个激光雷达点()通过以下方式转换为全局坐标系：

转换后的激光雷达点{}被插入到由ikd-Tree表示的地图中（参见第V章）。我们的状态估计总结在算法1中，如下。

5 建图

在本节中，我们将描述如何增量维护地图（即插入和删除），并通过ikd-Tree对其执行k-nearest搜索。为了从理论上证明ikd-Tree的时间效率，我们对时间复杂度进行了全面的分析。

A.地图管理

地图点被组织成一个ikd-Tree，它通过以里程计速率合并新的一帧点云而动态增长。为了防止地图的大小不受约束，在ikd树上仅保留激光雷达当前位置周围长度为L的大局部区域中的地图点。2D演示如图3所示。地图区域被初始化为一个边长为L的立方体，它以初始激光雷达位置p0为中心。假设激光雷达的检测区域是一个以从(15)中得到的激光雷达当前位置为球心的检测球。假设探测球的半径为r=γR，其中R是激光雷达视场角范围，γ是大于1的松参数。当激光雷达移动到新位置p'时（在p’处探测球接触到地图的边界），地图区域沿激光雷达检测区域与触摸边界之间距离增加的方向移动。地图区域移动的距离设置为常数d=(γ-1)R。新地图区域和旧地图区域之间的减法区域中的所有点都将通过V-C小节中详述的框式删除操作从ikd-Tree中被删除。

图2 地图区域管理的2D演示。在（a）中，蓝色矩形是长度为L的初始地图区域。红色圆圈是以初始LiDAR位置p0为圆心的初始检测区域。在（b）中，检测区域（红色虚线圆圈）移动到地图边界被触摸的新位置p'（红色实线圆圈）。地图区域移动到新位置（绿色矩形）距离d。减法区域（橙色区域）中的点从地图（即ikd-Tree）中移除

B.树状结构和构造

1)数据结构：ikd-Tree是一个二叉搜索树。ikd-Tree中树节点的属性在Data Structure中呈现。

k-d树的许多现有实现只在叶节点[43]–[45，53，54]上存储点的“桶”，与此不同，我们的ikd-Tree在叶节点和内部节点上都存储点,以更好地支持动态点插入和树的重平衡。当使用单个k-d树时，这种存储模式在kNN搜索中也表现出更高的效率[41]，我们的ikd-Tree就是这种情况。由于一个点对应于ikd-Tree上的单个节点，我们将交替使用点和节点。点信息（例如，点坐标、强度）存储在point中.属性leftchild和rightchild分别是指向其左右子节点的指针。分割空间的分割轴记录在axis中。以当前节点为根的(子)树的树节点数(包括有效和无效节点)保持在属性treesize中。当点从地图中删除时，ikd-Tree不会立即从树中删除节点，而只是将布尔变量detected置为真（详见第V-C2节）。如果删除以当前节点为根的整个（子）树，则treedeleted设置为true。从（子）树中删除的点数汇总到属性invalidnum中。属性range记录了（子）树上点的范围信息，它被解释为一个包含所有点的外接轴对齐长方体。外接长方体由其每个维度上分别具有最小和最大坐标的两个对角顶点表示。

2）构建：构建ikd-Tree类似于[40]中构建静态k-d树。ikd-Tree沿着最长维度递归地在中间点分割空间，直到子空间中只有一个点。Data Structure中的属性在构造过程中被初始化，包括计算树的大小和(子)树的范围信息。

C.增量更新

ikd-Tree上的增量更新指的在动态重新平衡之后而进行的增量操作，其中动态重新平衡详见第V-D节。ikd-Tree支持两种类型的增量操作：点式操作和框式操作。点式操作在k-d树中插入、删除或重新插入单个点，而框式操作在给定的轴对齐长方体中插入、删除或重新插入所有点。在这两种情况下，点插入都进一步与树上的下采样相结合，从而将地图保持在预定的分辨率。在本文中，我们只解释了FAST-LIO2的地图管理所需的点式插入和框式删除。读者可以参考我们在Github存储库中ikd-Tree的开源完整实现以及其中包含的技术文档以了解更多详细信息。

1)使用树上的下采样进行点插入：考虑到机器人的应用，我们的ikd-Tree支持同时进行点插入和地图下采样。该算法在算法2中有详细说明。

对于来自状态估计模块（见算法1）的中的给定点p和下采样分辨率l，算法将空间均匀地划分为边长为l的立方体，然后是找到包含点p的立方体CD（第2行）。该算法仅保留最接近CD中心点pcenter的点（第3行）。这是通过首先在k-d树上搜索CD中包含的所有点，并将它们与新点p一起存储在点数组V中来实现的（第4-5行）。最近的点pnearest是通过比较V中每个点到中心pcenter的距离来获得的（第6行）。然后删除CD中的现有点（第7行），然后将最近的点pnearest插入k-d树（第8行）。框式搜寻的实现类似于第V-C2节中介绍的框式删除。

ikd-Tree上的点插入（第11-24行）是递归实现的。该算法从根节点开始向下搜索，直到找到一个空节点以追加一个新节点（第12-14行）。新叶节点的属性初始化为表1。在每个非空节点上，新的点与存储在树节点上的点沿分割轴进行比较，以便进一步递归（第15-20行）。这些被访问过的节点的属性（例如，treesize、range）用第V-C3节中介绍的最新信息（第21行）进行更新。算法检查并维护用新点更新的子树的平衡标准，以保持ikd-Tree（第22行）的平衡特性，详见第V-D节。

表1 要插入的新树节点的属性初始化

2)使用懒惰标签进行框式删除：在删除操作中，我们使用了惰性删除策略。也就是说，这些点不会立即从树中删除，而只是通过将属性detected置为true来将点标记为“已删除”（参见Data Structure，第6行）。如果以节点T为根的子树上的所有节点都已被删除，则T的属性treedeleted被设置为true。因此detected和treedeleted的属性称为惰性标签。标记为“已删除”的点将在重建过程中从树中删除（参见第V-D节）。

框式删除是利用属性range中的范围信息和树节点上的惰性标签实现的。如V-B中所述，属性range由外接长方体CT表示。伪代码如算法3所示。

给定要从以T为根的（子）树中删除的长方体CO，该算法递归搜索树并将外接长方体CT与给定的长方体CO进行比较。如果CT和CO之间没有交集，递归直接返回而不更新树（第2行）。如果外接长方体CT完全包含在给定的长方体CO中，则框式删除将属性detected和treedeleted都置为真（第5行）。当（子）树上的所有点都被删除时，属性invalidnum等于treesize（第6行）。对于CT与CO相交但不包含在CO中的情况，如果当前点p包含在CO中，则首先将其从树中删除（第9行），然后算法递归查找子节点（第10-11行）。当前节点T的属性更新和平衡维护在框式删除操作之后进行（第12-13行）。

3)属性更新：在每次增量操作后，使用AttributeUpdate函数，用最新的信息更新被访问节点的属性。该函数通过汇总其两个子节点上的对应属性和自身的点信息来计算属性treesize和invalidnum；通过合并两个子节点的范围信息和存储在其上的点信息来确定属性range；如果两个子节点的treedeleted都为真，并且节点本身将被删除，则treedeleted被设置为真。

D.重平衡

ikd-Tree会在每次增量操作后主动监控balance特性，并通过仅重新构建相关子树来动态地重平衡自身。

1)平衡准则：平衡准则由两个子准则组成：α-平衡准则和α-删除准则。假设ikd-Tree的一个子树以T为根。当且仅当它满足以下条件时，该子树是α-平衡的：

其中，S(T)是节点T的treesize属性。

以T为根的一个子树的α-删除准则是：

其中，I(T)表示子树上无效节点的数量（即节点T的属性invalidnum）。

如果ikd-Tree的一个子树同时满足这两个准则，则该子树是平衡的。如果所有子树都是平衡的，则整个树是平衡的。违反任一准则将触发重建过程以重新平衡该子树：α-平衡准则保持树的最大高度。很容易证明，α-平衡树的最大高度是，其中n是树的大小；α-删除准则确保删除子树上的无效节点（即，标记为“已删除”）以减小树的大小。降低k-d树的高度和大小允许在未来进行高效的增量操作和查询。

2）重建和并行重建：假设在子树T上触发重建（见图4），首先将子树展平为点存储阵列V。标记为“已删除”的树节点在展平期间被丢弃。然后如第V-B节所述，用V中所有的点构建一个新的完美平衡的k-d树。当在ikd-Tree上重新构建大型子树时，可能会出现相当大的延迟并破坏FAST-LIO2的实时性能。为了保持较高的实时性，我们设计了一种双线程重建方法。我们提出的方法不是简单地在第二个线程中重新构建，而是通过一个操作记录器避免两个线程中的信息丢失和内存冲突，从而始终保持k-最近邻搜索的完全准确性。

图4 重建一个不平衡的子树

重建方法如算法4所示。当违反平衡准则且子树的树大小小于预定值Nmax时，子树将在主线程中被重建；否则，子树在第二个线程中被重新构建。第二个线程上的重建算法如函数ParRebuild所示。将第二个线程中要重建的子树表示为Г，将其根节点表示为T。第二个线程将锁定所有增量更新（即点插入和删除），但不查询该子树（第12行）。然后，第二个线程将包含在子树Г中的所有有效点复制到点数组V中（即，展平），同时保持原始子树不变，以便在重建过程中进行可能的查询（第13行）。展平后，原始子树被解锁，以便主线程接受进一步的增量更新请求(第14行)。这些请求将同时被记录在一个名为操作记录器的队列中。一旦第二个线程完成了从点数组V构建一个新的平衡k-d树Г'的这一过程（第15行），记录的更新请求将通过函数IncrementalUpdates（第16-18行）在Г'上再次执行。请注意，当并行重建开关已经在第二个线程中时，它会被设置为false。在所有待处理的请求被处理后，原始子树Г上的点信息与新子树Г'上的点信息完全相同，除了新子树的树结构比原始子树更加平衡。该算法从增量更新和查询中锁定节点T，并将其替换为新的T'（第20-22行）。最后，该算法释放原始子树的内存（第23行）。这种设计确保了在第二个线程的重建过程中，主线程中的建图过程仍然以里程计频率进行，而没有任何的中断，尽管此时的效率由于暂时不平衡的k-d树结构变得较低。需要注意的是，LockUpdates函数不会阻碍查询，查询可以在主线程中并行进行。相比之下，LockAl函数会阻止所有访问，包括查询，但它完成得非常快（即，只有一条指令），从而允许在主线程中进行及时查询。函数LockUpdates和LockAll是通过互斥（mutex）方式实现的。

E. K-最近邻搜索

尽管与那些著名的k-d树库[43]-[45]中的现有实例有些类似，但最近临搜索算法在ikd-Tree上被彻底优化。使用[41]中详述的“bounds-overlap-ball”测试，可以很好地利用树节点上的距离信息来加速最近邻搜索。我们维护一个优先级队列q来存储迄今为止遇到的k个最近的邻居以及它们到目标点的距离。当从树的根节点递归向下搜索树时，算法首先计算从目标点到树节点长方体CT的最小距离dmin。如果最小距离dmin不小于q中的最大距离，则无需处理该节点及其后代节点。此外，在 FAST-LIO2（和许多其他激光雷达里程计）中，只有当相邻点在目标点周围的给定阈值内时，它才会被视为内联点，并被用于状态估计，这自然为k最近邻的远程搜索提供了最大搜索距离[43]。在任何一种情况下，远程搜索都会通过将dmin与最大距离进行比较来删减算法，从而减少回溯量以提高时间性能。需要注意的是，我们的ikd-Tree支持多线程 k-近邻搜索并行计算架构。

F. 时间复杂度分析

ikd-Tree的时间复杂度可分为增量操作（插入和删除）、重建和k-最近邻搜索的时间。请注意，所有分析都是在低维度的假设下提供的（例如，FAST-LIO2 中的三个维度）。

1）增量操作：由于树上的下采样的插入依赖于框式删除和框式搜索，所以我们首先讨论框式的操作。假设n表示ikd-Tree的树大小，ikd-Tree上框式操作的时间复杂度为：

引理1. 假设ikd-Tree上的点在3-d空间Sx×Sy×Sz中，操作长方体为 CD = Lx×Ly×Lz。用长方体CD实施算法3的框式删除和搜索的时间复杂度为：

a,b,c≥0.∆min,∆med和∆max分别是a、b、c中的最小值、中值和最大值。α(u)，u∈[0,1]是flajolet-puech函数，其中提供了特定值：α(1/3)=0.7162和α(2/3)=0.3949

证明：[56]中提供了k-d树上轴对齐超立方体范围搜索的渐近时间复杂度。除了懒惰标签附加到树节点之外，框式删除可以被视为范围搜索，这是O(1)。因此，范围搜索的结论可以应用于ikd-Tree上的框式删除和搜索，这产生了O(H(n))。

树上的下采样插入的时间复杂度为：

引理2. 在ikd-Tree上的算法2中使用树上的下采样的点插入的时间复杂度为O(logn)。

证明。ikd-Tree上的下采样方法由框式搜索和点插入后的删除组成。通过应用引理1，下采样的时间复杂度为O(H(n))。通常，下采样立方体CD与整个空间相比非常小。因此，归一化范围Δx、Δy和Δz很小，Δmin的值满足时间复杂度为O(logn)的条件(*)

ikd-Tree的最大高度可以很容易地从方程(17)中证明为。而静态k-d树的值为log2n。因此引理直接从[40]中获得，其中k-d树上点插入的时间复杂度被证明为O(logn)。总结下采样和插入的时间复杂度得出结论，使用树上的下采样的点插入的时间复杂度为O(logn)。

2）重建：重建的时间复杂度分为单线程重建和并行双线程重建两种。在前一种情况下，重建是由主线程递归执行的。每个级别都花费排序时间（即O(n)），当维度k较低时，logn级别上的总时间为O(nlogn)[40]。对于并行重建，主线程消耗的时间只是扁平化（这使得主线程暂停进一步增量更新，算法4，第12-14行）和树更新（需要恒定时间O(1)，算法4，第20-22行）但没有构建（由第二个线程并行执行，算法4，第15-18行），拥有的时间复杂度为O(n)（从主线程看）。总之，重建ikd-Tree的时间复杂度对于双线程并行重建为O(n)，对于单线程重建为O(nlogn)。

3）K-最近邻搜索：由于ikd-Tree的最大高度保持不大于，其中n为树的大小，从根节点向下搜索到叶子节点的时间复杂度为O(logn)。在树上搜索k最近邻的过程中，回溯的次数与常数成正比，该常数与树的大小无关[41]。因此，在ikd-Tree上获得k最近邻的预期时间复杂度为O(logn)。

6 基准实验结果

在此部分中，我们在各种开放数据集上进行了准确性、鲁棒性和计算效率方面的大量实验。我们首先对比其他数据结构，评估了我们的数据结构ikd-树，在18个不同大小的数据集序列上进行了kNN搜索。然后在VI-C部分，我们比较了FAST-LIO2在19个序列上的准确性和处理时间。所有序列都是从固态激光雷达[15]和旋转激光雷达收集的5个不同数据集中选择的。第一个数据集来自LILI-OM[17]，由固态3D激光雷达LivoxHorizon4收集，它具有非重复扫描模式和81.7°（水平）×25.1°（垂直）视场，典型的扫描频率为10赫兹，第一个数据集简称lili。陀螺仪和加速度计测量值由6轴XsensMTi-670IMU以200Hz的频率采样。数据是在具有结构化场景的大学校园和城市街道中记录的。第二个数据集来自MIT校园的LIO-SAM[30]，包含由以10Hz采样的VLP-16激光雷达5和以1000Hz采样的MicroStrain3DM-GX5-259轴IMU收集的多个序列，第三个数据集简称为liosam。它包含不同类型的场景，包括校园内的结构化建筑和森林。第三个数据集“utbm”[57]由最高达50公里/小时速度的有人驾驶机器车收集，该车具有两个10HzVelodyneHDL-32E激光雷达和100HzXsensMTi-28A53G25IMU。在本文中，我们只考虑左侧的激光雷达。第四个数据集“ulhk”[58]包含来自VelodyneHDL-32E的10Hz激光雷达数据和来自9轴XsensMTi-10IMU的100HzIMU数据。来自utbm和ulhk的所有序列都是由有人驾驶车辆在结构化的城市区域中收集的，而ulhk也包含许多移动车辆。最后一个数据集“nclt”[59]，是在密歇根大学北校区收集的大规模、长期的自主UGV（无人地面车辆）数据集。nclt数据集包含来自VelodyneHDL-32E激光雷达的10Hz数据和来自MicrostrainMS25IMU的50Hz数据。与其他数据集相比，nclt数据集的持续时间和数据量要长得多，并且包含多个开放场景，例如大型开放停车场。包括传感器类型和数据速率在内的数据集信息总结在表2中。这部分中使用的所有37个序列的详细信息，包括名称、持续时间和距离记录在附录A的表8中。

表2 基准测试数据集

为了让LIO-SAM能工作，nclt数据集中的IMU频率通过零阶插值从50Hz增加到100Hz

A.实现过程

我们在C++和机器人操作系统(ROS)中实现了提出的FAST-LIO2系统。迭代卡尔曼滤波器是基于我们之前工作[55]中介绍的IKFOM工具箱实现的。在默认配置中，局部地图大小L选择为1000m，激光雷达原始点在1:4（四个激光雷达点之一）时间下采样后直接用于状态估计。此外，所有实验的空间下采样分辨率（见算法2）均设置为l=0.5m。ikd-树的参数设置为αbal=0.6，αdel=0.5，Nmax=1500。基准比较的计算平台是轻量级无人机机载计算机：DJIManifold2-C，它有1.8GHz四核Inteli7-8550UCPU和8GB内存。对于FAST-LIO2，我们还在ARM处理器上对其进行了测试，该处理器通常用于降低功耗和成本的嵌入式系统。ARM平台是KhadasVIM3，它具有低功耗2.2GHz四核Cortex-A73CPU和4GBRAM，以关键字“ARM”表示。我们将“FAST-LIO2(ARM)”表示为FAST-LIO2在基于ARM的平台上的实现。

B.数据结构评估

1)评估设置：我们选择了动态数据结构的三个最先进的实现来与我们的ikd-树进行比较：R*-树[60]的boost几何库实现、oc树[61]的点云库实现和nanoflann[54]动态k-d树的实现。之所以选择这些树型数据结构实现，是因为它们的实现效率高。此外，它们支持动态操作（即点插入、删除）和范围（或半径）搜索，这对于与FAST-LIO2集成并对ikd-树进行公平的比较是十分必要的。对于地图下采样，由于其他数据结构不支持像ikd-树那样的树上的下采样，因此我们应用了与V-C中详述类似的方法，利用它们的范围搜索（对于八叉树和R*树）或半径搜索的能力（对于nanoflannkd树）。更具体地说，对于八叉树和R*-树，它们的范围搜索直接返回目标长方体CD内的点（参见算法2）。对于nanoflannk-d树，目标长方体CD首先被分成小立方体，其边长等于长方体的最小边长。然后通过半径搜索得到每个小立方体外接圆内的点，然后通过线性方法过滤掉长方体外的点，而保留目标长方体CD内的点。最后，与算法2类似，将CD中除距离中心最近的点以外的点从地图中移除。对于地图移动所需的框式删除操作（见第V-A部分），是根据各自的范围或半径搜索得到的点索引再将指定长方体内的点一一删除来实现的。

所有四种数据结构实现都会与FAST-LIO2集成，并在18个不同大小的序列上评估它们的时间性能。我们使用每个序列的每个数据结构运行FAST-LIO2，并记录kNN搜索、点插入（使用地图下采样）、由于地图移动导致的框式删除、新的一帧点云的点的数量和地图数量的时间每一步的点（即树的大小）。要查找的最近邻数量为5。

2）比较结果：我们首先比较所有18个序列中不同树大小的点插入（使用地图下采样）和kNN搜索消耗的时间。对于每个评估的树大小S，我们收集树大小在[S−5%S,S+5%S]范围内的处理时间以获得足够数量的样本。图5显示了每个单个目标点的插入和kNN搜索的平均时间消耗。八叉树在点插入方面取得了最好的性能，虽然与另一个的差距很小（低于1μs），但由于树结构不平衡，它的查询时间要长得多。对于nanoflannk-d树，插入时间通常比ikd-树和R*-树略短，但由于其组织一系列k-d树的对数结构，偶尔会出现巨大的峰值。这样的峰值严重降低了实时能力，尤其是在维护大地图时。对于k-最近邻搜索，nanoflannk-d树比我们的ikd-树消耗更多的时间，尤其是当树的大小变大（105∼106）时。R*-树实现了与ikd-树相似的插入时间，但对于大树的搜索时间要长得多。最后我们可以看到用树上的下采样和ikd树kNN搜索的插入时间确实与logn成正比，这与第V-F节中的时间复杂度分析一致。

对于用于激光雷达里程计和制图的任何地图数据结构，地图查询（即kNN搜索）和增量地图更新（即由于地图移动而进行下采样和框删除的点插入）的总时间最终会影响实时能力。该总时间总结在表3中。可以看出，八叉树在大多数数据集中的增量更新中表现最好，紧随其后的是ikd-树和nanoflannk-d树。在kNN搜索中，ikd-树的性能最好，而ikd-树和nanoflannk-d树在很大程度上优于其他两个，这与过去的比较研究一致[42,43]。在所有其他数据结构中，ikd树实现了最佳的整体性能。

需要注意的是，虽然nanoflannk-d树实现了与ikd-树相似的性能，但插入时间峰有更为深刻的影响，并且它对激光雷达里程计和建图的影响是很严重的。Nanoflannk-d树删除一个点是仅通过掩蔽它，而不实际从树中删除它。因此，即使进行地图下采样和地图移动，删除的点仍会保留在树上，影响后续查询和插入性能。由此产生的树的大小比ikd-树和其他树增长得更快，从图5中也观察到了这一现象。对于短序列（ulhk和lili）的影响可能很小，但对于长序列（utbm和nclt）变得明显。Nanoflannk-d树的大小在utbm数据集中超过6×106，在nclt数据集中超过107，而ikd-树的最大树大小分别达到2×106和3.6×106。nanoflann增量更新的最大处理时间在七个utbm数据集中都超过3秒，在三个nclt数据集中超过7秒，而我们的ikd-树将最大处理时间保持在nclt_2中的214.4毫秒，其余17个序列中的最大处理时间小于150毫秒.虽然nanoflann的这种处理时间峰由于其发生率低而不会严重影响整体实时能力，但它会导致后续控制的灾难性延迟。

表3 每次扫描平均时间消耗在增量更新、KNN搜索和总时间上的比较

1每次扫描增量更新的平均时间消耗，包括使用树上的下采样的点插入和框删除。

2单线程kNN搜索每次扫描的平均时间消耗

图5 不同树大小的数据结构比较

C.精度评估

在本部分中，我们将FAST-LIO2整个系统与其他最先进的激光雷达-惯性里程计和建图系统进行比较，包括LILI-OM[17]、LIO-SAM[30]和LINS[31]。由于FAST-LIO2是一种没有任何环路检测或校正的里程计，为了公平比较，LILI-OM和LIO-SAM的环路闭合模块被停用，而所有其他功能如滑动窗口优化等所有功能都启用。我们还对FAST-LIO2进行了控制变量研究：为了了解地图大小的影响，除了默认的1000m之外，我们在2000m、800m、600m的各种地图尺寸L上运行算法；为了评估直接方法对基于特征的方法的有效性，我们从FAST-LIO[22]（针对固态激光雷达进行了优化）和BALM[20]（针对旋转激光雷达进行了优化）中添加了一个特征提取模块。结果以“Feature”为关键词报告。所有实验均在Manifold2-C平台（Intel）上进行。

我们对lili、lisam、utbm、ulhk和nclt全部五个数据集进行评估。由于并非所有序列都有地面实况（受天气、GPS质量等影响），我们从五个数据集中总共选择了19个序列。这19个序列要么具有良好的真实轨迹（如数据集作者所推荐），要么在起始位置结束。因此，我们计算和评估了绝对平移误差(RMSE)和端到端误差这两个标准。

1)RMSE基准：表4中对RMSE进行了计算和报告。可以看出，增加FAST-LIO2的地图大小会提高整体精度，因为当激光雷达重新访问以前的地方时，新的一帧点云的点会配准到较旧的历史点。当地图大小超过2000m时，精度增量不是持久的，因为里程计漂移可能导致与太旧的地图点可能出现错误点匹配，这是任何里程计的典型现象。此外，直接方法在大多数序列中优于FAST-LIO2基于特征的变体，除了在nclt_4和nclt_6这两个中的差异很小且可以忽略不计。这证明了直接法的有效性。

与其他LIO方法相比，FAST-LIO2或其变体在所有19个数据序列中的18个中取得了最佳性能，是所有实验中最稳健的LIO方法。唯一的例外是在ulhk_4上，LILI-OM的精度略高于FAST-LIO。值得注意的是，LILI-OM在utbm_9、nclt_4、nclt_6、nclt_8和nclt_10中显示出非常大的漂移。原因是它的滑动窗口后端融合（建图）随着地图点数量的增长而失效。因此，它的姿态估计仅依赖于快速累积漂移的前端里程计。LINS在nclt_5,nclt_6,nclt_7,nclt_10中的工作情况同样不好。LIO-SAM在nclt_4,nclt_10也表现出很大的漂移，这是由于后端因子图优化在很长时间和很长距离的数据下的失效。nclt_10序列的示例视频可以在https://youtu.be/2OvjGnxszf8上找到。此外，在LILI-OM、LIO-SAM和LINS可以正常工作的其他序列上，FAST-LIO2的性能仍然有很大的优势。最后，应该注意的是，序列liosam_1直接从LIO-SAM[30]中提取，因此该算法已针对数据进行了良好的调整。但是，FAST-LIO2仍然实现了更高的精度。

表4 有良好的地面实况序列的绝对平移误差

1数据集utbm不产生LIO-SAM所需的姿态四元数数据，因此LIO-SAM不适用于所有序列在utbm数据集中，表示为—；2×表示系统完全失效

2)漂移基准：表5展示了端到端误差。总体趋势与RMSE基准结果相似。FAST-LIO2或其变体在总共7个序列中的5个序列中实现了最低的漂移。我们在https://youtu.be/2OvjGnxszf8上的视频中展示了ulhk_6序列的例子。应该注意的是，LILI-OM已经为他们自己的每个序列lili调整了参数，而FAST-LIO2的参数在所有序列中保持相同。LIO-SAM在其自己的序列liosam2和liosam3中表现出良好的性能，但无法在其他序列（如ulhk）上保持它。LINS在liosam和ulhk数据集上的表现比LIO-SAM差，在liosam2（来自[30]的花园序列）中失效，因为这两个序列是以很大的旋转速度记录的，而LINS使用的特征点太少。此外，在大多数序列中，基于特征的FAST-LIO表现类似于直接方法，除了序列lili_7，它包含许多树木和大的开放区域，而特征提取将从树木和远处建筑物中去除了许多的有效点。

表5 端到端误差（米）

1由于LIO-SAM和LINS都是专为旋转激光雷达开发的，它们不适用于由固态激光雷达Livox Horizon记录的lili数据集；2表示系统完全失效

D.处理时间评估

表6展示了具有不同配置的FAST-LIO2、LILI-OM、LIO-SAM和LINS在所有序列中的处理时间。FAST-LIO2是一个集成的里程计和建图架构，在每一步，地图都会在里程计更新后立即更新。因此，总时间（表6中的“总计”）包括里程计中发生的所有可能过程，包括特征提取（例如，对于基于特征的变体）、运动补偿、kNN搜索和状态估计，以及建图。需要注意的是，建图包括点插入、框式删除和树重新平衡。另一方面，LILI-OM、LIO-SAM和LINS都有单独的里程计（包括特征提取和粗略姿态估计）和建图（例如LILI-OM[17]中的后端融合、增量平滑和在LIO-SAM[30]中的建图以及在LINS[31]中的建图优化），在每次激光雷达扫描过程中它们的平均处理时间在表6中分别用“Odo”和“Map”表示。我们，将两次处理时间相加与FAST-LIO2进行比较。

从表6中，我们可以看到FAST-LIO2比其他LIO方法消耗的时间少得多，比LILI-OM快8倍，比LIO-SAM快10倍，比LINS快6倍。即使只考虑其他方法对里程计的处理时间，FAST-LIO2在除了四个序列之外的大多数序列中仍然更快。FAST-LIO2的整体处理时间，包括里程计和建图，几乎与LIO-SAM的里程计部分相同，比LILI-OM快3倍，比LINS快2倍以上。比较FAST-LIO2的不同变体，不同地图大小的处理时间非常相似，这意味着使用我们的ikd-树进行映射和kNN搜索对地图大小不敏感。此外，基于特征的变体和使用直接方法的FAST-LIO2的处理时间大致相似。尽管特征提取需要额外的处理时间来提取特征点，但它会为后续的kNN搜索和状态估计带来更少的点（因此时间更少）。另一方面，直接方法节省了点配准的特征提取时间。考虑到FAST-LIO2卓越的计算效率，我们在KhadasVIM3(ARM)嵌入式计算机上使用默认地图大小（1000m，参见VI-C）进一步实现了它。运行时结果表明，FAST-LIO2还可以实现10Hz的实时性能，这是任何之前的工作都没有在基于ARM的平台上实现过的。

表6 每个扫描基准的平均处理时间（以毫秒为单位）

7 真实场景实验

A.平台

除了对主要是在地面上收集到的数据集进行基准评估，我们也用由其他平台收集到的各种各样的富有挑战性的数据来测试我们的FAST-LIO2（如图6所示），这些平台包括用于开发无人机导航技术的280mm轴距四旋翼无人机，用于移动制图的手持平台以及用于航空测绘的GPS导航750mm轴距四旋翼无人机。280mm轴距四旋翼无人机被用于室内剧烈飞行试验，参见VII-B2部分，因此其激光雷达面向前方安装。由Ambit-Geospatial公司开发的750mm轴距四旋翼无人机用于空中扫描，参见VII-C部分，因此其激光雷达面向地面。在所有平台中，我们均采用固态3D激光雷达LivoxAvia，它具有内置IMU单元（型号BMI088），横向70.4°纵向77.2°的圆形视场，以及一种不同于之前在VI部分中使用的LivoxHorizon和Velodyne激光雷达那样的非常规的非重复扫描模式。由于FAST-LIO2并不需要提取特征，自然地它适合这种新型激光雷达。在以下所有实验中，FAST-LIO2使用默认配置（即地图大小为1000m的直接法）。除非另有说明，扫描速率均设置为100Hz，计算平台为之前部分中使用过的DJImanifold2-C。

图6.三种不同的平台：（a）280毫米轴距小型四旋翼无人机搭载前视LivoxAvia激光雷达，（b）手持平台，（c）750毫米轴距四旋翼无人机搭载下视LivoxAvia激光雷达。所有三个平台都搭载相同的DJIManifold-2C机载计算机。真实世界实验的视频可在https://youtu.be/2OvjGnxszf8获得

B. 私有数据集

1）处理时间的详细评估：为了验证FAST-LIO2的实时性能，我们使用手持平台在大型室外-室内混合场景中以100Hz的扫描速率收集序列。传感器在前进约650m后返回起始位置。应该注意的是，LILI-OM也支持固态激光雷达，但它在此数据中效果不理想，因为其特征提取模块在100Hz扫描速率下产生的特征太少。FAST-LIO2实时构建的地图如图7所示，其漂移小（仅0.14m），与卫星地图吻合良好。

图7 大型场景实验

至于计算效率，我们将FAST-LIO2与其前身FAST-LIO[22]在Intel（Manifold2-C）计算机上比较。对于FAST-LIO2，我们还会在机载计算机上的ARM（KhadasVIM3）上进行测试。这两种方法的区别在于，FAST-LIO是一种基于特征的方法，它在每一步都会检索当前视场中的地图点，为kNN搜索构建一个新的静态k-d树。用于处理扫描的各个组件的详细时间消耗显示在表7中。预处理是指数据的接收和格式化，对于FAST-LIO和FAST-LIO2预处理是相同的，用时都低于0.1ms。FAST-LIO的特征提取每次扫描用时0.9ms，这是FAST-LIO2节省的。特征提取导致Fast-LIO比FAST-LIO2产生更少的点数（447对756），因此在状态估计上花费的时间更少（0.99毫秒对1.66毫秒）。因此，两种方法的总里程计时间非常接近（FAST-LIO为1.92毫秒，而FAST-LIO2为1.69毫秒）。这两种方法在制图模块呈现巨大差异，模块包括FAST-LIO的地图点检索和k-d树构建，以及FAST-LIO2的点插入和由于地图移动和树重新平衡而导致的框式删除。可以看出，FAST-LIO每次扫描的平均制图时间超过10毫秒，因此无法实时处理这个大场景。另一方面，FAST-LIO2的制图时间远低于采样周期。FAST-LIO2每次扫描处理756个点（包括里程计和制图）的总时间对于Intel处理器仅为1.82ms，对于ARM处理器为5.23ms。

表7 处理激光雷达扫描时各个组件的平均时间消耗（以毫秒为单位）

每次扫描的时间消耗和地图点数如图8所示。可以看出，在ARM处理器上运行的FAST-LIO2的处理时间偶尔超过采样周期10毫秒，但这种情况很少发生，平均处理时间远低于采样周期。偶尔的超时通常不会影响后续的控制器，因为IMU传播的状态估计可以在这个短时期内使用。在Intel处理器上，FAST-LIO2的处理时间始终低于采样周期。另一方面，由于地图点数量的增加，FAST-LIO的处理时间迅速增加到采样周期以上。注意到，即使面对大量地图点的情况下，FAST-LIO2的处理时间也显著减少。由于FAST-LIO仅保留其当前视场角内的地图点，如果LiDAR面对一个包含很少先前采样的地图点的新区域，该数字可能会下降。如上所述，即使地图点较少，FAST-LIO的处理时间仍然要长得多。此外，由于FAST-LIO在每一步构建一个新的k-d树，构建时间的时间复杂度为O(nlogn)[40]，其中n是当前视场中的地图点数。这就是FAST-LIO的处理时间几乎与地图大小呈线性相关的原因。相比之下，我们ikd-树的增量更新具有O(logn)的时间复杂度，导致处理时间的增长相对于地图大小的增加要慢得多。

图8 FAST-LIO和FAST-LIO2每次LiDAR扫描的处理时间

2）无人机剧烈飞行实验：为了展示FAST-LIO2在移动机器人平台中的应用，我们部署了一个携带LivoxAVIA激光雷达传感器的小型四旋翼无人机，并进行了如图9所示的激烈翻转实验。本次实验中，无人机先从地面起飞，在1.2m高度悬停一段时间，然后进行快速翻转，然后在从FAST-LIO2获取状态反馈的流形模型预测控制器[62]的控制下返回悬停飞行。FAST-LIO2估计的姿态如图9（d）所示，与实际的无人机姿态非常吻合。环境的实时建图如图10所示。另外，图11显示了实验过程中的位置、姿态、角速度和线速度。翻转期间的平均和最大角速度分别达到912度/秒和1198度/秒（从50.8秒到51.2秒）。FAST-LIO2平均每次扫描仅需2.01ms，足以满足控制器的实时性要求。通过提供高精度里程计和100Hz的高分辨率环境3D地图，FAST-LIO2非常适合机器人的实时控制和避障。例如，我们之前的工作[63]展示了FAST-LIO2在自主无人机上的应用，可在复杂的室内和室外环境中躲避动态小物体（低至9毫米）。

图9 翻转实验。(a)小型无人机；(b)在翻转期间显示第一人称视角图像的车载摄像头；(c)无人机翻转过程中的第三人称视角图像；(d)使用FAST-LIO2估计的无人机姿态

图10 翻转过程中FAST-LIO2构建的实际环境和3D地图

图11 无人机翻转实验中的姿态、位置、角速度和线速度

3)快速运动手持实验：在这里，我们在具有大速度和角速度的富有挑战性的快速运动中测试FAST-LIO2。在人行天桥上来回冲刺时，传感器被握在手上（见图12）。图13显示了快速运动手持实验中的姿态、位置、角速度和线速度。可以看出，最大速度达到了7m/s，角速度在±100度/秒左右变化。为了展示FAST-LIO2的性能，实验在同一点开始和结束。本实验的端到端误差小于0.06m（见图13），而总轨迹长度为81m。

图12 FAST-LIO2在快速运动手持实验中的建图结果

图13快速运动手持实验中的姿态、位置、角速度和线速度

C. 户外飞行实验

3D激光雷达的一项重要应用是机载测绘。为了验证FAST-LIO2是否适用于这种可能的应用，我们进行了一次飞行实验。我们部署了一架携带我们激光雷达传感器的更大的无人机。该无人机配备GPS、IMU等飞行航电设备，可根据机载GPS/IMU导航进行航点自动跟踪。需要注意的是无人机配备的GPS和IMU仅用于无人机导航，不用于FAST-LIO2，其仅使用来自LiDAR传感器的数据。本实验中LiDAR扫描速率设置为10Hz。在香港南生围的香港湿地公园的多个地点进行了几次飞行。实时建图结果如图14所示。可以看出FAST-LIO2在这些植被环境中效果很好。可以清楚地看到树冠、道路上的车道标记和路缘石等许多精细结构。图14还显示了FAST-LIO2计算的飞行轨迹。我们将这些轨迹与无人机车载GPS/IMU导航估计的轨迹进行了视觉比较，它们显示出良好的一致性。由于技术上的困难，这里不提供GPS轨迹用于定量评估。最后，这三种环境每次扫描的平均处理时间分别为19.6ms、23.9ms和23.7ms。需要注意的是，LILI-OM在这三个数据序列中都失效了，因为在面向地面时提取的特征太少了。

图14 使用FAST-LIO2进行机载建图的实时建图结果。数据是由无人机在香港湿地公园收集的，该无人机带有一个朝下的LivoxAvia激光雷达。飞行高度为30m(a)、30m(b)和50m(c)

8 结论

本文构造了FAST-LIO2算法，这是一种直接且稳健的LIO框架，其速度明显快于当前最先进的LIO算法，同时在各种数据集上都体现了极大的竞争力或更好的准确性。速度的提高是由于去除了特征提取模块和进行高效的建图。我们开发并验证了一种新颖的增量k-d树（ikd-树）数据结构，它支持动态点插入、删除和并行重建。在开放数据集中的大量实验表明，所提出的ikd-树，对于在激光雷达里程计中的kNN搜索，可以在最新数据结构中实现最佳的整体性能。由于制图效率的提高，在快速运动和稀疏场景中的准确性和鲁棒性也通过在里程计中使用更多点而被提高。FAST-LIO2的另一个好处是，由于去除了特征提取，因此它自然适用于不同的LiDAR，而特征提取必须根据其各自的扫描模式和密度为不同的LiDAR精心设计。

致谢：本项目由大疆（200009538）资助支持。作者感谢大疆提供的资金支持和Livox科技在整个工作过程中提供的设备支持。作者要感谢Ambit-Geospatial在户外空中实验中的帮助。作者还感谢ZhengLiu、GuozhengLu和FangchengZhu的有益讨论。

附录：第VI部分中使用的所有37个序列的详细信息列于表8中。

表8 基准的所有序列的详细信息

本文译自《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