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 movies

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

• global_localization (std_srvs/Empty)
  • Initiate global localization, wherein all particles are dispersed randomly through the free space in the map.

Services Called

• static_map (nav_msgs/GetMap)
  • Initiate 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
• ~footprint_frame_id (string, default: "base_footprint")
  • the frame of the localized robot's base
• ~base_frame_id (string, default: "base_link")
  • the frame of the robot's base. It is used for calculating the position and orientation of the LiDAR.
• ~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 of the alpha value for expansion resetting
• ~open_space_threshold (double, default: 0.05)
  • threshold of the valid beam rate for expansion resetting; the reset doesn't occur when the rate of beams in the valid range is smaller than this threshold
• ~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
• ~scan_increment (int, default: 1)
  • increment number when beams are picked from their sequence; the larger this number is, the fewer number of beams are used for calculation of likelihood

Parameters to be available

• ~laser_min_range (double, default: 0.0[m])
  • threshold for discarding scans whose ranges are smaller than this value
• ~laser_max_range (double, default: 100000000.0[m])
  • threshold for discarding scans whose ranges are larger than this value

Notes

limitation

Only one 2D LiDAR placed in a horizontal fashion is supported.

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 valid beams hit the 1.0 cells. A suitable ~alpha_threshold value exists in the range between 0.0 and 1.0. In a noisy environment, or with a noisy sensor, the value should be near zero so as to 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.

ROS version

• ROS Noetic Ninjemys (on Ubuntu 20.04 LTS, test on my note PC)
• ROS Melodic Morenia (on Ubuntu 18.04 LTS, test on GitHub Actions)

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. ↩