Polynomial-quadratic control design

The polynomial-quadratic regulator (PQR) is an extension to the Linear Quadratic Regulator (LQR). For continuous-time systems on the form,

\[\dot{x} = f(x, u) = Ax + Bu + A_2 (x \otimes x) + A_3 (x \otimes x \otimes x) + \cdots\]

PQR finds a state-feedback controller $u = K(x)$ that optimizes the familiar quadratic cost function

\[J(x, u) = \int_0^\infty \left( x^T Q x + u^T R u \right) dt\]

where $Q$ and $R$ are symmetric positive definite matrices.

Here, the dynamics $f$ is characterized by being polynomial in $x$ and linear in $u$ (input affine), while the cost is quadratic. If $A_i = 0 \, \forall\, i \geq 2$, PQR reduces to the familiar LQR controller. In many situations, non-polynomial terms in $f$ can be approximated using polynomials using the function poly_approx. Several examples of this will be shown below.

Polynomial-quadratic control problems can be solved using the function pqr, which takes either $f(x, u)$ as a function, or the matrices $A$ and $B$ as well as a vector of matrices $N = \left[ A_2, A_3, \cdots \right]$. pqr returns a PQRSolution which can be passed to build_K_function to perform code generation and return controller functions $u = K(x)$ and $K!(u, x)$. The solution can also be passed into the function predicted_cost to compute the predicted infinite-horizon cost achieved by the controller from a particular initial state.

Example

The following example, which is taken from ^[1], demonstrates how to use the PQR controller design to solve the Lorenz system with added control inputs:

\[\begin{aligned} \dot{x}_1(t) &= -10 x_1(t) + 10 x_2(t) + u(t) \\ \dot{x}_2(t) &= 28 x_1(t) - x_2(t) - x_1(t)x_3(t)\\ \dot{x}_3(t) &= x_1(t)x_2(t) - 8/3 x_3(t) \\ \end{aligned}\]

We start by defining the system dynamics and cost function, and then solve the optimal control problem using pqr. We then use build_K_function to generate a controller function $u = K(x)$ and simulate the closed-loop system using solve from OrdinaryDiffEq. To compute the cost achieved by the controller, we add the cumulative integral of the cost as an additional state to the closed-loop dynamics and let the ODE integrator compute the integral for us. We compare this cost to the cost predicted by predicted_cost which uses the approximative value function obtained from solving the PQR problem (similar to the Ricatti solution obtained when solving an LQR problem).

using JuliaSimControl, Test, Plots, LinearAlgebra, OrdinaryDiffEq, StaticArrays

nx = 3 # Number of state variables
nu = 1 # Number of control inputs
A = [
    -10 10 0
    28 -1 0
    0 0 -8/3
]
B = [1; 0; 0;;]
A2 = zeros(nx, nx^2)
A2[2,3] = A2[2,7] = -1/2
A2[3,2] = A2[3,4] = 1/2

Q = diagm(ones(3)) # State cost matrix
R = diagm(ones(1)) # Control input cost matrix

f = (x, u) -> A * x + B * u + A2 * kron(x, x) # Dynamics for simulation
l = (x, u) -> dot(x, Q, x) + dot(u, R, u)     # Cost function for simulation

function closed_loop(xc, (f,l,K), t)          # Closed-loop dynamics for simulation
    x = xc[1:end-1] # Last value is integral of cost
    u = K(x)
    dx = f(x, u)
    dc = l(x, u)
    [dx; dc]        # Return state and cost derivatives
end

x0 = Float64[10, 10, 10, 0] # Initial state, last value is integral of cost
tspan = (0.0, 50.0)
prob = ODEProblem(closed_loop, x0, tspan, (f, l, x->x))

# Design controller of degree 2
pqrsol = pqr(A, B, [A2], Q, R, 2) # It's possible to pass in f instead of A, B, A2
K, _ = build_K_function(pqrsol) # Returns u = K(x) and K!(u, x)
c = predicted_cost(pqrsol, x0[1:end-1])
@test c ≈ 7062.15 atol=0.02 # Compare to paper
sol = solve(prob, Rodas5P(), p=(f,l,K), reltol=1e-8, abstol=1e-8);
c = sol.u[end][end]
@test c ≈ 6911.03 rtol=1e-2 # Compare to paper

# Design controller of degree 4
pqrsol = pqr(A, B, [A2], Q, R, 4)
K, _ = build_K_function(pqrsol)
c = predicted_cost(pqrsol, x0[1:end-1])
@test c ≈ 6924.27 atol=0.02
sol = solve(prob, Rodas5P(), p=(f,l,K), reltol=1e-8, abstol=1e-8);
c = sol.u[end][end]
@test c ≈ 6906.21 rtol=1e-2

# Simulate shorter horizon for plotting
sol = solve(prob, Rodas5P(), p=(f,l,K), tspan = (0, 2.5), reltol=1e-8, abstol=1e-8);
plot(sol, layout=4, title=["x1" "x2" "x3" "Cumulative cost"], legend=false, framestyle=:zerolines)

Inspecting the controller $K(x)$ and value function $V(x)$

The controller $K(x)$ generated by solving the PQR problem is a polynomial function of the state:

\[K(x) = k_1 x + k_2 (x \otimes x) + k_3 (x \otimes x \otimes x) + \cdots\]

The struct pqrsol::PQRSolution = pqr(...) contains the fields Ks::Vector{Matrix} and Vs::Vector{Matrix}, where each entry in Ks corresponds to the Ks[i] = k_i arrays above. Similarly, Vs contains the matrices in the approximate value function $V(x)$ (an approximate Lyapunov function, sometimes also called cost-to-go):

\[V(x) = v_1 (x \otimes x) + v_2 (x \otimes x \otimes x) + \cdots\]

Note, the value function does not have a linear term like the controller does. The value function can be evaluated using the function predicted_cost, which returns the predicted infinite-horizon cost from a particular state $x$.

The controller corresponds to a polynomial, to have a look at this polynomial (rather than the matrices Ks), we can trace through it with symbolic variables. If we have obtained K from build_k_function, we would do

using JuliaSimControl.DynamicPolynomials
@polyvar x[1:nx]
K(x)

if we haven't built a function, we could also do

using JuliaSimControl.DynamicPolynomials
@polyvar x[1:nx] # Alternatively: Symbolics.@variables x[1:nx]
JuliaSimControl.Kfun(pqrsol, 2)(x) # Look at the second-degree controller

1-element Vector{DynamicPolynomials.Polynomial{DynamicPolynomials.Commutative{DynamicPolynomials.CreationOrder}, MultivariatePolynomials.Graded{MultivariatePolynomials.LexOrder}, Float64}}:
 -18.49064811180869x₂ - 23.71166406840945x₁ + 0.06701176224936817x₂x₃ + 0.5811664706751598x₁x₃

The controller is a now a vector of polynomials, in this case, there is only one entry since the system has a single input.

Example with non-zero reference

The following example uses the same dynamical system as Example: Dynamical extremum seeking

\[\begin{aligned} \dot{x}_1(t) &= x_1(t)^2 + x_2(t) + u(t) \\ \dot{x}_1(t) &= x_1(t)^2 - x_2(t)\\ \end{aligned}\]

In the extremum-seeking example, the goal was to reach the unknown state and control input that optimized an performance function. In this example, we assume that the optimizing operating point is known, and design a polynomial-quadratic controller that stabilizes the system around this operating point. We thus create two versions of the dynamics, $f$ operates in absolute coordinates, while $fr$ operates in coordinates relative to the operating point $\Delta x = x - x_r$ and $\Delta u = u - u_r$. The controller is designed using $f_r$ and operates in relative coordinates, but the closed-loop system is simulated using $f$ and operates in absolute coordinates.

xr = [0.5, 0.25]
ur = [-0.5]
f = function (x, u) # Operates in standard coordinates
    x1,x2 = x
    [
        x1^2 + x2 + u[]
        x1^2 - x2
    ]
end

fr = (Δx,Δu) -> f(Δx + xr, Δu + ur) # Operates in Δx, Δu coordinates

@assert f(xr, ur) ≈ [0, 0]                # Verify that xr, ur is an equilibrium point
@assert fr(zeros(2), zeros(1)) ≈ [0, 0]

function closed_loop_r(xc, (f,l,K), t) # Closed-loop dynamics for simulation
    x = xc[1:end-1] # Last value is integral of cost
    Δx = x - xr
    Δu = K(Δx)      # K operates on Δx
    u = Δu + ur
    dx = f(x, u)
    dc = l(Δx, Δu)
    [dx; dc]        # Return state and cost derivatives
end

Q = I(2)
R = I(1)

pqrsol = pqr(fr, Q, R, 3)
time = 0:0.01:6
plot(layout=4)
for deg = 1:3
    K, _ = build_K_function(pqrsol, deg)
    prob = ODEProblem(closed_loop_r, zeros(3), (0.0, 6.0), (f, l, K))
    sol = solve(prob, Tsit5());
    plot!(sol, layout=3, title=["x1" "x2" "Cumulative cost"], lab="Degree $deg")
    u = map(time) do t
        x = sol(t)[1:end-1]
        K(x - xr) + ur
    end
    u = reduce(vcat, u)
    plot!(time, u, sp=4, title="u")
end
hline!(xr', c=:black, ls=:dash, label="Reference")
hline!(ur', c=:black, ls=:dash, label="Reference", sp=4)

The figure above compares the closed-loop response for different degrees of the controller, in this case, a second-order controller appears sufficient.

Example: Quadrotor with non-affine inputs in MTK

This example will showcase a more realistic scenario that demonstrates a number of interesting things:

• How to use ModelingToolkit to model the system
• How to handle non-polynomial terms
• How to handle non-affine inputs

The system we will consider is a quadrotor with dynamics in a plane, that is, we consider one translation and one rotation degree of freedom. The dynamics are given by

\[\begin{aligned} \dot{y} &= v \\ \dot{v} &= -g + K_u \cos(\phi)u_1 \\ \dot{\phi} &= \omega \\ \dot{\omega} &= -d_0 \phi - d_1 ω + n_0 u_2 \end{aligned}\]

where $y$ is the vertical position, $v$ is the vertical velocity, $\phi$ is the pitch angle, and $\omega$ is the pitch rate. The control inputs are $u_1$ which is the thrust, and $u_2$ which is the pitching torque. The parameters $K_u$, $d_0$, $d_1$, and $n_0$ are constants, while $g$ is the gravitational constant.

This model exhibits the following problematic aspects for the PQR controller design:

• The dynamics are non-polynomial due to the $\cos$ term
• The input dynamics are non-affine due to the multiplication of the input $u_1$ with a state-dependent term.

We will solve the first problem by approximating the $\cos$ term using a third-order polynomial, and the second problem by introducing a low-pass filter on the control input $u_1$. In Exact handling of trigonometric terms below, we show an alternative approach to handle trigonometric terms, by introducing additional state variables.

Polynomial approximation

To approximate $\cos(\phi)$, we could either linearize it (small-angle approximation), use a Taylor-approximation of higher degree, or perform a least-squares approximation over a certain interval. Here, we will take the latter approach, and use

const cospoly = JuliaSimControl.poly_approx(cos, 5, -2, 2) |> splat(tuple)

(0.997550275987648, 0.0, -0.48881690784591164, 0.0, 0.03399571980756845, 0.0)

to approximate $\cos$ with a fifth-order polynomial over the interval $[-2, 2]$rad. Unsurprisingly, we see that the odd terms are zero. We use these coefficients to define a function cos5 that computes the approximate value of cos using the polynomial coefficients.

cos5(x) = evalpoly(x, cospoly)

cos5 (generic function with 1 method)

The approximation is reasonably accurate over the considered interval, much more accurate towards the edges than a Taylor approximation of the same degree:

using Plots
plot([
        cos,
        cos5,
        x->1-x^2/2+x^4/24, # 5:th order Taylor approximation
    ],
    -2, 2,
    lab=["cos" "5:th order LS-approx" "5:th order Taylor approx"]
)

Handling of non-affine inputs

The PQR design method assumes that $u$ enters the dynamics linearly, i.e., like $\dot x = \cdots + Bu$ where $B$ is a matrix. Here, we have a term $K_u \cos(\phi)u_1$ which is a nonlinear function of the input and a state variable $\phi$. To handle this, we introduce a first-order low-pass filter on the input, after which the input $u_1$ affects the filter state $x_f$ linearly $T \dot x_f = -x_f + u_1$, and the filter state instead appears in the equation $K_u \cos(\phi)x_f$. This trick can actually be motivated from a physical perspective, since the motor that turns the propeller has its own dynamics and those are not included in the simple model above: a change in voltage across the motor is not going to result in an immediate change in thrust. Here, we model the motor dynamics as a first-order low-pass filter.

The full dynamics is now given by

\[\begin{aligned} \dot{y} &= v \\ \dot{v} &= -g + K_u \cos(\phi)x_f \\ \dot{\phi} &= \omega \\ \dot{\omega} &= -d_0 \phi - d_1 ω + n_0 u_2 \\ \dot{x}_f &= (-x_f + u_1) / T \end{aligned}\]

The system model

We are now ready to assemble the full model:

using JuliaSimControl
using OrdinaryDiffEq
using ModelingToolkit
using ModelingToolkitStandardLibrary: Blocks
using ControlSystemsMTK
using Plots
using LinearAlgebra

gravity = 9.81
Ku = 0.89 / 1.4
d0 = 70
d1 = 17
n0 = 55

@named motor_dynamics = Blocks.FirstOrder(T = 0.001)

x0 = [0.85, 1, π/12, π/2]

@parameters t
@variables  u(t)[1:2]=0
u = collect(u)
@variables y(t)=0.85 v(t)=1 ϕ(t)=π/12 ω(t)=π/2

D = Differential(t)

eqs = [
    D(y) ~ v,
    D(v) ~ -gravity + Ku * cos5(ϕ)*(motor_dynamics.output.u + gravity/Ku),
    D(ϕ) ~ ω,
    D(ω) ~ -d0 * ϕ - d1 * ω + n0 * u[2],
    motor_dynamics.input.u ~ u[1]
]

@named model = ODESystem(eqs, t; systems=[motor_dynamics])

\[ \begin{align} \frac{\mathrm{d} y\left( t \right)}{\mathrm{d}t} &= v\left( t \right) \\ \frac{\mathrm{d} v\left( t \right)}{\mathrm{d}t} &= -9.81 + 0.63571 \left( 0.99755 + \left( \phi\left( t \right) \right)^{2} \left( -0.48882 + 0.033996 \left( \phi\left( t \right) \right)^{2} \right) \right) \left( 15.431 + \mathtt{motor\_dynamics.output.u}\left( t \right) \right) \\ \frac{\mathrm{d} \phi\left( t \right)}{\mathrm{d}t} &= \omega\left( t \right) \\ \frac{\mathrm{d} \omega\left( t \right)}{\mathrm{d}t} &= 55 u\left( t \right)_{2} - 70 \phi\left( t \right) - 17 \omega\left( t \right) \\ \mathtt{motor\_dynamics.input.u}\left( t \right) &= u\left( t \right)_{1} \\ \mathtt{motor\_dynamics.u}\left( t \right) &= \mathtt{motor\_dynamics.input.u}\left( t \right) \\ \mathtt{motor\_dynamics.y}\left( t \right) &= \mathtt{motor\_dynamics.output.u}\left( t \right) \\ \mathtt{motor\_dynamics.y}\left( t \right) &= \mathtt{motor\_dynamics.x}\left( t \right) \\ \frac{\mathrm{d} \mathtt{motor\_dynamics.x}\left( t \right)}{\mathrm{d}t} &= \frac{ - \mathtt{motor\_dynamics.x}\left( t \right) + \mathtt{motor\_dynamics.k} \mathtt{motor\_dynamics.u}\left( t \right)}{\mathtt{motor\_dynamics.T}} \end{align} \]

Here, we applied additional steady-state gravity compensation to the control input by adding gravity/Ku to the input. This is not strictly necessary, but it makes the controller tuning simpler and we do not have to worry about integral action. In practice, we should of course handle also this if we intend to accurately control the altitude $y$.

To obtain the dynamics in the form of a function $\dot x = f(x, u, p, t)$, we construct a FunctionSystem;

quadrotor = FunctionSystem(model, u, [y, ϕ])
(; nx, nu) = quadrotor

The cost function

To assemble the matrix $Q$, we make use of the function ControlSystemsMTK.build_quadratic_cost_matrix. This function takes care of the fact that we have no control over the order in which the state variables appear in the state vector. However, to build the matrix $R$, we can rely on the fact that the control inputs appear in the same order as they are defined when calling the FunctionSystem constructor above.

R = diagm([1.0, 10.0])
Q = ControlSystemsMTK.build_quadratic_cost_matrix(model, u,
    [y => 10, v => 10, ϕ => 1, ω => 0.01]
)

5×5 Matrix{Float64}:
 10.0   0.0  0.0  0.0   0.0
  0.0  10.0  0.0  0.0   0.0
  0.0   0.0  1.0  0.0   0.0
  0.0   0.0  0.0  0.01  0.0
  0.0   0.0  0.0  0.0   0.0

Simulation

To perform a simulation, we first extract the default values of the parameters and initial conditions from the model, and then use these to define a function f that computes the closed-loop dynamics. We then use pqr to solve the PQR problem, and build_K_function to generate a controller function u = K(x) that computes the control input. We can then use solve from OrdinaryDiffEq to simulate the closed-loop system and plot the result. We compare the result for different degrees of the controller, 1 t0 3.

tspan = (0.0, 8.0)
p = float.([ModelingToolkit.defaults(model)[p] for p in quadrotor.p])
x0sim = float.([ModelingToolkit.defaults(model)[p] for p in quadrotor.x])

f = (x,u)->quadrotor(x,u,p,nothing)
l = (x, u) -> dot(x, Q, x) + dot(u, R, u)

"Closed-loop dynamics with cumulative cost added as last state variable"
function f_closed_loop(xc, K, t)
    x = xc[1:end-1]
    u = K(x)
    ẋ = quadrotor(x, u, p, nothing)
    [ẋ; l(x, u)]
end
prob = ODEProblem(f_closed_loop, SVector{nx + 1}(x0sim..., 0), tspan, x->x)

pqrsol = pqr(f, Q, R, 3)

fig = plot(layout=8)
for deg in 1:3
    K, _ = build_K_function(pqrsol, deg)
    @info "Simulating degree $deg"
    sol = solve(prob, Tsit5(), p = K)
    plot!(sol, title=permutedims([state_names(quadrotor); "Cost"]), lab="Degree $deg")
    time = range(tspan..., 100)
    uv = map(time) do t
        x = sol(t)
        K(x[1:end-1])
    end
    um = reduce(hcat, uv)'
    plot!(time, um, title=permutedims(input_names(quadrotor)), sp=(1:2)' .+ (nx+1), lab="Degree $deg")
end
fig

If we start the simulation further away from the origin (5x further away than above), we need to re-tune the weights a bit for the controller to perform well. In this case, it's enough to increase the penalty on the pitch-torque input in order for the controller not to perform too aggressive pitching motions that bring the system too far away from the validity region of our polynomial approximation to $\cos$. This time, we add also a degree 5 controller since we are expecting a higher-order controller to perform better further away from the origin.

R = diagm([1.0, 100.0])
pqrsol = pqr(f, Q, R, 5)
fig = plot(layout=9)
for deg in 1:2:5
    K, _ = build_K_function(pqrsol, deg)
    sol = solve(prob, Tsit5(), p = K, u0 = SVector{nx + 1}(5x0sim..., 0))
    lab = "Degree $deg"
    plot!(sol; title=permutedims([state_names(quadrotor); "Cost"]), lab)
    time = range(tspan..., 100)
    uv = map(time) do t
        x = sol(t)
        K(x[1:end-1]) # Reconstruct control input for plotting
    end
    um = reduce(hcat, uv)'
    plot!(time, um; title=permutedims(input_names(quadrotor)), sp=(1:2)' .+ (nx+1), lab)

    # Evaluate approximate Lyapunov function along trajectory
    Vt = [predicted_cost(pqrsol, sol(t)[1:nx]) for t in time]
    plot!(time, Vt; title="Lyapunov function V(x)", sp=9, lab)
end
fig

Exact handling of trigonometric terms

In the example with the quadrotor above, we performed a polynomial least-squares approximation of the trigonometric term in order to enable PQR design. Simple trigonometric terms can also be handled exactly by introducing two new state variables: $s = \sin(\phi)$ and $c = \cos(\phi)$. The dynamics are then given by

\[\begin{aligned} \dot{x}_f &= (-x_f + u_1) / \tau \\ \dot{y} &= v \\ \dot{v} &= -g + K_u c x_f \\ \dot{\phi} &= \omega \\ \dot{\omega} &= -d_0 \phi - d_1 ω + n_0 u_2 \\ \dot{s} &= c \omega \\ \dot{c} &= -s \omega \end{aligned}\]

here, we use the same trick as in Handling of non-affine inputs to handle the nonlinear input term $K_u \cos(\phi)u_1$. The dynamics are now polynomial in the state and input, and we can use PQR to design a controller. One problem that will arise when adding the two new state variables is that the resulting system is not linearly controllable in the origin, and the solver for the Ricatti equation will fail when trying to find the first-order feedback gain. This is handled automatically by pqr, which will perform model-order reduction and design a feedback gain for the controllable subsystem.

Another slight complication is that the state variables $s$ and $c$ are fictional, and we must thus compute those as part of the controller. Below, we demonstrate the workflow for this approach. We

1. Define the system dynamics in expanded form, polyquad, and use this function for the pqr design.
2. Define the system dynamics standard form, quad, and use this function for simulation.
3. Define the closed-loop system dynamics, f_cl_polyquad, where we compute the extended state xe = [x; sin(x[4]); cos(x[4])] that serves as the input to the controller.
4. Simulate and plot the result.

using JuliaSimControl, OrdinaryDiffEq, Plots, LinearAlgebra

function polyquad(state, u, p, t)
    g = 9.81; Ku = 0.89 / 1.4; d0 = 70; d1 = 17; n0 = 55; τ = 0.001
    xf, y, v, ϕ, ω, s, c = state
    [
        (-xf + u[1]) / τ
        v
        -g + Ku * c * xf
        ω
        -d0 * ϕ - d1 * ω + n0 * u[2]
        c*ω
        -s*ω
    ]
end

function quad(state, u, p, t)
    g = 9.81; Ku = 0.89 / 1.4; d0 = 70; d1 = 17; n0 = 55; τ = 0.001
    xf, y, v, ϕ, ω = state
    [
        (-xf + u[1]) / τ
        v
        -g + Ku * cos(ϕ) * xf
        ω
        -d0 * ϕ - d1 * ω + n0 * u[2]
    ]
end

ϵ = 1e-6
nx = 5 # State dimension of original dynamics
nu = 2 # Input dimension

Q = diagm(Float64[ϵ, 10, 10, 1, 0.01, ϵ, ϵ])
R = diagm(Float64[1, 100])

xr = [0,0,0,0,0,sin(0), cos(0)] # Reference state
ur = [9.81/(0.89 / 1.4), 0]     # Reference input (counteract gravity using u1)
fr = (Δx,Δu) -> polyquad(Δx + xr, Δu + ur, 0, 0) # Dynamics that operates in Δx, Δu coordinates

pqrsol = pqr(fr, Q, R, 3)

function f_cl_polyquad(xc, K, t)
    x = xc[1:end-1]
    xe = [x; sin(x[4]); cos(x[4])]
    Δx = xe - xr
    Δu = K(Δx)
    u = Δu + ur
    dx = quad(x, u, 0, t)
    dc = dot(Δx, Q, Δx) + dot(Δu, R, Δu) # Cost integrand
    [dx; dc]
end

x0 = 5*[0, 0.85, 1, π/12, π/2, 0] # Initial state, last value is integral of cost
tspan = (0.0, 8.0)
prob = ODEProblem(f_cl_polyquad, x0, tspan)

fig = plot(layout=nx+3)
for deg in [1,3]
    K, _ = build_K_function(pqrsol, deg)
    sol = solve(prob, Tsit5(), p = K)

    t = range(tspan..., 300)
    u = map(t) do t
        x = sol(t)[1:end-1]
        xe = [x; sin(x[4]); cos(x[4])]
        K(xe-xr)
    end
    u = reduce(hcat, u)'
    if isinteractive()
        lab = "deg=$deg"
        plot!(sol; title=["xf" "y" "v" "ϕ" "ω" "cost"], lab)
        plot!(t, u; sp=(1:nu)' .+ (nx+1), title=["u1" "u2"], lab)
    end
end
fig

Vector-valued polynomial approximation

Above, we approximated trigonometric functions using low-order polynomials. However, we can also approximate complete vector-valued dynamics functions using polynomials. The function poly_approx takes a vector-valued dynamics function ẋ = f(x, u), lower and upper bounds on the domain of $x$ and $u$ as well as the monomial degrees for each state variables x[i].

In the example below, we will demonstrate PQR control design for a pendulum on a cart.

PQR for pendulum on cart

We choose a coordinate system such that $\phi = 0$ corresponds to the pendulum pointing upwards (the unstable equilibrium). The state is comprised of

\[x = [p, \phi, v, \dot \phi, x_f]\]

where $p$ is the position of the cart, $\phi$ is the angle of the pendulum, $v$ is the velocity of the cart, $\dot \phi$ is the angular velocity of the pendulum, and $x_f$ is the state of the motor dynamics. The input is the commanded force to be applied to the cart.

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

    ϕ  = x[2] - π # Origin is now at upward equilibrium
    qd = x[SA[3, 4]]
    xf = x[5] # Filter state
    s = sin(ϕ)
    c = cos(ϕ)

    H = [mc+mp mp*l*c; mp*l*c mp*l^2]
    C = [0 -mp*qd[2]*l*s; 0 0]
    G = [0, mp * g * l * s]
    B = [1, 0]
    qdd = -H \ (C * qd + G - B * xf)
    dxf = (-xf + u[1]) / τ
    return [qd; qdd; dxf]
end

nx = 5
nu = 1

l = (x, u) -> dot(x, Q, x) + dot(u, R, u)
f = (x, u) -> cartpole(x, u, 0, 0)

#37 (generic function with 1 method)

To approximate the function cartpole using polynomials, we choose upper and lower bounds for the state and inputs, $l_x$, $u_x$, $l_u$, and $u_u$. We also choose a degree deg for the polynomial approximation of the state dynamics, deg can be a single integer, or a vector of the same length as the number of state variables. In the latter case, monomials of x[i] including up to degree deg[i] are included. Here, we choose a high degree for the angle state $\phi$ to accurately capture the nonlinearity over a reasonably large domain, but lower orders for the other state variables. We approximate the trigonometric terms over the domain $±0.4\pi$ only, since the accuracy of the approximation appear to be more important close to the unstable equilibrium than far away from it. The function poly_approx returns a function special struct that can be passed to pqr in place of the standard dynamics function.

deg = [1, 5, 2, 2, 1] # p, ϕ, v, dϕ, xf
ux = [4, 0.4pi, 5, 5, 100]
lx = -ux
uu = fill(10, nu)
lu = -uu
cartpole_pol = JuliaSimControl.poly_approx(f; deg, ux, lx, uu, lu)

JuliaSimControl.DecomposedDynamics(DynamicPolynomials.Variable{DynamicPolynomials.Commutative{DynamicPolynomials.CreationOrder}, MultivariatePolynomials.Graded{MultivariatePolynomials.LexOrder}}[x₁, x₂, x₃, x₄, x₅], DynamicPolynomials.Variable{DynamicPolynomials.Commutative{DynamicPolynomials.CreationOrder}, MultivariatePolynomials.Graded{MultivariatePolynomials.LexOrder}}[u₁], DynamicPolynomials.Polynomial{DynamicPolynomials.Commutative{DynamicPolynomials.CreationOrder}, MultivariatePolynomials.Graded{MultivariatePolynomials.LexOrder}, Float64}[0.9999999999999993x₃, 0.9999999999999998x₄, -0.0009084946088784837 + 4.839283587432905e-5u₁ + 0.9978265685632994x₅ - 0.00022415510040985388x₄ - 4.2941011459089646e-5x₃ + 1.8583498765952033x₂ - 0.0003146651378084677x₁ + 9.961692058762909e-8x₄x₅ + 6.210128569897952e-5x₄² - 9.594575631500578e-7x₃x₅ + 6.988937295674496e-5x₃x₄ + 9.389636733014823e-5x₃² - 2.6277185971344058e-6x₂x₅ + 3.12381763485e-5x₂x₄ + 3.0432747666679293e-5x₂x₃ - 0.0016884084899590414x₂² + 5.745981380614403e-6x₁x₅ - 1.7989482075493274e-5x₁x₄ + 1.802795928880613e-5x₁x₃ + 4.878538789742367e-5x₁x₂ + 2.0316274963614243e-6x₄²x₅ + 1.6803538303841024e-6x₃x₄x₅ - 2.1359380753851665e-6x₃x₄² + 2.5363135468900783e-6x₃²x₅ - 1.0735596769633884e-5x₃²x₄ + 6.01586064799075e-6x₂x₄x₅ - 0.09455461445509741x₂x₄² - 8.697802095419577e-6x₂x₃x₅ - 8.432669268548519e-5x₂x₃x₄ + 8.765537671979539e-6x₂x₃² - 0.17294872682010365x₂²x₅ + 0.0009754822836321173x₂²x₄ - 9.739245853880924e-5x₂²x₃ - 1.2915248996100066x₂³ + 9.083571070924076e-8x₁x₄x₅ - 4.252176101655606e-6x₁x₄² + 4.0410806373322915e-7x₁x₃x₅ + 1.160185440612553e-5x₁x₃x₄ - 1.7024608598103528e-5x₁x₃² + 5.044789205288785e-6x₁x₂x₅ + 4.187464216149553e-5x₁x₂x₄ - 4.903998154668515e-5x₁x₂x₃ + 0.0007944085151994596x₁x₂² + 1.1992432367982458e-7x₃x₄²x₅ - 5.00218426784619e-8x₃²x₄x₅ - 6.673992872384485e-6x₃²x₄² + 2.8584281561215126e-7x₂x₄²x₅ - 4.1094095052263765e-7x₂x₃x₄x₅ + 9.974177189625368e-7x₂x₃x₄² - 1.9388250552967405e-7x₂x₃²x₅ + 2.5425719236521426e-5x₂x₃²x₄ - 4.7204140879636106e-7x₂²x₄x₅ + 3.258525032245283e-5x₂²x₄² - 4.951832178051218e-7x₂²x₃x₅ - 0.00010491534661626476x₂²x₃x₄ + 2.0282757476133267e-5x₂²x₃² - 1.6155782910899057e-6x₂³x₅ - 0.0002727105287974124x₂³x₄ - 0.00013595734746408645x₂³x₃ + 0.0005939393217717689x₂⁴ + 7.498486569306425e-8x₁x₄²x₅ - 1.503713638739248e-8x₁x₃x₄x₅ + 1.95676316492414e-6x₁x₃x₄² + 8.890257780973952e-8x₁x₃²x₅ + 2.193933129270299e-6x₁x₃²x₄ - 4.0653849252400975e-7x₁x₂x₄x₅ + 7.449207357682387e-6x₁x₂x₄² - 6.00972913055886e-7x₁x₂x₃x₅ + 3.103471823710805e-6x₁x₂x₃x₄ + 5.989925173917507e-7x₁x₂x₃² - 5.537778437599176e-6x₁x₂²x₅ - 7.15039315765965e-5x₁x₂²x₄ - 1.3209006318282386e-5x₁x₂²x₃ - 0.00017149448687775532x₁x₂³ - 1.38839867043084e-7x₃²x₄²x₅ + 2.4890357346797263e-7x₂x₃x₄²x₅ - 3.4280913413437665e-8x₂x₃²x₄x₅ - 5.172669778027736e-6x₂x₃²x₄² + 1.6129439912798643e-8x₂²x₄²x₅ - 9.177287290933373e-7x₂²x₃x₄x₅ - 8.330645542096608e-6x₂²x₃x₄² - 1.0756662088018386e-7x₂²x₃²x₅ + 3.5008954099192167e-6x₂²x₃²x₄ - 2.3903810798152663e-6x₂³x₄x₅ + 0.019858619575859014x₂³x₄² + 4.087128199545484e-6x₂³x₃x₅ + 4.832702139519315e-5x₂³x₃x₄ + 4.2027591423404814e-5x₂³x₃² + 0.049613846786353194x₂⁴x₅ - 0.0004705531022524639x₂⁴x₄ + 0.00018727616984226742x₂⁴x₃ + 0.22565285818216893x₂⁵ - 1.3897056187825217e-8x₁x₃x₄²x₅ + 2.0442044125633283e-8x₁x₃²x₄x₅ + 2.645273462548053e-6x₁x₃²x₄² + 3.03566607678312e-8x₁x₂x₄²x₅ - 8.070110291047261e-8x₁x₂x₃x₄x₅ + 1.2746361800604256e-6x₁x₂x₃x₄² - 2.5174465857408983e-7x₁x₂x₃²x₅ + 2.043368639801578e-6x₁x₂x₃²x₄ - 1.4541739463366339e-6x₁x₂²x₄x₅ - 2.414251206274964e-5x₁x₂²x₄² - 5.66621246178855e-7x₁x₂²x₃x₅ - 1.1392041603697382e-5x₁x₂²x₃x₄ + 3.1974718533655694e-6x₁x₂²x₃² - 1.438210066892001e-6x₁x₂³x₅ - 7.615612188833713e-5x₁x₂³x₄ + 4.049496121776025e-5x₁x₂³x₃ - 0.000245300247308347x₁x₂⁴, -0.004477944967021576 + 0.00023393395585298438u₁ + 1.9894500769937098x₅ - 0.0010931254814734459x₄ - 0.00022331072366210935x₃ + 23.009492303564933x₂ - 0.0015512066852667217x₁ + 4.335204195900591e-7x₄x₅ + 0.0003024537917152836x₄² - 4.4757824842218625e-6x₃x₅ + 0.00033817450300897454x₃x₄ + 0.00045841206123458294x₃² - 1.3181876481072843e-5x₂x₅ + 0.00015315236606673x₂x₄ + 0.00014919128254206604x₂x₃ - 0.007981193093157712x₂² + 2.7862415833824885e-5x₁x₅ - 8.767514803655872e-5x₁x₄ + 8.752630931320378e-5x₁x₃ + 0.0002527535099188405x₁x₂ + 9.835548842338549e-6x₄²x₅ + 8.148902772665982e-6x₃x₄x₅ - 9.808240020708587e-6x₃x₄² + 1.2314294341561378e-5x₃²x₅ - 5.155305538572e-5x₃²x₄ + 2.9192994400810732e-5x₂x₄x₅ - 0.17150313758860036x₂x₄² - 4.204737485539659e-5x₂x₃x₅ - 0.0004010755792809639x₂x₃x₄ + 3.958568149704991e-5x₂x₃² - 1.2684585908270611x₂²x₅ + 0.004738893157164347x₂²x₄ - 0.0004759262624030813x₂²x₃ - 6.553662325003106x₂³ + 4.57303033229648e-7x₁x₄x₅ - 1.9115139374629418e-5x₁x₄² + 1.940668705511408e-6x₁x₃x₅ + 5.6545327013919695e-5x₁x₃x₄ - 7.990399097831765e-5x₁x₃² + 2.417798158649754e-5x₁x₂x₅ + 0.0001962951270332522x₁x₂x₄ - 0.00023343031072625925x₁x₂x₃ + 0.003875452968464865x₁x₂² + 5.717340030605873e-7x₃x₄²x₅ - 2.3025934217722142e-7x₃²x₄x₅ - 3.230983954007386e-5x₃²x₄² + 1.3832943652505373e-6x₂x₄²x₅ - 2.025727652097519e-6x₂x₃x₄x₅ + 4.541770701119136e-6x₂x₃x₄² - 9.547601591014165e-7x₂x₃²x₅ + 0.0001236599259858169x₂x₃²x₄ - 2.3215803064079652e-6x₂²x₄x₅ + 0.00015499630499470112x₂²x₄² - 2.49164496160694e-6x₂²x₃x₅ - 0.0005058715001492719x₂²x₃x₄ + 9.383078110748242e-5x₂²x₃² - 7.246313520524696e-6x₂³x₅ - 0.00133275189400497x₂³x₄ - 0.0006518875927368651x₂³x₃ + 0.002798906144367635x₂⁴ + 3.552810579732247e-7x₁x₄²x₅ - 7.303415745916827e-8x₁x₃x₄x₅ + 9.51737233229998e-6x₁x₃x₄² + 4.4520624939467246e-7x₁x₃²x₅ + 1.0520178689601379e-5x₁x₃²x₄ - 1.981233013213566e-6x₁x₂x₄x₅ + 3.551866470434133e-5x₁x₂x₄² - 2.9142027084765052e-6x₁x₂x₃x₅ + 1.5069066710280108e-5x₁x₂x₃x₄ + 2.781207562134712e-6x₁x₂x₃² - 2.692782944039282e-5x₁x₂²x₅ - 0.00034359826387890573x₁x₂²x₄ - 6.456675291937563e-5x₁x₂²x₃ - 0.0008314021604626872x₁x₂³ - 6.744731400773186e-7x₃²x₄²x₅ + 1.1943474180458471e-6x₂x₃x₄²x₅ - 1.6776859613081187e-7x₂x₃²x₄x₅ - 2.5114845775344485e-5x₂x₃²x₄² + 1.1509464264397793e-7x₂²x₄²x₅ - 4.458143733650254e-6x₂²x₃x₄x₅ - 4.036127278570323e-5x₂²x₃x₄² - 5.597684439709186e-7x₂²x₃²x₅ + 1.7106828941919926e-5x₂²x₃²x₄ - 1.1558194375221158e-5x₂³x₄x₅ + 0.08653711259464145x₂³x₄² + 1.9817236801849078e-5x₂³x₃x₅ + 0.00022873068017670173x₂³x₃x₄ + 0.00020584662140133694x₂³x₃² + 0.2184846392760087x₂⁴x₅ - 0.0022866329753629144x₂⁴x₄ + 0.0009180394577126624x₂⁴x₃ + 0.9584905146884739x₂⁵ - 6.627440456150296e-8x₁x₃x₄²x₅ + 9.697589194245069e-8x₁x₃²x₄x₅ + 1.2698819439769824e-5x₁x₃²x₄² + 1.50637217520542e-7x₁x₂x₄²x₅ - 3.940249699789535e-7x₁x₂x₃x₄x₅ + 5.9771908203102405e-6x₁x₂x₃x₄² - 1.2017739529482054e-6x₁x₂x₃²x₅ + 1.0026090511064101e-5x₁x₂x₃²x₄ - 7.034750061811677e-6x₁x₂²x₄x₅ - 0.00011655359966245895x₁x₂²x₄² - 2.726993709844562e-6x₁x₂²x₃x₅ - 5.5906182758826346e-5x₁x₂²x₃x₄ + 1.3209366715453812e-5x₁x₂²x₃² - 6.9818661767248045e-6x₁x₂³x₅ - 0.00036754332031611875x₁x₂³x₄ + 0.00019543396926045774x₁x₂³x₃ - 0.0011912327271436883x₁x₂⁴, -3.957929986995163e-13 + 1000.0000000000001u₁ - 1000.0000000000001x₅ - 5.851616716436312e-13x₂ + 2.402939591138593e-13x₁ + 4.1745983468770843e-13x₂² - 2.457387328681391e-13x₁x₂ - 2.3515631638227166e-13x₂²x₃ - 1.0619212039026665e-12x₂³ + 1.3037375075844635e-12x₂⁵])

Next, we define the quadratic cost matrices $Q, R$ and solve the PQR problem. We then generate a controller function u = K(x) using build_K_function and simulate the closed-loop system using solve from OrdinaryDiffEq. To compute the cost achieved by the controller, we add the cumulative integral of the cost as an additional state to the closed-loop dynamics and let the ODE integrator compute the integral for us. We also define a function wrap_angle that makes sure that the angle stays within the interval [-π, π].

R = diagm([0.2])
Q = diagm(Float64[2,1,0.1,1,1e-6])
pqrsol = pqr(cartpole_pol, Q, R, maximum(deg))

wrap_angle(x::Number) = mod(x+pi, 2pi)-pi
wrap_angle(x) = [x[1], wrap_angle(x[2]), x[3], x[4], x[5]]

function f_cl_cartpole(xc, (f,l,K), t)
    x  = xc[1:end-1] # Last value is integral of cost
    Δx = wrap_angle(x)
    th = 100 # Input saturation
    Δu = clamp.(K(Δx), -th, th)
    dx = f(x, Δu)
    dc = l(Δx, Δu)
    [dx; dc]         # Return state and cost derivatives
end
x0 = [0, pi-deg2rad(15), 0, 0, 0, 0] # Initial state, start 15° from the downward equilibrium. This avoids the the initial chattering close to the downward equilibrium.

tspan = (0.0, 9.0)
prob = ODEProblem(f_cl_cartpole, x0, tspan, (f, l, x->x))

fig = plot(layout=nx+1)
for d = [3, 5]
    K, _ = build_K_function(pqrsol, d, simplify=false)
    sol = solve(prob, Tsit5(), reltol=1e-5, abstol=1e-5, p=(f, l, K))
    plot!(sol, layout=nx+1, title=["x" "ϕ" "dx" "dϕ" "u" "cost"], lab="Deg $d")
end
fig

Controllers of degrees 3 and 5 manage to swing the pendulum up (upward equilibria are located at $\phi = 0 ± 2π$), but controllers of lower order do not. Note, that the PQR controller does not handle input saturation explicitly, and as a consequence, does not perform nearly as well swinging the pendulum up as if trajectory optimization had been used. An example of pendulum swing-up with these dynamics is provided in Pendulum swing-up.

Index

• JuliaSimControl.build_K_function
• JuliaSimControl.poly_approx
• JuliaSimControl.poly_approx
• JuliaSimControl.pqr
• JuliaSimControl.predicted_cost

K_oop, K_ip = build_K_function(sol::PQRSolution)
K_oop, K_ip = build_K_function(Ks)

poly_approx(f, deg, l, u; N = 1000)

using DynamicPolynomials
const sinpol = tuple(JuliaSimControl.poly_approx(sin, 5, 0, 2pi)...)

function sin5(x)
    x = mod(x, 2pi) # Extend the domain to all real numbers
    evalpoly(x, sinpol)
end

sin5(x::Union{PolyVar, Polynomial}) = evalpoly(x, sinpol) # When evaluated using a polynomial variable, we do not include the call to mod

function cos5(x)
    sin5(x+pi/2)
end

poly_approx(f; deg = 3, lx::AbstractVector, ux::AbstractVector, lu, uu, N = 100000, verbose = true)

sol = pqr(f, Q, R, deg = 3; verbose)
sol = pqr(A, B, As, Q, R, deg = 3; verbose)

x' = f(x, u) = Ax + Bu + N[1]*kron(x, x) + N[2]*kron(x, x, x) + ...

J = x'Qx + u'Ru

using JuliaSimControl, Test

nx = 3 # Number of state variables
nu = 1 # Number of control inputs
A = [
    -10 10 0
    28 -1 0
    0 0 -8/3
]
B = [1; 0; 0;;]
A2 = zeros(nx, nx^2)
A2[2,3] = A2[2,7] = -1/2
A2[3,2] = A2[3,4] = 1/2

Q = diagm(ones(3)) # State cost matrix
R = diagm(ones(1)) # Control input cost matrix

f = (x, u) -> A * x + B * u + A2 * kron(x, x) # Dynamics for simulation
l = (x, u) -> dot(x, Q, x) + dot(u, R, u)     # Cost function for simulation

function closed_loop(xc, (f,l,K), t)          # Closed-loop dynamics for simulation
    x = xc[1:end-1] # Last value is integral of cost
    u = K(x)
    dx = f(x, u)
    dc = l(x, u)
    [dx; dc]        # Return state and cost derivatives
end

x0 = Float64[10, 10, 10, 0] # Initial state, last value is integral of cost
tspan = (0.0, 50.0)
prob = ODEProblem(closed_loop, x0, tspan, (f, l, x->x))

# Design controller of degree 2
pqrsol = pqr(A, B, [A2], Q, R, 2) # It's possible to pass in f instead of A, B, A2
K, _ = build_K_function(pqrsol)
c = predicted_cost(pqrsol, x0[1:end-1])
@test c ≈ 7062.15 atol=0.02 # Compare to paper
sol = solve(prob, Rodas5P(), p=(f,l,K), reltol=1e-8, abstol=1e-8);
c = sol.u[end][end]
@test c ≈ 6911.03 rtol=1e-2 # Compare to paper

# Design controller of degree 4
pqrsol = pqr(A, B, [A2], Q, R, 4)
K, _ = build_K_function(pqrsol)
c = predicted_cost(pqrsol, x0[1:end-1])
@test c ≈ 6924.27 atol=0.02
sol = solve(prob, Rodas5P(), p=(f,l,K), reltol=1e-8, abstol=1e-8);
c = sol.u[end][end]
@test c ≈ 6906.21 rtol=1e-2

predicted_cost(sol::PQRSolution, x)