概要
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で書きたいという人には、ほんとにおすすめ。