FAST-LIO(一)：论文简要介绍

---

前言

论文题目：FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter，FAST-LIO：一种基于紧耦合IKF算法的快速的，鲁棒性的激光雷达惯性里程计。这里主要按照自己的理解对论文内容要简短的介绍。

---

论文主要贡献

相比于其他的松耦合方案，该论文提出了一套激光雷达惯性紧耦合方案，采用了IEKF（迭代扩展卡尔曼滤波器）去减缓线性误差。

1.论文架构

论文架构图如上：输入为激光雷达数据和imu数据。雷达数据经过预处理环节：激光点累积，以及特征提取（平面特征，边缘特征），imu数据经过前向推算，后向推算，以及为激光雷达点云提供畸变去除，在更新过程中，联合IMU和雷达数据构建观测方程，滤波框架采用IEKF。IMU的后向过程是一个后向的推算，主要作用是为雷达去畸变提供运动约束。

2.系统介绍

2.1 符号定义

定义了两种符号，如下所示，可以支持在旋转矩阵以及流形两种符号的公式表述。当是旋转矩阵时，就代表加减。

2.2 连续域模型

假设IMU和激光雷达刚性连接，所以它们之间的安装角和杆臂是固定的。如下是连续域下的运动方程，为微分形式。这里把零偏建模成随机游走过程。

2.3 离散域模型

离散域下的模型如下所示，连续域转到离散域要乘以采样时间t。

从下面公式可以到，状态变量为18维，这里不太清楚为啥把世界坐标系下的重力放在状态变量中了，因为它本身就是个固定值。

2.4 雷达数据预处理

雷达原始数据频率非常高，200Hz，一般做法是累积一段时间，再一块接收，也就是一帧数据，实现降频，一般为50Hz，假设在20ms时间内雷达发生了转弯和线性运动，所以这一帧中的雷达点采集的时间戳是不一样的，就需要借助IMU来去除运动畸变。

3.状态估计

这是论文的核心内容。

3.1 前向过程

前向过程其实就是卡尔曼滤波的预测过程，有两个方程：状态变量的预测方程，以及协方差矩阵的预测方程。
如下是系统预测方程。

如下分别为系统转换矩阵和噪声转换矩阵。

下面是协方差先验过程。

3.2 后向过程&运动补偿

为何要进行后向过程，其实涉及到一个时间软同步的问题。IMU和激光雷达的时间不可能完全对比，相差在几个ms。激光雷达一帧数据由很多点组成，这些点显然不是同一时间测量得到的，所以需要补偿时间差带来的运动误差。如下所示，已知j刻度的激光雷达点坐标，如何求k时刻的激光雷达点坐标，k时刻代表真正的采集时间，当然，这个时间可以用线性插值方法获取，也存在一定的插值误差，但是一般误差足够小了。需要先转换到IMU坐标系，再通过后方推算得到k时刻IMU坐标下的激光点坐标，最后再转换回到激光雷达坐标系，就得到了k时刻的雷达点坐标系。

3.3 残差计算

残差计算是为了构建滤波器的观测方程。如下所示，雷达点坐标，先转到IMU坐标系，再转到世界坐标系下。

接下来，需要计算世界坐标系下的激光点坐标和地图中实际对应的点之间的距离，并且投影到对应的边的方向上来。如下公式所示：

如何寻找最近地图中的边，或者平面，可以建立KD树搜索。通过构建残差方程，对于残差过大的激光点观测，我们就可以把它当成野值处理。

3.4 迭代更新

我们建立的观测方程如下，其实和残差构建是一致的。

我们知道EKF需要做泰勒公式展开，求雅可比矩阵，如下所示：

H矩阵就是对应的雅可比矩阵，文中提到下面两种计算增益的公式是等价的，第一个公式相比第二个公式，在计算效率上会更高。


协方差的恶后验计算公式如下：

状态变量的后验如下：

从最优化的角度来看滤波器，则IEKF的最优化公式如下：

状态估计的整体流程如下，IEKF的迭代过程，有一个终止条件，就是看残差小于一个特定值。

4. 地图更新

地图更新过程就是把激光雷达点投影到世界坐标系的过程，如下公式所示：

5. 初始化

初始化需要静止2s，静止状态下可以计算陀螺仪的零偏，以及加速度计的重力向量。

6. 实验结果

如下所示，可以看到，相比LOAM以及LOAM+IMU的方案，FAST-LIO用时更短，并且LOAM+IMU是松耦合的方案。

从下图中可以看到，相比LOAM以及LOAM+IMU，FAST-LIO无论在慢速还是快速运动情况下，建图效果都更好。