【FAST-LIO2】FAST-LIO2论文详解

paper ： FAST-LIO2: Fast Direct LiDAR-inertial Odometry
code : link
ikd-Tree : link

1. 摘要

本文介绍了FAST-LIO2：一种快速、鲁棒的、通用的激光雷达惯性里程计框架，FASTLIO2以高效紧耦合的方式迭代卡尔曼滤波器为基础，具有两个关键的新颖之处，可实现快速、稳健和精确的激光雷达的建图和导航。

第一该方法是直接将原始点云配准到地图（随后更新地图），而不提取特征，这样可以利用环境中的细微特征，从而提高准确性，这里不采用人工设计的特征提取模块，使其自然适应不同扫描模式的新兴激光雷达；

第二个主要创新点是通过增量k-d树数据结构kd-tree来维护地图，该结构支持增量更新（即点插入、删除）和动态平衡。与现有的动态数据结构（八叉树，R-树，nanoflann k-d树），kd树实现了卓越的整体性能，同时自然支持下采样，我们对来自各种开放式激光雷达数据集的19个序列进行了详尽的基准比较。FAST-LIO2比其他最先进的激光雷达惯性导航系统低得多的计算量实现了持续更高的精度，在小视场的固态激光雷达上也进行了各种真实世环境的实验，总体而言，FAST-LIO2计算效率高，鲁棒性强，多功能，同时仍能实现比现有方法更高的精度，FAST-LIO2和数据结构kd树的实现都是在Github上开源了。

2. 主要贡献

1）开发了一种增量k-d树数据结构ikd树，以高效地表示大型稠密点云地图，除了高效的最近邻搜索外，新的数据结构还支持增量地图更新（即点插入、下采样、点删除）和以最小计算成本进行动态平衡。这些功能使ikd树非常适合于激光雷达里程计和地图应用，从而在计算训练平台上实现100 Hz里程计和建图，ikdTree数据结构工具箱在Github上是开源的（ https://github.com/hku-mars/ikd-Tree ）

2） 由于ikd树计算效率的提高，我们直接将原始点云配准到地图上，这样即使在剧烈运动和非常混乱的环境中，也能实现更精确和可靠的点云配准，我们将这种基于原始点的配准称为直接方法，消除了人工设计的特征提取，使系统自然适用于不同的激光雷达传感器；

3） 将这两项关键技术集成到我们最近开发的紧耦合激光雷达惯性里程计系统FAST-LIO中，系统使用IMU通过严格的反向传播步骤补偿每个点云的运动，并通过流形迭代卡尔曼滤波器估计系统的完整状态，为了进一步加快计算速度，使用了一个新的、数学上等价的卡尔曼增益计算公式，将计算复杂度降低到状态的维度，新系统被称为FAST-LIO2，并在Github开源以造福社区；

4） 作者进行了各种实验来评估开发的ikd树、直接点云配准和整个系统的有效性，在18个不同大小的序列上的实验表明，ikdTree相对于现有的动态数据结构在激光雷达里程计和建图中的应用，对来自各种开放式激光雷达数据集的19个序列进行了详尽的基准比较，结果表明FAST-LIO2以比其他最先进的激光雷达惯性导航系统低得多的计算量实现了更高的精度，最后，文章展示了FAST-LIO2对新兴固态激光雷达收集的具有挑战性的现实世界数据的有效性，这些数据的FoV非常小，包括剧烈运动和无结构环境中。

3. 内容简介

实时构建未知环境的密集三维(3D)地图并同时在地图中定位(即SLAM)对于自主机器人在未知环境中安全导航至关重要。定位为机器人的机载控制器提供状态反馈，而密集的3D地图提供了环境的必要信息(即自由空间和障碍物)用于轨迹规划。基于视觉的SLAM在定位上非常准确，但只维护一个稀疏的特征地图，并且受光照变化和严重运动模糊的影响。另一方面，基于视觉传感器的实时密集制图在高分辨率和精度上，仅使用机器人机载计算资源，仍然是一个巨大的挑战。

由于能够直接提供环境的密集、主动和精确的深度测量，3D激光雷达传感器已成为另一种对机器人至关重要的传感器。在过去的十年中，激光雷达在许多自主机器人中发挥了越来越重要的作用，如自动驾驶汽车和自主无人机。激光雷达技术的最新发展使得更轻巧、成本效益更高(成本范围与全球快门相机相似)、性能更高(在数百米的测量范围内厘米级精度)的固态激光雷达商业化和大规模生产成为可能，引起了最近的研究兴趣。这些激光雷达的成本、尺寸、重量和功耗的显著降低，有望惠及现有和新兴的机器人应用的广泛范围。

采用基于激光雷达的SLAM方法的核心要求是，在有限的机载计算资源下，获得准确、低延迟的状态估计和密集的3D地图。然而，高效准确的激光雷达里程计和制图仍然是具有挑战性的问题：
1)当前的激光雷达传感器每秒产生大量的3D点，从数十万到数百万不等。在实时和有限的机载计算资源上处理如此大量的数据需要激光雷达里程计方法具有高计算效率；
2)为了减少计算负载，通常基于局部平滑性提取特征点，如边缘点或平面点。然而，特征提取模块的性能很容易受到环境的影响。
例如，在没有大平面或长边缘的无结构环境中，特征提取将导致很少的特征点。如果**激光雷达视场(FoV)小，这种情况会大大恶化，这是新兴固态激光雷达的典型现象。**此外，特征提取也因激光雷达而异，取决于扫描模式(例如，旋转、基于棱镜的、基于MEMS的)和点密度。因此，采用激光雷达里程计方法通常需要大量的手工工程工作；
3)激光雷达点通常是顺序采样的，而传感器在连续运动中。这个过程会产生显著的运动失真，影响里程计和制图的性能，尤其是当运动严重时。惯性测量单元(IMU)可以缓解这个问题，但引入了额外的状态(例如，偏差，外参)来估计；
4)激光雷达通常具有较长的测量范围(例如，数百米)，但在扫描线上的分辨率相当低。结果点云测量在大的3D空间中稀疏分布，需要一个大型和密集的地图来注册这些稀疏点。此外，地图需要支持有效的查询以进行对应搜索，同时实时更新新的测量。维护这样一个地图是一个非常具有挑战性的任务，与视觉测量非常不同，视觉测量的图像测量具有高分辨率，因此只需要一个稀疏的特征地图，因为只要特征点落在FoV内，地图中的特征点总是可以找到对应点。

在这项工作中，我们通过两项关键新技术解决了这些问题：增量k-d树和直接点注册。更具体地说，我们的贡献如下：
1)我们开发了一种增量k-d树数据结构ikd-Tree，以高效地表示大型密集点云地图。除了高效的最近邻搜索外，新数据结构支持增量地图更新(即点插入、树上下采样、点删除)和动态再平衡，计算成本最小。这些特性使ikd-Tree非常适合激光雷达里程计和制图应用，实现了在计算受限的平台(如基于Intel i7的微无人机机载计算机甚至基于ARM的处理器)上100 Hz的里程计和制图。ikd-Tree数据结构工具箱在Github上开源。
2)由于ikd-Tree的计算效率提高，我们直接将原始点注册到地图上，这使得即使在激进的运动和非常复杂的环境下也能实现更准确可靠的扫描注册。我们将这种基于原始点的注册称为直接方法，以类比视觉SLAM。手工工程特征提取使系统自然适用于不同的激光雷达传感器；
3)我们将这两种关键技术集成到我们最近开发的一个完整的紧耦合激光雷达-惯性里程计系统FAST-LIO中。该系统使用IMU通过严格的反向传播步骤补偿每个点的运动，并通过流形迭代卡尔曼滤波器估计系统的全部状态。为了进一步提高计算速度，使用新的数学等价公式计算卡尔曼增益，将计算复杂度从测量维度降低到状态维度。新系统被称为FAST-LIO2，并在Github上开源，以惠及社区；
4)我们进行各种实验以评估开发的ikd-Tree、直接点注册和整个系统的有效性。对18个各种大小的序列的实验表明，ikd-Tree在激光雷达里程计和制图应用中的表现优于现有的动态数据结构(八叉树、R*树、nanoflann k-d树)。对来自各种开放激光雷达数据集的19个序列的详尽基准比较表明，FAST-LIO2在计算负载更低的情况下，比其他最先进的激光雷达-惯性导航系统实现更高的精度。最后展示了FAST-LIO2在由新兴固态激光雷达收集的具有非常小FoV的具有挑战性的现实世界数据上的有效性，包括激进的运动(例如，旋转速度高达1000度/秒)和无结构环境。

剩余的论文组织如下：第II节，我们讨论相关工作。我们在第III、IV和V节分别概述了完整的系统流程和每个关键组件的细节。第VI节展示了开放数据集上的基准比较，第VII节报告了现实世界实验，最后是第VIII节的结论。

A.激光雷达(-惯性)里程计

现有的3D激光雷达SLAM工作通常继承了[23]中提出的LOAM结构。它由三个主要模块组成：特征提取、里程计和制图。为了减少计算负载，新的激光雷达扫描首先通过基于局部平滑性的特征点(即边缘和平面)提取。然后里程计模块(扫描到扫描)匹配两个连续扫描的特征点，以获得粗略但实时(例如，10Hz)的激光雷达姿态里程计。有了里程计，多个扫描被组合成一个扫描，然后注册并合并到全局地图(即制图)。在这个过程中，地图点被用来构建一个k-d树，从而实现非常高效的k最近邻搜索(kNN搜索)。然后，通过迭代最近点(ICP)方法实现点云注册，其中在每次迭代中，地图中的几个最近点形成一个平面或边缘，目标点属于其中。为了降低构建k-d树的时间，地图点以规定的分辨率进行下采样。优化的制图过程通常以更低的速率(1-2Hz)进行。

后续的激光雷达里程计工作保持了与LOAM相似的框架。例如，Lego-LOAM引入了地面点分割以减少计算，并引入了一个环路闭合模块以减少长期漂移。此外，LOAM-Livox采用了LOAM以适应新兴的固态激光雷达。为了应对小FoV和非重复扫描，其中两个连续扫描的特征点很少有对应点，LOAM-Livox的里程计是通过直接将新扫描注册到全局地图获得的。这种直接扫描到地图的注册提高了里程计的精度，但增加了构建更新地图点的k-d树的计算量。将IMU纳入可以显著提高激光雷达里程计的准确性和鲁棒性，因为它提供了ICP所需的良好初始姿态。此外，高比率IMU测量可以有效地补偿激光雷达扫描中的运动失真。LION是一种松耦合的激光雷达-惯性SLAM方法，它保持了LOAM的扫描到扫描注册，并引入了可观测性意识检查到里程计以降低点数，从而节省计算。更紧密耦合的激光雷达-惯性融合工作在小尺寸本地地图上执行里程计，该局部地图由固定数量的最新激光雷达扫描(或关键帧)组成。与扫描到扫描注册相比，扫描到本地地图的注册通常更准确，因为它使用了更多的最新信息。更具体地说，LIOM提出了一种紧密耦合的激光雷达-惯性融合方法，其中IMU预积分被引入到里程计中。LILIOM为非重复扫描激光雷达开发了一种新的特片提取方法，并在由20个最新激光雷达扫描组成的小地图中执行扫描注册以进行里程计。LIO-SAM的里程计需要一个9轴IMU来产生姿态测量作为扫描注册的先验。LINS引入了一种紧密耦合的迭代卡尔曼滤波器和以机器人为中心的公式到里程计中的激光雷达姿态优化。由于上述作品中的本地地图通常很小以获得实时性能，里程计很快漂移，需要低速率的制图过程，如地图细化(LINS)、滑动窗口联合优化(LILI-OM和LIOM)和因子图平滑(LIO-SAM)。与上述方法相比，FAST-LIO引入了一种正式的反向传播，精确考虑了扫描中每个点的采样时间，并通过由IMU测量驱动的严格运动学模型补偿运动失真。此外，使用新的卡尔曼增益公式将计算复杂度从测量维度降低到状态维度。新
FAST-LIO2基于FAST-LIO[22]，因此继承了紧耦合融合框架，特别是反向传播解决运动失真和快速卡尔曼增益计算提高了效率。为了系统地解决日益增长的计算问题，我们提出了一种新的数据结构ikd-Tree，它支持每一步的增量地图更新和高效的kNN查询。得益于大幅度降低的计算负载，里程计是通过直接将原始激光雷达点注册到地图上来进行的，从而提高了里程计和制图的准确性和鲁棒性，特别是当新扫描没有显著特征时（例如，由于小FoV和/或无结构环境）。与上述所有使用特征点的紧耦合激光雷达-惯性方法相比，我们的方法更加轻量级，并实现了更高的制图速率和里程计精度，并消除了特征提取的参数调整需求。

我们工作中直接注册原始点的想法已经在LION[28]中探索过，然而，如上所述，它是一种松耦合方法。这个想法也与[26]中提出的广义ICP (G-ICP)非常相似，其中将点注册到地图中的一个小局部平面上。这最终假设环境是平滑的，因此可以被视为局部的平面。然而，广义ICP的计算负载通常很大[33]。基于法线分布变换（NDT）[34]–[36]的其他作品也注册原始点，但NDT的稳定性比ICP低，并且可能在某些场景中发散[36]。

B. 制图中的动态数据结构

为了实现实时制图，需要一个动态数据结构来支持增量更新和高效的kNN搜索。一般来说，kNN搜索问题可以通过为数据点构建空间索引来解决，可以分为两类：划分数据和分割空间。划分数据的一个著名实例是R树[37]，它根据空间中数据的接近度将数据集群到潜在的重叠轴对齐的长方体中。各种R树通过线性、二次和指数复杂度分割节点，都支持最近邻搜索和点状更新（插入、删除和重新插入）。此外，R树还支持在给定搜索区域或满足给定条件的情况下搜索目标数据点。R树的另一个版本是R树，它优于原始的[38]。R树通过最小重叠标准处理插入，并应用强制重新插入原则进行节点分割算法。

八叉树[39]和k维树（k-d树）[40]是两种用于kNN搜索的分割空间的著名数据结构类型。八叉树通过递归地将空间等分为八个轴对齐的立方体来组织3D点云。立方体的细分在立方体为空或满足停止规则（例如，最小分辨率或最小点数）时停止。新点被插入到八叉树的叶节点，如果需要，可以应用进一步的细分。八叉树支持kNN搜索和框状搜索，返回给定轴对齐长方体中的数据点。

k-d树是一种二叉树，其节点表示一个轴对齐的超平面将空间分成两部分。在标准构建规则中，分割节点被选为最长维度上的中点，以实现紧凑的空间划分[41]。当考虑到在制图中的低维数据特征和主存储器上的存储时，比较研究表明k-d树在kNN问题上实现了最佳性能[42, 43]。然而，向k-d树中插入新点和从k-d树中删除旧点会破坏树的平衡属性；因此，需要重建以重新平衡树。使用k-d树库的制图方法，如ANN[44]、libnabo[43]和FLANN[45]，完全重建k-d树以更新地图，这导致了大量的计算。尽管在3D图形应用中已经彻底研究了基于硬件重建k-d树的方法[46]–[49]，但提出的方法在很大程度上依赖于计算资源，这些资源在机器人应用的机载计算机上通常是有限的。

而不是全面重建树，Galperin等人提出了一个替代k-d树，其中重建部分应用于不平衡的子树，以维持整个树的松散平衡属性[50]。另一种实现增量操作的方法是维护一组k-d树，类似于[51, 52]中的方法，并重建精心选择的子集。Bkd树在主存储器中维护一个最大大小为M的k-d树T0，并在外部存储器上维护一组k-d树Ti，其中第i棵树的大小为2^(i−1)M[53]。当树T0满时，从T0中提取点并插入到第一个空树Tk中。最先进的实现nanoflann k-d树利用对数结构进行增量更新，而延迟标签仅标记已删除的点，而不会将它们从树（因此内存）中移除[54]。

我们提出了一种基于替代k-d树[50]的动态数据结构，称为增量k-d树(ikd-Tree)，以实现实时制图。我们的ikd-Tree支持点状插入，带树上下采样，这在制图中是一个常见要求，而下采样必须在将新点插入到其他动态数据结构之前在外面完成[38, 39, 54]。当需要在给定区域中删除不必要的点时，常规形状（例如，长方体），现有的R树和八叉树实现搜索给定空间内的点并逐个删除，而常见的k-d树使用半径搜索来获取点索引。与这种间接且低效的方法相比，ikd-Tree通过维护范围信息和延迟标签直接删除给定轴对齐长方体中的点。被标记为“已删除”的点在重建过程中被移除。此外，尽管在应用部分重新平衡方法（如替代k-d树[50]和nanoflann k-d树[54]）后可以进行增量更新，但使用k-d树的制图方法在大量点上重建时会遭受间歇性延迟。为了克服这一点，通过并行重建避免了ikd-Tree中的显著延迟，同时保证了主线线程中的实时能力和准确性。

4. 系统概述

FAST-LIO2的流程如图1所示。顺序采样的激光雷达原始点首先在10ms（用于100Hz更新）和100ms（用于10Hz更新）的时间间隔内累积。累积的点云称为扫描。为了执行状态估计，新扫描中的点通过紧耦合迭代卡尔曼滤波框架（见IV节，红色大虚线块）注册到在大局部地图中维护的地图点（即里程计）。大局部地图中的全局地图点由增量k-d树结构ikd-Tree（见V节，蓝色大虚线块）组织。如果当前激光雷达的视场范围跨越地图边界，则在激光雷达姿态最远的地图区域中的历史点将从ikd-Tree中删除。因此，ikd-Tree在一个大立方体区域内跟踪所有地图点（在本文中称为“地图大小”），并用于计算状态估计模块中的残差。优化后的姿态最终将新扫描中的点注册到全局框架，并通过以里程计的速率插入到ikd-Tree中将它们合并到地图中（即制图）。

状态估计
FAST-LIO2的状态估计是从FAST-LIO[22]继承的紧耦合迭代卡尔曼滤波器，但进一步整合了激光雷达-IMU外参的在线校准。在这里，我们简要解释了滤波器的基本公式和工作流程，并请读者参考[22]以获取更多详细信息。

4.1 运动学模型:

首先推导出系统模型，包括状态转换模型和测量模型。

1. 状态转换模型：

以第一个IMU框架（表示为I）作为全局框架（表示为G），并表示 ${}^I{T}_L={(}^I{R}_L{,}^I{p}_L)$ 为激光雷达和IMU之间的未知外参，运动学模型为：

$$\begin{array}{rl}{}^G{\dot{R}}_I& {=}^G{R}_I{\lfloor {\omega }_m-b_{\omega }-n_{\omega }\rfloor }_{\wedge },\\ {}^G{\dot{p}}_I& {=}^G{v}_I,\\ {}^G{\dot{v}}_I& {=}^G{R}_I(a_m-b_a-n_a){+}^{G}g\\ {\dot{b}}_{\omega }& =n_{b\omega },\\ {\dot{b}}_a& =n_{ba},\\ {}^G\dot{g}& =0,\\ {}^I{\dot{R}}_L& =0,\\ {}^I{\dot{p}}_L& =0\end{array}$$

其中 ${}^G{p}_I{,}^G{R}_I$ 表示全局框架中的IMU位置和姿态，${}^{G}g$ 是全局框架中的重力向量，$a_m$ 和 ${\omega }_m$ 是IMU测量值，$n_a$ 和 $n_{\omega }$ 表示 $a_m$ 和 ${\omega }_m$ 的测量噪声，$b_a$ 和 $b_{\omega }$ 是IMU偏置，建模为由 $n_{ba}$ 和 $n_{b\omega }$ 驱动的随机游走过程，符号 $\lfloor a\rfloor \wedge$ 表示向量 $a\in R^3$ 的斜对称叉积矩阵。

设i为IMU测量值的索引。基于[22]中定义的 $⊞$ 操作，连续运动学模型(1)可以在IMU采样周期 $\Delta t$ 处离散化[55]：

$$x_{i+1}=x_i⊞\left(\Delta tf(x_i,u_i,w_i)\right)(2)$$

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

$$\begin{array}{rl}& \mathcal{M}\triangleq SO(3)\times R^{15}\times SO(3)\times R^3;\dim (\mathcal{M})=24\\ & x\triangleq {\left[\begin{array}{llllllcc}{}^G{R}_I^T& {}^G{p}_I^T& {}^G{v}_I^T& b_{\omega }^T& b_a^T& {}^G{g}^T& {}^I{R}_L^T& {}^I{p}_L^T\end{array}\right]}^T\in \mathcal{M}\\ & u\triangleq {\left[\begin{array}{ll}{\omega }_m^T& a_m^T\end{array}\right]}^T,w\triangleq {\left[\begin{array}{llll}{n}_{\omega }^T& n_a^T& n_{b\omega }^T& n_{ba}^T\end{array}\right]}^T\\ & f(x,u,w)=\left[\begin{array}{c}{\omega }_m-b_{\omega }-n_{\omega }\\ {}^G{v}_I+\frac{1}{2}({}^G{R}_I(a_m-b_a-n_a)+{}^{G}g)\Delta t\\ {}^G{R}_I(a_m-b_a-n_a)+{}^{G}g\\ n_{b\omega }\\ n_{ba}\\ 0_{3\times 1}\\ 0_{3\times 1}\end{array}\right]\in R^{24}\\ & \end{array}$$

2. 测量模型

激光雷达通常依次采样点。因此，当激光雷达进行连续运动时，所得到的点以不同的位姿被采样。为了纠正这种扫描期间的运动，采用了[22]中提出的反向传播方法，该方法基于IMU测量，估计扫描中每个点相对于扫描结束时位姿的激光雷达位姿。估计的相对位姿使我们能够根据扫描中每个点的确切采样时间，将所有点投影到扫描结束时间。因此，扫描中的点可以被视为在扫描结束时间同时采样。

设k为激光雷达扫描的索引，$\left\{{}^L{p}_j,j=1,\cdots ,m\right\}$ 为在扫描结束时在局部激光雷达坐标系L中采样的k-th扫描中的点。由于激光雷达测量噪声，每个测量点${}^L{p}_j$通常受到噪声${}^L{n}_j$的污染，包括测距和指向波束的噪声。消除此噪声将导致局部激光雷达坐标系中的真实点位置${}^L{p}_j^{gt}$：

$${}^L{p}_j^{gt}={}^L{p}_j+{}^L{n}_j.(3)$$

这个真实的点，在利用相应的激光雷达位姿${}^G{T}_{I_k}=({}^G{R}_{I_k},{}^G{p}_{I_k})$和外参${}^I{T}_L$投影到全局框架后，应该完全位于地图中的一个局部小平面上，即：

$$0={}^G{u}_j^T({}^G{T}_{I_k}{}^I{T}_L({}^L{p}_j+{}^L{n}_j)-{}^G{q}_j)$$

其中${}^G{u}_j$是相应平面的法向量，${}^G{q}_j$是平面上的一点（见图2）。应当注意，${}^G{T}_{I_k}$和${}^I{T}_{L_k}$都包含在状态向量$x_k$中。因此，由第j个点测量${}^L{p}_j$贡献的测量可以(4)总结为更紧凑的形式如下：

$$0=h_j(x_k,{}^L{p}_j+{}^L{n}_j),(5)$$

这为状态向量$x_k$定义了一个隐式测量模型。

4.2 迭代卡尔曼滤波器

基于在流形 $\mathcal{M}\triangleq SO(3)\times R^{15}\times SO(3)\times R^3$ 上制定的状态模型(2)和测量模型(5)，我们采用一个直接在流形 $\mathcal{M}$ 上操作的迭代卡尔曼滤波器，遵循[55]和[22]中的程序。它包括两个关键步骤：每个IMU测量的传播和每个激光雷达扫描的迭代更新，这两个步骤都在流形 $\mathcal{M}$ 上自然估计状态，从而避免了任何重新归一化。由于IMU测量通常比激光雷达扫描的频率更高（例如，IMU测量为200Hz，激光雷达扫描为10Hz至100Hz），因此在更新之前通常执行多个传播步骤。

1) 传播：

假设在融合上一个（即第 $k-1$ 个）激光雷达扫描后的最佳状态估计为 ${\overline{x}}_{k-1}$，协方差矩阵为 ${\overline{P}}_{k-1}$。当IMU测量到达时，执行前向传播。更具体地说，通过将过程噪声 $w_i$ 设置为零，根据(2)传播状态和协方差：

$$\begin{array}{rl}{\hat{x}}_{i+1}& ={\hat{x}}_i⊞\left(\Delta tf\left({\hat{x}}_i,u_i,0\right)\right);{\hat{x}}_0={\overline{x}}_{k-1},\\ {\hat{P}}_{i+1}& =F_{{\tilde{x}}_i}{\hat{P}}_i{F}_{{\tilde{x}}_i}^T+F_{w_i}{Q}_i{F}_{w_i}^T;{\hat{P}}_0={\overline{P}}_{k-1},\end{array}(6)$$

其中 $Q_i$ 是噪声 $w_i$ 的协方差，矩阵 $F_{{\tilde{x}}_i}$ 和 $F_{w_i}$ 计算如下（更抽象的推导见[55]，更具体的推导见[22]）：

$$\begin{array}{rl}{F}_{{\tilde{x}}_i}& ={\frac{\partial \left(x_{i+1}\boxminus {\hat{x}}_{i+1}\right)}{\partial {\tilde{x}}_i}|}_{{\tilde{x}}_i=0,}{w}_i=0\\ F_{w_i}& ={\frac{\partial \left(x_{i+1}\boxminus {\hat{x}}_{i+1}\right)}{\partial w_i}|}_{{\tilde{x}}_i=0,}{w}_i=0\end{array}(7)$$

前向传播继续进行，直到到达新的（即第k个）扫描的结束时间，传播的状态和协方差表示为 ${\hat{x}}_k,{\hat{P}}_k$。

2) 残差计算：

假设当前迭代更新（见IV-B3节）时状态$x_k$的估计值为${\hat{x}}_k^{\kappa }$，当$\kappa =0$（即第一次迭代之前），${\hat{x}}_k^{\kappa }={\hat{x}}_k$，即来自步骤(6)中传播的预测状态。然后，我们将每个测量的激光雷达点${}^L{p}_j$投影到全局框架${}^G{\hat{p}}_j={}^G{\hat{T}}_{I_k}^{\kappa I}{\hat{T}}_{L_k}^{\kappa L}{p}_j$，并在ikd-Tree表示的地图中搜索其最近的5个点（见V-A节）。找到的最近邻点随后用于拟合具有法向量${}^G{u}_j$和质心${}^G{q}_j$的局部小平面，它们在测量模型中使用（见(4)和(5)）。此外，对测量方程(5)进行一阶近似得到：

$$\begin{array}{rl}0& =h_j\left(x_k,{}^L{n}_j\right)\simeq h_j\left({\hat{x}}_k^{\kappa },0\right)+H_j^{\kappa }{\tilde{x}}_k^{\kappa }+v_j\\ & =z_j^{\kappa }+H_j^{\kappa }{\tilde{x}}_k^{\kappa }+v_j\end{array}$$

其中${\tilde{x}}_k^{\kappa }=x_k\boxminus {\hat{x}}_k^{\kappa }$（或等价地$x_k={\hat{x}}_k^{\kappa }⊞{\tilde{x}}_k^{\kappa }$），$H_j^{\kappa }$是$h_j\left({\hat{x}}_k^{\kappa }⊞{\tilde{x}}_k^{\kappa },{}^L{n}_j\right)$相对于${\tilde{x}}_k^{\kappa }$的雅可比矩阵，在零处求值，$v_j\in \mathcal{N}\left(0,R_j\right)$是由于原始测量噪声${}^L{n}_j$，$z_j^{\kappa }$称为残差：

$$z_j^{\kappa }=h_j\left({\hat{x}}_k^{\kappa },0\right)=u_j^T\left({}^G{\hat{T}}_{I_k}^{\kappa }I{\hat{T}}_{L_k}^{\kappa }{}^L{p}_j-{}^G{q}_j\right)(9)$$

3) 迭代更新：

从IV-B1节中的传播状态${\hat{x}}_k$和协方差${\hat{P}}_k$对未知状态$x_k$施加先验高斯分布。更具体地说，${\hat{P}}_k$表示以下误差状态的协方差：

$$\begin{array}{rl}{x}_k\boxminus {\hat{x}}_k& =\left({\hat{x}}_k^{\kappa }⊞{\tilde{x}}_k^{\kappa }\right)\boxminus {\hat{x}}_k={\hat{x}}_k^{\kappa }\boxminus {\hat{x}}_k+J^{\kappa }{\tilde{x}}_k^{\kappa }\\ & \sim \mathcal{N}\left(0,{\hat{P}}_k\right)\end{array}$$

其中$J^{\kappa }$是$\left({\hat{x}}_k^{\kappa }⊞{\tilde{x}}_k^{\kappa }\right)\boxminus {\hat{x}}_k$相对于${\tilde{x}}_k^{\kappa }$的部分微分，在零处求值：

$$J^{\kappa }=\left[\begin{array}{cccc}A{\left({\delta }^G{\theta }_{I_k}\right)}^{-T}& 0_{3\times 15}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{15\times 3}& I_{15\times 15}& 0_{3\times 3}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 15}& A{\left({\delta }^I{\theta }_{L_k}\right)}^{-T}& 0_{3\times 3}\\ 0_{3\times 3}& 0_{3\times 15}& 0_{3\times 3}& I_{3\times 3}\end{array}\right]$$

其中$A(\cdot {)}^{-1}$在[22,55]中定义，${\delta }^G{\theta }_{I_k}={}^G{\hat{R}}_{I_k}^{\kappa }\boxminus {}^G{\hat{R}}_{I_k}$和${\delta }^I{\theta }_{L_k}={}^I{\hat{R}}_{L_k}^{\kappa }\boxminus {}^I{\hat{R}}_{L_k}$分别是IMU姿态和旋转外参的误差状态。对于第一次迭代，${\hat{x}}_k^{\kappa }={\hat{x}}_k$，然后$J^{\kappa }=I$。

除了先验分布，我们还有由于测量(8)导致的状态下的分布：

$$-v_j=z_j^{\kappa }+H_j^{\kappa }{\tilde{x}}_k^{\kappa }\sim \mathcal{N}\left(0,R_j\right)(12)$$

将(10)中的先验分布与(12)中的测量模型相结合，得到状态$x_k$的后验分布，等效地表示为${\tilde{x}}_k^{\kappa }$及其最大后验估计(MAP)：

$$\underset{{\tilde{x}}_k^{\kappa }}{\min }\left({\Vert x_k\boxminus {\hat{x}}_k\Vert }_{{\hat{P}}_k}^2+\sum _{j=1}^m{\Vert z_j^{\kappa }+H_j^{\kappa }{\tilde{x}}_k^{\kappa }\Vert }_{R_j}^2\right)$$

其中$\Vert x{\Vert }_M^2=x^T{M}^{-1}x$。这个MAP问题可以通过迭代卡尔曼滤波器解决如下（为了简化符号，设$H={\left[H_1^{{\kappa }^T},\cdots ,H_m^{{\kappa }^T}\right]}^T,R=\mathrm{diag}\left(R_1,\cdots R_m\right),P={\left(J^{\kappa }\right)}^{-1}{\hat{P}}_k{\left(J^{\kappa }\right)}^{-T}$，和$z_k^{\kappa }={\left[z_1^{{\kappa }^T},\cdots ,z_m^{{\kappa }^T}\right]}^T$）：

$$\begin{array}{rl}K& ={\left(H^T{R}^{-1}H+P^{-1}\right)}^{-1}{H}^T{R}^{-1},\\ {\hat{x}}_k^{\kappa +1}& ={\hat{x}}_k^{\kappa }⊞\left(-K{z}_k^{\kappa }-(I-KH){\left(J^{\kappa }\right)}^{-1}\left({\hat{x}}_k^{\kappa }\boxminus {\hat{x}}_k\right)\right).\end{array}(14)$$

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

上述过程重复进行直到收敛（即$\Vert {\hat{x}}_k^{\kappa +1}\Vert -\Vert {\hat{x}}_k^{\kappa }\Vert <ϵ$）。收敛后，最优状态和协方差估计值为：

$${\overline{x}}_k={\hat{x}}_k^{\kappa +1},{\overline{P}}_k=(I-KH)P(15)$$

有了状态更新${\overline{x}}_k$，每个激光雷达点$\left({}^L{p}_j\right)$在k-th扫描中通过以下方式转换到全局框架：

$${}^G{\overline{p}}_j={}^G{\overline{T}}_{I_k}{}^I{\overline{T}}_{L_k}{}^L{p}_j;j=1,\cdots ,m.$$

转换后的激光雷达点$\left\{{}^G{\overline{p}}_j\right\}$被插入到由ikd-Tree表示的地图中（见V节）。我们的状态估计在算法1中总结。

4.3 地图

A. 地图管理

地图点被组织进一个ikd-Tree中，该树通过以里程计的速率合并新的点云扫描动态增长。为了防止地图大小无限制增长，只有围绕当前激光雷达位置长度为L的大局部区域内的地图点才被维护在ikd-Tree上。

图3展示了一个2D示例。地图区域被初始化为一个以初始激光雷达位置$p_0$为中心长度为L的立方体。假设激光雷达的探测区域是一个以激光雷达当前位置为中心的探测球，该位置由(15)获得。探测球的半径假设为 $r=\gamma R$，其中R是激光雷达视场范围，$\gamma$是一个大于1的松弛参数。当激光雷达移动到新位置 $p’$ 使得探测球触碰地图边界时，地图区域会向增加激光雷达探测区域与触碰边界之间距离的方向移动。地图区域移动的距离设置为常数 $d=(\gamma -1)R$。所有在新旧地图区域之间差集区域内的点将通过在V-C中详述的框状删除操作从ikd-Tree中删除。

图3. 地图区域管理的2D演示。在(a)中，蓝色矩形是初始地图区域，长度为L。红色圆圈是以初始激光雷达位置$p_0$为中心的初始探测区域。在(b)中，探测区域（虚线红圈）移动到新位置 $p^{\prime }$（实线红圈），地图边界被触碰。地图区域移动到新位置（绿色矩形），移动距离为$d$。差集区域内的点（橙色区域）从地图（即ikd-Tree）中移除。

B 数据结构：树节点结构

树结构和构建

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

1. 结构体 TreeNode:

2. PointType point;
3. TreeNode* leftchild,* rightchild;
4. int axis;
5. int treesize, invalidnum;
6. bool deleted, treedeleted;
7. CuboidVertices range;
8. end

2. 构建：构建ikd-Tree与在[40]中构建静态k-d树类似。ikd-Tree递归地沿最长远维的中点分割空间，直到子空间中只有一个点。数据结构中的属性在构建期间初始化，包括计算（子）树的大小和范围信息。

C. 增量更新

ikd-Tree上的增量更新指的是在第V-D节详细说明的动态再平衡之后的增量操作。支持两种类型的增量操作：点状操作和框状操作。点状操作插入、删除或重新插入单个点到k-d树中，而框状操作插入、删除或重新插入给定轴对齐长方体中的所有点。在这两种情况下，点插入进一步与树上下采样集成，将地图维持在预定分辨率。在本文中，我们仅解释点状插入和框状删除，因为它们是FAST-LIO2的地图管理所必需的。有兴趣的读者可以访问我们的Github仓库3和其中包含的技术文档，以获取更多详细信息。

1) 带树上下采样的点插入：

考虑到机器人应用，我们的ikd-Tree支持同时点插入和地图下采样。算法详见算法2。对于来自状态估计模块(见算法1)的给定点$p$ 和下采样分辨率$l$ ，算法将空间均匀分割成长度为 $l$ 的立方体，然后找到包含点$p$ 的立方体$C_D$（第2行）。算法只保留最接近立方体$C_D$中心$p_{\text{center}}$的点。这是通过首先在k-d树上搜索所有包含在$C_D$ 中的点，并将它们与新点$p$一起存储在点数组$V$中（第4-5行）来实现的。通过比较$V$中每个点与中心$p_{\text{center}}$的距离来获得最近点$P_{\text{nearest}}$（第6行）。然后删除现有点（第7行），之后将最近点$P_{\text{nearest}}$插入k-d树（第8行）。框状搜索的实现与第V-C2节中介绍的框状删除类似。

算法 2：带树上下采样的点插入

输入：下采样分辨率( l )，要插入的新点( p )，是否并行重建的开关( SW )

1. 算法开始

2	$C_D\leftarrow \text{FindCube}(l,p)$
3	$P_{\text{center}}\leftarrow \text{Center}(C_D)$;
4	$V\leftarrow \text{BoxwiseSearch}(\text{RootNode},C_D)$;
5	$V.push(p);$
6	$P_{\text{nearest}}\leftarrow \text{FindNearest}(V,p_{\text{center}})$;
7	$\text{BoxwiseDelete}(\text{RootNode},C_D)$
8	$\text{Insert}(\text{RootNode},P_{\text{nearest}},NULL,SW);$
9	算法结束

11. Function Insert(T, p, father, SW)
12.     if T is empty then
13.         Initialize(T, p, father)
14.     else
15.         ax ← T.axis
16.         if p[ax] < T.point[ax] then
17.             Insert(T.leftchild, p, T, SW)
18.         else
19.             Insert(T.rightchild, p, T, SW)
20.     end
21.     AttributeUpdate(T)
22.     Rebalance(T, SW)
23. end
24. End Function

这段伪代码描述了在二叉树中插入一个新节点的过程。它递归地将新点p插入到适当的位置，并在插入后更新节点属性和进行树的再平衡。

2) 使用延迟标签的框状删除：

在删除操作中，我们使用延迟删除策略。也就是说，点不是立即从树中删除，而是通过将属性deleted设置为true来标记为“已删除”（见数据结构，第6行）。如果节点T上的所有子树的节点都已被删除，则将T的属性treedeleted设置为true。

因此属性deleted和treedeleted被称为延迟标签。被标记为“已删除”的点将在重建过程中从树中删除（见第V-D节）。

框状删除是利用属性范围中的范围信息和树上的延迟标签来实现的。如第V-B节所述，属性范围由轴对齐的外接长方体$C_T$表示。伪代码显示在算法3中。给定要删除的点的长方体$C_0$，从根节点T开始递归搜索树，并比较外接长方体$C_T$与给定长方体$C_0$)。如果$C_T$和$C_0$之间没有交集，则直接返回而不更新树（第2行）。如果外接长方体$C_T$完全包含在给定长方体$C_0$中，则将属性deleted和treedeleted设置为true（第5行）。由于（子）树上的所有点都被删除了，属性invalidnum等于treesize（第6行）。对于$C_T$与$C_O$相交但不包含的条件，如果当前点$p$包含在$C_0$中，则首先从树中删除该点（第9行），之后递归查看子节点（第10-11行）。在框状删除操作后，应用当前节点T的属性更新和平衡维护（第12-13行）。

算法 3：基于盒子的删除 (Box-wise Delete)

输入：操作立方体 ( C_O )，k-d 树节点 ( T )，并行重构开关 ( SW )

函数：BoxwiseDelete(( T, C_O, SW ))

1. ( C_T \leftarrow T.range )
2. 如果 ( C_T \cap C_O = \emptyset ) 则返回
3. 如果 ( C_T \subseteq C_O ) 则：
4. ( T.treedelete \leftarrow true )
5. ( T.delete \leftarrow true )
6. ( T.invalidnum \leftarrow T.treesize )
7. 否则：
   1. ( p \leftarrow T.point )
   2. 如果 ( p \subset C_O ) 则 ( T.treedelete \leftarrow true )
   3. 调用 BoxwiseDelete(( T.leftchild, C_O, SW ))
   4. 调用 BoxwiseDelete(( T.rightchild, C_O, SW ))
   5. 调用 AttributeUpdate(( T ))
   6. 调用 Rebalance(( T, SW ))

3) 属性更新：

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

D. 再平衡

ikd-Tree在每次增量操作后主动监控平衡属性，并通过仅重建相关子树来动态自我再平衡。

1) 平衡标准：

平衡标准由两个子标准组成：$\alpha$-平衡标准和$\alpha$-删除标准。假设ikd-Tree的一个子树以T为根。如果满足以下条件，则该子树是$\alpha$-平衡的：

$$\begin{array}{rl}& S(T\text{.leftchild})<{\alpha }_{\text{bal}}(S(T)-1)\\ & S(T\text{.rightchild})<{\alpha }_{\text{bal}}(S(T)-1)\end{array}$$

其中${\alpha }_{\text{bal}}\in (0.5,1)$，$S(T)$是节点T的treesize属性。

以T为根的子树的$\alpha$-删除标准是

$$I(T)<{\alpha }_{\text{del}}S(T)(18)$$

其中${\alpha }_{\text{del}}\in (0,1)$，$I(T)$表示子树上的无效节点数（即节点T的属性invalidnum）。

如果ikd-Tree的子树满足这两个标准，则该子树是平衡的。如果所有子树都是平衡的，则整个树是平衡的。违反任一标准将触发重建过程，以重新平衡该子树：$\alpha$-平衡标准维持树的最大高度。可以很容易地证明，$\alpha$-平衡树的最大高度是${\log }_{1/{\alpha }_{\text{bal}}}(n)$，其中n是树的大小；$\alpha$-删除标准确保子树上的无效节点（即，标记为“已删除”的节点）被移除，以减小树的大小。降低高度和大小的k-d树允许将来进行高效的增量操作和查询。

2) 重建与并行重建：

假设在子树$\mathcal{T}$上触发了重建（见图4），该子树首先被展平到点存储数组V中。在展平过程中丢弃标记为“已删除”的树节点。然后使用V中的所有点重建一个新的完全平衡的k-d树，如第V-B节所述。在ikd-Tree上重建一个大的子树时，可能会发生相当大的延迟，从而破坏FAST-LIO2的实时性能。为了保持高实时能力，我们设计了一种双线程重建方法。我们的提议方法不仅仅是在第二个线程中重建，而是通过操作记录器，避免两个线程中的信息丢失和内存冲突，从而在任何时候都保持k最近邻搜索的完全准确性。

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

E. K-最近邻搜索

尽管与那些著名的k-d树库中的现有实现[43]-[45]类似，但最近邻搜索算法在ikd-Tree上经过了彻底的优化。树节点上的范围信息得到了充分利用，通过“边界重叠球”测试【41】加速了最近邻搜索。维护一个优先队列q，用于存储到目前为止遇到的k个最近邻及其到目标点的距离。在从树的根节点递归搜索时，首先计算目标点到树节点的立方体区域 ( C_T ) 的最小距离 ( d_{min} )。如果最小距离 ( d_{min} ) 大于等于队列q中的最大距离，就不需要处理该节点及其子节点。

此外，在FAST-LIO2（以及许多其他LiDAR里程计）中，只有当邻近点位于目标点周围的给定阈值范围内时，才会被视为内点并用于状态估计，这自然为k最近邻的范围搜索提供了最大搜索距离【43】。在任何情况下，范围搜索通过比较 ( d_{min} ) 与最大距离来剪枝，从而减少了回溯的次数，改善了时间性能。值得注意的是，我们的ikd-Tree支持用于并行计算架构的多线程k最近邻搜索。

F. 时间复杂度分析

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

1. 增量操作：由于带树上下采样的插入依赖于框状删除和框状搜索，因此首先讨论框状操作。假设ikd-Tree上的点位于三维空间$S_x\times S_y\times S_z$，操作长方体是$C_D=L_x\times L_y\times L_z$。算法3中长方体$C_D$的框状删除和搜索的时间复杂度是：

引理 1。假设点在ikd-Tree上位于三维空间$S_x\times S_y\times S_z$，操作长方体是$C_D=L_x\times L_y\times L_z$。算法3中长方体$C_D$的框状删除和搜索的时间复杂度是

$$O(H(n))=\{\begin{array}{ll}O(\log n)& \text{如果 }{\Delta }_{\min }⩾\alpha \left(\frac{2}{3}\right)(\ast )\\ O\left(n^{1-a-b-c}\right)& \text{如果 }{\Delta }_{\max }⩽1-\alpha \left(\frac{1}{3}\right)(\ast \ast )\\ O\left(n^{\alpha \left(\frac{1}{3}\right)-{\Delta }_{\min }-{\Delta }_{\text{med}}}\right)& \text{如果}(\ast )\text{和}(\ast \ast )\text{失败且}\\ & {\Delta }_{\text{med}}<\alpha \left(\frac{1}{3}\right)-\alpha \left(\frac{2}{3}\right)\\ O\left(n^{\alpha \left(\frac{2}{3}\right)-{\Delta }_{\min }}\right)& \text{否则.}\end{array}$$

其中$a={\log }_n\frac{S_x}{L_x}$，$b={\log }_n\frac{S_y}{L_y}$，$c={\log }_n\frac{S_z}{L_z}$，且$a,b,c⩾0$。${\Delta }_{\min }$，${\Delta }_{\text{med}}$和${\Delta }_{\max }$分别是$a$，$b$和$c$中的最小值、中值和最大值。$\alpha (u)$是Flajlet-Martin函数，$u\in [0,1]$，特定值是：$\alpha \left(\frac{1}{3}\right)=0.7162$和$\alpha \left(\frac{2}{3}\right)=0.3949$。

证明。在[56]中提供了对数轴对齐超立方体在k-d树上的范围搜索的渐近时间复杂度。框状删除可以看作是范围搜索，除了在树节点上附加了延迟标签，这是O(1)。因此，范围搜索的结论可以应用于ikd-Tree上的框状删除和搜索，导致O(H(n))。

带树上下采样的插入的时间复杂度是

引理 2。算法2中ikd-Tree上带树上下采样的点插入的时间复杂度是$O(\log n)$。

证明。ikd-Tree上的下采样方法是框状搜索和删除后跟点插入组成的。应用引理1，下采样的时间复杂度是$O(H(n))$。一般来说，下采样立方体$C_D$与整个空间相比非常小。因此，归一化范围$\Delta x$，$\Delta y$和$\Delta z$很小，且${\Delta }_{\min }$的值满足条件(*)，时间复杂度为$O(\log n)$。

ikd-Tree的最大高度可以从方程(17)很容易地证明是${\log }_{1/{\alpha }_{bal}}(n)$，而静态k-d树的是${\log }_{2}n$。因此，引理直接来自[40]，其中证明了k-d树上点插入的时间复杂度是$O(\log n)$。总结下采样和插入的时间复杂度得出带树上下采样的插入时间复杂度是$O(\log n)$。

2. 重建：重建的时间复杂度分为两种类型：单线程重建和并行双线程重建。在前一种情况下，重建是由主线程递归执行的。每一层需要排序的时间（即，$O(n)$），在$k$维低的情况下，总共$logn$层的时间是$O(n\log n)$[40]。对于并行重建，主线程消耗的时间仅为展平（暂停主线程进一步增量更新，算法4，第12-14行）和树更新（需要常数时间$O(1)$，算法4，第20-22行），但不包括构建（由第二个线程并行执行，算法4，第15-18行），导致从主线程看时间复杂度为$O(n)$。总之，ikd-Tree的重建时间复杂度是并行双线程重建的$O(n)$和单线程重建的$O(n\log n)$。

3. k最近邻搜索：由于ikd-Tree的最大高度保持不超过${\log }_{1/{\alpha }_{bal}}(n)$，其中n是树的大小，从根到叶节点的搜索时间复杂度是$O(\log n)$。在树上搜索k最近邻的过程中，回溯的数量与常数$l$成正比，与树的大小无关[41]。因此，在ikd-Tree上获得k最近邻的预期时间复杂度是$O(\log n)$。

5. 基准测试结果

在本节中，我们在多个公开数据集上进行了大量实验，以评估算法在精度、鲁棒性和计算效率方面的表现。我们首先在18个不同大小的数据集序列上评估了我们的数据结构（即ikd-Tree）与其他数据结构在kNN搜索上的表现。然后在第VI-C节中，我们比较了FAST-LIO2在19个序列上的精度和处理时间。所有序列均来自5个不同的数据集，这些数据集由固态LiDAR【15】和旋转式LiDAR收集。

第一个数据集来自LILI-OM【17】的工作，使用固态3D LiDAR Livox Horizon4收集。该LiDAR具有非重复扫描模式，水平视场（FoV）为81.7°，垂直视场为25.1°，典型扫描频率为10 Hz，称为“lili”。陀螺仪和加速度计数据由6轴Xsens MTi-670 IMU以200 Hz采样。数据记录于大学校园和城市街道的结构化场景中。

第二个数据集来自LIO-SAM【30】，在麻省理工学院（MIT）校园中收集，包含多个序列。这些序列由VLP-16 LiDAR（采样频率为10 Hz）和MicroStrain 3DM-GX5-25 9轴IMU（采样频率为1000 Hz）收集，称为“liosam”。该数据集包含多种场景，包括校园内的建筑物和森林等结构化环境。

第三个数据集“utbm”【57】由一辆人驾驶的机器人车以最高50 km/h的速度收集。车辆配备两个10 Hz的Velodyne HDL-32E LiDAR和100 Hz的Xsens MTi-28A53G25 IMU。本文仅考虑左侧的LiDAR数据。

第四个数据集“ulhk”【58】包含Velodyne HDL-32E LiDAR的10 Hz数据和9轴Xsens MTi-10 IMU的100 Hz数据。utbm和ulhk的所有序列均由人驾驶车辆在城市结构化环境中收集，而ulhk还包含许多移动车辆。

最后一个数据集“nclt”【59】是一个大规模、长期自主UGV（无人驾驶地面车辆）数据集，在密歇根大学北校区收集。nclt数据集包含10 Hz的Velodyne HDL-32E LiDAR数据和50 Hz的Microstrain MS25 IMU数据。与其他数据集相比，nclt数据集具有更长的持续时间和数据量，并包含多个开放场景，如大型开放停车场。

各数据集的传感器类型和数据率信息总结在表II中。本节使用的37个序列的详细信息，包括名称、持续时间和距离，列于附录A的表VIII中。

A. 实现

我们在C++和机器人操作系统（ROS）中实现了所提出的FAST-LIO2系统。迭代卡尔曼滤波器基于我们之前工作【55】中提出的IKFOM工具箱实现。在默认配置下，局部地图大小L设置为1000米，并通过1:4的时间降采样（即每4个LiDAR点取1个）直接将LiDAR原始点输入状态估计。此外，空间降采样分辨率（见算法2）在所有实验中设置为l = 0.5米。ikd-Tree的参数设置为αbal = 0.6，αdel = 0.5，Nmax = 1500。基准测试比较的计算平台是一个轻量级的无人机机载计算机：DJI Manifold 2-C7，配备1.8 GHz的四核Intel i7-8550U处理器和8 GB RAM。对于FAST-LIO2，我们还在嵌入式系统中常用的ARM处理器上进行了测试，该处理器为Khadas VIM3，配备低功耗2.2 GHz的四核Cortex-A73处理器和4 GB RAM，简称为“ARM”。我们将“FAST-LIO2 (ARM)”定义为在ARM平台上实现的FAST-LIO2。

B. 数据结构评估

1. 评估设置：我们选择了三种最先进的动态数据结构实现与ikd-Tree进行比较：boost geometry库中R*-tree【60】的实现，Point Cloud Library中八叉树【61】的实现，以及nanoflann【54】中动态kd树的实现。这些树形数据结构的实现因其高效的实现效率而被选中。此外，它们支持动态操作（例如点插入、删除）以及与FAST-LIO2集成所必需的范围（或半径）搜索，以便与ikd-Tree进行公平比较。对于地图降采样，由于其他数据结构不支持ikd-Tree的树内降采样，我们应用了类似V-C节中详细介绍的方法，利用它们的范围搜索（用于八叉树和R*-tree）或半径搜索（用于nanoflann kd树）的能力。更具体地说，对于八叉树和R*-tree，它们的范围搜索直接返回目标长方体CD内的点（见算法2）。对于nanoflann kd树，目标长方体CD首先被分割为小立方体，其边长等于长方体的最小边长。然后，通过半径搜索获取每个小立方体的圆内点，接着通过线性方法过滤掉长方体外的点，保留长方体CD内的点。最后，类似于算法2，CD中的点中除了距离中心最近的点外，其余都从地图中删除。对于地图移动（见V-A节）所需的基于长方体的删除操作，是通过根据从各自的范围或半径搜索中获得的点索引逐个删除长方体内的点来实现的。

所有四种数据结构的实现都集成到FAST-LIO2中，并在18个不同大小的序列上评估它们的时间性能。我们在每个序列上运行FAST-LIO2，并记录kNN搜索、点插入（包括地图降采样）、由于地图移动的基于长方体的删除、新扫描点的数量以及每步的地图点数量（即树大小）。k最近邻的数量设置为5。

2. 比较结果：我们首先比较了在不同树大小下的点插入（包括地图降采样）和kNN搜索的时间消耗。在每个评估的树大小S下，我们收集在[S−5%S, S+5%S]范围内的处理时间，以获取足够的样本量。图5显示了每个目标点的平均插入和kNN搜索时间。八叉树在点插入方面表现最好，尽管与其他方法的差距很小（不到1微秒），但由于树结构的不平衡，其查询时间要高得多。对于nanoflann kd树，插入时间通常略短于ikd-Tree和R*-tree，但由于其通过组织一系列kd树的对数结构，偶尔会出现巨大峰值。这些峰值严重影响了实时性能，尤其是在维护大型地图时。对于k最近邻搜索，nanoflann kd树的时间消耗略高于ikd-Tree，特别是在树大小变大（105到106）时。R*-tree的插入时间与ikd-Tree相似，但在大型树结构下的搜索时间显著更高。最后，我们可以看到，ikd-Tree的插入时间（包括树内降采样）和kNN搜索时间确实与log n成正比，这与第V-F节中的时间复杂度分析一致。
   

对于LiDAR里程计和建图系统中的任何地图数据结构，地图查询（即kNN搜索）和增量地图更新（即带降采样的点插入和由于地图移动导致的基于长方体的删除）的总时间最终影响其实时性能。该总时间总结在表III中。可以看到，八叉树在大多数数据集中在增量更新方面表现最好，紧随其后的是ikd-Tree和nanoflann kd树。在kNN搜索方面，ikd-Tree表现最好，ikd-Tree和nanoflann kd树在很大程度上优于其他两种，这与之前的比较研究【42,43】一致。ikd-Tree在所有数据结构中表现最佳。

需要指出的是，尽管nanoflann kd树的性能似乎与ikd-Tree相近，但其插入时间峰值有更深层次的原因，并且对LiDAR里程计和建图的影响严重。nanoflann kd树仅通过掩码标记删除点，而没有真正将其从树中删除。因此，即使有地图降采样和地图移动，已删除的点仍然留在树中，影响后续查询和插入性能。结果是其树大小比ikd-Tree和其他树结构增长得更快，这一现象也在图5中得到验证。对于短序列（ulhk和lili），其影响可能较小，但对于长序列（utbm和nclt）则变得显著。nanoflann kd树的树大小在utbm数据集中超过6×106，在nclt数据集中超过107，而ikd-Tree的最大树大小分别为2×106和3.6×106。nanoflann在七个utbm数据集中增量更新的最大处理时间都超过3秒，而在三个nclt数据集中超过7秒，而ikd-Tree在nclt 2中的最大处理时间为214.4毫秒，其余17个序列中的处理时间都小于150毫秒。尽管nanoflann的峰值处理时间发生频率较低，不会严重影响整体实时性能，但它会导致后续控制的灾难性延迟。

C. 精度评估

在本节中，我们将 FAST-LIO2 与其他先进的 LiDAR-惯性里程计和建图系统进行对比，包括 LILI-OM [17]、LIO-SAM [30] 和 LINS [31]。由于 FAST-LIO2 不具备闭环检测或校正功能，为了公平起见，我们在对比时禁用了 LILI-OM 和 LIO-SAM 的闭环模块，但保留了其他功能，例如滑动窗口优化。我们还对 FAST-LIO2 进行消融研究：为了了解地图尺寸的影响，我们在地图尺寸为 2000 m、800 m、600 m 以及默认的 1000 m 下运行算法；为了评估直接方法与基于特征的方法的效果，我们从 FAST-LIO [22]（为固态 LiDAR 优化）和 BALM [20]（为旋转式 LiDAR 优化）中加入特征提取模块。实验结果以关键词“Feature”进行汇报。所有实验均在 Manifold 2-C 平台（Intel）上进行。
我们在五个数据集上进行了评估：lili、lisam、utbm、ulhk 和 nclt。由于并非所有序列都有地面真值（受天气、GPS 质量等影响），我们从五个数据集中选择了总共 19 个序列进行评估。这些序列要么有良好的地面真值轨迹（由数据集作者推荐），要么在起始位置结束。因此，计算并评估了两项标准：绝对平移误差（RMSE）和端到端误差。

1. RMSE 基准：RMSE 的计算结果汇报在表 IV 中。可以看出，增加 FAST-LIO2 的地图尺寸提高了整体精度，因为在 LiDAR 再次经过之前的位置时，新的扫描可以与较旧的历史点匹配。当地图尺寸超过 2000 m 时，精度的提升不再明显，因为里程计漂移可能会导致与过于旧的地图点的错误匹配，这是所有里程计的典型现象。此外，直接方法在大多数序列中优于基于特征的 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 仍然达到了更高的精度。
   

2. 漂移基准：端到端误差的计算结果汇报在表 V 中。总体趋势与 RMSE 基准结果相似。FAST-LIO2 或其变体在 7 个序列中的 5 个序列中实现了最低漂移。我们在视频中展示了 ulhk 6 序列的一个示例，视频可在 https://youtu.be/2OvjGnxszf8 查看。需要注意的是，LILI-OM 针对他们自己的序列 lili 进行了参数调优，而 FAST-LIO2 的参数在所有序列中保持不变。LIO-SAM 在其自己的序列 liosam 2 和 liosam 3 中表现良好，但在其他序列（如 ulhk）中无法保持同样的表现。LINS 在 liosam 和 ulhk 数据集中的表现比 LIO-SAM 差，并且在 liosam 2（来自 [30] 的花园序列）中失败，因为这两个序列记录了大旋转速度，而 LINS 使用的特征点太少。此外，在大多数序列中，基于特征的 FAST-LIO 表现与直接方法相似，除了 lili 7 序列，该序列包含大量树木和开阔区域，特征提取会移除许多来自树木和远处建筑物的有效点。

D. 处理时间评估

表 VI 显示了 FAST-LIO2 在不同配置下的处理时间，以及 LILI-OM、LIO-SAM 和 LINS 在所有序列中的处理时间。FAST-LIO2 是一个集成的里程计和建图架构，在每一步中，地图更新紧跟着里程计更新。因此，总时间（表 VI 中的“Total”）包括里程计过程中可能发生的所有步骤，包括特征提取（如果有，如基于特征的变体）、运动补偿、kNN 搜索、状态估计和建图。此外，建图包括点插入、框删除和树的重新平衡。另一方面，LILI-OM、LIO-SAM 和 LINS 都有独立的里程计（包括特征提取和粗略位姿估计）和建图（如 LILI-OM [17] 的后端融合、LIO-SAM [30] 的增量平滑与建图以及 LINS [31] 的地图优化），其每帧 LiDAR 扫描的平均处理时间分别在表 VI 中称为“Odo.” 和“Map.”。两者的处理时间相加以与 FAST-LIO2 进行比较。
从表 VI 中可以看到，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-Tree 进行建图和 kNN 搜索对地图尺寸不敏感。此外，基于特征的变体与直接方法 FAST-LIO2 的处理时间大致相同。尽管特征提取需要额外的时间来提取特征点，但这减少了后续 kNN 搜索和状态估计所需的时间。另一方面，直接方法省去了特征提取时间用于点注册。得益于 FAST-LIO2 出色的计算效率，我们进一步在 Khadas VIM3（ARM）嵌入式计算机上使用默认地图尺寸（1000 m，见 VI-C）进行了实现。运行结果表明，FAST-LIO2 也能在 ARM 平台上实现 10 Hz 的实时性能，这是之前任何工作未能在 ARM 平台上展示的。

VII. 实验测试

A. 平台

除了数据集主要是在地面收集的基准评估外，我们还在其他平台上测试了FAST-LIO2在各种挑战性数据中的表现（见图6），包括用于无人机导航的280毫米轴距四旋翼飞行器、用于移动测绘的手持平台和用于航空测绘的GPS导航750毫米轴距四旋翼无人机。280毫米轴距四旋翼飞行器用于室内激烈飞行测试（见第VII-B2节），因此LiDAR安装在正前方。750毫米轴距四旋翼无人机由Ambit-Geospatial公司开发，用于航空扫描（见第VII-C节），LiDAR面朝地面。在所有平台上，我们使用了固态3D LiDAR Livox Avia10，它内置IMU（型号BMI088），具有70.4°（水平）×77.2°（垂直）的圆形视场（FoV），其非重复扫描模式不同于之前在第VI节中使用的Livox Horizon或Velodyne LiDAR。由于FAST-LIO2不提取特征点，它自然适应这一新LiDAR。在接下来的所有实验中，FAST-LIO2使用默认配置（即，地图大小为1000米的直接法）。除非特别说明，扫描频率设为100Hz，计算平台为上一节中使用的DJI Manifold 2-C。

B. 私有数据集

1) 处理时间详细评估：

为了验证FAST-LIO2的实时性能，我们使用手持平台在大规模室内外混合场景中以100Hz的扫描频率收集了一段数据序列。传感器在行驶约650米后返回到起点。需要注意的是，LILI-OM也支持固态LiDAR，但在此数据中失败了，因为其特征提取模块在100Hz扫描频率下提取的特征点太少。FAST-LIO2实时构建的地图如图7所示，显示出较小的漂移（即0.14米），并与卫星地图良好吻合。

为了评估计算效率，我们在Intel（Manifold 2-C）计算机上比较了FAST-LIO2与其前身FAST-LIO [22]。对于FAST-LIO2，我们还在ARM（Khadas VIM3）板载计算机上进行了额外测试。这两种方法的区别在于FAST-LIO是基于特征的方法，它在每一步中在当前视场（FoV）中检索地图点以构建新的静态k-d树进行kNN搜索。处理每帧数据的各个组件的详细时间消耗如表VII所示。预处理指的是数据接收和格式化，FAST-LIO和FAST-LIO2的预处理步骤相同，耗时均低
于0.1毫秒。FAST-LIO的特征提取每帧耗时0.9毫秒，而FAST-LIO2节省了这部分时间。特征提取导致的点数量少于FAST-LIO2（447个 vs. 756个），因此状态估计的耗时也减少了（0.99毫秒 vs. 1.66毫秒）。因此，两种方法的总里程计时间非常接近（FAST-LIO为1.92毫秒，FAST-LIO2为1.69毫秒）。

这两种方法之间的差异在查看地图模块时变得显著，FAST-LIO包括地图点检索和k-d树构建，而FAST-LIO2包括点插入、由于地图移动而进行的区域删除以及树的重新平衡。可以看出，FAST-LIO每帧平均映射时间超过10毫秒，因此在这种大场景中无法实时处理。另一方面，FAST-LIO2的映射时间远低于采样周期。在处理756个点的情况下，FAST-LIO2的总处理时间（包括里程计和映射）在Intel处理器上仅为1.82毫秒，在ARM处理器上为5.23毫秒。

每帧的时间消耗和地图点数如图8所示。可以看到，FAST-LIO2在ARM处理器上运行时，处理时间偶尔会超过10毫秒的采样周期，但这种情况非常少见，平均处理时间远低于采样周期。偶尔的超时通常不会影响后续的控制器，因为在这段短时间内可以使用IMU传播的状态估计。而在Intel处理器上，FAST-LIO2的处理时间始终低于采样周期。另一方面，由于地图点数的增加，FAST-LIO的处理时间迅速超过采样周期。值得注意的是，即使在地图点数较高的情况下，FAST-LIO2仍然显著减少了处理时间。由于FAST-LIO仅保留当前FoV中的地图点，当LiDAR面向一个新区域且包含很少的先前采样地图点时，地图点数可能会减少。即使地图点数较少，FAST-LIO的处理时间仍然较高，如上所述的分析。此外，由于FAST-LIO在每一步都构建一个新的k-d树，构建时间的时间复杂度为O(n log n) [40]，其中n是当前FoV中的地图点数。这也是FAST-LIO的处理时间几乎线性增长的原因。相比之下，我们的ikd-Tree增量更新的时间复杂度为O(log n)，因此处理时间随地图大小的增长要慢得多。

2) 激烈无人机飞行实验：

为了展示FAST-LIO2在移动机器人平台中的应用，我们部署了一架搭载Livox AVIA LiDAR传感器的小型四旋翼无人机，进行了一次激烈的翻转实验，如图9所示。在此实验中，无人机首先从地面起飞并悬停在1.2米的高度，然后进行快速翻转，翻转后在基于流形的模型预测控制器[62]的控制下恢复到悬停飞行状态，控制器的状态反馈来自FAST-LIO2。FAST-LIO2估计的姿态如图9 (d)所示，与实际的无人机姿态高度吻合。实时构建的环境地图如图10所示。此外，图11显示了实验期间的位置、姿态、角速度和线速度。在翻转过程中，平均和最大角速度分别达到了912度/秒和1198度/秒（50.8秒至51.2秒）。FAST-LIO2每帧平均处理时间仅为2.01毫秒，足以满足控制器的实时需求。通过以100Hz的频率提供高精度的里程计数据和高分辨率的3D环境地图，FAST-LIO2非常适合机器人进行实时控制和避障。例如，我们的早期工作[63]展示了FAST-LIO2在复杂的室内外环境中自主无人机避开动态小物体（最小至9毫米）的应用。

3) 快速运动手持实验：

在此实验中，我们测试了FAST-LIO2在大速度和角速度下的挑战性快速运动表现。传感器由手持持握，在一座人行桥上快速往返奔跑（见图12）。图13展示了快速运动手持实验中的姿态、位置、角速度和线速度。可以看到，最大速度达到了7米/秒，角速度在±100度/秒左右波动。为了展示FAST-LIO2的性能，实验从同一点开始并结束。此次实验中的端到端误差小于0.06米（见图13），而总轨迹长度为81米。

C. 室外空中实验

3D LiDAR的一个重要应用是空中测绘。为了验证FAST-LIO2在此类应用中的表现，我们进行了空中实验。部署了一架搭载LiDAR传感器的更大无人机。无人机配备了GPS、IMU及其他飞行控制设备，并可以基于板载GPS/IMU导航自动执行航点飞行。需要注意的是，无人机配备的GPS和IMU仅用于无人机导航，而FAST-LIO2仅使用来自LiDAR传感器的数据。在此实验中，LiDAR的扫描频率设置为10Hz。在香港南生围的香港湿地公园的多个地点进行了几次飞行。实时构建的地图结果如图14所示，可以看出FAST-LIO2在这些植被环境中表现非常良好。许多精细结构，如树冠、道路上的标线和路缘，都清晰可见。图14还展示了FAST-LIO2计算的飞行轨迹。我们将这些轨迹与无人机板载GPS/IMU导航估算的轨迹进行了目视比较，结果显示二者高度一致。由于技术上的困难，此处无法获得GPS轨迹进行定量评估。最后，这三个环境中每帧的平均处理时间分别为19.6毫秒、23.9毫秒和23.7毫秒。需要注意的是，LILI-OM在这三个数据序列中都失败了，因为其特征提取在面对地面时提取的特征点太少。

6. 结论

本文提出了FAST-LIO2，一种直接且鲁棒的LIO框架，相较于当前最先进的LIO算法具有显著的速度优势，同时在各种数据集上实现了非常具有竞争力或更好的精度。这一速度提升得益于去除了特征提取模块以及高效的映射。我们开发并验证了一种新的增量式k-d树（ikd-Tree）数据结构，该结构支持动态点插入、删除和并行重建。大量开放数据集上的实验表明，提出的ikd-Tree在LiDAR里程计中的kNN搜索方面能实现当前最先进数据结构中的最佳综合性能。由于映射效率的提升，FAST-LIO2在快速运动和稀疏场景中的精度和鲁棒性也得到了提高，通过在里程计中利用更多的点。此外，FAST-LIO2的另一个优势是由于去除了特征提取，它能自然适应不同的LiDAR，而不需要根据各自的扫描模式和密度为不同LiDAR精心设计特征提取模块。

致谢
本项目得到了DJI的资助，资助编号为200009538。我们衷心感谢DJI的资金支持以及Livox Technology在整个工作过程中提供的设备支持。我们还要感谢Ambit-Geospatial在室外空中实验中的帮助。同时感谢Zheng Liu、Guozheng Lu和Fangcheng Zhu的有益讨论。

附录
第VI节中使用的所有37个序列的详细信息列在表VIII中。