台車型倒立振子の運動方程式を優しく導出してみた

　はじめまして、T-semiというサークルで学ロボに挑戦しているりょーつといいます。自分は高専からの編入生で、高専の頃は制御系の学科にいたにもかかわらず、ずっと機械設計や加工を担当していました。高専を卒業する少し前くらいから制御のおもしろさに気づき、たまに倒立振子とかで遊んでいます。

目次

1.はじめに
2.時間の無い方へ
3.式の導出
4.おわりに

1. はじめに

　ちょっと前に倒立振子というものを作ったので、それについて書こうと思います。倒立振子は支点より上に重心がある振子です。普通なら倒れてしまいますが、振子を固定している土台をいい感じに制御することで倒れにくくすることができます。難しそうに見えますが、機械系の大学生や高専生であれば、講義にでてくる極配置法や最適レギュレータなどを使えば案外簡単に制御できます。基本的に現代制御の講義の内容を使えば作れますが、倒立振子を自力で作るためには2つ重要な知識が足りません。それは倒立振子の状態方程式（運動方程式）の導出方法とパラメータ同定の方法です。講義ではゲインの計算方法などを中心に学びますが、ゲイン計算ができるようになったところで、これらが分からなかったら計算のしようがありません。
　倒立振子の状態方程式の導出についてまとめたサイトや記事はググればいくらでも出てきますが、その多くがラグランジュの運動方程式を使った複雑なものであり、倒立振子を作ろうと思った人のやる気をへし折るには十分な内容となっています。また、導出課程をすっとばしたところで、土台部分に与える電圧と振子の挙動の関連性がいまいち明らかにされていないものも多く、即効性のある記事は少ないように感じました。
　そこで本記事では、倒立振子を作るのに必要な、最低限の状態方程式を厳密性を放棄して優しく導出することを目標にします。倒立振子にもいろいろありますが、説明がラクそうな台車型倒立振子を対象としました。

2. 時間の無い方へ

　結論を先に述べておくと、台車型倒立振子の運動方程式と状態方程式は以下の式で表されます。

\ddot{x} = -a\dot{x} + bu
\tag{1}

J\ddot{\theta} = -ml\ddot{x} + mgl\theta
\tag{2}

\begin{pmatrix}
\dot{x} \\
\ddot{x}\\
\dot{\theta}\\
\ddot{\theta}
\end{pmatrix}
=
\begin{pmatrix}
0 & 1 & 0 & 0 \\
0 & -a & 0 & 0\\
0 & 0 & 0 & 1\\
0 & \frac{mla}{J} & \frac{mgl}{J} & 0
\end{pmatrix}
\begin{pmatrix}
x \\
\dot{x}\\
\theta\\
\dot{\theta}
\end{pmatrix}
+
\begin{pmatrix}
0 \\
b\\
0\\
-\frac{mlb}{J}
\end{pmatrix}
u
\tag{3}

ただし
$x$：台車の位置 [m]
$\theta$：振子の角度 [rad]
$a$:台車の定数 [/s]
$b$:台車の定数 [m/V$s^2$]
$m$:振子の質量 [kg]
$l$:振子の回転軸から重心までの距離 [m]
$J$:振子の回転軸まわりの慣性モーメント [kg$m^2$]
$g$:重力加速度 [m/$s^2$]
$u$：駆動系の電源電圧 [V]
です。

3. 式の導出

3.1 台車の運動方程式

　この節ではモータに与える電圧$u$ [V]によって、台車がどのように動くかについて考えていきます。最終的なゴールは(1)式の導出です。この問題の難しいところは力学の授業で電圧なんて出てこないところです。そのためまずは、電圧を力に変換する方法を考えます。モータから出力されるトルクは電磁力に起因します。電磁力は流れる電流に比例するため、モータに流れる電流を$i$ [A]、モータの出力トルクを$\tau$ [Nm]、比例係数を$k_\tau$ [Nm/A]として

\tau = k_\tau i
\tag{4}

が成り立ちます。$k_\tau$はモータのトルク定数と呼ばれることが多いです。力学っぽいワードの「トルク」と回路っぽいワードの「電流」をつなげることに成功しました。あとは回路の分野で電圧$u$と電流$i$の関係を求めることができればうまくいきそうです。
　電圧$u$と電流$i$の関係を求める前にモータの発電について押さえておきます。小学6年生くらいで学ぶと思いますが、モータを回すと発電ができます。その発電量は回す速度に比例しており、発電量を$e$ [V]、モータの回転速度を$\omega$ [rad/s]で表すと比例係数を$k_e$ [Vs]として

e = k_e \omega
\tag{5}

が成り立ちます。$k_e$はモータの逆起電力定数と呼ばれることが多いです。なぜ発電の話をしたかと言うと、モータを普通に回しただけでも、この発電の影響を考えないといけないからです。

　モータの中身はただ線が大量に巻かれているだけなので、抵抗値が$R$ [$\Omega$]のただの巻き線抵抗器であると考えると、慣れ親しんだオームの法則により

u - e = Ri
\tag{6}

が得られます。最後に(4)～(6)式から電流$i$を消去して整理すれば、モータに与える電圧$u$ [V]によって発生するモータの出力トルク$\tau$が

\tau = \frac{k_\tau}{R}u - \frac{k_{\tau}k_e}{R}\omega
\tag{7}

のように求まります。(7)式のおかげで入力電圧$u$ [V]を力学の世界で扱うことができるようになりました！

　次に台車の挙動を明らかにします。ところで一般的にはモータの出力をそのまま車輪につなげることはあまりなく、たいていの場合は間に歯車をかませます。したがって、車輪の回転数$\Omega$ [rad/s]と回転トルク$T$ [Nm]はギア比$k_G$を用いて

\Omega = \frac{\omega}{k_G}
\tag{8}

T = k_G\tau
\tag{9}

のように表されます。また、車輪の半径を$r$ [m]とし、地面と車輪のすべりを無視すると、台車の移動速度$\dot{x}$ [ms]と駆動力$F$ [N]の関係は

\dot{x} = r\Omega
\tag{10}

F = \frac{T}{r}
\tag{11}

のように求まります。

　ここで台車の質量を$M$ [kg]、振子の質量を$m$ [kg]とします。さらに振子が台車に与える影響が十分小さいと仮定すると、振子と台車は完全に一体化していると考えることができます。この場合、台車にはたらく外力は駆動力$F$のみであり、ニュートンの運動方程式より

(M + m)\ddot{x} = F
\tag{12}

が成り立ちます。
　あとは少しややこしい計算になりますが、(7)(8)(10)式から$\omega$と$\Omega$を消去することで

\tau = \frac{k_\tau}{R}u - \frac{k_\tau k_e k_G}{Rr}\dot{x}
\tag{13}

が、(9)(11)(12)式から$F$と$T$を消去することで

(M + m)\ddot{x} = \frac{k_G \tau}{r}
\tag{14}

が得らえます。最後に(13)(14)式から$\tau$を消去すれば

\ddot{x} = -\frac{k_\tau k_e {k_G}^2}{(M + m)Rr^2}\dot{x} + \frac{k_G k_\tau}{(M + m)rR}u
\tag{15}

となります。(15)式は長くて見づらいので定数$a$と$b$を用いて

\ddot{x} = -a\dot{x} + bu

とすれば見栄えがよくなります。これにて(1)式が無事導出されました。

3.2 振子の運動方程式

　次に振子の運動方程式を導出します。まず、振子がどのようにして台車から力を受けているのかについて説明します。ところで電車に立ち乗りしていたとき、発車する瞬間転びそうになったことはありませんか？それは電車が加速するときに体に慣性力がはたらくからです。発生する慣性力$f_h$ [N]は、慣性力を受ける人の体重を$M_h$ [kg]、電車の加速度を$a$ [m/$s^2$]とすると

f_h = M_h a
\tag{16}

で計算されます。慣性力は重心（人間で言うと腰らへん）にはたらくので、地面を中心としたモーメント人体にはたらき、電車に乗る人たちを転ばそうとしています。（つまり背が高いほうが転びやすい..？）
　実は倒立振子の振子も、電車に乗る人と同じように、台車から慣性力を通じて力を受け取っています。振子の質量を$m$ [kg]とすれば、振子が台車から受ける慣性力$f$ [N]は

f = m \ddot{x}
\tag{17}

で計算できます。
　また、忘れてはいけないのが重力です。重力$f_g$ [N]も振子の重心にはたらきます。その大きさは重力加速度を$g$ [m/$s^2$]とすれば

f_g = mg
\tag{18}

により求まります。これで式を作るのに必要な外力がすべて求まりました。

　あとは剛体の運動方程式を作るだけです。まず外力を振子に平行な力と垂直な力に分割します。モーメントに関係するのは振子に垂直な力のみなので、慣性力と重力の振子に垂直な成分$\tilde{f}$、$\tilde{f_g}$はそれぞれ

\tilde{f} = m \ddot{x}\cos{\theta}
\tag{19}

\tilde{f_g} = mg\sin{\theta}
\tag{20}

となります。ただし、振子の回転角度$\theta$ [rad]が十分に小さいときは、高校物理でよく出てきたように

\tilde{f} \simeq m \ddot{x}
\tag{21}

\tilde{f_g} \simeq mg\theta
\tag{22}

と近似することができます。
　必要な力は出そろったので、振子の回転軸まわりの慣性モーメントを$J$ [kgm^2]、回転軸から重心までの距離を$l$ [m]とすれば、剛体の運動方程式より

J\ddot{\theta} = l(\tilde{f_g} - \tilde{f})
\tag{23}

が得られます。最後に(21)～(23)式から$\tilde{f}$と$\tilde{f_g}$を消去すれば

J\ddot{\theta} = -ml\ddot{x} + mgl\theta

となり、(2)式が導出できます。

3.3 状態方程式の導出

(1)式と(2)式が導出できたので、あとはこれらを行列に落とし込むだけの作業です。この部分に関しては、現代制御の授業でもよく取り上げられていると思います。まずあたりまえですが

\dot{x} = \dot{x}
\tag{24}

\dot{\theta} = \dot{\theta}
\tag{25}

がなりたちます。次に(1)式より

\ddot{x} = -a\dot{x} + bu

がなりたちます。また(1)(2)式から$\ddot{x}$を消去すると

\ddot{\theta} = \frac{mla}{J}\dot{x} + \frac{mgl}{J}\theta - \frac{mlb}{J}u
\tag{26}

が得られます。最後に(1)(24)～(26)式を1つの行列にまとめれば、

\begin{pmatrix}
\dot{x} \\
\ddot{x}\\
\dot{\theta}\\
\ddot{\theta}
\end{pmatrix}
=
\begin{pmatrix}
0 & 1 & 0 & 0 \\
0 & -a & 0 & 0\\
0 & 0 & 0 & 1\\
0 & \frac{mla}{J} & \frac{mgl}{J} & 0
\end{pmatrix}
\begin{pmatrix}
x \\
\dot{x}\\
\theta\\
\dot{\theta}
\end{pmatrix}
+
\begin{pmatrix}
0 \\
b\\
0\\
-\frac{mlb}{J}
\end{pmatrix}
u

のように状態方程式が導出できます！

4. おわりに

　まあまあ丁寧な導出ができたと思います(しらんけど)。この記事をきっかけに、倒立振子に手を出す人が増えると嬉しいです。

参考文献

1. 川田昌克：物理法則に基づくモデリング、システム/制御/情報、Vol56、No.4、pp.166-169 (2012)

2. 川田昌克：MATLAB/Simulinkによる現代制御入門、森北出版 (2015)