AGV动态避障与通道占用重规划系统 —— 基于增量式A*的OOP实战周三上午10点巡检工发现3号反应釜旁边的蒸汽阀门法兰在滴漏维修班拉了警戒带把那条主通道给占了。正好有两辆AGV要从原料仓往那边送料——一辆刚出发一辆还在待命。调度系统屏幕上两条路径瞬间变红弹了一堆路径冲突报警。操作员慌了手动把两辆车都切到手动模式然后打电话问我你能不能让系统自己找条新路出来我当时就想如果路径规划模块能感知到通道被占这个事件自动触发重规划而不是等操作员介入就好了。—— 哈尔滨工程大学《工业过程控制》课程核心思想延伸一、实际应用场景描述在化工厂区AGV负责在仓库、反应釜区、包装线之间往返运送原料桶和成品。厂区通道布局相对固定但经常因为设备检修、临时堆放、人员作业等原因出现动态障碍物。┌──────────────────────────────────────────────┐│ 厂区通道网格地图 ││ ││ W仓库 R反应釜区 P包装线 ││ ││ ┌───┬───┬───┬───┬───┬───┐ ││ │ W │ . │ . │ . │ R │ . │ 反应釜区 ││ ├───┼───┼───┼───┼───┼───┤ ││ │ . │ # │ . │ # │ # │ . │ ← 阀门检修! ││ ├───┼───┼───┼───┼───┼───┤ (通道被占) ││ │ . │ . │ . │ . │ # │ # │ ││ ├───┼───┼───┼───┼───┼───┤ ││ │ # │ # │ . │ . │ . │ # │ ││ ├───┼───┼───┼───┼───┼───┤ ││ │ . │ . │ . │ # │ . │ . │ 包装线 ││ └───┴───┴───┴───┴───┴───┘ ││ ││ 事件: 阀门检修占用(2,1)-(2,2)通道 ││ AGV-01 需要从(0,0)到(4,0) ││ 原路径被阻断 → 需要实时重规划 │└──────────────────────────────────────────────┘动态障碍 vs 静态障碍类型 例子 持续时间 处理方式静态障碍 设备、墙壁、立柱 永久 建图时固化动态障碍 检修警戒、临时托盘、人员 数小时~数天 运行时动态更新瞬时障碍 其他AGV、叉车经过 数秒~数分钟 实时避障二、引入痛点2.1 现场的真实困境场景 现场发生了什么 根因路径断了 阀门检修拉了警戒带AGV走到一半发现路没了原地死等 无动态重规划人工接管 调度屏上一堆红条操作员挨个切手动模式绕路 系统不会自动处理全局重算慢 厂区那么大每次重算要好几秒AGV全堵着 增量更新能力不足多车互锁 两辆车同时重规划又选了同一条备用通道堵在一起 无路径锁定机制恢复后不回原路 检修结束了AGV还在走绕远的路 无恢复原路径逻辑2.2 核心矛盾AGV的路径不是一次性的而是需要根据环境变化持续调整的。 当通道被占时系统需要① 立即感知障碍事件② 判断当前路径是否受影响③ 只对受影响的部分做局部重规划④ 如果有其他AGV还要协调不撞车。这里的核心算法是增量式A*——不是每次从头搜而是在原有路径基础上修补。2.3 我们要解决什么用一段精简的 Python 程序构建一个AGV动态避障与通道占用重规划系统实现1. 栅格地图管理 —— 静态障碍 动态障碍分层存储2. A*全局路径规划 —— 带曼哈顿启发3. 动态障碍事件处理 —— 运行时添加/移除通道占用4. 受影响的路径段识别 —— 判断哪些AGV需要重规划5. 增量重规划 —— 从阻断点重新搜索到目标6. 可视化 —— 地图状态 路径对比三、核心逻辑讲解3.1 理论基础增量式重规划本工具基于哈工程《工业过程控制》第十二章运动控制与路径规划和第十四章生产调度① A*算法回顾f(n) g(n) h(n)② 增量式重规划思路当障碍出现在原路径上时原路径: S → A → B → C → D → G障碍阻断: B → C 段被占增量重规划:起点改为 B阻断点前的最后一个可达点终点仍为 G在新起点和新终点之间搜索新路径拼接: S → A → B → [新路径] → G③ 路径受影响判定affected (obstacle \in path\_cells)如果障碍物不在任何AGV的当前路径上无需重规划。3.2 系统数据流┌──────────────────────────────────────────────┐│ 地图 AGV车队 任务列表 │└──────────────┬───────────────────────────────┘│┌──────────────▼───────────────┐│ ① 初始路径规划 ││ 为每辆AGV计算全局最优路径 │└──────────────┬───────────────┘│┌──────────────▼───────────────┐│ ② 监听障碍事件 ││ 阀门检修/临时占道 → 更新地图 │└──────────────┬───────────────┘│┌──────────────▼───────────────┐│ ③ 影响分析 ││ 哪些AGV的路径穿过了障碍点 │└──────────────┬───────────────┘│┌──────────────▼───────────────┐│ ④ 增量重规划 ││ 从阻断点重新搜索到目标 │└──────────────┬───────────────┘│┌──────────────▼───────────────┐│ ⑤ 路径拼接 下发 ││ 新段替换旧段其余保持不变 │└──────────────┬───────────────┘│┌──────────────▼───────────────┐│ ⑥ 可视化 ││ 原路径 vs 新路径对比 │└──────────────────────────────┘四、代码讲解面向对象设计4.1 类结构总览类名 职责 设计模式GridMap 栅格地图静态动态障碍分离 值对象Waypoint 坐标点dataclass 值对象AgvTask AGV任务dataclass 值对象RouteResult 路径结果dataclass 值对象Heuristic 启发函数ABC 策略模式ManhattanHeuristic 曼哈顿距离实现 策略模式AStarPlanner A*路径规划器 封装DynamicMapManager 动态地图管理器 观察者模式IncrementalReplanner 增量重规划器 封装AgvFleetManager AGV车队管理器 封装RouteVisualizer 路径可视化器 封装AgvDispatchSystem 系统编排器聚合根 聚合根4.2 数据模型层from dataclasses import dataclass, fieldfrom typing import List, Dict, Set, Tuple, Optional, Callablefrom abc import ABC, abstractmethodimport heapqimport mathimport numpy as npimport matplotlib.pyplot as pltdataclass(frozenTrue)class Waypoint:坐标点 —— 值对象x: inty: intdef __add__(self, other: Waypoint) - Waypoint:return Waypoint(self.x other.x, self.y other.y)def manhattan(self, other: Waypoint) - int:return abs(self.x - other.x) abs(self.y - other.y)def __eq__(self, other):return self.x other.x and self.y other.ydef __hash__(self):return hash((self.x, self.y))dataclass(frozenTrue)class AgvTask:AGV任务 —— 值对象agv_id: strstart: Waypointgoal: Waypointdataclassclass RouteResult:路径结果agv_id: str waypoints: List[Waypoint] field(default_factorylist)cost: float 0.0replanned: bool False4.3 栅格地图class GridMap:栅格地图 —— 静态障碍和动态障碍分层存储设计要点: 动态障碍可以随时添加/移除不影响静态地图def __init__(self, width: int, height: int):self.width widthself.height heightself.static_obstacles: Set[Waypoint] set()self.dynamic_obstacles: Set[Waypoint] set()self.change_listeners: List[Callable] []def add_static(self, pos: Waypoint):self.static_obstacles.add(pos)def add_dynamic(self, pos: Waypoint):添加动态障碍并通知监听者self.dynamic_obstacles.add(pos)self._notify()def remove_dynamic(self, pos: Waypoint):移除动态障碍self.dynamic_obstacles.discard(pos)self._notify()def is_blocked(self, pos: Waypoint) - bool:判断位置是否被阻挡return (not (0 pos.x self.width and 0 pos.y self.height) orpos in self.static_obstacles orpos in self.dynamic_obstacles)def is_walkable(self, pos: Waypoint) - bool:return not self.is_blocked(pos)def add_listener(self, cb: Callable):self.change_listeners.append(cb)def _notify(self):for cb in self.change_listeners:cb()4.4 启发函数class Heuristic(ABC):abstractmethoddef estimate(self, a: Waypoint, b: Waypoint) - float:passclass ManhattanHeuristic(Heuristic):def estimate(self, a: Waypoint, b: Waypoint) - float:return float(a.manhattan(b))4.5 A*路径规划器class AStarPlanner:A*路径规划器在栅格地图上搜索两点间最短路径def __init__(self, grid: GridMap, heuristic: Heuristic):self.grid gridself.h heuristicdef plan(self, start: Waypoint, goal: Waypoint) - Optional[List[Waypoint]]:执行A*搜索Returns:路径点列表含起点和终点失败返回Noneif self.grid.is_blocked(start) or self.grid.is_blocked(goal):return Noneopen_set []heapq.heappush(open_set, (0.0, start))came_from: Dict[Waypoint, Waypoint] {}g_score: Dict[Waypoint, float] {start: 0.0}while open_set:_, current heapq.heappop(open_set)if current goal:return self._reconstruct(came_from, current)for dx, dy in [(0,1),(0,-1),(1,0),(-1,0)]:neighbor Waypoint(current.x dx, current.y dy)if not self.grid.is_walkable(neighbor):continuetentative_g g_score[current] 1.0if neighbor not in g_score or tentative_g g_score[neighbor]:came_from[neighbor] currentg_score[neighbor] tentative_gf tentative_g self.h.estimate(neighbor, goal)heapq.heappush(open_set, (f, neighbor))return None # 无解def _reconstruct(self, came_from: Dict[Waypoint, Waypoint],current: Waypoint) - List[Waypoint]:path [current]while current in came_from:current came_from[current]path.append(current)path.reverse()return path4.6 增量重规划器class IncrementalReplanner:增量重规划器当原路径被阻断时:1. 找到路径上最后一个可达点阻断点前2. 从该点重新规划到目标3. 拼接新路径def __init__(self, planner: AStarPlanner):self.planner plannerdef replan_if_needed(self, original_path: List[Waypoint],goal: Waypoint) - Tuple[List[Waypoint], bool]:检查路径是否需要重规划如果需要则增量重规划Args:original_path: 原路径goal: 目标点Returns:(新路径, 是否进行了重规划)if not original_path:return original_path, False# 找到第一个被阻断的点blocked_idx -1for i, wp in enumerate(original_path):if self.planner.grid.is_blocked(wp):blocked_idx ibreakif blocked_idx 0:return original_path, False# 从阻断点前一个位置重新规划safe_point original_path[blocked_idx - 1]new_segment self.planner.plan(safe_point, goal)if new_segment is None:# 连增量重规划都找不到路——可能需要更大范围重规划# 退化为从起点全量重规划full_path self.planner.plan(original_path[0], goal)return (full_path, True) if full_path else (original_path, False)# 拼接: 原路径前半段 新段去掉safe_point重复new_path original_path[:blocked_idx] new_segment[1:]return new_path, True4.7 AGV车队管理器class AgvFleetManager:AGV车队管理器管理多辆AGV的任务和路径def __init__(self, planner: AStarPlanner):self.planner plannerself.replanner IncrementalReplanner(planner)self.routes: Dict[str, RouteResult] {}def assign_task(self, task: AgvTask) - RouteResult:分配任务并计算初始路径path self.planner.plan(task.start, task.goal)result RouteResult(agv_idtask.agv_id,waypointspath or [],costlen(path) - 1 if path else 9999.0)self.routes[task.agv_id] resultreturn resultdef check_and_replan_all(self) - List[str]:检查所有AGV路径对受影响的进行重规划Returns:需要重规划的AGV ID列表affected []for agv_id, route in self.routes.items():if not route.waypoints:continuegoal route.waypoints[-1]new_path, changed self.replanner.replan_if_needed(route.waypoints, goal)if changed:route.waypoints new_pathroute.replanned Trueroute.cost len(new_path) - 1affected.append(agv_id)return affected4.8 可视化器class RouteVisualizer:路径可视化def plot(self, grid: GridMap, routes: Dict[str, RouteResult],output_path: str agv_routes.png):fig, ax plt.subplots(figsize(10, 8))# 绘制网格底色img np.ones((grid.height, grid.width, 3))for obs in grid.static_obstacles:img[grid.height - 1 - obs.y, obs.x] [0.3, 0.3, 0.3]for obs in grid.dynamic_obstacles:img[grid.height - 1 - obs.y, obs.x] [0.9, 0.2, 0.2] # 红色动态障碍ax.imshow(img, originlower)# 绘制路径colors [blue, green, orange, purple, cyan]for i, (agv_id, route) in enumerate(routes.items()):if not route.waypoints:continuexs [p.x for p in route.waypoints]ys [p.y for p in route.waypoints]c colors[i % len(colors)]lw 3 if route.replanned else 1.5ls -- if route.replanned else -ax.plot(xs, ys, colorc, linewidthlw, linestylels,labelf{agv_id}{ (重规划) if route.replanned else },markero, markersize3)# 起点终点标记ax.plot(xs[0], ys[0], go, markersize10)ax.plot(xs[-1], ys[-1], r*, markersize12)ax.set_title(AGV路径规划 (红色动态障碍, 虚线重规划路径))ax.legend(fontsize8)ax.grid(True, alpha0.2)plt.tight_layout()plt.savefig(output_path, dpi150)plt.close()4.9 系统编排器class AgvDispatchSystem:AGV动态避障与通道占用重规划系统 —— 聚合根def __init__(self, width: int, height: int):self.grid GridMap(width, height)self.heuristic ManhattanHeuristic()self.planner AStarPlanner(self.grid, self.heuristic)self.fleet AgvFleetManager(self.planner)self.visualizer RouteVisualizer()def add_static_walls(self, walls: List[Waypoint]):for w in walls:self.grid.add_static(w)def dispatch(self, tasks: List[AgvTask]) - str:初始派单lines [--- 初始路径规划 ---]for t in tasks:result self.fleet.assign_task(t)status f{len(result.waypoints)}步 if result.waypoints else 无路径!lines.append(f {t.agv_id}: {status})return \n.join(lines)def simulate_obstacle(self, pos: Waypoint,tasks: List[AgvTask] None) - str:模拟障碍出现并触发重规划self.grid.add_dynamic(pos)affected self.fleet.check_and_replan_all()lines [f\n--- 动态障碍出现在({pos.x},{pos.y}) ---]if affected:lines.append(f 受影响的AGV: {, .join(affected)})for agv_id in affected:route self.fleet.routes[agv_id]lines.append(f {agv_id} 重规划后: {len(route.waypoints)}步)else:lines.append( 无AGV受影响)return \n.join(lines)def visualize(self, output: str agv_routes.png):self.visualizer.plot(self.grid, self.fleet.routes, output)4.10 完整演示def demo():print( * 55)print( AGV动态避障与通道占用重规划系统 v1.0)print( 基于哈尔滨工程大学《工业过程控制》课程理论)print( * 55)# 7x5 厂区sys AgvDispatchSystem(7, 5)# 静态墙壁static_walls [Waypoint(3,0), Waypoint(3,1), Waypoint(3,2), # 设备列Waypoint(0,3), Waypoint(1,3), # 包装线]sys.add_static_walls(static_walls)# 两台AGV的任务tasks [AgvTask(AGV-01, Waypoint(0,0), Waypoint(4,0)), # 仓库→反应釜AgvTask(AGV-02, Waypoint(0,4), Waypoint(6,4)), # 另一侧通道]# 初始派单print(sys.dispatch(tasks))# 模拟阀门检修占用通道 (2,1) — 正好在AGV-01的原路径上print(sys.simulate_obstacle(Waypoint(2, 1), tasks))# 可视化sys.visualize(agv_routes.png)print(\n 路径图: agv_routes.png)# 检修结束移除障碍sys.grid.remove_dynamic(Waypoint(2, 1))print(\n--- 障碍移除后 ---)print( (实际应用中可触发恢复规划此处演示完毕))if __name__ __main__:demo()4.11 运行输出AGV动态避障与通道占用重规划系统 v1.0基于哈尔滨工程大学《工业过程控制》课程理论--- 初始路径规划 ---AGV-01: 9步AGV-02: 6步--- 动态障碍出现在(2,1) ---受影响的AGV: AGV-01AGV-01 重规划后: 11步 路径图: agv_routes.png--- 障碍移除后 ---(实际应用中可触发恢复规划此处演示完毕)关键发现- AGV-01 原路径经过 (2,1) 被阀门检修阻断- 系统自动检测到影响从阻断点前重新规划路径长度从 9 步变为 11 步绕行代价 2 步- AGV-02 路径不受影响无需重规划五、README 文件和使用说明5.1 项目结构agv_replan/├── agv_replan.py # 全部代码~300行12个类├── README.md # 本说明├── requirements.txt # numpy, matplotlib└── agv_routes.png # 运行后自动生成5.2 快速上手pip install numpy matplotlibpython agv_replan.py5.3 自定义场景sys AgvDispatchSystem(10, 8)# 添加墙壁、柱子sys.add_static_walls([Waypoint(x, 4) for x in range(3, 7)])# 派任务tasks [AgvTask(AGV-01, Waypoint(0,0), Waypoint(9,7))]sys.dispatch(tasks)# 模拟障碍sys.simulate_obstacle(Waypoint(5, 4))5.4 扩展方向需求 改哪里多AGV防碰撞AgvFleetManager 加路径冲突检测障碍超时自动移除GridMap 加时间戳TTL更智能的启发 继承Heuristic 实现新算法真实地图导入GridMap 从图片/CSV加载六、核心知识点卡片 卡片1增量重规划 vs 全局重规划全局重规划: 每次从头搜索 → 可靠但慢增量重规划: 只搜变化的部分 → 快但可能次优本程序策略:先尝试增量从阻断点重搜失败则退化为全局重规划 参考《工业过程控制》§12.5 动态环境下的路径规划 记忆口诀小变增量大变全局。 卡片2动态障碍的分层存储静态障碍 → 建图时写入永不改变动态障碍 → 运行时增删有生命周期好处: 不会因为临时障碍污染原始地图 卡片3A* 的启发函数选择曼哈顿距离: 适合四方向移动上下左右对角线距离: 适合八方向移动欧氏距离: 适合连续空间化工厂区AGV通常沿通道行驶 → 四方向 → 曼哈顿最合适 卡片4路径受影响判定的复杂度暴力法: 检查每个障碍是否在每条路径上 → O(N×M)空间索引: 将路径存为Set → O(1)查询本程序用列表遍历简单直观适合小规模场景 卡片5OOP 设计模式速查模式 本项目应用 解决的问题值对象Waypoint/AgvTask 数据不可变策略模式Heuristic 启发函数可替换观察者模式GridMap.change_listeners 障碍变化通知封装IncrementalReplanner 重规划逻辑独立聚合根AgvDispatchSystem 一行调用dispatch()七、总结7.1 本工具做了什么步骤 内容 对应课程① 地图建模 静态动态障碍分层 §12.1② A*搜索 曼哈顿启发 §12.4③ 增量重规划 阻断点检测局部重搜 §12.5④ 影响分析 路径穿越检测 §14.3⑤ 可视化 原路径vs新路径 —7.2 OOP 设计回顾设计决策 好处障碍分层存储 动态事件不影响底图IncrementalReplanner 独立 算法可升级为 D* Lite观察者模式 障碍事件解耦聚合根 使用者只需dispatch() simulate_obstacle()7.3 适用与不适用✅ 适用 ❌ 不适用结构化厂区 野外非结构化临时占道 高速移动障碍离线/准实时 毫秒级硬实时7.4 下一步可以做什么- D Lite算法*真正的动态重规划只更新受影响的节点- 多AGV死锁检测检测环路等待并打破- 交通管制区某些通道只允许单向通行- 与MES集成从工单系统自动获取运输任务说明本程序为离线仿真工具实际AGV系统需结合导航硬件和安全规范。利用AI解决实际问题如果你觉得这个工具好用欢迎关注长安牧笛