Model Predictive Control (MPC) for electromechanical system

Question

So I want to create an MPC controller for my seesaw-cart system . All the "grunt work" (getting equations of motion, state-space representation etc.) has been done, so I went into coding into MATLAB. Here's the following:

clc; clear all; close all;
yalmip('clear')

%% Parameters

a = 0.116553; % height of center mass of the pendulum, [m]
b = 0; % position of the weight B on the vertical rod (not taken into consideration)
c = 0.180047; % height of the center of mass of the cart, [m]
mA = 4.839; % mass of the pendulum, [kg]
mB = 0; % not taken into consideration
mC = 1; % mass of the cart, [kg]
g = 9.81; % gravity factor, [m/s^2]
kappa = 0.1; % coefficient of the viscous damping in the rotational joint
J = 0.68; % moment of inertia of the pendulum, [kgm^2]
Ke = 0.077; % motor constant of the EM force, [Vs^-1]
Kt = 0.077; % proportional moment motor constant, [NmA^-1]
Ra = 2.6; % electrical resistance, [ohm]
p = 1/3.71; % motor gearbox ratio
r = 7.7*10^(-3); % effective radius of the motor shaft, [m] 

%% Defining the continuous system

A = [0 1 0 0;
    (a*mA*g+b*mB*g)/(J+mB*b^2), -kappa/(J+mB*b^2), -mC*g/(J+mB*b^2), -Ke*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2);
    0 0 0 1;
    (a*mA*g+b*mB*g)/(J+mB*b^2)-g,  -kappa*c/(J+mB*b^2), -c*mC*g/(J+mB*b^2), -(J+mB*b^2+mC*c^2)*Ke*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2)];

B = [0; 
    Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2); 
    0; 
    (J+mB*b^2+mC*c^2)*Kt/(mC*(J+mB*b^2)*Ra*p^2*r^2)];

C = eye(4); % check

D = 0;

sysC = ss(A, B, C, D);

%% Defining the discrete system

Ts = 10e-3; % sample time
sysD = c2d(sysC,Ts); % discrete time
Ad = sysD.A;
Bd = sysD.B;

%% Formulation of the MPC problem

[nx, nu] = size(B);

Q = eye(nx);
R = eye(nu);
N = 1000;

% Input constraints, maximum and minimum voltage
umin = -6; 
umax = 6;

% Still have to find this
xmin = [-deg2rad(10); -deg2rad(50); -0.5; -10];
xmax = [deg2rad(10); deg2rad(50); 0.5; 10];

uVar = sdpvar(repmat(nu,1,N),ones(1,N));
xVar = sdpvar(repmat(nx,1,N+1),ones(1,N+1));
constraints = [];
objective = 0;
ops = sdpsettings('verbose',0,'solver','quadprog');

for k = 1:N
    objective = objective + xVar{k}'*Q*xVar{k} + uVar{k}*R*uVar{k}; 
    constraints = [constraints, xVar{k+1} == Ad*xVar{k} + Bd*uVar{k}];
    constraints = [constraints , umin <= uVar{k} <= umax , xmin <= xVar{k+1} <= xmax];
end
controller = optimizer(constraints, objective,ops,xVar{1},[uVar{:}]);

%% Simulation of the linearized model

nSim = 1000;
x0 = [0; 0; 0; 0.05]; % initial values, check

time = 0:Ts:nSim*Ts;
x = zeros(nx,nSim+1);
u = zeros(nu,nSim);
x(:,1) = x0;
for i = 2:nSim+1
    disp(strcat(['Sim iter: ', num2str(i-1)]));
    uOpt = controller{x(:,i-1)};
    u(:,i-1) = uOpt(:,1);
    x(:,i) = Ad*x(:,i-1) + Bd*u(:,i-1);
end

%% Plot

figure;
subplot(4,1,1);
plot(time,x(1,:), 'LineWidth', 2, 'Color', 'red'); grid; ylabel('{\theta} (rad)'); title('States');
subplot(4,1,2);
plot(time,x(2,:), 'LineWidth', 2, 'Color', [0.6350, 0.0780, 0.1840]); grid; ylabel('$\dot{\theta}$ (rad/s)', 'Interpreter', 'latex');
subplot(4,1,3);
plot(time,x(3,:), 'LineWidth', 2, 'Color', [0.4940, 0.1840, 0.5560]); grid; ylabel('s (m)'); 
subplot(4,1,4);
plot(time,x(4,:), 'LineWidth', 2, 'Color',[0, 0.7, 0.9]); grid; ylabel('$\dot{s}$ (m/s)', 'Interpreter', 'latex'); xlabel('t (s)');

figure;
stairs(time(1:end-1),u, 'LineWidth', 2, 'Color',[1, 0.647, 0]); grid; ylabel('Ua (V)'); xlabel('t (s)'); title('Input');

So I was wondering for any suggestions on improvements. What can I do to make my regulator more robust? Here are my outputs for this particular code:

Note: I'm using YALMIP for the optimization part of the MPC.

Answer 1

You lose feasibility after a few iterations,

   sol = 
  struct with fields:

    yalmipversion: '20210331'
    matlabversion: '9.9.0.1524771 (R2020b) Update 2'
       yalmiptime: 0.1159
       solvertime: 0.2331
             info: 'Infeasible problem (QUADPROG))'
          problem: 1

K>> i

i =

     4  

Your MPC controller is thus badly tuned (too short horizon being the obvious start)

Also, your definition of u is weird as it has length 4 instead of length N (thus meaning you have two trailing variables in u which never are used, leading to them having value Nan when you look at them)

There are numerous MPC-examples in the tutorials which you should be able to use directly https://yalmip.github.io/example/standardmpc/

YALMIP-specific question are better asked at

https://github.com/yalmip/YALMIP/discussions

https://groups.google.com/g/yalmip