基于动态窗口算法的AGV仿真避障 可设置起点目标点，设置地图，设置移动障碍物起始点目标点，未知静态障碍物 动态窗口方法(DynamicWindowApproach) 是一种可以实现实时避障的局部规划算法，通过将轮式机器人的位置约束转化为速度约束，根据约束进行速度采样，并由一系列的选定速度的动作生成轨迹，结合评价函数选择评分最高的轨迹，实现执行最优速度的问题。 黄色是静态障碍物，红色是可移动障碍物

一、系统整体定位与核心目标

本系统围绕多自动导引车（AGV）在协同作业场景下的实时路径规划与安全避障需求构建，以“优先级调度+动态窗口算法（DWA）”为核心技术框架，旨在解决多AGV同时运行时的路径冲突问题，确保各AGV在复杂动态环境中既能高效执行既定任务，又能通过精准的避障逻辑保障运行安全。系统设计聚焦“功能实用性”与“场景适配性”，所有模块均围绕“实时性”与“安全性”两大核心目标展开，可直接应用于需要多AGV协同作业的仓储、制造、物流等自动化场景。

二、核心功能模块详解

（一）AGV优先级调度模块：构建多设备协同基础规则

优先级调度是多AGV协同运行的核心前提，该模块通过预设明确的优先级规则，为不同AGV分配差异化的通行权限，从源头规避多设备并行时的路径冲突风险，具体功能如下：

1. 优先级规则固化与执行：系统明确设定AGV优先级层级为“AGV1 > AGV2 > AGV3”，该规则贯穿多AGV运行全过程，所有涉及设备相遇、路径交叉的场景均以该优先级作为通行权判定依据。在代码逻辑中，优先级参数将作为核心判断条件嵌入AGV状态监测与决策流程，当多AGV进入同一空间范围时，系统可自动读取各AGV的标识信息，匹配对应的优先级等级，快速完成通行权归属判定。
2. 优先级与避障逻辑的联动：优先级规则并非独立存在，而是与后续的避障处理模块深度绑定——高优先级AGV拥有“优先通过权”，低优先级AGV则承担“主动避让义务”。这种联动设计确保优先级规则不只是静态的等级划分，而是能转化为动态的设备行为指导，例如当AGV1与AGV2同时接近同一交叉区域时，系统可基于优先级判定结果，直接向AGV2发送“暂停指令”，向AGV1发送“继续行驶指令”，无需额外复杂的路径重新规划，大幅提升多设备协同效率。
3. 优先级状态实时监测与更新：模块具备实时监测各AGV优先级状态的功能，可通过AGV的实时通信数据获取设备当前的运行状态（如是否正常行驶、是否处于故障模式等），并在特殊场景下灵活调整优先级执行逻辑（如高优先级AGV出现故障暂停时，可临时提升低优先级AGV的通行权限，避免整体作业停滞）。不过基于“说明.doc”文档内容，当前系统暂以固定优先级为主，后续可基于该模块框架扩展动态优先级调整功能。

（二）动态窗口算法（DWA）避障模块：实现精准实时避障

动态窗口算法是系统实现AGV个体避障的核心技术支撑，该模块通过“轨迹评价+最优选择”的逻辑，让AGV在行驶过程中能实时响应环境变化，自主选择安全且高效的行驶轨迹，具体功能拆解如下：

1. 动态窗口构建：划定可行运动范围：算法首先基于AGV的物理性能参数（如最大速度、最大加速度、最大转向角速度等）与当前运行状态（如当前速度、当前位置），构建“动态窗口”——即AGV在当前时刻可实现的速度（线速度）与角速度（转向速度）的取值范围。该窗口的核心作用是“缩小轨迹搜索空间”，避免算法在无效的运动参数组合中浪费计算资源，确保避障决策的实时性。例如，当AGV当前速度为1m/s、最大加速度为0.5m/s²时，动态窗口会将下一时刻的线速度范围限定在0.5-1.5m/s之间（假设无反向减速需求），确保轨迹规划符合AGV的物理运动能力。
2. 候选轨迹生成：覆盖所有可行路径：在动态窗口确定的速度与角速度范围内，算法会通过离散化处理，生成多条不同参数组合对应的候选轨迹。每条候选轨迹均基于AGV当前位置、预设的时间步长（如未来1秒内的运动轨迹）与运动学模型计算得出，可精准描述AGV在该参数组合下的行驶路径（包括位置坐标、方向角变化等）。例如，在角速度取值范围内，算法会每隔1°取一个离散值，结合不同的线速度值，生成数十条甚至上百条候选轨迹，确保覆盖所有可能的安全行驶方向。
3. 轨迹评价函数：筛选最优行驶路径：该模块的核心功能在于通过“多维度评价函数”对所有候选轨迹进行量化评分，最终选择评分最高的轨迹作为AGV的实际行驶路径。评价函数的设计直接决定避障效果，基于“说明.doc”中“依据动态窗口评价函数选择最优轨迹移动”的描述，结合行业通用DWA算法逻辑，评价函数通常包含三大核心维度：
   - 目标方向适配性：评估候选轨迹终点与AGV既定目标点的方向偏差，偏差越小则评分越高，确保AGV行驶方向始终贴合任务目标，避免因避障导致任务偏离；
   - 障碍物距离安全性：计算候选轨迹上各点与周围障碍物（包括静态障碍物、其他AGV）的最小距离，距离越大则评分越高，保障AGV行驶过程中与障碍物保持安全距离；
   - 运动效率性：评估候选轨迹对应的线速度与角速度是否接近AGV的最优运行参数（如最大作业速度），速度越优则评分越高，确保避障过程中尽可能减少对作业效率的影响。
4. 轨迹执行与动态调整：最优轨迹确定后，模块会将对应的速度、角速度参数发送至AGV的运动控制单元，驱动AGV按照规划轨迹行驶。同时，由于环境可能实时变化（如其他AGV位置变动、临时出现新障碍物），算法会以固定的时间间隔（如每0.1秒）重复执行“动态窗口构建→候选轨迹生成→轨迹评价→最优轨迹选择”的流程，实现轨迹的实时动态调整，确保AGV在整个行驶过程中始终处于安全且高效的运动状态。

（三）多AGV相遇避障控制模块：解决设备间冲突

在多AGV同时运行场景中，仅依靠个体的DWA避障无法完全规避设备间的直接相遇冲突，因此系统专门设计“多AGV相遇避障控制模块”，通过“距离+角度”双条件触发避障动作，结合优先级规则实现精准的冲突化解，具体功能如下：

1. 相遇条件监测：精准识别冲突风险：模块会实时采集所有AGV的位置坐标、方向角数据，并基于这些数据计算任意两辆AGV之间的相对状态，当同时满足以下两个条件时，判定为“相遇冲突场景”，触发避障逻辑：
   - 距离条件：两辆AGV之间的直线距离d≤3m。系统通过坐标计算（如基于平面直角坐标系的两点间距离公式）获取实时距离，并与预设的3m安全阈值进行比对，该阈值的设定基于AGV的制动距离、体型尺寸等参数，确保留有足够的避障反应空间；
   - 角度条件：其中一辆AGV的方向角与另一辆AGV所在位置形成的夹角＜100°。方向角是AGV当前行驶方向与坐标系基准方向（如X轴正方向）的夹角，系统通过三角函数计算AGV“行驶方向”与“指向另一AGV的方向”之间的夹角，当该夹角小于100°时，说明两辆AGV的运动方向存在交叉或相向趋势，存在直接碰撞风险。
   上述两个条件需同时满足才会触发避障，避免单一条件（如仅距离近但方向无交叉）导致的误触发，确保避障逻辑的精准性。
2. 避障动作执行：基于优先级的差异化控制：当监测到相遇冲突场景后，模块会调用AGV优先级调度模块的规则，对冲突双方的优先级进行比对，并执行差异化的控制动作：
   - 高优先级AGV：保持原有行驶状态，继续按照DWA算法规划的最优轨迹行驶，无需进行减速或转向调整，确保高优先级任务的执行效率不受影响；
   - 低优先级AGV：立即执行“停止移动”动作，切断运动控制信号，同时保持对周围环境（尤其是高优先级AGV位置）的实时监测。待高优先级AGV行驶至安全距离外（即两车距离d＞3m，或角度条件不再满足）后，低优先级AGV重新启动DWA算法，规划后续行驶轨迹，恢复任务执行。
3. 冲突状态恢复与异常处理：模块具备“冲突解除判定”功能，会持续监测停止移动的低优先级AGV与高优先级AGV的相对状态，一旦满足“安全距离”或“角度脱离冲突范围”的条件，立即向低优先级AGV发送“恢复行驶”指令。同时，为应对异常场景（如高优先级AGV停滞导致低优先级AGV长期等待），模块还内置超时检测逻辑——当低优先级AGV停止时间超过预设阈值（如30秒）时，自动触发报警信号，并向系统管理端发送异常提示，确保多AGV运行的整体稳定性。

三、系统功能运行流程

系统各模块并非独立工作，而是通过“实时数据交互+逻辑联动”形成完整的运行闭环，具体流程如下：

1. 初始化阶段：系统启动后，首先加载AGV优先级配置（AGV1 > AGV2 > AGV3），并初始化各AGV的运动参数（速度、角速度范围）、DWA算法评价函数权重、相遇避障的距离与角度阈值（d=3m、夹角=100°）。同时，建立AGV状态数据采集通道，确保能实时获取各AGV的位置坐标、方向角、当前速度等核心数据。
2. 实时监测与状态判断：系统以固定时间间隔（如每0.05秒）循环采集所有AGV的实时数据，一方面将数据输入DWA算法模块，为各AGV生成个体避障所需的候选轨迹与最优路径；另一方面，对任意两辆AGV的相对距离与角度进行计算，判断是否满足“相遇冲突条件”。
3. 多场景分支处理：
   - 无相遇冲突场景：各AGV独立执行DWA算法规划的最优轨迹，系统仅需持续监测AGV状态与环境变化，确保无新的冲突或障碍物出现；
   - 存在相遇冲突场景：触发“多AGV相遇避障控制模块”，基于优先级判定结果，控制低优先级AGV停止移动，高优先级AGV继续行驶。待冲突解除后，低优先级AGV重新启动DWA算法，恢复轨迹行驶。
4. 任务终止与状态重置：当某一AGV完成既定任务（如到达目标位置）后，系统会将该AGV的状态标记为“空闲”，并停止对其的避障冲突监测；若所有AGV均完成任务，系统则重置所有模块参数，等待下一轮任务指令输入，形成完整的功能运行周期。

四、功能设计的核心优势与场景适配性

1. 功能优势：平衡“安全”与“效率”：系统通过“优先级调度”确保高重要性任务的高效执行，通过“DWA算法”保障AGV个体的实时避障安全，通过“距离+角度双条件避障”精准化解多AGV冲突，三者结合实现“安全无碰撞”与“任务高效率”的平衡，避免因过度避障导致的效率损耗，或因追求效率忽视安全的风险。
2. 场景适配性：聚焦多AGV协同核心需求：该系统的功能设计完全针对多AGV同时作业的核心痛点——路径冲突与避障安全，所有规则（如3m距离阈值、100°角度阈值、固定优先级）均可基于实际应用场景（如AGV体型大小、行驶速度、作业空间尺寸）进行参数调整，无需大规模修改代码逻辑，可快速适配仓储分拣、车间物料搬运、智能码头装卸等不同场景的多AGV协同需求。

基于动态窗口算法的AGV仿真避障 可设置起点目标点，设置地图，设置移动障碍物起始点目标点，未知静态障碍物 动态窗口方法(DynamicWindowApproach) 是一种可以实现实时避障的局部规划算法，通过将轮式机器人的位置约束转化为速度约束，根据约束进行速度采样，并由一系列的选定速度的动作生成轨迹，结合评价函数选择评分最高的轨迹，实现执行最优速度的问题。 黄色是静态障碍物，红色是可移动障碍物