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

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

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)，为了避免奇异性和保持结构特性，使用流形进行运算。

• 广义加与广义减

广义加

该公式表示一个从流形