Thursday, 5 September 2013

A* Algorithm for Path Planning


In the article we will look at implementation of A* graph search algorithm for robotic path planning and navigation.

A* Path Planning

The aim of path planning algorithms is to find a path from the source to goal position.

We work under the following assumptions :
  • Point Robot with Ideal Localization
  • Workspace is bounded and known
  • Static source,goal and obstacle locations
  • Number of obstacles are finite
  • Obstacles have finite thickness
The discrete path planning task which is posed as posed as graph search problem.

A* is a widely used graph traversal algorithm that uses Best First Search approach to find least cost path from the source to destination node.

  • Workspace divided into discrete grids
  • Each grid represents vertex of graph
  • Edges defined by connected components of grids
  • If adjacent grid points inside obstacles it is not connected
  • If adjacent grid points in free space it is connected
Adjacent grids of the workspace are considered to be connected by a edge.Each edge is associated with a cost defined using a cost function $f(n)$.

The cost can be defined using various criteria based on information about source,goal,obstacles,current node position in the graph.

Typical graph search algorithms find a minimum cost path from source to destination node.

For path finding application the cost is directly related to distance of grid/node from source and destination nodes.

The the minimum-cost path provides a path with shortest-distance from source to destination grid.

The input to simulation environment is provided in form of a image

Informed Search Algorithms

Breadth first or Depth First search are un-informed search algorithms where all the nodes along the breadth of depth of graph are explored till a goal node is found.

A large number of nodes are explored in the process.

We can reduce search space using additional information about the problem.

Such methods are called as informed search algorithms which Consider Cost of paths ,heuristic distance from goal state etc to decide path most likely to lead to a goal or some information about the problem at hand to choose the most likely node that will lead to goal position.

A* Uses Best First search strategy to explore the graph by expanding the best node (shortest path) according to a predefine criteria.

It Chooses the node that has the lowest value for the cost function which is defined as

$f(n) = g(n) + h(n)$,where

$g(n)$ is exact cost of the path - initial node to present node

$h(n)$ is estimated heuristic cost - current node to the goal

The A* algorithm can considered to operate in 2 states
  • Find the node associated with minimum cost
  • Expand the node
By expanding the node we mean that all the connected components of the nodes are considered to be potential candidates for best node in the next iteration of the algorithm

Implementation Details : Algorithm 1

The simplest data structure that can be used is a $ SET $.

Let call the set $OPENLIST$.Initially a $OPENLIST$ contains the only source node.

The algorithm is as follows
  • find element from minimum cost from the set
  • Stop if goal node is reach and trace back the path
  • add the successors of the element(expand the node),that lie in free position in workspace to set
  • If set is empty ,no path to the goal is found.
This algorithm is not completed.It does not always find a path between source and goal even if the path exists

It can get stuck in loop especially at dead ends,or large obstacles are encountered especially in situations when we encounter a node with only successor being its parent.

Below is the simulation output in which algorithm is stuck in a loop when using just open list.

Below is the simulation in which algorithm is stuck in a loop when we do not consider

Below is simulation output with open and closed list changes

Also it can happen that we visit a node already present in open list,thus algorithm will be stuck in a loop .This can happen when large obstacles are encountered

To solve the problem we maintain another set call $CLOSEDLIST$.

This will contain the nodes which were already expanded,ie minimum cost nodes. If a node is already present in the closed list ,it will not be considered again for expansion or we can visit this node only a finite number of times

This will avoid the problem of getting stuck in loops.

Also if a node is already present in the open list,we check the cost of the node present in the list.The cost is higher,the node is replaced with new node else the new node is not considered for expansion.

Thus we will consider a new node for expansion if it lies in the open list only if path through the node leads to lower cost than the path through the node already present in the list


If the heuristic does not overestimate the true cost to the goal,then the A* algorithm will always find the optimal solution and A* is said to use an admissible heuristic.

The admissible heuristic is too optimistic ,it will lead $A^*$ to search paths that turn out to be more costly than optimal paths.

It will not prevent A* from expanding a node that is on the optimal path ,which can happen if heuristic value is too high

Some heuristics are better than others,We want heuristic to be as close to the true cost as possible. A heuristic will mislead ,if the true cost to goal is very large compared to the estimate.

If we have two admissible heuristic functions $h_1,h_2$,then if $h_1 > h_2 for all N$ , then $h_1$ is said to dominate $h_2$ or $h_1$ is better than $h_2$.

Below is the simulation with heuristic $h(n)=0$,this is worst possible heuristic it does not help in reducing the search space at all,This is called as uniform cost search

we can see than it is admissible and finds the optimal path.

if we consider the euclidean distance as heuristic,the true distance will always be greater than or equal to euclidean distance hence lead to the optimal path

Let us consider a heuristic function $(1+\varepsilon) h(n),\varepsilon >1$.

Thus will lead to overestimation of true distance/cost to the goal. The overestimation causes the cost biased towards the heuristic ,rather than true cost to travel to the mode.

The path will tend to be skewed towards the goal position.

Below simulation is output with $\varepsilon=5$

Below simulation is output with $\varepsilon=1$

Thus we can see that overestimation of heuristic function produces a non optimal path.

Non Optimal Paths

A* expands all the equally potential nodes,large number of nodes are analyzed.

If we are able to relax the optimality condition,we can benefit from faster execution times.

For example we can use static weighing of the cost function f(n) = g(n) + (1+ \varepsilon) h(n) if $h(n)$ is admissible heuristic ,the algorithm will find path with optimal cost $L$. f(n)=g(n)+h(n) \le L

If we overestimate the cost,a point not on the optimal path would be selected,The algorithm will overlook the optimal solution .

Let $h^*(n)$ give true cost from node to goal. \begin{eqnarray*}\\ h(N) \le c(N,P) + h(P) \\\\ h(N_m) \le h^*(N_m) \\\\ h(N_{m+1}) \le C(N_m,N_{m+1})+h(N_m) \le C(N_m,N_{m+1}) + h^*(N_m) = h^*(N_{m+1})\\\end{eqnarray*} Thus if we estimate the cost as $h_1(n)$ which is not admissible heuristic

\begin{eqnarray*}\\(1+\varepsilon)h(N_{m+1}) > h^*(N_{m+1}) \\\\h_1(N_{m+1})=(1+\varepsilon)h(N_{m+1}) < (1+\varepsilon)h^*(N_{m+1}) \\\\\end{eqnarray*} Thus the worse heuristic can do $(1+\varepsilon)$ times the best path.

Implementation Details

$GNode$ is class that represents the node of a graph,it also contains information about the adjacent nodes of graph.

$AStar$ is class containing methods and objects for $AStar$ algorithm. It contains a queue for open and closed list.We use C++ $dequeue$ data structure as it provides dynamic allocation.

One of routines required is to check if node is present in the closed list Another method required is to add elements to open list

Instead of set we can also use a priority queue data structure for implementation.

The queue is sorted according to the cost associated with the node. Higher to cost lower is priority ,lower the cost higher the priority.

The higest priority node is placed at top of queue while lower once are placed at the bottom.

Thus when adding a new element to the queue,it must be added at proper location

Also if a element is present in the open list,we replace it only if cost is lower than element being inserted.

We insert an element in priority queue only if node being inserted has higher priority.

To insure we do operation in a single pass,we insert the node irrespective of whether it is present in queue or not at suitable position.And set a flag called insertflag to indicate that.

If the same node is encountered after insert flag is set to true that means node is of lower priority and we erase the node encountered as higher priority node has already been inserted in the queue.

If we encounter the node in the open list and its priority is higher,then we do not insert the node.

We also need to determine the node is accessible or not.A dense grid is precomputed which tell us if current position lies inside obstacle or not. the variable $obstaclemap$ contains this map.

Thus we are given the current node and successor.

We compute the unit vector along the direction of movement and take small steps along this direction every time checking if point lies in the obstacle or not.

This approach will give is node is accessible or not even in case of large grid sizes, where nodes may be on opposite side of obstacle.


Simulation outputs can be found at


The files AStar.hpp,AStar.cpp define the AStart Algorithm.The files gsim.hpp,gsim.cpp defines the simulation environment. The code can be found at

Images for test simulation and output simulation videos can also be found in the repository

Run the code as follows

AStar obstales1.png 10

where 10 is distance between nodes


The PDF version of the document can be found at