二輪型倒立振子の運動方程式をLagrange（ラグランジュ）法で導出する

みなさん，こんにちは．
おかしょです．

当ブログではさまざまなロボットを製作し，それを自律させることを目的としています．過去に作成したロボットについてはこちらを参照してください．

ロボットを自律させるためには，準備段階として数値シミュレーションをする必要があります．数値シミュレーションを行わずにいきなり実装する方もいますが，最悪の場合ロボットが暴走してせっかく作ったロボットが壊れてしまう可能性があります．今回は数値シミュレーションを行うために必要な，運動方程式を導出しようと思います．

この記事を読むと以下のようなことがわかる・できるようになります．

Lagrange法を使った運動方程式の求め方
二輪型倒立振子の運動方程式

この記事を読む前に
Lagrange（ラグランジュ）法とは
座標系の定義
運動方程式を求める
二輪型倒立振子の運動方程式
まとめ
続けて読む
1. 共有:
2. 関連

この記事を読む前に

この記事では数値シミュレーションを行うことを目的とした運動方程式の導出を行います．

まず，数値シミュレーションを行う方法について知りたい方は以下の記事を先に読んでおくことをおすすめします．

二輪型倒立振子の運動方程式やLagarange法による運動方程式の導出方法が知りたい方はこのまま読み進めてください．

また，運動方程式の結果のみを知りたい方は，記事の最後の方にある「二輪型倒立振子の運動方程式」にまとめてあるので目次からジャンプしてください．

Lagrange（ラグランジュ）法とは

みなさんは運動方程式をどのように導出していたか覚えているでしょうか．

中学や高校で習うような運動方程式は以下のような形をしていました．

$$ ma=F $$

上式において$m$は質量，$a$は加速度，$F$は力を表しています．

この式に合わせて，運動方程式を求めていたと思います．

このような運動方程式の求め方はNewton-Euler法と言います．

Newton-Euler法はイメージがしやすく，簡単に運動方程式を求めることができます．

しかし，多関節ロボットアームなどのような複雑なシステムは力の向きなどがわかりづらく，Newton-Euler法では求めるのが難しいです．

そのような構造が複雑なシステムに対してはLagrange法の方が運動方程式を容易に求めることができます．

Lagrange法と言うのは，Newton-Euler法が力の関係から求めるのに対して，Lagrange法はエネルギーから運動方程式を導出していきます．

「エネルギーから運動方程式なんて求められるの？」と思う方もいるかもしれませんが，エネルギーは微分すると力になるため，求められた運動方程式はどちらの方法で求めても一致します．

座標系の定義

運動方程式を求める前に座標系を定義しておきます．以下で解説する運動方程式の導出は，図に書いてある文字を使って解いていきます．

上の図において，$m$と$J$はそれぞれ質量と慣性モーメントを表します．添え字の$p$と$t$は振り子pendulumのpとタイヤtireのtを表しています．$r$はタイヤの半径，$L$は二輪型倒立振子の回転中心から振り子の重心までの距離を表します．$\theta$と$\phi$は振り子の角度とタイヤの回転角を表します．

運動方程式を求める

それでは運動方程式を求めていきます．Lagrange法ではエネルギーを基にして運動方程式を求めることは前述しました．エネルギーは運動エネルギー・位置エネルギー・損失エネルギーの3種類があります．

二輪型倒立振子は車輪と本体の二つで構成されています．それぞれに対して3種類のエネルギーを求める必要があります．

これらのエネルギーを求める前に，その準備として一般化座標と一般化力というものを定義しておきます．

一般化座標

この記事では一般化座標を以下のように定義します．

\begin{eqnarray}
q =
\begin{bmatrix}
\theta & \phi
\end{bmatrix} ^T
\end{eqnarray}

この一般化座標を基に，この後の一般化力やエネルギーを求めていきます．

一般化力

一般化エネルギーを振り子の角度$\theta $とタイヤの回転角度$\phi $としたので，振り子とタイヤに働くモーメントを求めます．振り子に対して働くモーメントはないので0として，タイヤはモーターによってトルクが加わるため，一般化力は以下のようになります．

\begin{eqnarray}
f =
\begin{bmatrix}
0 & \tau
\end{bmatrix} ^T
\end{eqnarray}

一般化座標と一般化力が定義できたら，エネルギーを求めます．

エネルギーの算出

二輪型倒立振子が有する各種エネルギーを算出します．

運動エネルギー

二輪型倒立振子がもつ運動エネルギーはタイヤのもつ運動エネルギー$K_t$と振り子の持つ運動エネルギー$K_p$の二つに分けられます．さらに各運動エネルギーは回転$K_r$と並進$K_t$の二つに分けることができます．

まずはタイヤの持つ回転運動エネルギーを求めると以下のようになります．

\begin{eqnarray}
K_rt&=&\frac{1}{2} J_t(\dot{\phi}+\dot{\theta})^2\\
&=&\frac{1}{2} J_t(\dot{\phi}^2+2\dot{\phi}\dot{\theta}+\dot{\theta}^2)
\end{eqnarray}

タイヤの回転角度は座標系の定義より，上のように$\phi+\theta$となります．次にタイヤの並進運動エネルギーを求めると

\begin{eqnarray}
K_tt&=&\frac{1}{2} m_t v_t^2
\end{eqnarray}

ここで，$v_t$はタイヤの並進方向速度でタイヤの半径$r$とタイヤの回転角度$\phi+\theta$によって以下のように求めることができます．

\begin{eqnarray}
v_t&=&r(\dot{\phi}+\dot{\theta})
\end{eqnarray}

したがって，タイヤの並進運動エネルギーは以下のようになります．

\begin{eqnarray}
K_tt&=&\frac{1}{2} m_t \{r(\dot{\phi}+\dot{\theta})\}^2\\
&=&\frac{1}{2} m_t r^2 (\dot{\phi}+\dot{\theta})^2\\
&=&\frac{1}{2} m_t r^2 (\dot{\phi}^2+2\dot{\phi}\dot{\theta}+\dot{\theta}^2)\\
\end{eqnarray}

次に振り子の運動エネルギーを求めます．振り子の持つ回転運動エネルギーは以下のようになります．

\begin{eqnarray}
K_rp&=&\frac{1}{2} J_p \dot{\theta}^2
\end{eqnarray}

並進運動エネルギーは

\begin{eqnarray}
K_tp&=&\frac{1}{2} m_p v_p^2
\end{eqnarray}

となります．ここで，$v_p$は振り子の重心の並進速度を表しています．振り子の重心は振り子が傾くことによってx軸方向やy軸方向に動き，$v_p$はこのx軸方向とy軸方向の合成速度を表します．

この合成速度$v_p$を求めるために，まずは振り子が角度$\theta$で傾いた時の重心の位置$(x_p,\ y_p)$を求めます．

\begin{eqnarray}
\begin{array}{l}
x_p=x_t+L\sin \theta\\
y_p=L\cos \theta
\end{array}
\end{eqnarray}

上式で$x$座標の算出の時のみ$x_t$を足しているのは，車輪の位置が$x$軸上を動くためです．一方，$y$座標には車輪は動くことはないため$y_t$は加えていません．

続いて，上式で求められた振り子の重心位置を微分します．

\begin{eqnarray}
\begin{array}{l}
\dot{x}_p=\dot{x}_t+L\dot{\theta}\cos \theta\\
\dot{y}_p=-L\dot{\theta} \sin \theta
\end{array}
\end{eqnarray}

ここで，上の式の右辺第1項の$\dot{x}_t$は

\begin{eqnarray}
\dot{x}_t&=&v_t\\
&=&r(\dot{\phi}+\dot{\theta})
\end{eqnarray}

であるので

\begin{eqnarray}
\dot{x}_p=r(\dot{\phi}+\dot{\theta})+L\dot{\theta}\cos \theta\\
\end{eqnarray}

以上より，合成速度$v_p$は

\begin{eqnarray}
v_p^2&=& \dot{x}_p^2+\dot{y}_p^2\\
&=& \{r(\dot{\phi}+\dot{\theta})+L\dot{\theta}\cos \theta\}^2 + \{-L\dot{\theta} \sin \theta\}^2\\
&=& r^2(\dot{\phi}+\dot{\theta})^2+2rL\dot{\theta}(\dot{\phi}+\dot{\theta})\cos \theta+L^2\dot{\theta}^2\cos^2 \theta + L^2\dot{\theta}^2 \sin^2 \theta\\
&=& r^{2} (\dot{\phi}^{2} +2\dot{\phi}\dot{\theta}+\dot{\theta}^{2}) +2rL\dot{\theta}(\dot{\phi}+\dot{\theta}) \cos \theta+L^{2}\dot{\theta}^{2} \\
&=& r^{2}\dot{\phi}^{2}+(r^{2}+2rL\cos \theta +L^{2}) \dot{\theta}^{2} +(2r^{2}+2rL\cos \theta)\dot{\phi}\dot{\theta}
\end{eqnarray}

となります．上式を振り子の運動エネルギーの式に代入すると以下のようになります．

\begin{eqnarray}
K_{tp}=\frac{1}{2} m_{p}\{ r^{2}\dot{\phi}^{2}+(r^{2}+2rL\cos \theta +L^{2})\dot{\theta}^{2}+(2r^{2}+2rL\cos \theta)\dot{\phi}\dot{\theta}\}
\end{eqnarray}

最後に，タイヤと振り子の運動エネルギーの総和を求めます．

\begin{eqnarray}
K&=&K_t+K_p\\
&=&(K_{rt}+K_{tt})+(K_{rp}+K_{tp})\\
&=&\frac{1}{2} J_{t}(\dot{\phi}^{2}+2\dot{\phi}\dot{\theta}+\dot{\theta}^{2}) +\frac{1}{2} m_{t}r^{2}(\dot{\phi}^{2}+2\dot{\phi}\dot{\theta}+\dot{\theta}^{2}) +\frac{1}{2} J_{p}\dot{\theta}^{2}+\frac{1}{2} m_{p}\{ r^{2}\dot{\phi}^{2}+(r^{2}+2rL\cos \theta +L^{2})\dot{\theta}^{2}+(2r^{2}+2rL\cos \theta)\dot{\phi}\dot{\theta}\}\\
&=& \left(\frac{1}{2} J_{t}+\frac{1}{2} m_{t}r^{2}+\frac{1}{2} m_{p} r^{2}\right) \dot{\phi}^{2}+\{J_t+m_t r^2+m_p(r^{2}+rL\cos \theta)\} \dot{\phi}\dot{\theta} +\left\{\frac{1}{2} J_{t}+\frac{1}{2} m_{t}r^{2}+\frac{1}{2} J_{p}+\frac{1}{2} m_{p}(r^2+2rL \cos \theta +L^2)\right\}\dot{\theta}^2\\
&=&\frac{1}{2}\{J_t+(m_t+m_p)r^2\}\dot{\phi}^2 +\{J_t+(m_t+m_p) r^2 +m_p rL\cos \theta\}\dot{\phi}\dot{\theta}+\frac{1}{2}\{J_t+(m_t+m_p)r^2+J_p+m_{p}(2rL \cos \theta +L^2)\}\dot{\theta}^2
\end{eqnarray}

位置エネルギー

次に位置エネルギーを求めます．運動エネルギーと同様に，車輪と振り子の位置エネルギーをそれぞれ求めていきます．

車輪の位置エネルギー$U_t$は座標系の定義にあるように，タイヤの中心を基準としているため位置エネルギーは0となります．

$$ U_{t} = 0 $$

続いて，振り子の位置エネルギー$U_p$を求めると以下のようになります．

\begin{eqnarray}
U_{p} &=&m_{p}gy_p\\
&=& m_{p}gL\cos \theta
\end{eqnarray}

したがって，総位置エネルギー$U$は

\begin{eqnarray}
U&=&U_{t}+U_{p}\\
&=& m_{p}gL\cos \theta\\
\end{eqnarray}

となります．

損失エネルギー

損失エネルギーというのは，非保存力によるエネルギーのことを言います．

非保存力とは何かというと，簡単に説明すると経路によって力の大きさが定義されるもののことを言います．

さて，今回の二輪型倒立振子に働く非保存力というのは，車輪にかかる摩擦や機体に働く空気抵抗などが考えられます．したがって，損失エネルギー$W$は以下のように求めることができます．

$$ W=\frac{1}{2} \mu_{\phi} \dot{\phi}^{2}+\frac{1}{2} \mu_{\theta} \dot{\theta }^{2} $$

上式において$\mu_{\phi}$および$\mu_{\theta}$はタイヤと振り子にかかる空気の摩擦係数を表しています．

Lagrange方程式

以上で機体のエネルギーの計算はすべて終わりました．あと少しの計算だけで運動方程式の導出は完了となります．

では，このエネルギーの式からどのようにて運動方程式を求めるのかというと，それぞれの運動方程式をLagrange方程式と呼ばれる以下の式に代入します．

$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}}\right)-\frac{\partial L}{\partial q}+\frac{\partial W}{\partial \dot{q}} = f$$

この式において

$$ L=K-U $$

を表しており，これはLagrangianと呼ばれます．

ラグランジュ方程式を解くには，一般化座標の要素ごと（$\phi$と$\theta$）に求める必要があります．

運動方程式

振り子の角度$\theta$

まずは振り子の角度$\theta$に関して，ラグランジュ方程式を解きます．

$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right)-\frac{\partial L}{\partial \theta}+\frac{\partial W}{\partial \dot{\theta}} = f$$

左辺第1項：

\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}}\right)&=&\frac{d}{dt}[\{J_t +(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta \}\dot{\phi}+\{J_{t}+(m_{t}+m_{p}) r^{2} +J_{p} +m_{p}(2rL\cos \theta+L^{2}) \}\dot{\theta}]\\
&=&\{J_t +(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta \}\ddot{\phi}-m_p rL \sin \theta \cdot \dot{\phi} \dot{\theta}+\{J_{t}+(m_{t}+m_{p}) r^{2} +J_{p} +m_{p}(2rL\cos \theta+L^{2}) \}\ddot{\theta}-2m_p rL \sin \theta \cdot \dot{\theta}^2
\end{eqnarray}

左辺第2項：

\begin{eqnarray}
\frac{\partial L}{\partial \theta}&=&-m_p rL \sin \theta \cdot \dot{\phi} \dot{\theta}-m_p rL \sin \theta \dot{\theta}^2+m_p gL \sin \theta
\end{eqnarray}

左辺第3項：

\begin{eqnarray}
\frac{\partial W}{\partial \dot{\theta}}&=&\mu_{\theta} \dot{\theta}
\end{eqnarray}

以上より

\begin{eqnarray}
\{J_t +(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta \}\ddot{\phi}-m_p rL \sin \theta \cdot \dot{\phi} \dot{\theta}+\{J_{t}+(m_{t}+m_{p}) r^{2} +J_{p} +m_{p}(2rL\cos \theta+L^{2}) \}\ddot{\theta}-2m_p rL \sin \theta \cdot \dot{\theta}^2-m_p rL \sin \theta \cdot \dot{\phi} \dot{\theta}-m_p rL \sin \theta \dot{\theta}^2+m_p gL \sin \theta &=& 0\\
\{J_t +(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta \}\ddot{\phi} +\{J_{t}+(m_{t}+m_{p}) r^{2} +J_{p} +m_{p}(2rL\cos \theta+L^{2}) \}\ddot{\theta} -m_p rL \sin \theta \cdot \dot{\theta}^2 – m_p gL \sin \theta +\mu_{\theta} \dot{\theta} &=&0\\
\end{eqnarray}

タイヤの回転角度$\phi$

次にタイヤの回転角度$\phi$についてラグランジュ方程式を解く．

$$ \frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}}\right)-\frac{\partial L}{\partial \phi}+\frac{\partial W}{\partial \dot{\phi}} = f$$

左辺第1項：

\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\phi}}\right)&=&\frac{d}{dt}[\{J_t +(m_{t}+m_{p}) r^2\}\dot{\phi}+\{J_{t}+(m_{t}+m_{p}) r^{2} +m_{p}rL\cos \theta \}\dot{\theta}]\\
&=&\{J_t +(m_{t}+m_{p}) r^2\}\ddot{\phi}+\{J_{t}+(m_{t}+m_{p}) r^{2} +m_{p} rL\cos \theta\}\ddot{\theta}-m_p rL \sin \theta \cdot \dot{\theta}^2
\end{eqnarray}

左辺第2項：

\begin{eqnarray}
\frac{\partial L}{\partial \phi} &=& 0
\end{eqnarray}

左辺第3項：

\begin{eqnarray}
\frac{\partial W}{\partial \dot{\phi}}&=&\mu_{\phi} \dot{\phi}
\end{eqnarray}

以上より

\begin{eqnarray}
\{J_t +(m_{t}+m_{p}) r^2\}\ddot{\phi}+\{J_{t}+(m_{t}+m_{p}) r^{2} +m_{p} rL\cos \theta\}\ddot{\theta}-m_p rL \sin \theta \cdot \dot{\theta}^2 +\mu_{\phi} \dot{\phi} &=&\tau\\
\end{eqnarray}