US 11,940,277 B2
Vision-aided inertial navigation system for ground vehicle localization
Stergios I. Roumeliotis, St Paul, MN (US); Kejian J. Wu, Minneapolis, MN (US); Chao Guo, Minneapolis, MN (US); and Georgios Georgiou, San Francisco, CA (US)
Assigned to Regents of the University of Minnesota, Minneapolis, MN (US)
Filed by Regents of the University of Minnesota, Minneapolis, MN (US)
Filed on May 29, 2019, as Appl. No. 16/425,422.
Claims priority of provisional application 62/677,438, filed on May 29, 2018.
Prior Publication US 2019/0368879 A1, Dec. 5, 2019
Int. Cl. G01C 21/00 (2006.01); G01C 21/16 (2006.01); G01C 21/28 (2006.01); G06T 7/246 (2017.01); G06T 7/277 (2017.01); G06T 7/73 (2017.01); G07C 5/08 (2006.01)
CPC G01C 21/1656 (2020.08) [G01C 21/28 (2013.01); G06T 7/246 (2017.01); G06T 7/277 (2017.01); G06T 7/73 (2017.01); G07C 5/08 (2013.01)] 20 Claims
OG exemplary drawing
 
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.