CN113311829B - 一种基于动态时间窗冲突搜索的多机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种基于动态时间窗冲突搜索的多机器人路径规划方法，属于人工智能与机器人控制技术领域。本发明能够在任务即时进入多机器人系统时，通过动态时间窗和动态重新规划频率的方法为各机器人规划任务执行路径，同时当机器人数量巨大时，仍能够为多机器人系统规划出无碰撞的路径。动态窗口和规划频率的引入，大大提高了求解效率，并解决了任务即时进入系统的问题。本发明具备良好的鲁棒性和实际应用价值，实现了快速为巨大数量的多机器人系统规划无碰撞路径。本发明均具备很强的扩展性，适用于各种移动机器人的动态任务路径规划场景。

Description

一种基于动态时间窗冲突搜索的多机器人路径规划方法

技术领域

本发明涉及一种机器人路径规划方法，具体涉及一种基于动态时间窗冲突搜索的多机器人路径规划方法，属于人工智能与机器人控制技术领域。

背景技术

双目标任务位置的多自主机器人系统，是指系统内每个机器人所执行的每个任务均有两个目标位置，即：提货地点和交货地点。机器人需要先移动到提货地点，然后再移动到交付地点，如港口自动运输机器人、物流分拣中心机器人、智能仓储机器人等。此类机器人系统的特点是：机器人以及承担得任务数量规模巨大，任务目标会即时进入系统，因此，其任务信息并非全部先验明确。

针对上述机器人系统路径控制问题，目前应用较为广泛的方法是采用终生 MAPF问题(Lifelong Multi-Agent Path Finding)模型，具体如下：

将机器人定义为Agent，Agent即表示机器人，各机器人具有自主性，不依赖人力遥控控制。

(1)Agent行动空间：给定一个无向图空间G＝(V,E),V代表图中的顶点集，表示Agent可能停留的位置；E是连接图中各定点的边集，表示Agent可能经过的位置。

(2)Agent：在该空间内，分布一定数量的Agent。假设有m个Agent，则用集合A＝{a₁,a₂,…,a_m}表示。

(3)任务目标：对于每个Agent a_i∈A，都需要从一个起始点s_i∈V和任务目标点g_i∈V，并且每个Agent都需要执行从起始点到一系列任务目标任务τ_i＝ {s_i,g_i}。

(4)时间序列：时间被离散化为时间点后，使用T＝{0,1,…,t}表示时间序列。在时间T＝0时，所有Agent已经在起始位置s_i。

(5)Agent行为：对于每个Agent a_i∈A，a_i的路径p_i表示T→V从时间序列 T到图中顶点集合V的一个映射。对于

p_i(t)表示t时刻Agent a_i的位置，且任意时刻Agent可以选择停留在当前位置或者移动到相邻位置。

(6)路径代价：对于每个Agent a_i∈A，cost(p_i)示为路径p_i的代价，且路径代价用距离来计算。所有Agent总路径代价d为：

其中，dist(x,y)表示位置x和y的距离，j为任务序号。

(7)顶点冲突：顶点冲突指在同一时刻，有两个Agent在同一位置，即代理a_i和a_j在t时刻占据相同的顶点v那么把(a_i,a_j,v,t)称为顶点冲突，如图1所示。因此，对于每个Agenta_i∈A,a_j∈A要求如下：

其中，p_i(t)表示t时刻Agent的位置。

(8)相向冲突：相向冲突指在同一时刻，任意两个Agent朝着对方方向运动。即代理a_i和a_j在t时刻遍历相同的边(u,v)，其中u表示p_i(t)＝p_j(t+1)，v表示 p_i(t+1)＝p_j(t)，如图2所示。因此，对于每个Agent a_i∈A,a_j∈A，要求如下：

李娇阳等人提出了“一种基于时间窗口滑动冲突搜索的路径规划方法 (Rolling-Horizon Collision Resolution，RHCR)”，用于解决终身MAPF问题。该方法改进了传统的基本的冲突搜索算法(Conflict-Based Search，CBS)，核心思想是通过在算法框架中提出两个人为可指定的参数，即时间范围w和重新规划频率h，时间范围w表示基于时间窗口滑动冲突搜索的方法必须解决w个时间步长范围内的冲突，重新规划频率h表示RHCR求解器必须每h个时间步长重新规划路径，为了避免发生冲突，窗口式终身MAPF求解器必须比每w时间步长更加频繁地重新规划路径，即要求w≥h。同时，RHCR的下层路径求解层泛化了多标签A*算法(Multi-label A*)以适应终生MAPF问题中为每个Agent分配一系列目标任务的场景。其中，Multi-label A*是Grenouilleau等人为了解决多Agent 的物流分拣场景的取件和交付问题，提出了多标签A星算法(Multi-Label A*)，该方法可以连续访问两个有序的目标位置，即系统指定的取货位置和交货位置。

整体而言，RHCR将双目标任务位置的多自主机器人系统路径规划问题分为两层进行求解，高层级为窗口式冲突搜索层，低层级为路径规划求解器。在低层级上，使用Multi-label A*为各个Agent规划路径，在高层级，对为各Agent规划的路径进行冲突搜索，并且将冲突搜索的范围限定在前w个时间步长范围内，搜索到冲突后，为各Agent根据其优先级来解决冲突，优先级的确定根据各自添加约束后的路径代价决定，然后再次调用下层路径规划求解器，根据各Agent优先级来为各Agent寻找一条最优路径，该路径可以避免其与较高优先级的Agent 发生冲突。然后每隔h个时间步长重新规划路径，并且重复上述过程。

RHCR主要有两方面的改进，一个是可以为每个Agent规划从起始位置到一系列目标任务位置的路径，另一个是在上层的冲突搜索阶段，仅需要关注搜索和解决前w个时间步长范围内的冲突。RHCR的框架确实给解决终身的MAPF的问题带来了不错的解决方案，它既保证了多Agent系统的吞吐量，同时最多可扩展到1000个Agent的系统中，可以适应目标任务被即时新增且不用先验的知道所有任务目标信息的场景，并且对Agent所处地图环境没有限制，大大推进了拓展了MAPF的在工业场景中的应用，例如终生MAPF可求解实际应用场景中物流仓库和分拣中心的问题。

但是，RHCR并未给出如何确定在多Agent系统中时间范围w重新规划频率 h的大小，目前时间范围w重新规划频率h大小由人为手动设置且恒定不变，这会带来诸多弊端。

第一，时间范围w的作用是将冲突搜索和解决的范围限制在前w时间步长内，即仅需要发现并解决时间步长w内的冲突，并且基本RHCR框架要求在求解时间上Runtime≤60s，即必须在一分钟内找到冲突解决方案，否则定义为本次求解失败。然而，实际上机器人在地图中的密度会影响冲突发生的概率(即Agent 密度)，当机器人较为密集时，发生冲突的概率变大，通常需要设置较小的时间窗w来搜索和解决冲突，否则当时间窗w较大时，此时需要解决的冲突较多，因而需要更多的计算时间来解决冲突，造成求解时间超过1分钟的概率变大，求解失败率将会提高。

第二，RHCR如为保证系统能够实现1000个以上Agent的路径求解，通常需要设置较小的时间窗口w。但这样的做法存在较大弊端，当w较小时，要求w≥ h，因此此时需要设置较小的h值，则重新规划频率变高，而事实上当机器人密度稀疏时，机器人发生冲突的概率变小，此时可以使用较大的时间窗口w去搜索和解决冲突，否则会耗费计算资源和运算时间。事实上，在实际的应用场景中，例如港口运输机器人机器人、物流分拣中心和智能仓储机器人系统等，系统内的 Agent数量会随着任务的密集度动态变化，任务量大时需要较多的Agent工作，此时需要设置较小的时间窗口w和重新规划频率h，任务量小时需要较少的Agent 工作，此时需要设置较大的时间窗口w和重新规划频率h，基本的RHCR方法中固定时间窗和频率一定会造成系统计算资源的浪费和计算效率的降低，从而影响系统整体的工作效率。

综上，若如基本的RHCH所设定的固定的时间窗口w和重新规划频率h，频繁重新规划会造成系统运算效率降低和计算资源浪费的问题，并且固定窗口大小对多机器人系统Agent数量动态变化的场景并不适用，同时参数设定依赖人为经验也会为造成风险。

发明内容

本发明的目的是为了克服现有技术存在的缺陷，针对双目标任务位置的多自主机器人系统路径规划的技术问题，为有效解决现有的RHCR方法因固定时间窗口和重新规划频率所带来的计算资源浪费、计算效率低、严重依赖人为经验设置参数，以及无法适配多机器人系统中机器人数量变化的场景等一系列技术问题，提出一种基于动态时间窗冲突搜索的多机器人路径规划方法(Dynamic Window Rolling-Horizon Collision Resolution，DWRHCR)。

本发明的目的是通过下述技术方案实现的。

一种基于动态时间窗冲突搜索的多机器人路径规划方法，包括以下步骤：

步骤1：计算本次路径规划的动态时间窗口Dw和重新规划频率Dh。

本发明通过计算多机器人系统中的Agent局部密度，来动态调整时间窗口 Dw和重新规划频率Dh的大小。

通过分析RHCR方法可知，合理的时间窗口大小与机器人Agent的密度有关，因此，动态时间窗口Dw由每次重新规划前通过计算Agent的局部密度来确定时间窗口Dw大小。

具体地，步骤1包括以下步骤：

步骤1.1：计算系统的Agent局部密度。

Agent局部密度通过遍历整个作业地图中L*L大小的网格中Agent的数量确定，即，使用该区域内的Agent的数量除以网格面积，具体如下：

Density(m)＝Agent_Num_m/L² (4)

其中，Density(m)表示Agent_num_m为以m为中心、其周围L*L网格区域内 Agent的数量，m∈V，V表示Agent所在空间地图的顶点集合。

步骤1.2：计算Agent局部密度的动态时间窗口Dw：

Dw＝γ+ε ×max {Density(m)} (5)

其中，γ和ε均为可调整参数。

步骤1.3：根据动态时间窗口Dw，计算重新规划频率Dh：

Dh＝ alpha*Dw (6)

其中，alpha表示动态时间窗和重新规划频率的倍数关系。当动态时间窗口Dd小于Dh时，表示机器人a_i可能在本次规划周期内已完成所有任务目标，从而在下次规划周期到来前出现机器人a_i的空闲状态。为避免出现该问题，要求至少在每 Dd时间步长内规划一次，因此需要Dw≥Dh，即alpha≤1。

步骤2：利用动态时间窗冲突搜索算法，进行上层冲突搜索。

具体地，包括以下步骤：

步骤2.1：输入各Agent起始位置s_i和目标任务序列g_i，并初始化起始优先级顺序＜₀＝φ和根节点路径Root.solution＝φ。其中，φ代表为空。

步骤2.2：调用UpdataPlan模块为每个Agent找到最优路径，并更新 Root.cost和OPEN表，Root.cost表示根节点的路径代价。

步骤2.3：当OPEN表非空时，取最低成本节点N进行扩展，搜索节点N中的路径是否存在冲突，当路径中不存在冲突，返回N.solution为最终的路径解决方案，并根据上一次计算的重新规划频率执行Dh个时间步长，否则，执行步骤 2.4。

步骤2.4：在N.solution中搜索动态时间窗口Dw范围内各Agent的顶点冲突和边缘冲突，若无冲突，则返回该路径为最终规划路径结果。若有冲突，则执行步骤2.5。

步骤2.5：对于每个发生冲突的机器人a_i生成其子节点N′并继承其父节点N 的路径解决方案N.solution和约束，并添加Agent优先级顺序{j＜i}到＜_N′中。其中，＜_N′表示节点＜_N′的Agent的优先级顺序，i＜j表示机器人a_i的优先级高于机器人a_j。

步骤2.6：调用UpdataPlan模块，为机器人a_i重新规划路径，并更新 N′.solution的路径代价N′.cost，然后将具有最小成本的优先级顺序子节点插入到OPEN表中。其中，UpdataPlan(N,a_i)是为机器人a_i寻找一条最优路径。该路径能够避免其与具有高优先级的Agent发生冲突。

步骤2.7：若上层冲突求解器超过1分钟未找到无冲突路径，则路径求解失败。

步骤2的具体实现过程如表1所示。

表1动态时间窗冲突搜索的多机器人路径规划算法(DWRHCR)上层求解过程

步骤3：利用动态时间窗冲突搜索算法，进行下层路径搜索。

本方法的下层路径求解器，与基本RHCR的求解器保持一致，使用泛化的Multi-label A*的基础结构。具体地，步骤3包括以下步骤：

步骤3.1：输入起始状态位置s_i和目标任务序列g_i。

步骤3.2：创建根节点并初始化Root.location←s_i，Root.time←0， Root.g←0。

步骤3.3：Root.label通过给每个节点N添加一个属性N.label来标记机器人 a_i从根节点到节点N已经完成的目标任务位置。

例如，当N.label＝3时，代表机器人a_i已经完成目标位置g_i[0]、g_i[1]和g_i[2] 的目标任务。

步骤3.4：当OPEN表非空时且OPEN表按照f值由小到大排列，选择f值最小的节点N进行扩展。

步骤3.5：若N已经是当前目标任务位置，则P.label递增加1并且回溯该节点已规划的路径并返回，否则，该节点N将生成子节点，执行步骤3.6。

步骤3.6：计算子节点的h值。

h值等于机器人a_i完成所有任务序列中g_i剩余目标任务所需的时间步长d：

其中，dist(x,y)表示位置x和y的距离。

当开始位置和目标位置不再更新，RHCR将调用时间窗口MAPF求解器搜索所有Agent的路径，这些路径将按其给定的目标位置序列，从Agent的起始位置移动到所有的目标任务位置，最后Agent沿已生成的路径移动h个时间步长，同时，从各Agent目标任务列表中删除已完成的任务序列。

步骤3的具体实现过程如表2所示。

表2动态时间窗冲突搜索的多机器人路径规划算法下层路径求解

有益效果

本发明对比现有技术，具有以下优点：

1.本发明能够在任务即时进入多机器人系统时，通过动态时间窗和动态重新规划频率的方法为各机器人规划任务执行路径，同时当机器人数量巨大时，仍能够为多机器人系统规划出无碰撞的路径。动态窗口和规划频率的引入，大大提高了求解效率，并解决了任务即时进入系统的问题。本发明具备良好的鲁棒性和实际应用价值，实现了快速为巨大数量的多机器人系统规划无碰撞路径。

2.本发明能够自适应动态计算机器人的局部密度，根据机器人局部密度调整动态时间窗和规划频率，从而泛化了当多机器人系统中机器人数量变化时的场景，通过计算地图中机器人的局部密度来确定时间窗口的大小，给出了更加合理的时间窗口和重新规划频率，避免了多机器人系统路径规划时固定窗引起的计算资源浪费和运算时间过程的问题，运算速度最快可提升70％以上，使得多机器人系统的计算效率得到较大的提升，具有很高的求解效率。

3.本发明具有很强的扩展性。除了可应用在大型的自动化仓库或物流分拣场景中，也适用于其他多机器人系统路径规划场景，只需要给出各机器人的任务序列，便可快速为各个机器人规划无碰撞的路径，无论是多机器人系统的使用场景还是机器人的数量，本发明均具备很强的扩展性，适用于各种移动机器人的动态任务路径规划场景。

附图说明

图1为顶点冲突示意图。

图2为相向冲突示意图。

图3为物流分拣中心地图示意图。

图4为本发明方法的流程图。

具体实施方式

下面结合附图和实施例对本发明做进一步详细说明。

实施例

本实施例结合物流分拣中心的实际场景中对本发明做详细说明。

如图3所示，为模拟物流分拣中心的地图，该地图是37*77大小并且具有 10％障碍物的四联通地图，地图周围上下第一行和最后一行的分别分布了25个提取物流包裹的站点，由深灰色网格表示。黑色网格代表分拣滑道。每个黑色网格四周的四个深灰色网格代表Agent可卸载物流包裹的站点。Agent需要将包裹从提取包裹站点运输到卸载包裹站点后，包裹通过滑道完成分拣。在实验验证中，在路径规划求解之前有一个任务分配器会随机均匀为各Agent随机生成卸载包裹站点位置后，为Agent选择最近的提取包裹站点位置，然后本方法路径求解器为每个Agent规划从提取包裹站点到卸载包裹站点的无碰撞路径。

一种基于动态时间窗冲突搜索的多机器人路径规划方法(DWRHCR)，如图4所示，包括以下步骤：

步骤1：计算本次路径规划的动态时间窗口Dw和重新规划频率Dh。

首先，将时间步长限定在前200个时间步长内，实验参数设定为局部面积 L＝5，γ＝10，ε＝10，alpha＝0.5，Agent数量m＝500。

通过针对基本RHCR算法的分析可知，合理的时间窗口大小跟Agent的密度有关，因此，动态时间窗口Dw由每次重新规划前通过计算Agent的局部密度来确定时间窗口大小。

步骤1.1：Agent的局部密度通过遍历整个作业地图中L*L大小的网格中 Agent的数量来决策，即使用该区域内的Agent的数量除以网格面积，机器人局部密度计算方式如下：

Density(m)＝Agent_Num_m/L² (8)

其中，Density(m)代表Agent_num_m为以m为中心，其周围L*L网格区域内中机器人的数量，m∈V，V代表机器人所在空间地图的顶点集合。

步骤1.2：根据机器人局部密度的动态时间窗口大小为Dw，其定义如下：

Dw＝γ+ε×max{Density(m)} (9)

其中，γ和ε均为可调整参数。

步骤1.3：基于机器人局部密度的动态重新规划频率大小为Dh，其定义如下：

Dh＝alpha*Dw (10)

其中，Dw为动态时间窗口大小。由于当动态时间窗口Dd小于Dh时则表示Agent a_i可能在本次规划周期内已完成所有任务目标，从而在下次规划周期到来前出现 Agent a_i的空闲的状态，为了避免该问题，要求至少在每Dd时间步长内规划一次，因此要求Dw≥Dh，即alpha≤1。

步骤1计算的路径规划的动态时间窗口Dw和重新规划频率Dh如表3所示：

表3 Dw和Dh大小

步骤2：利用动态时间窗冲突搜索算法，进行上层冲突搜索。

步骤2.1：输入各Agent起始位置s_i和目标任务序列g_i，并且初始化起始优先级顺序＜₀＝φ和根节点路径Root.solution＝φ。

步骤2.2：调用UpdataPlan为每个Agent找到最优路径，并且更新Root.cost 和OPEN表。

步骤2.3：当OPEN非空时，取最低成本节点N进行扩展，搜索节点N中路径是否存在冲突，当路径中不存在冲突则返回N.solution为最终的路径解决方案，并且根据上次计算的重新规划频率执行Dh个时间步长。否则，执行步骤2.4。

步骤2.4：在N.solution中搜索动态时间窗口Dw范围内各Agent的顶点冲突和边缘冲突，若无冲突，返回该路径为最终规划路径结果。否则，执行步骤2.5。

步骤2.5：对于每个发生冲突的a_i生成其子节点N′并继承其父节点N的路径解决方案N.solution和约束，并且添加Agent优先级顺序{j＜i}到＜_N′中。＜_N′表示节点＜_N′的Agent的优先级顺序，i＜j表示a_i的优先级高于a_j。

步骤2.6：调用UpdataPlan为a_i重新规划路径，并更新N′.solution的路径代价N′.cost，然后将具有最小成本的优先级顺序子节点插入到OPEN表中。 UpdataPlan(N,a_i)是为a_i寻找一条最优路径，该路径能够避免其与高优先级的 Agent发生冲突。

步骤2.7：若上层冲突求解器超过1分钟未找到无冲突路径，则路径求解失败。

步骤2搜索冲突搜索数量和求解冲突时间为表4所示：

表4搜索冲突数量和求解时间结果

步骤3：基于动态时间窗冲突搜索算法，进行下层路径搜索。

包括以下步骤：

步骤3.1：输入起始状态位置s_i和目标任务序列g_i。

步骤3.2：创建根节点并初始化Root.location←s_i，Root.time←0， Root.g←0。

步骤3.3：Root.label通过给每个节点N添加一个属性N.label来标记Agent a_i从根节点到节点N已经完成的目标任务位置。

例如，当N.label＝3时，代表a_i已经完成目标位置g_i[0]、g_i[1]和g_i[2]的目标任务。

步骤3.4：当OPEN表非空且OPEN表按照f值由小到大排列，选择f值最小的节点N进行扩展。

步骤3.5：若N已经是当前目标任务位置，则P.label递增加1并且回溯该节点已规划的路径并返回；否则，该节点N将生成子节点，执行步骤3.6。

步骤3.6：计算子节点的h值。h值等于计算a_i完成所有任务序列中g_i剩余目标任务所需的时间步长d：

其中，dist(x,y)表示位置x和y的距离。一旦开始位置和目标位置不再更新，则RHCR将调用时间窗口MAPF求解器来搜索所有Agent的路径，这些路径将按其给定的目标位置序列从Agent的起始位置移动到所有的目标任务位置，最后 Agent沿已生成的路径移动h个时间步长，同时从各Agent目标任务列表中删除已完成的任务序列。

本实施例中，由于Agent数量达500个，以1号Agent的路径顺序为例，展示步骤3所求得路径顺序.路径结果为表5所示：

表5 1号Agent路径结果

为验证本发明所提方法对求解性能的提升，将其与基本的RHCR算法从运算时间、吞吐量和路径代价误差三个维度上进行了对比，验证了引入动态时间窗和规划频率后的效果。为了确保实验验证的有效性，分别观察本发明提出的 DWRHCR和RHCR将时间步长限定在前500个时间步长内的各项评价指标，每次实验独立运行20次后取平均值进行观察。本发明提出的DWRHCR算法的实验参数设定为局部面积L＝5，γ＝10，ε＝10，alpha＝0.5，Agent数量m＝ {100,200,…,1000}，基本的RHCR算法的实验参数设定为基本RHCR最优时的参数，解决冲突时间范围w＝4和重新规划频率h＝2。

求解时间结果如表6所示，在求解效率上，DWRHCR的大大提高了算法的运算效率，在不同数量规模的Agent系统中为多Agent规划路径的运算效率均优于基本的RHCR算法，求解时间缩短了28％～72％，这证明了动态时间窗口和重新规划频率提高了多Agent系统路径规划的求解效率，并且解决了人为设置固定时间窗口和重新规划频率严重依赖经验值的问题，以及当多Agent系统内 Agent数量变化时固定的时间窗和规划频率会出现不必要的计算资源浪费的问题。同时观察了DWRHCR对吞吐量的影响，实验结果如表7所示，发现DWRHCR 在不同Agent数量规模上将吞吐量提升了0.2％～0.8％，这说明了动态时间窗和规划频率的引入对多Agent系统的吞吐量会起到一定改善作用。DWRHCR对路径误差的影响如表8所示，当Agent数量m>800时，DWRHCR降低了路径代价误差，这说明优化后算法提升了路径的求解质量。当Agent数量m≤800时， DWRHCR略提高了路径代价误差，但波动范围小于0.1％。

表6 DWRHCR和RHCR求解时间对比实验结果

表7 DWRHCR和RHCR吞吐量对比实验结果

表8 DWRHCR和RHCR路径代价误差对比实验结果

综上所述，通过实验对比本发明方法与基本的RHCR算法在不同规模Agent 数量的路径求解时的性能，动态时间窗冲突搜索算法可以将路径求解效率提高 28％～72％，将系统的吞吐量提升0.2％～0.8％，同时几乎不影响路径代价误差，从而验证了本方法所提出的DWRHCR为多机器人路径规划所带来的各项性能的优化。

Claims (2)

1.一种基于动态时间窗冲突搜索的多机器人路径规划方法，其特征在于，包括以下步骤：

步骤1：计算本次路径规划的动态时间窗口Dw和重新规划频率Dh；

步骤1.1：计算系统的机器人Agent局部密度；

Agent局部密度通过遍历整个作业地图中L*L大小的网格中Agent的数量确定，即，使用该区域内的Agent的数量除以网格面积，具体如下：

Density(m)＝Agent_Num_m/L² (1)

其中，Density(m)表示Agent_num_m为以m为中心、其周围L*L网格区域内Agent的数量，m∈V，V表示Agent所在空间地图的顶点集合；

步骤1.2：计算Agent局部密度的动态时间窗口Dw：

Dw＝γ+ε×max{Density(m)} (2)

其中，γ和ε均为可调整参数；

步骤1.3：根据动态时间窗口Dw，计算重新规划频率Dh：

Dh＝alpha*Dw (3)

其中，alpha表示动态时间窗和重新规划频率的倍数关系；

步骤2：利用动态时间窗冲突搜索算法，进行上层冲突搜索；

步骤2.1：输入各Agent起始位置s_i和目标任务序列g_i，并初始化起始优先级顺序＜₀＝φ和根节点路径Root.solution＝φ；其中，φ代表为空；

步骤2.2：调用UpdataPlan模块为每个Agent找到最优路径，并更新Root.cost和OPEN表，Root.cost表示根节点的路径代价；

步骤2.3：当OPEN表非空时，取最低成本节点N进行扩展，搜索节点N中的路径是否存在冲突，当路径中不存在冲突，返回N.solution为最终的路径解决方案，并根据上一次计算的重新规划频率执行Dh个时间步长，否则，执行步骤2.4；

步骤2.4：在N.solution中搜索动态时间窗口Dw范围内各Agent的顶点冲突和边缘冲突，若无冲突，则返回该路径为最终规划路径结果；若有冲突，则执行步骤2.5；

步骤2.5：对于每个发生冲突的机器人a_i生成其子节点N′并继承其父节点N的路径解决方案N.solution和约束，并添加Agent优先级顺序{j＜i}到＜_N′中；其中，＜_N′表示节点＜_N′的Agent的优先级顺序，i＜j表示机器人a_i的优先级高于机器人a_j；

步骤2.6：调用UpdataPlan模块，为机器人a_i重新规划路径，并更新N′.solution的路径代价N′.cost，然后将具有最小成本的优先级顺序子节点插入到OPEN表中，其中，UpdataPlan(N,a_i)是为机器人a_i寻找一条最优路径；

步骤2.7：若上层冲突求解器超过1分钟未找到无冲突路径，则路径求解失败；

步骤3：利用动态时间窗冲突搜索算法，进行下层路径搜索；

步骤3.1：输入起始状态位置s_i和目标任务序列g_i；

步骤3.2：创建根节点并初始化Root.location←s_i，Root.time←0，Root.g←0；

步骤3.3：Root.label通过给每个节点N添加一个属性N.label来标记机器人a_i从根节点到节点N已经完成的目标任务位置；

步骤3.4：当OPEN表非空时且OPEN表按照f值由小到大排列，选择f值最小的节点N进行扩展；

步骤3.5：若N已经是当前目标任务位置，则P.label递增加1并且回溯该节点已规划的路径并返回，否则，该节点N将生成子节点，执行步骤3.6；

步骤3.6：计算子节点的h值；

h值等于机器人a_i完成所有任务序列中g_i剩余目标任务所需的时间步长d：

其中，dist(x，y)表示位置x和y的距离；

当开始位置和目标位置不再更新，将调用时间窗口MAPF求解器搜索所有Agent的路径，这些路径将按其给定的目标位置序列，从Agent的起始位置移动到所有的目标任务位置，最后Agent沿已生成的路径移动h个时间步长，同时，从各Agent目标任务列表中删除已完成的任务序列。

2.如权利要求1所述的一种基于动态时间窗冲突搜索的多机器人路径规划方法，其特征在于，步骤1.3中，alpha≤1。

Images