Cartographer を用いた 3D SLAM / 3D SLAM using Cartographer

第6回3D勉強会＠関東 - connpass
https://3dvision.connpass.com/event/156241/

Transcript

1. Cartographer を用いた 3D SLAM 千葉工業大学 未来ロボット技術研究センター 原 祥尭（HARA, Yoshitaka） 3D勉強会 2019-12-15 @ystk_hara
2. 自己紹介（略歴） 1 3D勉強会 2019-12-15 2007年：筑波大 油田･坪内･大矢研 修士 2007年～2011年：日立製作所 日立研究所 （旧、機械研究所） 2012年～2015年：学振特別研究員DC1 2015年：筑波大 坪内･大矢研 博士 2015年～：千葉工大 未来ロボット技術研究センター（fuRo） 北陽電機さまと共同開発 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
3. 自己紹介（ROS 解説） 2 3D勉強会 2019-12-15 “ROS の活用による屋外の歩行者空間に適応した自律移動ロボットの開発”, RSJ 第94回ロボット工学セミナー, 2015. https://www.slideshare.net/hara-y/ros-slam-navigation-rsj-seminar “ROS を用いた自律移動ロボットのシステム構築”, RSJ 第99回ロボット工学セミナー, 2016. https://www.slideshare.net/hara-y/ros-nav-rsj-seminar “解説：ROS を用いた自律走行”, 日本ロボット学会誌, vol. 35, no. 4, 2017. https://www.jstage.jst.go.jp/article/jrsj/35/4/35_35_286/_article/-char/ja/ 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
4. 自己紹介（SLAM 解説） 3 3D勉強会 2019-12-15 日経 Robotics 連載 “SLAM とは何か”, 全18回, 2016～2017. “SLAM とは何か：自己位置推定と地図構築の全体像”, RSJ セミナー, 2018. “ロボティクスにおける SLAM 手法と実用化例”, SSII OS, 2019. https://www.slideshare.net/hara-y/slam-ssii-os-2019 “SLAM の基礎とオープンソース”, RSJ セミナー, 2019. https://www.slideshare.net/hara-y/slam-rsj-seminar-2019 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
5. 各種の SLAM アプローチ 4 3D勉強会 2019-12-15
6. Cartographer で構築した3次元地図による自律走行 5 3D勉強会 2019-12-15 https://youtu.be/njw9BPQUoa0 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
7. つくばチャレンジ全チームの SLAM アプローチ 6 3D勉強会 2019-12-15 Laser SLAM が圧倒的多数、 ただグラフベースはまだ少ない RGB-D、Visual SLAM は少数、 屋外リアルワールドは難しい SLAM 不使用チームの戦略 ◼ オドメトリ地図で教示再生 ◼ GNSS（GPS など） ◼ トポロジカル地図 ◼ End-to-End 深層学習 13 25 8 2 2 15 0 5 10 15 20 25 30 Laser スキャンマッチング Laser ベイズフィルタ Laser グラフベース SLAM RGB-D SLAM Visual SLAM SLAM 不使用 その他の統計は [原+, “つくばチャレンジ2018技術調査アンケートの報告”] を参照 https://www.slideshare.net/hara-y/tsukuba-challenge-2018-survey 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
8. 自律走行や SLAM のオープンソース利用状況 7 3D勉強会 2019-12-15 ROS で標準的な navigation と slam_gmapping が多い Autoware も多い Cartographer は意外に少ない （試用して諦めたとの声も） 製品である Stencil の利用も Visual SLAM は RTAB-Map と ORB-SLAM のみで少ない 18 20 1 1 8 2 1 1 2 1 0 5 10 15 20 25 ROS navigation ROS slam_gmapping ROS hector_slam ROS slam_karto Autoware Cartographer MRPT Kaarta Stencil RTAB-Map ORB-SLAM その他の統計は [原+, “つくばチャレンジ2018技術調査アンケートの報告”] を参照 https://www.slideshare.net/hara-y/tsukuba-challenge-2018-survey 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
9. オープンソース Laser SLAM の比較 8 3D勉強会 2019-12-15  上記以外にも数多くの手法が存在する  ループ検出とグラフ最適化によるループ閉じ込みが、特に広大な環境において重要 [友納, 原, “SLAM の現状と今後の展望”, システム制御情報学会誌, 2020] Laser SLAM (LIDAR SLAM) と Visual SLAM (Camera SLAM) の全体像を解説 分類 次元 地図形式 スキャン歪み 対策 ループ検出 グラフ最適化 GMapping RBPF 2D 占有格子 × 非明示的 × Autoware (ndt_mapping) スキャン マッチング 3D 点群 × × × Velodyne SLAM スキャン マッチング 3D 点群 ◦ × × LOAM スキャン マッチング 3D 点群 ◦ × × Karto SLAM グラフベース 2D 占有格子 × 相関マッチ ング (粗密探索) SPA Cartographer グラフベース 2D, 3D 占有格子 ◦ 相関マッチ ング (分枝限定法) Ceres Solver 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
10. Cartographer を用いた SLAM 9 3D勉強会 2019-12-15
11. Cartographer のアルゴリズム概要 10 3D勉強会 2019-12-15  Voxel Filter ◼ 固定サイズで点群を間引き  Adaptive Voxel Filter ◼ 最大サイズと最小点数で動的に間引き  Local Scan Matching（逐次SLAM） ◼ Ceres スキャンマッチング／ 相関スキャンマッチングから選択 ◼ Ceres は初期位置からの局所マッチング  Global Scan Matching（ループ検出） ◼ 相関スキャンマッチングで大域探索  Pose Adjustment（グラフ最適化） ◼ SPA でポーズ調整 ◼ 拘束の外れ値はM推定で対処 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
12. Cartographer は高性能だがハードルも高い 11 3D勉強会 2019-12-15 公式ドキュメント（順次更新されているが…） https://google-cartographer.readthedocs.io/en/latest/ https://google-cartographer-ros.readthedocs.io/en/latest/ システムが複雑 コード行数：約6万行（約500ファイル） パラメータ数：約100個 パラメータ調整が難しい（開発者ら自身が以下のように説明） “アルゴリズム内部の十分な理解が必要で、パラメータ調整は本当に困難” 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
13. 3次元 Cartographer のデフォルトパラメータで構築 12 3D勉強会 2019-12-15 占有格子地図の平行投影 大域的な整合性が破綻、 局所的にはそれなり 黄色のループ拘束がほぼ 生成されていない つくば市役所 研究学園駅前公園 再訪位置が同じ位置に マッピングされず、 ループが閉じていない 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
14. パラメータ調整によるループ閉じ込みの実現 13 3D勉強会 2019-12-15 ループ拘束が適切 に生成されるよう パラメータ調整 3次元は処理負荷大、 3～24時間程度 (Core i7 8750H) 処理時間が長く、 試行錯誤は困難 →自動化したいが… つくば市役所 研究学園駅前公園 1,000 m 走行距離 6 km、約2時間走行 ループ閉じ込みによって 全体の整合性が取れている 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
15. 3次元点群地図の出力 14 3D勉強会 2019-12-15
16. 3次元点群地図の出力 15 3D勉強会 2019-12-15 Cartographer 本体は、3次元地図を出力しない ◼ 内部では、サブマップで構築された3次元占有格子地図を持つ ◼ サブマップの可視化は可能だが、ファイル出力機能はない Assets Writer という後処理の枠組みにより、3次元点群地図を構築可能 ◼ 入力： ポーズグラフ（pbstream） スキャン点群（rosbag） ◼ パイプライン処理を設定する ◼ “write_pcd” 機能で点群地図を PCDファイルに出力できる Cartographer Assets Writer Front-end (Sequential/Loop Registration) Back-end (Pose Graph Optimization) Various Filtering “write_pcd” Scan Cloud IMU Odom Pose Graph Scan Cloud Cloud Map 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
17. 移動物体を除去するパイプラインを新たに追加 16 3D勉強会 2019-12-15 ボクセルを用いたバイナリベイズフィルタで移動物体を除去（新規実装） 生の点群地図には、歩行者や他ロボットなどの移動物体が残る 従来のバイナリベイズフィルタ では、地面も誤って消えてしまう →グリッド分割に伴う本質的問題 ビーム入射角を考慮したモデル により、地面の点群を適切に残す [AR, 2020 (accepted)] X, Y Z LIDAR hit (occupied) miss (free) unknown 地面はビームが浅い照射となるため、 多数のビームが通過して自由空間と誤判定される 側面図 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
18. フィルタリングによる移動物体除去の比較 17 3D勉強会 2019-12-15 生の点群 頻度占有モデル（従来手法） ベイズ占有モデル（従来手法） 入射角を考慮したベイズ占有モデル 移動物体やノイズが含まれる 歩行者などの軌跡が残る 地面が消えてしまい、ノイズは残ってしまう 移動物体は消えるが、地面も消えてしまう 移動物体が消え、地面は残る [AR, 2020 (accepted)] 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
19. Cartographer を利用して構築した3次元地図 18 3D勉強会 2019-12-15 colored by intensity 自動で地図構築、手動での修正なし ビーム入射角を考慮した移動物体除去 [AR, 2020 (accepted)] 受光強度で地面の舗装／芝生／白線などを区別できる つくば市役所 研究学園駅前公園 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
20. Cartographer のパラメータ調整 19 3D勉強会 2019-12-15
21. パラメータ調整の指針 20 3D勉強会 2019-12-15 確認すべきパラメータファイル ◼ cartographer/configuration_files/trajectory_builder_3d.lua ◼ cartographer/configuration_files/pose_graph.lua ◼ cartographer_ros/configuration_files/backpack_3d.lua 逐次 SLAM、ループ検出、グラフ最適化の順で調整すると良い 特にループ検出で十分な数のループ拘束を生成することが重要、 ループ拘束の外れ値はグラフ最適化（ポーズ調整）のM推定で対処 RViz で逐次拘束（サブマップ内）とループ拘束（サブマップ間）の 可視化を切り替えながら確認（重なって表示されると見にくいので） 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
22. 各パラメータの説明（センサ関連） 21 3D勉強会 2019-12-15  TRAJECTORY_BUILDER_3D.min/max_range TRAJECTORY_BUILDER_3D.high/low_resolution_adaptive_voxel_filter.max_range LIDAR の仕様をベースに調整、ただし high_resolution を長くすると処理が重い  TRAJECTORY_BUILDER_3D.voxel_filter_size 後述の submaps.high_resolution と整合するように調整  TRAJECTORY_BUILDER_3D.num_accumulated_range_data（スキャン歪み対策） LIDAR 点群のスキャン分割に合わせて調整、全周スキャン点群ごとに配信の場合は“1”、 センサハンドラでのスキャン分割（全周ごとではなく分割して配信）が必要 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
23. 各パラメータの説明（サブマップ関連） 22 3D勉強会 2019-12-15  TRAJECTORY_BUILDER_3D.submaps.num_range_data サブマップのサイズ、整合性が保てる範囲で大きい方がループ検出しやすい  TRAJECTORY_BUILDER_3D.submaps.high/low_resolution TRAJECTORY_BUILDER_3D.submaps.high_resolution_max_range サブマップの解像度や反映する点群の最大距離  TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.hit/miss_probability TRAJECTORY_BUILDER_3D.submaps.range_data_inserter.num_free_space_voxels サブマップを構築する逆計測モデル（占有と自由のバイナリベイズフィルタ） 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
24. 各パラメータの説明（逐次 SLAM） 23 3D勉強会 2019-12-15  TRAJECTORY_BUILDER_3D.ceres_scan_matcher.translation/rotation_weight Ceres スキャンマッチングの初期位置（オドメトリ）からのペナルティ、 大きいとマッチングせず小さいと軌跡がジグザグになる 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
25. 各パラメータの説明（ループ検出、グラフ最適化） 24 3D勉強会 2019-12-15  POSE_GRAPH.optimize_every_n_nodes グラフ最適化（ポーズ調整）の頻度、グラフ最適化しないとサブマップ間の形状はズレる  POSE_GRAPH.constraint_builder.sampling_ratio POSE_GRAPH.constraint_builder.max_constraint_distance ループ拘束を生成する軌跡ノードをサンプリングするレートと範囲、 サンプリングした軌跡ノードに対してループ検出する  POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.linear_xy/z_search_window POSE_GRAPH.constraint_builder.fast_correlative_scan_matcher_3d.angular_search_window ループ検出する相関スキャンマッチングの探索範囲、逐次 SLAM の誤差に合わせて調整 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
26. 各パラメータの説明（ループ検出、グラフ最適化） 25 3D勉強会 2019-12-15  POSE_GRAPH.constraint_builder.min_score ループ拘束を採用する最小スコア（0.0～1.0）  POSE_GRAPH.constraint_builder.loop_closure_translation/rotation_weight グラフ最適化でのループ拘束の重み  POSE_GRAPH.optimization_problem.odometry_translation/rotation_weight POSE_GRAPH.optimization_problem.acceleration/rotation_weight グラフ最適化でのオドメトリや IMU の重み  POSE_GRAPH.optimization_problem.huber_scale（拘束の外れ値対策） グラフ最適化でのM推定の Huber loss 各種 SLAM Cartographer 3次元地図出力 パラメータ調整
27. まとめ 26 3D勉強会 2019-12-15
28. まとめ 27 3D勉強会 2019-12-15 つくばチャレンジ全チームの SLAM アプローチの統計 →屋外リアルワールドでは、現状は Laser SLAM の方が強力 オープンソース Laser SLAM の比較 3次元 Cartographer を用いた地図構築の実例 Assets Writer の後処理による、3次元点群地図の出力 ビーム入射角を考慮したバイナリベイズフィルタによる移動物体除去 Cartographer のパラメータ調整ノウハウ 各種 SLAM Cartographer 3次元地図出力 パラメータ調整