## ​

This page will describe how to create and program a C++ Model Predictive Control (MPC) to control the steering, throttle and brake to follow a navigation path for an autonomous vehicle to negotiate a track at speed safely. The project uses vehicle dynamics models, system latency modelling, actuator & vehicle constraints with system cost functions. Below is an approach to the fundamental principles and methods of a Model Predictive controller used to control a self-driving car. The information supplied is not exhaustive, and it is recommended to view the source code and comments on GitHub for a deeper understanding of the code. A link to the GitHub repository is here. A video of the project in action can be found here.

This project page will describe the following sections

Model equations

Model trajectory costs and system parameters

MPC process

## Model Equations

The first part of an MPC is to determine the systems states that are desired to successfully predict the movement of the autonomous vehicle. For our example, we will design an MPC for a self-driving car. This means that the vehicle will move along a two-dimensional plane, so we will need to track position coordinates. As the simulator uses a grid with (x & y) coordinates, we will track (x & y), along with the heading angle (ψ) and as the vehicle is not stationary, we also require its velocity (v). The inclusion of acceleration requires taking into account the following;

• Complexity due to environment dependence. I.e. acceleration is dependent on gravity and will vary dependent on the road gradients.

• Road surface friction.   (x & y)

• Vehicle weight variations.

Including these environmental impacts on acceleration can be tricky as they are constantly changing. However, due to a 10Hz controller update rate, the above acceleration factors can be neglected and we can use the derivative of the velocity function to model the acceleration of the vehicle so we will not include it in our system state variables.

In addition to the states, we will include the system Cross Track Error (cte) or the difference between the path trajectory and the vehicle's current position and also the heading angle error (eψ) which is the difference between the vehicles heading angle and the trajectory paths heading angle.

The six observed state that we will track are (x, y, ψ, v, cte, eψ).

Now we know the states we need, we can develop the kinematics and dynamic mathematical model equations to predicting the performance of the system in the future. The models can be as simple or complex, dependent on our requirements and constraints. Since our vehicle moves along a 2D plane and we can assume that the steer wheels act together, as well as the ridged wheels, we can then simplify it and model it as a bicycle.

Now, lets derive the vehicle kinematics and dynamic mathematical model equations to predicting the performance of the system in the future.

We can update the x and y position using basic trigonometry.

X(t+1) = X(t) + V(t) * cos(ψ(t) ) * dt
Y(t+1) = Y(t) + V(t) * sin(ψ(t) ) * dt

In the above equation, X(t) and Y(t) are the current vehicle x, y coordinates and X(t+1), Y(t+1) are the future coordinates in one-time step. V(t) is the vehicle velocity at the current timestep, ψ( t ) is the vehicle heading, measured from the x-axis in the current timestep and dt is the time step change in time between calculations.

The vehicle heading, ψ, is updated with the following.

ψ(t+1) = ψ(t) + L f / V(t) * δ(t) * dt

ψ(t) is the current vehicles heading angle and ψ(t+1) is the future heading angle in one-time step. δ(t) is the vehicles current steer angle and Lf is the distance between the front of the vehicle and its centre of gravity (COG). The larger the vehicle, the slower the turn rate.

The velocity update V(t+1) is;

V(t+1) = V(t) + a(t) * dt

a(t) is the vehicle acceleration at the current timestep.

The cte equation;

cte(t+1) = f(x) - y(t) + V(t) * sin(ψ(t) ) * dt

The cte(t+1) is the next time step cross track error. f(x) is the path trajectory function that we will discuss later. In this system, f(x) is the polynomial of the path trajectory points.​

The final vehicle state is the heading angle error update equation.

(t+1) = eψ(t) - Y(t) + V(t) * sin(eψ(t) ) * dt

(t) is the current vehicles heading angle error and eψ(t+1) is the future heading angle error in one time step.

Before we can start programming the model equations and producing a controller, we must first take into account actuators and fundamental physics constraints of the system. As we are dealing with a non-omnidirectional vehicle, we cannot move in arbitrary directions. This model is called a nonholonomic system and as a result, we must take into account the limits to the steering angle, acceleration and speed.

As this project is implemented in a simulation environment, the simulation constraints are;

• Steering - Positive angles steer is in the right direction and is limited to ±25°.

• Throttle - Positive value accelerates the vehicle and negative applies the brakes and is limited to ±1.

• Speed - We will target 100MPH.

## Trajectory Cost and System Parameters

An MPC reframes the trajectory problem as an optimisation problem. It involves simulating actuator trajectories, predicting the result and then selecting the actuator commands that produce the trajectory with the lowest cost. Since this method is cost-based, we can create multiple weighted cost functions to force correct operation of the vehicle

In the vehicle model used, the cost is calculated in three ways.

1. The error between the desired state and the vehicles current state in position, heading angle and velocity.

2. The use of the controller throttle and steering inputs.

3. The rate of change between successive controller throttle and steering inputs.

// `fg` vector of the cost constraints

// `vars` is the vector of state & actuators variable values fg = 0;​​

// penalise the system for deviating of the planned path and for not
// traveling at the intended speed

for(int i=0; i < N; i++) {
fg += cte_penalty *
fg += epsi_penalty *
fg += speed_penalty *
CppAD::pow(vars[v_start + i] - ref_v, 2);
}

// limit the use of steering and accelerating

for(int i=0; i < N-1; i++) {
fg += steer_use_penalty *
fg += a_use_penalty *
}

// limit the change between timesteps of the acceleration and steering angles

for(int i=0; i < N-2; i++) {
fg += steer_change_penalty *

- vars[delta_start + i], 2);
fg += a_change_penalty *
CppAD::pow(vars[a_start + i + 1] - vars[a_start + i], 2);
}

The costs penalties for each error can be adjusted to produce the desired system trajectory. In our case, we will put a higher cost on system error as we wish to stay in the middle of the track. It would also be beneficial to prevent excessive use of steering to prevent oscillations around the desired trajectory path due to the high error weighting

The following table shows the penalties used and their corresponding system effect, and we would also like to travel around the track as fast as possible so that we will weight the error in the vehicles speed.

The following table describes each cost and its cost value towards the optimal trajectory.

Now that we have the models and cost functions for our trajectory optimiser, we must now choose how far into the future we should predict the trajectory. The choice of N, the number of time steps into the future, and dt are critical to the performance of the MPC. If N is too small, the MPC will not be able to see far enough into the future and won't be able to predict an accurate, current state and system controls to prevent trajectory overshoot or instabilities. However, a large N predicts unnecessary data for the future, thus increasing the computational cost.

A good dt value should be able to capture a system control response accurately. If dt is large, it will miss necessary control actions, and the vehicle will be less responsive. If it is too small, it will respond in a very oscillatory fashion, fluctuating the controls faster than the vehicle's actuators can respond.

The optimal N and dt combination should follow the rule; future prediction > system settling time. For vehicle control, we need to predict far enough into the future to be able to stop the car before it crashes.

During testing, it was observed that if the vehicle travels slower than 60 Mph, a smaller N value of six works but is less responsive to significant system changes. A larger N above 18 was found to predict the later N values as wayward points due to the quintic function fitting in certain parts of the track. N was chosen to be 10 as it predicted far enough into the future and offered system stability, with very little system performance loss.

It was observed that due to the systems 100ms latency, dt values smaller or equal to this produced control instabilities, while larger than 160ms was found to be slow responding to the situation. dt was chosen to be 120ms as this provided the best performance for the system.

## MPC Process

In the simulator, a path trajectory is provided in the world reference frame, but the vehicle controls and sensors are relative to the vehicle itself. To work between the two, we will convert the trajectory paths world coordinate points to the vehicle-centric reference frame. This involves rotation and translation between the two coordinate systems and is accomplished by the following equations.

Xc = Xt ∗ cos( Θ) - Yt * sin(Θ)
Yc = Xt ∗ sin( Θ) + Yt * cos(Θ)

Xc and Yc are the x and y coordinates in the vehicle-centric reference system. Xt and Yt are the translation position of the path point relative to the car and Θ is the angle of between the world coordinates x-axis to the vehicle coordinates x-axis about the world coordinates z-axis, provided it is perpendicular to the car frames x-axis. This translation is performed as follows.

// convert the trajectory coordinates from world frame into car frame
// world frame angle (psi) is the inverse of car frame angle

for(int i=0; i < ptsx.size(); i++) {
double transpose_x = ptsx[i] - px;
double transpose_y = ptsy[i] - py;
ptsx[i] = transpose_x * cos(0 - psi) - transpose_y * sin(0 - psi);
ptsy[i] = transpose_x * sin(0 - psi) + transpose_y * cos(0 - psi);
}

Once the path trajectory points have been converted into vehicle-centric coordinates, a line of best fit is calculated in the form of a quadratic equation. This equation is feed into the MPC for time stepped path prediction.

// convert the points std::vector to an Eigen::VectorXd
double *ptrx = &ptsx; // get the address of the points x vector
double *ptry = &ptsy;
Eigen::Map<Eigen::VectorXd> pathx(ptrx, ptsx.size());
Eigen::Map<Eigen::VectorXd> pathy(ptry, ptsy.size());

// fit a third order polynomial to the car framed trajectory points
auto poly = polyfit(pathx, pathy, 3);

We will use the quadratic equation to feed into the MPC as the desired trajectory path to follow, but we will first use it to obtain the state errors of the vehicle at the present time step.

double target_x = 0;
double target_y = 0;

double target_psi = 0;

double cte = polyeval(poly, target_x) - target_y;

// calculate the orientation error
double epsi = target_psi - atan(poly + 2*poly*target_x + 3*poly*target_x*target_x);

// create the current position state vector

Eigen::VectorXd state(6);
state << target_x, target_y, target_psi, v, cte, epsi;

As mentioned earlier, the vehicle has system constraints that we must address. We will do this by setting up the model variables and assigning limitations to the variable values.

// Set the number of model variables (includes both states and inputs).
// For example: If the state is a 4 element vector, the actuators is a 2
// element vector and there are 10 timesteps. The number of variables is:
// 4 * 10 + 2 * 9

size_t n_vars = N * 6 + (N - 1) * 2;
// Set the number of constraints
size_t n_constraints = N * 6;

// Initial value of the independent variables.
// SHOULD BE 0 besides initial state.

Dvector vars(n_vars);
for(int i = 0; i < n_vars; i++) {
vars[i] = 0;
}

Dvector vars_lowerbound(n_vars);
Dvector vars_upperbound(n_vars);
// Set lower and upper limits for variables.
for(int i = 0; i < delta_start; i++) {
vars_lowerbound[i] = -1.0e19;
vars_upperbound[i] = 1.0e19;
}

for(int i = 0; i < (N - 1); i++) {
// steer angle limitation
vars_lowerbound[delta_start + i] = -(25 * M_PI / 180) * Lf;
vars_upperbound[delta_start + i] = (25 * M_PI / 180) * Lf;
// throttle limitation
vars_lowerbound[a_start + i] = -1.0;
vars_upperbound[a_start + i] = 1.0;
}

// Lower and upper limits for the constraints
// Should be 0 besides initial state.

Dvector constraints_lowerbound(n_constraints);
Dvector constraints_upperbound(n_constraints);
for(int i = 0; i < n_constraints; i++) {
constraints_lowerbound[i] = 0;
constraints_upperbound[i] = 0;
}

double x = state;
double y = state;
double psi = state;
double v = state;
double cte = state;
double epsi = state;

constraints_lowerbound[x_start] = x;
constraints_lowerbound[y_start] = y;
constraints_lowerbound[psi_start] = psi;
constraints_lowerbound[v_start] = v;
constraints_lowerbound[cte_start] = cte;
constraints_lowerbound[epsi_start] = epsi;

constraints_upperbound[x_start] = x;
constraints_upperbound[y_start] = y;
constraints_upperbound[psi_start] = psi;
constraints_upperbound[v_start] = v;
constraints_upperbound[cte_start] = cte;
constraints_upperbound[epsi_start] = epsi;

Now we can begin to solve the MPC trajectory problem. To do this we will use Ipopt (interior point optimizer) a software package for nonlinear optimisation. It is designed to find local solutions of mathematical optimisation problems that have an objective and constraints functions. Another benefit of Ipopt is that we can put a time limit on the computation time to prevent bottlenecks. We will also require CppAD, A Package for Differentiation of C++ Algorithms.

The optimisation function is where we put our model equations we derived earlier to calculate the trajectory. To do this we will create a class that we pass to Ipopt to solve. The class is called from the Ipopt library and use the chosen Ipopt actuator control parameters and calculates the resulting trajectory cost associated with these parameters.

class FG_eval {
public:
// Fitted polynomial coefficients
Eigen::VectorXd coeffs;
FG_eval(Eigen::VectorXd coeffs) { this->coeffs = coeffs; }

// `fg` a vector of the cost constraints

// `vars` is a vector of variable values (state & actuators)

fg = 0;

// penalise the system for deviating off the planned path and for not
// traveling at the intended speed

for(int i=0; i < N; i++) {
fg += cte_penalty * CppAD::pow(vars[cte_start + i], 2);
fg += epsi_penalty * CppAD::pow(vars[epsi_start + i], 2);
fg += speed_penalty * CppAD::pow(vars[v_start + i] - ref_v, 2);
}

// limit the use of steering and accelerating
for(int i=0; i < N-1; i++) {
fg += steer_use_penalty * CppAD::pow(vars[delta_start + i], 2);
fg += a_use_penalty * CppAD::pow(vars[a_start + i], 2);
}

// limit the change between timesteps of the acceleration and steering angles
for(int i=0; i < N-2; i++) {
fg += steer_change_penalty * CppAD::pow(vars[delta_start + i + 1]                                     - vars[delta_start + i], 2);
fg += a_change_penalty * CppAD::pow(vars[a_start + i + 1] - vars[a_start + i],                                                       2);
}

// Initial constraints
// Add 1 to each of the starting indices due to cost being located in index 0 of `fg`.

fg[1 + x_start] = vars[x_start];
fg[1 + y_start] = vars[y_start];
fg[1 + psi_start] = vars[psi_start];
fg[1 + v_start] = vars[v_start];
fg[1 + cte_start] = vars[cte_start];
fg[1 + epsi_start] = vars[epsi_start];

// Update the system constraints using the systems model
for (int i=0; i < N - 1; i++) {
// get the future timestep values => t+1
AD<double> x1 = vars[x_start + i + 1];
AD<double> y1 = vars[y_start + i + 1];
AD<double> psi1 = vars[psi_start + i + 1];
AD<double> v1 = vars[v_start + i + 1];
AD<double> cte1 = vars[cte_start + i + 1];
AD<double> epsi1 = vars[epsi_start + i + 1];

// get the current timestep values => t
AD<double> x0 = vars[x_start + i];
AD<double> y0 = vars[y_start + i];
AD<double> psi0 = vars[psi_start + i];
AD<double> v0 = vars[v_start + i];
AD<double> epsi0 = vars[epsi_start + i];
AD<double> delta0 = vars[delta_start + i];
AD<double> a0 = vars[a_start + i];

// The idea here is to constraint this value to be 0.
// This is also CppAD can compute derivatives and pass
// these to the solver.

AD<double> fx0 = coeffs + coeffs*x0 + coeffs*x0*x0 + coeffs*x0*x0*x0;
AD<double> psi_des = coeffs + 2 * coeffs * x0 + 3 * coeffs * x0 * x0;

// Setup the rest of the model constraints
fg[2 + x_start + i] = x1 - (x0 + v0 * CppAD::cos(psi0) * dt);
fg[2 + y_start + i] = y1 - (y0 + v0 * CppAD::sin(psi0) * dt);
fg[2 + psi_start + i] = psi1 - (psi0 + v0 / Lf * delta0 * dt);
fg[2 + v_start + i] = v1 - (v0 + a0 * dt);
fg[2 + cte_start + i] = cte1 - ((fx0 - y0) + (v0 * CppAD::sin(epsi0) * dt));
fg[2 + epsi_start + i] = epsi1 - ((psi0 - psi_des) + (v0 / Lf * delta0 * dt));
}
}
};

To find an optimal solution now that we have our cost class defined is;

// object that computes objective and constraints
FG_eval fg_eval(coeffs);

// options for IPOPT solver
std::string options;

// Uncomment this if you'd like more print information
options += "Integer print_level  0\n";
// Setting sparse to true allows the solver to take advantage
// of sparse routines, this makes the computation MUCH FASTER.

options += "Sparse  true        forward\n";
options += "Sparse  true        reverse\n";

// Set the solver to have a maximum time limit of 0.5 seconds.
options += "Numeric max_cpu_time          0.5\n";

// place to return solution

// solve the optimised trajectory problem
options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
constraints_upperbound, fg_eval, solution);

All that we require now is get the first actuator values from the solution. It is only these first values that we need for our system. We can discard every other value. However, we would like to display the predicted trajectory path so we will also get the x and y path coordinates for display purposes.

One of the additional benefits of using an MPC is we can take into account system latency. To do this, the control values that are acted upon are taken one-time step in advance or at time step t1 from the current state. This means that as the MPC predicts the system from the current state, we act as if the state was in the past when the calculation was first performed. This prevents the model from lagging the system.

auto cost = solution.obj_value;
std::cout << "Cost " << cost << std::endl;

// Return the first actuator values.
vector<double> control = {solution.x[delta_start], solution.x[a_start]};

vector<double> path_x;
vector<double> path_y;
for(int i=1; i < N-1; i++) {
path_x.push_back(solution.x[x_start + i]);
path_y.push_back(solution.x[y_start + i]);
}

We have now designed an MPC controller for a self-driving car. The complete project code can be viewed and downloaded from GitHub here.

A video of the MPC navigating around a test track at up to 90Mph can be viewed below.

The

## CONTACT

I hope you enjoyed reviewing the project. - Let's get in touch. LinkedIn messages work best.