Simultaneous localization and mapping for a mobile robot
First Claim
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 particle map, robot pose, and weight, the particle map having cells;
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 each particle of the particle model with the accumulated synchronized sensor data by;
computing a range error as an absolute difference between a measured range of the accumulated sensor data and a predicted range; and
computing a probability of occupancy of an object in each cell of the associated particle map along a ray trace of the accumulated synchronized sensor data based on the range error by;
computing a first occupancy probability assuming detection of an expected object;
computing a second occupancy probability assuming detection of an expected close object of the robot;
computing a third occupancy probability assuming a failure to detect an expected close object within a threshold distance of the robot; and
computing a weighted average of the first, second, and third occupancy probabilities;
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.
4 Assignments
0 Petitions
Accused Products
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.
-
Citations
47 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 particle map, robot pose, and weight, the particle map having cells; 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 each particle of the particle model with the accumulated synchronized sensor data by; computing a range error as an absolute difference between a measured range of the accumulated sensor data and a predicted range; and computing a probability of occupancy of an object in each cell of the associated particle map along a ray trace of the accumulated synchronized sensor data based on the range error by; computing a first occupancy probability assuming detection of an expected object; computing a second occupancy probability assuming detection of an expected close object of the robot; computing a third occupancy probability assuming a failure to detect an expected close object within a threshold distance of the robot; and computing a weighted average of the first, second, and third occupancy probabilities; 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. - View Dependent Claims (2, 3, 4, 5, 6, 7, 8, 9, 10, 11)
-
-
12. 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 comprising; 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; and 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.
-
-
13. 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 associatedab ide map, robot pose, and weight, the particle map having cells; 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 each particle of the particle model with the accumulated synchronized sensor data by; computing a range error as an absolute difference between a measured range of the accumulated sensor data and a predicted range; and computing a probability of occupancy of an object in each cell of the associated particle map along a ray trace of the accumulated synchronized sensor data based on the range error by; computing a first occupancy probability assuming detection of an expected object; computing a second occupancy probability assuming detection of an expected close object of the robot; computing a third occupancy probability assuming failure to detect an expected close object within a threshold distance of the robot; and computing a weighted average of the first, second, and third occupancy probabilities; 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. - View Dependent Claims (14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 35, 36, 37)
-
-
26. 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 comprising; 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; and 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.
-
-
27. 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 particle map, robot pose, and weight, the particle map having cells; 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 each particle of the particle model with the accumulated synchronized sensor data by; computing a range error as an absolute difference between a measured range of the accumulated sensor data and a predicted range; and computing a probability of occupancy of an object in each cell of the associated particle map along a ray trace of the accumulated synchronized sensor data based on the range error by; computing a first occupancy probability assuming detection of an expected object; computing a second occupancy probability assuming detection of an expected close object of the robot; computing a third occupancy probability assuming failure to detect an expected close object within a threshold distance of the robot; and computing a weighted average of the first, second, and third occupancy probabilities; 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. - View Dependent Claims (28, 29, 30, 31, 32, 33, 34)
-
-
38. 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 comprising; 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; and 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.
-
-
39. A method executed on a controller of an autonomous mobile robot, the method comprising:
-
initializing a robot pose; initializing a particle model; 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 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 particle map, robot pose, and weight, the particle map having cells, 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; when the accumulated sensor data exceeds a threshold accumulation and the robot localization quality is greater than a threshold localization quality, updating each particle of the particle model with the accumulated synchronized sensor data by; computing a range error as an absolute difference between a measured range of the accumulated sensor data and a predicted range; and computing a probability of occupancy of an object in each cell of the associated particle map along a ray trace of the accumulated synchronized sensor data based on the range error by; computing a first occupancy probability assuming detection of an expected object; computing a second occupancy probability assuming detection of an expected close object of the robot; computing a third occupancy probability assuming failure to detect an expected close object within a threshold distance of the robot; and computing a weighted average of the first, second, and third occupancy probabilities; 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. - View Dependent Claims (40, 41, 42, 43, 44, 45, 46)
-
-
47. A method executed on a controller of an autonomous mobile robot, the method comprising:
-
determining a localization quality of the robot comprising; 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; and 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; and 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.
-
Specification