1. 项目缘起与整体设计思路宇树Go2这台四足机器狗玩过的人都知道它在运动控制层面已经相当成熟步态稳定、越障能力也不错。但原厂配置的感知方案主要依赖深度相机和超声波做SLAM建图和自主导航时遇到玻璃幕墙、强光直射、远距离稀疏障碍物这些场景深度相机的短板就暴露得很明显。我这次做的事情就是把Livox Mid360这款3D激光雷达集成到Go2的仿真环境里让它在Gazebo中真正跑起来并且把整套感知和导航链路调通。为什么选Mid360而不是别的雷达这里有几个很实际的考量。Mid360是非重复扫描体制等效线束密度高对远处小物体的检出率比传统机械式雷达好不少它的FOV是360度水平加59度垂直装在机器狗背部基本没有盲区体积小、重量轻对Go2这种本身就讲究负载平衡的平台来说很关键。另外它的点云数据量适中在仿真环境里不会把CPU吃满这对后续做实时导航很重要。整个项目的核心目标可以拆成三块第一在Gazebo仿真中正确加载Go2模型并挂载Mid360第二让雷达点云数据能够被Nav2导航栈正常消费实现基于3D雷达的自主导航第三针对仿真环境做性能优化保证整个系统在普通开发机上也能流畅运行。这三块缺一不可很多人卡在第一步模型加载上也有人点云出来了但导航跑不通还有人勉强跑通了但帧率低到没法用。适合谁来参考这篇内容如果你已经玩过Go2的基础仿真对ROS2和Gazebo有基本了解想进一步做感知层面的进阶那这篇就是写给你的。如果你完全没接触过ROS2建议先把基础的环境搭建和话题通信搞清楚再来看不然中间很多操作会一头雾水。提示整个项目基于ROS2 Humble和Gazebo Classic 11如果你用的是Gazebo Ignition或者ROS2其他版本部分配置需要调整我会在关键位置标注差异。2. 仿真环境搭建与雷达模型集成2.1 基础环境确认与依赖梳理动手之前先把环境理清楚这一步偷懒后面会加倍还回来。我用的组合是Ubuntu 22.04加ROS2 HumbleGazebo用的是Classic 11这是目前和Go2官方仿真包兼容性最好的搭配。你需要确认几个关键包已经装好gazebo_ros、gazebo_ros_pkgs、robot_state_publisher、xacro还有livox_laser_simulation这个专门做Livox雷达仿真的包。安装Livox仿真包的时候有个坑要注意官方仓库里的版本更新比较慢建议直接从源码编译。克隆下来之后注意检查它的package.xml里依赖的Gazebo版本如果是给Ignition写的你需要手动改回Classic的接口。我当时的做法是fork一份把插件里的ignition::命名空间全部替换成gazebo::重新编译就通了。Go2的仿真包来源有几个我用的是Unitree官方提供的unitree_ros2仓库里的描述文件。这个包里有完整的URDF和mesh文件直接拿来用就行。但要注意官方URDF里默认挂的是深度相机你需要把那段相机插件删掉或者注释掉换成雷达的link和joint。2.2 Mid360的URDF集成细节把Mid360装到Go2身上不是简单加个link就完事。首先要确定安装位置我选择装在背部中央偏前的位置高度大概在机身顶部往上5厘米。这个位置的好处是雷达的垂直FOV能覆盖到狗头前方和身体两侧同时不会被自己的腿遮挡。在URDF里你需要定义一个livox_mid360的link然后通过fixed joint连接到base_link或者trunk上。这里有个细节Mid360的实际坐标系原点和它的光学中心有偏移如果你直接拿官方给的CAD尺寸去设点云会出现系统性偏移。我的做法是在joint的origin里加一个微调具体数值根据实际点云和仿真环境的对齐情况来定一般z轴方向要往下调2到3厘米。雷达的Gazebo插件配置是核心。livox_laser_simulation包提供了一个livox_points_plugin你需要配置几个关键参数samples决定每帧点数downsample控制降采样比例csv_file_path指向Mid360的非重复扫描模式文件。这个CSV文件定义了每一帧激光的发射角度序列是Mid360仿真逼真度的关键。如果你随便拿一个机械式雷达的配置去套点云图案会完全不对。plugin namelivox_plugin filenameliblivox_points_plugin.so samples24000/samples downsample1/downsample csv_file_path$(find livox_laser_simulation)/scan_mode/mid360.csv/csv_file_path visualizetrue/visualize update_rate10/update_rate frameNamelivox_frame/frameName topicName/livox/lidar/topicName /plugin这里samples设24000是我实测下来的平衡点。设太高比如48000点云确实更密但Gazebo的物理线程会被拖慢整个仿真帧率掉到0.5以下。设太低比如8000远处障碍物的轮廓就糊了Nav2的代价地图会频繁出现空洞。10Hz的更新率对导航来说够用Mid360实机也是10Hz保持一致比较好。2.3 点云格式转换与话题桥接Livox仿真插件默认发布的是PointCloud2格式但它的字段布局和标准PointCloud2略有不同特别是intensity字段的位置。Nav2的代价地图层默认用的是PointCloud2但如果你直接用可能会遇到点云无法正确投影到代价地图的问题。我的做法是加一个pointcloud_to_laserscan的转换节点把3D点云压成2D激光扫描这样Nav2的obstacle_layer就能直接消费。但这里有个取舍压成2D会丢失高度信息对于机器狗来说低矮障碍物和悬空障碍物的区分就没了。所以我又额外配置了一个voxel_layer直接吃3D点云做三维代价地图。voxel_layer: plugin: nav2_costmap_2d::VoxelLayer enabled: true publish_voxel_map: true origin_z: 0.0 z_resolution: 0.05 z_voxels: 16 max_obstacle_height: 2.0 observation_sources: scan lidar lidar: topic: /livox/lidar data_type: PointCloud2 marking: true clearing: true min_obstacle_height: 0.1 max_obstacle_height: 1.5z_voxels设16z_resolution设0.05意味着垂直方向覆盖0.8米。对Go2来说这个高度范围覆盖了从地面到狗头以上的空间足够用了。max_obstacle_height设2.0是留余量防止高处障碍物被忽略。注意如果你发现代价地图里障碍物闪烁或者时有时无大概率是clearing参数的问题。3D雷达的 clearing 需要配合射线追踪Gazebo仿真里射线追踪的计算量不小可以适当降低update_rate来缓解。3. Nav2导航栈的3D雷达适配与调参3.1 导航栈整体架构调整Nav2默认的配置是给2D激光雷达设计的直接拿来跑3D雷达会有几个不兼容的地方。首先是obstacle_layer的observation_sources只认LaserScan你需要改成PointCloud2或者加voxel_layer。其次是全局代价地图的resolution默认0.05米对3D点云来说太细了会导致代价地图更新极慢。我改成0.1米牺牲一点精度换来了三倍以上的更新速度。局部代价地图我用的是voxel_layer加inflation_layer的组合。voxel_layer负责把3D点云转成三维栅格inflation_layer负责给障碍物加膨胀半径。膨胀半径设0.3米这是Go2机身宽度的一半多一点保证狗身不会蹭到障碍物。全局代价地图我用的是static_layer加obstacle_layerobstacle_layer的observation_sources指向压缩后的2D激光。这样全局规划用2D信息做长距离路径搜索局部规划用3D信息做实时避障各取所长。3.2 关键参数计算与实测调整voxel_layer的z_voxels和z_resolution需要根据实际场景算。假设你的场景里有高度0.5米的桌子和高度1.2米的架子那你的垂直覆盖至少要到1.5米。z_voxels乘以z_resolution就是覆盖高度。我设16乘0.05等于0.8米后来发现不够改成了20乘0.08等于1.6米。这样计算量反而下降了因为z_resolution变大意味着体素数量减少。max_obstacle_height和min_obstacle_height的设定也有讲究。min_obstacle_height设0.1米是为了过滤掉地面噪点。Gazebo仿真里地面反射有时候会产生虚假点云设太低会把地面当成障碍物。max_obstacle_height设1.5米是因为Go2身高大概0.4米加上雷达安装高度0.5米1.5米以上的障碍物对它的运动影响很小可以忽略以节省计算。局部规划器我用的是DWB但把critics列表里的ObstacleFootprint权重调高了。因为3D点云提供的障碍物信息比2D更丰富DWB的轨迹评分能更准确地避开悬空障碍。实测下来在有桌子的场景里2D雷达会让Go2直接往桌子底下钻3D雷达就能识别出桌面高度不够提前绕开。3.3 仿真环境下的性能瓶颈定位跑起来之后第一件事是看CPU占用。我在一台i7-10700加32G内存的机器上跑Gazebo物理线程占两个核雷达插件占一个核Nav2的代价地图更新占一个半核加起来快五个核了。如果机器配置低一点帧率会掉得很厉害。用top和htop看下来最大的瓶颈在voxel_layer的更新。每帧24000个点每个点都要做体素索引计算这个计算量是O(n)的n是点数。我的优化手段是加降采样在pointcloud_to_laserscan之前先过一个pcl::VoxelGrid把点云降到6000点左右。降采样后的点云虽然稀疏了但障碍物的轮廓还在导航效果没有明显下降。另一个瓶颈是Gazebo的渲染线程。如果你开了visualizeGazebo要把点云渲染出来这个很吃GPU。我的建议是调试阶段开正式跑导航的时候关掉用RViz看就行。RViz的渲染效率比Gazebo内置的高不少。# 降采样节点配置示例 pcl_voxel_grid: leaf_size: 0.05 input_topic: /livox/lidar output_topic: /livox/lidar_downsampledleaf_size设0.05米意味着5厘米见方的体素里只保留一个点。这个尺寸对Go2的避障来说足够了它的机身宽度就有30多厘米5厘米的精度完全够用。4. 实操全流程与关键环节记录4.1 从零启动的完整步骤第一步启动Gazebo并加载Go2模型。命令是ros2 launch unitree_go2_sim go2_gazebo.launch.py这个launch文件里会加载URDF、启动robot_state_publisher、生成Gazebo世界。等Gazebo窗口出来看到Go2站在地面上说明基础环境没问题。第二步确认雷达话题。用ros2 topic list看有没有/livox/lidar用ros2 topic hz /livox/lidar看频率是不是10Hz左右。如果话题没出来检查URDF里插件配置的filename路径对不对liblivox_points_plugin.so这个文件在livox_laser_simulation的lib目录下编译后才有。第三步启动点云转换和降采样。我写了一个launch文件把这些节点串起来包括pointcloud_to_laserscan、pcl_voxel_grid、还有tf2_ros的静态变换发布器。静态变换是把livox_frame和base_link的关系固定下来Nav2需要这个变换才能把点云投影到代价地图。第四步启动Nav2。用ros2 launch nav2_bringup navigation_launch.py加上自定义的参数文件。参数文件里重点改local_costmap和global_costmap的配置把voxel_layer加进去把obstacle_layer的observation_sources改成PointCloud2。第五步在RViz里设初始位置和目标点看Go2能不能自己走过去。第一次跑大概率会出问题别急看下面的排查部分。4.2 点云与代价地图的对齐验证点云和代价地图对不齐是新手最容易遇到的问题。表现是RViz里点云显示的位置和Gazebo里障碍物的位置有偏移或者点云的高度不对明明在地面上的点云显示在半空中。排查方法在RViz里同时显示/livox/lidar点云和/local_costmap/costmap代价地图看障碍物的轮廓是否重合。如果不重合先检查tf树用ros2 run tf2_tools view_frames生成tf树图看livox_frame到base_link的变换对不对。然后检查pointcloud_to_laserscan的target_frame参数这个参数决定了点云投影到哪个坐标系。我遇到过一次点云整体偏高0.3米的问题查了半天发现是URDF里joint的origin写错了z轴多加了0.3。这种问题没有捷径就是对着Gazebo里的模型和RViz里的点云一点点对。4.3 导航效果实测与调优记录第一次跑通导航之后我做了几组对比测试。场景是一个10米乘10米的房间里面放了几个不同高度的障碍物一个0.3米高的矮箱子、一个0.8米高的桌子、一个1.5米高的架子。用2D激光的方案Go2会直接往桌子底下钻因为2D激光只扫到一个平面桌子腿之间的空隙被当成可通行区域。用3D雷达加voxel_layer的方案Go2能识别出桌面高度不够提前绕开。但代价是路径规划时间变长了从原来的0.5秒变成了1.2秒。这个延迟在仿真里可以接受实机上可能需要进一步优化。速度方面我把max_vel_x从0.5降到0.3max_vel_theta从1.0降到0.6。降速之后导航成功率明显提升因为3D点云的处理延迟导致局部规划器的反应比2D慢降速给了它更多反应时间。提示如果你发现Go2在导航过程中频繁急停或者原地打转大概率是局部代价地图的更新频率跟不上。把update_frequency从5Hz提到10Hz同时把voxel_layer的z_voxels降下来能缓解这个问题。5. 常见问题排查与性能优化心得5.1 雷达点云不显示或显示异常点云完全不显示先看Gazebo里有没有雷达的可视化。如果Gazebo里也没有说明插件没加载成功。检查GAZEBO_PLUGIN_PATH环境变量有没有包含livox_laser_simulation的lib目录。用echo $GAZEBO_PLUGIN_PATH确认。点云显示但形状不对比如是一个圆环而不是Mid360特有的花瓣状图案说明CSV文件没加载对。检查csv_file_path的路径确保mid360.csv文件存在且格式正确。这个文件里每一行是一个激光发射的角度和时间偏移格式错了插件会静默失败。点云有但断断续续频率不稳定通常是Gazebo的实时率RTF太低了。在Gazebo窗口右下角看RTF值如果低于0.5说明仿真跑得比真实时间慢一倍以上。解决办法是降低物理更新频率在world文件里把max_step_size从0.001改成0.002或者减少场景里的物体数量。5.2 Nav2代价地图更新缓慢代价地图更新慢的表现是RViz里代价地图的刷新明显滞后于点云或者Go2已经走到障碍物前面了代价地图才更新出来。这个问题在3D雷达方案里很常见因为voxel_layer的计算量比obstacle_layer大一个数量级。优化手段有几个按效果排序第一降采样点云把24000点降到6000点计算量直接降到四分之一第二降低voxel_layer的update_frequency从10Hz降到5Hz代价地图更新慢一点但导航还能用第三增大z_resolution从0.05改成0.1体素数量减半第四把publish_voxel_map关掉这个选项会把三维体素地图发布出来很吃带宽和CPU。我最后的配置是点云降采样到6000点update_frequency设8Hzz_resolution设0.08publish_voxel_map关掉。在这个配置下i7-10700上CPU占用大概3.5个核RTF能保持在0.8以上导航流畅度可以接受。5.3 导航过程中机器人震荡或卡死震荡的表现是Go2在原地左右摇摆或者走到某个位置就不动了。先看局部代价地图如果障碍物膨胀层把可通行区域完全堵死了机器人就会卡死。把inflation_radius从0.3降到0.25试试或者把cost_scaling_factor调大让膨胀代价衰减得更快。另一个原因是DWB的轨迹评分参数不合适。ObstacleFootprint的scale设太高机器人会过度保守稍微有点障碍物就不敢走。我把它从1.0降到0.5同时把PathAlign的scale从32降到24机器人的行为就自然多了。还有一种情况是TF变换超时。3D点云的数据量大TF变换的计算偶尔会超时导致局部规划器拿不到最新的机器人位姿。解决办法是增大transform_tolerance从0.1秒改成0.3秒给TF计算留更多余量。5.4 仿真性能优化的几个实用技巧Gazebo仿真本身就很吃资源加上3D雷达和Nav2普通开发机很容易跑不动。除了上面说的降采样和降频还有几个技巧很实用。把Gazebo的渲染引擎从OGRE换成OGRE2在某些显卡上性能更好。在world文件里设render_engineogre2/render_engine就行。但要注意OGRE2对某些mesh格式的支持不如OGRE如果模型显示异常就换回来。关掉Gazebo的阴影和雾效这两个渲染特性很吃GPU。在world文件的scene标签里把shadows设成falsefog的type设成none。如果不需要看Gazebo的3D视图可以用gzserver单独启动服务端不启动gzclient。这样能省下一个GPU渲染线程的资源。命令是gzserver --verbose加上你的world文件。注意用gzserver无头模式跑的时候RViz里的点云显示可能会变慢因为点云数据要通过网络传输。如果RViz和Gazebo在同一台机器上用localhost通信延迟可以忽略。5.5 从仿真到实机的迁移注意事项仿真调通了不代表实机就能直接用。Mid360实机的点云噪声比仿真大地面反射、雨雾天气、强光环境都会产生噪点。实机上需要加一个pcl::StatisticalOutlierRemoval滤波器把离群点去掉。实机的TF变换需要重新标定仿真里的安装位置和实机不可能完全一致。用ros2 run tf2_tools view_frames确认实机的tf树然后手动调整livox_frame到base_link的变换。Nav2的参数在实机上要重新调。实机的运动学特性和仿真有差异max_vel_x和max_vel_theta要按实机的能力设。我的经验是实机速度先设仿真的一半跑稳了再慢慢往上加。最后再分享一个小技巧在仿真里调参数的时候用ros2 param set动态改不用重启节点。比如ros2 param set /local_costmap/local_costmap inflation_layer.inflation_radius 0.25改完立刻生效在RViz里就能看到效果。调好了再写回参数文件效率高很多。