SLAM (自己位置推定と環境地図作成)

SLAM (Simultaneous Localization and Mapping, 自己位置推定と環境地図作成の同時実行) とは文字通り移動体の移動体の自己位置推定と環境地図作成を同時に行う技術の総称です。SLAMを活用することで、自動運転、自律移動ロボット(AGV)、ドローンなどをはじめとした移動体が未知の環境下での環境地図を作成し、自分がどこにいるのか、周辺がどうなっているのかを把握することができます。構築した地図情報を使って障害物などを回避しつつ、特定のタスクを遂行します。

SLAMを活用する上で知っておきたい3つのことについて解説していきます。

なぜSLAMが重要か

SLAMは技術的には古くから研究されていましたが、近年、コンピューターの性能が向上し、かつ、カメラやレーザーレンジファインダーなどのセンサーが低価格で入手可能になったことで、様々な分野に活用が広がっています。また、SLAMの特徴として、GPSなどの衛星システムに依存せず(GPSが届かないトンネルや建物内でも利用可能)に環境地図の作成と自己位置の推定ができます。このため低コスト化だけでなく、汎用性が高いというメリットもあります。さらに、近年ではディープラーニング (深層学習) の技術を応用したSLAMの実装も発表されています。

なぜSLAMが重要か、その利点・メリットや応用例を解説します。

SLAMを活用することの利点

なぜSLAMが重要なのでしょうか。

お掃除ロボットを例として取り上げてみます。SLAMを搭載しないお掃除ロボットではランダムに部屋の中を動き回り、ほこりの除去を試みます。この場合、全ての床面を掃除できるとは限りません。また、電力も余分に消費してしまうため、バッテリーの持ちも悪くなってしまいます。一方、SLAMを搭載するとお掃除ロボットは車輪の回転量やカメラなどの画像センサーから移動量を推定することができます。これを自己位置推定(Localization)と呼びます。それと同時にカメラなどのセンサーを使って周辺の障害物の地図を作ります。これを環境地図作成(Mapping)と呼びます。自己位置推定と環境地図作成を同時に行うことをSLAMと呼びます。

SLAMを活用することで、あらかじめ部屋の形状が分かっていなくても、走行しながら部屋の地図情報を構築することができます。構築した地図情報を活用することで、一度掃除した場所を重複することなく、最短時間で隅々までほこりの除去が行えます。

お掃除ロボットでのSLAMの利点

お掃除ロボットでのSLAMの利点

SLAMを活用することによって人間が行っているような柔軟なタスクを移動体に行わせることができます。移動ロボットや自動運転、ドローンなどの移動体の自律化を実現する上で、SLAMは欠かせない技術になっているのです。

SLAMの応用例

SLAMは下記に挙げたように移動ロボット、自動運転、ドローンと用途が広がっています。身の回りの様々な用途に活用できる可能性を秘めています。

  • 自動運転車用の高精度地図 (HD Map) の作成
  • お掃除ロボットの部屋の間取り生成とほこりの効率的な除去
  • 芝刈りロボットによる効率的な芝のメンテンナンス
  • 工場内・生産ラインでの無人搬送車・無軌道型 AGVの自律運転
  • 探査機・ローバーによる未知の惑星探査
  • 潜水艦・UUV・水中探査ロボット・無人潜水機 (AUV)による水中探査
  • ドローン・UAV・移動ロボットの自動操縦による荷物配達
  • 介護ロボットによる訪問・見守り
  • 警備ロボットによる巡回
  • 施設内の案内をするサービスロボット
  • ドローン・UAVによる地形把握、災害現場の状況把握、工事現場の図面作成
Panel Navigation

SLAMの仕組みとは

SLAMの仕組みについて説明していきます。

SLAMを実現する技術要素は大きく分けて2つあります。1つはセンサー信号処理を中心としたフロントエンドの処理部です。こちらは使用するセンサーに大きく依存する処理となっています。2つ目はポーズグラフの最適化を中心としたバックエンドの処理部です。バックエンドの処理部はセンサーによらず活用できます。

ここではフロントエンドの処理部に注目し、Visual SLAMとLidar SLAM、RGB-D SLAM、ランドマーク SLAMという4つの手法についてご紹介します。

SLAMの処理の流れ

SLAMの処理の流れ

Visual SLAM

Visual SLAMは名称のとおり、カメラ・イメージセンサーからの画像を中心としたSLAM技術です。

カメラの種類としては単眼カメラ (広角カメラ、魚眼カメラ、全天球カメラ)、複眼カメラ (ステレオカメラ、マルチカメラ)、RGB-Dカメラ (深度カメラやToFカメラ)などが含まれます。

Visual SLAMは比較的安価に入手できるカメラを利用することでコストを抑える利点があります。また、カメラは情報量が多いため様々なランドマーク(以前観測した場所)を検出することができます。ランドマーク検出をグラフベースの最適化と組み合わせて柔軟なSLAMを実装できる可能性があります。

一方、単眼カメラを使用する場合は距離の単位(スケール)が不定となるため、ARマーカーやARタグ、QRコード、チェッカーボードなど既知の物体を検出して画像上から物理量を知る、もしくは、IMU (Inertial Measurement Unit、慣性計測装置) などの物理量を直接計測できるセンサーとフュージョンする必要があります。Visual SLAMのテクノロジーとしてはStructure from Motion (SfM) や Visual Odometry (ビジュアルオドメトリー)、Visual-Inertial Odometry (ビジュアルイナーシャルオドメトリー)、バンドル調整などが関連します。それぞれについて、簡単に概要をご説明します。

  Structure from Motion(SfM): オブジェクトに対して、カメラを動かしながら画像を撮影し、得られた複数の画像を処理して、撮影したオブジェクトの3次元像を再構成する技術です。

  Visual Odometry(ビジュアルオドメトリー): オドメトリーとは、ロボットなど移動体の姿勢(位置および向き)を推定する技術全般を指します。Visual Odometryは、ロボットなどに搭載したカメラ画像から得られた連続画像を処理して、プラットフォームの姿勢を推定する技術です。

  Visual-Inertial Odometry(ビジュアルイナーシャルオドメトリー): Visual OdometryにIMU(慣性計測装置)を組合わせて、カメラだけでは姿勢推定に情報が欠ける環境(白い壁のみであり移動によって画像変化がない、等)での移動情報を補完しより高精度に姿勢推定する技術です。

  バンドル調整: 上記のような撮影したオブジェクトの3次元再構成や、カメラ外部パラメータ(カメラの実世界での姿勢:ワールド座標)の推定では、撮影した複数の画像を処理します。三角測量の原理を使って3次元位置を推定していきますが、もちろん推定結果には誤差が含まれます。複数枚画像があるため、画像の組合せによって推定される位置は微妙に異なることになります。複数枚の画像のすべてに対して、オブジェクトの3次元再構成結果およびカメラの外部パラメータ推定結果が最もフィットするように、それぞれを微調整(再投影誤差を最小化)することをバンドル調整と言います。

Visual SLAMのアルゴリズムは大きく2つに分類できます。画像特徴点のマッチングによる疎な手法 (feature based method)と、画像全体の輝度を使った密な手法 (direct method)です。

  特徴量ベースの方法(Feature based method):異なる視点から撮影した画像からSIFTやORBなどのアルゴリズムを使って得られた特徴点およびそれらの特徴ベクトル同士をマッチングすることで、撮影対象の3次元形状およびカメラの外部パラメータを推定する方法

  直接法(Direct method):異なる視点から撮影した画像同士の輝度値を比較し、誤差が最小になるようにカメラの外部パラメータを推定する方法。一般的に正確ですが、計算量は多くなります。

有名なアルゴリズムとして、前者はPTAMやORB-SLAM、ORB-SLAM2、後者はDTAM、LSD-SLAM、DSO、SVOなどがよく知られています。

Structure from Motionの例

Structure from Motionの例

ORB-SLAMによる特徴点マッチング

ORB-SLAMによる特徴点マッチング

ORB-SLAMのカメラ軌跡と生成された地図

ORB-SLAMのカメラ軌跡と生成された地図

Lidar SLAM

Lidar (Light Detection and Ranging) というレーザーセンサー (距離センサー)を主に使用した方法です。

レーザーセンサーはカメラやToFなどの他のセンサーに比べて格段に精度が高く、自動運転やドローンなど速度が速い移動体で広く使用されています。レーザーセンサーの出力値は一般的に2D (X、Y座標)もしくは3D (X、Y、Z座標)の点群データとなります。点群とは、Lidarセンサーが検知した周囲の物体や地形から反射してきた一つ一つのパルスを点として表現したデータ形式です。

レーザーセンサーの点群は距離精度が高く、SLAMの地図構築に対して非常に有効に働きます。一般に点群同士をマッチングすることで移動量を逐次推定します。移動量を積算することで自己位置を推定することができます。Lidarからの点群のマッチング ではICP (Iterative Closest Point)アルゴリズム(2つの点群同士で最も近い点の組合せを決めそれらの距離の2乗和が最小となるように逐次一方の回転と並進を更新していく方法。初期姿勢の差が大きい場合、局所解に陥りやすい)やNDT (Normal Distributions Transform)アルゴリズム(探索空間を一定の大きさのグリッドに分割し、グリッド内での点群の分布の平均・分散を計算、それらが最適にマッチングするように逐次一方の回転と並進を更新していく方法)などが用いられます。FPFH特徴量のような特徴量ベースのマッチングアルゴリズムも高速に計算できるため、初期姿勢をFPFHで推定し、最終的にICPで高精度化する方法もあります。また、LOAM(Laser Odometry and Mapping)のように、点群からエッジ上の点と平面上の点の特徴点を抽出してレジストレーションに用いるものもあります。地図は2Dや3Dの点群地図やグリッドマップ、ボクセルマップとして表現することがあります。

一方、密度の観点において、点群は画像などに比べて粗く、点群同士のマッチングにおいて、特徴が十分でない側面もあります。例えば、周囲に障害物が少ないような場所では点群間の位置合わせが容易でなく、移動体の位置を見失ってしまうことがあります。また、点群のマッチングは一般に処理負荷が高いため、高速化の工夫が必要となります。例えば、LOAMでは、移動量の推定(オドメトリ)と地図作成(マッピング)の頻度を変えることでリアルタイム処理に対応しています。車両の位置推定にはホイールオドメトリー(車輪の回転角度やステアリング角を積算して、自己姿勢を推定する方法)やGNSS (RTKなどの高精度なもの:通常のGNSS測量に加えて、既知の正確な基準点の情報を使って誤差を低減し、さらに信号の位相を計測することで送受信の時間差から得られる距離分解能を超えて誤差cmの測位を可能にする技術)、IMU(慣性計測装置:加速度計+ジャイロスコープ)などの他の計測結果をフュージョンすることもあります。また、Lidarは高価なため赤外線センサーや超音波センサー、電波によるレーダーセンサーで代替する場合もあります。その場合、精度は大幅に低下してしまいます。

2D LidarによるLidar SLAM

2D LidarによるLidar SLAM

3D LidarによるLidar SLAM

3D LidarによるLidar SLAM

3D Lidar とカメラによる色付き点群によるLidar SLAM

3D Lidar とカメラによる色付き点群によるLidar SLAM

RGB-D SLAM (Depth SLAM)

RGB-D SLAMは、Visual SLAMの一種です。センサーから取得した深度画像(距離情報)によって実現するSLAM技術です。主にRGB-Dカメラ(深度カメラ)やToFセンサーを使用し、周囲に見える物体までの距離を計測します。RGB-D SLAM の特徴として、Visual SLAMの苦手とする特徴点の少ない環境や暗所の環境でもSLAMを実行することができます。ORB-SLAM2などのステレオVisual SLAMはRGB-D SLAMの1タイプです。

Visual SLAM (ORB-SLAM)の例

Visual SLAM (ORB-SLAM)の例

ステレオ Visual SLAM

ステレオ Visual SLAM

ランドマーク SLAM

ランドマークSLAMは、ランドマークと呼ばれる他と区別しやすい固定点を基準として自己位置推定と環境地図作成を行っていくものです。ランドマークにはAprilTagのようなQRコードや、はっきりとした特徴量を持ったLidar点群などが考えられます。

AprilTagを使ったランドマークSLAMの例

AprilTagを使ったランドマークSLAMの例

EKFベースのランドマークSLAMの例

EKFベースのランドマークSLAMの例

ランドマークに対しての相対位置から自己位置推定を行います。相対位置計測には誤差が含まれますがカルマンフィルタを使って計測誤差を運動モデルで低減することができます。

SLAMを実現するための課題

SLAMは一部実用化されているものの、より汎用的に活用するには様々な技術的課題があります。主要な3つの課題を取り上げ、解説します。

課題1. 位置推定の誤差が蓄積し、真値から大きく外れる

SLAMのもっとも知られた課題の1つに、誤差の蓄積(ドリフト)があります。SLAMは逐次移動量を推定します。移動量の推定には必ず誤差が発生します。この誤差が累積していくことで、真の位置と大きくずれる現象が発生します。この誤差の蓄積により、地図データが崩れたり、歪んだりするため、それ以降の探索が困難になります。

例としてロの字の通路を周回することを考えます。このとき、誤差が蓄積していくと始点と終点の位置が一致しないような現象が発生します。このようなループが閉じない現象をループの閉じこみ(閉ループ、Loop closure)問題と呼びます。位置推定の誤差は本質的に不可避なものです。ループの閉じこみを検出し、誤差の蓄積をどのように補正・キャンセルするかが重要となります。

元の位置に戻らない(地図が歪む)

元の位置に戻らない(地図が歪む)

対策の1つは、以前訪れた場所の特徴を何らかランドマークとして記憶しておき、過去の位置推定の誤差を最小化することです。誤差の補正には姿勢グラフ・ポーズグラフ(pose graph)を構築して実現します。誤差の最小化を最適化問題として解くことでもっともらしい地図データを生成します。このような最適化はVisual SLAMではバンドル調整(bundle adjustment)などとも呼ばれます。

例えばループ検出で用いられる手法の一つにセグメントマッチングがあります。セグメントマッチングでは、各キーフレームの点群をセグメンテーションし、セグメントごとの特徴量を計算しておきます。ループ検出では、現在のフレームで計算した各セグメントの特徴量と、保存していた各キーフレームでの特徴量を照合し、あらかじめ設定していた数以上のセグメントで一致した場合にループ閉じ込みをします。大規模な交通環境では路肩に駐車していた車両がいなくなる、など点群のマッチングではループ検出が難しい場合があり、そのようなケースで威力を発揮する手法です。

セグメントマッチングによりセグメントごとのマッチングによりループ検出を行う例

セグメントマッチングによりセグメントごとのマッチングによりループ検出を行う例

課題2. 位置推定が失敗し、地図上の位置を見失う

画像や点群によるマッチングではロボットの運動は考慮されていません。そのため、非連続的な姿勢推定結果を出力することがあります。例えば、秒速1mで移動しているロボットが急に10m先に飛ぶような計算結果を出力することもあります。

このような位置推定の失敗(ロスト)に対処する2つの方法があります。一つ目は移動体の運動モデルや複数のセンサーをフュージョンすることで位置推定の失敗を抑制することです。もう一つは位置推定が失敗した際に復帰するアルゴリズムを準備しておくことです。

運動モデルを活用したセンサーフュージョンにはいくつかの方法があります。代表的なものはカルマンフィルターを使った位置推定です。一般的な差動駆動型ロボットや四輪車は非線形の運動モデルとなるため、拡張カルマンフィルターやパーティクルフィルター(モンテカルロローカリゼーション)がよく使われます。他に、Unscentedカルマンフィルターなどのより柔軟なベイズフィルターを用いることもあります。センサーフュージョンとしてよく用いられるセンサーは慣性計測装置 (IMU、AHRS、INS、加速度センサー、ジャイロセンサー、地磁気センサー)があります。車両の車輪に取り付けたホイールオドメトリー (ホイールエンコーダー)を用いることもよくあります。

位置推定に失敗してしまった場合に復帰する方法として、過去に訪問した点をランドマークとして記憶しておく方法があります。このような点はキーフレームとも呼ばれます。ランドマークを探索する場合は何らか特徴量をインデックス化しておき、高速に走査できるようにしておきます。画像特徴量による手法としはBag of Features (Bof)、 Bag of Visual Words (BoVW)があります。近年ではディープラーニングによる特徴量から距離比較を行い、近いシーンかどうかの判断を行うこともあります。

課題3. 画像処理・点群処理・最適化の計算コストが高い

SLAMを実機ロボット上で実現する際に問題になるのが計算コストです。

ロボットの計算ユニットには一般的に省電力・小型の組み込みマイコン、FPGAなどが使用されます。位置推定を精度よく実現するためには、画像処理や点群処理によるマッチングを速い周期で実行する必要があります。また、ループ閉じこみのような最適化計算も負荷の高い処理になります。このような計算コストの高い処理をどのように組み込みマイコン上で実行するかが課題となります。

マッチング処理の前処理となる特徴抽出などは比較的、並列化に向いた処理が多く、マルチコアによる処理やSIMD演算、組み込みGPUの活用などで高速化ができる場合があります。また、ポーズグラフの最適化は比較的長い周期実行できればよいので、優先度を下げながら定期的に実行するなどの工夫がされています。

MATLABによるSLAM

MATLAB®ではSLAMを実装する際、上で示した3つの課題に対して活用できるコンポーネントを提供しています。これらのコンポーネントを組み合わせて対象のシステムにあったSLAMアプリケーションを試行錯誤しながら構築することができます。

位置推定の誤差の蓄積を抑制する上では姿勢グラフ(ポーズグラフ、pose graph)を利用した最適化が必要となります。2Dおよび3Dの姿勢グラフを表現するオブジェクトとその最適化の機能を提供しています。これらの機能を活用することで位置推定の誤差の蓄積を低減することが可能です。

自己位置推定でロストを起こしてしまった場合には以前訪問した場所をベースにリローカライゼーション(Relocalization)が必要となります。Visual SLAMの場合、画像特徴量による画像検索が役に立ちます。

さらに、SLAMのフロントエンド活用可能な様々なセンサー信号処理・画像処理・点群処理の機能を提供しています。

これらの関数は自動コード生成を活用し、高速化が期待できます。

その他、SLAMアルゴリズムの精度検証には仮想環境が用いられることもあります。Epic Games® Unreal Engine® と連携し、3D シミュレーション環境から画像や点群を生成することで、SLAM アルゴリズムを効率的に検証することが可能です。

MATLABとそのアドオン機能を活用することで効率的にSLAMアルゴリズムを開発検証いただくことが可能です。

MATLAB関連製品 SLAM関連機能・メリット
Navigation Toolbox™ 2D Lidar点群処理によるスキャンマッチングとSLAM Map BuilderアプリによるrosbagからLidar点群データのインポート、占有グリッドマップの生成、および閉ループの検出と修正
Computer Vision Toolbox™ 画像特徴量抽出、ORB-SLAMなどのVisual SLAM、および3D Lidar 点群処理によるマッチング
Lidar Toolbox™ 点群の特徴量抽出、Lidar SLAMの例題
Automated Driving Toolbox™ Unreal EngineによるカメラやLidarなどの合成センサーデータを活用したSLAMの例題
Sensor Fusion and Tracking Toolbox™ 拡張カルマンフィルタ(EKF)などの状態推定フィルターのサポート

SLAMについてさらに詳しく

SLAMによる自己位置推定と地図構築について解説します。スキャンマッチングやループの閉じこみを検出、誤差の蓄積を補正・キャンセルするための姿勢グラフの最適化について解説します。
自律移動ロボット開発に関連する認知、判断、行動計画、操舵までの各種機能をご紹介します。また、これらの機能を組み合わせて自律移動ロボットアプリケーションを開発するワークフローについてご紹介いたします。
この例では、ポーズグラフ最適化を使用して、収集された一連の2D Lidarスキャンを使ってSLAMアルゴリズムを実装する方法を示します。2D Lidarスキャンを使用して環境のマップを構築し、ロボットの位置軌道を推定します。
慣性測定ユニット(IMU)の読み取り値を活用して、車両に搭載されたセンサーからの3D Lidarデータを処理して、マップを構築する方法を紹介します。車両の軌道を全地球測位システム(GPS)記録と比較する方法も示します。
Structure from Motion (SfM) は、一連の 2 次元画像シーケンスからシーンの 3 次元構造を推定します。この例では、ビューのシーケンスからキャリブレーションされたカメラの姿勢を推定し、シーンの 3 次元構造を再構成します。
ビジュアルオドメトリは、画像シーケンスを分析することにより、カメラの位置姿勢を推定するプロセスです。一連の画像からキャリブレーションされた単一のカメラの軌跡を推定する方法を示します。