ROS2で3D LiDARを用いたGraph SLAMによる三次元地図作成

8689 ワード
ROS2 ROS SLAM Lidar PCL PCL テキストリンク

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のありがたさを実感しました・・・