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

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

目次

目的

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

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

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

• PointCloudMapクラス

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

• ScanMatcher2Dクラス

ポーズグラフの生成

• PoseGraphクラス

ループ検出

• LoopDetectorクラス

メイン処理の流れ

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

1. 初期化

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

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

2. スキャンマッチング

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

3. ポーズグラフの生成

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

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

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

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

5. ループ検出

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

GitHub

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

github.com