はじめに
SympyはPythonの記号計算パッケージとして有名ですが、ラグランジュ法（ラグランジュの運動方程式）を用いて質点系の運動モデルを導出できる機能についてはあまり知られていないようです。
少し調べてみたところなかなか便利そうなので、この記事では覚え書きも兼ねて、最適制御や強化学習の題材としてよく使われる「倒立振子（Inverted Pendulum）」を例に、運動方程式を導出して線形化するまでの一連の方法について説明してみたいと思います。
なお、この記事ではPython3.6 + Sympy1.2を使用しています。
倒立振子とは
倒立振子は、重心が支点より高い位置にある、つまり逆立ち状態の振り子のことを指します。特に制御工学では、振り子が台車に取り付けられた「台車駆動型倒立振子（CartPole）」を安定化させる問題がよく扱われます。問題設定にはいくつかバリエーションがありますが、今回はWikipediaの記事にもあるような以下の構成を取り上げます（記号などの細部は少し異なります）。
記号の定義は以下のとおりです。
- $m_1$ : 台車の質量 [kg]
- $m_2$ : 振子の質量 [kg]
- $l$ : 振子の長さ [m]
- $g$ : 重力加速度 [m/s^2]
- $p$ : 台車の位置 [m]
- $\theta$ : 振子の角度 [rad]
- $F$ : 台車への外力 [N]
- $(x_1, y_1)$ : 台車の位置座標 [m]
- $(x_2, y_2)$ : 振子の位置座標 [m]
ラグランジュの運動方程式
ここで、ラグランジュの運動方程式の導出過程について簡単におさらいしておきましょう。
系の運動エネルギーの総和を$K$、ポテンシャルエネルギーの総和を$U$とすると、対応するラグランジアン$L$は次式で定義されます。
L=K-U
ここで、一般化座標を$\boldsymbol{q}$、対応する一般化力を$\boldsymbol{f}$とすると、ラグランジュの運動方程式は以下のように求められます。
\frac{{\rm d}}{{\rm d}t} \frac{\partial L}{\partial \dot{\boldsymbol{q}}} - \frac{\partial L}{\partial \boldsymbol{q}} = \boldsymbol{f}
Sympyを使用する場合はこれらの式を直接意識する必要はありませんが、インプットとして以下の情報が必要となるということは覚えておいてください（通常は、外力を一般化座標と対応付けて「一般化力」として定義する必要がありますが、Sympyではその必要はなさそうです）。
| 項目 | 時変/時不変 | 倒立振子の場合 |
|---|---|---|
| 一般化座標（質点の位置を表現するための変数） | 時変 | $\boldsymbol{q} = \left[p , \theta\right]^T $ |
| 質点に加わる外力 | 時変 | $F$ |
| エネルギーを定義するためのパラメータ（質量・ばね定数など） | 時不変 | $m_1, m_2, l, g$ |
導出の流れ
ここから、Sympyを使った具体的な処理の流れを説明していきます。
1. 初期化
はじめに必要な関数などをインポートします。ラグランジュの運動方程式に関連するものについてはsympy.physics.mechanicsというモジュールに実装されています。
from sympy import sin, cos, symbols, diff, Matrix
from sympy import solve, simplify
from sympy.physics.mechanics import LagrangesMethod, Lagrangian
from sympy.physics.mechanics import ReferenceFrame, Particle, Point
from sympy.physics.mechanics import dynamicsymbols, kinetic_energy
from sympy.physics.mechanics import mprint, mlatex
2. シンボルの定義
次に、シンボル（記号）を定義します。時不変のものにはsymbols、時変のものにはdynamicsymbolsを使用します。ここで、時刻$t$を定義するのを忘れないようにしてください。
一般化座標ベクトル$\boldsymbol{q}$とその時間微分$\dot{\boldsymbol{q}}$も合わせて定義しておきましょう。
t = symbols('t')
m1, m2, l, g = symbols('m1 m2 l g')
p, theta, F = dynamicsymbols('p theta F')
q = Matrix([p, theta])
qd = q.diff(t)
3. 座標系と質点の定義
座標系と質点を定義します。
N = ReferenceFrame('N')
P1 = Point('P1')
P2 = Point('P2')
3. 位置の定式化
一般化座標などを使って各質点の位置を表現しておきます。
x1 = p
y1 = 0
x2 = p - l * sin(theta)
y2 = l * cos(theta)
4. 質量と速度の設定
各質点に質量と速度を設定します。これらはモジュール内部で運動エネルギーの計算に使用されます。なお、速度はベクトルで表現する必要があります（N.xとN.yはそれぞれX軸とY軸の単位ベクトル）。
Pa1 = Particle('Pa1', P1, m1)
Pa2 = Particle('Pa2', P2, m2)
vx1 = diff(x1, t)
vy1 = diff(y1, t)
vx2 = diff(x2, t)
vy2 = diff(y2, t)
P1.set_vel(N, vx1 * N.x + vy1 * N.y)
P2.set_vel(N, vx2 * N.x + vy2 * N.y)
5. 位置エネルギーの設定
各質点に位置エネルギーを設定します。
Pa1.potential_energy = m1 * g * y1
Pa2.potential_energy = m2 * g * y2
6. 外力の定義
各質点に加わる外力をベクトルで定義します（0の場合の表記に注意）。
fl = [(P1, F*N.x), (P2, 0*N.x)]
7. 運動方程式の計算
さて、これで必要な情報が揃いました。ラグランジアンを求めて、運動方程式を計算します。
Lagrangian()関数には座標系と質点を、LagrangesMethod()関数にはラグランジアン、一般化座標、外力のリスト、座標系を与えます。
LL = Lagrangian(N, Pa1, Pa2)
LM = LagrangesMethod(LL, q, forcelist = fl, frame = N)
eom = LM.form_lagranges_equations().simplify()
f = LM.rhs().simplify()
form_lagranges_equations()メソッドは、戻り値として以下の式の左辺を返します。
\frac{{\rm d}}{{\rm d}t} \frac{\partial L}{\partial \dot{\boldsymbol{q}}} - \frac{\partial L}{\partial \boldsymbol{q}} - \boldsymbol{f} = 0
今回は一般化座標ベクトルが2次元なので、式も2個となります。
実際にmprint()で表示させてみましょう。
Matrix([
[m1*p'' + m2*(l*sin(theta)*theta'**2 - l*cos(theta)*theta'' + p'') - F],
[ l*m2*(l*theta'' - g*sin(theta) - cos(theta)*p'')]])
mlatex()を使えばLaTeX形式で出力できるので、記事に数式を載せるのも簡単です（下の式は手で右辺を追加しています）。
m_{1} \ddot{p} + m_{2} \left(l \operatorname{sin}\left(\theta\right) \dot{\theta}^{2} - l \operatorname{cos}\left(\theta\right) \ddot{\theta} + \ddot{p}\right) - F = 0 \\
l m_{2} \left(l \ddot{\theta} - g \operatorname{sin}\left(\theta\right) - \operatorname{cos}\left(\theta\right) \ddot{p}\right) = 0
また、rhs()メソッドを使うと、状態量ベクトル$\boldsymbol{x}=[\boldsymbol{q}^T, \dot{\boldsymbol{q}}^T]^T$、入力（外力）ベクトル$\boldsymbol{u}$としたときの状態方程式$\dot{\boldsymbol{x}}=f(\boldsymbol{x},\boldsymbol{u})$を計算することができます。今回の場合は$\boldsymbol{x} = [p, \theta, \dot{p}, \dot{\theta}]^T, \boldsymbol{u} = F$として、
f(\boldsymbol{x},\boldsymbol{u}) =
\left[\begin{matrix}\dot{p}\\
\dot{\theta}\\
\frac{- l m_{2} \operatorname{sin}\left(\theta\right) \dot{\theta}^{2} + g m_{2} \operatorname{sin}\left(\theta\right)\operatorname{cos}\left(\theta\right) + F}{m_{1} + m_{2} \operatorname{sin}^{2}\left(\theta\right)}\\\frac{g \left(m_{1} + m_{2}\right) \operatorname{sin}\left(\theta\right) - \left(l m_{2} \operatorname{sin}\left(\theta\right) \dot{\theta}^{2} - F\right) \operatorname{cos}\left(\theta\right)}{l \left(m_{1} + m_{2} \operatorname{sin}^{2}\left(\theta\right)\right)}\end{matrix}\right]
となります。これで数値計算もできますね。
8. 線形化
最後にLinearizerクラスを使って線形化を試してみます。
線形化を行う点をop_pointとして与える必要がありますが、今回は安定化制御に使うことを考え、倒立状態（$[p, \theta, \dot{p}, \dot{\theta}] = [0, 0, 0, 0]$）を指定します。
linearizer = LM.to_linearizer(q_ind=q, qd_ind=qd)
op_point = {p:0, theta:0, p.diff(t):0, theta.diff(t):0}
A, B = linearizer.linearize(A_and_B = True, op_point = op_point)
これらを実行すると、以下のように線形状態方程式$\dot{\boldsymbol{x}} = A\boldsymbol{x}+B\boldsymbol{u}$の行列$A,B$が計算されます。
A = \left[\begin{matrix}0 & 0 & 1 & 0\\0 & 0 & 0 & 1\\0 & \frac{g m_{2}}{m_{1}} & 0 & 0\\0 & \frac{g \left(m_{1} + m_{2}\right)}{l m_{1}} & 0 & 0\end{matrix}\right] \\
B = \left[\begin{matrix}0\\0\\\frac{1}{m_{1}}\\\frac{1}{l m_{1}}\end{matrix}\right]
おわりに
Sympyで運動方程式を導出する流れをざっと説明してみました。その強力さがおわかりいただけたでしょうか。
今回導出した運動モデルを利用した倒立振子の安定化制御などについては、また機会を改めて紹介できればと思います。