Pythonでシミュレータをつくろう

はじめに

これはPython Advent Calendar 2015の10日目の記事です.

数値計算を使って動力学モデルをシミュレーションをする場合はMATLABをつかうのが主流ですが,対象が簡単な微分方程式で表せられる場合や特殊なToolBox等が必要でない場合はPythonでも十分なときが多いです.今回は倒立振子を例にしてPythonでシミュレータを作成してみます.

倒立振子について

今回考える倒立振子は図のような台車に振り子が固定されており,振り子は台車の進行方向に自由に動けるようになっています.振り子と台車が固定さている部分にモータなどのアクチュエータは備えていません.つまり,振り子は台車が動くことによって前後へと運動できます.このとき台車を動かして振り子をうまく揺らし,振り子を逆立ちにしたままにすることは制御工学ではよく取り扱われる問題です.どのようなものであるかは動画を見るとわかると思います.

倒立振子

この倒立振子のモデルは

\begin{align}
\begin{bmatrix}
\ddot{\theta} \newline
\ddot{y}
\end{bmatrix} &=
\cfrac{1}{\Delta(\theta)}
\begin{bmatrix}
m + M & -mL \cos \theta \newline
-mL \cos \theta & I + mL^2
\end{bmatrix}
\begin{bmatrix}
mgL \sin \theta \newline
F + m L \dot{\theta}^2 \sin \theta – k \dot{y}
\end{bmatrix} \newline
\Delta(\theta) &= \left( I + mL^2 \right) \left( m + M \right) – m^2 L^2 \cos^2 \theta
\end{align}

となります.ここで\(\theta\)は振り子が逆立ちしたところを基準に反時計回りを正としたときの角度,\(y\)は台車の位置,\(m\),\(M\)はそれぞれ振り子と台車の重さ,\(L\)は振り子と台車の固定点から振り子の重心までの距離,\(I\)は振り子の重心まわりの慣性モーメント,\(g\)は重力加速度,\(k\)は摩擦係数,\(F\)は台車に加わる外力です.

\begin{align}
\begin{bmatrix}
\dot{x}_1 \newline
\dot{x}_3
\end{bmatrix} &=
\begin{bmatrix}
1 & 0 \newline
0 & 1
\end{bmatrix}
\begin{bmatrix}
x_2 \newline
x_4
\end{bmatrix} \newline
\begin{bmatrix}
\dot{x}_2 \newline
\dot{x}_4
\end{bmatrix}
&= \cfrac{1}{\Delta(x_1)}
\begin{bmatrix}
m + M & -mL \cos x_1 \newline
-mL \cos x_1 & I + mL^2
\end{bmatrix}
\begin{bmatrix}
mgL \sin x_1 \newline
u + m L x_2^2 \sin x_1 – k x_4
\end{bmatrix}
\end{align}

と一階微分の連立方程式へと変形できます.この変形後のモデルをPythonで表現しましょう.

微分方程式を解く

Pythonで数値計算をする場合,主にSciPyとNumPyという2つのパッケージを使います.これらの詳しい使い方は公式のドキュメントや他のブログを参照してください.

微分方程式を解く場合はSciPyのodeintという関数を利用します.これは解きたい微分方程式を定義した関数,初期値,微分方程式を解く時間の配列を引数にしています.

from scipy.integrate import odeint
...
# 初期値(振子の角度[rad],角速度[rad/s],台車の位置[m],速度[m/s])
x0 = [math.pi/16, 0.0, 0.0, 0.0]
...
if __name__ == '__main__':
...
    x = odeint(pendulum, x0, t)
...

また,このときの時間の配列はNumPyのarangeをつかうのではなくlinspaceを使って用意します.

import numpy as np
...
tend = 20.0     # シミュレーション時間[s]
tstep = 0.01    # 時間の刻み幅[s]
...
if __name__ == '__main__':
...
        t = np.linspace(0, tend, tend/tstep)
...

arangeでは

> np.arange(0, 1, 0.1)
array([ 0. ,  0.1,  0.2,  0.3,  0.4,  0.5,  0.6,  0.7,  0.8,  0.9])

ですが,linspace

> np.linspace(0, 1, 10)
array([ 0.        ,  0.11111111,  0.22222222,  0.33333333,  0.44444444,
        0.55555556,  0.66666667,  0.77777778,  0.88888889,  1.        ])

と最後の数値までの配列を返してくれるので数値計算では重宝します.

odeintの第一引数は一階微分を戻り値とした関数を渡します.倒立振子のモデルは

import math
import numpy as np
...
m = 0.5     # 振子の重さ[kg]
M = 4.0     # 台車の重さ[kg]
I = 0.1     # 重心まわりの振子の慣性モーメント [kg m^2]
L = 0.3     # 振子の重心の位置[m]
g = 9.8     # 重力加速度[m/s^2]
k = 0.05    # 摩擦係数
...
def pendulum(x, t):
    theta = x[0]
    dtheta = x[1]
    y = x[2]
    dy = x[3]

    F = 0

    delta = (I + m*L**2)*(m + M) - m**2 * L**2 * math.cos(theta)**2
    A = [[m + M, -m*L*math.cos(theta)],
         [-m*L*math.cos(theta), I + m*L**2]]
    B = [m*g*L*math.sin(theta), F + m*L*dtheta**2*math.sin(theta) - k*dy]

    ddtheta, ddy = np.dot(A, B) / delta

    return [dtheta, ddtheta, dy, ddy]

と書き表すことができます.行列演算ではNumPyの機能を使うことが多いです.MATLABと違って最も困ったところは行列の積を

ddtheta, ddy = np.dot(A, B) / delta

と書くべきところを

ddtheta, ddy = A * B / delta

と書いていたところです.Pythonでは後者は要素同士の掛け算として扱われるので全く違う結果になります.odeintの計算結果はxに入っているのであとはこれを使ってグラフを描画したりして理解しやすい形に持って行きます.

グラフの描画

数字の羅列を見てもあまりおもしろくないのでグラフに書いてみます.グラフ描画にはmatplotlibというパッケージを利用します.先ほどのシミュレーション結果を利用するときには次のような書き方になります.

import matplotlib.pyplot as plt
...
def plot(t, x, label):
    for i, v in enumerate(label):
        plt.figure()
        plt.plot(t, x[:, i], linewidth=2.0) 
        plt.xlabel('t[s]')
        plt.ylabel(v)
        plt.grid()
        plt.savefig('%d.png' % i)

    plt.show()
...
if __name__ == '__main__':
...
    label = [r'$\theta$[rad]', r'$\dot{\theta}$[rad/s]', r'$y$[m]', r'$\dot{y}$[m/s]']
    plot(t, x, label)

plt.plotでグラフを描画しています.グラフの線の太さをlinewidthで指定しています.他にも線の色やスタイルも指定できます.また,plt.ylabel()では縦軸のラベルを設定しているのですが,このとき

[r'$\theta$[rad]', r'$\dot{\theta}$[rad/s]', r'$y$[m]', r'$\dot{y}$[m/s]']

のようにr’\$\LaTeX\$’とすることでLaTeXの数式が使えるようになります.グラフはplt.savefig()で名前指定して画像として保存することができます.

アニメーション

必須ではないですがアニメーションを利用することで対象がどう動いているのか視覚的にわかります.xには各時刻における台車の位置と振り子の角度が保存されているのでそれを元に台車と振り子を描画しアニメーションを表現しましょう.アニメーションにもmatplotlibの関数を使います.まず,最初に

...
import matplotlib.pyplot as plt
...
def video(y, theta):
    fig = plt.figure(figsize=(6, 6))
    ax = fig.add_subplot(111, xlim=(-0.8, 0.8), ylim=(-0.6, 1.0))
    ax.set_xlabel('位置[m]')
    ax.grid()

と書いて描画する範囲やラベル,グリッドを書いて下準備をします.plt.figure()では描画範囲の比率を指定しています.この場合,縦横が6:6の比,600×600で描画されます.fig.add_subplot()xlimylimで描画範囲を指定してます. 台車と振り子を表現するために矩形を描画します.矩形の描画にはRectangleを使います.台車の場合は

import matplotlib.pyplot as plt
...
carwidth = 0.30         # 台車の幅
carheight = 0.20        # 台車の高さ
...
def video(y, theta):
...
    car = plt.Rectangle((y[0] - carwidth/2, 0), carwidth, carheight, fill=False)
...

となります.矩形の始点の座標を与えています.始点は矩形の左下の頂点でそこから幅と高さを指定します.fillでは塗りつぶしの指定をしています.

振り子の場合は矩形を時刻ごとに回転させなければならないので少し厄介です.振り子の場合は

import matplotlib.pyplot as plt
import matplotlib as mpl
import matplotlib.animation as animation
...
L = 0.3     # 振子の重心の位置[m]
...
carwidth = 0.30         # 台車の幅
carheight = 0.20        # 台車の高さ
pendulumwidth = 0.04    # 振子の幅
pendulumheight = 2*L    # 振子の長さ
...
def video(y, theta):
...
    pendulum = plt.Rectangle((y[0] - pendulumwidth/2, carheight/2), pendulumwidth, pendulumheight, fill=False)
    ts0 = ax.transData
    coords0 = [y[0], carheight/2]
    tr0 = mpl.transforms.Affine2D().rotate_deg_around(coords0[0], coords0[1], 180*theta[0]/math.pi) + ts0
    pendulum.set_transform(tr0)

となります.ここでは振り子の端を台車の中心点と重ねてそこを回転軸として設定しています.

アニメーション部分は

import math
import matplotlib as mpl
import matplotlib.animation as animation
...
def video(y, theta):
...
    def init():
    ...
    def anime(i):
    ...

    ani = animation.FuncAnimation(fig, anime, np.arange(1, len(y)), interval=tstep*1.0e+3, blit=True, init_func=init)
   ani.save('pydulum.mp4', fps=1/tstep, extra_args=['-vcodec', 'libx264', '-pix_fmt', 'yuv420p'])
    plt.show()

となります.animation.FuncAnimation()でアニメーションを表現します.intervalでは一回描画するときの刻み時間をミリ秒で指定しています.blitでは描画の最適化の有無,init_funcでは再描画するときに画面をクリアするときに呼ばれる関数を指定します.ani.save()ではファイル名を指定してアニメーションを動画として保存しています.fpsで一秒あたりのフレーム数を指定しています.一秒あたりのフレーム数は刻み時間に依存しているので,刻み幅の逆数を入れています.extra_argsにはffmpegに渡すコマンドライン引数をしています.

animeinit

    def init():
        ax.add_patch(car)
        ax.add_patch(pendulum)
        return car, pendulum

    def anime(i):
        ts = ax.transData
        coords = [y[i], carheight/2]
        tr = mpl.transforms.Affine2D().rotate_deg_around(coords[0], coords[1], 180*theta[i]/math.pi) + ts

       car.set_x(y[i] - carwidth/2)
        pendulum.set_xy((y[i] - pendulumwidth/2, carheight/2))
        pendulum.set_transform(tr)

        ax.add_patch(car)
        ax.add_patch(pendulum)
        return car, pendulum

としています.initでは単に矩形を描画しているだけです.animeでは引数としてanimation.FuncAnimation()で指定したnp.arrange(1, len(y))の要素が渡されます.animeで台車はx座標だけ変えればいいのでcar.set_x()で指定してます.

シミュレーション結果

ではどのような動きになるかシミュレーションしてみて確認しましょう.初期値を\(\left[ x_1(0), x_2(0), x_3(0), x_4(0) \right] = \left[ \pi/16, 0.0, 0.0, 0.0 \right]\)とし,外力を\(F = 0\)としたときのシミュレーション結果は次のグラフになります.

theta dtheta y dy

また,このときの倒立振子の様子は次の動画の様になります.

倒立振子のような動きをしていることが確認できました.

おまけ

ここで,

\begin{align}
\dot{\sigma_1} &= x_1 \newline
\dot{\sigma_3} &= x_3
\end{align}

とし,\(F = 150 x_1 + 20 x_2 + 30 x_3 + 15x_4 + 50 \sigma_1 + 10 \sigma_3\)のようにPID制御器を実装した場合の倒立振子の様子は次のようになります.

theta dtheta y dy

倒立状態を維持していることがわかります.ここではどうしてこうなるかはテーマと関係ないので割愛します.

まとめ

Pythonを使って一通りの数値計算シミュレーションが実行できました.グラフまでは簡単ですが,アニメーションでは少し苦労すると思います.また,動画への変換も結構時間がかかります.SciPyにはまだ多くの便利なライブラリがそろっているので,簡単なものならPythonでサラッと書いてMATLABで本実装というのも良さそうです.

最後に作ったシミュレータのソースを載せておきます.

コメントを残す

メールアドレスが公開されることはありません。