多机器人协作系统
构建多机器人协作系统，学习任务分配、通信协调和协同控制的实现方法。
多机器人 协作 ROS2 任务分配 分布式系统
多机器人协作系统
多机器人协作是Embodied AI的重要应用方向。本文讲解如何构建一个多机器人系统，实现任务分配、通信协调和协同控制。
系统架构
┌─────────────────────────────────────────┐
│ 中央任务规划器 │
│ - 任务分解 │
│ - 资源分配 │
│ - 冲突解决 │
└─────────────────────────────────────────┘
↓
┌─────────────────────────────────────────┐
│ 通信层（ROS2 DDS） │
│ - 话题发布/订阅 │
│ - 服务调用 │
│ - 动作通信 │
└─────────────────────────────────────────┘
↓ ↓ ↓
┌─────────┐ ┌─────────┐ ┌─────────┐
│ Robot 1 │ │ Robot 2 │ │ Robot 3 │
│ 机械臂 │ │ 移动底盘 │ │ 无人机 │
└─────────┘ └─────────┘ └─────────┘任务分配
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import json
class TaskAllocator(Node):
def __init__(self):
super().__init__('task_allocator')
# 机器人状态
self.robots = {
'robot_1': {'status': 'idle', 'position': [0, 0, 0]},
'robot_2': {'status': 'idle', 'position': [1, 0, 0]},
'robot_3': {'status': 'idle', 'position': [2, 0, 0]},
}
# 订阅任务请求
self.task_sub = self.create_subscription(
String, '/task_requests', self.task_callback, 10
)
# 发布任务分配
self.assign_pub = self.create_publisher(
String, '/task_assignments', 10
)
def task_callback(self, msg):
"""处理任务请求"""
task = json.loads(msg.data)
task_id = task['id']
task_type = task['type']
task_location = task['location']
# 选择最合适的机器人
best_robot = self._select_robot(task_type, task_location)
if best_robot:
# 分配任务
assignment = {
'task_id': task_id,
'robot': best_robot,
'action': task_type,
'target': task_location
}
self.assign_pub.publish(String(data=json.dumps(assignment)))
self.robots[best_robot]['status'] = 'busy'
self.get_logger().info(
f'任务 {task_id} 分配给 {best_robot}'
)
def _select_robot(self, task_type, location):
"""根据任务类型和位置选择机器人"""
candidates = []
for robot_id, info in self.robots.items():
if info['status'] != 'idle':
continue
# 计算机器人到任务位置的距离
distance = self._compute_distance(info['position'], location)
candidates.append((robot_id, distance))
if not candidates:
return None
# 选择距离最近的
candidates.sort(key=lambda x: x[1])
return candidates[0][0]
def _compute_distance(self, pos1, pos2):
"""计算两点距离"""
import numpy as np
return np.linalg.norm(np.array(pos1) - np.array(pos2))机器人协同控制
class RobotCoordinator(Node):
def __init__(self, robot_id):
super().__init__(f'robot_{robot_id}')
self.robot_id = robot_id
# 订阅任务分配
self.assign_sub = self.create_subscription(
String, '/task_assignments', self.assignment_callback, 10
)
# 发布状态更新
self.status_pub = self.create_publisher(
String, f'/robot_{robot_id}/status', 10
)
# 当前任务
self.current_task = None
def assignment_callback(self, msg):
"""处理任务分配"""
assignment = json.loads(msg.data)
if assignment['robot'] != self.robot_id:
return # 不是分配给自己的任务
self.current_task = assignment
self.get_logger().info(
f'收到任务: {assignment["task_id"]}'
)
# 执行任务
self.execute_task(assignment)
def execute_task(self, task):
"""执行任务"""
action = task['action']
target = task['target']
if action == 'navigate':
self._navigate_to(target)
elif action == 'pick':
self._pick_object(target)
elif action == 'place':
self._place_object(target)
# 更新状态
self._publish_status('completed')
def _navigate_to(self, target):
"""导航到目标位置"""
self.get_logger().info(f'导航到 {target}')
# 调用导航服务
# ...
def _pick_object(self, target):
"""抓取物体"""
self.get_logger().info(f'抓取 {target}')
# 调用抓取服务
# ...
def _place_object(self, target):
"""放置物体"""
self.get_logger().info(f'放置到 {target}')
# 调用放置服务
# ...
def _publish_status(self, status):
"""发布状态"""
status_msg = {
'robot_id': self.robot_id,
'status': status,
'current_task': self.current_task
}
self.status_pub.publish(String(data=json.dumps(status_msg)))协同搬运任务
class CollaborativeTransport:
def __init__(self, robots):
self.robots = robots # 机器人列表
def transport_object(self, object_pose, target_pose):
"""协同搬运物体"""
# 1. 规划搬运策略
grasp_points = self._compute_grasp_points(object_pose)
# 2. 分配抓取点
assignments = self._assign_grasp_points(grasp_points)
# 3. 同步移动到抓取位置
self._synchronize_move(assignments)
# 4. 协同抓取
self._synchronize_grasp()
# 5. 协同搬运
self._synchronize_transport(target_pose)
# 6. 协同放置
self._synchronize_place()
def _compute_grasp_points(self, object_pose):
"""计算抓取点"""
# 根据物体形状计算多个抓取点
# 返回至少2个点（双机器人搬运）
return [
[object_pose[0] - 0.1, object_pose[1], object_pose[2]],
[object_pose[0] + 0.1, object_pose[1], object_pose[2]]
]
def _assign_grasp_points(self, grasp_points):
"""分配抓取点到机器人"""
assignments = {}
for i, robot in enumerate(self.robots):
if i < len(grasp_points):
assignments[robot.id] = grasp_points[i]
return assignments
def _synchronize_move(self, assignments):
"""同步移动"""
# 所有机器人同时开始移动
for robot_id, target in assignments.items():
robot = self._get_robot(robot_id)
robot.move_to(target, blocking=False)
# 等待所有机器人到达
self._wait_for_all()
def _synchronize_grasp(self):
"""同步抓取"""
# 所有机器人同时闭合夹爪
for robot in self.robots:
robot.close_gripper(blocking=False)
self._wait_for_all()
def _synchronize_transport(self, target_pose):
"""协同搬运"""
# 计算每个机器人的目标位置
# 保持物体在搬运过程中的稳定性
for robot in self.robots:
target = self._compute_transport_target(robot, target_pose)
robot.move_to(target, blocking=False)
self._wait_for_all()
def _wait_for_all(self, timeout=10.0):
"""等待所有机器人完成动作"""
start_time = time.time()
while time.time() - start_time < timeout:
all_done = all(
robot.is_action_complete() for robot in self.robots
)
if all_done:
return True
time.sleep(0.1)
return False冲突避免
class CollisionAvoidance:
def __init__(self, robots):
self.robots = robots
self.safety_distance = 0.5 # 安全距离
def check_collisions(self):
"""检查潜在碰撞"""
for i, robot1 in enumerate(self.robots):
for robot2 in self.robots[i+1:]:
distance = self._compute_distance(robot1, robot2)
if distance < self.safety_distance:
return True, (robot1, robot2)
return False, None
def resolve_conflict(self, robot1, robot2):
"""解决冲突"""
# 优先级策略：让优先级低的机器人停止
if robot1.priority > robot2.priority:
robot2.stop()
robot2.wait_until_clear()
else:
robot1.stop()
robot1.wait_until_clear()
def _compute_distance(self, robot1, robot2):
"""计算机器人之间的距离"""
pos1 = robot1.get_position()
pos2 = robot2.get_position()
return np.linalg.norm(np.array(pos1) - np.array(pos2))FAQ
多机器人通信延迟怎么办？
使用ROS2的QoS策略配置可靠通信。对于实时性要求高的场景，考虑使用共享内存。
如何处理机器人故障？
实现心跳检测机制，定期监控机器人状态。故障时重新分配任务。
可以扩展到多少个机器人？
理论上无限制，但实际受通信带宽和计算资源限制。10-20个机器人是常见规模。