Model Predictive Control

Self-Driving Car Engineer Nanodegree Program

Overview

Final project for the second term of the UDACITY's Self-Driving Car Nanodegree. Implementing a Model Predictive Controller capable of driving a car in a simulated environment by controlling both the steering wheel and the acceleration pedal.

Dependencies

cmake >= 3.5
make >= 4.1(mac, linux), 3.81(Windows)
gcc/g++ >= 5.4
uWebSockets
- Run either install-mac.sh or install-ubuntu.sh.
- If you install from source, checkout to commit e94b6e1, i.e.
```
git clone https://github.com/uWebSockets/uWebSockets
cd uWebSockets
git checkout e94b6e1
```
  Some function signatures have changed in v0.14.x. See this PR for more details.
Ipopt and CppAD: Please refer to this document for installation instructions.
Eigen. This is already part of the repo so you shouldn't have to worry about it.
Simulator. You can download it from here.
Not a dependency but read the DATA.md for a description of the data sent back from the simulator.

Basic Build Instructions

Clone this repo.
./clean.sh
./build.sh
./run.sh

Rubric Criteria

Vehicle Model

The state of the vehicle is defined as:

state = [x, y, ψ, v, cte, eψ]

where:

x, y, ψ represent the position and orientation of the car respectively
v represents the current velocity of the vehicle
cte and eψ represent the Cross Track Error and Orientation Error respectively.

The controller will drive the vehicle using two actuators: the steering angle δ and the acceleration a.

actuators = [δ a]

Given the control inputs [δ a] and an estimate of the elapsed time between actuations dt, we can update the state of the vehicle using the following equations:

x_t+1 = x_t + v_t * cos(ψ_t) * dt
y_t+1 = y_t + v_t * sin(ψ[t]) * dt
ψ_t+1 = ψ_t + v_t / Lf * δ_t * dt
v_t+1 = v_t + a_t * dt
cte_t+1 = f(x_t) - y_t + v_t * sin(eψ_t) * dt
eψ_t+1 = ψ_t - ψ_desired_t + v_t * δ_t / Lf * dt

where:

Lf measures the distance between the front of the vehicle and its center of gravity. This value needs to be found for each vehicle configuration and it can be done by driving the vehicle with a constant (and larger than 0) velocity and steering angle. Just adjust the value of Lf until the radius of the circular trajectory generated from driving the car is similar to what the model predicted.
f(x_t) is our reference trajectory. This trajectory is defined as a third order polynomial.
ψ_desired represents our desired orientation and can be calculated as the tangential angle of the polynomial f evaluated at x_t: ψ_desired_t = arctan(f'(x_t)).

Timestep Length and Elapsed Duration (N & dt)

The final values for N and dt are 10 and 0.1 seconds respectively. I tried several permutations for N and dt trying always to keep a time horizon (T = N*dt) lower than 1 second. The reason for this is that as T gets larger, the estimates computed by the model become more and more inaccurate and the vehicle behaves erratically.

Latency had also an important effect on the selection of N and dt. As mentioned below, dt was selected in order to easily account for computation delays.

Some of the values which were tried are: 10/0.2, 10/0.05, 5/0.1, 20/0.05 among others.

Polynomial Fitting

This file describes the details about the data which is sent back from the simulator. As it can be seen, the simulator sends two arrays ptsx and ptsy containing the global x and y coordinates of the reference trajectory or waypoints. These waypoints are preprocessed by transforming them to the vehicle's reference frame (main.cpp lines 100 to 108) and then used to fit a third order polynomial describing the desired trajectory.

Dealing with Latency

To solve the latency problem I took inspiration from the approaches provided by Jeremy Shannon and Andrew Stromme, among some other comments from the Udacity Forums. The main idea is to account the effect of latency on the state of the vehicle and/or the model equations.

The original kinematic equations presented above depend upon the actuations from the previous timestep. These actuations, however, are delayed by latency (~100ms). To simplify the problem, we can define our timestep interval dt to be the same as the expected latency; doing this allows us to easily account for the latency effect by considering the actuations from two timesteps ago whenever we update the model state (as shown in MPC.cpp lines 105-108).

camigord/Model-Predictive-Control