当前位置：首页 > news >正文

基于Jetson与ROS2的Reachy Mini机器人集群舞蹈控制系统设计与实现

news 2026/8/3 16:02:41

1. 项目缘起：从单机到集群的机器人编排挑战

最近在折腾一个挺有意思的项目，核心目标是想用一块Jetson开发板，通过一个统一的控制台界面，去同时指挥好几台Reachy Mini机器人，让它们能协同完成一些动作，比如跳个简单的集体舞。听起来有点像给机器人当编舞导演，对吧？这个想法的源头，其实是在做单台Reachy Mini开发时遇到的瓶颈。当你只控制一台机器人时，通过ROS2的节点发布指令，一切都很清晰。但一旦数量增加到两台、三台甚至更多，问题就来了：指令如何同步下发？状态如何统一监控？动作的时序和一致性怎么保证？总不能给每台机器人配一台电脑，然后靠人肉喊“1、2、3，走！”吧。

这就引出了“集群控制”的需求。这里的“集群”并非指Hadoop、Kubernetes那种用于数据计算或服务编排的IT集群，而是特指多台实体机器人为了完成协同任务而组成的群体。它们的核心诉求是集中管理、统一调度和协同作业。Jetson作为边缘AI计算平台，其强大的算力和对ROS等机器人框架的良好支持，让它成为充当这个“集群大脑”或“控制台”的理想选择。而“控制台”在这里，就是一个运行在Jetson上的软件应用，它提供了一个图形化或命令行的界面，允许我们一次性编排所有机器人的动作序列。

Reachy Mini是一款开源、模块化的仿人机器人平台，基于Python和ROS2，非常适合研究和教育。让它跳舞，本质上就是按照时间线，精确控制其多个关节（电机）的角度。单台控制是基础，多台同步则是工程上的深化。这个项目，就是要把这个“深化”的过程实现出来，构建一个从硬件连接、软件框架到上层应用的小型机器人集群控制系统。

2. 系统架构设计：Jetson如何扮演“指挥家”

要实现多台Reachy Mini的集群控制，我们需要一个清晰、可靠且易于扩展的系统架构。整个系统的核心是运行在Jetson上的“集群舞蹈控制台”，它负责高层逻辑；而每台Reachy Mini则是独立的“执行单元”，通过ROS2网络接收指令。

2.1 硬件连接与网络拓扑

首先，得把所有硬件连起来。假设我们有三台Reachy Mini机器人（R1, R2, R3）和一台Jetson Orin Nano作为控制主机。

网络连接：这是集群通信的基石。最稳定可靠的方式是使用一个千兆交换机，将所有设备（Jetson和三台Reachy Mini的主控板，通常是树莓派或类似的单板机）通过网线连接到同一个局域网（LAN）中。确保它们处于同一网段，例如192.168.1.0/24。无线网络（Wi-Fi）虽然方便，但在多机器人实时控制中，可能因延迟和抖动导致动作不同步，因此强烈推荐有线网络。
Jetson的角色：Jetson在这里不直接驱动Reachy Mini的电机。它的核心任务是：
- 运行动作编排算法：计算或读取预先设计好的舞蹈动作序列。
- 运行集群控制台应用：提供用户界面（UI）或API，用于启动、停止、监控集群任务。
- 作为ROS2 Master：在ROS2网络中，需要一个“主节点”来协调所有其他节点之间的发现与通信。我们将Jetson配置为ROS2 Master，所有Reachy Mini都将其ROS_DOMAIN_ID设置为与Jetson相同，并指向Jetson的IP地址作为ROS Master的发现地址（通过设置ROS_MASTER_URI环境变量，在ROS2中更常见的是设置ROS_DOMAIN_ID和确保组播连通）。
Reachy Mini的准备：每台Reachy Mini需要预先刷好系统，安装好Reachy SDK、ROS2 Humble或Foxy版本，并确保其电机驱动、传感器等底层功能正常。每台机器人的ROS2节点需要能够被Jetson上的控制台发现和通信。

网络拓扑示意图（逻辑上）：

[ 集群舞蹈控制台 (GUI/CLI) ] | v [ 动作序列管理器 & ROS2 节点 (运行于Jetson) ] | (ROS2 Topic/Service/Action) ------------------ | | | v v v [Reachy Mini 1] [Reachy Mini 2] [Reachy Mini 3]

2.2 软件栈选型与核心组件

软件层面，我们基于ROS2来构建通信中间件，这是机器人领域的标准选择。

通信中间件：ROS2 (推荐Humble版本)
- 为什么是ROS2？ROS1的通信机制对网络要求苛刻，在分布式系统中有时不够稳定。ROS2基于DDS，天生支持真正的分布式、跨平台通信，更适合多机集群场景。其/tf、/clock（如果使用仿真时间）等工具对多机器人系统也很友好。
- 关键概念利用：
  - Topic: 用于流式数据，例如控制台向所有机器人广播同步时钟信号或全局状态。
  - Service: 用于一对一的请求-响应，例如控制台查询某台机器人的当前关节状态。
  - Action: 用于执行可抢占、有反馈的长时任务，这是控制舞蹈动作的核心。我们可以为“执行一段舞蹈”定义一个Action，控制台作为Action Client，每台机器人运行一个Action Server。这样控制台可以同时向多台机器人发送目标，并接收各自的执行进度反馈。
集群控制台应用开发
- 语言：Python。因为Reachy SDK和ROS2的Python客户端（rclpy）生态丰富，开发效率高。
- 框架选择：
  - 方案A（轻量CLI）：直接使用Python脚本，通过argparse库处理命令行参数。适合快速测试和自动化。
  - 方案B（图形界面GUI）：使用PyQt5或Tkinter。这对于演示和教学非常直观，可以显示每台机器人的状态、提供动作序列的可视化编辑、一键启动/停止等。考虑到易用性，本项目更倾向于开发一个简单的PyQt5 GUI。
- 核心功能模块：
  - 机器人管理模块：维护一个机器人列表，包含每台机器人的名称、ROS2节点名称、IP地址、状态（在线/离线/忙碌/空闲）。
  - 动作序列加载与解析模块：从YAML或JSON文件加载预先设计好的舞蹈动作。动作文件应定义每个时间点、每个机器人、每个关节的目标角度或位姿。
  - 同步调度器模块：这是最关键的部件。它需要解决“同时开始”和“节奏一致”的问题。一个简单有效的策略是：
    1. 控制台向所有在线机器人发送一个“准备”指令（通过ROS2 Service）。
    2. 收到所有机器人的“准备就绪”应答后，控制台发布一个“开始”信号到某个同步Topic（例如/cluster_sync_start）。
    3. 所有机器人的Action Server订阅这个Topic，收到“开始”信号后，立刻开始执行本地存储或流式接收的动作序列。
    4. 使用ROS2的/clock话题发布仿真时间，可以让所有机器人在统一的时间线上运行，这对于复杂编排尤其有用，但会引入仿真时间管理的复杂度。对于舞蹈这种对绝对时间戳要求高的场景，依赖系统时钟和精确的网络延时估计可能更简单。
Reachy Mini端节点程序每台机器人上需要运行一个常驻的ROS2节点（Python程序），这个程序主要做两件事：
- 暴露Action Server：提供一个名为/execute_dance的Action接口，接收来自控制台的动作目标（可能是动作序列ID或直接的动作点列表）。
- 执行与反馈：调用Reachy SDK的API，将接收到的关节角度目标依次发送给机器人电机，并实时将执行进度（例如“已完成第5个动作点/共100个”）反馈给控制台。

3. 核心实现：从动作设计到同步执行

有了架构，我们来拆解具体的实现步骤。这个过程可以分为离线的“动作设计”和在线的“集群执行”两大部分。

3.1 舞蹈动作序列的设计与生成

让机器人跳舞，首先得有“舞谱”。我们不能实时手动遥操作，必须预先设计好动作。

单机器人动作录制（可选但推荐）：最直观的方式是先用Reachy提供的“示教”功能，手动摆弄一台机器人，让它完成一套你想要的舞蹈动作，同时用程序记录下每个时刻所有关节的角度。Reachy SDK通常有相关工具。这能得到非常自然流畅的动作数据。

动作序列文件格式：我们将录制的或手动设计的数据保存为结构化的文件。YAML是个好选择，因为它易读易写。

# dance_routine_v1.yaml name: "SimpleGroupWave" duration: 10.0 # 总时长，秒 robots: # 定义参与此套动作的机器人 - id: "reachy_01" joints: ["l_shoulder_pitch", "l_shoulder_roll", "l_arm_yaw", "l_elbow_pitch", "l_wrist_roll", "l_wrist_pitch", "l_wrist_yaw", "r_shoulder_pitch", "r_shoulder_roll", "r_arm_yaw", "r_elbow_pitch", "r_wrist_roll", "r_wrist_pitch", "r_wrist_yaw", "head_pan", "head_tilt"] keyframes: - time: 0.0 positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 初始位置，单位弧度 - time: 2.0 positions: [0.5, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] # reachy_01 左臂抬起 - time: 4.0 positions: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.5, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0] # 左臂放下，右臂抬起 - id: "reachy_02" joints: [...] # 关节名列表，同上 keyframes: - time: 0.0 positions: [...] # 初始位置 - time: 2.0 positions: [...] # reachy_02 可能做不同的动作，例如向右看 - time: 4.0 positions: [...] # 头转回

注意：这里每个机器人的keyframes里的time是相对于该套动作开始时刻的绝对时间。这意味着reachy_01在2.0秒时抬左臂，reachy_02在2.0秒时向右看，它们是同时发生的。这就是集群同步的基础。

动作插值：记录的关键帧（Keyframes）可能不够密集。为了让动作平滑，需要在执行时进行插值（例如线性插值或五次多项式插值）。这个插值逻辑可以放在控制台的调度器里（集中计算后下发），也可以放在每个机器人的Action Server里（分布式计算）。考虑到网络带宽和计算负载分散，推荐将插值任务下放到每个机器人。控制台只需要下发关键帧数据，机器人端根据当前时间和关键帧数据实时计算目标角度。

3.2 集群控制台的关键代码剖析

控制台的核心是同步调度。下面用伪代码展示一个简化版的同步启动逻辑：

# cluster_console.py (部分核心代码) import rclpy from rclpy.node import Node from rclpy.action import ActionClient from your_robot_interfaces.action import ExecuteDance import threading import time class DanceClusterController(Node): def __init__(self, robot_names): super().__init__('dance_cluster_controller') self.robot_names = robot_names self.action_clients = {} self.ready_flags = {name: False for name in robot_names} # 为每个机器人创建Action Client for name in robot_names: self.action_clients[name] = ActionClient(self, ExecuteDance, f'/{name}/execute_dance') # 同时，可以创建一个Service Client来查询或准备状态（此处略） # 创建一个Publisher用于发布同步开始信号 from std_msgs.msg import Bool self.sync_start_pub = self.create_publisher(Bool, '/cluster_sync_start', 10) def prepare_all_robots(self): """发送准备指令，并等待所有机器人回应""" # 这里简化处理，实际应用中应该用Service调用，并设置超时 print("发送准备指令...") # 模拟准备过程 time.sleep(1) for name in self.robot_names: self.ready_flags[name] = True # 假设都成功了 print("所有机器人准备就绪。") def send_dance_goal(self, robot_name, dance_routine_data): """向单个机器人发送舞蹈动作目标""" goal_msg = ExecuteDance.Goal() goal_msg.routine_id = dance_routine_data['id'] goal_msg.keyframes = dance_routine_data['keyframes'] # 传递关键帧数据 self.action_clients[robot_name].wait_for_server() future = self.action_clients[robot_name].send_goal_async(goal_msg) # 可以添加反馈和结果回调 return future def start_synchronized_dance(self, dance_data_dict): """ dance_data_dict: 字典，key为机器人名，value为该机器人的动作数据 """ # 1. 准备阶段 self.prepare_all_robots() # 2. 发送动作目标（但告诉机器人等待开始信号） futures = {} for name, data in dance_data_dict.items(): # 在动作数据中标记“等待信号” data['wait_for_sync'] = True future = self.send_dance_goal(name, data) futures[name] = future # 等待一小段时间，确保所有机器人都收到了目标并进入等待状态 time.sleep(0.5) # 3. 发布同步开始信号！ sync_msg = Bool() sync_msg.data = True self.sync_start_pub.publish(sync_msg) self.get_logger().info('*** 同步开始信号已发布！ ***') # 4. 等待所有动作执行完成 for name, future in futures.items(): # 这里需要处理future的结果 pass def main(): rclpy.init() robot_list = ['reachy_01', 'reachy_02', 'reachy_03'] controller = DanceClusterController(robot_list) # 加载舞蹈数据 dance_data = load_dance_from_yaml('dance_routine_v1.yaml') # dance_data 需要按机器人名组织成字典 # 启动同步舞蹈 controller.start_synchronized_dance(dance_data) rclpy.spin(controller) controller.destroy_node() rclpy.shutdown()

3.3 Reachy Mini端Action Server的实现

机器人端的节点需要响应控制台的调用。

# robot_dance_server.py import rclpy from rclpy.node import Node from rclpy.action import ActionServer from your_robot_interfaces.action import ExecuteDance from reachy_sdk import ReachySDK import numpy as np class DanceActionServer(Node): def __init__(self, robot_name): super().__init__(f'{robot_name}_dance_server') self.robot_name = robot_name self.reachy = ReachySDK(host='localhost') # 连接本地Reachy服务 self.sync_started = False # 创建Action Server self._action_server = ActionServer( self, ExecuteDance, 'execute_dance', self.execute_callback) # 订阅同步开始信号 from std_msgs.msg import Bool self.sync_sub = self.create_subscription( Bool, '/cluster_sync_start', self.sync_callback, 10) def sync_callback(self, msg): if msg.data: self.get_logger().info(f'{self.robot_name} 收到同步开始信号！') self.sync_started = True def execute_callback(self, goal_handle): """Action Server的回调函数，执行舞蹈""" goal = goal_handle.request routine_id = goal.routine_id keyframes = goal.keyframes # 假设是列表，每个元素是(time, positions) wait_for_sync = goal.wait_for_sync feedback_msg = ExecuteDance.Feedback() result_msg = ExecuteDance.Result() # 检查动作数据有效性 if not keyframes: goal_handle.abort() result_msg.success = False result_msg.message = "动作数据为空" return result_msg # 如果需要等待同步信号 if wait_for_sync: self.get_logger().info(f'{self.robot_name} 等待同步开始信号...') while not self.sync_started and rclpy.ok(): time.sleep(0.01) # 短暂休眠，避免空转耗CPU if not self.sync_started: goal_handle.abort() result_msg.success = False result_msg.message = "等待同步信号超时" return result_msg # 开始执行动作序列 self.get_logger().info(f'{self.robot_name} 开始执行舞蹈 {routine_id}') start_time = time.time() keyframes_sorted = sorted(keyframes, key=lambda x: x.time) for i in range(len(keyframes_sorted)-1): if not goal_handle.is_active: self.get_logger().info('动作被取消') return result_msg frame_start = keyframes_sorted[i] frame_end = keyframes_sorted[i+1] duration = frame_end.time - frame_start.time # 线性插值 num_steps = int(duration / 0.02) # 假设控制周期20ms if num_steps > 0: for step in range(num_steps): alpha = step / num_steps interp_positions = (1-alpha)*np.array(frame_start.positions) + alpha*np.array(frame_end.positions) # 使用Reachy SDK设置关节位置 self.reachy.set_joint_positions(joint_names, interp_positions.tolist()) # 发送反馈 feedback_msg.progress = (frame_start.time + step*0.02) / keyframes_sorted[-1].time goal_handle.publish_feedback(feedback_msg) time.sleep(0.02) # 控制周期 # 执行最后一个关键帧 self.reachy.set_joint_positions(joint_names, keyframes_sorted[-1].positions) time.sleep(0.1) goal_handle.succeed() result_msg.success = True result_msg.message = "舞蹈执行完成" return result_msg def main(args=None): rclpy.init(args=args) # 机器人名称可以从参数或环境变量获取 robot_name = os.getenv('ROBOT_NAME', 'reachy_01') dance_server = DanceActionServer(robot_name) rclpy.spin(dance_server) dance_server.destroy_node() rclpy.shutdown()

4. 部署、调试与实战避坑指南

将代码部署到真实的硬件上并跑通整个流程，会遇到许多在仿真或单机测试中遇不到的问题。以下是基于实际操作的详细步骤和避坑点。

4.1 系统环境部署与配置

Jetson系统准备：
- 在Jetson Orin Nano上安装Ubuntu 20.04或22.04 LTS。推荐使用NVIDIA官方提供的SDK Manager进行刷机，它会自动安装CUDA、TensorRT等基础AI堆栈。
- 安装ROS2 Humble：按照ROS官网指引安装Desktop版本。务必记得source /opt/ros/humble/setup.bash并将其加入~/.bashrc。
- 安装Python3依赖：pip3 install pyyaml numpy pyqt5（如果做GUI）。对于ROS2的Python包，通常通过apt安装，如sudo apt install ros-humble-rclpy ros-humble-std-msgs。
Reachy Mini系统准备：
- 每台Reachy Mini的主控板（如树莓派）需要安装与Jetson兼容的ROS2版本（同为Humble）。确保网络连通，能ping通Jetson。
- 安装Reachy SDK：按照Pollen Robotics官方文档，通过pip安装reachy-sdk。这一步可能会涉及一些硬件特定驱动，务必仔细阅读文档。
- 配置ROS2环境变量：这是多机通信的关键。在所有设备（Jetson和所有Reachy）的~/.bashrc中，设置相同的ROS_DOMAIN_ID（一个0-232之间的数字），并确保ROS_MASTER_URI指向Jetson的IP（ROS2中此变量作用已变，主要依赖组播，但设置ROS_DOMAIN_ID和确保防火墙允许组播流量是关键）。
```
# 在 ~/.bashrc 末尾添加 export ROS_DOMAIN_ID=42 export ROS_IP=<本机IP地址> # 有助于节点发现 source /opt/ros/humble/setup.bash
```
- 验证网络发现：在Jetson上运行ros2 topic list，然后在任意一台Reachy上运行ros2 topic list，两者应该能看到相同的topic列表（可能为空，但说明发现机制正常）。如果看不到，检查防火墙（sudo ufw disable临时关闭测试）和组播路由。

4.2 同步性问题的排查与优化

这是集群控制中最棘手的部分。动作不同步可能由以下原因导致：

网络延迟与抖动：
- 现象：机器人动作开始时间有肉眼可见的先后顺序。
- 排查：使用ping命令测试Jetson到各Reachy的往返延迟（RTT）。理想情况应小于1ms。如果延迟过大或有丢包，检查网线、交换机端口。
- 优化：
  - 使用有线网络：重申，无线网络不适合实时控制。
  - 优化交换机：使用非管理型千兆交换机，避免复杂的QoS策略引入不确定性。
  - 时间同步协议（NTP）：在所有设备上安装并配置NTP客户端，指向同一个时间服务器。虽然ROS2 Action的启动命令下发是即时的，但机器人的本地时钟如果偏差大，可能会影响日志分析和高级调度。运行sudo apt install chrony并配置。
动作序列加载与解析耗时不同：
- 现象：机器人收到“开始”信号后，到真正开始运动的时间点不一致。
- 原因：如果动作数据较大，每台机器人在收到目标后，需要解析YAML/JSON数据，这个耗时可能因CPU负载而异。
- 解决：在准备阶段完成所有耗时操作。在prepare_all_robots阶段，不仅发送“准备”指令，还可以将完整的动作序列数据预先发送给各机器人（通过Service或一个非实时的Topic），让机器人在后台提前解析好，只等开始信号。这样，开始信号到来时，所有机器人都已“蓄势待发”。
控制循环周期不一致：
- 现象：动作开始同步，但做着做着就逐渐错位。
- 原因：代码中用于插值和控制周期的time.sleep(0.02)（20ms）并不精确，且受系统调度影响。多台设备上的time.sleep累积误差会导致不同步。
- 解决：
  - 使用ROS2 Timer：在机器人端的Action Server中，使用ROS2的create_timer来替代time.sleep。ROS2的Timer基于节点的事件循环，精度和稳定性更好。
```
# 替代 time.sleep 循环 self.timer = self.create_timer(0.02, self._control_cycle_callback) # 20ms周期
```
  - 依赖绝对时间戳：在动作执行循环中，不依赖循环次数，而是依赖从动作开始经过的绝对时间（current_time - start_time）来查询目标位置。这样即使某次循环被延迟，下次循环也会努力“追上”正确的位置，但可能会造成动作抖动。更高级的做法是使用轨迹插值库，根据时间戳生成平滑的轨迹。

4.3 可视化监控与故障处理

一个健壮的控制台需要有状态监控能力。

在GUI中集成状态监控：
- 使用PyQt5的QTableWidget或QLabel列表来显示每台机器人的状态（在线、离线、执行中、错误）。
- 订阅每个机器人发布的特定状态Topic（例如/reachy_01/robot_status），在回调函数中更新UI。
- 为每行状态设置颜色（绿色-在线空闲，黄色-执行中，红色-错误/离线）。
实现简单的故障处理：
- 心跳机制：让每个机器人节点定期（如每秒）发布一个“心跳”消息到/heartbeat/<robot_name>话题。控制台订阅所有心跳话题，如果某个机器人的心跳超时（比如3秒未收到），则在UI上将其标记为“离线”，并尝试重新连接或通知用户。
- 动作执行异常处理：在Action的反馈中，除了进度，还可以包含错误码。如果机器人端在执行过程中遇到关节错误、碰撞检测等，应立即通过反馈通知控制台，控制台可以决定是暂停所有机器人还是仅停止出问题的那个。
日志记录：在Jetson上使用ros2 bag record录制所有相关的Topic（如同步信号、动作目标、机器人状态），这对于后期分析不同步问题至关重要。

5. 项目总结与扩展思考

通过以上步骤，我们搭建了一个基于Jetson和ROS2的Reachy Mini机器人集群舞蹈控制系统。从架构设计、通信协议、同步策略到具体的代码实现和调试技巧，整个流程覆盖了多机器人协同中的核心问题。

我个人在实测中的几点深刻体会：

第一，网络是基石，稳定压倒一切。在项目初期，我们尝试过用Wi-Fi，结果同步性惨不忍睹，动作看起来像“群魔乱舞”。换上千兆交换机后，问题立刻解决了80%。所以，在机器人集群项目中，对网络的投入绝对不能省。

第二，“准备阶段”的设计至关重要。最初我们图省事，把动作数据解析放在开始信号之后，结果就是开始信号发出后，有的机器人秒动，有的要卡顿半秒，同步无从谈起。后来把数据预加载、解析、甚至电机上电自检都放到准备阶段，让所有机器人在起跑线前就位，同步启动的效果就好多了。

第三，可视化监控不是锦上添花，而是雪中送炭。当你有三台以上的机器人时，光靠看它们的物理动作很难快速定位问题。一个能实时显示每台机器人状态、日志和简单数据曲线的控制台，能极大提升调试效率。我们后来在控制台里加了一个简单的时序图，显示每台机器人收到关键命令的时间点，一下子就把网络延迟的问题可视化出来了。

这个项目还可以向多个方向扩展：

动作编排可视化：开发一个图形化的动作编辑器，像音乐制作软件一样，用时间轴来为每个机器人编排动作，并实时预览。
引入感知反馈：为Jetson连接摄像头，利用YOLO等模型识别机器人的实际姿态，实现基于视觉的闭环控制，让舞蹈动作更精准，或者实现“人机共舞”。
动态角色分配：不预先固定每台机器人的动作，而是由控制台根据实时情况（如某台机器人故障）动态分配动作角色，提高系统的鲁棒性。
规模扩展：当前的架构对于10台以内的机器人是合适的。如果规模进一步扩大，可能需要引入更专业的集群管理中间件（如基于ROS2的Supervisor节点，或借鉴K8s的理念），进行分组管理和负载均衡。

从单台机器人的控制到多台机器人的协同，这一步跨越带来的挑战和乐趣是成倍增长的。希望这个详细的实现指南和踩坑记录，能为你开启自己的机器人集群项目提供扎实的参考。

查看全文

http://www.jsqmd.com/news/1320883/