差速小车仿真与控制

一、简介

本部分我们主要尝试完成这些部分：

1. 研究四轮差速小车的运动学原理
2. 使用URDF来搭建一个基础的四轮差速小车
3. 配置 Gazebo 仿真环境，加载小车模型到仿真环境
4. 为小车添加控制功能，实现小车的运动控制（使用键盘控制）

---

二、差速小车运动学建模

四轮差速（4WD Differential Drive）是一种常见的移动机器人运动控制方式。 与阿克曼转向不同，四轮差速小车的四个轮子均固定方向，无法单独转向。小车的前进、后退和转向完全依靠 左右两侧车轮的速度差 来实现。这使得它的控制比较简单，下面我们尝试去建立一个四轮差速小车的运动学模型：

基本运动原理：

• 直行：左右两侧车轮转速相同，方向相同。
• 转弯：左右两侧车轮转速不同。
  • 左转：右轮转速 > 左轮转速
  • 右转：左轮转速 > 右轮转速
• 原地旋转：左右两侧车轮反向旋转，速度相同。

运动学模型：

首先，我们要如何表示小车的当前状态，可以从两个角度出发：

• 小车当前机体位姿：$p=(x,y,\theta )$ ,其中其中 $(x,y)$为质心在全局坐标系的位置，$\theta$为车头相对于全局$𝑥$轴的航向角（弧度），右手系，逆时针为正。
• 小车当前速度：
  • $v$: 车体线速度（m/s），正为车头朝向前进方向。
  • $\omega$：车体角速度（rad/s），正为逆时针旋转。

而在实际的控制系统里，我们可以得到的一手数据是什么，主要是四个轮子的转速（通过正交编码器测量）。

所谓的运动学模型，就是我们通过已知的四个轮速和车辆参数去计算前面提到的两个可以用于表达小车状态的参数。或者反过来，基于一个理想的小车状态参数，去反向推算小车的四个轮子该是什么速度，以实现我们对小车的控制。

1. 符号约定

我们可以约定一些基本的表达：

符号	含义	说明
$p=(x,y,\theta )$	小车在全局坐标系下的位姿	$x,y$：质心位置，$\theta$：车头朝向角（弧度），右手系，逆时针为正
$v$	车体线速度（m/s）	沿车头方向，正为前进
$\omega$	车体角速度（rad/s）	绕质心逆时针为正
$L$	左右轮中心线间距（m）	即车体宽度
$R$	车轮半径（m）	将电机角速度换算为轮缘线速度用
A	前左轮（Front Left, FL）	电机编号 A
B	前右轮（Front Right, FR）	电机编号 B
C	后左轮（Rear Left, RL）	电机编号 C
D	后右轮（Rear Right, RR）	电机编号 D
${\omega }_A,{\omega }_B,{\omega }_C,{\omega }_D$	各电机角速度（rad/s）	从编码器或驱动器读取；注意各电机方向符号需统一定义

2. 从电机角速度到侧向速度

我们通过编码器可以测量出每个轮子的角速度，在已知轮子半径的情况下，可以求得每轮线速度：

$$v_i=R\cdot {\omega }_i,i\in \{A,B,C,D\}$$

四个轮子太多，我们简化为左右侧速度定义，同侧轮子速度取均值：

$$V_L=\frac{v_A+v_C}{2}=\frac{R({\omega }_A+{\omega }_C)}{2}$$$$V_R=\frac{v_B+v_D}{2}=\frac{R({\omega }_B+{\omega }_D)}{2}$$

矩阵（从 $\mathit{\omega }=[{\omega }_A,{\omega }_B,{\omega }_C,{\omega }_D{]}^{\mathrm{\top }}$ 到 $[V_L,V_R{]}^{\mathrm{\top }}$）：

$$\left[\begin{array}{c}{V}_L\\ V_R\end{array}\right]=\frac{R}{2}\left[\begin{array}{cccc}1& 0& 1& 0\\ 0& 1& 0& 1\end{array}\right]\left[\begin{array}{c}{\omega }_A\\ {\omega }_B\\ {\omega }_C\\ {\omega }_D\end{array}\right]$$

3. 从左右侧速度到车体速度

对于差速小车来说，左右两侧轮子行驶的距离不同会导致车辆绕着一个瞬时旋转中心转动。

设车辆两侧轮的线速度分别为 $V_L$、$V_R$，左右轮间距为 $L$，则在短时间 $\mathrm{\Delta }t$ 内：

• 左右轮行驶的弧长分别为 $V_L\mathrm{\Delta }t$、$V_R\mathrm{\Delta }t$；
• 它们绕同一圆心转动，因此角速度满足：$$\omega =\frac{\text{右侧线速度}-\text{左侧线速度}}{\text{轮距}}=\frac{V_R-V_L}{L}$$

该式反映了：当右轮比左轮快时（$V_R>V_L$），车辆逆时针旋转（$\omega >0$）。

同时，车体的线速度取左右两侧线速度的平均：

$$v=\frac{V_L+V_R}{2}$$

将上式写成矩阵形式：

$$\left[\begin{array}{c}v\\ \omega \end{array}\right]=\left[\begin{array}{cc}\frac{1}{2}& \frac{1}{2}\\ -\frac{1}{L}& \frac{1}{L}\end{array}\right]\left[\begin{array}{c}{V}_L\\ V_R\end{array}\right]$$

该形式简洁且清晰地表明了四轮差速车的运行模型：

• 第一行是平均（取左右平均得线速度）
• 第二行是差分（取左右差除以轮距得角速度）

结合第二部分，若已知四个轮子的角速度 ${\omega }_A,{\omega }_B,{\omega }_C,{\omega }_D$，结合

$$V_L=\frac{R({\omega }_A+{\omega }_C)}{2},V_R=\frac{R({\omega }_B+{\omega }_D)}{2}$$

可将上面的关系写为完整矩阵形式：

$$\left[\begin{array}{c}v\\ \omega \end{array}\right]=\frac{R}{4}\left[\begin{array}{cccc}1& 1& 1& 1\\ -\frac{2}{L}& \frac{2}{L}& -\frac{2}{L}& \frac{2}{L}\end{array}\right]\left[\begin{array}{c}{\omega }_A\\ {\omega }_B\\ {\omega }_C\\ {\omega }_D\end{array}\right]$$

即：

$$v=\frac{R}{4}({\omega }_A+{\omega }_B+{\omega }_C+{\omega }_D)$$$$\omega =\frac{R}{2L}({\omega }_B+{\omega }_D-{\omega }_A-{\omega }_C)$$

这就是最终的四轮角速度到车体线速度与角速度的线性映射关系

4. 逆运算：由车体速度求四轮角速度

先求左右侧线速度

给定期望的车体线速度 $v$ 和角速度 $\omega$，先求左右侧线速度 $V_L,V_R$：

$$V_L=v-\frac{\omega L}{2},V_R=v+\frac{\omega L}{2}$$

矩阵形式：

$$\left[\begin{array}{c}{V}_L\\ V_R\end{array}\right]=\left[\begin{array}{cc}1& -\frac{L}{2}\\ 1& \frac{L}{2}\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$

若前后同侧轮速度取均值（即前后同步），则对应的轮缘线速度为：

$$v_A=v_C=V_L,v_B=v_D=V_R$$

由此得到电机角速度：

$${\omega }_A={\omega }_C=\frac{V_L}{R},{\omega }_B={\omega }_D=\frac{V_R}{R}$$

合并写成矩阵形式（从 $[v,\omega {]}^{\mathrm{\top }}$ 到 $\mathit{\omega }=[{\omega }_A,{\omega }_B,{\omega }_C,{\omega }_D{]}^{\mathrm{\top }}$）：

$$\left[\begin{array}{c}{\omega }_A\\ {\omega }_B\\ {\omega }_C\\ {\omega }_D\end{array}\right]=\frac{1}{R}\left[\begin{array}{cc}1& -\frac{L}{2}\\ 1& \frac{L}{2}\\ 1& -\frac{L}{2}\\ 1& \frac{L}{2}\end{array}\right]\left[\begin{array}{c}v\\ \omega \end{array}\right]$$

注意：

• 以上分配假定前后同侧轮速度相等，适用于刚性车体且驱动/减速比相同时的常见实现。
• 若需要四轮独立控制（前后可不同），可把 $V_L,V_R$ 作为约束，再根据策略（如最小二乘或优先保持某一轮饱和约束）分配四个轮速；数学上可用伪逆或线性规划求解受限分配。
• 在实际控制里对计算得到的 ${\omega }_i$ 需做限幅、加速度限制与滤波，避免驱动器饱和或机械冲击。

---

5. 姿态的计算（里程计 / Odometry）

连续微分关系（运动学方程）

车体位姿 $(x,y,\theta )$ 满足：

$$\frac{dx}{dt}=v\cos \theta ,\frac{dy}{dt}=v\sin \theta ,\frac{d\theta }{dt}=\omega$$

离散欧拉近似（采样周期 $\mathrm{\Delta }t$）

控制循环常用的简单积分：

$$x_{k+1}=x_k+v_k\cos {\theta }_k\mathrm{\Delta }t$$$$y_{k+1}=y_k+v_k\sin {\theta }_k\mathrm{\Delta }t$$$${\theta }_{k+1}={\theta }_k+{\omega }_k\mathrm{\Delta }t$$

优点：实现简单，计算量小。缺点：当角速度较大或采样周期较长时误差较大。

四、使用URDF构建仿真模型

URDF（Unified Robot Description Format）是一种用于描述机器人结构、关节和惯量的 XML 格式，广泛用于 ROS 系统。 通过 URDF，可以在 Gazebo、RViz 中可视化和仿真小车，验证我们之前推导的运动学模型。

首先我们先复习一下ROS2的基本操作，在我们项目文件夹的part3部分建立一个ROS2工作空间，并建立我门的第一个功能包botlab_diff_description 用于描述机器人。（建议和下面保存相同的目录结构，便于复制后续的命令）：

cd ~/BotLab-Zero/part3
mkdir -p ros2_ws/src
cd ros2_ws/src
ros2 pkg create --build-type ament_python botlab_diff_description --license MIT

创建完成后，目录结构如下：

ros2_ws/
└── src/
    └── botlab_diff_description/
        ├── package.xml
        ├── setup.py
        ├── botlab_diff_description/
        │   ├── __init__.py

4.1 小车结构规划

假设我们的小车为标准四轮差速车：

• 底盘（chassis）：刚性体，挂载四个轮子。
• 轮子（wheel）：前后左右各一个。
• 轮距和轴距：
  • 左右轮距 20 cm
  • 前后轮距 18 cm（轴距）
• 轮半径 R = 0.065 m

结构图示：

  FL      FR
  O------O
  |      |
  |      |
  O------O
  RL      RR

4.2 URDF 文件编写

我们在功能包里新建urdf目录，在目录里新建文件 diff_drive_car.urdf.xacro：

这里不直接使用urdf，而是使用了xacro，不过最终还会转化为urdf

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="diff_drive_car">

  <!-- 参数定义 -->
  <xacro:property name="chassis_length" value="0.26"/>     <!-- 底盘长度 26cm -->
  <xacro:property name="chassis_width"  value="0.22"/>     <!-- 底盘宽度 22cm -->
  <xacro:property name="chassis_height" value="0.08"/>    <!-- 底盘高度 8cm -->
  <xacro:property name="wheel_radius"   value="0.0325"/>  <!-- 轮子半径 3.25cm（直径6.5cm） -->
  <xacro:property name="wheel_width"    value="0.02"/>    <!-- 轮胎厚度 2cm -->
  <xacro:property name="wheelbase"      value="0.18"/>    <!-- 前后轮距 -->
  <xacro:property name="track"          value="0.20"/>    <!-- 左右轮距 -->

  <!-- 底盘 -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
      </geometry>
      <!-- 底盘放在轮子上方 -->
      <origin xyz="0 0 ${wheel_radius + chassis_height/2}" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.1 0.4 0.8 1.0"/>
      </material>
    </visual>

    <collision>
      <geometry>
        <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
      </geometry>
      <origin xyz="0 0 ${wheel_radius + chassis_height/2}" rpy="0 0 0"/>
    </collision>

    <inertial>
      <mass value="5.0"/>
      <inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
    </inertial>
  </link>

  <!-- 定义轮子模板 -->
  <xacro:macro name="wheel" params="prefix x y">
    <link name="wheel_${prefix}">
      <visual>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
        <material name="black">
          <color rgba="0.02 0.02 0.02 1"/>
        </material>
      </visual>
      <collision>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
      </collision>
      <inertial>
        <mass value="0.2"/>
        <inertia ixx="0.001" ixy="0" ixz="0" iyy="0.001" iyz="0" izz="0.001"/>
      </inertial>
    </link>

    <joint name="joint_${prefix}" type="continuous">
      <parent link="base_link"/>
      <child link="wheel_${prefix}"/>
      <origin xyz="${x} ${y} ${wheel_radius}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>

  <!-- 四个轮子 -->
  <xacro:wheel prefix="fl" x="${ wheelbase/2}"  y="${ track/2}"/>
  <xacro:wheel prefix="fr" x="${ wheelbase/2}"  y="-${ track/2}"/>
  <xacro:wheel prefix="rl" x="-${wheelbase/2}"  y="${ track/2}"/>
  <xacro:wheel prefix="rr" x="-${wheelbase/2}"  y="-${ track/2}"/>

</robot>

4.3 编写launch可视化模型

我们编写的xacro不能直接使用Rviz进行可视化，而是要使用ros-humble-xacro这个工具，先将xacro转换为URDF，然后通过Joint State Publisher和 Robot State Publisher这两个工具去发布正确的机器人TF树，这样Rviz里才能正常进行渲染。

安装相关依赖：

sudo apt install ros-$ROS_DISTRO-xacro
sudo apt install ros-$ROS_DISTRO-joint-state-publisher 
sudo apt install ros-$ROS_DISTRO-robot-state-publisher

然后在 botlab_diff_description/launch/ 中添加一个启动文件 view_robot.launch.py：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro

def generate_launch_description():
    pkg_path = get_package_share_directory('botlab_diff_description')
    xacro_file = os.path.join(pkg_path, 'urdf', 'diff_drive_car.urdf.xacro')

    robot_description_config = xacro.process_file(xacro_file)
    robot_desc = robot_description_config.toxml()

    # joint_state_publisher 节点
    joint_state_publisher_node = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen'
    )

    # robot_state_publisher 节点
    robot_state_publisher_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': True, 'robot_description': robot_desc}],
    )

    # rviz2 节点
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen'
    )

    return LaunchDescription([
        joint_state_publisher_node, 
        robot_state_publisher_node,
        rviz_node
    ])

这个 launch 文件的作用是：用 xacro 生成机器人 URDF，然后启动 robot_state_publisher 和 RViz2，从而在 RViz 中查看你的机器人模型。

同时还需要在功能包里注册这两个资源目录，确保你的 botlab_diff_description/setup.py 文件里有这一段：

import os
from glob import glob

data_files=[
	# 让 ROS2 能找到资源
	('share/ament_index/resource_index/packages',
		['resource/' + package_name]),
	('share/' + package_name, ['package.xml']),

	# 安装 launch 文件夹
	(os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),

	# 安装 urdf 文件夹
	(os.path.join('share', package_name, 'urdf'), glob('urdf/*.xacro')),
]

编译运行：

colcon build
source install/setup.bash
ros2 launch botlab_diff_description view_robot.launch.py

4.4 视化模型展示

选择 RobotModel，选择/robot_description 话题，可以看到我们的底盘正常显示了：

这里我们可以把当前的rviz的配置保存起来，保存在包下面的rviz目录下（自己建一个），然后修改launch文件的rviz节点，使其默认加载配置文件 如下：

    # 配置文件路径
    rviz_config_file = os.path.join(pkg_path, 'rviz', 'config.rviz') 
    ······
    # rviz2 节点，加载配置文件
    rviz_node = Node(
        package='rviz2',
        executable='rviz2',
        name='rviz2',
        output='screen',
        arguments=['-d', rviz_config_file]  
    )

五、Gazebo 仿真配置

有了基本的底盘模型，我们先不着急添加各种传感器，我们先尝试使用基础的底盘在Gazebo仿真环境里跑一跑：

5.1 安装Gazebo并准备世界地图

安装：

sudo apt update
sudo apt install gazebo

下载模型(模型比较大，尽量科学上网)：

mkdir -p ~/.gazebo/models
# 手动从 GitHub 克隆官方模型仓库
git clone https://github.com/osrf/gazebo_models ~/.gazebo/models

打开gazebo：

gazebo

界面如下，默认是一个空世界：

下面我们自己建立一个简单的房间环境，在工具栏选择Edit->Building Editor项目：

就可以自行绘制房间了，我绘制的房间如下：

退出编辑，将文件保存在工作区的world目录下，命名为room

5.2 在Gazebo里加载机器人模型

安装相关依赖：

sudo apt install ros-$ROS_DISTRO-gazebo-ros-pkgs

在 botlab_diff_description/launch/ 下新建 gazebo_view.launch.py：

import os
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import ExecuteProcess, TimerAction
from ament_index_python.packages import get_package_share_directory
import xacro

def generate_launch_description():
    pkg_path = get_package_share_directory('botlab_diff_description')
    xacro_file = os.path.join(pkg_path, 'urdf', 'diff_drive_car.urdf.xacro')
    world_file = os.path.join(pkg_path, 'world', 'room.world')  # 自定义世界文件路径

    # 转换 xacro 为 URDF
    robot_description_config = xacro.process_file(xacro_file)
    robot_desc = robot_description_config.toxml()

    # 启动 Gazebo，加载自定义世界
    gazebo_launch = ExecuteProcess(
        cmd=['gazebo', '--verbose', world_file, '-s', 'libgazebo_ros_init.so', '-s', 'libgazebo_ros_factory.so'],
        output='screen'
    )

    # robot_state_publisher 发布 TF
    rsp_node = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        output='screen',
        parameters=[{'use_sim_time': True, 'robot_description': robot_desc}]
    )

    # Gazebo spawn 模型，延时 5 秒执行
    spawn_node = TimerAction(
        period=5.0,
        actions=[Node(
            package='gazebo_ros',
            executable='spawn_entity.py',
            arguments=['-topic', 'robot_description', '-entity', 'diff_drive_car'],
            output='screen'
        )]
    )

    return LaunchDescription([
        gazebo_launch,
        rsp_node,
        spawn_node
    ])

注意：setup.py也要修改，把世界文件目录添加进去，具体代码请参考仓库。

编译运行：

colcon build
source install/setup.bash
ros2 launch botlab_diff_description gazebo_view.launch.py

可以看到在gazebo 里正常显示了机器人，但是机器人没有颜色，并且现在还无法控制

5.3 在URDF文件里添加Gazebo相关标签

首先是颜色，我们这里把轮子改为黑色，车体改为蓝色：

在URDF添加 <gazebo> 块：

注意：<gazebo> 块与<link>块平级放到里面是识别不了的

<link name="base_link">
  <visual>
    <geometry>
      <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
    </geometry>
    <origin xyz="0 0 ${wheel_radius + chassis_height/2}" rpy="0 0 0"/>
    <material name="blue">
      <color rgba="0.1 0.4 0.8 1.0"/>
    </material>
  </visual>
</link>

<!-- Gazebo 可视化材质 -->
<gazebo reference="base_link">
<material>Gazebo/Blue</material>
</gazebo>

轮子类似：

<link name="wheel_fl">
  ...
  <visual>
    ...
  </visual>

</link>
<gazebo reference="wheel_fl">
<material>Gazebo/Black</material>
</gazebo>

其次是物理属性，我们改轮子添加摩擦系数： 在<gazebo>标签中添加 <mu1> 和 <mu2>：

<gazebo reference="wheel_fl">
  <mu1>10.0</mu1> <!-- 主摩擦系数 -->
  <mu2>10.0</mu2> <!-- 副摩擦系数 -->
  <material>Gazebo/Black</material>
</gazebo>

完整示例 xacro（整合颜色和摩擦）:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="diff_drive_car">
  <!-- 参数定义 -->
  <xacro:property name="chassis_length" value="0.26"/>     <!-- 底盘长度 26cm -->
  <xacro:property name="chassis_width"  value="0.22"/>     <!-- 底盘宽度 22cm -->
  <xacro:property name="chassis_height" value="0.08"/>    <!-- 底盘高度 8cm -->
  <xacro:property name="wheel_radius"   value="0.0325"/>  <!-- 轮子半径 3.25cm（直径6.5cm） -->
  <xacro:property name="wheel_width"    value="0.02"/>    <!-- 轮胎厚度 2cm -->
  <xacro:property name="wheelbase"      value="0.18"/>    <!-- 前后轮距 -->
  <xacro:property name="track"          value="0.20"/>    <!-- 左右轮距 -->

  <!-- 底盘 -->
  <link name="base_link">
    <visual>
      <geometry>
        <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
      </geometry>
      <origin xyz="0 0 ${wheel_radius + chassis_height/2}" rpy="0 0 0"/>
      <material name="blue">
        <color rgba="0.1 0.4 0.8 1.0"/>
      </material>
    </visual>

    <collision>
      <geometry>
        <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
      </geometry>
      <origin xyz="0 0 ${wheel_radius + chassis_height/2}" rpy="0 0 0"/>
    </collision>

    <inertial>
      <mass value="2.0"/>
      <inertia ixx="0.05" ixy="0" ixz="0" iyy="0.05" iyz="0" izz="0.08"/>
    </inertial>
  </link>

  <!-- Gazebo 物理标签 -->
  <gazebo reference="base_link">
    <material>Gazebo/Blue</material>
  </gazebo>

  <!-- 定义轮子模板 -->
  <xacro:macro name="wheel" params="prefix x y">
    <link name="wheel_${prefix}">
      <visual>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
        <material name="black">
          <color rgba="0.02 0.02 0.02 1"/>
        </material>
      </visual>

      <collision>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
      </collision>

      <inertial>
        <mass value="0.1"/>
        <inertia ixx="0.0005" ixy="0" ixz="0" iyy="0.0005" iyz="0" izz="0.0005"/>
      </inertial>


    </link>
    <!-- Gazebo 物理属性 -->
    <gazebo reference="wheel_${prefix}">
      <material>Gazebo/Black</material>
      <mu1>1.0</mu1>
      <mu2>1.0</mu2>
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <minDepth>0.001</minDepth>
      <maxVel>1.0</maxVel>
    </gazebo>
    <joint name="joint_${prefix}" type="continuous">
        <parent link="base_link"/>
        <child link="wheel_${prefix}"/>
        <origin xyz="${x} ${y} ${wheel_radius}" rpy="0 0 0"/>
        <axis xyz="0 1 0"/>
        <dynamics damping="0.1" friction="0.1"/>
      </joint>
  </xacro:macro>

  <!-- 四个轮子 -->
  <xacro:wheel prefix="fl" x="${ wheelbase/2}"  y="${ track/2}"/>
  <xacro:wheel prefix="fr" x="${ wheelbase/2}"  y="-${ track/2}"/>
  <xacro:wheel prefix="rl" x="-${wheelbase/2}"  y="${ track/2}"/>
  <xacro:wheel prefix="rr" x="-${wheelbase/2}"  y="-${ track/2}"/>

</robot>

最终显示效果如下：

PS，这里我尝试修改颜色后在 gazebo 中并没有生效，目前没找到解决方案，但是不影响下面的运动仿真，就继续往下走了。

六、运动控制实现

gazebo标签除了用于设置颜色、摩擦等物理参数，还可以用于添加各种插件，插件用于控制机器人运动，或者继续传感器仿真。

建立urdf/gazebo_control_plugin.xacro文件，用于实现机器人控制：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
    
    <xacro:macro name="gazebo_control_plugin">
        <!-- 差速驱动插件 - 使用后轮作为驱动轮 -->
        <gazebo>
            <plugin name='diff_drive' filename='libgazebo_ros_diff_drive.so'>
                <ros>
                    <namespace>/</namespace>
                    <remapping>cmd_vel:=cmd_vel</remapping>
                    <remapping>odom:=odom</remapping>
                </ros>
                
                <update_rate>50</update_rate>
                
                <!-- 后轮作为驱动轮 -->
                <left_joint>joint_rl</left_joint>
                <right_joint>joint_rr</right_joint>
                
                <!-- 运动学参数 -->
                <wheel_separation>${track}</wheel_separation>
                <wheel_diameter>${wheel_radius * 2}</wheel_diameter>
                
                <!-- 限制参数 -->
                <max_wheel_torque>50</max_wheel_torque>
                <max_wheel_acceleration>5.0</max_wheel_acceleration>
                
                <!-- 控制参数 -->
                <command_topic>cmd_vel</command_topic>
                <odometry_topic>odom</odometry_topic>
                <odometry_frame>odom</odometry_frame>
                <robot_base_frame>base_link</robot_base_frame>
                
                <!-- 输出设置 -->
                <publish_odom>true</publish_odom>
                <publish_odom_tf>true</publish_odom_tf>
                <publish_wheel_tf>false</publish_wheel_tf>
                
                <!-- 坐标系设置 -->
                <odometry_frame>odom</odometry_frame>
                <robot_base_frame>base_link</robot_base_frame>
            </plugin>
        </gazebo>
        
        <!-- 前轮跟随插件 - 让前轮被动跟随 -->
        <gazebo reference="joint_fl">
            <provideFeedback>true</provideFeedback>
        </gazebo>
        <gazebo reference="joint_fr">
            <provideFeedback>true</provideFeedback>
        </gazebo>
        
        <!-- 关节状态发布器 - 发布所有轮子的状态 -->
        <gazebo>
            <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
                <ros>
                    <namespace>/</namespace>
                    <remapping>~/out:=joint_states</remapping>
                </ros>
                <update_rate>50</update_rate>
                <joint_name>joint_fl</joint_name>
                <joint_name>joint_fr</joint_name>
                <joint_name>joint_rl</joint_name>
                <joint_name>joint_rr</joint_name>
            </plugin>
        </gazebo>
        
    </xacro:macro>
    
</robot>

上面的代码主要实现了：

• 差速驱动插件配置 ：使用 libgazebo_ros_diff_drive.so
• 四轮差速控制 ：同侧轮子速度保持一致
• 订阅 /cmd_vel 话题接收速度命令
• 发布 /odom 话题提供里程计信息
• 坐标系发布 ：自动发布odom到base_link的TF变换

并在diff_drive_car.urdf.xacro中引入：

  <!-- 引入Gazebo控制插件 -->
  <xacro:include filename="$(find botlab_diff_description)/urdf/gazebo_control_plugin.xacro"/>
  
  <!-- 调用差速驱动控制插件 -->
  <xacro:gazebo_control_plugin/>

重新编译运行：

colcon build
source install/setup.bash
ros2 launch botlab_diff_description gazebo_view.launch.py

可以尝试使用键盘控制节点进行控制：ros2 run teleop_twist_keyboard teleop_twist_keyboard

这里我们简单的控制小车离开原点：

但是由于 libgazebo_ros_diff_drive 插件 只能支持两个轮子的差速系统，我们这里设定使用后轮作为驱动轮，有的情况下，前轮还是会打滑，所以运行的效果并不理想。

在后续的部分，我们会改用ros2_control来进行底盘的控制，充分利用起四个电机的机动性。

七、运动控制改进

7.1 机器人模块化

从这部分我们在前面的基础上，对机器人的URDF进行模块化，便于下一节添加其他传感器，并且改用ros2_control来进行底盘的模拟控制，实现真正四轮差速模拟。

我们在part3/ros2_ws/src/botlab_diff_description/urdf下新建 botlab_zero 文件夹来存储整个机器人的URDF:

我们在 botlab_diff_description/urdf/botlab_zero/ 目录下建立以下结构：

urdf/
└── botlab_zero/
    ├── base.urdf.xacro          # 底盘主体
    ├── wheel.urdf.xacro         # 轮子模块
    ├── control.urdf.xacro       # ros2_control 配置
    └── botlab_zero.urdf.xacro   # 整体组合文件

这样拆分后，修改或替换任意模块都会更方便。

首先建立单个轮子的URDF：wheel.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="wheel" params="prefix x y wheel_radius wheel_width">
    <link name="wheel_${prefix}">
      <visual>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
        <material name="black">
          <color rgba="0.1 0.1 0.1 1"/>
        </material>
      </visual>

      <collision>
        <geometry>
          <cylinder radius="${wheel_radius}" length="${wheel_width}"/>
        </geometry>
        <origin xyz="0 0 0" rpy="1.5708 0 0"/>
      </collision>

      <inertial>
        <mass value="0.15"/>
        <inertia ixx="0.001" ixy="0" ixz="0"
                 iyy="0.001" iyz="0" izz="0.001"/>
      </inertial>
    </link>
  
  <gazebo reference="wheel_${prefix}">
	<mu1>8.0</mu1>
	<mu2>8.0</mu2>
	<material>Gazebo/Black</material>
  </gazebo>
      
    <joint name="joint_${prefix}" type="continuous">
      <parent link="base_link"/>
      <child link="wheel_${prefix}"/>
      <origin xyz="${x} ${y} ${wheel_radius}" rpy="0 0 0"/>
      <axis xyz="0 1 0"/>
    </joint>
  </xacro:macro>

</robot>

底盘模块：base.urdf.xacro

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <xacro:macro name="base" params="chassis_length chassis_width chassis_height wheel_radius">
    <link name="base_link">
      <visual>
        <geometry>
          <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
        </geometry>
        <origin xyz="0 0 ${wheel_radius + chassis_height/2}" rpy="0 0 0"/>
        <material name="blue">
          <color rgba="0.1 0.4 0.8 1"/>
        </material>
      </visual>

      <collision>
        <geometry>
          <box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
        </geometry>
        <origin xyz="0 0 ${wheel_radius + chassis_height/2}" rpy="0 0 0"/>
      </collision>

      <inertial>
        <mass value="5.0"/>
        <inertia ixx="0.1" ixy="0" ixz="0"
                 iyy="0.1" iyz="0" izz="0.1"/>
      </inertial>


    </link>
  
	<gazebo reference="base_link">
		<mu1>1.0</mu1>
		<mu2>1.0</mu2>
		<material>Gazebo/Blue</material>
    </gazebo>
  </xacro:macro>

</robot>

整体模块：botlab_zero.urdf.xacro

在这个文件中组装轮子和底盘，实现完整机器人模型：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="botlab_zero">

  <!-- 引入模块 -->
  <xacro:include filename="$(find botlab_diff_description)/urdf/botlab_zero/base.urdf.xacro"/>
  <xacro:include filename="$(find botlab_diff_description)/urdf/botlab_zero/wheel.urdf.xacro"/>

  <!-- 参数定义 -->
  <xacro:property name="chassis_length" value="0.26"/>     <!-- 底盘长度 26cm -->
  <xacro:property name="chassis_width"  value="0.22"/>     <!-- 底盘宽度 22cm -->
  <xacro:property name="chassis_height" value="0.08"/>     <!-- 底盘高度 8cm -->
  <xacro:property name="wheel_radius"   value="0.0325"/>   <!-- 轮子半径 3.25cm -->
  <xacro:property name="wheel_width"    value="0.03"/>     <!-- 轮胎厚度 3cm -->
  <xacro:property name="wheelbase"      value="0.18"/>     <!-- 前后轮距 -->
  <xacro:property name="track"          value="0.20"/>     <!-- 左右轮距 -->

  <!-- 底盘模块 -->
  <xacro:base
    chassis_length="${chassis_length}"
    chassis_width="${chassis_width}"
    chassis_height="${chassis_height}"
    wheel_radius="${wheel_radius}"/>

  <!-- 四个轮子 -->
  <xacro:wheel prefix="fl"
               x="${ wheelbase/2}"
               y="${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

  <xacro:wheel prefix="fr"
               x="${ wheelbase/2}"
               y="-${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

  <xacro:wheel prefix="rl"
               x="-${ wheelbase/2}"
               y="${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

  <xacro:wheel prefix="rr"
               x="-${ wheelbase/2}"
               y="-${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

</robot>

注意：setup.py 的 data_files 列表也要做对应的修改，添加一行，把我们新加的文件夹包含进去：

    (os.path.join('share', package_name, 'urdf/botlab_zero'), glob('urdf/botlab_zero/*.xacro')),

然后我们写一个新的launch文件来加载机器人到Gazebo 中：

在botlab_diff_description/launch/下新建一个用于预览的 launch 文件：view_botlab_zero.launch.py:

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os
import xacro

def generate_launch_description():
    pkg_path = get_package_share_directory('botlab_diff_description')
    xacro_file = os.path.join(pkg_path, 'urdf', 'botlab_zero.urdf.xacro')

    # 生成 robot_description
    robot_description_config = xacro.process_file(xacro_file)
    robot_desc = robot_description_config.toxml()

    # 启动 Gazebo
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
        )
    )

    # 将机器人模型加载到 Gazebo
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'botlab_zero', '-topic', 'robot_description'],
        output='screen'
    )

    # 可视化节点（可选）
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_desc}],
        output='screen'
    )

    return LaunchDescription([
        gazebo,
        robot_state_publisher,
        spawn_entity
    ])

编译运行：

colcon build
source install/setup.bash
ros2 launch botlab_diff_description view_botlab_zero.launch.py

可以看到机器人也是可以正常加载的，下面我们给机器人集成ros2_control控制插件：

7.2 集成ros2_control

安装依赖：

# 安装 ros2_control 核心包
sudo apt install ros-humble-ros2-control ros-humble-ros2-controllers

# 安装 ros2_control 的 Gazebo 插件
sudo apt install ros-humble-gazebo-ros2-control

这里的 gazebo-ros2-control 插件提供 libgazebo_ros2_control.so，用于把 ROS2 控制器和 Gazebo 的模拟物理接口对接。

下载或配置所需的控制器:

sudo apt install ros-humble-diff-drive-controller
sudo apt install ros-humble-joint-state-broadcaster

• diff_drive_controller：四轮差速或两轮差速控制
• joint_state_broadcaster：发布轮子关节状态

---

定义四轮差速控制接口：

在 botlab_diff_description/urdf/botlab_zero/ 下新建 control.urdf.xacro：

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- ros2_control 四轮差速底盘 transmission -->
  <xacro:macro name="diff_drive_control" params="prefix">
    <transmission name="${prefix}_fl_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_fl">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
      <actuator name="fl_wheel_motor">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

    <transmission name="${prefix}_fr_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_fr">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
      <actuator name="fr_wheel_motor">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

    <transmission name="${prefix}_rl_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_rl">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
      <actuator name="rl_wheel_motor">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>

    <transmission name="${prefix}_rr_wheel_trans">
      <type>transmission_interface/SimpleTransmission</type>
      <joint name="joint_rr">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
      </joint>
      <actuator name="rr_wheel_motor">
        <hardwareInterface>hardware_interface/VelocityJointInterface</hardwareInterface>
        <mechanicalReduction>1</mechanicalReduction>
      </actuator>
    </transmission>
  </xacro:macro>

</robot>

这个文件定义了四个轮子的 velocity 控制接口，方便用 ros2_control 来控制速度。

在botlab_zero.urdf.xacro 中引入 ros2_control:

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="botlab_zero">

  <!-- 引入模块 -->
  <xacro:include filename="$(find botlab_diff_description)/urdf/botlab_zero/base.urdf.xacro"/>
  <xacro:include filename="$(find botlab_diff_description)/urdf/botlab_zero/wheel.urdf.xacro"/>
  <xacro:include filename="$(find botlab_diff_description)/urdf/botlab_zero/control.urdf.xacro"/>

  <!-- 参数定义（保持与原机器人一致） -->
  <xacro:property name="chassis_length" value="0.26"/>     <!-- 底盘长度 26cm -->
  <xacro:property name="chassis_width"  value="0.22"/>     <!-- 底盘宽度 22cm -->
  <xacro:property name="chassis_height" value="0.08"/>     <!-- 底盘高度 8cm -->
  <xacro:property name="wheel_radius"   value="0.0325"/>   <!-- 轮子半径 3.25cm -->
  <xacro:property name="wheel_width"    value="0.03"/>     <!-- 轮胎厚度 3cm -->
  <xacro:property name="wheelbase"      value="0.18"/>     <!-- 前后轮距 -->
  <xacro:property name="track"          value="0.20"/>     <!-- 左右轮距 -->

  <!-- 底盘模块 -->
  <xacro:base
    chassis_length="${chassis_length}"
    chassis_width="${chassis_width}"
    chassis_height="${chassis_height}"
    wheel_radius="${wheel_radius}"/>

  <!-- 四个轮子：位置完全复刻原机器人 -->
  <xacro:wheel prefix="fl"
               x="${ wheelbase/2}"
               y="${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

  <xacro:wheel prefix="fr"
               x="${ wheelbase/2}"
               y="-${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

  <xacro:wheel prefix="rl"
               x="-${ wheelbase/2}"
               y="${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

  <xacro:wheel prefix="rr"
               x="-${ wheelbase/2}"
               y="-${ track/2}"
               wheel_radius="${wheel_radius}"
               wheel_width="${wheel_width}"/>

  <!-- ros2_control system -->
  <ros2_control name="botlab_zero_hw" type="system">
    <hardware>
      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
    </hardware>

    <joint name="joint_fl">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_fr">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_rl">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_rr">
      <command_interface name="velocity"/>
      <state_interface name="position"/>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>

  <!-- Gazebo 配置 -->
  <gazebo reference="base_link">
    <mu1>1.0</mu1>
    <mu2>1.0</mu2>
    <material>Gazebo/Blue</material>
  </gazebo>

  <!-- ROS2 Control 插件 -->
  <gazebo>
    <plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
      <parameters>$(find botlab_diff_description)/config/botlab_zero_controllers.yaml</parameters>
      <ros>
        <remapping>/diff_drive_controller/cmd_vel_unstamped:=/cmd_vel</remapping>
        <remapping>/diff_drive_controller/odom:=/odom</remapping>
      </ros>
    </plugin>
  </gazebo>
</robot>

最后在botlab_diff_description/config/下新建文件：botlab_zero_controllers.yaml：

controller_manager:
  ros__parameters:
    update_rate: 100
    use_sim_time: true

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    diff_drive_controller:
      type: diff_drive_controller/DiffDriveController

joint_state_broadcaster:
  ros__parameters:
    joints:
      - joint_fl
      - joint_fr
      - joint_rl
      - joint_rr

diff_drive_controller:
  ros__parameters:
    left_wheel_names: ["joint_fl", "joint_rl"]
    right_wheel_names: ["joint_fr", "joint_rr"]
    wheel_separation: 0.20
    wheel_radius: 0.0325
    publish_rate: 50.0
    odom_frame_id: odom
    base_frame_id: base_link
    pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]
    cmd_vel_timeout: 0.5
    use_stamped_vel: false

别忘了在setup.py里面新增：

(os.path.join('share', package_name, 'config'), glob('config/*.yaml')),

最后写一个新的launch文件： gazebo_botlab_zero.launch.py

from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import IncludeLaunchDescription, RegisterEventHandler
from launch.event_handlers import OnProcessExit
from launch.launch_description_sources import PythonLaunchDescriptionSource
from ament_index_python.packages import get_package_share_directory
import os
import xacro

def generate_launch_description():
    pkg_path = get_package_share_directory('botlab_diff_description')
    xacro_file = os.path.join(pkg_path, 'urdf', 'botlab_zero', 'botlab_zero.urdf.xacro')

    # 生成 robot_description
    robot_description_config = xacro.process_file(xacro_file)
    robot_desc = robot_description_config.toxml()

    # 启动 Gazebo
    gazebo = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            os.path.join(get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')
        )
    )

    # robot_state_publisher
    robot_state_publisher = Node(
        package='robot_state_publisher',
        executable='robot_state_publisher',
        parameters=[{'robot_description': robot_desc, 'use_sim_time': True}],
        output='screen'
    )

    # spawn_entity
    spawn_entity = Node(
        package='gazebo_ros',
        executable='spawn_entity.py',
        arguments=['-entity', 'botlab_zero', '-topic', 'robot_description'],
        output='screen'
    )

    # joint_state_broadcaster
    joint_state_broadcaster_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['joint_state_broadcaster'],
        output='screen'
    )

    # diff_drive_controller
    diff_drive_controller_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=['diff_drive_controller'],
        output='screen'
    )

    # 事件处理：spawn_entity 完成后启动 joint_state_broadcaster
    joint_state_handler = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=spawn_entity,
            on_exit=[joint_state_broadcaster_spawner]
        )
    )

    # 事件处理：joint_state_broadcaster 完成后启动 diff_drive_controller
    diff_drive_handler = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[diff_drive_controller_spawner]
        )
    )

    return LaunchDescription([
        gazebo,
        robot_state_publisher,
        spawn_entity,
        joint_state_handler,
        diff_drive_handler
    ])

然后编译运行项目：

colcon build
source install/setup.bash
ros2 launch botlab_diff_description gazebo_botlab_zero.launch.py

在Gazebo中显示如下：

再打开我们之前使用的Rviz2：

ros2 launch botlab_diff_description view_robot.launch.py

添加陀螺仪组件，也是可以显示的：

最后启动键盘控制节点：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

尝试遥控机器人再平地里走：

可以看到对应Rviz里小车也绘制出轨迹了，但是误差很大，Gazebo向右前移动一点点，rviz里角度就偏差很大，这是由于我们模拟的轮式里程计在小车转弯打滑的情况下，会导致很大的误差，这也是轮式里程计一个很大的缺点。