A* Algorithm for Path Planning

Introduction

In this article, we will look at the 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 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 an 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 minimum-cost path provides a path with shortest-distance from source to destination grid.

The input to simulation environment is provided in the form of an 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 the 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 predefined 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 the 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 reached 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 a 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, i.e., 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.

Admissibility

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 that 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$.

This 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 a 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 the routines required is to check if node is present in the closed list <script class="brush: cpp" type="syntaxhighlighter"> <![CDATA[ bool AStart::isClosed(GNode c) //iterate over elements in the closed list std::deque<gnode>::iterator it=closed.begin(); for(it=closed.begin();it!=closed.end();it++) GNode ix=*it; //if node is found return status if(ix.position==c.position) return true; return false; ]]</script>

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 highest priority node is placed at the 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 the proper location.

Also, if an 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.

<![CDATA[ bool addOpen(GNode &node) std::deque<gnode />::iterator it=open.begin(); 
std::deque<gnode />::iterator it1=open.end(); 
bool flag=false; //return status 
bool insert_flag=false;//insert flag //compute the cost of node being inserted 
float val2=node.getCost(goal,maxc); 
for(it=open.begin();it!=open.end();it++) //compute cost of current node in queue 
float val1=(*it).getCost(goal,maxc); 
if(((*it).position==node.position)) 
if(insert_flag==true) //reopening node it=open.erase(it);rccopen++; 
insert_flag=true; break; if(val2<val1 flag="true;" if(insert_flag="=false)" insert_flag="true;" it="open.insert(it,node);" &&insert_flag="=false)" /> 

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.

 <![CDATA[ bool isAccessible(GNode &n,GNode &n1,int mode) float dx=n.position.x-n1.position.x; 
float dy=n.position.y-n1.position.y; float mag=sqrt(dx*dx+dy*dy); 
dx=dx/mag; dy=dy/mag; Point2f p=n1.position; p.x=n1.position.x; 
p.y=n1.position.y; while(1) 
if(p.x==n.position.x && p.y==n.position.y) break; //current position and initial position 
float dx2=p.x-n1.position.x; float dy2=p.y-n1.position.y; 
float mag1=sqrt(dx2*dx2+dy2*dy2); //current position and goal position 
float dx1=p.x-n.position.x; float dy1=p.y-n.position.y; 
float dd=sqrt((dx1*dx1)+(dy1*dy1)); //goal position lies withing minimum grid resolution 
if(dd<=minr && mode==1) return true; //no obstacle found if(mag1>=mag) 
return true; prev=p; Point2f ff=Point2f(p.x,-p.y); 
if(ff.y >=obstacle_map.rows ff.x>=obstacle_map.cols ff.x<0 ff.y<0) n1.open=false; 
return false; int val=(int)obstacle_map.ptr<uchar />(((int)ff.y))[((int)ff.x)]; if(val>0) n.open=false; 
return false; //take small increment p.x=p.x+(1)*dx; p.y=p.y+(1)*dy; return true; ]]</script> 

Simulation

Simulation outputs can be found here.

Code

The files AStar.hpp, AStar.cpp define the AStart algorithm. The files gsim.hpp, gsim.cpp define the simulation environment. The code can be found here.

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

PDF

The PDF version of the document can be found here.