=======スラロームの作り方=======
=====ちょっと復習=====
この記事ではスラローム走行について書きたいと思います。前提条件として、物理量ベースであることを要求します。
ちょっと物理量ベースの復習をしてみましょう。普通、物理量ベースというと下のような関数を使うと思います。
  //**************************************//
  //関数名 void move(float v, float omega) //
  //                                      //
  //引数:   速度    v [m/s]                //
  //       角速度  omega [deg/sec]         //
  //                                      //
  //指定した速度と角速度で移動する関数         //
  //**************************************//
引数の単位系は別に同じでなくてもかまいません。物理量ベースになっているのなら、こんな感じの関数は自前で持っていると思います。ステッパでかつ二輪ならば左右のタイヤの回転数を調節することによって、所望の速度と角速度で走ることはさほど難しくないはずです。この辺は物理の話なのでよく分からない人は物理を勉強してください。
物理量ベースなので次のようなグローバル変数を持っていると思います
  位置,速度,加速度 :       x,v,a
  角度,角速度,角加速度 :  angle,omega,alpha
復習になりますが、物理量ベースというのは周期Tごとに上の変数たちを更新して、moveに代入みたいな感じです。全部書くのもつまらないので、この辺は自分で考えてください。ここに出ているものもあくまでもいろんな方法がある中の一つです。プログラムというのは同じ概念を実装するのに、無限の方法があります。要は自分で考えないとダメということです。

=====円弧によるスラローム=====
突然ですが、重心速度v,半径rでマウスを進ませることはできますか。これができればとりあえずスラローム走行ができると思います。しかし、この円弧によるスラロームには問題があります。それは直進から円弧に急に移ると角速度が不連続になるという問題です。だから、この欠点を補ったクロソイドという軌跡を使うことになります。しかし、速度がそんなに高くなかったら別にそこまで問題は無いでしょう。プログラム的には下のようになりますかね。
  float pre;         //スラローム前距離
  float fol;         //スラローム後距離
  float v_turn       //スラローム時の重心速度
  float omega_turn;  //スラローム時の角速度
  angle = 0;         //アングルの初期化
  
  距離preを速度v_turnで進む;
  角速度omegaを一定(適当)にし,重心速度v_turnにして,角度angleが90degになるまで積分する;
  距離folを速度v_turnで進む;

これで、物理量ベースが問題なければとりあえずスラロームで90deg回ることはできると思います。前後に直進を設けるのはこのpreとfolを調節することによって軌跡の調節をしたいからです。円弧軌道だったら旋回半径はあらかじめ計算できると思いますが、適当に上のパラメータを弄ってトライアンドエラーにより軌跡を作ってもいいと思います。この円弧によるスラロームというのは、直進運動のときに等速度で走るっていうのでやったことを重心速度一定で、角度や角速度でやればいいわけですね。だから、等速度運動ができていて直進と回転が分離できていたら、すぐできると思います。

=====クロソイドによるスラローム=====
ところで、円弧によるスラロームの話を書いたときに角速度の不連続がよくないと書いたのですが、これがなんでだかはわかりますか。理由が分からないならクロソイドはまだ早いと言いたいところですが、まあ、普通に説明しましょう。

ステッピングモータの一番の弱点ってなんでしょう。そう、脱調です。回転数を緩やかに変えないと脱調してしまうわけです。たとえば500mm/secくらいで直進しているとして、この時左右のモータの回転速度は500mm/secですよね。この状態からいきなり円弧軌道に入る場合、右と左のタイヤに一定の速度差をつけるわけで、片方は500 + x mm/sec もう片方は 500 - x mm/secみたいになるわけです。この速度差を作るx(物理量ベースが分かっている人にはこれがなんだかわかりますよね。そう、角速度を生じさせるものそのものです。)が大きいとステッピングモータは脱調します。だから、このxを徐々に大きくしていき、ある程度までいって、そこから徐々に小さくしようというのが、クロソイドです。xに急に大きな値を入れるというのが角速度不連続に相当します。

先ほど低速のターンであれば、円弧軌道でも問題ないと書いたのは、低速ならばステッピングモータが多少急な回転数変化にもついていけるからです。しかし、円弧がクロソイドに勝るものは正直何一つないです。しいて言えば簡単ってだけです。だから、ターンはクロソイドで作りましょう。勘のいい人はもうどうすればクロソイドが作れるか気づいているかもしれませんね。ヒントは等加速度で加減速するときに行った台形制御をそっくりそのまま、角速度と角加速度でやるということです。つまり**クロソイドというのは等角速度運動**というわけです。
  float pre;         //スラローム前距離
  float fol;         //スラローム後距離
  float v_turn       //スラローム時の重心速度
  float alpha_turn;  //スラローム時の角加速度
  angle = 0;         //アングルの初期化
  float ang1         //角速度が上がるのは0からang1まで
  float ang2         //角速度が一定なのはang1からang2まで
  float ang3         //角速度が下がるのはang2からang3まで
  
  距離preを速度v_turnで進む;
  
  重心速度v = v_turnで
  角加速度alpha = alpha_turnでangleがang1になるまで進む;
  角加速度alpha = 0でangleがang2になるまで進む;
  角加速度alpha = -alpha_turnでangleがang3になるまで進む;
  
  距離folを速度v_turnで進む;

このやり方だと、スラロームの軌跡がどうなるかが、実際にロボットに走らせてみるまで分からないです。だから、パラメータは決め打ちになってしまいます。だから、シュミレーターを用意するといいと思います。ちなみに普通の90degターンから斜め走行に必要なターンまですべてのターンはこの方法でパラメータを変えるだけで作れます。これをシュミレーターなしで全部作るのはなかなか大変です。