Vision-aided inertial navigation

Patent metadata

Field	Value
Publication number	US-11486707-B2
Application number	US-202117457022-A
Country	US
Kind code	B2
Filing date	Nov 30, 2021
Priority date	Mar 28, 2008
Publication date	Nov 1, 2022
Grant date	Nov 1, 2022

Abstract

Localization and navigation systems and techniques are described. An electronic device comprises a processor configured to maintain a state vector storing estimates for a position of the electronic device at poses along a trajectory within an environment along with estimates for positions for one or more features within the environment. The processor computes, from the image data, one or more constraints based on features observed from multiple poses of the electronic device along the trajectory, and computes updated state estimates for the position of the electronic device in accordance with the motion data and the one or more computed constraints without computing updated state estimates for the features for which the one or more constraints were computed.

First claim

What is claimed is:

1. A real-time vision aided inertial navigation system, comprising:
a camera capable of capturing a plurality of images;
an inertial measurement unit (IMU) capable of generating IMU measurements;
a set of one or more processors capable of receiving data from the camera and the IMU;
a visible display;
wherein the set of one or more processors is also capable of performing steps including:
receiving an IMU measurement from the IMU;
generating an IMU pose estimate based upon the received IMU measurement;
receiving image data from the camera comprising an image in which a specific feature is visible;
extracting at least one feature from the received image including the specific feature;
estimating a camera pose corresponding to the received image based upon the IMU pose estimate;
appending the camera pose estimate corresponding to the received image to a state vector of an Extended Kalman Filter to form an augmented state vector comprising an IMU state vector and a set of camera pose estimates, where the set of camera pose estimates comprises:
the camera pose estimate corresponding to the received image;
a plurality of additional camera pose estimates corresponding to a plurality of additional images in which the specific feature is visible;

estimating a position of the specific feature based upon the set of camera poses estimates;
computing an estimation error based upon:
the estimated position of the specific feature; and
observations of the specific feature based upon the received image and the plurality of additional images in which the specific feature is visible;

generating an updated state vector for the Extended Kalman Filter based upon the computed estimation error;
generating navigation information for the real-time inertial navigation system based upon the updated state vector for the Extended Kalman Filter; and
providing an output via the visible display, where the output is determined based upon the navigation information.

2. The system of claim 1 , wherein the output includes a visual representation of the navigation information.

3. The system of claim 1 , wherein:
the received IMU measurement comprises a measurement of angular velocity and a measurement of linear acceleration; and
the generated IMU pose estimate comprises a position estimate and an orientation estimate.

4. The system of claim 1 , wherein the set of one or more processors is also capable of performing an additional step comprising removing at least one camera pose estimate from the set of camera pose estimates in the augmented state vector when the number of camera pose estimates in the augmented state vector is greater than a maximum allowable number of camera pose estimates.

5. The method of claim 1 , wherein computing estimation error further comprises computing a residual based upon the difference between:
the estimated position of the specific feature; and
the observations of the specific feature based upon the received image and the additional images in which the specific feature is visible.

6. The method of claim 1 , wherein the set of one or more processors is also capable of computing the estimation error by performing an additional step comprising computing a first Jacobian of the observations of the specific feature with respect to the estimated position of the specific feature.

7. The method of claim 1 , wherein computing estimation error further comprises determining a linear approximation of the estimation error.

8. The method of claim 1 , wherein the computed estimation error is a linearized constraint between camera poses in the set of camera pose estimates.

9. The method of claim 1 , wherein the steps of estimating the position of the specific feature, computing the estimation error, and generating the updated state vector for the Extended Kalman Filter based upon the computed estimation error are performed in response to receiving additional image data from the camera comprising a subsequent image in which the specific feature is not visible.

10. The system of claim 2 , wherein the navigation information comprises at least one piece of navigation information selected from the group consisting of:
a position,
an attitude,
an orientation,
a heading,
a roll,
a pitch,
a yaw,
a velocity measure, and
an acceleration measure.

11. The system of claim 4 , wherein:
the augmented state vector comprises an initial camera pose estimate corresponding to an image received prior to receipt of images corresponding to other camera pose estimates in the set of camera pose estimates; and
the initial camera pose estimate is not removed from the set of camera pose estimates within the augmented state vector.

12. The method of claim 6 , wherein the set of one or more processors is also capable of computing the estimation error by performing additional steps comprising:
computing a second Jacobian of the observations of the specific feature with respect to the set of camera pose estimates;
performing a projection of the second Jacobian, where the projection utilizes a basis determined using the first Jacobian; and
calculating the error estimate using the second Jacobian and an estimate of error for the set of camera pose estimates.

13. A real-time vision aided inertial navigation system, comprising:
a camera capable of capturing a plurality of images;
an inertial measurement unit (IMU) capable of generating IMU measurements;
a set of one or more processors capable of receiving data from the camera and the IMU;
a visible display;
wherein the set of one or more processors is also capable of performing steps including:
receiving an IMU measurement from the IMU, wherein the IMU measurement comprises a measurement of angular velocity and a measurement of linear acceleration;
generating an IMU pose estimate based upon the received IMU measurement, where the generated IMU pose estimate comprises a position estimate and an orientation estimate;
receiving image data from the camera comprising an image in which a specific feature is visible;
extracting at least one feature from the received image including the specific feature;
estimating a camera pose corresponding to the received image based upon the IMU pose estimate;
appending the camera pose estimate corresponding to the received image to a state vector of an Extended Kalman Filter to form an augmented state vector comprising an IMU state vector and a set of camera pose estimates, where the set of camera pose estimates comprises:
the camera pose estimate corresponding to the received image;
a plurality of additional camera pose estimates corresponding to a plurality of additional images in which the specific feature is visible;

in response to receiving additional image data from the camera comprising a subsequent image in which the specific feature is not visible, updating the Extended Kalman Filter based upon observations of the specific feature in the received image and the additional images in which the specific feature is visible by:
estimating a position of the specific feature based upon the set of camera poses estimates;
generating an updated state vector for the Extended Kalman Filter based upon a residual, where the residual is computed by:
computing a first Jacobian of the observations of the specific feature with respect to the estimated position of the specific feature;
computing a second Jacobian of the observations of the specific feature with respect to a subsequent state vector for the Extended Kalman Filter;
performing a projection of the second Jacobian, where the projection utilizes a basis determined using the first

Assignees

• Univ Minnesota

Inventors

• Roumeliotis Stergios I
• Mourikis Anastasios I

Classifications

• G01C21/1656 Primary

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

• H04W4/027

  using movement velocity, acceleration information · CPC title

• G01C21/165

  combined with non-inertial navigation instruments · 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

• G01C21/16 Primary

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