理系的な戯れ

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

Pythonで考えるDCモータの制御(6)台形加減速

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

はじめに

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

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

kouhei-no-homepage.hatenablog.com

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

kouhei-no-homepage.hatenablog.com

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

制御のブロック線図

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モータの制御(5)制御の基礎の基礎と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_1p_2p_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_1p_2p_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_1p_2p_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}\\
}

応答の計算

kouhei-no-homepage.hatenablog.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_1p_2p_3とし、入力のランプ関数が0を重解とする極を持つので、それを考慮すると

応答式は

{\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

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

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

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

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

計算結果

f:id:kouhei_ito:20200211130439p:plain
台形制御計算結果
計算結果を見てみます。

グラフは上から

  • モータから出力された角速度(出力角速度)
  • モータに加えられた電圧(制御入力)
  • 台形的な速度目標との誤差(目標角速度ー出力角速度)

比例ゲイン積分ゲインはいくつか試して、結果が分かりやすく出たものを選びました。

最高速部分は246(rad/s)ほどで2350(rpm)ぐらいです。1724T006モータは理論的には1(V)当たり 150(rad/s)ほどに回るはずなので、結果は概ね一致してます。

1型の制御系なのでランプ状の目標値については定常誤差を生じます。 最高速度部分の一定の目標値部分は1型サーボが働いて、誤差0にできています。

積分ゲインを大きくすることにより誤差を減らす事ができます。 しかし、計算上は大変速い振動的な応答が発生します。

しかし、実際にはそのような速い応答はサンプリングできないので制御できず、おそらく制御が思い通りにならない可能性があります。

本ブログは、今のところ線形制御理論に基づいた解析解を計算していますので無限に細かくサンプリングできている世界ですが、実際のサンプリング周期が無限に小さくできない制御ではどうなるのかも今後、検討していきたいと考えます。

積分ゲインを10倍に上げた場合の応答を以下にしまします。 変曲点でヒゲが出ているのは、その部分で激しい振動が起きていることを表します。

ランプ部分の定常速度偏差は0.7(rad/s)から0.07(rad/s)に10分の1に減少しています。

f:id:kouhei_ito:20200211132436p:plain
積分ゲインを10倍にした倍の応答

おわりに

1型のPI制御で台形状の目標値入力に追随する台形制御の計算をしてみました。

これまでも同じ議論を繰り返してきたような気がします。 所々でデジャブを感じながら記事を書いていました。

同じことの繰り返しが見えてきましたので、抽象度を上げる見通しが見えてきました。

今回の計算では、台形の形を任意に変えられないサンプルになっていますが、次回以降は、その辺りの改変をしたみたいですね。

Gistにソースがアップされていますので。よろしければご自分のjupyter notebookで実行してみてください。

それでは、今後も細々と続けていきたいと思います。