Simultaneous localization and mapping for a mobile robot

• US 9,329,598 B2
• Filed: 04/13/2015
• Issued: 05/03/2016
• 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): Alharbi, Adam
• Application Number:
  US14/685,029
• Publication Number:
  US 20150212521A1
• Time in Patent Office: 386 Days
• Field of Search: None
• US Class Current: 1/1
• 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 comprising:

maneuvering a robot about a scene;

emitting light onto the scene;

capturing images of the scene using a depth-perceptive imaging sensor, the images comprising an active illumination image and an ambient illumination image, each image comprising three-dimensional depth data and brightness data;

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

updating the occupancy grid map associated with each particle based on the images;

for each image;

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

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

updating the corresponding feature map of each particle with the identified at least one feature point;

determining a location of an object in the scene based on the images and at least one particle of the particle filter;

assigning a confidence level for the location of the object based on the three-dimensional depth data and the brightness data of the images; and

maneuvering the robot in the scene based on the location of the object and the corresponding confidence level.

2. The method of claim 1, further comprising:

receiving a proximity indication of the robot being near the object from a proximity sensor of the robot; and

in response to the proximity indication, increasing an image capture rate of the depth-perceptive imaging sensor.

3. The method of claim 1, further comprising constructing an object occupancy map of the scene based on the images.

4. The method of claim 1, further comprising maneuvering the robot to avoid the object.

5. The method of claim 1, further comprising:

assessing a weight for each particle based on the images;

selecting at least one particle based on its weight; and

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

6. The method of claim 1, wherein the three-dimensional depth data of each image comprises a three-dimensional point cloud.

7. The method of claim 6, further comprising accumulating cloud points of the three-dimensional point cloud of each image in cells of the 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 cloud points.

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

9. The method of claim 8, 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.

10. The method of claim 9, 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.

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

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

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

14. The method of claim 13, 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.

15. The method of claim 14, further comprising, before sampling the feature points, at least one of:

blurring the scaled image before sampling the feature points;

or

rotating the feature points by a threshold angle.

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

17. The method of claim 14, further comprising:

producing a descriptor for each feature point of a set of feature points;

comparing feature descriptors of first and second images to identify a common feature point; and

searching respective image pyramids of the first and second images to find the common feature point.

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

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