一种基于冲突的多车路径规划优化方法与流程

1.本发明涉及自动化仓储、物流园区技术，更具体地说，涉及一种基于冲突的多车路径规划优化方法。


背景技术：

2.自动化仓储、物流园区指采用自动搬运设备如自动导引车(agv)、自动化叉车等自动化设备在固定空间内做搬运作业的应用场景。为了保证同一空间内多车作业的安全和高效问题，提前给车辆在时间和空间上做好路径规划就成为重点需要解决的问题。如图1所示，即为一个典型的多车工作的仓储场景，其中附图1中灰色区为不可通行区，白色区为可以通行区，圆形代表车辆起始位置，五角星表示目标位置，数字表示车辆编号，圆圈到五角星之间的连线表示规划的车辆行车路径。车辆在路径上行驶通过白色方格时，至少需要独占一个格子，所以两个或者两个以上的车需要在同一时间占用同一个格子时，则定义为一个时空冲突。此时，问题就转化为如何为每一辆车规划出合理的路径，使得所有车辆没有时空冲突，且又能耗时最短。
3.在理论上多车最优路径规划问题是np问题，不存在最优解。所以行业内追求的是如何在行业约束条件下寻求近似最优解。目前比较主流的做法有：
4.1)给每辆车使用带时间窗的单车路径规划算法(代表有dijkstra、a*及其相关变种)规划出时空最短路径。此类方法相对简单，速度快，使用时间窗来解决与其它车的冲突问题，但是考虑因素比较粗糙，通常采用设置优先级、规划次序等贪心算法思路来解决多车之间可能的复杂问题，在综合效率上不高，往往会为了一个小避让，而做一些绕远路的多余作业；
5.2)近年来兴起的多车路径共同规划的方法，即在车辆规划路径时，同时考虑其它车辆路径的可能变动，在多车博弈的过程中，寻找较优的组合。此类方法中比较有代表性的就是基于冲突的路径规划方法(cbs)，其思路是关注车辆之间的冲突(两辆或者两辆以上的车在同一时间通过同一区域)，按照时间排序依次处理。每次处理冲突时，分别尝试让冲突的一方采用基于时间窗的路径规划在时间和空间上进行避让，每一种避让产生一个新的局面，对每个局面按照后续需要处理的冲突数进行打分，从而经过迭代向着冲突数变少、且冲突发生在未来的方向上收敛，最终找到完全无冲突的路径。该方法能够找到较好的综合近似最优路径，但是存在两个缺点：一是相似的局面太多，导致搜索效率不高(典型的案例如图2所示，同类型的局面太多)；二是未能区分冲突解决的代价，即前面的决策虽然让冲突数减少了，但是导致解决剩余冲突付出的代价太大的尴尬局面(如图3所示，1号车先行会以最小的代价解决冲突a，b，但是会导致解决c付出极大代价)。虽然有人提出过一些基于先验知识的改进方法，但是要么局限于某一个应用场景，要么效果有限。


技术实现要素：

6.针对现有技术中存在的上述缺陷，本发明的目的是提供一种基于冲突的多车路径
规划优化方法，能够有效避免现有技术中的两大问题，具备适应性广，检索效率高的特性。
7.为实现上述目的，本发明采用如下技术方案：
8.一种基于冲突的多车路径规划优化方法：
9.在车辆运动之前，放松搜索期望，生成可行路径集合，对可行路径集合做持续优化，并持续优化至车辆运行阶段。
10.较佳的，所述放松搜索期望进一步包括：
11.使用dijkstra算法从终点开始向起点检索，记录各个节点到达终点的距离和父节点关系，在后续检索时直接查询任意节点到终点的最短路径。
12.较佳的，所述生成可行路径集合进一步包括：
13.s1、对每辆车单独生成空间最短路径，然后估算其各个节点的时间窗；
14.s2、按优先级将下一条路径加入到时空冲突图，两车通过同一个节点时，按照先来先走的方式，构建时间约束；
15.s3、判断步骤s2是否可行，若是，则结束，若否，则进入步骤s4；
16.s4、将新加入的车辆取消路径，并加入到待重新规划列表；
17.s5、判断是否存在未加入冲突的路径，若是，则返回步骤s2，若否，则进入步骤s6；
18.s6、对每一辆待重新规划的车辆实施可行路径规划。
19.较佳的，所述步骤s1中，单个车辆可行路径规划进一步包括：
20.s101、将起点和其它路径组成的有向连通图g、冲突树打包成搜索节点，并以当前最短路径作为权重保存到权重优先列表open_list；
21.s102、判断权重优先列表open_list是否为空，若是，则说明路径不存在，结束，若否，则进入步骤s103；
22.s103、选取权重优先列表open_list中最佳搜索节点，取其中路径节点c，查询c到终点构成的最短路径r2；
23.s104、尝试将最短路径r2加入到有向连通图g中；
24.s105、判断是否存在强联通分量，若是，则进入步骤s106，若否，则将起点到路径节点c的最短路径r1和路径节点c到终点构成的r2组合成最佳路径，并加入到有向连通图g中，生成新的有向连通图g；
25.s106、判断是否取到路径节点c的后续空间节点n，若是，则进入步骤s107，若否，则将当前搜索节点存入已检索列表close_list中；
26.s107、检查已检索列表close_list中是否已经存在，若存在，则进入步骤s108，若不存在，则返回步骤s106；
27.s108、继续判断是否存在权重优先列表open_list中，若存在，则松弛重复节点，再返回步骤s106，若不存在，则进入步骤s109；
28.s109、起点到后续空间节点n的路径为r1，并尝试加入到有向连通图g中；
29.s110、判断是否存在强联通分量，若是，则将强联通分量中关联的车辆加入驱赶树，若否，则进入步骤s111；
30.s111、判断是否存在驱赶无路径车辆，若是，则进入步骤s112，若否，则将新的后续空间节点n和有向连通图g、驱赶树组成新的搜索节点，并以后续空间节点n的最短路径计算权重，加入权重优先列表open_list，再返回步骤s106；
31.s112、将无路径车辆加入驱赶树；
32.s113、判断驱赶树是否有环冲突，若是，则返回步骤s106，若否，则进入步骤s114；
33.s114、被驱赶车辆递归使用本路径规划方法避让；
34.s115、判断是否避让成功，若是，则进入步骤s116，若否，则返回步骤s106；
35.s116、计算比如代价，将新的路径有向连通图g、驱赶树打包成新的搜索节点，以后续空间节点n最短路径+避让代价计算权重，加入权重优先列表open_list。
36.较佳的，所述步骤s6中，若车辆的当前路径与期望最短路径有相差，则执行以下步骤：
37.s601、将一个可行路径集在时间维上分割成固定的time_step，从而构成离散化的时空三维图；
38.s602、判断是否有需要优化的路径，若是，则进入步骤s603，若否，则结束；
39.s603、选择任意需要优化的车辆；
40.s604、在不改变其它车辆时空路径的前提下，在时空三维图上规划路径；
41.s605、判断是否存在更优路径，若存在，则更新路径，若不存在，则返回步骤s602。
42.本发明所提供的一种基于冲突的多车路径规划优化方法，还具有以下几点有益效果：
43.1)总体上采用前期规划多车可行路、执行期再各自优化的方式，缓解了前期求解的计算压力；
44.2)在求解多车路径可行解集阶段，尽可能多地保留空间最短路径不变，在时间维度上解决冲突，既保证了基础解集的质量，也避免了不必要的计算；
45.3)对于必须重新规划可行路径的任务，创造性地引入了冲突图的概念，保证新规划的路径是可行的，且能保证路径规划效率；
46.4)在最坏情况下(需要多车协作)，引入驱赶树概念，保证此时可以通过驱赶车辆的方式实现多车协同，规划出可行解集；
47.4)相比业内方法来说，适应性强(不需要依赖地图布局、业务知识等先验条件)，效率高(避免对称性重复检索，且前期使用可行解代替近似最优解)、未丢失可行解(最坏情况下依然有解)、未丢失近似最优解(执行期再优化)。
附图说明
48.图1是现有仓储多车工作场景的示意图；
49.图2是现有cbs相似局面的示意图；
50.图3是现有前后冲突互相影响的示意图；
51.图4是现有反向检索最短路径的示意图；
52.图5是本发明多车路径规划优化方法中a*路径检索的示意图；
53.图6是本发明多车路径规划优化方法的示例效果示意图；
54.图7是本发明多车路径规划优化方法中时空路径的示意图；
55.图8是本发明多车路径规划优化方法中多个时空路径关系的示意图；
56.图9是本发明多车路径规划优化方法中车辆驱赶关系的示意图；
57.图10是本发明多车路径规划优化方法的流程示意图；
58.图11是本发明多车路径规划优化方法中生成可行性路径集的流程示意图；
59.图12是本发明多车路径规划优化方法中车辆避让路径规划的流程示意图；
60.图13是本发明多车路径规划优化方法中优化路径集合的流程示意图。
具体实施方式
61.为了能更好地理解本发明的上述技术方案，下面结合附图和实施例进一步说明本发明的技术方案。
62.结合图10所示，本发明所提供的一种基于冲突的多车路径规划优化方法，分为二个阶段：
63.第一阶段在车辆运动之前，放松搜索期望，生成可行路径集合，第二阶段对可行路径集合做持续优化，寻找pareto最优，并持续优化至车辆运行阶段。
64.在第一阶段中，不再是关注最早的冲突，而是关注会导致死锁的冲突环域。具体来说就是把解决冲突的方式在时间和空间上隔离，相当于尽可能保留最短路径空间不变，通过在时间上等待来避开冲突，这样检索空间会大大缩小。如果只在时间维度上无法调整满足要求，则说明需要产生驱赶动作，通过驱赶关系树进行递归避让尝试，这样只有最坏情况下才需要进行扩大搜索范围，就避免图2案例中的大量无效局面计算。
65.而且在规划路径的过程中，迭代优化方向也不是完全按照冲突变少的方向，而是首先考虑存在的冲突会不会构成死锁环域。这样就是对有关联关系的冲突进行整体考虑，相当于对冲突的重要性即解决代价进行了刻画。这样就避免解决了前面冲突导致后续路径冲突更严重，或者解决后续冲突需要对前面已解决的冲突进行回溯调整的尴尬状况，如图3所示案例。
66.4.在寻求可行路径集的过程中，不可避免会引入过多等待的情况(因为使用尽可能不改变路径空间，而在时间维度上解决冲突的原则)，在完成可行性路径集合求解之后，在保证其他车辆路径不会变的更坏的情况下，再使用基于时间窗的路径规划优化自身的路径，即寻求pareto最优路径。多个车辆可以迭代、并发尝试，并且这一过程可以在行走的过程中进行，并不影响整体路径规划的效率，从而通过延后优化的方式，缓解了计算压力。
67.如图6所示，1车是自动导引车，2车是叉车，而a位置是叉车专用作业位，1车不可通行。这种冲突工况(如图6的(a)所示)下，采用时间窗按次序尝试，会规划出绕远路方案(如图6的(b)所示)。而采用cbs路径规划，则要么采用先验约束查找出绕远路方案，要么大量计算各种冲突局面后，才能规划合理方案。而本发明则可以通过2号车规划路径时，驱赶1号车两步即可算出优化方案(如图6的(c)所示)。
68.放松搜索期望进一步包括：
69.对于每一个搬运任务而言，先在不考虑其它车辆的情况下，使用dijkstra算法从终点开始向起点检索，记录各个节点到达终点的距离和父节点关系，搜索的节点空间只要扩大到最优路径的130％即可(可调)，即扩大一些搜索节点备用，在后续检索时直接查询任意节点到终点的最短路径。如图4所示，从终点五角星向起点圆圈规划，并保存最短距离在节点左上角，箭头则指向父节点。
70.在后续调整规划路径时，采用a*算法从起点向终点规划，而上述结果作为到终点的估算。如图5所示，从起点圆圈向终点五角星规划，距离起点的最短距离保存在节点右下
角，图上分别指向起点和终点的父节点关系省略未标注。
71.多个时空路径是否可行的判别：如图7所示，两条路径相向而行，实线曲线表示车辆起点约束(已经在节点上的车辆必须先占用)，虚线表示可以调整的约束通行次序，但是本图上无论上哪条虚线都会导致环，对应图上的l1和l2两个环。这就表示这两条路径是不可通行的，反之如果所有经过同一个节点的车辆都可以找到一个次序，使得这个路径不构成环，则表示可以路径集可以通行。
72.如果车辆发生驱赶，则驱赶关系构成一个树形关系如图9所示，a车驱赶b车，b车驱赶c车，则被驱赶车辆不可以再驱赶源车辆，即不可以成环，如图9中虚线所示。
73.如图11所示，生成可行路径集合，先对每辆车规划出空间最短路径，然后估算每个节点的时间窗，这一过程都是不考虑其它车辆位置和路径的，所以可以并行处理。然后按照优先级次序依次把路径加入到冲突图(如图8所示)，其中通过同一个空间节点的车辆默认按照先来先行的次序添加时间约束，如果新添加的路径导致冲突环(使用图论中的tarjan算法计算强连通分量，存在则有冲突环，无强连通分量则无冲突环)，则取消其路径，并加入到待重新规划的车辆列表。直到所有车辆处理完毕(这一步主要目的是使用贪心思路，先缩减规模)，最后对每一辆需要重新规划路径的车辆依次执行新的可行路径规划。具体步骤如下：
74.s1、对每辆车单独生成空间最短路径，然后估算其各个节点的时间窗；
75.s2、按优先级将下一条路径加入到时空冲突图，两车通过同一个节点时，按照先来先走的方式，构建时间约束；
76.s3、判断步骤s2是否可行，若是，则结束，若否，则进入步骤s4；
77.s4、将新加入的车辆取消路径，并加入到待重新规划列表；
78.s5、判断是否存在未加入冲突的路径，若是，则返回步骤s2，若否，则进入步骤s6；
79.s6、对每一辆待重新规划的车辆实施可行路径规划。
80.单个车辆可行路径规划，如图12所示，本方法框架是a*，但是搜索节点不仅考虑时空节点，还需要考虑与其它路径构成的冲突图、以及与其它车辆产生的驱赶关系树，其它路径权重的计算方式之类的正如图5所示。具体步骤如下：
81.s101、将起点和其它路径组成的有向连通图g、冲突树打包成搜索节点，并以当前最短路径作为权重保存到权重优先列表open_list；
82.s102、判断权重优先列表open_list是否为空，若是，则说明路径不存在，结束，若否，则进入步骤s103；
83.s103、选取权重优先列表open_list中最佳搜索节点，取其中路径节点c，查询c到终点构成的最短路径r2；
84.s104、尝试将最短路径r2加入到有向连通图g中；
85.s105、判断是否存在强联通分量，若是，则进入步骤s106，若否，则将起点到路径节点c的最短路径r1和路径节点c到终点构成的r2组合成最佳路径，并加入到有向连通图g中，生成新的有向连通图g；
86.s106、判断是否取到路径节点c的后续空间节点n，若是，则进入步骤s107，若否，则将当前搜索节点存入已检索列表close_list中；
87.s107、检查已检索列表close_list中是否已经存在，若存在，则进入步骤s108，若
不存在，则返回步骤s106；
88.s108、继续判断是否存在权重优先列表open_list中，若存在，则松弛重复节点，再返回步骤s106，若不存在，则进入步骤s109；
89.s109、起点到后续空间节点n的路径为r1，并尝试加入到有向连通图g中；
90.s110、判断是否存在强联通分量，若是，则将强联通分量中关联的车辆加入驱赶树，若否，则进入步骤s111；
91.s111、判断是否存在驱赶无路径车辆，若是，则进入步骤s112，若否，则将新的后续空间节点n和有向连通图g、驱赶树组成新的搜索节点，并以后续空间节点n的最短路径计算权重，加入权重优先列表open_list，再返回步骤s106；
92.s112、将无路径车辆加入驱赶树；
93.s113、判断驱赶树是否有环冲突，若是，则返回步骤s106，若否，则进入步骤s114；
94.s114、被驱赶车辆递归使用本路径规划方法避让；
95.s115、判断是否避让成功，若是，则进入步骤s116，若否，则返回步骤s106；
96.s116、计算比如代价，将新的路径有向连通图g、驱赶树打包成新的搜索节点，以后续空间节点n最短路径+避让代价计算权重，加入权重优先列表open_list。
97.上述步骤中，需要注意的是：
98.1)搜索节点是以空间节点为基础，附加了其它路径构成的冲突图、驱赶树信息，只有冲突图和驱赶树完全相同的情况下，才会去判定空间节点是否相同，即做松弛或者标注已搜索即加入到close_list；
99.2)计算节点权重时，除了到起点的距离、终点的距离外，还需要计算驱赶树中所有车辆(即被本车驱赶)被驱赶导致的额外消耗；
100.3)被驱赶车辆如果本来没有任务，则直接规划到可行点，且能完成避让即可；如果本来就有任务，则规划路径时，需要避让的情况下继续规划到终点，然后额外消耗为新路径与原路径的消耗差值；
101.4)如果场地限制比较大，确实存在复杂的驱赶关系，可能会导致搜索时间过长的现象，可以采用限制驱赶树深度的方式，减少过于复杂的路径。
102.在生成所有车辆的可行性路径集合之后，此时所有经过同一个空间节点的车辆都有了确定的通行次序，也就是通行的过程已经有了个可行的方案。但是该方案是存在优化空间的，比如：
103.1)实际通行时，会发生车辆没有预期到达，或者提前到达的现象，只要修改次序不会在多车路径冲突图上发生环状冲突，则允许变更。
104.2)如图13所示，如果车辆发现当前路径与期望最短路径相差较大，则可以在不改其它路径的影响下，重新规划路径，做时间和空间上的优化(生成可行解集的时候，优先使用了时间上的等待避让手法，可能忽略了更优的空间避让方法)。注意这里使用的是时空地图(初始规划空间最短路径时，采用的是空间地图，不考虑时间维度)，即在二维空间地图上，扩展了一个时间维度，并将其分割成固定的时间段，每一段为一个time_step，则时间窗由一个或者多个连续的time_step组成。从而构建在三维时空上构建有向连通图，路径搜索则直接使用普通的a*即可。具体步骤如下：
105.s601、将一个可行路径集在时间维上分割成固定的time_step，从而构成离散化的
时空三维图；
106.s602、判断是否有需要优化的路径，若是，则进入步骤s603，若否，则结束；
107.s603、选择任意需要优化的车辆；
108.s604、在不改变其它车辆时空路径的前提下，在时空三维图上规划路径；
109.s605、判断是否存在更优路径，若存在，则更新路径，若不存在，则返回步骤s602。
110.本技术领域中的普通技术人员应当认识到，以上的实施例仅是用来说明本发明，而并非用作为对本发明的限定，只要在本发明的实质精神范围内，对以上所述实施例的变化、变型都将落在本发明的权利要求书范围内。

技术特征：
1.一种基于冲突的多车路径规划优化方法，其特征在于：在车辆运动之前，放松搜索期望，生成可行路径集合，对可行路径集合做持续优化，并持续优化至车辆运行阶段。2.根据权利要求1所述的基于冲突的多车路径规划优化方法，其特征在于，所述放松搜索期望进一步包括：使用dijkstra算法从终点开始向起点检索，记录各个节点到达终点的距离和父节点关系，在后续检索时直接查询任意节点到终点的最短路径。3.根据权利要求2所述的基于冲突的多车路径规划优化方法，其特征在于，所述生成可行路径集合进一步包括：s1、对每辆车单独生成空间最短路径，然后估算其各个节点的时间窗；s2、按优先级将下一条路径加入到时空冲突图，两车通过同一个节点时，按照先来先走的方式，构建时间约束；s3、判断步骤s2是否可行，若是，则结束，若否，则进入步骤s4；s4、将新加入的车辆取消路径，并加入到待重新规划列表；s5、判断是否存在未加入冲突的路径，若是，则返回步骤s2，若否，则进入步骤s6；s6、对每一辆待重新规划的车辆实施可行路径规划。4.根据权利要求3所述的基于冲突的多车路径规划优化方法，其特征在于，所述步骤s1中，单个车辆可行路径规划进一步包括：s101、将起点和其它路径组成的有向连通图g、冲突树打包成搜索节点，并以当前最短路径作为权重保存到权重优先列表open_list；s102、判断权重优先列表open_list是否为空，若是，则说明路径不存在，结束，若否，则进入步骤s103；s103、选取权重优先列表open_list中最佳搜索节点，取其中路径节点c，查询c到终点构成的最短路径r2；s104、尝试将最短路径r2加入到有向连通图g中；s105、判断是否存在强联通分量，若是，则进入步骤s106，若否，则将起点到路径节点c的最短路径r1和路径节点c到终点构成的r2组合成最佳路径，并加入到有向连通图g中，生成新的有向连通图g；s106、判断是否取到路径节点c的后续空间节点n，若是，则进入步骤s107，若否，则将当前搜索节点存入已检索列表close_list中；s107、检查已检索列表close_list中是否已经存在，若存在，则进入步骤s108，若不存在，则返回步骤s106；s108、继续判断是否存在权重优先列表open_list中，若存在，则松弛重复节点，再返回步骤s106，若不存在，则进入步骤s109；s109、起点到后续空间节点n的路径为r1，并尝试加入到有向连通图g中；s110、判断是否存在强联通分量，若是，则将强联通分量中关联的车辆加入驱赶树，若否，则进入步骤s111；s111、判断是否存在驱赶无路径车辆，若是，则进入步骤s112，若否，则将新的后续空间节点n和有向连通图g、驱赶树组成新的搜索节点，并以后续空间节点n的最短路径计算权
重，加入权重优先列表open_list，再返回步骤s106；s112、将无路径车辆加入驱赶树；s113、判断驱赶树是否有环冲突，若是，则返回步骤s106，若否，则进入步骤s114；s114、被驱赶车辆递归使用本路径规划方法避让；s115、判断是否避让成功，若是，则进入步骤s116，若否，则返回步骤s106；s116、计算比如代价，将新的路径有向连通图g、驱赶树打包成新的搜索节点，以后续空间节点n最短路径+避让代价计算权重，加入权重优先列表open_list。5.根据权利要求3所述的基于冲突的多车路径规划优化方法，其特征在于，所述步骤s6中，若车辆的当前路径与期望最短路径有相差，则执行以下步骤：s601、将一个可行路径集在时间维上分割成固定的time_step，从而构成离散化的时空三维图；s602、判断是否有需要优化的路径，若是，则进入步骤s603，若否，则结束；s603、选择任意需要优化的车辆；s604、在不改变其它车辆时空路径的前提下，在时空三维图上规划路径；s605、判断是否存在更优路径，若存在，则更新路径，若不存在，则返回步骤s602。

技术总结
本发明公开了一种基于冲突的多车路径规划优化方法，在车辆运动之前，放松搜索期望，生成可行路径集合，对可行路径集合做持续优化，并持续优化至车辆运行阶段。本发明能够有效避免现有技术中的两大问题，具备适应性广，检索效率高的特性。效率高的特性。效率高的特性。


技术研发人员：王小进 金鑫 陈波
受保护的技术使用者：上海振华重工（集团）股份有限公司
技术研发日：2022.12.29
技术公布日：2023/9/22