ROS2で3D LiDARできるパッケージが無かったので、今回はROS2で3D LiDARを使用したGraph SLAMのプログラムを書いて三次元地図を作りました！
書いたコードはGithubにあります。
https://github.com/rsasaki0109/graphslam_ros2
ループクローズにより修正された経路
地図
事前知識
graphslam_ros2は逐次的なNDT/GICP-スキャンマッチングによるフロントエンドと一定周期で実行されるGraph SLAMによるバックエンドによって成り立っています。
スキャンマッチングに関しては以下のスライドを読めばわかると思います。
Scan Matching 自己位置推定手法
LiDAR-SLAM チュートリアル資料
NDT scan matchingについて詳しくは以下を参照ください。(GICPに関して、解説してる日本語記事はわかりませんが、元論文が比較的簡単に読めると思います。）
gggggraziegrazie NDT( Normal Distributions Transform )
NDTスキャンマッチング 第1回3D勉強会@PFN 2018年5月27日
Graph SLAMに関しては以下の解説論文に詳しく書いてあります。
[移動ロボットの環境認識 —地図構築と自己位置推定]
(https://www.jstage.jst.go.jp/article/isciesci/60/12/60_509/_pdf)
より詳しくは『SLAM入門』を参照ください。
環境
ubuntu：18.04
ros1：melodic
ros2：dashing
アルゴリズムの説明
-
frontend(scan-matcher)
以前の複数の入力点群と現在の入力点群で逐次スキャンマッチングをします。また、一定の距離毎にループ探索の候補となる部分地図(とその中心位置)を生成し、
保存していきます。 -
backend(graph-based-slam)
一定周期で直近の候補点と今までの候補点全てでndtスキャンマッチングを行い、マッチングスコアが閾値以下ならそこでループ制約を生成し、ポーズ最適化を行います。
パッケージ
frontendにgicp/ndt scan matching、backendにg2oによるgraph-based slamを使っています.
特徴
・ndt_ompによるOpenMPを使用した高速なスキャンマッチング
・g2oによるポーズ調整
・スキャンマッチングの初期値をオドメトリで補間
・(回転式LiDARのみ)9軸IMUで歪み補正
入出力
フロントエンド(scan-matcher)
-
input
/input_cloud (sensor_msgs/PointCloud2)
/tf(from "base_link" to LiDAR's frame)
/initial_pose (geometry_msgs/PoseStamed)(optional)
/imu (sensor_msgs/Imu)(optional)
/odom (nav_msgs/Odometry)(optional) -
output
/current_pose (geometry_msgs/PoseStamped)
/map (sensor_msgs/PointCloud2)
/path (nav_msgs/Path)
/tf(from "map" to "base_link")
/map_array(graphslam_ros2_msgs/MapArray)
scan-matcherはバックエンドなしでも使えます。
バックエンド(graph-based-slam)
- input
/map_array(graphslam_ros2_msgs/MapArray) - output
/modified_map_array(graphslam_ros2_msgs/MapArray)
/modified_path (nav_msgs/Path)
/modified_map (sensor_msgs/PointCloud2)
またgraph-based-slamは pose_graph.g2o と map.pcd をループ閉じ込み時または下記のサービスコール時に出力します。
ros2 service call /map_save std_srvs/Empty
パラメーター
- フロントエンド(scan-matcher)
| Name | Type | Default value | Description |
|---|---|---|---|
| registration_method | string | "NDT" | "NDT" か "GICP"を選べます |
| ndt_resolution | double | 5.0 | ボクセルの解像度[m] |
| ndt_num_threads | int | 0 | ndtに使うスレッド数(0にすると使用できる最大のスレッド数を使います) |
| trans_for_mapupdate | double | 1.5 | map updateに必要な移動距離[m] |
| vg_size_for_input | double | 0.2 | input cloudのdown sample size[m] |
| vg_size_for_map | double | 0.05 | map cloudのdown sample size[m] |
| scan_max_range | double | 100.0 | 使用する入力点群の最大距離[m] |
| scan_min_range | double | 1.0 | 使用する入力点群の最小距離[m] |
| scan_periad | double | 0.1 | LiDARの回転周期[sec] |
| map_publish_period | double | 15.0 | 点群地図のpuplish周期[sec] |
| num_targeted_cloud | int | 10 | registrationのtargetに使用する点群の数 |
| set_initial_pose | bool | false | paramファイルから初期位置(map原点)を決めるかどうか |
| initial_pose_x | double | 0.0 | 初期位置のx座標[m] |
| initial_pose_y | double | 0.0 | 初期位置のy座標[m] |
| initial_pose_z | double | 0.0 | 初期位置のz座標[m] |
| initial_pose_qx | double | 0.0 | 初期位置のqx |
| initial_pose_qy | double | 0.0 | 初期位置のqy |
| initial_pose_qz | double | 0.0 | 初期位置のqz |
| initial_pose_qw | double | 1.0 | 初期位置のqw |
| use_imu | bool | false | LiDARの歪み補正にimuを使うかどうか |
| use_odom | bool | false | スキャンマッチングの初期姿勢値にodomを使うかどうか |
| debug_flag | bool | false | スキャンマッチング結果の情報を表示するかどうか |
- バックエンド(graph-based-slam)
| Name | Type | Default value | Description |
|---|---|---|---|
| ndt_resolution | double | 5.0 | ndtの解像度[m] |
| ndt_num_threads | int | 0 | ndtに使うスレッド数(0にすると使用できる最大のスレッド数を使います) |
| voxel_leaf_size | double | 0.2 | input cloudのdown sample size[m] |
| loop_detection_period | int | 1000 | ループ探索を行う周期[ms] |
| threshold_loop_clousure_score | double | 1.0 | loop clousureしたと判定するためのndtのマッチングスコアの閾値 |
| distance_loop_clousure | double | 20.0 | 候補点をloop clousureの探索対象にするために候補点からどれくらい離れるか[m] |
| range_of_searching_loop_clousure | double | 20.0 | 現在地からloop clousureの探索範囲[m] |
| search_submap_num | int | 2 | registrationに使用する再訪点の前後のサブマップ点の数 |
デモ
デモデータはhdl_graph_slamにあるROS1のhdl_400.bag を使います。
後は以下のコマンドを順次実行してください。
rviz2 -d src/graphslam_ros2/scanmatcher/config/mapping.rviz
ros2 launch lidarslam lidarslam.launch.py
ros2 bag play -s rosbag_v2 hdl_400.bag
結果は以下のようになります！うまくループが閉じていることが確認できますね！
Green: path with loopclosure, Yellow: path without loopclosure
地図
さいごに
とりあえず動いて良かったです。
普段使ってるOSSのありがたさを実感しました・・・