|
| 1 | +# Manual : Nonlinear Design |
| 2 | + |
| 3 | +## Nonlinear Model |
| 4 | + |
| 5 | +In this example, the goal is to control the angular position ``θ`` of a pendulum |
| 6 | +attached to a motor. If the manipulated input is the motor torque ``τ``, the vectors |
| 7 | +are: |
| 8 | + |
| 9 | +```math |
| 10 | +\begin{aligned} |
| 11 | + \mathbf{u} &= \begin{bmatrix} τ \end{bmatrix} \\ |
| 12 | + \mathbf{y} &= \begin{bmatrix} θ \end{bmatrix} |
| 13 | +\end{aligned} |
| 14 | +``` |
| 15 | + |
| 16 | +The plant model is nonlinear: |
| 17 | + |
| 18 | +```math |
| 19 | +\begin{aligned} |
| 20 | + \dot{θ}(t) &= ω(t) \\ |
| 21 | + \dot{ω}(t) &= -\frac{g}{L}\sin\big( θ(t) \big) - \frac{K}{m} ω(t) + \frac{1}{m L^2} τ(t) |
| 22 | +\end{aligned} |
| 23 | +``` |
| 24 | + |
| 25 | +in which ``g`` is the gravitational acceleration, ``L``, the pendulum length, ``K``, the |
| 26 | +friction coefficient at the pivot point, and ``m``, the mass attached at the end of the |
| 27 | +pendulum. Here, the explicit Euler method discretizes the system to construct a |
| 28 | +[`NonLinModel`](@ref): |
| 29 | + |
| 30 | +```@example 1 |
| 31 | +using ModelPredictiveControl |
| 32 | +function pendulum(par, x, u) |
| 33 | + g, L, K, m = par # [m/s], [m], [kg/s], [kg] |
| 34 | + θ, ω = x[1], x[2] # [rad], [rad/s] |
| 35 | + τ = u[1] # [N m] |
| 36 | + dθ = ω |
| 37 | + dω = -g/L*sin(θ) - K/m*ω + τ/m/L^2 |
| 38 | + return [dθ, dω] |
| 39 | +end |
| 40 | +Ts = 0.1 # [s] |
| 41 | +par = (9.8, 0.4, 1.2, 0.3) |
| 42 | +f(x, u, _ ) = x + Ts*pendulum(par, x, u) # Euler method |
| 43 | +h(x, _ ) = [180/π*x[1]] # [°] |
| 44 | +nu, nx, ny = 1, 2, 1 |
| 45 | +model = NonLinModel(f, h, Ts, nu, nx, ny) |
| 46 | +``` |
| 47 | + |
| 48 | +The output function ``\mathbf{h}`` converts the angular position ``θ`` to degrees. It |
| 49 | +is good practice to first simulate `model` using [`sim!`](@ref) as a quick sanity check: |
| 50 | + |
| 51 | +```@example 1 |
| 52 | +using Plots |
| 53 | +u = [0.5] # τ = 0.5 N m |
| 54 | +plot(sim!(model, 60, u), plotu=false) |
| 55 | +``` |
| 56 | + |
| 57 | +## Nonlinear Predictive Controller |
| 58 | + |
| 59 | +An [`UnscentedKalmanFilter`](@ref) estimates the plant state : |
| 60 | + |
| 61 | +```@example 1 |
| 62 | +estim = UnscentedKalmanFilter(model, σQ=[0.5, 2.5], σQ_int=[0.5]) |
| 63 | +``` |
| 64 | + |
| 65 | +The standard deviation of the angular velocity ``ω`` is higher here (`σQ` second value) |
| 66 | +since ``\dot{ω}(t)`` equation includes an uncertain parameter: the friction coefficient |
| 67 | +``K``. The estimator tuning is tested on a plant simulated with a different ``K``: |
| 68 | + |
| 69 | +```@example 1 |
| 70 | +par_plant = (par[1], par[2], par[3] + 0.25, par[4]) |
| 71 | +f_plant(x, u, _) = x + Ts*pendulum(par_plant, x, u) |
| 72 | +plant = NonLinModel(f_plant, h, Ts, nu, nx, ny) |
| 73 | +res = sim!(estim, 30, [0.5], plant=plant, y_noise=[0.5]) # τ = 0.5 N m |
| 74 | +plot(res, plotu=false, plotx=true, plotx̂=true) |
| 75 | +``` |
| 76 | + |
| 77 | +The Kalman filter performance seems sufficient for control. As the motor torque is limited |
| 78 | +to -1.5 to 1.5 N m, we incorporate the input constraints in a [`NonLinMPC`](@ref): |
| 79 | + |
| 80 | +```@example 1 |
| 81 | +mpc = NonLinMPC(estim, Hp=20, Hc=2, Mwt=[0.1], Nwt=[1.0], Cwt=Inf) |
| 82 | +mpc = setconstraint!(mpc, umin=[-1.5], umax=[+1.5]) |
| 83 | +``` |
| 84 | + |
| 85 | +We test `mpc` performance on `plant` by imposing an angular setpoint of 180° (inverted |
| 86 | +position): |
| 87 | + |
| 88 | +```@example 1 |
| 89 | +res = sim!(mpc, 30, [180.0], x̂0=zeros(mpc.estim.nx̂), plant=plant, x0=zeros(plant.nx)) |
| 90 | +plot(res, plotŷ=true) |
| 91 | +``` |
| 92 | + |
| 93 | +The controller seems robust enough to variations on ``K`` coefficient. |
0 commit comments