Architect - マイクロマウス2016出場ライントレースロボット

新型のライントレースロボットを製作し 、2016/11/19にマイクロマウス2016のロボトレース競技へ出場してきました。

ロボットの名前はArchitect（アーキテクト）と言います。名前には『カメラ画像を入力とした小型自律ロボットの新しいあり方を創造する』という意味が込められています。

Architectの特徴は上部につけた魚眼カメラです。魚眼カメラは下向きに取り付けてあり、前後方向の視野が180度あるので、コースの広い範囲を一度に計測することができます。

ArchitectはスティックPCとマイコンの両方を搭載しています。

スティックPCはIntelのCompute Stick STK2MV64CCで、CPUはCore m5が搭載されています。Core m5はMacbook等にも搭載されており非常にパワフルです。このCompute Stickはコース画像の撮影や二値化、輪郭抽出などの画像処理、地図作成、起動計画まで担当します。

マイコンはSTM32F411で1kHz周期のリアルタイムで速度と角速度の制御およびオドメトリ、バッテリー電圧の監視、LEDの点灯制御を担当します。

マイコンとCompute Stickの間はHDMIを通じて通信しています。正確にはHDMIの中のDDC (Display Data Channel)を使って通信しています。DDCはI2Cと互換性があります。

Compute StickのOSはLinuxのUbuntu 16.04です。Ubuntuは標準でI2Cをサポートしており、非常に面白いことに、UbuntuからはDDCも他の通常のI2Cと同等に扱えることがわかりました。使い方は、特別なドライバは不要でI2CのデバイスファイルをRead/Writeするだけです。私の環境ではデバイスファイルは/dev/i2c-1でした。もちろんi2cdetectやi2csetコマンドが使えます。なおCompute Stick側が常にI2CのMaster、マイコン側がSlaveになるようです。

DDCをI2Cとして扱う時の注意点として、HDMIポートのHot Plug Detectピンをプルアップしておく必要がありました。このピンは通常は挿抜を検知するために使用されており、少なくともCompute Stickではこのピンがプルアップされていないと正常にI2Cの信号が出力されませんでした。プルアップ抵抗は仮に1kΩとしました。

I2Cの信号は5V 100kHzでした。I2Cの信号ライン(SCL, SDA)のプルアップ抵抗はCompute Stick側に内蔵されていました。

マイコン側の電源電圧は3.3Vでしたがピンに5V耐性があり直結できました。

さらにArchitectはROS Kineticに対応しています。特にカメラ画像の取得や、デバッグデータの記録用にrosbag、パラメータ調整のdynamic reconfigure等を活用しました。時間がなかったのでそれ以上の上位の制御には活用できませんでしたが、今後さまざまなROSパッケージを活用していきたいと思います。

以下に画像処理の様子を示します。

なお、Architectはアニキ氏との合作で、ハードウェアはほとんどアニキ氏に作っていただきました。非常に感謝しています。私が担当したハードウェアはCompute Stickとカメラの選定、カメラを固定する白い部品の設計だけです。

一方、ソフトウェア（マイコンのファームウェア、Ubuntu側のソフトウェア、I2Cの通信）はすべて私が担当しました。

今年のマイクロマウスにおいてArchitectは残念ながら予選敗退でしたが、来年はこの機体をさらにパワーアップさせて是非とも良い成績を残したいと思います。以下に参考までにPS3コントローラでリモートコントロールしている様子を示しますが、ハードウェア自体は非常に完成度が高くキビキビうごきます。潜在能力は非常に高いはずです。後は頭脳がよくなれば…自律走行でもビュンビュン走ることでしょう！

参考URL＆製品

http://www.hogetan.net/note/memo/pi_i2ccmd.html#id2

https://devtalk.nvidia.com/default/topic/856949/jetson-tk1/accessing-gpio-without-using-sudo-su/

http://d.hatena.ne.jp/penkoba/20131222/1387689702

http://www.aitendo.com/product/13682

マイクロマウス2015所感とMETEORA 4の紹介

マイクロマウス2015に出場された方々および運営の方々お疲れ様でした。

私のロボトレーサ、METEORA 4は予選敗退でしたが全力を尽くしましたし、様々な人から反響を得られたので満足でした。

METEORA 4はカメラ映像から白線を認識して走るロボトレーサです。認識処理は名刺サイズコンピュータのRaspberry Pi 2 Bをつかっています。

今年は昨年に比べてソフトウェアを大幅に進化させ、コースを認識する画像処理にSLAM(Simultaneous Localization And Mapping)という技術をつかいました。SLAMは近年自動運転車の制御にも用いられている、未知の環境下でロボットが自律移動するための技術でカメラなどの外界センサとオドメトリなどの内界センサからのデータから、ロボット周辺の環境地図の作成と、ロボット自身の自己位置推定を同時に実行します。

私はこのSLAM技術で、ロボトレースコースのS字カーブを連結させて直線みたいに走ったり、ルール違反にならない範囲でカーブの内側を走って走行距離を短縮したりという賢い動作をロボトレーサにさせたかったのです。

METEORA 4が使っている種類のSLAM(占有格子地図ベースのFastSLAM)では、計算の過程で少しずつ形の異なる地図を数万枚のメモリ上に記憶し、これらをカメラ映像とオドメトリの情報により常時更新する必要があるのですが、Raspberry Pi 2 BではCPUの動作速度とメモリの容量が限られているため地図は100枚だけつかいました。地図にはコースを12mm間隔で格子状に区切って白黒の濃淡を記録してあります。

以下の動画は手押しでコース上をロボットを走らせながらカメラとオドメトリの情報を録画して、あとからオフラインでSLAMをシミュレーションした様子の動画です。

ロボトレース予選では走行の途中からMETEORA 4が制御不能になりコースアウトしリタイアとなりました。 METEORA 4では地図をたった100枚だけしか持っていませんが、 SLAMでは機体の移動にともないそれぞれの地図が拡大していくため、メモリの消費量が急速に増大しどうやら処理速度が追いつかなかったようです。　

またRaspberry Pi 2 BはクアッドコアのARM Cortex-A7のCPUと1GBのRAMを搭載していますが、METEORA 4ではOpenMPで並列化し4個のコアをすべて使いきってました。
このハードウェアのままでは、これ以上計算速度を上げるのは難しそうです。次の一手を考えなくてはいけません。

制御方策の生成

前回の記事ではロボトレースの地図からショートカットする経路を生成しましたが今日、そこから更に制御方策(制御の計画)を立てることが出来ました。制御方策とは、状態ベクトル(x座標とy座標からなる二次元のベクトル)から、制御量(ロボットの角度θ)への射影です。すなわち制御方策とは、ロボットの自己位置から、姿勢の目標角度を決定するための対応表なのです。

下図は制御方策を可視化したものです。緑の直線が目標角度の方向を表しています。直線には赤い印で頭の方向を示してあります。

一般的にこの制御方策はロボットのxとyの位置に加えロボットの姿勢θが加わった三次元になるようですが、勉強不足のためMETEORAではとりあえずこの制御方策をxとyの二次元で作りました。二次元なら三次元よりはるかにメモリの占有量も少ないし…。

このような制御方策を作ることのメリットは、ロボットの行動や計測の不確かさを考慮した制御ができることらしいです。らしい、と書いたのは私自身がまだ腑に落ちていないため、ただ確率ロボティクスの本の言葉を引用しているためです。ロボトレースの大会が終わったらさらに勉強したい。

最短経路生成

前回FastSLAMでコース形状を取得するところまで行きましたが、この地図があればすぐロボットが走りだすというわけでは有りません。ロボットにどのような経路を通るべきか指示する経路計画が必要です。

経路計画の生成は、価値反復という手段が一般的に用いられるようですがロボトレースでは高級すぎます。ロボトレースでは単純に、スタート地点からロボットの初期の進行方向へ向けて、地図上の白いラインを追跡すれば大まかな経路は得られます。この追跡の結果が図の赤い十字です。一般的に、線の検出ならハフ変換が用いられますがハフ変換は計算量と精度がトレードオフの関係にあり、Raspberry Piにやらせるのは酷なので、今回は、逐次的な追従アルゴリズムを作り追跡させています。そのうちこのアルゴリズムを記事に書きたいなぁ…。

ちなみに、ロボットをただラインの追跡結果(図の赤い十字)に沿わせて走らせるのは非効率です。せっかくFastSLAMで広範囲の地図が取得できるわけですから、ロボットにカーブでは内側を走らせたり、スラロームではラインを無視しまっすぐ走らせたりするなど、ショートカットさせて探索走行や周回走行の所要時間を減らすのが理想です。

そこでショートカット経路生成アルゴリズムを考えました。アルゴリズム適用結果が図の緑の十字です。このアルゴリズムは、単純に注目点の前後2点の中点を、新たな点として採用することを逐次的に繰り返し、ショートカット経路を生成します。このアルゴリズムなら、逐次計算を繰り返すことでカーブでは経路がどんどん内側になるし、スラロームは経路が直線に変換されます。

なお、地図を生成にせっかく確率的手法であるFastSLAMを使ったのに、生成された図の経路は決定論的です。今後、これを確率論的な制御計画すなわち状態ベクトルから制御量への射影(制御方策)に変換する予定です。土曜日中に完成すると良いなぁ。


もう全国大会が近いですが、なんとかを完成させたいものです。

FastSLAMが動くようになった。

だんだんFastSLAMがそれらしく動くようになってきました。

メモリ容量とCPU速度の限界に挑む感じです。現在2fpsくらいで動作しています。

狭い間隔で直線やカーブが並ぶ似たようなパターンが連続する場所は地図の生成に失敗することがあります．

地図生成の成功率を上げるには、如何にパーティクルフィルタのパーティクルの数を増やすか、パーティクルの喪失を防ぐかにかかっている気がします。難しい。

計測結果のマッピング

カメラで計測したコース形状を、地図上にマッピングすることに挑戦しています。

上図は地図上にマッピングしたコース、下図はカメラの生画像です。

いまは計測した結果をオドメトリと合わせてコース上にマッピングしているだけなので、オドメトリの誤差のせいで地図の繋ぎ目は滑らかではなくギザギザします。またコースの傾きなども考慮されていないので形状も歪んでいます。

これから、こうしたオドメトリの誤差すなわち実世界に存在する不確実さを考慮した自己位置推定および地図生成、いわゆるSLAM(Simultaneous Localization and Mapping)のプログラムを開発していきます。SLAMを実装したあとは、地図の繋ぎ目は滑らかに接続されるはずです。

ROS (Robot Operating System)等のSLAMをすでに実装してあるオープンソースソフトウェアを使えばもっと簡単に出来たかもしれないのですが、SLAMを勉強するためにあえてすべて自分の手で実装しました。

本日の内容に達するまでSLAMに関する学習およびテストプログラムの作成、カメラの逆射影変換テーブルの作成や、地図管理クラス作成など勉強や下準備に何ヶ月もかかりました。

準備だけでもう疲労困憊ですが、これからSLAMの本領、楽しい領域に突入するわけです。

初志貫徹、今年度のロボトレースでSLAM走行を披露できるように頑張ります。

SLAMへの道

ロボトレース用ロボットのMETEORAの画像処理部分を作りこんでいます。

METEORAにはカメラが付いておりその画像からコース形状を判別します。

昨年度はコース形状判別のために、白線のエッジ検出を用いていましたが、データの扱い方法が難しくロバストなライントレースはできず、すぐコースアウトして暴走していました。

そこで今年はエッジ検出ではなくコースの地図(占有格子地図)を作成することでデータを簡単かつロバストに扱えるようにします。

図はカメラ画像と、カメラ画像を逆射影変換することで復元されたコースの上面図画像です。この上面図から占有格子地図を作る予定です。

上面図はよく見ると左上と右上の角が牛の角のように尖っていることがわかると思います。

これはカメラのレンズ歪を補正した結果です。レンズ歪補正を用いると直線がまっすぐ直線として写るメリットが有ります。

しかし、レンズ歪補正を含む逆射影変換は計算に非常に時間がかかるので予め変換テーブルを計算しておきます。Raspberry Pi 2とMathematicaで角度0.5度、位置5mmの分解能の変換テーブル作らせようとしたところ10日ぐらいかかるらしくこれでは実用に耐えないので対応策を考え中です…。

[ロボトレース]実世界情報処理

METEORA3.0の画像処理がだいぶ良い所まで来ました。

スタート時の追従すべきラインを赤線表示中

METEORA3.0とコースの位置関係

METEORA3.0はカメラの射影変換*により、カメラでとらえた画像から、実世界のコース形状を計算しています。すなわちピクセル単位はなく、実世界の座標[mm]でコース形状を捉えているわけです。

困ったことに2D画像からは奥行がわからないので、実世界(3D)のコース形状を計算するためにカメラとコース平面の位置関係から連立方程式を解きました。

結構がんばったので、大会が終わったらその詳細をこのブログに報告したいなぁ。

-----
*カメラの射影変換の資料はOpenCVの資料が詳しい。
http://opencv.jp/opencv-2svn/cpp/camera_calibration_and_3d_reconstruction.html

画像処理

METEORA 3.0ですが、ロボトレースのコースのライン形状をカメラで認識するために大まじめに画像処理しています。コース画像の二値化、輪郭抽出、ラインのセグメンテーションと、段階を踏んで実装中です。
あと二十日程度でちゃんと走るんだろうか。。

画像左がラインのセグメンテーション結果　右が生のモノクロ画像

METEORA 3.0には、本業で得た知識を私利私欲のためにふんだんに使っています。

まあ逆に、趣味で蓄えた技術を仕事に還元しているから許してもらいましょう。

私の場合、仕事と趣味がお互いのレベルを高め合う補完関係を成しています。

しかし、やっぱりOS、もといLinuxがロボットに入っているとすごく楽ですね。

OpenCVを使えるから画像のクラスとか関数とかを自分で作る必要はないし、

画像処理結果もWifiとVNC経由でPC画面に表示してできるので、

開発効率が半端なく高いです。

やはりプログラムは思ったとおりに動くのではなく、書いたようにとおりに動くので、
デバッグ時にはプログラム内部で何がどう動いているかを可視化することが重要です。

METEORA3.0

METEORA3.0がだいぶ形になってきました。
前輪駆動4輪車、センターピボット形式になります。

センターピボット部が本体、後部はおまけになります 笑

また変なロボットをつくってしまった