Dijkstra算法结合时间窗规划无冲突路径

欢迎来到 < Haoh-Smile > 的博客，觉得受用客官就点个赞评论一下呗！

Dijkstra算法结合时间窗规划无冲突路径

前面的几篇文章简单的介绍了Dijkstra算法的原理及MATLAB实现，在这篇文章开始系统的将规划无冲突路径（2条路径为例）的代码整体的呈现，功能整体考虑，需要对之前几篇文章中的功能代码需要修修补补，在之后的几篇文章中会更新，这篇文章先给大家呈现一个大体框架，详细的各功能代码文与讲解会后续更新。
后续也会有多AGV的无冲突路径规划，请查阅专栏多AGV无冲突路径规划
整个功能流程图如下：

执行文件代码如下：

%%%%%%%%%%%%%%%%%%%%%%建立环境矩阵map及参数初始化%%%%%%%%%%%%%%%%%%%%%
clear;
clc;
map=[0 0 0 1 0 0 1 0 0 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 1 0 0 0 1 1 0 0;
     0 0 0 0 0 0 0 0 0 0;
     0 0 0 0 0 1 0 0 1 0;
     1 0 0 0 0 1 1 0 0 0;
     0 0 0 1 0 0 0 0 0 0;
     1 1 1 0 0 0 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;
     0 0 0 0 0 1 1 0 0 0;];

SD1 = [1,65];               %AGV1起止点
SD2 = [10,41];              %AGV2起止点
L = 100;                    %AGV2路径备用条数
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
          %以上是参数可更改区域，以下所有功能函数已封装%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[W,Q,I,C] = MapInit(map);                      %环境初始化
[L1,sp1,spcost1]=dijkstraR(W,SD1(1),SD1(2),1); %设置始末栅格及路径条数(1)
[X1,Y1]=Get_xy(spcost1,sp1,map);               %得到X,Y坐标
[T1,N1] = GetTrastion(X1,Y1,sp1);              %得到所有路径的转弯次数矩阵
[R1] = rank(T1);                               %对转弯矩阵从小到大排序
[P1]=GetPath(L1,spcost1,R1);                   %得到约束条数的路径
[TW1] = Get_TimerWindow(spcost1,sp1,P1,N1,T1); %得到每条路径时间窗矩阵
[Q,I]=plotTW(map,TW1,sp1,1,P1,Q,C,I);          %画出时间窗
plotMap_Path(map,spcost1,1,P1,X1,Y1,C,I);  %得到环境地图
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% AGV2路径 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
[L2,sp2, spcost2]=dijkstraR(W,SD2(1),SD2(2),L);%设置始末栅格及路径条数
[X2,Y2]=Get_xy(spcost2,sp2,map);               %得到X,Y坐标
[T2,N2] = GetTrastion(X2,Y2,sp2);              %得到所有路径的转弯次数矩阵
[R2] = rank(T2);                               %对转弯矩阵从小到大排序
[P2]=GetPath(L2,spcost2,R2);                   %得到约束条数的路径
[TW2] = Get_TimerWindow(spcost2,sp2,P2,N2,T2); %得到每条路径时间窗矩阵
[TW2,OUT,sp,P,spcos2,X2,Y2] = Detection_TW(map,SD2,P1,P2,sp1,sp2,spcost2,X2,Y2,TW1,TW2);
[Q,I]=plotTW(map,TW2,sp2,OUT,P2,Q,C,I);        %画出时间窗
plotMap_Path(map,spcost2,OUT,P2,X2,Y2,C,I);%得到环境地图 

该工程的所有功能函数会后续更新，MATLAB执行效果如下图：