multi-agent-swarm

name	multi-agent-swarm
description	多智能体协同技能 - 蜂群机器人、分布式感知、协同规划、任务分配、ROS2 多机通信
argument-hint	多智能体 OR 蜂群 OR swarm OR 协同规划 OR multi-agent OR 多机协同
user-invocable	true

多智能体协同技能

用于开发 ROS2 多智能体协同系统，涵盖蜂群机器人、分布式感知、协同规划、任务分配和多机通信架构

---

何时使用

当需要以下帮助时使用此技能：

• 实现多机器人协同定位与建图
• 分布式感知与数据融合
• 协同路径规划与任务分配
• 蜂群机器人编队控制
• ROS2 多机通信架构设计
• 共识算法与分布式决策

---

快速参考

多机 ROS2 通信架构

单机:                      多机 (同一 ROS_DOMAIN):
+--------+               +--------+ +--------+ +--------+
|Node A  |               |Robot 1 | |Robot 2 | |Robot 3 |
+--------+               +--------+ +--------+ +--------+
|Node B  |   DDS 跨机    |  DDS   | |  DDS   | |  DDS   |
+--------+ <------------>|--------|<>|--------|<>|--------|
                          +--------+ +--------+ +--------+

不同域:
+--------+               +--------+   +--------+
|Robot 1 |               |Robot 2 |   |Robot 3 |
|DOMAIN=0| <--- Bridge -->|DOMAIN=1|   |DOMAIN=2|
+--------+               +--------+   +--------+

核心包

# 多机通信
sudo apt install -y ros-humble-rmw-cyclonedds-cpp  # 跨域 DDS
sudo apt install -y ros-humble-rosbridge-suite      # WebSocket 桥接

# 协同定位
sudo apt install -y ros-humble-multi-robot-map-merge
sudo apt install -y ros-humble-robot-localization

核心话题

话题	类型	说明
/robot_X/odom	nav_msgs/Odometry	第 X 台机器人里程计
/robot_X/scan	sensor_msgs/LaserScan	第 X 台机器人激光
/swarm/pose	geometry_msgs/PoseStamped	协同定位结果
/swarm/task	my_msgs/SwarmTask	协同任务分配

---

DDS 跨域配置

单一域（同一网络）

# 所有机器人设置相同 DOMAIN_ID
export ROS_DOMAIN_ID=42

# 或者在代码中配置
RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
CYCLONEDDS_URI='<General>
    <Network>
        <AllowInterface>192.168.1.*</AllowInterface>
    </Network>
</General>'

多域桥接

#!/usr/bin/env python3
"""ROS2 域桥接节点"""

import rclpy
from rclpy.node import Node
from rclpy.parameter import Parameter
from std_msgs.msg import String
import json


class DomainBridge(Node):
    """跨域消息桥接"""

    def __init__(self, src_domain: int, dst_domain: int):
        super().__init__(f'domain_bridge_{src_domain}_to_{dst_domain}')
        self.src_domain = src_domain
        self.dst_domain = dst_domain

        # 订阅源域话题
        self.create_subscription(
            String,
            '/swarm/leader_pose',
            self.bridge_callback,
            10
        )

        # 发布到目标域
        self.pub = self.create_publisher(
            String,
            '/swarm/leader_pose',
            10
        )

        self.get_logger().info(
            f'Bridging domain {src_domain} -> {dst_domain}'
        )

    def bridge_callback(self, msg: String):
        # 转发消息（实际应用中需要序列化/反序列化）
        self.pub.publish(msg)


def ():
    rclpy.init()
    
    bridge_0_1 = DomainBridge(, )
    bridge_1_2 = DomainBridge(, )

    executor = rclpy.executors.MultiThreadedExecutor()
    executor.add_node(bridge_0_1)
    executor.add_node(bridge_1_2)

    :
        executor.spin()
    :
        executor.shutdown()
        rclpy.shutdown()


 __name__ == :
    main()

---

协同定位 (Cooperative Localization)

分布式 EKF

import numpy as np
from dataclasses import dataclass
from typing import List, Dict
import rclpy
from rclpy.node import Node
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PoseWithCovarianceStamped


@dataclass
class RobotState:
    """单机器人状态"""
    x: float = 0.0
    y: float = 0.0
    theta: float = 0.0
    vx: float = 0.0
    vy: float = 0.0
    omega: float = 0.0
    covariance: np.ndarray = None

    def __post_init__(self):
        if self.covariance is None:
            self.covariance = np.eye(6) * 0.1


class CooperativeLocalization(Node):
    """协同定位 - 分布式扩展卡尔曼滤波"""

    def __init__(self, robot_name: str, num_robots: int):
        ().__init__()
        .robot_name = robot_name
        .num_robots = num_robots
        .robot_id = (robot_name.split()[-])

        
        .state_dim =  * num_robots
        .state = np.zeros(.state_dim)
        .P = np.eye(.state_dim) *   

        
        .neighbors = []

        
        .odom_sub = .create_subscription(
            Odometry,
            ,
            .odom_callback,
            
        )

        
         i  (num_robots):
             i != .robot_id:
                .create_subscription(
                    PoseWithCovarianceStamped,
                    ,
                     msg, idx=i: .relative_pose_callback(msg, idx),
                    
                )

        
        .pose_pub = .create_publisher(
            PoseWithCovarianceStamped,
            ,
            
        )

        
        .timer = .create_timer(, .broadcast_state)

     ():
        
        
        idx = .robot_id * 
        .state[idx] = msg.pose.pose.position.x
        .state[idx+] = msg.pose.pose.position.y
        

     ():
        
        
        z = np.array([
            msg.pose.pose.position.x,
            msg.pose.pose.position.y,
              
        ])

        H = .compute_measurement_jacobian(observer_id, .robot_id)
        R = np.diag([, , ])  

        innovation = z - H @ .state
        S = H @ .P @ H.T + R
        K = .P @ H.T @ np.linalg.inv(S)

        .state = .state + K @ innovation
        .P = (np.eye(.state_dim) - K @ H) @ .P

     ():
        
        msg = PoseWithCovarianceStamped()
        idx = .robot_id * 
        msg.pose.pose.position.x = .state[idx]
        msg.pose.pose.position.y = .state[idx+]
        .pose_pub.publish(msg)

     () -> np.ndarray:
        
        H = np.zeros((, .state_dim))
        
         H

---

协同建图 (Collaborative SLAM)

多机器人地图融合

#!/usr/bin/env python3
"""多机器人地图融合节点"""

import rclpy
from rclpy.node import Node
from nav_msgs.msg import OccupancyGrid
from geometry_msgs.msg import PoseWithCovarianceStamped
import numpy as np


class MapMerger(Node):
    """地图融合"""

    def __init__(self, num_robots: int):
        super().__init__('map_merger')
        self.num_robots = num_robots
        self.maps = {}
        self.poses = {}
        self.resolution = 0.05
        self.width = 2000
        self.height = 2000

        # 订阅各机器人的局部地图和位姿
        for i in range(num_robots):
            robot_name = f'robot_{i}'
            self.maps[i] = None

            self.create_subscription(
                OccupancyGrid,
                f'/{robot_name}/map',
                lambda msg, idx=i: self.map_callback(msg, idx),
                10
            )

            self.create_subscription(
                PoseWithCovarianceStamped,
                ,
                 msg, idx=i: .pose_callback(msg, idx),
                
            )

        .merged_map_pub = .create_publisher(
            OccupancyGrid,
            ,
            
        )

        .timer = .create_timer(, .merge_and_publish)

     ():
        .maps[robot_id] = msg

     ():
        .poses[robot_id] = msg.pose.pose

     ():
        
        merged = OccupancyGrid()
        merged.data = np.zeros(.width * .height, dtype=np.int8)
        merged.info.resolution = .resolution
        merged.info.width = .width
        merged.info.height = .height

         robot_id, local_map  .maps.items():
             local_map  :
                

            pose = .poses.get(robot_id)
             pose  :
                

            
            offset_x = ((pose.position.x - .width * .resolution / ) / .resolution)
            offset_y = ((pose.position.y - .height * .resolution / ) / .resolution)

            
             y  (local_map.info.height):
                 x  (local_map.info.width):
                    global_x = x + offset_x
                    global_y = y + offset_y
                      <= global_x < .width   <= global_y < .height:
                        idx = global_y * .width + global_x
                         local_map.data[y * local_map.info.width + x] > :
                            merged.data[idx] = (merged.data[idx], local_map.data[y * local_map.info.width + x])

        .merged_map_pub.publish(merged)


 ():
    rclpy.init(args=args)
    node = MapMerger(num_robots=)
    rclpy.spin(node)
    node.destroy_node()
    ricleanup()


 __name__ == :
    main()

---

协同任务分配 (Task Allocation)

拍卖算法 (Auction Algorithm)

import numpy as np
from dataclasses import dataclass
from typing import List, Dict, Tuple
import heapq


@dataclass
class Task:
    """任务定义"""
    task_id: int
    position: Tuple[float, float]  # (x, y)
    reward: float
    estimated_cost: float
    assigned_robot: int = -1


class MarketBasedAllocator:
    """基于市场拍卖的多机器人任务分配"""

    def __init__(self, num_robots: int, robot_positions: List[Tuple[float, float]]):
        self.num_robots = num_robots
        self.robot_positions = robot_positions
        self.tasks: List[Task] = []

    def add_task(self, task: Task):
        self.tasks.append(task)

    def compute_cost(self, robot_id: int, task: Task) -> float:
        """计算机器人到任务的行驶成本"""
        rx, ry = self.robot_positions[robot_id]
        tx, ty = task.position
        distance = np.sqrt((rx - tx)** + (ry - ty)**)
         distance + np.random.uniform(, )  

     () -> [, [Task]]:
        
        
         task  .tasks:
            task.assigned_robot = -

        assignments = {i: []  i  (.num_robots)}
        unassigned = .tasks.copy()

        iteration = 
         unassigned  iteration < :
            iteration += 

            
            bids = {}  

             robot_id  (.num_robots):
                 task  unassigned:
                    cost = .compute_cost(robot_id, task)
                    bid = task.reward - cost  
                    bids[(robot_id, task.task_id)] = bid

              bids:
                

            
            max_bid_robot, max_bid_task = (bids.keys(), key= k: bids[k])
            max_bid_value = bids[(max_bid_robot, max_bid_task)]

             max_bid_value <= :
                  

            
             task  unassigned:
                 task.task_id == max_bid_task:
                    task.assigned_robot = max_bid_robot
                    assignments[max_bid_robot].append(task)
                    unassigned.remove(task)
                    

            
            .robot_positions[max_bid_robot] = task.position

         assignments

     () -> [, [Task]]:
        
        
         .auction()

---

编队控制 (Formation Control)

虚拟结构法

import numpy as np
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, Pose
from typing import List, Tuple


class FormationController(Node):
    """编队控制器 - 虚拟结构法"""

    def __init__(self, num_robots: int, formation_type: str = "triangle"):
        super().__init__('formation_controller')
        self.num_robots = num_robots
        self.formation_type = formation_type

        # 虚拟结构中心目标
        self.center_target = np.array([0.0, 0.0])
        self.center_heading = 0.0

        # 编队形状相对偏移
        self.formation_offsets = self.get_formation_offsets()

        # 各机器人当前位置
        self.robot_poses = [None] * num_robots

        # 订阅各机器人位姿
        for i in range(num_robots):
            self.create_subscription(
                Pose,
                f'/robot_{i}/pose',
                lambda msg, idx=i: self.pose_callback(msg, idx),
                
            )

        
        .cmd_pubs = []
         i  (num_robots):
            pub = .create_publisher(Twist, , )
            .cmd_pubs.append(pub)

        .timer = .create_timer(, .control_loop)

     () -> [np.ndarray]:
        
         .formation_type == :
            r =   
             [
                np.array([, ]),           
                np.array([r, ]),           
                np.array([-r/, r*np.sqrt()/]),  
            ]
         .formation_type == :
            spacing = 
             [np.array([i * spacing, ])  i  (.num_robots)]
        :  
            r = 
             [
                np.array([r * np.cos(*np.pi*i/.num_robots),
                          r * np.sin(*np.pi*i/.num_robots)])
                 i  (.num_robots)
            ]

     ():
        .robot_poses[robot_id] = np.array([
            msg.position.x, msg.position.y, 
        ])

     ():
        
         (p    p  .robot_poses):
            

         i  (.num_robots):
            
            desired_pos = .center_target + .formation_offsets[i]

            
            error = desired_pos - .robot_poses[i][:]

            
            Kp = 
            v = Kp * np.linalg.norm(error)

            
             v > :
                desired_theta = np.arctan2(error[], error[])
            :
                desired_theta = 

            
            cmd = Twist()
            cmd.linear.x = (v, )
            cmd.angular.z = desired_theta * 
            .cmd_pubs[i].publish(cmd)

     ():
        
        .center_target = np.array([x, y])
        .center_heading = heading

---

ROS2 启动配置

# launch/swarm_bringup.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
import os


def generate_swarm_launch(num_robots: int = 3):
    nodes = []

    for i in range(num_robots):
        ns = f'robot_{i}'
        robot_pose = f'{i * 1.5} {i * 0.5} 0'

        # 各自的 ROS_DOMAIN
        nodes.append(
            DeclareLaunchArgument(f'robot_{i}_domain', default_value=str(i))
        )

        # SLAM 定位
        nodes.append(
            Node(
                package='slam_toolbox',
                executable='async_slam_toolbox_node',
                name='slam',
                namespace=ns,
                parameters=[{
                    'use_sim_time': False,
                    'map_frame': 'map',
                    'odom_frame': f'{ns}/odom',
                }],
                remappings=[
                    ('/scan', f'{ns}/scan'),
                ],
                domain_id=i,  # 不同域
            )
        )

        # 协同定位
        nodes.append(
            Node(
                package='coop_localization',
                executable=,
                name=,
                namespace=ns,
                parameters=[{
                    : num_robots,
                    : i,
                }],
            )
        )

        
         i == :
            nodes.append(
                Node(
                    package=,
                    executable=,
                    name=,
                    parameters=[{
                        : num_robots,
                        : ,
                    }],
                )
            )

    
    nodes.append(
        Node(
            package=,
            executable=,
            name=,
            parameters=[{: num_robots}],
        )
    )

     LaunchDescription(nodes)

---

故障排查

问题	原因	解决方案
多机 DDS 无法发现	防火墙阻止 UDP 多播	配置防火墙放行 239.255.0.0/24
地图融合错位	各机器人坐标系原点不一致	统一地图原点，使用 AMCL 全局定位
编队散开	通信延迟导致位置同步慢	减小控制周期，增加预测控制
拍卖算法不收敛	任务冲突激烈	增加任务收益多样性，减少竞争
协同定位发散	测量噪声太大	增加 EKF 过程噪声，过滤异常测量

调试命令

# 查看 DDS 发现状态
ros2 daemon stop && ros2 daemon start

# 查看跨机通信话题
ROS_DOMAIN_ID=42 ros2 topic list

# 查看地图融合结果
ros2 topic echo /swarm/map --type nav_msgs/msg/OccupancyGrid

# 手动发布编队中心目标
ros2 topic pub /formation_center geometry_msgs/msg/PoseStamped \
  '{header: {frame_id: world}, pose: {position: {x: 5.0, y: 3.0}}}'

# 网络诊断
ping <robot_ip>
tcpdump -i eth0 udp port 7400 -n  # DDS 端口

---

相关技能

• system-integration/ros2-communication — ROS2 通信基础
• navigation/nav2-integration — Nav2 导航集成
• wheeled_vehicle/navigation — 轮式车辆导航
• quadruped/navigation — 四足机器人导航
• humanoid/navigation — 人形机器人导航