聊到激光SLAMFast-LIO2几乎是绕不开的名字。2021年港大MaRS实验室把它放出来以后很多做机器人和自动驾驶感知的团队都拿它当底层方案。我第一次跑通Fast-LIO2的时候最直观的感受是它把“紧耦合”这件事做得非常彻底而且IKD-Tree这套增量建图结构在当时确实比传统体素地图方案轻快不少。这篇东西不是泛泛的原理科普而是我基于源码阅读和ROS2部署实战写的一份拆解笔记。你会看到Fast-LIO2到底强在哪、代码是怎么组织的、迁移到ROS2要改哪些东西、以及我在实际部署时踩过的一堆坑。适合正在用Fast-LIO2做项目、或者打算把它接到自己机器人平台上的朋友。1. Fast-LIO2的核心思路与整体设计拆解1.1 为什么Fast-LIO2一直是激光惯性SLAM的参考对象在聊代码之前先花点时间把Fast-LIO2的定位说清楚。它本质上是一个“激光雷达IMU”紧耦合的状态估计器输出的是高频里程计odometry和一致性很好的局部/全局点云地图。它不依赖GPS也不需要预先建好的环境模型全靠激光雷达扫到的几何特征和IMU积分出的运动约束就能把机器人的位姿估出来。很多人会把Fast-LIO2和LIO-SAM、LIO-SAM的变体以及LOAM系列放在一起比较。Fast-LIO2的核心差异主要体现在两块第一它使用迭代扩展卡尔曼滤波IEKF把IMU和激光雷达的观测耦合在一起而不是像LOAM那样做“先配准再优化”的两段式处理第二它用IKD-Tree作为地图数据结构支持增量式插入和删除点云避免了每一帧都全量重建KD-Tree的开销。我在代码里看到最典型的设计是每一帧激光点进来系统不会先用暴力配准去对齐全局地图而是用IMU前向传播给出的预测位姿作为初值然后通过IEKF对状态量做迭代修正。整个过程中地图不断被增量更新每次配准用到的点云邻域查询都直接打在IKD-Tree上。这套结构天然适合长时间运行因为“地图变大导致每帧配准变慢”的问题被增量更新大幅压低了。1.2 增量式地图IKD-Tree和紧耦合框架各自的角色IKD-Tree是我认为Fast-LIO2最值得拆的一块。传统的KD-Tree建好后如果地图持续增长你需要定期整树重建而IKD-Tree通过增量操作把新点插入、删除、重平衡都做了优化。它的核心思路是“按需更新”而不是“全局重建”。当机器人在环境中移动远处的点会不断被淘汰右下角的老区域点云会逐渐被移除这个过程会用一种带阈值判断的增量更新机制完成。在代码层面IKD-Tree维护了树节点的几何包围盒并记录每个节点下的点数量。插入新点时它会从根节点出发找到合适的叶子位置删除点时它不直接物理删除而是通过一定的惰性删除策略标记并在结构失衡时触发重建。这样带来的直接收益是在几十万甚至上百万点的地图上做最近邻搜索单次查询仍然能保持比较稳定的耗时。紧耦合框架方面Fast-LIO2的IEKF是在IMU预测的基础上把激光雷达的scan-to-map残差作为量测。每一帧lidar点云会被投影到预测位姿下然后在IKD-Tree地图里找最近邻平面残差作为量测更新。迭代几次后状态收敛再把这帧点云插入地图。整个流程读代码时很容易看晕因为里面穿插了多个坐标系变换和残差求解但核心逻辑就是“IMU积分预测激光配准修正更新后再建图”。2. Fast-LIO2关键代码模块解析2.1 工程结构总览从launch到核心算法源码拉下来以后你会发现FAST_LIO的工程结构非常清晰。它大体分为config、launch、include和src几个目录。include目录下有关键的头文件比如LIO-Preprocess.h负责点云预处理IMU_Processing.hpp负责IMU前向传播和状态预测ikd-Tree/ikd_Tree.cpp实现增量KD-Tree而laserMapping.cpp则是整个系统的入口和主循环。我在阅读时比较推荐的路线是先看laserMapping.cpp里两个回调函数std::functionvoid(const PointCloudType::Ptr) laserCloudHandler和std::functionvoid(const ImuMsg ) imuHandler。imuHandler负责把IMU数据不断送入队列进行积分laserCloudHandler则在每一帧激光到达时触发配准、状态更新和建图流程。理解了这两个入口后面再钻进IMU_Processing.hpp和ikd_Tree就会自洽得多。fast_lio ├── config // 不同雷达和平台的yaml配置 ├── launch // ROS1/ROS2的launch文件 ├── include │ ├── LIO-Preprocess.h │ ├── IMU_Processing.hpp │ ├── ikd-Tree │ │ ├── ikd_Tree.cpp │ │ └── ikd_Tree.h │ └── ... └── src ├── laserMapping.cpp └── preprocess.cpp2.2 核心状态估计器IESKF的前向传播与迭代更新Fast-LIO2的状态量包括位置、速度、姿态四元数、陀螺仪零偏和加速度计零偏以及重力向量。代码里有一个用MTK库封装的状态流state_ikfom它在IMU_Processing.hpp里被用来做前向传播。前向传播的意思就是在两个激光帧之间用IMU的角速度和线加速度按动力学模型把状态从上一帧推到当前时刻同时更新协方差矩阵。这个前向传播过程很吃对惯导公式的理解但你不一定要把每个雅可比都手推一遍。实际看代码时你只需要关注它在做什么以及为什么需要协方差传播。IEKF的迭代更新发生在激光帧到达之后它会在前向传播给出的预测位姿附近反复计算残差和卡尔曼增益直到收敛或达到最大迭代次数。这样做的好处是即使初始预测位姿有些偏差通过迭代也能逐步修正回来。我自己在拆代码时最大的体会是不要一头扎进公式里。先跑通整个流程再回头看ieskf.hpp里的update_iterated_dyn_share函数把残差计算和增益更新的代码打上断点一帧一帧看数值变化理解效率高得多。2.3 点云预处理与去畸变点云预处理是Fast-LIO2容易被人忽略但非常关键的部分。机械式雷达和固态雷达的点云格式不同livox雷达的点还带有相对时间戳因为固态雷达是逐点扫描的。LIO-Preprocess.h里会根据雷达类型做不同的点云处理比如livox点云要额外处理点的offset_time用IMU数据对每个激光点做运动补偿去畸变。这里有一个常见的坑如果IMU频率不够或者时间同步不准点云去畸变会直接把地图搞糊。Fast-LIO2的做法是用IMU积分出的相对位姿将每个点在扫描周期内的运动补偿掉因此IMU和雷达的时间戳必须对齐。很多人在实机上跑不通最后查下来都是IMU话题时间戳跳变或者驱动里时间基准不一致。3. 从ROS1到ROS2部署Fast-LIO2的完整流程3.1 环境准备Ubuntu 22.04 ROS2 Humble我的部署环境是Ubuntu 22.04 ROS2 Humble。相比ROS1 NoeticROS2在消息传递、节点生命周期和编译系统上都有很大变化。Fast-LIO2的官方代码主干是基于ROS1的好在社区维护了ROS2分支把依赖切到了rclcpp、tf2_ros和sensor_msgs/msg/PointCloud2。准备阶段建议先把ROS2基础环境装好。如果是从零开始ros-humble-desktop足够覆盖rviz2、tf2、common_interfaces这些组件。别忘了安装编译工具链和依赖库sudo apt update sudo apt install -y ros-humble-desktop ros-dev-tools sudo apt install -y libeigen3-dev libpcl-dev sudo apt install -y ros-humble-pcl-ros ros-humble-pcl-conversions sudo apt install -y ros-humble-tf2-ros ros-humble-tf2-geometry-msgsROS2里编译用的是colcon而不是ROS1的catkin_make。建议把source /opt/ros/humble/setup.bash写进.bashrc再配一个独立的workspacemkdir -p ~/fastlio2_ws/src cd ~/fastlio2_ws/src git clone https://github.com/hku-mars/FAST_LIO.git -b ros2拉完代码后先看一眼分支名。不同社区维护者可能用ros2、humble这样的分支名或者用fork仓库。如果直接拉默认分支大概率是ROS1版本编译时会报ros/ros.h找不到。3.2 编译Fast-LIO2与livox_ros_driver2的衔接Fast-LIO2本身不直接驱动雷达硬件。对livox雷达来说你需要同时编译livox_ros_driver2由它把雷达数据发布成ROS2消息Fast-LIO2再去订阅。这个衔接关系很容易踩坑特别是消息类型不一致的时候。livox_ros_driver2默认发布的是livox_interfaces/msg/CustomMsg这是一种带点相对时间信息的高效消息。Fast-LIO2为了兼容多种雷达在config里通过lidar_type字段区分雷达类型1是livox2是velodyne3是ouster。当你设置lidar_type: 1时Fast-LIO2的预处理节点内部会订阅livox/interfaces/msg/CustomMsg把点云解包成PCL点云再处理设置成2或3时则订阅sensor_msgs/msg/PointCloud2。如果你用的是livox雷达建议按这个顺序编译先编livox_ros_driver2再编fast_lio。因为Fast-LIO2的ROS2分支在编译时如果找不到livox接口消息有可能会跳过livox支持。我实测时是放在同一个workspace里cd ~/fastlio2_ws/src git clone https://github.com/Livox-SDK/livox_ros_driver2.git cd ~/fastlio2_ws colcon build --symlink-install编译过程中如果遇到PCL和Eigen的版本问题最常见的是C标准不匹配。Fast-LIO2的CMakeLists里默认用C14如果系统里某些库要求C17你需要统一修改。另外我遇到过Eigen 3.4版本下部分代码报static_assert错误最后通过把Eigen降到3.3或者给CMake加-DEIGEN_DONT_ALIGN_STATICALLY解决但后者不是所有场景都适用建议优先考虑版本对齐。3.3 launch文件编写与参数配置编译通过后部署的重头戏是launch文件和yaml参数配置。ROS2的launch文件推荐用Python写比ROS1的XML灵活不少。Fast-LIO2的launch目录里通常自带示例但你需要根据实际雷达和IMU话题名做调整。下面是一个典型的launch文件基本骨架from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): return LaunchDescription([ Node( packagefast_lio, executablefastlio_mapping, namefastlio_mapping, outputscreen, parameters[ {use_sim_time: False}, {config_path: /path/to/your/config.yaml} ], ) ])这个写法是一个参考。实际使用时包名、可执行文件名要以你拉下来的仓库为准。如果同一个workspace里还跑着livox_ros_driver2可以把driver节点也加进launch一次启动两个节点。yaml参数文件里最需要关心的是这样几个字段字段作用注意事项common.lid_topic激光雷达话题名要和driver实际发布的话题一致common.imu_topicIMU话题名要确认IMU驱动数据有效preprocess.lidar_type雷达类型1-livox2-velodyne3-ousterpreprocess.scan_line雷达线数velodyne 16/32/64与livox不同preprocess.timestamp_unit点的时间戳单位常见为0秒、1毫秒、2微秒mapping.filter_size_surf配准时的体素滤波尺寸太大丢特征太小计算量大mapping.max_iterationIEKF最大迭代次数默认3到5即可过大会增加耗时我通常会把filter_size_surf设在0.2到0.5之间场景空旷可以稍大室内结构化场景建议偏小。max_iteration不要一味堆高IMU初始化没做好的情况下迭代次数再多也救不回来反而会把CPU吃满。3.4 用bag包验证链路再接实机部署Fast-LIO2的时候我强烈建议先用别人录好的bag包把整个链路打通。不要一上来就接实机因为实机的问题变量太多了时间同步、雷达驱动参数、IMU安装位姿、话题命名、tf树任何一环出问题都会让你的排查过程非常痛苦。先把launch文件里的use_sim_time设为true再用ros2 bag play回放一条包含/livox/lidar和/livox/imu的bag。观察终端日志是否出现点云帧率输出同时在rviz2里订阅/Odometry和/PointCloud2看看是否有一条干净的轨迹和逐渐累积的地图。如果你手头没有现成bagFast-LIO2的GitHub仓库wiki里给过一些示例数据下载链接也可以去港大MaRS实验室的公开数据集页面找。实测下来用官方数据验证算法链路是最快的。链路通了以后再回到实机上把use_sim_time改回false把话题名对齐到自己的驱动基本就成功了一大半。4. Fast-LIO2 ROS2部署避坑与性能调优4.1 编译期的典型报错与处理我在ROS2部署过程中遇到的编译期报错主要集中在三块。第一块是ros/ros.h not found。这是拉错分支或者没有正确切到ROS2分支导致的。解决办法是确认源码分支或者检查CMakeLists里是否引用了ROS1相关的包。第二块是M_PI未定义。在C14标准下M_PI不是所有编译器环境都默认定义尤其是用了严格标准时。遇到这类报错在文件顶部加#define _USE_MATH_DEFINES或者直接包含cmath并确保定义宏在cmath之前通常能解决。第三块是Eigen版本导致的静态断言错误。Eigen 3.4对内存对齐检查更严格部分旧代码会出现STATIC_ASSERT报错。我的排查顺序是先看Eigen版本再看CMake里有没有对Eigen include路径的硬编码最后才考虑加编译期宏绕过。不要一上来就改宏隐藏问题可能会在后端爆出来。4.2 运行时无地图、掉线、点云漂移的排查逻辑编译通过只是第一步运行时的坑才是大头。这里列几个我实际遇到过的问题以及对应的排查思路。**问题一启动后没有点云输出。**先确认激光驱动节点是否正常发布用ros2 topic hz /livox/lidar查看频率。如果话题有数据再看Fast-LIO2是否订阅了正确的话题名。yaml里的lid_topic要和实际话题一致。这个过程不要靠猜直接ros2 topic list和ros2 topic echo看。**问题二rviz2里看不到地图或odom。**这大概率是tf关系缺失或者坐标系名称不匹配。Fast-LIO2默认发布的odometry是从map到body的变换而点云地图通常也基于map系。如果只订阅/Odometry在rviz2里还需要设置Fixed Frame为map。如果tf树里少了map - body检查是否把publish_tf参数打开了。**问题三地图模糊或轨迹发散。**这种问题十有八九出在时间戳和IMU质量上。先看bag包的IMU频率够不够一般200Hz以上比较稳妥再看IMU话题时间戳和激光话题时间戳是否在同一个时间基准上。ROS2的bag回放如果不加--clockuse_sim_time设置不正确时间戳会混乱从而直接导致去畸变和状态预测失败。4.3 性能调优让Fast-LIO2跑得更稳更省资源性能调优是部署中后期绕不开的工作。Fast-LIO2的耗时会受到点云分辨率、特征提取阈值、地图滤波尺寸和IKD-Tree维护频率的共同影响。一个比较直接的切入点是filter_size_surf和filter_size_map。filter_size_surf影响每帧配准使用的点数值越小保留的点越多配准精度通常更高但耗时也会上涨。filter_size_map则控制建图时下采样密度它影响IKD-Tree的节点数量和最近邻搜索效率。总的原则是在高动态场景优先保精度适当降低filter尺寸在长时间大范围建图时优先考虑控制地图规模避免内存和搜索耗时失控。如果你发现单帧处理时间有明显尖峰多半是IKD-Tree触发了重平衡或者部分区域点云密集度过高。可以检查一下配置里有没有对地图点数量的限制或者是否开了定时清远点的机制。Fast-LIO2源码中已经内置了范围管理逻辑但具体开关和阈值要看分支版本有的社区分支会默认关掉远程点删除。我实测时在室内的16线机械雷达场景filter_size_surf0.3、max_iteration3在i7处理器上单帧处理大概能维持在20毫秒左右。把filter_size_surf降到0.15后单帧耗时接近40毫秒但地图细节明显更丰富。这个取舍完全取决于你的机器人移动速度和下游任务对地图的要求不能照搬别人的参数。4.4 坐标系与时间同步容易被忽视的系统性问题最后想说一个很多人没意识到的问题Fast-LIO2不是一个完整的导航系统它只负责估计状态和建立地图但下游做规划、导航或者八叉树地图转换时坐标系和时间同步依然是你整个ROS2系统的基础。Fast-LIO2的输出是odometry和点云它的内部假设是IMU坐标系和雷达坐标系之间的外参已经在yaml里通过extrinsic_rot和extrinsic_trans配好了。如果外参给错地图必然发散而且这种发散通常表现为“转了几圈后地图开始重影、弯曲”。外参标定建议用单独的标定工具完成不要手估。我见过太多人因为“差不多就行”的外参把Fast-LIO2跑成了废数据。另外如果你的系统里还有相机、深度相机或者机器人底盘一定要在launch层面把各个传感器的时间同步策略理清楚。ROS2里常见的做法是使用message_filters做时间同步或者确保所有传感器都通过同一台PTP/NTP时间同步服务器校准。我自己的习惯是把所有传感器的话题频率和时间戳来源都记录下来做成一张表贴在项目文档最前面。看起来麻烦但一旦出现问题你不需要从头猜。5. 写在最后一些实操体会如果你正在从ROS1往ROS2迁移或者第一次尝试部署Fast-LIO2我个人最大的建议是不要把目光只盯在“编译通过”和“看到地图”这两件事上。编译通过只是起点真正难的是理解状态估计链路里的每个环节为什么存在以及你的机器人平台有哪些系统和它在交互。我踩过几次坑之后慢慢形成了一个固定套路先在仿真或bag包上把算法链路跑稳定再把参数、话题、tf和时间同步这些系统性问题彻底理顺最后才上实机。实机上遇到问题不要急着改算法参数先查数据链路。很多时候问题不在Fast-LIO2本身而在于你给它的数据到底对不对。后续如果你想在这个基础上做更深度的扩展比如把Fast-LIO2的地图输出转成八叉树地图或者接入导航栈做自主导航建议先把这个部署流程稳定复现两三遍直到你不用看教程也能独立完成配置。到那个时候再去看ikd-Tree的增量更新细节、去改IEKF的迭代策略你会有完全不一样的感受。Fats-LIO2这套代码值得反复读每读一遍你能看出的门道都会多一层。