在真机上跑过 FAST-LIO2 的朋友多少都经历过这样的场景Mid360 昨天还好好的今天一连上电就是点云断层IMU 温度一漂初始化飘出去几十米想去楼下车库复现一个回环场景结果真把车推下去绕了半小时内存里还是只有一条走廊。硬件连接、设备校准、场地调度、环境光照……随便一环出问题你就分不清到底是算法不行还是环境不行。这也是我后来把测试主战场从真机搬到 Gazebo 仿真的直接原因——我能在十分钟内重建一个可复现的测试环境把 FAST-LIO2 的收敛性能、参数鲁棒性和坐标系设计都压到极限去验证而不是把时间浪费在排查一根 USB 线缆上。这篇文章我会完整记录从零搭建一套基于 Gazebo ROS 的 Livox Mid360 仿真平台并跑通 FAST-LIO2 的全过程。内容包括环境选型、Mid360 非重复扫描特性在 Gazebo 里的建模方法、launch 文件的适配技巧以及我在调试中踩过的一批坑。文章默认读者了解 ROS 基础概念但即便你是刚装完 Ubuntu 的新手按步骤操作也能把平台跑起来。1. 为什么我坚持用 Gazebo 做 FAST-LIO2 的验证场很多人的第一个问题是FAST-LIO2 官方不是自带 rosbag 数据吗直接跑 bag 不香吗香但不够。bag 只能帮你验证算法能不能跑通不能帮你验证算法面对“新场景”时的行为。你没法改变 bag 里的场景布局没法换反射率的墙面没法让机器人走另一条轨迹。而仿真平台的意义恰恰在于——可重复、可参数化、可边界化。1.1 仿真要解决的真问题不只是“省一台雷达”先说一个很实际的计算成本。一块 Mid360 在 2024 年左右的流通价格还要小几千加上配套的 IMU 和工控机一套完整的 FAST-LIO2 真机测试平台没有一万下不来。Gazebo 用纯软件模拟零硬件损耗还能同时开多个机器人实例做多机联调——这在真机环境里是极其麻烦的事。更重要的是可重复性。FAST-LIO2 是紧耦合的激光惯性里程计它的初始化非常依赖 IMU 激励。真机上你把小车放在原地不动点云和 IMU 都在输出但算法死活不收敛那是因为重力对齐之后缺乏足够的加速度激励去收敛偏置。仿真里我可以设计一条正弦摆动轨迹保证初始化阶段 IMU 被充分激励也可以故意从静止开始观察算法在这种缺陷激励下的退化表现。这种“可控变量”的能力是回放 rosbag 给不了的。1.2 仿真平台的核心组成雷达模型、IMU 模型、环境模型在 Gazebo 里搭建这整套系统本质上是三块拼图传感器仿真用 Gazebo 插件模拟 Mid360 的非重复扫描点云同时输出 IMU 数据机器人模型用 URDF 把激光雷达、IMU、机体底盘组合成带正确坐标系变换的刚体环境模型用 sdf/world 文件构造室内或室外场景供激光雷达产生有价值的点云特征我见过不少人在这一步翻车雷达模型挂上了仿真里点云却是一个平面圆盘没有任何特征FAST-LIO2 根本起不来。原因是 Gazebo 默认的射线传感器是均匀扫描的旋转雷达模型跟 Livox 的非重复扫描模式有本质区别。后面我会专门讲怎么用官方插件解决这个问题。2. 版本组合与安装细节Ubuntu 20.04 下的 Noetic Gazebo 11版本选型是仿真平台搭建里决定成败的一步也是新手最容易忽略的一步。我在这个平台上线前先花了两天测试不同的 ros/gazebo 组合最终选择了 Ubuntu 20.04 ROS Noetic Gazebo 11。2.1 为什么没有选 Ubuntu 22.04 和 ROS 2如果你关注过 FAST-LIO2 的仓库会发现官方主分支明确支持 ROS 1Melodic/NoeticROS 2 版本分散在 livox_ros_driver2 和 FAST-LIO2 的适配分支里维护状态并不一致。FAST-LIO2 依赖 livox_ros_driver 的消息类型livox_ros_driver/CustomMsg这套消息在 ROS 1 生态里集成最顺畅官方 launch 文件也是按 ROS 1 写的。Ubuntu 22.04 只能装 ROS 2 Humble 或者自己编译 Noetic 源码后者在 Gazebo 插件编译时容易遇到 OpenGL 和 Qt 的兼容问题。Gazebo 11 在 Ubuntu 20.04 上是一等公民直接随 Noetic 安装不需要单独处理 Gazebo Garden 那套新架构。所以我的结论很直接为了跑通流程老老实实用 Noetic。提示如果你已经装了 Ubuntu 22.04并且不想重装系统可以用 Docker 跑一个 noetic 容器把 Gazebo GUI 通过 X11 转发出来。但这样性能损耗明显而且 3D 渲染偶尔会闪退。有条件还是建议单独装一台 Ubuntu 20.04 的机器或虚拟机。2.2 安装全流程笔记核心安装过程分几步这里给出我实际操作过的一整套命令。国内用户如果 apt 源慢记得先换阿里云或清华源。# 1. 更新系统源 sudo apt update sudo apt upgrade -y # 2. 设置 ROS 源清华镜像示例 sudo sh -c echo deb https://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu $(lsb_release -sc) main /etc/apt/sources.list.d/ros-latest.list sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654 # 3. 安装 ROS Noetic 完整版 sudo apt update sudo apt install ros-noetic-desktop-full -y # 4. 初始化 rosdep sudo rosdep init rosdep update # 5. 配置环境变量 echo source /opt/ros/noetic/setup.bash ~/.bashrc source ~/.bashrc装完后 Gazebo 11 会随desktop-full一起安装验证一下gazebo --version # 应该输出 Gazebo multi-robot simulator, version 11.x.x除此之外还需要安装编译 FAST-LIO2 所需的依赖库sudo apt install ros-noetic-pcl-ros ros-noetic-cv-bridge ros-noetic-image-transport -y sudo apt install libeigen3-dev libyaml-cpp-dev -y # Sophus 需要从源码编译 cd ~ git clone https://github.com/strasdat/Sophus.git cd Sophus mkdir build cd build cmake .. make -j$(nproc) sudo make install关于“鱼香ROS一键安装”我也试过它确实能省很多事尤其是装 ROS 本体时不用手动折腾 GPG key。但在模块化工作区管理和 Gazebo 插件编译上一键安装脚本不会帮你处理所以我更推荐手动装一遍至少踩过一次坑后你知道每一步在干嘛。3. Mid360 仿真建模的关键一步非重复扫描插件这是整个平台最核心的技术点。如果你只是想在 Gazebo 里挂一个“看起来像激光雷达”的东西用sensor typegpu_ray就够了。但如果你要跑 FAST-LIO2这个做法基本等于自杀——算法看到的是均匀旋转扫描的数据和真实 Livox 的扫描几何模式完全对不上退化、飘移是必然的。3.1 为什么不能用默认 Ray Sensor 替代 Mid360Livox Mid360 的硬件设计是棱镜式非重复扫描点云分布不是一条条均匀扫描线而是在视场内形成花瓣状的高密度覆盖时间累计后视场覆盖率逐渐趋于均匀。这个特性对 SLAM 算法有一个直接影响特征提取时非重复扫描对边缘和平面的采样密度更高能更稳定地提取角点。而 Gazebo 默认的 ray sensor 模拟的是旋转式机械雷达线束均匀分布点云特征形态完全不同。在 FAST-LIO2 的算法流程里LivoxLidar特征提取环节专门对 Livox 点云格式做了适配包括使用point_num做循环优化。如果喂进来的点云不是 Livox 格式虽然理论上也能跑但你在代码里需要额外转换效率也会受影响。3.2 使用官方 livox_laser_simulation 插件Livox 官方在 GitHub 上开源了一个仿真插件livox_laser_simulation专门解决这个问题。它实现了自定义 Gazebo 插件根据棱镜旋转模型实时生成非重复扫描点云并直接以livox_ros_driver/CustomMsg消息格式发布到 ROS 话题。这意味着从仿真节点出去的 Topic格式和真机 Livox 驱动输出完全一致FAST-LIO2 不需要任何改动就能订阅。安装过程如下cd ~/catkin_ws/src git clone https://github.com/Livox-SDK/livox_laser_simulation.git cd ~/catkin_ws catkin_make source ~/catkin_ws/devel/setup.bash注意编译这个插件需要确认你的 Gazebo 版本对应的开发库已安装。Noetic 自带的 Gazebo 11 对应的是libgazebo11-devsudo apt install libgazebo11-dev3.3 在 URDF 里挂载 Mid360 模型与 IMU有了插件就要把它挂到机器人的 URDF 上。URDF 文件是机器人模型的“骨骼”规定了每个部件的质量、惯量、坐标系和传感器挂载位置。下面是关键部分!-- 底盘 link作为承载基座 -- link namebase_link visual geometrybox size0.3 0.2 0.08//geometry origin rpy0 0 0 xyz0 0 0/ material namegray/ /visual collision geometrybox size0.3 0.2 0.08//geometry /collision inertial mass value4.0/ inertia ixx0.05 ixy0 ixz0 iyy0.05 iyz0 izz0.08/ /inertial /link !-- Mid360 传感器 link -- link namelivox_frame visual origin rpy0 0 0 xyz0 0 0.1/ geometrymesh filenamepackage://livox_description/meshes/mid360.dae scale0.001 0.001 0.001//geometry /visual /link joint namelivox_joint typefixed parent linkbase_link/ child linklivox_frame/ origin rpy0 0 0 xyz0 0 0.1/ /joint然后是 Gazebo 插件部分。这里我配置了 Livox 激光雷达插件和 IMU 插件gazebo referencelivox_frame plugin namelivox_laser filenameliblivox_laser_simulation.so topicName/livox/lidar/topicName frameIdlivox_frame/frameId updateRate10/updateRate pointCloudTopic/livox/lidar/pointcloud/pointCloudTopic laserCloudTopic/livox/lidar/laserCloudTopic imuTopic/livox/imu/imuTopic modelTypemid360/modelType /plugin /gazebo gazebo referencebase_link plugin nameimu_plugin filenamelibgazebo_ros_imu.so topicName/livox/imu/topicName frameNamelivox_frame/frameName updateRate200/updateRate noiseDensity0.0002/noiseDensity randomWalk0.00005/randomWalk alwaysOntrue/alwaysOn /plugin /gazebo这里的参数有几个讲究updateRateMid360 真机的出图频率是 10HzIMU 频率是 200Hz。FAST-LIO2 内部的状态估计器需要 IMU 高频数据来做传播点云低频做更新。我用 10Hz 雷达 200Hz IMU 是仿真环境里比较接近真机的配置。noiseDensity和randomWalk这两个参数控制 IMU 噪声。仿真环境默认什么都不加时 IMU 是理想传感器这会让 FAST-LIO2 的状态估计变得过于乐观掩盖真实场景里的退化问题。加上高斯噪声后更接近实战。modelTypelivox_laser_simulation插件支持 mid40、mid70、horizon 等多种型号mid360 是需要手动指定的参数。注意不同版本的 livox_laser_simulation 插件XML 标签名可能不同。比如某些分支使用topicName另一些分支使用laserTopic。编译前先打开源码里的livox_laser_simulation_plugin.cc确认标签定义避免 launch 后 topic 没输出还以为插件没编译成功。4. 构建测试世界并让仿真小车动起来传感器模型挂好了接下来需要把车放进一个能让激光雷达“看到东西”的环境里。FAST-LIO2 在空旷房间和结构化走廊里的表现差异巨大所以测试场景的设计要贴近你想验证的问题。4.1 搭建一个带纹理特征的室内场景Gazebo 的 world 文件可以用图形界面搭建也可以直接手写 SDF。我的做法是先用 Gazebo 图形界面摆上几面墙、几个货架模型然后把 world 保存下来再手工调整位置。以下是一个简化版的 world 文件结构sdf version1.6 world nametest_livox include urimodel://sun/uri /include include urimodel://ground_plane/uri /include include urimodel://construction_barrier/uri namebarrier_1/name pose1.0 0.5 0 0 0 1.57/pose /include include urimodel://cafe_table/uri nametable_1/name pose2.0 -1.0 0 0 0 0/pose /include include urimodel://bookshelf/uri nameshelf_1/name pose-1.5 1.0 0 0 0 0.5/pose /include /world /sdf在实际测试中我体会很深的一点是仿真环境的点云特征密度直接影响 FAST-LIO2 的初始化质量。如果你只放一个 ground_plane 和四面墙算法能收敛但退化方向比如沿走廊方向的表现不明显。为了测试退化场景我会故意构造一条长走廊观察算法在纯几何重复场景下的行为。这个可控性是仿真平台最有价值的地方。4.2 把 URDF 机器人模型写进 launch 文件场景有了机器人怎么放进去答案是写一个 launch 文件同时拉起 Gazebo、URDF 模型和 ROS 控制节点launch !-- 加载机器人 URDF -- param namerobot_description textfile$(find livox_description)/urdf/mid360_robot.urdf/ !-- 启动 Gazebo 并加载世界 -- include file$(find gazebo_ros)/launch/empty_world.launch arg nameworld_name value$(find livox_sim_worlds)/worlds/test_livox.world/ arg namepaused valuefalse/ arg namegui valuetrue/ /include !-- 在 Gazebo 中生成机器人 -- node namespawn_urdf pkggazebo_ros typespawn_model respawnfalse args-param robot_description -urdf -model mid360_robot -x 0.0 -y 0.0 -z 0.1/ /launch启动后你可以先在终端里确认话题是否正常发布rostopic list | grep livox # 应该看到 /livox/lidar 和 /livox/imu4.3 让小车动起来遥操作与轨迹控制FAST-LIO2 要求 IMU 在初始化阶段有足够的激励。如果你启动后把车停在原地雷达点云可能在 Rviz 里一直显示不出来那不是传感器坏了而是算法在等待 IMU 对齐。此时你需要让机器人运动起来。最简单的方案是用teleop_twist_keyboardsudo apt install ros-noetic-teleop-twist-keyboard rosrun teleop_twist_keyboard teleop_twist_keyboard.py然后在终端里按 i、j、k、l 控制机器人前进后退和转弯。实测下来z轴方向的小幅摆动对 IMU 初始化帮助最大建议先做一个“原地左右晃 20 秒”的动作再开始前进。如果你想要更可控的轨迹复现可以用gazebo_ros的set_model_state服务发送预设轨迹。我写过一个 Python 脚本按贝塞尔曲线路径移动机器人用来重复测试同一条轨迹下不同参数组合的表现。这个方法比键盘遥操作更可控尤其适合批量跑实验。5. FAST-LIO2 适配仿真环境launch 文件与主题对接Gazebo 和 URDF 都齐了接下来就是重头戏让 FAST-LIO2 在仿真数据上跑起来。这个环节最常见的坑是话题类型不匹配和时间戳问题我会把每一步怎么改拆开细说。5.1 编译 FAST-LIO2 的依赖准备先在工作空间里拉代码cd ~/catkin_ws/src git clone https://github.com/hku-mars/FAST_LIO.git cd ~/catkin_ws catkin_make如果编译报错提示找不到sophus/so3.hpp大概率是 Sophus 没有正确安装到系统路径或者版本不对。FAST-LIO2 要求 Sophus 使用fit3模板版本官方主线代码已经兼容直接sudo make install就行。还有一个高频报错是fatal error: pcl_conversions/pcl_conversions.h: No such file or directory这个只要装上ros-noetic-pcl-ros就可以解决不需要手动编译 PCL。5.2 修改 launch 文件适配仿真话题FAST-LIO2 自带的 launch 文件默认启动livox_ros_driver作为数据源但仿真环境下Livox 数据已经由 Gazebo 插件直接发布不需要再启动驱动节点。直接照搬原 launch 会导致端口冲突和时间源混乱。以下是我修改后的 launch 文件核心部分launch node pkgfast_lio typefastlio_mapping namefastlio_mapping outputscreen rosparam file$(find fast_lio)/config/livox_mid360.yaml / param namecommon/lid_topic value/livox/lidar/ param namecommon/imu_topic value/livox/imu/ /node /launch注意我完全没有启动livox_ros_driver节点而是直接把话题指向 Gazebo 插件生成的/livox/lidar。5.3 坐标系的坑从 base_link 到 livox_frame 的转换FAST-LIO2 的配置文件中body_T_lidar和body_T_imu矩阵定义了传感器在机器人本体坐标系下的外参。如果你的 URDF 里livox_frame相对于base_link有平移或旋转但配置文件中还保持默认值算法输出的点云地图会整体偏斜。检查坐标系最直接的方式是在 Rviz 里拉一个 TF 树查看base_link - livox_frame的关系是否与你配置的一致。我在 URDF 里把雷达放在z0.1m的位置所以配置文件里body_T_lidar的平移部分就要写0.0 0.0 0.1。IMU 同理。经验仿真环境下最容易犯的错误不是外参写错而是写错了还不自知。真机上你还能通过把雷达对着墙测距离来校准仿真里没人帮你验证。我后来专门写了一个脚本发布一个方向向量在 Rviz 里投影对比 TF 和点云的位置确保坐标系对齐。5.4 在线验证点云及时序启动 FAST-LIO2 后第一步不是看地图而是检查点云时间戳rostopic echo /livox/lidar/header/stamp如果时间戳全部为 0 或者跳变很大FAST-LIO2 的 IMU 传播和 LIDAR 更新会错位地图会扭曲甚至直接挂掉。Gazebo 插件默认发布的是仿真时间需要确保以下环境变量正确export ROS_MASTER_URIhttp://localhost:11311 export ROS_PACKAGE_PATH$ROS_PACKAGE_PATH:~/catkin_ws/src如果使用的是empty_world.launch仿真时钟默认启用/clock话题存在问题不大。比较隐蔽的坑是你在外部手动rosbag record后再播放bag 里的时钟源和 Gazebo 的仿真时钟冲突导致时间轴混乱。别问我怎么知道的。6. 调试中最常踩的五个问题以及我的排查链路这一节把所有我在调试过程中遇到的问题横着列一遍包括症状、原因、定位方法和修复操作。内容比较杂但每条都是实战总结。6.1 雷达话题有数据但 FAST-LIO2 不输出里程计这是仿真平台刚搭好时最容易碰到的问题。症状rostopic echo /livox/lidar有数据IMU 有数据但 FAST-LIO2 的/Odometry话题没有输出。大概率原因坐标变换不一致导致初始里程计发散代码抛出“Translation variance too large”之类的异常后自行退避。定位方法查看 FAST-LIO2 进程的完整日志找到std::cout输出的点云数量确认点云确实被正确处理。修复把配置文件里common/lid_topic和common/imu_topic改成仿真实际发布的话题名并把外参矩阵改成和 URDF 一致。6.2 点云在 Rviz 里显示为一条线而不是一片症状PointCloud2可视化时只有一行点没有形成雷达扫描面。原因livox_laser_simulation 插件的非重复扫描本质上是随时间累积的如果你只显示当前帧的原始点云它看起来确实像一条线或几片花瓣。这其实是“正确”的行为。解决不用改任何东西。确认 FAST-LIO2 输出的是累积后的地图cloud_registered而不是原始点云。在 Rviz 里选择/cloud_registered话题显示就行。6.3 IMU 数据频率太低导致初始化失败症状启动后 FAST-LIO2 一直停在“wait for imu”阶段或者初始化后很快漂移。原因Gazebo 里 IMU 插件的updateRate设成 10Hz 甚至更低但 FAST-LIO2 期望的 IMU 频率在 100Hz 以上。IMU 频率太低时状态传播阶段的积分误差迅速积累。修复把 IMU 插件的updateRate改成 200并确认rostopic hz /livox/imu输出频率在 190~210 之间。6.4 仿真环境下点云对墙面的“穿透”问题症状机器人靠近墙面时地图上出现墙面背后的几何噪点。原因Gazebo 的自带碰撞检测和传感器射线在某些边界条件下会漏检尤其是模型合并了多个 mesh 时。解决把墙体和障碍物模型从mesh改为简单几何体box/cylinder射线检测精度会显著提升。视觉上略粗糙但对 SLAM 仿真足够。6.5 多机同时仿真时的 TF 冲突症状开了两个机器人实例第二个机器人的 TF 树和第一个冲突地图错乱。原因每个机器人默认都叫base_link和livox_frame同一个 ROS 图上名冲突。解决给每个机器人的坐标系加命名空间前缀或者在 launch 里设置tf_prefix。FAST-LIO2 的订阅话题也要对应改成各自的/robot1/livox/lidar。提示在做的过程中建议全过程用一个脚本记录所有关键参数包括 URDF 外参、IMU 噪声、雷达频率、场景模型坐标。这样每次改动后如果算法行为有变化能快速定位是哪一项参数引起的变化。批量实验时这几乎是必备的。最后一点实操心得这篇文章从头到尾记录了我在 Gazebo 里复现 FAST-LIO2 测试环境的完整思路。如果说有什么是比步骤本身更值得你带走的那就是仿真平台不是“真机的替代品”而是一个让你能精确控制变量的实验装置。真机上你无法让同一堵墙的反射率改变两次但仿真里一句话就能做到真机上你无法让 IMU 在一条轨迹上产生完全一致的噪声序列但仿真可以。在多次反复调试之后我的日常流程已经变成先在仿真里快速验证算法和参数可行性再带着这个基准去真机做对标。我自己在实际操作中最大的收益并不是省了多少硬件费用而是建立了一套能随时重建、随时修改的实验闭环。希望这套流程能帮你省下我当初走过的弯路。