Sunday, October 23, 2011

Mapping the Environment

The next stage is to test out the mapping performance of the Turtlebot.  It's also interesting to compare the maps to those I created earlier with my larger GROK2 robot, which is using the same ROS/Gmapping software.

Initially I performed the calibration of the gyro and odometry, as described here.  Then I created a launch file using the same Gmapping parameters as for my other robot, and also including the calibration results.
<launch>

  <param name="robot_description" command="$(find xacro)/xacro.py '$(find turtlebot_description)/urdf/turtlebot.urdf.xacro'" />

  <param name="turtlebot_node/gyro_scale_correction" value="1.003914"/>
  <param name="turtlebot_node/odom_angular_scale_correction" value="0.925940"/>

  <!--- Run Robot Pose EKF -->
  <include file="$(find turtlebot_bringup)/base.launch" />

  <!--- Run the kinect -->
  <include file="$(find turtlebot_bringup)/kinect.launch" />

  <!--- Run gmapping -->
  <node pkg="gmapping" type="slam_gmapping" name="slam_gmapping" output="screen">
    <param name="odom_frame" value="odom"/>
    <param name="map_update_interval" value="30.0"/>
    <param name="maxUrange" value="16.0"/>
    <param name="sigma" value="0.05"/>
    <param name="kernelSize" value="1"/>
    <param name="lstep" value="0.05"/>
    <param name="astep" value="0.05"/>
    <param name="iterations" value="5"/>
    <param name="lsigma" value="0.075"/>
    <param name="ogain" value="3.0"/>
    <param name="lskip" value="0"/>
    <param name="srr" value="0.01"/>
    <param name="srt" value="0.02"/>
    <param name="str" value="0.01"/>
    <param name="stt" value="0.02"/>
    <param name="linearUpdate" value="0.5"/>
    <param name="angularUpdate" value="0.436"/>
    <param name="temporalUpdate" value="-1.0"/>
    <param name="resampleThreshold" value="0.5"/>
    <param name="particles" value="80"/>

    <param name="xmin" value="-1.0"/>
    <param name="ymin" value="-1.0"/>
    <param name="xmax" value="1.0"/>
    <param name="ymax" value="1.0"/>

    <param name="delta" value="0.02"/>
    <param name="llsamplerange" value="0.01"/>
    <param name="llsamplestep" value="0.01"/>
    <param name="lasamplerange" value="0.005"/>
    <param name="lasamplestep" value="0.005"/>
  </node>

  <!--- Run Move Base -->
  <include file="$(find turtlebot_navigation)/config/move_base_turtlebot.launch" />

</launch>

The only parameter which is different from the defaults here is the grid cell size delta

I also made a teleoperation launch file with reduces speeds to make the movement of the robot less jerky and prone to wheel slippage.


<launch>
  <node pkg="turtlebot_teleop" type="turtlebot_teleop_key" name="turtlebot_teleop_keyboard"  output="screen">
    <remap from="cmd_vel" to="/turtlebot_node/cmd_vel"/>
    <param name="scale_linear" value="0.2" type="double"/>
    <param name="scale_angular" value="0.3" type="double"/>
  </node>
</launch>

Using these I obtained the following map:


This compares quite favorably with previous mapping runs using my other robot.  I don't know what type or resolution the wheel encoders on the iRobot Create are, but I expect that they will be lower resolution than the quadrature encoders that I'm using elsewhere.

I also tried mapping with the default parameters, which have a grid cell size of 5cm.


As you can see the results are not as good, which might translate into poor localization performance in some areas of the map, so if your CPU is good enough then it's well worth using a smaller cell size.


Some thoughts on mapping in general

This kind of mapping performance using a hobby robot with a total cost in the order of $1000-2000 represents a massive advance over anything which was possible in that price range previously, but I expect that in future with improved software that even better will be possible.

The main limitation with Gmapping, and similar particle filter based SLAM systems is that there needs to be a distinct mapping phase, after which the map is assumed to be "done" as a one-time operation.  However, for practical service robotics I expect that lifelong learning is going to be needed, since the environment may change occasionally and the robot may need to relearn its surroundings.  To facilitate this sort of situation I think the way to go will be to use graph based 3D SLAM methods.

No comments: