Fusion of sensor and map data using constraint based optimization

• US 9,746,327 B2
• Filed: 06/12/2013
• Issued: 08/29/2017
• 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): Shaawat, Mussa A
• Assistant Examiner(s): JHA, ABDHESH K
• Application Number:
  US13/916,479
• Publication Number:
  US 20130332065A1
• Time in Patent Office: 1,539 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 tracking a subject and updating a path taken by the subject, the method being implemented by a computer that includes a physical processor, the method comprising:

determining a path taken by a subject by obtaining tracking data, from a dead reckoning sensor associated with the subject, the path taken including a series of dead reckoning path points from an initial point of the subject and a current location of the subject and a distance between each set of adjacent dead reckoning path points among the series of dead reckoning path points;

generating convex constraint data, while the subject is traveling the path taken, by obtaining data from at least one of a magnetic field sensor and a ranging sensor, wherein the convex constraint data is associated with at least one error bound; and

applying the tracking data and the convex constraint data in a convex optimization method to update at least a portion of the path taken by the subject and to identify the current location of the subject, wherein the convex optimization method includes defining a convex objective function for one or more parameters associated with one or more of the path taken and the current location and minimizing the convex objective function based on the convex constraint data to adjust the distance for at least one set of adjacent dead reckoning path points.

2. The method of claim 1, wherein the convex constraint data further includes obtaining data from one or more of an accelerometer, a gyroscope, a global positioning system sensor, and a barometric pressure sensor.

3. The method of claim 1, wherein the ranging sensor measures ranging data between one or more of:

the subject and an estimated location;

the subject and a known location; and

the subject and at least one other subject.

4. The method of claim 1, wherein the convex constraint data further includes at least one of pose constraint data and feature data.

5. The method of claim 1, further comprising propagating the convex constraint data by a motion model to produce error bounds that continue to constrain the updated path and the current location over time.

6. The method of claim 5, wherein propagating the convex constraint data is limited by at least one of other sensor data and map structural data.

7. The method of claim 6, wherein the other sensor data includes at least one of an accelerometer, a gyroscope, a global positioning system sensor, a magnetic field sensor, and a barometric pressure sensor.

8. The method of claim 6, wherein the map structural data includes at least one of a wall, a door, a building outline, and a mapped area of restricted access.

9. The method of claim 6, wherein generating the convex constraint data includes using intersections of constraints with spaces that are directly reachable by a straight line or by considering the motion model.

10. The method of claim 6, wherein the one or more parameters include one or more of heading, scale, and drift.

11. The method of claim 1, where the convex optimization method includes solving for at least one of a global offset, a rotation, a drift, and a scale.

12. The method of claim 1, further comprising, after using the convex optimization method, using a local optimization that supports enforcing non-convex constraints.

13. The method of claim 1, further comprising passing the updated path and the current location of the subject to a convex simultaneous localization and mapping algorithm, wherein the convex simultaneous localization and mapping algorithm uses convex optimization to enforce constraints on a dead reckoning track.

14. The method of claim 13, wherein the convex simultaneous localization and mapping algorithm receives a feature that has passed through a loop detector.

15. The method of claim 13, wherein the convex simultaneous localization and mapping algorithm creates an updated feature and pose pair based on at least one of:

the updated path and the current location of the subject,

a historical feature and pose pair that was previously processed by the convex simultaneous localization and mapping algorithm, and

a historical feature and pose pair associated with another tracking device.

16. The method of claim 15, further comprising processing, by a loop-closing detector, the historical feature and pose pair that was previously processed by the convex simultaneous localization and mapping algorithm.

17. The method of claim 1, wherein the convex constraint data further includes at least one of a user correction and a check-in.

18. The method of claim 1, wherein the dead reckoning sensor is at least one of an inertial sensor, an optical flow sensor, and a Doppler velocimeter.

19. A computing system used to track a trackee and updating a path taken by the trackee, the computing system comprising:

a dead reckoning sensor;

a processor in communication with the dead reckoning sensor; and

a memory coupled to the processor, the memory having stored thereon executable instructions that when executed by the processor cause the processor to effectuate operations comprising;

determining a path taken by the trackee by obtaining tracking data, from the dead reckoning sensor, the dead reckoning sensor being associated with the trackee, the path taken including a series of dead reckoning path points from an initial point of the trackee and a current location of the trackee and a distance between each set of adjacent dead reckoning path points among the series of dead reckoning path points;

generating convex constraint data, the trackee while the trackee is traveling the path taken, by obtaining data from at least one of a magnetic sensor and a ranging sensor, wherein the convex constraint data is associated with at least one error bound; and

applying the tracking data and the convex constraint data in a convex optimization method to update at least a portion of the path taken by the trackee and to identify a current location of the trackee, wherein the convex optimization method includes defining a convex objective function for one or more parameters associated with one or more of the path taken and the current location and minimizing the convex objective function based on the convex constraint data to adjust the distance for at least one set of adjacent dead reckoning path points.

20. The computing system of claim 19, wherein the convex constraint data further includes obtaining data from at one or more of an accelerometer, a gyroscope, a global positioning system sensor, and a barometric pressure sensor.

21. The computing system of claim 19, wherein the ranging sensor measures ranging data between one or more of:

the subject and an estimated location;

the subject and a known location; and

the subject and at least one other subject.

22. The computing system of claim 19, wherein the convex constraint data further includes at least one of a pose data and a feature data.

23. The computing system of claim 19, wherein the convex constraint data further includes at least one of a user correction and a check-in.

24. The computing system of claim 19, the instructions further comprising passing the updated path and the current location of the subject to a convex simultaneous localization and mapping algorithm, wherein the convex simultaneous localization and mapping algorithm uses an inertial track and associated error estimates to replace output of a Bayesian filter used in a hierarchical active ripple simultaneous localization and mapping algorithm.

25. The computing system of claim 24, wherein the convex simultaneous localization and mapping algorithm receives a feature that has passed through a loop detector.

26. The computing system of claim 24, wherein the convex simultaneous localization and mapping algorithm creates an updated feature and pose pair based on at least one of:

the updated path and the current location of the subject,

a historical feature and pose pair that was previously processed by the convex simultaneous localization and mapping algorithm, and

a historical feature and pose pair associated with another tracking device.

27. The computing system of claim 19, wherein the dead reckoning sensor is at least one of an inertial sensor, an optical flow sensor, and a Doppler velocimeter.

28. The computing system of claim 19, the instructions further comprising propagating the convex constraint data by a motion model to produce error bounds that continue to constrain the updated path and the current location of the subject over time.

29. The computing system of claim 28, wherein propagating the constraint data is limited by at least one of other sensor data and map structural data.

30. The computing system of claim 29, wherein the other sensor data includes at least one of an accelerometer, a gyroscope, a global positioning system sensor, a magnetic field sensor, and a barometric pressure sensor.

31. The computing system of claim 29, wherein the map structural data includes at least one of a wall, a door, a building outline, and a mapped area of restricted access.

32. The computing system of claim 29, wherein generating the convex constraint data includes using intersections of constraints with spaces that are directly reachable by a straight line or by considering the motion model.

33. The computing system of claim 29, wherein the one or more parameters include one or more of heading, scale, and drift.

34. The computing system of claim 19, where the convex optimization method includes solving for at least one of a global offset, a rotation, a drift, and a scale.

35. The computing system of claim 19, the instructions further comprising, after using the convex optimization method, using a local optimization that supports enforcing non-convex constraints.