Granted Patent B2

US 11,940,277 · App. 16/425,422 · Granted Mar 26, 2024

Inventors: Stergios I. Roumeliotis (St Paul, MN); Kejian J. Wu (Minneapolis, MN); Chao Guo (Minneapolis, MN); Georgios Georgiou (San Francisco, CA)

Assignee: Regents of the University of Minnesota

G01C21/1656G01C21/28G06T7/246G06T7/277G06T7/73G07C5/08

Abstract

Claims (74)

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

at least one image source configured to produce image data along a trajectory of the VINS within an environment, wherein the image data contains a plurality of features observed within the environment;

an inertial measurement unit (IMU) configured to produce IMU data indicative of motion of the VINS;

an odometry unit configured to produce odometry data for the IMU, the odometry data indicative of scale information, wherein the odometry unit includes at least two wheels and determines odometry data for each of the at least two wheels; and

a processor communicatively coupled to the at least one image source, the IMU, and the odometry unit, the processor configured to compute, based on the image data, the IMU data, and the odometry data, state estimates for a position and orientation of the VINS for a plurality of poses of the VINS along the trajectory,

wherein the processor computes the state estimates by:

maintaining a state vector for the VINS based upon measurements from the IMU and the at least one image source, where the state vector comprises state estimates for each of the plurality of poses of the VINS along the trajectory, where each state estimate comprises a position and orientation of the VINS;

estimating an error in the state vector based upon a recent state estimate and the measurements from the IMU and the at least one image source; and

updating the state estimates based on the odometry data and at least one feature observed from at least one of the plurality of poses of the VINS along the trajectory by:

generating a geometric constraint on at least one of a position or an orientation of the VINS based upon the odometry data, wherein generating the geometric constraint comprises:

computing a motion manifold for the environment, and

using the motion manifold as a cost term to a stochastic cost function to generate a planar-motion constraint; and

limiting the updated state estimates according to constraints including the geometric constraint generated based upon the odometry data.

2. The VINS of claim 1 , wherein the processor is further configured to compute, based on the at least one feature observed from at least one of the plurality of poses and the odometry data, a rotational component and a translational component that represent some of the state estimates.

3. The VINS of claim 1 , wherein the processor is further configured to update the state estimates according to:

an estimate of motion based on the at least one feature observed from at least one of the plurality of poses, and

a covariance indicating an uncertainty of the estimated motion.

4. The VINS of claim 3 , wherein the processor is further configured to:

update the uncertainty of the estimated motion by computing a Hessian matrix, wherein the Hessian matrix represents a subset of the IMU data and the image data along the trajectory, and

update the state estimates based on the Hessian matrix.

5. The VINS of claim 1 , wherein the processor is further configured to classify a first set of features as simultaneous localization and mapping (SLAM) features for computing the state estimates and a second set of features for updating the state estimates.

6. The VINS of claim 1 , wherein the motion manifold is a two-dimensional plane.

7. The VINS of claim 1 , wherein, to limit the updated state estimates according to constraints including the geometric constraint, the processor is further configured to:

compute an optimization to the stochastic cost function that incorporates the motion manifold; and

limit, based on the constraints and the optimization to the stochastic cost function, the updated state estimates.

8. The VINS of claim 1 , wherein, to limit the updated state estimates according to constraints including the geometric constraint, the processor is further configured to:

compute an optimization to a stochastic cost function that incorporates the motion manifold as an additional cost term; and

limit, based on the constraints and the optimization to the stochastic cost function, the state estimates.

9. A method comprising:

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

producing, by an inertial measurement unit (IMU) of the VINS, IMU data indicative of motion of the VINS;

producing, by an odometry unit of the VINS, odometry data for the IMU, the odometry data indicative of scale information, wherein the odometry unit includes at least two wheels and determines odometry data for each of the at least two wheels; and

computing, by a processor communicatively coupled to the at least one image source, the IMU, and the odometry unit and based on the image data, the IMU data, and the odometry data, state estimates for a position and orientation for a plurality of poses of the VINS along the trajectory,

wherein computing the state estimates comprises:

maintaining a state vector for the VINS based upon measurements from the IMU and the at least one image source, where the state vector comprises: state estimates for each of the plurality of poses of the VINS along the trajectory, where each state estimate comprises a position and orientation of the VINS;

estimating an error in the state vector based upon a recent state estimate and the measurements from the IMU and the at least one image source; and

updating the state estimates based on the odometry data and at least one feature observed from at least one of the plurality of poses of the VINS along the trajectory by:

generating a geometric constraint on at least one of a position or an orientation of the VINS based upon the odometry data, wherein generating the geometric constraint comprises:

computing a motion manifold for the environment, and

using the motion manifold as a cost term to a stochastic cost function to generate a planar-motion constraint; and

limiting the updated state estimates according to constraints including the geometric constraint generated based upon the odometry data.

10. The method of claim 9 , further comprising computing, based on the at least one feature observed from at least one of the plurality of poses and the odometry data, a rotational component and a translational component that represent some of the state estimates.

11. The method of claim 9 , further comprising updating each of the state estimates according to:

an estimate of motion based on the at least one feature observed from at least one of the plurality of poses, and

a covariance indicating an uncertainty of the estimated motion.

12. The method of claim 11 , further comprising:

updating the uncertainty of the estimated motion by computing a Hessian matrix, wherein the Hessian matrix represents a subset of the IMU data and the image data along the trajectory, and

updating the state estimates based on the Hessian matrix.

13. The method of claim 9 , further comprising classifying a first set of features as simultaneous localization and mapping (SLAM) features for computing the state estimates and a second set of features for updating the state estimates.

14. The method of claim 9 , wherein the motion manifold is a two-dimensional plane.

15. The method of claim 9 , wherein limiting the updated state estimates according to constraints including the geometric constraint comprises:

computing an optimization to the stochastic cost function that incorporates the motion manifold; and

limiting, based on the constraints and the optimization to the stochastic cost function, the state estimates.

16. A non-transitory, computer-readable medium comprising instructions that, when executed, are configured to cause a processor of a vision-aided inertial navigation system (VINS) to:

receive image data along a trajectory of the VINS within an environment, wherein the image data contains a plurality of features observed within the environment;

receive inertial measurement unit (IMU) data indicative of motion of the VINS;

receive odometry data, the odometry data indicative of scale information and comprising wheel encoder data for at least two wheels of the VINS; and

compute, based on the image data, the IMU data, and the odometry data, state estimates for a position and orientation for a plurality of poses of the VINS along the trajectory,

wherein computing the state estimates comprises:

maintaining a state vector for the VINS based upon the image data and IMU data, where the state vector comprises state estimates for each of the plurality of poses of the VINS along the trajectory, where each state estimate comprises a position and orientation of the VINS;

estimating an error in the state vector based upon a recent state estimate, the image data, and the IMU data; and

updating the state estimates based on the odometry data and at least one feature observed from at least one of the plurality of poses of the VINS along the trajectory by:

generating a geometric constraint on at least one of a position or an orientation of the VINS based upon the odometry data, wherein generating the geometric constraint comprises:

computing a motion manifold for the environment, and

using the motion manifold as a cost term to a stochastic cost function to generate a planar-motion constraint; and

limiting the updated state estimates according to constraints including the geometric constraint generated based upon the odometry data.

17. The non-transitory, computer-readable medium of claim 16 , wherein the processor is further caused to compute, based on the at least one feature observed from at least one of the plurality of poses and the odometry data, a rotational component and a translational component that represent some of the state estimates.

18. The non-transitory, computer-readable medium of claim 16 , wherein the processor is further caused to update each of the state estimates according to:

an estimate of motion based on the at least one feature observed from at least one of the plurality of poses, and

a covariance indicating an uncertainty of the estimated motion.

19. The non-transitory, computer-readable medium of claim 18 , wherein the processor is further caused to:

update the uncertainty of the estimated motion by computing a Hessian matrix, wherein the Hessian matrix represents a subset of the IMU data and the image data along the trajectory, and

update the state estimates based on the Hessian matrix.

20. The non-transitory, computer-readable medium of claim 16 , wherein the processor is further caused to classify a first set of features as simultaneous localization and mapping (SLAM) features for computing the state estimates and a second set of features for updating the state estimates.

Assignments (4)

Continuity (2)

Cited By (9)

US 12,221,119 US 12,284,058 US 12,394,156 US 12,403,601 US 12,442,638 US 12,444,067 US 12,479,099 US 12,630,280 US 12,714,332