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, 
    label=["Pdelay" "Pdelay" "Pade" "Pade"], title="",size=(450,400))
Out[16]: