Chapter 6

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

ナイキストの安定判別

In [2]:
P = tf([0, 1], [1, 1, 1.5, 1])
println(P)
println(pole(P))
TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}
            1.0
----------------------------
1.0s^3 + 1.0s^2 + 1.5s + 1.0

Continuous-time transfer function model
ComplexF64[-0.12040192275078712 + 1.1413527165187305im, -0.12040192275078712 - 1.1413527165187305im, -0.7591961544984255 + 0.0im]
In [3]:
P = tf([0,1],[1,1,1.5,1])
# 位相が 180[deg] 遅れる周波数を取得
wpc, _, _, _ = margin(P);

t = 0:0.1:30;
u = sin.(wpc[]*t);
y = 0 .* u;

p = [ plot() plot(); plot() plot() ]

for i in [1,2]
    for j in [1,2]
        # 出力をネガティブフィードバックして次の時刻の入力を生成
        u = sin.(wpc[]*t) - vec(y)
        y, t, x, uout = lsim(P, u', t)
        plot!(p[i,j], t, u, label="u");
        plot!(p[i,j], t, vec(y), label="y");
    end
end

plot( p[1,1], p[1,2], p[2,1], p[2,2], layout=(2,2))
Out[3]:
In [4]:
P = tf([0, 1], [1, 2, 1.9999, 1])
println(P)
println(pole(P))
TransferFunction{Continuous, ControlSystems.SisoRational{Float64}}
              1.0
-------------------------------
1.0s^3 + 2.0s^2 + 1.9999s + 1.0

Continuous-time transfer function model
ComplexF64[-1.0000999999990001 + 0.0im, -0.4999500000004993 + 0.8659965401198194im, -0.4999500000004993 - 0.8659965401198194im]
In [5]:
P = tf([0,1],[1,2,1.9999,1])
# 位相が 180[deg] 遅れる周波数を取得
wpc, _, _, _ = margin(P);

t = 0:0.1:30;
u = sin.(wpc[]*t);
y = 0 .* u;

p = [ plot() plot(); plot() plot() ]

for i in [1,2]
    for j in [1,2]
        # 出力をネガティブフィードバックして次の時刻の入力を生成
        u = sin.(wpc[]*t) - vec(y)
        y, t, x, uout = lsim(P, u', t)
        plot!(p[i,j], t, u, label="u");
        plot!(p[i,j], t, vec(y), label="y");
    end
end

plot( p[1,1], p[1,2], p[2,1], p[2,2], layout=(2,2))
Out[5]:
In [7]:
# 閉ループ系が不安定になる場合
P = tf([0, 1],[1, 1, 1.5, 1])

nyquistplot(P, gaincircle=true, lw = 2, xlims=(-2.5,2.5), ylims=(-2.5,2.5), size=(300,300))
Out[7]:
In [8]:
# 閉ループ系が安定になる場合
P = tf([0, 1],[1, 2, 1.9999, 1])

nyquistplot(P, gaincircle=true, lw = 2, xlims=(-2.5,2.5), ylims=(-2.5,2.5), size=(300,300))
Out[8]:

アームの角度制御(PID制御)

In [3]:
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[3]:
30

P制御

In [4]:
kp = (0.5, 1, 2)

H = [ P*tf([0, kp[i]], [0, 1]) for i = 1:length(kp) ]

setPlotScale("dB")
bodeplot(H, 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]:
In [5]:
for i in 1:length(kp)
    K = tf([0, kp[i]], [0, 1]) # P制御
    H = P * K  # 開ループ系
    
    println("kP=", kp[i])
    println("(wpc, GM, wgc, PM)")
    println(margin(H))
    println("-----------------")
end
kP=0.5
(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [12.048359830388655;;], [21.007168728281016;;])
-----------------
kP=1
(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [14.00755547266918;;], [12.0877310471827;;])
-----------------
kP=2
(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [17.231985129715717;;], [7.406517464524029;;])
-----------------

PI制御

In [6]:
kp = 2
ki = (0, 5, 10)

H = [ P*tf([kp, ki[i]], [1, 0]) for i = 1:length(ki) ]

setPlotScale("dB")
bodeplot(H, 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[6]:
In [7]:
for i in 1:length(ki)
    K = tf([kp, ki[i]], [1, 0])  # PI制御
    H = P * K  # 開ループ系
    
    println("kI=", ki[i])
    println("(wpc, GM, wgc, PM)")
    println(margin(H))
    println("-----------------")
end
kI=0
(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [17.231985129715717;;], [7.406517464524029;;])
-----------------
kI=5
(wpc, GM, wgc, PM)
([15.671318145841404;;], [0.7374341834142752;;], [17.29090924820676;;], [0.8699461934346004;;])
-----------------
kI=10
(wpc, GM, wgc, PM)
([11.849199557862834;;], [0.2113800049851713;;], [17.452928304499043;;], [8.761143190659993;;])
-----------------
In [8]:
plt = plot()

for i in 1:1:length(ki)
    K = tf([kp, ki[i]], [1, 0])  # PI制御
    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[8]:

PID制御

In [9]:
kp = 2
ki = 5
kd = (0, 0.1, 0.2)

H = [ P*tf([kd[i], kp, ki], [1,0]) for i = 1:length(kd) ]

setPlotScale("dB")
bodeplot(H, 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[9]:
In [10]:
for i in 1:length(kd)
    K = tf([kd[i], kp, ki], [1,0])  # PID制御
    H = P * K  # 開ループ系
    
    println("kD=", kd[i])
    println("(wpc, GM, wgc, PM)")
    println(margin(H))
    println("-----------------")
end
kD=0
(wpc, GM, wgc, PM)
([15.671318145841404;;], [0.7374341834142752;;], [17.29090924820676;;], [0.8699461934346004;;])
-----------------
kD=0.1
(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [18.809759446133874;;], [45.220116699008315;;])
-----------------
kD=0.2
(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [24.730502425798765;;], [71.27203690036902;;])
-----------------
In [11]:
plt = plot()

for i in 1:1:length(kd)
    K = tf([kd[i],kp,ki],[1,0])  # PID制御
    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[11]:

開ループ系の比較

In [12]:
kp = (2, 1)
ki = (5, 0)
kd = (0.1, 0)
Label = ("After", "Before")

H = [ P*tf([kd[i], kp[i], ki[i]], [1,0]) for i = 1:length(kp) ]

setPlotScale("dB")
bodeplot(H, lw=2, size=(450,400), 
    label=["After" "After" "Before" "Before"] , title=""  )
Out[12]:
In [13]:
for i in 1:length(kp)
    K = tf([kd[i], kp[i], ki[i]], [1,0])  # PID制御
    H = P * K  # 開ループ系
    
    print("kP=", kp[i], ", kI=", ki[i], ", kD=", kd[i])
    println("(wpc, GM, wgc, PM)")
    println(margin(H))
    println("-----------------")
end
kP=2, kI=5, kD=0.1(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [18.809759446133874;;], [45.220116699008315;;])
-----------------
kP=1, kI=0, kD=0(wpc, GM, wgc, PM)
([NaN;;], [Inf;;], [14.00755547266918;;], [12.0877310471827;;])
-----------------

閉ループ系の比較

In [14]:
kp = (2, 1)
ki = (5, 0)
kd = (0.1, 0)
Label = ("After", "Before")

plt = plot()

for i in 1:1:length(kd)
    K = tf( [kd[i], kp[i], ki[i]], [1, 0])
    Gyr = feedback(P*K, 1)
    Gyr = minreal(Gyr)
    y, t = step( Gyr, 0:0.01:2 )
    plot!(plt, t, y',
    xlabel="t",   #X軸のラベル
    ylabel="y",   #Y軸のラベル
    lw=2,           #線幅
    ls=:solid,        #線種
    label="$(Label[i])",
    legend=:bottomright,
    size=(300,230)   #プロットのサイズ 
    )
    
    println(Label[i])
    e_std = ( 1 - dcgain(Gyr)[])
    println("定常偏差 =", e_std)    
    println("------------------")
end

plot(plt)
After
定常偏差 =2.220446049250313e-16
------------------
Before
定常偏差 =0.49520444220090865
------------------
Out[14]:
In [15]:
H = [ P*tf([kd[i], kp[i], ki[i]], [1,0]) for i = 1:length(kp) ]
Gyr = [ H[i]/(1+H[i]) for i = 1:length(kp) ]

setPlotScale("dB")
bodeplot(Gyr, lw=2, size=(450,400),
    label=["After" "After" "Before" "Before"] , title=""  )
Out[15]: