多机器人任务分配算法 —— 基于距离-能耗均衡的巡检任务调度“那年厂里上了 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_{i1}^{m} \sum_{j1}^{n} c_{ij} x_{ij}约束条件\sum_{j1}^{n} x_{ij} \le 1 \quad \forall i \in R \quad \text{(一台机器人最多执行一个任务)}\sum_{i1}^{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, fieldfrom typing import List, Dict, Tuple, Optional, Setfrom enum import Enum, autoimport numpy as npimport matplotlib.pyplot as pltfrom collections import defaultdict, dequeimport mathimport 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() # 故障dataclassclass Task:巡检任务 —— 值对象id: strposition: Tuple[float, float]priority: TaskPriority TaskPriority.NORMALestimated_duration: float 60.0 # 预估执行时间秒required_soc: float 20.0 # 所需最低电量%created_at: float field(default_factorylambda: time.time())def __lt__(self, other):用于优先级队列排序return self.priority.value other.priority.valuedataclassclass Robot:巡检机器人 —— 实体对象id: strposition: Tuple[float, float]battery_capacity: float 100.0 # 电池容量Ah简化current_soc: float 100.0 # 当前电量%max_speed: float 1.0 # 最大速度m/senergy_consumption_rate: float 0.1 # 能耗率%/米state: RobotState RobotState.IDLEcurrent_task: Optional[Task] Nonepath_history: List[Tuple[float, float]] field(default_factorylist)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 Falsereturn Truedef estimate_energy_cost(self, distance: float, task: Task) - float:估算任务能耗travel_cost distance * self.energy_consumption_rateexecution_cost task.estimated_duration * 0.01return travel_cost execution_costdef 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 targetreturn True# 移动step min(self.max_speed * dt, dist)ratio step / distself.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_weightself.w_energy energy_weightself.w_prio priority_weightself.w_soc soc_weightdef 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.0return (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.0max_energy np.max(energies) if n 0 else 1.0max_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.infcontinue# 归一化各项指标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_prionorm_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] costreturn cost_matrix# # 3. 匈牙利算法实现Kuhn-Munkres# class HungarianAlgorithm:匈牙利算法Kuhn-Munkres实现用于求解指派问题的最优解def __init__(self):self.debug Falsedef solve(self, cost_matrix: np.ndarray) - List[Tuple[int, int]]:求解最优分配返回: [(robot_idx, task_idx), ...]if cost_matrix.size 0:return []# 确保矩阵是方阵通过填充无穷大m, n cost_matrix.shapesize 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 assignmentdef _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.0def register_robot(self, robot: Robot):注册机器人self.robots[robot.id] robotdef add_task(self, task: Task):添加任务到队列按优先级排序# 使用插入排序保持优先级顺序inserted Falsefor i, existing_task in enumerate(self.task_queue):if task.priority.value existing_task.priority.value:self.task_queue.insert(i, task)inserted Truebreakif 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):continuerobot 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] taskrobot.state RobotState.ASSIGNEDrobot.current_task taskassigned_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 allocationdef 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.ERRORrobot.current_task Noneself.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.MOVINGelif robot.state RobotState.MOVING and robot.current_task:# 移动中target robot.current_task.positionarrived robot.move_towards(target, dt)if arrived:robot.state RobotState.EXECUTINGprint(f 机器人 {robot.id} 到达任务 {robot.current_task.id})elif robot.state RobotState.EXECUTING and robot.current_task:# 执行任务简化立即完成robot.current_task.estimated_duration - dtif robot.current_task.estimated_duration 0:print(f✅ 机器人 {robot.id} 完成任务 {robot.current_task.id})robot.state RobotState.IDLErobot.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 allocatordef 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,fontsize16, fontweightbold)plt.tight_layout()plt.savefig(save_path, dpi150, bbox_inchestight)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解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛