全局路径规划算法Dijkstra(迪杰斯特拉算法)- matlab
参考博客链接:https://www.cnblogs.com/kex1n/p/4178782.html
Dijkstra是常用的全局路径规划算法,其本质上是一个最短路径寻优算法。算法的详细介绍参考上述链接。
本文与参考博文相比,主要有如下两个不同:
1、开发语言换成了matlab,代码部分稍作改动就可以实时运行在控制器上;
2、求取了从起点开始到达每一个顶点的最短路径所经历的顶点。
matlab代码:包含测试数据
%参考链接https://www.cnblogs.com/kex1n/p/4178782.html DefultMaxValue=1e3; NodesMap=[0 1 12 DefultMaxValue DefultMaxValue DefultMaxValue; DefultMaxValue 0 9 3 DefultMaxValue DefultMaxValue; DefultMaxValue DefultMaxValue 0 DefultMaxValue 5 DefultMaxValue; DefultMaxValue DefultMaxValue 4 0 13 15; DefultMaxValue DefultMaxValue DefultMaxValue DefultMaxValue 0 4; DefultMaxValue DefultMaxValue DefultMaxValue DefultMaxValue DefultMaxValue 0]; % NodesMap=[0 6 3 DefultMaxValue DefultMaxValue DefultMaxValue; % 6 0 2 5 DefultMaxValue DefultMaxValue; % 3 2 0 3 4 DefultMaxValue; % DefultMaxValue 5 3 0 2 3; % DefultMaxValue DefultMaxValue 4 2 0 5; % DefultMaxValue DefultMaxValue DefultMaxValue 3 5 0]; Distance=NodesMap(1,:); %book结果初始化 SearchedFlag=zeros(1,length(Distance)); SearchedFlag(1)=1; FootsResults=zeros(size(NodesMap)); FootsResults(:,1)=1; Preview=zeros(1,length(Distance)); Preview(1)=1; for i=2:length(SearchedFlag) i MinID=1; MinValue=DefultMaxValue; Distance SearchedFlag for j=1:length(SearchedFlag) if SearchedFlag(j)==0 && Distance(j)<MinValue MinID=j; MinValue=Distance(j); end end MinID SearchedFlag(MinID)=1; Preview if i>2 FootsResults(i,i-1)=Preview(MinID); for j=1:(i-1) Temp=FootsResults(j,:); FindFlag=0; for k=1:length(SearchedFlag) if Temp(k)==0 if Temp(k-1)==Preview(MinID) FindFlag=1; break; end end end if FindFlag==1 break; end end Temp FootsResults(i,:)=Temp; FootsResults(i,k)=MinID; else FootsResults(i,i)=MinID; end FootsResults for j=1:length(SearchedFlag) if NodesMap(MinID,j)<DefultMaxValue && SearchedFlag(j)==0 if Distance(j)>(Distance(MinID)+NodesMap(MinID,j)) Distance(j)=Distance(MinID)+NodesMap(MinID,j); % FootsResults(i,j)=MinID; Preview(j)=MinID; end end end end % Preview Distance
测试算例1 2所用数据都在代码中,大家可自行运行程序,查看结果。