マイクロマウス

マイクロマウス向け基準データ取得システムの紹介

はじめに

真値、知りたくないですか？

扉絵

マイクロマウスにおいて、真の走行軌跡を検証することは難しい課題の一つです。 一般的には高フレームレートのカメラを使ってロボットの動きを撮影し、動画を見て位置を確認する方法が用いられています。

高フレームレートのカメラを使った軌跡の検証には、いくつか大きな課題があります。

  • 定量的な位置情報を得るのが難しい
  • 時刻の同期が難しい
    • ロボットの内部ログとの突き合わせが難しい

このような課題を解決するために、車体LEDのストロボ発光を外部のカメラで撮影することで、位置・角度を定量的に取得して車体ログと対応付ける仕組みを構築したので紹介します。

このシステムで得られるデータは厳密な意味での真値ではありませんが、さまざまな検証を行うための基準としては十分に有用なレベルに達しました。 以降では、真値に近いものを目指して取得した外部計測データを「基準データ」と呼ぶことにします。

※このポストの作成にあたり一部生成AIを使用していますが、内容は人間が確認しています。

何のために基準データを取るのか

車体で動作させている推定器や物理モデルの出力と、外から見た定量的なデータを比較することで、モデルの妥当性や精度を評価できます。

具体的には、以下のような用途を想定しています。

  • 車体が実際にいた位置における光センサの値を用いて、センサモデルを構築する/精度を評価する
  • 実際の走行距離と推定距離を比較して、ホイール直径や加速度センサの感度を調整する
  • 実際の位置と推定位置を比較して、自己位置推定の誤差要因を分析する

システム全体像

定量的にデータが取れたとしても、精度が出ていなかったり、高速走行で使えなかったら意味がありません。 そこで、このシステムでは以下を満たすことを目標にしました。

  • 位置情報を定量的に得られる
  • 角度情報を得られる
  • 車体ログと時間同期できる
  • 高速走行していても測定できる

このシステムを実現するうえで最大のハードルは、車体ログと外部カメラの時間同期です。
そこで、車体に搭載したLEDのストロボ発光を外部カメラで撮影して両者の時間を同期するアイデアを採用しました。

システム構成

全体の流れは以下の通りです。

  1. 車体左右のLEDをストロボ発光させ、その時刻を車体ログに記録する
  2. 外部カメラでその発光を撮影する
  3. 映像から発光点を抽出して物理座標に変換する
  4. 左右LEDから車体中心座標と角度を算出する
  5. 発光時刻を手がかりに、車体ログと外部計測結果を対応付ける

最終的に得たいのは、外部から見た車体の$x, y, angle$と、それに同期した車体ログです。 なお、発光点の誤検出除去やログとの対応付けには人の判断が入るため、現状は完全自動ではなく半自動運用です。

運用フロー

ここからは、実際にどうやって基準データを得ているかを順番に説明します。

カメラによる撮影

ストロボスコープ方式なので、カメラは長時間露光で撮影する必要があります。 しかし、走行中ずっと露光を続けると、露出オーバーで真っ白になってしまいます(もしくは、対策するために部屋を真っ暗にする必要があります)。 これでは使いづらいので、フレームの間隔が開かないように動画モードで撮影し、後からフレームを結合して長時間露光のような映像を作る方法を採用しました。

$露光時間 = \frac{1}{フレームレート}$となるように設定する、ということです。一般的なカメラではこの設定ができないことが多く、Raspberry Pi Cameraや産業用カメラのような柔軟な設定が可能なものを使う必要があります。

ただし、解析用途でネックになりがちなローリングシャッターは、ここでは問題になりません。(私も実際にローリングシャッターのカメラを使っています)

ストロボ発光を実際に撮影すると、以下のような画像が取得できます。
本体はブレていますが、青色LEDのストロボ発光はくっきりと撮影できていることがわかります。

ストロボ発光を撮影したフレームの例

ストロボ発光を撮影したフレームの例

image
image
image
image

LEDの発光時間は(ソフトウェアと回路をきちんと設計しておけば)任意に短くできるため、高速走行でもブレません。 現在は10usに設定しているので、10m/sで走行したとしても残像は0.1mm程度です。

画像処理と物理座標への変換

撮影した画像からストロボ発光点を抽出し、物理座標に変換する処理を行います。
この部分はかなり泥くさいので、ここでは処理の流れだけをざっくり説明します。

画像処理の流れ

画像処理の流れ

ピクセル座標から物理座標への変換用情報の準備

「画像処理の流れ」の上側の流れです。$物理座標=f(ピクセル座標)$ となるような$f$を構築することが目的です。

最終地点から辿った方がわかりやすいので、逆順で説明します。

  • ホモグラフィ変換を使えば、ピクセル座標→物理座標の変換が行える
  • OpenCVには2つの平面同士の点の座標の対応のリストからホモグラフィ行列を推定する関数が存在する
  • 既知の座標のペアがほしい→柱、壁は検出が難しい&視差の問題もある→自宅迷路なら床面にグリッドが引いてあるので、この交点が使える
  • 画像上でグリッドの交点を検出したい→グリッドの直線をハフ変換で方程式にすれば計算できる
  • グリッドをハフ変換したい→きれいに二値化されたグリッドがほしい
  • 二値化されたグリッドがほしい→壁が邪魔、照明による床面の明度勾配が邪魔、車体が写っていると邪魔
    • 壁が邪魔→壁を検出してマスク
    • 明度勾配が邪魔→ぼかし画像を使って補正
    • 車体が写っていると邪魔→時系列が違う複数フレームのメディアン画像をベースにする

ストロボ発光点の抽出

「画像処理の流れ」の下側の流れです。

色を使って検出しています。 以前はHSV色空間に変換していろいろ試していましたが、今は$B*2-(R+G)>thresh$のような単純な式で二値化し、面積でフィルタリングした後にチャンクの重心を計算しています。

重心計算を行っているため分解能は上がっているはずですが、実際にどの程度向上しているかはまだ評価できていません。

画像処理をどれだけがんばっても、LEDを撮像したときに飽和して真っ白になっているとどうしようもないので、
LED発光強度やカメラの感度を調整するのが近道です。

物理座標への変換

「画像処理の流れ」の最終地点です。

ホモグラフィ変換で変換しますが、一工夫あります。

画像はレンズによる歪み、カメラ設置位置のばらつき、迷路自体の反りの影響で大きく歪んでいるため、画像全体に一括でホモグラフィ変換をかけても使い物になりません。
そこで、変換したいピクセル座標の付近の点だけを使ってホモグラフィ行列を求めることで、線形だと近似できる範囲内で座標変換を行うようにしています。

LED座標から車体座標・角度への変換

左右のLED座標から車体中心座標を算出するのは幾何学的に計算するだけなので、難しいことはありません。角度も同様です。

一方で、その前段階にちょっとした罠があります。
画像処理によって得られる物理座標系でのLED座標のリストには、ある程度のエラーが含まれます。
同じ座標が二重に検出されていたり(複数フレームにまたがってLEDが光っていた場合)、逆にノイズとして扱われて欠落していたりします。

画像処理の精度を限りなく高めるという方向性は画像処理の性質上非現実的なので、
そこそこの精度で妥協して後から人力でフィルタリングするソフトウェアを開発しました。

車体ログとの同期

時刻同期についても、半自動・半手動で対応点を選択するソフトウェアを開発しています。
上でフィルタリングした点に対して、車体側の発光ログをスキップするなどの判断が必要になるためです。

車体ログとの同期完了例

車体ログとの同期完了例 / 緑:カメラから算出した座標 赤:車体ログの推定座標

これで外部から座標と角度を取得し、車体ログと時刻を同期する基準データ取得システムは完成しました。

取得した基準データの使い道

パラメータの調整

  1. 総走行距離や角度からタイヤ直径やジャイロ感度などを調整
    これが地味に便利です。調整用の直進を何度も走らせなくても、タイヤ直径やジャイロ感度を調整できます。
  2. 壁切れ補正が発生したときに、機体が実際にはどこにいたのかがわかります。
    滑って位置がズレているのか壁切れのパラメータがズレているのか悩むことはありません。
  3. 同様に、滑りパラメータの調整。実際の軌跡がわかるので、スリップアングルのスティフネス的なパラメータを後から推定できます。
    (そして、スリップアングルのモデルでは説明しきれない挙動をしていることがわかり、頭を抱える)

光センサモデルの構築

光センサの反射強度は、壁までの距離と角度の関数になっているはずです。$反射強度=f(距離, 角度)$
位置と光センサの反射強度のペアが取れるので、この関係を観察することができます。

角度=直進の条件で距離-反射強度テーブルを作るのは一般的になってきていますが、もう1歩踏み込んで、
角度も含めた関係式やテーブルを構築できる可能性が見えてきています。

うまくモデルが構築できたら、ターン中なども積極的に光センサを使った補正ができるかもしれませんね。

一回の走行で得られたデータは以下のようになります。

どうしても普通に走らせると、直進中のデータに偏ってしまいます。
データ取得用に蛇行しながら走る機能があると、よりよいデータが取れそうです。

推定器の調整

X-Y-Angleだけでは少し使いづらいので、同期しているセンサのデータと組み合わせて、速度や角速度も含めた制御周期のリファレンス時系列データを作ります。
RTSスムーザーというカルマンフィルタの後処理版を使って補完を行っています。(基準データを、非常に高精度な位置・角度センサによる計測として扱う)

RTSスムーザーにより、制御周期のリファレンス時系列データができあがるので、これにぴったり一致するような推定器が理想の推定器となります。

また、車体ログにはすべてのセンサの生データとモータへの出力を記録しているので、推定器はオフラインで回すことが可能です。
正解に相当するデータと入力データが揃った状態で開発を進められるので、非常に効率よく開発できます。(できるはずです…環境を整備中。)

すべり挙動の解析

RTSスムーザーの状態ベクトル/モデルには横滑りを持たせているので、それらしい横滑り速度が出力されます。

これを元に、横滑り速度を横軸、横方向加速度を縦軸にプロットするとタイヤモデルのようなものが得られます。
観測手段を増やすことで経験的に考えていた事象を定量的に理解することができます。

(RTSスムーザーのモデルやパラメータの設定が妥当なのか、という論点はありますが…)

課題

大きく2つの課題が残っています。

1つ目は視差への対応で、迷路底面グリッドで算出したホモグラフィ行列で、迷路から3mmほど浮いたLEDを変換しているため、視差によるバイアス誤差が乗っています。
計算で補正できるはずなのですが、力尽きていて未実施です。

2つ目は、調整用のグリッドに依存していることです。将来的には壁や柱を特徴点にして座標変換を行えるようにし、大会会場などでも測定できる、より実用性の高いシステムにしたいと考えています。

おわりに

マイクロマウスをダシにしてシステムを開発し、実用できるところまで持っていけたのは非常に楽しい営みでした。
みなさんも変な開発をやりましょう。

Appendix:使用機材

  • カメラ系
    • Raspberry Pi Global Shutter Camera
      • 現在は使用していない
      • もう少し解像度がほしかったのでHQ Cameraに乗り換えた
    • Raspberry Pi HQ Camera
      • 現在は使用していない
      • 性能に問題はなかったが、ノートPCだけで運用したくなったので乗り換えた
    • DFK37BUX226
      • 使用中
      • 解像度4000x3000 12bitはRAWで録画しようとすると処理やディスクの書き込み速度がギリギリ
      • 設定項目が保証されているUSB接続のカメラは少ない
      • Webカメラを何個か買ってダメだったので諦めてこれを選択
    • Cマウント レンズ 8mm
      • 何個か試してよかったものを採用。
      • 焦点距離の表記は無法地帯なので気合で選定する
  • 三脚
    • 三脚
      • 高さが合えばなんでもいいはず
      • 大会会場に持ち込んで運用することを考えた時に、持ち運びが楽そうで高さが稼げるものを選定
    • スライディングアーム
      • 選択肢があまりない
    • 延長ポール
      • スライディングアームの長さ延長用
      • アルミ製は重くてバランスが悪かったのでカーボン製を採用
© 2019 - 2026 Shunsuke Sakura
Hugo で構築されています。
テーマ Stack は Jimmy によって設計されています。