Ubuntu20.04 Gazebo中仿真宇树机器狗和外置激光雷达

0 引言

     机器狗因其良好的通过性和环境适应性，在工业巡检、安防巡逻及科研教学等领域得到广泛应用。宇树科技（Unitree）四足机器狗作为当前主流的商用四足机器人平台之一，具有成熟的运动控制接口和良好的仿真支持。

     在算法开发和系统集成阶段，直接进行实机调试成本高、风险大，因此基于 Gazebo 的仿真环境搭建显得尤为重要。本文基于 Ubuntu20.04，详细介绍了宇树机器狗Go2在 Gazebo 中的仿真部署流程，并进一步扩展外置激光雷达传感器，同时说明了加载场景的步骤，实现了完整的感知与运动仿真验证，为后续定位、建图与导航算法开发提供可靠基础。

1 系统环境依赖

1.1 系统环境

相关教程很多，不多赘述.

• Ubuntu 20.04
• ROS noetic desktop-full
• Gazebo classic

1.2 软件依赖

• unitree ros
• unitree guide
• unitree ros to real

这里说明一下编译安装宇树官方驱动的步骤：

# 创建catkin工作空间 mkdir -p ~/unitree_sim/src cd ~/unitree/src && catkin_init_workspace # unitree ros 国内代理下载快点 git clone --recurse-submodules https://gh-proxy.com/https://github.com/unitreerobotics/unitree_ros.git # unitree guide git clone --recurse-submodules https://gh-proxy.com/https://github.com/unitreerobotics/unitree_guide.git # 如果下载不完整，请手动补全 # 编译，暂时不需要编译 unitree_move_base cd .. && catkin_make -DCATKIN_BLACKLIST_PACKAGES="unitree_move_base" 

等待编译完成后，需要修改官方模型加载的一处错误：
在~unitree_sim/src/unitree_ros/robots/go2_description/xacro/robot.xacro 中：

将48行中 "trunk.dae" 改为 "base.dae" ，保存修改。

2 验证基础功能

（本文以Go2为例）

2.1 Gazebo仿真

cd ~/unitree_sim && source devel/setup.bash # unitree guide roslaunch unitree_guide gazeboSim.launch rname:=go2 # 或 unitree gazebo roslaunch unitree_gazebo normal.launch rname:=go2 wname:=stairs

仿真器中加载出宇树go2模型和简易世界模型：

2.2 运动控制

cd ~/unitree_sim && source devel/setup.bash # 启动运动控制节点 rosrun unitree_guide junior_ctrl # 在该终端中，按键2-站立、按键4-运动模式、按键5-导航模式 # 进入运动模式或者导航模式，均需要先站立起来。 # 运动模式中，按键W-前进、按键S-后退、按键A-左平移、按键D右平移、按键JL分别控制左右转。 # 导航模式此处暂时不表。

按下按键2后，仿真器中机器狗站立起来：

按下按键4进入运动模式后，在按下前进W，机器狗开始一直向前行走：

再次按下按键2，机器狗停止运动。

2.3 进阶控制

    在2.2小节中所讲的导航模式，实际上是宇树官方开放给ros navigation导航栈的接口，当自主导航时由局部规划器输出 geometry_msgs/Twist  类型的速度命令话题 /cmd_vel，底层运动驱动会实时订阅该话题，进而控制机器人行走运动。

宇树官方驱动中，导航模式默认是关闭编译的，需要开启后重新编译 unitree guide。具体在~/unitree_sim/src/unitree_guide/unitree_guide/CMakeLists.txt中：

将第11行中 set(MOVE_BASE OFF) 改为 set(MOVE_BASE ON)，然后重新编译该包：

cd ~/unitree_sim # 单独编译 unitree guide 包 catkin_make -DCATKIN_WHITELIST_PACKAGES="unitree_guide"

虽然当依次按下按键2和4进入运动模式后，可行走控制，但十分不便，自行尝试可知。因此，利用导航模式中的速度命令话题 /cmd_vel，这里编写了一个简易有效的脚本发布控制命令：

#!/usr/bin/env python3 # -*- coding: utf-8 -*- import sys import select import termios import tty import rospy from geometry_msgs.msg import Twist" ========================================================= 键盘控制说明 Keyboard Control Help ========================================================= 移动控制（平移）: q w e 左前 前进 右前 a s d 左移 停止 右移 z x c 左后 后退 右后 对角线移动（45°平移）: f g h b 左前45° 右前45° 左后45° 右后45° 旋转控制（原地旋转）: r t 左转 右转 速度调节: u / i : 增加 / 减少 线速度 j / k : 增加 / 减少 角速度 m / , : 同时调整 线速度和角速度 其他: 其他按键 : 停止运动 CTRL + C : 退出程序 ========================================================= """ class KeyboardTeleop: def __init__(self): rospy.init_node("keyboard_control", anonymous=True) self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1) # 参数 self.max_lin = rospy.get_param("~speed", 0.8) self.max_ang = rospy.get_param("~turn", 0.8) # 速度状态 self.target = [0.0, 0.0, 0.0, 0.0] self.current = [0.0, 0.0, 0.0, 0.0] self.acc_step = 0.1 self.dec_step = 0.1 # 键位绑定 self.motion_map = { 'w': (1, 0, 0, 0), 'e': (1, 0, 0, -1), 'a': (0, 1, 0, 0), 'd': (0, -1, 0, 0), 'q': (1, 0, 0, 1), 'x': (-1, 0, 0, 0), 'c': (-1, 0, 0, 1), 'z': (-1, 0, 0, -1), 'r': (0, 0, 0, 1), 't': (0, 0, 0, -1), 'f': (1, 1, 0, 0), 'h': (-1, 1, 0, 0), 'g': (1, -1, 0, 0), 'b': (-1, -1, 0, 0), } self.speed_map = { 'u': (1.1, 1.0), 'i': (0.9, 1.0), 'j': (1.0, 1.1), 'k': (1.0, 0.9), 'm': (1.1, 1.1), ',': (0.9, 0.9), } self.settings = termios.tcgetattr(sys.stdin) self.last_key = None print("\033[2J\033[H") print(HELP_TEXT) # ---------------- 键盘读取 ---------------- def read_key(self): tty.setraw(sys.stdin.fileno()) readable, _, _ = select.select([sys.stdin], [], [], 0.1) key = sys.stdin.read(1) if readable else "" termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) return key # ---------------- 插值平滑 ---------------- @staticmethod def smooth(curr, tgt, acc, dec): step = acc if curr < tgt else dec if abs(curr - tgt) <= step: return tgt return curr + step if curr < tgt else curr - step # ---------------- 状态显示 ---------------- def display(self, key): print("\033[25;0H",) print("\033[K[Speed ] linear: %5.2f angular: %5.2f" % (self.max_lin, self.max_ang)) print("\033[K[ Key ] %s" % (key if key else "None")) print("\033[K--------------------------------------------------") print("\033[K[Target ] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.target)) print("\033[K[Current] X:%6.2f Y:%6.2f Z:%6.2f Th:%6.2f" % tuple(self.current)) print("\033[K--------------------------------------------------",) # ---------------- Twist 发布 ---------------- def publish_twist(self): msg = Twist() msg.linear.x = self.current[0] * self.max_lin msg.linear.y = self.current[1] * self.max_lin msg.linear.z = self.current[2] * self.max_lin msg.angular.z = self.current[3] * self.max_ang self.pub.publish(msg) # ---------------- 主循环 ---------------- def spin(self): rate = rospy.Rate(20) while not rospy.is_shutdown(): key = self.read_key() if key in self.motion_map: self.target = list(self.motion_map[key]) self.dec_step = 0.1 elif key in self.speed_map: self.max_lin *= self.speed_map[key][0] self.max_ang *= self.speed_map[key][1] else: self.target = [0.0] * 4 self.dec_step = 0.15 if key == "\x03": break for i in range(4): self.current[i] = self.smooth( self.current[i], self.target[i], self.acc_step, self.dec_step ) self.display(key) self.publish_twist() rate.sleep() self.shutdown() # ---------------- 退出清理 ---------------- def shutdown(self): stop = Twist() self.pub.publish(stop) termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings) rospy.loginfo("[keyboard_control] Exit.") if __name__ == "__main__": try: KeyboardTeleop().spin() except Exception as e: rospy.logerr(e) 

假设该脚本命名为 keycontrol.py，以下给出整个运动控制流程：

chmod +x keycontrol.py # 开启gazebo仿真器 roslaunch unitree_guide gazeboSim.launch rname:=go2 # 开启底层运动控制节点 # 在该终端按下按键2站立，然后按下按键5进入导航模式 rosrun unitree_guide junior_ctrl # 启动控制命令脚本 python3 keycontrol.py

执行情况如下：

​​​​​​

3 外置激光雷达

    在完成宇树机器狗本体仿真与基础运动控制验证后，为了进一步支持环境感知、定位与导航算法的开发，有必要在仿真环境中为机器狗添加外置激光雷达。本章将详细介绍外置激光雷达在 Gazebo 中的建模方法、与机器人本体的集成方式以及仿真驱动与数据验证过程。

3.1 激光雷达仿真模型

由于个人需要，本文选择了速腾聚创的多线机械激光雷达进行仿真：

编译安装 robosense simulator：

cd ~/unitree_sim/src # 拉取 robosense simulator 仿真驱动 git clone https://gh-proxy.com/https://github.com/tomlogan501/robosense_simulator.git cd .. catkin_make -DCATKIN_WHITELIST_PACKAGES="robosense_gazebo_plugins" catkin_make -DCATKIN_WHITELIST_PACKAGES="robosense_description" 

安装完毕后，进行下一步。

3.2 机器狗安装雷达配置

由于本章采用的是外置激光雷达，因此需要将激光雷达模型正确地安装到机器狗的仿真模型上。该过程在机器狗的 xacro 文件中完成。

在模型配置过程中，需要重点关注以下几个方面：

安装位置选择
激光雷达一般安装在机器人机身顶部或前部位置，以减少机器狗自身结构对激光扫描的遮挡。仿真中需根据实际硬件安装位置设置合理的平移和旋转参数。

坐标系定义
为激光雷达单独定义一个 link，并通过 joint 与机器狗本体 link 相连。该 link 将作为激光雷达数据的参考坐标系。

TF 关系正确性
确保激光雷达坐标系能够通过 TF 正确转换到机器狗基坐标系及世界坐标系。这对于后续的定位与建图算法至关重要。

3.2.1 机器狗上加载雷达

将 ~/unitree_sim/src/robosense_simulator/robosense_description/meshes/robosense_16.dae中的雷达模型复制到~/unitree_sim/src/unitree_ros/robots/go2_description/meshes/中：

cp ~/unitree_sim/src/robosense_simulator/robosense_description/meshes/robosense_16.dae ~/unitree_sim/src/unitree_ros/robots/go2_description/meshes/

修改~/go2_description/xacro/robot.xacro文件，添加以下内容：

<link name="lidar"> <inertial> <origin xyz="0 0 0" rpy="0 0 0"/> <mass value="0.05"/> <inertia ixx="1e-5" ixy="0" ixz="0" iyy="1e-5" iyz="0" izz="1e-5"/> </inertial> <visual> <origin xyz="0 0 0" rpy="-1.57079632679 0 0"/> <geometry> <mesh filename="package://go2_description/meshes/robosense_16.dae"/> </geometry> <material> <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1"/> </material> </visual> <collision> <origin xyz="0 0 0" rpy="0 0 0"/> <geometry> <mesh filename="package://go2_description/meshes/robosense_16.dae"/> </geometry> </collision> </link> <joint name="lidar_joint" type="fixed"> <origin xyz="0.23 0 0.085" rpy="0 0 0"/> <parent link="base"/> <child link="lidar"/> <axis xyz="0 0 0"/> </joint>

修改后保存，打开gazebo查看：

roslaunch unitree_guide gazeboSim.launch rname:=go2 

结果如下，雷达安装在机器狗头部上方：

3.2.2 雷达仿真驱动配置

将雷达添加到机器狗本体后，还需要配置雷达的物理仿真驱动：

修改 ~/unitree_sim/src/unitree_ros/robots/go2_description/xacro/gazebo.xacro ，添加以下：

<gazebo reference="lidar"> <sensor type="gpu_ray" name="robosense-RS16"> <pose>0 0 0 0 0 0</pose> <visualize>false</visualize> <update_rate>10</update_rate> <ray> <scan> <horizontal> <samples>900</samples> <resolution>1</resolution> <min_angle>-3.1415926535897931</min_angle> <max_angle>3.1415926535897931</max_angle> </horizontal> <vertical> <samples>16</samples> <resolution>1</resolution> <min_angle>-0.262</min_angle> <max_angle> 0.262</max_angle> </vertical> </scan> <range> <min>0.1</min> <max>100.0</max> <resolution>0.01</resolution> </range> <noise> <type>gaussian</type> <mean>0.0</mean> <stddev>0.01</stddev> </noise> </ray> <plugin name="gazebo_ros_laser_controller" filename="libgazebo_ros_robosense_gpu_laser.so"> <topicName>/rslidar_points</topicName> <frameName>lidar</frameName> </plugin> </sensor> </gazebo>

若没有GPU，将 gpu_ray改为ray，将 libgazebo_ros_robosense_gpu_laser.so 改为 libgazebo_ros_robosense_laser.so

修改后保存，进行下一步。

3.3 雷达数据验证可视化

启动 gazebo 和 rostopic 查看雷达点云数据：

# 打开gazebo roslaunch unitree_guide gazeboSim.launch rname:=go2

rostopic list 查看话题如下：

$ rostopic list /apply_force/trunk /clock /gazebo/link_states /gazebo/model_states /gazebo/parameter_descriptions /gazebo/parameter_updates /gazebo/performance_metrics /gazebo/set_link_state /gazebo/set_model_state /go2_gazebo/FL_calf_controller/command /go2_gazebo/FL_calf_controller/joint_wrench /go2_gazebo/FL_calf_controller/state /go2_gazebo/FL_hip_controller/command /go2_gazebo/FL_hip_controller/joint_wrench /go2_gazebo/FL_hip_controller/state /go2_gazebo/FL_thigh_controller/command /go2_gazebo/FL_thigh_controller/joint_wrench /go2_gazebo/FL_thigh_controller/state /go2_gazebo/FR_calf_controller/command /go2_gazebo/FR_calf_controller/joint_wrench /go2_gazebo/FR_calf_controller/state /go2_gazebo/FR_hip_controller/command /go2_gazebo/FR_hip_controller/joint_wrench /go2_gazebo/FR_hip_controller/state /go2_gazebo/FR_thigh_controller/command /go2_gazebo/FR_thigh_controller/joint_wrench /go2_gazebo/FR_thigh_controller/state /go2_gazebo/RL_calf_controller/command /go2_gazebo/RL_calf_controller/joint_wrench /go2_gazebo/RL_calf_controller/state /go2_gazebo/RL_hip_controller/command /go2_gazebo/RL_hip_controller/joint_wrench /go2_gazebo/RL_hip_controller/state /go2_gazebo/RL_thigh_controller/command /go2_gazebo/RL_thigh_controller/joint_wrench /go2_gazebo/RL_thigh_controller/state /go2_gazebo/RR_calf_controller/command /go2_gazebo/RR_calf_controller/joint_wrench /go2_gazebo/RR_calf_controller/state /go2_gazebo/RR_hip_controller/command /go2_gazebo/RR_hip_controller/joint_wrench /go2_gazebo/RR_hip_controller/state /go2_gazebo/RR_thigh_controller/command /go2_gazebo/RR_thigh_controller/joint_wrench /go2_gazebo/RR_thigh_controller/state /go2_gazebo/joint_states /rosout /rosout_agg /rslidar_points /tf /tf_static /trunk_imu /visual/FL_foot_contact/the_force /visual/FR_foot_contact/the_force /visual/RL_foot_contact/the_force /visual/RR_foot_contact/the_force 

其中 /rslidar_points 是仿真雷达的点云话题，/trunk_imu 是机器狗本体的 imu数据话题。

打开rviz查看实时点云结果如下：

至此，雷达模型和仿真驱动添加完毕，后续可以将机器狗导入任意世界场景中进行仿真使用。

本体与传感器外参如下：

T_base_lidar: translation: [0.23, 0.0, 0.085] rotation (rpy): [0, 0, 0] T_base_imu: translation: [0.0, 0.0, 0.0] rotation (rpy): [0, 0, 0] 

4 导入世界场景

    在完成宇树机器狗本体及外置激光雷达的仿真配置后，为了对机器人在复杂环境中的运动与感知能力进行验证，需要构建合适的仿真场景并导入 Gazebo。世界模型用于描述机器人所处的仿真环境，是运动控制、传感器感知以及算法验证的重要基础。本章将简单介绍 Gazebo 世界模型的构建和将自定义世界模型导入机器狗仿真系统的过程。

4.1 Gazebo世界模型概述

    Gazebo 世界模型（World）用于描述仿真环境中的所有静态与动态元素，包括地面、墙体、障碍物以及环境光照等。世界模型通常以 .world 文件的形式存在，其本质是基于 SDF（Simulation Description Format）的场景描述文件。

   一个完整的 Gazebo 世界模型通常包含以下内容：

• 仿真环境的物理属性，如重力、摩擦系数等；
• 地面模型及其尺寸和材质；
• 各类障碍物模型，用于构建复杂环境；
• 光源设置，用于改善仿真可视化效果。

合理设计世界模型不仅可以提升仿真的真实感，还能为激光雷达、视觉传感器等提供更加符合实际的感知输入。

4.2 基础世界模型构建

    在仿真初期，为了验证系统整体功能，通常先构建一个相对简单的世界模型。该模型一般包含平坦地面和少量基础障碍物，便于观察机器人运动和传感器数据的变化。

   基础世界模型的构建主要包括：

• 设置平面地面模型，作为机器人运动的基础支撑；
• 添加简单几何体（如立方体、墙体）作为障碍物；
• 调整模型尺寸与位置，确保机器人具有足够的活动空间。

通过基础世界模型，可以快速验证机器人在仿真中的稳定性以及激光雷达对障碍物的感知能力。

4.3 自定义复杂世界与导入

    为了方便，笔者直接选取了 aws_robomaker_racetrack_world 的世界模型，当然还有许多其它的优秀的 Gazebo 世界模型参考：https://github.com/PX4/PX4-SITL_gazebo-classic.git 。

说明一下编译安装步骤：

cd ~/unitree_sim/src git clone https://gh-proxy.com/https://github.com/aws-robotics/aws-robomaker-racetrack-world.git cd .. catkin_make install -DCATKIN_WHITELIST_PACKAGES="aws_robomaker_racetrack_world"

编译完成后，修改 ~/unitree_sim/src/unitree_guide/unitree_guide/launch/gazeboSim.launch ：

第2行的 <arg name="wname" default="earth"/> 改为 <arg name="wname" default="racetrack_day"/> 第4行的 <arg name="rname" default="go1"/> 改为 <arg name="rname" default="go2"/> 第17行的 <arg name="world_name" value="$(find unitree_gazebo)/worlds/$(arg wname).world"/> 改为 <arg name="world_name" value="$(find aws_robomaker_racetrack_world)/worlds/$(arg wname).world"/> 

修改后保存，打开机器狗仿真器：

# 激活环境变量 cd ~/unitree_sim && source devel/setup.bash # gazebo 启动 roslaunch unitree_guide gazeboSim.launch # 若加载失败，可能是之前gazebo相关节点没有正常关闭。请先关闭： killall -9 gzserver gzclient # 运动控制启动，按下按键2-站立 rosrun unitree_guide junior_ctrl 

加载效果：

5 小结

    该仿真系统为后续开展定位、建图和导航等算法研究提供了稳定、可复现的实验平台，并对机器狗仿真系统的搭建具有一定参考价值。后续有机会再更新适配主流建图定位和导航算法的教程。长文撰写不易，点个赞吧！