[发明专利]一种通行方向限制下的多机器人协同路径规划方法

权利要求书

1.一种用于通行方向限制下的多机器人协同路径规划方法，其特征是：首先，以路径长度与路径平滑度作为路径评价标准，使用A*算法进行单机器人路径规划；其次，建立可以动态更新的通道状态表，根据通道状态表与机器人综合优先级的判定方法，完成动态碰撞的避免；最后，判断是否存在由于通行方向限制导致的、有优化可能的局部环路现象，存在则判断局部环路优化的条件是否成立，生成优化后的路径并更新通道状态表；该方法的具体步骤如下：

步骤1.单机器人路径规划，即使用A*算法，以路径长度与路径平滑度作为路径评价标准，进行单机器人路径规划；

以A*算法作为单机器人路径规划基本方法，在基于Manhattan距离的估价函数中添加转弯代价：

其中，(x_pre，y_pre)为路径规划过程中的当前节点坐标，(x_g，y_g)为目标节点坐标，V_n为每次转弯需要耗费的额外转弯代价；

在单行进方向单通道的栅格地图中，每两个相邻的路口栅格之间路径是唯一的，故将A*算法的搜索区域从全部栅格缩小至路口栅格；使用A*算法，生成路口序列后，再按照通行方向的限制，将非路口栅格插入路口序列中，得到从起点栅格到终点栅格的完整路径，完成单机器人路径规划；

步骤2.建立通道状态表，即根据全局地图信息，结合机器人上报的位置信息及占位信息，建立动态更新的通道状态表；

通道状态表是一个大小尺寸与栅格地图完全相同的二维表格，其元素与栅格地图的路口栅格一一对应；通道状态表中的每个元素需要记录应以下两个字段：

(1)栅格的当前状态

根据动态碰撞避免的需要，将栅格的当前状态分为三类，分别为空闲、被预约与被占用；其中，空闲栅格代表当前既没有被任何机器人占用，也没有被预约的栅格；

(2)预约或占用栅格的机器人的信息

对于被预约或者被占用的栅格，需要通过与机器人的通信，记录预约或占用当前栅格的机器人的信息；信息包括机器人唯一的身份标识ID，机器人的剩余电量、是否载货、所装载货物的优先级、执行当前任务的累计等待时间；

步骤3.局部环路判断，即对于每一个路径规划层生成的路径，根据局部环路判断条件，判断是否存在由于通行方向限制导致的、有优化可能的局部环路现象；

机器人按照单机器人路径规划阶段规划好的路径行进时，进入栅格之前需要查看预约表中栅格的状态，若为空闲，则将栅格状态置为被预约，可以行进；若为被预约，则比较自身的优先级与已经预约该栅格的机器人的优先级，自身优先级较高则可以对栅格进行抢占，否则原地等待，直到解除占用；若为被占用则原地等待，直到栅格空闲；

综合优先级P_S按以下规则判定：

首先，若剩余电量E_pre小于电量安全阈值E_safe，则将电量优先级p₁设为最高V_max，剩余电量在安全范围内的机器人，将电量优先级p₁设为0；

其次，根据货物装载情况判定优先级，载货机器人优先级V_α高于未载货的机器人的优先级V_β；

最后，载货状况相同，则由货物的优先级p_g与当前任务的累计等待时间t_w决定，p_gmax为货物最大优先级，t_wmax为执行任务的最大累计等待时间；

V_max＞V_α＞V_β＞2 (5)

P_S＝w₁p₁+w₂p₂+w₃p₃ (6)

w₁+w₂+w₃＝1 (7)

w₁、w₂与w₃分别为优先级的权重系数，且三个权重系数取相同值；

步骤4.局部环路优化可行性分析，即对在步骤3中判断出的局部环路现象，结合通道状态表中相关栅格的当前状态与预约或占用栅格的机器人的综合优先级，判断局部环路优化的条件是否成立；

在起点处与终点路口处分别对局部环路进行判断：在起点处，根据已经使用A*算法规划好的原始路径进行判断，原始路径同时满足式(10)的三个条件，则判定为在起点处存在局部环路；

N₂＝N₁+(L_y+1)+2(L_x+1) (9)

(N₂＜L_R)∧(y_s＝R[N₂].y)∧(y_s≠y_g) (10)

式(8)至式(10)中，N₁为按通行方向限制，从起点至第一个路口处需要移动的栅格数，(x_s，y_s)为起点坐标，(x_s+1，y_s+1)为原始路径上第二个点的坐标，(x_g，y_g)为终点坐标，L_x为货架组X轴方向的尺寸，L_y为货架组Y轴方向的尺寸；N₂为按通行方向限制，从起点至反方向第一个路口处需要移动的栅格数，L_R为原始路径的路径长度，R[N₂].y为原始路径上第N₂个栅格的纵坐标；

终点处的局部环路，在机器人行进到终点路口时，由控制中心根据当前坐标与终点坐标的关系进行判断，满足式(11)的三个条件，则判定为在终点处存在局部环路；

(y_pre＝y_g)∧[|x_pre-x_g|＜(L_y+1)]∧[(L_R-N₃)＞(L_y+1)] (11)

其中：(x_pre，y_pre)为机器人的当前位置坐标，即终点路口的坐标，机器人已经运动到原始路径的第N₃个栅格；

步骤5.路径生成与执行：在取消起点区域与终点区域的通行方向限制的情况下，分别用起点至起点路口、终点路口至终点的最短路径，替换对应区间的局部环路路径；然后由控制中心访问通道状态表，查询在拟取消通行方向限制的区域内，是否有栅格处于被占用或被预约状态，如果均为空闲状态或机器人综合优先级高于预约栅格的机器人的优先级，则将通道状态表中需要解除限制的栅格的状态置为被占用，机器人通过该路段后更新通道状态表，将解除通行方向限制的栅格的状态置为空闲，并恢复通行方向限制，完成一次协同路径优化。