Autonomous Vehicle Path Planning
With Finite State Machines
David Rose
Overview
Path planning is one of the most difficult areas of development for autonomous vehicles as it involves an ensemble of various systems that must work together. It relies on sensory input to perceive the world around it and to subsequently output controls to see the computations to fruition. This creates an ongoing loop of operation that will be in operation until the car has arrived at its destination.
A car could have separate models for the various situations it may encounter such as: intersections, highways, parking lots, construction zones, etc. Different parameters of operation will be in effect for each of these. For this project I will describe a model I have written that involves a 3-lane road with no exits/entrances, and multiple other vehicles that may be going at different speeds.
It is a simplistic model, but it captures the essence of what is referred to as a finite state machine. Below is a visual representation of a 3-state machine that could represent basic human functioning. There are specific actions to take depending on your current state of being:
For this project the states involved are: speed up, hold speed, slow down, keep lane, change lane left, or change lane right.
Using the Unity game engine and C++, this program attempts to complete infinitely many laps around the course without breaking a few ‘rules’ I have put in place. These include:
- Speed limit — 50mph
- Max Acceleration — 10 m/s²
- Max Jerk (as a derivative of acceleration) — 10 m/s³
- Don’t hit other cars
- Stay on the road
Other than that, go as fast as possible!
In the 1940’s the US Air Force tested the different effects of high g-forces (acceleration) on the body, using Col. John Stapp as a test subject. He strapped himself to a rocket sled and reported his reactions. In the process he experienced >45g and proved these situations are survivable, though he suffered numerous injuries such as: headaches, concussion, a fractured rib and wrist, and a hemorrhaged retina.
How to Handle State Changes
This code block below sets the lane variable for other cars on the road by detecting the number of meters from the left-most edge of our direction (so 0 is the center-line dividing the two directions). For the sake of this project this information is known to us, but in the real world it must be gather via various perception methods.
What lane are the other cars in?
Using some finite state calculations, we can set the lane variable for other cars:
In the code above ‘d’ represents the distance from the center-lane in meters for another vehicle. So 0 represents the middle of the road, while 4–8 represent the middle lane in our direction (lanes are 4 meters wide), and so forth.
How near/far are the other cars?
With that lane information we can now analyze the near/far front/back positioning relative to ours:
Now that we know the lane of other cars, we would like to know the longitudinal distance. In this case we would like to know if a car is within 30 meters in front or back of us.
With this knowledge we can now use it in our next finite state machine in deciding how to act when coming upon on of these situations, such as changing lanes or slowing down to avoid hitting another car. In the code above we decided if a car was in a 60 meters buffer front/back, and below we can use that in our finite state machine to decide whether to change lanes, hold lane, or speed up/down.
Below is an example a vehicle state machine for driving down a highway:
The rest of the code is fairly straightforward in that I just need to follow the actions I decided on above, without breaking any of the rules defined at the beginning. the max_vel variable is set at 49.5 m/s, while max_acl is set at .224 m/s², which keeps the car under stated comfort levels of acceleration.
Splines, or how to smooth out discrete trajectory points
From the image above, you can see how the discrete points (‘P’) are smoothed out. This is a polynomial interpolation. These can be defined using an arbitrary amount of polynomials but in this case we will be using 5 (a quintic spline).
In the image below, you can see a bit of the curvature that it may produce. As the time horizon is low and road relatively straight, there is not much curvature to be found in this particular project.
Without this method, the car will be attempting to change positions as fast as mechanically possible to each new subsequent desired position and heading, which being a simulation in this case, is instantaneously. Fortunately, the code here is super simple as I just included the spline library, so I only need to write this:
With the spline ready, I can populate the next desired coordinates for my car using the following code block:
With the new X/Y coordinates inserted back to the Unity game engine via the JSON output, my car now smoothly follows my intended path, driving nice and safe!
