Motion planning module in Autonomous Vehicle — Mission planner
By: Dhanoop Karunakaran
Tags: Robotics, Autonomous Cars, Artificial Intelligence, Computer Science, Software Engineering

Source: [1]

The autonomous vehicle has the potential to change the way we live. Recently, there is an enormous amount of progress from the academic world and industry. Yet, there are many challenges to overcome. Nevertheless, the technology behind this fantastic technology always fascinates me. I want to open up a series of blogs that dedicates to this remarkable technology that can benefit me as a writer to bridge the more profound connection to the technology and support you as a reader to enhance the knowledge in this area.

In this blog, we are going to discuss one component(Mission planner) of the motion planning module in detail. In the next two sections, a brief introduction of software architecture and motion planning module in an autonomous vehicle is introduced. Let’s dig in.

The high-level software architecture of the autonomous vehicle

High-level overview of the software architecture

Mainly, there are three modules in autonomous driving systems: Perception, planning, and control. Perception modules are responsible for extracting the relevant knowledge from the surrounding world and estimate the state of the ego-vehicle and other objects. Planning module takes this as input and produces a collision-free trajectory for the control module. In the control module, they just execute the trajectory produced by the planner. In this blog, we are going to discuss a subcomponent of the motion planning module in detail.

Motion planning module

Sub-components of the motion planning module

As discussed earlier, the motion planning module responsible for generating trajectory from the start position to goal position. The problem is approached as a hierarchical optimisation problem where each layer has the correct level of abstraction to perform motion planning in real-time. At the top of the hierarchy is mission planner, and it is responsible for computing the optimal trajectory in the map level. The map level means there is no consideration of static object and dynamic object to calculate the trajectory and purely based on map level instruction such as distance to the destination, traffic rules, speed limit etc. The behaviour planner is responsible for deciding on what autonomous vehicle should depend on the current scenario. And the local planner computes the trajectory of the collision-free path and velocity profile based on the maneuver and the decision. The local planner considers the discrete search space that typically spans for 1 to 100 m ahead of the car. Finally, the motion plan is executed by the controller.

In this blog, we are going to discuss some of the algorithms that are widely used in mission planning.

Mission planner

The mission planning is the highest level task in motion planning. It is responsible for constructing the global level planning for reaching the destination. It is a map level planning that doesn’t need to deal with static and dynamic obstacles — however, road rules and other regulator aspect influences this planner.

Google map for navigation

For instance, we want to find a route from Darling tower to USYD. Our first response is to look at google map to see the high-level route for us to navigate. There can be multiple ways you can reach the destination. The search algorithm behind google map finds the best possible path in terms of time and distance. Similarly, the autonomous vehicle needs a high-level route planning to navigate. At this level, no need to deal with static and dynamic obstacles.

Usually, this problem is solved using the graph search algorithm. We construct a graph of vertices that will be a point in map and the edges that will be a road. The graph search method is employed to find the route to reach the destination. One of the well-known algorithm widely used in a similar context in the graph data structure is Breadth-First Seach(BFS).

Breadth-First Search(BFS)

A graph. Source: [2]

The algorithm traverses through breadthward motion. It means visiting all the adjacent unvisited vertices to current vertice before going further in-depth.

Example of a Queue

It employs a queue function to achieve this. In other words, all the vertices are processed in a First-In-First-Out(FIFO) manner. Let’s explain the algorithm with the above figure:

S is the root node/vertice. Pushes all the adjacent unvisited node of s to the queue starting from left. Now queue contains: [C, B, A] BFS uses FIFO. It means that A will process next and add all the adjacent unvisited vertices of A to the queue. Now queue contains: [D, C, B]. Next on will be and add E to the queue. Now queue contains: [E, D, C]. Now the turn of C to be process and adjacent unvisited vertice F is added to the queue. Now queue contains: [F, E, D]. If you pause here, think about what we have done so far is that the algorithm traverses through breadthward motion and processes all the adjacent unvisited vertices of S. This process continues until all the nodes/vertices of the graph visited/processed.

BFS cannot be directly used to find the shortest route as it does not have any cost function to identify the path has the quickest cost. But practical algorithms for finding the shortest path are based on BFS.

Djakithra’s algorithm

Djakithra’s algorithm is widely used to find the shortest path in a graph and was conceived by Edsger W. Dijkstra in 1956. The algorithm is applied in a weighted graph. The weighted graph is a graph in which each edge has a numerical weight. The overall concept is the same as BFS, and the main difference is in the order in which it processes the vertices. In this algorithm, the queue is based on the min-heap data structure. The min-heap data structure stores keys and values and sort the key based on the associated value from smallest to largest[3]. In our case key is the vertices and value is the distance is taken to reach the vertices, i.e. edge weight. The vertex with the smallest distance is processed first as compared First-In-First-Out(FIFO) strategy of BFS.

Consider the above figure to have a brief explanation of how the algorithm works:

S is the root node/vertice. Pushes all the adjacent unvisited node of s to the queue starting from left. Min-heap/priority-queue contains [B, A, C] in the order from smallest to largest based on the cost[6,5,3] Pushes all the adjacent unvisited node of C as it is first in the queue F is the next node that adjacent to c which contains the cost of 3+10 [F, B, A] = [13, 6, 5] A is the smallest now and D is the adjacent node of A which has the cost of 5+11=16 [D, F, B] = [16, 13, 6] B is the smallest now, and adjacent node E has the cost of 14 [D, E, F] = [16,14,13] F is the smallest and to next adjacent node of G has the cost of 18 [G, D, E] is the new order of min-heap = [18, 16, 14] G is also the adjacent node of E, and then we need to update the value of G if the cos is minimum. In this case, 19 is the cost of G through E and lowest cost is 18 and no need to update. [G,D] = [16.14] D is the next smallest item, and G is again the adjacent node. It has the cost of 17. Since 17 is the lowest cost, the path generated for that cost will be the shortest. The algorithm stops here as there are no other nodes to be visited/processed.

A* algorithm

A* is a faster shortest path compared to Djakithra’s algorithm. It will introduce a heuristic function in the search to make it faster and efficient than the previous algorithm. One of the drawbacks of Djakithra’s algorithm is that it has to process almost all the edges in the graph, and in reality, there is only a few of them needed to find the shortest path. It makes the algorithm computationally inefficient for larger problems. The significant difference to A* from the Djakithra’s algorithm is adding the heuristic function to the existing cost of Djakithra’s algorithm to search. In other words, A* select the path that minimises the below cost function.

The cost function of A*. Source: [4]

Where n is the next node/vertice, g(n) is the total edge weights from the start node to n(cost of Djakithra’s algorithm), and h(n) is the heuristic function that estimates the cost of the shortest path from node n to the goal.

Typically, the euclidean heuristic is being used in practice where it gives the euclidean distance or straight line between node n and goal.

One of the requirements of A* search is the cost computed using the heuristic function is always underestimate of the true cost. The heuristics that satisfy this requirement are called admissible heuristics.

If you like my write up, follow me on Github, Linkedin, and/or Medium profile.