Advanced MPC Example

The following example is taken from a real application, the automatic racing of 1:43 RC race cars. The main idea is to formulate the problem “race fast while avoiding obstacles and staying on the track” as a sequential QP.

Further details on the project can be found on the ORCA project website.

Introduction

We intend to solve the following MPC problem, which we term Model Predictive Contouring Control (MPCC) for the race cars:

\begin{equation}
\begin{aligned}
\min_{x,u}\ & x_{N+1}^T P x_{N+1} + \sum_{i=1}^{N} x_i^T Q_i x_i + f_i^T u_i + (u_{i+1} - u_{i})^T R (u_{i+1} - u_{i}) \\
\text{s.t.}\ & x_1 = 0 \\
& x_{i+1} = A_i x_i + B_i u_i + g_i \\
 & \underline{x}_i \leq x_i \leq \bar{x}_i\enspace,\quad \underline{u}_i \leq u_i \leq \bar{u}_i\enspace,\quad A_i x_i \leq b_i
\end{aligned}
\tag{MPCC}
\end{equation}

Note that except the matrix $R$, everything in this problem is a parameter, while we require the initial state $x_1$ to be zero. This stems from the fact that we re-linearize the nonlinear equations in the current operating point to yield a convex QP, particularly the car dynamics. Also note that we are penalizing the deviation from the previous control input. This is not directly supported by the multistage problem formulation, so we have to introduce lifting variables $v_i$:

\begin{equation}
\begin{aligned}
\min_{x,u}\ & x_{N+1}^T P x_{N+1} + \sum_{i=1}^{N} x_i^T Q_i x_i + f_i^T u_i + (u_i - v_{i})^T R (u_i - v_{i}) \\
\text{s.t.}\ & x_1 = 0\enspace,\quad v_1 = u_{\text{prev}} \\
& x_{i+1} = A_i x_i + B_i u_i + g_i \\
& v_{i+1} = u_i \\
 & \underline{x}_i \leq x_i \leq \bar{x}_i\enspace,\quad \underline{u}_i \leq u_i,v_i \leq \bar{u}_i\enspace,\quad A_i x_i \leq b_i
\end{aligned}
\end{equation}

where $v_1 = u_{\text{prev}}$ is the input that has been applied to the system at the previous sampling instant. Note that we have upper and lower bounds on all variables to ensure unboundedness of the solution even in case of positive semidefinite $Q$, while we demand that $R\succ 0$.

Problem transcription into FORCES format

Now let's begin transcribing the problem into the FORCES representation. Since everything depends on the way we define our stage variables, let us do this first.

Stage variables

We define our stage variables as $z_i = \left[x_i^T\ v_i^T\ u_i^T \right]^T$ for $i=1,\dots,N$ and $z_{N+1} = x_{N+1}$.

Now let us define the dimensions and cost matrices for the MPCC problem using these stage variables. We also get the stages data structure:

N = 13;
nx = 5;
nu = 3;
r = eye(nu);
R = [r -r; -r r];
Q = 10*eye(nx);
stages = MultistageProblem(N+1);

Initial stage

From the definition of the stage variables, we can express the first stage as follows:

%% first stage
i=1;
istr = sprintf('%02d',i);
 
% dimensions
stages(i).dims.n = nx+2*nu;            % number of stage variables
stages(i).dims.r = 2*nx+2*nu;          % number of equality constraints        
stages(i).dims.l = stages(i).dims.n;   % number of lower bounds
stages(i).dims.u = stages(i).dims.n;   % number of upper bounds
stages(i).dims.p = 0;                  % number of polytopic constraints
stages(i).dims.q = 0;                  % number of quadratic constraints
 
% cost
stages(i).cost.H = blkdiag(Q, R);
params(1) = newParam('f00', 1, 'cost.f');
 
% upper/lower bounds
params(end+1) = newParam(['lb',istr], i, 'ineq.b.lb');
stages(i).ineq.b.lbidx = 1:stages(i).dims.n;        
params(end+1) = newParam(['ub',istr], i, 'ineq.b.ub');        
stages(i).ineq.b.ubidx = 1:stages(i).dims.n;

Note that the linear term of the cost is a parameter. We still need to define the equality constraints. For the first stage, these are given by

\begin{equation*}
\begin{aligned}
C_1& z_1 &+& D_2 z_2 &=& c_1& \\
\begin{bmatrix} I_{nx} & 0_{nu} & 0_{nu} \\
                0_{nx} & I_{nu} & 0_{nu} \\                
                A_1    & 0_{nx} & B_1    \\
                0_{nx} & 0_{nx} & I_{nu} \\                
                \end{bmatrix}& \begin{bmatrix} x_1 \\ v_1 \\ u_1 \end{bmatrix} &+& 
\begin{bmatrix}
0_{nx\,\times\,nx} & 0_{nx\,\times\,nu} & 0_{nx\,\times\,nu} \\
0_{nu\,\times\,nx} & 0_{nu\,\times\,nu} & 0_{nu\,\times\,nu} \\
-I_{nx\,\times\,nx} & 0_{nx\,\times\,nu} & 0_{nx\,\times\,nu} \\
0_{nu\,\times\,nx} & -I_{nu\,\times\,nu} & 0_{nu\,\times\,nu} \\
\end{bmatrix} \begin{bmatrix} x_2 \\ v_2 \\ u_2 \end{bmatrix} &=& 
\begin{bmatrix} x \\ u_{\text{prev}} \\ -g_1 \\ 0} \end{bmatrix}&
\end{aligned}
\end{equation*}

Since $C_1$ and $c_1$ contain matrices that are varying, we define them as parameters, while $D_2$ is fixed and is defined directly in Matlab. Note that we have to define $D_2$ with stage 2!

% equality constraints
params(end+1) = newParam(['C',istr], i, 'eq.C');
params(end+1) = newParam(['c',istr], i, 'eq.c');
stages(2).eq.D = [zeros(nx+nu,nx+2*nu);
                  -eye(nx), zeros(nx,2*nu);
                  zeros(nu,nx), -eye(nu), zeros(nu)];

Stages along horizon

Similarly, the stages along the horizon with the difference that the cost is now a parameter and we additionally have affine inequalities:

%% stages along horizon
for i = 2:N
    istr = sprintf('%02d',i);
 
    % dimensions
    stages(i).dims.n = nx+2*nu;            % number of stage variables
    if( i < N )
        stages(i).dims.r = nx+nu;              % number of equality constraints
    else
        stages(i).dims.r = nx;              % number of equality constraints
    end
    stages(i).dims.l = stages(i).dims.n;   % number of lower bounds
    stages(i).dims.u = stages(i).dims.n;   % number of upper bounds
    stages(i).dims.p = 2;                  % number of polytopic constraints
    stages(i).dims.q = 0;                  % number of quadratic constraints
 
    % cost
    params(end+1) = newParam(['H',istr], i, 'cost.H');
    params(end+1) = newParam(['f',istr], i, 'cost.f');
 
    % upper/lower bounds
    params(end+1) = newParam(['lb',istr], i, 'ineq.b.lb');
    stages(i).ineq.b.lbidx = 1:stages(i).dims.n;
    params(end+1) = newParam(['ub',istr], i, 'ineq.b.ub');
    stages(i).ineq.b.ubidx = 1:stages(i).dims.n;
 
    % affine inequality constraints
    params(end+1) = newParam(['A',istr], i, 'ineq.p.A');
    params(end+1) = newParam(['b',istr], i, 'ineq.p.b');
 
    % equality constraints
    params(end+1) = newParam(['C',istr], i, 'eq.C');
    params(end+1) = newParam(['c',istr], i, 'eq.c');
    if( i < N )
        stages(i+1).eq.D = [-eye(nx), zeros(nx,2*nu);
                            zeros(nu,nx), -eye(nu), zeros(nu)];
    else
        stages(i+1).eq.D = -eye(nx);
    end
end

Final stage

The final stage is different in that it does not have $C_{N+1}$ and $c_{N+1}$ since $z_{N+1} = x_{N+1}$:

%% final stage
i = N+1;
istr = sprintf('%02d',i);
 
% dimensions
stages(i).dims.n = nx;                 % number of stage variables
stages(i).dims.r = 0;                  % number of equality constraints
stages(i).dims.l = stages(i).dims.n;   % number of lower bounds
stages(i).dims.u = stages(i).dims.n;   % number of upper bounds
stages(i).dims.p = 2;                  % number of polytopic constraints
stages(i).dims.q = 0;                  % number of quadratic constraints
 
% cost
params(end+1) = newParam('P', i, 'cost.H');
stages(i).cost.f = zeros(nx,1);
 
% upper/lower bounds
params(end+1) = newParam(['lb',istr], i, 'ineq.b.lb');
stages(i).ineq.b.lbidx = 1:stages(i).dims.n;
params(end+1) = newParam(['ub',istr], i, 'ineq.b.ub');
stages(i).ineq.b.ubidx = 1:stages(i).dims.n;
 
% affine inequality constraints
params(end+1) = newParam(['A',istr], i, 'ineq.p.A');
params(end+1) = newParam(['b',istr], i, 'ineq.p.b');