基于安全间隔的多机器人路径鲁棒规划方法、系统

技术领域

本发明涉及一种基于安全间隔的多机器人路径鲁棒规划方法、系统，属于多机器人路径规划领域。

背景技术

多机器人路径规划问题是为了找到多个机器人，对于给定起点和终点下的无冲突路径。鲁棒规划是指由于意外事件会使一些机器人产生延迟，在这种情况下机器人还能完成无冲突的路径的规划。

多机器人路径鲁棒规划的研究成果可以在许多领域中，例如仓储物流，无人驾驶系统，机场运输等等领域，解决了的实际工程中的问题。但是对于部分小地图的多机器人路径规划会出现求不出解，求解慢的情况，因此有必要研究考虑鲁棒规划的路径求解方法。

发明内容

本发明提供了一种基于安全间隔的多机器人路径鲁棒规划方法，以用于获得多机器人的路径。

本发明的技术方案是：

根据本发明的一方面，提供了一种基于安全间隔的多机器人路径鲁棒规划方法，包括：对地图中的多机器人输入起点、终点；对多机器人起点依据鲁棒缓冲距离进行起点保护；多机器人依据优先级的顺序进行路径规划。

所述对多机器人起点依据鲁棒缓冲距离进行起点保护，具体为：多机器人的起点依据鲁棒缓冲距离，获得多机器人起点的碰撞间隔；依据碰撞间隔，获得起点的安全间隔。

所述碰撞间隔的起始时间步为起点的时间步，碰撞间隔的结束时间步为起点的时间步与鲁棒缓冲距离之和。

所述安全间隔为整个时间域去除碰撞间隔的部分。

所述多机器人依据优先级的顺序进行路径规划，具体为按照优先级由高到低的顺序进行路径规则，当前机器从进行路径规划的步骤包括：当前优先级机器人利用路径规划算法获得可扩展节点，判断可扩展节点是否满足判断条件；将符合判断条件的扩展节点存入路径列表，继续扩展下一个节点直至到达终点；对当前优先级机器人除终点外的扩展节点依据鲁棒缓冲距离更新安全间隔；依据终点的时间步更新终点的安全间隔。

所述判断条件，包括：第一判断条件，扩展节点是否满足自身设置的鲁棒缓冲距离；第二判断条件，扩展节点是否与高优先级机器人路径有冲突。

所述符合判断条件表明，第一判断条件为“是”，第二判断条件为“否”。

所述依据终点的时间步更新终点的安全间隔具体为：依据终点的时间步，确定终点的非扩展时间域，将其作为终点的碰撞间隔；依据终点的碰撞间隔，获得终点的安全间隔。

根据本发明的另一方面，提供了一种基于安全间隔的多机器人路径鲁棒规划系统，包括：获得模块，用于对地图中的多机器人输入起点、终点；起点保护模块，用于对多机器人起点依据鲁棒缓冲距离进行起点保护；路径规划模块，用于将多机器人依据优先级的顺序进行路径规划。

根据本发明的另一方面，提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时上述中任意一项所述的基于安全间隔的多机器人路径鲁棒规划方法。

本发明的有益效果是：本发明将安全间隔加入了鲁棒缓冲距离，使得系统中机器人在出现意外延时情况时还能继续完成任务，与实际相结合，提高了系统安全性；有效地避免了机器人之间的冲突，为多机器人路径规划提供了安全保障；进一步通过实验表明，本发明在考虑可能出现延时的情况下，大大提高了系统的求解成功率，保证了多机器人的路径规划的执行，并且在正常求解的前提下大大缩短了系统求解路径的时间，提高了系统的求解速度。

附图说明

图1是本发明的流程图；

图2是起点保护解释示意图；

图3是安全间隔的设置解释示意图；

图4为展示的一可选实例图；

图5为本发明与其它算法对比实验结果展示图。

具体实施方式

下面结合附图和实施例，对发明做进一步的说明，但本发明的内容并不限于所述范围。

实施例1：如图1-5所示，一种基于安全间隔的多机器人路径鲁棒规划方法，包括：对地图中的多机器人输入起点、终点；对多机器人起点依据鲁棒缓冲距离进行起点保护；多机器人依据优先级的顺序进行路径规划。

可选地，所述对多机器人起点依据鲁棒缓冲距离进行起点保护，具体为：多机器人的起点依据鲁棒缓冲距离，获得多机器人起点的碰撞间隔，依据起点的碰撞间隔，获得起点的安全间隔。如图2所示，在图2左侧部分展示五个机器人的起点和终点用带标号的1-5号圆圈表示，路径用带箭头的线表示，在图2右边的部分，当机器人在扩展节点时，在起点保护的作用下，使所扩展的路径不包含机器人的起点位置。

可选地，所述碰撞间隔的起始时间步为起点的时间步，碰撞间隔的结束时间步为起点的时间步与鲁棒缓冲距离之和，所有多机器人起点的时间步为相同。如起点的时间步t＝0，鲁棒缓冲距离＝1，则碰撞间隔的起始时间步为0，碰撞间隔的结束时间步为1。

可选地，所述安全间隔为整个时间域去除碰撞间隔的部分。

可选地，所述多机器人依据优先级的顺序进行路径规划，具体为按照优先级由高到低的顺序进行路径规则，当前机器从进行路径规划的步骤包括：当前优先级机器人利用路径规划算法获得可扩展节点，判断可扩展节点是否满足判断条件；将符合判断条件的扩展节点存入路径列表，继续扩展下一个节点直至到达终点；对当前优先级机器人除终点外的扩展节点依据鲁棒缓冲距离更新安全间隔；依据终点的时间步更新终点的安全间隔。

对当前优先级机器人除终点外的扩展节点依据鲁棒缓冲距离更新安全间隔，具体为：对于任意起点至终点之间的扩展节点，依据鲁棒缓冲距离，获得扩展节点的碰撞间隔；依据扩展节点的碰撞间隔，获得扩展节点的安全间隔。其中，所述扩展节点碰撞间隔的起始时间步为扩展节点的时间步，碰撞间隔的结束时间步为扩展节点的时间步与鲁棒缓冲距离之和；所述扩展节点安全间隔为整个时间域去除扩展节点碰撞间隔的部分。在图3中展示一个机器人在编号地图中的完整路径展示图，表1中是机器人的路径信息，表2是该机器人对于整条路径的安全间隔展示，例如，在时刻t＝1，机器人运行到(6,4)这个非终点扩展节点，机器人可能会在该点产生延时，考虑到鲁棒缓冲距离k＝1，则t＝1、2两个时刻是碰撞间隔，进而t＝0和t＝3-∞是该节点(6,4)的安全间隔，即(6,4)节点只有在安全间隔内才会被其他机器人作为扩展节点。

表1

表2

通过上述可知，对于起点及除终点外的扩展节点的安全间隔，在地图上，每一个节点对应一个时间域，时间域初始化为0到无穷大，当有机器人在时刻t经过地图上的节点时，该时刻t便从该地图点的安全间隔可扩展时间域中删除，使得节点扩展后，安全间隔都会减少一点；在考虑延时的情况下，针对每个机器人每个节点，除了起点/扩展节点的当前时刻及延时外都是安全间隔；最终，高优先级机器人扩展完成的路径余下的安全空间再由低优先级机器人完成设置。

可选地，所述依据终点的时间步更新终点的安全间隔具体为：依据终点的时间步，确定终点的非扩展时间域，将其作为终点的碰撞间隔；依据终点的碰撞间隔，获得终点的安全间隔；非扩展时间域的起始时间步为终点的时间步，非扩展时间域的结束时间步为设置的路径规划整个时间域最大值，本实施例中取无穷大；终点的安全间隔为整个时间域去除终点的碰撞间隔的部分；即获得起点的时间步至终点的时间步的前一时间步作为终点的安全间隔，比如起点时间步为t＝0，t＝2时刻获得的终点为(3,3)，进而确定终点的非扩展时间域为t＝2-∞，将其作为终点的碰撞间隔，依据终点的碰撞间隔，确定t＝0-1是该节点(3,3)的安全间隔。

可选地，所述判断条件，包括：第一判断条件，扩展节点是否满足自身设置的鲁棒缓冲距离；第二判断条件，扩展节点是否与高优先级机器人路径有冲突。

可选地，所述符合判断条件表明，第一判断条件为“是”，第二判断条件为“否”。

应用上述技术方案，可以将多机器人路径规划问题分解为前后联系的单机路径规划问题，按机器人优先级由高到低的顺序依次对机器人做单机路径规划，并把前面规划的机器人路径信息添加到碰撞间隔中作为障碍约束添加到后面的机器人规划当中。即在单机A*搜索基础上配合安全间隔完成规划，当完成第i个机器人a

下面对本发明一种可选的实施方式进行详细说明：

如图4，在4*4的无障碍地图中，三个机器人编号分别为0,1,2，设置鲁棒缓冲距离为1。已知当初始时刻，t＝0时，机器人分别在各自的起点位置，图中标有数字符号六边形是各个机器人的终点，标有数字符号三角形是各个机器人的终点。路径规划步骤如下：

步骤1，由给定地图G＝V,E)，V代表地图中节点集合，共计16个节点，E代表地图中边的集合，而地图中n个机器人组成的一组多机器人A＝(a0,a1,a2)，每个机器人都有一个起点、一个终点，分别在图中用带数字的符号表示，六边形代表起点，三角形代表终点。

步骤2，一组机器人在规划路径之前引入起点保护技术，是指由于安全间隔的多机器人路径鲁棒规划方法是按优先级规划多机器人的路径，故结合安全间隔方法，优先级低的机器人在优先级高的机器人安全间隔约束下，不会产生冲突，但是，若优先级高的机器人正常规划路径与在起点出现延时的低优先级的机器人相遇就产生了冲突，由此，设置鲁棒缓冲距离k，将各个机器人的起点对于可能发生的延时的缓冲距离预留出来。鲁棒缓冲安全距离k是一个可人为设置的正整数参数，在使用本方法时，可以结合实际情况更改k的大小。特别地，当k＝1时，表明机器人在拓展路径后，每个节点都存在一个缓冲距离。

系统被给定一组机器人在加入起点保护技术后，机器人的起点位置的安全间隔得到了更新。

步骤3，系统被给定一组机器人首先进行机器人的优先级规划。本实例依据执行任务前后顺序的优先级进行规划，执行顺序在前的为高优先级，执行任务在后的为低优先级，据此实例，优先级从高到低的排序为：a0,a1,a2；

步骤4，任务管理，系统将按照优先级规划好的顺序(0>1>2)传送给下层，进一步进行规划：

当前优先级机器人利用A*算法获得可扩展节点，判断可扩展节点是否满足判断条件：每个节点在扩展时需要判断两点，第一点：扩展出的节点是否和高优先级机器人路径有冲突，即扩展出的节点能不能满足安全间隔；第二点：机器人扩展出的节点是否满足自身设置的鲁棒缓冲距离，即该节点加上鲁棒缓冲距离是否会与其他机器人出现冲突；A*算法选取扩展节点的规则是使路径所消耗的代价值最小，且时间最短。如图4中，2号机器人在扩展节点时，由于地图的限制，可以选择上下左四个可扩展的节点，在判断了与0和1号机器人的行进路径和自身鲁棒缓冲距离，2号机器人在t＝1时刻原地等待一个时刻，是为了避免与0号机器人的预留缓冲距离碰撞。

将符合判断条件的扩展节点存入路径列表，继续扩展下一个节点直至到达终点；对当前优先级机器人除终点外的扩展节点依据鲁棒缓冲距离更新安全间隔；依据终点的时间步更新终点的安全间隔。

步骤5，所有机器人规划出的无冲突路径整合即完整规划方案。

具体而言，

在时刻t＝0，机器人a0位于(3,2)，考虑到延时，鲁棒缓冲距离k＝1，则t＝0、1两个时刻是碰撞间隔，进而t＝2-∞是该节点(3,2)的安全间隔，即(3,2)节点只有在安全间隔内才会被其他机器人作为扩展节点；在时刻t＝0，机器人a1位于(2,1)，考虑到延时，鲁棒缓冲距离k＝1，则t＝0、1两个时刻是碰撞间隔，进而t＝2-∞是该节点(2,1)的安全间隔，即(2,1)节点只有在安全间隔内才会被其他机器人作为扩展节点；在时刻t＝0，机器人a2位于(3,1)，考虑到延时，鲁棒缓冲距离k＝1，则t＝0、1两个时刻是碰撞间隔，进而t＝2-∞是该节点(3,1)的安全间隔，即(3,1)节点只有在安全间隔内才会被其他机器人作为扩展节点；基于此处给出的碰撞间隔，对于地图中的所有机器人的起点具备起点保护功能；

机器人a0的路径扩展：在时刻t＝0，机器人a0位于(3,2)，依据A*算法获得扩展节点(3,3)这个节点，其扩展节点(3,3)在t＝1时刻满足鲁棒缓冲距离的情况下且不会与其它机器人冲突，因此t＝1时刻，a0的节点为(3,3)，进一步判断该节点为终点，进而t＝0是该节点(3,3)的安全间隔，即(3,3)节点只有在安全间隔内才会被其他机器人作为扩展节点；由此获得a0机器人的路径为：t＝0时刻为(3,2)—>t＝1时刻为(3,3)；

接着下一优先级机器人a1的路径扩展：在时刻t＝0，机器人a1位于(2,1)，依据A*算法获得扩展节点(2,0)这个节点，其扩展节点(2,0)在t＝1时刻满足鲁棒缓冲距离的情况下且不会与其它机器人冲突，因此t＝1时刻，a1的节点为(2,0)；进一步判断(2,0)节点为最终节点，进而t＝0是该节点(2,0)的安全间隔，即(2,0)节点只有在安全间隔内才会被其他机器人作为扩展节点；获得a1机器人的路径为：t＝0时刻为(2,1)—>t＝1时刻为(2,0)；

接着下一优先级机器人a2的路径扩展：在时刻t＝0，机器人a2位于(3,1)，依据A*算法获得扩展节点(3,2)这个节点，在t＝1时刻，节点(3,2)会与a0机器人出现冲突，所以t＝1时刻，机器人a2位于(3,1)，(3,2)这个节点在t＝2时刻满足鲁棒缓冲距离的情况下且不会与其它机器人冲突，处于其它机器人安全间隔内，接着更新(3,2)这个节点的安全间隔为t＝0-1和t＝4-∞；依据A*算法获得扩展节点(2,2)这个节点，该节点(2,2)在t＝3时刻满足鲁棒缓冲距离的情况下且不会与其他机器人冲突，机器人可能会在(2,2)点产生延时，考虑到鲁棒缓冲距离k＝1，则t＝3、4两个时刻是碰撞间隔，进而t＝0-2和t＝5-∞是该节点(2,2)的安全间隔；依据A*算法获得扩展节点(2,3)这个节点，该节点(2,3)在t＝4时刻满足鲁棒缓冲距离的情况下且不会与其他机器人冲突，因此t＝4时刻，a2的节点为(2,3)，且该节点为终点，进而t＝0-3是该节点(2,3)的安全间隔；获得a2机器人的路径为：t＝0时刻为(3,1)—>t＝1时刻为(3,1)—>t＝2时刻为(3,2)—>t＝3时刻为(2,2)—>t＝4时刻为(2,3)；

获得上述所有机器人最大时间步为4；依据最大时间步，更新小于最大时间步的机器人的路径信息；即将小于最大时间步机器人的终点时间步延长至最大时间步。即上述a0机器人的路径修改为：t＝0时刻为(3,2)—>t＝1-4时刻为(3,3)；a1机器人的路径修改为：t＝0时刻为(2,1)—>t＝1-4时刻为(2,0)。

应用上述技术方案可知，从图4中可以看出，t＝1时，机器人按照系统探索出的路径行进，2号机器人会向下移动，如若0号机器人因为意外产生延时，会使得两个机器人产生冲突，但此时我们可以从图中可知，2号机器人在t＝1时刻没有运动，是因为我们设置鲁棒缓冲距离在起作用，因而本发明提高系统的安全性。

如图2所示地图，应用上述技术方案，获得路径规划结果如下表3所示：

表3

进一步地，结合实验数据验证本发明的性能：

一、选取典型地图算例32*32地图中，以执行时间为终止条件，分别随机选取20,40,50个机器人进行一百次实验，实验成功率分别为98％，92％，87％；运行平均时间分别为0.02，0.03，0.05秒；平均代价值分别为450，1065，1355个步长，见表4。

表4

二、选取典型空地图16*16地图中，分别随机选取n＝10,15个机器人进行三十次本发明方法K-SIPP(K-robust Safe Interval Path Planning)和K-CBS(K-robustConflict-Based Search)的对比实验，结果显示在k＝1-7的范围内，以执行时间为终止条件，本发明方法对比K-CBS在代价值有小幅度升高的基础上，可以大大提高求解成功率，缩短系统运行时间，如图5所示。

实施例2：一种基于安全间隔的多机器人路径鲁棒规划系统，包括：获得模块，用于对地图中的多机器人输入起点、终点；起点保护模块，用于对多机器人起点依据鲁棒缓冲距离进行起点保护；路径规划模块，用于将多机器人依据优先级的顺序进行路径规划。

实施例3：一种处理器，所述处理器用于运行程序，其中，所述程序运行时上述中任意一项所述的基于安全间隔的多机器人路径鲁棒规划方法。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

上面结合附图对本发明的具体实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。