fast-lio2论文阅读 《FAST-LIO2: Fast Direct LiDAR-inertial Odometry》

【摘要】 

        本文提出了FAST-LIO2：一个快速、鲁棒和通用的激光雷达惯性里程计框架。FAST-LIO2基于一个高效的紧密耦合迭代卡尔曼滤波器，FAST-LIO2有两个关键的新特性，允许快速、鲁棒和精确的激光雷达导航(和建图)。第一个方法是将原始点直接配准到地图上(随后更新地图，即建图），而不提取特征。这使得我们能够利用环境中的微妙特征，从而提高了准确性。消除了手工设计的特征提取模块，也使它自然地适应不同扫描模式的激光雷达；第二个主要的新颖之处是通过增量k-d树数据结构ikd-Tree维护地图，从而实现增量更新（即点插入、删除）和动态重新平衡。与现有的动态数据结构(八叉树、R∗树、naniflflann k-d tree)相比， ikd-Tree实现了卓越的整体性能，同时合理的支持对树的降采样操作。

        我们对来自各种开源的激光雷达数据集的19个序列进行了详尽的基准测试比较。与其他最先进的激光雷达-惯性导航系统相比，FAST-LIO2在更低的计算负载下实现了持续到达更高的精度。在具有小视场的固态激光雷达(livox)上也进行了各种现实世界的实验。总的来说，FAST-LIO2具有计算效率高（例如，在大型户外环境中高达100Hz的里程计定位状态估计和建图），鲁棒（例如，在杂乱的旋转高达1000度/秒的室内环境中可靠的姿态估计），多用途(即，适用于多线旋转和固态激光雷达、无人机和手持平台，以及基于英特尔和arm的处理器)，同时仍然实现比现有方法更高的精度。

【I. INTRODUCTION】

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

        由于能够提供直接、密集、主动和精确的环境深度测量的能力，3D光探测和测距（激光雷达）传感器已经成为另一种机器人所必备的传感器。在过去的十年里，激光雷达在许多自动驾驶机器人中扮演着越来越重要的角色，比如自动驾驶汽车[11]和自动驾驶无人机[12,13]。激光雷达技术的最新发展使商业化和大规模生产更轻量级，成本效益（成本范围类似于全局快门相机），和高性能（厘米精度数百米测量范围）固态激光雷达[14,15]，吸引了最近[16]-[20]的研究兴趣。这些激光雷达大大降低了成本、尺寸、重量和功率，有可能使现有和新兴的机器人应用广泛受益。

        在这些广泛的应用中，采用基于激光雷达的SLAM方法的核心要求是在有限的板载计算资源下获得准确、低延迟的状态估计和密集的三维地图。然而，高效和准确的激光雷达里程计和建图仍然是一个具有挑战性的问题：1)当前的激光雷达传感器产生大量的从数十万到数百万的三维点。在有限的板载计算资源上实时处理如此大量的数据，需要激光雷达里程计方法具有较高的计算效率；2)为了减少计算负荷，通常基于局部平滑度提取特征点、边缘点或平面点等特征点。然而，特征提取模块的性能很容易受到环境的影响。

        例如，在没有大平面或长边的无结构环境中，特征提取将导致很少的特征点。如果激光雷达视场(FoV)很小，这种情况就会严重恶化，这是新出现的固态激光雷达[16]的典型现象。此外，特征提取也从激光雷达到激光雷达，这取决于扫描模式(例如，旋转，基于棱镜的[15]，基于MEMSs的[14])和点密度。因此，采用激光雷达里程计通常需要大量的人工处理的工程工作；3)当传感器进行连续运动时，激光雷达点通常是按顺序采样的。这一过程产生了显著的运动失真，影响里程计和建图的性能，特别是当运动严重时。惯性测量单位(IMU)可以缓解这个问题，但引入额外的状态（例如偏差，外参）需要估计；4)激光雷达通常有一个很长的测量范围（例如，几百米），但在扫描的扫描线之间的分辨率相当低。所得到的点云测量值稀疏地分布在一个大的三维空间中，需要一个大的和密集的地图来配准这些稀疏的点。此外，该地图需要支持高效的查询对应的搜索，同时实时更新整合新的测量。

       维护这样的一个地图是一项非常具有挑战性的任务，与视觉测量明显不同的是，视觉测量的图像测量值具有很高分辨率，所以只需要一个稀疏的特征图，因为地图中的一个特征点总是可以找到对应关系，只要它落在FOV中。

        在这项工作中，我们通过两个关键的新技术来解决这些问题：增量k-d树和直接点配准。更具体地说，我们的贡献如下：

        1)我们开发了一个增量的k-d树数据结构，ikd-Tree，以有效地表示一个大的密集点云图。除了有效的最近邻搜索外，新的数据结构还支持增量地图更新（即点插入、树上降采样、点删除）和以最小的计算成本进行动态重新平衡。这些特性使得ikd-Tree非常适合激光雷达状态估计和建图应用，实现100hz里程计和在计算约束平台上的建图，如基于英特尔i7的微型无人机机载计算机，甚至基于arm的处理器。ikdTree数据结构工具箱是在Github上开源的。

        2)由于ikd-Tree的计算效率有所提高，我们直接将原始点注册到地图上，即使在剧烈的运动和非常杂乱的环境中，也能实现更准确可靠的扫描配准。我们将这种基于原始点的配准称为直接方法，类似于视觉SLAM[21]。消除了人工处理的特征提取，使该系统自然地适用于不同的激光雷达传感器；

        3)我们将这两种关键技术集成到我们最近开发的一个完全紧密耦合的激光雷达-惯性里程计系统FAST-LIO[22]中。该系统使用一个IMU，通过一个严格的反向传播步骤来补偿每个点的运动，并通过一个流形上迭代卡尔曼滤波器来估计系统的全状态。为了进一步加快计算速度，我们使用了一个新的、数学上等价的计算卡尔曼增益的公式，将计算复杂度降低到状态的维数（而不是测量量的维数）。

        新系统被称为FAST-LIO2，在Github开源以造福社区；4)我们进行各种实验来评估开发的ikd-Tree、直接点云配准和整个系统的有效性。对18个不同大小的序列进行的实验表明，ikdTree在应用激光雷达里程计和建图方面，比现有的动态数据结构(八叉树、R∗树、nanoflflann k-d树)具有优越的性能。对来自各种开放激光雷达数据集的19个序列进行详尽的基准比较表明，与其他最先进的激光雷达惯性导航系统相比，FAST-LIO2在更低的计算负载下始终获得更高的精度。最后，我们展示了FAST-LIO2在具有挑战性的新兴固体激光雷达收集的真实世界数据上的有效性，包括剧烈性运动(例如，旋转速度高达1000度/s)和无结构环境。

【II. RELATED WORKS 】

A. LiDAR(-Inertial) Odometry

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

        随后的激光雷达里程计法工作保持了一个类似于LOAM的框架。例如，Lego-LOAM[27]引入了一个地面点分割来减少计算量，以及一个闭环模块来减少长期漂移。此外，LOAM-Livox[16]采用了LOAM到一个新兴的固态激光雷达。为了处理小视场和非重复扫描，即连续两次扫描的特征点对应点很少，通过直接将新的scan配准到全局地图上得到LOAM-Livox的里程计。这种直接扫描到地图配准增加了里程计的精度，但代价是增加了在每一步建立更新的地图点的k-d树的计算量。

        通过提供ICP所需的良好初始姿态，加入IMU可以显著提高激光雷达里程计的精度和鲁棒性。此外，高速率IMU测量可以有效地补偿激光雷达扫描中的运动失真。LION[28]是一种松散耦合的激光雷达惯性SLAM方法，它保持LOAM的帧到帧的配准，并在里程计中引入可观测意识检查，以降低点数，从而节省计算量。更紧密耦合的激光雷达-惯性融合框架成果有，[17,29]-[31]在一个由固定数量的最近的激光雷达扫描（或关键帧）组成的小型局部地图(固定关键帧的local map)中执行里程计测量。与scan到scan配准相比，scan to local map局部地图配准通常更准确。更具体地说，LIOM[29]提出了一种紧密耦合的激光雷达惯性融合方法，其中IMU的预积分被引入到里程计中。LILIOM[17]开发了一种新的非重复扫描激光雷达特征提取方法，并在一个由最近20次的激光雷达帧数据组成的小local地图中进行扫描配准。

        LIO-SAM[30]的里程计需要一个9轴的IMU来产生姿态测量，作为在一个小的局部地图内的扫描配准的预先测量值。

        LINS[31]在里程计中引入了紧密耦合迭代卡尔曼滤波器和自动中心公式。由于上述工作中的局部图local map通常较小，无法获得实时性能，里程计的漂移迅速，需要一个低速率的建图过程，如地图提纯(LINS[31])、滑动窗口联合优化(LILI-OM[17]和LIOM[29])和因子图平滑[32](LIO-SAM[30])。

        与上述方法相比，FAST-LIO[22]引入了一种整齐的反向传播，它精确地考虑scan中每个单个点的采样时间，并通过一个由IMU测量驱动的严格运动学模型来补偿运动失真(去畸变)。此外，利用一种新的卡尔曼增益公式，将计算复杂度从测量值的维数降低到状态的维数。证明了新公式在数学上与传统公式等价，但将计算量减少了几个数量级。显著提高的计算效率能够直接和实时在里程计中实现scan to map的配准，并在每一步更新地图（即建图）。及时配准所有最近的扫描点可以增加里程计的准确性。然而，为了防止构建地图的k-d树的时间不断增加，该系统只能在较小的环境中工作（例如，数百米）----这就是fast-lio的缺陷，因为地图的扩大，到达一定值后scan to map的计算和map的维护无法保证实时性。

        为此，作者优化了其缺陷，引入了FAST-LIO2。FAST-LIO2建立在FAST-LIO[22]的基础上，因此继承了紧密耦合的融合框架，特别是反向传播解决运动失真和快速卡尔曼增益计算，提高了效率。为了系统地解决日益增长的计算问题，我们提出了一种新的数据结构ikd-Tree，它支持每一步的增量地图更新和高效的kNN查询。得益于计算负荷的大幅减少，里程计是通过直接配准原始激光雷达点到地图来实现的(不需要降采样)，这样它提高了里程计和建图的准确性和鲁棒性，特别是当一个新的scan不包含显著的特征（例如，由于小视场雷达和/或无结构环境）。与上述所有使用特征点的紧密耦合激光雷达惯性方法相比，我们的方法轻量，实现了更高的建图速率和里程计精度，并消除了特征提取的参数调整的需要。

        我们的工作中直接配准原始点云的想法已经在LION[28]中探索过，然而，这是如上所述的一种松散耦合的方法。这个想法也与[26]中提出的广义ICP(G-ICP)非常相似，其中一个点被配准到地图上的一个小的局部平面上。这最终假设环境是平滑的，因此可以视为一个平面。然而，广义icp的计算负荷通常是较大的[33]。其他基于正态分布变换(NDT)[34]-[36]的工作也配准了原始点，但NDT比ICP的稳定性较低，在某些场景[36]中可能存在发散不收敛情况。

B. Dynamic Data Structure in Mapping

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

        八叉树[39]和k维树(k-d树)[40]是两种众所周知的数据结构类型，用于分割kNN搜索的空间。八叉树通过递归地将空间平均分成8个轴对齐的立方体来组织三维点云。当立方为空，或满足停止规则（如最小分辨率或最小点数）时，立方的细分将停止。新的点被插入到八叉树上的叶节点上，同时在必要时应用进一步的细分。八叉树支持kNN搜索和按框式搜索，它返回给定轴对齐的长方体中的数据点。

        k-d树是一种二叉树，其节点表示一个轴对齐的超平面，将空间分成两部分。在标准构造规则中，选择分割节点作为沿最长维数的中值点，以实现一个紧凑的空间分割[41]。在建图中考虑低维和主内存存储的数据特征时，对比研究表明，k-d树在kNN问题[42,43]中的性能最好。但是，在k-d树中插入新点和删除旧点会恶化树的平衡属性；因此，需要重新构建来重新平衡树。使用k-d树库的建图方法，如ANN[44]，libbabo[43]和FLANN[45]，完全重建了k-d树来更新地图，这导致了相当多的计算。

        虽然在三维图形应用[46]-[49]中对基于硬件重建k-d树的方法进行了深入的研究，但所提出的方法严重依赖于计算源，这些方法通常在车载计算机的计算源非常局限。Galperin等人提出了全规模重建树，而是提出了一种替身k-d树，其中应用于重建部分不平衡的子树，以保持整个树[50]的松散平衡特性。另一种实现增量操作的方法是以类似于[51,52]的对数方法维护一组kd树，并重建一个精心选择的子集。Bkd-树在主内存中维护一个最大大小为M的kd树T0，在外部内存中维护一组kd树Ti，其中第i棵树的大小为2(i−1)M[53]。当树T0已满时，将这些点从T0提取到Tk−1，并插入到第一个空树Tk中。最先进的实现 nanoflflann kd树利用对数结构进行增量更新，而惰性标签只标记被删除的点，而没有从树（从此处的内存）[54]中删除它们。

        我们提出了一种基于替身k-d树树的动态数据结构，称为增量树(ikd-tree)[50]，以实现实时的建图。我们的ikd-tree支持点级插入，且能够使用在树上降采样，这是建图中的一个常见要求，而降采样必须在向其他动态数据结构[38,39,54]中插入新的点之前在外部进行。当需要在给定区域中去除具有规则形状的不必要的点时（如长方体）时，现有的r树和八叉树实现搜索给定空间内的点并一个一个地删除它们，而常见的k-d树使用半径搜索来获得点索引。

        与这种间接和低效的方法相比，ikd-Tree通过维护范围信息和惰性标签，直接删除给定轴对齐长方体中的点。标记为“已删除”的点将在重建过程中被删除。此外，虽然将部分重平衡方法作为替身k-d树[50]和nanoflflann k-d树[54]后，可以进行增量更新，但使用k-d树的建图方法在大量点上重建时存在间歇性延迟。为了克服这一问题显著延迟，ikd-tree中通过并行重建，同时保证主线程的实时能力和准确性。 

【III. SYSTEM OVERVIEW】

        顺序采样的激光雷达原始点首先在10ms(100Hz更新)和100ms(10Hz更新)之间的一段时间内累积，累积的点云被称为一帧scan。为了执行状态估计，一个新scan中的点就与地图点云进行配准，通过一个紧密耦合的迭代卡尔曼滤波框架来，在一个大的局部地图中进行维护(大的红色虚线块)。在大的局部地图中的全局地图点由一个增量kd树结构ikd-Tree来组织管理(蓝色大虚线块)。如果当前激光雷达的视场范围越过地图边界，则将从ikd-Tree中删除距离LiDAR姿态最远的历史点云(一旦越过边界获得更大地图范围触发ikd-tree重构最远点)。因此，ikd-Tree跟踪一个具有一定长度（本文称为“地图大小”）的大立方体区域中的所有地图点，（也可以理解为滑窗形式）并用于计算状态估计模块中的残差。优化后的姿态最终将新scan中的点配准到全局帧中，并通过以里程计（即映射）的频率插入到ikd-Tree中，将它们合并到建图中。

IV. STATE ESTIMATION

        FAST-LIO2的状态估计是一个继承自FAST-LIO[22]的紧密耦合迭代卡尔曼滤波器，但进一步改进包括激光雷达-IMU外参的在线校准。在这里，我们简要地解释了滤波器的基本公式和工作流程，并参考读者到[22]了解更多的细节。    即参看fast-lio的推导过程  https://mp.youkuaiyun.com/mp_blog/creation/editor/124033588

A. Kinematic Model

首先推导出由状态转移模型和测量模型组成的系统模型。

1) State Transition Model:

        以第一个IMU坐标系（记为I）为全局帧（记为G），表示ITL = 

GpI，GRI表示IMU在全局坐标系下的位置和姿态，Gg是全局坐标系的重力向量，am和ωm IMU测量，na和nω表示测量噪声ωm，ba和bωIMU偏差建模为随机行走过程由nba和nbω，符号b∧表示向量∈的斜对称交叉积R3矩阵。