This is machine translation

Translated by Microsoft
Mouseover text to see original. Click the button below to return to the English version of the page.

Note: This page has been translated by MathWorks. Click here to see
To view all translated materials including this page, select Country from the country navigator on the bottom of this page.

robotics.MonteCarloLocalization

Localize robot using range sensor data and map

Description

The robotics.MonteCarloLocalization System object™ creates a Monte Carlo localization (MCL) object. The MCL algorithm is used to estimate the position and orientation of a robot in its environment using a known map of the environment, lidar scan data, and odometry sensor data.

To localize the robot, the MCL algorithm uses a particle filter to estimate the robot’s position. The particles represent the distribution of likely states for the robot, where each particle represents a possible robot state. The particles converge around a single location as the robot moves in the environment and senses different parts of the environment using a range sensor. An odometry sensor measures the robot’s motion.

A robotics.MonteCarloLocalization object takes the pose and lidar scan data as inputs. The input lidar scan sensor data is given in its own coordinate frame, and the algorithm transforms the data according to the SensorModel.SensorPose property that you must specify. The input pose is computed by integrating the odometry sensor data over time. If the change in pose is greater than any of the specified update thresholds, UpdateThresholds, then the particles are updated and the algorithm computes a new state estimate from the particle filter. The particles are updated using this process:

  1. The particles are propagated based on the change in the pose and the specified motion model, MotionModel.

  2. The particles are assigned weights based on the likelihood of receiving the range sensor reading for each particle. These likelihood weights are based on the sensor model you specify in SensorModel.

  3. Based on the ResamplingInterval property, the particles are resampled from the posterior distribution, and the particles of low weight are eliminated. For example, a resampling interval of 2 means that the particles are resampled after every other update.

The outputs of the object are the estimated pose and covariance, and the value of isUpdated. This estimated state is the mean and covariance of the highest weighted cluster of particles. The output pose is given in the map’s coordinate frame that is specified in the SensorModel.Map property. If the change in pose is greater than any of the update thresholds, then the state estimate has been updated and isUpdated is true. Otherwise, isUpdated is false and the estimate remains the same. For continuous tracking the best estimate of a robot's state, repeat this process of propagating particles, evaluating their likelihood, and resampling.

To estimate robot pose and covariance using lidar scan data:

  1. Create the robotics.MonteCarloLocalization object and set its properties.

  2. Call the object with arguments, as if it were a function.

To learn more about how System objects work, see What Are System Objects? (MATLAB).

Creation

Syntax

mcl = robotics.MonteCarloLocalization
mcl = robotics.MonteCarloLocalization(Name,Value)

Description

example

mcl = robotics.MonteCarloLocalization returns an MCL object that estimates the pose of a robot using a map, a range sensor, and odometry data. By default, an empty map is assigned, so a valid map assignment is required before using the object.

mcl = robotics.MonteCarloLocalization(Name,Value) creates an MCL object with additional options specified by one or more Name,Value pair arguments.

Name is a property name and Value is the corresponding value. Name must appear inside single quotes (''). You can specify several name-value pair arguments in any order as Name1,Value1,...,NameN,ValueN.

Properties

expand all

Initial pose of the robot used to start localization, specified as a three-element vector, [x y theta], that indicates the position and heading of the robot. Initializing the MCL object with an initial pose estimate enables you to use a smaller value for the maximum number of particles and still converge on a location.

Covariance of the Gaussian distribution for the initial pose, specified as a diagonal matrix. Three-element vector and scalar inputs are converted to a diagonal matrix. This matrix gives an estimate of the uncertainty of the InitialPose.

Flag indicating whether to perform global localization, specified as false or true. The default value, false, initializes particles using the InitialPose and InitialCovariance properties. A true value initializes uniformly distributed particles in the entire map and ignores the InitialPose and InitialCovariance properties. Global localization requires a large number of particles to cover the entire workspace. Use global localization only when the initial estimate of robot location and orientation is not available.

Minimum and maximum number of particles, specified as a two-element vector, [min max].

Likelihood field sensor model, specified as a LikelihoodFieldSensorModel object. The default value uses the default robotics.LikelihoodFieldSensorModel object. After using the object to get output, call release on the object to make changes to SensorModel. For example:

mcl = robotics.MonteCarloLocalization(_); 
[isUpdated,pose,covariance] = mcl(_); 
release(mcl) 
mcl.SensorModel.PropName = value; 

Odometry motion model for differential drive, specified as an OdometryMotionModel object. The default value uses the default robotics.OdometryMotionModel object. After using the object to get output, call release on the object to make changes to MotionModel. For example:

mcl = robotics.MonteCarloLocalization(_); 
[isUpdated,pose,covariance] = mcl(_); 
release(mcl) 
mcl.MotionModel.PropName = value; 

Minimum change in states required to trigger update, specified as a three-element vector. The localization updates the particles if the minimum change in any of the [x y theta] states is met. The pose estimate updates only if the particle filter is updated.

Number of filter updates between resampling of particles, specified as a positive integer.

Use a lidarScan object as scan input, specified as either false or true.

Usage

For versions earlier than R2016b, use the step function to run the System object algorithm. The arguments to step are the object you created, followed by the arguments shown in this section.

For example, y = step(obj,x) and y = obj(x) perform equivalent operations.

Syntax

[isUpdated,pose,covariance] = mcl(odomPose,scan)
[isUpdated,pose,covariance] = mcl(odomPose,ranges,angles)

Description

example

[isUpdated,pose,covariance] = mcl(odomPose,scan) estimates the pose and covariance of a robot using the MCL algorithm. The estimates are based on the pose calculated from the specified robot odometry, odomPose, and the specified lidar scan sensor data, scan. mcl is the robotics.MonteCarloLocalization object. isUpdated indicates whether the estimate is updated based on the UpdateThreshold property.

To enable this syntax, you must set the UseLidarScan property to true. For example:

mcl = robotics.MonteCarloLocalization('UseLidarScan',true);
...
[isUpdated,pose,covariance] = mcl(odomPose,scan);

example

[isUpdated,pose,covariance] = mcl(odomPose,ranges,angles) specifies the lidar scan data as ranges and angles.

Input Arguments

expand all

Pose based on odometry, specified as a three-element vector, [x y theta]. This pose is calculated by integrating the odometry over time.

Lidar scan readings, specified as a lidarScan object.

Dependencies

To use this argument, you must set the UseLidarScan property to true.

mcl.UseLidarScan = true;

Range values from scan data, specified as a vector with elements measured in meters. These range values are distances from a laser scan sensor at the specified angles. The ranges vector must have the same number of elements as the corresponding angles vector.

Angle values from scan data, specified as a vector with elements measured in radians. These angle values are the angles at which the specified ranges were measured. The angles vector must be the same length as the corresponding ranges vector.

Output Arguments

expand all

Flag for pose update, returned as a logical. If the change in pose is more than any of the update thresholds, then the output is true. Otherwise, it is false. A true output means that updated pose and covariance are returned. A false output means that pose and covariance are not updated and are the same as at the last update.

Current pose estimate, returned as a three-element vector, [x y theta]. The pose is computed as the mean of the highest-weighted cluster of particles.

Covariance estimate for current pose, returned as a matrix. This matrix gives an estimate of the uncertainty of the current pose. The covariance is computed as the covariance of the highest-weighted cluster of particles.

Object Functions

To use an object function, specify the System object as the first input argument. For example, to release system resources of a System object named obj, use this syntax:

release(obj)

expand all

getParticlesGet particles from localization algorithm
stepRun System object algorithm
releaseRelease resources and allow changes to System object property values and input characteristics
resetReset internal states of System object

Examples

expand all

Create a MonteCarloLocalization object, assign a sensor model, and calculate a pose estimate using the step method.

Note: Starting in R2016b, instead of using the step method to perform the operation defined by the System object, you can call the object with arguments, as if it were a function. For example, y = step(obj,x) and y = obj(x) perform equivalent operations.

Create an MCL object.

mcl = robotics.MonteCarloLocalization;

Assign a sensor model with an occupancy grid map to the object.

sm = robotics.LikelihoodFieldSensorModel;
p = zeros(200,200);
sm.Map = robotics.OccupancyGrid(p,20);
mcl.SensorModel = sm;

Create sample laser scan data input.

ranges = 10*ones(1,300);
ranges(1,130:170) = 1.0;
angles = linspace(-pi/2,pi/2,300);
odometryPose = [0 0 0];

Estimate robot pose and covariance.

[isUpdated,estimatedPose,covariance] = mcl(odometryPose,ranges,angles)
isUpdated = logical
   1

estimatedPose = 1×3

    0.0350   -0.0126    0.0280

covariance = 3×3

    0.9946   -0.0012         0
   -0.0012    0.9677         0
         0         0    0.9548

References

[1] Thrun, Sebatian, Wolfram Burgard, and Dieter Fox. Probabilistic Robotics. MIT Press, 2005.

[2] Dellaert, F., D. Fox, W. Burgard, and S. Thrun. "Monte Carlo Localization for Mobile Robots." Proceedings 1999 IEEE International Conference on Robotics and Automation.

Introduced in R2016a