Pythonで考えるDCモータの制御（８）Interact＆Bokehによるインタラクティブな台形制御の計算

はじめに

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

先日、モータの台形加減速制御について考えてみました。

kouhei-no-homepage.hatenablog.com

比例ゲインや積分ゲインをインタラクティブに変えて、応答の変化を見たいと思い、 jupyter notebookでintractを使用して改良してみます。

ipywidgets.readthedocs.io

グラフも拡大縮小が簡単にできるのでBokehを使ってみました。

bokeh.org

必要なモジュールのインポート

from ipywidgets import interact
import numpy as np
from bokeh.io import push_notebook, show, output_notebook
from bokeh.plotting import figure
from bokeh.layouts import column
output_notebook()

自作制御用ライブラリ

伝達関数の定義

tf(num,den) - num:分子の係数リスト - den:分母の係数リスト

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

    zz=z
    pp=p
    
    #分母分子から共通の極を見つけ出して削除する
    for x in z:
        pp=pp[~np.isclose(pp,x)]

    for x in p:
        zz=zz[~np.isclose(zz,x)]

    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:伝達関数

def tf_add(sys1, sys2):
    num=sys1[0]*sys2[1]+sys1[1]*sys2[0]
    den=sys1[1]*sys2[1]
    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)

ステップ応答とランプ応答

step(sys,st,et,step,debug)
ramp(sys,st,et,step,debug)
sys:伝達関数
st:初期時間
et:最終時間
step:計算ポイント数(デフォルト1000)
debug:デバグスイッチ（デフォルトFalse）

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

出力シフト関数

#出力を遅らせる関数
def shift(x, s):
    l=len(x)
    x=np.roll(x, s)
    dummy_ones =np.ones(l-s)
    dummy_zeros=np.zeros(s)
    dummy=np.concatenate([dummy_zeros,dummy_ones],0)
    return x*dummy

台形制御計算関数

def simulation(Kp=0.1, Ki=10.0):
    #閉ループ伝達関数の分子と分母

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

    #コントローラのゲイン
    #Kp=0.1
    #Ki=10.0

    Dcom=np.poly1d([J*L, D*L+J*R, D*R+K**2+K*Kp, K*Ki])
    No=np.poly1d([K*Kp, K*Ki])
    Nu=np.poly1d([Kp, Ki])*np.poly1d([J*L, D*L+J*R, D*R+K**2])
    Ne=np.poly1d([J*L, D*L+J*R, D*R+K**2, 0])
    
    sys_omega=tf(No, Dcom)
    sys_u=tf(Nu, Dcom)
    sys_e=tf(Ne, Dcom)
    
    #応答計算
    starttime=0.0
    endtime=1.0
    points=10000

    t,omega1=ramp(sys_omega, starttime, endtime, step=points)
    t,u1=ramp(sys_u, starttime, endtime, step=points)
    t,e1=ramp(sys_e, starttime, endtime, step=points)

    #出力を遅らせて合成
    omega2=shift(omega1,int(points/4))
    omega3=shift(omega1,int(2*points/4))
    omega4=shift(omega1,int(3*points/4))
    omega=omega1-omega2-omega3+omega4
    omega=1000*omega

    u2=shift(u1,int(points/4))
    u3=shift(u1,int(2*points/4))
    u4=shift(u1,int(3*points/4))
    u=u1-u2-u3+u4
    u=1000*u

    e2=shift(e1,int(points/4))
    e3=shift(e1,int(2*points/4))
    e4=shift(e1,int(3*points/4))
    e=e1-e2-e3+e4
    e=1000*e    
    
    p1 = figure(plot_height=200, plot_width=800,
           background_fill_color='#efefef',y_axis_label='Omega(rad/s)')
    p1.line(t, omega, color="#ff0000", line_width=1.5, alpha=0.8)
    
    p2 = figure(plot_height=200, plot_width=800,
           background_fill_color='#efefef',y_axis_label='Input(V)')
    p2.line(t, u, color="#008800", line_width=1.5, alpha=0.8)
    
    p3 = figure(plot_height=230, plot_width=800,
           background_fill_color='#efefef', x_axis_label='Error(rad/s)')
    p3.line(t, e, color="#000088", line_width=1.5, alpha=0.8)


    show(column(p1, p2, p3))

Bokehを用いた、インタラクティブなグラフ表示

interact(simulation, 
         Kp=FloatSlider(0.1,min=0.01, max=1.0, step=0.01, continuous_update=False), 
         Ki=FloatSlider(10,min=1, max=200, step=1, continuous_update=False)
        )