Systems and methods for incrementally updating a pose of a mobile device calculated by visual simultaneous localization and mapping techniques
The invention is related to methods and apparatus that use a visual sensor and dead reckoning sensors to process Simultaneous Localization and Mapping (SLAM). These techniques can be used in robot navigation. Advantageously, such visual techniques can be used to autonomously generate and update a map. Unlike with laser rangefinders, the visual techniques are economically practical in a wide range of applications and can be used in relatively dynamic environments, such as environments in which people move. One embodiment further advantageously uses multiple particles to maintain multiple hypotheses with respect to localization and mapping. Further advantageously, one embodiment maintains the particles in a relatively computationally-efficient manner, thereby permitting the SLAM processes to be performed in software using relatively inexpensive microprocessor-based computer system.
1. A method of updating a map for autonomous localization and mapping for a device, the method comprising:
receiving an indication that a landmark has been observed, where the landmark is associated with a landmark pose, and where the map comprises one or more landmarks;
predicting a first device pose based on a change in pose from a prior update to device pose, wherein the change in pose is computed at least partly from dead reckoning data;
predicting a second device pose based on a visual measurement relative to the observed landmark and based on a stored estimate of landmark pose;
predicting a new pose for the device based on at least one of the first device pose and the second device pose; and
using a statistical filter to update the corresponding landmark pose estimate based at least partly on the new device pose.
2. The method as defined in claim 1 , wherein the device corresponding landmark pose estimate robot, the method further comprising using the map for navigation in the mobile robot.
3. The method as defined in claim 1 , wherein predicting a second device pose further comprises using a model of visual measurement uncertainty.
4. The method as defined in claim 1 , wherein predicting a first device pose further comprises using a model of uncertainty in a dead reckoning measurement.
5. The method as defined in claim 1 , wherein the statistical filter corresponds to a Kalman filter.
6. The method as defined in claim 1 , wherein the visual measurement is derived from a single camera as a visual sensor.
7. The method as defined in claim 1 , wherein the method is performed in real time.
8. The method as defined in claim 1 , wherein predicting the new pose for the device is performed without iterative estimations of device poses between measurements.
9. The method as defined in claim 1 , wherein updating the map is performed without iterative estimations of device poses between measurements.
10. The method as defined in claim 1 , further comprising maintaining multiple hypotheses as multiple particles, where a particle includes a map and a device pose estimate.
11. A circuit for updating a map for autonomous localization and mapping for a device, the circuit comprising:
a circuit configured to receive an indication that a landmark has been observed, where the landmark is associated with a landmark pose, and where the map comprises one or more landmarks;
a circuit configured to predict a first device pose based on a change in pose from a prior update to device pose, wherein the change in pose is computed at least partly from dead reckoning data;
a circuit configured to predict a second device pose based on a visual measurement relative to the observed landmark and based on a stored estimate of landmark pose;
a circuit configured to predict a new pose for the device based on at least one of the first device pose and the second device pose; and
statistical filter that updates the corresponding landmark pose estimate based at least partly on the new device pose.
12. The circuit as defined in claim 11 , wherein the device corresponds to a mobile robot, where the circuit is embodied in the mobile robot for navigation of the mobile robot.
13. The method as defined in claim 11 , wherein the circuit configured to predict a second device pose is further configured to model visual measurement uncertainty.
14. The method as defined in claim 11 , wherein the circuit configured to predict a first device pose is further configured to model uncertainty in a dead reckoning measurement.
15. A computer program embodied in a tangible medium for updating a map for autonomous localization and mapping for a device, the computer program comprising:
a module with instructions configured to receive an indication that a landmark has been observed, where the landmark is associated with a landmark pose, and where the map comprises one or more landmarks;
a module with instructions configured to predict a first device pose based on a change in pose from a prior update to device pose, wherein the change in pose is computed at least partly from dead reckoning data;
a module with instructions configured to predict a second device pose based on a visual measurement relative to the observed landmark and based on a stored estimate of landmark pose;
a module with instructions configured to predict a new pose for the device based on at least one of the first device pose and the second device pose; and
a module with instructions configured to use a statistical filter to update the corresponding landmark pose estimate based at least partly on the new device pose.
16. The computer program as defined in claim 15 , wherein the device corresponds to a mobile robot, wherein the computer program is used by the mobile robot for navigation.
17. The computer program as defined in claim 15 , wherein, the module with instructions configured to predict a second device pose further comprises instructions configured to use a model of visual measurement uncertainty.
18. The computer program as defined in claim 15 , wherein the module with instructions configured to predict a first device pose further comprises instructions configured to use a model of uncertainty in a dead reckoning measurement.
19. A circuit for updating a map for autonomous localization and mapping for a device, the circuit comprising:
a means for receiving an indication that a landmark has been observed, where the landmark is associated with a landmark pose, and where the map comprises one or more landmarks;
a means for predicting a first device pose based on a change in pose from a prior update to device pose, wherein the change in pose is computed at least partly from dead reckoning data;
a means for predicting a second device pose based on a visual measurement relative to the observed landmark and based on a stored estimate of landmark pose;
a means for predicting a new pose for the device based on at least one of the first device pose and the second device pose; and
a statistical filter configured to update the corresponding landmark pose estimate based at least partly on the new device pose.
20. The circuit as defined in claim 19 , wherein the device corresponds to a mobile robot, and where the circuit is embodied in the mobile robot for navigation.
21. A method of automatically updating a map for navigation with a Kalman filter, the method comprising:
providing the map, where the map comprises one or more landmarks, where a landmark includes a landmark pose estimate and an uncertainty measure, where a landmark pose estimate is initially based on a device pose estimate in a global reference frame corresponding to when the landmark was added to the map, and where the landmark pose estimate is updated as the landmark is re-encountered;
predicting a landmark pose based at least in part on a reference device pose and one or more measurements of relative pose between the device pose and the landmark, wherein the one or more measurements of relative device pose are based at least in part on one or more visual measurements;
computing the uncertainty measure for error in the predicted landmark pose;
using the Kalman filter, wherein:
inputs to the Kalman filter comprise the predicted landmark pose, the uncertainty measure thereof, a previous estimate of landmark pose, and the uncertainty measure thereof; and
outputs of the Kalman filter comprise an updated estimate of landmark pose and the uncertainty measure thereof; and
storing the updated landmark pose estimate and the updated uncertainty measure in the map such that the updated landmark pose estimate and the updated uncertainty measure can be updated again when the landmark is re-encountered.
22. The method as defined in claim 21 , further comprising using the map for robot navigation.
23. The method as defined in claim 21 , wherein computing the uncertainty measure further comprises computing uncertainty in a visual measurement and computing uncertainty in a dead reckoning measurement.
24. The method as defined in claim 21 , wherein the reference device pose corresponds to a device pose corresponding to a prior update of the device pose in the global reference frame.
25. The method as defined in claim 21 , where the prior update corresponds to the most recent update.
26. The method as defined in claim 21 , wherein the measured landmark pose equals:
L
^
m
k
=
[
s
m
(
2
)
+
Δ
1
vis
cos
(
s
m
(
4
)
+
Δ
2
vis
)
s
m
(
3
)
+
Δ
1
vis
sin
(
s
m
(
4
)
+
Δ
2
vis
)
[
(
s
m
(
4
)
+
Δ
3
vis
+
π
)
mod
2
π
]
-
π
]
.
27. The method as defined in claim 21 , further comprising maintaining multiple hypotheses as multiple particles, where a particle includes a map and a device pose estimate.