5  Block on a slope

Position Control with friction. Using Pole Placement + PD.

Author

Iacopo Moles

5.1 Response Analysis

Code 
using CCS
using ControlSystems, Plots, LinearAlgebra, RobustAndOptimalControl
CCS.setupEnv()

contSys = CCS.blockModel.csys(;g = 0, α = 0 , μ = 1, τ =20)
plot!(bodeplot(contSys[1,1]),pzmap(contSys))
10 0 10 3 10 −5 10 0 Magnitude u → S 10 0 10 3 Frequency [rad/s] −250 −200 −150 −100 Phase (deg) −20 −15 −10 −5 0 0.00 0.25 0.50 0.75 1.00 Pole-zero map

Starting Bode Plot and PZ Map

It has the shape we expect from a motor + friction. Slow pole for the mass + friction and a faster pole for the current & inductance.

Numerically they are:

Code 
display(eigvals(contSys.A))
3-element Vector{Float64}:
 -20.0
  -1.0
   0.0

We see that we start with all the poles in the left-half plane, which is good.

5.2 Pole Placement

We can design a controller with pole placement.

For some reason pole placement doesn’t work for the observer, I use a Kalman Filter with random fast values.

Code 
observability(contSys.A,contSys.C).isobservable || error("System is not observable")
controllability(contSys.A,contSys.B).iscontrollable || error("System is not controllable")

ε = 0.01;
pp = 15.0;
poles_cont = - [pp + ε, pp - ε, pp];
L = real(place(contSys, poles_cont, :c));


poles_obs = poles_cont * 10.0;
K = place(contSys, poles_obs, :o)
obs_controller = observer_controller(contSys, L, K; direct=false);
fsf_controller = named_ss(obs_controller, u = [:ref_S, :ref_V], y = [:u])
NamedStateSpace{Continuous, Float64}
A = 
  -150.0                         8.033684828490095e-12    0.0
     1.0915557686859107e-5    -279.9999999999851          1.0
 -3374.9970813429577        -17530.98989999806          -44.0
B = 
 150.0                        0.9999999999919663
  -1.0915557686859107e-5    278.9999999999851
  -0.0014186570429435138  16899.989999998063
C = 
 168.74992500000002  31.549995000000003  1.2000000000000002
D = 
 0.0  0.0

Continuous-time state-space model
With state  names: x1 x2 x3
     input  names: ref_S ref_V
     output names: u

We can check the effect of the new controller on the loop

Code 
closedLoop = feedback( contSys * fsf_controller);
print(poles(closedLoop));
setPlotScale("dB")
plot!(bodeplot(closedLoop[1,1], 0.1:40), pzmap(closedLoop))
ComplexF64[-14.990000366343788 + 0.0im, -14.999999266673722 + 0.0im, -15.010000366982666 + 0.0im, -149.99999999999986 + 0.0im, -150.10000000002432 + 0.0im, -149.8999999999607 + 0.0im]
10 −1 10 0 10 1 −25 −20 −15 −10 −5 0 Magnitude (dB) ref_S → S 10 −1 10 0 10 1 Frequency [rad/s] −200 −150 −100 −50 0 Phase (deg) −150 −100 −50 0 0.00 0.25 0.50 0.75 1.00 Pole-zero map

We can compare this to the open-loop response in @start-bode. We can see that we achieve unitary gain throughout the whole low-frequency range.

We can convert the pole placement controller into the standard PD gain form.

Code 
using DiscretePIDs
Ts = 0.02 # sampling time
Tf = 2.5; #final simulation time

K = L[1];
Ti = 0;
Td = L[2] / L[1];

pid = DiscretePID(; K, Ts, Ti, Td);

5.3 Simulation

We can simulate this with a motor that only outputs the position:

Code 
sysreal = ss(contSys.A, contSys.B, [1 0 0], 0.0)
ctrl = function (x, t)
    y = (sysreal.C*x)[] # measurement
    d = 0 * [1.0]        # disturbance
    r = 2.0 * (t >= 1) # reference
    # u = pid(r, y) # control signal
    # u + d # Plant input is control signal + disturbance
    # u =1
    e = x - [r; 0.0; 0.0]
    e[3] = 0.0 # torque not observable, just ignore it in the final feedback
    u = -L * e + d
    u = [maximum([-20.0 minimum([20.0 u])])]
end
t = 0:Ts:Tf

res = lsim(sysreal, ctrl, t)

display(plot(res, 
    plotu=true, 
    plotx=true, 
    ploty=false
    ))
ylabel!("u", sp=1);
ylabel!("x", sp=2);
ylabel!("v", sp=3);
ylabel!("T", sp=4);
0.0 0.5 1.0 1.5 2.0 2.5 Time (s) −20 −10 0 10 20 u 0.0 0.5 1.0 1.5 2.0 2.5 Time (s) 0.0 0.5 1.0 1.5 2.0 x(1) 0.0 0.5 1.0 1.5 2.0 2.5 Time (s) 0 1 2 3 4 5 x(2) 0.0 0.5 1.0 1.5 2.0 2.5 Time (s) −20 −10 0 10 20 x(3)

For more stats:

Code 
si = stepinfo(res);
plot(si);title!("Step Response")
0.0 0.5 1.0 1.5 2.0 2.5 0.0 0.5 1.0 1.5 2.0 Step Response Final value: 2.000 Rise time: 0.360 Rise time threshold: 10.0%-90.0% Peak: 2.032 Overshoot: 1.6% Settling time: 1.620 Settling threshold: 2.0% Undershoot: 0.0%

We can also simulate it in a SIMULINK-like environment:

Code 
using FMI, DifferentialEquations
fmuPath = abspath(joinpath(@__DIR__,
  "..","..","..",
  "modelica",
  "ControlChallenges",
  "ControlChallenges.BlockOnSlope_Challenges.Examples.WithFriction.fmu"))
fmu = loadFMU(fmuPath);
simData = simulateME(
    fmu,
    (0.0, 5.0);
    recordValues=["blockOnSlope.x", 
    "blockOnSlope.xd", 
    "blockOnSlope.usat"],
    showProgress=false);
unloadFMU(fmu);
plot(simData, states=false, timeEvents=false)
0 1 2 3 4 5 t [s] −20 −10 0 10 20 blockOnSlope.x (1) blockOnSlope.xd (2) blockOnSlope.usat (7) State event(s)

There is a slight difference between the lsim simulation and the FMU simulation. I need to recheck some stuff.

Open Simulation