Path planning is one of the most important primitives for autonomous mobile robots. The ability to be able to travel on its own by finding a collision free, optimal path is an important aspect of making robots autonomous. Path, as the name suggests is a set of waypoints which a Robot is expected to travel. There can be many criterions for deciding a path that the Robot should follow. Various optimisations, checks are made before deciding an optimial path.

Path planning for Autonomous Robots

Path planning, as illustrated above is an important aspect of autonomous robots. There are various methods how a path is planned. There are various algorithms on path planning. Some of the common features of path planners are:

1. Given a start and a goal position(or pose), give out a set of states(positions or velocities) that the robot should take to reach the goal from start.
2. The path generated should be collision free with the obstacles in the environment.
3. Generally the path generated should optimise some hueristic(or parameter).
4. The path generated should be traversable by a robot given its dynamics.

Path Planning algorithms

The problem to find an optimal path has been studied since many decades. There are many algorithms that are graph-based, sampling-based. Each branch follows a particular approach to solve the path planning problem.

1. Graph based algorithms:

Graph based algorithms overlay a topological graph on a robots configurational space and perform search for an optimal path. Some of the notable graph-based algorithms are:

• Dijkstra’s Algorithm
• A-Star (A*)
• D-Star (D*)

2. Sampling based algorithms:

Sampling based algorithms represent the configuration space with a roadmap or build a tree, generated by randomly sampling states in the configuration space. Some of the notable sampling-based algorithms are:

• Rapidly exploring Random Tree (RRT)
• RRT Star (RRT*)
• Informed RRT Star
• Batch Informed Trees Star (BIT*)

Sampling based Algorithms

Rapidly Exploring Random Trees

Salient Features

1. Randomly samples nodes in the Configuration space of the robot to build a tree of valid configurations.
2. It is Probabilistically Complete,having the probability to find a solution if it exists. In worst case, time taken to find a solution can be very long (longer than exhaustive search). The probability of finding a solution goes to $1$ as number of sampled nodes goes to $\infty$.
3. In practise, the algorithm tends to be very effecitve in high dimensional spaces.
4. There is no gaurantee regarding the optimality of the solution. The path produced my bot the the shortest path.
5. Post processing of the path generated is required as the path generated is often very unordered or in zig-zag fashion.

Collision Checking Function

One important requirement of sampling algorithms, is the ability to check if a configuration is valid or not. To check if a configuration $X$ is valid in a configuration free space $\mathbb{C}$, a function as such can be used:

Psuedo Code

def RRT(START, GOAL):
	TREE = []
	TREE.add(START)
	DELTA = maximum distance between sampled node and nearest node. 
	REPEAT n times:
		X = generateNewConfiguration()
		if X in FREE_SPACE:
			for nodes in TREE:
				Y = argmin(nodes, criteria=distance)
			if DIST(X, Y) < DELTA:
				Find a configuration Z that is at DELTA distance along the path from X to Y
				if TRAVERSABLE(X, Z):
					X.parent = Y
					TREE.add(X)
			else:
				if TRAVERSABLE(X, Y):
					X.parent = Y
					TREE.add(X)
			if X is GOAL:
				report "SUCCESS"
				break

Notations and Functions used in Psuedo Code:

• Function used to check if a path is traversable:

• In case of Rotations:

Graph based Algorithms

Dijkstra’s Algorithm

Dijkstra’s Algorithm is an algorithm for finding the shortest path between one source node and all the other nodes in a graph, thereby producing a shortest-path-tree.

Psuedo Code

# Set the distances if all nodes in the graph to infinty
for node in graph:
    node.distance = INFINITY

# Create an empty list
nodes = []

# Set the start distance to ZERO
START.distance = 0

# Add start to the list
nodes.add(START)

# Loop to update distances
while nodes is not empty:
    CURRENT = argmin(node, criteria=node.distance)

    if CURRENT == GOAL:
        report "Success"
        break

    for adjacent_node in CURRENT.adjacent_nodes:
        if adjacent_node.distance > CURRENT.distance + cost of edge from CURRENT to adjacent_node:
            adjacent_node.distance = CURRENT.distance + cost of edge from CURRENT to adjacent_node
            adjacent_node.parent = CURRENT

            # Add adjacent_node to the list, if it is not already present
            if adjacent_node not in nodes:
                nodes.add(adjacent_node)

A-Star

A-star is a graph-based, path search algorithm. It is used in many fields of computer science as a search algorithm. It is often used due to its completeness, optimality, and optimal efficiency.

Salient Features of the Algorithm

1. Resolution complete and Resolution optimal : The algorithm finds the optimal solution to the given problem at a chosen discretization, if one exits.
2. A-Star uses a hueristic to estimate the total cost of a solution constrained to pass through a state. Thus, it searches in order of decreasing solution quality and is optimally efficient.
3. Any other optimal algorithm using the same hueristic will expand at least as many vertices as A-Star.

   Idea of Hueristics Functions

   • Hueristic functions are used to map every node in the graph to a non-negative value.

   • Criteria for Hueristics Functions

   • Should be a monotonic function
   • Should satisfy $H(goal) = 0$
   • For any two adjacent nodes $x$ and $y$: - $H(x, y) \leq H(y) + d(x, y)$
     - $d(x, y) = EdgeCost(x, y)$
   • These properties ensure that for all nodes $n$: - $H(n) \leq length of Shortest Path(n, GOAL)$

• For path Planning on a grid:

  • Euclidean Distance: $H(x_n, y_n) = \sqrt{(x_n-x_g)^2 + (y_n-y_g)^2}$
  • Manhattan Distance: $H(x_n, y_n) = \lvert(x_n - x_g) + (y_n - y_g)\rvert$

Where $x_n$, $y_n$ and $x_g$, $y_g$ are the $x$, $y$ coordinates of a the node and the goal respectively.

Psuedo Code for the Algorithm

Here is the most basic version of A-Star search algorithm.

def Astar(start, goal, graph):
    # Set the g, f values for all nodes in the graph
    for node in graph:
        node.f = Infinity
        node.g = Infinity

    # Create an empty list to store visited nodes
    nodes = []

    # Add Start to nodes list
    nodes.add(start)

    # Loop to traverse the graph
    while nodes is not EMPTY:
        # Obtain bode with the least f-value
        CURRENT = argmin(node, criteria=node.f)

        # Check if current node is the goal Node
        # which means the graph has been completely traversed
        if CURRENT == goal:
            report "SUCCESS"
            break
        # Update parameters for adjacent nodes
        for adjacent_node in CURRENT.adjacent_nodes:
            if adjacent_node.f > CURRENT.g + cost of edge from CURRENT to adjacent_node:
                adjacent_node.g = CURRENT.g + cost of edge from CURRENT to adjacent_node
                adjacent_node.f = adjacent_node.g + H(node)
                adjacent_node.parent = CURRENT

                # Add the adjacent node to nodes list if not there already
                if adjacent_node not in nodes:
                    nodes.add(adjacent_node)

Notations in the Psuedo Code explained:

g-value = distance between a node and the start node
H-function = Hueristic funciton
f-value = g-value + Hueristic value of the node

Additional References

1. For a better understanding of the path planning problem refer here.
2. Understand configuration spaces from this video.
3. Original paper on A-Star path planning algorithm.
4. Psuedo Code can be found here
5. Video explainig A-star can be found here
6. Psuedo Code for Dijkstra’s Algorithm can be found here
7. A video explaining Dijkstra’s Algorithm can be found here
8. Refer this article for more information about RRT and RRT*
9. A video explaining RRT algorithm.
10. Refer to the paper here