Simultaneous Localization And Mapping For A Mobile Robot

• US 20140129027A1
• Filed: 03/08/2013
• Published: 05/08/2014
• Est. Priority Date: 11/02/2012
• Status: Active Grant

• Current Assignee: iRobot Corporation
• Original Assignee: iRobot Corporation
• Inventors: Schnittman, Mark
• Granted Patent: US 9,020,637 B2
• Time in Patent Office: Days
• US Class Current: 700/253
• CPC Class Codes: G05D 1/0219 ensuring the processing of the whole working surface
• CPC Class Codes: G05D 1/0227 using mechanical sensing means, e.g. for sensing treated area
• CPC Class Codes: G05D 1/0242 using non-visible light signals, e.g. IR or UV signals
• 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 2201/0203 Cleaning or polishing vehicle
• CPC Class Codes: Y10S 901/01 Mobile robot

Abstract

A method of simultaneous localization and mapping includes initializing a robot pose and a particle model of a particle filter. The particle model includes particles, each having an associated map, robot pose, and weight. The method includes receiving sparse sensor data from a sensor system of the robot, synchronizing the received sensor data with a change in robot pose, accumulating the synchronized sensor data over time, and determining a robot localization quality. When the accumulated sensor data exceeds a threshold accumulation and the robot localization quality is greater than a threshold localization quality, the method includes updating particles with accumulated synchronized sensor data. The method includes determining a weight for each updated particle of the particle model and setting a robot pose belief to the robot pose of the particle having the highest weight when a mean weight of the particles is greater than a threshold particle weight.

Claims

1. A method of simultaneous localization and mapping executed on a controller of an autonomous mobile robot, the method comprising:

initializing a robot pose;

initializing a particle model of a particle filter executing on the controller, the particle model having particles, each particle having an associated map, robot pose, and weight;

receiving sparse sensor data from a sensor system of the robot;

synchronizing the received sensor data with a change in robot pose;

accumulating the synchronized sensor data over time in non-transitory memory;

determining a localization quality of the robot;

when the accumulated sensor data exceeds a threshold accumulation and the robot localization quality is greater than a threshold localization quality, updating particles of the particle model with accumulated synchronized sensor data;

determining a weight for each updated particle of the particle model; and

setting a robot pose belief to the robot pose of the particle having the highest weight when a mean weight of the particles is greater than a threshold particle weight.

2. The method of claim 1, further comprising sampling the particle model for a particle population.

3. The method of claim 2, further comprising resampling the particle model for the particle population when the robot localization quality is greater than the threshold localization quality.

4. The method of claim 3, further comprising replacing a portion of the particle population with randomly selected particles of the particle model when resampling fails to occur within a threshold sampling time period.

5. The method of claim 3, further comprising replacing a portion of the particle population with particles selected from the particle model based on the accumulated synchronized sensor data when resampling fails to occur within a threshold sampling time period.

6. The method of claim 1, further comprising determining the change in robot pose using odometry received from a drive system of the robot and inertial measurements from an inertial measurement unit of the robot.

7. The method of claim 1, wherein determining the robot localization quality comprises:

computing an instantaneous localization;

receiving the instantaneous localization through a fast low-pass filter;

receiving the instantaneous localization through a slow low-pass filter; and

receiving the instantaneous localization through a leaky integrator.

8. The method of claim 1, wherein the sensor data comprises range data and the synchronized sensor data comprises range measurement and bearing pairs.

9. The method of claim 8, wherein updating particles comprises, for each particle:

updating its particle pose by applying a robot motion model to the accumulated synchronized sensor data, the robot motion model modeling movement of the robot based on odometry and inertial measurements;

updating its particle map by;

executing a ray trace of the accumulated synchronized sensor data; and

applying a range sensor model to the accumulated synchronized sensor data; and

updating its particle weight based on a number of measured ranges of the accumulated synchronized sensor data matching predicted ranges.

10. The method of claim 9, wherein executing the ray trace comprises, for each range measurement and bearing pair:

receiving the bearing and an existing particle map; and

computing a predicted range.

11. The method of claim 10, wherein applying the range sensor model comprises:

receiving a measured range;

receiving the predicted range; and

computing a probability of occupancy of an object in each cell of the particle map along the ray trace.

12. The method of claim 11, wherein applying the range sensor model further comprises only updating particle map cells corresponding to the synchronized sensor data.

13. The method of claim 11, wherein applying the range sensor model further comprises:

computing a range error as an absolute difference between the measured range and the predicted range; and

computing the occupancy probability within each cell of the particle map along the ray trace based on the range error.

14. The method of claim 13, wherein applying the range sensor model further comprises, for each particle map cell along the ray trace:

computing a first occupancy probability assuming detection of an expected object;

computing a second occupancy probability assuming detection of an expectedly close object;

computing a third occupancy probability assuming failure to detect of an expected close object within a threshold distance of the robot; and

computing a weighted average of the first, second, and third occupancy probabilities.

15. An autonomous mobile robot comprising:

a drive system configured to maneuver the robot over a floor surface;

a sensor system comprising at least one infrared range finding sensor producing range data of a scene about the robot;

a controller in communication with the drive system and the sensor system, the controller executing a simultaneous localization and mapping routine comprising;

initializing a robot pose;

initializing a particle model of a particle filter executing on the controller, the particle model having particles, each particle having an associated map, robot pose, and weight;

receiving sparse sensor data from a sensor system;

synchronizing the received sensor data with a change in robot pose;

accumulating the synchronized sensor data over time in non-transitory memory;

determining a localization quality of the robot;

when the accumulated sensor data exceeds a threshold accumulation and the robot localization quality is greater than a threshold localization quality, updating particles of the particle model with accumulated synchronized sensor data;

determining a weight for each updated particle of the particle model; and

setting a robot pose belief to the robot pose of the particle having the highest weight when a mean weight of the particles is greater than a threshold particle weight.

16. The robot of claim 15, wherein the simultaneous localization and mapping routine further comprises sampling the particle model for a particle population.

17. The robot of claim 16, wherein the simultaneous localization and mapping routine further comprises resampling the particle model for the particle population when the robot localization quality is greater than the threshold localization quality.

18. The robot of claim 17, wherein the simultaneous localization and mapping routine further comprises replacing a portion of the particle population with randomly selected particles of the particle model when resampling fails to occur within a threshold sampling time period.

19. The robot of claim 17, wherein the simultaneous localization and mapping routine further comprises replacing a portion of the particle population with particles selected from the particle model based on the accumulated synchronized sensor data, when resampling fails to occur within a threshold sampling time period.

20. The robot of claim 15, further comprising an inertial measurement unit, the simultaneous localization and mapping routine further comprises determining the change in robot pose using odometry received from the drive system and inertial measurements from the inertial measurement unit.

21. The robot of claim 15, wherein determining the robot localization quality comprises:

computing an instantaneous localization;

receiving the instantaneous localization through a fast low-pass filter;

receiving the instantaneous localization through a slow low-pass filter; and

receiving the instantaneous localization through a leaky integrator.

22. The robot of claim 15, wherein the sensor data comprises range data from the at least one range finding sensor and the synchronized sensor data comprises range measurement and bearing pairs.

23. The robot of claim 22, wherein updating particles comprises, for each particle:

updating its particle pose by applying a robot motion model to the accumulated synchronized sensor data, the robot motion model modeling movement of the robot based on odometry and inertial measurements;

updating its particle map by;

executing a ray trace of the accumulated synchronized sensor data; and

applying a range sensor model to the accumulated synchronized sensor data; and

updating its particle weight based on a number of measured ranges of the accumulated synchronized sensor data matching predicted ranges.

24. The robot of claim 23, wherein executing the ray trace comprises, for each range measurement and bearing pair:

receiving the bearing and an existing particle map; and

computing a predicted range.

25. The robot of claim 24, wherein applying the range sensor model comprises:

receiving a measured range;

receiving the predicted range; and

computing a probability of occupancy of an object in each cell of the particle map along the ray trace.

26. The robot of claim 25, wherein applying the range sensor model further comprises only updating particle map cells corresponding to the synchronized sensor data.

27. The robot of claim 25, wherein applying the range sensor model further comprises:

computing a range error as an absolute difference between the measured range and the predicted range; and

computing the occupancy probability within each cell of the particle map along the ray trace based on the range error.

28. The robot of claim 27, wherein applying the range sensor model further comprises, for each particle map cell along the ray trace:

computing a first occupancy probability assuming detection of an expected object;

computing a second occupancy probability assuming detection of an expectedly close object;

computing a third occupancy probability assuming failure to detect of an expected close object within a threshold distance of the robot; and

computing a weighted average of the first, second, and third occupancy probabilities.

29. The robot of claim 15, wherein the sensor system comprises:

a first infrared range finding sensor arranged to aim along a forward drive direction of the robot;

a second infrared range finding sensor arranged to aim about 90 degrees to the right of the forward drive direction of the robot;

a third infrared range finding sensor arranged to aim about 90 degrees to the left of the forward drive direction of the robot; and

a fourth infrared range finding sensor arranged to aim rearward, opposite of the forward drive direction of the robot;

wherein the sensor data received by the simultaneous localization and mapping routine comprises range data from the infrared range finding sensors.

30. The robot of claim 29, wherein the sensor system further comprises at least one bump sensor, the sensor data received by the simultaneous localization and mapping routine comprising bump data from the at least one bump sensor.

31. A computer program product encoded on a non-transitory computer readable storage medium comprising instructions that when executed by a data processing apparatus cause the data processing apparatus to perform operations of a method comprising:

initializing a robot pose of a mobile robot;

initializing a particle model of a particle filter executing on the data processing apparatus, the particle model having particles, each particle having an associated map, robot pose, and weight;

receiving sparse sensor data from a sensor system of the robot;

synchronizing the received sensor data with a change in robot pose;

accumulating the synchronized sensor data over time in non-transitory memory;

determining a localization quality of the robot;

when the accumulated sensor data exceeds a threshold accumulation and the robot localization quality is greater than a threshold localization quality, updating particles of the particle model with accumulated synchronized sensor data;

determining a weight for each updated particle of the particle model; and

setting a robot pose belief to the robot pose of the particle having the highest weight when a mean weight of the particles is greater than a threshold particle weight.

32. The computer program product of claim 31, wherein the method further comprises sampling the particle model for a particle population.

33. The computer program product of claim 32, wherein the method further comprises resampling the particle model for the particle population when the robot localization quality is greater than the threshold localization quality.

34. The computer program product of claim 33, wherein the method further comprises replacing a portion of the particle population with randomly selected particles of the particle model when resampling fails to occur within a threshold sampling time period.

35. The computer program product of claim 33, wherein the method further comprises replacing a portion of the particle population with particles selected from the particle model based on the accumulated synchronized sensor data when resampling fails to occur within a threshold sampling time period.

36. The computer program product of claim 31, wherein the method further comprises determining the change in robot pose using odometry received from a drive system of the robot and inertial measurements from an inertial measurement unit of the robot.

37. The computer program product of claim 31, wherein determining the robot localization quality comprises:

computing an instantaneous localization;

receiving the instantaneous localization through a fast low-pass filter,

receiving the instantaneous localization through a slow low-pass filter; and

receiving the instantaneous localization through a leaky integrator.

38. The computer program product of claim 31, wherein the sensor data comprises range data and the synchronized sensor data comprises range measurement and bearing pairs.

39. The computer program product of claim 38, wherein updating particles comprises, for each particle:

updating its particle pose by applying a robot motion model to the accumulated synchronized sensor data, the robot motion model modeling movement of the robot based on odometry and inertial measurements;

updating its particle map by;

executing a ray trace of the accumulated synchronized sensor data; and

applying a range sensor model to the accumulated synchronized sensor data; and

updating its particle weight based on a number of measured ranges of the accumulated synchronized sensor data matching predicted ranges.

40. The computer program product of claim 39, wherein executing the ray trace comprises, for each range measurement and bearing pair:

receiving the bearing and an existing particle map; and

computing a predicted range.

41. The computer program product of claim 40, wherein applying the range sensor model comprises:

receiving a measured range;

receiving the predicted range; and

computing a probability of occupancy of an object in each cell of the particle map along the ray trace.

42. The computer program product of claim 41, wherein applying the range sensor model further comprises only updating particle map cells corresponding to the synchronized sensor data.

43. The computer program product of claim 41, wherein applying the range sensor model further comprises:

computing a range error as an absolute difference between the measured range and the predicted range; and

computing the occupancy probability within each cell of the particle map along the ray trace based on the range error.

44. The computer program product of claim 43, wherein applying the range sensor model further comprises, for each particle map cell along the ray trace:

computing a first occupancy probability assuming detection of an expected object;

computing a second occupancy probability assuming detection of an expectedly close object;

computing a third occupancy probability assuming failure to detect of an expected close object within a threshold distance of the robot; and

computing a weighted average of the first, second, and third occupancy probabilities.

45. A method executed on a controller of an autonomous mobile robot, the method comprising:

determining a localization quality of the robot;

when the localization quality is below a threshold quality;

accessing a good particle from a particle model of a particle filter executing on the controller, the particle model having particles, each particle having an associated map, robot pose, and weight, wherein the good particle has weight greater than a threshold weight;

generating new particles based on the last known good particle; and

re-populating at least a portion of the particle model with the new particles.

46. The method of claim 45, wherein accessing a good particle comprises accessing a last known good particle.

47. The method of claim 45, further comprising ceasing map building when the localization quality is below the threshold quality.

48. The method of claim 45, wherein determining the robot localization quality comprises:

computing an instantaneous localization;

receiving the instantaneous localization through a fast low-pass filter;

receiving the instantaneous localization through a slow low-pass filter; and

receiving the instantaneous localization through a leaky integrator.

49. The method of claim 45, further comprising:

initializing a robot pose;

initializing the particle model;

receiving sparse sensor data from a sensor system of the robot;

synchronizing the received sensor data with a change in robot pose; and

accumulating the synchronized sensor data over time in non-transitory memory.

50. The method of claim 49, further comprising:

when the accumulated sensor data exceeds a threshold accumulation and the robot localization quality is greater than a threshold localization quality, updating particles of the particle model with accumulated synchronized sensor data;

determining a weight for each updated particle of the particle model; and

setting a robot pose belief to the robot pose of the particle having the highest weight when a mean weight of the particles is greater than a threshold particle weight.

51. The method of claim 50, further comprising sampling the particle model for a particle population.

52. The method of claim 51, further comprising resampling the particle model for the particle population when the robot localization quality is greater than the threshold localization quality.

53. The method of claim 52, further comprising replacing a portion of the particle population with randomly selected particles of the particle model when resampling fails to occur within a threshold sampling time period.

54. The method of claim 52, further comprising replacing a portion of the particle population with particles selected from the particle model based on the accumulated synchronized sensor data when resampling fails to occur within a threshold sampling time period.

55. The method of claim 50, wherein the sensor data comprises range data and the synchronized sensor data comprises range measurement and bearing pairs, and wherein updating particles comprises, for each particle:

updating its particle pose by applying a robot motion model to the accumulated synchronized sensor data, the robot motion model modeling movement of the robot based on odometry and inertial measurements;

updating its particle map by;

executing a ray trace of the accumulated synchronized sensor data; and

applying a range sensor model to the accumulated synchronized sensor data; and

updating its particle weight based on a number of measured ranges of the accumulated synchronized sensor data matching predicted ranges.