An Introduction To Graph Based Planning In Static Environments

I was always fascinated about robotics since my early school days.With the advent of big companies like Google and Tesla,robotics moulded in the shape of self driving cars was no longer a fantasy.As an undergraduate student of IIT Kharagpur, I was fortunate enough to get into the Autonomous Ground Vehicle Research Group,which has given me a lot of exposure to the notion of automation.I spent a month of my first year summers working with the team during which I happened to stumble upon the problem of Path Planning.I’ll describe two of the simplest graph based algorithms that can achieve this feat.

The code for these planners is available here:(https://github.com/shreyas-kowshik/Planning)

The first algorithm that we’ll talk about is Djikstra’s algorithm.It is amongst the most popular graph search algorithm that is used to obtain the shortest distance between two nodes in a graph with positive weights.For our purposes,we’ll take the map to be a 2D image where obstacles are white in colour.Each of our node is a pixel in the image and it is connected to it’s neighbours by edges that have weights corresponding to their distances.Our Problem Formulation is : “To find the shortest path between any two co-ordinates of the map such that is does not intersect any obstacles”.

Djikstra’s Algorithm

The algorithm is pretty self explanatory.It simply selects the next node with the minimum distance and explores all possibilities and finds the most optimal path.One thing I’ve not added above is initialise the distance of all nodes other than source to infinity. Here are the results obtained:

Path planned from top left point on path to bottom right (The origin is the top left of the image with x axis as topmost row and y-axis as leftmost column)

The path above is indeed the shortest one and this guarantees optimality. But this comes at the cost of searching the whole space for all possible sets of paths. As the distance between the source and the goal increases, the algorithm becomes slower.

Image showing all the visited nodes by grey colour.Clearly almost the entire space has been searched by Djikstra for finding the path.

Djikstra has no incentive at any step to go towards the goal.All it does is updates the distances and the parents and moves on. The result is thatFor any path planning problem, we are more concerned between the goal rather than other nodes and so we need an algorithm that smartly decides it’s next step so as to reach the goal faster and in an optimal way.

A* Algorithm

The A* algorithm overview

Djikstra in a way was minimising a function g = distance_from_source. A* minimises a different cost function given by : f = g + h. Here h is called a heuristic function. It can be any function designed to guide the path to the goal. Generally the euclidean distance from the goal is taken to be the heuristic. Our planning problem now is the same a Djikstra but just that we use the function f for finding the appropriate nodes in the path. A* is much more flexible than Djikstra in a sense that it is more goal oriented to suit our need whereas Djikstra is more generic in nature. A* quits once it reaches the goal unlike Djikstra which keeps on iterating on the whole space even if you move from (1,1) to (2,2)!

A* output for the same source and goal

Grey Coloured Nodes Show All The Expanded Nodes

So we have gained on speed here, so we must have lost upon something right? What we have lost on is optimality. A* does not give the shortest path always. However there are tricks to control the time-cost tradeoff. If you carefully notice A* is a sum of two algorithms…The first one is Djikstra and the second one is a greedy based algorithm called Best First Search that greedily chooses the next node that is nearest to the parent. By controlling the relative amounts of g and h,we can make A* as close to Djikstra or as greedy as possible.

In fact by setting h = constant,and removing the greedy goal reached condition,you’ll see that A* gets converted to Djikstra.

A* output for a constant heuristic (Mathematically equivalent to Djikstra)

A* Expanded Nodes For The Above Case Are The Same As For Djikstra

It is because of it’s inherent intelligence that A* has found applications for path planning in games,and even Self-Driving-Cars for that matter (in the form of Hybrid A*).

The notion of writing a planner judged by minimising some cost mathematically and having weights to control relative trade-offs is very common even in sophisticated planners. The idea of this post was to analyse the simplest planners one can start with to see how such algorithms work.

Repository for the code https://github.com/shreyas-kowshik/Planning