从零入门激光SLAM（二十一）——看不懂FAST-LIO？进来

大家好呀，我是一个SLAM方向的在读博士，深知SLAM学习过程一路走来的坎坷，也十分感谢各位大佬的优质文章和源码。随着知识的越来越多，越来越细，我准备整理一个自己的激光SLAM学习笔记专栏，从0带大家快速上手激光SLAM，也方便想入门SLAM的同学和小白学习参考，相信看完会有一定的收获。如有不对的地方欢迎指出，欢迎各位大佬交流讨论，一起进步。博主创建了一个科研互助群Q：772356582，欢迎大家加入讨论。

FAST-LIO: A Fast, Robust LiDAR-inertial Odometry Package by Tightly-Coupled Iterated Kalman Filter

论文地址：https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=9372856

代码：https://github.com/hku-mars/FAST_LIO

一、文章概述

1.问题导向

固态激光雷达最近成为激光雷达发展的主要趋势：成本低、重量轻、高性能。尽管潜力巨大，但固态激光雷达给SLAM带来了新的挑战。

1）固态激光雷达视场角小，更容易退化

2）高分辨率的点云融合到IMU中需要巨大的计算资源

3）无人机螺旋桨和发动机的持续旋转也给IMU测量带来了显著的噪声，同时引起点云的运动畸变

2.目标

使LiDAR导航适用于小型移动机器人，如无人机。构建一个计算高效且坚固耐用的激光雷达-惯性里程计算法。

3.摘要

本文提出了一种计算高效和健壮的激光雷达-惯性里程计框架。我们使用紧密耦合的迭代扩展卡尔曼滤波将LiDAR特征点与IMU数据融合，以便在发生退化的快速运动、噪声或杂乱环境中进行稳健导航。为了在大量测量的情况下降低计算量，我们提出了一种新的卡尔曼增益计算公式。新公式的计算量依赖于状态维而不是测量维。在不同的室内外环境下对该方法及其实现进行了测试。在所有测试中，我们的方法实时产生可靠的导航结果：运行在四旋翼车载计算机上，它在一次扫描中融合了1200多个有效特征点，并在25ms内完成了iEKF步骤的所有迭代。我们的代码在Github上是开源的。

4.贡献

1) 为了适应快速运动、噪声或杂乱的环境，我们采用了一种紧耦合的迭代卡尔曼滤波来融合LiDAR特征点和IMU测量，并提出了一种形式化的反向传播过程来补偿运动失真。

2) 为了降低LiDAR特征点的计算量，提出了一种新的卡尔曼增益计算公式，新公式的计算复杂度依赖于状态维度而不是测量维度。

3) 该系统能够在小型四旋翼机载计算机上运行。

4) 我们在不同的室内和室外环境中进行了实验，并用实际的无人机飞行测试来验证系统在快速运动或强烈振动噪声存在时的鲁棒性。

二、方法解析

2.1 系统pipeline

激光雷达的输入被送入特征提取模块，以获得平面和边缘特征。然后将提取的特征和惯性测量反馈到我们的状态估计模块中，以进行10 Hz−50 Hz的状态估计。然后，估计的姿势将特征点注册到全局框架，并将它们与到目前为止构建的特征点地图合并。在下一步中，最终使用更新后的地图来配准进一步的新点。

2.2 系统符号定义

2.2.1符号说明

2.2.2流形

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

广义加与广义减

广义加

该公式表示一个从流形 𝑀和欧几里得空间 𝑅𝑛到流形 𝑀的映射：把一个流形上的点和欧几里得空间中的一个向量结合起来，并将结果映射回流形上

描述从初始旋转𝑅出发，经过一个旋转增量 𝑟后的新的旋转矩阵

广义减

从两个流形 𝑀上的点到欧几里得空间 𝑅𝑛的映射：

描述了两个旋转矩阵之间的相对旋转，以旋转向量的形式表示。

对于欧氏空间广义加和广义减就是常用的加减法。

则有

2.2.3 IMU的运动学模型

l 连续模型

P为位置、R为姿态、v为速度

即为：

l 离散模型

系统状态分解为两个组成部分：流型中的状态量加上正切空间中的误差量，表达如下

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

其实就是

2.2.4 雷达数据预处理

雷达原始数据频率非常高，200KHz，在这么高的频率下，不可能做到实时接收，我理解在激光雷达的内部信号处理中，一般做法是累积一段时间，再一块接收，也就是一帧数据，实现降频，在FAST-LIO中，一般为50Hz，按照这个频率计算的话，一帧数据包括4K的原始数据。在论文中，累积处理的时间被记为𝑡𝑘，对于原始数据，我们可以通过高平滑性得到平面特征，低平滑性得到边特征。假设在20ms时间内雷达发生了转弯和线性运动，所以这一帧中的雷达点采集的时间戳是不一样的，每个时间戳对应的位姿也是不一样的，就需要借助IMU来去除运动畸变。

2.3 预测步

定义误差状态向量

l 前向传播

向前传播是计算IMU的预测值和预测协方差矩阵，主要公式如下。

1. 预测值计算，用离散模型

2. 计算真实值和估计值之间的差值，使用了近似线性化的方法

3. 预测协方差矩阵计算公式

P预测阶段的误差协方差

Q过程噪声协方差矩阵

F 是状态转移矩阵

l 向后传播和运动补偿

点云是在各自不同的实践内采集的，坐标系不相同，所以需要IMU预测的位姿向后传播去除运动畸变。之前IMU的向前传播得到了tk-1到tk的运动状态，以及tk时刻的位姿。反向传播就是用tk时刻的位姿反推pj时刻的位姿j=i—tk，可以将对应的云点坐标系全部转换到tk时刻坐标系。

转换公式如下

反向传播只针对位移、速度和旋转进行反向传播，不考虑噪声

通过后向传播可以得到p;到scan结束时间如之间的相对运动，即旋转和平移变换。这一相对变换将每个点的坐标系都转换到scan结束时间t下的坐标系。

2.4 计算残差

假设迭代卡尔曼滤波器的当前迭代次数为k，对应的位姿估计是

将点云转换到世界坐标系，公式如下

假设其附近特征点在地图中定义的最近的平面或边缘是该点真正所属的位置。激光部分的残差被定义为坐标变换后的激光点到全局地图中对应边缘或平面的距离：

地图上最近的平面或边缘点则由kd-Tree中搜索得到，并且将残差高于阈值的点会被定义为离群值或新观测到的点。

2.5 迭代更新

2.5.1 点云残差

为了融合激光点云匹配残差z和上述IMU前向传播得到的当前状态估计x和误差协方差矩阵P。需要将点云残差与真实状态和LIDAR的测量噪声相关联。

测量噪声来源于激光雷达测距噪声和波束定向噪声

去除点云噪声的公式如下

我们建立的观测方程如下

观测模型是hj，输入是系统状态xk和观测噪声n

泰勒展开线性化如下

其中z为匹配残差，H为残差的雅可比矩阵，v表示LIDAR点的测量噪声

2.5.2 IMU的残差

2.5.3 迭代更新

总残差方程包含IMU残差和点云残差，表达如下。

卡尔曼滤波中更新过程中的卡尔曼增益和第k+1次迭代的估计值，如果第k+1次迭代时对应的残差小于一定阈值，则本次迭代结果为最优状态估计和协方差。卡尔曼增益和更新方程如下

如果观测噪声很大（𝑅很大），卡尔曼增益𝐾较小，说明观测对状态更新的影响较小，主要依赖于预测状态。

如果预测误差协方差𝑃较大，卡尔曼增益𝐾较大，说明观测对状态更新的影响较大，主要依赖于新观测。

但由于激光雷达中的点数量庞大，求解卡尔曼增益时会对一个维度很大的矩阵求逆，在本文中将对激光数据的求解转移到对状态的求解，使维度极大得降低，得到新的卡尔曼增益形式。新公式大大节省了计算量，因为状态维数通常比LIO中的测量值要低得多(例如，在10hz扫描速率下，扫描中有1000多个有效特征点，而状态维数只有18)

2.6 算法流程

2.7 地图更新

通过将求解得到的状态转化为旋转矩阵和平移向量，然后将雷达坐标系下的点转换到全局坐标系下，最后添加到全局地图中。

2.8 初始化

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

三、总结

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