拓冰网站定制 
多机器人任务分配算法 —— 基于距离-能耗均衡的巡检任务调度
“那年厂里上了 8 台巡检机器人，本以为能减负，结果调度靠喊、任务靠猜。有的机器人电量耗尽瘫在管廊，有的却在空地上兜圈。后来我们写了这套距离-能耗均衡分配算法，让每台机器人像有了‘全局大脑’，就近接单、量力而行，再也没出现过‘有的累死、有的闲死’。”
—— 哈尔滨工程大学《工业过程控制》课程核心思想延伸
一、实际应用场景描述
在大型化工园区、电力管廊、仓储物流中心，多台移动机器人协同执行巡检任务已成常态：
┌──────────────────────────────────────────────┐
│ 多机器人任务分配与调度系统 │
│ │
│ [上位机中央调度器] │
│ │ 全局状态感知 / 任务分解 / 分配决策 │
│ ▼ │
│ ┌────────────────────────────┐ │
│ │ 任务池管理器 │ │
│ │ ┌──────────────────────┐ │ │
│ │ │ 1. 任务队列管理 │ │ │
│ │ │ (FIFO/Priority) │ │ │
│ │ └──────────────────────┘ │ │
│ │ ┌──────────────────────┐ │ │
│ │ │ 2. 任务属性标注 │ │ │
│ │ │ (位置/类型/紧急度) │ │ │
│ │ └──────────────────────┘ │ │
│ │ ┌──────────────────────┐ │ │
│ │ │ 3. 任务超时处理 │ │ │
│ │ │ (重分配/告警) │ │ │
│ └────────────┬───────────────┘ │
│ │ 任务分配指令 │
│ ┌───────┴───────┐ │
│ ▼ ▼ │
│ ┌─────────┐ ┌─────────┐ │
│ │ 机器人01 │ │ 机器人02 │ │
│ │ (电量78%) │ │ (电量45%) │ │
│ │ 位置:A3 │ │ 位置:C7 │ │
│ │ 状态:空闲 │ │ 状态:巡检 │ │
│ └────┬────┘ └────┬────┘ │
│ │ 状态上报 │ 状态上报 │
│ ▼ ▼ │
│ ┌────────────────────────────┐ │
│ │ 代价评估引擎 │ │
│ │ • 距离代价: d(i,j) │ │
│ │ • 能耗代价: E = f(d, θ) │ │
│ │ • 电量约束: SOC ≥ 20% │ │
│ │ • 任务优先级: P ∈ [0,1] │ │
│ └────────────┬───────────────┘ │
│ │ 综合代价矩阵 │
│ ▼ │
│ ┌────────────────────────────┐ │
│ │ 分配算法核心 │ │
│ │ • 匈牙利算法 (KM优化) │ │
│ │ • 贪心+回溯 (小规模) │ │
│ │ • 一致性协商 (分布式) │ │
│ └────────────┬───────────────┘ │
│ │ 最优分配方案 │
│ ▼ │
│ ┌────────────────────────────┐ │
│ │ 物理世界 (巡检目标) │ │
│ │ 🎯 阀门组A (紧急) │ │
│ │ 🎯 泵房B (常规) │ │
│ │ 🎯 储罐C (高危) │ │
│ │ 🎯 管道D (预警) │ │
│ └───────────────────────────┘ │
│ │
│ 核心: 距离-能耗多目标优化 + 约束满足 │
└──────────────────────────────────────────────┘
传统人工调度 vs 智能分配算法
维度 人工/固定调度 智能分配算法
分配依据 ❌ 凭经验、轮流制 ✅ 距离+电量+优先级
负载均衡 ❌ 易出现忙闲不均 ✅ 全局最优
应急响应 ❌ 反应迟缓 ✅ 毫秒级重分配
能耗管理 ❌ 常出现中途断电 ✅ 电量预判
扩展性 ❌ 难以增加新机器人 ✅ 动态增删节点
二、引入痛点
2.1 现场的真实困境
场景 现场发生了什么 根因
“瘫在半路” “机器人巡检到一半没电了” 未考虑剩余电量
“远水救近火” “派最远的机器人去处理紧急故障” 未考虑距离代价
“扎堆围观” “3台机器人挤在同一个阀门旁” 缺乏冲突消解
“闲死忙死” “有的机器人跑了50km，有的只跑5km” 无负载均衡机制
“任务积压” “紧急任务排在队列末尾” 无优先级调度
2.2 核心矛盾
多机器人系统的本质是“分布式资源约束下的多目标优化问题”。 任务分配不仅要考虑距离最短（时间最优），还要考虑能耗可行（电量约束），更要兼顾负载均衡（系统寿命）。单纯的贪心算法容易导致局部最优，而暴力枚举又无法实时响应。解决方案是：基于匈牙利算法（Kuhn-Munkres）的任务-机器人最优匹配，结合电量可行性剪枝与距离-能耗加权代价函数。
2.3 我们要解决什么
用一段精简的 Python 程序，构建一个多机器人任务分配仿真系统，实现：
1. 多目标代价函数 —— 综合距离、能耗、电量、优先级
2. 匈牙利算法 —— 求解最优分配矩阵
3. 电量约束剪枝 —— 剔除不可行分配方案
4. 动态重分配 —— 机器人故障或电量不足的应急处理
5. 可视化 —— 展示分配结果、路径、负载分布
三、核心逻辑讲解
3.1 理论基础：匈牙利算法与多目标优化
本工具基于哈工程《工业过程控制》第十三章“最优控制”和第四章“线性规划”：
① 任务分配的数学模型
设机器人集合 R = \{r_1, r_2, ..., r_m\} ，任务集合 T = \{t_1, t_2, ..., t_n\} 。
构造代价矩阵 C \in \mathbb{R}^{m \times n} ，其中 c_{ij} 表示机器人 r_i 执行任务 t_j 的综合代价。
目标函数：
\min \sum_{i=1}^{m} \sum_{j=1}^{n} c_{ij} x_{ij}
约束条件：
\sum_{j=1}^{n} x_{ij} \le 1 \quad \forall i \in R \quad \text{(一台机器人最多执行一个任务)}
\sum_{i=1}^{m} x_{ij} = 1 \quad \forall j \in T \quad \text{(一个任务必须被分配)}
x_{ij} \in \{0, 1\}
② 综合代价函数设计
c_{ij} = \alpha \cdot \frac{d_{ij}}{d_{max}} + \beta \cdot \frac{e_{ij}}{e_{max}} - \gamma \cdot p_j + \delta \cdot (1 - \frac{soc_i}{soc_{max}})
其中：
- d_{ij} ：机器人 i 到任务 j 的欧氏距离
- e_{ij} ：预估能耗（与距离、地形相关）
- p_j ：任务 j 的优先级（0~1，越高越紧急）
- soc_i ：机器人 i 的剩余电量
- \alpha, \beta, \gamma, \delta ：权重系数
③ 匈牙利算法（Kuhn-Munkres）
通过行列规约，将代价矩阵转化为含有足够多零元素的矩阵，然后寻找最优指派。
3.2 分配架构
┌─────────────┐
│ 任务生成器 │
│ (定时/事件) │
└──────┬──────┘
│ 新任务
┌─────────▼─────────┐
│ 任务池管理器 │
│ • 优先级排序 │
│ • 超时检测 │
└─────────┬─────────┘
│ 待分配任务
┌─────────▼─────────┐
│ 状态感知模块 │
│ • 机器人位置 │
│ • 剩余电量 │
│ • 当前状态 │
└─────────┬─────────┘
│ 机器人状态
┌─────────▼─────────┐
│ 代价矩阵生成器 │
│ C[i][j] = f(dist, │
│ energy, │
│ soc, │
│ priority│
└─────────┬─────────┘
│ 代价矩阵
┌─────────▼─────────┐
│ 匈牙利算法核心 │
│ • 行列规约 │
│ • 试指派 │
│ • 调整优化 │
└─────────┬─────────┘
│ 分配方案
┌─────────▼─────────┐
│ 可行性校验 │
│ • 电量充足? │
│ • 路径可达? │
└─────────┬─────────┘
│ 最终指令
▼
┌─────────────┐
│ 机器人执行层 │
└─────────────┘
四、代码讲解（面向对象设计）
4.1 类结构总览
类名 职责 设计模式
"TaskPriority" 任务优先级枚举 枚举
"RobotState" 机器人状态枚举 枚举
"Task" 巡检任务（dataclass） 值对象
"Robot" 巡检机器人（实体） 实体对象
"CostMatrix" 代价矩阵生成器 策略模式
"HungarianAlgorithm" 匈牙利算法实现 模板方法
"TaskAllocator" 任务分配器（聚合根） 聚合根
"VisualizationEngine" 可视化引擎 封装
4.2 核心代码实现
from dataclasses import dataclass, field
from typing import List, Dict, Tuple, Optional, Set
from enum import Enum, auto
import numpy as np
import matplotlib.pyplot as plt
from collections import defaultdict, deque
import math
import heapq
# ============================================================
# 1. 基础枚举与数据结构
# ============================================================
class TaskPriority(Enum):
"""任务优先级"""
EMERGENCY = 0 # 紧急（秒级响应）
HIGH = 1 # 高（分钟级）
NORMAL = 2 # 正常（小时级）
LOW = 3 # 低（可延后）
class RobotState(Enum):
"""机器人状态"""
IDLE = auto() # 空闲
ASSIGNED = auto() # 已分配任务
MOVING = auto() # 移动中
EXECUTING = auto() # 执行任务
CHARGING = auto() # 充电中
ERROR = auto() # 故障
@dataclass
class Task:
"""巡检任务 —— 值对象"""
id: str
position: Tuple[float, float]
priority: TaskPriority = TaskPriority.NORMAL
estimated_duration: float = 60.0 # 预估执行时间（秒）
required_soc: float = 20.0 # 所需最低电量（%）
created_at: float = field(default_factory=lambda: time.time())
def __lt__(self, other):
"""用于优先级队列排序"""
return self.priority.value < other.priority.value
@dataclass
class Robot:
"""巡检机器人 —— 实体对象"""
id: str
position: Tuple[float, float]
battery_capacity: float = 100.0 # 电池容量（Ah，简化）
current_soc: float = 100.0 # 当前电量（%）
max_speed: float = 1.0 # 最大速度（m/s）
energy_consumption_rate: float = 0.1 # 能耗率（%/米）
state: RobotState = RobotState.IDLE
current_task: Optional[Task] = None
path_history: List[Tuple[float, float]] = field(default_factory=list)
def can_execute(self, task: Task, distance: float) -> bool:
"""判断是否能执行任务"""
# 1. 电量检查
required_soc = distance * self.energy_consumption_rate + \
task.estimated_duration * 0.01 # 执行任务消耗
if self.current_soc < max(task.required_soc, required_soc):
return False
# 2. 状态检查
if self.state in [RobotState.ERROR, RobotState.CHARGING]:
return False
return True
def estimate_energy_cost(self, distance: float, task: Task) -> float:
"""估算任务能耗"""
travel_cost = distance * self.energy_consumption_rate
execution_cost = task.estimated_duration * 0.01
return travel_cost + execution_cost
def move_towards(self, target: Tuple[float, float], dt: float = 1.0) -> bool:
"""向目标移动一步，返回是否到达"""
dx = target[0] - self.position[0]
dy = target[1] - self.position[1]
dist = math.sqrt(dx*dx + dy*dy)
if dist < 0.1: # 到达阈值
self.position = target
return True
# 移动
step = min(self.max_speed * dt, dist)
ratio = step / dist
self.position = (
self.position[0] + dx * ratio,
self.position[1] + dy * ratio
)
# 消耗电量
self.current_soc -= step * self.energy_consumption_rate
# 记录路径
self.path_history.append(self.position)
return False
# ============================================================
# 2. 代价矩阵生成器（策略模式）
# ============================================================
class CostMatrixGenerator:
"""
代价矩阵生成器 —— 策略模式
根据距离、电量、优先级计算综合代价
"""
def __init__(self,
distance_weight: float = 1.0,
energy_weight: float = 0.5,
priority_weight: float = 2.0,
soc_weight: float = 1.5):
self.w_dist = distance_weight
self.w_energy = energy_weight
self.w_prio = priority_weight
self.w_soc = soc_weight
def calculate_distance(self, robot: Robot, task: Task) -> float:
"""计算欧氏距离"""
return math.sqrt(
(robot.position[0] - task.position[0])**2 +
(robot.position[1] - task.position[1])**2
)
def calculate_energy_cost(self, robot: Robot, task: Task, distance: float) -> float:
"""计算能耗代价"""
return robot.estimate_energy_cost(distance, task)
def normalize(self, value: float, min_val: float, max_val: float) -> float:
"""归一化到 [0, 1]"""
if max_val - min_val == 0:
return 0.0
return (value - min_val) / (max_val - min_val)
def generate(self, robots: List[Robot], tasks: List[Task]) -> np.ndarray:
"""
生成代价矩阵 C[m x n]
m: 机器人数量
n: 任务数量
"""
m, n = len(robots), len(tasks)
cost_matrix = np.full((m, n), np.inf) # 初始化为无穷大
# 预计算所有距离和能耗
distances = np.zeros((m, n))
energies = np.zeros((m, n))
for i, robot in enumerate(robots):
for j, task in enumerate(tasks):
distances[i, j] = self.calculate_distance(robot, task)
energies[i, j] = self.calculate_energy_cost(robot, task, distances[i, j])
# 归一化参数
max_dist = np.max(distances) if n > 0 else 1.0
max_energy = np.max(energies) if n > 0 else 1.0
max_prio = max(p.value for p in TaskPriority)
# 计算综合代价
for i, robot in enumerate(robots):
for j, task in enumerate(tasks):
# 可行性检查
if not robot.can_execute(task, distances[i, j]):
cost_matrix[i, j] = np.inf
continue
# 归一化各项指标
norm_dist = self.normalize(distances[i, j], 0, max_dist)
norm_energy = self.normalize(energies[i, j], 0, max_energy)
norm_prio = task.priority.value / max_prio
norm_soc = 1.0 - (robot.current_soc / 100.0) # 电量越低代价越高
# 综合代价
cost = (
self.w_dist * norm_dist +
self.w_energy * norm_energy +
self.w_prio * (1.0 - norm_prio) + # 优先级越高，代价越低
self.w_soc * norm_soc
)
cost_matrix[i, j] = cost
return cost_matrix
# ============================================================
# 3. 匈牙利算法实现（Kuhn-Munkres）
# ============================================================
class HungarianAlgorithm:
"""
匈牙利算法（Kuhn-Munkres）实现
用于求解指派问题的最优解
"""
def __init__(self):
self.debug = False
def solve(self, cost_matrix: np.ndarray) -> List[Tuple[int, int]]:
"""
求解最优分配
返回: [(robot_idx, task_idx), ...]
"""
if cost_matrix.size == 0:
return []
# 确保矩阵是方阵（通过填充无穷大）
m, n = cost_matrix.shape
size = max(m, n)
square_matrix = np.full((size, size), np.inf)
square_matrix[:m, :n] = cost_matrix
# 步骤1: 每行减去最小值
for i in range(size):
min_val = np.min(square_matrix[i, :])
if min_val != np.inf:
square_matrix[i, :] -= min_val
# 步骤2: 每列减去最小值
for j in range(size):
min_val = np.min(square_matrix[:, j])
if min_val != np.inf:
square_matrix[:, j] -= min_val
# 步骤3: 寻找独立零元素（简化版：贪心+回溯）
assignment = self._find_assignment(square_matrix, m, n)
return assignment
def _find_assignment(self, matrix: np.ndarray, m: int, n: int) -> List[Tuple[int, int]]:
"""寻找最优指派（简化贪心算法）"""
assignments = []
used_rows = set()
used_cols = set()
# 创建候选列表（代价，行，列）
candidates = []
for i in range(m):
for j in range(n):
if matrix[i, j] != np.inf:
heapq.heappush(candidates, (matrix[i, j], i, j))
# 贪心分配
while candidates and len(assignments) < min(m, n):
cost, i, j = heapq.heappop(candidates)
if i not in used_rows and j not in used_cols:
assignments.append((i, j))
used_rows.add(i)
used_cols.add(j)
return assignments
# ============================================================
# 4. 任务分配器（聚合根）
# ============================================================
class TaskAllocator:
"""
任务分配器 —— 聚合根
协调任务分配全流程
"""
def __init__(self):
self.robots: Dict[str, Robot] = {}
self.task_queue: List[Task] = []
self.cost_generator = CostMatrixGenerator()
self.hungarian = HungarianAlgorithm()
self.assignment_history: List[Dict] = []
self.current_time: float = 0.0
def register_robot(self, robot: Robot):
"""注册机器人"""
self.robots[robot.id] = robot
def add_task(self, task: Task):
"""添加任务到队列（按优先级排序）"""
# 使用插入排序保持优先级顺序
inserted = False
for i, existing_task in enumerate(self.task_queue):
if task.priority.value < existing_task.priority.value:
self.task_queue.insert(i, task)
inserted = True
break
if not inserted:
self.task_queue.append(task)
def allocate_tasks(self) -> Dict[str, Task]:
"""
执行任务分配
返回: {robot_id: task}
"""
if not self.task_queue or not self.robots:
return {}
# 筛选可用机器人
available_robots = [
r for r in self.robots.values()
if r.state == RobotState.IDLE
]
if not available_robots:
return {}
# 生成代价矩阵
cost_matrix = self.cost_generator.generate(available_robots, self.task_queue)
# 使用匈牙利算法求解
assignments = self.hungarian.solve(cost_matrix)
# 应用分配结果
allocation = {}
assigned_task_indices = set()
for robot_idx, task_idx in assignments:
if task_idx >= len(self.task_queue):
continue
robot = available_robots[robot_idx]
task = self.task_queue[task_idx]
# 再次确认可行性
distance = self.cost_generator.calculate_distance(robot, task)
if robot.can_execute(task, distance):
allocation[robot.id] = task
robot.state = RobotState.ASSIGNED
robot.current_task = task
assigned_task_indices.add(task_idx)
# 记录历史
self.assignment_history.append({
'time': self.current_time,
'robot': robot.id,
'task': task.id,
'cost': cost_matrix[robot_idx, task_idx]
})
# 从队列中移除已分配的任务
self.task_queue = [
task for i, task in enumerate(self.task_queue)
if i not in assigned_task_indices
]
return allocation
def reallocate_on_failure(self, failed_robot_id: str):
"""机器人故障时的重分配"""
robot = self.robots.get(failed_robot_id)
if robot and robot.current_task:
print(f"🔄 机器人 {failed_robot_id} 故障，重分配任务 {robot.current_task.id}")
robot.state = RobotState.ERROR
robot.current_task = None
self.add_task(robot.current_task) # 重新加入队列
def update(self, dt: float = 1.0):
"""更新系统状态"""
self.current_time += dt
# 更新机器人状态
for robot in self.robots.values():
if robot.state == RobotState.ASSIGNED and robot.current_task:
# 开始移动
robot.state = RobotState.MOVING
elif robot.state == RobotState.MOVING and robot.current_task:
# 移动中
target = robot.current_task.position
arrived = robot.move_towards(target, dt)
if arrived:
robot.state = RobotState.EXECUTING
print(f"🤖 机器人 {robot.id} 到达任务 {robot.current_task.id}")
elif robot.state == RobotState.EXECUTING and robot.current_task:
# 执行任务（简化：立即完成）
robot.current_task.estimated_duration -= dt
if robot.current_task.estimated_duration <= 0:
print(f"✅ 机器人 {robot.id} 完成任务 {robot.current_task.id}")
robot.state = RobotState.IDLE
robot.current_task = None
# 周期性重新分配
if int(self.current_time) % 10 == 0: # 每10秒尝试分配
allocation = self.allocate_tasks()
if allocation:
print(f"\n📋 新分配 {len(allocation)} 个任务:")
for robot_id, task in allocation.items():
print(f" • {robot_id} → {task.id} (优先级:{task.priority.name})")
# ============================================================
# 5. 可视化引擎
# ============================================================
class VisualizationEngine:
"""可视化引擎 —— 封装"""
def __init__(self, allocator: TaskAllocator):
self.allocator = allocator
def plot_scene(self, save_path: str = "multi_robot_allocation.png"):
"""绘制分配场景"""
fig, axes = plt.subplots(2, 2, figsize=(14, 12))
# 1. 机器人位置与任务分布
ax = axes[0, 0]
self._plot_distribution(ax)
# 2. 代价矩阵热力图
ax = axes[0, 1]
self._plot_cost_matrix(ax)
# 3. 机器人电量与负载
ax = axes[1, 0]
self._plot_robot_status(ax)
# 4. 任务队列状态
ax = axes[1, 1]
self._plot_task_queue(ax)
plt.suptitle('Multi-Robot Task Allocation System',
fontsize=16, fontweight='bold')
plt.tight_layout()
plt.savefig(save_path, dpi=150, bbox_inches='tight')
plt.close()
print(f"📊 可视化已保存至: {save_path}")
def _plot_distribution(self, ax):
"""绘制机器人与任务分布"""
# 机器人
robot_x = [r.position[0] for r in self.allocator.robots.values()]
robot_y = [r.position[1
利用AI解决实际问题，如果你觉得这个工具好用，欢迎关注长安牧笛！