Vision-aided inertial navigation

1. A vision-aided inertial navigation system comprising:

at least one image source to produce image data for a plurality of poses of a frame of reference along a trajectory within an environment over a period of time, wherein the image data includes features that were each observed within the environment at poses of the frame of reference along the trajectory, wherein one or more of the features were each observed at multiple ones of the poses of the frame of reference along the trajectory;

a motion sensor configured to provide motion data of the frame of reference in the environment for the period of time; and

a hardware-based processor communicatively coupled to the image source and communicatively coupled to the motion sensor, the processor configured to compute estimates for at least a position and orientation of the frame of reference for each of the plurality of poses of the frame of reference along the trajectory,

wherein the processor is configured to:

determine, from the image data, feature measurements corresponding to the features observed from the poses along the trajectory;

group the feature measurements according to the features observed within the image data;

for one or more of the features observed from multiple poses along the trajectory, compute based on the respective group of feature measurements for the feature, one or more constraints that geometrically relate the multiple poses from which the respective feature was observed; and

determine the position and orientation of the frame of reference for each of the plurality of poses along the trajectory by updating, in accordance with the motion data and the one or more computed constraints, state information within a state vector representing estimates for the position and orientation of the frame of reference along the trajectory while excluding, from the state vector, state information representing estimates for positions within the environment for the features that were each observed from the multiple poses and for which the one or more constraints were computed.

2. The vision-aided inertial navigation system of claim 1 wherein the processor is configured to generate each of the one or more constraints by manipulating a residual of a measurement for the respective feature.

3. The vision-aided inertial navigation system of claim 1 ,

wherein the image source includes a camera, and

wherein the vision-aided inertial navigation system comprises one of a robot or a vehicle.

4. The vision-aided inertial navigation system of claim 1 wherein the motion sensor includes an inertial measurement unit (IMU).

5. The vision-aided inertial navigation system of claim 1 wherein the processor is configured to implement an extended Kalman filter to compute the one or more constraints.

6. The vision-aided inertial navigation system of claim 1 further including an output device coupled to the processor, the output device including at least one of a memory, a transmitter, a display, a printer, an actuator, and a controller.

7. A method comprising:

receiving, with a processor and from at least one image source communicatively coupled to the processor, image data for a plurality of poses of a frame of reference along a trajectory within an environment over a period of time, wherein the image data includes features that were each observed within the environment at poses of the frame of reference along the trajectory, wherein one or more of the features were each observed at multiple ones of the poses of the frame of reference along the trajectory;

receiving, with the processor and from a motion sensor communicatively coupled to the processor, motion data of the frame of reference in the environment for the period of time;

computing, with the processor, state estimates for at least a position and orientation of the frame of reference for each of the plurality of poses of the frame of reference along the trajectory,

wherein computing the state estimates comprises:

determining, from the image data, feature measurements corresponding to the features observed from the poses along the trajectory;

grouping the feature measurements according to the features observed within the image data;

for one or more of the features observed from multiple poses along the trajectory, computing, based on the respective group of feature measurements for the feature, one or more constraints that geometrically relate the multiple poses from which the respective feature was observed; and

determining the position and orientation of the frame of reference for each of the plurality of poses along the trajectory by updating, in accordance with the motion data and the one or more computed constraints, state information within a state vector representing estimates for the position and orientation of the frame of reference along the trajectory while excluding, from the state vector, state information representing estimates for positions within the environment for the features that were each observed from the multiple poses and for which the one or more constraints were computed; and

controlling, responsive to the computed state estimates, navigation of the frame of reference.

8. The method of claim 7 wherein computing one or more constraints comprises manipulating a residual of a measurement of the respective feature to reduce an effect of a feature estimate error.

9. The method of claim 7 wherein computing state estimates further includes computing states estimates for at least a velocity, or acceleration of the frame of reference.

10. The method of claim 7 wherein receiving image data includes receiving data from at least one of a camera-based sensor, a laser-based sensor, a sonar-based sensor, and a radar-based sensor.

11. The method of claim 7 wherein receiving motion data from the motion sensor includes receiving data from at least one of a wheel encoder, a velocity sensor, a Doppler radar based sensor, a gyroscope, an accelerometer, an airspeed sensor, and a global positioning system (GPS) sensor.

12. The method of claim 7 wherein computing, based on the image data, one or more constraints comprises executing an Extended Kalman Filter (EKF).

13. A non-transitory computer-readable storage medium comprising instructions that configure a processor to:

receive, with the processor and from at least one image source communicatively coupled to the processor, image data for a plurality of poses of a frame of reference along a trajectory within an environment over a period of time, wherein the image data includes features that were each observed within the environment at poses of the frame of reference along the trajectory, wherein one or more of the features were each observed at multiple ones of the poses of the frame of reference along the trajectory;

receive, with the processor and from a motion sensor communicatively coupled to the processor, motion data of the frame of reference in the environment for the period of time;

determine, from the image data, feature measurements corresponding to the features observed from the poses along the trajectory;

group the feature measurements according to the features observed within the image data;

for one or more of the features observed from multiple poses along the trajectory, compute, based on the respective group of feature measurements for the feature, one or more constraints that geometrically relate the multiple poses from which the respective feature was observed;

determine state estimates for at least a position and an orientation of the frame of reference for each of the plurality of poses along the trajectory by updating, in accordance with the motion data and the one or more computed constraints, state information within a state vector representing estimates for the position and orientation of the frame of reference along the trajectory while excluding, from the state vector, state estimates for positions within the environment for the features that were each observed from the multiple poses and for which the one or more constraints were computed; and

output, for display, information responsive to the computed state estimates for the frame of reference.

14. The computer-readable storage medium of claim 13 wherein the instructions configure the processor to compute the one or more constraints by manipulating a residual of a measurement of the respective feature to reduce an effect of a feature estimate error.

15. The computer-readable storage medium of claim 13 wherein the instructions configure the processor to compute states estimates for at least a velocity, or acceleration of the frame of reference.

16. The computer-readable storage medium of claim 13 wherein the image data includes data from at least one of a camera-based sensor, a laser-based sensor, a sonar-based sensor, and a radar-based sensor.

17. The computer-readable storage medium of claim 13 wherein the motion data includes data from at least one of a wheel encoder, a velocity sensor, a Doppler radar based sensor, a gyroscope, an accelerometer, an airspeed sensor, and a global positioning system (GPS) sensor.

18. The computer-readable storage medium of claim 13 wherein the instructions configure the processor to execute an Extended Kalman Filter (EKF).

19. The computer-readable storage medium of claim 13 wherein one or more of the constraints are computed based on the feature measurements for features observed at three or more of the poses along the trajectory.

20. The vision-aided inertial navigation system of claim 1 wherein the processor computes one or more of the constraints based on the feature measurements for features observed at three or more of the poses along the trajectory.

21. The method of claim 7 wherein one or more of the constraints are computed based on the feature measurements for features observed at three or more of the poses along the trajectory.