【论文阅读】FAST-LIO

零、摘要

本文提出了一种计算效率高、鲁棒的激光雷达-惯性里程计框架。我们利用紧密耦合的迭代扩展卡尔曼滤波器将激光雷达特征点与IMU数据融合，从而在快速运动、噪声或杂波环境中实现鲁棒导航。为了降低在大量测量条件下的计算量，我们提出了一个新的计算卡尔曼增益的公式。新公式的计算量不再依赖于测量维数，而是依赖于状态维数。提出的方法及其实现在各种室内和室外环境中进行了测试。在所有的测试中，我们的方法产生了可靠的实时导航结果:在一台四旋翼机载计算机上运行，它在一次扫描中融合了超过1200个有效特征点，并在25毫秒内完成了iEKF步骤的所有迭代。我们的代码在Github上是开源的。

一、介绍

1. 相机相比于激光雷达的一些缺陷，如：缺乏深度测量，对光照敏感。
2. 固态激光雷达的优点：成本低，重量轻，性能高，适用于无人机。
3. 激光雷达+IMU的一些问题：在嘈杂环境中容易退化，计算量大（点多），存在运动畸变，无人机螺旋桨和马达的持续旋转也会给IMU的测量带来显著的噪声。
4. FAST-LIO介绍：
   a. 防止退化采用紧耦合迭代卡尔曼滤波器来融合激光雷达特征点和IMU测量值，提出了 一种形式的反向传播过程来补偿运动失真；
   b. 提出了一种新的卡尔曼增益计算公式，降低计算量。

二、相关工作

1. LOAM及其变体：适用于结构化环境和大视场的激光雷达，但非常容易受到无特征环境或小视场激光雷达的影响。
2. 松耦合LIO：扫描配准与数据融合的分离降低了计算量。然而，它忽略了系统的其他状态(例如速度)和新扫描的姿态之间的关系。此外，在无特征环境下，扫描配准会在一定方向上退化，导致后期融合不可靠。
3. 紧耦合LIO：与松耦合方法不同，紧耦合的LIO通常将激光雷达的原始特征点(而不是扫描配准结果)与IMU数据融合。紧耦合LIO有两种主要方法:基于优化和基于滤波器。
4. 论文的方法属于紧耦合方法。采用迭代扩展卡尔曼滤波器来减小线性化误差。卡尔曼滤波器(及其变体)的时间复杂度为$O(m^2)$，其中m为测量维数，这可能导致在处理大量的LiDAR测量时计算量非常高。下采样会减少测量的数量，但以信息损失为代价。可以通过提取和拟合类似的地平面，减少测量的数量。然而，这并不适用于地面可能不总是出现的空中应用。

三、方法

A.框架概述

1. 一些重要的符号

2. 流程框架

B.系统描述

1. 符号定义

2. 连续模型

a. LiDAR和IMU的外参

$${}^I{\mathbf{T}}_L=\left({}^I{\mathbf{R}}_L,{}^I{\mathbf{p}}_L\right)$$

b. IMU运动学模型

$$\begin{array}{rl}{}^G{\dot{\mathbf{p}}}_I& ={{}^G\mathbf{v}}_I\\ {}^G{\dot{\mathbf{v}}}_I& ={{}^G\mathbf{R}}_I\left({\mathbf{a}}_m-{\mathbf{b}}_{\mathbf{a}}-{\mathbf{n}}_{\mathbf{a}}\right)+{}^G\mathbf{g}\\ {}^G\dot{\mathbf{g}}& =0\\ {}^G{\dot{\mathbf{R}}}_I& ={}^G{\mathbf{R}}_I[{\mathbf{\omega }}_m-{\mathbf{b}}_{\mathbf{\omega }}-{\mathbf{n}}_{\mathbf{\omega }}{]}_{\wedge }\\ {\dot{\mathbf{b}}}_{\mathbf{\omega }}& ={\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}\\ {\dot{\mathbf{b}}}_{\mathbf{a}}& ={\mathbf{n}}_{\mathbf{ba}}\end{array}$$

c. 离散模型

$$\mathcal{M}=SO(3)\times {\mathbb{R}}^{15},\dim (\mathcal{M})=18$$$$\begin{array}{rl}\mathbf{x}& \doteq {\left[\begin{array}{cccccc}G{\mathbf{R}}_I^T& G{\mathbf{p}}_I^T& {}^G{\mathbf{v}}_I^T& {\mathbf{b}}_{\omega }^T& {\mathbf{b}}_{\mathbf{a}}^T& {}^G{\mathbf{g}}^T\end{array}\right]}^T\in \mathcal{M}\\ \mathbf{u}& \doteq {\left[\begin{array}{cc}{\mathbf{\omega }}_m^T& {\mathbf{a}}_m^T\end{array}\right]}^T\\ \mathbf{w}& \doteq {\left[\begin{array}{cccc}{\mathbf{n}}_{\mathbf{\omega }}^T& {\mathbf{n}}_{\mathbf{a}}^T& {\mathbf{n}}_{\mathbf{b}\mathbf{\omega }}^T& {\mathbf{n}}_{\mathbf{ba}}^T\end{array}\right]}^T\end{array}$$

d.激光雷达测量数据预处理

激光雷达测量的是点在其自身坐标系下的坐标，由于原始的雷达点在非常高的速率下采集的，所以新点一接收到就处理是不可能的。更实际的方法是在一定时间内积累这些点，并一次性处理它们。也就是每获得一帧点云再进行处理。处理方式是提取平面点和角点这样的特征点。假设有m个特征点，每个特征点的采样时间为${\rho }_j\in (t_{k-1},t_k]$并表示为${}^{L_j}{p}_{f_i}$，其中$L_j$为${\rho }_j$时刻的激光雷达坐标。同时在一帧激光雷达中有多个IMU测量，每个在${\tau }_i\in [t_{k-1}，t_k]$处采样，每个都有
离散模型中相应的状态$x_i$。注意，最后一个LiDAR特征点是扫描的结束，即，${\rho }_m=t_k$，而IMU测量可能不一定与扫描的开始或结束对齐。

3. 状态估计

论文使用IEKF实现状态估计，并且在状态估计的切空间中表征估计协方差。假设在$t_{k-1}$时最后一次雷达扫描的最佳状态估计为${\overline{x}}_{k-1}$,协方差矩阵为${\overline{P}}_{k-1}$，其中${\overline{P}}_{k-1}$表示如下定义的随机误差状态向量的协方差：

a.前向传播

其中$∆t={\tau }_{i+1}-{\tau }_i$
为了传播协方差，论文使用下面获得的误差状态动态模型：

$F_{\tilde{x}}$和$F_w$的计算过程见附录A

所以有协方差的传播公式：
$${\hat{\mathbf{P}}}_{i+1}={\mathbf{F}}_{\tilde{\mathbf{x}}}{\hat{\mathbf{P}}}_i{\mathbf{F}}_{\tilde{\mathbf{x}}}^T+{\mathbf{F}}_{\mathbf{w}}\mathbf{Q}{\mathbf{F}}_{\mathbf{w}}^T;{\hat{\mathbf{P}}}_0={\bar{\mathbf{P}}}_{k-1}.$$
这个传播过程一直到新一帧到来。其中的传播状态和协方差分别表示为${\hat{x}}_k,{\hat{P}}_k$,其中${\hat{P}}_k$表示真值$x_k$和状态传播值${\hat{x}}_k$之间误差的协方差。

b.反向传播和运动补偿

由于特征点采集的时间不同（特征点采集时间${\rho }_j$<$t_k$），所以会产生运动畸变，这就需要进行运动补偿，反向传播公式如下：
$${\check{\mathbf{x}}}_{j-1}={\check{\mathbf{x}}}_j(-\Delta t\mathbf{f}({\check{\mathbf{x}}}_j,{\mathbf{u}}_j,0))$$
后向传播是在特征点的采集频率下执行的，该频率通常远高于IMU速率。对于在两个IMU测量之间采样的所有特征点，使用左侧IMU测量作为反向传播中的输入(这句话为理解下面三个方程的关键)。此外，注意到$f(x_j，u_j，0)$的噪声为零，反向传播可以被简化为：
$$\begin{gathered} {}^{I_k}{\check{\mathbf{p}}}_{I_{j+1}}={}^{I_k}{\check{\mathbf{p}}}_{I_j}-{}^{I_k}{\check{\mathbf{v}}}_{I_j}\Delta t,s.f.{}^{I_k}{\check{\mathbf{p}}}_{I_m}=0; \\ \begin{array}{c}{}^{I_k}{\check{\mathbf{v}}}_{I_{j-1}}={}^{I_k}{\check{\mathbf{v}}}_{I_j}-{}^{I_k}{\check{\mathbf{R}}}_{I_j}({\mathbf{a}}_{m_{i-1}}-{\hat{\mathbf{b}}}_{{\mathbf{a}}_k})\Delta t-{}^{I_k}{\hat{\mathbf{g}}}_k\Delta t,\\ s.f.{}^{I_k}{\check{\mathbf{v}}}_{I_m}={}^G{\hat{\mathbf{R}}}_{I_k}^T{}^G{\hat{\mathbf{v}}}_{I_k},{}^{I_k}{\hat{\mathbf{g}}}_k={}^G{\hat{\mathbf{R}}}_{I_k}^T{}^G{\hat{\mathbf{g}}}_k;\end{array} \\ {}^{I_k}{\check{\mathbf{R}}}_{I_{j-1}}={}^{I_k}{\check{\mathbf{R}}}_{I_j}\mathrm{Exp}(({\hat{\mathbf{b}}}_{{\omega }_k}-{\omega }_{m_{i-1}})\Delta t),s.f.{}^{I_k}{\mathbf{R}}_{I_m}=\mathbf{I}. \end{gathered}$$
其中${\rho }_{j-1}\in [{\tau }_{i-1},{\tau }_i)，∆t={\rho }_j-{\rho }_{j-1}，s.f.$means “starting from”.
通过上三个方程就能获得激光雷达${\rho }_j$时刻和$t_k$时刻的相对位姿：${}^{I_k}{\check{T}}_{I_j}=({}^{I_k}{\check{R}}_{I_j},{}^{I_k}{\check{p}}_{I_j})$，
这个相对姿态可以将局部测量${}^{L_j}{p}_{f_j}$投影到扫描结束端${}^{L_k}{p}_{f_j}$
$${}^{L_k}{\mathbf{p}}_{f_j}={}^I{\mathbf{T}}_L^{-1I_k}{\check{\mathbf{T}}}_{I_j}{}^I{\mathbf{T}}_L{}^{L_j}{\mathbf{p}}_{f_j}.$$
然后就可以利用${}^{L_k}{p}_{f_j}$去构造残差。（去畸变的思路和LOAM差不多）

c. 残差计算

通过上一步的运动补偿，可以获得所有在同一时刻$t_k$采样得到的特征点集
{${}^{L_k}{p}_{f_j}$}，并用它来构造残差。假设当前迭代卡尔曼滤波的迭代数为$k$，其对应的状态为${\hat{x}}_k^k$。当$k=0,{\hat{x}}_k^k={\hat{x}}_k$，传播的预测状态用方程为
所以特征点集{${}^{L_k}{p}_{f_j}$}可以通过如下方式变换到全局坐标下。
$${}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }={}^G{\hat{\mathbf{T}}}_{I_k}^{\kappa I}{\mathbf{T}}_L{}^{L_k}{\mathbf{p}}_{f_j};j=1,\cdots ,m.$$
接下来就是计算点—线，点—面的残差。
$${\mathbf{z}}_j^{\kappa }={\mathbf{G}}_j\left({}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }-{}^G{\mathbf{q}}_j\right)$$
${}^G{\hat{\mathbf{p}}}_{f_j}^{\kappa }$表示特征点的全局坐标系坐标，
${}^G{\mathbf{q}}_j$表示线/面上的点，
${\mathbf{G}}_j$表示线/面特征，
${\mathbf{z}}_j^{\kappa }$表示残差。
方法和LOAM中类似。

d.迭代状态更新

为了将残差${\mathbf{z}}_j^{\kappa }$和IMU预测状态${\hat{x}}_k$,协方差${\hat{P}}_k$融合，需要将残差${\mathbf{z}}_j^{\kappa }$与真值$x_k$和测量噪声联系起来的测量模型线性化。测量噪声来源于测量点${}^{L_j}{p}_{f_j}$时的雷达测距和定向噪声${}^{L_j}{n}_{f_j}$。从测量点${}^{L_j}{p}_{f_j}$中去除这一噪声，就得到了真实点的位置。
$${}^{L_j}{\mathbf{p}}_{f_j}^{\text{gt}}={}^{L_j}{\mathbf{p}}_{f_j}-{}^{L_j}{\mathbf{n}}_{f_j}$$
这个真实点带入残差方程理应结果为0，即
$${0=\mathbf{h}}_j({\mathbf{x}}_k,{}^{L_j}{\mathbf{n}}_{f_j}){=\mathbf{G}}_j({}^G{\mathbf{T}}_{I_k}{}^{I_k}{\check{\mathbf{T}}}_{I_j}{}^I{\mathbf{T}}_L{(}^{L_j}{\mathbf{p}}_{f_j}-{}^{L_j}{\mathbf{n}}_{f_j}){-}^G{\mathbf{q}}_j)$$
在${\hat{x}}_k^k$处进行一阶近似(这一部分对应于EKF的线性化)
$$\begin{array}{rl}0& ={\mathbf{h}}_j\left({\mathbf{x}}_k,{}^{L_j}{\mathbf{n}}_{f_j}\right)\simeq {\mathbf{h}}_j\left({\hat{\mathbf{x}}}_k^{\kappa },0\right)+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }+{\mathbf{v}}_j\\ & ={\mathbf{z}}_j^{\kappa }+{\mathbf{H}}_j^{\kappa }{\tilde{\mathbf{x}}}_k^{\kappa }+{\mathbf{v}}_j\end{array}$$
其中，
$H_j^k$是${\tilde{\mathbf{x}}}_k^{\kappa }$关于$h_j$的雅可比矩阵，${\tilde{\mathbf{x}}}_k^{\kappa }$取值为0，${\mathbf{v}}_j\in \mathcal{N}(0,{\mathbf{R}}_j)$来自测量噪声${}^{L_j}{\mathbf{n}}_{f_j}$
注意：先验$x_k$通过正向传播获得，将其线性化

其中$J^k$

构建MAP

得到标准的迭代卡尔曼滤波器
定义$$\begin{gathered} \mathbf{H}=[{\mathbf{H}}_1^{{\kappa }^T},\cdots ,{\mathbf{H}}_m^{{\kappa }^T}{]}^T, \\ \mathbf{R}=\mathbf{diag}({\mathbf{R}}_1,\cdots {\mathbf{R}}_m), \\ P=\begin{array}{r}({\mathbf{J}}^{\kappa }{)}^{-1}{\hat{\mathbf{P}}}_k({\mathbf{J}}^{\kappa }{)}^{-T},\mathrm{and}{\mathbf{z}}_k^{\kappa }={\left[{\mathbf{z}}_1^{{\kappa }^T},\cdots ,{\mathbf{z}}_m^{{\kappa }^T}\right]}^T\end{array} \end{gathered}$$
这个更新后的估计值${\hat{x}}_k^{k+1}$为第k+1次迭代的结果，循环迭代直到收敛至。收敛后优化的状态估计和方差为：
$${\bar{\mathbf{x}}}_k={\hat{\mathbf{x}}}_k^{\kappa +1},{\bar{\mathbf{P}}}_k=\left(\mathbf{I}-\mathbf{KH}\right)\mathbf{P}$$
论文重点
常用的卡尔曼增益形式的一个问题是，它需要对度量维度内的矩阵$HP{H}^T+R$求逆。在实践中，LiDAR特征点的数量是非常大的，这种大小的矩阵的逆是禁止的。因此，现有的一些方案只使用了少量的测量方法。论文证明了这种限制是可以避免的。直觉源于
，其中代价函数在状态之上，因此解决方案的计算复杂度取决于状态维。通过该式可以得到新的Kalman增益形式，如下图所示:$$\mathbf{K}={\left({\mathbf{H}}^T{\mathbf{R}}^{-1}{\mathbf{H}+\mathbf{P}}^{-1}\right)}^{-1}{\mathbf{H}}^T{\mathbf{R}}^{-1}$$
我们在附录B中证明了基于矩阵逆引理的两种形式的卡尔曼增益确实是等价的。由于激光雷达测量是独立的，因此协方差矩阵R是(块)对角线，因此新公式只需要两个矩阵的状态维，而不是测量。由于在LIO中状态维数通常远低于测量值(例如，在10 Hz扫描速率下一次扫描1000多个有效特征点，而状态维数只有18)，新公式大大节省了计算量。

e.算法流程

4.地图更新

$${}^G{\bar{\mathbf{p}}}_{f_j}={}^G{\bar{\mathbf{T}}}_{I_k}{}^I{\mathbf{T}}_L{}^{L_k}{\mathbf{p}}_{f_j};j=1,\cdots ,m.$$

5.初始化

为了获得系统状态的良好初始估计(例如重力矢量Gg、偏差和噪声协方差)，从而加速状态估计，需要初始化。在FAST-LIO中，初始化很简单:保持LiDAR静态数秒(论文中所有实验均为2秒)，然后使用收集到的数据初始化IMU偏差和重力矢量。如果激光雷达支持非重复扫描(如Livox AVIA)，保持静态也允许激光雷达捕获初始的高分辨率地图，这对后续导航是有益的。

四、附录

A.$F_{\tilde{x}}$和$F_w$的计算

其中式子（5）为

B.两种卡尔曼增益的等价证明