Articles & Blogs > アルゴリズム的見地から見たSLAM（パート2）

アルゴリズム的見地から見たSLAM（パート2）

筆者： Aswin S Babu 2024-06-20

タグ

パート1で説明したように、SLAMとは、未知の領域でロボット車両が周辺の地図を徐々に作成しながら自己の位置を推定するプロセスです。SLAMアルゴリズムは、主に3つの手法に分類できます。フィルタベースのSLAM、グラフベースのSLAM、ディープラーニングベースのSLAMです。

フィルタベースのSLAMでは、SLAMを状態推定問題として扱います。その場合、一般的に、拡張カルマンフィルタ（EKF）やアンセンテッドカルマンフィルタ（UKF）などの確率的フィルタを用いてロボットの状態を再帰的に推定し、センサ測定値に基づいてマップを更新します。フィルタはロボットの動作モデルに基づいて次の状態を予測し、センサの測定値を用いてその予測値を修正します。

グラフベースSLAMでは、フィルタベースSLAMとは対照的に、問題をグラフ最適化問題として扱います。その場合、SLAM問題はグラフとして定式化されます。グラフでは、ノード（頂点）がロボットの姿勢または環境内のランドマークを表し、エッジ（辺）がノード間の測定値または制約を表します。グラフベースSLAMの目標は、測定された制約条件（たとえば、ランドマーク間の距離、ロボットの姿勢と姿勢の間の相対姿勢）を可能な限り正確に満たすロボットの姿勢とランドマークの位置を最適化することです。

ディープラーニングベースのSLAM手法では、ニューラルネットワークを活用してセンサデータから環境の特徴表現を直接学習するため、特徴やモデルを作成する必要がありません。これらの手法では、センサの測定値とロボットの姿勢やマップとの間の複雑な対応関係を学習できるので、エンドツーエンドでSLAMソリューションを使用することが可能になります。

SLAMの核心であるマッピングとローカリゼーションは複雑に関連し合っており、ロボットはセンサデータに基づいてマップを絶えず更新し、それに応じて自己位置の推定を調整します。SLAMとその各種コンセプト（アルゴリズム）は組み合わせて使うことができ、途中で変更や修正が可能です。したがって、複数のアルゴリズムが開発され併用されることが非常に多く、SLAMを単一のアルゴリズムとして一般化して説明することは容易ではありません。そのため、SLAMを理解する最も良い方法は、SLAMの特定の実装形態に注目することです。そこで今回は、フィルタベースのVisual SLAM（vSLAM）について詳しく説明します。

Visual SLAM

その名称からわかるように、vSLAMは視覚センサ、つまりカメラを主要センサとして使用します。さらに、エンコーダ、慣性計測ユニット（IMU）やその他のセンサを備えている場合もあります。SLAM実装の一般的なブロック図を図1に示します。

図1：特徴ベースのSLAMプロセスを簡略化した一般的なブロック図。（kudan.ioより転載）

カメラ測定

カメラは、ランドマーク、エッジ、テクスチャなど、ロボットの周囲の画像を取得します。しかし、ほとんどのカメラレンズではある程度の歪みが生じてしまうため、取得された画像で歪みを補正する必要があります。カメラは、ステレオカメラ、単眼カメラ、RGB-Dカメラなど、ToF（Time of Flight）方式の深度センサのついたカメラを使用します。ステレオカメラやRGB-Dカメラの利点は、深度情報が簡単に得られることです。しかし、単眼カメラの場合は、スケールが不定という欠点があります。つまり、単眼SLAMでは、特徴の対応関係だけから並進移動の長さ（スケールファクタ）を特定することはできないのです。ただ、この欠点を軽減する方法はありますが、この記事では対象外となるため割愛します。

特徴の抽出

カメラセンサを使用して画像を取得した後、将来参照できるように、特定のフレームの特徴を把握してフレームを一意に識別する必要があります。ここでいう特徴とは、一貫して識別できる一意のピクセルの集まりのことです。言い換えると、回転、拡大縮小、歪みに対して不変であるために画像処理の後でも再識別が容易な、画像内の特徴的な点です。ステレオカメラを一次センサとして使用していることを考慮すると、カメラで取得されるステレオ画像間で重複する特徴が見られるはずです。これらの同一の特徴を用いて、センサからの距離を推定することができます。しかし、それを行うには、前述したように、2つのステレオ画像に共通する特徴を特定する必要があります。これは、特徴検出器とマッチャによって行われます。特徴検出器の一般的な例としては、SIFT（Scale-invariant feature transform）、ORB（Oriented FAST and rotated BRIEF）、GFTT（Good Features to Track）などがあります。図2は、いくつかの一般的な特徴検出器を使用して特定された特徴を示しています。検出器は、特徴を特定すると、その特徴の説明を付与します。このプロセスにより、将来その特徴を簡単に再確認することができるようになります。

図2：a）GFTT法、b）SIFT法の各手法でXRPロボットの画像から検出されたキーポイント。（画像提供：SparkFun Electronics）

キーポイントを見つけ出したら、マッチングによってポイント間の対応関係を確定します。使用できる特徴マッチングアルゴリズムとしては、総当たりマッチング法（Brute Force Matcher）やFLANN（Fast Library for Approximate Nearest Neighbors）などがあります。図3は、マッチングアルゴリズムの動作を視覚的に表したものです。一致した点同士を結ぶ線が示されていますが、画像を鏡合わせにしているため、システムに問題がなければ、理想的には水平（平行）な線だけが得られるはずです。しかし、残念ながら、特徴マッチングのアルゴリズムは完璧とは言い難いため、マッチングの誤りが生じてしまい、そうした誤合致の一部が斜線で示されています。RANSAC（ランダムサンプルコンセンサス）のような、外れ値を除去するツールが必要なのはこのためです。

図3：画像とそれを反転した画像においてFLANNマッチングで検出された特徴点の一致を示す線。（画像提供：SparkFun Electronics）

RANSAC

RANSACのようなアルゴリズムがこうした不正確なマッチをフィルタして取り除くために使用され、それによりインライア（正しいマッチ）のみでその後の処理を実施できるようになります。RANSACは、提供されたデータからランダムに取り出した一部の要素からモデルを構築することで機能します。つまり、いくつかのランダムな点を正しいマッチとみなして、その選択されたいくつかの点に基づいて残りの点すべてをマッチングさせることを試みます。そして、そのモデルがデータセット全体にどの程度マッチするかを評価します。この処理を、コスト関数によって決定される最小の誤差でデータを正確に記述するモデルが見つかるまで繰り返します。

特徴とデータの関連付け

このステップでは、空間で検出された特徴とその推定位置を取り込み、特徴のマップを作成します。この処理が何フレームも続くにつれて、新しい特徴がマップの既知の要素に関連付けられ、不確実な特徴は除外されます。

何フレームにもわたってカメラの動作が追跡されるので、既知の特徴と、カメラの動きから生じると予測される特徴の変化に基づいて、予測を行うことができます。しかし、特にリアルタイムのアプリケーションでは、計算のリソースと時間の制約によりSLAM機能が制限されます。時間の経過とともに、システムはより多くの特徴の測定値を収集し、位置や姿勢を更新していきますが、それに応じて環境表現を制約し最適化することが不可欠になります。

位置、姿勢、マップの更新

カルマンフィルタ

SLAMのプロセスをさらに進めていくと、ノイズが蓄積され、カメラが取得する画像とそれに関連する動作との間に不確実性が生じます。カルマンフィルタは、観測された測定値に対して継続的に予測値を生成し、モデルを更新し微調整することにより、異なる測定値間のノイズや不確実性の影響を軽減することができます。これは線形システムモデルの作成において役立ちます。SLAMを実際に実装する場合、非線形システムを取り込み、予測値と測定値をその平均値付近で線形化する拡張カルマンフィルタ（EKF）を使用します。EKFは、センサフュージョン（カメラやIMUなど）を行うことで複数のセンサからのデータを統合し、状態やマップの推定の精度を向上させることができます。このようにデータソースを融合させることで、より信頼性の高いSLAM結果を得ることができます。EKFベースのSLAMにおける状態ベクトルには、ロボットの姿勢（位置と向き）とマップ内のランドマークの位置が含まれます。

キーフレーム選択

取得された画像の中からキーフレームを選択することで、取得されたすべての画像を処理する際の高い計算負荷を軽減できます。すべての画像を処理するのではなく、環境を適切に表現するフレームのみを選んで計算に使用します。この手法も、精度と効率のバランスを取る手法です。

ループ閉じ込みとリローカライゼーションによるエラー修正

環境モデルの構築が進むと測定誤差やセンサのドリフトが徐々に蓄積され、生成されるマップに影響を与えます。これはループ閉じ込みによってある程度軽減できます。ループ閉じ込みは、システムが以前にマッピングされた領域を再訪問していることをシステムが認識した際に起こります。SLAMシステムは、現在のマップを以前に確立された同じ領域のマップに合わせて再調整することで、蓄積された誤差を修正することができます。

リローカライゼーション

リローカライゼーションは、システムが自己の位置と向き（姿勢）を見失った場合に必要となります。その場合、現在観測可能な特徴を利用して、姿勢を再推定する必要があります。現在利用可能な特徴と利用可能なマップとのマッチングに成功すれば、その後は通常通りSLAMプロセスを進めることができます。

まとめ

SLAMは、未知の領域をマッピングしながら、ロボット車両の位置を推定するプロセスです。SLAM技術には、フィルタベースの手法、グラフベースの手法、（ニューラルネットワークを用いた）ディープラーニングベースの手法などがあります。Visual SLAMは、カメラで画像を取得し、特徴を抽出し、特徴のマッチングを行い、不正なマッチングをフィルタで除外します。このシステムは、新しい特徴を既知の要素に関連付け、カルマンフィルタを使用してロボットの位置と姿勢を更新し、キーフレームを選択し、ループ閉じ込みとリローカライゼーションでエラーを修正することにより、マップを作成します。

リファレンス

著者について

Aswin S Babu氏はロボット工学とAIを専門とするソフトウェアエンジニアです。彼はこれらのスキルを社会貢献のために活用することに情熱を注いでいます。ホームオートメーションの社会的起業家からロボット工学のエンジニアまで幅広い経験を持っています。また、ロボットの位置特定のための単眼ビジュアルオドメトリシステムなど、革新的なプロジェクトに取り組んできました。さらに、さまざまな年齢の学生にロボット工学やAIを教えた経験もあります。趣味はパブリックスピーチ、養蜂、ガーデニング、社会貢献のためのボランティア活動です。