FAST-LIO2:论文和算法解析

题目：FAST-LIO2:快速直接激光雷达惯性里程计
参考链接：
ikd-Tree: 港大开源增量式 kd-tree 结构
kd-tree 原理深入理解

本文对FAST-LIO2论文进行翻译解析，非逐句翻译，对一些介绍性内容进行总结，对关键算法给出一些自己的理解。

整体上看，FAST-LIO2的改进点主要在第一个版本上增加了ikd-tree和去除了特征匹配，采用直接匹配的方式构建残差。

摘要

本文介绍了FAST-LIO2算法：一种快速、鲁棒、通用的激光雷达-惯性里程计框架。FAST-LIO2基于高效的紧密耦合迭代卡尔曼滤波器，具有两个关键的新颖之处，可实现快速，稳健和准确的激光雷达导航(和测绘)。

第一种方法是直接将原始点注册到地图上然后更新地图，而不提取特征。这样可以利用环境中的细微特征，从而提高准确性。消除手工设计的特征提取模块，也使其能够适应不同扫描模式的新兴激光雷达;第二个主要的新颖之处是通过增量k-d树数据结构(ikd-Tree)来维护映射，这使得增量更新(即激光点插入、删除)和动态重新平衡成为可能。与现有的动态数据结构(八叉树，R*-tree，nanoflann k-d tree)相比，ikd-Tree进行下采样时可以获得更好的整体性能。

我们对来自各种公开LiDAR数据集的19个序列进行了详尽的基准比较。FAST-LIO2与其他先进的激光雷达惯性导航系统相比，在更低的计算负荷下实现了更高的精度。在小视场的固态激光雷达上进行了各种实际实验。总体而言，FAST-LIO2具有计算效率高(例如，在大型室外环境中高达100Hz的里程计和建图)，鲁棒性强(例如，在旋转高达1000度/秒的杂室内环境中得到可靠的姿态估计)，多用途(即适用于多线机械雷达和固态激光雷达，无人机和手持平台，以及基于英特尔和arm的处理器)，同时仍然实现比现有方法更高的精度。我们实现的系统FAST-LIO2和数据结构ikd-Tree在Github上都已经开源。

一、简介

视觉SLAM可以提供精确的定位结果，但是无法提供3D稠密地图用于避障和路径规划。

激光LIDAR可以解决上述问题，且当前激光雷达的体积结构、成本和性能都在不断优化。使得广泛应用和商业化成为可能。

激光雷达在SLAM技术中的应用同样也存在一些问题：

1. LIDAR数据量庞大，对计算效率要求高
2. 面对无结构环境，基于特征提取的激光SLAM方法效果不佳，同时也会收到FOV、扫描方式和点密度的影响，需要很多额外数据处理的适配工作
3. IMU可以弥补LIDAR的运动畸变，但本身的偏置和外参等信息需要进行标注和更新
4. 针对点云在空间中的稀疏分布问题，需要高效的点云检索方法

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

1. 增量k-d树数据结构
   提供高校的最近邻搜索，支持增量式地图更新，包括云点的插入与删除、树的上采样和下采样，极大减少计算成本，在计算资源受限可以达到100HZ的里程计和建图频率。
2. 基于IKD-TREE的原始点云配准
   基于ikd-Tree，实现原始点云的注册，而非基于特征提取方法，使算法可以适应不同的环境和LIDAR传感器。
3. 将上述改进应用到FAST-LIO中
4. 在丰富的数据集和工作环境测了算法的性能，并和其他算法进行对比分析。

二、相关工作

2.1雷达惯导里程计

1. 纯激光SLAM

目前纯激光SLAM多参考了LOAM结构，分为特征提取，里程计和建图三个模块。为减小匹配计算量对点云进行了特征提取，然后通过scan-to-scan和scan-to-map获取精确的位姿信息。LEGO-LOAM在匹配时分离的地面点减小计算，并引入回环检测。LOAM-Livox算法直接基于scan-to-map实现里程计位姿估计提高了位姿估计精度，但是由于每一步都需要更新地图点k-d树使得计算量增加。

2. 雷达-惯导里程计

IMU在激光SLAM中的作用主要有3个：
（1）提供点云匹配的初始位姿
（2）用于去除点云的运动畸变
（3）将基于IMU的状态估计加入到SLAM的残差方程，实现紧耦合

FAST-LIO2基于FAST-LIO算法实现，在之前工作的基础上提出一种新的数据结构数据结构ikd-Tree，同时基于该数据结构实现点云到地图的直接匹配，在减小计算量的同时scan-to-map的匹配方式提高了位姿估计精度。

2.2 建图过程中的动态数据结构

为了实现建图，需要一种动态的数据结构支持增量更新和高校的kNN搜索。

1. R-TREE
   基于数据在空间上的接近程度聚类为一个矩形，该矩形包括了相互距离接近的几个叶节点。然后该矩形和其同一节点的其他矩形被更大的矩形包含，依次类推直到根节点。
   

2. R*-TREE
   R* 树和R树一样允许矩形的重叠，但在构造算法R*树不仅考虑了索引空间的“面积”，而且还考虑了索引空间的重叠。

3. 八叉树
   八叉树通过递归地将空间均匀地分成八个轴对齐的立方体来组织三维点云。当立方体为空或满足停止规则(例如，最小分辨率或最小点数)时，立方体的细分停止。新的点被插入到八叉树上的叶节点上，如果有必要，将应用进一步的细分。八叉树同时支持kNN搜索和箱形搜索，后者返回给定轴对齐长方体中的数据点。

4. K-D树
   （1）最上层为根节点，最下层为叶子节点，其余为中间节点
   （2）上层节点是下层节点的父节点
   （3）下层节点是上层节点的子节点，子节点根据维度的比较分左右子节点，也即父节点的左子树和右子树
   （4）每一层都需要指定一个维度(如3D坐标XYZ中的X维度)，并在该维度上取一个值，X小于该值的点被划分到左子树，大于该值被划分为右子树
   （5）每到下一层，需要选择新的划分维度和划分值，采用方差较大的维度作为本层分割的维度

5. 增量kd树：ikd-Tree
   传统的静态 kd-tree 结构存在着大量数据增删的情况下会失去平衡性，导致深度不均衡和搜索效率下降等问题，如果为了解决静态 kd-tree 的不均衡问题而频繁重新构建 kdtree 会产生巨大的时间开销。
   

   基于这一问题，本文作者提出了ikd-tree,支持增量式的插入和删除，支持自行调整 tree 的结构、始终保持动态平衡。对于删除策略，采用了lazy delete算法，即当需要删除一个节点时，ikd-Tree不直接删除他，而是将这个节点标记为删除状态，在检索时直接跳过。等到下次真正需要更新树时再统一进行删除。

三、系统架构

FAST-LIO2的系统架构如上图所示。连续采样的LiDAR原始点首先在10ms和100ms之间的时间段内累积(一般将20ms作为一个周期)，累积的点云称为Scan。新Scan中累计的点云会和地图点进行匹配以实现状态估计，该局部地图通过一个紧耦合的迭代卡尔曼滤波器实现(流程图的红色部分)。局部地图中的地图点由ikd-Tree(蓝色大虚线块，见章节5)维护。如果当前LiDAR的FoV跨越地图边界，则从ikd-Tree中删除距离LiDAR姿态最远的地图区域的历史点。因此，ikd-Tree跟踪具有一定长度(本文称为“地图大小”)的大立方体区域内的所有地图点，并在状态估计模块中用于计算残差。优化后的姿态最终将新扫描中的点注册到全局帧中，并以里程计的频率插入到ikd-Tree中实现将它们合并到地图中，这个过程即建图。

四、状态估计

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

A. 卡尔曼模型

1.状态转换模型

定义第一帧IMU为全局坐标系，定义$T_L^I$为未知的外参。定义IMU积分模型如公式1，运动学模型如公式2：
公式2表示：当前时刻状态 = 上一时刻状态 + 两个时刻之间的运动

2.测量模型

运动学激光雷达通常一个接一个地对点进行采样。因此，当激光雷达经历连续运动时，所得到的点在不同的姿势下进行采样。为了纠正这种扫描中的运动，我们采用了[22]中提出的反向传播方法，该方法根据IMU测量值估计扫描中每个点相对于扫描结束时的姿态的LiDAR姿态。估计的相对姿态使我们能够根据扫描中每个单独点的精确采样时间将所有点投影到扫描结束时间。因此，可以将扫描中的点视为在扫描结束时同时采样的所有点。

记第K个Scan中第j个点为$p_j^L$，Scan中所有的点均已经进行运动补偿投影至Scan结束时刻对应的坐标系下，上标L表示改组点的坐标系为Scan结束时刻对应的坐标系。则考虑噪声影响后的LIDAR点坐标为：

Scan中的某个点通过外参变换到IMU坐标系，然后通过所估计的位姿变换到世界坐标系，该点会存再于全局地图中一个小的平面块上。连接Scan中的投影点与该平面块中的某个点可以得到一个向量(公式4右侧括号中的部分)，理想情况下该向量会和平面的法向量垂直，即点乘结果为0，具体表现为公式4。要注意的是k时刻下LIDAR到IMU的外参也是状态变量的一部分。

更进一步的，可以将上式写为公式5的形式。

B.迭代卡尔曼滤波

基于IMU积分结果实现预测过程，基于LIDAR的匹配残差实现更新过程。整个过程和FAST-LIO不一样的地方在于LIDAR匹配残差的构建方式，原来通过特征提取和匹配实现帧间匹配和地图匹配，在本文FAST-LIO2中，直接使用scan-to-map的方法进行匹配(具体见2残差计算)。

本篇只做简述，详细过程见博客：
《FAST-LIO论文解析》

1. 预测过程

即IMU积分过程，见公式6：

2. 残差计算

对于k个Scan的状态$x_k$的第k次迭代，更新结果为${\hat{x}}_k^k$，当$k=0$时，${\hat{x}}_k^k=x_k$。然后将Scan中的每个点投影到世界坐标系下，并通过ikd-tree找到与该点最接近的5个点。由这5个点可以得到平面的法向量$u_j$和质心 q j q_j