倒立振子の運動方程式を求めた話

倒立振子の制御がやってみたいのでシミュレーターを作ります。

運動方程式をｲﾝﾀｰﾈｯﾂの海から拾ってきて組み込んだんですが、

地面近くで加速したり、
なにもないところで跳ね返ったり、
外力がないのに左右に振動しだしたり、

↑正しくない物理法則

と振り子君が我々の宇宙における常識的な挙動から外れてエネルギー保存則に違反しまくるため自分で導出することにしました。やはり人から与えられたものは信用してはいけませんね。この世界で信じられるのは自分だけです。

座標系の定義

「神は『座標あれ』と言われた。すると座標があった。」(創世記 1章3節)

$M$ ：台車の質量

$m$ ：一様な棒の質量

$l$ ：一様な棒の長さ

$g$ ：重力加速度

$x$ ：台車の位置

$\theta$ ：棒の傾き

$F$ ：台車を引く力

ごく普通の座標定義です。棒の半分の長さを $l$ としたり棒の慣性モーメントを別に定義するやり方もあるらしいですが、シミュレーターを作るだけなので一様な棒です。

運動エネルギーと位置エネルギーの計算

台車の運動エネルギー

$K_M=\frac{1}{2}M\dot{x}^2$

棒の重心の速度

$v_m=\sqrt{\left(\dot{x}+\frac{l}{2}\cos{\theta}\dot{\theta}\right)^2+\left(\frac{l}{2}\sin{\theta}\dot{\theta}\right)^2} =\sqrt{\dot{x}^2+l\cos{\theta}\dot{x}\dot{\theta}+\frac{l^2}{4}\dot{\theta}^2}$

一様な棒の重心まわりの慣性モーメント

$I_m=\frac{1}{12}ml^2$

棒の運動エネルギー

$K_m=\frac{1}{2}mv_m^2+\frac{1}{2}I_m\dot{\theta}^2=\frac{1}{2}m\left(\dot{x}^2+l\cos{\theta}\dot{x}\dot{\theta}+\frac{l^2}{4}\dot{\theta}^2\right)+\frac{1}{2}\left(\frac{1}{12}ml^2\right)\dot{\theta}^2$

系の運動エネルギー

$K=K_M+K_m$

位置エネルギーは棒の部分のみです。
系の位置エネルギー

$U=\frac{mgl}{2}\cos{\theta}$

ラグランジュ方程式

ラグランジアン $L=K-U$ が求まったのでラグランジュ方程式を解いていきます。

まず $x$ について

$\frac{\partial L}{\partial \dot{x}}=M\dot{x}+\frac{1}{2}m\left(2\dot{x}+l\cos{\theta}\dot{\theta}\right)\\ \frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{x}}\right)=(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2$

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

これを代入すると

$\frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{x}}\right)-\frac{\partial L}{\partial x}=(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2=F$

次に $\theta$ について

$\frac{\partial L}{\partial \dot{\theta}}=\frac{1}{2}m\left(l\cos{\theta}\dot{x}+\frac{l^2}{2}\dot{\theta}\right)+\frac{1}{2}\left(\frac{1}{12}ml^2\right)2\dot{\theta}^2=\frac{lm}{2}\cos{\theta}\dot{x}+\frac{l^2m}{3}\dot{\theta}\\ \frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{\theta}}\right)=\frac{lm}{2}\cos{\theta}\ddot{x}-\frac{lm}{2}\sin{\theta}\dot{x}\dot{\theta}+\frac{l^2m}{3}\ddot{\theta}$

$\frac{\partial L}{\partial \theta}=\frac{1}{2}m\left(-l\sin{\theta}\dot{x}\dot{\theta}\right)+\frac{mlg}{2}\sin{\theta}$

これを代入すると

$\frac{\partial}{\partial t}\left(\frac{\partial L}{\partial \dot{\theta}}\right)-\frac{\partial L}{\partial \theta}=\frac{lm}{2}\cos{\theta}\ddot{x}-\frac{lm}{2}\sin{\theta}\dot{x}\dot{\theta}+\frac{l^2m}{3}\ddot{\theta}+\frac{ml}{2}\sin{\theta}\dot{x}\dot{\theta}-\frac{mlg}{2}\sin{\theta}=0$

$\frac{lm}{2}\cos{\theta}\ddot{x}+\frac{l^2m}{3}\ddot{\theta}-\frac{mlg}{2}\sin{\theta}=0$

両辺を $\frac{ml}{2}$ で割って

$\cos{\theta}\ddot{x}+\frac{2l}{3}\ddot{\theta}-g\sin{\theta}=0$

式変形

答えとしては以下のようになります。

$\left \lbrace \begin{array}{l}(M+m)\ddot{x}+\frac{lm}{2}\cos{\theta}\ddot{\theta}-\frac{lm}{2}\sin{\theta}\dot{\theta}^2=F\\ \cos{\theta}\ddot{x}+\frac{2l}{3}\ddot{\theta}-g\sin{\theta}=0\end{array}\right.$

ただプログラマーからすると「連立方程式渡されても…」って感じですよね。
なのでそれぞれについて解いた形にします。

$\ddot{x}=\frac{4F-3mg\sin{\theta}\cos{\theta}+2lm\sin{\theta}\dot{\theta}^2}{4\left(M+m\right)-3m\cos ^2\theta}$

$\ddot{\theta}=\frac{6\left(M+m\right)g\sin{\theta}-3\cos{\theta}\left(2F+lm\sin{\theta}\dot{\theta}^2\right)}{l\left(4\left(M+m\right)-3m\cos ^2\theta\right)}$