一种基于拓扑地图和冲突消除策略的多无人车寻路方法

多主体寻路问题

  多主体寻路问题(Multi-Agent Pathfinding, MAPF)被定义为：存在多个主体，它们具有不同的目标点，为这些主体寻找无冲突路径计划的问题。具体地，在一幅图 G(V,E)中，存在k个主体，即： $a_1$， $a_2$ ,…, $a_k$。每个主体 $a_i$ 有一个起点 $s_i$和一个终点 $g_i$。将时间离散为时间步长，主体 $a_i$在 $t_0$时刻在 $s_i$。在连续的时间步骤之间，主体可以移动到相邻的空位置，也可以在当前位置等待。将主体从$s_i$到$g_i$的单个主体移动（或等待）行动序列称为路径。问题的解决方案指的是k个路径计划的集合，其中，每个主体对应一个路径计划。最终返回无冲突的路径计划集，使其中每个主体不与其他主体发生碰撞，同时有最小化累积成本函数。

在图1(i)中的小鼠需要得到它们各自的奶酪。小鼠的起点分别为$S_1$和$S_2$，终点为$G_1$和$G_2$。图1(ii)中给出了所有可能的路径计划，图1(iii)中通过CBS算法的求解过程，最终得到了一种无冲突的路径计划集，即：<1-$S_1$,$A_1$,$A_1$, C ,$G_1$> 和 <2-$S_2$,$B_1$, C ,$G_2$>。

  上面的问题就是多主体寻路问题，本文中要解决的问题是多机器人路径规划问题。多机器人路径规划问题是多主体寻路问题(Multi-Agent Pathfinding, MAPF)在机器人领域的延伸。和MAPF一样，多机器人路径规划问题也是NP难问题。由于在多机器人路径规划问题中，采用网格地图进行路径规划，计算量大，很难在短时间内获得合理的解决方案；而拓扑地图以其数据量少，处理方便的优点，被用来作为多机器人路径规划的环境。将CCBS算法[1]用于拓扑地图上的多机器人路径规划问题，不仅能够生成高质量的解决方案，而且能够极大地减少计算时间。但是实际使用过程中，会出现不能及时提供无冲突路径计划的现象。此现象不仅降低了机器人的工作效率，而且降低了用户体验。为了消除此现象，本文提出基于拓扑地图和冲突消除策略的多机器人寻路算法。此算法将根据当前路况及时地提供机器人的路径计划，来准确高效地指导机器人的路线调度。

基于拓扑地图和冲突消除策略的多机器人寻路方法

  基于拓扑地图和冲突消除策略的多机器人寻路方法将根据当前路况及时地提供机器人的路径计划，来准确高效地指导机器人的路线调度。其原理为：开始时，将真实世界的拓扑地图处理成加权有向图，以便实现机器人的精准路径规划，再调用CCBS算法生成机器人的动作计划；若一定时间内找不到无冲突的动作计划，就调用冲突消除策略来消除冲突，从而加速无冲突动作计划的查找。
  具体地：一开始，将真实世界的拓扑地图处理为加权有向图，并输入每台机器人的起点和终点，再调用提出的算法生成合理的解，其过程如下。根据机器人在加权有向图中的起点与终点，调用CCBS算法生成机器人的动作计划；然后判断生成的动作计划是否有冲突，若没有冲突，就保存此动作计划到合理的解中；若存在冲突，就检查当前动作计划中是否存在多个机器人同时经过的结点，若存在此类结点，调用“中间点冲突消除法”选择此位置之前的结点作为临时终点；若不存在此类结点，调用“相邻点冲突消除法”选择当前位置相邻的未使用的结点作为临时终点，重复这个过程，直到机器人到达输入的终点。最后，返回合理的解。图1是提出方法的流程图。

本文提出的算法主要由仿真地图（加权有向图）的获得、冲突消除策略、CCBS算法三部分组成，接下来将详细叙述。

仿真地图的获得

  本项目中，仿真地图的获得需要对真实数据进行以下两个方面的处理，分别为：根据真实的地形获取相应的拓扑地图的数据和将拓扑地图转化为加权有向图。具体如下所述：
  根据真实的地形获取相应的拓扑地图的数据，即：将真实地形的数据处理成拓扑地图格式的数据。拓扑地图的数据包含：node(x,y)，track(startNode,endNode,weight)，topoMap (tracks,nodes)；其中，node表示拓扑地图中结点，track表示拓扑地图的边，其由开始顶点(startNode)、结束顶点(endNode)和权重(weight)组成，topoMap表示整个拓扑地图，由边集(tracks)和顶点集(nodes)构成。
  将拓扑地图转化为加权有向图，具体地：将拓扑地图中边集的每条边，加上方向生成有向图，再在其边上等距离(0.05倍的单位长度)插入点，点的数目为边的权重，从而生成加权有向图。其仿真地图如下图所示。

冲突消除策略

  在多任务多机器人调度场景中，机器人的运行会出现不能及时提供无冲突路径计划的现象。此现象的根源有两个，一个是多个机器人同时经过某个结点，如图3(a)中O点，在此结点处存在大量的冲突；另一个是有冲突的结点较多，且机器人彼此占用需要经过的结点，如图3(b)机器人的位置相邻，造成一部分机器人无可移动的结点。对此，本文提出了两种启发式方法来加速算法的运行，分别是：中间点冲突消除法和相邻点冲突消除法。第一种启发式方法的原理是：通过分步式到达的思想来避开冲突结点，即：让冲突的机器人先到达此结点之前的结点，再到达目标点，来加速获得无冲突的路径计划。第二种启发式方法的原理为：当多个机器人位置相邻时，将冲突的无人车移动到相邻的没有冲突的位置进行避障。
具体地，为了消除由多个机器人同时经过某个结点带来的冲突，本文采用了中间点冲突消除法，其过程如下：图2(a)中机器人$a_1$、$a_2$和$a_3$的动作计划分别为${\pi }_1$={A→B,B→F,F→I}、${\pi }_2$={E→F,F→I,I→J}和${\pi }_3$={G→H,H→C,C→D}。要避开图3(a)中结点O处的冲突，可使机器人$a_3$在H点之前等待。为了实现这个目标，定义了动作计划中每个结点的选择概率，如公式(1)所示：

  通过第一种启发式方法，虽然高效的为机器人生成有效路径计划，但当多个机器人的位置相邻且彼此占用可用的资源时，如图3(b)，很难生成有效的路径计划。对此，本发明提出了第二种启发式方法，即：相邻点冲突消除法。当多个机器人位置相邻时，可以将冲突的机器人移动到相邻未冲突的位置进行避障。相邻点冲突消除法的过程如下：

1. 针对每辆冲突的机器人建立其所在结点的邻接表，随机地将其未使用的邻居顶点加入到邻接表中。
2. 根据机器人的邻接表长度对机器人进行升序排序，从而使邻居少的机器人优先被处理。
3. 按顺序对每个冲突的机器人进行分配相邻顶点，并将分配过的顶点从整个邻接表中删除，若机器人没有无冲突的相邻顶点，则机器人的目标为当前位置。
4. 重复3），直到冲突的机器人被重新分配相邻点完毕。

  两种冲突消除方法的使用机制如下，在CCBS不能提供合理的动作计划时，本发明对生成的动作计划进行分析，找到其中的冲突结点，如果其中最大的冲突数大于M-1时，就调用“中间点冲突消除法”，即：先计算每辆车经过结点的选择概率，再用轮盘赌的方法选择一个结点作为临时终点；否则，就调用“相邻点冲突消除法”选择当前位置相邻的未使用的结点作为临时终点。通过这两种冲突消除方法来生成临时终点，再重复调用CCBS生成合理的动作计划，直到机器人到达输入终点，可以有效的消除冲突，从而快速的得到机器人到达终点的联合计划。

CCBS算法

  CCBS算法是一个两级搜索算法，包括高级搜索和低级搜索。在多机器人路径规划问题中，高级搜索的主要功能是：检查联合计划中的冲突、选择冲突和为多机器人生成约束来解决冲突；低级搜索的主要功能是查找满足约束的所有多机器人的联合计划。CCBS算法通过高级搜索与低级搜索的嵌套调用从而生成无冲突的联合计划，指导多多机器人的运行。
  在CCBS的高级搜索中，检查联合计划中的冲突采用“快速闭环碰撞检测机制”，在这个机制中，同时考虑多机器人的形状、当前位置和当前速度等因素，来预测两个多机器人是否发生碰撞，由于选择有潜力的冲突进行解决，可以减小状态树的规模，减少计算成本。本发明采用两种启发式方法，即：先验冲突启发式方法（H1）和混合启发式方法(H2)，来加快找到有潜力的冲突，进而减少算法运行时间，将有潜力的冲突转化成约束，需要根据冲突多机器人的动作计算它的不安全时间间隔，然后将这个不安全时间间隔加入到此无人车上，来解决冲突。在CCBS的低级搜索中，CCBS的低级求解器是SIPP算法。
 &emspCCBS算法的细节请读者参照文献[1].

参考文献

[1] Andreychuk A , Yakovlev K , Atzmon D , et al. Multi-Agent Pathfinding with Continuous Time[J]. 2019.