ROS2+Gazebo仿真Mid360与FAST-LIO：从建图到导航的完整实践

简介基于ROS2-Gazebo搭建的导航模拟包面向机器人自主导航的开发者与研究者以全向移动小车为载体集成Livox Mid360激光雷达与惯性测量单元IMU并结合FASTLIO算法实现在室内外复杂场景中的定位与导航仿真适合快速搭建导航仿真平台并对比多种定位方案。资源共194个文件、约90MB主要包含yaml/config参数配置、C与Python算法源码、sdf/world仿真模型、rviz可视化配置、launch启动文件、Dockerfile等覆盖环境建模、传感器配置、地面分割、障碍物检测到运动控制的完整流程文件类型以算法源码和仿真配置为主便于按模块检索和二次开发。目前已有358人学习下载。包内包含地面分割、障碍物检测等核心算法实现并选用RMUC/RMUL仿真地图贴近真实对抗赛场地便于验证算法在复杂障碍环境下的鲁棒性设计上也将参数调整放在显眼位置模拟调优后可较平滑地迁移到真实机器人降低从仿真到部署的转换成本。整体而言这是一份可用于科研预研、课程设计或备赛参考的完整导航模拟包。1. 这套 ROS2 模拟包为什么值得先跑通Mid360 FASTLIO 的真机预演做机器人的都懂这个场景算法在录好的 rosbag 上跑得很顺一上真机就这里怪那里飘。我这两年迭代室内导航方案最划算的一步就是把 Mid360 和 FASTLIO 这套激光惯性方案先搬进 ROS2 的 Gazebo 模拟包在仿真里把建图、定位、导航整条链路跑通才开始买真机传感器。标题里的 ROS2、Gazebo、Mid360、FASTLIO 放到一起本质上是一条完整的“仿真 → 建图 → 导航”流水线适合预算有限的实验室、做预研的工程团队也适合想快速验证导航参数的老手。下文按“为什么这样选 → 模拟包怎么搭 → FAST-LIO 怎么配 → 导航怎么收尾 → 坑在哪”的顺序展开配置可以直接抄。2. 把 Mid360 和 FAST-LIO 的角色盘顺一条从点云到位姿的导航链路2.1 非重复扫描在 Gazebo 里的还原难度Mid360 在导航里的地位不只是“看得远”它最值钱的是水平 360°、垂直约 59° 的大视场角以及非重复扫描特性。非重复扫描的意思是传感器每一帧的采样线束并不落在完全相同的角度上长期积分会让点云覆盖越来越密。这个特性对 FAST-LIO 这类基于连续帧配准的里程计特别友好帧与帧之间总有一些“新看到的”点约束质量比固定重复式扫描高不少。但 Gazebo 默认的传感器仿真并不懂什么叫“非重复”。它最常见的是gpu_ray固定水平角步长、固定垂直角步长每帧采出来的点云格式高度一致更像传统的重复式机械雷达。如果你不做任何处理FAST-LIO 在仿真里确实能跑但地图看起来会非常“假”墙角一条条放射线的痕迹特别规则局部地图密度也均匀得不对。原因是重复式点云在帧间配准时约束过强容易把噪声当成特征吃掉。所以在搭模拟包时我一般不会直接用 Gazebo 默认生成的laser_scan二维话题而是用 PointCloud2 输出并且把水平和垂直采样密度设置得接近真实 Mid360 的规格。另外一个常见做法是把livox_ros_driver2的仿真模式接进来它内部会按 Livox 的工作方式生成非重复点云。这个方案更真实代价是驱动编译和参数都要多花一点时间但久而久之你会发现这步省不了——如果传感器模型本身就错了后面所有导航参数调得再好都只是在给错误模型打补丁。2.2 FAST-LIO 作为“里程计正解”的选型理由导航栈最需要的是一个连续、低延迟、频率够高的位姿来源而不是一张“建完就再也不变”的地图。Cartographer 和 gmapping 也能做但它们要么重、要么极度依赖 2D 激光Mid360 的 3D 点云优势发挥不出来。FAST-LIO 走的是另一条路用 IMU 做运动先验再用迭代误差状态卡尔曼滤波器IESKF把原始点云配准到局部地图全程没有显式的特征提取环节功耗和计算量都控制得很好。这个选型理由在仿真里同样成立。Gazebo 里的 IMU 是干净得多的传感器没有真机那么多温漂和安装误差FAST-LIO 在这种环境下收敛速度会非常快。它默认输出Odometry、cloud_registered和点云地图直接给导航用。你会发现机器人走一圈rviz2 里的轨迹几乎没有毛刺这就是它“正解”的位置。2.3 话题与 TF 图谱谁发布、谁订阅、谁在导航里做决策在我习惯的方案里整条链路是这样分工的Gazebo 仿真插件发布/livox/lidar点云和/imu/data惯性数据FAST-LIO 订阅这两个话题输出/Odometry里程计、/cloud_registered点云地图以及odom - base_link的坐标变换。到了导航阶段map - odom这个变换由导航侧维护常见做法是先发一个静态map - odom因为仿真里没有真实定位漂移用 FAST-LIO 的里程计当导航位姿已经足够。组件发布/订阅话题坐标系角色用途Gazebo 雷达插件/livox/lidarlidar_link建图与避障观测Gazebo IMU 插件/imu/dataimu_linkFAST-LIO 惯性先验FAST-LIO订阅点云IMU发布/Odometry/cloud_registeredodom - base_link里程计与建图Nav2订阅/Odometry与点云map - odom规划与避障这套链路看着简单但实际搭的时候很多人会在“谁的 TF 该由谁发”上翻车。后面避坑章会单独说。3. 搭建 Gazebo 模拟包从 URDF 雷达插件到可启动的 bringup3.1 Humble 环境下依赖安装顺序与版本匹配如果你现在用的是 Ubuntu 22.04ROS2 版本基本就是 HumbleGazebo 会作为一条依赖一起装进来。最省事的顺序是先把 ROS2 Humble 装完然后一次性把导航和仿真相关包补齐。网上很多 ros2 菜鸟教程会让人先装 Gazebo 再装 ROS2结果两边的库版本不一致Gazebo 插件经常加载不出来VMware 里跑还会闪屏。这个坑我在用虚拟机时踩过后来直接改成在原生 Linux 或实体机里跑仿真省掉一大堆显示问题。依赖安装建议分三步走先装ros-humble-gazebo-ros-pkgs和ros-humble-robot-state-publisher再装ros-humble-nav2-bringup和ros-humble-nav2-map-server最后编译livox_ros_driver2和 FAST-LIO 源码。前两步用 apt 就能解决后两步必须源码编译因为它们的消息定义和接口版本经常跟着 ROS2 发版节奏走apt 里的老版本容易和 Humble 不兼容。编译时有个容易忽略的细节FAST-LIO 依赖的 PCL 版本、livox 驱动依赖的 message 类型都要和你的 ROS2 发行版一致。如果编译过程中报CustomMsg找不到多半是 livox 驱动没先编译、没先 source。养成“编译完一个包就source install/setup.bash再编下一个”的习惯能少走很多弯路。3.2 Mid360 雷达仿真插件写入 Xacro一份能跑的 URDF我现在把雷达挂在底盘顶部的lidar_link上Xacro 里给这个 link 挂一个gazebo传感器标签。下面是简化后可直接改到你自己 Xacro 里的写法!-- 在 lidar_link 上挂 Mid360 仿真传感器发布 PointCloud2 到 /livox/lidar -- gazebo referencelidar_link sensor namemid360 typegpu_ray pose0 0 0.2 0 0 0/pose update_rate20/update_rate visualizefalse/visualize plugin namemid360_plugin filenamelibgazebo_ros_ray_sensor.so ros namespace/livox/namespace remapping~/out:lidar/remapping /ros output_typePointCloud2/output_type frame_namelidar_link/frame_name min_range0.1/min_range max_range40.0/max_range horizontal_fov6.28319/horizontal_fov vertical_fov1.02974/vertical_fov horizontal_samples360/horizontal_samples vertical_samples24/vertical_samples /plugin /sensor /gazebo这份配置里几个关键参数值得说清楚。horizontal_fov是 6.28319 弧度即完整的 360°Mid360 的水平视场就是 360°不要随手写 90° 或 270°那会让 FAST-LIO 在转身时直接丢约束。vertical_fov用 1.02974 弧度约等于 59°但真实 Mid360 的垂直视场是 -7° 到 52°并不对称如果你的 Gazebo 插件支持vertical_fov_min和vertical_fov_max建议分开设置成-0.122和0.908弧度比统一给一个对称角更接近真机。horizontal_samples和vertical_samples控制每帧点云密度仿真里不必追平真机的 20 万点每秒360×24 已经够 FAST-LIO 用了密度太高反而会让 CPU 跑满。这里要提醒一句gpu_ray做出来的只是 Mid360 的“形”不是“神”。它没有非重复扫描的随机性。想彻底还原非重复特性就用 livox 驱动自带的 Gazebo 仿真模式替换掉这个插件那份配置基本把lidar_type、scan_pattern这些参数都封装好了模型效果更接近真机。我的建议是第一步先用gpu_ray把链路跑通等 Nav2 能正常导航了再换 livox 仿真模式把两个阶段分开排查别一上来就追求高保真。3.3 IMU 别只当附属品它决定 FAST-LIO 的收敛质量FAST-LIO 之所以比纯点云里程计稳就是因为它把 IMU 积分当运动先验。仿真里 IMU 通常是 Gazebo 插件生成的如果你完全不去配噪声参数出来的是零噪声理想 IMU那 FAST-LIO 初始收敛的确快但会让你误以为真机上也会这么顺。真机上 IMU 有零偏、有白噪声所以我习惯在模拟包里就把噪声调到接近真实器件量级让 FAST-LIO 的滤波器从一开始就面对“不那么完美”的 IMU。!-- 绑定在 base_link 上的 IMUname 要与 FAST-LIO 配置一致 -- gazebo referencebase_link sensor nameimu_sensor typeimu update_rate200/update_rate plugin nameimu_plugin filenamelibgazebo_ros_imu.so ros namespace/imu/namespace remapping~/out:data/remapping /ros frame_nameimu_link/frame_name initial_orientation_as_referencetrue/initial_orientation_as_reference /plugin /sensor /gazebo注意 IMU 话题发布为/imu/data这正好对应 FAST-LIO 配置里的imu_topic。另外frame_name要写imu_link别直接复用base_link否则你后面的 TF 树里会出现两个父节点共用同一个坐标系名rviz2 里表现为机器人模型和点云地图“对不上”的玄学错位。IMU 最好放在接近底盘中心的位置模拟包里的标准做法是让imu_link与base_link重合或者只有很小偏移我这个示例里就把frame_name设成imu_link并在 TF 里base_link - imu_link偏移写 0。3.4 一个 bringup 把 world、雷达、IMU 和控制器串起来模拟包最后要能一行命令启动干净。我一般用一个bringup.launch.py同时拉起 Gazebo world、机器人 URDF、robot_state_publisher、spawn_entity和底盘控制器。这样做的目的是把“启动顺序”固化成文件避免每次手动开一堆终端也方便其他人复现。# bringup.launch.py 的核心片段 from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): return LaunchDescription([ IncludeLaunchDescription( PythonLaunchDescriptionSource(src/mid360_nav_sim/launch/world.launch.py) ), IncludeLaunchDescription( PythonLaunchDescriptionSource(src/mid360_nav_sim/launch/robot.launch.py) ), ])这里有个顺序上的讲究robot.launch.py里会调用spawn_entity把机器人放进 Gazebo而 Gazebo 的物理引擎初始化需要一点时间。如果spawn_entity跑得太快会报“模型注入失败”。我常用的妥协办法是在world.launch.py里给 Gazebo 加一小段sleep再继续或者把spawn_entity的spawn_entity_timeout参数调大。很多菜鸟教程不会提这个但它几乎是你第一次启动模拟包必然遇到的一个坎。4. 跑通 FAST-LIO 建图配置文件、启动顺序和 rviz2 验收4.1 FAST-LIO 的 mid360 配置调整 voxel_size 和 point_filter_numFAST-LIO 源码里通常会带一份针对 Mid360 的配置文件你需要在启动前把话题名和几个关键参数确认一遍。仿真环境里我最常改的是voxel_size、point_filter_num和blind这三项common: lid_topic: /livox/lidar imu_topic: /imu/data time_sync_en: false point_filter_num: 3 feature_detection_en: false max_iteration: 2 preprocess: lidar_type: 1 scan_line: 24 blind: 0.3 voxel_size: 0.8point_filter_num表示每隔多少个点取一个送入匹配相当于抽稀。Gazebo 的gpu_ray点云没有真实雷达那么密我一般设 3也就是每 3 点取 1 点如果发现 CPU 占用高再往 5、6 调但地图边缘会变得更毛糙。blind是近场盲区半径Mid360 近场覆盖很好仿真里设 0.3 就够了别设成 1.0 以上否则机器人脚底下的小障碍全被滤掉。voxel_size是局部地图体素分辨率0.8 是比较平衡的室内走廊取值想要细节更清晰可以降到 0.5代价是计算量上涨仿真在低配机器上会明显掉帧。lidar_type: 1表示 Livox 类型对应 Mid360。这里容易踩坑的是如果你把 livox 驱动仿真模式接到的是另一个话题比如/livox/points更要提前确认lid_topic与实际发布话题完全一致否则 FAST-LIO 一直“等数据”rviz2 里什么也不出。4.2 启动顺序先原地静止三秒再移动FAST-LIO 这类紧耦合里程计在启动后需要一个初始化窗口。真机上的标准流程是开机后静止几秒让 IMU 零偏估计和初始姿态收敛仿真里虽然 IMU 干净但如果一启动就让机器人立刻转弯、加速后面的轨迹照样会飞。我每次跑建图都保持这样的终端顺序# 终端 1启动模拟包 ros2 launch mid360_nav_sim bringup.launch.py # 终端 2启动 FAST-LIO 建图 ros2 launch fastlio mid360_mapping.launch.py # 终端 3观察建图状态 ros2 run rviz2 rviz2 -d src/mid360_nav_sim/rviz/mapping.rviz启动完成后再等三到五秒。确认 rviz2 里出现缓慢累计的彩色点云后再用ros2 run teleop_twist_keyboard teleop_twist_keyboard手动遥控车辆或者直接用 rviz2 里的2D Goal让它自动巡游。不要在点云还没建立起来时就发/cmd_vel否则 FAST-LIO 会在“没有地图锚点”的情况下纯靠 IMU 积分几分钟内就会看到轨迹像喝了酒一样甩出去。这个三秒静止习惯我建议写进团队的建图 checklist因为它是免费的“后悔药”。4.3 rviz2 验收建图看频率看漂移看重复覆盖建图过程中我只看三个指标话题频率、位姿轨迹、地图重复覆盖。先看/Odometry的话题频率用ros2 topic hz /Odometry如果稳定在 20Hz 以上就说明整体负载可以低于 10Hz 就查是不是点云密度太高或者 CPU 瓶颈。再看 rviz2 里 FAST-LIO 发布的/path如果机器人转了一圈回来路径首尾能重合到差不多一个车道宽度以内说明没有明显累积漂移。最后是地图质量控制机器人沿走廊走“回”字形比如先走直线到头再横向换道让 Mid360 的大垂直视场把墙壁上部和下部都覆盖到。这里有个 gazebo 仿真 mid360 建图定位时的关键技巧如果只走一条直线再原路返回地图只有单条扫描带角落和墙面细节全部缺失导航时机器人在空旷处看到的地图很薄避障就会犹豫。走几趟交叉路线之后你再看/cloud_registered应该是连续、无明显锯齿的墙面。如果墙面上有规律性的一道道“空洞”说明gpu_ray的垂直采样不均匀回头调vertical_samples和垂直视场范围。5. 导航前必须排掉的 5 个坑点云、IMU、TF 与 costmap5.1 点云带刺或地图缺层现象rviz2 里点云朝四面八方伸出一根根“刺”或者一面墙中间多出一层游离点。 原因gpu_ray的垂直视场范围没设对点云落到了 Gazebo 模型的边缘上或者是雷达 link 与底盘模型发生了碰撞一部分射线被车体自身挡掉后形成了近场飞点。 解决先关掉可视化模型单独看点云。然后把vertical_fov_min和vertical_fov_max按 Mid360 的 -7° 和 52° 重设并把lidar_link的 z 轴提高到车体最高点之上。仿真里点云飞行物 80% 是模型自遮挡真机上同样成立。5.2 FAST-LIO 跑着跑着位姿起飞现象建图过了三五分钟轨迹突然朝一个方向偏出去点云地图开始“拉花”。 原因机器人没有静止初始化就开跑IMU 噪声设为零导致滤波器过于信任惯性预测或者voxel_size太大配准时约束不足。 解决停止运动等轨迹拉回正常再重新起步。把 IMU 噪声设置从零改成接近真实 IMU 的规格例如角速度噪声 0.01 量级、加速度噪声 0.05 量级。voxel_size调回 0.50.8不要超过 1.0。这一步看起来像玄学但它真的是我踩过最深的坑。5.3 TF 树缺了 map→odom导航必然“看得到图走不动”现象Nav2 起来了地图也显示了但是设置目标点后机器人不动或者规划路径一直报错。 原因导航栈要求存在map - odom - base_link的完整 TF 树。FAST-LIO 只发布odom - base_linkmap - odom没人管规划器认为机器人还没“落”到地图上。 解决在导航 launch 里显式发布静态变换map - odom位置和姿态都设为零。这样在仿真里等同于“导航位姿由 FAST-LIO 里程计直接提供”省掉 amcl 的配准环节。等以后上真机再把静态变换换成 amcl 或其它定位节点输出的动态变换。5.4 3D 点云进不了 costmap机器人在仿真里撞墙现象Nav2 正常规划机器人也走到目标点附近了但遇到临时摆放的箱子直接压过去不问不顾。 原因costmap 默认订阅的是二维laser_scan话题Mid360 发布的是 3D 点云两者话题类型和数据内容都对不上。 解决常见做法是把 Mid360 点云降维成二维scan用一个pointcloud_to_laserscan节点设置高度裁剪在 0.2~0.5 米之间投射成/scan喂给 Nav2。另一个做法是让 costmap 直接订阅 PointCloud2在 obstacle 参数里设置min_obstacle_height和max_obstacle_height这样能保留部分上层障碍信息比单纯转 2D 更符合 3D 雷达的定位。切记两套方案别同时开否则同一面墙会变成双层障碍轨迹规划会无谓地绕远路。5.5 时间戳与 DDS 抖动仿真里也会出现的玄学问题现象FAST-LIO 间歇性报“点云时间戳比最新里程计还旧”或者话题频率正常但地图一卡一卡。 原因Gazebo 使用仿真时间/clock而部分节点默认使用系统时间两者不同步。在虚拟化环境里ROS2 的 DDS 发现和数据传输有时也会出现几十毫秒的抖动。 解决启动命令里统一设置use_sim_timeTrue让所有节点都走/clock。FAST-LIO 配置里把time_sync_en先保持 false因为仿真环境下点云和 IMU 本来就是同一时刻生成的不需要跨传感器时间同步。这个话题是 ros2 和 DDS 相关问题的重灾区如果你在真实机器人上反而要反过来打开时间同步仿真和真机的差别就在这里。6. 把地图和里程计交给 Nav2验证一套可用的仿真导航6.1 导航落地的最小参数清单建图完成后用ros2 run nav2_map_server map_saver_cli -f map保存栅格地图然后建一个导航 launch里面只保留四个组件静态地图服务器、FAST-LIO 的里程计、静态map - odom变换、Nav2 导航服务器。在仿真验证阶段没必要急着接 amcl因为 FAST-LIO 足够稳。我对导航参数的要求只有一个机器人正前方两米出现障碍时costmap 的膨胀半径能明显体现且规划路径能绕开。达不到这个标准先回去检查避坑 5.4不要急着调运动学参数。6.2 三个验证方法回环重合、栅格对齐、目标导航验证项方法通过标准里程计漂移沿走廊走一个闭环回到起点终点与起点偏差小于 0.3m建图精度在 rviz2 里对比点云地图与栅格地图的墙线墙体轮廓误差不大于一个体素导航可用性在走廊两头各设一个目标点连续跑 10 次无碰撞、无规划失败第一个验证最省时间却是检验 FAST-LIO 配置是否合理的硬指标。仿真里没有风、没有打滑如果回环偏差还超过 0.3 米说明前面 4.2 节或者 5.2 节的哪一步做得不对。第二个验证关注的是地图质量栅格地图不是点云地图直接看灰色栅格是否和墙壁重合即可。第三个验证整个系统的最终可用性。6.3 最后一公里的建议我给这套方案收个尾。你会发现真正让你的导航参数有价值的不是最细的 costmap 参数而是前面的传感器模型、IMU 配置和 TF 是否像样。我的习惯是每一步改动只动一个变量先保点云干净再保里程计不飘最后才碰 Nav2 的膨胀半径和速度限制。养成这个顺序之后你拿到任何一台带 Mid360 的机器人都能迁移这套模拟包里的参数只不过把静态map - odom换回真实定位节点。希望这套从 Gazebo 模拟包到 FAST-LIO 建图再到 Nav2 导航的路线能帮你把“真机前先仿真”这个习惯坚持下去。本文还有配套的精品资源点击获取