Implementing a Model Predictive Control for a self-driving car

Udacity Self-Driving Engineer Nanodegree. Term 2, assignment 4

We’re now approaching the end of term 2 of the Udacity Self-Driving Engineer Nanodegree. This term has been all about hardcore robotics. It’s been a blast to learn how to build the core robotic functions of an autonomous vehicle system: sensor fusion, localization, and control.

This last module was built and presented by the team from the Uber Advanced Technologies Group. They presented the Model Predictive Control as one of the core control algorithms that autonomous vehicles use.

Model Predictive Control

The MPC considers the task of following a trajectory as an optimization problem in which the solution is the path the car should take. The idea is to simulate different actuator inputs (steering, acceleration and braking) and predict a resulting trajectory by selecting the one with the minimum cost. The car follows that trajectory and gets new input to calculate a new set of trajectories to optimize. The model utilizes the called “receding horizon controller” which performs a trajectory recalculation for every new state, since the defined trajectory is just an approximation.

Trajectory

The trajectory parameters are the number of time steps N separated in time bydt. It’s not necessary to use a large number of steps since the algorithm recalculates the trajectory on every step. Besides, a large N is more costly to compute. Running the algorithm with values greater than 20 caused the car to go off-track. Same for dt, smaller time steps are more costly but larger values mean a lot of things happen between each calculation. Larger values, of for example, 0.1 caused the car run off-track.

// Set the timestep length and duration
size_t N = 12;
double dt = 0.06;

Vehicle Model

This is the set of equations describing the system behavior the updates across dt. In this case we used a simplified kinematic model defined by a state of six parameters:

• x, y — position.
• psi(ψ) — orientation.
• v — velocity.
• cte — cross-track error. The difference between the trajectory defined by the waypoints and the current vehicle position y in the coordinate space of the vehicle.
• epsi (eψ) — orientation error.

// equations for the model:
x_[t] = x[t-1] + v[t-1] * cos(psi[t-1]) * dt
y_[t] = y[t-1] + v[t-1] * sin(psi[t-1]) * dt
psi_[t] = psi[t-1] + v[t-1] / Lf * delta[t-1] * dt
v_[t] = v[t-1] + a[t-1] * dt
cte[t] = f(x[t-1]) - y[t-1] + v[t-1] * sin(epsi[t-1]) * dt
epsi[t] = psi[t] - psides[t-1] + v[t-1] * delta[t-1] / Lf * dt

Actuator constraints

These are limiting parameters defined by the design of the vehicle and fundamental physics — e.g. a car never makes a hard 90° turn. This is called a nonholonomic model. In our case:

• a — acceleration is in the range [-1, 1] = [full brake, full throttle]
• delta — steering angle is in the range [-25°, 25°]

Cost Function

The cost function penalizes state — velocity, cross-track and orientation errors. We also include the control input so to penalize the magnitude of the input and its change rate. This will allow for some temporal smoothness (e.g during lane changes).

To calculate the total cost we include a constant multiplier to each factor. We’re able then to refine the car behavior in the simulator.

const double cte_weight = 5000;
const double epsi_weight = 5000;
const double v_weight = 10;
const double actuator_cost_weight = 5;
const double steer_rate_cost_weight = 200000;
const double accel__rate_cost_weight = 10;

Latency

A latency of 100ms was artificially included before sending actuations to the simulator to mimic typical response times for real world cars. s

The latency factor was then included in the state vector before sending it to the model calculation.

// Latency
const double delay_t = 0.1;
// Future state considering latency into
double delay_x = v * delay_t;
double delay_y = 0;
double delay_psi = v * -steer_value / Lf * delay_t;
double delay_v = v + throttle_value * delay_t;
double delay_cte = cte + v * sin(epsi) * delay_t;
double delay_epsi = epsi + v * -steer_value /Lf * delay_t;
// State values
Eigen::VectorXd state(6);
state << delay_x, delay_y, delay_psi, 
         delay_v, delay_cte, delay_epsi;
auto vars = mpc.Solve(state, coeffs);

Simulation

The car manages to stay close to the limit set in the program of 70mph