AMCL - Adaptive Monte Carlo Localization
Adaptive Monte Carlo Localization (AMCL) is a probabilistic localization algorithm commonly used in robotics. It's an extension of the basic Monte Carlo Localization (MCL) algorithm, designed to handle non-uniformly distributed uncertainties in the environment.
The basic idea behind AMCL is to represent the robot's belief about its pose (position and orientation) as a set of weighted particles. Each particle corresponds to a possible pose of the robot, and the weights associated with these particles represent the likelihood of each pose being the true pose of the robot.
Let's break down the key equations and steps in AMCL:
Initialize Particles:
Start with a set of particles, each with an initial pose estimate. The particles are sampled from the initial belief about the robot's pose.
Prediction Step:
Update the pose of each particle based on the motion model (kinematics or odometry). This step predicts where the robot is expected to be next.
is the probability of the robot being at pose given the control input and the previous pose .
Update Step (Likelihood Calculation):
Use sensor measurements to calculate the likelihood of the observed data given each particle's pose.
is the probability of observing sensor measurements given the robot's pose .
Weight Update:
Update the weight of each particle based on both the prediction and likelihood.
Normalize the weights to ensure they sum to 1.
Resampling:
Resample particles based on their weights. Particles with higher weights are more likely to be sampled, representing poses that are more consistent with sensor measurements.
Adaptive Resampling:
Introduce adaptive techniques to dynamically adjust the number of particles based on the uncertainty in the environment. This helps in efficiently allocating computational resources.
In summary, AMCL adapts the number of particles based on the changing uncertainty, giving more emphasis to areas where the robot is more likely to be. This adaptability allows the algorithm to handle scenarios with varying levels of uncertainty, making it more robust in real-world environments.
NAV2_AMCL
Source code on Github.
AMCL implements the server for taking a static map and localizing the robot within it using an Adaptive Monte-Carlo Localizer.
Parameters
alpha1
Type | Default |
double | 0.2 |
Description
Expected process noise in odometry’s rotation estimate from rotation.
alpha2
Type | Default |
double | 0.2 |
Description
Expected process noise in odometry’s rotation estimate from translation.
alpha3
Type | Default |
double | 0.2 |
Description
Expected process noise in odometry’s translation estimate from translation.
alpha4
Type | Default |
double | 0.2 |
Description
Expected process noise in odometry’s translation estimate from rotation.
alpha5
Type | Default |
double | 0.2 |
Description
For Omni models only: translation noise.
base_frame_id
Type | Default |
string | “base_footprint” |
Description
Robot base frame.
beam_skip_distance
Type | Default |
double | 0.5 |
Description
Ignore beams that most particles disagree with in Likelihood field model. Maximum distance to consider skipping for (m).
beam_skip_error_threshold
Type | Default |
double | 0.9 |
Description
Percentage of beams after not matching map to force full update due to bad convergance.
beam_skip_threshold
Type | Default |
double | 0.3 |
Description
Percentage of beams required to skip.
do_beamskip
Type | Default |
bool | False |
Description
Whether to do beam skipping in Likelihood field model.
global_frame_id
Type | Default |
string | “map” |
Description
The name of the coordinate frame published by the localization system.
lambda_short
Type | Default |
double | 0.1 |
Description
Exponential decay parameter for z_short part of model.
laser_likelihood_max_dist
Type | Default |
double | 2.0 |
Description
Maximum distance to do obstacle inflation on map, for use in likelihood_field model.
laser_max_range
Type | Default |
double | 100.0 |
Description
Maximum scan range to be considered, -1.0 will cause the laser’s reported maximum range to be used.
laser_min_range
Type | Default |
double | -1.0 |
Description
Minimum scan range to be considered, -1.0 will cause the laser’s reported minimum range to be used.
laser_model_type
Type | Default |
string | “likelihood_field” |
Description
Which model to use, either beam, likelihood_field, or likelihood_field_prob. Same as likelihood_field but incorporates the beamskip feature, if enabled.
set_initial_pose
Type | Default |
bool | False |
Description
Causes AMCL to set initial pose from the initial_pose* parameters instead of waiting for the initial_pose message.
initial_pose
Type | Default |
Pose2D | {x: 0.0, y: 0.0, z: 0.0, yaw: 0.0} |
Description
X, Y, Z, and yaw coordinates of initial pose (meters and radians) of robot base frame in global frame.
max_beams
Type | Default |
int | 60 |
Description
How many evenly-spaced beams in each scan to be used when updating the filter.
max_particles
Type | Default |
int | 2000 |
Description
Maximum allowed number of particles.
min_particles
Type | Default |
int | 500 |
Description
Minimum allowed number of particles.
odom_frame_id
Type | Default |
string | “odom” |
Description
Which frame to use for odometry.
pf_err
Type | Default |
double | 0.05 |
Description
Particle Filter population error.
pf_z
Type | Default |
double | 0.99 |
Description
Particle filter population density.
recovery_alpha_fast
Type | Default |
double | 0.0 |
Description
Exponential decay rate for the fast average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.1.
recovery_alpha_slow
Type | Default |
double | 0.0 |
Description
Exponential decay rate for the slow average weight filter, used in deciding when to recover by adding random poses. A good value might be 0.001.
resample_interval
Type | Default |
int | 1 |
Description
Number of filter updates required before resampling.
robot_model_type
Type | Default |
string | “nav2_amcl::DifferentialMotionModel” |
Description
The fully-qualified type of the plugin class. Options are “nav2_amcl::DifferentialMotionModel” and “nav2_amcl::OmniMotionModel”. Users can also provide their own custom motion model plugin type.
Note for users of galactic and earlier
The models are selectable by string key (valid options: “differential”, “omnidirectional”) rather than plugins.
save_pose_rate
Type | Default |
double | 0.5 |
Description
Maximum rate (Hz) at which to store the last estimated pose and covariance to the parameter server, in the variables ~initial_pose_* and ~initial_cov_*. This saved pose will be used on subsequent runs to initialize the filter (-1.0 to disable).
sigma_hit
Type | Default |
double | 0.2 |
Description
Standard deviation for Gaussian model used in z_hit part of the model.
tf_broadcast
Type | Default |
bool | True |
Description
Set this to false to prevent amcl from publishing the transform between the global frame and the odometry frame.
transform_tolerance
Type | Default |
double | 1.0 |
Description
Time with which to post-date the transform that is published, to indicate that this transform is valid into the future.
update_min_a
Type | Default |
double | 0.2 |
Description
Rotational movement required before performing a filter update.
update_min_d
Type | Default |
double | 0.25 |
Description
Translational movement required before performing a filter update.
z_hit
Type | Default |
double | 0.5 |
Description
Mixture weight for z_hit part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand..
z_max
Type | Default |
double | 0.05 |
Description
Mixture weight for z_max part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand.
z_rand
Type | Default |
double | 0.5 |
Description
Mixture weight for z_rand part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand..
z_short
Type | Default |
double | 0.005 |
Description
Mixture weight for z_short part of model, sum of all used z weight must be 1. Beam uses all 4, likelihood model uses z_hit and z_rand.
always_reset_initial_pose
Type | Default |
bool | False |
Description
Requires that AMCL is provided an initial pose either via topic or initial_pose* parameter (with parameter set_initial_pose: true) when reset. Otherwise, by default AMCL will use the last known pose to initialize.
scan_topic
Type | Default |
string | scan |
Description
Laser scan topic to subscribe to.
map_topic
Type | Default |
string | map |
Description
Map topic to subscribe to.
first_map_only
Type | Default |
bool | False |
Description
Allows AMCL to accept maps more than once on the map_topic. This is especially useful when you’re using the LoadMap service in map_server. Prior to Humble, this is first_map_only_
.
Example
Last updated