Systems and methods for using multiple hypotheses in a visual simultaneous localization and mapping system

• US 7,679,532 B2
• Filed: 11/13/2006
• Issued: 03/16/2010
• Est. Priority Date: 12/17/2002
• Status: Active Grant

• Current Assignee: iRobot Corporation
• Original Assignee: Evolution Robotics Inc. (iRobot Corporation)
• Inventors: Bernardo, Enrico Di, Karlsson, L. Nicklas, Pirjanian, Paolo, Goncalves, Luis Filipe Domingues
• Primary Examiner(s): Swarthout; Brent
• Application Number:
  US11/559,359
• Publication Number:
  US 20070090973A1
• Time in Patent Office: 1,219 Days
• Field of Search: 340/995.1, 340/988, 34099514-99527, 318/568.11, 318/568.12, 318/568.16, 700245-253, 701/23, 701/301, 901/1, 901/50
• US Class Current: 340/995.1
• CPC Class Codes: G01C 21/12 executed aboard the object being navigated; Dead reckoning
• CPC Class Codes: G01C 21/206 specially adapted for indoor navigation
• CPC Class Codes: G05D 1/0231 using optical position detecting means position-fixing by using electromagnetic waves other than radio waves, e.g. optical position detecting means G01S5/16
• CPC Class Codes: G05D 1/0234 using optical markers or beacons optical beacons per se G01S1/70
• 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/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: G05D 1/0278 using satellite positioning signals, e.g. GPS
• CPC Class Codes: G06T 2207/30204 Marker
• CPC Class Codes: G06T 7/55 from multiple images
• CPC Class Codes: G06T 7/70 Determining position or orientation of objects or cameras camera calibration G06T7/80
• CPC Class Codes: G06T 7/74 involving reference images or patches
• CPC Class Codes: Y10S 901/01 Mobile robot
• CPC Class Codes: Y10S 901/46 Sensing device
• CPC Class Codes: Y10S 901/47 Optical
• CPC Class Codes: Y10S 901/50 Miscellaneous

Abstract

The invention is related to methods and apparatus that use a visual sensor and dead reckoning sensors to process Simultaneous Localization and Mapping (SLAM). These techniques can be used in robot navigation. Advantageously, such visual techniques can be used to autonomously generate and update a map. Unlike with laser rangefinders, the visual techniques are economically practical in a wide range of applications and can be used in relatively dynamic environments, such as environments in which people move. One embodiment further advantageously uses multiple particles to maintain multiple hypotheses with respect to localization and mapping. Further advantageously, one embodiment maintains the particles in a relatively computationally-efficient manner, thereby permitting the SLAM processes to be performed in software using relatively inexpensive microprocessor-based computer systems.

Claims

1. A method of localizing a mobile device via a multiple-particle autonomous localization and mapping system, the method comprising:

performing autonomous localization and mapping with a plurality of particles, where a particle includes a device pose estimate and a map, where the map includes one or more landmarks;

receiving an indication that a landmark has been recognized and a visually-measured relative pose to the landmark has been estimated, where the landmark has been recognized using visual features from a visual sensor coupled to the mobile device, where the relative pose corresponds to a visually-measured difference in pose between a landmark pose and a pose corresponding to the visual observation; and

updating at least one of the plurality of particles at least partly in response to receiving the indication of the recognized landmark, wherein updating further comprises;

using a prior pose estimate and dead reckoning sensor data to compute a new pose estimate for particles in a selected group based on the estimated change in pose; and

using the landmark pose and the visually-measured relative pose estimate to compute the new pose estimate for particles not in the selected group;

wherein performing autonomous localization and mapping, receiving the indication, and updating at least one of the plurality of particles are performed by computer hardware.

2. The method as defined in claim 1, wherein updating at least one of the plurality of particles is performed each time that a landmark is recognized.

3. The method as defined in claim 1, wherein particles are grouped to be in the selected group or not in the selected group each time that a landmark is recognized.

4. The method as defined in claim 1, wherein particles are independently categorized into the selected group such that a probability of one particle being categorized into the selected group is about the same as the probability of another particle being categorized into the selected group.

5. The method as defined in claim 1, wherein using the prior pose estimate and dead reckoning sensor data to compute the new pose estimate for particles in the selected group further comprises:

using the prior pose estimate, the dead reckoning sensor data, and simulated random noise to compute a perturbed predicted pose;

using the perturbed predicted pose and the visually-measured relative pose to compute a hypothetical landmark pose;

obtaining an uncertainty measure associated with the particle based at least in part on an uncertainty measure associated with the landmark and particle and an uncertainty measure associated with the visual sensor;

computing an importance factor for the particle, where the importance factor is based at least in part on the uncertainty measure associated with the particle and a comparison between the calculated hypothetical landmark pose and a prior landmark pose estimate; and

using the perturbed predicted pose with the importance factor to calculate the new pose.

8. The method as defined in claim 1, wherein using the landmark pose estimate and the visually-measured relative pose further comprises:

using the landmark pose estimate corresponding to the landmark and the visually-measured relative pose to compute a hypothetical device pose;

perturbing the hypothetical device pose with simulated random noise;

obtaining an uncertainty measure associated with the particle based at least in part on an uncertainty measure associated with the landmark and particle and an uncertainty measure associated with the dead reckoning measurements;

computing an importance factor for the particle, where the importance factor is based at least in part on the uncertainty measure associated with the particle and a comparison between the hypothetical device pose and a prior device pose estimate and the dead reckoning sensor data; and

using the perturbed predicted pose with the importance factor to calculate the new pose.

11. The method as defined in claim 1, further comprising resampling selected particles from the plurality of particles.

6. The method as defined in 5, wherein the simulated random noise exhibits an uncertainty measure estimated for the dead reckoning sensor data.

7. The method as defined in 5, wherein the prior landmark pose estimate corresponds to a most recent update of landmark pose.

9. The method as defined in 8, wherein the simulated random noise exhibits an uncertainty measure estimated for a visual sensor.

10. The method as defined in 8, where the uncertainty measure of the dead reckoning measurements corresponds to an odometer covariance matrix C_odom.

12. A computer program embodied in a computer-readable medium for localizing a mobile device via a multiple-particle autonomous localization and mapping system, the computer program comprising:

a module with instructions configured to perform autonomous localization and mapping with a plurality of particles, where a particle includes a device pose estimate and a map, where the map includes one or more landmarks;

a module with instructions configured to receive an indication that a landmark has been recognized and a visually-measured relative pose to the landmark has been estimated, where the landmark has been recognized using visual features from a visual sensor coupled to the mobile device, where the relative pose corresponds to a visually-measured difference in pose between a landmark pose and a pose corresponding to the visual observation; and

a module with instructions configured to update at least one of the plurality of particles at least partly in response to receiving the indication of the recognized landmark, wherein the module with instructions configured to update further comprises;

instructions configured to use a prior pose estimate and dead reckoning sensor data to compute a new pose estimate for particles in a selected group based on the estimated change in pose; and

instructions configured to use the landmark pose and the visually-measured relative pose estimate to compute the new pose estimate for particles not in the selected group.

13. The computer program as defined in claim 12, wherein particles are independently categorized into the selected group such that a probability of one particle being categorized into the selected group is about the same as the probability of another particle being categorized into the selected group.

14. The computer program as defined in claim 12, wherein the module with instructions configured to use the prior pose estimate and dead reckoning sensor data to compute the new pose estimate for particles in the selected group further comprises instructions configured to:

use the prior pose estimate, the dead reckoning sensor data, and simulated random noise to compute a perturbed predicted pose;

use the perturbed predicted pose and the visually-measured relative pose to compute a hypothetical landmark pose;

obtain an uncertainty measure associated with the particle based at least in part on an uncertainty measure associated with the landmark and particle and an uncertainty measure associated with the visual sensor;

compute an importance factor for the particle, where the importance factor is based at least in part on the uncertainty measure associated with the particle and a comparison between the calculated hypothetical landmark pose and a prior landmark pose estimate; and

use the perturbed predicted pose with the importance factor to calculate the new pose.

15. The computer program as defined in claim 12, wherein module with instructions configured to use the landmark pose estimate and the visually-measured relative pose further comprises instructions configured to:

use the landmark pose estimate corresponding to the landmark and the visually-measured relative pose to compute a hypothetical device pose;

perturb the hypothetical device pose with simulated random noise;

obtain an uncertainty measure associated with the particle based at least in part on an uncertainty measure associated with the landmark and particle and an uncertainty measure associated with the dead reckoning measurements;

compute an importance factor for the particle, where the importance factor is based at least in part on the uncertainty measure associated with the particle and a comparison between the hypothetical device pose and a prior device pose estimate and the dead reckoning sensor data; and

use the perturbed predicted pose with the importance factor to calculate the new pose.

16. The computer program as defined in claim 12, further comprising a module with instructions configured to resample selected particles from the plurality of particles.