[HackDay2021] 群れるかわいいロボットのつくり方

今回[HackDay2021](https://hackday.jp "HackDay2021")に初参加しました。24時間でゴリゴリ作品を作ってそれを90秒のプレゼンで表現するというのはなかなか刺激的な体験でした。うまくいかなくて辛いと思うこともありましたがとても楽しかったです。せっかく作ったのでここでは全体システムとロボットの作り方について紹介したいと思います。

[追記]
HadkDay2021テクノロジー賞のJij賞を受賞しました！！！まさかいただけるとは思ってもおらず、とてもうれしいです！審査員の皆さま、スタッフの皆さま、本当にありがとうございました！

## 完成品
<iframe width="560" height="315" src="https://www.youtube.com/embed/gBVhLk7g6oc" title="YouTube video player" frameborder="0" allow="accelerometer; autoplay; clipboard-write; encrypted-media; gyroscope; picture-in-picture" allowfullscreen></iframe>
<br>
こんな感じのロボット9台を24時間で作りきりました。何度見てもほんとかわいいですよね。

## 全体システム
![Vent.003.jpeg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/04c21bea-abca-4401-3c4f-13aed37a3ace.jpeg)
　今回は全部で9台のロボットを作製し、それらが任意のフォーメーションに移動するシステムを作りました。フォーメーションの移り変わりにおいてどのロボットが次にどの位置に行くのがよいかをHackDay2021での提供技術であるJij Cloudの量子アニーリング技術を使った最適化により計算しました。量子アニーリング技術は離散的な組み合わせの最適化を得意とするのでフォーメーションの前後で各ロボットの移動距離が最小となるような組み合わせを見つけるという問題設定にし利用しました。ここについてはチームメンバーがわかりやすくまとめてくれたのでそちらをご欄ください。

[[Qiita] OpenJij（量子アニーリング）を使って群れるロボを動かす](https://qiita.com/NGOhiroshi/items/9c3f7b2c68335a5fc78e)

　ロボットは独立2輪の駆動方式のためその場での旋回と前進後退が可能です。WiFi搭載のマイコンとモバイルバッテリを使用することで完全に無線化しました。通信はhttp通信のみとなります。それぞれのロボットはエンコーダを持たないので、天井につけたカメラから各ロボットの位置と向きを算出し、目的地まで誘導するという方式をとりました。今回は各ロボットの判別も必要だったのでOpenCVの[Arucoマーカ](https://docs.opencv.org/master/d5/dae/tutorial_aruco_detection.html)を使用しました。フォーメーションの入力は今回は事前に定義したものをWeb UIから入力できるようにしました。サーバ側はFlaskを使用し入力を受けると同じサーバPCに接続したカメラの画像から現在のロボットの位置を取得し、次のフォーメーションに行くにはどのロボットがどの配置に行くかをJij Cloudにアクセスして決定します。それが決まるとカメラの画像で監視しながら各ロボットに回転・前進の指示を与え、目的地に到着すると停止させることでフォーメーションへの移動を実現しています。


## ロボット設計
### ロボットボディ
今回はハッカソンということもあり、ロボットボディは下記を使用することにしました。
#####２ＷＤ Ｍｉｎｉ Ｓｍａｒｔ Ｒｏｂｏｔ Ｍｏｂｉｌｅ Ｐｌａｔｆｏｒｍ Ｋｉｔ ｆｏｒ ｅｄｕｃａｔｉｏｎ
https://akizukidenshi.com/catalog/g/gK-13651/

中の部品はこんな感じでした。
![IMG_8921.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/04c7b1e2-74d2-b7c9-d9e7-d63aa1f0305a.jpeg)
ギヤドモータが2つ入っています。アウトプット軸が両面に出ているので、今回はやってないですが、エンコーダ用のスリットなどつけれそうでいいですね。ケーブルにジャンパピン（メス）がついているのもありがたいです。中に説明書も入っていますが、上記の秋月電子さんの商品ページにある補足説明書が分かりやすかったです。
組み立つとこんな感じになります。
![IMG_9200.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/e9d3e829-e78e-b0c9-1b97-094ad7efc68c.jpeg)

かっこいいですし、かわいいですよね。こんなに洗練されたロボットボディがこんなにかんたんに組み立てられるなんていい時代です。ちなみにジャンパピンは後ほどブレッドボードにつける関係でオスオスのものをつけておきました。


モータドライバはDCモータを2つ回せるものでキット化されている下記のものを使用しました。上記の補足説明書でも紹介されていたので間違いないです。
#####ＤＲＶ８８３５使用ステッピング＆ＤＣモータドライバモジュール
https://akizukidenshi.com/catalog/g/gK-09848/

またマイコンには今流行？のESP32の開発ボードを使用してみました。各ロボットは無線化したかったのでWiFi搭載というのが決め手でした。Arduino環境で書き込めるのも手軽でいいですね。
#####ＥＳＰ３２－ＤｅｖＫｉｔＣ－３２Ｅ　ＥＳＰ３２－ＷＲＯＯＭ－３２Ｅ開発ボード　４ＭＢ
https://akizukidenshi.com/catalog/g/gM-15673/

### 電源
モバイルロボットやるときの一番のネックは電源ですね。最初は乾電池を使おうと思っていたのですが、調べたところESP32でWiFiを使用していると結構電流を吸うらしく安定しないそうです。そこでダイソーで500円で買えるモバイルバッテリー(4000mAh)を使用することにしました。これなら充電すれば何回も使えていいですね。こんなに扱いやすいものが500円で手に入るなんて感激です。ということでたくさん買っておきました。
![IMG_9009.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/9abee480-f3fb-566e-00f7-758e5230c42a.jpeg)


### 配線
今回は24時間で9台ということもありはんだ付けは極力少なくするため、ブレッドボードを使用しました。ESP32の評価ボードとブレッドボードで調べると、取り付けたときの片方の列をすべて覆ってしまうため、6穴版をおすすめする記事が多かったのですが、今回は後で書くように評価ボードの下に配線を収めることにしたのでブレッドボードはいつものやつにしました。

######ブレッドボード　ＥＩＣ－８０１
https://akizukidenshi.com/catalog/g/gP-00315/

配線はコンパクトに仕上げるために、固いジャンパワイヤを使用しました。

######ブレッドボード・ジャンパーワイヤ　１４種類×１０本
https://akizukidenshi.com/catalog/g/gP-00288/

配線完了図がこちらです。
![IMG_9198.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/7f67b491-7dbe-a53d-09b8-8538ab246512.jpeg)
といわれてもわからないですよね。先ほど書いたとおりこの上にESP32評価ボードを載せます。それをイメージしながら配線すると、まずモータドライバのVM（モータ用）には5V、VCC（ロジック用）には3.3VをESP32から供給します。モータドライバのMODEピンはGNDに落とすとDCモータ用になるようです。信号線については

- AIN1 →　GPIO25
- AIN2 →　GPIO26
- BIN1 →　GPIO12
- BIN2 →　GPIO13

になるようにしました。また出力側は

- AOUT1 →　右モータの+端子
- AOUT2 →　右モータの-端子
- BOUT1 →　左モータの-端子
- BOUT2 →　左モータの+端子

となるようにしています。このあたりの調整はプログラム側でも可能です。配線できたブレッドボードは先ほどの写真のように裏側の両面テープを使ってモバイルバッテリに固定しました。このときモバイルバッテリのインジケータが見えるように少しずらすのがポイントです。
　ここまでの配線と先ほどのロボットボディを組み合わせるとこんな感じになります。バッテリとボディの固定はテープにしました。これでもう走り出す準備はできました。
![IMG_9201.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/781bcf0e-360e-0d74-3672-7b71adf61bac.jpeg)
と理想的にはそうなのですが、実は作ったうち3台はモバイルバッテリが省電力モードに入ってしまい自動OFFになってしまいました。検知側のバラつきなのかESP32側のバラつきなのか。とはいえ時間がなかったのでとりあえずどこの家にでもある小型ファンを急遽取り付け消費電力を底上げすることで自動OFFにならないようにしました。こうゆう応急処置みたいなのもハッカソンの楽しみの1つですね。

### マーカ
今回のシステムでは天井から各ロボットの位置と向きを検出するのがひとつのポイントで、OpenCVで使用可能なArucoマーカを使用しています。60mmのArucoマーカを印刷してカットしたものを厚紙に貼り、ダイソーで売っていた食品保存用プラスチックケースをカットして取り付けられるようにしました。こんな感じです。
![IMG_9202.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/63875b54-90d6-c07a-fbe5-df591c1ec933.jpeg)
ここだけ急に工作感出ました。ロボットへの取り付けはつけ外しができるようにマジックテープを使用しました。実際にとりつけるとこんな感じになります。
![IMG_9203.JPG](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/b3ad3b33-5c34-bf5c-c15f-ed0541455ee0.jpeg)
ビジュアル的にほぼマーカになりましたがこれでロボットは完成です。

## ソフトウェア
### ESP32プログラム
　ESP32ではそれぞれhttp serverとして機能し、アクセスのURLによって動作を変えるという実装にしました。なお、今回はMacOSから書き込みをしましたが、ESP32への書き込みはArduino IDEのデフォルトではできず、設定が必要です。このあたりはすでに記事が多くあるので割愛しますが、ドライバのインストールをしておらず若干ハマりました。Windowsだと自動でインストールされるのに。プログラム自体はサンプルのSimpleWiFiServerを修正したものになります。
![スクリーンショット 2021-03-25 23.44.31.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/bcc5c972-f341-edb0-c4af-f25ca2497207.png)
　このプログラムはESP32へのhttpリクエストが何になっているかで条件分岐しています。これを応用すると、前進・右回転・左回転を用意すればロボット動作をhttp通信で制御できそうです。一応IPアドレスはルータ側で固定しておきました。
続いてモータへの指令ですが、今回はPWMを使用します。ESP32ではArduinoでいう`AnalogWrite()`はなく、`ledcWrite()`というメソッドを使用します。ここにはchannelというものがあり、まず`ledcSetup`でchannelを設定してからそのchannelをGPIOに紐付けるという作業が必要です。

``` arduino
void setup(){

...

    ledcSetup(0, 12800, 8);  // 0 channel を12800 Hz, 8 bit(0~255)で設定
    ledcAttachPin(25, 0);    // 設定した0 channelを GPIO25に設定

...

}
```
これでようやくGPIO 25でPWM出力ができるようになります。今回は4PIN必要なので4 channel用意しました。あとは`AnalogWrite()`と同じで`ledcWrite(0, 175)`のように使用できます。今回はすべてPWMは175としました。

### ビジュアルサーボプログラム
　ビジュアルサーボというにはおこがましいですが、毎フレーム目的地に向かっているかを調べています。今回は画像座標系で位置姿勢を扱うことにしました。ユーザからの入力とJij Cloudの最適化により各ロボットが次にどの点に向かえばいいかは与えられます。なのでここではその位置に各ロボットがたどり着けるようなアルゴリズムを書きました。
　ロボットの向きと位置のとり方ですが、Arucoマーカの検出をOpenCVを用いておこなうと、マーカの四隅の点の座標を教えてくれます。`corners`にはマーカの左上から時計回りに4つのコーナーの座標が入ります。そこで向きについてはマーカの横軸のベクトルを算出し、それが画像のx軸に対してどれくらい角度があるかを内積を用いて算出しました。位置についてはマーカの四隅の重心がマーカの中心になるので、その点をロボットの位置の座標とし目的地とのユークリッド距離を算出しました。

``` python
def get_pos_theta(corners, dst):
    # corners: 4 corners of Aruco marker
    # dst: robot destination point

    xvec = corners[1] - corners[0]
    xvec = xvec / np.linalg.norm(xvec)
    rot_sign = np.sign(np.cross(np.array([1, 0]), xvec))
    theta = math.acos(np.array([1, 0]).dot(xvec)) / np.pi * 180. * rot_sign

    pos = np.sum(corners, axis=0) / 4.

    d_pos = np.linalg.norm(dst - pos)

    fai = - math.atan2(pos[0] - dst[0], pos[1] - dst[1]) /np.pi * 180.
    d_theta = fai - theta

    if abs (fai) > 90 and abs(d_theta) > 180:
        d_theta = - d_theta

    return d_pos, d_theta
```
右回りするか左回りするかを決定するためにも角度の符号も重要なので外積を用いて算出しています。
![スクリーンショット 2021-03-27 9.58.52.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/341055/86c52127-6252-cf2c-c9ef-72bb88142006.png)
　続いて算出された角度の差があるしきい値以上であった場合は回転させるように指令を与えます。これは各ロボットのIPアドレスに向けて`requests.get()`を使用しました。今回はIPの最後が10番台で各ロボットのIDと紐付けて固定IPを振りました。なので`id_num`のロボットを下記のようにして動かすことができます。

``` python
    requests.get('http://192.168.0.1' + str(id_num) + '/right', 
                 timeout=timeout_duration)
```
ここで`timeout`を忘れずに入れましょう。そうしないとどこかのロボットが通信できなくなった途端そこでプログラムがwaitし続けてしまいます。この指令により回転するとロボットが移動先の方向に徐々に向いていきます。移動方向との角度差がしきい値よりも小さくなったらそこで回転を止め、前進の指令を与えます。これは上記と同様の指令の与え方になります。前進すると徐々にまた角度がずれることがあります。なので常に移動先へのベクトルとの角度差を監視し、しきい値を超えたら回転する、というようにしました。最後に位置誤差がしきい値を下回ったら目的地に到達したとして動作を終了します。
　この処理を9台に対して毎フレーム行いましたが、そのままの実装だとどうしても`requests.get()`の戻る時間が積み上がってしまいます。そこで今回は9台の位置姿勢の算出と動作指令までをthreadにして並列処理をしました。これによりロボット動作が遅れることがなくなり、フォーメーションを実現することができるようになりました。
　ソースコードについては少し整備してからgithubで公開したいと思っています。

## まとめ
　このようなシステムで9台のロボットのフォーメーションを作り上げることができました。最初に円をきれいに描けたときは徹夜明けの大人3人で歓声を上げてしまいました。HackDayの作品としてはひとまず満足のいくところまで作り上げることができましたが、まだまだ改善したいところはありますので、今後もぜひ開発は続けていきたいなと思いました。
　とくに今回使用したロボットボディはすごく出来がよいと思うので、エンコーダを取り付けたり他のいろいろなセンサを取り付けて面白い移動ロボットに進化させられるのではと思っています。まだあまりこのロボットの記事は見ないので、ぜひいろいろな方がこのキットを使って面白いものを作っていただけたらとも思っています。
　最後になりますが、このような刺激的な1日を用意してくださったHackDayスタッフのみなさま、また夜中に即レスで対応してくださったJij inc.の皆様、そしてあいかわらず一緒にものづくりを楽しめるチームのメンバーにも改めてお礼申し上げます。本当にありがとうございました。ぜひ来年も出場したいです。