阅读FAST-LIO2（Lidar-IMU）论文

一、摘要

本文介绍了 FAST-LIO2：一种快速、稳健且通用的 LiDAR 惯性里程计框架。 FASTLIO2 建立在高效紧耦合迭代卡尔曼滤波器的基础上，具有两个关键创新，可实现快速、稳健和准确的 LiDAR 导航（和映射）。第一个是直接将原始点注册到地图（并随后更新地图，即映射）而不提取特征。这可以利用环境中的细微特征，从而提高准确性。手动设计的特征提取模块的消除也使其自然适应不同扫描模式的新兴激光雷达；第二个主要创新是通过增量 k-d 树数据结构 ikd-Tree 维护映射，该结构支持增量更新（即点插入、删除）和动态重新平衡。与现有的动态数据结构（octree、R∗-tree、nanoflann k-d tree）相比，ikd-Tree 在整体性能优越的同时自然支持对树的下采样。我们对来自各种开放 LiDAR 数据集的 19 个序列进行了详尽的基准比较。与其他最先进的 LiDAR 惯性导航系统相比，FAST-LIO2 始终以更低的计算负载实现更高的精度。还对具有小 FoV 的固态 LiDAR 进行了各种真实世界的实验。总体而言，FAST-LIO2 具有计算效率（例如，在大型室外环境中高达 100 Hz 的里程计和映射）、鲁棒性（例如，在旋转高达 1000 度 = s 的杂乱室内环境中可靠的姿态估计）、多功能（即，适用于多线旋转和固态激光雷达、无人机和手持平台，以及基于英特尔和 ARM 的处理器），同时仍能实现比现有方法更高的精度。

二、问题

目前的激光雷达传感器每秒产生数十万到数百万的大量三维点。在有限的星载计算资源下实时处理如此大量的数据，要求激光雷达测程方法具有较高的计算效率;
为了减少计算量，通常基于局部光滑性提取特征点，如边缘点或平面点。然而，特征提取模块的性能容易受到环境的影响。例如，在没有大平面或长边缘的无结构环境中，特征提取会导致特征点较少。当激光雷达视场(FoV)很小时，这种情况会更加严重，这是出现固态激光雷达[16]的典型现象。此外，根据扫描模式(如旋转、基于棱镜的[15]、基于mems的[14])和点密度的不同，LiDAR和LiDAR的特征提取也有所不同。因此采用激光雷达测程方法通常需要大量的手工工作;
激光雷达点的采样通常是连续的，而传感器经历连续的运动。这一过程造成了严重的运动失真，影响了里程计和映射的性能，特别是当运动是严重的。惯性测量单元(imu)可以缓解这个问题，但引入了额外的状态(如偏差、外部)来估计;
激光雷达通常有一个很长的测量范围(例如，数百米)，但在扫描中扫描线之间的分辨率相当低。生成的点云测量稀疏地分布在一个大的3D空间中，需要一个大而密集的地图来登记这些稀疏的点。此外，地图需要支持有效的查询通信搜索，同时实时更新，纳入新的测量。

三、具体工作

1）我们开发了一种增量k-d树数据结构，ikd-Tree，以有效地表示一个大的稠密点云图。除了高效的最近邻搜索外，该数据结构还支持增量映射更新(即点插入、树上下采样、点删除)和以最小计算代价进行动态重新平衡。这些特性使得ikd-Tree非常适合激光雷达里程计和地图应用，在计算受限的平台上可以实现100hz里程计和地图，比如基于Intel i7的微型无人机车载计算机，甚至是基于arm的处理器。

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

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

4）我们进行了各种实验来评估开发的ikd-Tree、直接点配准和整个系统的有效性。在18个不同大小序列上的实验表明，ikdTree在激光雷达里程测量和映射应用中，相对于现有的动态数据结构(八叉树、R∗树、纳米翼面k-d树)具有更好的性能。通过对来自各种开放LiDAR数据集的19个序列进行详尽的基准比较，FAST-LIO2与其他最先进的LiDAR惯性导航系统相比，在较低的计算负荷下始终保持较高的精度。最后，我们展示了FAST-LIO2在具有挑战性的现实世界数据上的效果，这些数据是由新兴的固态激光雷达在非常小的视场下收集的，包括剧烈运动(例如，旋转速度高达1000℃=s)和无结构环境。

4.1 Overall pipeline

顺序采样的LiDAR原始点每10ms (100Hz更新)到100ms (10Hz更新)积累一次，累积的点云称为一次扫描。为了执行状态估计，新扫描中的点通过紧密耦合迭代卡尔曼滤波框架(红色的大虚线块)注册到大型局部地图中维护的映射点(即里程计)。大型局部地图中的全球地图点由增量k-d树结构的ikd-Tree组织(蓝色的大虚线块)。如果当前LiDAR的FoV范围越过地图边界，到LiDAR姿态的最远地图区域的历史点将从ikd-Tree中删除。因此，ikd-Tree跟踪一个大立方体区域内具有一定长度(本文中称为“地图大小”)的所有地图点，并用于计算状态估计模块中的残差。优化后的姿态最终将新扫描中的点注册到全局框架中，并通过以里程数计的速度插入到ikd-Tree中将它们合并到地图中。

Fov - 视场角 https://www.colorspace.com.cn/kb/2013/02/24/fov/
这里指 雷达视场：探测视野，包括水平和垂直两个方向，就像是我们打开手电筒照一面墙，光能覆盖的范围一样。机械式激光雷达能够360°旋转，所以水平FOV是360°。固态激光雷达的水平FOV会小一些，比如华为某款120°已经算是大视角了。水平FOV越大，能够探测的范围越广。垂直FOV只对多线束激光雷达有用。是指最上面一束激光和最下面一束激光形成的夹角。

雷达的其他参数：https://www.eet-china.com/kj/62185.html

4.2 状态估计

FAST-LIO2的状态估计继承了FAST-LIO[22]的特点，是一种紧耦合迭代卡尔曼滤波器，并进一步引入了LiDAR-IMU外部参数的在线校准。

4.2.1 运动学模型

我们首先推导出(derive)系统模型，该模型由状态转移模型和度量模型组成。

(1) 状态转换模型

取第一个IMU框架(记为I)作为全局框架(记为G)，记为ITL = (IRL, IpL)为LiDAR和IMU之间的未知外部属性，运动学模型为：

式中，GpI, GRI 表示IMU在全局框架中的位置和姿态，Gg是全局坐标系中的重力矢量，am和wm是IMU的测量值，na和nw表示am和wm的测量噪声，ba和bw表示nba和nbw驱动下的随机游动过程模型的IMU偏差，符号|a|^表示一个映射了叉乘运算的向量a∈R3的斜对称矩阵。

设i为IMU测量值的索引。根据[22]中定义的运算符□+，可以在IMU采样周期∆t处对连续运动学模型(1)进行离散化：

函数f、状态x、输入u和噪声w，定义如下:

(2) 测量模型

激光雷达通常一个接一个地采样点。因此，当 LiDAR 进行连续运动时，会以不同的姿势对结果点进行采样。为了纠正这种扫描中的运动，我们采用了[22]中提出的反向传播，它根据IMU测量值估计扫描中每个点相对于扫描结束时间的位姿的激光雷达位姿。估计的相对姿态使我们能够根据扫描中每个单独点的精确采样时间，将所有点投射到扫描结束时间。因此，扫描中的点可以被视为在扫描结束时同时采样的所有点。
设k为LiDAR扫描的索引，{Lpj, j=1,...,m}为第k次扫描在扫描结束时在本地LiDAR坐标系L上采样的点。由于激光雷达的测量噪声，每个测量点Lpj通常会受到由测距和定向噪声组成的噪声Lnj的污染。去除这些噪声，就得到了在激光雷达局部坐标系中的真点位置Lpjgt：

该真实的点，在使用相应的LiDAR姿态GTIk = (GRIk, GpIk)和外部属性ITL= (IRL, IpL)而投影到全球框架后，应该正好位于地图上的一个局部小平面补丁上，即：

式中，Guj是相应平面的法向量，Gqj是平面上的一点，如下图。

需要注意的是，GTIk和ITLk都包含在状态向量xk中。因此，第j个测量值Lpj可以从式(4)归纳为更紧凑的形式，如下。该式定义了状态向量xk的隐式测量模型。