Swing-Up Control of a Pendulum on a Cart using Multistage Nonlinear MPC with Nonlinear Grey-Box Model

Modeling and Estimating the Pendulum/Cart Assembly

The nonlinear plant is the pendulum and cart assembly system as seen below.

The plant model consists of four states which are the cart position, cart velocity, pendulum angle and pendulum angular velocity. The control variable is the force (F) with an impulsive disturbance dF acting on the cart. The measured outputs are the cart position and pendulum angle.

This model is also used (and illustrated in more detail) in the example Swing-Up Control of Pendulum Using Nonlinear Model Predictive Control. For a more thorough explanation of the conventions and how to derive the underlying system equations, see Derive Equations of Motion and Simulate Cart-Pole System (Symbolic Math Toolbox).

To estimate the model, load the available input-output data from the MAT-file. The data consists of a uniformly sampled data set with 4001 samples in the variables u and y with a sample time of 0.01 seconds. The input (u) is the force applied on the cart which is a periodic sinusoidal signal and the output (y) is a vector containing the cart position and pendulum angle.

load("pendulum_cart_data.mat")

Create an iddata (System Identification Toolbox) object to represent and store the data. For more information, see Representing Time- and Frequency-Domain Data Using iddata Objects (System Identification Toolbox).

z = iddata(y, u, 0.01, Name="Pendulum Cart Data");

Assign names and units to the stored signals.

z.InputName = {"Angle"};
z.InputUnit = {"rad"};
z.OutputName = {"x"         ... % Position of the Cart
                "theta"     ... % Angle of the Pendulum
                };
z.OutputUnit = {"m" "rad"};
z.Tstart =  0;
z.TimeUnit = "s";

The system has 5 parameters, the mass of the cart (mCart), mass of the pendulum (mPend), gravitational acceleration (g), length of the pendulum (L) and the damping coefficient (Kd). Of these five parameters, three of the parameters (mPend, g and L) are assumed to be known. To create the idnlgrey (System Identification Toolbox) object, first, organize the parameter details into a structure. The structure contains parameter names, units, initial values, limits, and a boolean variable indicating whether the true parameter value is known or not.

Parameters = struct("Name", ...
   {"Mass of the Cart"              ...  % mCart
   "Mass of the Pendulum"           ...  % mPend
   "Gravitational Accerleration"    ...  % g
   "Length of the Pendulum"         ...  % L
   "Damping Coefficient"},          ...  % Kd
   "Unit", ...
   {"Kg", "Kg", "m/s^2", "m", "Ns/m"},   ...
    "Value", ...
    {0.4, 1, 9.81, 0.5, 6},      ...
    "Minimum", {-Inf -Inf -Inf -Inf -Inf}, ...
    "Maximum", {Inf Inf Inf Inf Inf},... % Ignore constraints.
     "Fixed", ...
    {false true true true false});       % mCart and Kd are unknown.

The system dynamics are described in the file swingup_m.m.

type("swingup_m.m");

function [dxdt, y] = swingup_m( ...
    t, x, u, mCart, mPend, g, L, Kd, varargin)
%% Continuous-time nonlinear dynamic model 
% of a pendulum on a cart
%
% 4 states (x): 
%   cart position (z)
%   cart velocity (z_dot): positive right
%   angle (theta): 0 at upright position
%   angular velocity (theta_dot): 
%                       anti-clockwise positive
% 
% 1 inputs: (u)
%   force (F): when positive, 
%                       force pushes cart to right 
%
% Copyright 2024 The MathWorks, Inc.


%% Obtain x, u and y

% x
z_dot = x(2);
theta = x(3);
th_dot = x(4);

% u
F = u;

%% Compute dxdt
dxdt = [
    
    z_dot;...

    (F - Kd*z_dot - mPend*L*th_dot^2*sin(theta) ...
    + mPend*g*sin(theta)*cos(theta))/ ...
    (mCart + mPend*sin(theta)^2); ...

    th_dot; ...

    ((F - Kd*z_dot - mPend*L*th_dot^2*sin(theta))* ...
    cos(theta)/(mCart + mPend) + g*sin(theta))/ ...
    (L - mPend*L*cos(theta)^2/(mCart + mPend))

    ];

% y
y = [x(1);x(3)];

Use the model file, along with the order, parameters, and initial states data to create an idnlgrey object describing the system.

FileName      = "swingup_m";           % ODE File
Order         = [2 1 4];               % Model orders [ny nu nx]
InitialStates = [0; 0; -pi; 0];        % Initial states values
Ts            = 0;                     % Continuous-time system
nlgr = idnlgrey(FileName, Order, Parameters, InitialStates, Ts); 

The system described by nlgr has four states, one input, and two outputs. The model also contains 5 parameters, 3 of which are fixed and the other 2 need to be estimated.

nlgr

nlgr =

Continuous-time nonlinear grey-box model defined by 'swingup_m' (MATLAB file):

   dx/dt = F(t, x(t), u(t), p1, ..., p5)
    y(t) = H(t, x(t), u(t), p1, ..., p5) + e(t)

 with 1 input(s), 4 state(s), 2 output(s), and 2 free parameter(s) (out of 5).

Status:                                                         
Created by direct construction or transformation. Not estimated.

To estimate the unknown system parameters, first, create an nlgreyestOptions object to display the estimation progress and to set the maximum number of iterations.

opt = nlgreyestOptions(Display="on");
opt.SearchOptions.MaxIterations = 5;

Then, use nlgreyest (System Identification Toolbox) to estimate the unknown parameters.

nlgr = nlgreyest(z, nlgr, opt);

To evaluate the quality of the estimated model, simulate the estimated model and compare the measured and simulated output.

compare(z, nlgr); 

Generate State Function and its Jacobian from Nonlinear Grey-Box

The multistage nonlinear MPC controller requires state and Jacobian functions defined as MATLAB® function files. The nonlinear grey-box model provides the generateMATLABFunction command, which you can use to generate these function files from the estimated idnlgrey object. If you name the state function as stateFcnPend then the software names the corresponding Jacobian function as stateFcnPendJacobian.

generateMATLABFunction(nlgr, "stateFcnPend");

The software creates the files stateFcnPend and stateFcnPendJacobian in your current folder to store the state function and its Jacobian, respectively. These functions utilize the estimated parameters and invoke the corresponding ODE file.

Design Multistage Nonlinear MPC Using the Provided Functions

In this example, the plant has four states and one input. Set the prediction horizon h to 10 and sample time Ts to 0.1 s.

Create the multistage nonlinear MPC controller.

Ts = 0.1;
h = 10; 
msobj = nlmpcMultistage(h,4,1);
msobj.Ts = Ts; 

Specify the state and Jacobian functions obtained from generateMATLABFunction.

msobj.Model.StateFcn = "stateFcnPend";
msobj.Model.StateJacFcn = "stateFcnPendJacobian";
msobj.Model.IsContinuousTime = true; 

Specify the constraints on the cart position (first state) and the force (the manipulated variable).

msobj.ManipulatedVariables.Min = -100;
msobj.ManipulatedVariables.Max = 100;
msobj.States(1).Min = -10; 
msobj.States(1).Max = 10;
msobj.UseMVRate = true;

For this example, the cost function and its Jacobians for all stages are specified in the files myCostSwingup.m and myCostSwingupGradientFcn.m. In both functions, the last input argument p is the parameter vector containing the reference signal for the cart position and the pendulum angle.

type("myCostSwingup.m")

function cost =  myCostSwingup(stage,x,u,dmv,p)

Wmv = 0.01;
output = [x(1) x(3)]; 
ref = [p(1) p(2)];

if stage == 1
    cost = Wmv*(dmv^2);
elseif stage == 11
    cost = (output-ref)*(output-ref)';
else
    cost = (output-ref)*(output-ref)' + Wmv*(dmv^2);
end

end

type("myCostSwingupGradientFcn.m")

function [Gx, Gmv, Gdmv] = myCostSwingupGradientFcn(stage,x,u,dmv,p)

Wmv = 0.01; 
Gmv = zeros(1,1);
 
if stage == 1
    Gx = zeros(4,1);
    Gdmv = 2*Wmv*dmv;
elseif stage == 11
    Gx = [2*(x(1)-p(1)); 0; 2*(x(3)-p(2)); 0];
    Gdmv = zeros(1,1);
else
    Gx = [2*(x(1)-p(1)); 0; 2*(x(3)-p(2)); 0];
    Gdmv = 2*Wmv*dmv;
end

Set the cost function, Jacobian, and parameter length for each stage.

for ct=1:h+1
    msobj.Stages(ct).CostFcn = "myCostSwingup";
    msobj.Stages(ct).CostJacFcn = "myCostSwingupGradientFcn";
    msobj.Stages(ct).ParameterLength = 2;
end

Validate the state and cost functions along with their Jacobians.

simdata = getSimulationData(msobj);
simdata.StageParameter = repmat([0;-pi], h+1, 1); 
validateFcns(msobj, [0.1 0.2 -pi/2 0.3], 0.4, simdata)

Model.StateFcn is OK.
Model.StateJacFcn is OK.
"CostFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11] are OK.
"CostJacFcn" of the following stages [1 2 3 4 5 6 7 8 9 10 11] are OK.
Analysis of user-provided model, cost, and constraint functions complete.

Closed-Loop Simulation in Simulink

Validate the performance of the controller by simulating it in closed-loop with the original nonlinear model.

Open the Simulink® model of the closed-loop system.

mdl = "swingup_pendulum_multistageNMPC"; 
open_system(mdl)

In this model, the Multistage MPC block is configured to use the msobj object in the workspace.

Run the simulation for 30 seconds.

open_system(mdl+'/Scope')
sim(mdl);

As seen from the simulation, the multistage nonlinear MPC controller designed from the estimated grey-box model can successfully balance the pendulum in the inverted equilibrium, track the reference position, and reject disturbances.