Edited at

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


環境

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

項目

CPU
Core i5-8250U

Ubuntu
16.04

ROS
Kinetic

Gazebo
7.14.0

インストールについては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フレームがずれていくのが見えます。

nav_gmapping.gif


地図の保存

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


地図の保存

roscd nav_lecture/resouces/room1 #地図ファイルを保存する場所に移動

rosrun map_server map_saver _map:=/dtw_robot1/map

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


地図がある場合

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


nav_amcl.gif

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


参考

gmappingのパラメーター調整

amclのパラメーター調整


目次ページへのリンク

ROS講座の目次へのリンク