FAST-LIO2详细解析

太极幻宇

于 2024-11-16 23:24:16 发布

阅读量2.5k

点赞数 15

文章标签：人工智能数码相机机器人自动驾驶算法

本文链接：https://blog.csdn.net/weixin_41150257/article/details/143825450

版权

FAST-LIO2（Fast Direct LiDAR-Inertial Odometry 2）是一种快速、稳健且通用的激光雷达惯性里程计框架。以下是对FAST-LIO2的详细解析：

一、背景与动机

在SLAM（即时定位与地图构建）技术中，激光雷达是最常使用的传感器之一，它能够提取环境中微小的细节特征。然而，激光雷达在缺少结构特征的环境中（如四壁光滑的通道）或快速运动、复杂场景下，仅使用激光雷达测程容易出现问题。因此，激光雷达通常会与惯性测量单元（IMU）耦合，以提高系统的稳健性。此外，随着激光雷达成本的降低和性能的提升，其在机器人应用程序中的应用范围也在不断扩大。但现有的LIDAR-SLAM仍面临诸多挑战，如点云数据量大、特征提取模块易受环境影响、地图更新与维护复杂等。针对这些问题，FAST-LIO2应运而生。

二、系统架构与关键技术

FAST-LIO2建立在高效的紧耦合迭代卡尔曼滤波器的基础上，具有两个关键的新颖性：

直接点配准：FAST-LIO2不提取特征，而是直接将原始点配准到地图上（并随后更新地图），这利用了环境中的细微特征，提高了准确性。同时，取消了手工设计的特征提取模块，使其自然地适应不同扫描模式的新兴激光雷达。
增量k-d树（ikd-tree）：FAST-LIO2通过增量k-d树数据结构来维护地图，该结构支持增量更新（即插入和删除点）和动态重新平衡。与现有的动态数据结构（如八叉树、R*-tree和nanoflann k-d树）相比，ikd-tree在获得更好的整体性能的同时，自然地支持对树的下采样。

三、工作流程

FAST-LIO2的工作流程主要包括以下几个步骤：

点云累积：连续采样的LiDAR原始点在一定的时间段内（如10ms至100ms之间，一般将20ms作为一个周期）累积，形成Scan。
状态估计：新Scan中的点云与地图点进行匹配，以实现状态估计。该局部地图通过一个紧耦合的迭代卡尔曼滤波器实现。
地图更新：优化后的姿态将新扫描中的点注册到全局帧中，并以里程计的频率插入到ikd-tree中，实现将它们合并到地图中，即建图。

四、性能与优势

FAST-LIO2在计算效率、稳健性和通用性方面具有显著优势：

计算效率高：在大型室外环境中，FAST-LIO2能够实现高达100Hz的里程计和建图频率。
稳健性强：在杂乱的室内环境中，FAST-LIO2能够提供可靠的姿态估计，即使旋转速度高达1000度/秒。
通用性广：FAST-LIO2可同时适用于多线旋转和固态LiDAR、无人机（UAV）和手持平台以及基于Intel和ARM的处理器。

同时，FAST-LIO2仍获得了比现有方法更高的精度。

五、开源与社区贡献

FAST-LIO2及其数据结构ikd-tree均在Github上开源，为社区提供了宝贵的资源和参考。这有助于推动SLAM技术的进一步发展和应用。

FAST-LIO2是一种高效、稳健且通用的激光雷达惯性里程计框架，具有广泛的应用前景和重要的研究价值。

FAST-LIO2（Fast Direct LiDAR-Inertial Odometry 2）是香港大学火星实验室（MARS）开源的一种具有高计算效率、高鲁棒性的雷达惯性里程计（LIO）算法。以下是对FAST-LIO2代码的详细解析：

六、代码结构

FAST-LIO2的代码结构清晰，主要包含两个文件夹：fast-lio和包含scan-context的回环检测与位姿图优化文件夹。fast-lio文件夹主要是FAST-LIO2算法的核心实现，包括前向处理和ikd-tree的实现。

七、核心组件

CMakeLists.txt：这是FAST-LIO2的构建脚本，用于定义编译选项和依赖关系。它定义了ROOT_DIR，寻找机器的CPU核数来选择是否采用多核计算，并设置了OpenMP、PythonLibs、MATPLOTLIB_CPP_INCLUDE_DIRS等依赖项。
Pose6D.msg：定义了雷达在IMU坐标系下的预积分值，包括IMU和第一帧雷达点的时延、预积分的总加速度、无偏角速度、预积分速度、预积分位置和预积分旋转等。
主程序：主程序入口在src/laserMapping.cpp中，其他两个cpp文件（include/ikd-Tree/ikd_Tree.cpp和src/preprocess.cpp）以库的形式给主程序使用。主程序的流程包括ROS节点初始化、参数读取、参数初始化、指针初始化、读取雷达和IMU外参矩阵、IMU积分参数设置、设置卡尔曼滤波器的参数、日志记录初始化等。

八、关键函数与数据结构

预处理（Preprocess）：
- 预处理的主要作用是对激光雷达点云数据进行处理，以获得特征点云。这通常包括降噪、滤波、特征提取等步骤。
- 在FAST-LIO2中，预处理函数位于src/preprocess.cpp中，通过调用不同的预处理类来处理不同类型的激光雷达数据。
IMU处理（IMU_Processing）：
- IMU处理函数负责读取IMU数据，并进行时间对齐和积分处理，以获得IMU的位姿估计。
- 在FAST-LIO2中，IMU处理函数与预处理函数类似，也是通过回调函数来读取IMU数据，并进行相应的处理。
ESF（Edge Surface Feature）与IKD-Tree：
- ESF是一种用于提取激光雷达点云特征的方法，它可以根据点云的几何属性（如边缘和平面）来提取特征点。
- IKD-Tree是一种动态二叉搜索树，用于高效地处理激光雷达点云数据。它支持增量更新和地图降采样，保证树的平衡性。在FAST-LIO2中，IKD-Tree用于维护全局地图，并实现点云的快速检索和匹配。
卡尔曼滤波器：
- 卡尔曼滤波器是FAST-LIO2中的核心算法之一，它用于融合激光雷达和IMU的数据，以实现高精度的位姿估计。
- 在FAST-LIO2中，卡尔曼滤波器采用了紧耦合的方式，将激光雷达和IMU的数据进行联合优化，以提高位姿估计的精度和鲁棒性。

九、代码执行流程

初始化：包括ROS节点初始化、参数读取、指针初始化等。
订阅传感器数据：通过回调函数订阅激光雷达和IMU的传感器数据。
预处理与IMU处理：对订阅到的激光雷达和IMU数据进行预处理和IMU处理。
特征提取与匹配：使用ESF方法提取激光雷达点云的特征点，并使用IKD-Tree进行快速检索和匹配。
卡尔曼滤波：将激光雷达和IMU的数据进行融合，通过卡尔曼滤波器进行位姿估计。
地图更新：将优化后的位姿估计结果用于更新全局地图。

十、总结

FAST-LIO2是一种高效、稳健的雷达惯性里程计算法，它通过紧耦合误差状态卡尔曼滤波器实现IMU和激光雷达融合的状态估计。FAST-LIO2的代码结构清晰，包含预处理、IMU处理、特征提取与匹配、卡尔曼滤波和地图更新等关键组件。通过对代码的详细解析，可以深入理解FAST-LIO2的工作原理和实现细节，为进一步优化和扩展提供基础。