理系的な戯れ

理工学系とくにロボットやドローンに関する計算・プログラミング等の話題を扱って、そのようなことに興味がある人たちのお役に立てればと思っております。

Pythonで考えるDCモータの制御(7)伝達関数計算用関数

伝達関数計算用関数

はじめに

こんにちは、こうへいです。

これまで、繰り返し繰り返し、伝達関数などの計算を行い、そろそろ同じことの繰り返しに飽きてきたので Pythonの関数に落とし込もうと思い、プロトタイプを作ってみました。

伝達関数を分母と分子の多項式の係数を与える形で、定義してやるとステップ応答や、ランプ応答を求める事ができます。

制御に関する基本的なことは以下を参考にしてみてください。

blog.rikei-tawamure.com

制御用関数の再発明

趣味で制御用の関数を作って、モータの制御などを考察しようとしています。

車輪の再発明系ですね(笑)

よく使うモジュールのインポート

import numpy as np
import matplotlib.pyplot as plt

伝達関数を定義する関数

numpyの多項式関数poly1d()を駆使して、伝達関数を定義する関数を作ってみました。

分母と分子の共通の極とゼロ点については約分する機能付きです。 実はこの機能の実装が一番難しくて、ソース見るとごちゃごちゃ泥臭くやっていて、もう少しエレガントにできないものかと思うのですが、今のところ限界です。

# ##### 伝達関数の定義
# tf(num,den)
# - num:分子の係数リスト
# - den:分母の係数リスト

# In[ ]:


def tf(num,den):
    
    num=np.poly1d(num)
    den=np.poly1d(den)
    
    z=num.r
    p=den.r

    
    zreal=z[np.isclose(z.imag,0.0)].real
    zcomp=z[~np.isclose(z.imag,0.0)]
    preal=p[np.isclose(p.imag,0.0)].real
    pcomp=p[~np.isclose(p.imag,0.0)]

    zzreal=zreal
    zzcomp=zcomp
    ppreal=preal
    ppcomp=pcomp
    
    #分母分子から共通の極を見つけ出して削除する
    for x in zreal:
        ppreal=ppreal[~np.isclose(ppreal, x)]

    for x in preal:
        zzreal=zzreal[~np.isclose(zzreal, x)]

    for x in zcomp:
        ppcomp=ppcomp[~np.isclose(ppcomp, x)]

    for x in pcomp:
        zzcomp=zzcomp[~np.isclose(zzcomp, x)]
    
    zz=np.concatenate([zzreal, zzcomp], 0)
    pp=np.concatenate([ppreal, ppcomp], 0)
    
    
    num=np.poly1d(zz, r=True, variable = "s")*num.coef[0]
    den=np.poly1d(pp, r=True, variable = "s")*den.coef[0]

    return [num, den]

伝達関数の演算用関数

制御系を設計したり解析したりするために伝達関数を結合したり、掛け合わせたり、逆数をとったりして 開ループ伝達関数や閉ループ伝達関数を作り出すための演算用関数群

# ##### 伝達関数演算ライブラリ
# 
# - tf_add(sys1, sys2):伝達関数同士の和
# - tf_multi(sys1, sys2):伝達関数同士の積
# - tf_inv(sys):伝達関数の逆
#  - sys1,sys2,sys:伝達関数 

# In[ ]:


def tf_add(sys1, sys2):
    num=sys1[0]*sys2[1]+sys1[1]*sys2[0]
    den=sys1[1]*sys2[1]
    #print('d',num)
    #print('d',den)
    return  tf(num.coef, den.coef)


def tf_multi(sys1, sys2):
    num=sys1[0]*sys2[0]
    den=sys1[1]*sys2[1]
    return tf(num.coef, den.coef)

def tf_inv(sys):
    return tf(sys[1].coef, sys[0].coef)

伝達関数表示用関数

これは思いっきりショボいです。変数がsではなくxででます。

時間があったら改善したいですが、優先順位は低いです。

def tf_print(sys):
    num=sys[0].coef
    den=sys[1].coef
    m=len(num)
    n=len(den)
    s=''
    for i in range(m):
        if i!=m-1:
            if ~np.isclose(num[i],0.0):
                s=s+'{:.2e} s^{} +'.format(num[i],m-i-1)
                
        else:
            if ~np.isclose(num[i],0.0):
                s=s+'{:.2e}'.format(m-i-1)
        
    print(s)
    print('---------------------')
    print(sys[1])

ステップ応答計算用関数

単位ステップ応答を計算します。

ヘビサイドの展開定理の自分なりの実装です。

def step(sys,st,et,step=1000, debug=False):

    n=len(sys[1])
    p=sys[1].r
    if debug==True:
        print('order={}'.format(n))
        print('Pole={}'.format(p))
    
    y=np.zeros(step)
    t=np.linspace(st, et, step)

    for i in range(n):
        k=sys[0](p[i])/sys[1].deriv()(p[i])/p[i]
        
        if debug==True:
            print('k{}={}'.format(i+1,k))

        y=y+(k*np.exp(p[i]*t)).real
    
    k=sys[0](0)/sys[1](0)
    if debug==True:
        print('k{}={}'.format(i+2,k))

    y=y+k
    
    return t,y    

単位ランプ応答計算用関数

単位ランプ応答を計算します。

ヘビサイドの展開定理の自分なりの実装です。

def ramp(sys,st,et,step=1000, debug=False):

    n=len(sys[1])
    p=sys[1].r
    if debug==True:
        print('order={}'.format(n))
        print('Pole={}'.format(p))
    
    y=np.zeros(step)
    t=np.linspace(st, et, step)

    for i in range(n):
        k=sys[0](p[i])/sys[1].deriv()(p[i])/p[i]/p[i]
        
        if debug==True:
            print('k{}={}'.format(i+1,k))

        y=y+(k*np.exp(p[i]*t)).real
    
    k=sys[0](0)/sys[1](0)
    if debug==True:
        print('k{}={}'.format(i+2,k))

    y=y+k*t
    
    k=(sys[0].deriv()(0)*sys[1](0)-sys[0](0)*sys[1].deriv()(0))/(sys[1](0))**2
    if debug==True:
        print('k{}={}'.format(i+3,k))
    
    y=y+k
    
    return t,y

計算サンプル

以降は計算のサンプルです

1次遅れシステムのステップ応答
sys=tf([1],[1, 1])
t,y=step(sys,0, 10, debug=True)
plt.plot(t,y)
plt.grid()
plt.show()

実行出力

order=1
Pole=[-1.]
k1=-1.0
k2=1.0

1次遅れのステップ応答のグラフ
1次遅れのステップ応答

2次振動系のステップ応答
z=0.3
omega=2*np.pi

sys=tf([0,omega**2],[1, 2*z*omega, omega**2])
t,y=step(sys,0, 10, debug=True)

plt.plot(t,y)
plt.grid()
plt.show()

実行結果

order=2
Pole=[-1.88495559+5.99377677j -1.88495559-5.99377677j]
k1=(-0.5+0.15724272550828775j)
k2=(-0.5-0.15724272550828775j)
k3=1.0

2次振動系のステップ応答のグラフ
2次振動系のステップ応答

1次遅れシステムのランプ応答
sys=tf([1],[1, 1])
t,y=ramp(sys,0, 10, debug=True)
plt.plot(t,y)
plt.grid()
plt.show()

実行結果

order=1
Pole=[-1.]
k1=1.0
k2=1.0
k3=-1.0

1次遅れシステムのランプ応答のグラフ
1次遅れシステムのランプ応答

DCモータ、PI制御器、定数(1)の伝達関数の生成

以下はDCモータを例にとって2次振動系の伝達関数を作っているのと、PI制御器の伝達要素、及び1という定数の伝達要素を作る例を示しています。

伝達要素を必ず分母分子がある形にしなければならないので定数要素の表現が不自然ですので改善事項だと思っています。

#1724DCモータの諸元
R=3.41
K=6.59e-3
L=75e-6
D=1.4e-7
J=1e-7

kp=0.1
ki=10

motor=tf([0,K],[J*L, D*L+J*R, D*R+K**2])
cont=tf([kp, ki],[1, 0])
one=tf([0,1],[0,1])
print(motor)
print(cont)
print(one)

実行結果

[poly1d([0.00659]), poly1d([7.500000e-12, 3.410105e-07, 4.390550e-05])]
[poly1d([ 0.1, 10. ]), poly1d([1., 0.])]
[poly1d([1.]), poly1d([1.])]
DCモータのステップ応答

以下は、今作ったDCモータの伝達関数でステップ応答を計算しています。

t,y=step(motor,0, 0.1, step=10000, debug=True)

plt.plot(t,y)
plt.grid()
plt.show()

実行結果

order=2
Pole=[-45338.94883714   -129.11782952]
k1=0.4286667719668313
k2=-150.52375736426163
k3=150.09509059229478

DCモータのステップ応答のグラフ
DCモータのステップ応答

PI制御閉ループ伝達関数の計算

更に、PI制御器を含めて閉ループを作って計算してみましょう。

t,y=step(motor_pi_control,0, 0.1, step=10000, debug=True)

plt.plot(t,y)
plt.grid()
plt.show()

実行結果

order=3
Pole=[-43308.73679798  -2060.88457336    -98.44529533]
k1=0.04918488593736748
k2=-1.032820727876966
k3=-0.016364158060413938
k4=1.0000000000000129

モータのPI制御ステップ応答のグラフ
モータのPI制御ステップ応答

解析的に解いた伝達関数との比較

以下はて計算で伝達関数を求めた結果と今回作った関数で算出した伝達関数両方で計算した結果の比較です。

以下は閉ループ伝達関数の分母分子の係数がどうなるのか予め式として求めて、そてをそのまま用いて、モータの伝達関数を作っています。

#閉ループ伝達関数の分子と分母

Dcom=np.poly1d([J*L, D*L+J*R, D*R+K**2+K*kp, K*ki])
No=np.poly1d([K*kp, K*ki])

mot_pi=tf([K*kp, K*ki], [J*L, D*L+J*R, D*R+K**2+K*kp, K*ki])

以下は、これまでに照会した自前の伝達関数加算,積算関数を用いて作りだしたモータの閉ループ伝達関数を求めたもののステップ応答を、上で作った伝達関数でのステップ応答をグラフ化して比較しています。

t,y1=step(motor_pi_control,0, 0.02, step=1000, debug=True)
t,y2=step(mot_pi,0, 0.02, step=1000, debug=True)

plt.plot(t,y1,linewidth=5)
plt.plot(t,y2)

plt.grid()
plt.show()

実行結果

order=3
Pole=[-43308.73679798  -2060.88457336    -98.44529533]
k1=0.04918488593736748
k2=-1.032820727876966
k3=-0.016364158060413938
k4=1.0000000000000129
order=3
Pole=[-43308.73679798  -2060.88457336    -98.44529533]
k1=0.049184885937367355
k2=-1.0328207278769626
k3=-0.016364158060403866
k4=1.0000000000000004

解析的に伝達関数を求めたものと数値的に求めた場合のステップ応答の比較のグラフ
解析的に伝達関数を求めたものと数値的に求めた場合のステップ応答の比較

DCモータのPI制御系のランプ応答

DCモータのPI制御系の目標入力としてランプ入力を入れた場合の応答です

t,y=ramp(motor_pi_control,0, 0.005, step=10000, debug=True)

plt.plot(t,y)
plt.grid()
plt.show()

実行結果

order=3
Pole=[-43308.73679798  -2060.88457336    -98.44529533]
k1=-1.1356804555809423e-06
k2=0.0005011540875350965
k3=0.0001662259024805328
k4=1.0000000000000129
k5=-0.0006662443095600539

モータのPI制御系のランプ応答のグラフ
モータのPI制御系のランプ応答

以上の計算はJupyter用ですがGistで落とせるのでご確認ください。

motor_control_lib_dev.ipynb · GitHub

おわりに

今回の作業は中々楽しかったです。 うまく計算できてほっとしました。

ですが、MatlabやPython Control Systems Libraryを使えば一瞬にしてできる事ですので、普通はやらなくても良いことです。

制御の勉強していて、最初の方の計算練習に当たる部分だと思いますが、ずーっと教えてくるとこのあたりの計算ができることが基礎として大事なのかなと思うんですよ。 線形制御は指数関数の和に帰着するので、ヘビサイドの展開定理を使えたらそれでおしまいというのは言い過ぎかもしれないけれど、すごく大事だと思っていて、学生には毎年何回も何回も部分分数展開やらせて不評を買っています。

それにしても制御工学は1年ぐらいの授業ではちっとも面白くなくて、何やっているのか抽象的過ぎて判らんという感想をよく聞きます。 制御の面白さ伝えるのが夢なんですが、もしかしたら自分が人と面白さポイントが違うのかと?!と最近いぶかっているところです。

では、また!

参考文献

あまり関係ないのもありますが、僕の制御の先生や先輩の本なので上げておきます。

最後の本はこんなに高いのかとびっくり