理系的な戯れ

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

Pythonで考えるDCモータの制御(8)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]
伝達関数演算ライブラリ
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)
        )

f:id:kouhei_ito:20200214131520p:plain

環境によっては重いです。きっと改善の手立てがあると思うのですが、調査中です。

Gistにアップ

ここまでのjupyter notebook用のソースをGistにアップしています。

Bokeh_Trapezoid_control_sample.ipynb · GitHub

おわりに

今日は、前にやった台形制御の計算をインタラクティブなものにしてみました。