ROS講座91 Laserでmap位置を推定する

環境

この記事は以下の環境で動いています。

項目	値
CPU	Core i5-8250U
Ubuntu	20.04
ROS	melodic
Gazebo	11.10.2

インストールについてはROS講座02 インストールを参照してください。
またこの記事のプログラムはgithubにアップロードされています。ROS講座11 gitリポジトリを参照してください。

概要

前回何種類かの位置情報の統合の例を出しました。今回は実用的な方法としてLaserマッチング+オドメトリーでの位置の推定を行います。

• odom位置（odom->base_link）にwheel odometryとIMUを統合したものを使います。
• map位置（map->base_link）にLaserマッチングをしたものを使います。

またLaserマッチングには2通りの方法があります。

• 地図が無いパターン
  事前に環境の地図を持っていなく、地図の作成を同時に行う方法です。SLAMと呼ばれるものです。gmappingというパッケージを使います。
• 地図があるパターン
  事前にある地図とパターンマッチングを行って位置を割り出すものです。amclというパッケージを使います。

インストール

必要なパッケージのインストール

apt-get install -y ros-kinetic-gmapping ros-kinetic-amcl ros-kinetic-map-server 

#地図が無い場合
gmappingを使用します。

ソースコード

config

gmapping用のconfigです。デフォルトだと5秒ごとにマッチング動作を行うのですがこれを1秒ごとに変更します。

nav_lecture/config/gmapping.yaml

map_update_interval: 1.0
temporalUpdate: 1.0
xmin: -4.0
xmax:  4.0
ymin: -4.0
ymax:  4.0

launch

nav_lecture/launch/gmapping.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find nav_lecture)/xacro/dtw_robot.xacro" />
  <arg name="rvizconfig" default="$(find nav_lecture)/rviz/gmapping.rviz" />  
  
  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find nav_lecture)/worlds/test1.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- dtw_robot -->
  <include file="$(find nav_lecture)/launch/dtw_spawn.launch">
    <arg name="robot_name" value="dtw_robot1"/>
  </include>

  <group ns="dtw_robot1">
    <!-- to increase the wheel odometry error -->
    <param name="/dtw_robot1/diff_drive_controller/wheel_radius" value="0.055" />
    <!-- robot localization -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true" output="screen">
      <rosparam command="load" file="$(find nav_lecture)/config/ekf_localization/mode1.yaml" />
      <param name="tf_prefix" value="dtw_robot1"/>
      <remap from="/odometry/filtered" to="fusion/odom" />
      <remap from="odom0" to="diff_drive_controller/odom" />
      <remap from="imu0"  to="imu/data" />
    </node>

    <!-- gmapping -->
    <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" clear_params="true">
      <rosparam command="load" file="$(find nav_lecture)/config/gmapping.yaml" />
      <remap from="scan" to="front_laser/scan" />
      <param name="base_frame" value="dtw_robot1/base_link" />
      <param name="odom_frame" value="dtw_robot1/odom" />
      <param name="map_frame"  value="dtw_robot1/map" />
    </node>
  </group>

  <!-- rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

実行

gmappingの実行

roslaunch nav_lecture gmapping.launch 

以下の画像のように進むたびにどんどんと地図が広がっていきます。1秒ごとにマップマッチングが走り自己位置を修正をしています。だんだんとwheel odometryがドリフトするので地図原点当たりのmapフレームとodomフレームがずれていくのが見えます。

地図の保存

後でamclを使うためにgmappingで作成した地図を保存します。

地図の保存

roscd nav_lecture/resouces/room1 #地図ファイルを保存する場所に移動
rosrun map_server map_saver _map:=/dtw_robot1/map

以下のようなmap画像が取得できます。

別ウィンドウで実行する

gmappingを起動すると以下のようなログが出ます。これはprintfで直接出力しているので、抑制することが出来ません。他のROSのログが埋もれてしまいます。

gmappingのログ

m_count 5
Average Scan Matching Score=195.33
neff= 30
Registering Scans:Done
update frame 48
update ld=9.61784e-07 ad=1.36017e-05
Laser Pose= 0.0699991 0.0100065 -1.57061

以下のようにすると別のXtermのウィンドウを起動して分離して起動できます。

launchでの記述

<node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" launch-prefix="xterm -e"/>

地図がある場合

amclを使って位置の推定をするパターンです。

ソースコード

config

amclのパラメーターです。githubのページに置いています。

launch

nav_lecture/launch/amcl.launch

<?xml version="1.0" encoding="UTF-8"?>
<launch>
  <arg name="model" default="$(find nav_lecture)/xacro/dtw_robot.xacro" />
  <arg name="rvizconfig" default="$(find nav_lecture)/rviz/amcl.rviz" />  
  
  <!-- gazebo world -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="world_name" value="$(find nav_lecture)/worlds/test1.world"/>
    <arg name="paused" value="false"/>
    <arg name="use_sim_time" value="true"/>
    <arg name="gui" value="true"/>
    <arg name="headless" value="false"/>
    <arg name="debug" value="false"/>
  </include>

  <!-- dtw_robot -->
  <include file="$(find nav_lecture)/launch/dtw_spawn.launch">
    <arg name="robot_name" value="dtw_robot1"/>
  </include>

  <group ns="dtw_robot1">
    <!-- to increase the wheel odometry error -->
    <param name="/dtw_robot1/diff_drive_controller/wheel_radius" value="0.055" />
    <!-- robot localization -->
    <node pkg="robot_localization" type="ekf_localization_node" name="ekf_se" clear_params="true" output="screen">
      <rosparam command="load" file="$(find nav_lecture)/config/ekf_localization/mode1.yaml" />
      <param name="tf_prefix" value="dtw_robot1"/>
      <remap from="/odometry/filtered" to="fusion/odom" />
      <remap from="odom0" to="diff_drive_controller/odom" />
      <remap from="imu0"  to="imu/data" />
    </node>

    <!-- amcl -->
    <node pkg="map_server" type="map_server" name="map_server" args="$(find nav_lecture)/resources/room1/map.yaml">
      <param name="frame_id" value="dtw_robot1/map"/>
    </node>
    <node pkg="amcl" type="amcl" name="amcl" clear_params="true">
      <rosparam command="load" file="$(find nav_lecture)/config/amcl.yaml" />
      <param name="base_frame_id" value="dtw_robot1/base_link" />
      <param name="odom_frame_id" value="dtw_robot1/odom" />
      <param name="global_frame_id" value="dtw_robot1/map" />
      <remap from="scan" to="front_laser/scan" />
    </node>
  </group>

  <!-- rviz -->
  <node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />
</launch>

amclは事前の地図が必要です。この地図をROSトピックとしてpublishするのがmap_serverです。

実行

amclの実行

roslaunch nav_lecture amcl.launch

以下の画像で赤い矢印で表示しているのがロボットの位置の候補です。このようにマッチングの候補をばらまいて位置の推定を行っているのが見えます。

パラメーターチューニング

amclの初期パラメーターは室内用ロボットには向いていない設定なので調節する必要があります。

• 移動系のパラメーター
  • odom_model_typeはロボットの特性に合わせてdiff_corrected/omni_correctedを選択しましょう。****_correctedがついていない方は非推奨です。
• 更新系のパラメーター
  • update_min_d、update_min_aはそれぞれ次の観測更新を行うまでの最小距離/角度です。これらの距離/角度のどちらか以上動かないと更新されません。更新頻度が長くですぐずれるという場合はここを小さくしましょう。
• レーザー系のパラメーター
  • laser_max_beams: laser_scanのうち使用する点数です。この数になるように間引かれるので、もう少し大きい値にしましょう
  • laser_z_rand/laser_sigma_hit: ランダムノイズと誤差の標準偏差です。室内ロボットならデフォルトよりも小さい値(0.01程度)まで絞れます。これがないとmapとlaser_scanがあっているのにいまいちマッチしないなという状態になります。

参考

gmappingのパラメーター調整
amclのパラメーター調整

目次ページへのリンク

ROS講座の目次へのリンク