/model-predictive-control

Code for controlling a car in a simulated environment using a kinematic vehicle model.

Primary LanguageC++

Model Predictive Control

Driving a car in a simulator using a predictive model based on the car's kinematics.

Driving car in isimulator

About

This repository contains the code for model predictive control project of Udacity's Self-driving Car Nanodegree. Model Predictive Control or MPC casts the problem of controlling a system, in this case a car, as an optimization problem. The MPC algorithm takes as input a reference trajectory and a vehicle model and produces a set of control inputs, steering angle, brake, throttle etc., that minimize a cost function over a finite number of timesteps, called the horizon.

The input model used here is a kinematic model which contains parameters like speed, position, heading etc. A more comprehensive dynamic model would also include forces, like friction and centripetal force and the torques produced by these forces. Dynamic models are computationally heavy.

The cost function used here includes the lateral deviation of the car called cross-track error, the difference between the actual and a reference velocity, and the error between the desired heading and the actual heading. Additionally steering angle and throttle value is added to the cost function so that thes variables do not assume very large variables during the optimization.

Getting a Reference Trajectory

A reference trajectory is needed to calculate the initial values of the state variables. The reference trajectory is computed in two steps from raw waypoints.

  1. The waypoints are transformed from the a global coordinate system to the car's coordinate system with x axis pointing in the direction of the car's motion and y axis 90 degree clock wise from x axis. This also means that the current position of the car becomes x=0, y=0 and psi=0. This simplifies a lot of computation and plotting.

  2. A third order polynomial is fit to the transformed coordinates. Such a ploynomial is flexible enough to fit almost all kinds of trajectories without being to wiggly.

Building a Model.

The MPC algorithm iimplemented here uses the velocity (v), x position (x), y position (y), heading (psi), cross-track error (ct_err), the heading error (psi_err) as input state variables. Steering angle (delta) and throttle (a) as additional state variables.

The MPC algorithm MPC::solve() takes the initial values of the input state variables (v, x, y, psi, ct_err, psi_err, delta, a) and wraps them using the CppAD::AD type. The type is defined in the CppAD library and allows us to build diffferentiable types from ordinary values. The algorithm also computes future values of these variables (Cpp::AD type) over N timesteps, each dt seconds long, using the equation of kinematics:

x_{t+1} = x_{t} + v_{t} * cos(psi_{t}) * dt;
y_{t+1} = y_{t} + v_{t} * sin(psi_{t}) * dt;
psi_{t+1} = psi_{t} * v_{t} * delta_{t} * dt / Lf
v_{t+1} = v_{t} + a_{t} * dt
cte_{t+1} = cte_{t} + v_{t} * sin(psi_{t}) * dt
epsi_{t+1} = epsi_{t} + v_{t} * delta_{t} * dt / Lf

Lf = Distance between the center of gravity of the car and its front axle.

The MPC algorithm then calculates a cost function that is the sum of squares of quantities that we want to minimize, for example the sum of square of ct_err values and psi_err over all N timesteps.

cost =  (cte_{t}^2) + (cte_{t+1}^2) + (cte_{t+2}^2) + ... + (cte_{t+N-1}^2)
cost += (epsi_{t}^2) + (epsi_{t+1}^2) + (epsi_{t+2}^2) + ... + (epsi_{t+N-1}^2)

Additional terms have been added to the cost function to ensure that optimization does not produce very large values of the variables or abrupt changes in the values from one timesptep to the next. For example the square of the steering angle at all N-1 timesteps is also added:

cost += (delta_{t+1}^2) + (delta_{t+2}^2) + ... + (delta_{t+N-1}^2)

The MPC algorithm then minimizes the cost function with respect to all the variables, using the dependency generated by the equations of kinematics to constrain the possible values of the variables.

Afte the optimization is over, the values of the steering angle (delta_{t}) and throttle(a_{t}) are used to control the vehicle for a short time period, after which the MPC algorithm is rerun with the updated values of the state variables.

Hyperparameter Tuning

N and dt, as described above, are hyper parameters of the algorithm. The granularity of the MPC algorithm depends on these hyperparameters and in turn affects the stability and accuracy of the car's motion. These parameters depend to a large extent on the vehicles velocity and the latency (fixed at 100ms). For a reference velocity of 40m/s the values of N=15 and dt=0.1s were found using trial and error. The initial values of N=60 and dt=0.05s produced oscillation and the car overshot the track several times for a reference velocity of just 20m/s. Later N=20 and dt=0.1 worked well at 20m/s but the car left the track at sharp turns becasue of predictions being too far ahead in future. So N was reduced to 15 while dt was maintained at 0.1, which translates to a 1.5s look ahead.

If N*dt is made too small, the prediction horizon shrinks close to the present time and optimal values of steeing angle and throttle become obsolete by the time they're calculated. If N*dt is very large the cost is based on predictions far ahead in the future and the values of steeering and throttle are not so relevant to current time and the computation time of the algorithm also increases.

In addition to these explicit hypermaters, different components of the cost variable were multiplied by different weights. The sums of the steering angle and throttle were emphasized some 50X more than other variables so that they don't grow too large. For exact values of these hypermaters please refer to the code.

Installation

Dependencies

Basic Build Instructions

  1. Clone this repo.
  2. Make a build directory: mkdir build && cd build
  3. Compile: cmake .. && make
  4. Run it: ./mpc.