二輪型倒立振子の運動方程式をLagrange法で導出する

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

当ブログではさまざまなロボットを製作し，それを自律させることを目的としています．

過去に作成したロボットについてはこちらを参照してください．

ロボットを自律させるためには，準備段階として数値シミュレーションをする必要があります．

数値シミュレーションを行わずにいきなり実装する方もいますが，最悪の場合ロボットが暴走してせっかく作ったロボットが壊れてしまう可能性があります．

今回は数値シミュレーションを行うために必要な，運動方程式を導出しようと思います．

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

逆ラプラス変換のやりかた
いろいろな関数の逆ラプラス変換

この記事を読む前に
Lagrange法とは
運動方程式を求める
二輪型倒立振子の運動方程式
まとめ
続けて読む
1. 共有:
2. 関連

この記事を読む前に

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

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

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

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

Lagrange法とは

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

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

$$ ma=F $$

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

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

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

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

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

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

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

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

運動方程式を求める

今回，求める二輪型倒立振子の運動方程式はやや複雑なためLagrange法によって求めます．

Newton-Euler法でも求めることは可能ですが，Lagrange法の方が容易に求められるので，Lagrange法で求めていきます．

Lagrange法ではエネルギーを基にして運動方程式を求めることは前述しました．

エネルギーは運動エネルギー・位置エネルギー・損失エネルギーの3種類があります．

二輪型倒立振子は車輪と本体の二つで構成されています．

それぞれに対して3種類のエネルギーを求める必要があります．

運動方程式を立てるにあたって，パラメータを以下のような文字で置きます．

タイヤの回転角を$\phi$
振子の角度を$\theta$
タイヤの質量を$m_{t}$
振子の質量を$m_{p}$
タイヤの慣性モーメントを$J_{t}$
振子の慣性モーメントを$J_{p}$
振子の回転中心から重心までの距離を$L$
振子の位置を$(x, y)=(x_{p}, y_{p})$
タイヤの位置を$(x, y)=(x, 0)$
とします．

まずは二輪型倒立振子の運動エネルギーから求めていきます．

運動エネルギー

車輪の運動エネルギーから求めます．

車輪の回転の運動エネルギーは以下の式で求められます．

$$ K_{rt}=\frac{1}{2} J_{t}(\dot{\phi}+\dot{\theta})^{2} $$

同様に並進運動エネルギーは

$$ K_{tt}=\frac{1}{2} m_{t}v_{t}^{2} $$

ここで，車輪の速度$v$は$速度=半径\times角速度$で求められるので，以下の式によって求められます．

$$ v_{t}= r(\dot{\phi}+\dot{\theta}) $$

従って，並進運動エネルギーは

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

となります．

次に機体の回転運動エネルギーを求めます．

$$ K_{rp}=\frac{1}{2} J_{p}\dot{\theta}^{2} $$

並進運動エネルギーは以下のようになります．

$$ K_{tp}=\frac{1}{2} m_{p}v_{p}^{2} $$

車輪の時と同様に，速度を求めます．

このときの速度$v_{p}$は機体の重心の速度を表していて，x軸方向とy軸方向の合成速度となります．

従って，x軸方向とy軸方向それぞれの速度を求める必要があります．

まず，重心の位置は次式によって求まります．

\begin{eqnarray} x_{p}&=&x+L \sin \theta\\y_{p}&=&L \cos \theta\\ \end{eqnarray}

位置を微分すれば速度になるので

\begin{eqnarray} v_{xp}&=&\dot{x}+L\dot{\theta} \cos \theta\\ &=& r(\dot{\phi}+\dot{\theta})+L\dot{\theta} \cos \theta\\ v_{yp}&=&-L \dot{\theta} \sin \theta\\ \end{eqnarray}

これらを合成すると．

\begin{eqnarray} v_{p}^{2}&=&( v_{xp}^{2}+ v_{yp}^{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}+2r^{2}\dot{\phi}\dot{\theta}+r^{2}\dot{\theta}^{2}+2rL\dot{\theta}^{2}\cos \theta+2rL\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}+2(r^{2}+2rL\cos \theta)\dot{\phi}\dot{\theta} \\ \end{eqnarray}

となります．

これを運動エネルギーの式に代入すると

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

以上で運動エネルギーの計算は終了となります．

全てのエネルギーの総和を以下に示します．

\begin{eqnarray} K&=&K_{rt}+K_{tt}+K_{rp}+K_{tp}\\ &=&\frac{1}{2} J_{t}(\dot{\phi}+\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}+2(r^{2}+2rL\cos \theta)\dot{\phi}\dot{\theta}\} \end{eqnarray}

位置エネルギー

位置エネルギーは車輪の中心を基準とすると，車輪の位置エネルギーは

$$ U_{t} = 0 $$

機体の位置エネルギーは

$$ U_{p} = m_{p}gL\cos \theta$$

これらの総和は

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

となります．

損失エネルギー

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

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

さて，今回の二輪型倒立振子に働く非保存力というのは，車輪にかかる摩擦や機体に働く空気抵抗などが考えられます．

それらの非保存力によって生じるエネルギーをまとめて以下の式で定義します．

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

上式において$\mu_{\phi}$および$\mu_{\theta}$は$\phi$または$\theta$の抵抗係数を表しています．

Lagrange方程式

以上で機体のエネルギーの計算はすべて終わりました．

あと少しの計算だけで運動方程式の導出は完了となります．

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

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

この式において

$$ L=K-U $$

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

$q$は一般化座標，$f$は一般化力を表しています．

一般化座標というのは状態量のことで，任意に設定ができます．

今回の場合は車輪の回転角度$\phi$と機体の傾斜角$\theta$とします．

一般化力というのは一般化座標に対応した力，もしくはモーメントのことを言います．

二輪型倒立振子の場合は$\phi$に対応するモーメントに$\tau$とします．

それではLagrange方程式を解くために，それぞれの項を計算していきます．

Lagrangianはこれまでの計算結果から次式のように表されます．

$$ L =\frac{1}{2} J_{t}(\dot{\phi}+\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}+2(r^{2}+2rL\cos \theta)\dot{\phi}\dot{\theta}\}- m_{p}gL\cos \theta $$

まずは一般化座標に$\theta$をとった場合のLagrange方程式を解きます．

Lagrangianを$\dot{\theta}$で偏微分します．

\begin{eqnarray} \frac{\partial L}{\partial \dot{\theta}} &=& J_{t}(\dot{\phi}+\dot{\theta})+J_{p}\dot{\theta}+m_{t}r^2\dot{\phi}+m_{t}r^2\dot{\theta}+m_{p}( r^{2}+2rL\cos \theta +L^{2})\dot{\theta}+m_{p}( r^{2}+rL\cos \theta)\dot{\phi}\\ &=& \{J_{t}+ m_{t}r^2+m_{p}( r^{2}+rL\cos \theta)\}\dot{\phi}+\{J_{t}+J_{p}+ m_{t}r^2+ m_{p}( r^{2}+2rL\cos \theta +L^{2})\}\dot{\theta} \\&=& \{(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta +J_{t}\}\dot{\phi}+\{(m_{t}+m_{p})r^{2}+2m_{p}rL\cos \theta+m_{p}L^{2}+J_{t}+J_{p} \}\dot{\theta} \end{eqnarray}

これを時間で微分する．

$$ \frac{d}{dt}(\frac{\partial L}{\partial \dot{\theta}})= -m_{p}rL\sin \theta \cdot \dot{\phi}\dot{\theta}+\{(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta +J_{t}\}\ddot{\phi}-2m_{p}rL\sin \theta \cdot \dot{\theta}^{2} +\{(m_{t}+m_{p})r^{2}+2m_{p}rL\cos \theta+m_{p}L^{2}+J_{t}+J_{p} \}\ddot{\theta} $$

これがLagrange方程式の第一項目になる．

次にLagrangianを$\theta$で偏微分します．

$$\frac{\partial L}{\partial \theta} =-m_{p}rL\sin \theta \cdot \dot{\theta}^{2}-m_{p}rL\sin \theta \cdot \dot{\phi}\dot{\theta}+m_{p}gL\sin \theta $$

次に損失エネルギーを$\dot{\theta}$で偏微分します．

$$ \frac{\partial W}{\partial \dot{\theta}} =\mu_{\theta} \dot{\theta} $$

以上の結果をLagrange方程式に代入します．

$$ -m_{p}rL\sin \theta \cdot \dot{\phi}\dot{\theta}+\{(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta +J_{t}\}\ddot{\phi}-2m_{p}rL\sin \theta \cdot \dot{\theta}^{2} +\{(m_{t}+m_{p})r^{2}+2m_{p}rL\cos \theta+m_{p}L^{2}+J_{t}+J_{p} \}\ddot{\theta}+m_{p}rL\sin \theta \cdot \dot{\theta}^{2}+m_{p}rL\sin \theta \cdot \dot{\phi}\dot{\theta}-m_{p}gL\sin \theta +\mu_{\theta} \dot{\theta} = 0$$

このとき，一般化力$f$は0であることに注意してください．

上式を整理すると以下のようになります．

$$ \{(m_{t}+m_{p}) r^2+m_{p}rL\cos \theta +J_{t}\}\ddot{\phi}+\{(m_{t}+m_{p})r^{2}+2m_{p}rL\cos \theta+m_{p}L^{2}+J_{t}+J_{p} \}\ddot{\theta}-m_{p}rL\sin \theta \cdot \dot{\theta}^{2}-m_{p}gL\sin \theta +\mu_{\theta} \dot{\theta} = 0$$

これが一つ目の運動方程式となります．

続いて，一般化座標に$\phi$をとった場合のLagrange方程式を解きます．

先程と同様にLagrangianを$\dot{\phi}$で偏微分します．

\begin{eqnarray} \frac{\partial L}{\partial \dot{\phi}} &=& J_{t}(\dot{\phi}+\dot{\theta})+m_{t}r^2\dot{\phi}+m_{t}r^2\dot{\theta}+m_{p}r^{2}\dot{\phi}+m_{p}( r^{2}+rL\cos \theta)\dot{\theta} \\&=& \{(m_{t}+m_{p}) r^2 +J_{t}\}\dot{\phi}+\{(m_{t}+m_{p})r^{2}+m_{p}rL\cos \theta+J_{t}\}\dot{\theta} \end{eqnarray}

これを時間で微分する．

$$ \frac{d}{dt}(\frac{\partial L}{\partial \dot{\phi}})=\{(m_{t}+m_{p}) r^2 +J_{t}\}\ddot{\phi}-m_{p}rL\sin \theta \cdot \dot{\theta}^{2} +\{(m_{t}+m_{p})r^{2}+m_{p}rL\cos \theta+J_{t}\}\ddot{\theta} $$

次にLagrangianを$\phi$で偏微分します．

$$\frac{\partial L}{\partial \phi} =0 $$

Lagrangianには$\phi$の微分に関する項はありますが，$\phi$に関する項が存在しないため，0になります．

次に損失エネルギーを$\dot{\phi}$で偏微分します．

$$ \frac{\partial W}{\partial \dot{\phi}} =\mu_{\phi} \dot{\phi } $$

以上の結果をLagrange方程式に代入します．

$$ \{(m_{t}+m_{p}) r^2 +J_{t}\}\ddot{\phi}-m_{p}rL\sin \theta \cdot \dot{\theta}^{2} +\{(m_{t}+m_{p})r^{2}+m_{p}rL\cos \theta+J_{t}\}\ddot{\theta}+\mu_{\phi} \dot{\phi }=\tau$$

これが二つ目の運動方程式となります．

二輪型倒立振子の運動方程式

以上で求めた運動方程式を以下にまとめておきます．

$$ \{(m_{t}+m_{p}) r^2 +J_{t}\}\ddot{\phi}-m_{p}rL\sin \theta \cdot \dot{\theta}^{2} +\{(m_{t}+m_{p})r^{2}+m_{p}rL\cos \theta+J_{t}\}\ddot{\theta}+\mu_{\phi} \dot{\phi }=\tau$$