Dijkstra —— Path Planning

版权声明：本文为博主原创博文，未经允许不得转载，若要转载，请说明出处并给出博文链接

       假设您想要找到城市地图上两个交叉点之间的最短路径:起点和终点。Dijkstra算法最初将距离(从起始点)标记为地图上其他每一个交点的无穷大。这样做并不是要暗示存在无限的距离，而是要注意那些交叉点还没有被访问过。这种方法的一些变体不标记交叉点的距离。现在在每个迭代中选择当前的交集。对于第一次迭代，当前交集将是起点，到它的距离(交集的标签)将为零。对于随后的迭代(第一次迭代之后)，当前交集将是距离起点最近的未访问交集(这很容易找到)。

       从当前交集，更新每个未访问的直接连接到它的交集的距离。这是通过确定未访问的交集和当前交集值之间的距离的总和来实现的，然后如果未访问的交集小于未访问的交集的当前值，则使用该值(总和)重新标记未访问的交集。实际上，如果通过当前交点的路径比之前已知的路径短，则该交点被重新标定。为了方便最短路径的识别，如果你标记或者重新标记它，用铅笔在重新标记过的十字路口上标记一个箭头，然后擦掉所有指向它的箭头。更新到每个相邻交叉口的距离后，将当前交叉口标记为已访问，并选择一个未访问的交叉口，其距离最小(从起点开始)—或最低标签—作为当前交叉口。标记为已访问的交叉点将用从起点到它的最短路径进行标记，并且不会重新访问或返回。

       继续这个过程，用最短的距离更新邻近的交叉口，将当前交叉口标记为已访问，并移动到最近的未访问的交叉口，直到将目的地标记为已访问。一旦您将目的地标记为已访问(与任何已访问交集的情况一样)，您就确定了从起点到它的最短路径，并可以沿着箭头反向返回。在算法实现中，这通常是通过跟随节点的父节点从目标节点到开始节点(在算法到达目的地节点之后)来完成的;这就是为什么我们还要跟踪每个节点的父节点。

       这个算法并不像人们所期望的那样试图直接“探索”目的地。相反，在确定下一个“当前”交叉点时，唯一要考虑的是它到起点的距离。因此，该算法从起始点开始向外扩展，交互式地考虑每一个最短路径距离较近的节点，直到到达目的地。这样理解的话，算法如何找到最短路径就很清楚了。然而，这也揭示了该算法的一个弱点:在某些拓扑结构中，它的速度相对较慢。

       本算法优点是易于实现在各式各样的环境。缺点是时间复杂度过高，且障碍物需要是静止的。

       下面借用知乎大神的代码学习下：

clc;
clear;
close all;
%% load obs
obs = load('obsdata.txt'); % txt文件在下面
numofobs = length(obs(:,1));
%% set up color map for display 
cmap = [1 1 1; ...% 1 - white - 空地
        0 0 0; ...% 2 - black - 障碍 
        1 0 0; ...% 3 - red - 已搜索过的地方
        0 0 1; ...% 4 - blue - 下次搜索备选中心 
        0 1 0; ...% 5 - green - 起始点
        1 1 0];...% 6 - yellow -  到目标点的路径 
colormap(cmap); 
map = zeros(25); 
% 设置障碍
for j=1:numofobs
    xobs(j) = obs(j,1);
    yobs(j) = obs(j,2);
    map(xobs(j),yobs(j)) = 2;
end
map(1,25) = 5; % 起始点
map(25,1) = 6; % 目标点
image(1.5,1.5,map);
grid on;
axis image;
%% 
nrows = 25; 
ncols = 25; 
start_node = sub2ind(size(map), 1, 25); 
dest_node = sub2ind(size(map), 25, 1); 
% Initialize distance array 
distanceFromStart = Inf(nrows,ncols); 
distanceFromStart(start_node) = 0; 
% For each grid cell this array holds the index of its parent 
parent = zeros(nrows,ncols); 
t0=clock;
% Main Loop 
while true 
 % Draw current map 
 map(start_node) = 5; 
 map(dest_node) = 6; 
 image(1.5, 1.5, map); 
 grid on; 
 axis image; 
 drawnow; 
  % Find the node with the minimum distance 
 [min_dist, current] = min(distanceFromStart(:)); 
  if ((current == dest_node) || isinf(min_dist)) 
       break; 
  end; 

 map(current) = 3; 
 distanceFromStart(current) = Inf; 
 [i, j] = ind2sub(size(distanceFromStart), current);
 neighbor = [i-1,j;... 
            i+1,j;... 
            i,j+1;... 
            i,j-1] 
outRangetest = (neighbor(:,1)<1) + (neighbor(:,1)>nrows) +...
                   (neighbor(:,2)<1) + (neighbor(:,2)>ncols ) 
locate = find(outRangetest>0); 
neighbor(locate,:)=[] 
neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2)) 
for i=1:length(neighborIndex) 
if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5) 
    map(neighborIndex(i)) = 4; 
  if distanceFromStart(neighborIndex(i))> min_dist + 1      distanceFromStart(neighborIndex(i)) = min_dist+1; 
        parent(neighborIndex(i)) = current; 
  end 
 end 
end 
end
TimeCost=etime(clock,t0)
%%
if (isinf(distanceFromStart(dest_node))) 
    route = []; 
else 
    %提取路线坐标
   route = [dest_node]; 
      while (parent(route(1)) ~= 0) 
              route = [parent(route(1)), route]; 
       end 
  % 动态显示出路线     
        for k = 2:length(route) - 1 
          map(route(k)) = 7; 
                pause(0.1); 
                image(1.5, 1.5, map); 
              grid on; 
              axis image; 
              end 
end
axis off;  

obsdata.txt 见下：

10 1 0.5
20 1 0.5
21 1 0.5
17 2 0.5
18 2 0.5
1 3 0.5
5 3 0.5
18 3 0.5
3 4 0.5
9 4 0.5
15 4 0.5
22 4 0.5
23 4 0.5
2 5 0.5
7 5 0.5
9 5 0.5
13 5 0.5
18 5 0.5
3 6 0.5
7 6 0.5
24 7 0.5
5 7 0.5
9 7 0.5
18 7 0.5
13 8 0.5
15 8 0.5
5 9 0.5
19 9 0.5
21 9 0.5
6 11 0.5
11 11 0.5
16 11 0.5
17 11 0.5
2 12 0.5
11 12 0.5
19 12 0.5
5 14 0.5
12 14 0.5
13 14 0.5
5 15 0.5
2 16 0.5
7 16 0.5
10 16 0.5
18 16 0.5
5 17 0.5
11 17 0.5
21 17 0.5
23 17 0.5
16 18 0.5
2 19 0.5
9 19 0.5
17 19 0.5
24 19 0.5
5 20 0.5
13 20 0.5
15 20 0.5
16 20 0.5
21 20 0.5
25 20 0.5
3 21 0.5
4 21 0.5
7 21 0.5
15 21 0.5
20 21 0.5
24 21 0.5
6 22 0.5
5 23 0.5
9 23 0.5
11 23 0.5
16 23 0.5
20 23 0.5
23 23 0.5
4 25 0.5
9 25 0.5
18 25 0.5

 经过小修改后，仿真效果如下：

                                                        

                                                        

 

鉴于博主能力有限，如有错误或问题，还请各路大神不吝赐教，定当虚心修改。。。。。。