CN112362063B - 基于碰撞类型划分的多机器人路径规划方法和系统 - Google Patents

Abstract

本发明提供了一种基于碰撞类型划分的多机器人路径规划方法和系统，应用于多机器人控制系统；包括：获取多机器人控制系统中每个机器人的运动信息；基于每个机器人的运动信息，以每两个机器人为一组，判断两个机器人之间是否存在碰撞风险；如果是，则基于两个机器人的运动信息，确定碰撞风险所属的目标碰撞类型；基于目标碰撞类型，在两个机器人的运动平面上设置多个人工不可越节点；基于两个机器人的当前节点坐标、目标节点坐标和多个人工不可越节点，分别对两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。本发明缓解了现有技术中存在的有时会无法成功避免机器人间的碰撞和死锁的技术问题。

Description

基于碰撞类型划分的多机器人路径规划方法和系统

技术领域

本发明涉及机器人路径规划技术领域，尤其是涉及一种基于碰撞类型划分的多机器人路径规划方法和系统。

背景技术

基于网格环境的多机器人路径规划是多机器人系统导航中很常见的方法。许多研究人员已经将为单机器人系统设计的基于网格环境的路径规划方法扩展到多机器人系统。Kim等人提出了一种结合APF和FIS的机器人间避碰方法，每个机器人可以独立的选择其目标位置，并将其他机器人视为动态障碍物。但是该方法规划出的路径通常偏离最优路径较远。现有的文献中，研究人员提出了一种分布式算法，它通过反复停止和启动机器人来避免多个机器人之间的碰撞和死锁。但是停止和重新启动机器人需要复杂的控制系统，并且会增加完成任务所需的时间。文献还基于D*Lite提出了一种多机器人路径规划方法，它通过机器人间的交互并将其他机器人视为障碍物为原机器人寻找路径。然而，如果仅仅将其他机器人视为障碍物，有时会无法成功避免机器人间的碰撞，甚至还可能带来其它问题，如移动机器人之间的死锁。

发明内容

有鉴于此，本发明的目的在于提供一种基于碰撞类型划分的多机器人路径规划方法和系统，以缓解现有技术中存在的有时会无法成功避免机器人间的碰撞和死锁的技术问题。

第一方面，本发明实施例提供了一种基于碰撞类型划分的多机器人路径规划方法，应用于多机器人控制系统；包括：获取所述多机器人控制系统中每个机器人的运动信息；所述每个机器人的运动信息包括：当前节点坐标、当前运动路径和目标节点坐标；基于所述每个机器人的运动信息，以每两个机器人为一组，判断所述两个机器人之间是否存在碰撞风险；如果是，则基于所述两个机器人的运动信息，确定所述碰撞风险所属的目标碰撞类型；所述目标碰撞类型为多个预设碰撞类型中的一种碰撞类型；基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点；所述人工不可越节点为机器人在路径规划时视为障碍物的节点；基于所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

进一步地，所述预设碰撞类型包括：第一碰撞类型，表征所述两个机器人都沿着对角线方向运动，且碰撞节点为所述两个机器人的共同的下一个节点；第二碰撞类型，表征所述两个机器人的碰撞节点为第一机器人的当前节点；第三碰撞类型，表征所述两个机器人的碰撞节点为第二机器人的当前节点；第四碰撞类型，表征所述两个机器人的当前节点为相邻节点，且碰撞节点为所述第二机器人基于当前运动路径的下一个节点；第五碰撞类型，表征所述两个机器人的当前节点不相邻，且碰撞节点为所述第二机器人基于当前运动路径的下一个节点；其中，所述第一机器人为所述两个机器人当中优先级低的机器人，所述第二机器人为所述两个机器人当中优先级高的机器人。

进一步地，若所述目标碰撞类型为所述第一碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：将所述第二机器人基于当前运动路径的下一个节点设置为所述第一机器人的人工不可越节点；所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点。

进一步地，若所述目标碰撞类型为所述第二碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：将所述运动平面上与所述第一机器人的当前运动路径相关的多个节点设置为所述第二机器人的人工不可越节点；将所述运动平面上与所述第二机器人的当前运动路径相关的多个节点设置为所述第一机器人的人工不可越节点；所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点；所述第二机器人的人工不可越节点为所述第二机器人在路径规划时视为障碍物的节点。

进一步地，若所述目标碰撞类型为所述第三碰撞类型或者所述第四碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：将所述第一机器人的当前节点设置为所述第二机器人的人工不可越节点；基于所述第二机器人的当前节点坐标、目标节点坐标和所述第二机器人的人工不可越节点，对所述第二机器人的运动路径进行重新规划，得到第二机器人的目标运动路径；将所述运动平面上与所述第二机器人的目标运动路径相关的多个节点设置为所述第一机器人的人工不可越节点；所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点；所述第二机器人的人工不可越节点为所述第二机器人在路径规划时视为障碍物的节点。

进一步地，若所述目标碰撞类型为所述第五碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：将所述运动平面上与所述第二机器人的当前运动路径相关的多个节点设置为所述第一机器人的人工不可越节点；所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点。

进一步地，基于所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径，包括：以所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点作为初始变量，基于D*Lite路径规划算法，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

第二方面，本发明实施例还提供了一种基于碰撞类型划分的多机器人路径规划系统，应用于多机器人控制系统；包括：获取模块，判断模块，确定模块，设置模块和路径规划模块，其中，所述获取模块，用于获取所述多机器人控制系统中每个机器人的运动信息；所述每个机器人的运动信息包括：当前节点坐标、当前运动路径和目标节点坐标；所述判断模块，用于基于所述每个机器人的运动信息，以每两个机器人为一组，判断所述两个机器人之间是否存在碰撞风险；所述确定模块，用于如果是，则基于所述两个机器人的运动信息，确定所述碰撞风险所属的目标碰撞类型；所述目标碰撞类型为多个预设碰撞类型中的一种碰撞类型；所述设置模块，用于基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点；所述人工不可越节点为机器人在路径规划时视为障碍物的节点；所述路径规划模块，用于基于所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

第三方面，本发明实施例还提供了一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述第一方面所述的方法的步骤。

第四方面，本发明实施例还提供了一种具有处理器可执行的非易失的程序代码的计算机可读介质，所述程序代码使所述处理器执行上述第一方面所述方法。

本发明实施例提供了一种基于碰撞类型划分的多机器人路径规划方法和系统，通过对多机器人的碰撞类型进行划分，然后基于碰撞类型和多个机器人的运动信息，在运动平面上设置多个人工不可越节点，可以把为单机器人系统设计的路径规划算法应用到多机器人导航中，使得多机器人系统中的机器人因避碰而产生的绕行距离更短，效率更高，同时缓解了现有技术中存在的有时会无法成功避免机器人间的碰撞和死锁的技术问题。

附图说明

为了更清楚地说明本发明具体实施方式或现有技术中的技术方案，下面将对具体实施方式或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1为本发明实施例提供的一种基于碰撞类型划分的多机器人路径规划方法的流程图；

图2为本发明实施例提供的一种两机器人的第一类碰撞的碰撞场景示意图；

图3为本发明实施例提供的一种两机器人的第二类碰撞的碰撞场景示意图；

图4为本发明实施例提供的一种第二机器人方向朝北时的第三碰撞类型的碰撞场景示意图；

图5为本发明实施例提供的一种第二机器人方向朝东北时的第三碰撞类型的碰撞场景示意图；

图6为本发明实施例提供的一种第二机器人方向朝东时的第三碰撞类型的碰撞场景示意图；

图7为本发明实施例提供的一种第二机器人方向朝东南时的第三碰撞类型的碰撞场景示意图；

图8为本发明实施例提供的一种第二机器人方向朝南时的第三碰撞类型的碰撞场景示意图；

图9为本发明实施例提供的一种第二机器人方向朝西南时的第三碰撞类型的碰撞场景示意图；

图10为本发明实施例提供的一种第二机器人方向朝西时的第三碰撞类型的碰撞场景示意图；

图11为本发明实施例提供的一种第二机器人方向朝西北时的第三碰撞类型的碰撞场景示意图；

图12为本发明实施例提供的一种第二机器人方向朝北时的第四碰撞类型的碰撞场景示意图；

图13为本发明实施例提供的一种第二机器人方向朝东北时的第四碰撞类型的碰撞场景示意图；

图14为本发明实施例提供的一种第二机器人方向朝东时的第四碰撞类型的碰撞场景示意图；

图15为本发明实施例提供的一种第二机器人方向朝东南时的第四碰撞类型的碰撞场景示意图；

图16为本发明实施例提供的一种第二机器人方向朝南时的第四碰撞类型的碰撞场景示意图；

图17为本发明实施例提供的一种第二机器人方向朝西南时的第四碰撞类型的碰撞场景示意图；

图18为本发明实施例提供的一种第二机器人方向朝西时的第四碰撞类型的碰撞场景示意图；

图19为本发明实施例提供的一种第二机器人方向朝西北时的第四碰撞类型的碰撞场景示意图；

图20为本发明实施例提供的一种第二机器人方向朝北时的第五碰撞类型的碰撞场景示意图；

图21为本发明实施例提供的一种第二机器人方向朝东北时的第五碰撞类型的碰撞场景示意图；

图22为本发明实施例提供的一种第二机器人方向朝东时的第五碰撞类型的碰撞场景示意图；

图23为本发明实施例提供的一种第二机器人方向朝东南时的第五碰撞类型的碰撞场景示意图；

图24为本发明实施例提供的一种第二机器人方向朝南时的第五碰撞类型的碰撞场景示意图；

图25为本发明实施例提供的一种第二机器人方向朝西南时的第五碰撞类型的碰撞场景示意图；

图26为本发明实施例提供的一种第二机器人方向朝西时的第五碰撞类型的碰撞场景示意图；

图27为本发明实施例提供的一种第二机器人方向朝西北时的第五碰撞类型的碰撞场景示意图；

图28为本发明实施例提供的第一种人工不可越节点的八种情况示意图；

图29为本发明实施例提供的第二种人工不可越节点的八种情况示意图；

图30为本发明实施例提供的第三种人工不可越节点的八种情况示意图；

图31为本发明实施例提供的一种基于第一碰撞类型的人工不可越节点的设置示意图；

图32为本发明实施例提供的一种基于第二碰撞类型的人工不可越节点的设置示意图；

图33为本发明实施例提供的一种基于第三碰撞类型的人工不可越节点的设置示意图；

图34为本发明实施例提供的一种基于第四碰撞类型的人工不可越节点的设置示意图；

图35为本发明实施例提供的一种基于第四碰撞类型的人工不可越节点的设置示意图；

图36为本发明实施例提供的一种多机器人系统的示意图；

图37为本发明实施例提供的一种多机器人系统的人工不可越节点设置的示意图；

图38为本发明实施例提供的一种多机器人系统中任意一个机器人Ri路径规划的流程图；

图39为本发明实施例提供的一种基于碰撞类型划分的多机器人路径规划系统的示意图；

图40为本发明实施例提供的一种设置模块的示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例一：

图1是根据本发明实施例提供的一种基于碰撞类型划分的多机器人路径规划方法的流程图，该方法应用于多机器人控制系统。如图1所示，该方法具体包括如下步骤：

步骤S102，获取多机器人控制系统中每个机器人的运动信息；每个机器人的运动信息包括：当前节点坐标、当前运动路径和目标节点坐标。

在本发明实施例中，假设多个机器人从当前节点移动到下一个节点所花费的时间是相同的，而与运动方向无关。

步骤S104，基于每个机器人的运动信息，以每两个机器人为一组，判断两个机器人之间是否存在碰撞风险。

具体地，判断两个机器人在运动到下一个节点时，是否有节点冲突，如果是，则判断存在碰撞风险，如果否，则不存在碰撞风险。

优选地，本发明实施例使用网格法判断机器人之间的碰撞风险。在网格法中，机器人只能移动到其相邻的八个节点，所以机器人可能发生碰撞的节点(即冲突节点)一定是其原始节点或其相邻节点之一。

步骤S106，如果是，则基于两个机器人的运动信息，确定碰撞风险所属的目标碰撞类型；目标碰撞类型为多个预设碰撞类型中的一种碰撞类型。

步骤S108，基于目标碰撞类型，在两个机器人的运动平面上设置多个人工不可越节点；人工不可越节点为机器人在路径规划时视为障碍物的节点。

步骤S110，基于两个机器人的当前节点坐标、目标节点坐标和多个人工不可越节点，分别对两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

可选地，以两个机器人的当前节点坐标、目标节点坐标和多个人工不可越节点作为初始变量，基于D*Lite路径规划算法，分别对两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

其中，由Koenig等人提出的D*Lite路径规划算法广泛应用于自主移动机器人导航，它可以利用先前路径规划的信息来动态快速地重新搜寻机器人的路径，因此可以将其应用于未知环境中的动态路径规划。以下符号用于描述D*Lite路径规划算法：S代表图形的节点集。

代表节点s∈S的后继节点集合，它表示可以到达节点s的所有节点。

代表节点s∈S前继节点的集合，表示所有节点s能到达的节点。在网格地图中，某一节点的前继节点和后继节点集合是其所有非障碍物近邻节点的集合。0<c(s,s’)<∞代表从节点s移到节点s’∈Succ(s)的距离代价。D*Lite路径规划算法有两个估计节点s目标距离的变量：g(s)和rhs(s)。rhs(s)始终满足以下关系：

其中，当g(s)＝rhs(s)时，节点s处于局部一致状态，反之节点s处于局部不一致状态。对于局部不一致的节点s，若g(s)>rhs(s)，节点s处于局部过一致状态；若g(s)<rhs(s)，节点s处于局部欠一致状态。

本发明实施例提供了一种基于碰撞类型划分的多机器人路径规划方法，通过对多机器人的碰撞类型进行划分，然后基于碰撞类型和多个机器人的运动信息，在运动平面上设置多个人工不可越节点，可以把为单机器人系统设计的路径规划算法应用到多机器人导航中，使得多机器人系统中的机器人因避碰而产生的绕行距离更短，效率更高，同时缓解了现有技术中存在的有时会无法成功避免机器人间的碰撞和死锁的技术问题。

在本发明实施例中，机器人每次移动之前，都会按从高优先级到低优先级的顺序检测与其他机器人的潜在冲突，并在必要时重新规划其路径。因此，本发明实施例给出了多机器人系统中成对机器人(即每两个机器人)之间的碰撞类型。

具体地，预设碰撞类型包括如下五种类型：第一碰撞类型，第二碰撞类型，第三碰撞类型，第四碰撞类型和第五碰撞类型。

具体地，如图2所示，在第一碰撞类型的碰撞场景(Type 1 Collision Scenario，以下简称CST1)中，两个机器人(即图2中的Ri和Rj)都沿着对角线方向运动，且碰撞节点为两个机器人的共同的下一个节点。

如图3所示，在第二碰撞类型的碰撞场景(Type 2 Collision Scenario，以下简称CST2)中，两个机器人的碰撞节点为第一机器人Rj的当前节点。在网格法中，机器人只有八个移动方向，在图3中，Rj的方向可以是除指向Ri以外的任何其他七个方向。

如图4～图11所示，在第三碰撞类型的碰撞场景(Type 3 Collision Scenario，以下简称CST3)中，两个机器人的碰撞节点为第二机器人Ri的当前节点。其中，图4～图11分别对应第二机器人Ri的八个移动方向的情况。具体地，图4为第二机器人方向朝北时的第三碰撞类型的碰撞场景示意图，图5为第二机器人方向朝东北时的第三碰撞类型的碰撞场景示意图，图6为第二机器人方向朝东时的第三碰撞类型的碰撞场景示意图，图7为第二机器人方向朝东南时的第三碰撞类型的碰撞场景示意图，图8为第二机器人方向朝南时的第三碰撞类型的碰撞场景示意图，图9为第二机器人方向朝西南时的第三碰撞类型的碰撞场景示意图，图10为第二机器人方向朝西时的第三碰撞类型的碰撞场景示意图，图11为第二机器人方向朝西北时的第三碰撞类型的碰撞场景示意图。

如图12～图19所示，在第四碰撞类型的碰撞场景(Type 4 Collision Scenario，以下简称CST4)中，两个机器人的当前节点为相邻节点，且碰撞节点为第二机器人Ri基于当前运动路径的下一个节点。其中，图12～图19分别对应第二机器人Ri的八个移动方向的情况。具体地，图12为第二机器人方向朝北时的第四碰撞类型的碰撞场景示意图，图13为第二机器人方向朝东北时的第四碰撞类型的碰撞场景示意图，图14为第二机器人方向朝东时的第四碰撞类型的碰撞场景示意图，图15为第二机器人方向朝东南时的第四碰撞类型的碰撞场景示意图，图16为第二机器人方向朝南时的第四碰撞类型的碰撞场景示意图，图17为第二机器人方向朝西南时的第四碰撞类型的碰撞场景示意图，图18为第二机器人方向朝西时的第四碰撞类型的碰撞场景示意图，图19为第二机器人方向朝西北时的第四碰撞类型的碰撞场景示意图。

如图20～图27所示，在第五碰撞类型的碰撞场景(Type 5 Collision Scenario，以下简称CST5)中，两个机器人的当前节点不相邻，且碰撞节点为第二机器人Ri基于当前运动路径的下一个节点。其中，图20～图27分别对应第二机器人Ri的八个移动方向的情况。具体地，图20为第二机器人方向朝北时的第五碰撞类型的碰撞场景示意图，图21为第二机器人方向朝东北时的第五碰撞类型的碰撞场景示意图，图22为第二机器人方向朝东时的第五碰撞类型的碰撞场景示意图，图23为第二机器人方向朝东南时的第五碰撞类型的碰撞场景示意图，图24为第二机器人方向朝南时的第五碰撞类型的碰撞场景示意图，图25为第二机器人方向朝西南时的第五碰撞类型的碰撞场景示意图，图26为第二机器人方向朝西时的第五碰撞类型的碰撞场景示意图，图27为第二机器人方向朝西北时的第五碰撞类型的碰撞场景示意图。

其中，第一机器人Rj为两个机器人当中优先级较低的机器人，第二机器人Ri为两个机器人当中优先级较高的机器人。

在本发明实施例中，人工不可越节点(artificial untraversable vertex，简称AUV)为人工设置的节点，其中，在网格环境中，环境信息中障碍物的出现或消失与网格图中不可越节点(untraversable vertex)的变化相对应。在本发明实施例中，由于设置不可越节点时并没有对应真正障碍物的出现，因此说不可越节点是人工设置的。AUV的数量和位置应使机器人的绕行距离和转向角尽量小。

具体地，根据碰撞类型的不同，AUV有不同的设置方法，在本发明实施例中，有三种AUV的设置方法，分别为AUV1、AUV2和AUV3，这三种AUV是根据可能与原机器人Ro碰撞的另一个机器人R的坐标和方向进行设置的。对应于Ri的八个移动方向，每种AUV有八种情况。图28为本发明实施例提供的第一种人工不可越节点的八种情况示意图，应用于较高优先级和较低优先级的机器人。图29为本发明实施例提供的第二种人工不可越节点的八种情况示意图，显示了适用于优先级较低的机器人的AUV2的八种情况。图30为本发明实施例提供的第三种人工不可越节点的八种情况示意图，适用于优先级较低的机器人。这里优先级的高和低是针对有碰撞风险的一对机器人来说的。在图28～图30中，黑色方格是针对原机器人Ro设置的AUV。注意机器人的当前节点不会被设为针对它自身的AUV。

可选地，若目标碰撞类型为第一碰撞类型，则步骤S108包括：

将第二机器人基于当前运动路径的下一个节点设置为第一机器人的人工不可越节点；第一机器人的人工不可越节点为第一机器人在路径规划时视为障碍物的节点。

具体地，图31为本发明实施例提供的一种基于第一碰撞类型的人工不可越节点的设置示意图。机器人的下一个位置是同一节点。在这种情况下，较高优先级的机器人Ri可以继续沿其已建立的路径移动，而较低优先级的机器人Rj需要绕过棕色的冲突节点。因此，将Ri的下一个节点设置为机器人Rj的AUV以避免碰撞。Rj的AUV设置为如图31所示，图31是一个八连接网格，其网格间的距离代价最初为1，当机器人发现节点无法越过时变为无穷大。在本发明实施例中，机器人监视的网格范围是其周围48个节点，如图31阴影中的网格所示。由于D*Lite可以随着栅格间距离代价变化而快速重新规划出机器人当前节点到目标节点之间的最短路径，因此，根据搜索顺序的不同，Rj的移动方向将从西南变为西或南。本发明实施例中的搜索顺序是顺时针方向，因此方向将变为如图31所示。

可选地，若目标碰撞类型为第二碰撞类型，则步骤S108包括：

将运动平面上与第一机器人的当前运动路径相关的多个节点设置为第二机器人的人工不可越节点；将运动平面上与第二机器人的当前运动路径相关的多个节点设置为第一机器人的人工不可越节点；第一机器人的人工不可越节点为第一机器人在路径规划时视为障碍物的节点；第二机器人的人工不可越节点为第二机器人在路径规划时视为障碍物的节点。

具体地，图32为本发明实施例提供的一种基于第二碰撞类型的人工不可越节点的设置示意图。其中，图32中的左图为针对第二机器人Ri设置AUV的示意图，右图为针对第一机器人Rj设置AUV的示意图。因为较低优先级的机器人Rj的方向不指向Ri，所以它可以继续沿其建立的原始路径移动。较高优先级的机器人Ri的下一步位置被Rj占据，所以它必须重新规划路径。此时，如果仅将Rj的当前节点设置为Ri的AUV，由于Ri也可能会由于其他机器人而更新路径，则不能保证重新规划后的路径不会与Rj冲突。因此，还应考虑Rj的方向来设置Ri的AUV。于是对于CST2，Ri和Rj的AUV都根据AUV1进行设置。如图32所示，假设Rj的移动方向是东南，Ri和Rj的AUV分别如图32中的左图和右图所示，Ri的方向从北变为西北。

可选地，若目标碰撞类型为第三碰撞类型或者第四碰撞类型，则步骤S108包括：

将第一机器人的当前节点设置为第二机器人的人工不可越节点；基于第二机器人的当前节点坐标、目标节点坐标和第二机器人的人工不可越节点，对第二机器人的运动路径进行重新规划，得到第二机器人的目标运动路径；将运动平面上与第二机器人的目标运动路径相关的多个节点设置为第一机器人的人工不可越节点；第一机器人的人工不可越节点为第一机器人在路径规划时视为障碍物的节点；第二机器人的人工不可越节点为第二机器人在路径规划时视为障碍物的节点。

具体地，图33为本发明实施例提供的一种基于第三碰撞类型的人工不可越节点的设置示意图。其中，图33中的左图为针对第二机器人Ri设置AUV的示意图，右图为针对第一机器人Rj设置AUV的示意图。优先级较低的机器人Rj的下一个位置被Ri占据，因此Rj应重新规划其路径。较高优先级的机器人Ri只需绕过Rj的当前位置。MR-D*Lite规划路径是按照机器人的优先级从高到低的顺序进行的，因此仅需要将Rj的当前节点设置为Ri的AUV。然后根据Ri的位置和重新规划后的方向(即根据AUV1)为Rj设置AUV。Ri的AUV设置为如图33左图所示，Rj的AUV设置为如图33右图所示。经过重新规划后，Ri的方向从北变为东北，Rj的方向从南变为西南。

具体地，图34为本发明实施例提供的一种基于第四碰撞类型的人工不可越节点的设置示意图。其中，图34中的左图为针对第二机器人Ri设置AUV的示意图，右图为针对第一机器人Rj设置AUV的示意图。机器人的下一个位置发生冲突且机器人彼此相邻。较高优先级的机器人Ri可以继续沿其已建立的路径移动。但是为了防止Ri更新后的路径(由于多机器人系统中的其他机器人，Ri可能会更新路径)与Rj的当前节点冲突，需要将Rj的当前节点设置为Ri的AUV。然后，根据AUV1为Rj设置AUV。即CST3的解决方案与CST4的解决方案相同。Ri的AUV设置为如图34左图所示，Rj的AUV设置为如图34右图所示，在D*Lite更新路径后，Rj的方向从西南变为西北。

可选地，若目标碰撞类型为第五碰撞类型，则步骤S108包括：

将运动平面上与第二机器人的当前运动路径相关的多个节点设置为第一机器人的人工不可越节点；第一机器人的人工不可越节点为第一机器人在路径规划时视为障碍物的节点。

具体地，图35为本发明实施例提供的一种基于第四碰撞类型的人工不可越节点的设置示意图。其中，图35中的右图为针对第二机器人Ri设置AUV的示意图，左图为针对第一机器人Rj设置AUV的示意图。如图35所示，机器人的下一个位置发生冲突，但机器人彼此不相邻。在这种情况下，机器人发生碰撞的位置不可能是其当前节点，因此可以根据AUV2为Rj设置AUV。Rj的AUV设置为如图35左图所示，Rj的方向经D*Lite更新后从东南变为南。由于在机器人移动一步到下一个节点后，所有的AUV均被清除且路径也会随之更新。因此，在图35左图中的机器人移动一步并到达图35右图中所示的位置后，之前为Rj设置的所有AUV都会消失。再次重新规划路径后，Rj的方向从南方又变为原始的东南方向。

如果将上述方法应用于有三个或更多机器人的系统，由于机器人直到碰撞发生或将要发生时才检测到风险，所以有可能会出现机器人之间的拥挤问题。如图36所示，图36为本发明实施例提供的一种多机器人系统的示意图。如图36左图所示的机器人，如果它们到达图36右图所示的位置，则将很难再为每个机器人重新规划出平滑的无碰撞路径。

AUV3可以使机器人对潜在的碰撞提前做出反应，因此本发明实施例用AUV3来避免图36中所示的拥挤情况的发生。如果机器人的下一个位置彼此相邻，则根据AUV3为优先级较低的机器人设置AUV。以图36左图为例，机器人的AUV设置为如图37所示。在图36左图所示的位置，低优先级机器人R2检测到其下一步节点与高优先级机器人R1的下一步节点相邻。R2的AUV设置为如图37左图所示，在重新规划路径后，其方向从西南变为西。然后，根据R1和R2最新的路径信息为R3设置AUV。在图37左图所示的位置，低优先级机器人R3检测到其下一节点仅与高优先级机器人R1的下一节点相邻。R3的AUV设置为如图37中图所示，其方向从西北变为东北。在图37中图所示的位置，低优先级机器人R4检测到其下一节点仅与高优先级机器人R1的下一节点相邻。R4的AUV设置为如图37右图所示，其方向从东北变为北。

图38是本发明实施例提供的一种多机器人系统中任意一个机器人Ri路径规划的流程图。由D*Lite算法规划每个机器人的初始路径，如果Ri在沿着该路径移动时发现与其他机器人存在碰撞的风险，它将首先考虑具有最高优先级的机器人(Rh)的位置和方向来重新搜索路径。然后，Ri将继续考虑与重新规划后的路径仍然冲突的具有最高优先级的机器人(Rh)，依次类推。当与所有其他机器人都没有碰撞风险之后，该机器人将移动到下一个节点，然后重复该过程，直到最终到达目标点或检测到不存在实际的路径为止。

本发明实施例提供了一种基于碰撞类型划分的多机器人路径规划方法，解决了多机器人之间的碰撞和死锁问题，同时保持了D*Lite算法的鲁棒性和实时性。仿真实验结果表明，与其他方法相比，该算法有较好的应对复杂障碍物的能力，同时在轨迹长度和绕行距离方面优于经典的APF和FIS算法。

实施例二：

图39为根据本发明实施例提供的一种基于碰撞类型划分的多机器人路径规划系统的示意图，该系统应用于多机器人控制系统。如图39所示，该系统包括：获取模块10，判断模块20，确定模块30，设置模块40和路径规划模块50。

具体地，获取模块10，用于获取多机器人控制系统中每个机器人的运动信息；每个机器人的运动信息包括：当前节点坐标、当前运动路径和目标节点坐标。

判断模块20，用于基于每个机器人的运动信息，以每两个机器人为一组，判断两个机器人之间是否存在碰撞风险。

确定模块30，用于如果是，则基于两个机器人的运动信息，确定碰撞风险所属的目标碰撞类型；目标碰撞类型为多个预设碰撞类型中的一种碰撞类型。

可选地，预设碰撞类型包括：第一碰撞类型，表征两个机器人都沿着对角线方向运动，且碰撞节点为两个机器人的共同的下一个节点；第二碰撞类型，表征两个机器人的碰撞节点为第一机器人的当前节点；第三碰撞类型，表征两个机器人的碰撞节点为第二机器人的当前节点；第四碰撞类型，表征两个机器人的当前节点为相邻节点，且碰撞节点为第二机器人基于当前运动路径的下一个节点；第五碰撞类型，表征两个机器人的当前节点不相邻，且碰撞节点为第二机器人基于当前运动路径的下一个节点；其中，第一机器人为两个机器人当中优先级较低的机器人，第二机器人为两个机器人当中优先级较高的机器人。

设置模块40，用于基于目标碰撞类型，在两个机器人的运动平面上设置多个人工不可越节点；人工不可越节点为机器人在路径规划时视为障碍物的节点。

路径规划模块50，用于基于两个机器人的当前节点坐标、目标节点坐标和多个人工不可越节点，分别对两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

可选地，路径规划模块50，还用于以两个机器人的当前节点坐标、目标节点坐标和多个人工不可越节点作为初始变量，基于D*Lite路径规划算法，分别对两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

本发明实施例提供了一种基于碰撞类型划分的多机器人路径规划系统，通过对多机器人的碰撞类型进行划分，然后基于碰撞类型和多个机器人的运动信息，在运动平面上设置多个人工不可越节点，可以把为单机器人系统设计的路径规划算法应用到多机器人导航中，使得多机器人系统中的机器人因避碰而产生的绕行距离更短，效率更高，同时缓解了现有技术中存在的有时会无法成功避免机器人间的碰撞和死锁的技术问题。

可选地，图40是根据本发明实施例提供的一种设置模块的示意图。如图40所示，设置模块40还包括：第一设置单元41，第二设置单元42，第三设置单元43和第四设置单元44。

具体地，第一设置单元41，用于若目标碰撞类型为第一碰撞类型，则将第二机器人基于当前运动路径的下一个节点设置为第一机器人的人工不可越节点。

第二设置单元42，用于若目标碰撞类型为第二碰撞类型，则将运动平面上与第一机器人的当前运动路径相关的多个节点设置为第二机器人的人工不可越节点；将运动平面上与第二机器人的当前运动路径相关的多个节点设置为第一机器人的人工不可越节点；

第三设置单元43，用于若目标碰撞类型为第三碰撞类型或者第四碰撞类型，则将第一机器人的当前节点设置为第二机器人的人工不可越节点；基于第二机器人的当前节点坐标、目标节点坐标和第二机器人的人工不可越节点，对第二机器人的运动路径进行重新规划，得到第二机器人的目标运动路径；将运动平面上与第二机器人的目标运动路径相关的多个节点设置为第一机器人的人工不可越节点。

第四设置单元44，用于若目标碰撞类型为第五碰撞类型，则将运动平面上与第二机器人的当前运动路径相关的多个节点设置为第一机器人的人工不可越节点。

其中，第一机器人的人工不可越节点为第一机器人在路径规划时视为障碍物的节点；第二机器人的人工不可越节点为第二机器人在路径规划时视为障碍物的节点。

本发明实施例还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现上述实施例一中的方法的步骤。

本发明实施例还提供了一种具有处理器可执行的非易失的程序代码的计算机可读介质，程序代码使处理器执行上述实施例一中的方法。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种基于碰撞类型划分的多机器人路径规划方法，其特征在于，应用于多机器人控制系统；包括：

获取所述多机器人控制系统中每个机器人的运动信息；所述每个机器人的运动信息包括：当前节点坐标、当前运动路径和目标节点坐标；

基于所述每个机器人的运动信息，以每两个机器人为一组，判断所述两个机器人之间是否存在碰撞风险；

如果是，则基于所述两个机器人的运动信息，确定所述碰撞风险所属的目标碰撞类型；所述目标碰撞类型为多个预设碰撞类型中的一种碰撞类型；

基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点；所述人工不可越节点为机器人在路径规划时视为障碍物的节点；

基于所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径；

所述预设碰撞类型包括：

第一碰撞类型，表征所述两个机器人都沿着对角线方向运动，且碰撞节点为所述两个机器人的共同的下一个节点；

第二碰撞类型，表征所述两个机器人的碰撞节点为第一机器人的当前节点；

第三碰撞类型，表征所述两个机器人的碰撞节点为第二机器人的当前节点；

第四碰撞类型，表征所述两个机器人的当前节点为相邻节点，且碰撞节点为所述第二机器人基于当前运动路径的下一个节点；

第五碰撞类型，表征所述两个机器人的当前节点不相邻，且碰撞节点为所述第二机器人基于当前运动路径的下一个节点；

其中，所述第一机器人为所述两个机器人当中优先级低的机器人，所述第二机器人为所述两个机器人当中优先级高的机器人；

若所述目标碰撞类型为所述第二碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：

将所述运动平面上与所述第一机器人的当前运动路径相关的多个节点设置为所述第二机器人的人工不可越节点；

将所述运动平面上与所述第二机器人的当前运动路径相关的多个节点设置为所述第一机器人的人工不可越节点；

所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点；所述第二机器人的人工不可越节点为所述第二机器人在路径规划时视为障碍物的节点。

2.根据权利要求1所述的方法，其特征在于，若所述目标碰撞类型为所述第一碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：

将所述第二机器人基于当前运动路径的下一个节点设置为所述第一机器人的人工不可越节点；所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点。

3.根据权利要求1所述的方法，其特征在于，若所述目标碰撞类型为所述第三碰撞类型或者所述第四碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：

将所述第一机器人的当前节点设置为所述第二机器人的人工不可越节点；

基于所述第二机器人的当前节点坐标、目标节点坐标和所述第二机器人的人工不可越节点，对所述第二机器人的运动路径进行重新规划，得到第二机器人的目标运动路径；

将所述运动平面上与所述第二机器人的目标运动路径相关的多个节点设置为所述第一机器人的人工不可越节点；

所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点；所述第二机器人的人工不可越节点为所述第二机器人在路径规划时视为障碍物的节点。

4.根据权利要求1所述的方法，其特征在于，若所述目标碰撞类型为所述第五碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：

将所述运动平面上与所述第二机器人的当前运动路径相关的多个节点设置为所述第一机器人的人工不可越节点；

所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点。

5.根据权利要求1所述的方法，其特征在于，基于所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径，包括：

以所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点作为初始变量，基于D* Lite路径规划算法，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径。

6.一种基于碰撞类型划分的多机器人路径规划系统，其特征在于，应用于多机器人控制系统；包括：获取模块，判断模块，确定模块，设置模块和路径规划模块，其中，

所述获取模块，用于获取所述多机器人控制系统中每个机器人的运动信息；所述每个机器人的运动信息包括：当前节点坐标、当前运动路径和目标节点坐标；

所述判断模块，用于基于所述每个机器人的运动信息，以每两个机器人为一组，判断所述两个机器人之间是否存在碰撞风险；

所述确定模块，用于如果是，则基于所述两个机器人的运动信息，确定所述碰撞风险所属的目标碰撞类型；所述目标碰撞类型为多个预设碰撞类型中的一种碰撞类型；

所述设置模块，用于基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点；所述人工不可越节点为机器人在路径规划时视为障碍物的节点；

所述路径规划模块，用于基于所述两个机器人的当前节点坐标、目标节点坐标和所述多个人工不可越节点，分别对所述两个机器人中的每个机器人的运动路径进行重新规划，得到每个机器人的目标运动路径；

所述预设碰撞类型包括：

第一碰撞类型，表征所述两个机器人都沿着对角线方向运动，且碰撞节点为所述两个机器人的共同的下一个节点；

第二碰撞类型，表征所述两个机器人的碰撞节点为第一机器人的当前节点；

第三碰撞类型，表征所述两个机器人的碰撞节点为第二机器人的当前节点；

第四碰撞类型，表征所述两个机器人的当前节点为相邻节点，且碰撞节点为所述第二机器人基于当前运动路径的下一个节点；

第五碰撞类型，表征所述两个机器人的当前节点不相邻，且碰撞节点为所述第二机器人基于当前运动路径的下一个节点；

其中，所述第一机器人为所述两个机器人当中优先级低的机器人，所述第二机器人为所述两个机器人当中优先级高的机器人；

所述设置模块还包括：第二设置单元，用于若所述目标碰撞类型为所述第二碰撞类型，则基于所述目标碰撞类型，在所述两个机器人的运动平面上设置多个人工不可越节点，包括：

将所述运动平面上与所述第一机器人的当前运动路径相关的多个节点设置为所述第二机器人的人工不可越节点；

将所述运动平面上与所述第二机器人的当前运动路径相关的多个节点设置为所述第一机器人的人工不可越节点；

所述第一机器人的人工不可越节点为所述第一机器人在路径规划时视为障碍物的节点；所述第二机器人的人工不可越节点为所述第二机器人在路径规划时视为障碍物的节点。

7.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现上述权利要求1至5任一项所述的方法的步骤。

8.一种具有处理器可执行的非易失的程序代码的计算机可读介质，其特征在于，所述程序代码使所述处理器执行所述权利要求1-5任一项所述方法。

Images