Vision-aided inertial navigation

Patent metadata

Field	Value
Publication number	US-9766074-B2
Application number	US-38337109-A
Country	US
Kind code	B2
Filing date	Mar 23, 2009
Priority date	Mar 28, 2008
Publication date	Sep 19, 2017
Grant date	Sep 19, 2017

Abstract

This document discloses, among other things, a system and method for implementing an algorithm to determine pose, velocity, acceleration or other navigation information using feature tracking data. The algorithm has computational complexity that is linear with the number of features tracked.

First claim

What is claimed is:

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 m

Assignees

• Roumeliotis Stergios I
• Mourikis Anastasios I
• Univ Minnesota

Inventors

• Roumeliotis Stergios I
• Mourikis Anastasios I

Classifications

• G01C21/1656 Primary

  with passive imaging devices, e.g. cameras · CPC title

• G01C21/16 Primary

  by integrating acceleration or speed, i.e. inertial navigation · CPC title

• G01C21/165

  combined with non-inertial navigation instruments · CPC title

• H04W4/027

  using movement velocity, acceleration information · CPC title

• G05D1/00

  Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots (drive control systems specially adapted for autonomous road vehicles B60W60/00) · CPC title