Pythonで考えるDCモータの制御（６）台形加減速

台形加減速アイキャッチ画像

はじめに

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

制御工学についてもおさらいができました。

rikei-tawamure.com

前々回のお話までで、台形信号をラプラス変換の世界でどのように表現し、計算ではどのように扱えば良いのかの見通しが立ちました。

rikei-tawamure.com

いよいよ、PI制御（１型）を施されたモータに台形状に変化する速度目標値を与えた場合の応答を計算してみたいと思います。

制御のブロック線図

f:id:kouhei_ito:20200210135606p:plain — 制御のブロック線図

モータの伝達関数

${\displaystyle G(s) =\frac{K}{JL s^2 + (DL+JR)s + DR+K^2} \\ }$

PI制御器の伝達関数

${\displaystyle C(s) =\frac{K_p s + K_i}{s} \\ }$

出力、制御入力、誤差に関する閉ループの伝達関数

出力だけ見ても、誤差がどうなっているのか、制御入力は装置の定格を守っているのかなど、判らない事が多いので、それらも調べる必要があります。誤差などは出力と目標値の差を取ればいいので以下に示すことは、最も簡単な方法ではないですが、考え方としては、全て同じように扱えてシンプルです。

Pythonで考えるDCモータの制御（５）制御の基礎の基礎とPython - 理系的な戯れ

でも述べましたが、出力、制御入力、誤差についての閉ループ伝達関数を求めて、逆ラプラス変換により応答を求める方法があります。いかにそれを述べます。

出力の閉ループ伝達関数と応答式

${\displaystyle \Omega(s)=\frac{CG}{1+CG} R(s) \\ }$

モータと制御器の伝達関数を代入します。

${\displaystyle \Omega(s)=\frac{\frac{K_p s + K_i}{s} \frac{K}{JL s^2 + (DL+JR)s + DR+K^2}}{1+\frac{K_p s + K_i}{s} \frac{K}{JL s^2 + (DL+JR)s + DR+K^2}} R(s) \\ }$

整理すると

${\displaystyle \Omega(s)=\frac{K K_p s + K K_i}{JLs^3 + (DL+JR)s^2 +(DR+K^2 +KK_p)s +KK_i} R(s)\\ }$

極を $p_1$ 、 $p_2$ 、 $p_3$ とすると

${\displaystyle \Omega(s)=\frac{K K_p s + K K_i}{JL(s-p_1)(s-p_2)(s-p_3)} R(s)\\ }$

制御入力の閉ループ伝達関数と応答式

${\displaystyle U(s)=\frac{C}{1+CG} R(s) \\ }$

モータと制御器の伝達関数を代入します。

${\displaystyle U(s)=\frac{ \frac{K_p s + K_i}{s} }{1+\frac{K_p s + K_i}{s} \frac{K}{JL s^2 + (DL+JR)s + DR+K^2}} R(s) \\ }$

整理すると

${\displaystyle U(s)=\frac{(K_p s + K_i)(JL s^2 + (DL+JR)s + DR+K^2)}{JLs^3 + (DL+JR)s^2 +(DR+K^2 +KK_p)s +KK_i} R(s)\\ }$

極を $p_1$ 、 $p_2$ 、 $p_3$ とすると

${\displaystyle U(s)=\frac{(K K_p s + K K_i)(JL s^2 + (DL+JR)s + DR+K^2)}{JL(s-p_1)(s-p_2)(s-p_3)} R(s)\\ }$

誤差の閉ループ伝達関数と応答式

${\displaystyle E(s)=\frac{1}{1+CG} R(s) \\ }$

モータと制御器の伝達関数を代入します。

${\displaystyle E(s)=\frac{ 1 }{1+\frac{K_p s + K_i}{s} \frac{K}{JL s^2 + (DL+JR)s + DR+K^2}} R(s) \\ }$

整理すると

${\displaystyle E(s)=\frac{s(JL s^2 + (DL+JR)s + DR+K^2)}{JLs^3 + (DL+JR)s^2 +(DR+K^2 +KK_p)s +KK_i} R(s)\\ }$

極を $p_1$ 、 $p_2$ 、 $p_3$ とすると

${\displaystyle E(s)=\frac{s(JL s^2 + (DL+JR)s + DR+K^2)}{JL(s-p_1)(s-p_2)(s-p_3)} R(s)\\ }$

台形入力

前々回の結論より、台形入力関数は以下です。

$\displaystyle{ R(s)= \frac{a}{s^2} -\frac{a}{s^2} e^{-T_1 s}-\frac{b}{s^2} e^{-T_2 s} +\frac{b}{s^2} e^{-T_3 s}\\ }$

応答の計算

rikei-tawamure.com

上の記事で述べたように、台形上の目標値入力はランプ入力の重ね合わせで、線形制御理論の範疇では、出力についてもそれぞれのランプ応答を適切な時間遅らせて重ね合わせればいいはずです

従って、まずは、単位ランプ応答を計算します。

これより、 $\Omega(s) (\omega(t))$ 、 $U(s) (u(t))$ 、 $E(s) (e(t))$ を全て計算しなければなりませんが、計算手順的には全く同じ操作になります。

少し抽象度をあげて、これらの計算したい、それぞれの出力を全て $Y(s)$ とみなします。入力の $R(s)$ は全て共通です。

すると、上記の全ての式は、以下の形である事が分かります。

${\displaystyle Y(s)=\frac{N(s)}{D(s)} R(s)\\ }$

そして、今はとりあえず単位ランプ応答を計算したいので

${\displaystyle R(s)=\frac{1}{s^2} \\ }$

です。従って

${\displaystyle Y(s)=\frac{N(s)}{D(s) s^2}\\ }$

となります。全ての閉ループ伝達関数の極は共通である事が判っていますから、極を $p_1$ 、 $p_2$ 、 $p_3$ とし、入力のランプ関数が０を重解とする極を持つので、それを考慮すると

応答式は

${\displaystyle y(t)=K_1 e^{p_1 t} + K_2 e^{p_2 t} + K_3 e^{p_3 t} + K_{42} t + K_{41}\\ }$

となるはずです。

各係数を求めます。

以下では、ロピタルの定理を適用すべき場面には適用しています。

${\displaystyle K_i=\lim_{s \to p_i} \frac{(s-p_i) N(s)}{D(s) s^2}= \frac{N(p_i)}{D^\prime(p_i) p_i^2} \\ }$

${\displaystyle K_{42}=\lim_{s \to 0} \frac{s^2 N(s)}{D(s) s^2}= \frac{N(0)}{D(0) } \\ }$

${\displaystyle K_{41}=\lim_{s \to 0} \left\{\frac{N(s)}{D(s)}\right\}^\prime= \frac{N^\prime(0)D(0)-N(0)D^\prime(0)}{\{D(0)\}^2 } \\ }$

Pythonで計算

さて、Pythonで計算していきます

僕はJupyter notebookで試しています。

~~今回は初めてGistからJupyter notebookで作ったものを直接貼り付けてみます。~~

まずはnumpyやmatplotlibモジュールのインポート

そして、モータの各種諸元を設定します。

PIコントローラの比例ゲインと積分ゲインをここで設定しています。

import numpy as np
import matplotlib.pyplot as plt

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

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

続いて応答を計算するための閉ループ伝達関数の分母と分子の多項式をpoly1d()を使って定義します。

Dcom：共通の分母
No:角速度の分子
Nu:制御入力の分子
Ne:誤差の分子

#閉ループ伝達関数の分子と分母
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])

極を計算します！

#極の計算
p=Dcom.r

以下では応答式の係数を求める値ごとに分けて計算しています。

#角速度の応答式係数
ko1=No(p[0])/Dcom.deriv()(p[0])/p[0]/p[0]
ko2=No(p[1])/Dcom.deriv()(p[1])/p[1]/p[1]
ko3=No(p[2])/Dcom.deriv()(p[2])/p[2]/p[2]
ko42=No(0)/Dcom(0)
ko41=(No.deriv()(0)*Dcom(0)-No(0)*Dcom.deriv()(0))/(Dcom(0)**2)

#制御入力の応答式係数
ku1=Nu(p[0])/Dcom.deriv()(p[0])/p[0]/p[0]
ku2=Nu(p[1])/Dcom.deriv()(p[1])/p[1]/p[1]
ku3=Nu(p[2])/Dcom.deriv()(p[2])/p[2]/p[2]
ku42=Nu(0)/Dcom(0)
ku41=(Nu.deriv()(0)*Dcom(0)-Nu(0)*Dcom.deriv()(0))/(Dcom(0)**2)

#誤差の応答式係数
ke1=Ne(p[0])/Dcom.deriv()(p[0])/p[0]/p[0]
ke2=Ne(p[1])/Dcom.deriv()(p[1])/p[1]/p[1]
ke3=Ne(p[2])/Dcom.deriv()(p[2])/p[2]/p[2]
ke42=Ne(0)/Dcom(0)
ke41=(Ne.deriv()(0)*Dcom(0)-Ne(0)*Dcom.deriv()(0))/(Dcom(0)**2)

いよいよ、応答を計算します。

本記事では、まず最初に、単位ランプ応答を計算します。

#応答計算
starttime=0.0
endtime=1.0
points=1000000

t=np.linspace(starttime, endtime, points)

omega1=ko1*np.exp(p[0]*t) + ko2*np.exp(p[1]*t) + ko3*np.exp(p[2]*t) + ko42*t + ko41 
omega1=omega1.real

u1=ku1*np.exp(p[0]*t) + ku2*np.exp(p[1]*t) + ku3*np.exp(p[2]*t) + ku42*t + ku41 
u1=u1.real

e1=ke1*np.exp(p[0]*t) + ke2*np.exp(p[1]*t) + ke3*np.exp(p[2]*t) + ke42*t + ke41 
e1=e1.real

適切にランプ応答を遅らせる必要があります。コードが汚くなるので、遅らせる関数を作りました。

#出力を遅らせる関数
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

ランプ応答から台形入力に対する応答を合成するには４つのランプ応答を合成する必要があります。

以下で、それをしています。

また、単位ランプ応答ではなく、大きさを持ったランプ入力を計算すため、出力は適宜定数倍します。

#出力を遅らせて合成
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

あとは可視化です。

plt.figure()
plt.subplot(311)
plt.plot(t, omega)
plt.grid()
plt.ylabel('Anguler Velocity(rad/s)')

plt.subplot(312)
plt.plot(t, u)
plt.grid()
plt.ylabel('Input(V)')

plt.subplot(313)
plt.plot(t, e)
plt.grid()
plt.ylabel('Error(rad/s)')
plt.xlabel('Time(s)')
plt.show()

以上のコードはJupter notobook用ですがGistで取得可能です． Trapezoid_control.ipynb · GitHub