emcl: mcl with expansion resetting

emcl is an alternative Monte Carlo localization (MCL) package to amcl (http://wiki.ros.org/amcl). Differently from amcl, KLD-sampling and adaptive MCL are not implemented. Instead, the expansion resetting is implemented¹.

expansion resetting

The expansion resetting had been used in the classical RoboCup 4-legged robot league as a robust localization mechanism since the robots had made frequent localization errors². This method expands the distribution of particles when the robot suffers surprising sensor data. This mechanism is effective toward skidding and small range kidnaps of robots.

demo movie

Nodes

mcl_node

This node transforms laser scans and odometry transform messages to pose estimations by using an occupancy grid map.

Subscribed Topics

• scan (sensor_msgs/LaserScan)
  • laser scans
• tf (tf/tfMessage)
  • transforms
• initialpose (geometry_msgs/PoseWithCovarianceStamped)
  • pose of particles for replacement

Published Topics

• mcl_pose (geometry_msgs/PoseWithCovarianceStamped)
  • the mean pose of the particles with covariance
• particlecloud (geometry_msgs/PoseArray)
  • poses of the particles
• tf (tf/tfMessage)
  • the transform from odom (which can be remapped via the ~odom_frame_id parameter) to map
• alpha (std_msgs/Float32)
  • marginal likelihood of particles after sensor update

Services Called

• static_map (nav_msgs/GetMap)
  • mcl initializes the map for localization

Parameters

• ~odom_freq (int, default: 20 [Hz])
  • frequency of odometry update
• ~num_particles (int, default: 1000)
  • number of particles
• ~odom_frame_id (string, default: "odom")
  • the frame for odometry
• ~base_frame_id (string, default: "base_footprint")
  • the frame of the localized robot's base
• ~global_frame_id (string, default: "map")
  • the frame for localization
• ~initial_pose_x (double, default: 0.0 [m])
  • initial x coordinate of particles
• ~initial_pose_y (double, default: 0.0 [m])
  • initial y coordinate of particles
• ~initial_pose_a (double, default: 0.0 [rad])
  • initial yaw coordinate of particles
• ~odom_fw_dev_per_fw (double, default: 0.19 [m/m])
  • standard deviation of forward motion noise by forward motion
• ~odom_fw_dev_per_rot (double, default: 0.0001 [m/rad])
  • standard deviation of forward motion noise by rotational motion
• ~odom_rot_dev_per_fw (double, default: 0.13 [rad/m])
  • standard deviation of rotational motion noise by forward motion
• ~odom_rot_dev_per_rot (double, default: 0.2 [rad/rad])
  • standard deviation of rotational motion noise by rotational motion
• ~laser_likelihood_max_dist (double, default: 0.2 meters)
  • maximum distance to inflate occupied cells on the likelihood field map
• ~alpha_threshold (double, default: 0.0)
  • threshold for expansion resetting
• ~expansion_radius_position (double, default: 0.1)
  • maximum change of the position on the xy-plane when the reset replaces a particle
• ~expansion_radius_orientation (double, default: 0.2)
  • maximum change of the yaw angle when the reset replaces a particle

Notes

likelihood field and alpha value

This implementation uses an ad-hoc likelihood field model. Occupied cells on the map are inflated so that each collision detection between a laser beam and an occupied cell is relaxed. The likelihood for each cell is given with a pyramidal kernel function. The parameter ~laser_likelihood_max_dist gives the length from the center cell to the edge of the pyramid.

The likelihoods on the field are normalized. The maximum value is 1.0. The alpha value becomes 1.0 when all beams hit the 1.0 cells. A suitable ~alpha_threshold value exists in the range between 0.0 and 1.0. In the noisy environment, or with a noisy sensor, the value should be near zero so as to prohibit prohibit excess resets. However, please note that a reset doesn't change the center of particles largely. So it's okay even if resettings occur sporadically. Please check the /alpha topic under various conditions so as to find a suitable ~alpha_threshold value.

citation

Footnotes

1. R. Ueda: "Syokai Kakuritsu Robotics (lecture note on probabilistic robotics)," Kodansya, 2019. ↩

2. R. Ueda, T. Arai, K. Sakamoto, T. Kikuchi, S. Kamiya: Expansion resetting for recovery from fatal error in Monte Carlo localization - comparison with sensor resetting methods, IEEE/RSJ IROS, pp.2481-2486, 2004. ↩