アーム型倒立振子の運動方程式をラグランジュ法で導出する方法

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

倒立振子と言うものをご存知でしょうか．
倒立振子とは普通の振り子とは違い，振り子を直立させたもののことを言います．

この倒立振子は制御対象の例としてよく扱われ，制御器の有効性を確認するために利用されることがあります．

しかし，倒立振子と一言で言ってもさまざまなタイプがあります．この記事ではそのうちの一つのアーム型倒立振子の運土方程式の導出方法を解説します．

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

この記事を読む前に

この記事ではアーム型倒立振子の数値シミュレーションを行うために必要な運動方程式を導出します．

これから数値シミュレーションをやってみたいけど，どのような手順でやっていけばいいのかわからないという方は，以下の記事を参考にしてください．

数値シミュレーションを行う方法

みなさん，こんにちはおかしょです． 数値シミュレーションを行う上で必要となる手順を解説していきます． 数値シミュレーションにもさまざまな種類がありますが，私の専門が制御工学のため，そのための数値シミュレーションの方法を解説していきます． こ...

okasho-engineer.com

2020.04.13

アームが倒立振子とは

アーム型倒立振子と言うのは，モーターを土台としてその上に連結された２つの振り子で構成されます．モーターで下の振り子を操作し，一番上の振り子の直立状態を維持するように制御することが目的として扱われます．

このシステムは他の倒立振子と同様に非線形システムとなるので，制御工学としてはやや難易度は高めです．

運動方程式をラグランジュ法で求める

今回用いるラグランジュ法ではエネルギーを用いて運動方程式を求めます．エネルギーには運動・位置・損失の3種類のエネルギーがあります．

座標系の定義

そのため，各エネルギーを求める必要があるのですが，その前にアーム型倒立振子の座標系を以下に示します．

運動エネルギー

まずは運動エネルギーから求めます．運動エネルギーは並進と回転の２種類があるので順番に求めていきます．

並進運動エネルギー

一般的に並進の運動エネルギーは以下の式で求められる．

\begin{eqnarray}
W_v &=& \frac{1}{2} mv^2 \tag{1}
\end{eqnarray}

ここで，\(m\)は質量，\(v\)は速度を表します．

上のアーム型倒立振子の場合は，振り子が二つあるので並進運動エネルギーも各振り子でそれぞれ求める必要があります．そのために，まずは各振り子の重心の速度を求めます．根元にある振り子を添え字1，先端の振り子を添え字2で表すとすると，重心の位置は以下のように表現でききます．

\begin{eqnarray}
\begin{pmatrix}
x_1 & y_1
\end{pmatrix}&=&
\begin{pmatrix}
l_1 \sin \theta_1 & l_1 \cos \theta_1
\end{pmatrix} \tag{2} \\
\begin{pmatrix}
x_2 & y_2
\end{pmatrix}&=&
\begin{pmatrix}
L_1 \sin \theta_1+l_2 \sin \theta_2 & L_1 \cos \theta_1+l_2 \cos \theta_2
\end{pmatrix}\tag{3}
\end{eqnarray}

運動エネルギーを求めるには各アームの重心の速度が必要なため，上の重心の座標を時間微分します．

\begin{eqnarray}
\begin{pmatrix}
v_{x1} & v_{y1}
\end{pmatrix}&=&
\begin{pmatrix}
l_1 \dot{\theta}_1 \cos \theta_1 & -l_1 \dot{\theta}_1 \sin \theta_1
\end{pmatrix} \tag{4} \\
\begin{pmatrix}
v_{x2} & v_{y2}
\end{pmatrix}&=&
\begin{pmatrix}
L_1 \dot{\theta}_1 \cos \theta_1+l_2 \dot{\theta}_2 \cos \theta_2 & -L_1 \dot{\theta}_1 \sin \theta_1-l_2 \dot{\theta}_2 \sin \theta_2
\end{pmatrix}\tag{5}
\end{eqnarray}

以上より，各アームの運動エネルギーは以下のようになります．

\begin{eqnarray}
W_v &=& \frac{1}{2} m_1(v_{x1}^2+ v_{y1}^2)+ \frac{1}{2} m_2(v_{x2}^2+ v_{y2}^2)\\
&=& \frac{1}{2} m_1\{(l_1 \dot{\theta}_1 \cos \theta_1)^2+ (-l_1 \dot{\theta}_1 \sin \theta_1)^2\}+ \frac{1}{2} m_2\{( L_1 \dot{\theta}_1 \cos \theta_1+l_2 \dot{\theta}_2 \cos \theta_2)^2+ (-L_1 \dot{\theta}_1 \sin \theta_1-l_2 \dot{\theta}_2 \sin \theta_2)^2\}\\
&=& \frac{1}{2} m_1l_1^2 \dot{\theta}_1^2+ \frac{1}{2} m_2\{L_1^2\dot{\theta}_1^2+ l_2^2\dot{\theta}_2^2+2L_1 l_2 \dot{\theta}_1 \dot{\theta}_2 (\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)\} \tag{6}
\end{eqnarray}

回転運動エネルギー

次に回転運動エネルギーを求めます．一般的に回転運動エネルギーは次式によって求められます．

\begin{eqnarray}
W_\omega &=& \frac{1}{2} J\omega^2 \tag{7}
\end{eqnarray}

ここで，\(J\)は慣性モーメント，\omegaは角速度を表します．この回転運動エネルギーも各アームで求める必要があり，以下のようになります．

\begin{eqnarray}
W_\omega &=& \frac{1}{2} J_1 \dot{\theta}_1^2+ \frac{1}{2} J_2 \dot{\theta}_2^2 \tag{8}
\end{eqnarray}

総運動エネルギー

以上より，アーム型倒立振子の運動エネルギーは並進と回転の和によって求めることができます．

\begin{eqnarray}
W &=& W_v+W_\omega\\
&=& \frac{1}{2} m_1l_1^2 \dot{\theta}_1^2+ \frac{1}{2} m_2\{L_1^2\dot{\theta}_1^2+ l_2^2\dot{\theta}_2^2+2L_1 l_2 \dot{\theta}_1 \dot{\theta}_2 (\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)\}+ \frac{1}{2} J_1 \dot{\theta}_1^2+ \frac{1}{2} J_2 \dot{\theta}_2^2 \tag{9}
\end{eqnarray}

位置エネルギー

次に位置エネルギーを求めます．位置エネルギーは以下の式によって求めることができます．

\begin{eqnarray}
U&=& mgh \tag{10}
\end{eqnarray}

アーム型倒立振子の位置エネルギーは各アームの重心の高さ(y座標)によって以下のように求められます．

\begin{eqnarray}
U &=& m_1 gy_1+m_2 gy_2 \\
&=& m_1 gl_1 \cos \theta_1+m_2 g(L_1 \cos \theta_1 +l_2 \cos \theta_2) \tag{11}
\end{eqnarray}

損失エネルギー

最後に損失エネルギーは対象とするシステムによってさまざまなものが考えられますが，ここでは振り子が揺れることによって生じる空気抵抗による損失エネルギーを求めることにします．

空気抵抗による損失エネルギーは以下の式で求められます．

\begin{eqnarray}
D &=& \frac{1}{2} \mu \omega^2 \tag{12}
\end{eqnarray}

アーム型倒立振子の場合は振り子が2つあるので，それぞれの振り子に対する空気抵抗を求める必要があります．

\begin{eqnarray}
D &=& \frac{1}{2} \mu_1 \dot{\psi}_1^2 +\frac{1}{2} \mu_2 \dot{\psi}_2^2 \tag{13}
\end{eqnarray}

ラグランジアン

以上で，アーム型倒立振子のエネルギーをすべて求めることができました．以上の結果によってラグランジアンは以下のようになります．

\begin{eqnarray}
L &=& W-U\\
&=& \frac{1}{2} m_1l_1^2 \dot{\theta}_1^2+ \frac{1}{2} m_2\{L_1^2\dot{\theta}_1^2+ l_2^2\dot{\theta}_2^2+2L_1 l_2 \dot{\theta}_1 \dot{\theta}_2 (\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)\}+ \frac{1}{2} J_1 \dot{\theta}_1^2+ \frac{1}{2} J_2 \dot{\theta}_2^2 – m_1 gl_1 \cos \theta_1-m_2 g(L_1 \cos \theta_1 +l_2 \cos \theta_2) \tag{14}
\end{eqnarray}

運動方程式を求める準備

以上の式を用いて運動方程式を求める準備をします．

まず，一般化座標を\(q=\begin{bmatrix} \theta_1 & \theta_2 \end{bmatrix}^T\)，一般化力を\(f=\begin{bmatrix} \tau & 0\end{bmatrix}^T\)とします．

この後の計算のために，式(14)を\(\theta_1\)で微分します．

\begin{eqnarray}
\frac{\partial L}{\partial \theta_1} &=& m_2 L_1 l_2 \dot{\theta}_1 \dot{\theta}_2(\sin \theta_1 \cos \theta_2 +\cos \theta_1 \sin \theta_2)+m_1 gl_1 \sin \theta_1+m_2 gL_1 \sin \theta_1\\
&=& -m_2 L_1 l_2 \dot{\theta}_1 \dot{\theta}_2(\sin \theta_1 \cos \theta_2 -\cos \theta_1 \sin \theta_2)+m_1 gl_1 \sin \theta_1+m_2 gL_1 \sin \theta_1\\
&=& -m_2 L_1 l_2 \dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2)+m_1 gl_1 \sin \theta_1+m_2 gL_1 \sin \theta_1\tag{15}
\end{eqnarray}

\(L\)を\(\dot{\theta}_1\)で微分します．

\begin{eqnarray}
\frac{\partial L}{\partial \dot{\theta}_1} &=& m_1 l_1^2 \dot{\theta}_1+ m_2\{L_1^2\dot{\theta}_1+ L_1 l_2\dot{\theta}_2(\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)\}+J_1 \dot{\theta}_1 \tag{16}
\end{eqnarray}

さらに，上式を時間微分します．

\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}_1}\right) &=& m_1 l_1^2 \ddot{\theta}_1+ m_2\{L_1^2\ddot{\theta}_1+ L_1 l_2\ddot{\theta}_2(\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)+ L_1 l_2\dot{\theta}_2(-\dot{\theta}_1 \sin \theta_1 \cos \theta_2 -\dot{\theta}_2 \cos \theta_1 \sin \theta_2+\dot{\theta}_1 \cos \theta_1 \sin \theta_2 +\dot{\theta}_2 \sin \theta_1 \cos \theta_2) \}+J_1 \ddot{\theta}_1\\
&=& (m_1 l_1^2 +m_2 L_1^2+J_1) \ddot{\theta}_1+ m_2 L_1 l_2\ddot{\theta}_2(\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)- m_2 L_1 l_2\dot{\theta}_1 \dot{\theta}_2 (\sin \theta_1 \cos \theta_2 -\cos \theta_1 \sin \theta_2)+ m_2 L_1 l_2\dot{\theta}_2^2 (\sin \theta_1 \cos \theta_2 -\cos \theta_1 \sin \theta_2)\\
&=& (m_1 l_1^2 +m_2 L_1^2+J_1) \ddot{\theta}_1+ m_2 L_1 l_2\ddot{\theta}_2 \cos (\theta_1 -\theta_2) – m_2 L_1 l_2\dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2)+m_2 L_1 l_2\dot{\theta}_2^2 \sin (\theta_1 -\theta_2) \tag{17}
\end{eqnarray}

\(L\)を\(\theta_2\)で微分します．

\begin{eqnarray}
\frac{\partial L}{\partial \theta_2} &=& m_2 L_1 l_2 \dot{\theta}_1 \dot{\theta}_2(-\cos \theta_1 \sin \theta_2 +\sin \theta_1 \cos \theta_2)+m_2 gl_2 \sin \theta_2\\
&=& m_2 L_1 l_2 \dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2)+m_2 gl_2 \sin \theta_2 \tag{18}
\end{eqnarray}

\(L\)を\(\dot{\theta}_2\)で微分する

\begin{eqnarray}
\frac{\partial L}{\partial \dot{\theta}_2} &=& m_2\{l_2^2\dot{\theta}_2+ L_1 l_2\dot{\theta}_1(\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)\}+J_2 \dot{\theta}_2 \tag{19}
\end{eqnarray}

さらに上式を時間微分します．

\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}_2}\right) &=& m_2\{l_2^2\ddot{\theta}_2+ L_1 l_2\ddot{\theta}_1(\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)+ L_1 l_2\dot{\theta}_1(-\dot{\theta}_1 \sin \theta_1 \cos \theta_2 -\dot{\theta}_2 \cos \theta_1 \sin \theta_2+\dot{\theta}_1 \cos \theta_1 \sin \theta_2 +\dot{\theta}_2 \sin \theta_1 \cos \theta_2) \}+J_2 \ddot{\theta}_2\\
&=& (m_2 l_2^2+J_2) \ddot{\theta}_2+ m_2 L_1 l_2\ddot{\theta}_1(\cos \theta_1 \cos \theta_2 +\sin \theta_1 \sin \theta_2)- m_2 L_1 l_2\dot{\theta}_1^2 (\sin \theta_1 \cos \theta_2 -\cos \theta_1 \sin \theta_2)+ m_2 L_1 l_2\dot{\theta}_1\dot{\theta}_2 (\sin \theta_1 \cos \theta_2 -\cos \theta_1 \sin \theta_2)\\
&=& (m_2 l_2^2+J_2) \ddot{\theta}_2+ m_2 L_1 l_2\ddot{\theta}_1 \cos (\theta_1 -\theta_2) – m_2 L_1 l_2\dot{\theta}_1^2 \sin (\theta_1 -\theta_2)+ m_2 L_1 l_2\dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2) \tag{20}
\end{eqnarray}

損失エネルギーを絶対角で表すと\(\psi_1=\theta_1,\ \psi_2=\theta_2-\theta_1\)であるから以下のようになります．

\begin{eqnarray}
D &=& \frac{1}{2} \mu_1 \dot{\psi}_1^2 +\frac{1}{2} \mu_2 \dot{\psi}_2^2\\
&=& \frac{1}{2} \mu_1 \dot{\theta}_1^2 +\frac{1}{2} \mu_2 (\dot{\theta}_2-\dot{\theta}_1)^2 \tag{21}
\end{eqnarray}

\(D\)を\(\dot{\theta}_1\)で微分します．

\begin{eqnarray}
\frac{\partial D}{\partial \dot{\theta}_1} &=& \mu_1 \dot{\theta}_1-\mu_2 (\dot{\theta}_2-\dot{\theta}_1) \tag{22}
\end{eqnarray}

\(D\)を\(\dot{\theta}_2\)で微分します．

\begin{eqnarray}
\frac{\partial D}{\partial \dot{\theta}_2} &=& \mu_2 (\dot{\theta}_2-\dot{\theta}_1) \tag{23}
\end{eqnarray}

運動方程式の導出

以上の準備の下，以下のラグランジュ方程式に代入します．

\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{q}}\right)- \frac{\partial L}{\partial q}+\frac{\partial D}{\partial \dot{q}} &=& f \tag{24}
\end{eqnarray}

1行目から計算すると以下のようになります．

\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}_1}\right)- \frac{\partial L}{\partial \theta_1}+\frac{\partial D}{\partial \dot{\theta}_1} &=& \tau\\
\{(m_1 l_1^2 +m_2 L_1^2+J_1) \ddot{\theta}_1+ m_2 L_1 l_2\ddot{\theta}_2 \cos (\theta_1 -\theta_2) – m_2 L_1 l_2\dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2)+m_2 L_1 l_2\dot{\theta}_2^2 \sin (\theta_1 -\theta_2)\}-\{ -m_2 L_1 l_2 \dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2)+m_1 gl_1 \sin \theta_1+m_2 gL_1 \sin \theta_1\}+\mu_1 \dot{\theta}_1-\mu_2 (\dot{\theta}_2-\dot{\theta}_1)&=&\tau\\
(m_1 l_1^2 +m_2 L_1^2+J_1) \ddot{\theta}_1+ m_2 L_1 l_2\ddot{\theta}_2 \cos (\theta_1 -\theta_2)+m_2 L_1 l_2\dot{\theta}_2^2 \sin (\theta_1 -\theta_2)-(m_1 l_1+m_2 L_1)g \sin \theta_1+(\mu_1+\mu_2 ) \dot{\theta}_1-\mu_2 \dot{\theta}_2&=&\tau\\
(m_1 l_1^2 +m_2 L_1^2+J_1) \ddot{\theta}_1+ m_2 L_1 l_2\ddot{\theta}_2 \cos (\theta_1 -\theta_2)&=& -m_2 L_1 l_2\dot{\theta}_2^2 \sin (\theta_1 -\theta_2)+(m_1 l_1+m_2 L_1)g \sin \theta_1-(\mu_1+\mu_2 ) \dot{\theta}_1+\mu_2 \dot{\theta}_2+\tau \tag{25}
\end{eqnarray}

次に2行目を計算すると以下のようになります．

\begin{eqnarray}
\frac{d}{dt}\left(\frac{\partial L}{\partial \dot{\theta}_2}\right)- \frac{\partial L}{\partial \theta_2}+\frac{\partial D}{\partial \dot{\theta}_2} &=& 0\\
\{(m_2 l_2^2+J_2) \ddot{\theta}_2+ m_2 L_1 l_2\ddot{\theta}_1 \cos (\theta_1 -\theta_2) – m_2 L_1 l_2\dot{\theta}_1^2 \sin (\theta_1 -\theta_2)+ m_2 L_1 l_2\dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2)\}-\{ m_2 L_1 l_2 \dot{\theta}_1 \dot{\theta}_2 \sin (\theta_1 -\theta_2)+m_2 gl_2 \sin \theta_2\}+ \mu_2 (\dot{\theta}_2-\dot{\theta}_1)&=&0\\
(m_2 l_2^2+J_2) \ddot{\theta}_2+ m_2 L_1 l_2\ddot{\theta}_1 \cos (\theta_1 -\theta_2) – m_2 L_1 l_2\dot{\theta}_1^2 \sin (\theta_1 -\theta_2)-m_2 gl_2 \sin \theta_2+ \mu_2 (\dot{\theta}_2-\dot{\theta}_1)&=&0\\
(m_2 l_2^2+J_2) \ddot{\theta}_2+ m_2 L_1 l_2\ddot{\theta}_1 \cos (\theta_1 -\theta_2)&=&m_2 L_1 l_2\dot{\theta}_1^2 \sin (\theta_1 -\theta_2)+m_2 gl_2 \sin \theta_2+ \mu_2 \dot{\theta}_1-\mu_2 \dot{\theta}_2 \tag{26}
\end{eqnarray}

以上の2式がアーム型倒立振子の運動方程式である．

まとめ

この記事ではアーム型倒立振子の運動方程式をラグランジュ法で導出しました．最後に運動方程式の2式をまとめると以下のようになります．

\begin{eqnarray}
(m_1 l_1^2 +m_2 L_1^2+J_1) \ddot{\theta}_1+ m_2 L_1 l_2\ddot{\theta}_2 \cos (\theta_1 -\theta_2)&=& -m_2 L_1 l_2\dot{\theta}_2^2 \sin (\theta_1 -\theta_2)+(m_1 l_1+m_2 L_1)g \sin \theta_1-(\mu_1+\mu_2 ) \dot{\theta}_1+\mu_2 \dot{\theta}_2+\tau\\
(m_2 l_2^2+J_2) \ddot{\theta}_2+ m_2 L_1 l_2\ddot{\theta}_1 \cos (\theta_1 -\theta_2)&=&m_2 L_1 l_2\dot{\theta}_1^2 \sin (\theta_1 -\theta_2)+m_2 gl_2 \sin \theta_2+ \mu_2 \dot{\theta}_1-\mu_2 \dot{\theta}_2
\end{eqnarray}

続けて読む

この記事で求めた運動方程式を用いることで数値シミュレーションをすることができます．以下の記事では，運動方程式から数値シミュレーションをする方法について解説しているので参考にして下さい．

運動方程式から数値シミュレーションを行う方法

みなさん，こんにちはおかしょです． 数値シミュレーションを行うには運動方程式が必要です． この記事では，その運動方程式を使ってどのようにしてシミュレーションを行うのかを解説します． この記事を読むと以下のようなことがわかる・できるようになり...

okasho-engineer.com

2020.04.28

Twitterでは記事の更新情報や活動の進捗などをつぶやいているので気が向いたらフォローしてください．

YouTubeでも制御工学や電子工作などの解説をしているので，ぜひのぞいてみてください．

それでは最後まで読んでいただきありがとうございました．