Control design for a pendulum on a cart

In this example we will consider control design for the basic inverted pendulum on a cart. This system has two equilibria, one where the pendulum is hanging straight down, and one where it's balancing straight up. The upper one is unstable, making it slightly more interesting to design a controller for (even if the lower equilibrium is highly relevant, it's a good model for an overhead crane moving goods).

System model

In this tutorial, we assume that we have the nonlinear dynamics of the system encodeed as a julia function ẋ = cartpole(x, u), and linearize this to get a statespace system

\[\begin{aligned} ẋ &= Ax + Bu\\ y &= Cx \end{aligned}\]

We make use of ForwardDiff.jl for the linearization. We start by defining the dynamics function

using ControlSystemsBase, RobustAndOptimalControl, ForwardDiff, LinearAlgebra, Plots

function cartpole(x, u)
    mc, mp, l, g = 1.0, 0.2, 0.5, 9.81

    q  = x[1:2]
    qd = x[3:4]

    s = sin(q[2])
    c = cos(q[2])

    H = [mc+mp mp*l*c; mp*l*c mp*l^2]
    C = [0.1 -mp*qd[2]*l*s; 0 0]
    G = [0, mp * g * l * s]
    B = [1, 0]

    qdd = -H \ (C * qd + G - B * u[1])
    return [qd; qdd]
end

nu = 1    # number of control inputs
nx = 4    # number of states
ny = 2    # number of outputs (here we assume that the cart position and the pendulum angle are measurable)

Linearization

The next step is to choose an operating point around which to linearize and to calculate the Jacobians $A$ and $B$:

x0 = [0, π, 0, 0]
u0 = [0]

Ac = ForwardDiff.jacobian(x->cartpole(x, u0), x0)
Bc = ForwardDiff.jacobian(u->cartpole(x0, u), u0)
Cc = [1 0 0 0; 0 1 0 0]
Λ = Diagonal([0.4, deg2rad(25)]) # Maximum output ranges
Cc = Λ\Cc # This normalizes expected outputs to be ∈ [-1, 1], a good practice for MIMO systems

we package everything into a StateSpace object and visualize its poles and zeros:

sys = ss(Ac, Bc, Cc, 0)

StateSpace{Continuous, Float64}
A = 
 0.0   0.0                  1.0  0.0
 0.0   0.0                  0.0  1.0
 0.0   1.9620000000000002  -0.1  0.0
 0.0  23.544               -0.2  0.0
B = 
 0.0
 0.0
 1.0
 2.0
C = 
 2.5  0.0                0.0  0.0
 0.0  2.291831180523293  0.0  0.0
D = 
 0.0
 0.0

Continuous-time state-space model

pzmap(sys)

Control design

We will design a number of different controllers. We will start with a basic PID controller. Since the PID controller in its standard form really only handles SISO systems, we will also design a state-feedback controller with an observer to estimate the full state vector $x$ based on the two measurements $y$. Lastly, we will attempt to "robustify" the state-feedback controller using the glover_mcfarlane procedure.

Since the system has an unstable pole $p \approx 4.85$rad/s, there wil be fundamental limitations on the performance of the closed loop system. A common rule-of-thumb (see, e.g., Åström and Murray) is that a single RHP pole $p$ puts a lower limit on the gain crossover frequency $\omega_{gc} > 2p$, something to take into consideration when tuning our controllers.

PID controller

Since the PID controller only accepts a single measurement, we choose the measurement of the pendulum angle for feedback. While doing so, we notice that the number of states in the model can be reduced by the function sminreal

P = sminreal(sys[2,1]) # Position state goes away, not observable

StateSpace{Continuous, Float64}
A = 
  0.0                  0.0  1.0
  1.9620000000000002  -0.1  0.0
 23.544               -0.2  0.0
B = 
 0.0
 1.0
 2.0
C = 
 2.291831180523293  0.0  0.0
D = 
 0.0

Continuous-time state-space model

this indicates that the state corresponding to the position of the cart is not observable from the measurement of the pendulum angle. This is slightly worrisome, but we nevertheless proceed to design a controller. By using a single measurement only, we have also introduced a zero in the system

pzmap(P)

A PID controller can be constructed using the function pid. We start our tuning by a simple P controller

C = pid(1, 0, 0)

TransferFunction{Continuous, ControlSystemsBase.SisoRational{Int64}}
1
-
1

Continuous-time transfer function model

We will attempt to perform loop shaping using the PID controller, and plot the stability margins in a Bode plot using the function marginplot

w = exp10.(LinRange(-2.5, 3, 500))
function pid_marginplot(C)
    f1 = marginplot(P*C, w)
    vline!([2*4.85], sp=1, lab="Fundamental limitation", l=(:dash, :black))
    ylims!((1e-3, 1e2), sp=1)
    f2 = nyquistplot(P*C)
    plot(f1, f2)
end
pid_marginplot(C)

We notice that the gain of the loop-transfer function $L = PC$ is much too low, and increase it, we also notice that the Nyquist plot fails to encircle to critical point, which it has to do once since we have one unstable pole. We will solve this in the end by adding integral action, but proceed for now to shape other parts of the loop. We start by lifting the Bode curve by increasing the gain:

C = pid(20, 0, 0)
pid_marginplot(C)

we are now getting close to the rule-of-thumb for $\omega_{gc}$, but have a low loop gain at low frequencies. Remember, to get good disturbance rejection, we typically want a high loop gain at low frequencies. We also have an extremely small phase margin at 0.66 degrees. To fix the phase margin, we add some derivative gain. While adding derivative gain, it's also a good idea to add noise filtering (with a pure derivative term, the PID controller is not proper and can not be realized as a statespace system)

C = pid(20, 0, 0.2, Tf=0.01)
pid_marginplot(C)

The derivative term lifted the phase at $\omega_{gc}$ and we now have very nice phase margins. We also got a slight increase in $\omega_{gc}$ while at it.

The closed-loop system will still be unstable since the Nyquist curve fails to encircle the point -1, something we can check by calling

isstable(feedback(P*C))

false

We make the Nyquist curve wrap around the -1 point by adding integral gain:

C = pid(20, 1.25, 0.2, Tf=0.01)
pid_marginplot(C)

Now, the Nyquist curve looks fine and the system is stable

isstable(minreal(feedback(P*C)))

true

If we simulate a disturbance acting on this system (feedback(P, C) is the transfer function from load disturbance to output)

plot(step(feedback(P,C), 8), ylab="ϕ")

we see that we have a reasonable disturbance response.

To verify robustness properties, we plot the gang-of-four sensitivity functions:

f1 = gangoffourplot(P,C,w, Ms_lines=[1.4], Mt_lines=[1.5])
f2 = nyquistplot(P*C, Ms_circles=[1.4], Mt_circles=[1.5], ylims=(-2, 2), xlims=(-4,1))
plot(f1, f2, size=(1000,800))

This all looks nice and we appear to have reasonable robustness margins, the Nyquist curve stays outside the $M_S = 1.4$ circle and the $M_T = 1.5$ circle.

However, there is a dragon lurking behind these plots. Remember the state corresponding the the cart position that was removed above? What has happened to this state? To investigate this, we form an ExtendedStateSpace model where we have both cart position and pendulum angle as controlled outputs, while keeping only the pendulum angle as measured output:

Pe = ExtendedStateSpace(sys, C2 = sys.C[2:2, :]) # Indicate that we can only measure the pendulum angle
Gecl = feedback(Pe, ss(C)) |> minreal
plot(step(Gecl, 8), ylab=["Cart pos" "ϕ"])