Fusion of sensor and map data using constraint based optimization

Patent metadata

Field	Value
Publication number	US-9746327-B2
Application number	US-201313916479-A
Country	US
Kind code	B2
Filing date	Jun 12, 2013
Priority date	Jun 12, 2012
Publication date	Aug 29, 2017
Grant date	Aug 29, 2017

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.

First claim

What is claimed:

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 fi

Assignees

• Trx Systems Inc

Inventors

• Hakim Daniel
• Giles Christopher
• Karvounis John
• Funk Benjamin
• Napora Jared
• Teolis Carole

Classifications

• G01C21/12 Primary

  executed aboard the object being navigated; Dead reckoning · CPC title

• H04W64/00 Primary

  Locating users or terminals {or network equipment} for network management purposes, e.g. mobility management · CPC title

• G01C21/206

  specially adapted for indoor navigation · CPC title

• G01C21/1654

  with electromagnetic compass · CPC title

• G01C21/1652

  with ranging devices, e.g. LIDAR or RADAR · CPC title