11.14. High-level interface: Soft Constraints

As in section High-level interface: Basic example we consider the following linear MPC problem with lower and upper bounds on state and inputs, and a terminal cost term:

\begin{align*} \text{minimize} \quad & x_N^\top P x_N + \sum_{i=0}^{N-1} \left( x_i^\top Q x_i + u_i^\top R u_i \right) \\ \text{subject to} \quad & x_0 = {\color{red} x} \\ & x_{i+1} = A x_i + B u_i \\ & \underline{x} \leq x_i \leq \bar{x} \\ & \underline{u} \leq u_i \leq \bar{u} \end{align*}

This problem is parametric in the initial state \(\color{red} x\) and the first input \(u_0\) is typically applied to the system after a solution has been obtained. For the sake of this example, we assume \(u_i \in \mathbb{R}\) and \(x_i \in \mathbb{R}^2\) and write

\begin{align*} A = \begin{pmatrix} A_{11} & A_{12} \\ A_{21} & A_{22} \end{pmatrix}, \quad B = \begin{pmatrix} B_1 \\ B_2 \end{pmatrix}. \end{align*}

Suppose we want to allow the inequality constraints for \(u_i\) to be slightly violated. In this case, we introduce a slack variable \(s_i \geq 0\) and write

\begin{align*} \underline{u} - s_i \leq u_i \leq \overline{u} + s_i. \end{align*}

We want to punish positive values of \(s_i\) by adding a penalty term to the objective function. We use a hyperparameter \(\lambda \geq 0\) and write

\begin{align*} x_N^\top P x_N + \sum_{i=0}^{N-1} \left( x_i^\top Q x_i + u_i^\top R u_i + \lambda s_i\right) . \end{align*}

Here, we use a penalty term linear in \(s_i\). For \(\lambda\) large enough, the slack variables \(s_i\) are only chosen positive if the problem is infeasible with a hard constraint. For small \(\lambda\), it may be optimal to slightly violate the constraints even though the original problem is feasible. It is also common to choose a penalty term quadratic in \(s_i\). In order to use the FORCESPRO framework, we need to recast the new inequality constraints:

\begin{align*} \text{minimize} \quad & x_N^\top P x_N + \sum_{i=0}^{N-1} \left( x_i^\top Q x_i + u_i^\top R u_i + \lambda s_i \right), \\ \text{subject to} \quad & x_0 = {\color{red} x}, \\ & x_{i+1} = A x_i + B u_i, \\ & \underline{x} \leq x_i \leq \bar{x}, \\ & h_1(u_i,s_i) \leq \bar{u}, \\ & \underline{u} \leq h_2(u_i, s_i), \end{align*}

with \(h_1(u_i,s_i) = u_i - s_i\) and \(h_2(u_i,s_i) = u_i + s_i\).

11.14.1. Implementation in MATLAB

The following code is based on the MATLAB code in High-level interface: Basic example. The modified inequality constraints can be implemented as follows:

MatlabPython

%% relaxed inequality constraints
% assume variable ordering zi = [si, ui, xi]

model.nh    = 2;                     % number of inequality constraints
model.ineq  = @(z) [ z(2) - z(1);    % h_1
                    z(2) + z(1)];   % h_2
model.hu    = [umax, +inf];          % upper bound on inequality constraints
model.hl    = [-inf, umin];          % lower bound on inequality constraints

The resulting code is depicted below.

MatlabPython

%% system
A = [1.1 1; 0 1];
B = [1; 0.5];
[nx,nu] = size(B);
lambda = 8; % measure for penalty term

%% MPC setup
N = 10;
Q = eye(nx);
R = eye(nu);
if( exist('dlqr','file') )
    [~,P] = dlqr(A,B,Q,R);
else
    P = 10*Q;
end
umin = -0.5;     umax = 0.5;
xmin = [-5, -5]; xmax = [5, 5];

%% FORCESPRO multistage form
% assume variable ordering zi = [si; ui; xi] for i=0...N

% dimensions
model.N     = 11;   % horizon length N+1
model.nvar  = 4;    % number of variables
model.neq   = 2;    % number of equality constraints
model.nh    = 2;    % number of inequality constraints

% objective with penalty term
model.objective = @(z) z(2)*R*z(2) + [z(3);z(4)]'*Q*[z(3);z(4)] + lambda*z(1);
model.objectiveN = @(z) [z(3);z(4)]'*P*[z(3);z(4)];

% equalities
model.eq = @(z) [ A(1,:)*[z(3);z(4)] + B(1)*z(2);
                  A(2,:)*[z(3);z(4)] + B(2)*z(2)];

model.E = [zeros(2,2), eye(2)];

% initial state
model.xinitidx = 3:4;

% relaxed inequalities
model.ineq  = @(z) [ z(2) - z(1);
                    z(2) + z(1)];
model.hu    = [umax, +inf];
model.hl    = [-inf, umin];


model.lb = [0,      -inf, xmin  ];
model.ub = [+inf,   +inf, xmax  ];

You can download the code of this example to try it out for yourself: MATLAB, PYTHON.

11.14.2. Results

We run the simulation for \(\lambda = 2, 8\). The results of the simulation are presented in Figure 11.46 below. The plot on the top shows the system’s states over time, the plot on the bottom shows the input commands. For \(\lambda = 2\), the constraints on \(u\) are clearly violated, for \(\lambda = 8\), these constraints are only slightly violated.

Figure 11.46 Simulation results of the states (top), input (middle) over time. The constraints are plotted in black dashed lines.