MATLAB Dijkstra算法得到栅格图中所有最短路径

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

得到所有最短路径

再次欢迎大家来到“郝搞笑”的博客，通过前几篇文章的介绍，相信大家对Dijkstra算法有了清楚的了解，大家也使用MATLAB软件自己实现了在栅格地图中画出一条路径，接下来的文章会持续更新，包含有得到所有最短路径、时间窗路径冲突检测、各冲突类型等待与更改路径选择等功能（以上都以2条路径为例），我把这些统一放到我的专栏dijkstra 路径规划
欢迎大家订阅。接下来，一起实现栅格地图中的所有最短路径吧！

传统Dijkstra算法是在搜索路径时生成回溯向量，通过查询回溯向量，可以得到一条最短路径，但在栅格地图中两节点间的最短路径或许不止一条，为得到所有最短路径，就要保存各节点的所有前驱节点，不断更新回溯向量，MATLAB实现如下：

%%%%%%%%%%%%%%%%%%%%dijkstra得到所有最短路径%%%%%%%%%%%%%%%%%%%%%
function [sp, spcost] =dijkstraR(W, s, d)
% W 邻接矩阵   % s 起点  % d 终点 
% L 约束路径条数  % sp 所有路径矩阵  %spcost 最短路径距离
n=size(W,1);
S(1:n) = 0;     %s, vector, set of visited vectors
dist(1:n) = inf;   % it stores the shortest distance between the source node and any other node;
prev = cell(1,n);
% prev(1:n) = n+1;    % Previous node, informs about the best previous node known to reach each  network node 
prev(1:n) = {n+1};
dist(s) = 0;
counter(1:n) = 1;
while sum(S)~=n
    candidate=[];
    for i=1:n
        if S(i)==0
            candidate=[candidate dist(i)];
        else
            candidate=[candidate inf];
        end
    end
    [u_index,u]=min(candidate);
    S(u)=1;
    for i=1:n
       if(dist(u)+W(u,i))<dist(i) % SEE DOCUMENTATION Switch 1 (add)
       %if (1 - (1 - dist(u)) * (1 - W(u,i))) < dist(i) % SEE DOCUMENTATION Switch 1 (mult)
          % dist(i) = (1 - (1 - dist(u)) * (1 - W(u,i))); % SEE DOCUMENTATION Switch 2 (mult)
            dist(i)=dist(u)+W(u,i); % SEE DOCUMENTATION switch 2 (add)
            prev(1,i) = {u};
            if counter > 1
                for j=2:counter
                    prev(counter,i) = {NaN};
                end
            end
            counter(i) = 1;
        elseif(dist(u)+W(u,i)) == dist(i) % SEE DOCUMENTATION Switch 3 (add)
%elseif(1 - (1 - dist(u)) * (1 - W(u,i))) == dist(i) % SEE DOCUMENTATION Switch 3 (mult) 
            prev(counter(i)+1,i) = {u};
            counter(i) = counter(i)+1;            
        end
    end
end
% Define a vector to represent how many ties are contained in the prev cell
% array corresponding to each node 
prevcolindex = ones(1,n); 
for i = 1:n
    for j = 2:size(prev,1)
        if isempty(prev{j,i}) == 1
            break;
        else
            prevcolindex(i) = j;
        end
    end
end
% Trace the branches of the path backwards beginning at d using the prev cell array. Record 
% the history of all steps each branch of the shortest path in backpaths cell array. 
% (Each step backwards along any branch in the path trace occupies a new cell in backpaths.)
backpaths = cell(1,1);
backpaths(1,1) = {d};
counters = 1;
counterf = 1;
test = 0;
while all(test == s) == 0 % The stopping criteria is that all remaining active branches (ie step histories that have not yet arrived at s) arrive at s simultaneuously
    test = s;
    for j = counters:counterf
        if backpaths{j}(1) == s 
            continue; % If a step history his already arrived at s, it is skipped
        end
        counterm = length(backpaths);
        for i = 1:prevcolindex(backpaths{j}) % Step backwards along each branch of the shortest path, and record the resulting history in a new cell in backpaths
            backpaths{counterm+i} = cat(2,prev{i,backpaths{j}(1)},backpaths{j});
        end
        test = cat(1,test,prev{i,backpaths{j}(1)});
    end
    counters = 1+counterf;
    counterf = length(backpaths);
end
% Extract the step histories that end (ie begin) at node s, and save them
% to sp (the shortest path array)
sp = cell(1,1);
for i = 1:counterf
    if backpaths{i}(1) == s && isempty(sp{1}) == 1
        sp = backpaths(i);
    elseif backpaths{i}(1) == s && isempty(sp{1}) == 0
        sp = cat(1,sp,backpaths(i));
    end
end
spcost = dist(d);
end

相应的绘制路径要做一些改变，代码如下：

function plotMap_Path(map,spcost,P,X,Y,C)
% map 地图矩阵
% P   路径矩阵
% spcost 路径距离值
% X Y 路径坐标矩阵
n = size(map);
step = 1;
a = 0 : step :n(1);
b = 0 : step :n(2);
figure(1)
axis([0 n(2) 0 n(1)]); %设置地图横纵尺寸
set(gca,'xtick',b,'ytick',a,'GridLineStyle','-',...
'xGrid','on','yGrid','on');
hold on
r = 1;
for(i=1:n(1))         %设置障碍物的左下角点的x,y坐标
    for(j=1:n(2))
        if(map(i,j)==1)
            p(r,1)=j-1;
            p(r,2)=i-1;
            fill([p(r,1) p(r,1) + step p(r,1) + step p(r,1)],...
                 [p(r,2) p(r,2) p(r,2) + step p(r,2) + step ],'k');
            r=r+1;
            hold on
        end
    end
end
  %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%栅格数字标识%%%%%%%%%%%%%%%%%%%%%%%%%%%%
x_text = 1:1:n(1)*n(2); %产生所需数值.
for i = 1:1:n(1)*n(2)
    [row,col] = ind2sub([n(2),n(1)],i);
    text(row-0.9,col-0.5,num2str(x_text(i)),'FontSize',8,'Color','0.7 0.7 0.7');
end
hold on
axis square
 %%%%%%%%%%%%%%%%%%%%%%%%%%%画出栅格及路径%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 
if(spcost<999)    %判断是否为可达路径
      place = 0.6-0.03;
      n = size(P,1);
      for i = 1:1:n
          plot(X(i,:)-place,Y(i,:)-place,'Color',C(i,:),'LineWidth',2);%画出路径
          hold on
      end
 else
    disp('路径不可达');
end

end

为了后续功能的模块化设计，将初始化工作封装成一个功能块：

function [W,C] =MapInit(map)
W=G2D(map);                                   %得到环境地图的邻接矩阵
W(W==0)=Inf;                                  %邻接矩阵数值处理
W=OPW(map,W);                                 %优化邻接矩阵
W(W==Inf)=999;                                %邻接矩阵数值处理
C=[1 0 0;0 1 0;0 0 1; 0 0 0;1 0 1;0 1 1;1 0 0.5;0.5 0 1;0.5 0 0.5;0 1 0.5;0 0.5 0.5;1 0.5 0.2;]; %颜色矩阵
end

其实现如下：

%%%%%%%%%%%%%%%%%%%%%%建立环境矩阵map及参数初始化%%%%%%%%%%%%%%%%%%%%%
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,100];                             %设置起止栅格节点
[W,C] = MapInit(map);                      %环境初始化
[sp1,spcost1]=dijkstraR(W,SD1(1),SD1(2));  %设置始末栅格及路径条数(1)
[X1,Y1]=Get_xy(spcost1,sp1,map);           %得到X,Y坐标
plotMap_Path(map,spcost1,sp1,X1,Y1,C);     %得到环境地图

其中[X1,Y1]=Get_xy(spcost1,sp1,map);函数在专栏“在栅格地图中，画出一条路径及＜MATLAB＞实现”文章中。

路径图如下：

附：
如果出现图示错误：

是因为路径数太多，颜色矩阵表示不够造成，可以在颜色矩阵C中继续添加自己想要的颜色。