路径搜索算法 - Dijkstra算法 MATLAB实现_simscape

http://blog.sina.com.cn/u/3002401407

首页博文目录关于我

个人资料

微博

加好友发纸条

写留言加关注

博客等级：
博客积分：

博客访问：
关注人气：
获赠金笔：0支
赠出金笔：0支
荣誉徽章：

正文字体大小：大中小

路径搜索算法 - Dijkstra算法 MATLAB实现

(2016-02-25 00:34:20)

标签：

matlab

在网上看了很多关于这个算法的说明，各种大神写得太高深了，脱离了我的认知水平，看了很久也看不懂。于是，用自己能理解的语言把它记录一下。
如果只是要用搜索算法的话，可以参考：
http://www.mathworks.com/help/releases/R2015b/matlab/graph-and-network-algorithms.html

路径搜索问题：

路径搜索问题，就类似于下图中，机器人要从绿色起点走到黄色终点，沿什么样路线过去？
(因为这里只是为了了解 Dijkstra 算法，所以用这种路径搜索问题作为例子，不考虑道路地图。)

http://s4/mw690/003hbLL9gy6ZEfHJJDR43&690- Dijkstra算法 MATLAB实现" TITLE="路径搜索算法 - Dijkstra算法 MATLAB实现" />

这是我们从视觉上来理解这个问题，可以沿着红线走过去。先不说怎么让机器人自己能能找到这条线，先来说怎么把诸如“地图”、"线路"、"位置"、“障碍物”等等概念表示成代码呢？

地图

说算法之前，看看这个地图怎么表示。

首先，把地图划分为网格，这样就可以用矩阵来表示，矩阵的行、列就直观的表示地图上的每一个位置坐标。

比如，ones 命令就生成了一个 10*10 的全 1 阵 map，就表示一张 10*10 的地图。那数字 1 表示什么呢？目前为止，什么也不表示，没有任何意义。

rows = 10;

ncols = 10;

map = ones(rows, ncols);

额外信息，比如起点，有终点，有障碍物等等，怎么在这个地图上表示出来呢？那就给 map 矩阵赋不同的值呗，比如我们约定好：空地用 1 表示，墙用 2 表示。

我们约定好以后，矩阵的值就变得有意义了。将来判断 map(2,2)~=2，我们就知道（2,2）这个位置是不是有墙。( 考虑到浮点数判断的精度问题，将来地图的数据类型可以设置为整型，以防后患。)

map (1:5, 7) = 2; % 设置一堵墙

map(6, 2) = 5; % 设置起点

map(4, 8)  = 6; % 设置终点

而，我们所能直观看到的就是一张图，自然，就可以用不同的颜色来表示出 map 的不同的值。

比如，我希望空地的1对应画白色；而墙的2对应画黑色：

%            R G B

cmap = [1 1 1; ...% 1 - white - clear cell 

        0 0 0; ...% 2 - black - obstacle
        

               1 0 0; ...% 3 - red = visited
        

               0 0 1; ...% 4 - blue  - on list
        

               0 1 0; ...% 5 - green - start
        

              1 1 0];% 6 - yellow - destination
colormap(cmap);

这样设置之后，map 矩阵由于是 double 类型的，所以它的值为1-6时，画什么颜色就按照这里配置的颜色映射来定。

画地图：

image(1.5,1.5,map);   % image 命令画图时，对于超出上下限的值，依旧按照上下限对应的颜色来画。

grid on;

axis image;

这样，这张地图就画出来了：

http://s14/middle/003hbLL9zy6ZD04Oa4Z4d&690

最后我们的目的是，要找到这么条黄色的路线：

所以可以看到图中还有其他颜色，这是在算法里为了表示算法的搜索过程，特意修改了地图的值来做显示用。

比如红色，就是我搜索过的区域，蓝色，就是下一步的搜索候选区。

http://s14/middle/003hbLL9zy6ZD04NpYpfd&690

算法的简单版本

这个算法的大概意思就是(发现好难说清楚，对照上图)，

从绿点开始，找到上下左右邻居，其实就是起点行列坐标i-1,j之类(变成蓝色)，计算它们到起点的路程(其实就是+1)，保存到路程矩阵里。

然后再从蓝色邻居里挑 “距离起点路程最近的那个”当做新一轮的搜索中心(把它变成红色)。

从新的搜索中心坐标开始，再找到它的上下左右邻居(增加新的蓝色)，计算到起点的路程(在当前搜索中心路程1的基础上，再+1)，保存到路程矩阵里。

再从所有蓝色邻居里挑选搜索中心，

再从这个搜索中心开始，再找到它的上下左右邻居，计算到起点的路程。

一直这么循环，总有一天，会一直搜索到终点。

PS：用不同的方法从蓝色邻居里来挑选搜索中心，就构成了 Dijkstra 算法和 A* 算法的主要区别。
Dijkstra 里只有一个原则，就是距离起点最近，其实就是无方向向外扩散的。而 A*算法还会加上一个跟终点的距离考量，所以就是带有方向性的扩散。一般情况下，A* 会搜索的快一点。

可以看到，这个算法要保存一些信息，所以先定义一些变量。

start_node = sub2ind(size(map), 6, 2);

% 为了方便，把起点和终点的行列下标换成索引，所以 map(6,2)和 map(start_node)就是同一个意思。

dest_node  = sub2ind(size(map), 4,  8);

a. 定义一个变量来保存蓝色邻居以及它们到起始格的路程

所以这里定义了 distanceFromStart 来保存这些信息，初始化为 Inf，表示从没有访问过。一旦有值，就说明是蓝色邻居，赋值的大小就表示改点跟起始点的路程。一旦变成红色，就把它的值再改回 Inf。
至于具体要显示动态图，要修改的是 map 的值。

distanceFromStart = Inf(nrows,ncols);

distanceFromStart(start_node) = 0;

最后，这个矩阵会更新成这个样子。路程更新到了第4行第8列的终点位置，而且它距离起点有8步。那么这八步是怎么走的呢，就需要有矩阵来保存路线信息。

http://s15/middle/003hbLL9zy6ZD05Voiq9e&690

b. 定义一个变量来保存路径。

parent = zeros(nrows,ncols);

这个矩阵最后会更新成什么样，它怎么表示路线呢？

比如下图，第4行第8列的值是75，就表示它的上家的格子编号是75，其实也就是矩阵的索引位置，按列数过来就是第8列第5行。这里的值是76，就表示它的上家的格子第8列第6行。一次类推，这样就可以反推出一条线路来了。

http://s4/middle/003hbLL9zy6ZD066CJ5f3&690

====================================================

变量定义完了，那么开始循环搜索路径

while true

先画一下当前的地图

    map(start_node) = 5;

map(dest_node) = 6;

image(1.5, 1.5, map);

 grid on;
    axis image;
    drawnow;

这时就出来了这张图

http://s8/middle/003hbLL9zy6ZD04Yo4v17&690

那么，就开始搜索了，可是以哪个点为搜索中心呢。

当然是从蓝色邻居列表选离初始点最近的那一个点。

因为 distanceFromStart 的初始值是 Inf，已知的只有起始点的路程，为0。所以如果是第一次循环，那min函数得到的current自然就是16了，这也是初始点的索引坐标；这时得到的 min_dist =0。

[min_dist, current] = min(distanceFromStart(:));

%搜索中心的索引坐标：current,

%搜索中心与起始点的路程：min_dist

% 这两个值后面会用。

%这里做一些简单判断，如果已经扩张到终点了，或者没有路径，则退出循环。

if ((current == dest_node) || isinf(min_dist))

    break;

end;

% 为了画图效果，把 map 的前点坐标赋值为 3 ，表示本次循环已经以此为中心搜索一次了。

map(current) = 3;

可以看到这是现在 map 的值，如果画出来就是这样：

http://s14/middle/003hbLL9zy6ZD04Vuc51d&690

% 同时把distanceFromStart的这个位置赋值为 Inf，表示它已经当过搜索中心了。

% 上面的 min 函数将来就不可能再找到这个坐标。

distanceFromStart(current) = Inf;

把搜索中心的邻居的坐标点找出来，这里只找上下左右的邻居没有计算斜角：

% 把索引坐标变成行列坐标，方便计算邻居的坐标。

[i, j] = ind2sub(size(distanceFromStart), current);

neighbor = [i-1, j ;...

                      i+1, j ;...

                      i, j+1 ;...

                       i, j-1]

所以(6,2)的四个邻居的坐标计算出来，得到一个四行两列的结果：

neighbor =

     5     2
     7     2
     6     3
     6     1

为了方便，现在把这种行列形式变为索引形式：

neighborIndex = sub2ind(size(map),neighbor(:,1),neighbor(:,2))

neighborIndex =

    15
    17
    26
     6

下面就要计算每个邻居的路程，保存每个邻居的路线。

for i=1:length(neighborIndex)

   if (map(neighborIndex(i))~=2) && (map(neighborIndex(i))~=3 && map(neighborIndex(i))~= 5)

          map(neighborIndex(i)) = 4;  %  在地图上把邻居变成蓝色。这里纯为了显示用。

            % distanceFromStart这个矩阵初始值是 Inf ，所以第一次找到它的时候肯定会更新路径的，

            % 循环多次以后，可能会有多次机会走到这个邻居，所以要看哪条路近。

% 只有邻居已有的路线比“从当前搜索中心走过去”要长的话，才会更新这个邻居的信息。

           if distanceFromStart(neighborIndex(i))> min_dist + 1

                  distanceFromStart(neighborIndex(i)) = min_dist+1;  %更新邻居的路程信息

                  parent(neighborIndex(i)) = current;   % 更新邻居的路径信息

end

end

end

第一次搜索后，可以看到这四个邻居的距离值 distanceFromStart，更新为 1 了：

http://s1/middle/003hbLL9zy6ZD058uU860&690

然后，这四个邻居的线路，16 就是表示是从起始点(坐标索引是16)走过来的。

http://s14/middle/003hbLL9zy6ZD05evpjbd&690

map 就变成这样：

http://s11/middle/003hbLL9zy6ZD05dmCSea&690

end   %这是 while true 对应的end

这是第一轮循环，然后再接着循环第二次。

从之前的代码可以看到，会先把起始点的颜色重置一下。然后上一轮找到的四个邻居已经标记为蓝色了。

http://s1/middle/003hbLL9zy6ZD05ohTG90&690

同样，第二次也会先定搜索中心，根据上一次循环得到的 distanceFromStart 矩阵的结果，其实最小值有四个，都是1，Dijkstra算法没有别的考量权重，所以这里只是按顺序，取了第一个 1 作为搜索中心：

[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);

得到的current就是左边这个红点，min_dist就是刚才计算出来的1。

http://s13/middle/003hbLL9zy6ZD05no8A4c&690

所以，你看这时候，搜索中心在边界，左边那个(6,0)超出了边界，就是假邻居了：

neighbor = [i-1,j;...

                      i+1,j;...

                    i,j+1;...

                   i,j-1]

neighbor =

     5     1
     7     1
     6     2
     6     0

需要把它给去掉：

outRangetest = (neighbor(:,1)<<span style="color: #800080">1) + (neighbor(:,1)>nrows) + (neighbor(:,2)<<span style="color: #800080">1) + (neighbor(:,2)>ncols ) 
locate = find(outRangetest>0);


neighbor(locate,:)=[]  % =[]就是删除的意思。

neighbor =

     5     1
     7     1
     6     2

同样，开始更新每一个邻居的信息：

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

最后，map 更新，红点就是已经当过搜索中心了，蓝点就是全部的待搜索邻居。

http://s1/middle/003hbLL9zy6ZD05tXnG50&690

下面的 distanceFromStart 就是所有蓝点的位置(有数值的地方)，以及这些蓝点到起始点的路程（数值）。

下一个循环，便接着再找 distanceFromStart 的最小值所在的位置，作为搜索中心。

http://s9/middle/003hbLL9zy6ZD05CHcQ28&690

可以看到，有1，1就是从起始点1步就走到了这里；2表示从起始点两步就走到了这里。

那，是沿着哪条线路走过来的呢？

就得对照着看 parent 矩阵的数据了，上面那张图里的 2 步，就是下图的箭头这么走出来的：

6,16表示的是矩阵的索引编号。

http://s13/middle/003hbLL9zy6ZD05NPHK6c&690

那么，一直这么循环下去，最后会得到什么呢，也就是这样图：

http://s5/middle/003hbLL9zy6ZD05Uffmc4&690

最后得到的这个 distancdFromStart 矩阵，可以看出，从起始点到目标点[4,8]，需要走8步。

http://s15/middle/003hbLL9zy6ZD05Voiq9e&690

那怎么走呢？parent里表示出了路线，按照格子里所显示的索引坐标，倒回来追溯：

http://s4/middle/003hbLL9zy6ZD066CJ5f3&690

http://s14/middle/003hbLL9zy6ZD0676KFcd&690

脚本，不讲代码效率，只是为了大概理解算法：

%%
% set up color map for display

cmap = [1 1 1; ...% 1 - white - clear cell 

        0 0 0; ...% 2 - black - obstacle
        

               1 0 0; ...% 3 - red = visited
        

               0 0 1; ...% 4 - blue  - on list
        

               0 1 0; ...% 5 - green - start
        

               1 1 0];% 6 - yellow - destination

colormap(cmap);

map = zeros(10);

% Add an obstacle

map (1:5, 7) = 2;


map(6, 2) = 5; % start_coords 

map(4, 8)  = 6; % dest_coords

image(1.5,1.5,map);


grid on;


axis image;

%%


nrows = 10;


ncols = 10;


start_node = sub2ind(size(map), 6, 2);


dest_node  = sub2ind(size(map), 4,  8);

% 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);

% 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

%%

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

阅读┊ 收藏 ┊ 喜欢 ▼ ┊打印┊举报/Report

前一篇：Robotics System Toolbox 环境初始化指南

新浪BLOG意见反馈留言板　欢迎批评指正