FAST-LIO笔记

1.FAST-LIO

FAST-LIO 是一个计算效率高、鲁棒性强的激光-惯性里程计系统。该系统通过紧耦合的迭代扩展卡尔曼滤波器（IEKF）将激光雷达特征点与IMU数据进行融合，使其在快速运动、噪声较大或环境复杂、存在退化的情况下仍能实现稳定的导航。

1.1问题

1）LiDAR测量中提取的特征点通常是环境中的几何结构（如边缘和平面）。当无人机在特征不明显的复杂环境中运行时，LiDAR方案容易出现退化问题。该问题在视场角（FoV）较小的LiDAR中尤为明显；（特征稀缺的环境中或使用小FoV的LiDAR时，性能容易退化）

2）由于在扫描方向上具有高分辨率，一帧LiDAR扫描通常包含大量特征点（例如几千个）。尽管这些特征点在退化场景中无法稳定估计位姿，但若将其与IMU测量进行紧耦合融合，将导致巨大的计算负担，而这对于无人机的机载计算机来说是不可承受的；（嵌入式平台对算力有要求）

3）由于LiDAR使用少数激光发射/接收对逐点扫描，因此每个激光点的采样时间不同，这种时间差造成的运动畸变会显著降低点云配准的准确性。此外，无人机螺旋桨和电机的持续旋转还会对IMU测量引入显著噪声。（点云去畸变以及外部因素影响IMU测量）

1.2解决方法

为使LiDAR导航适用于小型移动机器人（如无人机），提出了FAST-LIO——一个计算高效且鲁棒的LiDAR-IMU里程计算法。主要贡献如下：

1）为应对高速运动、有噪声或复杂环境中发生退化的情况，我们采用紧耦合的迭代卡尔曼滤波器，将LiDAR特征点与IMU测量数据融合。我们提出了正式的后向传播方法来补偿运动畸变；

2）为减少大量LiDAR特征点带来的计算负担，我们提出了一种新的卡尔曼增益计算公式，并证明其与传统公式等价。新公式的计算复杂度取决于状态维度，而不是测量维度；

3）我们将这些公式实现为一个快速且鲁棒的LiDAR-IMU里程计算软件包，系统可以运行在小型四旋翼的机载计算机上；

4）我们在多种室内和室外环境下，以及实际无人机飞行测试中，验证了系统在快速运动或强烈振动噪声条件下的鲁棒性。

1.3系统框架

LiDAR原始数据首先输入到特征提取模块，提取出平面特征和边缘特征。
提取出的特征与IMU测量数据一同输入到状态估计模块中，以10Hz或50Hz的频率进行状态估计。IMU数据前向传播（粗略地位姿估计），后向传播（运动补偿去畸变）
估计得到的位姿用于将当前帧的特征点配准到全局坐标系中，并与当前已构建的特征点地图进行融合。更新后的地图将用于下一步中新点云的配准操作。

1.4系统描述

关于流行的解释

流形(Manifold )是局部具有欧几里得空间性质的空间，是欧几里得空间中的曲线、曲面等概念的推广。参考文章中关于二维圆和三维球的例子解释的很好，二维空间中的圆就是一个一维流形，三维空间中的球面是一个二维流形。

参考：https://www.cnblogs.com/icmzn/p/11082509.html

IMU运动模型

本质上是对P、V、Q的求导

离散模型

需要估计的状态量是个18维的量，包括位置、速度、姿态、偏置和加速度g .

数据预处理

由于原始LiDAR点是以非常高的频率（例如200kHz）采样的，因此通常无法在每接收到一个新点时立即处理它们。更实际的方法是将这些点积累一段时间，然后一次性处理它们。

在FAST-LIO中，最小的积累间隔设置为20毫秒，从而实现高达50Hz的全状态估计（即里程计输出）和地图更新。这种积累后的点集被称为一个扫描（scan），并且处理该扫描的时间为 tk。从原始点云中，我们提取具有较高局部平滑性的平面点和具有较低局部平滑性的边缘点。

1.5状态估计

使用迭代扩展卡尔曼滤波估计状态量X

前向传播 Forward Propagation

在 FAST-LIO 系统中，一旦接收到 IMU 输入，就会执行一次前向传播，用于预测系统当前的状态（积分得到粗略地位姿估计）。

后向传播与运动补偿 Backward Propagation and Motion Compensation

激光雷达一帧数据由很多点组成，这些点显然不是同一时间测量得到的，所以需要补偿时间差带来的运动误差。把一帧不同采样时间的点转换到最后的帧末时间（反向传播去运动畸变）

残差计算和迭代状态更新

计算lidar里程计的残差，和LOAM、LIO-SAM一样，计算点面之间的距离作为残差。

利用迭代卡尔曼滤波估计位姿。

算法流程

1.6地图更新 MapUpdate

根据求解的状态量 X 把激光雷达点投影到世界坐标系的过程.

1.7初始化

为了获得系统状态（例如重力向量 Gg、偏差和噪声协方差）的良好初始估计，从而加速状态估计，需要进行初始化。在 FAST-LIO 中，初始化非常简单：保持 LiDAR 静止几秒钟（本文中的所有实验为 2 秒），然后将收集的数据用于初始化 IMU 偏差和重力向量。如果 LiDAR 支持非重复扫描（例如，Livox AVIA），保持静止还允许 LiDAR 捕捉一个初始的高分辨率地图，这对后续的导航非常有利。

2.代码相关

注：FAST-LIO 的代码是6e1fa94之前的commit的，之后的都是FAST-LIO2，注意区分。

代码有两个节点loam_feat_extract、loam_laserMapping。

2.1loam_feat_extract

1.支持四种激光雷达的点云：MID40、HORIZON、VELO16、OUST64，分别对应不同的回调函数

MID40：msg转为PCL点云，然后进行特征提取；
HORIZON（点云是livox自定义的类型）把每条线束scan的点看作一个点云，单帧点云把所有线束的点云放到容器vector中；会剔除一些重复性扫描到的点；

2.特征提取 give_feature

判断每个点类型是什么，如果是Edge_Jump或者Edge_Plane，加入到角点点云中；如果是Poss_Plane或者Real_Plane加入到面点点云中。最后将两个点云和单帧点云以话题形式发布。

2.2loam_laserMapping

1.订阅面点点云和IMU话题，把数据放到队列中，主函数中根据当前lidar的时间戳找到小于它时间戳的IMU数据 sync_packages()；

2.对一组面点点云和IMU数据进行处理，计算状态量和去除畸变后的面点点云 Process()

第一次执行会进行初始化，利用开始的IMU帧的平均值初始化状态量x（初始化重力、陀螺仪偏差、加速度计和陀螺仪协方差）。只执行一次，执行完就退出了；
后续帧会点云去畸变处理 UndistortPcl()

这个函数首先会把上一帧尾部的IMU数据添加到当前帧头部，遍历这些IMU数据，通过离散中值积分计算每两个IMU数据之间的状态量，更新协方差矩阵（对应论文公式7、8）

注：关于误差卡尔曼ESKF的推导建议参考高博的简明推导

然后将点云的每个点映射到采样结束时刻（后向传播，去畸变）。具体代码结合下面这张图更容易理解，参考[2]。

假设某帧lidar采样开始时间是tb，采样结束时间是te，后向传播就是把单帧所有点的坐标映射到采样结束时刻te。

1.采样结束那一刻不一定有对应的IMU数据，因此先计算最后一个IMU数据到 te时刻的时间差 dli，以 dli 再做一次中值积分就得到了 te时刻系统的状态；

2.按时间从后往前遍历点云，假设当前点p对应的时刻为 tn，其所属的IMU区间时刻为[ tib , tie ]（代码这部分设计的很巧妙），可以求得 tn 与 tib 的时间差 dt ，通过 dt 和 tib 时刻的位姿，便可求得 tn 时刻IMU的位姿。

3.上一步求得了 tn 时刻IMU的位姿，结合外参可以求出点p的世界坐标。

4.结合第一步te时刻IMU的位姿与第三步该点的世界坐标，可将该点世界坐标通过te时刻IMU位姿和外参反算回lidar系，就得到了该点在te时刻的lidar坐标系坐标。

总结：从 te 时刻至 tb 时刻，对每一个点完成上述操作，系统便完成了反向传播，即将一帧中所有点云都补偿到了这一帧结束的时刻。总之，通过向前与反向传播，得到了在点云最终时刻系统名义状态的先验值 x（计算了PVR，没有算bias），误差状态的先验协方差 P及运动补偿后的点云。

3.局部地图动态管理，目的：为了始终维持lidar所在的世界坐标的位置在局部地图的中心，这样的匹配效果会更好 lasermap_fov_segment()

4.判断历史地图是否为空，若为空，用当前帧的点云降采样填充 featsFromMap；否则，对已有的 featsFromMap 再次降采样，如果降采样后点的个数大于5，会进入一个很大的if条件里。