統失に飼われているお魚かわいそう

統失ですが海水魚飼育をします。海水魚をメインとした日常の日記です。

ちょっとだけ掃除

水槽をちょっとだけ掃除したので、久しぶりにお魚の様子でも載せようと思います。

f:id:cmsan:20201129202818j:plain

スミレは、結構成長しました。3か月で1cmほど大きくなったと思います。それに伴って、頭の黄色い部分の面積が増えてきました。私は青いスミレが好きなので、黄色部分が広がるのは少し残念ではありますね。しかし、せっかく運よく健康な個体に久々にあたったのでこれからもかわいがって行こうと思います。

他はサンゴをちょこちょこ増やしてます。成長が楽しみだったコモンレッドも少しずつ大きくなっています。

f:id:cmsan:20201129203008j:plain

3か月たちましたが、1cmほど成長したと思います。赤い大きなウスコモンに成長してほしいです。マルケサスも元気です。

f:id:cmsan:20201129203058j:plain

キッカサンゴとスギサンゴとスカンクシュリンプ。

f:id:cmsan:20201129203321j:plain

バーゲスとウズマキも元気です。

f:id:cmsan:20201129203722j:plain

欲を言えばシマヤッコの飼育にも挑戦してみたいのですが、今の水槽は安定してるのでしばらくお魚を増やす計画はないです。追加するとすれば、何かいいサンゴがあれば買ってみようかな?

特に何もないですが

11月後半になっても沖縄は25℃前後と暖かい日が続いておりますが、もう私が取れる幼魚サイズのお魚はあまりとれないと思い、しばらく海にも行ってません。来年の6月までもうお休みシーズンかもしれません。

今日は特に何もないですがそばを食べに行ってきました。

f:id:cmsan:20201127203757j:plain

西原町にあるちょーでぐぁさんですね。アーサそばが食べたくなったときはここにきています。また、フリーもずくもあるのでもずく好きの方にはお勧めです。ちなみにアーサとは本土ではあおさという海藻のことです。

f:id:cmsan:20201127204228j:plain

まあまあ美味しいです。子供の頃アーサ汁が大好きだったんですが、ここ十年以上はもう食べてないですね。悲しいです。

 

水槽の近況としては、最近全然掃除してないので、掃除したら写真を撮ろうと思いますが、生体はみんな元気に生きています。薬物採取が怖くて心配していたスミレも沖縄産だったためか、無事3か月飼育できてます。この個体は病気もなく丈夫な子だと思っています。

 

また、倒立振子のほうは、あとはPIDパラメータを探す作業があるのですが、やる気が出なくて一週間ほど触っていませんでした。今日、一週間ぶりに触ってみたのですが、パラメータ探索はかなり難しそうです。やる気が出たときに製作がんばろうと思います。

 

では。

パラメータ推定

振り子が安定しないので、シミュレーションで数値計算して安定する\sigmaを探ってみたのですが、どんな\sigmaを与えてもこのモデルでは安定しないことがわかりました。ということで考え方を改めてパラメータK_pK_iK_dを地道に探っていきます。なので近々過去記事も大幅に修正する必要がありそうです。以前にも書きましたが、振り子が完成するまでは私の記事は信用しないほうがいいです。
 まず、大雑把に推定するために、ラウス・フルビッツの安定判別法という方法を使いました。安定判別のやり方についてこちらのサイトを参考にしました。特性方程式
   
   m_as^3+(\xi+K_d)s^2+(K_p+k)s+K_i =0

からm_a<0なので安定条件は

   K_i<0
   K_d<-\xi
   \displaystyle K_p<-k+\frac{m_a K_i}{\xi+K_d}

となりました。この範囲でK_pK_iK_dをまんべんなく変えて安定する値を求めるプログラムを作成しました。

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

double RungeKutta(double dtheta, double theta, double u)
{
  double k1, k2, k3, k4;
  double m1, m2, m3, m4;
  double dt = 0.1;
  int counter = 0;

  k1 = dt*( -5.3753e-2*dtheta + 110.812*theta - u/0.025444 );
  m1 = dt*dtheta;

  k2 = dt*( -5.3753e-2*(dtheta + k1*0.5) + 110.812*(theta + m1*0.5) - u/0.025444 );
  m2 = dt*( dtheta + 0.5*k1 );
    
  k3 = dt*( -5.3753e-2*(dtheta + k2*0.5) + 110.812*(theta + m2*0.5) - u/0.025444 );  
  m3 = dt*( dtheta + 0.5*k2 );

  k4 = dt*( -5.3753e-2*(dtheta + k3) + 110.812*(theta + m3) - u/0.025444 );
  m4 = dt*( dtheta + k3 );

  dtheta += (k1 + 2*k2 + 2*k3 + k4) / 6.0;
  theta += (m1 + 2*m2 + 2*m3 + m4) / 6.0;
    
  return theta;
}


int main(void)
{
  double u;
  double Kp;
  double Ki;
  double Kd;
  double Kp_max;
  double theta;
  double theta_s;
  double intTheta;
  double lastTheta;
  double divTheta;
  double dt = 0.1;
  int flag;
  int counter;
  int counter2;
   
  for(Ki=-0.1; Ki>-10.0; Ki -=0.1)
  {
    for(Kd=1.36e-3; Kd>-10.0; Kd-=0.1)
    {
      Kp_max = -2.8195 + (-2.5444e-2*Ki)/(Kd-1.3677e-3);

      if(Kp_max > -100.0)
      {
	for(Kp=Kp_max-0.1; Kp>-100.0; Kp-=0.1)
	{
	  counter2 = 0;
	 
	  for(theta_s=0.01; theta_s<1.0; theta_s+=0.01)
	  {
	    flag = 1;
	    counter = 0;
	    intTheta = 0.0;
	    lastTheta = 0.0;
	    theta = theta_s;

	    while( fabs(theta)<M_PI*0.5 )
	    {
	      divTheta = (theta - lastTheta)/dt;
	      intTheta += theta*dt;
    
	      u = Kp*theta + Ki*intTheta + Kd*divTheta; 
	      u *= -1;
	    
	      lastTheta = theta;
	      theta = RungeKutta(divTheta, theta, u);
    
	      counter++;
    
	      if(fabs(theta) < 1.0e-5)
	      {
		flag = 0;
		break;
	      }
	    }
	  
	    if(flag == 0)
	    {
	      if(counter<10) counter2++;
	    }
	  }
	  if(counter2 == 99) printf("%lf\t %lf\t %lf\n", Kp, Ki, Kd);
	}
      }
    }
  }
  return 0;
}

内容はラウス・フルビッツで求めた範囲のK_pK_iK_dを使って求めた力uを(6)式

   m_a \ddot \theta + \xi \dot \theta + k \theta = u

に代入して、ルンゲクッタで2階微分方程式を解いています。そして10回以内に\theta \approx 0に収束したK_pK_iK_dを抽出します。\theta=0.01 \sim1.0\mathrm{rad}の全ての初期値で安定したものを表示させます。そして出てきた結果がこちらです。それぞれ左からK_pK_iK_dです。

-10.260210	 -0.800000	 -0.498640
-9.584883	 -2.600000	 -0.398640
-10.348492	 -3.600000	 -0.398640
-6.788066	 -4.200000	 -0.098640
-9.843905	 -5.100000	 -0.398640
-9.850266	 -5.200000	 -0.398640
-9.856627	 -5.300000	 -0.398640
-9.862987	 -5.400000	 -0.398640
-9.869348	 -5.500000	 -0.398640
-9.875709	 -5.600000	 -0.398640
-9.882070	 -5.700000	 -0.398640
-6.745679	 -6.700000	 -0.398640
-3.377483	 -7.200000	 -0.398640
-4.472546	 -7.700000	 -0.298640
-13.560175	 -8.500000	 -0.398640
-6.639364	 -8.800000	 -0.698640
-13.630144	 -9.600000	 -0.398640

これをみると大体K_p \approx -9.8K_i \approx -5K_d \approx -0.4付近がよさそうですね。果たしてこの値でホントに倒立するんでしょうか。
 実際にはギアの遊びやarduinoの出力波形などがあるので、シミュレーションのようにうまくはいかないと思いますが、次回これらパラメータを突っ込んで振り子の動きを見てみようと思います。
 結果を確認してないのに毎回記事だけ飛ばして書いてドキドキしますね。

2階微分のルンゲクッタ

倒立振子はパラメータがうまくいかず行き詰っています。そのため、色々方法を模索しているのですが、その中で2階微分方程式のルンゲクッタ法を勉強したのでメモしときます。(6)式より

   m_a \ddot \theta + \xi \dot \theta + k \theta = u

ですが、\thetaについて一階微分と二階微分

   \displaystyle \dot \theta= \frac{d \theta}{dt}=p
   \displaystyle \ddot \theta = \frac{d \dot \theta}{dt} = -\frac{\xi}{m_a}\dot \theta - \frac{k}{m_a} \theta +\frac{u}{m_a} = f(p, \theta)

となります。一階微分方程式が2つできます。あとはこれを解くだけです。

   \displaystyle h_1=h \cdot p
   \displaystyle k_1 = h\cdot f(p, \theta)

   \displaystyle h_2=h\cdot \left(p + \frac{k_1}{2} \right)
   \displaystyle k_2 = h\cdot f \left( p+ \frac{k_1}{2}, \theta+\frac{h_1}{2}  \right)

   \displaystyle h_3=h\cdot\left(p + \frac{k_2}{2} \right)
   \displaystyle k_3 = h\cdot f \left( p+ \frac{k_2}{2}, \theta+\frac{h_2}{2}  \right)

   \displaystyle h_4=h\cdot\left(p + \frac{k_3}{2} \right)
   \displaystyle k_4 = h\cdot f \left( p+ k_3, \theta+h_3  \right)

   \displaystyle \theta(t + \delta t) = \theta(t) + \frac{1}{6}(h_1 + 2h_2 + 2h_3 + h_4)
   \displaystyle p(t + \delta t) = p(t) + \frac{1}{6}(k_1 + 2k_2 + 2k_3 + k_4)

各種物理量

 後は物理量を測ってプログラムを書けば完成なんですが、ここ最近体調が悪く、ほとんど動けずベッドで横になっていたため5日ほどお休みしてました。まーたまによくあります。

 さて、今日は物理量を測っていこうと思います。

f:id:cmsan:20201110173833j:plain

釣り糸で釣って適当に重心を測りました。

   L_j=7.7×10^{-2}\mathrm{m}

   m=0.273\mathrm{kg}

   M=1.45×10^{-2}\mathrm{kg}

   g=9.807\mathrm{m/s^2}

   r=2.9×10^{-2}\mathrm{m}

さて、慣性モーメントJ_pを求めるため次のようなモデルを考えます。

   f:id:cmsan:20201110174103p:plain

    \displaystyle \begin{eqnarray} J_p  =  \int^{\frac{h_z}{2}}_{- \frac{h_z}{2}} \int^{\frac{h_x}{2}}_{- \frac{h_x}{2}} \int^{l_2}_{-l_1} \frac{m}{V}(x^2 +y^2) dy dx dz \\  =  \frac{m}{V} \{ \frac{{h_x}^3}{12}(l_2 + l_1) + \frac{h_x}{3}({l_2}^3 + {l_1}^3) \} h_z  \end{eqnarray}

実測すると、h_x=3.1×10^{-2}\mathrm{m}h_z=6.0×10^{-2}\mathrm{m}l_1=1.0×10^{-2}\mathrm{m}l_2=1.5×10^{-1}\mathrm{m}なので代入して、

   J_p=1.942×10^{-3} \mathrm{kgm^2}

タイヤ周りの粘性係数\etaについては、調べてみたんですがちょっとわからなかったので、とりあえずネットで誰かが使ってた係数

   \eta=1.0×10^{-4}\mathrm{kgm^2/s}

を使ってみようと思います。うまくいかなかったらこの辺もちょこちょこ変えて調整してみようと思います。

これらを(7)式に代入して、

   m_a=-2.5444×10^{-2}

   \xi=-1.3677×10^{-3}

   k=2.8195

となりました。さらに(15)式に代入して、

   \displaystyle K_p=2.8195-\frac{7.6332×10^{-2}}{\sigma^2}

   \displaystyle K_i=-\frac{2.5444×10^{-2}}{\sigma^3}

   \displaystyle K_d=1.3677×10^{-3}-\frac{7.6332×10^{-2}}{\sigma}

となります。\sigmaの値によるんですが、どんな動きになるんでしょうかね?楽しみです。

ボディ作成

今日はボディと電気回路を作成します。まず、タミヤのあれこれ用意します。

f:id:cmsan:20201106181337j:plain

そしたら完成です。

表面。

f:id:cmsan:20201106181512j:plain

裏面。

f:id:cmsan:20201106181552j:plain

へたっぴですが電気回路も組みました。

f:id:cmsan:20201106181649j:plain

最後にモーターの動作確認して終了です。

半日で作る倒立振子さんを参考にすると最初に宣言してましたが、構造も制御方法が違うのができました。これで倒立しなかったら元も子もないのですが、私はいい加減な性格なのです。

大学進学で本土に行ったとき、本土の人の頭の良さと完璧主義さに感動して、自分もこうなりたい思ってつくづく努力したのですが、8年間統失で腐ってる間に昔の精神を忘れてしまったようです。

ま、いっか。回復したらまた頑張りましょ。

出力パラメータの決定

さて、(22)式をN_2の形に直すと、

   \displaystyle N_2 = N_1 +\frac{u \delta t}{2 \pi r M}

で、さらに(21)式を使って、N_2を消去すれば、

   \displaystyle E_a - I_a R = K_e \left( N_1 +\frac{u \delta t}{2 \pi r M} \right)

となります。これを(23)式を代入してI_aを消去します。

   \displaystyle E_a - \alpha \mathrm{log} \left( \beta x + \gamma \right)R = K_e \left( N_1 +\frac{u \delta t}{2 \pi r M} \right)

簡単のために次に置き換えます。

   \displaystyle \frac{3}{86} x - \alpha \mathrm{log}\left( \beta x + \gamma \right)R = \kappa   (26)

   \displaystyle \kappa = K_e \left( N_1 +\frac{u \delta t}{2 \pi r M} \right)

   \displaystyle x = \frac{86}{3}E_a

86/3は電圧をanalogWrite関数パラメータに直した時に係る係数です。\alpha\beta\gamma\kappaは定数なので、(26)の非線形方程式を解けば出力するパラメータxが決まりそうです。

 (26)式がどのような形をするかざっと見積もってみました。\alpha\beta\gammaは大体(24)式の値、\kappaを変化させて描いています。その結果…

f:id:cmsan:20201104180313p:plain

ニュートンラフソン法で解を見つけられそうですが、(26)式の解がモーターの動くArduinoのanalogWrite関数パラメータの36~86で収まってほしいので、大体\kappa=1.2\sim2.9の範囲で収まってほしいことがわかります。K_e N_1 \approx 1.5\sim3程度のオーダーになると思われますので、まあこんなもんだと思います。

ちなみに\kappa=2.9の時のニュートンラフソン法のプログラムは

#include <stdio.h>
#include <math.h>

int main(void)
{
  double x, x2, dx;
  double fx, fx2;
  
  x = 200.0; //初期値200
  dx = 1.0;  //初期値1

  while(dx > 1e-4)
  {

    fx = 3.0/86.0*x - 32.0e-3 * log(1.5*x -41.0) -2.9; //kappa=2.9
    fx2 = 3.0/86.0 - 32.0e-3 * 1.5 / (1.5*x - 41.0);

    x2 = x - fx/fx2;
    
    dx = x - x2;

    printf("%lf %lf %lf\n", x, x2, dx);

    x = x2;
  } 
  return 0;
}

 終わり。

シッタカ貝

コケ対策にシッタカ貝を買ってきました。

f:id:cmsan:20201103152242j:plain

すでに6匹のシッタカがいるので、これで10匹になりました。ヒフキアイゴもいますがコケは全然減りません。

f:id:cmsan:20201103152411j:plain

アラガミルクもパープルアップも添加してないんですがね。何が原因なんでしょう?どっちにしろコケは得るんなら、また添加材添加してみますか。

最近のお気に入りのサンゴです。

f:id:cmsan:20201103152711j:plain

ライブロックを買ったらついてきました。キッカサンゴの一種なのかな?

お魚はみんな元気です。

f:id:cmsan:20201103152945j:plain

f:id:cmsan:20201103153100j:plain

 

角度算出

ジャイロと3軸加速度センサから、角度を算出しました。使ったジャイロはENC-03R、3軸加速度センサはKXR94-2050です。加速度センサの使用は問題ないのですが、ジャイロは扱いが難しくて少し困りました。ENC-03Rは通常角加速度をアウトプットするので、角度を求めるには2回積分しないといけません。が、積分するたびにドリフトして強制的にゼロに戻す作業が必要になります。こういう物理的に根拠のないことを強制的に行うのは、後で挙動がうまくいかないときに原因の一つとなりかねないので、それを避けるために今回はカルマンフィルタを使うことにしました。カルマンフィルタとは、加速度、角速度、単位時間を突っ込むと正確な角度を出してくれる魔法の関数です。カルマンフィルタの内容については、7年前に一度勉強しようと思ったことがあったのですが、あまりに難解すぎてすぐに諦めた記憶があります。なので、フィルタの原理的なことは私は説明できません。
まず、ENC-03RのC6,C7コンデンサをショートします。こうすることで、アウトプットされるのが角加速度から角速度になります。詳しくは検索するといっぱい出てきます。
f:id:cmsan:20201102081847j:plain
で、あれこれつないで以下のスイッチができました。Arduinoでのカルマンフィルタの使い方は、ネットにあったのを丸々パクりました。

#include <MsTimer2.h>
#include <Kalman.h>

#define PINGYRO 1
#define PINACCY 2
#define PINACCZ 3

Kalman kalmanX;

int accY, accZ;
double kalAngleX;
double accXangle;
static unsigned long int accY0 = 528;
static unsigned long int accZ0 = 513;

//初期電圧
double Volt0 = 0.0;
void InitVolt()
{
  for(int i=0; i<1024; i++)  Volt0 += analogRead(PINGYRO);
  Volt0 /= 1024.0;
}

//加速度取得
double AccX()
{
  accY = analogRead(PINACCY) - accY0;
  accZ = analogRead(PINACCZ) - accZ0;

  return atan2(accY,accZ)*RAD_TO_DEG;
}

//角速度計算
double DegVelo()
{
  double volt;
  
  volt = analogRead(PINGYRO) * 1.0;

  return ( (volt - Volt0) / 1024.0 * 5.0 / 0.67e-3 / 10.1);  
}

unsigned long int Time; 
void setup() {
  Serial.begin(115200);
  InitVolt();
  kalmanX.setAngle(AccX());
  Time = micros(); 
}


void loop() {
  double dt;
  unsigned long int Time2;

  Time2 =  micros();
  dt = ( Time2 - Time ) * 1e-6;
  Time = Time2;

  kalAngleX = kalmanX.getAngle(AccX(), DegVelo(), dt);

  Serial.println(kalAngleX);

}

久しぶりにプログラム書くのでちゃんと書けてるか少し不安です。独習Cからまた始めようかな?

 あと話を進めているうちに、モデリングや解析計算で間違いや気になった点があった場合は、過去記事をちょこちょこ書き直したりしてます。振り子がちゃんと安定して倒立するまでは、「これ大丈夫かなぁ…?」くらいの半信半疑の気持ちで見ていただけると幸いです。

11月ですがまだまだ泳げます

今日は大潮なので海に来ました。

f:id:cmsan:20201101160425j:plain

f:id:cmsan:20201101160604j:plain

f:id:cmsan:20201101160654j:plain

観光客の方も多く家族連れが多かったです。子供たちも魚を取ったり泳いだりしてました。私はコロナは風邪やインフルエンザみたいなもんだと思ってますので、沖縄に来た方はいっぱい楽しんでいってもらいたいと思います。

子供たちに取れた魚を見せびらかしながら、今日とれたお魚はこちらです。

f:id:cmsan:20201101161059j:plain

ナミチョウとフウライ一匹ずつ。チョウハンも捕れたのですが、水辺で盛大にコケて逃がしてしまいました。

帰りに久々に南国亭に来ました。

f:id:cmsan:20201101161313j:plain

暖簾も出てないので何のお店かわかりませんが、ここでアグーそばを食べます。

f:id:cmsan:20201101161511j:plain

美味しかったです。