fast_lio 的详细解析

最新推荐文章于 2025-03-22 23:02:11 发布

太极幻宇

最新推荐文章于 2025-03-22 23:02:11 发布

阅读量2.9k

点赞数 8

文章标签：人工智能算法机器学习自动驾驶机器人

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

版权

FAST-LIO（Fast LiDAR-Inertial Odometry）是一种高效的激光雷达-惯性里程计算法，由香港大学（HKU）开发。以下是对FAST-LIO的详细解析：

FAST-LIO结合了激光雷达（LiDAR）和惯性测量单元（IMU）的数据，通过紧耦合的迭代扩展卡尔曼滤波器（EKF）实现高精度的位姿估计。该算法在快速运动、噪声或杂乱环境中表现出色，具有计算效率高和鲁棒性强的特点。

紧耦合迭代扩展卡尔曼滤波器：
- FAST-LIO使用紧耦合的迭代扩展卡尔曼滤波器来融合LiDAR特征点和IMU数据。这种滤波器能够处理非线性系统，并通过迭代优化来逼近真实状态。
- 在滤波器中，LiDAR特征点和IMU数据被共同用于构建残差函数，并通过最小化残差来更新状态估计。
特征提取与匹配：
- FAST-LIO从LiDAR点云中提取平面特征和边缘特征，这些特征在后续的状态估计中被用作观测值。
- 特征提取通常基于局部平滑度等几何属性进行，边缘特征提取时还会考虑强度信息。
- 提取的特征点与IMU积分结果一起用于点云匹配，以估计相对位姿。
IMU积分与运动补偿：
- IMU数据被用于积分以获得相对位姿的初步估计，这有助于在LiDAR数据稀疏或噪声较大的情况下保持算法的稳定性。
- 同时，IMU积分结果还被用于对LiDAR点云进行运动补偿，以消除由于传感器运动引起的畸变。
卡尔曼增益计算优化：
- FAST-LIO提出了一个新的计算卡尔曼增益的公式，该公式的计算量依赖于状态维数而不是测量维数，从而降低了计算负荷。

数据输入：
- 接收LiDAR点云数据和IMU数据作为输入。
预处理：
- 对LiDAR点云数据进行预处理，包括降噪、滤波等步骤，以提取特征点。
- 对IMU数据进行积分处理，以获得相对位姿的初步估计。
特征提取与匹配：
- 从预处理后的LiDAR点云中提取平面特征和边缘特征。
- 将提取的特征点与IMU积分结果进行匹配，以估计相对位姿。
状态估计与更新：
- 使用紧耦合的迭代扩展卡尔曼滤波器对状态进行估计和更新。
- 在滤波器中，结合LiDAR特征点和IMU数据构建残差函数，并通过最小化残差来更新状态估计。
地图构建与更新：
- 将优化后的位姿估计结果用于构建和更新全局地图。
- 地图的构建有助于后续的位姿估计和导航任务。

应用场景：
- FAST-LIO广泛应用于自动驾驶、无人机导航、机器人定位等领域。
- 在这些场景中，FAST-LIO能够提供精确且实时的位姿估计结果，支持自主移动和导航任务。
优势：
- 计算效率高：FAST-LIO通过优化卡尔曼增益计算公式和特征提取方法，降低了计算负荷，提高了算法的运行效率。
- 鲁棒性强：该算法在快速运动、噪声或杂乱环境中表现出色，能够保持高精度的位姿估计结果。
- 易于扩展：FAST-LIO的代码结构清晰，易于与其他算法或传感器数据进行集成和扩展。

FAST-LIO是一种高效、稳健的激光雷达-惯性里程计算法，具有广泛的应用前景和显著的优势。

FAST-LIO的代码结构清晰，主要流程集中在laserMapping.cpp的main函数中。以下是代码的主要组成部分：

变量初始化：在main函数的开始部分，会进行各种变量的初始化。
数据处理循环：主要处理流程在while (status){}循环中。该循环会不断处理IMU和LiDAR接收到的数据。
数据同步与处理：通过bool sync_packages(MeasureGroup &meas)函数处理IMU和LiDAR接收buffer中的数据，将一帧激光点云以及这一帧点云扫描期间的IMU数据一起打包放到MeasureGroup里。
点云畸变补偿与状态预测：调用p_imu->Process(Measures, kf, feats_undistort)函数，其中UndistortPcl函数用于补偿点云畸变，并用IMU进行状态预测。
局部地图更新与点云降采样：调用lasermap_fov_segment()函数更新localmap边界，然后降采样当前帧点云。
状态更新：调用kf.update_iterated_dyn_share_modified(LASER_POINT_COV, solve_H_time)函数进行状态更新，这是算法的核心部分。
里程计发布与地图更新：发布里程计信息，并调用map_incremental()函数更新kdtree。

状态更新函数：void update_iterated_dyn_share_modified(double R, double &solve_time)是状态更新的核心函数。它负责根据当前更新的状态，将当前帧特征点与地图特征点匹配，并更新状态估计。
- 在每一次迭代中，该函数会根据当前更新的状态，计算观测值（即残差z）以及观测值相对于当前状态x的雅克比矩阵。
- 接着，根据公式计算卡尔曼增益K，并更新状态。
- 如果已收敛，则将最后一次迭代的状态作为后验估计，并更新协方差。
点云匹配与残差计算：void h_share_model(state_ikfom &s, esekfom::dyn_share_datastruct<double> &ekfom_data)函数用于计算点云匹配的残差（即观测h）以及相对于x的雅克比矩阵（即论文中的H）。
- 该函数会遍历所有的特征点，将特征点从体坐标系转换到世界坐标系。
- 然后，在地图中搜索特征点的最近邻，并判断是否是有效匹配点。
- 如果是有效匹配点，则计算点到平面的距离作为残差，并计算相应的雅克比矩阵。
数据发布与保存：
- 发布的Topic话题包括配准后的单帧点云（/cloud_registered）和未经配准的单帧点云（/cloud_registered_body）。
- 配准后的点云以IMU为基准坐标系，当前帧往第一帧配准。
- 还可以选择是否保存PCD文件，以及设置保存间隔等参数。