Skip to content

Udacity CarND Model Predictive Control Project

Notifications You must be signed in to change notification settings

sgalkin/CarND-T2P5

 
 

Repository files navigation

Model Predictive Control Project

Udacity - Self-Driving Car NanoDegree Build Status codecov CircleCI


Overview

This project implements controlling application for Udacity self-driving car simulator using MPC (Model Predictive Control) controller mechanism.

Project objective: No tire may leave the drivable portion of the track surface. The car may not pop up onto ledges or roll over any surfaces that would otherwise be considered unsafe (if humans were in the vehicle).

Demo

./t2p5 -N 7

Video


Usage

Usage:
  t2p5 [options]
Available options:
  -o, --output  CSV output
  -d, --delay   Control delay, ms (default: 100)
  -h, --help    print this help screen
  -N, --depth   Prediction depth (default: 8)
  -p, --port    Port to use (default: 4567)
  -t, --dt      Prediction time step, ms (default: 125)

Dependencies

Runtime

Tools

Libraries not included into the project

  • uWebSocketIO == v0.13.0
  • Ipopt >= 3.12.7
    • Ubuntu/Debian/Mac: the repository includes install_ipopt.sh that can be used to set up and install Ipopt
    • Please refer to this document for installation instructions.
  • CppAD
    • Ubuntu/Debian: it is possible to use standard packet manager apt to install

Libraries included into the project

  • Eigen - C++ template library for linear algebra: matrices, vectors, numerical solvers, and related algorithms
  • JSON for Modern C++ - JSON parser
  • Catch2 - Unit-testing framework
  • ProgramOptions.hxx - Single-header program options parsing library for C++11

Build

Pre-built Docker container

  1. docker pull sgalkin/carnd-t2p5

Manual build using containerized development environment

  1. Clone this repo.
  2. mkdir build
  3. cd build
  4. cmake .. -DCMAKE_BUILD_TYPE=Release -DLOCAL_BUILD=OFF -DDOCKER_BUILD=ON
  5. make docker-build
  6. make docker-test
  7. make docker-run or make docker-shell; ./t2p5

Local manual build

  1. Clone this repo.
  2. mkdir build
  3. cd build
  4. cmake .. -DCMAKE_BUILD_TYPE=Release -DLOCAL_BUILD=ON -DDOCKER_BUILD=OFF
  5. make
  6. make test

Protocol

The project uses uWebSocketIO request-response protocol in communicating with the simulator.

INPUT: values provided by the simulator to the c++ program

{
  "ptsx": "(Array<float>) - The global x positions of the waypoints",
  "ptsy": "(Array<float>) - The global y positions of the waypoints.
          This corresponds to the z coordinate in Unity since y is the up-down
          direction",
  "psi": "(float) - The orientation of the vehicle in **radians** converted
         from the Unity format to the standard format expected in most mathemetical functions (more details below)",
  "psi_unity": "(float) - The orientation of the vehicle in **radians**",
  "x": "(float) - The global x position of the vehicle",
  "y": "(float) - The global y position of the vehicle",
  "steering_angle": "(float) - The current steering angle in **radians**",
  "throttle": "(float) - The current throttle value [-1, 1]",
  "speed": "(float) - The current velocity in **mph**"
}

psi

//            90
//
//  180                   0/360
//
//            270

psi_unity

//            0/360
//
//  270                   90
//
//            180

OUTPUT: values provided by the c++ program to the simulator

{
  "steering_angle": "desired steering angle [-1, 1]",
  "throttle": "desired throttle [-1, 1]",
  "mpc_x": "list of x-coordinates of MPC prediction (debug output)",
  "mpc_y": "list of y-coordinates of MPC prediction (debug output)",
  "next_x": "list of x-coordinates of reference trajectory (debug output)",
  "next_y": "list of y-coordinates of reference trajectory (debug output)",
}

Model

The project uses kinematic model presented in the class.

State

  • psi - orientation of the car (related to heading in the first step)
  • v - car velocity
  • x - x-coordinate (related to initial car position)
  • y - y-coordinate (related to initial car position)
  • cte - cross track error - the distance between current position and reference trajectory
  • epsi - orientation error - difference between car orientation and a tangent of reference trajectory in the given point

Actuators

  • a - acceleration of the car, range limited to [-0.05, 1]
  • d - steering angle, range limited to [-18 ,18] degrees

Main terms of the cost function

  • pow(cte, 2) - enforces following the trajectory

  • pow(epsi, 2) - enforces following the trajectory

  • pow((v - vref)/vref, 2) - enforces car to go, instead of staying in the local optima

  • max(cte) over depth - reduces oscillation around trajectory

  • max(epsi) over depth - reduces oscillation around trajectory

  • a/(a_upper - a_lower) - relative acceleration, penalize use of acceleration

  • d/(d_upper - d_lower) - relative steering angle, penalize use of steering wheel

  • (a_t+1 - a_t)/(a_upper - a_lower) - relative acceleration change, improves smoothness of acceleration

  • (d_t+1 - d_t)/(d_upper - d_lower) - relative steering angle, improves smoothness of steering, reduces oscillation arount trajectory

Pipeline

Each state was passed through the following pipeline

  • Coordinates transformation - converts world coordinates system into car coordinates system
  • Control latency processing - the current values of actuators was applied to input in order to compensate control latency. dt - exected latency
  • MPC - which consists of
    • Fitting a polynomial into waypoints. 3rd degree polynomial was used in the projects
    • Actuators optimization using polynomial from the previous step as a reference trajectory. Depth and time span between control inputs are configurable via command line parameters

Results

TODO

  1. Try to use different cost function.
  2. Increase code coverage

Packages

No packages published

Languages

  • C++ 82.2%
  • CMake 12.7%
  • Shell 3.4%
  • Gnuplot 1.6%
  • C 0.1%