emcl2: mcl with expansion resetting (version 2)

test

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 and other features are implemented12.

This package is an improved version of ryuichi/emcl. This version works well in crowded or outdoor environments.

demo movies

Nodes

emcl2_node

This node calculates the alpha value with a different algorithm than emcl_node in ryuichi/emcl. This node counts the particles that make lasers penetrate occupancy cells. Specifically, this node chooses some particles at a rate of ~extraction_rate and checks each of them with the following procedure:

  • maps a set of laser scan on the occupancy grid map based on the pose of the particle
  • judges the pose of the particle as wrong if all of lasers in a ~range_threshold[rad] range penatrate occupancy grids

If the rate of the wrong particles is greater than ~alpha_threshold, the node invokes a reset.

This node also has a sensor resetting algorithm. When ~sensor_reset is true, a particle with laser penetration is dragged back from occupied cells.

Subscribed Topics

Published Topics

Services

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

Services Called

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
  • ~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
  • ~alpha_threshold (double, default: 0.5)
    • threshold of the alpha value for expansion resetting
  • ~expansion_radius_position (double, default: 0.1[m])
    • maximum change of the position on the xy-plane when the reset replaces a particle
  • ~expansion_radius_orientation (double, default: 0.2[rad])
    • maximum change of the yaw angle when the reset replaces a particle
  • ~extraction_rate (double, default: 0.1)
    • rate of particles that are checked by the node
  • ~range_threshold (double, default: 0.1[rad])
    • threshold of the range of lasers; if all lasers on this range penetrate occupancy cells, the pose of the particle is judged as wrong
  • ~sensor_reset (bool, default: true)
    • flag for sensor resettings

The followings have never been implemented yet.

  • ~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

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.

GitHub

View Github