Computational budget estimation for vision-aided inertial navigation systems

• US 10,371,529 B2
• Filed: 05/22/2017
• Issued: 08/06/2019
• Est. Priority Date: 07/11/2014
• Status: Active Grant

• Current Assignee: Regents of The University of Minnesota (University of Minnesota)
• Original Assignee: Regents of The University of Minnesota (University of Minnesota)
• Inventors: Roumeliotis, Stergios I., Wu, Kejian J.
• Primary Examiner(s): Black, Thomas G
• Assistant Examiner(s): Huynh, Luat T
• Application Number:
  US15/601,261
• Publication Number:
  US 20170261324A1
• Time in Patent Office: 806 Days
• Field of Search: 701501
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G05D 1/0246 using a video camera in combination with image processing means
• CPC Class Codes: G05D 1/027 comprising intertial navigation means, e.g. azimuth detector inertial navigation G01C21/16; inertial navigation combined with non-inertial navigation instruments G01C21/165
• CPC Class Codes: G05D 1/0274 using mapping information stored in a memory device navigation using map-matching G01C21/30
• CPC Class Codes: G06T 2207/30241 Trajectory
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/20 Analysis of motion motion estimation for coding, decoding, compressing or decompressing digital video signals H04N19/43, H04N19/51
• CPC Class Codes: G06T 7/70 Determining position or orientation of objects or cameras camera calibration G06T7/80
• CPC Class Codes: G06V 20/56 exterior to a vehicle by using sensors mounted on the vehicle

Abstract

This disclosure describes inverse filtering and square root inverse filtering techniques for optimizing the performance of a vision-aided inertial navigation system (VINS). In one example, instead of keeping all features in the system'"'"'s state vector as SLAM features, which can be inefficient when the number of features per frame is large or their track length is short, an estimator of the VINS may classify the features into either SLAM or MSCKF features. The SLAM features are used for SLAM-based state estimation, while the MSCKF features are used to further constrain the poses in the sliding window. In one example, a square root inverse sliding window filter (SQRT-ISWF) is used for state estimation.

Claims

1. A vision-aided inertial navigation system comprising:

at least one image source to produce image data along a trajectory of the vision-aided inertial navigation system (VINS) within an environment, wherein the image data contains a plurality of features observed within the environment at a plurality of poses of the VINS along the trajectory;

an inertial measurement unit (IMU) to produce IMU data indicative of motion of the vision-aided inertial navigation system; and

a hardware-based processing unit executing an estimator that determines, based on the image data and the IMU data, estimates for at least a position and orientation of the vision-aided inertial navigation system for a plurality of poses of the VINS along the trajectory,

wherein the estimator determines the estimates by;

computing a budget specifying an amount of resources that can be allocated to state estimations for a current pose of the VINS,

classifying, for each of the poses and based on the budget, each of the features observed at the respective pose into either a first set of the features or a second set of the features,

maintaining a state vector having states for a position and orientation of the VINS and for positions with the environment for the first set of features for a sliding window of two or more of the most recent poses along the trajectory without maintaining states for positions of the second set of features within the state vector, and

computing state estimates in accordance with the selected features.

2. The vision-aided inertial navigation system of claim 1, wherein the estimator classifies the features of the first set of features as simultaneous localization and mapping (SLAM) features for computing the state estimates and the second set of features as Multi-state Constraint Kalman Filter (MSCKF) features for computing constraints between the poses within the sliding window.

3. The vision-aided inertial navigation system of claim 2, wherein the estimator maintains the state vector to have a threshold number of SLAM features and to exclude all of the MSCKF features, where the estimator sets threshold based on current resource utilization within the VINS.

4. The vision-aided inertial navigation system of claim 2, wherein, to classify, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features:

determines that an amount of available processing resources of the VINS has increased;

based on the determination that the amount of available processing resources of the VINS has increased, increases a number of the SLAM features and decreases a number of the MSCKF features.

5. The vision-aided inertial navigation system of claim 2, wherein, to classify, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features:

determines that an amount of available processing resources of the VINS has decreased;

based on the determination that the amount of available processing resources of the VINS has decreased, decreases a number of the SLAM features and increases a number of the MSCKF features.

6. The vision-aided inertial navigation system of claim 2, wherein the estimator further classifies the MSCKF features into a third set of features and a fourth set of features, and wherein computing state estimates in accordance with the selected features comprises:

computing state estimates and updated uncertainty data using the third set of features, and

computing state estimates for the fourth set of features without computing updated uncertainty data using the fourth set of features.

7. The vision-aided inertial navigation system of claim 6, wherein the third set of data comprises Long Features and Old-Short-Features, and the fourth set of data comprises New-Short-Features and Hope Features.

8. The vision-aided inertial navigation system of claim 1, wherein the estimator applies an inverse sliding window filter to compute constraints between the poses within the sliding window based on the second set of features and compute, in accordance with the constraints, the state estimates within the state vector for the sliding window.

9. The vision-aided inertial navigation system of claim 8, wherein, to apply the inverse sliding window filter, the estimator, for each update of the state estimates within the state vector:

computes a Hessian matrix that excludes at least a portion of the second features set, wherein the Hessian matrix represents a subset of the IMU data and the image along the trajectory, and

updates the state estimates based on the Hessian.

10. The vision-aided inertial navigation system of claim 8, wherein the states within the state vector represent, for each of the poses within the sliding window, an orientation and position of the VINS at a corresponding location along the trajectory.

11. The vision-aided inertial navigation system of claim 1, wherein the vision-aided inertial navigation system is integrated within a tablet computer, a laptop computer, a mobile phone, a robot, or a vehicle.

12. A method comprises:

producing image data along a trajectory of the vision-aided inertial navigation system (VINS) within an environment, wherein the image data contains a plurality of features observed within the environment at a plurality of poses of the VINS along the trajectory;

producing inertial measurement data from an inertial measurement unit (IMU) indicative of motion of the vision-aided inertial navigation system; and

computing state estimates for a position and orientation of the vision-aided inertial navigation system for a plurality of poses of the VINS along the trajectory, based on the image data and the IMU data, with a processing unit comprising an estimator,

wherein computing the state estimates comprises;

computing a budget specifying an amount of resources that can be allocated to state estimations for a current pose of the VINS;

classifying, for each of the poses and based on the budget, each of the features observed at the respective pose into either a first set of the features or a second set of the features,

maintaining a state vector having states for a position and orientation of the VINS and for positions with the environment for the first set of features for a sliding window of two or more of the most recent poses along the trajectory without maintaining states for positions of the second set of features within the state vector, and

computing state estimates in accordance with the selected features.

13. The method of claim 12, wherein classifying each of the features observed at the respective pose into either the first set of features or the second set of features comprises classifying the features of the first set of the features as simultaneous localization and mapping (SLAM) features for computing the state estimates and the second set of the features as Multi-state Constraint Kalman Filter (MSCKF) features for computing constraints between the poses within the sliding window.

14. The method of claim 13, further comprising:

setting a threshold number of SLAM features based on current resource utilization within the VINS; and

maintaining the state vector to have the threshold number of SLAM features and to exclude all of the MSCKF features.

15. The method of claim 13, wherein classifying, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features comprises:

determining that an amount of available processing resources of the VINS has increased;

based on the determination that the amount of available processing resources of the VINS has increased, increasing a number of the SLAM features and decreases a number of the MSCKF features.

16. The method of claim 13, wherein classifying, for each of the poses and based on the budget, each of the features observed at the respective pose into either the SLAM features or the MSCKF features comprises:

determining that an amount of available processing resources of the VINS has decreased;

based on the determination that the amount of available processing resources of the VINS has increased, decreasing a number of the SLAM features and increases a number of the MSCKF features.

17. The method of claim 13, further comprising classifying the MSCKF features into a third set of features and a fourth set of features, and wherein computing state estimates in accordance with the selected features comprises:

computing state estimates and uncertainty data for the third set of features, and

computing state estimates for the fourth set of features without computing uncertainty data for the fourth set of features.

18. The method of claim 13, wherein the third set of data comprises Long Features and Old-Short-Features, and the fourth set of data comprises New-Short-Features and Hope Features.

19. The method of claim 12, further comprising:

applying an inverse sliding window filter to compute constraints between the poses within the sliding window based on the second set of features; and

computing, in accordance with the constraints, the state estimates within the state vector for the sliding window.

20. The method of claim 19, wherein applying the inverse sliding window filter comprises, for each update of the state estimates within the state vector:

computing a Hessian matrix that excludes at least a portion of the second features set, wherein the Hessian matrix represents a subset of the IMU data and the image along the trajectory, and

updating the state estimates based on the Hessian.

21. The method of claim 19, wherein the states within the state vector represent, for each of the poses within the sliding window, an orientation and position of the VINS at a corresponding location along the trajectory.

22. The method of claim 12, wherein the VINS is integrated within a tablet computer, a laptop computer, a mobile phone, a robot, or a vehicle.