A worked explanation of shortest-path search using accumulated movement cost, with the class implementation below.
Dijkstra Algorithm
On each iteration, Dijkstra’s algorithm selects the unvisited cell with the smallest accumulated path cost from the start. This is distance along permitted paths, not straight-line distance through obstacles. The grid example uses four neighbours and unit movement costs.
Implementation Context
The class snippet below updates neighbour costs using the current accumulated cost plus the movement cost and records the parent index directly. Unlike A*, it does not use a goal-directed heuristic; historical screenshots require their original maps and plotting settings for exact reproduction.
Testing and Real-World Use
This could be tested on matched free and obstructed grids by checking route adjacency, obstacle avoidance and accumulated cost against an independent shortest-path reference. Dijkstra’s method is useful for minimum-cost search with nonnegative edge costs, while this unit-cost grid implementation is not a physical robot collision checker.
function [route,numExpanded] = DijkstraGrid (input_map, start_coords, dest_coords)
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
drawMapEveryTime = true;
[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));
% A blocked endpoint is not a traversable start or goal.
if input_map(start_node) || input_map(dest_node)
route = [];
numExpanded = 0;
return;
end
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);
% *********************************************************************
% Visit each neighbor of the current node and update the map, distances
% and parent tables appropriately.
numExpanded = numExpanded + 1;
currentNode = [i,j];
% This is for putting the nodes around the starting node
upNode = [i+1,j];
downNode = [i-1,j];
leftNode = [i,j-1];
rightNode = [i,j+1];
% For the neighbors
neighbours = [upNode; downNode; leftNode; rightNode];
for n = 1:4
% checks boundaries
if (neighbours(n,1)>0 && neighbours(n,2)>0) && (neighbours(n,1)<= nrows && neighbours(n,2)<=ncols)
% checks positions 1- white 2-obstacle 3-visited 4-onlist
% 5-start 6-destination if the position is not an obstacle,
% visited or the start or if it's destination, then calculate
if( map(neighbours(n,1),neighbours(n,2)) ~= 2 && map(neighbours(n,1),neighbours(n,2)) ~= 3 && ...
map(neighbours(n,1),neighbours(n,2)) ~= 5 || map(neighbours(n,1),neighbours(n,2)) == 6)
% adds to list and calculates distance to nodes
if(distanceFromStart(neighbours(n,1),neighbours(n,2)) > min_dist + 1)
map(neighbours(n,1),neighbours(n,2)) = 4; % adds to list of expansion
distanceFromStart(neighbours(n,1),neighbours(n,2)) = min_dist + 1;
parent(neighbours(n,1),neighbours(n,2)) = current;
end
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
For testing the code
%% Define a small map
map = false(10);
% Add an obstacle
map (1:5, 6) = true;
start_coords = [6, 2];
dest_coords = [9, 8];
%%
close all;
[route, numExpanded] = DijkstraGrid (map, start_coords, dest_coords);
Dijkstra Test 10x10
Dijkstra Test 25x25
