MATLAB Examples

# Swing-up Control of a Pendulum Using Nonlinear Model Predictive Control

This example uses a nonlinear model predictive controller (nonlinear MPC) to achieve swing-up and balancing control of an inverted pendulum on a cart.

## Product Requirement

This example requires Optimization Toolbox™ to solve a nonlinear programming problem at each control interval.

```if ~mpcchecktoolboxinstalled('optim') disp('Optimization Toolbox is required to run this example.') return end ```

## Pendulum/Cart Assembly

The plant for this example is a pendulum/cart assembly, where z is the cart position and theta is the pendulum angle. The manipulated variable for this system is a variable force F acting on the cart. The range of the force is between -100 and 100. An impulsive disturbance dF can happen to push the pendulum as well.

## Control Objectives

Assume the following initial conditions for the pendulum/cart assembly:

• The cart is stationary at z = 0.
• The pendulum is in a downward equilibrium position where theta = -pi.

The control objectives are:

• Swing-up control - Initially swing the pendulum up to an inverted equilibrium position where z = 0 and theta = 0.
• Cart position reference tracking - Move the cart to a new position with a step setpoint change, keeping the pendulum inverted.
• Pendulum balancing - When an impulse disturbance of magnitude of 2 is applied to the inverted pendulum, keep the pendulum balanced and return the cart to its original position.

The downward equilibrium position is stable and the inverted equilibrium position is unstable, which makes the swing-up control more challenging for a single controller, which is the main reason that nonlinear MPC is chosen over linear MPC in this example.

## Control Structure

In this example, use a nonlinear MPC with the following I/O configuration:

• One manipulated variable - variable force (F).
• Two measured outputs - cart position (z) and pendulum angle (theta).

Two other states, cart velocity (zdot) and pendulum angular velocity (thetadot) are not measurable.

While the setpoint of the cart position, z, can vary, the setpoint of the pendulum angle, theta, is always 0 (inverted equilibrium position).

## Nonlinear Plant Model

The major benefit of nonlinear model predictive control is that it uses a nonlinear dynamic model to predict plant behavior in the future across a wide range of operating conditions.

This nonlinear model is usually a first principle model consisting of a set of differential and algebraic equations (DAEs). In this example, the cart and pendulum dynamic system is defined in the pendulumCT0 function. For more details, see pendulumCT0.m.

To use the model for run-time prediction in MPC, you need an ODE solver to compute the plant state x(k+1) from the previous state x(k) and input u(k). This solver is defined in the pendulumDT0 function. For more information, see pendulumDT0.m. This function uses the simple Euler method for integration because it is more efficient for plants with nonstiff ODEs. You can choose other ODE solvers, such as:

• ode23 or ode45 for better accuracy.
• ode15s or ode23s for stiff ODEs.

In this example, because only two states are measurable, an Extended Kalman Filter block is employed to provide nonlinear state estimation for all plant states. Its state transition function is defined in pendulumStateFcn.m and its measurement function is defined in pendulumMeasurementFcn.m.

## Nonlinear MPC Design

Like linear MPC, nonlinear MPC also solves a constrained optimization problem at each control interval. However, since the plant model is nonlinear, the nonlinear MPC converts the optimal control problem into a nonlinear optimization problem with a nonlinear cost function and nonlinear constraints.

The cost function used in this example is defined in the pendulumObjectiveFCN function. It is similar to the standard cost function used by linear MPC, where state reference tracking and manipulated variable move suppression are enforced. For more details, see pendulumObjectiveFCN.m.

The constraint function used in this example is defined in the pendulumConstraintFCN function. Cart position is limited to the range -10 to 10. These constraints are applied across the prediction horizon. For more details, see pendulumConstraintFCN.m.

Like linear MPC, the sample time and prediction horizon should be chosen to provide satisfactory control performance avoiding excessive computational effort. This requirement is especially important for nonlinear MPC because nonlinear optimization is performed at each control interval.

Set the sample time:

```Ts = 0.1; ```

Set the prediction horizon:

```N = 10; ```

## Closed-Loop Simulation in MATLAB®

The initial conditions of the simulation are:

• The cart is stationary at z = 0.
• The pendulum is in a downward equilibrium position, theta = -pi.
• The force applied to the cart is zero.
```x = [0;0;-pi;0]; uopt = zeros(N,1); ```

x is the reference setpoint for the plant states, with the states in the following order - z, zdot, theta, thetadot.

uopt is the input force profile across the prediction horizon.

In the first stage of the simulation, the pendulum swings up from a downward equilibrium position to an inverted equilibrium position. The state references for this stage are all zero.

```xref1 = [0;0;0;0]; ```

At a time of 10, the cart moves from position 0 to 5 and the references for the states become:

```xref2 = [5;0;0;0]; ```

In this example, to compute the optimal control sequence, use the fmincon function from the Optimization Toolbox as the nonlinear programming solver. Select the sequential quadratic programming (SQP) algorithm, which is one of the most common approaches in nonlinear MPC applications.

```options = optimoptions('fmincon','Algorithm','sqp','Display','none'); ```

Run the simulation for 20 seconds, using fmincon with the defined nonlinear cost function and constraint function, pendulumObjectiveFCN and pendulumConstraintFCN, respectively. The MV constraints are the lower and upper bounds of optimization variables in fmincon.

```Duration = 20; % Apply the MV constraints because the force has lower and upper bounds. LB = -100*ones(N,1); UB = 100*ones(N,1); % Start simulation fprintf('Simulation started. It might take a while...\n') xHistory = x; for ct = 1:(20/Ts) % Set references. if ct*Ts<10 xref = xref1; else xref = xref2; end % Nonlinear MPC computation with full state feedback (no state estimator) COSTFUN = @(u) pendulumObjectiveFCN(u,x,Ts,N,xref,uopt(1)); CONSFUN = @(u) pendulumConstraintFCN(u,x,Ts,N); uopt = fmincon(COSTFUN,uopt,[],[],[],[],LB,UB,CONSFUN,options); % Implement first optimal control move and update plant states. x = pendulumDT0(x, uopt(1), Ts); % Save plant states for display. xHistory = [xHistory x]; %#ok<*AGROW> end fprintf('Simulation finished!\n') ```
```Simulation started. It might take a while... Simulation finished! ```

Plot the closed-loop response.

```figure; subplot(2,2,1); plot(0:Ts:Duration,xHistory(1,:)); xlabel('time'); ylabel('z'); title('cart position'); subplot(2,2,2); plot(0:Ts:Duration,xHistory(2,:)); xlabel('time'); ylabel('zdot'); title('cart velocity'); subplot(2,2,3); plot(0:Ts:Duration,xHistory(3,:)); xlabel('time'); ylabel('theta'); title('pendulum angle'); subplot(2,2,4); plot(0:Ts:Duration,xHistory(4,:)); xlabel('time'); ylabel('thetadot'); title('pendulum velocity'); ```

The pendulum angle plot shows that the pendulum is successfully swung up around a time of 2 seconds. During the swing-up process, the cart is displaced with a peak deviation of -1, and returned to its original position around a time of 3 seconds.

The cart position plot shows that the cart successfully moves to z = 5 around a time of 3 seconds. During the moving process, the pendulum is displaced with a peak deviation of -0.85 radians (49 degrees), and returned to an inverted equilibrium position around a time of 3 seconds as well.

This example also requires Simulink software to simulate nonlinear MPC control of a pendulum model in Simulink.

```if ~mpcchecktoolboxinstalled('simulink') disp('Simulink(TR) is required to run this example.') return end ```

Validate the nonlinear MPC with a closed-loop simulation in Simulink.

```mdlMPC = 'mpc_pendcartNMPC'; open_system(mdlMPC); ```

Run the simulation for 30 seconds, using fmincon as an extrinsic function inside the Nonlinear MPC MATLAB Function block.

```open_system([mdlMPC '/Scope']); sim(mdlMPC); ```

The nonlinear simulation in Simulink produces identical swing-up and cart position tracking results compared to the MATLAB simulation. Additionally, a push (impulse disturbance dF) is applied to the inverted pendulum at a time of 20 seconds. The nonlinear MPC successfully rejects the disturbance and returns the cart to z = 5 and the pendulum to an inverted equilibrium position.

Note that fmincon does not support code generation.

## Conclusion

This example illustrates a general workflow to design and simulate nonlinear MPC in MATLAB and Simulink. Depending on the specific nonlinear plant characteristics and control requirements, the implementation details can vary significantly. The key design challenges include:

• Choosing an ODE solver
• Designing a nonlinear state estimator
• Choosing a nonlinear optimization solver
• Designing the nonlinear cost function and constraint function

You can use the functions and Simulink model in this example as templates for other nonlinear MPC design and simulation tasks.

```bdclose(mdlMPC); ```