探査ローバーにVisual-SLAMを導入したい
この記事はCanSatチームFUSiONのアドベントカレンダー2023、6日目(12月8日)の記事です。
Visual-SLAMについて
そもそもSLAMとは
SLAM(Simultaneous Localization And Mapping)とは自己位置推定(Localization)と環境地図作成(Mapping)を行う技術です．
この技術を使うことで，自分がどれだけ進んだかやどれくらいの速度を持っているか，自分の周囲の環境がどのような形状や運動をしているかを計測・把握する技術です．最近では，主にロボティクスや自動運転において使われている印象があります．
Credit: mathworks
身近なところだと，家庭用の掃除ロボットにこの技術が搭載されています．ロボットが自分の位置と周囲の環境を認識しながら移動を続けることで，効率的に掃除を行うことができるようになります．
SLAMは点群を取得し自己位置推定と環境地図作成を行うものが多い(Credit: MAP Ⅳ)
そのような，SLAMの中にもいくつか種類があります．
- Lidar SLAM
- Liderと呼ばれるレーザー光の反射によって距離を測るセンサーを用いて大量の点群を取得し周囲の環境を把握します．この方式は点群までの距離を測るので物体の色は分かりません．
- Visual SLAM
- 今回の記事のメインになるSLAMの方式です．人間の目のようにカメラに写った情報をもとに周囲の環境や自己位置を推定します．詳しくは後述します．
- RGB-D SLAM
- Visual SLAMの一種で，RGB(色)の情報に加えて，深度(Depth)の情報を用いるSLAMの手法．Liderに近い働きをするToFセンサや，ステレオ視を用いるORB-SLAM2がこの方式に該当します．
- ランドマークSLAM
- ランドマークと呼ばれる他と区別しやすい特徴を基準としたSLAMの手法です．ランドマークにはQRコードなどが用いられます．
ロボティクス制御系の画像処理
Visual SLAMと混同されやすい画像処理技術に，Visual OdometryやSfM(Structure from Mothion)と呼ばれる技術があります．
Visual SLAM
SLAMの目的はロボットの経路の推定値を得ることにあります．これは環境マップを保存し，それまで蓄積してきたデータとその時その時で取得したデータを処理(ループクロージング)させることで実現されます．このSLAMの処理をイメージセンサで実現するのがVisual-SLAMになります．
Visual Odometry
Visual SLAMの構成要素
Visual OdometryはSLAMの処理の中でも使われる技術です．また，それ単体でも使える技術です．SLAMは処理を開始してから，最新のデータの取得地までを用いて自己位置推定と環境地図の作成を行うのに対して，Visual Odometryでは直近のnフレームのみを処理の範囲としており，全体的な整合性の確保は重要視されていません．誤差が蓄積してドリフトが生じるのがデメリットですが，処理が比較的軽く実行できるため，瞬間的な変化(速度情報の取得)には有効になることがあります．
SfM:Structure from Motion
SfMとVisual SLAMとVisual Odometryの概念
SfMはVisuaL SLAMのより一般的な概念です．処理が行われる画像の時系列情報を必要としない点がもっとも大きな差異です．
SfMは異なる視点から撮影した複数の画像を用いて環境のマップを作成します．
Visual-SLAMをやってみよう
サンプルコード
ここまで聞いてVisual SLAMってなんだかおもしろそう！やってみたい！と思った方もいるかもしれません．そんな時にはMatLabの開発者，MathWorks様のWebサイトを訪ねましょう．
単眼の Visual Simultaneous Localization and Mappingという名前でソースコード付きの記事が公開されています．
この記事ではVisual SLAMの中でもORB-SLAMと呼ばれる手法を使用しています．
ORB SLAMの名前は特徴点検出のために「Oriented FAST」「Rotated BRIEF」と呼ばれる2つのアルゴリズムを用いていることに由来し，比較的低コストで高精度な特徴点マッチングを実現することが可能です．
より詳細なアルゴリズムはMathWorksの記事がとても参考になります．
実行結果
今回は自分で撮影したこちらの２枚の画像に対して特徴点検出とマッチングを実施しました．
撮影に使ったカメラはiphone 12 miniです．
特徴点を抽出した段階での画像がこちら．
代表的な特徴点のみですが、緑のマークが付いて検出されていることがわかります．
2枚の画像の特徴点を結合した後にはこうなります．
今回の画像は回転の成分が強く平行移動の要素はあまりありません．
これらの特徴点を検出し点群データを作成したものがこちら．
それっぽい立体が出来上がっているのが見えますね．実際にコンピュータのウインドウではくるくる立体を回しながら楽しむ事ができます．
今回は画像中央付近の花壇と周辺の地面しか生成されていませんが、マシン性能をあげてプログラムを変更すればより多くの特徴点を検出し、マッチングさせることができるはずです．
実行結果(失敗例)
ここで、さらに上のような実行例を試してみます．
先例と異なり、こちらは平行移動の成分が強いです．
出力結果．うまくいかずに平面がそのまま出力されたような結果になりました．
これは2枚の画像の特徴点がどれも同じ幅で移動しているために特徴点ごとの視差による奥行きの推定が出来なかったために起こる出力です．
このようにVisual SLAMは平行移動のみの視点から実行するのがそこそこ難しいよ，という特徴があります．
さて、一般的なVisual SLAMの話はこれで以上です．
ここまでお読みいただきありがとうございました．
ここから先は自分のやっているモビリティ(CanSat)における適用を考えた話になるので，興味ない方はブラウザバック🏃，興味を持ってくれた方は引き続きお付き合いください．
CanSatでの実用
CanSatとは航空宇宙工学における学生の教育で使われる模擬衛星の総称で，国内外で競技大会が開かれています．
上空から落下させ，目標地点への接近を，落下制御にて行うフライバック部門と地上モビリティにより行うランバック部門があります．私の所属するチームFUSiONはランバック部門でCanSatに挑戦しています．
基本的に競技中は操縦などは認められておらず，完全に自律制御させなくてはいけないのも難しい点です．
これを開発するにあたって以下のような課題がありました．
- 競技が屋外フィールドでありスタックする可能性があるため何とかしてスタックを検知したい
- 一方でCanSatはレギュレーションがとても厳しく，約1kgの中にパラシュートや落下機構を含めた全ての機構を納めなくてはいけない
- GNSSは搭載し移動できていないことは検知できるが重量制限によりバッテリーに余裕がなく，即位誤差があるGNSSのみでは即応性が十分ではない
- 加速度による積分も誤差が蓄積し信頼性が確保できない
そこで目をつけたのがVisual Odometryによる速度算出でした．
加えて，探査機を模擬しているわけなので環境地図も作成してみたいよね，と思いVisual SLAMの実現までが目標となりました．
しかし，Visual SLAMをリアルタイムで実現しようとするとべらぼうな計算リソースと電力が必要になります．
それに加えて，小型軽量な制約のために不整地走行時の振動が撮像に影響し画像がブレるため，特徴点の抽出が難しいというのが検討の結果でした．
これを解消するには走行効率度外視で撮像のたびに止まったり，優れたアブソーバやアイソレータを開発する必要がありそうです．
終わりに
今回の記事では自分の開発におけるVisual SLAM導入を検討した際のあれこれをまとめてみました．
家庭用ロボットや産業モビリティから，宇宙探査機にまで幅広く使われているのがSLAMであり，今後のロボティクスやハードウェアの発展に伴いますます重要度が増してくる技術だと思います．
まだまだ多くの課題も残っていますが，機械学習などを用いた手法や低レイヤでの処理など，様々なアプローチで高精度化が進められていますので，もし興味を持ってくれた方がいらっしゃいましたら，色々調べてみてください．
ありがとうございました〜〜
Discussion