ROS2多机器人协同开发：从TF2坐标变换到Gazebo仿真与通信实战

在ROS2机器人开发中，单车仿真与导航是基础，但如何将单车能力扩展到多车协同，并实现从仿真到真实坐标系的统一管理，是迈向复杂机器人系统开发的关键一步。很多开发者在尝试构建车队时，常常卡在坐标变换的复杂性、Gazebo仿真环境的搭建以及多车通信的同步问题上，网上资料往往只聚焦于单一环节。本文将整合一套从单车到车队的完整进阶方案，涵盖TF2坐标变换原理、Gazebo多机器人仿真环境搭建、以及基于ROS2通信的多车协同逻辑实现。无论你是想深入学习ROS2的进阶开发者，还是正在规划多机器人项目的工程师，都能从本文获得可直接复用的代码与配置。

1. 背景与核心概念：从单车到车队的挑战

在机器人学中，坐标变换是描述机器人各部件（如底盘、激光雷达、相机）以及多个机器人之间相对位置关系的数学基础。ROS2中的TF2库是管理这种变换关系的核心工具。对于单车，我们通常处理的是机器人本体坐标系（如base_link）与传感器坐标系（如laser_link）之间的静态或动态变换。而对于多车协同，挑战则升级为管理一个全局坐标系（如map）下，多个机器人本体坐标系（如robot1/base_link,robot2/base_link）之间的动态关系，这是实现车队编队、避障和任务分配的前提。

Gazebo仿真为我们提供了一个安全、可重复的测试环境。在Gazebo中模拟多机器人，不仅需要为每个机器人创建独立的模型（包括外观、物理属性和传感器），还需要为每个机器人实例化独立的ROS2节点和控制插件，并确保它们在同一个仿真世界中互不干扰地运行。

多车协同的本质是分布式系统的通信与协调。在ROS2的语境下，这意味着每个机器人作为一个独立的节点（或节点集合），通过话题（Topic）、服务（Service）或动作（Action）进行数据交换和指令同步。协同逻辑可以很简单，比如让跟随车追踪领航车的位置；也可以很复杂，如基于拍卖算法的动态任务分配。

将这三者结合，就构成了“坐标变换+Gazebo仿真+多车协同”的完整技术栈。掌握它，你就能构建出用于物流、巡检、编队表演等场景的多机器人系统原型。

2. 环境准备与版本说明

本文的实战环境基于当前ROS2的长期支持版本。请确保你的系统已安装以下软件：

• 操作系统: Ubuntu 22.04 LTS (Jammy Jellyfish)
• ROS2 发行版:Humble Hawksbill(推荐) 或 Rolling Ridley。本文示例代码主要基于Humble测试。
• 仿真器: Gazebo Garden (与ROS2 Humble配套) 或 Gazebo Classic (Gazebo 11)。本文使用Gazebo Garden进行演示，因为它与ROS2集成更紧密。
• 构建工具: Colcon
• 编程语言: Python 3.10 或 C++ 20。本文将提供Python示例，因其更易于理解和快速原型开发。
• 必要的ROS2功能包:

  # 更新系统并安装ROS2 Humble (桌面完整版包含Gazebo) sudo apt update && sudo apt upgrade -y sudo apt install ros-humble-desktop-full -y # 安装Gazebo Garden (如果桌面完整版未包含) sudo apt install ros-humble-ros-gz -y # 安装本文相关的其他功能包 sudo apt install ros-humble-turtlebot3-* ros-humble-nav2-* ros-humble-gazebo-ros-pkgs -y # 注意：turtlebot3和nav2用于提供机器人模型和导航栈，非必须，但便于演示。

版本兼容性提示：ROS2版本、Gazebo版本以及各功能包（如ros_gz）的版本必须匹配。使用非LTS版本（如Rolling）时，部分API可能有变，请以官方文档为准。本文示例将重点演示原理和通用方法，你可以用自己的机器人模型替换TurtleBot3。

3. 核心原理与组件拆解

3.1 TF2坐标变换深度解析

TF2库维护着一个“坐标变换树”。任何两个坐标系之间的变换都可以通过一条连接它们的路径上的变换连续相乘得到。

• 广播器 (Broadcaster): 发布两个坐标系间的变换关系。例如，发布从base_link到laser_link的变换。

  # 文件: robot_tf2_broadcaster.py import rclpy from rclpy.node import Node from tf2_ros import TransformBroadcaster from geometry_msgs.msg import TransformStamped import math class RobotTFBroadcaster(Node): def __init__(self, robot_name='robot1'): super().__init__(f'{robot_name}_tf_broadcaster') self.robot_name = robot_name self.tf_broadcaster = TransformBroadcaster(self) # 假设我们发布一个从 `map` 到 `robot1/base_link` 的虚拟变换 # 在实际中，这个变换可能来自定位系统（如AMCL） self.timer = self.create_timer(0.1, self.broadcast_timer_callback) self.x, self.y, self.yaw = 0.0, 0.0, 0.0 # 机器人位姿 def broadcast_timer_callback(self): t = TransformStamped() t.header.stamp = self.get_clock().now().to_msg() t.header.frame_id = 'map' # 父坐标系 t.child_frame_id = f'{self.robot_name}/base_link' # 子坐标系 t.transform.translation.x = self.x t.transform.translation.y = self.y t.transform.translation.z = 0.0 # 将偏航角转换为四元数 from tf_transformations import quaternion_from_euler q = quaternion_from_euler(0, 0, self.yaw) t.transform.rotation.x = q[0] t.transform.rotation.y = q[1] t.transform.rotation.z = q[2] t.transform.rotation.w = q[3] self.tf_broadcaster.sendTransform(t) # 简单让机器人绕圈 self.yaw += 0.01

• 监听器 (Listener): 查询两个坐标系间的变换。这是多车协同中，一辆车获取另一辆车位置的关键。

  # 文件: robot_tf2_listener.py import rclpy from rclpy.node import Node from tf2_ros import Buffer, TransformListener from geometry_msgs.msg import PointStamped import tf2_geometry_msgs # 用于转换带坐标系的点 class RobotTFListener(Node): def __init__(self, source_robot='robot1', target_robot='robot2'): super().__init__(f'tf_listener_{source_robot}_to_{target_robot}') self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.source_frame = f'{source_robot}/base_link' self.target_frame = f'{target_robot}/base_link' self.timer = self.create_timer(1.0, self.lookup_transform) def lookup_transform(self): try: # 查找从 target_frame 到 source_frame 的变换 # 即：在 target_frame 中，source_frame 的位置 trans = self.tf_buffer.lookup_transform( self.target_frame, self.source_frame, rclpy.time.Time() ) self.get_logger().info( f'{self.source_frame} 在 {self.target_frame} 中的位置: ' f'x={trans.transform.translation.x:.2f}, ' f'y={trans.transform.translation.y:.2f}' ) except Exception as e: self.get_logger().warn(f'无法获取变换: {e}')

关键点：在多机器人系统中，为每个机器人的坐标系添加命名空间（如robot1/）是避免冲突的最佳实践。map坐标系通常作为所有机器人共享的全局父坐标系。

3.2 Gazebo多机器人仿真模型

在Gazebo中生成多个机器人，主要有两种方式：

1. 在SDF世界文件中复制模型：直接在.world文件中定义多个<model>，每个都有独立的名称和初始位姿。这种方式简单，但所有机器人实例共享同一个模型定义，如果模型插件通过硬编码的机器人名访问话题，会产生冲突。
2. 通过启动文件动态生成：使用ROS2启动文件，配合spawn_entity.py这样的工具，为同一个URDF/SDF模型文件生成多个实例，并在生成时为每个实例指定唯一的ROS命名空间和话题重映射。这是推荐的生产级方法，它能实现真正的节点隔离。

3.3 多车协同通信模式

• 话题 (Topics) - 数据流: 适用于持续性的数据发布，如领航车的实时位姿 (/robot1/odom)。跟随车可以订阅领航车的位姿话题。
• 服务 (Services) - 请求/响应: 适用于触发一次性动作并获取结果，如请求某辆车执行一个特定任务。
• 动作 (Actions) - 长时任务: 适用于有持续时间、可反馈、可取消的任务，如让一辆车导航到某个目标点，并在过程中反馈进度。
• 参数 (Parameters) - 配置: 用于动态调整车队行为，如编队间距、最大速度。

命名空间是核心：通过为每辆车的节点、话题、服务、动作添加独立的命名空间（如/robot1/,/robot2/），可以完美隔离各车的通信，避免话题重名导致的混乱。

4. 完整实战：搭建两车协同仿真系统

我们将创建一个项目，包含两辆TurtleBot3机器人（一辆领航，一辆跟随），在Gazebo中仿真，并通过TF2和话题通信实现简单的跟随行为。

4.1 创建项目工作空间与结构

mkdir -p ~/multi_robot_ws/src cd ~/multi_robot_ws/src # 克隆必要的功能包（以TurtleBot3为例） git clone -b humble-devel https://github.com/ROBOTIS-GIT/turtlebot3_simulations.git # 创建我们自己的功能包 ros2 pkg create multi_robot_demo --build-type ament_python --dependencies rclpy geometry_msgs tf2_ros tf2_geometry_msgs cd ~/multi_robot_ws

项目结构规划如下：

multi_robot_ws/ └── src/ ├── turtlebot3_simulations/ # 第三方仿真包 └── multi_robot_demo/ # 我们的功能包 ├── launch/ │ ├── multi_robot.launch.py # 主启动文件 │ └── spawn_robot.launch.py # 生成单个机器人的子启动文件 ├── worlds/ │ └── empty.world # Gazebo世界文件（可复用现有的） ├── config/ │ └── follower.yaml # 跟随车控制器参数 ├── multi_robot_demo/ │ ├── __init__.py │ ├── robot_tf2_broadcaster.py # 3.1节中的TF广播器（需修改支持多车） │ ├── robot_tf2_listener.py # 3.1节中的TF监听器 │ └── follower_controller.py # 跟随车核心控制器 ├── package.xml └── setup.py

4.2 编写多机器人启动文件

这是最关键的一步，它负责启动Gazebo、生成机器人、为每个机器人启动独立的节点。

# 文件: launch/multi_robot.launch.py import os 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 def generate_launch_description(): # 定义机器人列表 robots = [ {'name': 'robot1', 'x': '0.0', 'y': '0.0', 'yaw': '0.0'}, {'name': 'robot2', 'x': '2.0', 'y': '0.0', 'yaw': '0.0'}, ] # 启动Gazebo仿真世界 gazebo_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare('gazebo_ros'), 'launch', 'gazebo.launch.py' ]) ]), launch_arguments={ 'world': PathJoinSubstitution([ FindPackageShare('multi_robot_demo'), 'worlds', 'empty.world' ]), }.items() ) ld = LaunchDescription([gazebo_launch]) # 为每个机器人生成实例 for robot in robots: spawn_robot_launch = IncludeLaunchDescription( PythonLaunchDescriptionSource([ PathJoinSubstitution([ FindPackageShare('multi_robot_demo'), 'launch', 'spawn_robot.launch.py' ]) ]), launch_arguments={ 'robot_name': robot['name'], 'x': robot['x'], 'y': robot['y'], 'yaw': robot['yaw'], }.items() ) ld.add_action(spawn_robot_launch) # 启动每个机器人的TF广播节点（使用修改后的广播器） tf_broadcaster_node = Node( package='multi_robot_demo', executable='robot_tf2_broadcaster', name=f'{robot["name"]}_tf_broadcaster', namespace=robot['name'], # 关键：放入独立命名空间 parameters=[{'robot_name': robot['name']}] ) ld.add_action(tf_broadcaster_node) # 启动一个全局的TF监听节点（用于调试或全局监控） tf_listener_node = Node( package='multi_robot_demo', executable='robot_tf2_listener', name='global_tf_listener', parameters=[{'source_robot': 'robot1', 'target_robot': 'robot2'}] ) ld.add_action(tf_listener_node) # 启动robot2的跟随控制器 follower_controller_node = Node( package='multi_robot_demo', executable='follower_controller', name='follower_controller', namespace='robot2', # 控制器在robot2的命名空间下运行 parameters=[PathJoinSubstitution([ FindPackageShare('multi_robot_demo'), 'config', 'follower.yaml' ])] ) ld.add_action(follower_controller_node) return ld

# 文件: launch/spawn_robot.launch.py from launch import LaunchDescription from launch.actions import ExecuteProcess, RegisterEventHandler from launch.event_handlers import OnProcessExit from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node def generate_launch_description(): robot_name = LaunchConfiguration('robot_name') x = LaunchConfiguration('x') y = LaunchConfiguration('y') yaw = LaunchConfiguration('yaw') # 使用 gazebo_ros 提供的 spawn_entity 节点生成机器人模型 spawn_entity = Node( package='gazebo_ros', executable='spawn_entity.py', arguments=[ '-entity', robot_name, '-topic', f'/world/empty/model/{robot_name}/pose', # 注意：这里需要根据实际模型调整 # 更通用的方法是使用 -file 指定模型文件，并为每个实例重命名 # '-file', $(find-pkg-share turtlebot3_gazebo)/models/turtlebot3_waffle/model.sdf', # '-robot_namespace', robot_name, '-x', x, '-y', y, '-z', '0.1', '-Y', yaw, ], output='screen', ) # 注意：上述spawn_entity参数可能需要根据你的具体模型和Gazebo版本调整。 # 一个更可靠的方法是先启动一个发布机器人描述（robot_description）的节点，然后spawn_entity订阅它。 # 这里为了简化，假设模型已存在于Gazebo世界中。 return LaunchDescription([ spawn_entity, ])

4.3 实现跟随车控制器

跟随车（robot2）的核心逻辑：监听领航车（robot1）在map坐标系下的位置，计算自身与目标位置的误差，并发布速度指令。

# 文件: multi_robot_demo/follower_controller.py import rclpy from rclpy.node import Node from tf2_ros import Buffer, TransformListener from geometry_msgs.msg import Twist, PointStamped import math class FollowerController(Node): def __init__(self): super().__init__('follower_controller') # 初始化TF监听器 self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) # 创建速度指令发布器，发布到本命名空间下的cmd_vel self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10) # 控制定时器 self.timer = self.create_timer(0.1, self.control_loop) # 10Hz # 控制器参数 self.declare_parameter('leader_name', 'robot1') self.declare_parameter('follow_distance', 1.0) # 期望跟随距离 self.leader_name = self.get_parameter('leader_name').value self.follow_distance = self.get_parameter('follow_distance').value self.get_logger().info(f'跟随控制器启动，跟随目标: {self.leader_name}， 距离: {self.follow_distance}m') def control_loop(self): try: # 1. 获取领航车在map中的位置 leader_pose = self.tf_buffer.lookup_transform( 'map', f'{self.leader_name}/base_link', rclpy.time.Time() ) # 2. 获取自身在map中的位置 follower_pose = self.tf_buffer.lookup_transform( 'map', 'base_link', # 注意：因为本节点在robot2命名空间，所以base_link就是robot2/base_link rclpy.time.Time() ) # 3. 计算位置误差（简单的P控制器） dx = leader_pose.transform.translation.x - follower_pose.transform.translation.x dy = leader_pose.transform.translation.y - follower_pose.transform.translation.y distance = math.sqrt(dx**2 + dy**2) desired_dx = dx * (self.follow_distance / distance) if distance > 0 else 0 desired_dy = dy * (self.follow_distance / distance) if distance > 0 else 0 error_x = dx - desired_dx error_y = dy - desired_dy # 4. 生成速度指令（简化版，仅向目标点移动） cmd_vel = Twist() linear_gain = 0.5 angular_gain = 1.0 cmd_vel.linear.x = linear_gain * math.sqrt(error_x**2 + error_y**2) # 计算朝向误差 target_yaw = math.atan2(error_y, error_x) current_yaw = self.get_yaw_from_quaternion(follower_pose.transform.rotation) yaw_error = target_yaw - current_yaw # 角度归一化到[-pi, pi] while yaw_error > math.pi: yaw_error -= 2 * math.pi while yaw_error < -math.pi: yaw_error += 2 * math.pi cmd_vel.angular.z = angular_gain * yaw_error # 5. 发布速度指令 self.cmd_vel_pub.publish(cmd_vel) except Exception as e: self.get_logger().warn(f'控制循环出错，可能TF数据尚未就绪: {e}') # 发布零速度，确保安全 self.cmd_vel_pub.publish(Twist()) def get_yaw_from_quaternion(self, quat): """从四元数中提取偏航角（绕Z轴旋转）""" import math x, y, z, w = quat.x, quat.y, quat.z, quat.w siny_cosp = 2 * (w * z + x * y) cosy_cosp = 1 - 2 * (y * y + z * z) yaw = math.atan2(siny_cosp, cosy_cosp) return yaw def main(args=None): rclpy.init(args=args) node = FollowerController() rclpy.spin(node) node.destroy_node() rclpy.shutdown() if __name__ == '__main__': main()

4.4 配置文件与依赖

创建跟随控制器的参数文件：

# 文件: config/follower.yaml follower_controller: ros__parameters: leader_name: "robot1" follow_distance: 1.5 # 期望保持1.5米距离

修改package.xml和setup.py以确保依赖正确和节点可执行。 在setup.py中注册节点：

# 文件: setup.py (片段) from setuptools import setup import os from glob import glob setup( # ... 其他参数 ... entry_points={ 'console_scripts': [ 'robot_tf2_broadcaster = multi_robot_demo.robot_tf2_broadcaster:main', 'robot_tf2_listener = multi_robot_demo.robot_tf2_listener:main', 'follower_controller = multi_robot_demo.follower_controller:main', ], }, )

4.5 编译与运行

# 在工作空间根目录编译 cd ~/multi_robot_ws colcon build --symlink-install # 加载环境 source install/setup.bash # 启动仿真与多车系统 ros2 launch multi_robot_demo multi_robot.launch.py

4.6 运行验证与调试

1. 启动后观察：Gazebo界面应出现两辆机器人。RViz2中，添加TF显示，你应该能看到map,robot1/base_link,robot2/base_link等坐标系。
2. 查看话题：打开一个新终端，运行ros2 topic list。你应该能看到类似/robot1/cmd_vel,/robot2/cmd_vel,/robot1/odom,/robot2/odom等带命名空间的话题。
3. 测试跟随：在终端中，你可以通过发布指令控制robot1：

   ros2 topic pub /robot1/cmd_vel geometry_msgs/msg/Twist "{linear: {x: 0.2}, angular: {z: 0.0}}" -1

   观察robot2是否开始移动并试图与robot1保持一定距离。
4. 查看变换：使用ros2 run tf2_ros tf2_echo map robot2/base_link来实时查看robot2在全局地图中的位姿。

5. 常见问题与排查思路

6. 最佳实践与工程建议

1. 清晰的坐标系命名规范：

   • 全局坐标系：map或odom。
   • 机器人本体：<robot_namespace>/base_link。
   • 传感器：<robot_namespace>/sensor_link(如robot1/laser_link)。
   • 避免使用无前缀的通用名称（如base_link），除非在机器人命名空间内。

2. 使用启动文件进行系统管理：

   • 将整个多机器人系统的启动逻辑封装在一个主启动文件中。
   • 使用IncludeLaunchDescription和GroupAction来组织不同机器人的启动组，便于管理。
   • 通过LaunchConfiguration传递参数，使系统易于配置（如机器人数量、初始位姿）。

3. 仿真与实车代码隔离：

   • 控制器、决策算法等核心逻辑应编写在独立的ROS节点中。
   • 在启动文件中，通过参数或重映射来切换话题来源。例如，仿真时订阅/gazebo/odom，实车时订阅/odom（来自真实传感器）。
   • 考虑使用robot_state_publisher和joint_state_publisher来统一管理机器人描述，无论是在仿真还是实车中。

4. 通信优化与QoS配置：

   • 对于高频数据（如里程计、激光雷达），使用SensorDataQoS配置（rmw_qos_profile_sensor_data），它允许丢弃旧数据，适合实时性要求高的场景。
   • 对于命令和控制指令，使用ServicesDefault或SystemDefaultQoS，确保可靠性。
   • 在多机器人系统中，合理使用DDS域（Domain）可以隔离不同的通信组，减少网络流量干扰。

5. 安全与容错：

   • 在控制器中实现“看门狗”机制。如果一段时间内未收到领航车的位姿信息，跟随车应进入安全模式（如停止运动）。
   • 对速度指令进行限幅，防止因控制误差累积或传感器噪声导致的速度突变。
   • 在Gazebo仿真中，可以添加虚拟的“碰撞”传感器，并在代码中实现简单的防撞逻辑。

6. 进阶协同策略：

   • 编队控制：本文的跟随是简单的点对点跟踪。对于真正的编队（如三角形、直线），需要为每辆车定义在编队坐标系中的期望位置，并基于此计算控制律。
   • 任务分配：可以使用基于拍卖的算法（如CBBA）或优化方法，将一组任务动态分配给车队中的机器人。
   • 集中式 vs 分布式：小型车队可采用集中式调度（一个主节点分配任务）；大型系统更适合分布式协商，提高鲁棒性。

从单车到车队，是ROS2机器人开发能力的一次重要跃迁。本文详细拆解了坐标变换管理、Gazebo多机器人仿真环境搭建以及基于话题通信的协同控制这三个核心环节，并提供了一个完整、可运行的两车跟随示例。关键在于理解并实践“命名空间隔离”和“TF树统一管理”这两个核心思想。掌握了这个框架后，你可以进一步集成SLAM建图、导航规划、任务调度等更复杂的功能，构建出真正实用的多机器人协同系统。建议你以本文代码为起点，尝试增加第三台机器人，或者实现更复杂的编队形状，在实践中深化理解。