# Chapter 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              # 粘性摩擦係数[kg*m^2/s]
J  = 1.0e-2              # 慣性モーメント[kg*m^2]

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

ref = 30 # 目標角度 [deg]

Out[2]:
30

### P制御¶

In [3]:
plt = plot()

kp = (0.5, 1, 2)

for i in 1:1:length(kp)
K = tf([0, kp[i]], [0, 1])
Gyr = feedback(P*K, 1)
y, t = step( Gyr, 0:0.01:2 )
plot!(plt, t, ref.*y',
xlabel="t",   #X軸のラベル
ylabel="y",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="kₚ=$(kp[i])", legend=:bottomright, size=(300,230) #プロットのサイズ ) end plot(plt)  Out[3]: In [4]: K = [ tf([0, kp[i]], [0, 1]) for i = 1:length(kp) ] Gyr = [ feedback(P*K[i], 1) for i = 1:length(kp) ] setPlotScale("dB") bodeplot(Gyr, lw=2, size=(450,400), label=["kₚ=$(kp[1])" "kₚ=$(kp[1])" "kₚ=$(kp[2])" "kₚ=$(kp[2])" "kₚ=$(kp[3])" "kₚ=$(kp[3])"], legend=:best, title="" )  Out[4]: ### PD制御¶ In [5]: plt = plot() kp = 2 kd = (0, 0.1, 0.2) for i in 1:1:length(kd) K = tf([kd[i], kp], [0, 1]) Gyr = feedback(P*K, 1) y, t = step( Gyr, 0:0.01:2 ) plot!(plt, t, ref.*y', xlabel="t", #X軸のラベル ylabel="y", #Y軸のラベル lw=2, #線幅 ls=:solid, #線種 label="kd=$(kd[i])",
legend=:bottomright,
size=(300,230)   #プロットのサイズ
)
end

plot(plt)

Out[5]:
In [6]:
K = [ tf([kd[i], kp], [0, 1]) for i = 1:length(kd) ]
Gyr = [ feedback(P*K[i], 1) for i = 1:length(kd) ]

setPlotScale("dB")
bodeplot(Gyr, lw=2, size=(450,400),
label=["kd=$(kd[1])" "kd=$(kd[1])" "kd=$(kd[2])" "kd=$(kd[2])" "kd=$(kd[3])" "kd=$(kd[3])"],
legend=:best, title=""  )

Out[6]:

### PID制御¶

In [7]:
kp = 2
kd = 0.1
ki = (0, 5, 10)

plt = plot()

for i in 1:1:length(ki)
K = tf([kd, kp, ki[i]], [1, 0])
Gyr = feedback(P*K, 1)
y, t = step( Gyr, 0:0.01:2 )
plot!(plt, t, ref.*y',
xlabel="t",   #X軸のラベル
ylabel="y",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="ki=$(ki[i])", legend=:bottomright, size=(300,230) #プロットのサイズ ) end plot(plt)  Out[7]: In [8]: K = [ tf([kd, kp, ki[i]], [1, 0]) for i = 1:length(ki) ] Gyr = [ feedback(P*K[i], 1) for i = 1:length(ki) ] setPlotScale("dB") bodeplot(Gyr, lw=2, size=(450,400), label=["ki=$(ki[1])" "ki=$(ki[1])" "ki=$(ki[2])" "ki=$(ki[2])" "ki=$(ki[3])" "ki=$(ki[3])"], legend=:best, title="" )  Out[8]: ### 練習問題（外乱抑制）¶ In [9]: kp = 2 kd = 0.1 ki = (0, 5, 10) plt = plot() for i in 1:1:length(ki) K = tf([kd, kp, ki[i]], [1, 0]) Gyr = feedback(P, K) y, t = step( Gyr, 0:0.01:2 ) plot!(plt, t, y', xlabel="t", #X軸のラベル ylabel="y", #Y軸のラベル lw=2, #線幅 ls=:solid, #線種 label="ki=$(ki[i])",
legend=:right,
size=(300,230)   #プロットのサイズ
)
end

plot(plt)

Out[9]:
In [10]:
K = [ tf([kd, kp, ki[i]], [1, 0]) for i = 1:length(ki) ]
Gyd = [ feedback(P, K[i]) for i = 1:length(ki) ]

setPlotScale("dB")
bodeplot(Gyd, lw=2, size=(450,400),
label=["ki=$(ki[1])" "ki=$(ki[1])" "ki=$(ki[2])" "ki=$(ki[2])" "ki=$(ki[3])" "ki=$(ki[3])"],
legend=:best, title=""  )

Out[10]:

## 2自由度制御¶

In [11]:
kp = 2
ki = 10
kd = 0.1

K1 = tf([kd, kp, ki], [1, 0]);
K2 = tf([0, ki], [kd, kp, ki]);
K3 = tf([kp, ki], [kd, kp, ki]);


### PI-D制御¶

In [12]:
Gyz = feedback(P*K1, 1)

Td = 0:0.01:2
r = [ 1 for i in Td]

yorg, t, x, uout = lsim(Gyz, r', Td);

z, t, x, uout = lsim(K3, r', Td);
y, t, x, uout = lsim(Gyz, z, Td);

p = [ plot(), plot()]

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

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

plot!(p[2], t, ref*yorg',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="PID",
size=(300,230)   #プロットのサイズ
)

plot!(p[2], t, ref*y',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="PI-D",
legend=:bottomright,
size=(300,230)   #プロットのサイズ
)

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

Out[12]:

PID制御では，$G_{ur}$ がインプロパーになるので，擬似微分を用いて計算する

In [13]:
tau = 0.0000001 # ローパスフィルタ
Klp = tf([kd, 0], [tau, 1]) # 擬似微分器
Ktau = tf([kp, ki], [1, 0]) + Klp

Gyz = feedback(P*Ktau, 1)
Guz = Ktau/(1+P*Ktau)

Td = 0:0.01:2
r = [ 1 for i in Td]

yorg, t, x, uout = lsim(Gyz, r', Td);
ur, t, x, uout = lsim(Guz, r', Td);

z, t, x, uout = lsim(K3, r', Td);
y, t, x, uout = lsim(Gyz, z, Td);
uz, t, x, uout = lsim(Guz, z, Td);

p = [ plot(), plot()]

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

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

plot!(p[2], t, ref*yorg',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="PID",
size=(300,230)   #プロットのサイズ
)

plot!(p[2], t, ref*y',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="PI-D",
size=(300,230)   #プロットのサイズ
)

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

Out[13]:

### I-PD制御¶

In [14]:
Gyz = feedback(P*K1, 1)

Td = 0:0.01:2
r = [ 1 for i in Td]

yorg, t, x, uout = lsim(Gyz, r', Td);

z, t, x, uout = lsim(K2, r', Td);
y, t, x, uout = lsim(Gyz, z, Td);

p = [ plot(), plot()]

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

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

plot!(p[2], t, ref*yorg',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="PID",
size=(300,230)   #プロットのサイズ
)

plot!(p[2], t, ref*y',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="I-PD",
size=(300,230)   #プロットのサイズ
)

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

Out[14]:
In [15]:
tau = 0.0000001 # ローパスフィルタ
Klp = tf([kd, 0], [tau, 1]) # 擬似微分器
Ktau = tf([kp, ki], [1, 0]) + Klp

Gyz = feedback(P*Ktau, 1)
Guz = Ktau/(1+P*Ktau)

Td = 0:0.01:2
r = [ 1 for i in Td]

yorg, t, x, uout = lsim(Gyz, r', Td);
ur, t, x, uout = lsim(Guz, r', Td);

z, t, x, uout = lsim(K2, r', Td);

y, t, x, uout = lsim(Gyz, z, Td);
uz, t, x, uout = lsim(Guz, z, Td);

p = [ plot(), plot()]

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

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

plot!(p[2], t, ref*yorg',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="PID",
size=(300,230)   #プロットのサイズ
)

plot!(p[2], t, ref*y',
xlabel="t",   #X軸のラベル
ylabel="x",   #Y軸のラベル
lw=2,           #線幅
ls=:solid,        #線種
label="I-PD",
size=(300,230)   #プロットのサイズ
)

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

Out[15]:

# 限界感度法¶

### 無駄時間要素¶

In [16]:
Pdelay = series(P, delay(0.005))

Pdelay_appr = series(P, pade( 0.005, 1)) # パデ近似

w = exp10.(range(-2, 3,1000))
bodeplot([Pdelay, Pdelay_appr], w; lw = 2,