EurekaMoments

ロボットや自動車の自律移動に関する知識や技術、プログラミング、ソフトウェア開発について勉強したことをメモするブログ

書籍「SLAM入門」の読書録~参照スキャンデータ編~

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

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

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

目次

目的

  • 書籍「SLAM入門」を参考に、SLAMでスキャンマッチングをする際の参照スキャン生成について理解する。
  • サンプルコードを参考に、C++での設計思想について理解する。

参照スキャンデータとは

  • スキャンマッチングでは、異なる時刻と位置で計測された2つのスキャン点群を重ね合わせる。
  • 現在時刻をtとした場合、その時のロボット位置と計測されたスキャン点群を下記のようにする。

f:id:sy4310:20200723135248p:plain

  • また、一つ前の時刻t-1におけるロボット位置と、そこで計測されたスキャン点群を下記のようにする。

f:id:sy4310:20200723142306p:plain

  • スキャンマッチングでは、こういった現在時刻と過去の時刻の計測データを対応付ける事で、その時刻間のロボットの移動量を求める。
  • この時、現在時刻のスキャンと対応付けられる過去のスキャンを参照スキャンと呼ぶ。

f:id:sy4310:20200723143644p:plain

参照スキャン生成のソフト設計

基底クラス

  • 過去のスキャンデータを地図データから取得するため、PointCloudMapクラスのインスタンスをメンバ変数として持つ。
  • 地図から取得した過去のスキャンデータを、参照スキャンとして保持するためにScan2D構造体のrefScanをメンバ変数として持つ。
  • 参照スキャンの生成はmakeRefScan()メソッドで行うが、中身の実装は継承で作られる派生クラス内で定義される。

f:id:sy4310:20200723151836p:plain

  • 派生クラスには、RefScanMakerBSクラスとRefScanMakerLMクラスの2種類がある。
  • RefScanMakerBSクラスは、直前に計測されたスキャンを地図から取得して、参照スキャンとして利用する。
  • RefScanMakerLMクラスは、直前のスキャンだけでなく地図に登録された全てのスキャンを参照スキャンとして利用する。

f:id:sy4310:20200723155700p:plain

RefScanMakerBSクラスによる参照スキャン生成

  • PointCloudMapクラスのgetLastPoseメソッドとメンバ変数のlastScan.lpsより、直前のロボット位置とスキャンデータを取得する。
  • 取得したスキャンデータはロボット座標系なので、ロボットの位置と方位角より地図座標系に変換する。

f:id:sy4310:20200723160542p:plain

RefScanMakerLMクラスによる参照スキャン生成

  • PointCloudMapクラスが持つlocalMapを丸ごと参照スキャンとして利用する。
  • localMapは既に地図座標系になっているため、ロボット位置による座標変換は必要ない。

f:id:sy4310:20200723162544p:plain

参照スキャンに直前スキャンのみを使うデメリット

  • 周囲の物体によるオクルージョンなどでスキャンが欠損する場合がある。
  • スキャン点の数が少ない場合がある。
  • スキャンマッチングはスキャンが広範囲の方がマッチング精度が向上する。
  • それまでに構築した地図データを参照した方が、欠損や点数不足の影響を受けにくくできる。

f:id:sy4310:20200723165837p:plain

GitHub

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

github.com