こんにちは。rotarymarsです。
K10-K10の基板の記事やutsukushiiotoのハードウェア面の記事で、ハードウェアの方はだいぶ紹介されてきたと思います。なので今回は、2027年度のロボットのプログラムが現時点でどうなっているのか、全体をざっくり紹介したいと思います。
全てを書くととても長くなってしまうので今回は概要のみ書いていきたいと思っています。
なお、プログラムはすべて開発途中のものなので、あくまで現時点での状態です。大きな変更などがあればまた記事にしたいと思っています。
全体の構成
まず、ロボットの中には大きく分けて2つのコンピュータが載っています。
| コンピュータ | 担当 | 言語 |
|---|---|---|
| Raspberry Pi | カメラの画像処理、モーターなどの指令の送信 | C++ |
| ESP32 | モーター、センサー、サーボなどハードウェアの操作 | C++ (Arduino) |
Raspberry Piはモーターやセンサーを直接触らず、すべてUSBシリアル(UART)でESP32にコマンドを送信し、モーターなどを制御しています。
リポジトリとしては以下のようになっています。
| リポジトリ | 中身 |
|---|---|
| robocup2027-raspberrypi-program（非公開） | Raspberry Piで動くメインのプログラム |
| robocup2027-esp32-program | ESP32のファームウェア |
| robocup2027-pcb-data, robocup2027-robot-caddata | 基板と機体のデータ（今回は触れません） |
ちなみに、昨年度まではRaspberry Pi側のプログラムはPythonで書いていましたが、今年度はすべてC++で書き直しています。理由としては、処理速度を上げたかったのと、機械学習のモデルもLibTorch（PyTorchのC++版）で動かせるので、言語を一本化したかったからです。
また、Raspberry Piのプログラムが非公開な理由として、管理の容易さから今年は機械学習に用いる画像などもリポジトリに保管しており、公開しないという判断になりました（意図しないものが画像に写ってしまう可能性などもあるため）。
ESP32側のプログラム
先に、簡単な方のESP32から見ていきます。
ESP32が担当しているのは以下のものです。
| 部品 | 用途 |
|---|---|
| STS3032 ×4 | 車輪（FEETECH様にいただいたサーボです） |
| STS3032 ×1 | レスキュー用のアーム |
| PWMのサーボ | 坂道（回収したボールを入れておく部分）の開閉 |
| VL53L0X ×4 | ToF距離センサー（左×2・前方×2） |
| BNO055 | 9軸センサー（主に向きを取るのに使っています） |
| ブザー | 状態を音で知らせる |
| スイッチ×2 | スタート・ストップ |
通信の形式
Raspberry Piとは、テキストで1行ずつ命令を送り合うという、かなり素朴な方式でやり取りしています。
送信: 12 MOTOR 3000 3000
返信: 12 ok
送信: 13 TOF f
返信: 13 ok 210 205
送信: 14 SWITCH
返信: 14 ok on
先頭の数字はIDで、Raspberry Pi側が命令ごとに新しい番号を振っています。返ってきた返信のIDが送ったものと一致することを確認してお互いのやり取りに整合性をとっています。
なぜこんなことをしているかというと、ESP32が起動した直後に出すメッセージ（IDは0）や、タイムアウトした命令の返事が遅れて返ってきたときに、それを今の命令の返事だと勘違いしてしまうことがあったからです。これで返信がずれてもおかしなことにならないようになりました。
命令の種類はこんな感じです。
| 命令 | 内容 |
|---|---|
MOTOR <l> <r> |
左右の車輪の速度を指定 |
ARM <angle> |
アームの角度を指定 (0 ~ 4095) |
CAGE <O/C> |
坂道を開ける・閉める |
BUZZ <周波数> <時間> |
ブザーを鳴らす |
BNO |
向き・傾き・加速度を返す |
TOF <l/f/r> |
指定した方向のToFの距離を返す |
ODOM |
向きと、左右の車輪の回転量をまとめて返す |
SWITCH |
スイッチの状態を返す |
車輪について
今年度は車輪にSTS3032というシリアルバスサーボを使っています。普通のモーターと違って、1本の線に複数のサーボをつないで、IDで区別して命令を送ることができます。
また、STS3032は車輪として回している間も軸の角度（1周で0 ~ 4095）を返してくれるので、それを10msごとに読んで積算することで、どれだけ進んだかを測れるようにしています（これをODOMで返しています）。
ToFセンサーについて
VL53L0Xは、電源を入れた時点ではすべて同じI2Cアドレス(0x29)になっています。そのまま4つつなぐとぶつかってしまうので、起動時にいったん全部のXSHUTピンをLOWにして寝かせておき、1つずつ起こしてはアドレスを0x30、0x31、……と書き換えていく、ということをしています。
初期化に失敗したセンサーはもう一度寝かせておき、読み取ろうとしたときには-1を返すようにしています。
Raspberry Pi側のプログラム
内部では「ライントレースモード」と「レスキューモード」の2つを切り替えて動いています。
カメラ
カメラは2つ使っています。
| カメラ | 解像度 | 用途 |
|---|---|---|
| ライントレース用 | 320 × 240 | 床の黒線と緑マークを見る |
| レスキュー用 | 2304 × 1796 | 被災者（ボール）と避難所を探す |
カメラの制御はlibcameraをC++から直接叩いています。昨年度のPythonのpicamera2のpre_callbackと同じような仕組みも作ってありますが、今は後で説明する理由で使っていません。
ライントレース
機械学習はやめました
実は今年度、最初は機械学習でライントレースをしようとしていました。
具体的には、人間がロボットをラジコンのように操作して「カメラの画像」と「そのときのモーターの値」の組を記録し、それを小さなCNNに学習させて、画像からモーターの値を直接出させるという方法です（behavior cloningと呼ばれるものです）。
ただ、30分程度の走行ではなかなか学習ができず、また機械学習という手法では万が一本番環境などで動かなくなってしまった場合にデバッグができないという問題があると感じました。
そこで、昨年度のPythonのアルゴリズムをベースにして、C++でアルゴリズムベースのライントレースを作り直すことにしました。
制御ループ
ライントレースの処理は、20Hzのループで以下を繰り返しています。
- 1秒に1回、スイッチの状態を確認する（OFFなら止まって待つ）
- カメラから画像を1枚取る
ODOMで向きと進んだ距離を読む- 画像から左右のモーターの値を決める
MOTORを送る- 銀色のテープの判定に画像を渡す（後述）
最初はカメラのコールバックの中で画像処理をしていたのですが、そうするとカメラのフレームレートで処理が回ってしまい、「何フレーム連続で〜だったら」という判定がループの速さによって変わってしまうという問題がありました。なので、今は普通のループの中で画像を取ってくるようにしています。
また、6のように重い処理は必ずモーターの命令を送った後に置いて、判断してからモーターが動くまでの間に余計な処理が挟まらないようにしています。
画像からどう判断しているか
ざっくりとした流れはこのようになっています。
- 画像を二値化して、黒線のマスクと緑のマスクを作る
- 緑マークを見つけて、それが「どちらに曲がるべきか」を判定する
- 曲がらない方の線を画像から消す
- 残った線の「先端」に向かって進む
一般的に緑のマークは曲がる処理として例外的に処理をしますが、今回のプログラムでは緑マークで右に曲がると決まったら、左や直進の線を画像から消してしまいます。そうすると、その後のライントレースの処理は曲がる方向のことを何も知らなくても、そもそも見える線が1本しかないので正しい方向に進めます。これは昨年からのプログラムの考えを引き継いでいますが、不要な線の除去の部分を最適化し、本当に必要な線のみが残るように調整しました。
緑マークの判定
今年のロボットはライントレース用のカメラがかなり低い位置についているので、緑マークが画面の1/5 ~ 1/3くらいを占めてしまい、交差点全体を見ることができません。
そこで、緑マーク自体ではなく、マークの周りの床を見て判断しています。黒線によって床はいくつかの領域に区切られるので、マークがある領域が
- 画面の下の端の、来た線の片側だけにつながっている → 交差点の手前のマーク。そっちに曲がる
- 画面の下の端につながっていない → 交差点を過ぎた後のマーク。無視する
- 下の端の両側につながっている → どちらとも言えない
という具合です。これならロボットが少し斜めに入ってきても同じように判定できます。
また、1フレームだけで決めずに、何フレームか続けて同じ判定が出たら曲がることにしています。マークはロボットが曲がりきる前に車体の下に隠れてしまうので、一度決めたら1.5秒は曲がり続けるようにもしています。
両側にマークがあったらUターンです。Uターンは「どこで回るのをやめるか」がとても難しくて、交差点の上で回っているので、「前に線が見えたら止まる」だと90度回ったところで止まってしまいます。そこで、画像の中の線の向きの変化から、自分がどれだけ回ったかを数えて、150度以上回ったうえで前に線が見えたら止まる、ということをしています。
どこに向かって進むか
進む方向は、線の「先端」に向かうようにしています。
ここでの「先端」は、画面の上の方という意味ではなく、線に沿って一番遠いところです。直角のカーブだと、画面で一番上にあるのは角の部分ですが、線に沿って一番遠いのは横に伸びている方なので、ちゃんとそっちを向けるようになります。
先端が複数ある（交差点など）ときには、前回の線の向きに一番近いものを選んでいます。
線が見えなくなったとき
直角に曲がると線はほぼ必ず一度画面から外れるので、線が見えなくなることは普通に起こります。
線が見えなくなっても止まらずに、
- 曲がっている途中なら、そのまま曲がり続ける
- 線が途切れている（ギャップ）なら、線が向いていた方向にまっすぐ進む
ようにしています。このとき、ギャップとカーブを見分けるのが難しいのですが、直前まで大きく曲がっていたかどうかで区別しています。
銀色のテープの検出
レスキューエリアの入り口には銀色のテープが貼ってあるので、それを見つけたらレスキューモードに切り替えます。
これは80×60に縮めた画像を入力にした小さなCNNで判定しています。ラベル付けと学習のツールもC++で自作しています。
この判定は別のスレッドで200msに1回だけ動かしているので、ライントレースのループが遅くなることはありません。2回続けて銀色だと判定されたらブザーを鳴らしてレスキューモードに切り替えます。
レスキュー
ボールなどの検出
レスキュー用のカメラの画像からは、YOLOで以下の5種類を検出しています。
| クラス | 内容 |
|---|---|
| Black_ball | 黒い被災者 |
| Silver_ball | 銀色の被災者 |
| Green_zone | 緑の避難所 |
| Red_zone | 赤の避難所 |
| Exit | 出口 |
Pythonで学習させたYOLOのモデルをTorchScriptという形式に書き出して、それをC++のLibTorchで読み込んでいます。Raspberry Piで動かすので、入力画像は160×160と小さくしています。こちらも別スレッドで、13fpsくらいで回しています。
状態の管理
レスキュー中は、今何をしているのかを状態として持っていて、それに応じて動きを変えています。
| 状態 | 内容 |
|---|---|
| SEARCH | その場で回ってボールを探す |
| APPROACH | 見つけたものに向かって進む |
| CATCH | ボールをつかむ（未実装） |
| RELEASE | 避難所にボールを置く（未実装） |
| EXIT | 左の壁に沿って進んで出口を探す |
| STOP | 止まる |
APPROACHでは、検出したものの位置が画面の中央下あたりに来るように、ずれに比例してモーターの値を決めています（比例制御）。
EXITでは、左のToFセンサーとの距離を一定に保ちながら壁に沿って進み、前に壁があれば左に90度回り、左の壁がなくなれば左に曲がって進む、ということをしています。90度回ったかどうかはBNO055の向きで判断しています。
また、3分経ったら強制的にEXITの状態になるようにしています。
CATCHとRELEASEはまだ中身がなく、ボールをつかむ部分はこれから作っていきます。また、ライントレースで銀色のテープを見つけた後、レスキューの処理に入るところもまだつながっていないので、ここも早めに直したいと思っています。
その他のツール
ロボット本体のプログラム以外にも、開発用のツールをいくつか作っています。
| ツール | 内容 |
|---|---|
| 遠隔操作・記録ツール | PCからロボットをラジコンのように操作して、走行を記録する |
| 走行記録 | ロボットが自分で走ったときの画像と、そのとき出した命令を記録する |
| ビューア | 記録した走行を、ロボットと全く同じライントレースの処理にかけて、1フレームずつ何を判断したかを見る |
| レスキュー用ストリーム | レスキュー用カメラの検出結果をリアルタイムでPCに送って見る |
| ToFチェッカー | ToFセンサーの確認 |
特にビューアは便利で、ロボットが交差点で間違えたときに、そのときの画像を持ってきて「緑マークをどう判定したのか」「どの線を消したのか」を1フレームずつ確認できます。ロボットと同じライントレースの処理をそのまま使っているので、ロボットとビューアで判断がずれることもありません。ライントレースのパラメータの調整は、ほとんどこれを見ながらやっています。
ただし、記録した画像を使っているので、命令を変えてもロボットの動きは変わりません。なので、「その画像に対してより早く・強く反応できているか」はわかりますが、実際にそれで線に復帰できるかどうかは、結局コースで走らせてみないとわかりません。
また、Raspberry Pi上でのビルドがとても遅い（LibTorchを使っているファイルは1つで1GBくらいメモリを使います）ので、PCでRaspberry Pi用にクロスコンパイルできるようにもしました。Raspberry Pi OSのイメージからDockerのイメージを作り、そこからヘッダーやライブラリをコピーしてきて、PCのclangでarm64向けにビルドしています。Raspberry Piの上でビルドするよりずっと速く、数秒で終わるようになりました。
最後に
今回は、2027年度のロボットのプログラムが現時点でどうなっているのかを、ざっくりと紹介しました。
今回は全体の紹介だったので、それぞれの部分については、また別の記事で詳しく書けたらいいなと思っています。
チームメンバーにも、この記事を読んでプログラム全体の雰囲気をつかんでもらえたら嬉しいです。
ありがとうございました。