1. 先说结论FAST_LIO_ROS2能跑gazebo仿真但别指望开箱即用第一次看到“FAST_LIO_ROS2 可以用gazebo仿真吗”这个问题时我能猜到提问的人是什么状态刚在B站或GitHub上刷到FAST_LIO的建图视频觉得效果很惊艳想在自己电脑上复现一遍但没有实车、没有雷达第一反应就是“那我能不能在gazebo里搭个环境先跑跑看”。答案是能而且不限于“能不能跑”很多团队已经在用gazebo做FAST_LIO的算法验证、退化场景测试和自动驾驶仿真。但我要先把丑话说在前面FAST_LIO_ROS2不像turtlebot3仿真那样装完包launch一下就能看到小车在gazebo里跑SLAM。它依赖雷达点云和IMU的紧耦合仿真环境必须同时提供这两类传感器数据还要保证时间戳、坐标系外参、噪声模型都对齐否则仿真结果会非常难看——点云发散、地图扭曲、里程计飘出天际这些都是常见现象。所以这篇内容我会把“FAST_LIO_ROS2 gazebo仿真”这条路上你会踩的坑、需要配置的点、以及我实测下来比较稳的方案一次性讲清楚。包括gazebo版本怎么选、激光雷达和IMU插件怎么配、launch文件里最关键的那几个参数是什么意思、仿真发散怎么排查。适合的目标读者是想在仿真环境里验证FAST_LIO建图效果的学生、刚接触激光SLAM的工程师以及准备用FAST_LIO做导航或探索项目但还没买硬件的开发者。1.1 先花一分钟搞懂FAST_LIO靠什么工作要理解仿真为什么“麻烦”先得知道FAST_LIO的本质。FAST_LIOFast LiDAR-Inertial Odometry是港大MARS Lab开源的激光惯性里程计算法核心思路是用IMU做状态传播用激光雷达点云做迭代卡尔曼滤波更新从而在高动态、快速运动、甚至剧烈旋转的场景下依然保持低漂移。它不像纯激光里程计那样只依赖雷达也不像纯视觉惯性里程计那样需要良好的光照它是“IMU做粗预测 雷达点云做精修正”。这就意味着它在运行过程中需要持续读入两类消息IMU消息通常话题类型是sensor_msgs/Imu频率一般要求100Hz到400Hz包含三轴角速度和三轴线性加速度。激光雷达点云通常是sensor_msgs/PointCloud2频率10Hz到20Hz包含三维点的坐标和强度值。FAST_LIO_ROS2内部正是通过这两类话题的时间戳来做数据关联的IMU按时间戳累积成队列雷达帧到来时用这段时间里的IMU数据做状态预测和点云去畸变再用去畸变后的点云去匹配地图。因此仿真环境能不能复现这套数据流决定了FAST_LIO能不能跑起来。1.2 在gazebo里还原这个数据流需要什么gazebo本身是一个强大的物理仿真引擎但它不会自己“吐”出SLAM算法需要的传感器话题你需要通过插件来模拟传感器。具体来说要在gazebo里还原FAST_LIO的工作条件至少要解决三件事有一个能产生多线激光雷达点云的传感器插件输出sensor_msgs/PointCloud2并且要能模拟出多线扫描的分布和噪声。有一个能产生IMU数据的插件输出sensor_msgs/Imu包含线性加速度和角速度。这两个传感器要固定在机器人模型上并且它们的坐标变换关系和外参配置必须一致。换句话说gazebo可以用但你需要一个“传感器齐全”的机器人模型。这也是为什么很多人下载了FAST_LIO_ROS2包之后直接跑去launch结果发现gazebo里只有地面没有任何点云话题根本配不起来。2. 仿真前的环境准备版本搭配是第一步如果你从来没有搭过ROS2 gazebo仿真环境这一章建议从头看。如果已经跑通过gazebo的基本仿真可以直接跳到第3章看FAST_LIO相关的配置。2.1 ROS2发行版和gazebo版本怎么选很多新手在ROS2版本上栽过跟头。FAST_LIO_ROS2官方主要支持的是ROS2的Foxy、Humble和Galactic版本但实际大家现在用的最多的是Humble因为对应的文档多、踩坑帖子多、gazebo生态也成熟。以Ubuntu 22.04为例装ROS2 Humble后deb包默认会带gazebo 11但ROS2 Humble下更推荐用Gazebo Classic也就是原来的gazebo 11加gazebo_ros_pkgs这套组合的文档最全网上能找到的教程也多。这里有个很多人搞混的坑ROS2 Humble时代主流教程提到“gazebo”时默认都指Gazebo Classic 11。而Ubuntu 24.04 ROS2 Jazzy搭配的是新版Gazebo即Gazebo Harmonic / Ignition系统虽然新版Gazebo更现代、渲染更好但很多老插件的包名、参数格式都变了FAST_LIO相关教程和现成工作区多数还是按Classic写的。所以我的个人建议是操作系统推荐ROS2版本推荐gazebo版本备注Ubuntu 20.04Foxy / GalacticGazebo 11老方案能用但资料开始变少Ubuntu 22.04HumbleGazebo 11最推荐教程最多稳定性最好Ubuntu 24.04JazzyGazebo Harmonic新生态但移植FAST_LIO_ROS2需要处理依赖如果你只是想在仿真里验证FAST_LIO我的建议是老老实实用Ubuntu 22.04 ROS2 Humble Gazebo 11省下的都是调环境的时间。2.2 让gazebo输出传感器话题的几种途径gazebo本身不带“雷达”“IMU”这种东西它们都是plugin。在ROS2里最常用的是gazebo_ros_pkgs提供的传感器插件系列。常见的有libgazebo_ros_imu_sensor.soIMU插件能在机器人模型上产生IMU话题。libgazebo_ros_ray_sensor.so射线传感器插件能模拟单线或多线激光雷达但输出的是sensor_msgs/LaserScan不是点云。libgazebo_ros_velodyne_laser.so/libgazebo_ros_velodyne_gpu_laser.soVelodyne雷达插件直接输出sensor_msgs/PointCloud2而且是多线雷达分布非常适合FAST_LIO仿真。你可能会问gazebo_ros_ray_sensor也能转成点云为什么不直接用因为LaserScan是单线雷达的典型输出FAST_LIO处理的是多线雷达点云。虽然理论上可以用laserscan_to_pointcloud这样的转换节点把单线转成点云但单线雷达信息量太少FAST_LIO在这种数据上很难发挥出紧耦合的优势地图质量会很差。所以我强烈建议在gazebo仿真FAST_LIO时直接上Velodyne插件输出多线的PointCloud2。后面我会给出具体的SDF配置示例。3. 搭建一个带雷达和IMU的gazebo机器人模型既然标准turtlebot3模型不带多线雷达我们要么自己写URDF/SDF要么用现成的带Velodyne的模型。这里我选择从头写一个最简模型这样你能理解每个配置项的意义后面改参数也有底。3.1 在URDF里加入Velodyne雷达插件假设你有一个机器人底盘URDF现在我们给它加一个Velodyne VLP-16风格的雷达。在URDF的robot节点里通过gazebo扩展标签来添加插件gazebo referencevelodyne_link sensor typegpu_ray namevelodyne_sensor pose0 0 0.5 0 0 0/pose visualizetrue/visualize update_rate10/update_rate ray scan horizontal samples1800/samples resolution1/resolution min_angle-3.1415926/min_angle max_angle3.1415926/max_angle /horizontal vertical samples16/samples resolution1/resolution min_angle-0.261799/min_angle max_angle0.261799/max_angle /vertical /scan range min0.5/min max100.0/max resolution0.01/resolution /range noise typegaussian/type mean0.0/mean stddev0.02/stddev /noise /ray plugin namegazebo_ros_velodyne_controller filenamelibgazebo_ros_velodyne_laser.so ros remapping~/out:/velodyne_points/remapping /ros frame_namevelodyne_link/frame_name min_range0.5/min_range max_range100.0/max_range /plugin /sensor /gazebo这里有几个参数值得专门说一下samples为1800意味着水平一圈扫描1800个点约0.2度一个点对应16线激光雷达的水平分辨率。vertical层的min_angle和max_angle是正负15度一共16线基本符合VLP-16的垂直视场角。update_rate为10Hz和常见机械式雷达帧率一致。noise里的高斯噪声是为了模拟真实雷达的测距误差这个噪声对FAST_LIO来说是好事因为算法本身就假设观测有噪声。如果没有噪声点云过于“完美”反而会让仿真结果显得不真实。3.2 在URDF里加入IMU插件IMU插件相对简单同样挂在gazebo标签里gazebo referenceimu_link sensor typeimu nameimu_sensor always_ontrue/always_on update_rate200/update_rate imu angular_velocity xnoise typegaussianmean0/meanstddev0.0002/stddev/noise/x ynoise typegaussianmean0/meanstddev0.0002/stddev/noise/y znoise typegaussianmean0/meanstddev0.0002/stddev/noise/z /angular_velocity linear_acceleration xnoise typegaussianmean0/meanstddev0.017/stddev/noise/x ynoise typegaussianmean0/meanstddev0.017/stddev/noise/y znoise typegaussianmean0/meanstddev0.017/stddev/noise/z /linear_acceleration /imu plugin namegazebo_ros_imu_controller filenamelibgazebo_ros_imu_sensor.so ros remapping~/out:/imu/data/remapping /ros frame_nameimu_link/frame_name /plugin /sensor /gazebo这里有个容易踩的坑很多教程里IMU的update_rate只设成50Hz或者100Hz。对普通仿真来说没问题但FAST_LIO对IMU频率比较敏感因为算法靠IMU在雷达帧之间做运动积分。如果IMU频率太低积分误差会很大导致雷达点云去畸变效果变差。建议至少200Hz。噪声也不能设为零。如果没有噪声IMU数据相当于理想真值FAST_LIO做状态估计时协方差会非常小这会让后续真实验证失去意义。当然噪声也别设得太大加速度计的stddev设在0.01到0.05之间即可太大算法会直接飞。3.3 TF树不能缺否则launch起来就是一团乱麻有了模型还不够还需要发布TF关系。FAST_LIO内部会查询激光雷达、IMU和机器人基座之间的坐标变换通常需要知道map - body/odom - imu_link - velodyne_link这样一条完整的TF树。在仿真里最简单的办法是先用robot_state_publisher发布URDF里的关节和link间的TF再用static_transform_publisher发布从base_link到imu_link、velodyne_link的静态TF。如果你直接照搬FAST_LIO_ROS2的默认launch它只会启动FAST_LIO自己的节点不会帮你发布机器人的TF所以这一步一定不能省。一个典型的launch文件会包含类似这样的部分node_robot_state_publisher Node( packagerobot_state_publisher, executablerobot_state_publisher, parameters[{robot_description: robot_description, use_sim_time: True}], outputscreen, ) node_static_tf_imu Node( packagetf2_ros, executablestatic_transform_publisher, arguments[0, 0, 0.5, 0, 0, 0, base_link, imu_link], outputscreen, ) node_static_tf_lidar Node( packagetf2_ros, executablestatic_transform_publisher, arguments[0, 0, 0, 0, 0, 0, imu_link, velodyne_link], outputscreen, )外参这部分很多人一开始不重视直到看到地图斜着甚至发散了才回头查。记住gazebo里雷达和IMU的安装位置和FAST_LIO参数文件里配置的外参必须严格一致。4. FAST_LIO_ROS2的launch与参数配置实操仿真环境就绪后真正让FAST_LIO跑起来的关键就是launch文件和YAML参数。网上很多“为什么我的FAST_LIO地图发散”的提问十有八九都出在这一步。4.1 配置文件中必须确认的5个参数FAST_LIO_ROS2的配置文件是一个YAML文件通常在config目录下。打开后你会看到很多参数但真正影响仿真运行的核心参数就这几个参数作用仿真建议值lid_topic雷达点云话题名设置成gazebo里雷达插件输出的话题如/velodyne_pointsimu_topicIMU话题名设置成gazebo里IMU插件输出的话题如/imu/datamax_iteration单个雷达帧内迭代卡尔曼的最大迭代次数仿真中可以设成3速度和精度比较均衡body_extrinsic雷达坐标系到IMU坐标系的变换矩阵必须和你URDF里的安装一致这是发散头号原因lidar_type雷达类型0Velodyne1Ouster2RS-LiDAR如果你用Velodyne插件设0即可很多人拿到默认配置后直接跑结果雷达话题都接不上原因就是lid_topic和imu_topic名字不对。gazebo里的插件输出的默认话题是/velodyne_points和/imu/data而FAST_LIO默认配置可能是/points_raw、/imu_raw不对齐就必然没数据。4.2 launch文件里别忘了打开use_sim_time这是新手最容易忽略的一个点。gazebo仿真有一个独立的仿真时钟如果你不在FAST_LIO节点上开启use_sim_timeROS2节点会默认使用系统墙钟时间。gazebo里的传感器时间戳和系统时间根本不在一个坐标系下结果是IMU和雷达消息的时间戳永远对不上FAST_LIO会直接卡在初始化阶段或者点云去畸变全乱。正确做法是在launch文件里对FAST_LIO节点设置Node( packagefast_lio, executablefastlio_mapping, parameters[config_file, {use_sim_time: True}], outputscreen, )注意不仅仅是FAST_LIO节点其他参与数据流的节点比如robot_state_publisher、rviz2最好也统一设置use_sim_timeTrue。否则你可能会看到RVIZ里点云在动但TF不动或者地图和模型错位的怪异现象。4.3 第一次跑通后的检查方法按照上面的配置把launch启动起来如果一切正常你会看到gazebo里小车在跑RVIZ里FAST_LIO慢慢构建出点云地图。但第一次跑通不代表结果就是对的我一般会做三个快速检查看话题时间戳运行ros2 topic echo /imu/data --once看一下header.stamp的sec和nanosec是否和ros2 topic echo /clock --once的仿真时间一致。如果时间对不上说明某个节点的use_sim_time没开。看RVIZ里点云是否随小车运动而正确变化静止状态下点云应该稳定存在运动状态下点云会出现轻微拖影但整体轮廓清楚。如果静止时点云都在乱跳大概率是IMU噪声或外参问题。看IMU和雷达是否同步用ros2 topic hz /imu/data和ros2 topic hz /velodyne_points分别看频率IMU要在100Hz以上雷达要在10Hz左右。这三步走完你的FAST_LIO_ROS2仿真就已经算真正跑通了。5. 仿真发散问题排查我把常见原因和解决办法都列在这说实话FAST_LIO在gazebo仿真里最容易出现的不是“跑不起来”而是“跑起来之后地图开始扭曲、发散、甚至越来越稀碎”。这个现象让人很崩溃但绝大多数情况都能从下面几个方向里找到原因。5.1 时间戳不同步导致的数据关联错乱这是最隐蔽也最常见的问题。具体表现是FAST_LIO节点启动后IMU消息偶尔能进来但雷达点云消息一直不触发处理或者建图时地图每隔一段时间就突然跳一块。原因基本就是某个节点没有打开use_sim_time或者IMU和雷达的话题时间戳相差太大。gazebo的传感器插件默认会用仿真时间给消息打时间戳但如果你的机器人在多个命名空间下运行或者有节点把时间戳重新映射到了系统时间就会出问题。排查方法很直接分别在静止状态下查看/imu/data和/velodyne_points的header.stamp再对比当前仿真时钟。如果相差超过几十毫秒就说明有问题。修复方式是把launch文件里所有数据相关节点的use_sim_time都设为true。5.2 IMU噪声设置不当引起的地图抖动仿真中IMU噪声有两个极端噪声设为零算法会过度信任IMU结果一点小误差就可能导致滤波发散噪声设得太大算法又无法从IMU数据中提取有效的运动信息前端里程计会漂移得很厉害。我的建议是加速度计噪声stddev控制在0.01到0.05之间单位是m/s^2陀螺仪噪声stddev控制在0.0001到0.001之间单位是rad/s。如果你用的是gazebo默认插件且不手动设置噪声它默认给的noise其实是零这种情况下反而要小心。你可以先按上面的数值手动设置然后观察地图在静止和运动状态下的表现再微调。5.3 雷达扫描参数与算法假设不符FAST_LIO内部对点云的处理依赖“多线雷达”这个前提它会把点按scan id分组用来做去畸变和特征提取。如果你用的雷达插件设置不合理比如垂直方向的线数太少或者水平角度范围和实际模型对不上就会导致点云信息不足匹配算法的鲁棒性大幅下降。在gazebo中如果你用的不是Velodyne插件而是自己改的ray sensor那么至少保证垂直方向有16层。水平方向samples不一定要1800但最低不要少于900否则点云太稀疏特征匹配容易失败。5.4 外参错误导致地图倾斜这个原因很经典你URDF里把雷达放在车体前方0.3米、高度0.5米的位置但FAST_LIO的YAML里body_extrinsic仍然是默认的单位矩阵或者写的是另一个位置。结果算法假设雷达就在IMU的原点上实际数据却来自另一个坐标点云投影后自然错位地图就歪了。判断方法很简单建图过程中把RVIZ里点云和机器人模型同时显示如果点云和模型明显错开或者地面点云和真实高度对不上基本就是外参问题。你需要回去对齐URDF中link的pose和YAML里的body_extrinsic。6. 常见的仿真运行问题与实操心得总结一下我在仿真过程中实际遇到过的典型问题以及对应的解决办法。这些问题可能不会一次性全部出现但提前看一下能省不少时间。6.1 gazebo界面一直闪烁或渲染异常这是一个相对独立的问题但会影响调试体验。在Ubuntu 22.04上gazebo界面闪烁多数是显卡驱动或者默认渲染后端的问题。如果你用的是虚拟机推荐先退出虚拟机图形加速或者在启动gazebo前设置环境变量LIBGL_ALWAYS_SOFTWARE1。如果是实体机先检查显卡驱动是否正常再试export LIBGL_ALWAYS_INDIRECT0。另外gazebo 11和某些NVIDIA驱动组合会出现双屏闪烁可以尝试把gazebo窗口固定在某个显示器上运行或者降低渲染画质。6.2 仿真速度特别慢点云更新跟不上gazebo的物理仿真和传感器仿真都会消耗CPU。如果场景中有大量物体模型或者传感器更新频率设置得太高仿真速度会明显低于实时速度。结果就是FAST_LIO看到的时间戳和实际运行速度不匹配建图效果异常。解决办法是降低仿真场景复杂度或者适当降低雷达水平方向的samples。samples900配合update_rate10Hz通常已经够用既保持点云密度又减轻CPU负担。IMU的update_rate也不一定要200Hz如果CPU吃紧150Hz也行。6.3 用turtlebot3做底盘时速度过快导致点云模糊很多教程会用turtlebot3作为底盘模型因为现成、好改、包也多。但有一个坑是turtlebot3的线速度和角速度上限设置偏大尤其在gazebo里手动键盘遥控时很容易把速度拉满。运动太快时雷达一个扫描周期内机器人的位移变化太大点云畸变严重即使FAST_LIO有能力补偿也有一定极限。建议在仿真调试阶段把最大线速度限制在0.5m/s以内最大角速度限制在0.5rad/s以内。等算法稳定性验证通过再逐步提高速度测试极限。7. 写在最后的实际使用建议我自己在仿真环境里反复折腾FAST_LIO_ROS2的这段时间最大的感触是仿真跑通只是第一步难的是让结果有意义。gazebo给你的传感器数据再真实也是经过建模的IMU噪声、雷达噪声都比真实传感器“乖巧”很多所以仿真跑出来的效果通常比实车好。这反过来说明一个问题如果你在仿真里都调不出稳定的地图那实车上大概率更糟糕。我的习惯做法是先让机器人在gazebo里静止30秒观察FAST_LIO是否完成IMU初始化RVIZ里点云是否静止不变然后手动控制机器人走一条直线确认地图轨迹和真值基本重合最后再让它在房间里绕圈观察回到起点后地图是否闭合。这三个阶段都过关这个仿真配置才算真正可用。另外如果你后续想在gazebo里做FAST_LIO 导航、FAST_LIO 探索这类项目建议在搭建机器人模型时就把雷达、IMU的话题命名和FAST_LIO的参数统一起来这样后面加导航、加路径规划时不用再回头改。真要遇到问题多看看ros2 topic info的输出多确认时间戳和外参而不是漫无目的地调卡尔曼滤波参数。