- 作者:友納 正裕
- 発売日: 2018/03/03
- メディア: 単行本（ソフトカバー）
目次
目的
- 書籍「SLAM入門」を参考に、SLAMシステムにおけるスキャンマッチング部分について理解する。
- サンプルコードを参考に、C++での設計思想について理解する。
スキャンデータの構造
- ロボットの中心にはスキャンレーザが搭載されているとする。
- スキャンレーザが周囲を観測して得られるひとまとまりのデータをスキャンデータと呼び、レーザビームの方向Φと距離dの列となっている。
- 周囲の壁やランドマークなどをスキャンし、各スキャン点の位置座標を計算する事で、このような点群が得られる。
ロボットとスキャンレーザの座標系
- 搭載されるセンサとロボットとでは座標系が異なる場合があるので、SLAMを行う際にはこれらの座標系を統一しなければならない。
- ここではロボットの座標系を、x軸が前方、y軸が左方向を向くように設定する。
- 回転方向は、x軸を基準に反時計回りを正とする。
- レーザの座標系は、x軸を基準に時計回りに0~360°の角度を出力する。
- ロボットには、これを後ろ向きにした状態で搭載されている。
- ロボットの座標系と合わせるために、レーザの出力角度には180°のオフセットを加え、-180°~180°の範囲で出力するセンサとみなす。
スキャンの歪み補正
- レーザのスキャン中にロボットは動いているため、スキャン点のビームを投光・受光する時の位置は異なる。
- これによりロボットの移動中に取得されたスキャン点には歪みが生じ、それはロボットの回転中に顕著になる。
- この歪みを補正する最も単純な方法は、スキャン開始から終了までの移動量の線形近似でロボット位置を計算し、スキャン点位置を補正する方法。
- ロボットの移動量は、スキャンと同期したオドメトリを用いるのが簡単。
スキャンマッチングの処理の流れ
0. 全体
フローチャート
メインフレームのソフト設計: matchScan
SlamFrontEndクラスからこのメソッドを呼び出す事でスキャンマッチングが実行される。
地図生成メソッドのソフト設計: growMap
- 観測したスキャンデータとその時の推定位置(あるいはオドメトリによる予測位置)を地図データに追加する
- 求めたロボット位置を基準にスキャン点を座標変換し、その結果をPointCloudMapクラスに渡す。
1. 初期地図の生成
- 初回スキャン時(cnt = 0)は、その時のスキャンデータとロボット位置を地図データに追加するのみ。
2. ロボットの予測位置の計算
- 前回スキャン時の位置から現在スキャン位置までの移動量を求める。
前回の推定位置に移動量を加えて現在時刻の予測位置を求める。
ここでのオドメトリ値は、ロボットの内界センサより得られる2次元の位置座標x, yと方位角θと定義される。
3. 参照スキャンの生成
- 現在スキャンとマッチングさせるための参照スキャンを過去のデータから生成する。
- RefScanMakerクラスのmakeRefScanメソッドを呼び出す。
- 生成した参照スキャンは、次に位置推定を行うPoseEstimatorICPに渡す。
4. ロボットの位置の推定
- オドメトリ値による予測位置を初期値として最適化計算を回し、推定位置を計算する。
位置を推定する手法には、ICP(Iterative Closest Points)アルゴリズムが用いられる。
ICPアルゴリズム実行時は現在スキャンと参照スキャンのとの間の残差が計算される。
- この残差が閾値以下、計算に使用されたスキャン点数が閾値以上となればICPによる位置推定は成功となる。
成功しなかった場合は、オドメトリ値による予測位置を推定位置として採用する。
予測位置が採用された場合は、その予測の分散を加味した予測位置の共分散行列を計算する。
5. センサフュージョン
- ICPによる位置推定が成功した場合は、オドメトリによる予測値と融合させる。
- それぞれの誤差分布は正規分布と仮定する。
6. 地図の更新
- 現在スキャンデータと、求めた推定位置を地図データに統合する。
- スキャンデータはロボット座標系なので、ロボット推定位置を基準に地図座標系に変換する。
- これらのデータをPointCloudMapクラスに渡す事で、地図データが更新される。
GitHub
記載されている各UMLのダイアグラムは、全て下記のGitHubで公開済み。
