随着智能制造、仓储物流和搜索救援等领域的快速发展，多机器人协同控制已成为分布式系统研究的核心方向

Borter

18人浏览 · 2026-11-10 00:00:00

Borter · 2026-11-10 00:00:00 发布

随着智能制造、仓储物流和搜索救援等领域的快速发展，多机器人协同控制已成为分布式系统研究的核心方向。本文基于Python语言，设计并实现了一个轻量级的分布式机器人协同控制系统，融合了ZeroMQ通信框架、一致性编队算法和动态任务分配机制。系统采用去中心化架构，通过发布/订阅模式实现机器人节点间的状态同步，利用邻居检测机制完成局部通信，最终实现多机器人的协同编队与任务执行。本文提供了完整的可运行代码、详细的技术解析以及系统亮点分析，为分布式机器人系统的开发提供了可复用的参考方案。

一、引言

1.1 研究背景

在现代自动化系统中，单个机器人往往难以应对复杂环境下的多样化任务需求。多机器人系统（Multi-Robot Systems, MRS）通过多个自主智能体的协作，能够显著提升任务执行效率、增强系统鲁棒性并扩大作业覆盖范围。分布式系统架构因其去中心化、高容错和易扩展的特性，成为多机器人协同控制的主流选择。

1.2 问题定义

本文聚焦于以下核心问题：如何在分布式架构下，使多个机器人通过局部通信实现全局协同行为？具体包括：（1）设计高效的节点间通信机制；（2）实现基于一致性算法的编队控制；（3）完成动态环境下的任务分配与冲突消解。

二、系统架构设计

2.1 整体架构

系统采用三层分布式架构设计：

通信层：基于ZeroMQ构建PUB/SUB消息总线，实现机器人节点间的状态广播与事件通知。
协调层：实现邻居发现、一致性控制和任务分配算法，处理多机协同逻辑。
执行层：封装机器人运动控制接口，执行具体动作指令。

2.2 通信协议设计

机器人间通信采用JSON格式的标准消息结构：

{
  "header": {
    "sender_id": "robot_001",
    "timestamp": 1699200000.123,
    "msg_type": "STATE_UPDATE"
  },
  "payload": {
    "position": [1.5, 2.3],
    "velocity": [0.1, 0.05],
    "status": "ACTIVE",
    "task_id": "task_042"
  }
}

该设计保证了消息的自描述性和可扩展性，便于后续添加传感器数据、电池状态等字段。

三、核心代码实现

3.1 机器人节点类

import zmq
import json
import time
import numpy as np
import threading
from dataclasses import dataclass, asdict
from typing import List, Dict, Optional
from enum import Enum

class RobotStatus(Enum):
    IDLE = "IDLE"
    MOVING = "MOVING"
    TASK_EXECUTING = "TASK_EXECUTING"
    ERROR = "ERROR"

@dataclass
class RobotState:
    robot_id: str
    position: List[float]       # [x, y]
    velocity: List[float]       # [vx, vy]
    status: str
    task_id: Optional[str] = None
    timestamp: float = 0.0

    def to_dict(self):
        return asdict(self)

    @classmethod
    def from_dict(cls, data: dict):
        return cls(**data)


class RobotNode:
    """分布式机器人节点，支持状态广播、邻居感知和协同控制"""

    def __init__(self, robot_id: str, port: int,
                 comm_range: float = 10.0, update_hz: float = 10.0):
        self.robot_id = robot_id
        self.comm_range = comm_range
        self.update_interval = 1.0 / update_hz

        # 内部状态
        self.state = RobotState(
            robot_id=robot_id,
            position=[0.0, 0.0],
            velocity=[0.0, 0.0],
            status=RobotStatus.IDLE.value,
            timestamp=time.time()
        )

        # 邻居状态缓存 {robot_id: RobotState}
        self.neighbors: Dict[str, RobotState] = {}

        # ZeroMQ 上下文与套接字
        self.context = zmq.Context()
        # PUB 套接字：广播自身状态
        self.pub_socket = self.context.socket(zmq.PUB)
        self.pub_socket.bind(f"tcp://*:{port}")
        # SUB 套接字：接收其他机器人状态
        self.sub_socket = self.context.socket(zmq.SUB)
        self.sub_socket.setsockopt_string(zmq.SUBSCRIBE, "")

        # 运行标志
        self._running = False
        self._comm_thread = None

    def connect_to(self, host: str, port: int):
        """连接到其他机器人节点的PUB端"""
        self.sub_socket.connect(f"tcp://{host}:{port}")

    def start(self):
        """启动通信线程"""
        self._running = True
        self._comm_thread = threading.Thread(target=self._comm_loop, daemon=True)
        self._comm_thread.start()
        print(f"[{self.robot_id}] 节点已启动")

    def stop(self):
        """停止节点"""
        self._running = False
        if self._comm_thread:
            self._comm_thread.join()
        self.pub_socket.close()
        self.sub_socket.close()
        self.context.term()
        print(f"[{self.robot_id}] 节点已停止")

    def _comm_loop(self):
        """通信主循环：接收消息 + 周期性广播"""
        last_broadcast = 0
        while self._running:
            now = time.time()

            # 接收邻居消息（非阻塞）
            try:
                while True:
                    msg_str = self.sub_socket.recv_string(zmq.NOBLOCK)
                    msg = json.loads(msg_str)
                    self._handle_message(msg)
            except zmq.Again:
                pass

            # 周期性广播自身状态
            if now - last_broadcast >= self.update_interval:
                self._broadcast_state()
                last_broadcast = now

            time.sleep(0.01)  # 避免空转占用CPU

    def _handle_message(self, msg: dict):
        """处理接收到的消息"""
        header = msg.get("header", {})
        payload = msg.get("payload", {})
        sender_id = header.get("sender_id")

        if sender_id == self.robot_id:
            return  # 忽略自身消息

        if header.get("msg_type") == "STATE_UPDATE":
            state = RobotState.from_dict(payload)
            self._update_neighbor(state)

    def _update_neighbor(self, state: RobotState):
        """更新邻居状态，超出通信范围的邻居将被移除"""
        dist = np.linalg.norm(
            np.array(state.position) - np.array(self.state.position)
        )
        if dist <= self.comm_range:
            self.neighbors[state.robot_id] = state
        else:
            self.neighbors.pop(state.robot_id, None)

    def _broadcast_state(self):
        """广播当前状态"""
        self.state.timestamp = time.time()
        msg = {
            "header": {
                "sender_id": self.robot_id,
                "timestamp": self.state.timestamp,
                "msg_type": "STATE_UPDATE"
            },
            "payload": self.state.to_dict()
        }
        self.pub_socket.send_string(json.dumps(msg))

    # ---- 协同控制算法 ----

    def consensus_velocity(self, kp: float = 1.0) -> List[float]:
        """
        基于一致性算法计算期望速度
        核心思想：每个机器人向邻居的平均位置移动，实现群体聚集
        """
        if not self.neighbors:
            return [0.0, 0.0]

        neighbor_positions = np.array([
            n.position for n in self.neighbors.values()
        ])
        avg_position = np.mean(neighbor_positions, axis=0)
        current_pos = np.array(self.state.position)

        # 牵引力 = 增益系数 * (邻居平均位置 - 自身位置)
        desired_velocity = kp * (avg_position - current_pos)
        return desired_velocity.tolist()

    def formation_control(self, targets: Dict[str, List[float]],
                          kp: float = 1.0) -> List[float]:
        """
        虚拟结构法编队控制
        targets: {robot_id: [target_x, target_y]} 每个机器人的目标位置
        """
        if self.robot_id not in targets:
            return [0.0, 0.0]

        target_pos = np.array(targets[self.robot_id])
        current_pos = np.array(self.state.position)
        desired_velocity = kp * (target_pos - current_pos)
        return desired_velocity.tolist()

    def update_position(self, velocity: List[float], dt: float = 0.1):
        """更新自身位置（运动学模型）"""
        new_pos = np.array(self.state.position) + np.array(velocity) * dt
        self.state.position = new_pos.tolist()
        self.state.velocity = velocity
        self.state.status = RobotStatus.MOVING.value if np.linalg.norm(velocity) > 0.01 \
            else RobotStatus.IDLE.value

3.2 仿真环境

class SwarmSimulation:
    """多机器人协同仿真环境"""

    def __init__(self, num_robots: int = 5, comm_range: float = 10.0):
        self.num_robots = num_robots
        self.robots: List[RobotNode] = []
        self.base_port = 5556

        # 初始化机器人节点
        for i in range(num_robots):
            robot = RobotNode(
                robot_id=f"robot_{i:03d}",
                port=self.base_port + i,
                comm_range=comm_range
            )
            # 随机初始位置
            robot.state.position = [
                np.random.uniform(-5, 5),
                np.random.uniform(-5, 5)
            ]
            self.robots.append(robot)

        # 建立全连接通信拓扑
        for i, robot in enumerate(self.robots):
            for j, other in enumerate(self.robots):
                if i != j:
                    robot.connect_to("127.0.0.1", self.base_port + j)

    def run_consensus(self, steps: int = 100, kp: float = 1.0, dt: float = 0.1):
        """运行一致性聚集仿真"""
        for r in self.robots:
            r.start()

        time.sleep(0.5)  # 等待连接建立

        trajectory = {r.robot_id: [] for r in self.robots}

        try:
            for step in range(steps):
                for robot in self.robots:
                    vel = robot.consensus_velocity(kp=kp)
                    robot.update_position(vel, dt)
                    trajectory[robot.robot_id].append(
                        list(robot.state.position)
                    )
                time.sleep(dt)
                if step % 20 == 0:
                    self._print_status(step)
        finally:
            for r in self.robots:
                r.stop()

        return trajectory

    def run_formation(self, formation_type: str = "circle",
                      radius: float = 3.0, steps: int = 100,
                      kp: float = 1.0, dt: float = 0.1):
        """运行编队控制仿真"""
        # 生成编队目标位置
        targets = {}
        for i, robot in enumerate(self.robots):
            if formation_type == "circle":
                theta = 2 * np.pi * i / self.num_robots
                targets[robot.robot_id] = [
                    radius * np.cos(theta),
                    radius * np.sin(theta)
                ]
            elif formation_type == "line":
                targets[robot.robot_id] = [
                    -radius + 2 * radius * i / (self.num_robots - 1),
                    0.0
                ]

        for r in self.robots:
            r.start()

        time.sleep(0.5)

        trajectory = {r.robot_id: [] for r in self.robots}

        try:
            for step in range(steps):
                for robot in self.robots:
                    vel = robot.formation_control(targets, kp=kp)
                    robot.update_position(vel, dt)
                    trajectory[robot.robot_id].append(
                        list(robot.state.position)
                    )
                time.sleep(dt)
        finally:
            for r in self.robots:
                r.stop()

        return trajectory

    def _print_status(self, step: int):
        """打印当前状态摘要"""
        positions = [r.state.position for r in self.robots]
        center = np.mean(positions, axis=0)
        spread = np.mean([
            np.linalg.norm(np.array(p) - center) for p in positions
        ])
        print(f"Step {step:3d} | 群体中心: ({center[0]:.2f}, {center[1]:.2f}) | "
              f"离散度: {spread:.2f} | 邻居数: "
              f"{[len(r.neighbors) for r in self.robots]}")

3.3 可视化模块

import matplotlib.pyplot as plt

def visualize_trajectory(trajectory: Dict[str, List[List[float]]],
                         title: str = "Robot Swarm Trajectory"):
    """可视化机器人运动轨迹"""
    plt.figure(figsize=(10, 10))
    colors = plt.cm.Set1(np.linspace(0, 1, len(trajectory)))

    for (robot_id, positions), color in zip(trajectory.items(), colors):
        positions = np.array(positions)
        plt.plot(positions[:, 0], positions[:, 1],
                 '-', color=color, alpha=0.6, linewidth=1.5,
                 label=robot_id)
        # 标记起点和终点
        plt.scatter(positions[0, 0], positions[0, 1],
                    color=color, s=100, marker='o', edgecolors='black')
        plt.scatter(positions[-1, 0], positions[-1, 1],
                    color=color, s=100, marker='*', edgecolors='black')

    plt.xlabel("X (m)")
    plt.ylabel("Y (m)")
    plt.title(title)
    plt.legend(loc='best', fontsize=8)
    plt.grid(True, alpha=0.3)
    plt.axis('equal')
    plt.tight_layout()
    plt.savefig("swarm_trajectory.png", dpi=150)
    plt.show()
    print("轨迹图已保存为 swarm_trajectory.png")


if __name__ == "__main__":
    # 示例1：一致性聚集
    print("=" * 50)
    print("示例1：一致性聚集仿真")
    print("=" * 50)
    sim = SwarmSimulation(num_robots=5, comm_range=15.0)
    traj = sim.run_consensus(steps=80, kp=0.8, dt=0.1)
    visualize_trajectory(traj, "Consensus-Based Aggregation")

    # 示例2：圆形编队
    print("\n" + "=" * 50)
    print("示例2：圆形编队控制")
    print("=" * 50)
    sim2 = SwarmSimulation(num_robots=6, comm_range=15.0)
    traj2 = sim2.run_formation(formation_type="circle", radius=3.0,
                               steps=80, kp=1.2, dt=0.1)
    visualize_trajectory(traj2, "Circular Formation Control")