The Negligible Lab

Astray in the forest of electrical and electronic circuits. Adrift in the gap between time and frequency domains. 独立独歩を是とする。

Pythonによる台車形倒立振子のシミュレーション

制御工学 Python SciPy SymPy Matplotlib JupyterLab

はじめに

P計画

車輪形倒立振子を作る──この目標に対して問題を分解しながらアタックするため，前回にて床を転がる車輪のシミュレーションとアニメーションをPython (+ NumPy, SciPy, Matplotlib)で実現した経緯をまとめました。次は振り子をシミュレーションしようと思っていましたが，勢い余って（？）振り子だけでなく台車も含めた台車形倒立振子まで進むことができたので，今回はその様子について記述していきます*1。

床を転がる車輪とは異なり，台車形倒立振子では運動方程式がもろに非線形になります。本記事では非線形のままの運動方程式を表現するPythonの関数を定義し，SciPyのsolve_ivp関数にて初期値問題を解きます。また，これも前回同様にMatplotlibにて見て楽しい（！）アニメーションを生成します。運動方程式を整理する際にSymPyも用いています*2。

完成イメージ

図1に完成したアニメーションを示します。20秒間の現象をシミュレーションしております。振り子が180°，つまり真下を向いた状態から（テキトーな力を台車に加えた）振り上げ制御にて振り子を上向きに立てた後に，時刻t = 2 sにて状態フィードバック制御に入ります。

図1: 台車形倒立振子のアニメーション例（振り上げ，状態フィードバック，外乱応答，無制御）

その後，時刻t = 7.5 sから0.5秒間，振り子の先端に外乱を加えていますが，状態フィードバックのおかげで転倒することなく倒立し続けていることが分かります。最後に時刻t = 12.5 s以降で制御を止めて無制御状態に移行すると，振り子は倒立できなくなって下向きにブラブラと減衰振動している様子が見て取れます。このアニメーションを生成するJupyter NotebookをGitHubに置きましたので適宜ご参照下さい。

github.com

アニメーションの作り方については前回とほぼ同様で，舞台を作ってから台車，振り子，力を表す矢印，時刻などの文字列のアーティストの属性（プロパティ）を書き換えながらフレームを生成し，HTML5のmp4動画やGIFアニメーションを生成しています。

目次

運動方程式を立てる

台車形倒立振子の運動方程式については幾多の先賢たちが既に述べているため，ここでは特に新しいことは書いていません。例えばY-engineerさんのQiita記事を参考にさせて頂いております。

qiita.com

以下は筆者としての理解の確認を目的にしています。

考える系

図2に，今回考える台車形倒立振子の全体図を示します。水平方向をx軸，垂直方向をz軸とします。今は考えませんが奥行方向がy軸ですね。なお，図2では台車を長方形で描いていますが，本記事では力学的に質点として扱います。そのため，台車のz座標は零で，振り子を取り付けたピボットのz座標も零とします（後述のアニメーションを作る際には，台車が適当な大きさをもつものとして描きます）。

図2: 台車形倒立振子の全体図

質量Mの台車の重心*3に質量m，長さ2lの振り子が取り付けられています。振り子の重心はちょうど真ん中，ピボットからlの距離にあるとします。台車のx座標をxc，振り子の重心のx座標をxp，同z座標をzpとし，振り子の鉛直上向きからの角度をθとします。

振り子を倒立させ続けるため，台車には制御力fを加えます。また，外乱として振り子の先端には外乱力fdが加わるものとします。さらに，台車には速度に比例した摩擦力μc dxc / dt，振り子には角速度に比例した摩擦力μp dθ / dtが生じるものとします。

台車と振り子に働く力

台車と振り子にはそれぞれ系の外部からfやfdが働くだけでなく，お互いの間にも力が働きます。台車が加減速する場合には当然ながら振り子にも力が働くはずです。振り子の方も重力で台車を下向きに押すだけはなさそうです。振り子が鉛直上向きから倒れようとする場合を想像すると，振り子はピボットを介して倒れこむ方向とは反対の水平方向に台車を押しそうです。これらの力が台車と振り子のダイナミクスを決めることになります。図3に台車と振り子のそれぞれに働く力を図示しました。

図3: 台車と振り子のそれぞれに働く力

台車と振り子の間に働く力の水平成分をひとまずH，鉛直成分をVとします。このHとVは今のところ具体的に書き下せませんが，実は後から消去できます*4。

図3より，台車の並進運動，振り子の水平・鉛直方向の並進運動，振り子の回転運動に関する4つの運動方程式を記述できます*5。

\begin{align} M \ddot{x}_{c} &= f - \mu_{c} \dot{x}_{c} - H \tag{1} \\[10pt] m \ddot{x}_{p} &= H + f_{d} \tag{2} \\[10pt] m \ddot{z}_{p} & = V - mg \tag{3} \\[10pt] J \ddot{\theta} & = V l \sin \theta - H l \cos \theta + f_{d} l \cos \theta \tag{4} \end{align}

ここで，振り子が台車から離れない，つまり，振り子の重心の座標(xp, zp)に拘束条件があることを利用すると，台車と振り子の間に働く力HとVを消去できます。まず，振り子の重心のx座標xpは，台車のx座標xcと振り子の角度θでも表現できます。

x_{p} = x_{c} + l \sin \theta \tag{5}

また，z座標についても

z_{p} = l \cos \theta \tag{6}

ですね。

(1) ~ (4)式から未知数であるHとVを消去するために，(5)，(6)式を時間tで2階微分します。これによって，HとVを消去するのみならず，系をxcとその時間微分，θとその時間微分だけで表現できるようにします。まず，(5)式をtで微分します。

\dot{x}_{p} = \dot{x}_{c} + l \dot{\theta} \cos \theta \tag{7}

さらに，もう一度微分すると，

\ddot{x}_{p} = \ddot{x}_{c} + l \ddot{\theta} \cos \theta - l \dot{\theta}^{2} \sin \theta \tag{8}

となります。ここで，数学弱者たる筆者は(7)式右辺第2項につまづきそうになりました。

(l \sin \theta)' = l \dot{\theta} \cos \thetaだと…？ (l \sin \theta)' = l \cos \thetaでないの？ \dot{\theta}は何処から来た…？

しかし，今はθではなくtで微分していること，そして，θも時間tの関数θ(t)であることを思い起こせば，高校数学で出会った合成関数の微分公式

\{f(g(x))\}' = f'(g(x)) g'(x) \tag{9}

を適用していると理解できました。もう一度微分している(8)式右辺第2項と第3項はそれに加えて積の微分公式

\{f(x) g(x)\}' = f'(x) g(x) + f(x) g'(x) \tag{10}

を適用していますね*6。同様に，(6)式もtで微分していくと，

\dot{z}_{p} = -l \dot{\theta} \sin \theta \tag{11}

さらに，

\ddot{z}_{p} = -l \ddot{\theta} \sin \theta - l \dot{\theta}^{2} \cos \theta \tag{12}

を得ます。

台車の位置xcと振り子の角度θのみで系を表現する

(2)式をHについて解き，さらに\ddot{x}_{p}に(8)式を代入すると，

H = m\ddot{x}_{c} + ml \ddot{\theta} \cos \theta - ml \dot{\theta}^{2} \sin \theta - f_{d} \tag{13}

となります。同様に，(3)式をVについて解き，さらに\ddot{z}_{p}に(12)式を代入すると，

V = -ml \ddot{\theta} \sin \theta - ml \dot{\theta}^{2} \cos \theta + mg \tag{14}

を得ます。

(13)式を(1)式に代入して整理すると*7

(M + m) \ddot{x}_{c} + m l \ddot{\theta} \cos \theta = -\mu_{c} \dot{x}_{c} + m l \dot{\theta}^{2} \sin \theta + f \tag{15}

同様に，(13)，(14)式を(4)式に代入して整理*8すると，

m l \ddot{x}_{c} \cos \theta + (J + m l^{2}) \ddot{\theta} = -\mu_{p} \dot{\theta} + m g l \sin \theta + f_{d} \cos \theta \tag{16}

を得ます。以上で台車の位置xcと振り子の角度θのみで表現した台車形倒立振子の運動方程式を導出できました。

シミュレーションできる形に運動方程式を変形 ~ SymPy活用 ~

SciPyのsolve_ivp関数で解ける形

以上のように(15)，(16)式の運動方程式を導出できましたが，このままではSciPyのsolve_ivp関数で解くことができません…！ どうしてか…？ そう，高次の微分を低次の微分で表す形になっていないからです。(15)，(16)式を\ddot{x}_{c}と\ddot{\theta}の連立方程式だとみなして解き，

\begin{align} \ddot{x}_{c} &= F_{c} (x_{c}, \dot{x}_{c}, \theta, \dot{\theta}, f, f_{d}) \tag{17} \\[10pt] \ddot{\theta} &= F_{p} (x_{c}, \dot{x}_{c}, \theta, \dot{\theta}, f, f_{d}) \tag{18} \end{align}

という形，もっと言えば，状態変数を

\boldsymbol{x} = [ x_{c}, \dot{x}_{c}, \theta, \dot{\theta} ]^T \tag{19}

と定義して，

\dot{\boldsymbol{x}} = F(\boldsymbol{x}, f, f_{d}) \tag{20}

という形にまとめなければなりません。前回述べたように，(20)式を表現するPythonの関数を定義できればSciPyのsolve_ivp関数にて初期値問題を解くことができます。

SymPyで連立方程式を解く

(15)，(16)式は\ddot{x}_{c}と\ddot{\theta}の単純な連立1次方程式ですが，数学弱者である筆者は既に心が折れたのでSymPyに頼ることにしました。早速，SymPyをインポートします。

import sympy as sp
from sympy.physics.mechanics import dynamicsymbols, init_vprinting

init_vprinting()

ここで，dynamicsymbolsは時間tの関数として簡単にシンボルを定義できるようにするものです。またinit_vprinting関数を実行しておくと，時間tの関数とその時間微分について，例えば\frac{d}{dt} \theta(t)ではなく\dot{\theta}のようにJupyterLab上で表示してくれます。

早速，シンボルを定義しましょう。

# Symbols for cart
x_c = dynamicsymbols('x_c')
M, mu_c = sp.symbols('M mu_c')

# Symbols for pendulum
theta = dynamicsymbols('theta')
m, l, J, mu_p = sp.symbols('m l J mu_p')

# Symbol for external force
f = sp.symbols('f')

# Symbol for disturbance at tip of pendulum
f_d = sp.symbols('f_d') 

# Symbol for gravity acceleration
g = sp.symbols('g')

# Time
t = sp.symbols('t')

ここで，dynamicsymbolsを用いて定義したシンボルはデフォルトで時間tの関数となります。次に，(15)，(16)式をSymPyで定義します。ここではeq1，eq2と名付けました。

# Define system of equations
eq1 = sp.Eq((M + m) * x_c.diff(t, 2) + m * l * theta.diff(t, 2) * sp.cos(theta), \
    -mu_c * x_c.diff(t) + m * l * theta.diff(t)**2 * sp.sin(theta) + f)
eq2 = sp.Eq(m * l * x_c.diff(t, 2) * sp.cos(theta) + (J + m * l**2) * theta.diff(t, 2), \
    -mu_p * theta.diff(t) + m * g * l * sp.sin(theta) + f_d * sp.cos(theta))

SymPyよ，今こそ解くのだ…！

# Solve system of equations
sol = sp.solve([eq1, eq2], [x_c.diff(t, 2), theta.diff(t, 2)])

変数solに解を格納しておきます。JupyterLabで以上を実行し，解solを表示させてみると，図4のような結果を得ます。

図4: SymPyで連立方程式を解いた結果のスクリーンショット

これはまた随分と複雑になります。やはり手計算ではなかなか厄介です。解を1つずつ取り出し，とりあえずsimplifyしてみます。

A = sol[x_c.diff(t, 2)]
B = sol[theta.diff(t, 2)]

display(sp.simplify(A))
display(sp.simplify(B))

JupyterLab上ではこのdisplay関数で\mathrm{\LaTeX}ベースの数式を表示してくれます。結果として，

\begin{align} \ddot{x}_{c} &= \left ( J f + J m l \dot{\theta}^{2} \sin \theta - J \mu_{c} \dot{x}_{c} \right . \\[10pt] &+ m l^{2} f - m l f_{d} \cos^{2} \theta - m^{2} l^{2} g \sin \theta \cos \theta \\[10pt] &\left . + m^{2} l^{3} \dot{\theta}^{2} \sin \theta - m l^{2} \mu_{c} \dot{x}_{c} + m l \mu_{p} \dot{\theta} \cos \theta \right ) / D \end{align} \tag{21}

および

\begin{align} \ddot{\theta} =& \left (M f_{d} \cos \theta + M m g l \sin \theta - M \mu_{p} \dot{\theta} \right . \\[10pt] &- m l f \cos \theta + m f_{d} \cos \theta + m l^{2} g \sin \theta \\[10pt] &\left . - m^{2} l^{2} \dot{\theta}^{2} \sin \theta \cos \theta + m l \mu_{c} \cos \theta \dot{x}_{c} - m \mu_{p} \dot{\theta} \right ) / D \end{align} \tag{22}

を得ます。ただし，

D = J (M + m) + M m l^{2} + (m l \sin \theta)^{2} \tag{23}

です。これは運動方程式というよりは非線形な状態方程式と呼んだ方が良さそうですね。(22)，(23)式の各項について物理的な意味を具体的に読み解くのは筆者には難しいため，ごく一部を除いて特定の因子で括り出すなどはできませんでした。ひとまず(22)，(23)式のままPythonの関数として実装し，SciPyで求解していきます。

SciPyでの初期値問題の求解 ~自律系の場合~

以上で運動方程式を非線形な状態方程式\dot{\boldsymbol{x}} = F(\boldsymbol{x}, f, f_{d})に整理しました。これをPythonの関数として実装し，SciPyのsolve_ivp関数で解いていきます。ここではひとまず制御力fも外乱力fdもない場合を想定します。用語が適切か分かりませんが，非線形力学系で言うところの「自律系」というものに相当すると筆者は理解しました。当然ですが，振り子は倒立状態を保てないため，最終的には真下(θ = 180°)に向かってブラブラと減衰振動することになりますね。

以下では

import numpy as np
from scipy.integrate import solve_ivp
from scipy.constants import g
from IPython.display import HTML

のように各ライブラリ・モジュールをインポートしているものとします。

非線形状態方程式をPythonの関数として定義する

(22)，(23)式をsolve_ivp関数で使える形にPythonの関数として実装します。

# Differential equation
# State x = [x_c, x_c_dot, theta, theta_dot]^T
def func(t, x):
    # Extract variables
    x_c, x_c_dot, theta, theta_dot = x
    
    # External force applied to cart as control input    
    f = 0
    
    # External force applied at tip of pendulum as disturbance
    f_d = 0

    # Calculate x_ddot and theta_ddot
    denom = J * (M + m) + M * m * l**2 + (m * l * np.sin(theta))**2
    
    x_c_ddot = (J * f + J * m * l * theta_dot**2 * np.sin(theta) \
        - J * mu_c * x_c_dot + m * l**2 * f \
        - m * l * f_d * (np.cos(theta))**2 \
        - m**2 * l**2 * g * np.sin(theta) * np.cos(theta) \
        + m**2 * l**3 * theta_dot**2 * np.sin(theta) \
        - m * l**2 * mu_c * x_c_dot \
        + m * l * mu_p * theta_dot * np.cos(theta)) / denom
    
    theta_ddot = (M * f_d * np.cos(theta) \
        + M * m * g * l * np.sin(theta) - M * mu_p * theta_dot \
        - m * l * f * np.cos(theta) + m * f_d * np.cos(theta) \
        + m**2 * l * g * np.sin(theta) \
        - m**2 * l**2 * theta_dot**2 * np.sin(theta) * np.cos(theta) \
        + m * l * mu_c * x_c_dot * np.cos(theta) \
        - m * mu_p * theta_dot) / denom
    
    # Prepare empty array to store derivatives
    dxdt = np.zeros_like(x)
    
    # Differential equation
    dxdt[0] = x[1]
    dxdt[1] = x_c_ddot
    dxdt[2] = x[3]
    dxdt[3] = theta_ddot
    
    return dxdt

前回の車輪の場合に比較してかなり複雑になりました。言及し忘れていましたが，

\begin{align} \displaystyle \frac{d}{dt} x_{c} &= \dot{x}_{c} \tag{24}\\[10pt] \displaystyle \frac{d}{dt} \theta &= \dot{\theta} \tag{25} \end{align}

です。上記Pythonプログラムの36，38行目はそれぞれ(24)，(25)式に相当します。

初期値問題を解く

例として台車の質量M = 1 kg，振り子の質量m = 0.75 kg，振り子の長さ2l = 60 cm，台車と床面の摩擦に関する減衰係数μc = 0.05 N/(m/s)，振り子の摩擦に関する減衰係数μp = 0.05 N/(rad/s)とします。振り子を一様な細い棒と考えると，振り子の重心の周りの慣性モーメントJ = ml2 / 3となります。

# Parameters
M = 1.00                  # Mass of cart
m = 0.75                  # Mass of pendulum, kg
l = 0.30                  # Half length, m
J = m * l**2 / 3          # Inertia around center of mass, kg m**2
mu_c = 0.050              # Friction of cart, N/(m/s)
mu_p = 0.050              # Friction of pendulum, Nm/(rad/s)

さらに初期条件として，時刻t = 0 sでの台車の位置がxc(0) = xc0 = 0 m，同じく振り子の角度がでθ(0) = θ0 = 1°であった場合*9における初期値問題をいよいよ解いてみましょう*10。

# Solve initial value problem
t_end = 15
dt = 0.01
t = np.arange(0, t_end, dt)
x_c0 = 0
theta_0 = 1
x0 = [x_c0, 0, np.radians(theta_0), 0]

sol = solve_ivp(func, [0, t_end], x0, t_eval = t)

solve_ivp関数の使い方に関しては前回をご覧下さい。ここでは15秒間の現象をシミュレーションし，結果を0.01秒毎に評価することとしました。解を変数solに格納します。早速，解として得られた各状態変数をプロットしてみましょう。ここでは

import matplotlib.pyplot as plt
import matplotlib.patches as patches
import matplotlib.animation as animation
from IPython.display import HTML

をインポートしておくこととします。

図5: 振り子の初期角度θ0 = 1°で外力がない場合の解

図5に結果の時間波形を示します。プロットの際には分かりやすくするため，台車の速度をv_c (= \dot{x}_c)，振り子の角速度を\omega (= \dot{\theta})として記載しています*11。図5より，最初は倒立状態に近かった振り子がブラブラと下向きに減衰振動している様子が読み取れます（ただしθを−180° ~ 180°でプロットしているため，真下である180°と−180°を跨ぐ際にグラフが不連続に見えて，まるで矩形波のようになっています）。また，台車の方も振り子に振られて左右に10 cmあまり動いていることが分かりますね。

アニメーションにしてみると，図6のようになります。

図6: 振り子の初期角度θ0 = 1°で外力がない場合のアニメーション

アニメーションにすると動きのイメージが格段にクリアになりますね。

状態フィードバックの初期値応答と外乱応答

台車の初期位置xc0 = 1 mの場合の初期値応答

ここまで来れば倒立させたいというのが人情です。きちんとした制御設計をまったく経ていませんが，状態フィードバックを組んで試行錯誤しているうちに，うまく倒立するゲインを見つけることができました。まず，現在の状態\boldsymbol{x} = [x_c, \dot{x}_c, \theta, \dot{\theta} ]^Tと時刻tから制御力fを求める関数を定義します。

# Force controller
# State x = [x_c, x_c_dot, theta, theta_dot]^T
def force_control(t, x):
    # Clip angle (theta) from -180 to 180 degree
    _x = x.copy()
    np.put(_x, 2, (x[2] + np.pi) % (2 * np.pi) - np.pi)
    
    # State feedback gain and offset
    F = np.array([20, 30, 150, 30])
    f_max = 20

    f = np.clip(np.dot(F, _x), -f_max, f_max)

    return f

5，6行目で角度を−180° ~ 180°にクリップしています。9行目にて状態フィードバックゲイン\boldsymbol{F}を

\boldsymbol{F} = [20, 30, 150, 30] \tag{26}

と定義しています。この状態フィードバックゲインでは，角度θについては0°，つまり鉛直上向きに戻すような力を生じますが，台車の位置xcだけに着目すると，台車が原点x = 0から少しでも離れると，ますます離れるような不安定なゲインになっています。このような組み合わせで全体としては安定な系になっているようです。線形化してから（フィードバックありの系の）安定性や固有値を見てみたいと思っています。

次の行では制御力の最大値を20 Nに制限しています。無限の力を出せない場合という現実的なシミュレーションにするため，このような最大値をお試しで設定してみました。12行目が状態フィードバックの本体であり，状態フィードバックゲイン\boldsymbol{F}と状態\boldsymbol{x}の内積を計算して制御力fを求め，returnしています。ただし，NumPyのclip関数を使って最大値を±20 Nに制限しています。

ここで，前節で定義した台車形倒立振子の非線形状態方程式を表すfunc関数のうち，制御力fを計算する部分である8行目を書き換えます。

# Differential equation
# State x = [x_c, x_c_dot, theta, theta_dot]^T
def func(t, x):
    # Extract variables
    x_c, x_c_dot, theta, theta_dot = x
    
    # External force applied to cart as control input    
    f = force_control(t, x)
    
    # External force applied at tip of pendulum as disturbance
    f_d = 0

    # Calculate x_ddot and theta_ddot
    denom = J * (M + m) + M * m * l**2 + (m * l * np.sin(theta))**2

    （以下略）

台車の初期位置xc0 = 1 mとして初期値問題を解いてみましょう。ここでは振り子の初期角度θ0 = 0°とします。

# Solve
t_end = 15
dt = 0.01
t = np.arange(0, t_end, dt)
x_c0 = 1
theta_0 = 0
x0 = [x_c0, 0, np.radians(theta_0), 0]

sol = solve_ivp(func, [0, t_end], x0, t_eval = t)

時間波形は図7のようになります。

図7: 台車の初期位置xc0 = 1 mとして状態フィードバックを適用した場合の初期値応答
おお…倒立している…ように読み取れます…！ アニメーションでは図8のようになります。
図8: 台車の初期位置 = 1 ｍとして状態フィードバックを適用した場合の初期値応答のアニメーション
まず右方向に振り出してから左方向に戻りますね。これは，箒などを手で立てながら移動する場合の挙動と同じだと思います。

振り子の先端に1 Nの力を0.5秒間加えた場合

今度は外乱応答を試してみましょう。時刻t = 8 sから8.5 sまでの0.5秒間にわたって，振り子の先端に右向きに1 Nの力を加えた場合を考えます。倒立している振り子を指でつっつくようなイメージですね。外乱力fdを与える関数を定義します。

# Disturbance generator
# State x = [x_c, x_c_dot, theta, theta_dot]^T
def force_disturbance(t, x):
    if 8 <= t < 8.5:
        f_d = 1
    else:
        f_d = 0
    return f_d

同じように，非線形状態方程式を表現した関数からこの関数を呼び出すように，以下のように11行目を書き換えます。

# Differential equation
# State x = [x_c, x_c_dot, theta, theta_dot]^T
def func(t, x):
    # Extract variables
    x_c, x_c_dot, theta, theta_dot = x
    
    # External force applied to cart as control input    
    f = force_control(t, x)
    
    # External force applied at tip of pendulum as disturbance
    f_d = force_disturbance(t, x)

    # Calculate x_ddot and theta_ddot
    denom = J * (M + m) + M * m * l**2 + (m * l * np.sin(theta))**2

    （以下略）

外乱力があまり強いと振り子は倒れ，状態フィードバック制御の結果として台車ごとx座標のはるか遠くまで行ってしまいますが，ある程度弱い外乱であれば，状態フィードバックの効果によって倒立を維持できます。同じようにsolve_ivp関数で解いてみます。

図9: 振り子の先端に外乱を与えた場合の応答
時刻t = 8 sから8.5 sで台車が右方向に移動し，倒れそうになる振り子を元に戻そうとしています。時刻t = 8.5 s以降で外乱力が零になると，再びxc = 0 m，θ = 0°に向かって収束していく様子が見て取れます。

これもアニメーションにしてみると図10のようになります。

図10: 振り子の先端に外乱を与えた場合の応答のアニメーション
なかなか倒立振子らしくなって参りました。

振り上げ制御

振り子が真下を向いた状態から台車をぐわんぐわんと動かして，振り子を振り上げることができます。これを振り上げ制御といい，様々な方式が検討されているようです。しかし，筆者は不勉強であるためそれらについても知識がまったくありません（これから勉強するかも）。そこで，台車と振り子の動きを頭の中で想像してみると，まず，ひとつの方向に台車を押すと振り子がその逆方向に振り上がり，次に，その反対方向に台車を押してあげると，振り子が真下を経由して反対方向（最初に台車を押した方向）に振り上がって，勢いが十分であれば真上まで振り上がるのではないかと考えました。制御力fを与える関数には引数として時刻tも与えているので，時刻tの値を見ながら台車を押す方向や強さなどをトライ & エラーで探りました。その結果，次のような実装で振り上げることができました。

# Force controller
# State x = [x_c, x_c_dot, theta, theta_dot]^T
def force_control(t, x):
    # Clip angle (theta) from -180 to 180 degree
    _x = x.copy()
    np.put(_x, 2, (x[2] + np.pi) % (2 * np.pi) - np.pi)
    
    # Try swing up!
    if t < 1:
        # Standstill
        f = 0
    elif 1 <= t < 1.3:
        f = -20
    elif 1.3 <= t < 2:
        f = 12
    else:
        # Normal state feedback control
        f = np.clip(np.dot(F, _x - x_ofs), -f_max, f_max)

    return f

まず，時刻t = 1 s以前は制御力f = 0 Nとして待機します（振り上げ開始を分かり易くするため）。時刻t = 1 sから1.3 sまでは−20 Nの力を台車に加えて左方向に押します。すると振り子は右側にある程度振り上がります。時刻t = 1.3 sから2 sまでは逆に台車を右方向に12 Nの力で押します。すると振り子はほぼ鉛直上向きに立ち上がり，時刻t = 2 s以降は通常の状態フィードバック制御に移行します。

台車の初期位置xc0 = 0 m，振り子の初期角度θ0 = 180°としてSciPyのsolve_ivp関数にて初期値問題を解けば，真下を向いた振り子を振り上げ，その後は倒立を維持していることが見て取れます。時間波形は図11のようになります。

図11: 振り上げ制御の時間波形
アニメーションを生成すると，図12のようになります。
図11: 振り上げ制御のアニメーション

この振り上げ制御と状態フィードバック，外乱応答，無制御の集大成が冒頭の図1となります。

まとめ

以上で台車形倒立振子の運動方程式の導出から，一部SymPyを活用した非線形状態方程式への変形，SciPyを用いたシミュレーションについて述べました。今回はシミュレータを作るのみで，線形化や極配置法や最適レギュレータといった制御設計については触れておりません。それらは筆者が（再）勉強して理解し，この台車形倒立振子に適用できた時点で改めて記事にしようと思います。また，やはり今回もSciPyで解きましたが，Python Control System Libraryで取り扱った方が線形化などの取り回しが良いかもしれませんので，引き続き検討して参ります。

また，今回は状態変数をすべて観測できるという前提で状態フィードバック制御の例を記載しましたが，実際，加速度センサやジャイロセンサでは位置や角度を直接検知することはできません。さらに，センサを振り子に相当する機構のどの位置に取り付けるかによって，得られる信号も違ってくるはずですが，そのあたりの検討はまだ進んでいません。これに対応するにはオブザーバを構築する必要があります。台車形，あるいは当初の目標である車輪形倒立振子を実際に組み立てていく際には，この辺りの理解がクリティカルになるのではと考えています。次回は，線形化して様々なC行列について可観測性を確認し，オブザーバを作ってみたいと考えています。

または，軽い息抜きの記事も書いてみたいですね。

付録：アニメーションの作り方

アニメーションの基本的な作り方は前回と同様ですが，ご参考までにPythonの実装を書いておきます。まずは，舞台と時間波形（台車の速度，振り子の角速度，外力）をこのように準備します。アニメーション中で動かしたいアーティスト（Matplotlibにおける図形の呼び方）についてはそれぞれを描く関数の戻り値を変数に格納しています。

# Animate
# Animation setting
w_cart = 0.30          # Width of cart, m
h_cart = 0.15          # Height of cart, m
K_tscale = 3           # Size of theatre

# Plot theatre
plt.rcParams["font.size"] = 11
fig, ax = plt.subplots(4, 1, figsize = (9, 9), gridspec_kw={'height_ratios': [3, 1, 1, 1]})
fig.patch.set_facecolor('lavender')
ax[0].set_aspect('equal')
fig.suptitle(f'Inverted pendulum on a cart, Non-linear dynamics, by SciPy & Matplotlib\nCart: $M = {M}$ kg, $\mu_c = {mu_c}$ N/(m/s), $x_0 = {x_c0}$ m\nPendulum:$m = {m}$ kg, $2l = {2 * l}$ m, $\mu_p = {mu_p}$ Nm/(rad/s), $\\theta_0 = {theta_0}\degree$')

# Set axis for animation
ax[0].set_xlim(-K_tscale, K_tscale)
ax[0].set_xlabel('x [m]')
ax[0].set_ylim(-K_tscale / 5, K_tscale / 2)
ax[0].set_ylabel('z [m]')
ax[0].grid()
ax[0].set_axisbelow(True)

# Show state feedback gain
ax[0].text(-K_tscale + 0.1, -K_tscale / 20, '$x = [x_{c}, \\dot{x}_{c}, \\theta, \\dot{\\theta}]^T$', va = 'center')
ax[0].text(-K_tscale + 0.1, -2 * K_tscale / 20, f'$F = ${F}', va = 'center')

# Show time
text_time = ax[0].text(-K_tscale + 0.1, -3 * K_tscale / 20, f'$t = {t[0]:.2f} s$', va = 'center')

# Draw floor
ax[0].plot([-10, 10], [0, 0], lw = 2, color = 'black')

# Draw cart
cart = patches.Rectangle(xy = (x_c[0] - w_cart / 2, 0), width = w_cart, height = h_cart, lw = 1.5, ec = 'blue', fc = 'lightblue')
ax[0].add_patch(cart)

# Draw pendulum
pendulum, = ax[0].plot([x_c[0], x_c[0] + 2 * l * np.sin(theta[0])],[h_cart / 2, h_cart / 2 + 2 * l * np.cos(theta[0])], \
    lw = 3, color = 'blue')

# Show angle of pendulum over it
text_angle = ax[0].text(x_c[0] + 2 * l * 1.1 * np.sin(theta[0]), h_cart / 2 + 2 * l * 1.1 * np.cos(theta[0]), \
    f'${np.degrees(theta[0]):.3f}\degree$', ha = 'center', va = 'bottom')

# Show control and disturbance forces as arrows
K_fscale = K_tscale / 40
arrow_cont = ax[0].arrow(x_c[0] - w_cart / 2 * np.sign(f_cont[0]) - K_fscale * f_cont[0], \
    h_cart / 2, K_fscale * f_cont[0], 0, length_includes_head = True, width = K_tscale * 0.01, color = 'blue', alpha = 0.6)

arrow_disturb = ax[0].arrow(x_c[0] + 2 * l * np.sin(theta[0]) - K_fscale * f_disturb[0], h_cart / 2 + 2 * l * np.cos(theta[0]), \
    K_fscale * f_disturb[0], 0, length_includes_head = True, width = K_tscale * 0.01, color = 'orange', alpha = 0.6)

# Time-domain plots
# Plot speed of cart
ax[1].plot(t, v_c)
line_time_v_c, = ax[1].plot([t[0], t[0]], [np.min(v_c), np.max(v_c)], lw = 1, c = 'red') 
ax[1].set_xlabel('Time [s]')
ax[1].set_ylabel('Speed of\ncart, $x_{c}$ [m/s]')
ax[1].grid()

# Plot angular speed of pendulum
ax[2].plot(t, omega)
line_time_omega, = ax[2].plot([t[0], t[0]], [np.min(omega), np.max(omega)], lw = 1, c = 'red') 
ax[2].set_xlabel('Time [s]')
ax[2].set_ylabel('Angular speed of\npendulum, $\\omega$ [rad/s]')
ax[2].grid()

# Plot forces
ax[3].plot(t, np.array([f_cont, f_disturb]).T, label = ['Control', 'Disturbance'])
line_time_force, = ax[3].plot([t[0], t[0]], [np.min(f_cont), np.max(f_cont)], lw = 1, c = 'red') 
ax[3].set_xlabel('Time [s]')
ax[3].set_ylabel('Force, $f$, $f_{d}$ [N]')
ax[3].legend(loc = 'upper right')
ax[3].grid()

fig.tight_layout()

フレーム毎にアップデートする部分を取り扱う関数を定義します。

# Function to update frame
def animate(i):
    # Update cart position
    cart.set_x(x_c[i] - w_cart / 2)
    
    # Update pendulum attitude
    pendulum.set_data([x_c[i], x_c[i] + 2 * l * np.sin(theta[i])],[h_cart / 2, h_cart / 2 + 2 * l * np.cos(theta[i])])
    
    # Update pendulum angle text
    text_angle.set_position((x_c[i] + 2 * l * 1.1 * np.sin(theta[i]), h_cart / 2 + 2 * l * 1.1 * np.cos(theta[i])))
    text_angle.set_text(f'${np.degrees(theta[i]):.3f}\degree$')
    
    # Update time text
    text_time.set_text(f'$t = {t[i]:.2f}$ s')
    
    # Update force arrow
    kwargs = {'x': x_c[i] - w_cart / 2 * np.sign(f_cont[i]) - K_fscale * f_cont[i], 'dx': K_fscale * f_cont[i]}
    arrow_cont.set_data(**kwargs)
    kwargs = {'x': x_c[i] + 2 * l * np.sin(theta[i]) - K_fscale * f_disturb[i], 'y': h_cart / 2 + 2 * l * np.cos(theta[i]), 'dx': K_fscale * f_disturb[i]}
    arrow_disturb.set_data(**kwargs)
    
    # Update time lines
    line_time_v_c.set_data([t[i], t[i]], [np.min(v_c), np.max(v_c)])
    line_time_omega.set_data([t[i], t[i]], [np.min(omega), np.max(omega)])
    line_time_force.set_data([t[i], t[i]], [np.min(f_cont), np.max(f_cont)])
    
    return (cart, pendulum, text_angle, text_time, line_time_v_c, line_time_omega, line_time_force)

最後にアニメーションを定義して保存します。FuncAnimationでは前回同様にリアルタイム（シミュレーション内での1秒がアニメーションの表示でも1秒）となるようにしました。

# Create animation
ani = animation.FuncAnimation(fig, animate, \
    frames = np.arange(0, len(t), 5), interval = dt * 5 * 1000, blit = True)

# Show animation
HTML(ani.to_html5_video())

# Save animation GIF file
ani.save('inverted-pendulum.gif')

FuncAnimation関数を用いて定義したアニメーションaniから，HTML5のmp4動画としてもGIFアニメとしても保存できます。本ブログではGIFアニメを貼り付けています。

*1:車輪と振り子の間でトルクが分配されることから，台車形倒立振子よりも車輪形倒立振子の方が複雑で難易度が高いのではないかと筆者は思っています。

*2:SymPyにはラグランジュ方程式を取り扱う機能もあるようで，SymPyのみで運動方程式を導くこともできるようです。いずれかの時点で挑戦してみます。

*3:本記事では台車を質点として扱っています。台車が回転しないという条件下では，必ずしも重心でなくても良さそうです。「台車」と呼びながら車輪はありませんね…。

*4:この考え方がひとつのコツではないかと筆者は幼稚ながらに考えています。

*5:台車は鉛直方向には動かないものとします。

*6:本当に数学弱者はこんなところで立ち止まってしまいます。

*7:魔法のワード！

*8:さらに魔法のワード！

*9:正確にθ0 = 0°にしてしまうと計算上は倒れません。

*10:すみません。台車の速度と振り子の角速度も時刻t = 0 sでは零とします。

*11:やっぱり速度がv，角速度がωの方が分かり易いです。