Fusion of sensor and map data using constraint based optimization

• US 10,571,270 B2
• Filed: 08/07/2017
• Issued: 02/25/2020
• Est. Priority Date: 06/12/2012
• Status: Active Grant

• Current Assignee: TRX Systems, Inc.
• Original Assignee: TRX Systems, Inc.
• Inventors: Hakim, Daniel, Giles, Christopher, Karvounis, John, Funk, Benjamin, Napora, Jared, Teolis, Carole
• Primary Examiner(s): Jha, Abdhesh K
• Application Number:
  US15/670,881
• Publication Number:
  US 20170363426A1
• Time in Patent Office: 932 Days
• Field of Search: None
• CPC Class Codes: G01C 21/12 executed aboard the object being navigated; Dead reckoning
• CPC Class Codes: G01C 21/1652 with ranging devices, e.g. LIDAR or RADAR
• CPC Class Codes: G01C 21/1654 with electromagnetic compass
• CPC Class Codes: G01C 21/206 specially adapted for indoor navigation
• CPC Class Codes: G01S 5/013 Identifying areas in a building
• CPC Class Codes: G01S 5/0264 at least one of the systems being a non-radio wave positioning system
• CPC Class Codes: H04W 64/00 Locating users or terminals or network equipment for network management purposes, e.g. mobility management

Abstract

Disclosed herein are methods and systems for fusion of sensor and map data using constraint based optimization. In an embodiment, a computer-implemented method may include obtaining tracking data for a tracked subject, the tracking data including data from a dead reckoning sensor; obtaining constraint data for the tracked subject; and using a convex optimization method based on the tracking data and the constraint data to obtain a navigation solution. The navigation solution may be a path and the method may further include propagating the constraint data by a motion model to produce error bounds that continue to constrain the path over time. The propagation of the constraint data may be limited by other sensor data and/or map structural data.

Claims

1. A computer-implemented method of simultaneously tracking a tracked device and determining a location of features, comprising:

obtaining a sensor data from the tracked device, the sensor data including a location data and a dead reckoning sensor data;

determining a dead reckoning path data based on the sensor data;

detecting one or more features based on the sensor data;

detecting a location for each feature based on the dead reckoning path data;

determining at least one constraint based on the sensor data; and

optimizing the dead reckoning path data and updating the location of each feature using a convex simultaneous location and mapping algorithm comprising;

defining a convex objective function for the dead reckoning path data;

minimizing the convex objective function based on the at least one constraint;

applying the convex objective function to update the dead reckoning path data; and

updating the location of each feature based on the updated dead reckoning path data.

2. The method of claim 1, wherein the sensor data further comprises one or more of inertial sensor data, gyroscope data, magnetic sensor data, barometric pressure sensor data, and ranging data.

3. The method of claim 1, wherein the dead reckoning path data comprises one or more of an offset, a rotation, a scale, and a drift for one or more path segments.

4. The method of claim 1, wherein the one or more features include one or more of a stairwell, an elevator, a hallway, and a building feature.

5. The method of claim 1, wherein the at least one constraint is based on historical data.

6. The method of claim 5, wherein the historical data comprises one or more of a previously detected feature, dead reckoning path data from the tracked device, dead reckoning path data from another tracked device, and a prior constraint.

7. The method of claim 1, wherein the at least one constraint is one or more of an error bound on a location, a drift, a heading, and a scale of one or more segments of the dead reckoning path data.

8. The method of claim 7, wherein the location error bound is based on one or more of a radiofrequency (RF) range, a GPS constraint, a detected feature, and a prior constraint.

9. The method of claim 1, wherein optimizing the dead reckoning path data further comprises removing an outlier in the sensor data.

10. The method of claim 9, wherein the outlier is detected by a medium absolute deviation from the at least one constraint.

11. The method of claim 1, wherein optimizing the dead reckoning path data further comprises removing the at least one constraint based on one or more of an outlier in the sensor data, a sensor error, and a computational performance measure.

12. The method of claim 1, wherein the at least one constraint is iteratively enforced when there is more than one constraint.

13. A computer readable storage medium comprising instructions that, when executed on a computing system to simultaneously track a tracked device and determine a location of features, cause the computing system to at least:

obtain a sensor data from the tracked device, the sensor data comprising a location data and a dead reckoning sensor data;

determine a dead reckoning path data based on the sensor data;

detect one or more features based on the sensor data detect a location for each feature based on the dead reckoning path data;

determine at least one constraint based on the sensor data; and

optimize the dead reckoning path data and update the location of each feature using a convex simultaneous location and mapping algorithm comprising instructions that further cause the computing system to at least;

define a convex objective function for the dead reckoning path data;

minimize the convex objective function based on the at least one constraint;

apply the convex objective function to update the dead reckoning path data; and

update the location of each feature based on the updated dead reckoning path data.

14. The computer readable storage medium of claim 13, further comprising instructions that cause the computing system to at least:

store, in a memory, the optimized dead reckoning path data, including the at least one constraint and one or more features, for use in a subsequent convex simultaneous location and mapping algorithm.

15. The computer readable storage medium of claim 13, wherein the sensor data further comprises one or more of inertial sensor data, gyroscope data, magnetic sensor data, barometric pressure sensor data, and ranging data.

16. The computer readable storage medium of claim 13, wherein the dead reckoning path data comprises one or more of an offset, a rotation, a scale, and a drift for one or more path segments.

17. The computer readable storage medium of claim 13, wherein the at least one constraint is based on historical data comprising one or more of a previously detected feature, dead reckoning path data from the tracked device, dead reckoning path data from another tracked device, and a prior constraint.

18. The computer readable storage medium of claim 17, wherein the at least one constraint is an error bound on a location, a drift, a heading, and a scale of one or more segments of the dead reckoning path data.

19. The computer readable storage medium of claim 18, wherein the location error bound is based on one or more of a radiofrequency (RF) range, a GPS constraint, a detected feature, and a prior constraint.

20. The computer readable storage medium of claim 13, wherein the instructions that optimize the dead reckoning path data further cause the computing system to at least:

remove an outlier in the sensor data.

21. The computer readable storage medium of claim 20, wherein the outlier is detected by a medium absolute deviation from the at least one constraint.

22. The computer readable storage medium of claim 13, wherein the instructions that optimize the dead reckoning path data further cause the computing system to at least:

remove the at least one constraint based on one or more of an outlier in the sensor data, a sensor error, and a computational performance measure.

23. The computer readable storage medium of claim 13, wherein the instructions that optimize the dead reckoning path data further cause the computing system to at least:

apply a particle solver to resolve the at least one constraint.

24. A computing system for simultaneously tracking a tracked device and determining a location of features comprising:

a processor;

a memory communicatively coupled to the processor, the memory bearing instructions that, when executed on the processor, cause the computing system to at least;

obtain a sensor data from the tracked device, the sensor data comprising a location data and a dead reckoning sensor data;

determine a dead reckoning path data based on the sensor data;

detect one or more features based on the sensor data;

detect a location for each feature based on the dead reckoning path data;

determine at least one constraint based on the sensor data; and

optimize the dead reckoning path data and update the location of each feature using a convex simultaneous location and mapping algorithm comprising instructions that further cause the computing system to;

define a convex objective function for the dead reckoning path data;

minimize the convex objective function based on the at least one constraint;

apply the convex objective function to update the dead reckoning path data; and

update the location of each feature based on the updated dead reckoning path data.

25. The computing system of claim 24, wherein the sensor data further comprises one or more of inertial sensor data, gyroscope data, magnetic sensor data, barometric pressure sensor data, and ranging data.

26. The computing system of claim 24, wherein the dead reckoning path data comprises one or more of an offset, a rotation, a scale, and a drift for one or more path segments.

27. The computing system of claim 24, wherein the at least one constraint is based on historical data comprising one or more of a previously detected feature, dead reckoning path data from the tracked device, dead reckoning path data from another tracked device, and a prior constraint.

28. The computing system of claim 27, wherein the at least one constraint is one or more of an error bound on a location, a drift, a heading, and a scale of one or more segments of the dead reckoning path data.

29. The computing system of claim 28, wherein the location error bound is based on one or more of a radiofrequency (RF) range, a GPS constraint, a detected feature, and a prior constraint.

30. The computing system of claim 24, wherein the instructions that optimize the dead reckoning path data, when executed on the processor, further cause the computing system to at least:

remove an outlier in the sensor data.

31. The computing system of claim 24, wherein the instructions that remove the at least one constraint is based on one or more of an outlier in the sensor data, a sensor error, and a computational performance measure.