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

权利要求书

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。