Helping autonomous vehicles learn to drive
Insights from completing Udacity’s Path Planning project
I just completed the Path Planning project for Udacity’s Self-Driving Car Engineer nano-degree program. Path planning is a key piece of the self-driving car software stack, and it’s been fun to see it in action. My code is available here.
Self-driving cars run many different algorithms simultaneously to operate safely on the road. These algorithms are modular and pass data among themselves in a hierarchical fashion. Modules at the bottom of the hierarchy operate at fast timescales, while ones at the top operate at slower timescales.
The diagram below illustrates this. The modules at the bottom of the stack are those that control the physical motion of the vehicle (steering, braking, acceleration) and ingest and integrate data from the sensors (lidar, radar, etc.) to form a quasi-real-time model of all nearby objects (sensor fusion). Because hundredths of a second might matter in a life-or-death traffic scenario, these modules operate with the highest frequency, updating extremely rapidly.
Above them is the Localization module, which determines the car’s physical location and orientation. GPS alone isn’t precise enough for this, so localization combines GPS with sensor data and a basemap to narrow down the car’s location/orientation more precisely. Because the car generally moves smoothly, its location usually changes in predictable ways, and this module only needs to update on a medium timescale.
Skipping the Trajectory module for the moment, the final two modules are Prediction and Behavior. Prediction attempts to forecast where other objects will be in the future, including vehicles, pedestrians, and even animals. Generally, this is based on extrapolating their current position, velocity and acceleration. The Behavior module determines the overall actions the car will take, such as changing lanes, accelerating, or slowing down to make a turn. Since the car shouldn’t keep changing its overall actions very frequently, this module operates at a relatively slow timescale.
The decision from the Behavior module is passed to the Trajectory module, which converts the overall behavior (which might be called “strategic”) into a defined set of points on a corresponding specific trajectory (which might be called “tactical”). If a self-driving car’s OS were the crew of a ship, the Sensor Fusion module would be the lookout, the Trajectory module would be the helmsman, and the Behavior module would be the captain.
Implementing path planning
The “path planning” uber-module encompasses the Prediction, Behavior and Trajectory modules. In this Udacity project, the goal is to drive a (simulated) car around a 4-mile, 3-lane highway loop in the presence of other cars, while staying as close to the speed limit (50 mph) as possible, passing cars as appropriate, staying on the road, avoiding excessive acceleration and jerk, and — the most important goal! — avoiding collisions. The Udacity simulator provides localization and sensor fusion data and implements motion control.
If a self-driving car’s OS were the crew of a ship, the Sensor Fusion module would be the lookout, the Trajectory module would be the helmsman, and the Behavior module would be the captain.
My path planning algorithm approaches this task by first predicting the position of other cars and checking whether the car is getting too close to a car ahead in the same lane (the Prediction module). If so, it checks if adjacent lane(s) are open for passing.
This information is passed to the Behavior module, which chooses what to do in the given situation. If we are too close to a car ahead of us and passing is feasible (and safe) this is the chosen behavior. Other possible behaviors are to speed up, slow down or stay at the same speed; or to attempt to return to the center lane if we’re not in it.
The chosen behavior is passed to the Trajectory module, which uses previous trajectory points that the car has already passed and future ones in our chosen lane to calculate a spline giving a smooth set of path points to follow. These are passed back to the simulator, which handles the motion control to achieve the vehicle following those points.
Reducing oscillation
A common problem in path planning (which many human drivers have experienced) is what to do when following a slower car, while being blocked from passing. Under these conditions my car tended to repeatedly speed up and fall back as it tried to get back to 50 mph and then detected it was approaching a leading vehicle.
To handle this, I added an additional behavior that detected when these oscillating “fallbacks” were occurring by timing the spacing between velocity minima. If this hit a certain threshold the car entered “slow-down mode” in which it intentionally lowered its speed to fade back, in the hopes that a passing lane would open up. This was sometimes successful and sometimes not — the same as in real highway driving.
Performance of the path planner
My path planning algorithm successfully drove the vehicle around the track without any collisions, road departures, excessive acceleration and jerk, or other problems. Overall, the project gave me good insight into one approach to high-level path planning (essentially a finite state machine encompassing high-level highway-driving behaviors). This isn’t necessarily the best way to approach this problem and tends to fail in less structured environments such as parking lots, but it still performs remarkably well given the relative simplicity of the code.
The original repo for this project from Udacity is available here.
