Robust sensor fusion for mapping and localization in a simultaneous localization and mapping (SLAM) system
First Claim
Patent Images
1. A method in a visual simultaneous localization and mapping (VSLAM) system of fusing sensor measurements for localization of a mobile device, the method comprising:
- using a probability density function p(x) represented by a plurality of particles to estimate a value for a pose of the mobile device, where the pose includes both location and orientation;
observing visual features that are associated with a landmark pose;
receiving an indication of whether dead reckoning data corresponding to a dead reckoning measurement is trustworthy, where the dead reckoning measurement corresponds to an incremental change in pose from a prior pose;
receiving an indication of whether visual data for a visual measurement is trustworthy, where the visual measurement is associated with the landmark pose;
if the dead reckoning data and the visual data are trustworthy, then performing;
apportioning a particle in the plurality of particles to one of at least a first subset and a second subset of the plurality of particles, where the first subset and the second subset are mutually exclusive;
performing for a particle associated with the first subset;
computing a predicted robot pose by combining the dead reckoning measurement and the prior pose for the particle;
perturbing the predicted robot pose to generate a perturbed predicted pose; and
using the perturbed predicted robot pose as the estimate for the pose associated with the particle;
performing for a particle associated with the second subset;
retrieving the associated landmark pose;
using the associated landmark pose and the visual measurement to generate an estimated visual pose;
perturbing the estimated visual pose to generated a perturbed estimated visual pose; and
using the perturbed estimated visual pose as the estimate for the value of the pose of the mobile device for the particle;
and, if one of the dead reckoning measurement and the visual measurement is not trustworthy, then performing;
estimating a first pose by using a selected one of the dead reckoning measurement and the visual measurement, where the selected one is trustworthy;
perturbing the first pose to generate a perturbed pose; and
using the perturbed pose as the estimate for the value of the pose of the mobile device for the particle.
7 Assignments
0 Petitions
Accused Products
Abstract
This invention is generally related to methods and apparatus that permit the measurements from a plurality of sensors to be combined or fused in a robust manner. For example, the sensors can correspond to sensors used by a mobile device, such as a robot, for localization and/or mapping. The measurements can be fused for estimation of a measurement, such as an estimation of a pose of a robot.
-
Citations
21 Claims
-
1. A method in a visual simultaneous localization and mapping (VSLAM) system of fusing sensor measurements for localization of a mobile device, the method comprising:
-
using a probability density function p(x) represented by a plurality of particles to estimate a value for a pose of the mobile device, where the pose includes both location and orientation; observing visual features that are associated with a landmark pose; receiving an indication of whether dead reckoning data corresponding to a dead reckoning measurement is trustworthy, where the dead reckoning measurement corresponds to an incremental change in pose from a prior pose; receiving an indication of whether visual data for a visual measurement is trustworthy, where the visual measurement is associated with the landmark pose; if the dead reckoning data and the visual data are trustworthy, then performing; apportioning a particle in the plurality of particles to one of at least a first subset and a second subset of the plurality of particles, where the first subset and the second subset are mutually exclusive; performing for a particle associated with the first subset; computing a predicted robot pose by combining the dead reckoning measurement and the prior pose for the particle; perturbing the predicted robot pose to generate a perturbed predicted pose; and using the perturbed predicted robot pose as the estimate for the pose associated with the particle; performing for a particle associated with the second subset; retrieving the associated landmark pose; using the associated landmark pose and the visual measurement to generate an estimated visual pose; perturbing the estimated visual pose to generated a perturbed estimated visual pose; and using the perturbed estimated visual pose as the estimate for the value of the pose of the mobile device for the particle; and, if one of the dead reckoning measurement and the visual measurement is not trustworthy, then performing; estimating a first pose by using a selected one of the dead reckoning measurement and the visual measurement, where the selected one is trustworthy; perturbing the first pose to generate a perturbed pose; and using the perturbed pose as the estimate for the value of the pose of the mobile device for the particle. - View Dependent Claims (2, 3, 4, 5)
-
-
6. A method in a simultaneous localization and mapping (SLAM) system of fining sensor measurements for localization of a mobile device, the method comprising:
-
(a) using a probability density function p(x) represented by a plurality of particles to estimate a value for a pose of the mobile device, where the pose includes both location and orientation (b) in the absence of an observation of a globally-referenced measurement, using a dead reckoning measurement and a prior pose to estimate a current pose for a particle, (c) when a globally-referenced measurement is observed, then; (d) allocating the particles according to sensors that are providing trustworthy measurements so that a particle is associated with a sensor; (e) performing (f) and (g), for a particle; (f) estimating a new pose for the particle by using a measurement from an associated sensor for estimation of pose end perturbing the estimation of pose to generate the new pose; (g) generating an importance factor for the particle based at least partially on whether at least one trustworthy measurement other than the measurement used to compute the pose for the particle is available; and (h) repeating at least a portion of (t) and (g) for other particles; and (i) resampling the particles with replacement using the generated importance factors to at least partially influence a probability with which a particle will be resampled. - View Dependent Claims (7, 8, 9, 10, 11, 12, 13, 14)
-
-
15. A method of integrating measurements from a plurality of sensors to estimate a multi dimensional value x, the method comprising:
-
using a probability density function p(x) to estimate the multi-dimensional value x, where the probability density function p(x) is represented by a plurality of particles; determining whether at least two measurements from the plurality of sensors are trustworthy, and if so, then performing for a particle; selecting a trustworthy measurement from the plurality of sensors, using the selected trustworthy measurement to update the multi-dimensional value x for the particle; otherwise, if only one measurement is trustworthy, updating the particles with the trustworthy measurement; and otherwise, if no measurement is trustworthy, then not updating the particles. - View Dependent Claims (16, 17, 18)
-
-
19. A method of integrating measurements from a plurality of sensors to estimate a value x, the method comprising:
-
using a probability density function p(x) to estimate the value x in a mobile device, where the probability density function p(x) is represented by a plurality of particles; determining which of the measurements from the plurality of sensors are trustworthy; and at least partially in response to determining that two or more of the measurements are trustworthy, updating the probability density function p(x) with the two or more trustworthy measurements to integrate the measurements, wherein for a particular particle of the probability density function p(x), a new value x for the particle is calculated without using at least a selected one of the two or more trustworthy measurements. - View Dependent Claims (20, 21)
-
Specification