PID制御と状態方程式

# はじめに
PID制御は、フィードバック制御の一種として有効な制御方法である。出力の比例要素であるP、積分要素であるI、微分要素であるDを入力に還元することで対象を制御しようとするものである。そこで、今回は状態方程式をPythonで解くプログラムを改造することによって、PID制御がどのようなものなのかを、時系列のグラフを作成することで調査するものとする。具体的には、以下のようなグラフを作成する。ただし、横軸が時間で縦軸が制御対象である。

![PID_Kp_re2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1686019/e0c80ad5-c26b-884b-870c-56473750eb1c.png)

# モデル
以下の記事のモデルを改造する。

https://qiita.com/arairuca/items/24c3e25b4a47647be31c

また、PID制御のアルゴリズム自体は以下の記事を参考にした。

https://nagasaki-u.repo.nii.ac.jp/record/2483/files/Chapter%202.pdf

したがって、モデルとなるブロック図は、以下のようになる。

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1686019/ace992f3-a461-483b-1ac1-f3a8cebc26b5.png)

ただし、$y_{opt}$を目標値、$y$を制御対象とした。

```math
u(t)=K_p(y_{opt}-y)+K_i\int_0^t(y_{opt}-y)dt+K_d\frac{d}{dt}(y_{opt}-y) 
```

という入力を用いるものとする。

ここで、新たに状態変数$z$を以下のように定義する。
```math
z=\int_0^t(y_{opt}-y)dt
```

また、目標値は定数なので、時間微分すると0になることから、入力である$u(t)$は、最終的に以下のように表すことができる。

```math
u(t)=K_p(y_{opt}-y)+K_i z-K_d\frac{dy}{dt} 
```

これをもとにして以下のようなプログラムを作成した。
ただし、今回は$K_p,K_i,K_d$の3種類のパラメータの影響を調べたいので、3パターンのプログラムを作成した。

# プログラム

モデルとしては、電験一種平成27年度二次試験機械・制御問4を参考にした。詳細な問題情報は電験王様を参照されたい。

https://denken-ou.com/c1/kikaiseigyoh27-4/

```math
\dot{\boldsymbol{x}}(t)=\boldsymbol{A}\boldsymbol{x}+\boldsymbol{b}u(t)　 
```

ただし、

```math
\boldsymbol{A}
=
\begin{pmatrix}
-4 & -6 \\
3 & 5 \\
\end{pmatrix}
,
\boldsymbol{b}=

\begin{pmatrix}
-2\\
3 \\
\end{pmatrix}

,
\boldsymbol{x}=

\begin{pmatrix}
x_1\\
x_2 \\
\end{pmatrix}

```

で表されるものとする。この対象をフィードバック制御をかけることで安定化させたい。そのときに用いるのは、P,I,D動作である。
## P要素
``` python model_P.py
import numpy as np
import matplotlib.pyplot as plt
import japanize_matplotlib
import matplotlib.animation as animation

# アニメを作る初期設定
fig = plt.figure()
#plt.axes().set_aspect('equal')
ims = []


A=np.array([[-4,-6],[3,5]])
b=np.array([-2,3])
c=np.array([1,0])
#f=np.array([3/4,13/6])

y_opt=5
z=0
#y=c@x

delta_t=1.0*10**-5


def control_main(K_p,K_i,K_d):
  
  global x,v,z
  #初期条件
  t=0
  x_1=1
  x_2=2

  v_1=0
  v_2=0
  v=np.array([v_1,v_2])
  x=np.array([x_1,x_2])
  #変数セット
  x_1_ary=[]
  x_2_ary=[]
  v_ary=[]
  t_ary=[]
  z_ary=[]
  y_ary=[]
  im=[]
  while t<10:
    y=c@x
    y_dot=c@v
    u=K_p*(y_opt-y)+K_i*z+K_d*(0-y_dot)

    v= A@x+b*u
    x=x+v*delta_t
    z=z+(y_opt-y)*delta_t
    v_ary.append(v)
    x_1_ary.append(x[0])
    x_2_ary.append(x[1])
    t_ary.append(t)
    y_ary.append(y)
    z_ary.append(z)
    t=t+delta_t
  #im=plt.plot(t_ary,y_ary)

  
  plt.plot(t_ary,y_ary,label='K_p='+str(round(float(K_p),3)))
  
  #im2=plt.legend()
  #im2=plt.title('K_p='+str(K_p))
  #ims.append(im)

  #plt.plot(t_ary,x_1_ary)
  #plt.show()
num=5
K_p=np.linspace(1,2,num)*(-1)

#K_p=-1
K_i=-0
K_d=-0

#control_main(-1.5,K_i,k_d)

#plt.show()
#control_main(K_p,K_i,-0.3)
for i in range(num):
  control_main(K_p[i],K_i,K_d)
plt.plot([0,10],[y_opt,y_opt],label='目標値')
plt.legend()
plt.savefig('PID_Kp_re2.png')
plt.show()

# # 複数枚のプロットを 20ms ごとに表示
# ani = animation.ArtistAnimation(fig, ims, interval=20)
# #保存
# ani.save("PID_Kp4.gif", writer="pillow")
```

これを実行すると以下のようなグラフが出力される。

![PID_Kp_re2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1686019/49954ac2-83c3-3cba-ad3d-59dc2f891b52.png)

このように、P要素を大きくしていけば確かに安定はする。しかし、目標値が定常値と等しくなることはなく、いくらかの偏差（オフセット）は残ってしまう。

これは、以下の考察によっても明らかである。

### 理論的な考察

定常偏差とは、十分に時間を与えて系が安定したとき
($t \to \infty $)の物性値を示すものである。

したがって、$\boldsymbol{x}$の時間変化分である、
$\dot{\boldsymbol{x}}=0$が成立していると考えることができる。

つまり、

```math
\dot{\boldsymbol{x}}=\boldsymbol{A}\boldsymbol{x}+\boldsymbol{b}u=0
```

ここで、P動作より、
```math
u=K_P(y_{opt}-y)=K_P(y_{opt}-\boldsymbol{c}\boldsymbol{x})
```
となるので、$\dot{\boldsymbol{x}}$の式に代入すると、

```math
\dot{\boldsymbol{x}}=\boldsymbol{A}\boldsymbol{x}+\boldsymbol{b}u=\boldsymbol{A}\boldsymbol{x}+\boldsymbol{b}\{K_P(y_{opt}-\boldsymbol{c}\boldsymbol{x})\}
```
したがって、

```math
\dot{\boldsymbol{x}}=(\boldsymbol{A}-K_P\boldsymbol{b}\boldsymbol{c})\boldsymbol{x}-K_Py_{opt}\boldsymbol{b}=0
```

```math
\boldsymbol{x}=-(\boldsymbol{A}-K_P\boldsymbol{b}\boldsymbol{c})^{-1}(K_Py_{opt}\boldsymbol{b})
```

```math
y=\boldsymbol{c}\boldsymbol{x}=-\boldsymbol{c}(\boldsymbol{A}-K_P\boldsymbol{b}\boldsymbol{c})^{-1}(K_Py_{opt}\boldsymbol{b})
```

この式より、$\boldsymbol{A}\ne 0$なので、$y\ne y_{opt}$となってしまい、P動作だけでは定常偏差と目標値が離れてしまう。

そこで、I制御を用いることで、目標値に定常値を漸近させることを考える。


## I要素

``` python model_I.py
import numpy as np
import matplotlib.pyplot as plt
import japanize_matplotlib
import matplotlib.animation as animation

# アニメを作る初期設定
fig = plt.figure()
#plt.axes().set_aspect('equal')
ims = []


A=np.array([[-4,-6],[3,5]])
b=np.array([-2,3])
c=np.array([1,0])
#f=np.array([3/4,13/6])

y_opt=5
z=0
#y=c@x

delta_t=1.0*10**-5


def control_main(K_p,K_i,K_d):
  
  global x,v,z
  #初期条件
  t=0
  x_1=1
  x_2=2

  v_1=0
  v_2=0
  v=np.array([v_1,v_2])
  x=np.array([x_1,x_2])
  #変数セット
  x_1_ary=[]
  x_2_ary=[]
  v_ary=[]
  t_ary=[]
  z_ary=[]
  y_ary=[]
  im=[]
  while t<10:
    y=c@x
    y_dot=c@v
    u=K_p*(y_opt-y)+K_i*z+K_d*(0-y_dot)

    v= A@x+b*u
    x=x+v*delta_t
    z=z+(y_opt-y)*delta_t
    v_ary.append(v)
    x_1_ary.append(x[0])
    x_2_ary.append(x[1])
    t_ary.append(t)
    y_ary.append(y)
    z_ary.append(z)
    t=t+delta_t
  #im=plt.plot(t_ary,y_ary)

  
  plt.plot(t_ary,y_ary,label='K_i='+str(round(float(K_i),3)))
  
  #im2=plt.legend()
  #im2=plt.title('K_p='+str(K_p))
  #ims.append(im)

  #plt.plot(t_ary,x_1_ary)
  #plt.show()
num=5
K_i=np.linspace(0,0.1,num)*(-1)

K_p=-1
#K_i=-0.5
K_d=-0

#control_main(-1.5,K_i,k_d)

#plt.show()
#control_main(K_p,K_i,-0.3)
for i in range(num):
  control_main(K_p,K_i[i],K_d)
plt.plot([0,10],[y_opt,y_opt],label='目標値')
plt.legend()
plt.savefig('PID_Ki_re2.png')
plt.show()

# # 複数枚のプロットを 20ms ごとに表示
# ani = animation.ArtistAnimation(fig, ims, interval=20)
# #保存
# ani.save("PID_Kp4.gif", writer="pillow")
```

![PID_Ki_re2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1686019/ad678f73-0bcf-226f-369c-99b4a1d735fe.png)

このように、I要素を変化させることで、目標値と定常値の差である偏差（オフセット）を少なくさせることができるようになる。

## D要素

``` python model_D.py
import numpy as np
import matplotlib.pyplot as plt
import japanize_matplotlib
import matplotlib.animation as animation

# アニメを作る初期設定
fig = plt.figure()
#plt.axes().set_aspect('equal')
ims = []


A=np.array([[-4,-6],[3,5]])
b=np.array([-2,3])
c=np.array([1,0])
#f=np.array([3/4,13/6])

y_opt=5
z=0
#y=c@x

delta_t=1.0*10**-5


def control_main(K_p,K_i,K_d):
  
  global x,v,z
  #初期条件
  t=0
  x_1=1
  x_2=2

  v_1=0
  v_2=0
  v=np.array([v_1,v_2])
  x=np.array([x_1,x_2])
  #変数セット
  x_1_ary=[]
  x_2_ary=[]
  v_ary=[]
  t_ary=[]
  z_ary=[]
  y_ary=[]
  im=[]
  while t<10:
    y=c@x
    y_dot=c@v
    u=K_p*(y_opt-y)+K_i*z+K_d*(0-y_dot)

    v= A@x+b*u
    x=x+v*delta_t
    z=z+(y_opt-y)*delta_t
    v_ary.append(v)
    x_1_ary.append(x[0])
    x_2_ary.append(x[1])
    t_ary.append(t)
    y_ary.append(y)
    z_ary.append(z)
    t=t+delta_t
  #im=plt.plot(t_ary,y_ary)

  
  plt.plot(t_ary,y_ary,label='K_d='+str(round(float(K_d),3)))
  
  #im2=plt.legend()
  #im2=plt.title('K_p='+str(K_p))
  #ims.append(im)

  #plt.plot(t_ary,x_1_ary)
  #plt.show()
num=5
K_d=np.linspace(0.1,0.2,num)*(-1)

K_p=-1
K_i=-0.5
#K_d=-0

#control_main(-1.5,K_i,k_d)

#plt.show()
#control_main(K_p,K_i,-0.3)
for i in range(num):
  control_main(K_p,K_i,K_d[i])
plt.plot([0,10],[y_opt,y_opt],label='目標値')
plt.legend()

plt.savefig('PID_Kd_re.png')
plt.show()

# # 複数枚のプロットを 20ms ごとに表示
# ani = animation.ArtistAnimation(fig, ims, interval=20)
# #保存
# ani.save("PID_Kp4.gif", writer="pillow")

```

![PID_Kd_re.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/1686019/b1f2a45c-6258-cc5b-c582-489c8ee1ac72.png)

最後にD要素であるが、上の画像のように値を少し変化させてあげるだけで制御対象の挙動が大きく変化してしまうことがわかる。

# まとめ
今回は、フィードバック制御の一種であるPID制御について、現代制御の観点から考察した。なぜ古典制御ではなく現代制御を用いて考察を行ったのかというと、コンピュータはラプラス変換のような特殊な演算よりも線形代数のような演算のほうが得意だからである。また、I制御である積分については、少し実装に悩んだが$z$という状態変数を新たに定義することで、上手く扱うことができた。

# 参考文献

補足情報として、今回使用しなかったがMatplotlibのアニメーション機能について述べる。

https://qiita.com/kumamupooh/items/5273b0b98a4b6ee976fc

https://qiita.com/yubais/items/c95ba9ff1b23dd33fde2

https://qiita.com/AnchorBlues/items/3acd37331b12e844d259

https://www.sejuku.net/blog/61007