EurekaMoments

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

書籍「SLAM入門」の読書録~フロントエンド編~

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

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

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

f:id:sy4310:20200704203737p:plain

目次

目的

  • 書籍「SLAM入門」を参考に、SLAMシステムにおけるフロントエンド部分について理解する。
  • サンプルコードを参考に、C++での設計思想について理解する。

フロントエンド部分が担う機能

スキャンデータによる地図生成と管理

  • PointCloudMapクラス

f:id:sy4310:20200705122003p:plain

スキャンマッチングによるロボット位置推定

  • ScanMatcher2Dクラス

f:id:sy4310:20200705145055p:plain

ポーズグラフの生成

  • PoseGraphクラス

f:id:sy4310:20200705145946p:plain

ループ検出

  • LoopDetectorクラス

f:id:sy4310:20200705150021p:plain

メイン処理の流れ

  • フロントエンド内のメイン処理は、下記のprocess()メソッドによって実行される。

f:id:sy4310:20200705151329p:plain

1. 初期化

  • init()メソッドで地図データを初期化する

f:id:sy4310:20200705154823p:plain

  • init()メソッドのフロー

f:id:sy4310:20200705154525p:plain

2. スキャンマッチング

  • ScanMatcher2DクラスのmatchScan()メソッドでロボット位置推定と地図生成。
  • 全スキャン点を地図に格納する場合は、ここで地図が生成される。

f:id:sy4310:20200705155706p:plain

3. ポーズグラフの生成

  • 初回であれば、スキャンマッチングで最後に推定した位置をノードとしてセット。
  • 2回目以降は、直前のノードと現在のノードとの間に辺(オドメトリアーク)を張る。

f:id:sy4310:20200705161833p:plain

  • makeOdometryArc()メソッドでアークを張る。
  • アークには情報行列(共分散行列の逆行列)を持たせるため、ScanMatcher2Dクラスから推定位置の共分散行列を受け取っておく。

f:id:sy4310:20200705162151p:plain

4. 格子テーブルによる地図生成

  • 入っているスキャン点数が閾値以上のセルのみから代表点(重心)を計算する。
  • 初回の点数閾値は1、それ以降は5点を閾値に設定する。
  • 計算負荷を軽くするために、地図生成は10フレーム毎に実行する。

f:id:sy4310:20200705172002p:plain

5. ループ検出

  • LoopDetectorクラスのdetectLoop()メソッドを実行。
  • 現在位置に近く、現在スキャンに形状が一致する場所を探索してポーズアークを張る。

f:id:sy4310:20200705173237p:plain

GitHub

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

github.com