基于ROS 2与Gazebo为Unitree Go2四足机器人集成SLAM与Nav2自主导航

1. 项目缘起为什么要在Gazebo里折腾一只“机器狗”如果你和我一样对足式机器人特别是像Unitree Go2这样灵活的四足机器人着迷但又苦于没有一台价值不菲的实体机器来上手实验那么Gazebo仿真环境就是你的最佳“平替”。最近我在GitHub上发现了一个宝藏项目——unitree-go2-ros2它成功地将Go2的模型、CHAMP控制器以及ROS 2 Humble环境整合到了一起。这个项目最吸引我的地方在于它已经完成了从模型加载、Gazebo仿真到键盘遥控行走的基础闭环但官方README里明确标注了两个未完成项SLAM和Nav2导航集成。这不就是一块绝佳的“技术试验田”吗想象一下你有一台虚拟的、完全听你指挥的Go2你可以在一个完全可控的仿真世界里为它赋予“眼睛”激光雷达和“大脑”SLAM与导航算法让它学会自己建图、规划路径、自主走到目标点。这个过程不仅成本为零还能让你深入理解从传感器数据融合到运动控制的完整机器人自主导航链路。对于学生、研究者或是像我这样的机器人爱好者来说这比单纯看论文或文档要直观和深刻得多。所以我决定基于这个开源项目把SLAM和导航这两个“TODO”给填上并记录下完整的实现过程与踩坑心得。2. 环境准备搭建你的虚拟机器人实验室在开始让Go2“认路”之前我们需要一个稳定、功能齐全的仿真开发环境。这个环境就像你的虚拟机器人实验室所有工具和依赖都必须提前备好。2.1 基础系统与ROS 2环境项目明确要求Ubuntu 22.04和ROS 2 Humble。这是必须严格遵守的版本匹配因为ROS 2不同发行版之间的API和包依赖可能存在不兼容。如果你用的是其他版本强烈建议使用虚拟机或Docker创建一个纯净的Humble环境。安装ROS 2 Humble的步骤官方文档很详细这里不再赘述。安装完成后务必确认核心组件工作正常source /opt/ros/humble/setup.bash ros2 doctorros2 doctor命令会检查你的ROS 2环境健康状况任何警告或错误都需要在继续之前解决。2.2 克隆与构建项目源码接下来我们按照项目README的指引把unitree-go2-ros2项目及其依赖拉取到本地。首先创建一个工作空间并进入src目录mkdir -p ~/go2_ws/src cd ~/go2_ws/src然后克隆项目仓库git clone https://github.com/anujjain-dev/unitree-go2-ros2.git项目依赖了一些ROS 2包我们需要使用rosdep工具自动安装它们。rosdep是ROS的依赖管理工具它能根据项目中的package.xml文件解析并安装系统依赖。cd ~/go2_ws sudo apt update sudo apt install -y python3-rosdep sudo rosdep init # 如果之前没初始化过 rosdep update rosdep install --from-paths src --ignore-src -r -y注意rosdep install命令中的-r递归和-y自动确认参数很重要可以避免安装过程中频繁的确认提示。如果遇到网络问题导致某些依赖下载失败可以多尝试几次或者根据错误信息手动apt install对应的包。依赖安装完成后使用colcon构建工作空间。colcon是ROS 2的构建工具类似于Catkin。cd ~/go2_ws colcon build --symlink-install--symlink-install参数会创建符号链接这样你在修改源码后无需重新install直接生效非常适合开发阶段。构建成功后别忘了“激活”这个工作空间的环境source ~/go2_ws/install/setup.bash为了方便你可以把这行命令添加到~/.bashrc文件中这样每次打开新终端都会自动source。2.3 安装额外的Gazebo与导航相关包基础项目提供了Gazebo仿真和遥控但要实现SLAM和导航我们还需要几个关键的ROS 2功能包。导航栈核心包nav2是ROS 2的官方导航系统包含了地图服务器、定位、路径规划、控制器等全套组件。sudo apt install ros-humble-nav2-bringup ros-humble-navigation2SLAM工具箱我们将使用slam_toolbox这是一个功能强大且高效的激光SLAM算法包支持在线和离线建图。sudo apt install ros-humble-slam-toolboxGazebo模型与插件确保Gazebo能正确加载世界和传感器模型。sudo apt install ros-humble-gazebo-ros-pkgs ros-humble-gazebo-ros2-control安装完这些你的虚拟实验室就初步搭建完成了。接下来我们先验证一下基础功能是否正常。3. 基础功能验证让Go2在Gazebo里“走两步”在添加复杂功能前确保基础仿真和遥控工作正常至关重要。这能帮我们排除环境配置问题把精力集中在SLAM和导航本身。3.1 启动Gazebo仿真环境项目提供了几个启动文件。我们从一个简单的、带2D激光雷达的配置开始因为后续的SLAM主要依赖2D激光数据。根据README我们需要先修改一个配置文件。进入机器人描述文件目录cd ~/go2_ws/src/unitree-go2-ros2/robots/description/go2_description/xacro找到robot_VLP.xacro文件用文本编辑器打开。找到关于激光雷达的包含语句按照注释说明进行操作!-- 注释掉Velodyne 3D雷达 -- !-- xacro:include filename$(find go2_description)/xacro/velodyne.xacro/ -- !-- 取消注释2D激光雷达 -- xacro:include filename$(find go2_description)/xacro/laser.xacro/保存文件后需要重新编译工作空间以使修改生效cd ~/go2_ws colcon build --packages-select go2_description source install/setup.bash现在启动带2D激光雷达的Gazebo仿真ros2 launch go2_config gazebo_velodyne.launch.py如果一切顺利Gazebo客户端会启动并加载一个包含Go2机器人的空世界。你应该能看到Go2模型以及附着在其上的一个圆柱体状的2D激光雷达通常是Hokuyo的模型。同时在终端里会滚动大量启动信息包括加载URDF、启动控制器管理器、关节状态发布器等。请耐心等待所有节点启动完毕直到终端输出趋于稳定没有明显的红色错误信息。3.2 使用RVIZ可视化与键盘遥控新开一个终端同样先source工作空间环境然后启动RVIZ来可视化机器人状态和传感器数据source ~/go2_ws/install/setup.bash ros2 launch go2_config gazebo_velodyne.launch.py rviz:true这个命令会同时启动Gazebo和RVIZ。在RVIZ中你需要手动添加一些显示项来查看数据点击左下角的Add按钮。选择By topic标签页找到/scan话题添加一个LaserScan显示。这样你就能看到2D激光雷达扫描出的点云线了。同样可以添加TF和RobotModel来查看机器人的坐标变换和模型。再新开一个终端启动键盘遥控节点source ~/go2_ws/install/setup.bash ros2 run teleop_twist_keyboard teleop_twist_keyboard按照终端提示i前进,后退j左转l右转等你就可以遥控Gazebo里的Go2行走了。同时在RVIZ中你应该能看到激光扫描线随着机器人的移动而动态更新。这一步的成功至关重要它证明了Gazebo物理仿真和ROS 2控制接口工作正常。机器人的关节控制器CHAMP能正确接收速度指令并生成步态。激光雷达传感器插件能正常发布/scan话题数据。如果遥控时机器人不动或者RVIZ里看不到激光数据请回头检查Gazebo启动终端的错误信息常见问题包括控制器配置错误、关节名称不匹配或传感器插件加载失败。4. 集成SLAM工具箱为Go2装上“绘图”能力SLAM同步定位与建图是机器人自主导航的第一步。它的目标是让机器人在未知环境中移动时一边估算自己的位置定位一边逐步构建出环境的地图。我们使用slam_toolbox来实现这个功能。4.1 配置与启动SLAM节点slam_toolbox提供了多种模式的启动文件对于在线建图我们通常使用online_async_launch.py。但直接使用默认配置可能不适合我们的机器人我们需要为其创建一个自定义的启动文件或参数文件。首先在工作空间内创建一个新的功能包来管理我们的SLAM和导航配置是个好习惯但为了快速验证我们可以直接修改或复制现有的启动配置。更清晰的做法是在项目目录下创建一个新的启动文件。例如在go2_config包目录下~/go2_ws/src/unitree-go2-ros2/go2_config/launch/新建一个名为go2_slam_launch.py的文件。# go2_slam_launch.py from launch import LaunchDescription from launch.actions import IncludeLaunchDescription from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import PathJoinSubstitution from launch_ros.substitutions import FindPackageShare def generate_launch_description(): # 启动Gazebo仿真环境 gazebo_launch IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare(go2_config), launch, gazebo_velodyne.launch.py ]) ]), launch_arguments{rviz: false}.items() # 我们稍后单独启动RVIZ ) # 启动slam_toolbox的在线异步建图节点 slam_toolbox_launch IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare(slam_toolbox), launch, online_async_launch.py ]) ]), launch_arguments{ params_file: PathJoinSubstitution([ FindPackageShare(go2_config), config, slam_toolbox_params.yaml ]), use_sim_time: true # 在仿真中必须使用仿真时间 }.items() ) return LaunchDescription([ gazebo_launch, slam_toolbox_launch, ])这个启动文件做了两件事1) 启动我们之前验证过的Gazebo仿真2) 启动slam_toolbox节点。注意use_sim_time参数被设置为true这在Gazebo仿真中是必须的它告诉ROS 2节点使用/clock话题发布的仿真时间而不是系统实时。接下来我们需要创建slam_toolbox的参数配置文件。在go2_config包下创建config/slam_toolbox_params.yaml。# slam_toolbox_params.yaml slam_toolbox: ros__parameters: # 基础参数 odom_frame: odom map_frame: map base_frame: base_link scan_topic: /scan # Go2的2D激光雷达话题 mode: mapping # 建图模式 # 地图分辨率 (meters per pixel) resolution: 0.05 # 优化与匹配参数可根据实际情况调整 max_laser_range: 12.0 # 激光最大有效距离 minimum_time_interval: 0.5 # 处理扫描数据的最小时间间隔 transform_timeout: 0.2 # 坐标变换查询超时 map_update_interval: 5.0 # 地图更新间隔秒 # 使用仿真时间 use_sim_time: true关键参数解析odom_frame,map_frame,base_frame定义了ROS中关键的坐标框架。base_link是机器人本体坐标系odom是由轮式编码器或足式机器人的里程计累积的坐标系map是全局地图坐标系。SLAM的任务就是估算base_link在map中的位姿。scan_topic必须与我们Go2发布的激光数据话题名称一致之前验证时是/scan。use_sim_time: true再次强调仿真环境必备。保存文件后编译并运行cd ~/go2_ws colcon build --packages-select go2_config source install/setup.bash ros2 launch go2_config go2_slam_launch.py4.2 在RVIZ中查看建图过程新开一个终端启动RVIZ并配置相关显示source ~/go2_ws/install/setup.bash rviz2在RVIZ中将Fixed Frame设置为map。点击Add添加一个Map显示将其Topic设置为/map。添加LaserScan显示Topic设置为/scan。添加TF和RobotModel。现在回到第一个终端再次启动键盘遥控节点控制Go2在Gazebo的虚拟环境中行走。你应该能在RVIZ中看到随着机器人移动灰色的地图被逐渐绘制出来黑色代表障碍物白色代表空闲区域灰色代表未知区域。这就是SLAM在建图实操心得与常见问题地图不更新或更新慢检查/scan话题是否有数据ros2 topic echo /scan并确认slam_toolbox节点的参数scan_topic设置正确。另外确保use_sim_time已设置并且/clock话题有发布ros2 topic echo /clock。地图错乱或漂移严重这是SLAM的常见挑战。可以尝试调整slam_toolbox_params.yaml中的resolution调低如0.025会增加精度但计算量增大、max_laser_range确保不超过传感器实际范围以及minimum_time_interval避免处理过于频繁的扫描。对于足式机器人其运动不如轮式平稳里程计噪声可能更大这会给SLAM带来挑战。一个技巧是尝试在Gazebo中创建一个特征更明显如更多直角墙壁、柱状物的环境帮助算法进行闭环检测。TF变换错误如果RVIZ中提示TF相关错误通常是odom-base_link或map-odom的变换缺失。Go2的CHAMP控制器应该会发布odom-base_link的变换。map-odom的变换则由slam_toolbox发布。使用ros2 run tf2_ros tf2_echo map odom可以查看这个变换是否存在。5. 集成Nav2导航栈让Go2学会自己“走路”建好地图只是第一步我们还需要让机器人能够利用这张地图进行自主导航。这就是Nav2的任务。Nav2是一个行为树驱动的导航系统它接收一个目标位姿然后结合地图、定位信息、代价地图和规划算法计算出安全的路径并控制机器人移动。5.1 配置Nav2启动与参数Nav2的配置相对复杂涉及多个节点和大量参数。我们可以从Nav2自带的示例配置开始然后针对Go2进行适配。首先将Nav2的示例配置复制到我们的配置目录cd ~/go2_ws/src/unitree-go2-ros2/go2_config mkdir -p config/nav2_params cp /opt/ros/humble/share/nav2_bringup/config/nav2_params.yaml config/nav2_params/ cp /opt/ros/humble/share/nav2_bringup/config/bt_navigator_params.yaml config/nav2_params/我们需要修改这些参数文件以适应我们的机器人。主要修改nav2_params.yaml# nav2_params.yaml (部分关键修改) amcl: ros__parameters: use_sim_time: True odom_frame_id: odom base_frame_id: base_link global_frame_id: map # 初始位姿可以设置为建图时的起点 initial_pose: {x: 0.0, y: 0.0, z: 0.0, yaw: 0.0} bt_navigator: ros__parameters: use_sim_time: True global_frame: map robot_base_frame: base_link odom_topic: /odom controller_server: ros__parameters: use_sim_time: True # 控制器频率 controller_frequency: 20.0 # 里程计话题 odom_topic: /odom # 机器人轮廓用于碰撞检测。对于Go2需要定义一个多边形区域。 footprint: [[-0.3, -0.2], [-0.3, 0.2], [0.3, 0.2], [0.3, -0.2]] # 局部代价地图参数 local_costmap: local_costmap: ros__parameters: update_frequency: 5.0 publish_frequency: 2.0 global_frame: odom robot_base_frame: base_link # 使用激光扫描层 plugins: [static_layer, obstacle_layer, inflation_layer] obstacle_layer: enabled: True observation_sources: scan scan: topic: /scan data_type: LaserScan marking: True clearing: True planner_server: ros__parameters: use_sim_time: True expected_planner_frequency: 20.0 # 全局代价地图参数 global_costmap: global_costmap: ros__parameters: update_frequency: 1.0 publish_frequency: 1.0 global_frame: map robot_base_frame: base_link plugins: [static_layer, obstacle_layer, inflation_layer] obstacle_layer: enabled: True observation_sources: scan scan: topic: /scan data_type: LaserScan marking: True clearing: True最重要的修改包括所有节点的use_sim_time设置为True。正确设置各坐标系IDglobal_frame_id地图、odom_frame_id、base_frame_id。设置odom_topic为Go2发布的里程计话题通常是/odom。定义机器人的footprint轮廓这是一个多边形顶点列表用于在代价地图中表示机器人本体避免与障碍物碰撞。这里给了一个矩形的近似你需要根据Go2的实际尺寸调整。在代价地图的obstacle_layer中配置observation_sources为scan并指定话题为/scan这样激光数据才能用于实时障碍物检测。5.2 创建完整的导航启动文件现在我们创建一个启动文件将Gazebo、SLAM或者加载已有地图、Nav2全部整合起来。这里我们演示加载刚才SLAM建好的地图进行导航。假设我们已经通过SLAM建图并保存了地图使用map_saver工具ros2 run nav2_map_server map_saver_cli -f ~/my_go2_map会得到my_go2_map.pgm和my_go2_map.yaml两个文件。新建启动文件go2_navigation_launch.py# go2_navigation_launch.py from launch import LaunchDescription from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory import os def generate_launch_description(): # 定义启动参数例如地图文件路径 map_yaml_file LaunchConfiguration(map) declare_map_arg DeclareLaunchArgument( map, default_valuePathJoinSubstitution([ FindPackageShare(go2_config), maps, my_go2_map.yaml # 你保存的地图YAML文件 ]), descriptionFull path to map yaml file to load ) use_sim_time LaunchConfiguration(use_sim_time) declare_use_sim_time_arg DeclareLaunchArgument( use_sim_time, default_valuetrue, descriptionUse simulation (Gazebo) clock if true ) # 1. 启动Gazebo仿真不带SLAM gazebo_launch IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare(go2_config), launch, gazebo_velodyne.launch.py ]) ]), launch_arguments{rviz: false}.items() ) # 2. 启动地图服务器加载预先建好的地图 map_server_node Node( packagenav2_map_server, executablemap_server, namemap_server, outputscreen, parameters[{yaml_filename: map_yaml_file, use_sim_time: use_sim_time}] ) # 3. 启动AMCL定位节点用于在已知地图中定位 amcl_node Node( packagenav2_amcl, executableamcl, nameamcl, outputscreen, parameters[os.path.join(get_package_share_directory(go2_config), config, nav2_params, nav2_params.yaml), {use_sim_time: use_sim_time}] ) # 4. 启动完整的Nav2生命周期管理器 nav2_bringup_launch IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare(nav2_bringup), launch, navigation_launch.py ]) ]), launch_arguments{params_file: PathJoinSubstitution([ FindPackageShare(go2_config), config, nav2_params, nav2_params.yaml ]), use_sim_time: use_sim_time, autostart: true}.items() ) # 5. 启动RVIZ使用Nav2的默认配置 rviz_config_file PathJoinSubstitution([ FindPackageShare(nav2_bringup), rviz, nav2_default_view.rviz ]) rviz_node Node( packagerviz2, executablerviz2, namerviz2, arguments[-d, rviz_config_file], parameters[{use_sim_time: use_sim_time}], outputscreen ) return LaunchDescription([ declare_map_arg, declare_use_sim_time_arg, gazebo_launch, map_server_node, amcl_node, nav2_bringup_launch, rviz_node, ])5.3 运行与测试自主导航编译并运行这个完整的导航启动文件cd ~/go2_ws colcon build --packages-select go2_config source install/setup.bash ros2 launch go2_config go2_navigation_launch.py启动后RVIZ会打开Nav2的默认视图。你需要进行以下关键操作初始定位在RVIZ中点击顶部工具栏的“2D Pose Estimate”按钮然后在地图上点击机器人实际所在的大概位置并拖拽箭头方向以匹配机器人实际朝向。这为AMCL提供了初始位姿估计。发送导航目标点击“Navigation2 Goal”按钮然后在地图上点击你想要机器人去往的目标点同样可以拖拽调整目标朝向。如果一切配置正确你应该能看到Nav2的全局规划器如NavFn或Smac Planner规划出一条从当前位置到目标点的全局路径通常显示为绿色线条。局部规划器如Regulated Pure Pursuit或DWB会计算并发布速度指令/cmd_vel话题给Go2的控制器。Gazebo中的Go2开始自主移动绕过障碍物走向目标点。踩坑实录与调优经验AMCL定位发散如果给出初始位姿后机器人的定位在RVIZ中显示为一簇粒子迅速发散或跑飞说明定位失败。可能的原因有1) 初始位姿给得太不准2) 激光数据与地图匹配度极低可能是地图建得不好或者环境动态变化太大3) AMCL参数需要调整如laser_model_type、odom_model_type以及initial_pose的协方差initial_cov_xx,initial_cov_yy,initial_cov_aa可以设置得大一些表示初始不确定性高。机器人不移动或原地打转检查/cmd_vel话题是否有数据发布。如果没有可能是局部代价地图中机器人的footprint设置过大导致机器人认为前方全是障碍物也可能是局部规划器的参数如最大速度、加速度与Go2的CHAMP控制器不匹配。需要调整controller_server的参数特别是与速度相关的限制。规划路径穿过障碍物检查全局代价地图的inflation_layer参数。inflation_radius定义了障碍物的膨胀半径确保规划路径与障碍物保持安全距离。对于Go2这样的四足机器人可能需要比轮式机器人更大的转弯半径和避障距离。CHAMP控制器与Nav2速度指令的适配这是本项目特有的难点。Nav2的controller_server默认输出的是geometry_msgs/msg/Twist消息包含线速度和角速度。而CHAMP控制器可能期望的是更底层的关节控制指令或特定的步态命令。在当前的unitree-go2-ros2项目中teleop_twist_keyboard节点发布的/cmd_vel话题是被一个champ_teleop节点或类似功能订阅并转换成了CHAMP能理解的步态指令。你需要确保Nav2发布的/cmd_vel话题能被同一个节点订阅。通常这意味着你需要重映射Nav2控制器的输出话题或者修改champ_teleop的订阅话题。在nav2_params.yaml的controller_server部分可以设置cmd_vel_topic参数来指定发布的话题名确保它与控制节点订阅的话题一致。6. 性能调优与进阶思考让一个基础Demo跑起来是一回事让它稳定、高效、鲁棒地工作则是另一回事。基于Go2和Gazebo的仿真导航有几个方向值得深入探索和调优。6.1 仿真环境与传感器配置优化Gazebo仿真毕竟不是现实其物理引擎和传感器模型都存在简化。为了得到更贴近真实的结果可以考虑更复杂的环境不要只在空荡荡的Gazebo默认世界里测试。搭建一个有走廊、房间、动态障碍物如移动的箱子的环境更能考验导航算法的鲁棒性。Gazebo的模型库libgazebo11-model-props或在线模型仓库如Gazebo Model Database提供了丰富的选择。传感器噪声在Gazebo的激光雷达插件配置中可以添加噪声模型Gaussian noise使扫描数据更接近真实传感器测试SLAM和导航算法在噪声下的表现。多传感器融合当前只用了2D激光雷达。Go2模型还支持IMU和3D Velodyne雷达。你可以尝试在SLAM中融合IMU数据slam_toolbox支持来改善在快速运动或颠簸地形下的定位精度。3D点云则可以用于更精细的3D避障或语义建图。6.2 CHAMP控制器与导航速度的协同足式机器人的运动控制比轮式复杂得多。CHAMP控制器接收的是高层速度指令并将其分解为复杂的腿部关节轨迹。这中间可能存在延迟和动态响应问题。速度限制匹配在Nav2的controller_server参数中设置的max_vel_xmax_vel_theta等最大值不应超过CHAMP控制器中gait.yaml文件里配置的Max Linear Velocity X和Max Angular Velocity Z。否则Nav2可能会发出机器人无法执行的速度指令。控制频率调整controller_frequency使其与CHAMP控制器的期望指令更新频率相匹配。频率太高可能导致指令队列堆积太低则控制不连续。定制控制器接口对于更高级的应用可以考虑开发一个专门的controller_server插件直接生成CHAMP所需的步态参数或更底层的控制指令而不是通用的Twist消息以实现更精准的运动控制。6.3 从仿真到实机的挑战这个仿真项目的最终价值是为真实Unitree Go2机器人的开发提供参考。但仿真到实机Sim2Real存在巨大鸿沟动力学差异Gazebo的物理引擎ODE Bullet等无法完全模拟真实的电机特性、地面摩擦、腿部柔性等。传感器差异仿真激光是理想的而真实雷达存在镜面反射、雨雾干扰、运动畸变等问题。状态估计仿真中我们可以获得完美的关节状态和里程计而实机需要依赖状态估计算法如通过IMU和腿部运动学来推算odom其精度和延迟直接影响SLAM和导航。因此在仿真中调好的参数在实机上几乎肯定需要重新调整。仿真的主要作用是验证算法流程、进行安全且低成本的前期开发以及生成大量的训练数据如果涉及机器学习。将本Demo迁移到实机需要替换Gazebo为真实的机器人驱动、传感器驱动和状态估计节点这是一个更大的工程但本仿真项目无疑提供了一个极佳的软件框架和算法起点。