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 + \mathrm{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) \\ \mathrm{motor}_{dynamics_{+}input_{+}u}\left( t \right) &= u\left( t \right)_{1} \\ \mathrm{motor}_{dynamics_{+}u}\left( t \right) &= \mathrm{motor}_{dynamics_{+}input_{+}u}\left( t \right) \\ \mathrm{motor}_{dynamics_{+}y}\left( t \right) &= \mathrm{motor}_{dynamics_{+}output_{+}u}\left( t \right) \\ \mathrm{motor}_{dynamics_{+}y}\left( t \right) &= \mathrm{motor}_{dynamics_{+}x}\left( t \right) \\ \frac{\mathrm{d} \mathrm{motor}_{dynamics_{+}x}\left( t \right)}{\mathrm{d}t} &= \frac{ - \mathrm{motor}_{dynamics_{+}x}\left( t \right) + motor_{dynamics_{+}k} \mathrm{motor}_{dynamics_{+}u}\left( t \right)}{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
- Define the system dynamics in expanded form,
polyquad
, and use this function for thepqr
design. - Define the system dynamics standard form,
quad
, and use this function for simulation. - Define the closed-loop system dynamics,
f_cl_polyquad
, where we compute the extended statexe = [x; sin(x[4]); cos(x[4])]
that serves as the input to the controller. - 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}[x₃, 1.0000000000000002x₄, 0.001483681197642767 - 9.61653200222117e-6u₁ + 0.997883436672055x₅ - 0.00023132692948826117x₄ + 0.00014883594092297397x₃ + 1.8567316895377013x₂ - 0.0004043007990020922x₁ - 2.8606327818830378e-6x₄x₅ - 0.00012007723417456682x₄² + 2.7429507943784842e-6x₃x₅ + 1.5707636715556968e-5x₃x₄ - 2.6264752804583595e-5x₃² + 1.6239460335528758e-5x₂x₅ + 0.00027235820859587787x₂x₄ + 7.968188373766058e-5x₂x₃ + 0.0008907506610318199x₂² - 6.073604836220835e-6x₁x₅ + 0.00010105642405060216x₁x₄ - 2.5824045955577097e-5x₁x₃ + 0.00019180465379332415x₁x₂ - 2.141314921721657e-7x₄²x₅ - 4.881128872188806e-7x₃x₄x₅ + 1.422483723380675e-6x₃x₄² - 6.469914962637903e-7x₃²x₅ + 1.1424332120932099e-6x₃²x₄ + 2.088338058550952e-6x₂x₄x₅ - 0.0943431163291031x₂x₄² + 8.352193678838926e-7x₂x₃x₅ + 0.00014510276167899492x₂x₃x₄ + 6.452076979619222e-5x₂x₃² - 0.17296289427632155x₂²x₅ + 0.00044926061522125794x₂²x₄ - 0.00023198918278603603x₂²x₃ - 1.2883230165529689x₂³ + 1.1032532642279878e-7x₁x₄x₅ + 9.317907214583009e-6x₁x₄² - 1.6952225675635464e-6x₁x₃x₅ + 2.0295843385127047e-5x₁x₃x₄ - 1.2908832866930397e-5x₁x₃² - 4.734068219177696e-6x₁x₂x₅ - 0.00030581648835944684x₁x₂x₄ - 0.00012066806914292465x₁x₂x₃ + 0.0010956013046454457x₁x₂² - 2.0225325974531341e-7x₃x₄²x₅ + 1.0904308459758311e-7x₃²x₄x₅ + 8.896739051475274e-7x₃²x₄² - 1.287745400953841e-6x₂x₄²x₅ + 2.2972451236159165e-7x₂x₃x₄x₅ - 4.476479073378453e-6x₂x₃x₄² - 2.6949949008897486e-7x₂x₃²x₅ - 7.3060397071666065e-6x₂x₃²x₄ - 9.697047805986685e-7x₂²x₄x₅ + 9.329317594725244e-5x₂²x₄² + 1.9630146808111742e-6x₂²x₃x₅ - 5.030027793663986e-6x₂²x₃x₄ + 3.535765369190597e-5x₂²x₃² + 5.183669584795136e-6x₂³x₅ - 0.00017896649499069198x₂³x₄ - 0.00014306808912827634x₂³x₃ - 0.0019338917868919013x₂⁴ + 4.2433651253096064e-8x₁x₄²x₅ + 6.878182701779957e-8x₁x₃x₄x₅ + 4.950272601511784e-6x₁x₃x₄² + 2.2717733144795868e-8x₁x₃²x₅ - 2.4031803095354597e-6x₁x₃²x₄ - 6.681853901759267e-9x₁x₂x₄x₅ + 1.9271020076054608e-5x₁x₂x₄² + 2.1861423721610848e-7x₁x₂x₃x₅ + 1.3009509362074702e-5x₁x₂x₃x₄ + 3.687181908472671e-6x₁x₂x₃² + 2.2964887257585646e-6x₁x₂²x₅ - 9.26554614909165e-5x₁x₂²x₄ - 3.922558274366399e-5x₁x₂²x₃ - 0.0004322837167187177x₁x₂³ + 1.409135342604412e-8x₃²x₄²x₅ - 2.1938263366211442e-7x₂x₃x₄²x₅ + 3.024224019831332e-8x₂x₃²x₄x₅ + 2.8730486663584105e-6x₂x₃²x₄² + 2.3737544358198674e-7x₂²x₄²x₅ + 1.472001541410328e-7x₂²x₃x₄x₅ - 6.425907173649548e-6x₂²x₃x₄² + 9.711706618883367e-7x₂²x₃²x₅ - 5.895835211790468e-6x₂²x₃²x₄ + 4.5329446884659294e-8x₂³x₄x₅ + 0.019549607281826434x₂³x₄² + 2.836807107151542e-8x₂³x₃x₅ - 9.185649448861379e-5x₂³x₃x₄ - 5.844535016951293e-5x₂³x₃² + 0.049605060134776974x₂⁴x₅ - 0.00021564664925006696x₂⁴x₄ + 0.00015930344132801682x₂⁴x₃ + 0.22524821263557973x₂⁵ + 6.632605067034761e-8x₁x₃x₄²x₅ - 4.021428164633532e-8x₁x₃²x₄x₅ + 7.246860829556171e-7x₁x₃²x₄² + 3.44475117841423e-7x₁x₂x₄²x₅ + 1.4144260094408075e-7x₁x₂x₃x₄x₅ - 1.4369016448227118e-7x₁x₂x₃x₄² + 1.7677957880982195e-7x₁x₂x₃²x₅ + 6.570021493473551e-7x₁x₂x₃²x₄ + 7.594086661788512e-8x₁x₂²x₄x₅ - 1.2739290860311543e-5x₁x₂²x₄² + 4.016095222479186e-8x₁x₂²x₃x₅ - 2.5297149077113384e-5x₁x₂²x₃x₄ + 2.369611241661849e-6x₁x₂²x₃² + 1.0567402093697786e-7x₁x₂³x₅ + 0.00022380808846244829x₁x₂³x₄ + 8.855997342938541e-5x₁x₂³x₃ - 0.0003737337253848347x₁x₂⁴, 0.007184616738535827 - 4.441945266039017e-5u₁ + 1.989724702442459x₅ - 0.001125564325486609x₄ + 0.0006971996856166982x₃ + 23.00176653382646x₂ - 0.001988598450566185x₁ - 1.3712784692658197e-5x₄x₅ - 0.000584118741498888x₄² + 1.3362639098173907e-5x₃x₅ + 7.650270614619243e-5x₃x₄ - 0.00012577908349228222x₃² + 7.77153242760647e-5x₂x₅ + 0.0013009515772036922x₂x₄ + 0.0003682397153496485x₂x₃ + 0.004320844894271048x₂² - 2.977046370319064e-5x₁x₅ + 0.0004913470986161834x₁x₄ - 0.00012553915087927346x₁x₃ + 0.0009570309496123927x₁x₂ - 9.880134321158908e-7x₄²x₅ - 2.3482065109383316e-6x₃x₄x₅ + 7.912814840968947e-6x₃x₄² - 3.098733217045161e-6x₃²x₅ + 5.3848093137280315e-6x₃²x₄ + 1.0385591712060497e-5x₂x₄x₅ - 0.1704806068442066x₂x₄² + 3.864455351107465e-6x₂x₃x₅ + 0.0007105070420280697x₂x₃x₄ + 0.00030764845345476304x₂x₃² - 1.2685243453644324x₂²x₅ + 0.002172389270270997x₂²x₄ - 0.0011029805278968492x₂²x₃ - 6.538387008433274x₂³ + 4.888801163867726e-7x₁x₄x₅ + 4.60472184341111e-5x₁x₄² - 8.245618534915715e-6x₁x₃x₅ + 9.741291379575626e-5x₁x₃x₄ - 6.257450699026643e-5x₁x₃² - 2.293743779290343e-5x₁x₂x₅ - 0.0014837156920828928x₁x₂x₄ - 0.0005797561679130521x₁x₂x₃ + 0.0053937331635711405x₁x₂² - 9.865836379127846e-7x₃x₄²x₅ + 5.221624489709282e-7x₃²x₄x₅ + 4.492454502534381e-6x₃²x₄² - 6.224865602981665e-6x₂x₄²x₅ + 1.1089080184653067e-6x₂x₃x₄x₅ - 2.0857963414173495e-5x₂x₃x₄² - 1.3055100707934752e-6x₂x₃²x₅ - 3.4731072871990346e-5x₂x₃²x₄ - 4.7689704038708124e-6x₂²x₄x₅ + 0.0004527328196064193x₂²x₄² + 9.473095380105817e-6x₂²x₃x₅ - 2.524413042672586e-5x₂²x₃x₄ + 0.00016526008605786765x₂²x₃² + 2.5714641693115485e-5x₂³x₅ - 0.0008632622648173896x₂³x₄ - 0.000688998988470619x₂³x₃ - 0.009355745097270204x₂⁴ + 2.0927143947102893e-7x₁x₄²x₅ + 3.386028912025007e-7x₁x₃x₄x₅ + 2.4118376358051334e-5x₁x₃x₄² + 1.2051213308398395e-7x₁x₃²x₅ - 1.1687432678352257e-5x₁x₃²x₄ - 6.402360007095026e-8x₁x₂x₄x₅ + 9.203713041325767e-5x₁x₂x₄² + 1.0755702885930456e-6x₁x₂x₃x₅ + 6.240457615799286e-5x₁x₂x₃x₄ + 1.8152965138210903e-5x₁x₂x₃² + 1.135809927098332e-5x₁x₂²x₅ - 0.00044871812581736106x₁x₂²x₄ - 0.00019179405866589212x₁x₂²x₃ - 0.002113982131389522x₁x₂³ + 6.753818495547942e-8x₃²x₄²x₅ - 1.0594104190466044e-6x₂x₃x₄²x₅ + 1.4340880753912075e-7x₂x₃²x₄x₅ + 1.4087565649769321e-5x₂x₃²x₄² + 1.1075114623727782e-6x₂²x₄²x₅ + 7.178926701433406e-7x₂²x₃x₄x₅ - 3.1816710267318554e-5x₂²x₃x₄² + 4.701891875315121e-6x₂²x₃²x₅ - 2.799246202393772e-5x₂²x₃²x₄ + 9.686064437780015e-10x₂³x₄x₅ + 0.08504272020223103x₂³x₄² + 2.4969400958206155e-7x₂³x₃x₅ - 0.0004523079584073135x₂³x₃x₄ - 0.00028062030197686586x₂³x₃² + 0.21843996560091986x₂⁴x₅ - 0.0010367702269660248x₂⁴x₄ + 0.0007636518076380867x₂⁴x₃ + 0.9566266667251904x₂⁵ + 3.231635027287029e-7x₁x₃x₄²x₅ - 1.9362810387045136e-7x₁x₃²x₄x₅ + 3.526644040881642e-6x₁x₃²x₄² + 1.6816091003870455e-6x₁x₂x₄²x₅ + 6.730244517894908e-7x₁x₂x₃x₄x₅ - 1.015317733283186e-6x₁x₂x₃x₄² + 8.643292139665062e-7x₁x₂x₃²x₅ + 2.90838529923075e-6x₁x₂x₃²x₄ + 3.9081208102165564e-7x₁x₂²x₄x₅ - 6.261153489233884e-5x₁x₂²x₄² + 2.005986434652891e-7x₁x₂²x₃x₅ - 0.00012183105601779253x₁x₂²x₃x₄ + 1.0739046950920108e-5x₁x₂²x₃² + 3.0102850559002533e-7x₁x₂³x₅ + 0.0010892982784630057x₁x₂³x₄ + 0.00042855336588448864x₁x₂³x₃ - 0.0018445224033091798x₁x₂⁴, 1000.0000000000005u₁ - 999.9999999999992x₅ + 1.2654811276093324e-12x₂ + 3.635550000170153e-13x₂² + 3.8005158935716935e-13x₁x₂ - 5.369416496929236e-13x₂²x₄ + 3.443471905846309e-13x₂²x₃ - 1.0360882475236931e-12x₂³ - 2.7624926706324806e-13x₁x₂³ - 3.4673676014278587e-13x₂⁴x₅ + 3.370108217667148e-13x₂⁴x₄ - 3.2865924057666754e-13x₂⁴x₃])
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
JuliaSimControl.build_K_function
— FunctionK_oop, K_ip = build_K_function(sol::PQRSolution)
K_oop, K_ip = build_K_function(Ks)
Build a function for the controller K(x)
obtained from pqr
for the given Ks
or PQRSolution
.
Optionally, the degree of the controller can be specified as the second argument, provided that the chosen degree is no higher than the degree for which the solution sol
was obtained from pqr
.
Keyword arguments are forwarded to Symbolics.build_function
. Enabling common-subexpression elimination through cse=true
might be useful for large controllers.
JuliaSimControl.poly_approx
— Methodpoly_approx(f, deg, l, u; N = 1000)
Approximate scalar function f
in the least-squares sense on the interval [l, u]
using a polynomial of degree deg
.
Returns a vector of polynomial coefficients of length deg + 1
starting at order 0.
Arguments:
f
: Scalar functiondeg
: Polynomial degreel
: Lower bound of the domainu
: Upper bound of the domainN
: Number of discretization points
Example
The following creates 5:th order approximations of sin and cos on the interval [0, 2pi]
, and extends the valid domain to all real numbers using periodicity (mod
).
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
JuliaSimControl.poly_approx
— Methodpoly_approx(f; deg = 3, lx::AbstractVector, ux::AbstractVector, lu, uu, N = 100000, verbose = true)
Polynomial approximation of dynamics ẋ = f(x, u)
in the least-squares sense.
Arguments:
deg
: An integer or a vector of the same length asx
specifying the maximum degree of each state variable. Defaults to 3.lx
: Lower bounds of the state domain over which to approximate the dynamics.ux
: Upper bounds of the state domain over which to approximate the dynamics.lu
: Lower bounds of the input domain over which to approximate the dynamics.uu
: Upper bounds of the input domain over which to approximate the dynamics.N
: Number of samples to use for the approximation.verbose
: Print maximum error after fitting?
JuliaSimControl.pqr
— Functionsol = pqr(f, Q, R, deg = 3; verbose)
sol = pqr(A, B, As, Q, R, deg = 3; verbose)
Polynomial-quadratic control design for systems of the form
x' = f(x, u) = Ax + Bu + N[1]*kron(x, x) + N[2]*kron(x, x, x) + ...
where N[i]
is a matrix. The cost function is
J = x'Qx + u'Ru
Ref: "On Approximating Polynomial-Quadratic Regulator Problems", Jeff Borggaard, Lizette Zietsman
Arguments:
f
: Functionf(x, u)
for the dynamics. IF a function is supplied, it will be decomposed into the required Kronecker form automatically usingdecompose_poly
.A
: Linear dynamics matrixB
: Control input matrixAs
: Array of matrices for nonlinearitiesQ
: State cost matrixR
: Control input cost matrixdeg
: Desired degree of the controllerverbose
: Print progress. Defaults to true for systems with more than 10 states.
Returns
A PQRSolution
object with fields
Ks
: Array of matricesK[i]
for the controllerVs
: Array of matricesV[i]
for the Lyapunov function
This solution object supports the following functions:
Example
Controlled Lorenz Equations, example 5.1 from the paper referenced above
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
JuliaSimControl.predicted_cost
— Methodpredicted_cost(sol::PQRSolution, x)
Compute the infinite-horizon cost from state x
predicted using the approximate value function (an approximate Lyapunov function) V(x)
obtained from pqr
.
- 1"On Approximating Polynomial-Quadratic Regulator Problems", Jeff Borggaard, Lizette Zietsman