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:

Note that except the matrix , everything in this problem is a parameter, while we require the initial state 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 :

where 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 , while we demand that .

## 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 for and .

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

Since and contain matrices that are varying, we define them as parameters, while is fixed and is defined directly in Matlab. Note that we have to define 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 and since :

%% 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');