【路径规划】Dijkstra算法求解带时间窗规划的AGV小车路径规划【含Matlab源码 3540期】

最新推荐文章于 2024-04-26 15:08:56 发布

Matlab领域

最新推荐文章于 2024-04-26 15:08:56 发布

阅读量1.3k

点赞数 17

分类专栏： Matlab路径规划（初级版）文章标签： matlab

本文链接：https://blog.csdn.net/weixin_63266434/article/details/134758678

版权

Matlab路径规划（初级版）专栏收录该内容

111 篇文章 7 订阅

订阅专栏

在这里插入图片描述

⛄一、Dijkstra算法机器人编队路径规划简介

1 传统Dijkstra算法步骤
Dijkstra算法是一种经典的求解最短路径的算法，用于计算一个节点去往其他各个不相关节点的最小移动代价。本思想是把在图中无论如何出现的所有节点分离为2组，第1组包含被准确认定是最短路径的节点，第2组存放待检查的不确定的节点。根据最小移动代价逐渐增大的顺序，逐个将第2组需要检查的节点加入到第1组中，一直到从起始点出发可以到达的所有节点都包含在第1组中。Dijkstra算法运行的主要特征是以机器人抽象的出发点为中心向外层层延伸，直到延伸到整个区域的末端为止。Dijkstra算法一般能够在最终得到最优的路径，但是这个路径偶尔会出现冗余拐点，由于其遍历节点多，所以有时算法的效率不高。Dijkstra算法流程如图2所示。

分别创建2个表，为：START与FINISH。START表中存放环境中已经生成存在但是未经计算的节点，FINISH表中存放所有经过计算考察的节点。

(1）观测寻找在环境路网中离起始源点近而且是没有被计算过的点，把满足调节的点放在START数组中等待检查。

(2）从START表中找出距离起始点最小的点，然后把该点放进FINISH表中。
在这里插入图片描述
(3）遍历去寻找考察当前此点的子节点，计算出起始源点到这些子节点的距离值，将值存入数组中用来排序，将子节点放在START表中。

(4）重复（2）和（3），直到把START表中的点全部清空，或者找到真正要去的目标点。

2 Dijkstra算法的思想
(1）将环境可视图中出现的节点或是顶点分别加入2个组，其中一组存放经过求解验证为最短路径所经过的顶点，该集合用FINISH表示，刚开始时START表中只有1个起始点，在后面的运算过程中每得到1条最短路径，就把相关节点加入到集合FINISH中。如果全部的顶点都加入到FINISH后，此算法运算完成。另一组存放未经过考察且还未被认定为最短路径的顶点，用START表示。

(2）在将顶点逐渐加入到FINISH表的过程中，在运行算法期间总保持从出发点到FINISH表中各个点的最短路径的长度不超过出发点到START表中任何顶点的最短路径长度。

典型的Dijkstra算法在寻找最优路径的准则上较为单一，以某一个点为中心向外层层扩展就类似于画圆，随着搜索半径的逐渐增大，最后总能找到想要的目标点，但同时最优路径会出现一些无关的冗余点，这样在一定程度上给实际的移动机器人造成寻找路径的困难。如果能够对经典算法的计算准则进行优化，达到减少路径中拐点的目的，就可以减少一大部分机器人寻路所耗费的时间。在大面积环境中使用Dijkstra算法时，由于每次都要从START列表中挑选离原始点最近的点，这样导致规划效率不高，在排序过程消耗很多不必要的工作时间。

⛄二、部分源代码

close all;
clear;
clc;
map=[0 1 1 1 0 0 1 1 1 0 ;
0 0 0 0 0 0 0 0 0 0 ;
0 1 1 1 0 0 1 1 1 0 ;
0 1 1 1 0 0 1 1 1 0 ;
0 0 0 0 0 0 0 0 0 0 ;
0 0 0 0 0 0 0 0 0 0 ;
0 1 1 1 0 0 1 1 1 0 ;
0 1 1 1 0 0 1 1 1 0 ;
0 0 0 0 0 0 0 0 0 0 ;
0 1 1 1 0 0 1 1 1 0 ;];

SD1 = [10,21]; %AGV1起止点
SD2 = [41,25]; %AGV2起止点
L = 1000; %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); %得到环境地图

[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); %得到环境地图