Dijkstra法路径规划matlab

dijkstra法路径规划matlab

Dijkstra法路径规划matlab

前言
一、dijkstra算法是什么？
二、Dijkstra算法路径规划matlab程序
总结

前言

随着无人驾驶时代的来临，路径规划技术也越来越为人所熟知，本文简单介绍了Dijkstra算法，并给出了其matlab程序

一、dijkstra算法是什么？

Dijkstra’s algorithm，可译为迪杰斯特拉算法，亦可不音译而称为Dijkstra算法，是由荷兰计算机科学家Edsger W. Dijkstra 在1956年发现的算法，并于3年后在期刊上发表。Dijkstra算法使用类似广度优先搜索的方法解决赋权图的单源最短路径问题，但需要注意绝大多数的Dijkstra算法不能有效处理带有负权边的图。Dijkstra算法在计算机科学的人工智能等领域也被称为均一开销搜索，并被认为是最良优先搜索的一个特例。
具体运行步骤可参考数据结构–Dijkstra算法最清楚的讲解。

二、Dijkstra算法路径规划matlab程序

Dijkstara算法路径规划GitHub地址https://github.com/Say-Hello2y/Path-planning-for-robots
程序包括七个子程序，运行时一定要把matlab工作目录调到程序所在目录，运行时只需运行debug即可出来结果。
下面分别介绍各个程序的作用：
debug.m ：为运行程序，里面定义了起始点和终止点以及其他必要的准备，并将规划出的路线在栅格地图上画出来。

clear;
clc;
close all;
[map]=creMap()%Map采用的是按要求生成18-20个障碍物，每个障碍物占6-16格
start=[1 100]%起点的坐标
goal=[100 1]%终点的坐标
Dmap=G2D(map);%生成栅格地图的邻接矩阵
PlotGrid(map,start,goal)%画出二维栅格地图
[distance,route]=Dijkstras(Dmap,9901,100)%由算法得到最短距离和具体路先
[x y]=Get_xy(distance,route,map);%将具体路线用栅格坐标表示出来
Plot(distance,x,y)%在栅格图上画出具体路线

creMap.m ：创造地图矩阵

function [Map]=creMap()
%地图矩阵生成函数，生成的是100*100的地图矩阵
Map=ones(100)
for i=1:randi([19 21]) %障碍物个数
p=randi([1 90]);
q=randi([1 90]);
j1=randi([2 4]);
j2=randi([3 4]);
Map(p:p+j1,q:q+j2)=0;
end
end

G2D.m：由于Dijkstra为基于图的算法，此程序用于将栅格地图转换为邻接矩阵。

%%%%%%%%%%%%%%%%%%%%%%%%得到环境地图的邻接矩阵%%%%%%%%%%%%%%%%%%%%%%%%
function W=G2D(map)
l=size(map);
W=zeros(l(1)*l(2),l(1)*l(2));
for i=1:l(1)for j=1:l(2)if map(i,j)==1for m=1:l(1)for n=1:l(2)if map(m,n)==1im=abs(i-m);jn=abs(j-n);if im+jn==1||(im==1&&jn==1)W((i-1)*l(2)+j,(m-1)*l(2)+n)=(im+jn)^0.5;endendendendendend
endW(W==0)=Inf;     %不相邻栅格距离值为Inf（无穷大），将对角元素也设置为了Inf，在Dijkstra算法中使用无影响。
end

Get_xy.m: 由dijkstra算法生成的路径点和地图计算出具体的栅格地图路线。

%%%%%%%%%%%%%%%%%%%%%根据栅格标识，得到相应的x，y坐标%%%%%%%%%%%%%%%%%%%%%function [x y]=Get_xy(distance,path,map)
% distance 路径长度 若不可达路径不执行该函数程序
% path 路径节点向量
% map  栅格地图信息
if(distance~=Inf)                %判断路径是否可达n = size(map);N = size(path,2);for(i=1:N)     %根据栅格标识，得到相应的x，y坐标if rem(path(i),n(2))==0x(i)=n(2);y(i)= floor(path(i)/n(2));elsex(i)= rem(path(i),n(2));y(i)= floor(path(i)/n(2))+1;endend
elsex = 0;y = 0;
end
4

Plot.m :画出具体的路线

%%%%%%%%%%%%%%%%%%%%%绘制具体路径%%%%%%%%%%%%%%%%%%%%%
function  Plot(distance,x,y)
if(distance~=Inf)            %判断路径是否可达plot(x+0.5,y+0.5,'r');axis image xyhold on
elsedisp('路径不可达');hold on
end

Dijkstra.m:算法的核心，利用基于图的Dijkstra算法计算两点之间的最短路径，函数输入起始点，终止点以及邻接矩阵输出两点之间最短距离以及路径点，需要注意的是这里输入的起始点为转化成图后的点，在本算法中为按照列的顺序排列，及起始点在栅格地图中若为[1 100]则对应第9901个节点。

%%%%%%%%%%%%%%%%%%%%%dijkstraËã·¨ÊµÏÖ%%%%%%%%%%%%%%%%%%%%%
function [Cost, Route] = Dijkstras( Graph, SourceNode, TerminalNode )
%Dijkstras.m Given a graph with distances from node to node calculates the
%optimal route from the Source Node to the Terminal Node as defined by the
%inputs.
%
% Inputs:
%
% Graph - A graph of the costs from each node to each other also known as
% an adjacency matrix. In the context of this project it is a matrix of the
% distances from cities within North Carolina to each other. You can mark
% unconnected nodes as such by assigning them values of Inf. For instance,
% if nodes 3 & 5 are unconnected then Graph(3,5)=Graph(5,3)=Inf;
%
% SourceNode - The index of the node (city) which you wish to travel from
%
% TerminalNode - The index of the node (city) which you wish to travel to
%
% Outputs:
%
% Cost - The cost to travel from the source node to the terminal node.
% There are several metrics this could be but in the case of route planning
% which I am working on this is distance in miles.
%
% Route - A vector containing the indices of the nodes traversed when going
% from the source node to the terminal node. Always begins with the source
% node and will always end at the terminal node unless the algorithm fails
% to converge due to there not existing a valid path between the source
% node and the terminal node. % Check for valid parameters
if size(Graph,1) ~= size(Graph,2)fprintf('The Graph must be a square Matrix\n');return;
elseif min(min(Graph)) < 0fprintf('Dijkstras algorithm cannot handle negative costs.\n')fprintf('Please use Bellman-Ford or another alternative instead\n');return;
elseif SourceNode < 1 && (rem(SourceNode,1)==0) && (isreal(SourceNode)) && (SourceNode <= size(Graph,1))fprintf('The source node must be an integer within [1, sizeofGraph]\n');return;
elseif TerminalNode < 1 && (rem(TerminalNode,1)==0) && isreal(TerminalNode) && (TerminalNode <= size(Graph,1))fprintf('The terminal node must be an integer within [1, sizeofGraph]\n');return;
end% Special Case so no need to waste time doing initializations
if SourceNode == TerminalNodeCost = Graph(SourceNode, TerminalNode);Route = SourceNode;return;
end% Set up a cell structure so that I can store the optimal path from source
% node to each node in this structure. This structure stores the
% antecedents so for instance if there is a path to B through A-->C-->D-->B
% you will see [A,C,D] in cell{B} (as well as a bunch of filler 0's after
% that)
PathToNode = cell(size(Graph,1),1);% Initialize all Node costs to infinity except for the source node
NodeCost = Inf.*ones(1,size(Graph,1));
NodeCost(SourceNode) = 0;% Initialize the Current Node to be the Source Node
CurrentNode = SourceNode;% Initialize the set of Visited and Unvisited Nodes
VisitedNodes = SourceNode;
UnvisitedNodes = 1:size(Graph,2);
UnvisitedNodes = UnvisitedNodes(UnvisitedNodes ~= VisitedNodes);while (CurrentNode ~= TerminalNode)% Extract the Costs/Path Lengths to each node from the current nodeCostVector = Graph(CurrentNode, :);% Only look at valid neighbors ie. those nodes which are unvisitedUnvisitedNeighborsCostVector = CostVector(UnvisitedNodes);% Extract the cost to get to the Current NodeCurrentNodeCost = NodeCost(CurrentNode);% Extract the path to the current nodePathToCurrentNode = PathToNode{CurrentNode};% Iterate through the Unvisited Neighbors assigning them a new tentative costfor i = 1:length(UnvisitedNeighborsCostVector)if UnvisitedNeighborsCostVector(i) ~= Inf % Only Check for update if non-infinitetempCost = CurrentNodeCost + UnvisitedNeighborsCostVector(i); % The tentative cost to get to the neighbor through the current node% Compare the tentative cost to the currently assigned cost and% assign the minimumif tempCost < NodeCost(UnvisitedNodes(i))NewPathToNeighbor = [PathToCurrentNode(PathToCurrentNode~=0) CurrentNode]; % The new path to get to the neighborNewPath = [NewPathToNeighbor zeros(1,size(Graph,1)-size(NewPathToNeighbor,2))];PathToNode{UnvisitedNodes(i)}(:) = NewPath;NodeCost(UnvisitedNodes(i)) = tempCost;endendend% Search for the smallest cost remaining that is in the unvisited setRemainingCosts = NodeCost(UnvisitedNodes);[MIN, MIN_IND] = min(RemainingCosts);% If the smallest remaining cost amongst the unvisited set of nodes is% infinite then there is no valid path from the source node to the% terminal node. if MIN == Inffprintf('There is no valid path from the source node to the');fprintf('terminal node. Please check your graph.\n')return;end% Update the Visited and Unvisited NodesVisitedNodes = [VisitedNodes CurrentNode];CurrentNode = UnvisitedNodes(MIN_IND);UnvisitedNodes = UnvisitedNodes(UnvisitedNodes~=CurrentNode);
endRoute = PathToNode{TerminalNode};
Route = Route(Route~=0);
Route = [Route TerminalNode];
Cost = NodeCost(TerminalNode);
end

PlotGrid.m:画出由creMap创造出的栅格地图。

%%%%%%%%%%%%%%%%%%%%%地图绘制函数%%%%%%%%%%%%%%%%%%%%%
function PlotGrid(map,start,goal)
n = size(map,1);
b = map;
b(end+1,end+1) = 0;
figure;
colormap([0 0 0;1 1 1])
pcolor(b); % 赋予栅格颜色
set(gca,'XTick',10:10:n,'YTick',10:10:n);  % 设置坐标
axis image xy% displayNum(n);%显示栅格中的数值.text(start(1)+0.5,start(2)+0.5,'START','Color','red','FontSize',10);%显示start字符
text(goal(1)+0.5,goal(2)+0.5,'GOAL','Color','red','FontSize',10);%显示goal字符hold on
%pin strat goal positon
scatter(start(1)+0.5,start(2)+0.5,'MarkerEdgeColor',[1 0 0],'MarkerFaceColor',[1 0 0], 'LineWidth',1);%start point
scatter(goal(1)+0.5,goal(2)+0.5,'MarkerEdgeColor',[0 1 0],'MarkerFaceColor',[0 1 0], 'LineWidth',1);%goal pointhold on
end

总结

Dijkstra算法作为经典的基于图的最短路径算法被广泛用于路径规划上，同样著名的有Astar算法，人工势场法，遗传算法，蚁群算法，强化学习算法等，其中强化学习路径规划的python代码可见于我的另一篇博客中强化学习路径规划，需要注意的是这些上述的路径规划方法都没考虑到机器人或车辆的动力学模型，如果有这方面需要的话建议学习ROS或者Carla，ros与Carla之间可以相互连接实现更好的仿真效果，且ROS与Carla都为开源软件，使用免费，ROS自带的路径规划程序用的就是Dijkstra算法。