Path Planning for self-driving cars

9 min readJul 17, 2018

This article has been updated on https://www.thinkautonomous.ai/blog/?p=path-planning-for-self-driving-cars

Autonomous Navigation

This article follows :
- AI… And the vehicle went autonomous
- Sensor Fusion
- Self-Driving Cars & Localization
We’re now at the Path Planning step in which our car uses its knowledge of the environment and its position to plan trajectories.

In Planning, we now make decisions. This subject is one of the most difficult of all, it’s about implementing the brain of an autonomous vehicle.

This image is a popular moral problem where we must choose who to sacrifice if we have no brakes. Although unlikely, this topic makes us wonder how a self-driving car would do if it had to take its own decisions.

Before we start, I invite you to join the Think Autonomous Mailing List and learn every day about self-driving cars, Computer Vision, and Artificial Intelligence.

Predictions

The first step is about predicting what each element of the environment will do a few seconds in the future. A pedestrian might be moving while a traffic sign would probably stand still.

We can use two approaches:

For each possible situation (highway insertion, intersection, …), model all possible trajectories for a vehicle.
Use Machine Learning to, according to current observations, establish a similarity with the training data and thus correlate this to a trajectory.
Model-Based approach

When modeling a trajectory, we have to imagine all the possible choices for the car we track. Take the case where we get on a highway.

The detected vehicle can do several things:

Stay in its lane, which means :
- Speed Up,
- Slow Down, to let us pass in front of him,
- Stay at constant speed, and ignore us
Change lane, which would make it easier for us.

So we have four possible situations to define for a motorway insertion. Our sensors work in real time so it is possible to decide whether a vehicle changes gears or lanes simply by calculating its position and speed at every moment.
In our case, we are updating four possible scenarios, and thus have a multimodal distribution. This means that each scenario has a probability that changes with the observations we do.

This technique implements the feasibility of a trajectory, and therefore eliminates impossible behaviors. It only focuses on what is possible and not on what has been done in the past.

2. Data-driven (Machine Learning) approach

This approach is very different. Like every Machine Learning algorithm, we define two stages, a training phase and a prediction phase.

The training phase gathers massive data on the history of vehicles and learns from these data. We can have hundreds of vehicles that have done hundreds of different behaviors in an intersection.

We perform unsupervised learning. Clustering algorithms help us to define, for the current observation, which trajectory group the vehicle is approaching. As a reminder, clustering is a technique where we define a number of clusters (trajectories) and where we ask an algorithm to indicate which data are similar.

Each cluster is actually a typical trajectory that a vehicle can follow. The advantage of this technique is to rely on data and therefore on past scenarios. The more we drive and collect data, the more precise we will be in estimating behavior.

These two approaches are very different and actually reflect the reality of the autonomous vehicle industry. While some rely on deterministic cases with mathematical prediction, others rely on statistics using artificial intelligence. This choice of companies is more broadly extended to many issues such as the perception using LiDAR against the perception using cameras.

Decision-Making

Once we have an estimate of the future of the environment, we can make a decision. How to brake if a pedestrian is detected? How to accelerate or change lanes?

The first thing we have to do is environment classification. The choices are not the same whether we are driving on a highway or in a parking lot. Several criteria are taken into account when generating a trajectory, in particular safety, feasibility, efficiency and legality. Other variables can also be taken into account such as passenger’s comfort.

Finite-State Machine

The first decision-making method that can be used is a finite state machine. The principle is to define, according to the situations, the possible states of a car. On a highway, the state of a car may be to stay in a lane, change lanes to the left, or change lanes to the right. Depending on the traffic conditions, we change state to, for example, overtake a car.

The choice of states is usually made using cost functions. For each possible scenario, we calculate independent costs (distance to obstacles, legality, …), and add them up. The lowest cost scenario wins.

Here we define what is important. We cannot do an impossible or dangerous maneuver.
Total_Cost = Feasibility_Cost * 5+ Security_Cost * 4 + Legality_Cost * 3+ Comfort_Cost * 2 + Speed_Cost * 1

In the cost function for speed, we do not want our vehicle to be too slow or exceed the maximum speed allowed. We therefore define a decreasing cost according to the speed then maximum after the speed limit.

Decision making is a very delicate subject when talking about autonomous driving. We must take into account the current situation and decide on everything that can be done from this point. Then we must weigh the pros and cons of each possibility and finally choose the best solution.

Trajectory Generation

The final step is trajectory generation. In this step, it is necessary to use a different coordinate system than the Cartesian coordinate system. For good reason, the cartesian coordinate system takes into account the dimension (x; y) but does not make sense if we want to find one’s bearings in relation to the road. The Frenet coordinates contains two axes, an s axis indicating the advance relative to the track and a d axis indicating the distance to the center of the lane. This marker is the one on which we base ourselves to estimate if our trajectory deviates from the center of the lane or if a vehicle is ahead of us or behind us.

When we take the decision to overtake a vehicle, the algorithm generates several trajectories for a decision and chooses the best one according to the criteria of feasibility, safety, legality, efficiency, comfort, …
In this case of overtaking, the red / orange trajectories are dangerous, the yellows are acceptable but incomplete, the green is the most efficient and safe.

To generate this trajectory, we create a level five polynomial that passes through waypoints. Waypoints are points on the way that contain 3 dimensions :
S: The longitudinal distance
D: The lateral distance
T: The moment at which one must pass through this point; giving speed

A trajectory is a curve that goes through all these points. These points are positioned in space and time. They tell us when to move to a specific (x;y) position and how fast. If you want to brake at a pedestrian crossing, we create points up to the pedestrian crossing and set a decreasing velocity to the speeds of the points up to the stopping zone.

Path Planning

We have just studied the generation of low-level trajectories. What about higher level? How to decide which street to take?

What are other algorithms ?

We have several families of algorithms to plan a path from a starting point to an end point. In these algorithms, we consider the world to be a grid containing obstacles, a starting point, and a goal.

Among the different families, we will focus on two in this article.

Sampling-Based algorithms
Reinforcement Learning algorithms

— Sampling-Based algorithms

They are very popular since they are very efficient in terms of calculation time. We have two types of algorithms: Discrete and Continuous.

In the discrete planner, we consider the world as a grid. We can mention the algorithms of breadth-first search, Dijkstra, A* … that can find the shortest way very quickly and without necessarily exploring the entire map.

In Dijkstra’s algorithm, we explore a lot of possible paths and finally choose the path that contains the fewest steps to arrive at a destination.

A*
In the very popular A* (A-star) algorithm, we only explore part of the map using a heuristic function. At each point of the map, we indicate the distance to the objective. Rather than systematically exploring every possible path, A* chooses to explore only paths that bring us closer to the goal.

A* is a variant of Dijkstra’s algorithm by focusing on finding the best path to a specific location by doing the minimum amount of work. It is thus very efficient in a self-driving car. This animation shows A* in action.

In a continuous planner, we don’t discretize the world, it’s continuous. Algorithms like Hybrid A*, Rapidly-exploring Random Tree (RRT), and many more implement this.

Hybrid A*
Hybrid A * tries to get closer to the reality of the continuous world by breaking down the movement. Instead of going from box to box, we try several short moves and always choose the one that brings us closer to the goal. This means that the algorithm is both discrete and continuous at the same time. The trajectories generated can also be more fluid because they take into account maximum steering angles, physical trajectories, …

— Reinforcement learning

A more and more popular approach is reinforcement learning. This machine learning technique consists of learning from experience. If we want to turn right, we ask the car to make a random choice, if it is good, it receives a positive reward, if not, a negative. Over the course of the training, the car is able to learn what has caused a positive reward, and to reproduce it. This technology is at this day the one that comes closest to human learning.

Thanks to Reinforcement Learning, we taught AIs to walk, to run, to beat the Go’s World Champion,…

It is possible to integrate this technology in robotics and especially in autonomous vehicles. The MobilEye planning system works with reinforcement learning. The recent demo of Wayve perfectly demonstrates the use of this concept.

(source)

Results

As part of my Nanodegree on autonomous vehicles, I realized a project on highway driving. I had to develop an algorithm that could drive 7.5 km among other vehicles on a highway. The Finite-State Machine introduced earlier is used to overtake a slow vehicle or slow down if overtaking is not possible.

Conclusion

Autonomous navigation is an exciting subject. We drive with our intuition and our eyes respecting the rules of the road. To reproduce this in a computer, you have to go through all the steps that we have done. We must see, position ourselves, predict the behavior of other vehicles, and finally make a decision by integrating constraints such as the law or the comfort of passengers. Behind the machine, a human tells what are the actions we must priviledge in certains situations. The machine only reproduces what it’s taught. This subject leaves room for a very large number of research and experimentation works. It allows to reach the level 5 of autonomy and democratize permanently the arrival of self-driving vehicles.