Dijkstra搜尋最短路徑：

整體思路

從起始節點開始，將鄰域節點進行遍歷，標註好鄰域節點最小的累計路徑長度，直到遍歷到終止節點。

演算法複雜度

• naive的方式，演算法複雜度為O(|V|2)，其中|V|是節點數量
• 聰明的方式，使用優先佇列，演算法複雜度為O((|E|+|V|)log(|V|))，其中|E|是邊界數量，|V|是節點數量。

虛擬碼

• 對所有圖中的節點n
  
  • n.distance = Infinity
  • n.parent = nil
• 建立一個List。
• 起始節點start.distance = 0, start.parent = nil，將start節點放入List
• While(List 非空)
  
  • 令current = List中distance最小的那個節點，然後將這個節點從List取出（表示這個節點已經完全訪問過了，不需要以後在訪問了）
  • 對所有和current節點相鄰的節點n
    
    • tempDistance = current.distance + length of edge from n to current
    • if(n.distance > tempDistance)
      
      • n.distance = tempDistance;
      • n.parent = current
      • 如果整個過程還沒有完成，那麼就將n加入到list裡邊去（或者更新list裡邊n的distance值和parent值）。

Matlab程式碼

本節來自Cousera的Robotics課程

function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords, drawMapEveryTime)
% Run Dijkstra's algorithm on a grid.
% Inputs : 
%   input_map : a logical array where the freespace cells are false or 0 and
%   the obstacles are true or 1
%   start_coords and dest_coords : Coordinates of the start and end cell
 

%   respectively, the first entry is the row and the second the column.
% Output :
%    route : An array containing the linear indices of the cells along the
%    shortest route from start to dest or an empty array if there is no
%    route. This is a single dimensional vector
%    numExpanded: Remember to also return the total number of nodes
%    expanded during your search. Do not count the goal node as an expanded node.


% set up color map for display
% 1 - white - clear cell
% 2 - black - obstacle
% 3 - red = visited
% 4 - blue  - on list
% 5 - green - start
% 6 - yellow - destination

cmap = [1 1 1; ...
        0 0 0; ...
        1 0 0; ...
        0 0 1; ...
        0 1 0; ...
        1 1 0; ...
    0.5 0.5 0.5];

colormap(cmap);

% variable to control if the map is being visualized on every
% iteration


[nrows, ncols] = size(input_map);

% map - a table that keeps track of the state of each grid cell
map = zeros(nrows,ncols);

map(~input_map) = 1;   % Mark free cells
map(input_map)  = 2;   % Mark obstacle cells

% Generate linear indices of start and dest nodes
start_node = sub2ind(size(map), start_coords(1), start_coords(2));
dest_node  = sub2ind(size(map), dest_coords(1),  dest_coords(2));

map(start_node) = 5;
map(dest_node)  = 6;

% Initialize distance array
distanceFromStart = Inf(nrows,ncols);

% For each grid cell this array holds the index of its parent
parent = zeros(nrows,ncols);

distanceFromStart(start_node) = 0;

% keep track of number of nodes expanded 
numExpanded = 0;

% Main Loop
while true

    % Draw current map
    map(start_node) = 5;
    map(dest_node) = 6;

    % make drawMapEveryTime = true if you want to see how the 
    % nodes are expanded on the grid. 
    if (drawMapEveryTime)
        image(1.5, 1.5, map);
        grid on;
        axis image;
        drawnow;
    end

    % Find the node with the minimum distance
    [min_dist, current] = min(distanceFromStart(:));

    if ((current == dest_node) || isinf(min_dist))
        break;
    end;

    % Update map
    map(current) = 3;         % mark current node as visited
    distanceFromStart(current) = Inf; % remove this node from further consideration

    % Compute row, column coordinates of current node
    [i, j] = ind2sub(size(distanceFromStart), current);

   % ********************************************************************* 
    % YOUR CODE BETWEEN THESE LINES OF STARS

    % Visit each neighbor of the current node and update the map, distances
    % and parent tables appropriately.
    numExpanded = numExpanded + 1;
    if(i-1>=1) %upper
        id = sub2ind(size(map), i-1, j);    
        if((map(id) ~= 2) ... %if not obst
            && (map(id) ~= 3) ... % if not visited
            && (map(id) ~= 5)) ... % if not start
            if(distanceFromStart(id) >= min_dist + 1)
                distanceFromStart(id) = min_dist + 1;
                parent(id) = current;
                map(id) = 4;

            end            
        end
    end

    if(i+1 <= nrows) %lower
        id = sub2ind(size(map), i+1, j);
        if((map(id) ~= 2) ... %if not obst
            && (map(id) ~= 3) ... % if not visited
            && (map(id) ~= 5)) ... % if not start
            if(distanceFromStart(id) >= min_dist + 1)
                distanceFromStart(id) = min_dist + 1;
                parent(id) = current;
                map(id) = 4;
            end            
        end
    end

    if(j-1 >= 1) %left
        id = sub2ind(size(map), i, j-1);
        if((map(id) ~= 2) ... %if not obst
            && (map(id) ~= 3) ... % if not visited
            && (map(id) ~= 5)) ... % if not start
            if(distanceFromStart(id) >= min_dist + 1)
                distanceFromStart(id) = min_dist + 1;
                parent(id) = current;
                map(id) = 4;
            end            
        end
    end

    if(j+1 <= ncols) %left
        id = sub2ind(size(map), i, j+1);
        if((map(id) ~= 2) ... %if not obst
            && (map(id) ~= 3) ... % if not visited
            && (map(id) ~= 5)) ... % if not start
            if(distanceFromStart(id) >= min_dist + 1)
                distanceFromStart(id) = min_dist + 1;
                parent(id) = current;
                map(id) = 4;
            end            
        end
    end


    %*********************************************************************

end

%% Construct route from start to dest by following the parent links
if (isinf(distanceFromStart(dest_node)))
    route = [];
else
    route = [dest_node];

    while (parent(route(1)) ~= 0)
        route = [parent(route(1)), route];
    end

        % Snippet of code used to visualize the map and the path
    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

end

然後執行程式碼

map = false(10); %Input Map Parameters
map (1:5, 6) = true; %Obstacle Declaration
start_coords = [6, 2]; %Starting Coordinates
dest_coords  = [8, 9]; %Destination Coordinates
drawMapEveryTime = false; %Display Outputs
[route, numExpanded] = DijkstraGrid(map,start_coords,dest_coords,drawMapEveryTime) %Implementation

最終結果：