Dijkstra’s Algorithm is used for finding the shortest path in a graph where edges’/arcs’ weights are already known. Dijkstra’s algorithm is a special form of dynamic programming and it is also a breadth first search method. The basic idea behind is to keep a running cost for nodes and pick the lowest cost one in the frontier to explore until you reach the goal.

Dijkstra’s Pseudocode

A-Star (𝐴*)

A-Star (𝐴*) is an extension of Dijkstra’s algorithm, which reduces the total number of states by introducing a heuristic estimation of the cost from the current state to the goal state. Cost function is given by

𝑓(𝑥) =𝑔(𝑥) +ℎ(𝑥) , where 𝑔(𝑥) is the cost from start state to current state 𝑥, ℎ(𝑥) is the heuristic estimation of the cost of an optimal path from current state 𝑥 to goal state. 𝑓(𝑥) is the total cost of the node.

Steps:

1) Add the starting square (or node) to the open list.

2) Repeat the following:

a) Look for the lowest F cost square on the open list. We refer to this as the current square.

b) Switch it to the closed list.

c) For each of the 8 squares adjacent to this current square …

If it is not walkable or if it is on the closed list, ignore it. Otherwise do the following.

If it isn’t on the open list, add it to the open list. Make the current square the parent of this square. Record the F, G, and H costs of the square.

If it is on the open list already, check to see if this path to that square is better, using G cost as the measure. A lower G cost means that this is a better path. If so, change the parent of the square to the current square, and recalculate the G and F scores of the square. If you are keeping your open list sorted by F score, you may need to resort the list to account for the change.

d) Stop when you:

Add the target square to the closed list, in which case the path has been found (see note below), or Fail to find the target square, and the open list is empty. In this case, there is no path.

3) Save the path. Working backwards from the target square, go from each square to its parent square until you reach the starting square. That is your path.

A* Pseudocode

Following the example below, you should be able to implement A* in any language.

// A* (star) Pathfinding// Initialize both open and closed list
let the openList equal empty list of nodes
let the closedList equal empty list of nodes// Add the start node
put the startNode on the openList (leave it’s f at zero)// Loop until you find the end
while the openList is not empty // Get the current node
let the currentNode equal the node with the least f value
remove the currentNode from the openList
add the currentNode to the closedList // Found the goal
if currentNode is the goal
Congratz! You’ve found the end! Backtrack to get path // Generate children
let the children of the currentNode equal the adjacent nodes

for each child in the children // Child is on the closedList
if child is in the closedList
continue to beginning of for loop // Create the f, g, and h values
child.g = currentNode.g + distance between child and current
child.h = distance from child to end
child.f = child.g + child.h // Child is already in openList
if child.position is in the openList’s nodes positions
if the child.g is higher than the openList node’s g
continue to beginning of for loop // Add the child to the openList
add the child to the openList

A* vs Dijkstra’s

Dijkstra’s is essentially the same as A*, except there is no heuristic (H is always 0). Because it has no heuristic, it searches by expanding out equally in every direction. As you might imagine, because of this Dijkstra’s usually ends up exploring a much larger area before the target is found. This generally makes it slower than A*.

Robotics Enthusiast. Well versed with computer vision, path planning algorithms, SLAM and ROS