Processingで倒立振子のシミュレーターを作る話（オイラー法・ルンゲ=クッタ法）

kogakudanshi.hatenablog.jp

前回求めた倒立振子の運動方程式を組み込んだシミュレーターを作ります。

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

育ちが悪くてMatlabがちゃんと使えないのでProcessingで可視化します。

オイラー法

シンプルなやつです。
$x(t+dt)=x(t)+\dot{x}(t,y)\times dt$

みたいなのをそのまま実装します。

float x = -20;
float x_ = 5;
float t = PI / 180 * 20;
float t_ = -PI / 180 * 51;



final float m = 2;
final float M = 1;
final float l = 4;
final float g = 9.8;


float fx(float x,float t,float x_,float t_,float F){
  return (4*F-3*m*g*sin(t)*cos(t)+2*l*m*sin(t)*t_*t_)/(4*(M+m)-3*m*cos(t)*cos(t));
}


float ft(float x,float t,float x_,float t_,float F){
  return (6*(M+m)*g*sin(t)-3*cos(t)*(l*m*sin(t)*t_*t_+2*F))/(l*(4*(M+m)-3*m*cos(t)*cos(t)));
}


void update(float F,float dt){
  float x__ = fx(x,t,x_,t_,F);
  float t__ = ft(x,t,x_,t_,F);
  x_ += x__ * dt;
  t_ += t__ * dt;
  x += x_ * dt;
  t += t_ * dt;
  if(t > 2*PI)t -= 2*PI;
  if(t < -2*PI)t += 2*PI;
}


void setup(){
  size(1280,720,P2D);
}


void draw(){
  background(255);
  pushMatrix();
  translate(1280 / 2, 600);
 
  strokeWeight(1);
  stroke(0,0,0);
  line(-1280/2,0,1280/2,0);
 
  final int N = 100;
  for(int i = 0;i < N;i++)update(0,1.0/60.0/N);
 
  stroke(0,0,0);
  strokeWeight(3);
  rect(10*x-30,-20,60,40);
 
  stroke(255,0,0);
  strokeWeight(6);
  line(10*x,0,10*x + 400*sin(t),-400*cos(t));
 
  popMatrix();
}

コードは上のようになります。 $\dot{x}$ をx_（アンダーバー1つ）、 $\ddot{x}$ をx__（アンダーバー2つ）のように表記しています。グローバル変数とローカル変数が同名なのは助走付きでぶん殴られても文句が言えないくらい良くないのですが、書き写す時便利なのでこういう書き方をしました。
update関数をfor文で回しているのはフレームレートに対してシステムの固有振動数が高いためdtを短くするためです。分割数Nは適当に決めましたが、処理速度とfloatの精度が許す限り大きい方が良いと思います。

ルンゲ=クッタ法

計算がめんどくさくて精度が良いやつです。

$k_1=\dot{x}(t,y(t))$
$k_2=\dot{x}(t+\frac{dt}{2},y(t)+\frac{dt}{2}k_1)$
$k_3=\dot{x}(t+\frac{dt}{2},y(t)+\frac{dt}{2}k_2)$
$k_4=\dot{x}(t+dt,y(t)+dt k_1)$

$x(t+dt)=x(t)+\frac{k_1+2k_2+2k_3+k4}{6}dt$

これを実装するのですが、今回のケースでは $y$ がベクトルでありまた微分がそれ自体の関数になっています。まぁ解決法はあるんでしょうが数弱の自分には手のつけようがなかったので内部的にはオイラー法を使いました。（二階微分にルンゲクッタ適用するだけでもそれなりに精度向上するやろ！！のお気持ち）

さきほどのコードのupdate関数を以下に書き換えます。

void update(float F,float dt){
  float kx1 = fx(x,t,x_,t_,F);
  float kt1 = ft(x,t,x_,t_,F);
  float kx2 = fx(x+x_*dt/2, t+t_*dt/2, x_+kx1*dt/2,t_+kt1*dt/2, F);
  float kt2 = ft(x+x_*dt/2, t+t_*dt/2, x_+kx1*dt/2,t_+kt1*dt/2, F);
  float kx3 = fx(x+x_*dt/2, t+t_*dt/2, x_+kx2*dt/2,t_+kt2*dt/2, F);
  float kt3 = ft(x+x_*dt/2, t+t_*dt/2, x_+kx2*dt/2,t_+kt2*dt/2, F);
  float kx4 = fx(x+x_*dt, t+t_*dt, x_+kx3*dt,t_+kt3*dt, F);
  float kt4 = ft(x+x_*dt, t+t_*dt, x_+kx3*dt,t_+kt3*dt, F);
 
  x_ += (kx1 + 2*kx2 + 2*kx3 + kx4) * dt / 6;
  t_ += (kt1 + 2*kt2 + 2*kt3 + kt4) * dt / 6;
 
  x += x_ * dt;
  t += t_ * dt;
  if(t > 2*PI)t -= 2*PI;
  if(t < -2*PI)t += 2*PI;
}