EurekaMoments

This is my studying logs about Autonomous driving, Machine learning technologies and etc.

書籍「SLAM入門」の読書録~センサフュージョンとループ閉じ込み~

SLAM入門: ロボットの自己位置推定と地図構築の技術

SLAM入門: ロボットの自己位置推定と地図構築の技術

  • 作者:友納 正裕
  • 発売日: 2018/03/03
  • メディア: 単行本(ソフトカバー)

目次

目的

  • 書籍「SLAM入門」を参考に、SLAMでロボット位置を推定する際のセンサフュージョンとループ閉じ込みについて理解する。
  • サンプルコードを参考に、C++での設計思想について理解する。

SLAMにおける退化とは

  • 一般的には、計測されたスキャンデータにランドマークが多く含まれているほど、安定して解が求まりやすくなる。
  • ただし、スキャンされた周囲の環境があまり特徴的でなく、スキャンマッチングがどこの位置でも合うようになってしまい、ロボット位置が一意に定まらなくなる。
  • このように、ロボット位置を一意に定めるために必要な情報が不足した状態になる状況を退化と呼ぶ。

f:id:sy4310:20200726213441p:plain

退化を防ぐためのセンサフュージョン

  • 複数のセンサから得られたデータを統合してロボットの状態推定を行う技術をセンサフュージョンという。
  • これによってロボット位置推定に必要な情報の不足を補い、SLAMの退化を防ぐ事が可能になる。
  • 複数のセンサを用いるため、同時に複数パターンのロボット推定位置が求められる。
  • 各センサデータの信頼性に基づいて、ロボット推定位置の重み付き平均を計算する。

f:id:sy4310:20200730215511p:plain

  • オドメトリによる位置をx_o、スキャンマッチングによる位置をx_s、また、それぞれの重みをW_o, W_sとする。
  • これらの重み付き平均でセンサフュージョンによるロボット位置x_fを求めると下記のようになる。

f:id:sy4310:20200730222200p:plain

  • この重みパラメータは、推定されるロボット位置誤差の共分散より決定されるのが一般的である。
  • x_oの誤差分散を{\sigma_o}^2x_sの誤差分散を{\sigma_s}^2x_fの誤差分散を{\sigma_f}^2とする。
  • この時の重みW_o, W_sは、求めたい位置x_fの誤差分散である{\sigma_f}^2が最小となるように決めると考える。
  • 分散{\sigma_f}^2は下記の計算式で求められる。

f:id:sy4310:20200731221358p:plain

  • これが最小とするW_o, W_sは下記のように求められる。

f:id:sy4310:20200731224728p:plain

  • 以上より、求めたい位置x_fと、その誤差分散{\sigma_f}^2は下記のように求められる。

f:id:sy4310:20200801005239p:plain

ループ閉じ込みとは

  • ここで言うループとは、以前通った場所を再び通る経路である。
  • SLAMではランドマーク位置を推定し、それに基づいて自己位置推定もするため、それによる累積誤差でループは閉じない事が多い。
  • ループが閉じない事で地図が歪み、ロボットが障害物に衝突したり、目的地までの経路がおかしくなってしまう。

f:id:sy4310:20200728211830p:plain

  • ロボットが再び同じ場所を通った事を検出し、そのループが閉じた位置を正しく求める必要がある。(ループ検出)
  • 求めた位置でループを閉じると、それまでに構築してきた地図上のランドマークや、ロボットの移動軌跡との辻褄が合わなくなる。
  • そのため、ループ検出位置から過去を遡る形でロボットの移動軌跡と地図を修正する処理が後工程として必要になる。(ポーズ調整)

f:id:sy4310:20200729201431p:plain

ソフト設計

センサフュージョン

クラス

  • PoseFuser
  • スキャンマッチングで求めた位置の共分散、オドメトリで求めた位置の共分散をメンバ変数に持つ。
  • スキャンマッチング時の対応付けや、共分散の計算自体は、それらを担うクラスのインスタンスをメンバに持つ事で実行する。

f:id:sy4310:20200801134501p:plain

メソッド

共分散による重み付き平均の計算
  • fusePose()
  • スキャンマッチングとオドメトリの共分散は、CovarianceCalculatorクラスのメソッドを呼び出す事で計算する。
  • fuseメソッドを呼び出して、それぞれの推定位置と共分散を融合させる。

f:id:sy4310:20200801135402p:plain

位置と共分散の融合
  • fuse()
  • それぞれの共分散は正規分布に従うと仮定して融合する。
  • 方位角は-180°~180°に正規化されているが、このままでは境界付近で値が飛び、後の処理が破綻する可能性がある。
  • スキャンマッチングとオドメトリの方位角の差をとり、それが-180°~180°の範囲を超える場合は、スキャンマッチングの方位角に360°を加算あるいは減算して値の飛びを防ぐ。

f:id:sy4310:20200801144111p:plain

共分散の計算

クラス

  • CovarianceCalculator
  • スキャンマッチングとオドメトリのそれぞれの誤差共分散を計算する。

f:id:sy4310:20200801144523p:plain

メソッド

スキャンマッチングの共分散の計算
  • calIcpCovariance()

f:id:sy4310:20200801151429p:plain

  • 計算した共分散行列を関数calEigen()で固有値分解し、退化具合をチェックできる。

f:id:sy4310:20200801152212p:plain

  • 2つの固有値の比が大きい場合、ある軸の不確実性は大きく、もう一方の軸の不確実性は小さいという事になる。
  • 廊下のような環境における退化の指標になる。

f:id:sy4310:20200801152605p:plain

qiita.com

kriver-1.hatenablog.com

convexbrain.github.io

オドメトリの共分散の計算
  • calMotionCovarianceSimple()
  • 共分散行列は対角成分のみを計算する。
  • 対角成分は、位置座標x, yと方位角の分散。それぞれに相関はない独立な状態と仮定すると、他の共分散成分は0と考える事が出来る。

f:id:sy4310:20200801152720p:plain

ループ検出

クラス

  • LoopDetector
  • 現在位置と再訪点の距離閾値を4mに設定。この範囲内で最短距離にある点を再訪点とする。
  • 累積走行距離の差の閾値を10mに設定。これが現在位置と近すぎる場合は探索対象から外す。

f:id:sy4310:20200801163136p:plain

メソッド

現在位置と近く、現在スキャンに形状が合う場所を検出する
  • detectLoop()
  • 現在位置に最も近い部分地図と再訪点候補を求める。
  • 最も近い部分地図を参照スキャンとし、現在スキャンと形状が一致するところを再訪点として決定。

f:id:sy4310:20200801163752p:plain

再訪点位置の計算
  • estimateRevisitPose()
  • 最も近い部分地図の中から、現在スキャンと参照スキャンが最も合うところをICPで求める。
  • 点の対応率0.9以上、マッチング点数が50点以上なら最適とする。

f:id:sy4310:20200801164323p:plain

GitHub

記載されているUMLのダイアグラムは、全て下記のGitHubリポジトリで公開済み。

github.com