Vision-aided inertial navigation
First Claim
Patent Images
1. A vision-aided inertial navigation system comprising:
- at least one image source to produce image data for a plurality of poses of a frame of reference along a trajectory within an environment over a period of time, wherein the image data includes features that were each observed within the environment at poses of the frame of reference along the trajectory, wherein one or more of the features were each observed at multiple ones of the poses of the frame of reference along the trajectory;
a motion sensor configured to provide motion data of the frame of reference in the environment for the period of time; and
a hardware-based processor communicatively coupled to the image source and communicatively coupled to the motion sensor, the processor configured to compute estimates for at least a position and orientation of the frame of reference for each of the plurality of poses of the frame of reference along the trajectory,wherein the processor is configured to;
determine, from the image data, feature measurements corresponding to the features observed from the poses along the trajectory;
group the feature measurements according to the features observed within the image data;
for one or more of the features observed from multiple poses along the trajectory, compute based on the respective group of feature measurements for the feature, one or more constraints that geometrically relate the multiple poses from which the respective feature was observed; and
determine the position and orientation of the frame of reference for each of the plurality of poses along the trajectory by updating, in accordance with the motion data and the one or more computed constraints, state information within a state vector representing estimates for the position and orientation of the frame of reference along the trajectory while excluding, from the state vector, state information representing estimates for positions within the environment for the features that were each observed from the multiple poses and for which the one or more constraints were computed.
2 Assignments
0 Petitions
Accused Products
Abstract
This document discloses, among other things, a system and method for implementing an algorithm to determine pose, velocity, acceleration or other navigation information using feature tracking data. The algorithm has computational complexity that is linear with the number of features tracked.
80 Citations
21 Claims
-
1. A vision-aided inertial navigation system comprising:
-
at least one image source to produce image data for a plurality of poses of a frame of reference along a trajectory within an environment over a period of time, wherein the image data includes features that were each observed within the environment at poses of the frame of reference along the trajectory, wherein one or more of the features were each observed at multiple ones of the poses of the frame of reference along the trajectory; a motion sensor configured to provide motion data of the frame of reference in the environment for the period of time; and a hardware-based processor communicatively coupled to the image source and communicatively coupled to the motion sensor, the processor configured to compute estimates for at least a position and orientation of the frame of reference for each of the plurality of poses of the frame of reference along the trajectory, wherein the processor is configured to; determine, from the image data, feature measurements corresponding to the features observed from the poses along the trajectory; group the feature measurements according to the features observed within the image data; for one or more of the features observed from multiple poses along the trajectory, compute based on the respective group of feature measurements for the feature, one or more constraints that geometrically relate the multiple poses from which the respective feature was observed; and determine the position and orientation of the frame of reference for each of the plurality of poses along the trajectory by updating, in accordance with the motion data and the one or more computed constraints, state information within a state vector representing estimates for the position and orientation of the frame of reference along the trajectory while excluding, from the state vector, state information representing estimates for positions within the environment for the features that were each observed from the multiple poses and for which the one or more constraints were computed. - View Dependent Claims (2, 3, 4, 5, 6, 20)
-
-
7. A method comprising:
-
receiving, with a processor and from at least one image source communicatively coupled to the processor, image data for a plurality of poses of a frame of reference along a trajectory within an environment over a period of time, wherein the image data includes features that were each observed within the environment at poses of the frame of reference along the trajectory, wherein one or more of the features were each observed at multiple ones of the poses of the frame of reference along the trajectory; receiving, with the processor and from a motion sensor communicatively coupled to the processor, motion data of the frame of reference in the environment for the period of time; computing, with the processor, state estimates for at least a position and orientation of the frame of reference for each of the plurality of poses of the frame of reference along the trajectory, wherein computing the state estimates comprises; determining, from the image data, feature measurements corresponding to the features observed from the poses along the trajectory; grouping the feature measurements according to the features observed within the image data; for one or more of the features observed from multiple poses along the trajectory, computing, based on the respective group of feature measurements for the feature, one or more constraints that geometrically relate the multiple poses from which the respective feature was observed; and determining the position and orientation of the frame of reference for each of the plurality of poses along the trajectory by updating, in accordance with the motion data and the one or more computed constraints, state information within a state vector representing estimates for the position and orientation of the frame of reference along the trajectory while excluding, from the state vector, state information representing estimates for positions within the environment for the features that were each observed from the multiple poses and for which the one or more constraints were computed; and controlling, responsive to the computed state estimates, navigation of the frame of reference. - View Dependent Claims (8, 9, 10, 11, 12, 21)
-
-
13. A non-transitory computer-readable storage medium comprising instructions that configure a processor to:
-
receive, with the processor and from at least one image source communicatively coupled to the processor, image data for a plurality of poses of a frame of reference along a trajectory within an environment over a period of time, wherein the image data includes features that were each observed within the environment at poses of the frame of reference along the trajectory, wherein one or more of the features were each observed at multiple ones of the poses of the frame of reference along the trajectory; receive, with the processor and from a motion sensor communicatively coupled to the processor, motion data of the frame of reference in the environment for the period of time; determine, from the image data, feature measurements corresponding to the features observed from the poses along the trajectory; group the feature measurements according to the features observed within the image data; for one or more of the features observed from multiple poses along the trajectory, compute, based on the respective group of feature measurements for the feature, one or more constraints that geometrically relate the multiple poses from which the respective feature was observed; determine state estimates for at least a position and an orientation of the frame of reference for each of the plurality of poses along the trajectory by updating, in accordance with the motion data and the one or more computed constraints, state information within a state vector representing estimates for the position and orientation of the frame of reference along the trajectory while excluding, from the state vector, state estimates for positions within the environment for the features that were each observed from the multiple poses and for which the one or more constraints were computed; and output, for display, information responsive to the computed state estimates for the frame of reference. - View Dependent Claims (14, 15, 16, 17, 18, 19)
-
Specification