Gazebo仿真实战：用ros_control实现两轮机器人自动巡航（附完整URDF文件）

你是否已经厌倦了在ROS中反复调试那些基础的“Hello World”式机器人模型，渴望构建一个真正能“动起来”、能执行任务的仿真机器人？对于许多从ROS基础教程过渡到实际项目开发的工程师来说，Gazebo仿真与ros_control的结合，常常是第一个需要啃下的硬骨头。这不仅是将静态URDF模型变成动态实体的关键一步，更是后续进行导航、SLAM、乃至强化学习等高级应用不可或缺的基石。今天，我们就抛开那些零散的代码片段，从一个完整的、可立即复用的两轮差速机器人自动巡航案例出发，手把手带你打通从模型定义、控制器配置到路径跟踪的完整链路。我会分享在调试过程中积累的实战经验，并提供一套经过验证的配置文件，让你能快速搭建起自己的仿真测试平台。

1. 构建一个“仿真友好”的机器人URDF模型

很多教程会告诉你如何写一个URDF，但很少强调什么样的URDF才适合Gazebo仿真。一个能在Gazebo中稳定、准确运行的模型，远不止是视觉上的“像”，它必须包含精确的物理属性、合理的关节定义以及与ros_control对接的“接口”。

1.1 超越视觉：为模型注入物理灵魂

在Gazebo中，一个模型如果只有<visual>标签，它就是一个漂亮的“幽灵”，无法与物理世界互动。我们必须为其添加<collision>和<inertial>标签。<collision>定义了模型的碰撞边界，通常可以简化（比如用一个圆柱体近似机器人的底盘），以提升仿真效率。而<inertial>（惯性矩阵）则是物理仿真的核心，它决定了物体对力和扭矩的响应。一个常见的错误是忽略或随意设置惯性参数，这会导致机器人像羽毛一样飘忽不定，或者像铅块一样难以推动。

对于规则几何体，惯性矩阵有标准公式。我们可以将其封装为Xacro宏，方便复用：

<!-- inertia.xacro -->
<xacro:macro name="cylinder_inertial_matrix" params="m r h">
  <inertial>
    <mass value="${m}" />
    <inertia ixx="${m*(3*r*r+h*h)/12}" ixy="0" ixz="0"
             iyy="${m*(3*r*r+h*h)/12}" iyz="0"
             izz="${m*r*r/2}" />
  </inertial>
</xacro:macro>

在定义机器人底盘base_link时，这样调用它：

<link name="base_link">
  <visual> ... </visual>
  <collision> ... </collision>
  <!-- 关键：添加惯性参数，m为质量，r为半径，h为高度 -->
  <xacro:cylinder_inertial_matrix m="0.5" r="0.1" h="0.08"/>
</link>

注意：质量（m）的单位是千克，尺寸单位是米。请务必根据你设计的机器人实际尺寸估算一个合理的质量，过轻或过重都会影响控制效果。

1.2 关节定义：运动控制的基石

对于两轮差速机器人，驱动轮关节的类型必须设置为type="continuous"。这意味着关节可以无限旋转（不像revolute关节有角度限制），并且其旋转轴（<axis>）需要正确定义。通常，车轮绕其轮轴旋转，对于安装在机器人两侧的轮子，其旋转轴（y轴）是平行于机器人前进方向的。

<joint name="left_wheel2base_link" type="continuous">
  <parent link="base_link"/>
  <child link="left_wheel"/>
  <origin xyz="0 0.1 -0.03" rpy="0 0 0"/>
  <axis xyz="0 1 0"/>
</joint>

这里origin的xyz参数决定了轮子相对于底盘的位置，axis xyz="0 1 0"表示绕局部坐标系的Y轴旋转。务必确保左右轮的axis方向一致，否则一个轮子向前转，另一个向后转，机器人就无法直行了。

1.3 模块化设计：使用Xacro提升可维护性

将机器人拆分为多个Xacro文件（如robot_base.xacro、camera.xacro、laser.xacro）是一个好习惯。这不仅让主文件robot_car.xacro结构清晰，也便于复用和单独调试传感器模块。

<!-- robot_car.xacro -->
<robot name="robot_car" xmlns:xacro="http://wiki.ros.org/xacro">
  <xacro:include filename="$(find your_package)/urdf/inertia.xacro" />
  <xacro:include filename="$(find your_package)/urdf/robot_base.xacro" />
  <xacro:include filename="$(find your_package)/urdf/camera.xacro" />
  <xacro:include filename="$(find your_package)/urdf/laser.xacro" />
  <xacro:include filename="$(find your_package)/urdf/gazebo/move.xacro" />
</robot>

2. 打通仿真与控制：配置ros_control与Gazebo插件

模型准备好后，下一步是告诉Gazebo如何控制它。这需要两步：定义传动装置（Transmission）和加载Gazebo控制插件。

2.1 传动装置（Transmission）：连接控制器与关节

ros_control框架通过控制器管理器来管理各种控制器（如速度控制器、位置控制器）。传动装置的作用是将控制器输出的“广义力”映射到具体关节的执行器命令。对于速度控制的轮子，我们需要为每个驱动关节定义一个传动。

<!-- move.xacro 中定义传动宏 -->
<xacro:macro name="joint_trans" params="joint_name">
  <transmission name="${joint_name}_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="${joint_name}">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
    </joint>
    <actuator name="${joint_name}_motor">
      <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
</xacro:macro>

<!-- 为左右轮关节应用传动 -->
<xacro:joint_trans joint_name="left_wheel2base_link" />
<xacro:joint_trans joint_name="right_wheel2base_link" />

这里hardwareInterface指定为VelocityJointInterface，意味着我们将使用速度接口来控制这个关节。mechanicalReduction是减速比，设为1表示电机输出轴与关节轴直接耦合。

2.2 Gazebo ROS差分驱动插件：仿真的执行器

传动定义了接口，但Gazebo中实际的物理执行需要由插件来完成。对于两轮差速机器人，最常用的是libgazebo_ros_diff_drive.so插件。这个插件订阅cmd_vel话题（geometry_msgs/Twist），并根据机器人的几何参数将其转换为左右轮的目标转速。

<gazebo>
  <plugin name="differential_drive_controller" filename="libgazebo_ros_diff_drive.so">
    <updateRate>100.0</updateRate>
    <leftJoint>left_wheel2base_link</leftJoint>
    <rightJoint>right_wheel2base_link</rightJoint>
    <wheelSeparation>${base_link_radius * 2}</wheelSeparation>
    <wheelDiameter>${wheel_radius * 2}</wheelDiameter>
    <commandTopic>cmd_vel</commandTopic>
    <odometryTopic>odom</odometryTopic>
    <odometryFrame>odom</odometryFrame>
    <robotBaseFrame>base_footprint</robotBaseFrame>
    <publishWheelTF>true</publishWheelTF>
    <publishTf>true</publishTf>
  </plugin>
</gazebo>

关键参数解析：

参数	说明	常见错误
leftJoint, rightJoint	必须与URDF中关节名严格一致。	拼写错误或关节名不匹配，插件将无法找到关节，控制失效。
wheelSeparation	两轮中心之间的距离（米）。	计算错误会导致机器人转弯半径与实际不符。
wheelDiameter	轮子直径（米）。	使用半径而非直径，会导致速度计算错误。
commandTopic	订阅的控制指令话题，默认为cmd_vel。	自定义后，发布指令的话题也需相应改变。
odometryFrame	里程计坐标系，通常为odom。	与导航栈的坐标系配置需统一。
robotBaseFrame	机器人基座标系，通常为base_footprint或base_link。	与TF树中的定义保持一致。

提示：一个快速验证插件是否生效的方法是，启动仿真后，在终端运行rostopic list。如果你能看到/cmd_vel和/odom等话题，通常意味着插件加载成功。如果看不到，请首先检查Gazebo启动日志中是否有关于插件加载的错误信息。

3. 启动世界与调试：让机器人动起来

模型和控制器都配置好后，我们需要一个启动文件将它们整合起来，并放入一个仿真环境中。

3.1 编写集成启动文件

一个典型的启动文件display.launch需要完成三件事：加载URDF到参数服务器、启动Gazebo仿真环境、将模型生成到Gazebo世界中。

<launch>
  <!-- 1. 加载机器人描述 -->
  <param name="robot_description" command="$(find xacro)/xacro '$(find your_package)/urdf/robot_car.xacro'" />

  <!-- 2. 启动Gazebo服务器和客户端，并加载自定义世界 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find your_package)/worlds/my_room.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- 3. 将URDF模型生成到Gazebo中 -->
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-urdf -model my_robot -param robot_description -x 0 -y 0 -z 0.05" />
</launch>

使用spawn_model节点时，-z 0.05参数非常重要，它让机器人稍微悬空一点再落下，可以避免模型因碰撞检测而卡在地面以下。

3.2 基础运动测试与常见问题排查

启动成功后，你应该能在Gazebo中看到你的机器人。现在进行最简单的开环测试：

# 在新的终端中，发布一个让机器人原地转圈的速度指令
rostopic pub -r 10 /cmd_vel geometry_msgs/Twist "linear:
  x: 0.0
  y: 0.0
  z: 0.0
angular:
  x: 0.0
  y: 0.0
  z: 0.5"

如果机器人没有按预期运动，请按以下顺序排查：

1. 检查话题：rostopic echo /cmd_vel确认消息是否发出，rostopic echo /joint_states查看左右轮关节的状态（位置、速度）是否有变化。如果/joint_states没数据，问题可能出在传动（Transmission）配置。
2. 检查TF树：运行rosrun tf view_frames生成TF树图，查看odom->base_footprint的变换是否发布。差分驱动插件会发布这个变换。如果没有，检查插件参数<publishTf>和坐标系名称。
3. 检查Gazebo日志：Gazebo客户端窗口或启动终端中可能有警告或错误信息，特别是关于找不到关节或插件加载失败的。
4. 验证物理参数：如果机器人运动异常（如打滑、翻倒），回头检查URDF中的质量、惯性矩阵以及车轮的摩擦系数（可在Gazebo的<gazebo>标签内为link设置<mu1>, <mu2>等参数）。

4. 从手动控制到自动巡航：实现路径跟踪

让机器人动起来只是第一步。我们的目标是实现自动巡航，即让机器人能够跟踪一条预设的路径。这里我们实现一个简单的**纯追踪（Pure Pursuit）**算法作为示例。该算法思想简单，在ROS的nav_msgs/Path话题支持下很容易实现。

4.1 路径生成与发布

首先，我们需要一条路径。可以手动录制，也可以程序化生成。这里创建一个节点来发布一个方形的路径：

#!/usr/bin/env python
# path_publisher.py
import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
import tf

rospy.init_node('path_publisher')
path_pub = rospy.Publisher('/course_path', Path, queue_size=10, latch=True)

path = Path()
path.header.frame_id = 'odom' # 路径坐标系，通常设为地图或odom帧
poses = [
    (0.0, 0.0, 0.0),
    (1.0, 0.0, 0.0),
    (1.0, 1.0, 1.57), # 90度转角
    (0.0, 1.0, 3.14), # 180度转角
    (0.0, 0.0, 4.71)  # 回到起点
]

for (x, y, yaw) in poses:
    pose = PoseStamped()
    pose.header.frame_id = 'odom'
    pose.pose.position.x = x
    pose.pose.position.y = y
    q = tf.transformations.quaternion_from_euler(0, 0, yaw)
    pose.pose.orientation.x = q[0]
    pose.pose.orientation.y = q[1]
    pose.pose.orientation.z = q[2]
    pose.pose.orientation.w = q[3]
    path.poses.append(pose)

rate = rospy.Rate(1)
while not rospy.is_shutdown():
    path.header.stamp = rospy.Time.now()
    path_pub.publish(path)
    rate.sleep()

4.2 纯追踪（Pure Pursuit）控制器实现

纯追踪算法的核心是：在路径上寻找一个“前瞻点”（lookahead point），然后计算使机器人朝向该点的转向指令。我们编写一个控制器节点：

#!/usr/bin/env python
# pure_pursuit.py
import rospy
import numpy as np
from geometry_msgs.msg import Twist, PoseStamped
from nav_msgs.msg import Path, Odometry
import tf

class PurePursuit:
    def __init__(self):
        rospy.init_node('pure_pursuit_controller')
        self.cmd_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
        self.path = None
        self.current_pose = None
        self.lookahead_distance = 0.3  # 前瞻距离，根据机器人速度和路径曲率调整
        self.linear_speed = 0.2  # 恒定前进速度

        rospy.Subscriber('/course_path', Path, self.path_callback)
        rospy.Subscriber('/odom', Odometry, self.odom_callback)
        self.tf_listener = tf.TransformListener()

        self.rate = rospy.Rate(20) # 控制频率
        self.control_loop()

    def path_callback(self, msg):
        self.path = msg

    def odom_callback(self, msg):
        # 直接从odom话题获取机器人位姿（相对于odom坐标系）
        self.current_pose = msg.pose.pose

    def find_lookahead_point(self, robot_x, robot_y):
        """在路径上找到距离机器人最近点之后，且距离为lookahead_distance的点"""
        if self.path is None or len(self.path.poses) == 0:
            return None
        # 简化：寻找路径上距离机器人最近的点
        poses = np.array([(p.pose.position.x, p.pose.position.y) for p in self.path.poses])
        distances = np.linalg.norm(poses - np.array([robot_x, robot_y]), axis=1)
        closest_idx = np.argmin(distances)
        # 从最近点开始，向后寻找第一个距离超过前瞻距离的点
        for i in range(closest_idx, len(poses)):
            dx = poses[i][0] - robot_x
            dy = poses[i][1] - robot_y
            if np.hypot(dx, dy) >= self.lookahead_distance:
                return poses[i]
        # 如果找不到，返回最后一个点
        return poses[-1]

    def control_loop(self):
        while not rospy.is_shutdown():
            if self.path is None or self.current_pose is None:
                self.rate.sleep()
                continue

            x = self.current_pose.position.x
            y = self.current_pose.position.y
            # 获取机器人当前偏航角 (yaw)
            q = self.current_pose.orientation
            _, _, yaw = tf.transformations.euler_from_quaternion([q.x, q.y, q.z, q.w])

            target = self.find_lookahead_point(x, y)
            if target is None:
                self.rate.sleep()
                continue

            # 计算目标点在机器人坐标系下的位置
            target_x, target_y = target
            dx = target_x - x
            dy = target_y - y
            target_local_x = dx * np.cos(yaw) + dy * np.sin(yaw)
            target_local_y = -dx * np.sin(yaw) + dy * np.cos(yaw)

            # 纯追踪算法核心公式：曲率 = 2 * lateral_error / (lookahead_distance^2)
            # 角速度 = 线速度 * 曲率
            curvature = 2.0 * target_local_y / (self.lookahead_distance ** 2)
            angular_z = self.linear_speed * curvature
            # 简单的角速度限幅
            angular_z = np.clip(angular_z, -1.0, 1.0)

            cmd = Twist()
            cmd.linear.x = self.linear_speed
            cmd.angular.z = angular_z
            self.cmd_pub.publish(cmd)

            self.rate.sleep()

if __name__ == '__main__':
    try:
        PurePursuit()
    except rospy.ROSInterruptException:
        pass

4.3 参数调试与性能优化

将上述节点运行起来，你的机器人应该能沿着方形路径运动了。但效果可能不完美，需要调试几个关键参数：

• lookahead_distance（前瞻距离）：这是最重要的参数。值太小，机器人会对路径误差过于敏感，产生振荡；值太大，机器人会“切割”弯道，跟踪不精确。通常建议设置为机器人长度的0.5到2倍，并通过实验调整。
• linear_speed（线速度）：速度越快，对控制器的响应要求越高。在仿真中，可以从较低速度开始测试。
• 角速度限幅：为了防止在急弯处计算出的角速度过大，导致机器人失控，必须进行限幅。np.clip(angular_z, -1.0, 1.0)就是一个简单的例子。

提升跟踪精度的技巧：

1. 路径预处理：对发布的路径点进行插值，使其更加密集和平滑，可以让纯追踪控制器运行得更稳定。
2. 动态前瞻距离：让前瞻距离与机器人速度成正比，高速时看远些，低速时看近些。
3. 加入PID调节：纯追踪只给出了角速度指令。你可以将计算出的角速度作为期望值，再与机器人当前角速度（可从/odom话题的twist中获取）做差，通过一个PID控制器进行微调，以消除稳态误差。

启动所有节点后，在RViz中添加Path显示，订阅/course_path，同时添加RobotModel和TF，你就能直观地看到机器人的跟踪效果了。第一次看到自己搭建的仿真机器人按照预设路线自动行走时，那种成就感是无可替代的。