概要

Pythonにデフォルトでインストールされているcontrolライブラリの使用方法の個人的メモです。

ただ、一部の関数を使用するには、slycotを追加でインストールする必要があり、そのインストールがなかなかうまくいかなかったので覚悟してください。

include

import control

Matlab関数も使用する場合

from control import matlab

伝達関数定義

tf = matlab.tf([1],[1 , 0])
print(tf)

->
1
-
s

状態方程式定義

A=[[1 , 0],
   [1 , 2]]
B=[[1],
   [0]]
C=[[1 , 0]]
D=[[0]]

sys = matlab.ss(A,B,C,D)

伝達関数・状態方程式相互変換

tf2 = control.ss2tf(sys)
sys2 = control.tf2ss(tf2)

A,B,C,D行列へのアクセス

A=sys.A
B=sys.B
C=sys.C
D=sys.D

np.linalg.eig(A)

最後の行は安定判別に使えます

bode線図

matlab.bode(sys)

ちなみに

import matplotlib.pyplot as plt

fig = plt.figure()
matlab.bode(sys)

fig2 = plt.figure()
matlab.nyquist(sys)

とすれば、ナイキストも連続して書けます

離散化

tf_d = control.c2d(tf,0.01,method='zoh')

を参考にさせていただきました。

リカッチ方程式（連続）

を参考にさせていただきました。

ModuleNotFoundError: No module named 'slycot'

と出たら

うえのリンクの内容で、slycotを探すときは、ctr+Fの方が現実的です（一応）。あと、下の内容で、

from pip._internal.utils.compatibility_tags import get_supported
print(get_supported())

を実行し、cp38,cp,39等を確認し、どれをインストールするか決める必要があるかもしれません。

を参考にしてください。
先人の方ありがとうございます。
助かりました。

と思ったらだめでした。

（追記）

で

conda install -c conda-forge control slycot

をすれば、解決！
先人の方ありがとうございました。

疲れた

リカッチ方程式の解法

import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt



A=[[1 , 0],
   [1 , 2]]
B=[[1],
   [0]]
C=[[1 , 0]]
D=[[0]]

sys = matlab.ss(A,B,C,D)

Q=np.array([[1,0],
            [0,1]]  )*10


R=np.array([[1]] )

N =[[0],
    [0]]

K,P,e = matlab.lqr(A,B,Q,R,N)

さらに、正しく解が得られているか確認します。

hoge = np.dot(sys.A.T,P) + np.dot(P,sys.A) 
hoge2 = np.dot(P,sys.B)
hoge3 = np.dot(np.dot(hoge2,np.linalg.inv(R)),hoge2.T)

hoge_final = hoge - hoge3 + Q

print(hoge_final)

->
[[9.80548975e-13 5.37170308e-12]
 [5.37170308e-12 2.91606739e-11]]

ほぼ０になったので、リカッチ方程式が成立していることが確認できました。

ちなみに、Kに関しては

u = -Kx

とする前提で設計されています。

離散リカッチ方程式の解法

原理的な話はこちら

関数の説明

dareが普通にPythonでも使えるのすごい

引数はnumpyでないとエラーが出るので、注意

[P,L,K] = matlab.dare(sys.A,sys.B,Q,R)

とすれば

\begin{eqnarray}
P &=& A^TPA+Q-(B^TPA)^T(R+B^TPB)^{-1}(B^TPA)\\
K &=& (R+ B^TPB)^{-1} B^T PA\\
u &=& -Kx
\end{eqnarray}

となる、$P,K$を見つけてきてくれます。

検算もしてみます

hoge = np.dot(np.dot(sys.A.T,P),sys.A) + Q
hoge2 = np.dot(np.dot(sys.B.T,P),A)
hoge2_2 = np.linalg.inv(R+np.dot(np.dot(sys.B.T,P),B) )
hoge3 = np.dot(np.dot(hoge2.T, hoge2_2) ,hoge2)

print(hoge-hoge3-P)
->
[[-8.52651283e-14 -8.52651283e-14]
 [-1.42108547e-13 -2.55795385e-13]]

ほぼ、０になったので確認できました。

可制御・可観測


Ctr = matlab.ctrb(sys.A,sys.B)

print(np.linalg.matrix_rank(Ctr))
print("Aの次元と一致すれば、可制御")

Obs = matlab.obsv(sys.A,sys.C)

print(np.linalg.matrix_rank(Obs))
print("Aの次元と一致すれば、可観測")

極配置

K = matlab.place(sys.A,sys.B,[-1,-2])

eig = np.linalg.eig(sys.A-np.dot(sys.B,K))
print(eig)

これで、配置と、確認ができます。[-1,-2]は配置したい場所です。

ただし、同じ場所にBの次元以上の個数、配置しようとするとエラーが出ました。数学的なものかな

伝達関数の分母・分子の取得

tf.den      #分母
tf.num      #分子

伝達関数に　s = jw を代入する

fr = matlab.evalfr(tf,1j*w)                  #s = jw　の代入結果を取得

便利すぎ～
全部あるやん

bode線図を関数でない方法で書く

import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt
import math
import cmath

tf = matlab.tf([1],[1 , 0])

w_max = 1000
w_min = 0.01
w = np.linspace(w_min,w_max,10000000)
fr = matlab.evalfr(tf,1j*w)                  #s = jw　の代入結果を取得

gain = abs(fr)

phase = []
for i in fr:
    phase.append(cmath.phase(i))   
phase = np.array(phase)                      #もっといい書き方あるかも


# グラフ化

fig = plt.figure()

# top
ax1 = fig.add_subplot(2, 1, 1)
ax1.plot(w, gain)
ax1.set_xlabel("w[rad/s]")
ax1.set_ylabel("gain")
ax1.set_yscale('log')
ax1.set_xscale('log')
ax1.set_xlim(w_min,w_max)

# bottom

ax2 = fig.add_subplot(2, 1, 2)
ax2.plot(w, phase)
ax2.set_xlabel("w[rad/s]")
ax2.set_ylabel("phase")
ax2.set_xlim(w_min,w_max)
ax2.set_xscale('log')
ax2.set_ylim(-np.pi-0.1,np.pi+0.1)

# show plots
fig.tight_layout()

悪くないね。
Phaseはdeg表示に変えた方がわかりやすいかな。

z = exp(jwT)を代入して、離散システムのボード線図を書く

離散システムのボード線図を書く場合は、代入してやれば、周波数特性がわかるのでやってみる
0.01sでサンプリングするシステムの場合、0Hzと100Hzの見分けがつかない。つまり、このシステムは0~100Hzまでの伝達関数がわかれば、周りの周波数に関しては、0~100Hzの繰り返しになるということである。


import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt
import math
import cmath

T_samp = 0.01

#グラフ表示範囲
w_max = 50 * 2*np.pi
w_min = 0.01

tf = matlab.tf([1],[1 , 0])

fig = plt.figure()
matlab.bode(tf)
               
tf_d = control.c2d(tf,T_samp,method='zoh')     
print(tf_d)

fig2 = plt.figure()
matlab.bode(tf_d)


w = np.linspace(w_min,w_max,10000000)
fr = matlab.evalfr(tf_d,np.exp(1j*w*T_samp))                  #s = exp(jwT)　の代入結果を取得

gain = abs(fr)

phase = []
for i in fr:
    phase.append(cmath.phase(i))   
phase = np.array(phase)  *180 / np.pi                    #もっといい書き方あるかも


#グラフ化

fig = plt.figure()

# top
ax1 = fig.add_subplot(2, 1, 1)
ax1.plot(w, gain)
ax1.set_xlabel("w[rad/s]")
ax1.set_ylabel("gain")
ax1.set_yscale('log')
ax1.set_xscale('log')
ax1.set_xlim(w_min,w_max)

# bottom

ax2 = fig.add_subplot(2, 1, 2)
ax2.plot(w, phase)
ax2.set_xlabel("w[rad/s]")
ax2.set_ylabel("phase")
ax2.set_xlim(w_min,w_max)
ax2.set_xscale('log')
ax2.set_ylim(-190,190)

# show plots
fig.tight_layout()

離散化前

離散化後(T=0.01s)

Z=exp(jwT)を代入してグラフ化

0.1rad/sで、20dB(10倍)なのが共通しており、形状もかなり近いので、離散システムに対して、bodeは正しく働いていることが分かった

離散ローパスフィルタの設計

を利用

一次

import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt

cut_of_freqency = 10    #Hz   :カットオフ周波数
T_samp = 0.01           #s    :制御周期


tf = matlab.tf([1],[1/(cut_of_freqency*2*np.pi),1])

fig = plt.figure()
matlab.bode(tf)
               
tf_d = control.c2d(tf,T_samp,method='zoh')     
print(tf_d)

fig2 = plt.figure()
matlab.bode(tf_d)

print("分母")
print(tf_d.den)

print("分子")
print(tf_d.num)

二次

import control
from control import matlab
import numpy as np
import matplotlib.pyplot as plt

cut_of_freqency = 1/2/np.pi    #Hz   :カットオフ周波数
T_samp = 0.01           #s    :制御周期

rad = cut_of_freqency*2*np.pi
tf = matlab.tf([1],[1/(rad**2),np.sqrt(2)/rad,1])

fig = plt.figure()
matlab.bode(tf)
               
tf_d = control.c2d(tf,T_samp,method='zoh')     
print(tf_d)

fig2 = plt.figure()
matlab.bode(tf_d)

print("分母")
print(tf_d.den)

print("分子")
print(tf_d.num)

離散化前

離散化後

1rad/sでカットオフ、平坦特性、-40dB/decがきれいに表れています。

###Arduinoへの実装テンプレート
後で書きます

感想

クッソ便利。
Scilabを使っていたけど、やっぱPythonで書きたいという人には、ほんとにおすすめ。