从零入门激光SLAM（二十一）—

FAST-LIO2: Fast Direct LiDAR-Inertial Odometry

论文地址：https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9697912

代码：https://github.com/hku-mars/FAST_LIO

一、文章概述

1.问题导向

基于视觉传感器的高分辨率和高精度的实时密集测绘仍然是一个巨大的挑战，同时受到光照和运动模糊的影响。新兴激光雷达显著降低了成本、尺寸、重量和功率，有可能使现有的和新兴的机器人应用程序大范围受益。但目前LIDAR-SLAM存在如下挑战。

LIDAR每秒产生数十万的点甚至更多，在有限的机载计算资源上，需要LiDAR里程计方法具有较高的计算效率。
特征提取模块的性能很容易受到环境的影响。
点云在运动过程中会产生显着的运动失真，影响里程计和绘图的性能
地图需要支持有效的对应搜索查询，同时实时更新并纳入新的测量结果。维护这样的地图是一项非常具有挑战性的任务。

2.目标

构建一个快速、健壮、通用的激光雷达惯性里程计

3.摘要

这篇文章介绍了FAST-LIO2：一个快速、健壮、通用的激光雷达惯性里程计框架。FAST-LIO2建立在高效紧耦合迭代卡尔曼滤波器的基础上，具有两个关键的新颖性，可以实现快速、稳健和准确的LiDAR导航(和测绘)。第一种是直接将原始点配准到地图上(并随后更新地图，即映射)，而不提取特征。这使得能够利用环境中的细微特征，从而提高了准确性。取消了手工设计的特征提取模块，也使其自然地适应不同扫描模式的新兴LiDAR；第二个主要创新是通过增量kD树数据结构-增量k-d树(IKD-Tree)来维护地图，该结构支持增量更新(即插入和删除点)和动态重新平衡。与现有的动态数据结构(八叉树、R∗-tree和纳米k-d树)相比，它在获得更好的整体性能的同时，自然地支持对树的下采样。我们对来自各种开放的LiDAR数据集的19个序列进行了详尽的基准比较。与其他激光雷达惯性导航系统相比，FAST-LIO2具有更高的精度和更低的运算量。我们还在小视场固体激光雷达上进行了各种真实世界的实验。总体而言，FAST-LIO2在计算效率(例如，大型室外环境中高达100赫兹的里程计和测绘)、稳健性(例如，在旋转高达1000度/S的杂乱室内环境中的可靠姿势估计)、通用性(即，可同时适用于多行旋转和固态LiDAR、无人机(UAV)和手持平台以及基于Inteland ARM的处理器)方面具有优势，同时仍获得了比现有方法更高的精度。我们实现的系统FASTLIO2和数据结构IKD-Tree都是在Github上开源的。

4.贡献

开发了一种增量k-d树数据结构，增量k-d树（ikd-Tree），以有效地表示大型密集点云图。
由于ikdTree计算效率的提高，我们直接将原始点注册到地图上，从而实现更准确、更可靠的扫描配准。
将前两个改变集成到FAST-LIO2上，新系统被称为FAST-LIO2，并在Github2上开源以造福社区。
进行了各种实验来评估所开发的 ikd-Tree、直接点配准和整个系统的有效性。

二、方法解析

2.1 系统pipeline

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

2.2 紧耦合迭代卡尔曼滤波

FAST-LIO2的论文中简要地解释了过滤器的基本公式和工作流程，如果读者需要了解更多，请读者参阅原论文并且结合FAST-LIO了解更多细节。

2.2.1 IMU预测步

1.IMU运动学模型

2.IMU离散运动学模型

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

2.2.2 观测模型

1.点云残差计算

激光雷达通常一个接一个地对点进行采样。因此，当激光雷达经历连续运动时，所得到的点在不同的坐标系下。为了纠正这种扫描中的运动，我们采用了中提出的反向传播方法对点云运动补偿。

去除该噪声可获得局部LiDAR坐标系中的真实点位置：

点云残差的计算公式如下

和FAST-LIO不一样的地方是不再提取特征点，而是使用源点计算残差。

2.迭代

IMU的预测残差如下

构建了关于IMU预测过程以及基于LIDAR匹配残差的最大后验分布，求解使得误差最小时对应的系统状态。

3.更新

迭代优化直至达到阈值，计算最后一次迭代的卡尔曼增益和状态量。

4.映射

2.3 映射

2.3.1 地图管理

地图点被组织成一棵IKD-Tree，它通过以里程计合并新扫描的点云来动态增长。为了防止地图的大小脱离绑定，在IKD-Tree上只保留激光雷达当前位置周围L长度的大局部区域中的地图点。

改进:1.重建树时，使用两个线程。2.删除点时，先标记为deleted，需删除的点达到一定比例后再重建树。3.类似的，添加点时，先放到未端，不平衡的部分达到一定比例后再重建树。

2.3.2 树的结构和构建

1.构造

ikd-tree是一种二叉树，在kdtree的基础上进行改进，支持高效的插入和删除操作，与kdtree不同的是在叶子节点与内部节点均存储点。ikd-tree的构建过程：与静态kd-tree的构建过程类似，ike-tree不断根据最长维度的终点进行划分空间，直到子空间中只有一个点。ikd-tree的数据结构如下

Point 点的坐标强度信息
Leftchild/rightchild 指向其左右子节点的指针
Axis 用于分割空间的分割轴记录
deleted 当点从地图中删除时，节点不会立即从树中删除，而只是将布尔变量 deleted 设置为 true
treedeleted 删除以节点为根的整个子树，则将 treedeleted 设置为 true
invalidnum 从（子）树中删除的点的数目
range 子树上点的范围信息

2.增量更新

ikd-Tree 上的增量更新指的是增量操作，支持 逐点操作和按框操作。

逐点操作在 k-d 树中插入、删除或重新插入单个点。

按框操作在给定的轴对齐长方体中插入、删除或重新插入所有点。

3.逐点插入与地图下采样

考虑到机器人的应用，ikd-tree支持同时进行点插入和地图下采样操作

4.惰性标签按框删除

在删除操作中，我们使用惰性删除策略。也就是说，这些点不会立即从树中删除，而只是通过将属性deleted(参见数据结构，第6行)设置为true来标记为“已删除”。如果以节点T为根的子树上的所有节点都已被删除，则T的属性treedeleted将被设置为true。因此，属性deleted和treedeleted被称为懒惰标签。标有“已删除”的点将在重建过程中(参见SectionV-D)从树中移除。

5.属性更新

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

6.Ikd-tree重平衡

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

平衡准则由两个子准则组成:α-平衡准则和α-删除准则。

假设ikd树的子树以T为根，当且仅当它满足以下条件时子树是α-平衡的:

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

如果ikd-Tree的子树满足这两个标准，则该子树是平衡的。如果所有子树都是平衡的，则整个树是平衡的。违反任何一个标准都将触发重新构建过程来重新平衡该子树:α平衡标准保持着树的最大高度。α-删除准则确保子树上的无效节点被移除以减小树的大小。通过降低k-d树的高度和大小允许将来高效的增量操作和查询。

7.重建及并行重建子树

假设对于一个子树触发了重建准则(如下图所示)，首先把子树展开为一个向量V，并去除其中的无效点(被lazy label标记删除的点)。然后，对向量V中的点进行重建一个更加完美的子树。为了保持较高的实时性，我们设计了一种双线程重建方法。我们提出的方法不是简单地在第二个线程中重建，而是通过操作记录器避免了两个线程中的信息丢失和内存冲突，从而始终保持knearest邻居搜索的完全准确性。

重建的方法参见 Algorithm 4。当违反平衡标准时，当子树的树大小小于预定值Nmax时，在主线程中重建子树；否则，在第二线程中重建子树。第二个线程上的重建算法如函数ParRebuild所示。将第二个线程中要重建的子树表示为t，将其根节点表示为T。第二个线程将锁定所有增量更新(即点插入和删除)，但不锁定该子树上的查询(第12行)。然后，第二线程将子树t中包含的所有有效点复制到点数组V中(即，展平)，同时保持原始子树不变，以用于重建过程中可能的查询(第13行)。展平后，原始子树被解锁，以便主线程进一步执行增量更新的请求(第14行)。这些请求将同时记录在一个名为operation logger的队列中。一旦第二个线程完成从点数组V构建新的平衡k-d树t`(第15行)，记录的更新请求将由函数IncrementalUpdates在t`上再次执行(第16-18行)。

上述的并行重建设计能够保证在第二个线程中重建过程中，主线程中的建图过程仍然能够在里程计的频率进行而不被任何中断。

8.最邻近搜索

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

三、总结

本文提出了FAST-LIO2，这是一个直接和健壮的LIO框架，其速度明显快于当前最先进的LIO算法，同时在各种数据集中实现了极具竞争力的或更好的准确性。速度的提高归功于删除了特征提取模块和高效的映射。提出并验证了一种新的支持动态点插入、删除和并行重建的增量k-d树(IKD-Tree)数据结构。在开放数据集上的大量实验表明，在LiDAR里程计的KNN搜索数据结构中，所提出的IKD-Tree能够获得最好的整体性能。通过利用里程表中更多的测点，提高了测绘效率，提高了对快速运动和稀疏场景的准确性和稳健性。FAST-LIO2的另一个好处是，由于取消了特征提取，它自然能够适应不同的激光雷达，而特征提取必须根据不同的激光雷达各自的扫描模式和密度仔细设计。作为里程计，FAST-LIO2可能会漂移很远的距离。

将来，我们可以在FAST-LIO2中集成闭环检测和LiDAR束调整来纠正这种长期漂移。此外，将探索与其他传感器的融合，如GPS或照相机，以使FAST-LIO2能够在部分或完全退化的环境中工作。