Efficient vision-aided inertial navigation using a rolling-shutter camera with inaccurate timestamps

• US 10,012,504 B2
• Filed: 06/08/2015
• Issued: 07/03/2018
• Est. Priority Date: 06/19/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., Guo, Chao
• Primary Examiner(s): Rink, Ryan
• Assistant Examiner(s): Castro, Paul A
• Application Number:
  US14/733,468
• Publication Number:
  US 20150369609A1
• Time in Patent Office: 1,121 Days
• Field of Search: None
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G06T 2207/30241 Trajectory
• CPC Class Codes: G06T 2207/30244 Camera pose
• CPC Class Codes: G06T 7/277 involving stochastic approaches, e.g. using Kalman filters

Abstract

Vision-aided inertial navigation techniques are described. In one example, a vision-aided inertial navigation system (VINS) comprises an image source to produce image data at a first set of time instances along a trajectory within a three-dimensional (3D) environment, wherein the image data captures features within the 3D environment at each of the first time instances. An inertial measurement unit (IMU) to produce IMU data for the VINS along the trajectory at a second set of time instances that is misaligned with the first set of time instances, wherein the IMU data indicates a motion of the VINS along the trajectory. A processing unit comprising an estimator that processes the IMU data and the image data to compute state estimates for 3D poses of the IMU at each of the first set of time instances and 3D poses of the image source at each of the second set of time instances along the trajectory. The estimator computes each of the poses for the image source as a linear interpolation from a subset of the poses for the IMU along the trajectory.

Claims

1. A vision-aided inertial navigation system (VINS) comprising:

an image source configured to produce image data at a first set of time instances along a trajectory within a three-dimensional (3D) environment, wherein the image data captures feature observations within the 3D environment at each of the first set of time instances;

an inertial measurement unit (IMU) configured to produce IMU data for the VINS along the trajectory at a second set of time instances that is misaligned in time with the first set of time instances, wherein the IMU data indicates a motion of the VINS along the trajectory; and

a processing unit comprising an estimator configured to process the IMU data and the image data to compute state estimates for poses of the IMU at each of the first set of time instances and poses of the image source at each of the second set of time instances along the trajectory,

wherein the estimator is configured to compute each of the poses for the image source as an interpolation from a subset of the poses for the IMU along the trajectory.

2. The vision-aided inertial navigation system of claim 1, wherein the estimator is configured to compute each of the poses for the image source as linear interpolation or as a higher order interpolation from the subset of the poses for the IMU along the trajectory.

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

wherein the poses of the IMU and poses of the image source are computed as positions and 3D orientations at each of the second set of time instances along the trajectory, and

wherein the estimator is configured to compute state estimates for a position, orientation and velocity of the VINS.

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

wherein the estimator is configured to store a sliding window of the poses computed for the IMU along the trajectory, and

wherein, when computing each of the poses for the image source, the estimator selects the poses for the IMU that are adjacent in the sliding window and that have time instances closest to the time instance for the pose being computed for the image source, and computes the respective poses for the image source as an interpolation of the selected poses for the IMU.

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

wherein the estimator is configured to compute estimated positions for features observed within the environment at each of the second set of time instances, and

wherein, for each of the second set of time instances, the estimator is configured to compute the positions for the features by applying an interpolation ratio to the subset of poses for the IMU that were used to compute the pose for the image source at that time instance.

6. The vision-aided inertial navigation system of claim 1, wherein, for each of poses for the image source, the interpolation ratio represents a ratio of the distances along the trajectory from the pose of the image source to the subset of the poses for the IMU used to compute the pose for the image source.

7. The vision-aided inertial navigation system of claim 1, wherein the estimator is configured to build a map of the environment.

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

wherein the image source comprises at least one sensor having a plurality of rows of image data,

wherein the sensor reads the plurality of rows of image data row-by-row so that each of the rows are read at a different one of the first time instance, and

wherein, when computing the poses, the estimator is configured to compute a different one of the poses for the image source for each of the rows of the image data using a different interpolation ratio.

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

wherein the estimator is configured to compute each of the poses for the image source by storing and updating a state vector having a sliding window of poses for the image source, each of the poses for the image source corresponding to a different one of the first set of time instances at which image data was received from the image source, and

wherein, responsive to the image source producing image data, the estimator is configured to clone a most recent pose computed for the IMU and inserts the cloned pose into the state vector as an image source pose having six dimensions for the image source and a scalar representing a time offset for the time at which the image data was received and excluding state estimates for linear and rotational velocities.

10. The vision-aided inertial navigation system of claim 9, wherein each of clones of the IMU pose x_Ii is represented in the state vector as:

x_Ii=[^Iiq_G^{T G}p_Ii^T λ

_di]^T

where ^Iiq_G^T, ^Gp_Ii^T, λ

_di are cloned at the time instant that the i-th image data was recorded.

11. The vision-aided inertial navigation system of claim 9,

wherein estimator represents the state vector estimate as:

X=[x_I x<sub>Ik+n−

1</sub> . . . x_Ik,

where x_I denotes a current pose for the image source, and x_Ii, for I=k+n−

1, . . . , k are the cloned IMU poses in the sliding window, corresponding to the time instants of the last n camera measurements, and

wherein the current pose for the image source comprises;

x_I=[^Iq_G^{T G}v_I^{T G}p_I^T b_a^T b_g^T λ

_d λ

_r]^T,

where ^Iq_G is the quaternion representation of the orientation of a global frame of reference in the IMU'"'"'s frame of reference, ^Gv_I and ^Gp_I are the velocity and position of a frame of reference for the IMU in the global frame of reference respectively, while b_a and b_g correspond to gyroscope and accelerometer biases for the IMU.

12. The vision-aided inertial navigation system of claim 1, wherein the vision-aided inertial navigation system is integrated within a mobile phone or a robot.

13. The vision-aided inertial navigation system of claim 1, wherein the first set of time instances and the second set of time instances are offset in time by an amount.

14. A method for computing state estimates for a vision-aided inertial navigation system (VINS) comprising:

receiving image data associated with each of a first set of time instances along a trajectory within a three-dimensional (3D) environment, wherein the image data captures features within the 3D environment at each of the first set of time instances time instances;

receiving, from an inertial measurement unit (IMU), IMU data indicative of a motion of the VINS along the trajectory at a second set of time instances that is misaligned in time with the first set of time instances; and

processing the IMU data and the image data to compute state estimates for poses of the IMU at each of the first set of time instances and poses of the image source at each of the second set of time instances along the trajectory,

wherein each of the poses for the image source is computed as a linear interpolation from a subset of the poses for the IMU along the trajectory.

15. The method of claim 14, wherein computing each of the poses for the image source comprises applying a linear interpolation or a higher order interpolation from the subset of the poses for the IMU along the trajectory.

16. The method of claim 14, wherein the poses of the IMU and poses of the image source are computed as positions and 3D orientations at each of the second set of time instances along the trajectory.

17. The method in claim 14, further comprising:

storing a sliding window of the poses previously computed for the IMU; and

selecting the subset of the poses from the sliding window.

18. The method in claim 14, further comprising

for each of the second set of time instances, computing positions for features within the environment by applying an interpolation ratio to the subset of poses for the IMU that were used to compute the pose for the image source at that time instance.

19. The method in claim 18, wherein, for each of poses for the image source, the interpolation ratio represents a ratio of the distances along the trajectory from the pose of the image source to the subset of the poses for the IMU used to compute the pose for the image source.

20. The method of claim 14, further comprising constructing a map of the environment to include the state estimates of the VINS.

21. The method of claim 14, wherein the first set of time instances and the second set of time instances are offset in time by an amount.

22. The method of claim 14, wherein computing each of the poses for the image source comprises:

maintaining a state vector having a sliding window of poses for the image source, each of the poses for the image source corresponding to a different one of the first set of time instances at which image data was received from the image source;

responsive to the image source producing data, cloning a most recent pose computed for the IMU and inserts the cloned pose into the state vector, wherein the cloned pose includes six dimensions for the image source and a scalar representing a time offset for the time at which the image data was received, and wherein the cloned pose excludes state estimates for linear and rotational velocities; and

updating, upon receiving IMU data from the IMU source, each of the poses within the state vector.

23. A non-transitory computer-readable storage device comprising program code to cause a processor to:

receive image data associated with each of a first set of time instances along a trajectory within a three-dimensional (3D) environment, wherein the image data captures features within the 3D environment at each of the first set of time instances;

receive, from an inertial measurement unit (IMU), IMU data indicative of a motion of the VINS along the trajectory at a second set of time instances that is misaligned in time with the first set of time instances;

process the IMU data and the image data to compute state estimates for 3D poses of the IMU at each of the first set of time instances and 3D poses of the image source at each of the second set of time instances along the trajectory,

wherein each of the poses for the image source is computed as a linear interpolation from a subset of the poses for the IMU along the trajectory.

24. An inertial navigation system comprising:

a first sensor to produce a first set of spatial data at a first set of time instances along a trajectory within a three-dimensional (3D) environment, wherein the first set of spatial data captures features within the 3D environment at each of the first set of time instances;

a second sensor to produce a second set of spatial data for the inertial navigation system along the trajectory at a second set of time instances that is misaligned in time with the first set of time instances, wherein the second set of spatial data indicates a motion of the inertial navigation system along the trajectory; and

a processing unit comprising an estimator that processes the first set of spatial data and the second set of spatial data to compute state estimates for poses of the second sensor at each of the first set of time instances and poses of the first sensor at each of the second set of time instances along the trajectory,

wherein the estimator computes each of the poses for the second sensor as an interpolation from a subset of the poses for the first sensor along the trajectory.

25. A vision-aided inertial navigation system (VINS) comprising:

an image source to produce image data at a first set of time instances along a trajectory within a three-dimensional (3D) environment, wherein the image data captures features within the 3D environment at each of the first set of time instances;

an inertial measurement unit (IMU) to produce IMU data for the VINS along the trajectory at a second set of time instances that is misaligned in time with the first set of time instances, wherein the IMU data indicates a motion of the VINS along the trajectory; and

a processing unit comprising an estimator that processes the IMU data, the image data, or a set of features data to compute state estimates for poses of the IMU at each of the first set of time instances and poses of the image source at each of the second set of time instances along the trajectory,

wherein the estimator computes each of the poses for the IMU as an interpolation from a subset of the poses for the image source along the trajectory.

26. A vision-aided inertial navigation system (VINS) comprising:

an image source configured to produce image data that captures feature observations along a trajectory within a three-dimensional (3D) environment, wherein the image source comprises a sensor having a plurality of rows of image data, and wherein the sensor is configured to read the image data row-by-row so that each of the rows of image data corresponds to a different one of a first set of time instances along the trajectory;

an inertial measurement unit (IMU) configured to produce IMU data for the VINS along the trajectory at a second set of time instances that is misaligned in time with the first set of time instances; and

a processing unit comprising an estimator configured to process the IMU data and the image data to compute state estimates for poses of the IMU at each of the first set of time instances corresponding to the different rows of image data.

27. The VINS of claim 26, where the estimator is configured to compute state estimates for poses of the image source at each of the second set of time instances along the trajectory.

28. The VINS of claim 27, wherein the estimator is configured to compute each of the poses for the image source as an interpolation from a subset of the poses for the IMU along the trajectory.