Robust sensor fusion for mapping and localization in a simultaneous localization and mapping (SLAM) system

• US 7,689,321 B2
• Filed: 02/10/2005
• Issued: 03/30/2010
• Est. Priority Date: 02/13/2004
• Status: Active Grant

• Current Assignee: iRobot Corporation
• Original Assignee: Evolution Robotics Inc. (iRobot Corporation)
• Inventors: Karlsson, Lars Niklas
• Primary Examiner(s): Tran; Khoi
• Assistant Examiner(s): JEN, MINGJEN
• Application Number:
  US11/055,210
• Publication Number:
  US 20050182518A1
• Time in Patent Office: 1,874 Days
• Field of Search: 700/253, 700/245, 701/24, 701/26, 701/250, 701/28, 701/23, 701/207, 701/211, 701/209, 219/540, 382/153, 382/157, 318/587, 318/568.16, 318/103, 318/568.12, 348/169, 348/119
• US Class Current: 700/253
• CPC Class Codes: G05D 1/024 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0242 using non-visible light signals, e.g. IR or UV signals
• CPC Class Codes: G05D 1/0246 using a video camera in combination with image processing means
• CPC Class Codes: G05D 1/0248 in combination with a laser lasers per se H01S
• CPC Class Codes: G05D 1/0255 using acoustic signals, e.g. ultra-sonic singals sonar systems designed for anti-collision purposes G01S15/93
• CPC Class Codes: G05D 1/0272 comprising means for registering the travel distance, e.g. revolutions of wheels measuring distance traversed on the ground by vehicles, e.g. using odometers G01C22/00
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06F 18/29 Graphical models, e.g. Bayesian networks
• CPC Class Codes: G06V 10/245 by locating a pattern; Special marks for positioning
• CPC Class Codes: G06V 10/84 using probabilistic graphical models from image or video features, e.g. Markov models or Bayesian networks
• CPC Class Codes: G06V 20/10 Terrestrial scenes scenes under surveillance with static cameras G06V20/52; scenes perceived from the exterior of a vehicle G06V20/56; scenes perceived from the interior of a vehicle G06V20/59

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.

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.

2. The method as defined in claim 1, wherein the sensor measurements are fused to compensate for errors in sensor measurements.

3. The method as defined in claim 1, further comprising if none of the dead reckoning measurement and the visual measurement is trustworthy, then not updating the estimate for the value of the pose with the dead reckoning measurement or the visual measurement.

4. The method as defined in claim 1, further comprising determining that a dead reckoning measurement is trustworthy when both the dead reckoning measurement itself is trustworthy and the prior pose is trustworthy.

5. The method as defined in claim 1, wherein if the dead reckoning data and the visual data are trustworthy, further updating a global landmark pose corresponding to the visual measurement for the maps of the particles using the dead reckoning measurement and visual data.

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.

7. The method as defined in claim 6, wherein the sensor measurements are fined to compensate for errors in sensor measurements.

8. The method as defined in claim 6, wherein estimating the new pose for the particle further comprises, when the measurement corresponds to a dead reckoning measurement, using the measurement from the dead reckoning sensor and a prior pose for the particle.

9. The method as defined in claim 6, wherein generating an importance factor further comprises:

if measurements from one or more other sensors are also considered trustworthy, then using corresponding measurements from the one or more other sensors to compute a second pose for the particle and to generate the importance factor at least partially based on a comparison between the second pose and the new pose; and

otherwise, setting the importance factor to a uniform value.

10. The method as defined in claim 6, wherein generating an importance factor further comprises:

if measurements from one or more other sensors are also considered trustworthy, then using corresponding measurements from the one or more other sensors to compute a second pose for the particle and to generate the importance factor at least partially based on a comparison between the second pose and the new pose; and

otherwise, setting the importance factor using predefined criteria.

11. The method as defined in claim 6, wherein generating an importance factor further comprises:

if measurements from one or more other sensors are also considered trustworthy, then using corresponding measurements from the one or more other sensors to compute a second pose for the particle and to generate the importance factor at least partially based on a comparison between the second pose and the new pose; and

otherwise, setting the importance factor to a predetermined value.

12. The method as defined in claim 6, further comprising determining that a dead reckoning measurement is trustworthy when both the dead reckoning measurement itself is trustworthy and the prior pose is trustworthy.

13. The method as defined in claim 6, wherein the globally-referenced measurement corresponds to a difference between a global landmark pose that is visually recognized and a pose that is being estimated.

14. The method as defined in claim 6, further comprising:

determining that at least the globally-referenced measurement and a dead reckoning measurement are both trustworthy; and

at least partially in response to the determination of trustworthiness, updating a global landmark pose corresponding to the globally-referenced measurement for a map corresponding to the particle by using the dead reckoning measurement and the globally referenced measurement.

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.

16. The method as defined in claim 15, wherein using the selected trustworthy measurement to update the multi-dimensional value x for the particle further comprises computing the updated value for the multi-dimensional value x with the selected trustworthy measurement independently of other measurements from other sensors.

17. The method as defined in claim 15, wherein using the randomly selected trustworthy measurement to update the multi-dimensional value x km the particle further comprises combining a prior estimate of the multi-dimensional value x with the trustworthy measurement to update the multi-dimensional value x.

18. The method as defined in claim 16, wherein performing for the particle further comprises:

using a trustworthy measurement other than the trustworthy measurement used to calculate the updated multi-dimensional value x to calculate an alternative estimate for the multi-dimensional value x;

perturbing the updated multi-dimensional value x;

comparing the updated multi-dimensional value x to the alternative estimate for the multi-dimensional value x; and

generating an importance factor based on the comparison that is associated with the particle and is used for resampling of particles.

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.

20. The method as defined in claim 19, further comprising randomly selecting the one or more trustworthy measurements that are not used to calculate the new value x.

21. The method as defined in claim 19, further comprising:

using the at least one unused trustworthy measurement to compute a second value associated with the particle; and

resampling the updated particles to generate an updated probability density function p(x), wherein the second value affects a probability with which the particle is resampled.