#Pythonで学ぶ制御工学< P・PI・PID制御（開ループ系） >
##はじめに
基本的な制御工学をPythonで実装し，復習も兼ねて制御工学への理解をより深めることが目的である．
その第21弾として「P・PI・PID制御（開ループ系）」を扱う．
##P・PI・PID制御
制御については，以前(https://qiita.com/Yuya-Shimizu/items/8570640e6e03c3d1e09a )に学習したが，ここでは開ループ特性を確認しながらPID制御器を設計する．まず，P・PI制御を実装して，ゲインを変化させたときの開ループ伝達関数$H(s)=P(s)K(s)$の特性がどのように変化するのかを確認する．なお，ここでの制御対象も垂直駆動アームの角度制御とする．
##実装
###P制御
以下にソースコードと出力を示す．出力はパラメータと図とがある．
#####ソースコード
"""
2021/03/26
@Yuya Shimzu
P制御（アーム）
"""
import numpy as np
import matplotlib.pyplot as plt
from control import tf, feedback
from control.matlab import bode, logspace, margin
from tf_arm import arm_tf #自作関数
#https://qiita.com/Yuya-Shimizu/items/c7b69b4dfd63fb8facfa
from for_plot import linestyle_generator, plot_set, bodeplot_set #自作関数
#https://qiita.com/Yuya-Shimizu/items/f811317d733ee3f45623
##パラメータ設定
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 = arm_tf(J, mu, M, g, l)
#目標値(指示値=refference)
ref = 30 #目標角度30[deg]
#比例ゲイン
kp = (0.5, 1, 2) #比較のために3つ用意
##P制御を用いたときのステップ応答の描画
LS = linestyle_generator()
fig, ax = plt.subplots(2, 1)
for i in range(len(kp)):
K = tf([0, kp[i]], [0, 1]) #P制御
H = P*K #開ループ系
gain, phase, w = bode(H, logspace(-1, 2), Plot = False) #ボード線図
#ゲイン線図と位相線図
pltargs = {'ls':next(LS), 'label':f"$k_P$={kp[i]}"}
ax[0].semilogx(w, 20*np.log10(gain), **pltargs)
ax[1].semilogx(w, phase*180/np.pi, **pltargs)
#ゲイン余裕，位相余裕，位相交差周波数，ゲイン交差周波数
print('Kp=', kp[i])
print('(GM, PM, wpc, wgc)')
print(margin(H))
print('--------------------------')
ax[0].set_title('P control - Bode Plot')
bodeplot_set(ax, 3)
plt.show()
#####出力
Kp= 0.5
(GM, PM, wpc, wgc)
(inf, 21.156175957298814, nan, 12.030378476260191)
--------------------------
Kp= 1
(GM, PM, wpc, wgc)
(inf, 12.118321095140175, nan, 13.995414100411576)
--------------------------
Kp= 2
(GM, PM, wpc, wgc)
(inf, 7.419183191955369, nan, 17.217014751495988)
--------------------------
出力された図に赤丸でゲイン交差周波数に印をつけた．比例ゲインを大きくすると，ゲイン交差周波数が大きくなることがわかる．また図からは分かりにくいが出力された位相余裕GMを見ると，小さくなることが分かる．このことから，比例ゲインを大きくすると，応答が速くなるかわりに，応答が振動的になる．また低周波ゲインは大きくなるが，$\infty$でないため，定常偏差が残る．
###PI制御
以下にソースコードと出力を示す．出力はパラメータと図とがある．
#####ソースコード
"""
2021/03/28
@Yuya Shimzu
PI制御（アーム）
"""
import numpy as np
import matplotlib.pyplot as plt
from control import tf, feedback
from control.matlab import bode, logspace, margin, step
from tf_arm import arm_tf #自作関数
#https://qiita.com/Yuya-Shimizu/items/c7b69b4dfd63fb8facfa
from for_plot import linestyle_generator, plot_set, bodeplot_set #自作関数
#https://qiita.com/Yuya-Shimizu/items/f811317d733ee3f45623
##パラメータ設定
g = 9.8 #重力加速度[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 = arm_tf(J, mu, M, g, l)
#目標値(指示値=refference)
ref = 30 #目標角度30[deg]
#比例ゲインと積分ゲイン
kp = 2
ki = (0, 5, 10) #比較のために3つ用意
##PI制御を用いたときのボード線図
LS = linestyle_generator()
fig, ax = plt.subplots(2, 1)
for i in range(len(ki)):
K = tf([kp, ki[i]], [1, 0]) #PD制御
H = P * K #開ループ系
gain, phase, w = bode(H, logspace(-1, 2), Plot=False) #ボード線図
#ゲイン線図と位相線図
pltargs = {'ls':next(LS), 'label':f"$k_I$={ki[i]}"}
ax[0].semilogx(w, 20*np.log10(gain), **pltargs)
ax[1].semilogx(w, phase*180/np.pi, **pltargs)
#ゲイン余裕，位相余裕，位相交差周波数，ゲイン交差周波数
print('kp=', kp, 'ki=', ki[i])
print('(GM, PM, wpc, wgc)')
print(margin(H))
print('--------------------------')
ax[0].set_title(f"PI control : $k_P$={kp} $k_I$={ki} - Bode Plot")
bodeplot_set(ax, 'lower left')
plt.show()
##PI制御を用いたときのステップ応答の描画
LS = linestyle_generator()
fig, ax = plt.subplots()
for i in range(len(ki)):
K = tf([kp, ki[i]], [1, 0]) #PI制御
Gyr = feedback(P*K, 1) #閉ループ系
y, t = step(Gyr, np.arange(0, 2, 0.01)) #ステップ応答
pltargs = {'ls':next(LS), 'label':f"$k_I$={ki[i]}"}
ax.plot(t, y*ref, **pltargs)
ax.set_title(f"PI control: $k_P$={kp} $k_I$={ki}")
ax.axhline(ref, color='k', linewidth=0.5)
plot_set(ax, 't', 'y', 'best')
plt.show()
#####出力
kp= 2 ki= 0
(GM, PM, wpc, wgc)
(inf, 7.417931704458113, nan, 17.214126785924563)
--------------------------
kp= 2 ki= 5
(GM, PM, wpc, wgc)
(0.7349999999999999, -0.8676868998766736, 15.652475842498527, 17.27470341959991)
--------------------------
kp= 2 ki= 10
(GM, PM, wpc, wgc)
(0.21, -8.765023417097723, 11.832159566199232, 17.4470150837457)
--------------------------
出力された図に赤丸でゲイン交差周波数に印を，青丸で位相交差周波数に印をつけた．積分ゲインを大きくすると，低周波ゲインが大きくなることがわかる．さらに直流ゲインは$\infty$となるので，ステップ目標値に対する定常偏差は0になる．一方で位相余裕は小さくなり，さらには0degを下回ってしまう．そして，$\omega_{pc}>\omega_{gc}$の関係が崩れてしまう．これは閉ループ系が不安定となる．このことから，積分ゲインを大きくすると，定常偏差を小さくできるかわりに，振動的な応答になったり，不安定になったりする．
実際にこのときの閉ループ系のステップ応答を示す．
発散しており，確かに不安定になっていることが確認できる．
###PID制御
以下にソースコードと出力を示す．出力はパラメータと図とがある．
#####ソースコード
#####出力
Kp= 2 Ki= 5 Kd= 0
(GM, PM, wpc, wgc)
(0.7349999999999999, -0.8676868998766736, 15.652475842498527, 17.27470341959991)
--------------------------
Kp= 2 Ki= 5 Kd= 0.1
(GM, PM, wpc, wgc)
(inf, 45.204662149872405, nan, 18.800416410143438)
--------------------------
Kp= 2 Ki= 5 Kd= 0.2
(GM, PM, wpc, wgc)
(inf, 71.268850312253, nan, 24.726943509483277)
--------------------------
PI制御では，積分ゲインを大きくすると閉ループ系が不安定になっていたが，微分ゲインを大きくすると位相余裕が大きくなり，不安定化を回避できている．また，微分ゲインを大きくするとゲイン交差周波数が大きくなる一方，低周波ゲインは変わらないことも分かる．このことから，D制御を加えることで，振動が小さくなり，少し応答が速くなることが分かる．
これは閉ループ系からも確認できる．
以上のまとめとして，2つの制御器
- 設計前：$k_{P}=1, k_{I}=0, k_{D}=0$
- 設計前：$k_{P}=2, k_{I}=5, k_{D}=0.1$
の性能を比較する．
そのソースコードは先ほど示したソースコードの後半に記述している．出力のみを以下に示す．
開ループ系の設計仕様
- 安定性（$\omega_{gc}<\omega_{pc}$）
- 速応性（$\omega_{gc}$をできるだけ大きくする）
- 減衰性（位相余裕PMを大きくする）
- 定常偏差（低周波ゲインを大きくする）
設計後のパラメータにおいては，これらを満たしていることが分かる．次に閉ループ系についての出力も確認しておく．
閉ループ系の設計仕様
- 速応性（$G_{yr}$のゲイン線図においてバンド幅$\omega_{bw}$が十分大きい）
- 減衰性（$G_{yr}$のゲイン線図においてピークゲイン$M_{p}$が小さい）
- 定常偏差（$G_{yr}$の低周波域のゲインが0dB）
設計後のパラメータにおいては，これらも満たしていることが分かる．
さいごに，このときのステップ応答を図示し，そのときの挙動についても確認する．
この図から，設計後のパラメータにおいて，振動を抑えつつ速やかに目標値に追従していることが分かる．
##感想
今回は，開ループ系の設計仕様を満足する設計にすることで，閉ループ系の設計仕様も満足し，うまく制御器を設計できることを確認できた．開ループ系も閉ループ系も設計仕様においていろいろな用語があり，図において見るべきところがいくつかあるため，図から設計仕様を満たしているか判断するのには，まだ時間はかかるが，理解はできているのではないかと思う．
##参考文献
Pyhtonによる制御工学入門 南 祐樹 著 オーム社