Real-time pose estimation system using inertial and feature measurements

• US 9,798,929 B2
• Filed: 06/10/2016
• Issued: 10/24/2017
• Est. Priority Date: 05/16/2013
• Status: Expired due to Fees

• Current Assignee: Regents of the University of California (University of California)
• Original Assignee: Regents of the University of California (University of California)
• Inventors: Li, Mingyang, Mourikis, Anastasios
• Primary Examiner(s): Li, Ruiping
• Application Number:
  US15/179,631
• Publication Number:
  US 20170061195A1
• Time in Patent Office: 501 Days
• Field of Search: 382160, 382103, 348 36
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G06V 20/10 Terrestrial scenes scenes under surveillance with static cameras G06V20/52; scenes perceived from the exterior of a vehicle G06V20/56; scenes perceived from the interior of a vehicle G06V20/59

Abstract

A hybrid estimator system using visual and inertial sensors for real-time pose tracking on devices with limited processing power using at least one processor, a memory, a storage and communications through a protocol and one or more than one software module for a hybrid estimator, real-time algorithm selection to process different measurements, statistical learning for these characteristics to compute the expected device computing cost of any strategy for allocating measurements to algorithms, and algorithm selection based on the statistical learning module.

Claims

1. A method for performing real-time pose tracking of a platform moving in an environment comprising:

capturing data from at least an inertial sensor and a visual sensor on the moving platform, the data from the visual sensor including images;

identifying in the data captured by the visual sensor a plurality of features in the environment and performing measurements on the identified features;

estimating characteristics of the features from the measurements, including the number of images in which the features can be tracked;

selecting from a plurality of algorithms an algorithm to be used for processing each of the measurements using the estimated characteristics of the features; and

processing each of the measurements using the selected algorithm for that measurement to track the pose of the platform.

2. The method of claim 1 further comprising:

selecting the algorithm based on the characteristics of the environment, the trajectory of the moving object, and the availability of processing resources; and

adjusting the number of features to be extracted based on the availability of processing resources by determining the number of features to be extracted from raw sensor data.

3. The method of claim 1 wherein:

a multistate-constraint Kalman filter is one of the plurality of algorithms for processing each of the feature measurements; and

for each feature processed using the multistate-constraint Kalman filter algorithm, calculating a residual and Jacobian matrix for each feature to be processed, performing a Mahalanobis gating test, and forming a residual vector and a Jacobian matrix using all features that passed the Mahalanobis gating test.

4. The method of claim 1 further comprising:

learning statistical characteristics of the feature measurements; and

solving an optimization problem with an objective function representing an expected computation time to select the algorithm;

computing in real time an expected computation time for processing measurements each of the plurality of algorithms to reduce the computational requirements; and

selecting the algorithm to be used that has the lowest expected computation time for processing measurements.

5. The method of claim 1 wherein a state vector that includes the current IMU state, a number of previous platform poses, and a plurality of features is used for pose tracking.

6. The method of claim 5 further comprising reducing computation time by adjusting the number of previous platform poses in the state vector.

7. The method of claim 6, comprising:

processing features having a track length smaller than the number, m, of previous platform poses in the state vector using a multistate-constraint Kalman filter algorithm; and

processing features having a track length larger than m of previous platform poses in the state vector using an extended Kalman filter-simultaneous localization and mapping algorithm.

8. The method of claim 1, further comprising:

employing within the algorithms linearized approximations of nonlinear mathematical models that describe motion and sensor measurements, wherein the linearized approximations utilize linearization matrices computed using a unique estimate of each platform position and velocity state to linearize all measurement equations.

9. The method of claim 1, further comprising:

employing within the algorithms linearized approximations of nonlinear mathematical models that describe motion and sensor measurements, wherein the linearized approximations utilize linearization matrices computed using estimates of platform position states and feature positions that are modified so that their difference remains constant through time.

10. The method of claim 1, wherein the choice of algorithm to process each individual feature is made based on the distribution of the feature track lengths of features.