mpc_lib

Main library for implementation of Model Predictive Control system.

File system

mpc_lib
├── include
│   ├── cppad
│   ├── Eigen-3.3
│   ├── Ipopt-3.12.7
│   └── mpc_lib
│       └── MPC.h
└── src
    └── MPC.cpp

cppad

A C++ Algorithmic Differentiation package

Eigen-3.3

Eigen is a high-level C++ library of template headers for linear algebra, matrix and vector operations, geometrical transformations, numerical solvers and related algorithms.

Ipopt-3.12.7

IPOPT, short for "Interior Point OPTimizer, pronounced I-P-Opt", is a software library for large scale nonlinear optimization of continuous systems.

Installation Instructions

• Install cppad

sudo apt-get install cppad

• Install IPOPT

Warnings maybe ignored.

sudo apt-get install gfortran
sudo apt-get install unzip
wget https://www.coin-or.org/download/source/Ipopt/Ipopt-3.12.7.zip && unzip Ipopt-3.12.7.zip && rm Ipopt-3.12.7.zip
sudo bash install_ipopt.sh ./Ipopt-3.12.7/

MPC Algorithm

Setup :

    1. Define length of prediction horizon, N, and duration of each timestep, dt.

    2. Define vehicle dynamics and actuator limitations along with other constraints.

    3. Define cost function

Loop :

    1. Pass current state as initial state to model predictive controller.

    2. Call the optimization solver (we used Ipopt). It will return a vector of control inputs that minimizes the cost function.

    3. Apply first control input to vehicle.

    4. Back to 1

Plant Model

Dynamics

Constraints

Cost function

MPC Preprocessing

Preprocessing of setpoints is done in the main controller node

The setpoints are passed to the controller with respect to an arbitrary global coordinate system by the path planning module. This is then transformed into the vehicle's local coordinate system.

for (int i = 0; i < ptsx.size(); i++)
{
    // translation
    double shift_x = ptsx[i] - px;
    double shift_y = ptsy[i] - py;
    // rotation
    ptsx[i] = shift_x * cos(-theta) - shift_y * sin(-theta);
    ptsy[i] = shift_x * sin(-theta) + shift_y * cos(-theta);
}

This is then used to get a 3rd order polynomial as an estimate of the current road curve ahead. Using a smaller order polynomial runs the risk of underfitting and likewise using a higher order polynomial would be prone to overfitting or an inefficient unnecessary added complexity.

Function used to get fitted curve - GitHub

The current Cross track error and Orientation error is calculated from the polynomial coefficients as follows:

double cte = coeffs[0];
double etheta = -atan(coeffs[1]);

Performance

References

Starter code : Udacity Model Predictive Control (MPC) Project

Bouzoualegh, Samir & Guechi, Elhadi & Kelaiaia, Ridha. (2018). Model Predictive Control of a Differential-Drive Mobile Robot. Acta Universitatis Sapientiae Electrical and Mechanical Engineering. 10. 20-41. 10.2478/auseme-2018-0002.