This is my submission for the Udacity Self-Driving Car Nanodegree Model Predictive Control (MPC) Project. You can find my C++ source code here. The goal of this project was to implement the MPC method to drive a car around a simulated track. A similar task was accomplished by my Behavioral Cloning and PID Controller projects.
I applied the MPC method to the—now familiar—task of controlling a simulated car’s steering and throttle.
The MPC method optimizes the car's trajectory based on its current state by calculating trajectories and their corresponding costs for different steering and throttle actuations. The actuations with the lowest associated cost are then executed. This process is repeated for the state following the actuations.
The state is a vector consisting of the car's position in and
coordinates (meters), heading
(radians), speed
(meters/second), cross track error
(meters), and heading error
(radians) at time
(seconds).
The actuator vector contains the steering (radians) and throttle
(unitless) actuations at time
.
They are bounded by the following limits:
The following equations predict the car's future state. They are used to solve the latency problem (explained below) and to constrain the future state and error equations that are sent to the optimizer.
The cost is a summation of the squares of the ,
, difference between current and reference velocity, actuations, and difference between sequential actuations for all timesteps
.
The number of timesteps and their duration was set to 10 steps and 0.1 seconds. This predicts the car's state 1 second into the future. During testing, I found this was enough for the car to anticipate a turn while not too far ahead as to cause the car to oscillate while driving on a straight portion of the track.
With less timesteps the car was not able to make sharp turns because the shorter predicted trajectory did not have to be as curved as the reference trajectory to yield a small cost, i.e. the predicted trajectory was more of a straight line tangent to the reference trajectory. With more timesteps the time horizon stretched too far ahead such that the car would turn on a straight portion of the track once a curve has met the time horizon. A shorter time step duration meant that the number of timesteps had to be increased in order to maintain turning performance and therefore, increase computational time. A longer time step duration resulted in less precise actuations, i.e. choppier steering.
Experimentation with different cost multipliers lead me to heavily weigh the heading error (x100), steering actuation (x100), and difference between sequential steering actuations (x1000). These three costs most significantly influence the car's steering and in turn can dampen steering oscillation.
Before returning the actuations to the simulator, the throttle is multiplied by a fraction inversely related to the steering angle: a steering angle of 0 degrees yields a multiplier of 1 and 25 degrees yields 1/6. This was the last parameter that I tuned to keep the car from flying off the track on sharp turns while still being able to reach high speeds on the straights.
throttle = solution.x[a_start] * (M_PI / (fabs(solution.x[delta_start]) * 36.0 + M_PI));
To replicate actuation latency, as in a real world self-driving car, the system has a delay of 100 milliseconds before the actuations are sent to the simulator. This causes a problem because the actuations for the car's predicted future state are no longer correct, e.g. a turn command when the car has already made the turn. The solution was to predict the car's state ahead of the latency using the current state and actuations before sending it to the MPC to predict the future trajectory and actuations.
With only manual weight tuning, the car managed to reach a top speed of 95 MPH while staying in control.
The MPC method is superior to the PID Controller in this application because it optimizes the car’s controls based on its current state and the planned path. As a result, the maximum attainable speed is almost doubled when compared to the results of my PID Controller project.