基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法与流程

1.本发明用于导航或多路径规划的搜索方法，该方法基于迪杰斯特拉算法实施时间窗防冲突判断与决策，属于自动化控制领域。


背景技术：

2.随着科学技术的快速发展和产业结构的优化，目前国内制造业的自动化管理水平越来越高，大幅带动了移动智能体在各领域的使用，相关领域包括自动泊车，仓储物流，码头装卸，户外巡检等，涉及这些领域的路径规划设计环节正成为当前技术研究热点。
3.迪杰斯特拉算法是求解单源最短路径算法中最成熟的算法之一，具体地从一个顶点到其余各顶点最短路径进行搜索计算，以求得有向图中的最短路径。该算法的主要特点是以起始点为中心向外层层扩展，直到扩展到终点为止，能够很好的适应网络拓扑的变化，性能稳定，因而在移动智能体的路径规划，计算机网络拓扑路径选择以及gis中应用广泛。
4.但不可否认的是，迪杰斯特拉算法在应用到多条路径规划时，其规划结果只能给出各任务的距离最短路径，在算法执行过程中，并没有将各路径任务可能存在的冲突考虑在内，因此无法去除多路径规划结果中包含的冲突节点或区间，搜索路径能力尚需进一步优化。
5.有鉴于此，特提出本专利申请。


技术实现要素：

6.本技术所述的基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，在于解决上述现有技术存在的问题而提出在实施迪杰斯特拉算法中进行时间窗防冲突判断与决策，以期解决多条路径任务规划时可能存在的冲突问题，从而实现路径搜索结果的优化设计。
7.为实现上述设计目的，所述基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，包括下述实施步骤：
8.步骤1)、构建运动场景模型；
9.步骤2)、创建路径任务列表并初始化参数，包括状态与位置信息；
10.步骤3)、采用优先级策略对处于空闲状态的移动智能体分配任务；
11.步骤4)、规划任务列表中优先级最高的任务路径并生成该路径的时间窗；
12.步骤5)、规划任务列表中其它路径的任务时，每规划出一条路径即生成该路径的时间窗；规划出的当前路径与之前已经规划完成的所有路径任务进行时间窗冲突判断；
13.步骤6)、若存在时间窗冲突，则根据冲突节点更改描述运动场景模型的邻接矩阵，以得到无冲突的路径。
14.进一步地，所述的步骤1)包括以下过程：
15.1-1)、根据移动智能体(如agv小车)的运动场景构建一个强连通图，在图中给定任务，规划出至少一条从起点到终点的、能够连通的路径；
16.1-2)、引入图论思想中的邻接矩阵以描述上述强连通图，如在矩阵中第i行第j列
元素表示顶点i和顶点j的连接关系；
17.若顶点i可由拓扑结构图某一条边直接到达顶点j时，矩阵第i行第j列的元素为拓扑结构图中两顶点之间的距离；否则，为不相邻节点，元素为无穷大；
18.1-3)、在运动场景中设定以下规则；
19.①
当移动智能体在移动时，将其视为质点而忽略其形状及大小；
20.②
强连通图的每条边为单行道，但可双向通行；节点处作为路径点时，忽略其长度；
21.③
强连通图中的权值代表两节点之间边的距离，且同一时间区间内的每条边只允许被一个移动智能体占用；
22.④
移动智能体始终保持匀速移动状态、忽略其加减速或转弯所耗费的时间。
23.进一步地，所述的步骤4)包括以下过程：
24.4-1)、利用邻接矩阵中各节点的距离数据，在运用迪杰斯特拉算法规划出每条路径时计算得出到达路径各节点的时间时刻，以生成该路径的时间窗；
25.4-2)、生成的路径时间窗模型如下，
26.twn＝[tk,t
k+1
]k＝1,2k end
[0027][0028]
在上式中，twn为第n个任务机器人路径的时间窗；tk，t
k+1
为该任务机器人到达路径中第k个节点和第k+1个节点的实时标签；d
k(k+1)
为该任务机器人路径中第k个节点和第k+1个节点之间的距离；v0为移动体的移动速度。
[0029]
进一步地，所述的步骤5)包括以下过程：
[0030]
5-1)、在对多条路径任务进行规划时，每规划出一条路径生成该路径的时间窗，判断规划出的当前路径与之前已经规划完成的所有路径任务是否存在相同的路径节点或路径区间；若存在，则判断路径时间窗是否存在冲突；
[0031]
5-2)、判断存在路径冲突的条件是多条路径在同一时间点共同占用了同一位置点，若满足以下条件则判断不存在路径冲突；
[0032]
若存在相同节点i，
[0033]
to(i)≠t
n(i)[0034]
若存在相同的i-j路径区间，
[0035]
startn＞endo||endn＜starto[0036]
在上式中，to(i)为规划完成的路径任务中i节点的占用时刻；
[0037]
tn(i)为当前所规划任务路径在i节点的占用时刻；
[0038]
starto(i,j)为规划完成的任务路径中i-j路径区间的占用开始时刻；
[0039]
startn(i,j)为当前所规划任务路径在i-j路径区间的占用开始时刻；
[0040]
endo(i,j)为规划完成的任务路径中i-j路径区间的占用结束时刻；
[0041]
endn(i,j)为当前所规划任务路径在i-j路径区间的占用结束时刻。
[0042]
进一步地，所述的步骤6)包括以下过程：
[0043]
6-1)、在规划路径过程中，若根据时间窗的冲突判断发现有冲突节点，则记录该节点，在该路径任务规划中将该节点作为不可达节点处理，其实现方式为将邻接矩阵的相应
元素更改为无穷大；
[0044]
6-2)、在某个路径任务的规划中，作为不可达节点的处理逐次累加，直到规划出该路径任务与之前规划完成的路径不存在冲突；
[0045]
6-3)、在规划完成某个路径任务时，释放所有不可达节点，为规划新的路径任务做准备。
[0046]
如上内容，本技术具有的优点与有益效果是，在迪杰斯特拉算法的基础上引入各路径任务的时间窗冲突判断与决策，通过冲突存在与否的计算分析有效地解决各路径间存在的节点类型和区间类型冲突，从而能够得出多路径之间相互无冲突的最短路径结果，显著地提高路径规划优化能力。
附图说明
[0047]
现结合以下附图来进一步地说明本技术。
[0048]
图1为根据运动场景构建的8节点强连通示意图；
[0049]
图2为现有技术迪杰斯特拉算法规划的3个路径任务时间窗；
[0050]
图3为应用本技术所述搜索方法规划得出的3个路径任务时间窗；
[0051]
图4为本技术所述基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法的流程图；
具体实施方式
[0052]
实施例1，如图1至图4所示，本技术提出一种基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，引入了图论思想中的邻接矩阵以描述路网结构，在路径任务列表中选取优先级别最高的路径任务，运用迪杰斯特拉算法规划出该任务的路径并为该路径所要到达的各节点打上实时标签，以生成该路径的时间窗；选取路径任务列表中优先级别次高的路径任务，运用迪杰斯特拉算法规划出该任务的路径，并生成该路径的时间窗，生成的时间窗与已经存在的路径时间窗做冲突判断，若存在冲突，更改描述路网结构的邻接矩阵，重新运用迪杰斯特拉算法规划该任务路径，直到规划出与存在的路径任务无冲突的路径。
[0053]
该方法包括以下实施步骤：
[0054]
步骤1)、构建运动场景模型；
[0055]
1-1)、根据移动智能体(如agv小车)的运动场景构建一个强连通图，在图中给定任务，任务内容包含起点和终点，规划出至少一条从起点到终点的、能够连通的路径；
[0056]
1-2)、引入图论思想中的邻接矩阵以描述上述强连通图，如在矩阵中第i行第j列元素表示顶点i和顶点j的连接关系；
[0057]
若顶点i可由拓扑结构图某一条边直接到达顶点j时，矩阵第i行第j列的元素为拓扑结构图中两顶点之间的距离；否则，为不相邻节点，元素为无穷大；
[0058]
1-3)、在运动场景中设定以下规则；
[0059]
①
当移动智能体在移动时，将其视为质点而忽略其形状及大小；
[0060]
②
强连通图的每条边为单行道，但可双向通行；节点处作为路径点时，忽略其长度；
[0061]
③
强连通图中的权值代表两节点之间边的距离，且同一时间区间内的每条边只允
许被一个移动智能体占用；
[0062]
④
移动智能体始终保持匀速移动状态、忽略其加减速或转弯所耗费的时间；
[0063]
步骤2)、创建路径任务列表并初始化参数，包括各移动智能体的状态、位置信息；
[0064]
步骤3)、采用优先级策略对处于空闲状态的移动智能体分配任务；
[0065]
步骤4)、规划任务列表中优先级最高的任务路径并生成该路径的时间窗；
[0066]
4-1)、利用邻接矩阵中各节点的距离数据，在运用迪杰斯特拉算法规划出每条路径时计算得出到达路径各节点的时间时刻，以生成该路径的时间窗；
[0067]
4-2)、生成的路径时间窗模型如下，
[0068]
twn＝[tk,t
k+1
]k＝1,2k end
[0069][0070]
在上式中，twn为第n个任务机器人路径的时间窗；tk，t
k+1
为该任务机器人到达路径中第k个节点和第k+1个节点的实时标签；d
k(k+1)
为该任务机器人路径中第k个节点和第k+1个节点之间的距离；v0为移动体的移动速度；
[0071]
步骤5)、规划任务列表中其它路径的任务时，每规划出一条路径即生成该路径的时间窗；规划出的当前路径与之前已经规划完成的所有路径任务进行时间窗冲突判断；
[0072]
5-1)、在对多条路径任务进行规划时，每规划出一条路径生成该路径的时间窗，判断规划出的当前路径与之前已经规划完成的所有路径任务是否存在相同的路径节点或路径区间；若存在，则判断路径时间窗是否存在冲突；
[0073]
5-2)、判断存在路径冲突的条件是多条路径在同一时间点共同占用了同一位置点，若满足以下条件则判断不存在路径冲突；
[0074]
若存在相同节点i，
[0075]
to(i)≠t
n(i)[0076]
若存在相同的i-j路径区间，
[0077]
startn＞endo||endn＜starto[0078]
在上式中，to(i)为规划完成的路径任务中i节点的占用时刻；
[0079]
tn(i)为当前所规划任务路径在i节点的占用时刻；
[0080]
starto(i,j)为规划完成的任务路径中i-j路径区间的占用开始时刻；
[0081]
startn(i,j)为当前所规划任务路径在i-j路径区间的占用开始时刻；
[0082]
endo(i,j)为规划完成的任务路径中i-j路径区间的占用结束时刻；
[0083]
endn(i,j)为当前所规划任务路径在i-j路径区间的占用结束时刻；
[0084]
步骤6)、若存在时间窗冲突，则根据冲突节点更改描述运动场景模型的邻接矩阵，以得到无冲突的路径；
[0085]
6-1)、在规划路径过程中，若根据时间窗的冲突判断发现有冲突节点，则记录该节点，在该路径任务规划中将该节点作为不可达节点处理，其实现方式为将邻接矩阵的相应元素更改为无穷大；
[0086]
6-2)、在某个路径任务的规划中，作为不可达节点的处理逐次累加，直到规划出该路径任务与之前规划完成的路径不存在冲突；
[0087]
6-3)、在规划完成某个路径任务时，释放所有不可达节点，为规划新的路径任务做
准备。
[0088]
基于上述多路径搜索方法的设计原理，本技术提出下述多路径规划的优选搜索实施例，本实施例并非是最优方案但能够充分地表达申请内容。
[0089]
步骤一、如图1所示的强连通图，构建以下邻接矩阵a0：
[0090][0091]
步骤二、构建路径任务列表并初始化下述参数：
[0092]
在t＝0s，t＝20s，t＝40s不同时刻依次生成三个路径任务task1，task2和task3，设定任务优先级task1》task2》task3；
[0093]
任务详情为：task1任务的起始节点为节点1，目标节点为节点8；
[0094]
task2任务的起始节点为节点5，目标节点为节点4；
[0095]
task3任务的起始节点为节点7，目标节点为节点2。
[0096]
其中，空闲的移动智能体有3个，分别为r1、r2和r3且停靠在节点1、节点5和节点7；
[0097]
步骤三、采用优先级策略对空闲的移动智能体分配以下任务；
[0098]
task1任务分配给智能体r1，task2任务分配给智能体r2，task3任务分配给智能体r3；
[0099]
步骤四、规划任务列表中优先级最高的任务路径并生成该路径的时间窗；
[0100]
选取任务优先级最高的路径任务task1，起点节点为节点1，目标节点为节点8；
[0101]
运用迪杰斯特拉算法生成该任务路径并生成时间窗，生成的时间窗如图2中task1任务路径时间窗所示；
[0102]
步骤五、选取任务优先级较低的任务task2，为其规划路径并生成时间窗；
[0103]
task2任务的起始节点是节点为节点5，目标节点为节点4；
[0104]
运用迪杰斯特拉算法生成该任务路径并生成时间窗，生成的时间窗如图2中task2任务路径时间窗所示；
[0105]
生成的task2任务路径时间窗与已经规划完成的task1任务路径时间窗做时间窗冲突判断；
[0106]
由于task2和task1的任务路径包含相同的节点3和节点6且都包含3-6节点区间，时间窗冲突判断发现task1任务路径的时间窗与task2任务路径的时间窗在40s-60s的时间区间内同时占用了3-6节点区间，说明task1与task2的任务路径在3-6节点区间内存在冲突；
[0107]
步骤六、根据冲突节点更改描述运动场景模型的邻接矩阵，得到无冲突的路径；
[0108]
在对task2任务路径时间窗冲突判断发现该路径在节点6到节点3区间内会与task1任务路径发生冲突，将节点3作为节点6不可达节点处理，更改邻接矩阵如a1：
[0109][0110]
在执行迪杰斯特拉算法过程中，将邻接矩阵a0中a
63
的值从原来的的40改为了∞，即6-3节点区间为禁行区间，以避免与task1任务路径发生的节点区间冲突；
[0111]
进一步地，在更改邻接矩阵的基础上，运用迪杰斯特拉算法重新规划该任务路径并生成如图3中task2任务路径时间窗；
[0112]
生成的该路径时间窗重新与task1任务路径时间窗做冲突判断，发现task2与task1任务路径只存在一个相同的节点3，但在节点3处时间窗未存在相互冲突；
[0113]
task2的任务路径规划完成，释放临时不可达节点3，恢复邻接矩阵a0；
[0114]
步骤七、选择优先级别最低的路径任务task3，为其规划路径并生成该路径的时间窗；
[0115]
task3任务的起始节点是节点为节点7，目标节点为节点2。运用迪杰斯特拉算法生成该任务路径并生成时间窗，生成的时间窗如图2中task3任务路径时间窗所示；
[0116]
生成的task3任务路径时间窗与已经规划完成的task1，task2任务路径时间窗做时间窗冲突判断；
[0117]
task3与task1任务路径都包含相同的节点6，与task2任务路径不存在相同的节点，发现task3任务路径的时间窗与task1任务路径的时间窗在第60s时刻同时占用了节点6，说明task3与task1的任务路径在节点6处存在冲突；
[0118]
步骤八、根据冲突节点更改描述运动场景模型的邻接矩阵，得到无冲突的路径；
[0119]
在对task3任务路径时间窗冲突判断发现该路径在节点6处会与task1任务路径发生冲突，将task3路径节点6作为节点7不可达节点处理，更改邻接矩阵如a2：
[0120][0121]
在对task3进行路径规划时，算法执行过程中将邻接矩阵a0中a
76
的值从原来的的20改为了∞，即节点6为不可达节点，以避免与task1任务路径发生的节点冲突；
[0122]
在更改邻接矩阵的基础上，运用迪杰斯特拉算法重新规划该任务路径并生成如图3所示的task3任务路径时间窗；
[0123]
生成的该路径时间窗重新与task1，task2任务路径时间窗做冲突判断，发现task3
与task1，task2任务路径时间窗未存在相互冲突；
[0124]
task3的任务路径规划完成，释放临时不可达节点3，恢复邻接矩阵a0。
[0125]
通过在迪杰斯特拉算法的基础上引入各路径任务的时间窗冲突判断与决策，能够判断出各路径间是否存在冲突，从而得出各路径之间不存在冲突的最短路径。
[0126]
如上内容，结合附图中给出的实施例仅是实现本发明目的的优选方案。对于所属领域技术人员来说可以据此得到启示，而直接推导出符合本发明设计构思的其他替代结构。由此得到的其他结构特征，也应属于本发明所述的方案范围。

技术特征：
1.一种基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，其特征在于：包括下述实施步骤，步骤1)、构建运动场景模型；步骤2)、创建路径任务列表并初始化参数，包括状态与位置信息；步骤3)、采用优先级策略对处于空闲状态的移动智能体分配任务；步骤4)、规划任务列表中优先级最高的任务路径并生成该路径的时间窗；步骤5)、规划任务列表中其它路径的任务时，每规划出一条路径即生成该路径的时间窗；规划出的当前路径与之前已经规划完成的所有路径任务进行时间窗冲突判断；步骤6)、若存在时间窗冲突，则根据冲突节点更改描述运动场景模型的邻接矩阵，以得到无冲突的路径。2.根据权利要求1所述的基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，其特征在于：所述的步骤1)包括以下过程，1-1)、根据移动智能体(如agv小车)的运动场景构建一个强连通图，在图中给定任务，规划出至少一条从起点到终点的、能够连通的路径；1-2)、引入图论思想中的邻接矩阵以描述上述强连通图，如在矩阵中第i行第j列元素表示顶点i和顶点j的连接关系；若顶点i可由拓扑结构图某一条边直接到达顶点j时，矩阵第i行第j列的元素为拓扑结构图中两顶点之间的距离；否则，为不相邻节点，元素为无穷大；1-3)、在运动场景中设定以下规则；
①
当移动智能体在移动时，将其视为质点而忽略其形状及大小；
②
强连通图的每条边为单行道，但可双向通行；节点处作为路径点时，忽略其长度；
③
强连通图中的权值代表两节点之间边的距离，且同一时间区间内的每条边只允许被一个移动智能体占用；
④
移动智能体始终保持匀速移动状态、忽略其加减速或转弯所耗费的时间。3.根据权利要求1所述的基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，其特征在于：所述的步骤4)包括以下过程，4-1)、利用邻接矩阵中各节点的距离数据，在运用迪杰斯特拉算法规划出每条路径时计算得出到达路径各节点的时间时刻，以生成该路径的时间窗；4-2)、生成的路径时间窗模型如下，tw
n
＝[t
k
,t
k+1
] k＝1,2kend在上式中，tw
n
为第n个任务机器人路径的时间窗；t
k
，t
k+1
为该任务机器人到达路径中第k个节点和第k+1个节点的实时标签；d
k(k+1)
为该任务机器人路径中第k个节点和第k+1个节点之间的距离；v0为移动体的移动速度。4.根据权利要求1所述的基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，其特征在于：所述的步骤5)包括以下过程，5-1)、在对多条路径任务进行规划时，每规划出一条路径生成该路径的时间窗，判断规划出的当前路径与之前已经规划完成的所有路径任务是否存在相同的路径节点或路径区
间；若存在，则判断路径时间窗是否存在冲突；5-2)、判断存在路径冲突的条件是多条路径在同一时间点共同占用了同一位置点，若满足以下条件则判断不存在路径冲突；若存在相同节点i，t
o
(i)≠t
n
(i)若存在相同的i-j路径区间，start
n
＞end
o
||end
n
＜start
o
在上式中，t
o
(i)为规划完成的路径任务中i节点的占用时刻；t
n
(i)为当前所规划任务路径在i节点的占用时刻；start
o
(i,j)为规划完成的任务路径中i-j路径区间的占用开始时刻；start
n
(i,j)为当前所规划任务路径在i-j路径区间的占用开始时刻；end
o
(i,j)为规划完成的任务路径中i-j路径区间的占用结束时刻；end
n
(i,j)为当前所规划任务路径在i-j路径区间的占用结束时刻。5.根据权利要求1所述的基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，其特征在于：所述的步骤6)包括以下过程，6-1)、在规划路径过程中，若根据时间窗的冲突判断发现有冲突节点，则记录该节点，在该路径任务规划中将该节点作为不可达节点处理，其实现方式为将邻接矩阵的相应元素更改为无穷大；6-2)、在某个路径任务的规划中，作为不可达节点的处理逐次累加，直到规划出该路径任务与之前规划完成的路径不存在冲突；6-3)、在规划完成某个路径任务时，释放所有不可达节点，为规划新的路径任务做准备。

技术总结
本申请提出基于迪杰斯特拉算法的时间窗防冲突多路径搜索方法，在迪杰斯特拉算法规划路径的基础上引入了多路径时间窗的冲突判断，可在规划路径的计算过程中有效地避免多路径任务规划时存在的冲突问题，同时保证了搜索路径最短。该方法包括下述实施步骤：步骤1)、构建运动场景模型；步骤2)、创建路径任务列表并初始化参数；步骤3)、采用优先级策略对处于空闲状态的移动智能体分配任务；步骤4)、规划任务列表中优先级最高的任务路径并生成该路径的时间窗；步骤5)、规划出的当前路径与之前已经规划完成的所有路径任务进行时间窗冲突判断；步骤6)、若存在时间窗冲突，则根据冲突节点更改描述运动场景模型的邻接矩阵以得到无冲突的路径。的路径。的路径。


技术研发人员：郝国笑 李云杰 李超 王振 李帅帅
受保护的技术使用者：青岛蚂蚁机器人有限责任公司
技术研发日：2023.05.04
技术公布日：2023/8/23