Fusion Of Sensor And Map Data Using A Contraint Based Optimization

• US 20130332065A1
• Filed: 06/12/2013
• Published: 12/12/2013
• Est. Priority Date: 06/12/2012
• Status: Active Grant

• Current Assignee: TRX Systems, Inc.
• Original Assignee: TRX Systems, Inc.
• Inventors: Hakim, Daniel, Karvounis, John, Funk, Benjamin, Napora, Jared, Teolis, Carole, Giles, Christopher
• Granted Patent: US 9,746,327 B2
• Time in Patent Office: Days
• US Class Current: 701/411
• 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 trackee, the method being implemented by a computer that includes a physical processor, the method comprising:

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.

2. The method of claim 1, wherein the constraint data includes data from a first sensor.

3. The method of claim 2, wherein the first sensor includes at least one of an accelerometer, a gyroscope, a global positioning system sensor, a magnetic field sensor, a barometric pressure sensor, and a ranging sensor.

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

5. The method of claim 1, wherein the navigation solution is a path, further comprising propagating the constraint data by a motion model to produce error bounds that continue to constrain the path over time.

6. The method of claim 5 wherein propagating the 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 obtaining the 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, further comprising constraining heading growth and scale growth based on the other sensor data.

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 navigation solution to a convex simultaneous localization and mapping algorithm, wherein the convex simultaneous localization and mapping algorithm uses convex optimization to enforce constraints on the 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 navigation solution,

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 constraint data 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, and optical flow sensor, and a Doppler velocimeter.

19. A computing system used to track a 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;

obtaining tracking data for a tracked subject, the tracking data including data from the 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.

20. The computing system of claim 19, wherein the constraint data includes data from a first sensor.

21. The computing system of claim 20, wherein the first sensor is one of an accelerometer, a gyroscope, a global positioning system sensor, a magnetic field sensor, a barometric pressure sensor, and a ranging sensor.

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

23. The computing system of claim 19, wherein the constraint data 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 navigation solution 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 navigation solution,

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, and optical flow sensor, and a Doppler velocimeter.

28. The computing system of claim 19, wherein the navigation solution is a path, the instructions further comprising propagating the constraint data by a motion model to produce error bounds that continue to constrain the path 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 obtaining the 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, the instructions further comprising constraining heading growth and scale growth based on the other sensor data.

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.