LiDARを用いたSLAM

SLAM

LiDARとAMCLを用いた自己位置推定 では、 Turtlebotを使ってマップを作る 等で事前に作成した地図を利用して、自己位置推定を行いました。

ノードとトピックの概略図は以下のとおりです。 点線内は事前にマップを作る作業におけるノードの例で、この場合はcostmap_2dを用いた例を示しています。 costmap_2dはセンサ情報を元に作成した占有格子地図を /map トピックにパブリッシュするので、パブリッシュされた地図をmap_saverノードを用いてファイルに保存します。

map_serverノードはこのようにして作成された既存の地図を読み込んで /map トピックにパブリッシュします。 amclノードがパブリッシュされた地図の情報とセンサー情報をもとに自己位置推定を行い、TFをブロードキャストします。

../_images/amcl_diagram.png

点線内の地図作成と、点線の外の自己位置推定は同時に行われないことに注意してください。 これを同時に行うことをSLAM（Simultaneous Localization and Mapping）と言います。

../_images/hector_diagram.png

gmappingとHector SLAM

今回はHector SLAMアルゴリズムを用いて地図生成と自己位置推定を行います。 Gmappingを使って地図を生成する で使用したgmappingもSLAMアルゴリズムの一つです。

gmappingはロボットのオドメトリが必要である一方で、Hector SLAMはオドメトリフリーなSLAM手法です。 ドローンを用いる場合には、オドメトリを得ることが難しいことが多いので、ドローン用のSLAM手法としては、Hector SLAMが適しているといえます。

ROSパッケージとして提供されているLiDARを用いたSLAMアルゴリズムの比較については、 ROSのLidarSLAMまとめ - Qiita が参考になります。

パッケージのインストール

hector_slam パッケージを使用するので、以下のコマンドでインストールしましょう。

sudo apt install ros-kinetic-hector-slam

Launchファイル

PX4 SITLシミュレータを起動する

SITLシミュレータを起動するLaunchファイルは以下のとおりです。 mymodel_sitl_amcl.launchとほとんど同じですが、 odom から base_link へのTFをパブリッシュする必要が無いので、パラメータを設定する部分を消してあります。

mymodel_sitl_hector.launch
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
<launch>

    <node pkg="tf" name="base2lidar" type="static_transform_publisher" args="0 0 0.1 0 0 0 base_link lidar_link 100"/>

    <arg name="vehicle" default="iris_2d_lidar"/>
    <arg name="world" default="$(find px4_sim_pkg)/worlds/willow_garage.world" />
    <arg name="sdf" default="$(find px4_sim_pkg)/models/iris_2d_lidar/model.sdf" />
    <arg name="verbose" default="false"/>
    <arg name="debug" default="false"/>

    <include file="$(find px4)/launch/mavros_posix_sitl.launch" >
        <arg name="sdf" value="$(arg sdf)" />
        <arg name="vehicle" value="$(arg vehicle)" />
        <arg name="verbose" value="$(arg verbose)" />
        <arg name="debug" value="$(arg debug)" />
        <arg name="world" value="$(arg world)" />
    </include>

    <param name="/mavros/setpoint_velocity/mav_frame"  type="str" value="BODY_NED" />
</launch>

hector_mappingノードを起動する

hector_mapping パッケージのmapping_default.launchをベースに変更を行うので、 roscp コマンドを使ってファイルをコピーします。

roscp hector_mapping mapping_default.launch catkin_ws/src/px4_sim_pkg/launch/

変える必要があるのは、以下の3つの値だけです。

それぞれ、以下の通りなので、自分のロボットに適したように設定します。

base_frame

ロボットのベース座標系（フレーム）

odom_frame

ロボットのオドメトリ座標系（フレーム）

scan_topic

レーザースキャンのパブリッシュされるトピック

また、ロボットのオドメトリが得られない場合は、 base_frame の値と odom_frame の値を同じにします。

hector_mappingパッケージを使う際のTFの設定については How to set up hector_slam for your robot に情報があるので、参考にしましょう。

<arg name="base_frame" default="base_footprint"/>
<arg name="odom_frame" default="nav"/>
<arg name="scan_topic" default="scan"/>

今回は以下のとおりにします。

<arg name="base_frame" default="base_link"/>
<arg name="odom_frame" default="base_link"/>
<arg name="scan_topic" default="/laser/scan"/>

最終的に、Launchファイルは以下のとおりになります。

mapping_default.launch
 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
<?xml version="1.0"?>

<launch>
  <arg name="tf_map_scanmatch_transform_frame_name" default="scanmatcher_frame"/>
  <arg name="base_frame" default="base_link"/>
  <arg name="odom_frame" default="base_link"/>
  <arg name="pub_map_odom_transform" default="true"/>
  <arg name="scan_subscriber_queue_size" default="5"/>
  <arg name="scan_topic" default="/laser/scan"/>
  <arg name="map_size" default="2048"/>

  <node pkg="hector_mapping" type="hector_mapping" name="hector_mapping" output="screen">

    <!-- Frame names -->
    <param name="map_frame" value="map" />
    <param name="base_frame" value="$(arg base_frame)" />
    <param name="odom_frame" value="$(arg odom_frame)" />

    <!-- Tf use -->
    <param name="use_tf_scan_transformation" value="true"/>
    <param name="use_tf_pose_start_estimate" value="false"/>
    <param name="pub_map_odom_transform" value="$(arg pub_map_odom_transform)"/>

    <!-- Map size / start point -->
    <param name="map_resolution" value="0.050"/>
    <param name="map_size" value="$(arg map_size)"/>
    <param name="map_start_x" value="0.5"/>
    <param name="map_start_y" value="0.5" />
    <param name="map_multi_res_levels" value="2" />

    <!-- Map update parameters -->
    <param name="update_factor_free" value="0.4"/>
    <param name="update_factor_occupied" value="0.9" />
    <param name="map_update_distance_thresh" value="0.4"/>
    <param name="map_update_angle_thresh" value="0.06" />
    <param name="laser_z_min_value" value = "-1.0" />
    <param name="laser_z_max_value" value = "1.0" />

    <!-- Advertising config -->
    <param name="advertise_map_service" value="true"/>

    <param name="scan_subscriber_queue_size" value="$(arg scan_subscriber_queue_size)"/>
    <param name="scan_topic" value="$(arg scan_topic)"/>

    <!-- Debug parameters -->
    <!--
      <param name="output_timing" value="false"/>
      <param name="pub_drawings" value="true"/>
      <param name="pub_debug_output" value="true"/>
    -->
    <param name="tf_map_scanmatch_transform_frame_name" value="$(arg tf_map_scanmatch_transform_frame_name)" />
  </node>

  <!--<node pkg="tf" type="static_transform_publisher" name="map_nav_broadcaster" args="0 0 0 0 0 0 map nav 100"/>-->
</launch>

実行する

はじめに、PX4 SITLシミュレータを起動します。

roslaunch px4_sim_pkg mymodel_sitl_hector.launch

今回はゲームパッドを用いてドローンを操作するので、mavteleopノードを起動します。

roslaunch px4_sim_pkg mavros_teleop.launch

ドローンを離陸させたあとに、hector_mappingノードを起動します。

roslaunch px4_sim_pkg mapping_default.launch

Rvizを起動して、地図がきちんとできているか確認しましょう。

rviz

最後に、OFFBOARDモードに変更して、ドローンを飛行させてみましょう。

rosrun mavros mavsys mode -c OFFBOARD
../_images/hector.gif

参考

ROS and Hector SLAM for Non-GPS Navigation

Hector SLAMを使った非GPS環境でのナビゲーション

A Flexible and Scalable SLAM System with Full 3D Motion Estimation

Hector SLAMアルゴリズムの論文

rosbash - ROS Wiki

ROS向けbashコマンド