FAST-LIO论文梳理及公式理解

一、论文整体流程

1.1引言 Introduction

        SLAM技术是移动机器人的必备的基础需求。视觉SLAM方法缺乏深度信息、计算量较大且受光线影响大，激光SLAM更适合小型移动机器人搭载。

        固态激光雷达作为一个趋势，具有成本低、轻量化、高性能的特点，很适合搭载于需要建图操作且受光线影响较大的小型UAV上。

        尽管有这些好处，但是固态激光雷达也给SLAM带来了一些挑战：1）激光SLAM通常要提取环境中结构化的点、线特征。当移动机器人工作在嘈杂环境下无法提取有效的结构化特征，观测容易失效，而固态激光更小的视角会加剧这个退化；2）固态激光雷达点云数量大，当这些点云在退化场景下不足以计算出位姿时，紧耦合大量的点云与imu对板载工控机来说计算量较大；3）由于雷达具有多组激光收发器，在车辆运动时采样的点云容易造成畸变，螺旋桨和发动机的转动也会给imu的数据采集带来显著的噪声。

        为了解决小型移动机器人的雷达导航问题，我们提出了一种计算效率高、鲁棒的激光惯性里程计算法。具体贡献如下：

         1）为了解决快速移动、噪声、杂乱环境下的退化问题，我们提出了使用紧耦合迭代拓展卡尔曼滤波（也就是迭代误差状态卡尔曼滤波ESKF,类似LINS）来紧耦合激光特征点和IMU的测量值，提出一个反向传播来补偿激光点云的运动畸变。

         2）为了降低由于大量雷达特征点造成的高计算负载，我们提出了一个新的卡尔曼增益计算公式，并证明它与传统卡尔曼增益计算结果是等价的。新计算公式计算的复杂度取决于状态的维度而不是观测量值的维度。

         3）我们将本方法配置成为一个快速、鲁棒的激光惯性里程计软件包在小型四旋翼无人机板载电脑上进行了测试验证。

        4）我们在多个室内外场景下，使用小型四旋翼无人机平台进行了验证本算法在快速移动和强烈震动下的鲁棒性。

1.2 相关工作 Related works

A.单雷达里程计与建图

         ICP是激光里程的基础。ICP在稠密点云配准中效果好，但是在稀疏点云中缺少其配准所需要的精确的配准点对，因此后续提出了点-面的GICP被提出了。然后张绩组合点-面和点-线的ICP提出LOAM框架。因此，后续基于loam的多个框架被提出，lego-loam，lio-sam。但是，这些方法在结构化环境下效果好，在特征稀疏的非结构环境下容易退化。

B.松耦合激光惯性里程计

       松耦合的过程就是通过点云配准得到一个位姿，然后跟IMU观测的位姿来融合。将扫描配准和数据融合分开进行虽然可以降低计算负载。然而，在计算过程中忽略了系统中的其他状态对与新配准点云之间的关联。此外，在特征较少的环境下，激光匹配依旧会在特定方向退化，对后续的融合造成较大误差。

C.紧耦合激光惯性里程计

       紧耦合方法是将原始的点云特征点与IMU数据融合。紧耦合激光里程计主要有两个方法：基于优化的方法和基于滤波的方法。（当时条件下，随着点云特征点数量的增多，滤波方法更快，但是现在因子图优化方法也解决这个问题）

      Geneva等人用图优化方法，将IMU预积分约束与激光雷达特征点平面约束融合。最近，Ye等人提出了LIOM框架，该框架使用类似的图优化方法，但不仅包含激光雷达特征点平面约束，还添加了边缘约束。

       基于滤波器的方法，Bry等人使用了高斯粒子滤波器(GPF)来融合IMU与平面2D激光雷达的数据。这种方法也被用于波士顿动力公司的Atlas类人机器人上面。由于粒子滤波器的计算复杂度随着特征点的数量与系统的维数的增加而快速增加，卡尔曼滤波器及其变种通常更加受欢迎，如果扩展卡尔曼滤波器(EKF)，无迹卡尔曼滤波(UKF)，还有迭代卡尔曼滤波器。

      本文方法属于基于滤波的紧耦合方法，使用拓展卡尔曼滤波来减缓线性化的误差。卡尔曼滤波器(及其变种)具有时间复杂度O(m²)，卡尔曼滤波计算的复杂度取决于观测维度（m），如果激光点云特征点较多，将会造成较大的计算负载。单纯的进行降采样处理点云，虽然可以减少点云的数量但是也会损失一定的特征信息。lego-loam对地面进行拟合提取可以有效降低计算量，但是这个方对本文的不一定总能观测到地面的四旋翼无人机不适合。

重点总结()

--SLAM问题中紧耦合与松耦合的概念
--紧耦合与松耦合的优缺点
--什么时候用紧耦合？什么时候用松耦合？
--紧耦合滤波器的激光雷达SLAM问题中，测量维度太高会有什么问题？如何减少测量维度？
--ESKF相比传统卡尔曼滤波有什么优势？
答案： ESKF推导

FAST-LIO论文精读及公式推导：

https://zhuanlan.zhihu.com/p/617068992?utm_id=0

二、本文方法

2.1 本文框架概述 Framwork overview

1）点云数据输入到特征提取模块，提取出线、面特征；

2）提取的点面特征和IMU的观测数据输入到状态估计模块，IMU输入进行前向传播积分得到粗略位姿估计，反向传播进行运动补偿，计算雷达里程计残差利用eskf估计位姿变化，直到收敛；

3）估计的位姿将特征点云注册到全局坐标系，将之前的所有特征点云组合起来构建一个点云地图。更新后的特征地图用于在后续注册更多的新特征点。

重点关注以下问题：

①点云特征提取是提取的什么特征？是如何提取的？(输入？输出？)

②imu前向传播是如何传播的？本质是什么(可以KF的运动方程、观测方程方面思考)(输入？输出？)

③ESKF的运动方程？观测方程？

④残差是如何计算的？(输入？输出？)

⑤地图是如何更新的？

2.2 系统描述：

  2.2.1 文中的字符解释：

        流形是局部类似于欧几里得空间的空间，在流形上的每个点，我们可以定义一个与该点相切的线性空间，这个空间就是正切空间。正切空间是局部近似流形的一个线性空间。在姿态中，旋转矩阵的李群就是一个SO(3)流形，其局部对应的欧氏空间是李代数so(3)，为了避免奇异性和保持结构特性，使用流形进行运算。

2.2.2 连续模型

       这一节主要为IMU的运动模型及其积分。IMU的观测模型P为位置、R为姿态、v为速度，IMU的运动模型如下（第一帧IMU的坐标系定义为全局坐标系，G）：

       上述矢量中，Gg为重力矢量，其不随时间变化，导数为0；和为IMU测量的白噪声，与分别是IMU的零偏，认为其符合随机游走模型，即它们的倒数服从高斯分布。要理解其他几个量，先要知道IMU测量值和真值之间的关系为：

其中，a，表示加速度和角速度真值。这个式子是通用的IMU测量模型，即测量值=真值+零偏+噪声。加速度测量值多包含了一项重力分量，由于它不随时间变化，因此只需要将第一帧的重力分量乘上变换矩阵转换到当前IMU的坐标系即可。

上述矢量对时间的求导即为：

2.2.3 离散模型

       在IMU采样周期 Δ t处离散化公式(1)中的连续模型，系统状态分解为两个组成部分：流型中的状态量加上正切空间中的误差量，表达如下：

状态变量x、输入u、噪声w、方程为f（）：

即为：

2.2.4 点云预处理

        激光雷达测量的数据是其自身坐标系中点坐标。由于原始激光雷达点采样频率是非常高的(例如，200kHZ)，因此，通常不可能在每次接收到新的点后都对其进行处理。一种更实用的方法是将这些点累计一段时间，然后一次处理所有这些点。在FAST-LIO中，最小累积间隔设置为20ms，这使得状态估计(LIO里程计输出)与地图更新频率最高可达50hz。

        从原始点云中提取特征点。

2.3 状态估计

2.3.1 前向传播（forward propagation）

        所谓的前向传播算法就是：通过IMU积分计算一个粗略的状态量，这个状态量用于后续的反向传播来补偿运动失真；船舶误差量，并计算对应的协方差。

       在论文中是卡尔曼滤波的预测过程，有两个方程：状态变量的预测方程，以及协方差矩阵的预测方程

误差 = 真实值 ⊖ 传播值：

下面为系统转换矩阵和噪声转换矩阵计算：

的推导过程;FAST-LIO 雅可比推导方法一_fastlio 公式推导-CSDN博客

将白噪声w的协方差表示为Q，则可以根据下面方程迭代计算传播的协方差矩阵：

前向传播总结：
1-前向传播到底干了什么？
①通过IMU积分计算一个粗略的状态量x^k，也就是ESKF的先验，也就是公式(4)。x^k用于反向传播点云去畸变
②计算传播误差量，并计算对应的协方差矩阵，也就是公式(5)。公式(5)就是ESFK的运动方程，运动方程在KF中的作用。

2-前向传播的输入输出？
①输入：IMU数据，也就是三轴加速度、三轴角速度
②输出：粗略的状态量x^k；误差量 𝑥~ k与协方差矩阵P^i；
3-公式(5)如何理解，为什么要求这样一个公式？它里面的xi是真值吗？从哪里获得的？
公式(5)是ESKF的运动方程。
传统的KF中有两个重要的方程，运动方程与观测方程，状态转移、卡尔曼增益、测量修正的计算都离不开这两个方程。对于ESKF来说，同样如此，但是ESKF相比传统的KF，它是以误差量作为待估计量，而不是状态量。
传统的KF，运动方程与观测方程通常长这样：

而ESKF的长这样：

也就是说ESKF估计的是误差量 x~，而不是直接估计状态量x。而有了误差量的估计，再直接加上状态量的估计就是我们用滤波求得的最优估计 𝑥¯ ，利用下面公式：

2.3.2 反向传播与运动补偿（back propagation, BP）

        为何要进行后向过程，其实涉及到一个时间软同步的问题，点云是在各自不同的实践内采集的，坐标系不相同，所以需要IMU预测的位姿向后传播去除运动畸变。

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

转换公式如下：

      之前IMU的向前传播得到了tk-1到tk的运动状态，以及tk时刻的位姿。反向传播就是用tk时刻的位姿反推pj时刻的位姿j=i—tk，可以将对应的云点坐标系全部转换到tk时刻坐标系。

2.3.3 残差构建

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

        计算世界坐标系下激光点坐标和地图中实际对应的边和面之间的距离，并且投影到对应的边的方向上来：

例如，当前帧角点与地图匹配点：

①更新位姿，将当前帧角点坐标系转换到map坐标系；

②遍历当前点角点，从地图中找到距离当前点最近的5个点；

    ——拟合5个点直线；

    ——计算当前帧角点到直线的距离、垂涎的单位向量，存储为角点的参数。

2.3.4 迭代更新

         为了融合残差、状态估计值、预测协方差，需要将残差与全局真是状态、测量噪声关联并将观测模型线性化。测量噪声来自于雷达测量，文中公式（13）将测量点减去噪声得到争取点位：

         过程：点云降噪->点云运动补偿(也可以叫做去畸变)->点云转换到全局坐标系G->利用地图组成的KD-tree搜索最近点->计算残差；该残差应该为0，接下来我们就需要迭代优化。

上述公式为非线性，我们需要将其线性化，通过在x^kk处进行一阶泰勒展开来近似上述方程，得到：

式中（误差=真实-估计）：

的先验分布是来自前向传播，由下面式子计算得到：

H为下面式子的雅可比矩阵，x~kk为0处的雅克比矩阵(在x=0处一阶泰勒展开)，：

H计算可以用链式法则:

vj∈N(0, Rj)源自于原始测量噪声

         上述，h(x, n)表示ESKF的观测方程，公式(14)是其线性化后的结果。至此，我们已经得到了ESKF的运动方程与观测方程。接下来，我们就需要求真实状态与卡尔曼增益。对于传统的KF是这样，但是论文中用到了IEKF(迭代扩展卡尔曼滤波)，其思想就是在xk->xk+1的过程中多次迭代，以达到削减非线性带来的影响。IEKF可以证明与高斯牛顿法是等价的，因此IEKF可以保证全局收敛。接下来的任务就是求x~kk，然后求卡尔曼增益。

J是公式在为0时的偏导：

结合公式(15)计算的先验与公式(14)计算的后验，可以得到极大后验估计的计算公式：

式中

 公式(17)解析(公式理解与算法理解一样，首先要弄清楚它的功能与输入输出)
功能：迭代 𝑥~ kk使得，公式(17)最小，最终求出一个误差值 𝑥~ 。当残差小于某个阈值的时候，我们就可以认为它收敛了
输入：①前向传播估计状态xk与前向传播预测状态x^k，前向传播计算出的协方差矩阵P^i②点到线、点到面距离残差和，与测量噪声Rj
输出：令残差最小的 𝑥~ ,协方差矩阵P

       将公式(15)中先验的线性化代入公式(17)，并优化最终的二次成本，可以得到标准的迭代卡尔曼滤波器论文[21]，可以用于如下计算：

        公式(18)中，第一个为卡尔曼增益的计算，第二个为迭代公式，类似高斯牛顿，具体的推导过程见：https://zhuanlan.zhihu.com/p/587500859

      更新过后的估计值x^k+1k，接下来将会用于模块5.3中的残差计算，并一直持续重复该过程，直到收敛（如下阈值）：

收敛后，可得到最优状态估计与协方差：

        公式(18)中计算卡尔曼增益的常规方式，这种方式有缺陷，需要对矩阵HPH ⊤ +R求逆，但是该矩阵是测量维度矩阵，实践中，点云特征点的数量很多，该矩阵维度也就非常大(为mxm，Hmx18,P18x18,Rmxm)，特征点m的数量16线激光雷达中都是上千，如果高维度的矩阵求逆是十分耗时的。因此，现有的工作（如[21, 26]）只使用了一小部分测量的数据点。而此文章中，我们将提出一个针对上述问题的解决方案。灵感来源于公式(17)，即成本函数在状态之上。由此可知，解决方案的计算复杂度应取决于状态维度。实际上，如果直接求解公式(17)，我们可以得到与(18)相同的解，以及一个新的卡尔曼增益形式如下：

2.3.5 算法总览

2.4 地图更新

       得到更新后的状态值后，将点云线转换到imu坐标系再转换到地图坐标系。

       这些特征点最终被添加到包含前面所有步骤中的特征点的现有地图中：

2.5 初始化

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

三、实验结果与结论

        本文提出了 FAST-LIO，这是一种计算效率高且稳健的 LiDAR-惯性里程计框架，采用紧耦合迭代卡尔曼滤波器。我们使用前向和后向传播来预测 LiDAR 扫描中的状态并补偿运动。此外，我们证明并实施了一个等效公式，该公式可以大大降低卡尔曼增益计算的复杂度。FAST-LIO 在无人机飞行实验中进行了测试，在具有大旋转速度的具有挑战性的室内环境和室外环境中进行了测试。在所有测试中，我们的方法都产生了精确、实时且可靠的导航结果。