樱花客栈

FAST-LIO（docker）

作者：

大嘴巴小牙齿

在

一、FAST-LIO是什么？

FAST-LIO（Fast LiDAR-Inertial Odometry）是香港大学MARS实验室提出的一套激光-惯性紧耦合里程计算法。它的核心目标只有一个：用激光雷达和IMU的高频数据，实时估计机器人在三维空间中的位姿，并拼接出全局一致的点云地图。

相比传统的滤波方案（如LOAM系列），FAST-LIO的计算效率极高。在普通CPU上就能跑到100Hz以上，同时精度不输甚至超越LOAM。这得益于它采用了基于误差状态卡尔曼滤波（Error-State Kalman Filter, ESKF）的紧耦合框架，以及增量式地图更新机制（ikd-Tree）。

二、算法原理详解

2.1整体架构概览

FAST-LIO的数据流可以概括为：

LiDAR点云 ──┐

├── 点云预处理 ── 特征提取 ── ESKF更新 ── 位姿估计 ── 地图更新IMU数据 ─────┘

整个系统运行在一个ROS节点中（fastlio_mapping），订阅LiDAR和IMU话题，发布里程计和地图。核心流程如下：

  • IMU预积分：在两帧激光之间，用IMU的高频数据（200-1000Hz）推算相对运动
  • 点云预处理：运动补偿（去畸变）、特征提取（平面特征 / 角点特征）
  • ESKF更新：将激光里程计观测和IMU预测融合，估计误差状态
  • 地图更新：将当前帧点云投影到全局地图中，增量式维护ikd-Tree

2.2 IMU预积分与运动补偿

激光雷达的扫描频率通常只有10-20Hz（Livox AVIA是10Hz，Velodyne 32线也是10Hz），而IMU的频率可以达到200-1000Hz。在两次激光扫描之间，IMU能提供大量的高频姿态和速度信息。

FAST-LIO使用IMU预积分（Preintegration）技术：给定上一帧激光时刻t_k的IMU状态，对t_k到t_{k+1} 之间的所有IMU测量值进行积分，得到两帧之间的相对旋转、速度和位移。这个预积分结果作为ESKF的预测步骤（Prediction）。

同时，由于激光雷达扫描一帧需要一定时间（例如100ms），而机器人在这期间可能已经运动了相当距离，所以点云会产生运动畸变。FAST-LIO利用IMU数据对每一帧点云做运动补偿（Deskew），把每个点都投影到统一的坐标系下。这是高精度建图的关键步骤。

2.3误差状态卡尔曼滤波（ESKF）

FAST-LIO的核心状态估计器是误差状态卡尔曼滤波。这里需要区分两个概念：

  • 名义状态x：由IMU数据直接积分得到，不考虑噪声，包含了大的非线性运动
  • 误差状态delta_x：代表名义状态的微小偏差，是线性的，用卡尔曼滤波估计

名义状态和误差状态组合成完整的状态向量：

x = [p, v, R, b_a, b_g, g]
    p: 位置 (3D)
    v: 速度 (3D)
    R: 旋转 (四元数或旋转矩阵)
    b_a: 加速度计偏置 (3D)
    b_g: 陀螺仪偏置 (3D)
    g: 重力向量 (3D)

ESKF的工作流程：

  • 预测（Predict）：用IMU预积分更新名义状态，同时更新误差状态的协方差
  • 更新（Update）：用激光里程计观测（当前帧与地图的匹配结果）计算残差，更新误差状态
  • 注入（Inject）：将估计的误差状态注入名义状态，然后重置误差状态

这种设计的优势在于：名义状态可以处理大的非线性运动，而误差状态始终保持线性，可以用标准的卡尔曼滤波高效求解。这也是FAST-LIO能跑到100Hz的重要原因。

2.4点云特征提取

FAST-LIO不需要使用全部点云进行匹配，而是提取特征点来降低计算量。特征分为两类：

  • 平面特征（Planar）：位于平面上的点，法向量与激光方向接近垂直，对平移敏感
  • 角点特征（Edge）：位于边缘或角上的点，对旋转敏感

特征提取的策略是：将一帧点云按曲率排序，曲率大的作为角点特征，曲率小的作为平面特征。FAST-LIO使用局部邻域（通常取最近的25个点）计算曲率，然后根据阈值分类。

在ESKF更新步骤中，每个平面特征点与地图中最近的平面片计算点到平面距离，每个角点特征点与地图中最近的边缘线计算点到线距离。这些距离构成观测残差，用于更新状态估计。

2.5 ikd-Tree增量式地图

FAST-LIO使用ikd-Tree（incremental k-d Tree）作为全局地图的数据结构。ikd-Tree是一种支持增量插入和删除的k-d树，专门为三维点云设计。

ikd-Tree的核心优势：

  • 增量更新：新帧点云直接插入地图，不需要重建整棵树
  • 降采样：每个体素格子只保留一个点（或几个点），控制地图大小
  • 近邻搜索：支持快速的最近邻查询，用于特征匹配
  • 删除操作：可以删除旧的或不需要的点，保持地图整洁

地图的降采样分辨率由filter_size_map参数控制（通常0.5m）。这意味着地图中每个0.5m x 0.5m x 0.5m的体素只保留一个点。这个分辨率直接影响建图精度和内存占用。

三、ROS使用指南

3.1话题与数据流

FAST-LIO作为ROS节点运行，通过话题（Topic）与其他节点通信。理解这些话题是正确使用FAST-LIO的前提。

话题名消息类型方向说明
/livox/lidarlivox_ros_driver2/CustomMsg订阅Livox雷达原始数据
/livox/imusensor_msgs/Imu订阅Livox IMU数据
/velodyne_pointssensor_msgs/PointCloud2订阅Velodyne雷达数据
/imu/datasensor_msgs/Imu订阅外部IMU数据
/Odometrynav_msgs/Odometry发布里程计（高频位姿）
/cloud_registeredsensor_msgs/PointCloud2发布注册到世界坐标系的点云
/cloud_registered_bodysensor_msgs/PointCloud2发布注册到IMU坐标系的点云
/pathnav_msgs/Path发布运动轨迹
/LaserCloudMapsensor_msgs/PointCloud2发布局部地图点云

注意：Livox雷达使用自定义消息类型CustomMsg，不是标准的PointCloud2。这是因为Livox雷达的点云数据包含每个点的反射强度、时间戳等额外信息，标准消息无法容纳。FAST-LIO内部会将CustomMsg转换为PointCloud2再处理。

3.2 Launch文件与参数配置

FAST-LIO使用ROS的launch文件启动。不同雷达型号对应不同的launch文件：

Launch文件适用雷达说明
mapping_avia.launchLivox AVIALivox非重复扫描雷达
mapping_velodyne.launchVelodyne 16/32/64Velodyne机械旋转雷达
mapping_mid360.launchLivox Mid-360Livox中距雷达
mapping_mid70.launchLivox Mid-70Livox短距雷达
mapping_horizon.launchLivox HorizonLivox单线雷达

Launch文件的核心作用是加载参数配置文件（YAML）并启动fastlio_mapping节点。参数文件是配置FAST-LIO行为的关键，下面详细介绍。

3.3参数配置详解（avia.yaml为例）

参数文件位于config/ 目录下，以YAML格式组织。以下是各参数的含义：

common:
    lid_topic:  "/livox/lidar"      # LiDAR话题名imu_topic:  "/livox/imu"        # IMU话题名time_sync_en: false             # 是否启用时间同步time_offset_lidar_to_imu: 0.0   # LiDAR到IMU的时间偏移preprocess:
    lidar_type: 1                   # 雷达类型: 1=AVIA, 2=Velodyne
    scan_line: 6                    # 扫描线数: AVIA=6, Velodyne=16/32/64
    scan_rate: 10                   # 扫描频率 (Hz)
    timestamp_unit: 1               # 时间戳单位: 0=秒, 1=毫秒, 2=微秒
blind: 0.5                      # 盲区距离 (m)，过滤近距离噪声mapping:
    filter_size_surf: 0.5           # 平面特征降采样分辨率 (m)
    filter_size_map: 0.5            # 全局地图降采样分辨率 (m)
    max_iteration: 3                # ESKF最大迭代次数
acc_cov: 0.1                    # 加速度噪声协方差
gyr_cov: 0.1                    # 陀螺仪噪声协方差
b_acc_cov: 0.0001               # 加速度计偏置噪声
b_gyr_cov: 0.0001               # 陀螺仪偏置噪声
fov_degree: 360                 # 视场角 (度)
    det_range: 100.0                # 最大探测距离 (m)
    extrinsic_est_en: false         # 是否在线估计外参
extrinsic_T: [0, 0, 0]          # LiDAR到IMU的平移外参
extrinsic_R: [1,0,0, 0,1,0, 0,0,1]  # LiDAR到IMU的旋转外参
publish:
    path_en: true                   # 是否发布轨迹
scan_publish_en: true            # 是否发布注册点云
dense_publish_en: true          # 是否发布稠密点云scan_bodyframe_pub_en: true     # 是否发布IMU坐标系点云pcd_save:
pcd_save_en: true               # 是否保存PCD
    interval: -1                    # 保存间隔: -1=结束时保存, >0=每N帧保存

关键参数调优建议：

  • filter_size_surf / filter_size_map：降低分辨率可以加快建图速度但会损失细节。室内场景0.3-0.5m，室外场景0.5-1.0m
  • max_iteration：ESKF迭代次数。精度要求高可以设为5，实时性要求高设为3
  • blind：盲区距离。需要过滤雷达自身和机器人车体的点，Livox AVIA设0.5m，Velodyne设2.0m
  • extrinsic_T / extrinsic_R：LiDAR和IMU之间的外参。如果安装位置精确已知，直接填入；如果不确定，可以设extrinsic_est_en: true让FAST-LIO在线估计

3.4多雷达型号适配

FAST-LIO支持多种雷达型号，切换雷达只需要修改YAML配置文件中的几个参数：

雷达型号lidar_typescan_line话题名消息类型
Livox AVIA16/livox/lidarCustomMsg
Velodyne 16/32/64216/32/64/velodyne_pointsPointCloud2
Livox Mid-36024/livox/lidarCustomMsg
Livox Mid-7034/livox/lidarCustomMsg
Livox Horizon46/livox/lidarCustomMsg
Livox Tele51/livox/lidarCustomMsg

注意：修改参数后不需要重新编译，FAST-LIO会在每次启动时读取最新的YAML文件。

3.5外参标定（Extrinsic Calibration）

LiDAR和IMU之间的外参（Translation + Rotation）对建图精度至关重要。如果外参不准确，建图会出现明显的重影或漂移。

FAST-LIO提供两种方式处理外参：

  • 手动填入：如果已知LiDAR和IMU的相对安装位置，直接在YAML中填写extrinsic_T和extrinsic_R
  • 在线估计：设extrinsic_est_en: true，FAST-LIO会在运行过程中自动优化外参。但初始值不能偏差太大，否则会收敛到错误值

外参的格式：

extrinsic_T: [t_x, t_y, t_z]           # 平移 (米)
extrinsic_R: [r11, r12, r13,        # 旋转矩阵 (3×3, 行优先)
              r21, r22, r23,
              r31, r32, r33]

3.6使用rosbag离线建图

在实际使用中，我们通常先录制rosbag包，再用FAST-LIO离线处理生成点云地图。这样做的好处是：

  • 可以反复调整参数，找到最佳配置
  • 避免实时建图时机器人运动不稳定导致的数据质量问题
  • 可以加速播放（Livox AVIA可以10倍速甚至20倍速）

离线建图需要两个终端（或两个SSH会话）：

# 终端1: 启动FAST-LIO
roslaunch fast_lio mapping_avia.launch rviz:=false

# 终端2: 播放rosbag
rosbag play -r 10 your_bag_file.bag

当终端2显示Done. 后，切回终端1按Ctrl+C。FAST-LIO会在退出时自动保存PCD文件。

注意：Livox AVIA数据量小，可以加速播放（-r 10甚至 -r 20），150秒的数据15秒就能播完。但Velodyne 32线数据量大，必须原速播放（-r 1），否则消息队列会爆满导致数据丢失。

四、常见问题与排查

4.1没有生成PCD

PCD文件保存在退出钩子（Exit Hook）中，只有在按Ctrl+C正常退出时才会触发。如果直接关闭终端或kill进程，PCD不会保存。

4.2报Error during open!

这通常是PCD保存路径不存在或权限问题。检查：

ls -ld /root/fastlio_ws/src/FAST_LIO/PCD
# 应该显示: PCD -> /workspace (软链接)
# 如果不是，重建软链接:
rm -rf /root/fastlio_ws/src/FAST_LIO/PCD
ln -s /workspace /root/fastlio_ws/src/FAST_LIO/PCD

4.3卡在No point, skip this scan!

这说明激光点云数据缺少必要字段。可能的原因：

  • PointCloud2缺少ring（线束ID）或time（单点时间戳）字段
  • bag包中的点云数据格式与配置不匹配
  • 雷达话题名配置错误，FAST-LIO没有接收到数据

4.4建图漂移严重

建图漂移通常由以下原因导致：

  • IMU和LiDAR的外参不准确，需要重新标定外参
  • IMU噪声参数设置不合理，调整acc_cov和gyr_cov
  • 环境缺乏几何特征（长走廊、白墙），增加max_iteration或降低filter_size_map
  • 运动补偿不准确，检查IMU频率是否足够高

4.5内存占用过高

如果建图范围很大，ikd-Tree会占用大量内存。可以：

  • 增大filter_size_map（如从0.5m改为1.0m）来减少地图点数
  • 设置pcd_save.interval为正值（如200），定期保存并清空地图
  • 限制建图范围（减小det_range参数）

五、总结

FAST-LIO是一套高效、实用的激光-惯性紧耦合SLAM方案。它的核心优势在于：

  • 速度快：ESKF + ikd-Tree的设计使其在普通CPU上就能跑到100Hz
  • 精度高：紧耦合框架充分利用了IMU和LiDAR的互补优势
  • 易用性：ROS接口清晰，参数配置简单，支持多种雷达型号

在实际使用中，关键是理解参数的含义并根据场景调优。希望本文能帮助你更好地使用FAST-LIO进行机器人建图。

更多文章