Autonomous platform guidance systems with unknown environment mapping
The described positional awareness techniques employing sensory data gathering and analysis hardware with reference to specific example implementations implement improvements in the use of sensors, techniques and hardware design that can enable specific embodiments to find new area to cover by a robot performing an area coverage task of an unexplored area. The sensory data are gathered from an operational camera and one or more auxiliary sensors.
1 . A system for obtaining position features for guiding a mobile platform, the system including:
a mobile platform;
a sensory interface coupled to one or more sensors, including at least some visual sensors and at least some inertial sensors, wherein at least one sensor of the one or more sensors senses one or more of: a position, a motion, or an environment of the mobile platform;
a database comprising feature point data obtained from sets of feature points identified in frames of images previously obtained by the at least some visual sensors;
a processor coupled to the sensory interface, the mobile platform, the database, and a non-transitory computer readable storage medium, wherein the non-transitory computer readable storage medium stores computer instructions, which, when executed, cause the processor to guide and control the mobile platform and further cause the processor to:
determine, based upon a parameter of the one or more sensors, a statistical window of confidence, wherein the statistical window of confidence comprises a threshold number of sensor readings of the one or more sensors per a threshold length of time, wherein the statistical window of confidence provides a statistical degree of confidence in the sensor readings;
obtain sensor readings above the statistical degree of confidence using the statistical window of confidence;
maintain time dependent tracking state data using the sensor readings obtained according to the statistical window of confidence, wherein the time dependent tracking state data includes: (i) 3D coordinate pose data (x, y, and theta) of the mobile platform, and (ii) one or more frames of sensor readings including at least some frames from the at least some visual sensors, and wherein sensor readings can include sensory information received from any one of a plurality of sensors available to the mobile platform;
track a set of inertial readings from the at least some inertial sensors;
compute a variance for each dimension of a total of 3 dimensions of the set of inertial readings;
determine whether the mobile platform is moving or at rest based upon the variance computed; and
(a) when the mobile platform is moving, perform extended Kalman filtering on the time dependent tracking state data and determine pose information from the time dependent tracking state data; and
(b) when the mobile platform is static, provide a computed pose
wherein the processor computes the computed pose by performing at least one of:
(i) propagating, using information obtained from a gyroscopic sensor, a zero velocity applied to location information of a pose and change orientation information of the mobile platform; or
(ii) adding a noise factor to a previous pose.
2 . The system of claim 1 , wherein the processor synchronizes a time associated with one or more time dependent tracking states of the time dependent tracking state data with a time maintained by a controller unit controlling the mobile platform, by updating the time associated with at least one time dependent tracking state by (i) a constant offset and (ii) a transfer delay.
3 . The system of claim 1 , wherein the processor: detects a number of IMU updates occurring after a successful image update; and performs gyroscopic sensor propagation after N consecutive IMU updates are detected.
4 . The system of claim 1 , wherein the processor: detects if one or more visual sensors has failed or is not updating or is insufficiently updating and controls pose estimation using the at least some inertial sensors.
5 . The system of claim 1 , wherein the processor interprets a variance above a threshold as indicating a moving platform and a variance below a threshold as indicating a static platform.
6 . The system of claim 1 , wherein the processor:
calculates a location of one or more 2D features in an image using optical flow;
reduces the one or more 2D features to at least some features that are existing along an epipolar line based upon a location calculated and a prediction of a camera pose;
checks for similarity between descriptors of the at least some features and descriptors for a corresponding feature from a previous frame;
determines whether a number of features through optical flow is less than a number of features detected in a current frame, and when the number of features determined through optical flow is less than the number of features detected in a current frame, detects 2D corner features directly from an image at image regions without features; and
bins the image spatially, and adjusts a number of features in each bin so that the number of features in the bins is balanced.
7 . The system of claim 6 , wherein the processor: links features in different images captured at different times.
8 . The system of claim 6 , wherein the processor: correlates unmatched features for a camera, by:
collecting a set of features that are not matched to any other features by existing links;
collecting a set of available links that do not match to a feature from the camera in a current time frame;
calculating a similarity score between each member in the set of features and each member in the set of available links;
wherein similarity between a feature and a link is a best similarity score between a descriptor of the feature and descriptors of features in the link; and when the similarity score is greater than a threshold, adding the feature to the link.
9 . The system of claim 1 , wherein the processor:
searches in a plurality of maps for a shared view map point using a current frame image and locates a first set of feature points in the database, wherein the first set of feature points is a set of feature points identified in a first frame image associated with a first view map point, and wherein the first set of feature points matches a set of feature points identified in the current frame image;
when a second frame associated with a second set of feature points stored in the database is not associated with a first map of an area on which an operation is conducted:
records an association of the second set of feature points with the first map and determine a relationship between feature points in the first map and corresponding feature points in at least one map of the plurality of maps using a position in one or more other maps of the plurality of maps associated with the second set of feature points;
forms an updated map based on the first map and the at least one map of the plurality of maps using a relationship between feature points in the first map and corresponding feature points in at least one of the plurality of maps; and
guides the mobile platform using the updated map.
10 . A non-transitory computer readable medium having instructions stored thereon, which, when executed, cause a processor to obtain position features for guiding a mobile platform, including causing the processor to:
determine, based upon a parameter of one or more sensors, a statistical window of confidence, wherein the statistical window of confidence comprises a threshold number of sensor readings of the one or more sensors per a threshold length of time, wherein the statistical window of confidence provides a statistical degree of confidence in the sensor readings;
obtain sensor readings above the statistical degree of confidence using the statistical window of confidence;
maintain time dependent tracking state data using the sensor readings obtained according to the statistical window of confidence, wherein the time dependent tracking state data includes: (i) 3D coordinate pose data (x, y, and theta) of the mobile platform, and (ii) one or more frames of sensor readings including at least some frames from one or more visual sensors, and wherein sensor readings can include sensory information received from any one of a plurality of sensors available to the mobile platform;
track a set of inertial readings from at least some inertial sensors;
compute a variance for each dimension of a total of 3 dimensions of the set of inertial readings;
determine whether the mobile platform is moving or at rest based upon the variance computed; and
(a) when the mobile platform is moving, perform extended Kalman filtering on the time dependent tracking state data and determine pose information from the time dependent tracking state data; and
(b) when the mobile platform is static, provide a computed pose
wherein a processor computes the computed pose by performing at least one of:
(i) propagating, using information obtained from a gyroscopic sensor, a zero velocity applied to location information of a pose and change orientation information of the mobile platform; or
(ii) adding a noise factor to a previous pose.
11 . A method of obtaining position features for guiding a mobile platform, including:
determine, based upon a parameter of one or more sensors, a statistical window of confidence, wherein the statistical window of confidence comprises a threshold number of sensor readings of the one or more sensors per a threshold length of time, wherein the statistical window of confidence provides a statistical degree of confidence in the sensor readings;
obtain sensor readings above the statistical degree of confidence using the statistical window of confidence;
maintain time dependent tracking state data using the sensor readings obtained according to the statistical window of confidence, wherein the time dependent tracking state data includes: (i) 3D coordinate pose data (x, y, and theta) of the mobile platform, and (ii) one or more frames of sensor readings including at least some frames from one or more visual sensors, and wherein sensor readings can include sensory information received from any one of a plurality of sensors available to the mobile platform;
track a set of inertial readings from at least some inertial sensors;
compute a variance for each dimension of a total of 3 dimensions of the set of inertial readings;
determine whether the mobile platform is moving or at rest based upon the variance computed; and
(a) when the mobile platform is moving, perform extended Kalman filtering on the time dependent tracking state data and determine pose information from the time dependent tracking state data; and
(b) when the mobile platform is static, provide a computed pose
wherein the computed pose is computed by performing at least one of:
(i) propagating, using information obtained from a gyroscopic sensor, a zero velocity applied to location information of a pose and change orientation information of the mobile platform; or
(ii) adding a noise factor to a previous pose.
12 . The method of claim 11 , further including: synchronizing a time associated with one or more time dependent tracking states of the time dependent tracking state data with a time maintained by a controller unit controlling the mobile platform, by updating the time associated with at least one time dependent tracking state by (i) a constant offset and (ii) a transfer delay.
13 . The method of claim 11 , further including: detecting a number of IMU updates occurring after a successful image update; and performing gyroscopic sensor propagation after N consecutive IMU updates are detected.
14 . The method of claim 11 , further including: detecting if one or more visual sensors has failed or is not updating or is insufficiently updating and controls pose estimation using at least some inertial sensors.
15 . The method of claim 11 , further including: interpreting a variance above a threshold as indicating a moving platform and a variance below a threshold as indicating a static platform.
16 . The method of claim 11 , further including:
calculating a location of one or more 2D features in an image using optical flow;
reducing the one or more 2D features to at least some features that are existing along an epipolar line based upon a location calculated and a prediction of a camera pose;
checking for similarity between descriptors of the at least some features and descriptors for a corresponding feature from a previous frame;
determining whether number of features through optical flow is less than a number of features detected in a current frame, and when the number of features determined through optical flow is less than the number of features detected in a current frame, detects 2D corner features directly from an image at image regions without features; and
bining the image spatially and adjusting a number of features in each bin so that the number of features in the bins is balanced.
17 . The method of claim 16 , further including: linking features in different images captured at different times.
18 . The method of claim 16 , further including: correlating unmatched features for a camera, by:
collecting a set of features that are not matched to any other features by existing links;
collecting a set of available links that do not match to a feature from the camera in a current time frame;
calculating a similarity score between each member in the set of features and each member in the set of available links;
wherein similarity between a feature and a link is a best similarity score between a descriptor of the feature and descriptors of features in the link; and when the similarity score is greater than a threshold, adding the feature to the link.