FAST-LIO2: 快速直接的激光雷达-惯性里程计

3 系统总览

FAST-LIO2的流程图如图1所示。顺序采样的激光雷达原始点首先在10ms（对于100Hz更新）和100ms（对于10Hz更新）之间的时间段内累积。累积的点云称为一帧点云。为了执行状态估计，新的一帧点云中的点和地图点配准（即里程计），其中地图点通过紧耦合的迭代卡尔曼滤波器框架（红色大虚线块，参见第四章）保存在一个大型局部地图中。大型局部地图中的全局地图点由增量k-d树结构ikd-Tree（蓝色大虚线块，参见第五章）组织。如果当前激光雷达视场角范围越过地图边界，则距离激光雷达姿态最远的地图区域中的历史点将从 ikd-Tree中被删除。因此，ikd-Tree以一定长度(本文中称为“地图大小”)跟踪大立方体区域中的所有地图点，并用于计算状态估计模块中的残差。优化后的位姿最终将新的一帧点云中的点配准到全局坐标系中，并把通过以里程计的速率插入ikd-Tree的方式，将新的一帧点云中的点合并到地图中（即建图）。

图1 FAST-LIO2系统的总览

4 状态估计

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

A.运动学模型

我们首先推导出系统模型，它由一个状态转换模型和测量模型成。

1) 状态转移模型

取第一个IMU坐标系（记为I）作为全局坐标系（表示为G）并记

为激光雷达和IMU之间未知的外参，运动模型是：

其中

表示IMU在全局坐标系中的位置和姿态，Gg是全局坐标系中的重力矢量，am和ωm是IMU测量值，na和nω表示am和ωm的测量噪声，ba和bw是建模为由nba和nbw驱动的随机游走过程的IMU偏差，符号

表示向量

的斜对称叉积矩阵。

i表示IMU测量值的索引。基于[22]中定义的

操作，连续运动学模型（1）可在IMU采样周期∆t进行离散化[55]：

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

2）测量模型：激光雷达通常逐个地采样点。因此，当激光雷达进行连续运动时，会在不同的姿态下采样得到点。为了纠正这种扫描中的运动，我们采用了[22]中提出的反向传播，该反向传播基于惯性测量单元的测量，估计一帧点云中每个点相对于一帧点云结束时姿态的激光雷达姿态。估计的相对位姿使我们能够根据一帧点云中每个单独点的精确采样时间，将所有点投影到扫描结束时间。因此，可以将一帧点云中的点视为在扫描结束时间时同时采样得到的。