Model Predictive Control

In this post we will be going over a simple MPC Controller for 2D path tracking in a robot that uses the bicycle model.

This post complements an implementation of MPC I made in my LearnRoboticsCpp repository.

Model Predictive Control

Model predictive control is common method used for path tracking in mobile robotics. MPC uses a nonlinear model of the plant and a parametrized controller to predict errors of the system within a small future window. The controller then numerically optimizes a sum of weight squared errors in the system to compute a series of control inputs for the system. This optimization process is repeatedly applied to counteract the effect of disturbances.

Bicycle Model

We denote our time-varying state using a four dimensional vector (x, y) for position, v for velocity and for the yaw angle.

The robot accepts two control inputs: a for acceleraton and for steering angle.

The differential equations describing the robot's motion are as follows. We omit for brevity and express the first derivative of each state as

For small time steps, we can approximate a new state as

Optimization

At each time step t, we seek to find a control policy . denotes predicted control at time for time step .

This control policy should minimize a cost function that considers the following information: motion model of the robot, closeness to trajectory, the magnitude of controls required and the difference between controls applied at consecutive time steps (this helps ensure passenger comfort).

The mathematical expression that encodes these four factors is as follows:

is the time horizon we are optimizing over. The larger this value, the more computationally expensive your optimization problem will be.

This first term constrains the motion of the robot using the bicycle model.

. This is the difference between the robot's desired and current state at some time t. So the second term penalizes differences from the reference trajectory.

The third term penalizes large control inputs into the system.

. This is the difference between consecutive control inputs. The fourth term ensures a somewhat smooth control input.

and are matrices that denote the penalty apply to each term in the cost function. For instance, a matrix with high Q values means that you want to discentivize the optimization problem from deviating from the reference trajectory too much.

Additionally, our optimization problem is subject to the following contraints:

Maximum acceleration

Maximum steering angle

Allowable velocities

First state in the optimization output should be equal to our current state

Implementation

This part provides implementation details regard the MPC optimization part of the code in mpcSolve function.

Let us first define the variables used in our optimization. vars is a CPPAD vector that will store all the variables in the optimization. Note that the velocity and yaw was swapped in this implementation.

dimension of vars = dimensions of state vector x time horizon + dimensions of control vector x (time horizon - 1)

We use the variables VAR_start (e.g. x_start, y_start, yaw_start ) to denote the start index for a variable in the vector.


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

vars[x_start_] = x;
vars[y_start_] = y;
vars[yaw_start_] = yaw;
vars[v_start_] = v;

vars_lowerbound and vars_upperbound will be the same size of vars and defines the lower and upperbound for each variable. Thes are the variable constraints (1), (2) and (3).


// Lower and upper limits for x
Dvector vars_lowerbound(n_vars_);
Dvector vars_upperbound(n_vars_);

// Set all non-actuators upper and lowerlimits
// to the max negative and positive values.
// NOTE there mush be both lower and upper bounds for all vars!!!!!
for (auto i = 0; i < n_vars_; i++)
{
    vars_lowerbound[i] = -10000000.0;
    vars_upperbound[i] = 10000000.0;
}

for (auto i = delta_start_; i < delta_start_ + T_ - 1; i++)
{
    vars_lowerbound[i] = -  static_cast<double>(state.max_steer);
    vars_upperbound[i] = static_cast<double>(state.max_steer);
}

for (auto i = a_start_; i < a_start_ + T_ - 1; i++)
{
    vars_lowerbound[i] = -static_cast<double>(state.max_accel);
    vars_upperbound[i] = static_cast<double>(state.max_accel);
}

for (auto i = v_start_; i < v_start_ + T_; i++)
{
    vars_lowerbound[i] = static_cast<double>(state.min_speed);
    vars_upperbound[i] = static_cast<double>(state.max_speed);
}

For IPOPT, we also need to define the function constraints of the problem. The only function constraint we need to apply is that the initial conditions stay the same.


Dvector constraints_lowerbound(n_constraints_);
Dvector constraints_upperbound(n_constraints_);
for (auto i = 0; i < n_constraints_; i++)
{
    constraints_lowerbound[i] = 0;
    constraints_upperbound[i] = 0;
}
constraints_lowerbound[x_start_] = x;
constraints_lowerbound[y_start_] = y;
constraints_lowerbound[yaw_start_] = yaw;
constraints_lowerbound[v_start_] = v;

constraints_upperbound[x_start_] = x;
constraints_upperbound[y_start_] = y;
constraints_upperbound[yaw_start_] = yaw;
constraints_upperbound[v_start_] = v;

We define a struct FUNC_EVAL with a function void operator()(ADvector &fg, const ADvector &vars) which will be called by the solver.

Here we add the third and fourth terms in from above. We chose


fg[0] = 0;

for (int i = 0; i < T_ - 1; i++)
{
    fg[0] += 0.01 * CppAD::pow(vars[a_start_ + i], 2);
    fg[0] += 0.01 * CppAD::pow(vars[delta_start_ + i], 2);
}

for (int i = 0; i < T_ - 2; i++)
{
    fg[0] += 0.01 * CppAD::pow(vars[a_start_ + i + 1] - vars[a_start_ + i], 2);
    fg[0] += 1 * CppAD::pow(vars[delta_start_ + i + 1] - vars[delta_start_ + i], 2);
}

Finally, we encode the first and second terms in . Here we chose


// The rest of the constraints
for (int i = 0; i < T_ - 1; i++)
{
    // The state at time t+1 .
    AD<double> x1 = vars[x_start_ + i + 1];
    AD<double> y1 = vars[y_start_ + i + 1];
    AD<double> yaw1 = vars[yaw_start_ + i + 1];
    AD<double> v1 = vars[v_start_ + i + 1];

    // The state at time t.
    AD<double> x0 = vars[x_start_ + i];
    AD<double> y0 = vars[y_start_ + i];
    AD<double> yaw0 = vars[yaw_start_ + i];
    AD<double> v0 = vars[v_start_ + i];

    // Only consider the actuation at time t.
    AD<double> delta0 = vars[delta_start_ + i];
    AD<double> a0 = vars[a_start_ + i];

    // constraint with the dynamic model
    fg[2 + x_start_ + i] = x1 - (x0 + v0 * CppAD::cos(yaw0) * dt_);
    fg[2 + y_start_ + i] = y1 - (y0 + v0 * CppAD::sin(yaw0) * dt_);
    fg[2 + yaw_start_ + i] = yaw1 - (yaw0 + v0 * CppAD::tan(delta0) / L_ * dt_);
    fg[2 + v_start_ + i] = v1 - (v0 + a0 * dt_);

    // cost with the ref traj
    fg[0] += CppAD::pow(xref_(0, i + 1) - (x0 + v0 * CppAD::cos(yaw0) * dt_), 2);
    fg[0] += CppAD::pow(xref_(1, i + 1) - (y0 + v0 * CppAD::sin(yaw0) * dt_), 2);
    fg[0] += 0.5 * CppAD::pow(xref_(2, i + 1) - (yaw0 + v0 * CppAD::tan(delta0) / L_ * dt_), 2);
    fg[0] += 0.5 * CppAD::pow(xref_(3, i + 1) - (v0 + a0 * dt_), 2);
}

With all the required variables defined we can can now ask IPOPT to solve our equation.


// place to return solution
CppAD::ipopt::solve_result<Dvector> solution;

CppAD::ipopt::solve<Dvector, FUNC_EVAL>(
    options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
    constraints_upperbound, func_eval, solution);