Sequential selective integration of sensor data

• US 20050234679A1
• Filed: 02/10/2005
• Published: 10/20/2005
• Est. Priority Date: 02/13/2004
• Status: Abandoned Application

• Current Assignee: Evolution Robotics Inc. (iRobot Corporation)
• Original Assignee: Evolution Robotics Inc. (iRobot Corporation)
• Inventors: Karlsson, Lars Niklas
• Application Number:
  US11/055,307
• Publication Number:
  US 20050234679A1
• Time in Patent Office: Days
• US Class Current: 702/181
• 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

Abstract

This invention is generally related to sequential methods and apparatus that permit the measurements from a plurality of sensors to be combined or fused in a robust manner. For example, the sensors can correspond to sensors used by a mobile device, such as a robot, for localization and/or mapping. The measurements can be fused for estimation of a measurement, such as an estimation of a pose of a robot.

Claims

1. A method of sequentially integrating measurements from a plurality of sensors to estimate a multi-dimensional value x, where the plurality of sensors include at least a first sensor and a second sensor, the method comprising:

using a probability density function p(x) to estimate the multi-dimensional value x, where the probability density function p(x) is represented by a plurality of particles;

receiving measurements from the plurality of sensors comprising at least a measurement M₁ from the first sensor and a measurement M₂ from the second sensor;

determining whether the measurement M₁ and the measurement M₂ are trustworthy, where a first condition is true when both the measurement M₁ and the measurement M₂ are trustworthy, and if the first condition is determined to be true, then performing;

allocating particles among a plurality of groups, where a group corresponds to a sensor with a trustworthy measurement so that there is at least a first group corresponding to the measurement M₁ and a second group corresponding to the measurement M₂ in the plurality of groups;

for the particles in the first group, performing;

updating an estimate {tilde over (x)} for a particle for the multi-dimensional value x based at least partially on measurement M₁ and on a prior estimate x_old from a prior estimate of the probability density function p(x_old);

computing an importance factor w for the particle based at least in part on the updated estimate {tilde over (x)} and one or more trustworthy measurements other than measurement M₁, where the one or more trustworthy measurements include at least measurement M₂; and

associating the updated estimate {tilde over (x)} and the importance factor w with the particle;

for the particles in the second group, performing;

updating an estimate {tilde over (x)} for a particle for the multi-dimensional value x based at least partially on measurement M₂ and on a prior estimate x_old from a prior estimate of the probability density function p(x_old);

computing an importance factor w for the particle based at least in part on the updated estimate {tilde over (x)} and one or more trustworthy measurements other than measurement M₂, where the one or more trustworthy measurements include at least measurement M₁; and

associating the updated estimate {tilde over (x)} and the importance factor w with the particle; and

resampling with replacement the updated estimates {tilde over (x)} of the particles from at least the first group and the second group to generate an updated estimate for the multi-dimensional value x, where a probability with which a particle will be sampled depends at least partially on the importance factor w associated with the particle;

and, otherwise, if the first condition is determined not to be true, then not using at least one of the measurement M₁ or the measurement M₂.

2. The method as defined in claim 1, wherein the measurements are integrated to compensate for errors in sensor measurements.

3. The method as defined in claim 1, wherein allocating particles among a plurality of groups further comprises randomly allocating such that prior to allocating, a first particle and a second particle from the plurality of particles have the same probability of being allocated to one of the groups.

4. The method as defined in claim 1, wherein computing the importance factor w for the particles of the second group comprises computing a different mathematical expression than computing the importance factor w for the particles of the first group.

5. The method as defined in claim 1, wherein resampling with replacement further comprises randomly resampling such that prior to resampling, a first particle and a second particle have the same probability of sampling an updated particle.

6. The method as defined in claim 1, where the measurement M₁ corresponds to an incremental measurement, wherein using the measurement M₁ further comprises computing a conditional probability density function p(x|x_old, M₁).

7. The method as defined in claim 1, where the measurement M₁ corresponds to a global measurement, wherein using the measurement M₁ further comprises computing a conditional probability density function p(x|M₁).

8. The method as defined in claim 1, wherein the multi-dimensional value x corresponds to a pose, and wherein the plurality of sensors are coupled to a mobile device and correspond to sensors for measuring pose.

9. The method as defined in claim 1, wherein the multi-dimensional value x corresponds to a pose in a simultaneous localization and mapping system (SLAM).

10. The method as defined in claim 1, wherein the multi-dimensional value x corresponds to a pose in a visual simultaneous localization and mapping system (VSLAM).

11. The method as defined in claim 1, further comprising determining whether the prior estimate x_old is trustworthy, and wherein the first condition is true when the measurement M₁, the measurement M₂, and the prior estimate x_old are trustworthy.

12. The method as defined in claim 1, further comprising:

determining whether the prior estimate x_old is trustworthy, and wherein the first condition is true when the measurement M₁, the measurement M₂, and the prior estimate x_old are trustworthy; and

where a second condition is true when the measurement M₁ is trustworthy, the measurement M₂ is trustworthy, and the prior estimate x_old is untrustworthy, wherein the measurement M₂ corresponds to a dead reckoning measurement, and if the second condition is determined to be true, then using the measurement M₁ to update estimates for the multi-dimensional value x for the particles of the plurality of particles.

13. The method as defined in claim 1, where a second condition is true when only the measurement M₁ is trustworthy, and if the second condition is determined to be true, then using the measurement M₁ to update estimates for the multi-dimensional value x for the particles.

14. The method as defined in claim 1, where a third condition is true when none of the measurements from the plurality of sensors is trustworthy, and if the third condition is determined to be true, then setting values in the prior estimate for the probability density function p(x_old) to a uniform distribution.

15. The method as defined in claim 1, where a third condition true when one of the measurements from the plurality of sensors is trustworthy and when the prior estimate is untrustworthy, where the one trustworthy measurement is from a dead reckoning sensor, and if the third condition is determined to be true, then setting values in the prior estimate for the probability density function p(x_old) to a uniform distribution.

16. The method as defined in claim 1, wherein allocating particles further comprising allocating each particle to a group, where each group corresponds to each sensor with a trustworthy measurement.

17. The method as defined in claim 1, wherein determining whether the M₁ measurement and the M₂ measurement are trustworthy is performed each time new measurements are available.

18. A method of sequentially integrating measurements from a first plurality of sensors to estimate a multi-dimensional value x, the method comprising:

using a probability density function p(x) to estimate the multi-dimensional value x, where the probability density function p(x) is represented by a plurality of particles;

receiving measurements from the first plurality of sensors;

determining whether the measurements from the first plurality of sensors are trustworthy, where a first condition is true when measurements from a plurality of trustworthy sensors are trustworthy, where the plurality of trustworthy sensors are a subset of the first plurality of sensors, and if the first condition is determined to be true, then performing;

grouping particles into a plurality of groups, where a group corresponds to a trustworthy sensor so that the plurality of groups correspond to the plurality of trustworthy sensors;

updating the estimates for the multi-dimensional value x for the particles, where the estimate {tilde over (x)} for a particle is computed at least partially based on the measurement from the sensor corresponding to the group associated with the particle and on a previous value x_old from a prior estimate of the probability density function p(x_old);

computing importance factors for the particles, where an importance factor for a particle is computed at least partially based on the updated estimate {tilde over (x)} for the particle and at least partially based on the measurements from one or more other sensors of the plurality of trustworthy sensors, where the one or more other sensors correspond to one or more other groups that are not used to generate the updated estimate {tilde over (x)} for that particle; and

resampling the particles with replacement using the importance factors to generate the probability density function p(x).

19. The method as defined in claim 18, wherein the measurements are integrated to compensate for errors in sensor measurements.

20. The method as defined in claim 18, where the plurality of trustworthy sensors are the same as the first plurality of sensors.

21. The method as defined in claim 18, further comprising grouping particles into the plurality of groups such that each group corresponds to each trustworthy sensor.

22. The method as defined in claim 18, further comprising resampling all the particles from all the groups with replacement using the computed importance factors associated with each particle to generate the probability density function p(x).

23. The method as defined in claim 18, wherein the multi-dimensional value x corresponds to a pose, and wherein the plurality of sensors are coupled to a mobile device and correspond to sensors for measuring pose.

24. The method as defined in claim 18, wherein the multi-dimensional value x corresponds to a pose in a simultaneous localization and mapping system (SLAM).

25. The method as defined in claim 18, wherein the multi-dimensional value x corresponds to a pose in a visual simultaneous localization and mapping system (VSLAM).

26. The method as defined in claim 18, further comprising determining that a measurement from a sensor is untrustworthy, the sensor termed an untrustworthy sensor, where the untrustworthy sensor is part of the plurality of sensors but not part of the plurality of untrustworthy sensors, where the measurement from the untrustworthy sensor is not used for updating an estimate for a particle.

27. The method as defined in claim 18, where a second condition is true when only a measurement from a first sensor is trustworthy, and if the second condition is determined to be true, then associating all the particles to one group and using the measurement from the first sensor to update estimates for the multi-dimensional value x for the particles.

28. The method as defined in claim 18, where a third condition is true when no measurement from the plurality of sensors is trustworthy, and if the third condition is determined to be true, then not updating estimates for the multi-dimensional value x for the particles of the probability density function p(x) with the measurements from the plurality of sensors.

29. A method for autonomously computing an updated estimate of position of a mobile device, where the mobile device includes a plurality of sensors adapted to at least measure position, the method comprising:

receiving measurements from the plurality of sensors, where the measurement include at least a first measurement indicating an absolute position and a second measurement indicating an incremental position;

determining how many of the measurements are trustworthy;

at least partially in response to determining that at least one measurement is trustworthy, then performing;

retrieving a prior estimate of the position of the mobile device; and

using the at least one trustworthy measurement and the prior estimate to update the estimate of the position of the mobile device;

and, at least partially in response to determining that none of the measurements is trustworthy, then resetting the estimated position to a predetermined value.

30. The method as defined in claim 29, wherein the measurements are integrated to compensate for errors in sensor measurements.

31. The method as defined in claim 29, further comprising receiving one or more indications of reliability for one or more of the measurements from the plurality of sensors.

32. The method as defined in claim 29, wherein the estimate also includes orientation such that a pose is estimated.

33. The method as defined in claim 29, wherein the estimate and the prior estimate are represented by a probability density function and wherein resetting the estimated position corresponds to setting the particles of the previous estimate to a uniform value.

34. The method as defined in claim 29, wherein the sensors comprise at least one sensor selected from the group of a visual sensor, an odometer, a pedometer, a range sensor, an inertial sensor, a global positioning system (GPS) sensor.