Ubuntu20.04 Gazebo 仿真宇树机器狗及外置激光雷达部署

在该终端中，按键 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

HELP_TEXT = """
=========================================================
键盘控制说明 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", end="")
        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--------------------------------------------------", end="")

    # ---------------- 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

外置激光雷达

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

3.1 激光雷达仿真模型

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

编译安装 robosense simulator：

cd ~/unitree_sim/src
# 拉取 robosense simulator 仿真驱动
git clone 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
... (省略部分标准话题) ...
/rslidar_points
/trunk_imu
...

其中 /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]

导入世界场景

在完成宇树机器狗本体及外置激光雷达的仿真配置后，为了对机器人在复杂环境中的运动与感知能力进行验证，需要构建合适的仿真场景并导入 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://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

加载效果良好。

小结

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