本发明涉及一种多主体寻路方法，具体是一种基于拓扑地图和冲突消除策略的多无人车寻路方法。
背景技术：
近年来，无人驾驶的研究发展迅速，其涉及机器人、计算机科学和工程学等多个领域。多无人车路径规划问题是多主体寻路问题(multi-agentpathfinding，mapf)在无人驾驶领域的延伸。mapf被定义为：存在多个主体，它们具有不同的目标点，为这些主体寻找无冲突路径计划的问题。mapf问题为无人车路径规划提供理论基础，解决mapf问题的算法通过改进可以用于解决无人车路径规划问题。
目前最受欢迎的解决mapf的算法是基于冲突的搜索算法(conflict-basedsearch，cbs)。cbs是一个面向mapf问题的求解算法，它通过运行两级搜索算法(高级搜索和低级搜索)，来保证算法的完整性和最优性。以前绝大数工作针对网格地图，并假设主体的动作具有统一的持续时间，如：cbs算法等；也有少部分只针对网格地图，不作假设，如：ccbs(cbswithcontinuoustimes)算法等。ccbs也可用于拓扑地图上的mapf问题中。在无人车路径规划问题中，采用网格地图进行路径规划，计算量大，很难在短时间内获得合理的解决方案；拓扑地图以其数据量少，处理方便的优点，被用来作为无人车路径规划的环境。将ccbs算法用于拓扑地图上的多无人车路径规划问题，不仅能够生成高质量的解决方案，而且能够极大地减少计算时间。但是实际使用过程中，会出现不能及时提供无冲突路径计划的现象。此现象不仅降低了无人车的工作效率，而且降低了用户体验。为消除此现象，本发明提出基于拓扑地图和冲突消除策略的多无人车寻路算法。此算法将根据当前路况及时地提供无人车的路径计划，来准确高效地指导无人车的路线调度。
技术实现要素：
本发明提出了一种基于拓扑地图和冲突消除策略的多无人车寻路方法。此方法将根据当前路况及时地提供无人车的路径计划，来准确高效地指导无人车的路线调度。其原理为：开始时，将真实世界的拓扑地图处理成加权有向图，以便实现无人车的精准路径规划，再调用ccbs算法生成无人车的动作计划；若一定时间内找不到无冲突的动作计划，就调用冲突消除策略来消除冲突，从而加速无冲突动作计划的查找。具体地：一开始，将真实世界的拓扑地图处理为加权有向图，并输入每辆无人车的起点和终点，再调用提出的算法生成合理的解，其过程如下。根据无人车在加权有向图中的起点与终点，调用ccbs算法生成无人车的动作计划；然后判断生成的动作计划是否有冲突，若没有冲突，就保存此动作计划到合理的解中；若存在冲突，就检查当前动作计划中是否存在多个无人车同时经过的结点，若存在此类结点，调用“中间点冲突消除法”选择此位置之前的结点作为临时终点；若不存在此类结点，调用“相邻点冲突消除法”选择当前位置相邻的未使用的结点作为临时终点，重复这个过程，直到无人车到达输入的终点。最后，返回合理的解。本发明提出的基于拓扑地图和冲突消除策略的多无人车寻路算法的步骤为：
1)根据真实的地形获取相应的拓扑地图的数据，拓扑地图的数据包含：node(x，y)，track(startnode，endnode，weight)，topomap(tracks，nodes)；其中，node表示拓扑地图中结点，track表示拓扑地图的边，其由开始顶点(startnode)、结束顶点(endnode)和权重(weight)组成，topomap表示整个拓扑地图，由边集(tracks)和顶点集(nodes)构成；
2)将拓扑地图转化为加权有向图，具体地：将拓扑地图中边集的每条边，加上方向生成有向图，再在其边上等距离(0.05倍的单位长度)插入点，点的数目为边的权重，从而生成加权有向图；
3)输入m(m≥3)辆车在加权有向图中的起点与终点，并设无人车的路径集chrome为空，其中，chrome由无人车的动作计划s构成，s表示m辆无人车在一段时间内的动作计划；
4)调用ccbs算法生成无人车的动作计划s，具体地，ccbs算法以无人车的起点与终点为输入，重复调用高级搜索来检查、选择和解决冲突，调用低级搜索来生成新的动作计划s，直到生成合理解或到达最大运行时间，返回s；
5)判断s中是否存在冲突，若不存在，进入步骤11)；
6)若存在，获取s中所有的冲突点，记录下含有冲突最多的结点p及其冲突数nt；
7)分析在p点存在冲突的无人车数量，即：nt≥m-1，若是，进入步骤8)；若否，进入步骤9)；
8)调用“中间点冲突消除法”，选择最大冲突点之前的位置作为无人车的目标位置；
9)调用“相邻点冲突消除法”，选择当前位置的相邻位置作为无人车的目标位置；
10)以选择的每辆无人车的目标位置作为其终点，进入步骤4)；
11)将新生成的无人车的动作计划s加入到无人车的路径集chrome中，并判断无人车是否到达输入的终点，若到达，进入步骤12)；若没有到达，以当前位置为无人车的起点，输入终点为无人车的终点，然后进入步骤4)；
12)对多无人车的路径集chrome进行处理，即：将按时间段的路径集chrome＝{s0，s1，...}转化成面向无人车的联合计划π＝{π1，π2，...，πm}，然后返回π；
其中，中间点冲突消除法，具体包括：定义了动作计划中每个结点的选择概率，如下所示：
其中，ni表示无人车i动作计划中顶点的数目，
其中，相邻点冲突消除法，具体包括：
1)针对每辆冲突的无人车建立其所在结点的邻接表，随机地将其未使用的邻居顶点加入到邻接表中。
2)根据无人车的邻接表长度对无人车进行升序排序，从而使邻居少的无人车优先被处理。
3)按顺序对每个冲突的无人车进行分配相邻顶点，并将分配过的顶点从整个邻接表中删除，若无人车没有无冲突的相邻顶点，则无人车的目标为当前位置。
4)重复3)，直到冲突的无人车被重新分配相邻点完毕。
由上述对本发明的描述可知，与现有技术相比，本发明具有如下有益效果：
(1)引入拓扑地图作为无人车调度环境，来减少多无人车路径规划的计算量；
(2)通过将拓扑地图转化成加权有向图来生成精准调度方案，再对精准调度方案进行rviz仿真，证明算法的有效性；
(3)提出两种冲突消除方法，即：中间点冲突消除法和相邻点冲突消除法，并根据动作计划中冲突结点的属性，选择冲突消除方法，来消除冲突，从而快速生成无冲突的路径计划，在本发明中，拓扑地图的和两种冲突消除方法引入与处理，不仅降低了解决多无人车路径规划问题的难度，而且加快了生成路径计划的速度，为无人驾驶提供了一个参考算法。
附图说明
图1一种基于拓扑地图和冲突消除策略的多无人车寻路算法流程图；
图23辆无人车的路径规划问题；
图3冲突根源(a)多无人车同时经过某结点；(b)多无人车位置相邻；
以下结合附图和具体实施例对本发明作进一步详述。
具体实施方式
以下通过具体实施方式对本发明作进一步的描述。
本发明针对多无人车路径规划问题，公开了一种基于拓扑地图和冲突消除策略的多无人车寻路算法，如图1是本发明的方法流程图，
1)根据真实的地形获取相应的拓扑地图的数据，拓扑地图的数据包含：node(x，y)，track(startnode，endnode，weight)，topomap(tracks，nodes)；其中，node表示拓扑地图中结点，track表示拓扑地图的边，其由开始顶点(startnode)、结束顶点(endnode)和权重(weight)组成，topomap表示整个拓扑地图，由边集(tracks)和顶点集(nodes)构成；
2)将拓扑地图转化为加权有向图，具体地：将拓扑地图中边集的每条边，加上方向生成有向图，再在其边上等距离(0.05倍的单位长度)插入点，点的数目为边的权重，从而生成加权有向图；
拓扑地图是对环境结构信息的提取，但现实场景中获得的拓扑地图中，各个边的长度不尽相同，且差距有时很大，直接用于多无人车路径规划会造成很大的误差；因此，本发明对拓扑地图中的边进行插值处理，将其转化成加权有向图，来实现精准的多无人车路径规划。这样生成的多无人车的联合计划，就可以用rviz进行仿真实验，模拟真实情况下的多无人车的运行。
将拓扑地图转化成加权有向图的过程如下：
1)依次将拓扑地图中的边加入到边集tracks，再依次遍历拓扑地图中的边，并将其起点和终点进行替换，将生成的新边加入到tracks中。
2)将tracks中每一条边按照下列方法构造成一条加权有向边。首先，由边的起点与终点，根据如下公式计算每条边中要插入点的数目pnum。
其中，length＝0.05。其次，向此边中均匀的插入pnum个点；最后，将边的权重weight设为pnum。
3)将加权有向图中的每一条边按一定长度(0.05倍的单位长度)进行分割，使地图更加精细以便精准的进行多无人车路径规划，同时更新拓扑地图的边集和顶点集。
3)输入m(m≥3)辆车在加权有向图中的起点与终点，并设无人车的路径集chrome为空，其中，chrome由无人车的动作计划s构成，s表示m辆无人车在一段时间内的动作计划；
4)调用ccbs算法生成无人车的动作计划s，具体地，ccbs算法以无人车的起点与终点为输入，重复调用高级搜索来检查、选择和解决冲突，调用低级搜索来生成新的动作计划s，直到生成合理解或到达最大运行时间，返回s；
ccbs算法是一个两级搜索算法，包括高级搜索和低级搜索。在多无人车路径规划问题中，高级搜索的主要功能是：检查联合计划中的冲突、选择冲突和为无人车生成约束来解决冲突；低级搜索的主要功能是查找满足约束的所有无人车的联合计划。ccbs算法通过高级搜索与低级搜索的嵌套调用从而生成无冲突的联合计划，指导多无人车的行驶。
在ccbs的高级搜索中，检查联合计划中的冲突采用“快速闭环碰撞检测机制”，在这个机制中，同时考虑无人车的形状、当前位置和当前速度等因素，来预测两个无人车是否发生碰撞，由于选择有潜力的冲突进行解决，可以减小状态树的规模，减少计算成本。本发明采用两种启发式方法，即：先验冲突启发式方法(h1)和混合启发式方法(h2)，来加快找到有潜力的冲突，进而减少算法运行时间，将有潜力的冲突转化成约束，需要根据冲突无人车的动作计算它的不安全时间间隔，然后将这个不安全时间间隔加入到此无人车上，来解决冲突。在ccbs的低级搜索中，ccbs的低级求解器是sipp算法。
5)判断s中是否存在冲突，若不存在，进入步骤11)；
6)若存在，获取s中所有的冲突点，记录下含有冲突最多的结点p及其冲突数nt；
7)分析在p点存在冲突的无人车数量，即：nt≥m-1，若是，进入步骤8)；若否，进入步骤9)；
8)调用“中间点冲突消除法”，选择最大冲突点之前的位置作为无人车的目标位置；
9)调用“相邻点冲突消除法”，选择当前位置的相邻位置作为无人车的目标位置；
关于两种冲突消除方法，在多任务多无人车调度场景中，无人车的运行会出现不能及时提供无冲突路径计划的现象。此现象的根源有两个，一个是多个无人车同时经过某个结点，如图3(a)中o点，在此结点处存在大量的冲突；另一个是有冲突的结点较多，且无人车彼此占用需要经过的结点，如图3(b)无人车的位置相邻，造成一部分无人车无可移动的结点。对此，本发明提出了两种启发式方法来加速算法的运行，分别是：中间点冲突消除法和相邻点冲突消除法。第一种启发式方法的原理是：通过分步式到达的思想来避开冲突结点，即：让冲突的无人车先到达此结点之前的结点，再到达目标点，来加速获得无冲突的路径计划。第二种启发式方法的原理为：当多个无人车位置相邻时，将冲突的无人车移动到相邻的没有冲突的位置进行避障。
具体地，为了消除由多个无人车同时经过某个结点带来的冲突，本发明采用了中间点冲突消除法，其过程如下：图2中无人车a1、a2和a3的动作计划分别为π1＝{a→b，b→f，f→i}、π2＝{e→f，f→i，i→j}和π3＝{g→h，h→c，c→d}。要避开图3(a)中结点o处的冲突，可使无人车a3在h点之前等待。为了实现这个目标，定义了动作计划中每个结点的选择概率，如公式(3)所示：
其中，ni表示无人车i动作计划中顶点的数目，
通过第一种启发式方法，虽然高效的为无人车生成有效路径计划，但当多个无人车的位置相邻且彼此占用可用的资源时，如图3(b)，很难生成有效的路径计划。对此，本发明提出了第二种启发式方法，即：相邻点冲突消除法。当多个无人车位置相邻时，可以将冲突的无人车移动到相邻未冲突的位置进行避障。相邻点冲突消除法的过程如下：
1)针对每辆冲突的无人车建立其所在结点的邻接表，随机地将其未使用的邻居顶点加入到邻接表中。
2)根据无人车的邻接表长度对无人车进行升序排序，从而使邻居少的无人车优先被处理。
3)按顺序对每个冲突的无人车进行分配相邻顶点，并将分配过的顶点从整个邻接表中删除，若无人车没有无冲突的相邻顶点，则无人车的目标为当前位置。
4)重复3)，直到冲突的无人车被重新分配相邻点完毕。
两种冲突消除方法的使用机制如下，在ccbs不能提供合理的动作计划时，本发明对生成的动作计划进行分析，找到其中的冲突结点，如果其中最大的冲突数大于m-1时，就调用“中间点冲突消除法”，即：先计算每辆车经过结点的选择概率，再用轮盘赌的方法选择一个结点作为临时终点；否则，就调用“相邻点冲突消除法”选择当前位置相邻的未使用的结点作为临时终点。通过这两种冲突消除方法来生成临时终点，再重复调用ccbs生成合理的动作计划，直到无人车到达输入终点，可以有效的消除冲突，从而快速的得到无人车到达终点的联合计划。
10)以选择的每辆无人车的目标位置作为其终点，进入步骤4)；
11)将新生成的无人车的动作计划s加入到无人车的路径集chrome中，并判断无人车是否到达输入的终点，若到达，进入步骤12)；若没有到达，以当前位置为无人车的起点，输入终点为无人车的终点，然后进入步骤4)；
12)对多无人车的路径集chrome进行处理，即：将按时间段的路径集chrome＝{s0，s1，...}转化成面向无人车的联合计划π＝{π1，π2，...，πm}，然后返回π；
本发明参照mapf问题的定义，将多无人车寻路问题定义为：在一个加权有向图g(v，e)上，一组以1，...，m标注的无人车车队，每辆无人车i有起点si和终点gi，同时考虑它的几何形状，需要给出最优的无人车路径集，来指导无人车的移动，其中，顶点集v表示无人车可以停留的位置。当一个无人车在位置v∈v时，它既可以进行移动动作，也可以进行等待动作。移动动作指无人车沿边(v，v′)∈e移动，等待动作表示无人车在位置v处等待。每个动作都有持续时间。移动动作的持续时间是无人车通过边的长度(即：边的权重)。等待动作的持续时间可以是任何正值。因此，每个无人车在每个位置可以等待任意时间。无人车i的动作计划πi由一系列的动作构成，如果无人车i执行这些动作，将会达到目标位置。一组针对每辆无人车的动作计划称为联合计划。无人车寻路问题的解是一个联合计划(或路径集)，如果所有的无人车同时开始执行它们各自的动作计划，那么所有的无人车都能在不发生冲突的情况下，到达它们的目标位置。本发明中，解决方案的最优形式是最小化动作成本(sum-of-costs，soc)的方案。
用数学的语言描述多无人车路径规划问题，在加权有向图中，存在m辆无人车，a1、a2...am，每辆无人车有其起点si和终点gi。每辆无人车执行对应的动作计划
其中，c(j，i)表示无人车ai在行动计划πi中第j个动作
为了验证本发明提出方法的性能，我们将2012年sturtevant提出的dragonage：origins(dao)游戏的地图作为测试地图。dao地图使用网格作为内部映射格式，有5种地形，分别为：平地、浅水、树木，水和禁区。游戏地图被提取为一个规模为156的地图集，它允许对不同算法在其上进行比较。其中，最大的地图上有137375个可行的状态，而有些地图上只有几百个可行的状态。我们将提出的算法和ccbs算法在dao数据集上进行测试，并生成了36个配置文件，每个配置文件由4种规模n的无人车集，分别为10、15、20和25；3种规模为2k的邻居，分别为：2、3和4；3种任务规模t组成，分别为：20、30和40构成。每个配置文件可以表示为n-k-t，表示n个无人车的车队规模，每步可以从2k个方向走，完成t次任务的调度配置。在这36种配置文件的情景下，我们将提出的算法与ccbs算法，在156个地图集上进行仿真，取运行10次的运行时间和成本花费的平均值，发现提出的算法在大多数地图中减少50％左右的运行时间，而成本也只比ccbs增加10％左右。这证明：我们提出的算法，能够解决复杂地形多无人车的调度问题，并且减少了调度时间。
为了验证提出的方法，能够实现多无人车在社区流畅、无碰撞的运行，本发明以某社区的拓朴地图为多无人车路径规划的环境，调用提出的算法生成合理的联合计划，并将联合计划通过rviz进行仿真实现，来证明提出算法的性能。具体地，某社区有48条主干道、62主要路口，我们将其抽象为拓扑地图，作为实验环境，最终结果显示，提出的算法能够同时调度10辆无人车2h不发生碰撞。
本申请首先引入拓扑地图作为无人车调度环境，来减少多无人车路径规划的计算量；其次，通过将拓扑地图转化成加权有向图来生成精准调度方案，再对精准调度方案进行rviz仿真，证明算法的有效性；最后，提出两种冲突消除方法，即：中间点冲突消除法和相邻点冲突消除法，并根据动作计划中冲突结点的属性，选择冲突消除方法，来消除冲突，从而快速生成无冲突的路径计划。在本发明中，拓扑地图的和两种冲突消除方法引入与处理，不仅降低了解决多无人车路径规划问题的难度，而且加快了生成路径计划的速度，为无人驾驶提供了一个参考方法。
上述仅为本发明的具体实施方式，但本发明的设计构思并不局限于此，凡利用此构思对本发明进行非实质性的改动，均应属于侵犯本发明保护范围的行为。
1.一种基于拓扑地图和冲突消除策略的多无人车寻路方法，其特征在于，包括以下步骤：
1)根据真实的地形获取相应的拓扑地图的数据，拓扑地图的数据包含：node(x，y)，track(startnode，endnode，weight)，topomap(tracks，nodes)；其中，node表示拓扑地图中结点，track表示拓扑地图的边，其由开始顶点(startnode)、结束顶点(endnode)和权重(weight)组成，topomap表示整个拓扑地图，由边集(tracks)和顶点集(nodes)构成；
2)将拓扑地图转化为加权有向图，具体地：将拓扑地图中边集的每条边，加上方向生成有向图，再在其边上等距离(0.05倍的单位长度)插入点，点的数目为边的权重，从而生成加权有向图；
3)输入m(m≥3)辆车在加权有向图中的起点与终点，并设无人车的路径集chrome为空，其中，chrome由无人车的动作计划s构成，s表示m辆无人车在一段时间内的动作计划；
4)调用ccbs算法生成无人车的动作计划s，具体地，ccbs算法以无人车的起点与终点为输入，重复调用高级搜索来检查、选择和解决冲突，调用低级搜索来生成新的动作计划s，直到生成合理解或到达最大运行时间，返回s；
5)判断s中是否存在冲突，若不存在，进入步骤11)；
6)若存在，获取s中所有的冲突点，记录下含有冲突最多的结点p及其冲突数nt；
7)分析在p点存在冲突的无人车数量，即：nt≥m-1，若是，进入步骤8)；若否，进入步骤9)；
8)调用“中间点冲突消除法”，选择最大冲突点之前的位置作为无人车的目标位置；
9)调用“相邻点冲突消除法”，选择当前位置的相邻位置作为无人车的目标位置；
10)以选择的每辆无人车的目标位置作为其终点，进入步骤4)；
11)将新生成的无人车的动作计划s加入到无人车的路径集chrome中，并判断无人车是否到达输入的终点，若到达，进入步骤12)；若没有到达，以当前位置为无人车的起点，输入终点为无人车的终点，然后进入步骤4)；
12)对多无人车的路径集chrome进行处理，即：将按时间段的路径集chrome＝{s0，s1，...}转化成面向无人车的联合计划π＝{π1，π2，...，πm}，然后返回π。
2.根据权利要求1所述的基于拓扑地图和冲突消除策略的多无人车寻路方法，其特征在于，中间点冲突消除法，具体包括：定义了动作计划中每个结点的选择概率，如下所示：
其中，ni表示无人车i动作计划中顶点的数目，
3.根据权利要求1所述的基于拓扑地图和冲突消除策略的多无人车寻路方法，其特征在于，相邻点冲突消除法，具体包括：
1)针对每辆冲突的无人车建立其所在结点的邻接表，随机地将其未使用的邻居顶点加入到邻接表中。
2)根据无人车的邻接表长度对无人车进行升序排序，从而使邻居少的无人车优先被处理。
3)按顺序对每个冲突的无人车进行分配相邻顶点，并将分配过的顶点从整个邻接表中删除，若无人车没有无冲突的相邻顶点，则无人车的目标为当前位置。
4)重复3)，直到冲突的无人车被重新分配相邻点完毕。
技术总结