System and method for improved simultaneous localization and mapping

• US 9,651,388 B1
• Filed: 10/25/2007
• Issued: 05/16/2017
• Est. Priority Date: 10/25/2007
• Status: Active Grant

• Current Assignee: Rockwell Collins, Inc. (Raytheon Technologies Corporation d/b/a RTX)
• Original Assignee: Rockwell Collins, Inc. (Raytheon Technologies Corporation d/b/a RTX)
• Inventors: Chapman, Mark D., Hamilton, John S., Hight, Dalayr W.
• Primary Examiner(s): Zeender, Florian
• Assistant Examiner(s): Haider, Fawaad
• Application Number:
  US11/977,721
• Time in Patent Office: 3,491 Days
• Field of Search: 701200
• CPC Class Codes: G01C 21/1656 with passive imaging devices, e.g. cameras
• CPC Class Codes: G01C 21/20 Instruments for performing navigational calculations G01C21/24, G01C21/26 take precedence
• CPC Class Codes: G01C 21/26 specially adapted for navigation in a road network
• CPC Class Codes: G01C 21/30 Map- or contour-matching
• CPC Class Codes: G01C 21/3848 Data obtained from both position sensors and additional sensors
• CPC Class Codes: G01S 13/08 Systems for measuring distance only indirect measurement G01S13/46
• CPC Class Codes: G01S 13/931 of land vehicles
• CPC Class Codes: G01S 17/931 of land vehicles
• CPC Class Codes: G01S 19/05 providing aiding data
• CPC Class Codes: G01S 19/14 specially adapted for specific applications
• CPC Class Codes: G01S 19/45 by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
• CPC Class Codes: G01S 19/48 by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
• CPC Class Codes: G06T 2207/30252 Vehicle exterior; Vicinity of vehicle
• CPC Class Codes: G06T 7/246 using feature-based methods, e.g. the tracking of corners or segments
• CPC Class Codes: G06T 7/70 Determining position or orientation of objects or cameras camera calibration G06T7/80

Abstract

A system and method for simultaneous localization and mapping (SLAM), comprising an improved Geometric Dilution of Precision (GDOP) calculation, a reduced set of feature landmarks, the use of Inertial Measurement Units (IMU) to detect measurement motion, and the use of one-time use features and absolute reference landmarks.

Claims

1. A system comprising:

a first sensor configured to collect data for Simultaneous Localization And Mapping (SLAM) comprising angular measurements and delta parallax measurements associated with a plurality of features as measured with respect to an input/output device;

a second sensor configured to detect motion of the first sensor; and

a processor in communication with the first sensor and the second sensor and configured to;

sort the collected data per region of a vision field of the input/output device according to the angular measurements and according to the delta parallax measurements, the vision field comprising a plurality of regions;

select a feature of the plurality of features from a two-dimensional database corresponding to a respective region to provide a selection of dispersed and unique feature candidates, the two-dimensional database storing the sorted data;

generate a position of the first sensor based on the selection of dispersed and unique feature candidates;

determine a navigational solution for the system based on the position of the first sensor and the motion detection of the second sensor; and

provide the navigational solution to a display of the input/output (I/O) device.

2. The system according to claim 1, wherein the navigational solution for the system comprises a position or an orientation of the system.

3. The system according to claim 1, wherein the first sensor is a non-Inertial Measurement Unit (non-IMU) sensor and the second sensor is an IMU sensor, and wherein the plurality of features are recognized by the system to associate the plurality of features with a plurality of landmarks.

4. The system according to claim 1, wherein the collected data is sorted according to the delta parallax measurements when the angular measurements of the respective region are equal, the angular measurements are measured from a central origin of the field of vision, the delta parallax is a parallax between two points, and the parallax is an angle with angle-error.

5. The system according to claim 4, wherein each region is divided into a plurality of sub-regions, the respective region being sorted according to the angular measurements and a respective sub-region being sorted according to the delta parallax measurements.

6. The system according to claim 1, wherein the plurality of features comprise a primary feature set and a secondary feature set, the primary feature set and the secondary feature set comprising a smaller feature set than a feature set comprising all the features in the data collected by the first sensor.

7. The system according to claim 6, wherein the processor is further configured to select the feature of the plurality of features from the two-dimensional database corresponding to the respective region by selecting the feature from the primary feature set or the secondary feature set, and wherein the selected feature is unavailable for a second selection from a second database corresponding to a second respective region.

8. The system according to claim 6, wherein the primary feature set comprises features located at extreme points of the field of vision of the camera, the extreme points comprising apex points and horizon points, and wherein the secondary feature set comprises features located intermediate to the primary feature set.

9. The system according to claim 6, wherein the processor is further configured to:

calculate a Geometric Dilution of Precision (GDOP) number based on features from the primary feature set and the secondary feature set, the GDOP number implemented to determine a minimum number of features required to provide the navigational solution.

10. The system according to claim 1, wherein the plurality of features comprise one or more absolute reference landmarks from GPS satellite measurements.

11. The system according to claim 2, wherein the plurality of features comprise a plurality of stationary features that are stationary with respect to an environment of the system and further comprise a one-use feature with an associated time tag.

12. The system according to claim 11, wherein the plurality of stationary features comprise a door or a window, and wherein the one-use feature is further associated with a circular error of probability (CEP), and wherein the navigational solution further comprises a re-orientation based on the CEP of the one-use feature.

13. The system according to claim 2, wherein the processor is further configured to implement an Extended Kalman Filter (EKF) sub-system, the EKF sub-system communicatively coupled to receive the selection of dispersed and unique feature candidates to provide an update for the navigational solution.

14. A method for determining a navigational solution for a simultaneous location and mapping (SLAM) system comprising:

collecting angular measurements and delta parallax measurements corresponding to a plurality of features with a first sensor;

detecting motion of the first sensor with a second sensor;

sorting the collected data of the first sensor into an array per region of a vision field according to the angular measurements and the delta parallax measurements, the vision field comprising a plurality of regions;

selecting a feature candidate from a two-dimensional database corresponding to a respective region to provide a selection of dispersed and unique feature candidates, the two-dimensional database storing the sorted data;

generating a position of the first sensor based on a plurality of selected feature candidates;

determine a navigational solution for the system based on the position of the first sensor and the motion detected by the second sensor; and

provide the navigational solution to an input/output (I/O) device.

15. The method according to claim 14, wherein the collected data is sorted according to the delta parallax measurements when two or more features of the respective region share angular measurements, wherein the angular measurements and the delta parallax measurements are collected using a non-Inertial Measurement Unit (non-IMU) sensor and the motion of the first sensor is detected using an IMU sensor, and wherein the plurality of features are recognized by the system to associate the plurality of features with a plurality of landmarks.

16. The method according to claim 15, further comprising:

dividing the respective region into a plurality of sub-regions, the respective region being sorted according to the angular measurements and a respective sub-region being sorted according to the delta parallax measurements, wherein the angular measurements are Cartesian measurements with an X component and a Y component, and wherein sorting according to the angular measurements further comprises sorting the plurality of features according to maximum angular distance in the X component and according to the maximum angular distance in the Y component.

17. The method according to claim 14, wherein the plurality of features comprise a primary feature set and a secondary feature set, the primary feature set and the secondary feature set comprising a smaller feature set than a feature set comprising all the features in the data collected by the first sensor.

18. The method according to claim 17, further comprising:

calculating a Geometric Dilution of Precision (GDOP) number based on features from the primary feature set and the secondary feature set, the GDOP number implemented to determine a minimum number of features required to provide the navigational solution.

19. The method according to claim 14, wherein determine the navigational solution for the system based on the position of the first sensor and the motion detected by the second sensor comprises determining a camera pointing angle, wherein the method further comprises:

updating the camera pointing angle with an Extended Kalman Filter (EKF) based on the position of the first sensor and the motion detected by the second sensor.

20. The method according to claim 14, further comprising:

providing a cut-off range for the first sensor to stop the collecting angular measurements and delta parallax measurements corresponding to features of the plurality of features that are outside the cut-off range;

removing features from a respective database that are outside the cut-off range of the first sensor; and

initiating a new feature search to collect second angular measurements and second delta parallax measurements corresponding to a second plurality of features with the first sensor, wherein detecting motion of the first sensor with the second sensor comprises detecting motion only between feature searches.