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

SLAMの仕組みとは

SLAMの手法・種類

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

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などがよく知られています。

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は高価なため赤外線センサーや超音波センサー、電波によるレーダーセンサーで代替する場合もあります。その場合、精度は大幅に低下してしまいます。

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タイプです。

ランドマーク SLAM

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

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

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の活用などで高速化ができる場合があります。また、ポーズグラフの最適化は比較的長い周期実行できればよいので、優先度を下げながら定期的に実行するなどの工夫がされています。