# 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

------------------
Before

------------------

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]: