これをどうやって動かしているかを簡単ですがまとめました。

toioとは

SONYが出しているCube型のおもちゃです。

技術仕様が公開された時に記事を書いたので詳しくはそちらを参照ください。

toioのjavascriptライブラリが公開されたのでexample動かしてみた

WHILL Model CRとは

人が乗れるパーソナルモビリティWHILL Model Cの、外部から制御出来る研究開発用のモデルがModel CRです。

M5Stackとは

ESP32というWiFi/BLEを搭載したLCD,ボタン付きでモジュール追加するといろいろなI/Fを追加できる電子工作でよく使われるデバイスです。

こちらは以前Model CRをM5Stackで動かしたときの記事です。

M5StackのJoystickでWHILL Model CRを動かしてみた

システム構成

toioとModel CRは発売されている製品の仕様通りに制御するだけなので、開発は全てM5Stackのみで行っています。

toioの記事にある通り、toioは専用のマット上では位置(x,y)とz軸周りの回転角度が取れます。

この位置姿勢の初期値とゴール位置をM5Stackで設定して、それによって移動するための入力値を計算して、Model CR側に送ります。

その結果Model CRが動いた移動量はモーターの回転角からホイールオドメトリとして計算して自己位置を更新します。

toioの初期位置と目標位置の取得

toioはマット上で座標(x, y)とz軸周りの回転角度が取れますので、それを覚えるようにしました。

最初はデバッグ用にM5StackのAボタンを押すと、現在位置を初期位置としてリセットします。（そのとき同時にWHILLの現在の自己位置姿勢もリセットするようにしています。）

次にtoioをマット上で移動させてボタンBを押すと、そのときの現在位置をゴール位置として与えました。

またtoioから取得出来る座標位置は0 - 数百のオーダーでしたので、これをModel CRの移動量とオーダーを合わせるために、toio移動量:100 = Model CR移動量:1mとするように係数0.01をかけています。

void checkPushButton(void)
{
  if(M5.BtnA.wasPressed()){
    setStartPosition();
    is_requested_track = false;
  }

  if(M5.BtnB.wasPressed()){
    if(setGoalPosition()) is_requested_track = true; 
  }
}

bool setStartPosition(void)
{
  tracker.resetArea();

  bool is_set_start = false;

  position_t position = convertPosition(position_id);
  Serial.println("reset pos");
  Serial.printf("%f %f %f\n", position.x, position.y, position.theta);
  if(tracker.setStartPosition(position)){
    is_set_start = true;
    odom.reset(position);
  }
  return is_set_start;
}

bool setGoalPosition(void)
{
  bool is_set_goal = false;

  if(position_id.x_cube_center != 0 || 
     position_id.y_cube_center != 0 ||
     position_id.angle_cube_center != 0){
    position_t position = convertPosition(position_id);

    if(tracker.setGoalPosition(position)){
      is_set_goal = true;
    }
  }
  return is_set_goal;
}

移動目標軌跡の計算

これはPythonRoboticsのmove to poseという初期位置とゴール位置を指定するとその間の軌跡（Pose）を算出するアルゴリズムを使わせてもらいました。(Path tracking)

これを見つけられたので結構シンプルにやりたいことが実現出来ました。
Python Roboticsはいろいろなアルゴリズムがあるので大変参考になります。

PythonRobotics move-to-a-pose-control

こちらのコードをC++で実装し、係数を調整しています。

実際にModel CR無しで、toioの位置からPoseの軌跡を描画した様子です。
初期位置とゴール位置をつなぐパスが求められているのがわかると思います。

座標修正 pic.twitter.com/5RNKgnAT0C

— Katsu Shun (@katsushun89) July 8, 2019