Chapter 7

In [1]:
using ControlSystems
using Plots; gr()
using LinearAlgebra

オブザーバ

In [2]:
A = [0 1; -4 5]
B = [0; 1]
C = [1 0]
D = 0
P = ss(A, B, C, D)
println(P)

A = [0 1; -4 5]
B = [0; 1]
C = [1 0; 0 1]
D = [0; 0]
Ps = ss(A, B, C, D)
println(Ps)
StateSpace{Continuous, Int64}
A = 
  0  1
 -4  5
B = 
 0
 1
C = 
 1  0
D = 
 0

Continuous-time state-space model
StateSpace{Continuous, Int64}
A = 
  0  1
 -4  5
B = 
 0
 1
C = 
 1  0
 0  1
D = 
 0
 0

Continuous-time state-space model

オブザーバゲインの設計(極配置)

In [3]:
# オブザーバ極
observer_poles=[-15+5im,-15-5im]

# オブザーバゲインの設計(状態フィードバックの双対) 
L = -place(P.A', P.C', observer_poles)'
println(L)
ComplexF64[-35.0 + 0.0im; -421.0 + 0.0im;;]
In [4]:
L = [ convert(Float64, L[i]) for i in 1:length(L) ]
Out[4]:
2-element Vector{Float64}:
  -35.0
 -421.0
In [5]:
eigvals(P.A + L * P.C)
Out[5]:
2-element Vector{ComplexF64}:
 -15.000000000000002 - 4.9999999999999964im
 -15.000000000000002 + 4.9999999999999964im
In [6]:
# レギュレータ極
regulator_poles = [-5+5im, -5-5im]
# 極配置
F = -place(P.A, P.B, regulator_poles)
println(F)
ComplexF64[-46.0 - 0.0im -15.0 - 0.0im]
In [7]:
F = [ convert(Float64, F[i]) for i in 1:length(F) ]'
Out[7]:
1×2 adjoint(::Vector{Float64}) with eltype Float64:
 -46.0  -15.0
In [8]:
Gsf = ss(P.A + P.B*F, P.B, [1 0; 0 1], [0; 0]);
Obs = ss(P.A + L*P.C, [P.B -L], [1 0; 0 1], [0 0; 0 0]);
In [9]:
Td = 0:0.01:3;
X0 = [-1, 0.5];
u(x,t) = 0;
y, t, x, uout = lsim(Gsf, u, Td, x0=X0);

Ud = F*x;
y, t, xhat, uout = lsim(Obs, [Ud' y[1,:]]', Td, [0, 0]);

p = [ plot(), plot()]

for i=1:2
plot!(p[i], t, x[i,:],
    xlabel="t",   #X軸のラベル
    ylabel="x",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    label="x",
    size=(300,230)   #プロットのサイズ 
)
    
plot!(p[i], t, xhat[i,:],
    xlabel="t",   #X軸のラベル
    ylabel="x",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    label="x̂",
    size=(300,230)   #プロットのサイズ 
)
end

plot( p[1], p[2], layout=(1,2), size=(600,230) )
Out[9]:

出力フィードバック

In [10]:
# 出力フィードバック(オブザーバ+状態フィードバック)
K = ss(P.A+P.B*F+L*P.C, -L, F, 0)
println("K:\n", K)
println("----------------")
println("K(s)=\n", tf(K))
 
# フィードバック系
Gfb = feedback(P, K);
K:
StateSpace{Continuous, Float64}
A = 
  -35.0    1.0
 -471.0  -10.0
B = 
  35.0
 421.0
C = 
 -46.0  -15.0
D = 
 0.0

Continuous-time state-space model
----------------
K(s)=
TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}
-7925.000000000001s - 9215.999999997051
---------------------------------------
        1.0s^2 + 45.0s + 821.0

Continuous-time transfer function model
In [11]:
# Td = 0:0.01:3;

# u(x,t) = 0;
# y, t, x, uout = lsim(Gfb, Td, u, [-1, 0.5, 0, 0])

# plot(t, x',
#     xlabel="t",   #X軸のラベル
#     ylabel="x",   #Y軸のラベル
#     lw=2,           #線幅
#     ls=:solid,        #線種
#     legend=false,
#     size=(300,230)   #プロットのサイズ 
# )

外乱オブザーバ

In [2]:
using ControlSystems
using Plots; gr()
using LinearAlgebra
In [3]:
A = [0 1; -4 5]
B = [0; 1]
C = [1 0]
D = 0
P = ss(A, B, C, D)
println(P)

A = [0 1; -4 5]
B = [0; 1]
C = [1 0; 0 1]
D = [0; 0]
Ps = ss(A, B, C, D)
println(Ps)
StateSpace{Continuous, Int64}
A = 
  0  1
 -4  5
B = 
 0
 1
C = 
 1  0
D = 
 0

Continuous-time state-space model
StateSpace{Continuous, Int64}
A = 
  0  1
 -4  5
B = 
 0
 1
C = 
 1  0
 0  1
D = 
 0
 0

Continuous-time state-space model

出力に0.5の定値外乱が加わるとする

In [4]:
# オブザーバ極
observer_poles=[-15+5im,-15-5im]
# オブザーバゲインの設計(状態フィードバックの双対) 
L = -place(P.A', P.C', observer_poles)'
L = [ convert(Float64, L[i]) for i in 1:length(L) ]

# レギュレータ極
regulator_poles = [-5+5im, -5-5im]
# 極配置
F = -place(P.A, P.B, regulator_poles)
F = [ convert(Float64, F[i]) for i in 1:length(F) ]'


Gsf = ss(P.A + P.B*F, P.B, [1 0; 0 1], [0; 0]);
Obs = ss(P.A + L*P.C, [P.B -L], [1 0; 0 1], [0 0; 0 0]);
In [5]:
Td = 0:0.01:3;
d = 0.5
X0 = [-1, 0.5];
u(x,t) = 0;
y, t, x, uout = lsim(Gsf, u, Td, x0=X0);

Ud = F*x;
y, t, xhat, uout = lsim(Obs, [Ud' y[1,:].+d]', Td, [0, 0]);

p = [ plot(), plot()]

for i=1:2
plot!(p[i], t, x[i,:],
    xlabel="t",   #X軸のラベル
    ylabel="x",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    label="x",
    size=(300,230)   #プロットのサイズ 
)
    
plot!(p[i], t, xhat[i,:],
    xlabel="t",   #X軸のラベル
    ylabel="x",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    label="x̂",
    size=(300,230)   #プロットのサイズ 
)
end

plot( p[1], p[2], layout=(1,2), size=(600,230) )
Out[5]:
In [6]:
# オブザーバ極
observer_poles=[-15+5im,-15-5im, -3] 

# オブザーバゲインの設計(状態フィードバックの双対)
Abar = [ P.A zeros(2,1) ; 0 0 0 ] 
Bbar = [ P.B; 0 ]
Cbar = [ P.C 1 ]

Lbar = -place(Abar', Cbar', observer_poles)'
Lbar = [ convert(Float64, Lbar[i]) for i in 1:length(Lbar) ]

println(Lbar)
[149.5, -526.0, -187.5]
In [7]:
Fbar = [ F 0 ]

Aob = Abar + Lbar*Cbar
Bob = [Bbar -Lbar]
Obs = ss(Aob, Bob, [1 0 0; 0 1 0; 0 0 1], [0 0; 0 0; 0 0] );
In [8]:
pole(Obs)
Out[8]:
3-element Vector{ComplexF64}:
 -15.000000000000306 + 4.999999999999643im
 -15.000000000000306 - 4.999999999999643im
 -2.9999999999994746 + 0.0im
In [9]:
Td = 0:0.01:3;
d = 0.5
X0 = [-1, 0.5];
u(x,t) = 0;
y, t, x, uout = lsim(Gsf, u, Td, x0=X0);

Ud = F*x;
y, t, xhat, uout = lsim(Obs, [Ud' y[1,:].+d]', Td, [0, 0, 0]);

p = [ plot(), plot()]

for i=1:2
plot!(p[i], t, x[i,:],
    xlabel="t",   #X軸のラベル
    ylabel="x",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    label="x",
    size=(300,230)   #プロットのサイズ 
)
    
plot!(p[i], t, xhat[i,:],
    xlabel="t",   #X軸のラベル
    ylabel="x",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    label="x̂",
    size=(300,230)   #プロットのサイズ 
)
end

plot( p[1], p[2], layout=(1,2), size=(600,230) )
Out[9]:
In [10]:
plot(t, xhat[3,:],
    xlabel="t",   #X軸のラベル
    ylabel="d̂",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    legend=false,
    size=(300,230)   #プロットのサイズ 
)
Out[10]:

外乱 0.5 が推定できている

ロバスト制御

In [1]:
using ControlSystems
using Plots; gr()
using LinearAlgebra
In [2]:
g  = 9.81                # 重力加速度[m/s^2]
l  = 0.2                 # アームの長さ[m]
M  = 0.5                 # アームの質量[kg]
mu = 1.5e-2              # 粘性摩擦係数
J  = 1.0e-2              # 慣性モーメント

P = tf( [0,1], [J, mu, M*g*l] )

ref = 30 # 目標角度 [deg]
Out[2]:
30

乗法的不確かさ

In [3]:
# 垂直駆動アームのノミナルモデル
Pn = tf( [0,1], [J, mu, M*g*l] )

# 不確かさ
delta = LinRange(-1, 1, 100)
WT = tf( [10, 0], [1, 150])

P = TransferFunction[ (1 + WT*delta[i])*Pn for i in 1:length(delta) ];
DT = TransferFunction[ WT*delta[i] for i in 1:length(delta) ];

setPlotScale("dB")
w = exp10.(LinRange(-2, 3, 1000))
bodeplot(P, w; lc=:black, lw=0.5, size=(450,400), legend=false, title=""  )
Out[3]: