Simultaneous localization and mapping for a mobile robot

• US 9,037,396 B2
• Filed: 05/23/2013
• Issued: 05/19/2015
• Est. Priority Date: 05/23/2013
• Status: Active Grant

• Current Assignee: iRobot Corporation
• Original Assignee: iRobot Corporation
• Inventors: Pack, Robert Todd, Lenser, Scott R., Kearns, Justin H., Taka, Orjeta
• Primary Examiner(s): Tarcza, Thomas
• Assistant Examiner(s): ALHARBI, ADAM MOHAMED
• Application Number:
  US13/900,767
• Publication Number:
  US 20140350839A1
• Time in Patent Office: 726 Days
• Field of Search: None
• US Class Current: 701/409
• CPC Class Codes: G01C 21/20 Instruments for performing navigational calculations G01C21/24, G01C21/26 take precedence
• CPC Class Codes: G01C 21/30 Map- or contour-matching
• CPC Class Codes: G05D 1/0214 in accordance with safety or protection criteria, e.g. avoiding hazardous areas monitoring the location of vehicles within a certain area, e.g. forbidden or allowed areas, in traffic control systems for road vehicles G08G1/13
• 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/0251 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision stereoscopic image analysis H04N13/00; depth recovery from images G06T7/593
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06T 2207/10021 Stereoscopic video; Stereoscopic image sequence
• CPC Class Codes: G06T 2207/10024 Color image
• CPC Class Codes: G06T 2207/10028 Range image; Depth image; 3D point clouds
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/277 involving stochastic approaches, e.g. using Kalman filters
• CPC Class Codes: G06T 7/73 using feature-based methods
• CPC Class Codes: Y10S 901/01 Mobile robot
• CPC Class Codes: Y10S 901/47 Optical

Abstract

A method of localizing a mobile robot includes receiving sensor data of a scene about the robot and executing a particle filter having a set of particles. Each particle has associated maps representing a robot location hypothesis. The method further includes updating the maps associated with each particle based on the received sensor data, assessing a weight for each particle based on the received sensor data, selecting a particle based on its weight, and determining a location of the robot based on the selected particle.

Claims

1. A method of localizing a mobile robot, the method comprising:

receiving sensor data of a scene about the robot, the sensor data comprising a three-dimensional point cloud;

executing a particle filter having a set of particles, each particle having an associated variance occupancy grid map and a robot location hypothesis;

accumulating cloud points in cells of the variance occupancy grid map based on first and second coordinates of the cloud points, each cell accumulating a height variance based on a third coordinate of the accumulated cloud points;

updating the variance occupancy grid map associated with each particle based on the received sensor data;

assessing a weight for each particle based on the received sensor data;

selecting at least one particle based on its weight; and

determining a location of the robot based on the at least one selected particle.

2. The method of claim 1, wherein receiving senor data comprises receiving two-dimensional image data and three-dimensional image data of a scene about the robot.

3. The method of claim 1, wherein the particle filter comprises a Rao-Blackwellized particle filter.

4. The method of claim 1, further comprising:

associating a ground plane occupancy grid map with each particle;

identifying ground plane cloud points;

fitting a ground plane to the ground plane cloud points; and

updating the ground plane occupancy grid map.

5. The method of claim 1, further comprising:

receiving an image of the scene about the robot;

associating a feature map with each particle;

instantiating an image pyramid comprising a set of scaled images, each scaled image having a scale relative to the received image;

identifying at least one feature point in the scaled images; and

updating the feature map.

6. The method of claim 5, further comprising calculating a Harris Corner Score to identify feature points associated with a corner feature of the scene.

7. The method of claim 6, further comprising selecting feature points as candidate feature points that have at least one of a local maximum Harris Corner Score or a Harris Corner Score substantially equal to the local maximum Harris Corner Score in a threshold area.

8. The method of claim 7, further comprising selecting feature points as candidate feature points that have at least one of a local maximum Harris Corner Score or a Harris Corner Score within about 20% of the local maximum Harris Corner Score within a 10 pixel radius of the feature point having the local maximum Harris Corner Score.

9. The method of claim 5, further comprising selecting a feature point of a scaled image as a key point for tracking and producing a descriptor of that key point.

10. The method of claim 9, further comprising identifying the key point in a subsequent image using the descriptor.

11. The method of claim 9, wherein the descriptor comprises feature points within a threshold distance of the key point on the corresponding scaled image of the key point.

12. The method of claim 11, further comprising:

sampling feature points of the descriptor;

recording a brightness level for each feature point; and

normalizing the brightness levels to have a mean of zero and a variance of one.

13. The method of claim 12, further comprising blurring the scaled image before sampling the feature points.

14. The method of claim 12, further comprising rotating the feature points by a threshold angle before sampling the feature points.

15. The method of claim 12, further comprising sampling feature points within a threshold area of the scaled image about the key point.

16. The method of claim 12, further comprising producing a descriptor for each feature point of a set of feature points.

17. The method of claim 16, further comprising comparing feature descriptors of first and second images to identify a common key point.

18. The method of claim 17, further comprising searching respective image pyramids of the first and second images to find the common feature point.

19. The method of claim 18, further comprising searching within a threshold area of the scaled images of the images pyramids for the common feature point.

20. The method of claim 18, further comprising determining the threshold area based on at least one of a known previous feature point location or a robot drive trajectory.