Tracking, auto-calibration, and map-building system

• US 20040073360A1
• Filed: 08/11/2003
• Published: 04/15/2004
• Est. Priority Date: 08/09/2002
• Status: Active Grant

• Current Assignee: Thales Visionix Incorporated (Thales SA)
• Original Assignee: Intersense Incorporated
• Inventors: Foxlin, Eric
• Granted Patent: US 6,922,632 B2
• Time in Patent Office: Days
• US Class Current: 701/207
• CPC Class Codes: G01C 21/188 for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
• CPC Class Codes: G01C 25/005 initial alignment, calibration or starting-up of inertial devices

Abstract

A navigation or motion tracking system includes components associated with particular sensors, which are decoupled from a tracking component that takes advantage of information in the sensor measurements. The architecture of this system enables development of sensor-specific components independently of the tracking component, and enables sensors and their associated components to be added or removed without having to re-implement the tracking component. In a software implementation of the system, sensor-specific software components may be dynamically incorporated into the system and the tracking component is then automatically configured to take advantage of measurements from the corresponding sensors without having to modify the tracking component.

Claims

1. A method for tracking an object comprising:

coupling a sensor subsystem to an estimation subsystem, said sensor subsystem enabling measurement related to relative locations or orientations of sensing elements;

accepting configuration data from the sensor subsystem;

configuring the estimation system according to the accepted configuration data;

repeatedly updating a state estimate, including

accepting measurement information from the sensor subsystem, and

updating the state estimate according to the accepted configuration data and the accepted measurement data.

2. The method of claim 1 wherein coupling the sensor subsystem to the estimation subsystem includes coupling software modules each associated with one or more of the sensing elements.

3. The method of claim 2 wherein each of the software modules provides a software interface for receiving information related to an expected sensor measurement and providing measurement information that depends on said received information.

4. The method of claim 3 wherein each of the software modules implements calculations that are independent of a representation of the state in the estimation subsystem.

5. The method of claim 1 wherein the state estimate characterizes an estimate of a location of the object.

6. The method of claim 1 wherein the state estimate characterizes configuration information for one or more sensing elements fixed to the object.

7. The method of claim 6 wherein the configuration information for the one or more sensing elements fixed to the object includes information related to position or orientation of said sensing elements relative to the object.

8. The method of claim 6 wherein the configuration information for the one or more sensing elements fixed to the object includes operational parameters for the one or more sensing elements.

9. The method of claim 1 wherein the state estimate characterizes configuration information for one or more sensing elements fixed in an environment of the object.

10. The method of claim 9 wherein the configuration information for one or more sensing elements fixed in the environment of the object includes a map of the locations of said sensing elements.

11. The method of claim 1 wherein repeatedly updating the state further includes:

providing to the sensor subsystems information related to an expected sensor measurement; and

wherein accepting the measurement information from the sensor subsystem includes accepting information related to an actual sensor measurement.

12. The method of claim 11 wherein providing the information related to an expected sensor measurement includes providing information related to a relative geometric configuration of two of the sensing elements.

13. The method of claim 12 wherein providing information related to a relative geometric configuration of the two of the sensing elements includes providing information characterizing a relative location of said sensing elements.

14. The method of claim 11 wherein accepting the information related to an actual sensor measurement includes accepting information enabling the estimation subsystem to calculate a difference between the actual measurement and the expected measurement.

15. The method of claim 11 wherein accepting the information related to an actual sensor measurement includes accepting information for correlating measurements and geometric relationships between sensing elements.

16. The method of claim 15 wherein the information for correlating measurements and geometric relationships between sensing elements includes a mapping between a relative pose of the sensing elements and a sensor measurement.

17. The method of claim 16 wherein the mapping between the relative pose of the sensing elements and the sensor measurement characterizes a linear mapping.

18. The method of claim 11 wherein accepting the information related to an actual sensor measurement includes accepting information characterizing an uncertainty in the actual measurement.

19. The method of claim 18 wherein the information characterizing the uncertainty in the actual measurement includes parameters of a statistical distribution of an error of the actual measurement.

20. The method of claim 1 wherein repeatedly updating the state further includes:

selecting a pair of sensing elements for measurement; and

providing an identification of the selected pair to the sensing subsystem.

21. The method of claim 20 wherein selecting the pair of sensing elements includes selecting said elements according to an expected utility of a measurement associated with said elements to the updating of the state.

22. The method of claim 11 wherein repeatedly updating the state further includes:

updating the state according to the accepted information related to an actual sensor measurement.

23. The method of claim 20 wherein repeatedly updating the state further includes:

updating the state according to accepted measurements from inertial sensors.

24. The method of claim 1 wherein updating the state estimate includes applying a Kalman Filter approach.

25. The method of claim 1 wherein each of said sensing elements comprises at least one of a sensor and a target.

26. The method of claim 25 wherein the target comprises an active device that interacts with the sensor.

27. The method of claim 26 wherein the target comprises at least one of a man-made signal reflector and a natural feature of an environment.

28. The method of claim 1 wherein the object is selected from a group consisting of a vehicle, a robot, a person, a part of a person, a flying object, a floating object, an underwater moving object, an animal, a camera, a sensing apparatus, a helmet, a tool, a piece of sports equipment, a shoe, a boot, an article of clothing, a personal protective equipment, a rigid object having a dimension between 1 nanometer to 10⁹ meters.

29. The method of claim 1 wherein the state estimate comprises information related to a position or an orientation of the object relative to a reference coordinate frame.

30. A tracking system comprising:

an estimation subsystem; and

a sensor subsystem coupled to the estimation module and configured to provide configuration data to the estimation module and to provide measurement information to the estimation module for localizing an object;

wherein the estimation module is configured to update a location estimate for the object based on configuration data and measurement information accepted from the sensor subsystem.

31. The system of claim 30 wherein the sensor subsystem includes one or more sensor modules, each providing an interface for interacting with a corresponding set of one or more sensing elements.

32. The system of claim 31 wherein the interface enables the sensor module to perform computations independently of an implementation of the estimation subsystem.

33. The system of claim 31 wherein the interface enables the estimation subsystem to perform computations independently of an implementation of the sensor modules.

34. The system of claim 30, further comprising a navigation subsystem to navigate the object in an environment based on the location estimate for the object.

35. A sensor module comprising:

a sensor interface for communicating with a measurement sensor;

a communication interface for communication with an estimation system;

wherein the sensor module is configured to

receive information related to an expected sensor measurement over the communication interface,

receive a measurement signal over the sensor interface,

provide measurement information based on the measurement signal over the communication interface.

36. The sensor module of claim 35 wherein the sensor module is configured to provide information over the communication interface related to an uncertainty in the measurement information.

37. The sensor module of claim 35 wherein the received information related to an expected sensor measurement includes a predicted pose of a sensing element relative to the measurement sensor.

38. A method comprising:

enumerating a set of sensing elements available to a tracking system that includes an estimation subsystem that estimates a position or orientation of an object; and

providing parameters specific to the set of sensing elements to the tracking system to enable the estimation subsystem to be configured based on the parameters specific to the subset of sensing elements.

39. The method of claim 38, further comprising generating a sequence of candidates of pairs of sensing elements selected from the set of sensing elements, the sequence based on an expected utility of a measurement associated with said elements to the estimation subsystem.

40. The method of claim 39, further comprising selecting a pair of sensing elements from the sequence of candidates, the selected pair of sensing elements being ready to make a measurement at the time of selection of the pair or at a predefined time after the time of selection of the pair, the selected pair having a highest expected utility of a measurement among the sequence of candidates.

41. The method of claim 38 wherein the set of sensing elements comprises at least one sensor and at least one target, the sensor making a measurement with respect to the target.

42. The method of claim 41 wherein the target comprises a natural feature in an environment.

43. A method comprising:

computing an estimate of a pose of a target element relative to a sensor element based on an estimate of a pose of a tracked object relative to an environment having affixed thereto either the sensor element or the target element,

the computing of the estimate of the pose of the target element relative to the sensor element further based on an estimate of a pose of the affixed element relative to the tracked object and the other element relative to the environment;

computing an estimate of a measurement of the target made by the sensor based on the estimate of the pose of the target relative to the sensor;

making an actual measurement of the target by using the sensor; and

computing a deviation between the actual measurement and the estimated measurement; and

generating a new estimate of the pose of the tracked object based on the deviation.

44. The method of claim 43, further comprising computing a first observation matrix that characterizes a linearized model of a function relating the measurement made by the sensor to the pose of the target relative to the sensor.

45. The method of claim 44, further comprising computing a second observation matrix that characterizes a linearized model of a function relating the pose of the target relative to the sensor to the estimate of the pose of the tracked object relative to the environment.

46. The method of claim 45, further comprising computing an observation matrix that characterizes a linearized model of a function relating the measurement made by the sensor to the pose of the tracked object relative to the environment by combining the first observation matrix and the second observation matrix.

47. A method comprising:

estimating a first value associated with a pose of a first sensing element relative to a second sensing element, the first sensing element fixed to an environment and the second sensing element fixed to an object being tracked, one of the first and second sensing elements being a sensor and the other being a target;

estimating a second value associated with a pose of the second sensing element relative to the first sensing element;

determining which of the first and second sensing elements is the sensor; and

generating an innovation of a measurement of the target made by the sensor based on the first value when the second sensing element is the sensor.

48. The method of claim 47, further comprising generating the innovation based on the second value when the first sensing element is the sensor.

49. The method of claim 47 wherein estimating the first value and estimating the second value are performed by a process ignorant of which of the first and second sensing elements is a sensor.

50. A method comprising

estimating a calibration parameter of a sensing element that is either a sensor or a target, the sensing element being fixed either to an environment or to an object being tracked;

determining whether the sensing element is the sensor or the target; and

assigning the calibration parameter as a sensor calibration parameter when the sensing element is a sensor, and generating an innovation of a measurement of a target made by the sensing element based in part on the sensor calibration parameter.

51. The method of claim 50, further comprising assigning the calibration parameter as a target calibration parameter when the sensing element is a target, and generating an innovation of a measurement of the sensing element made by a sensor based in part on the target calibration parameter.

52. The method of claim 50 wherein estimating the calibration parameter is performed by a process ignorant of whether the sensing element is a sensor or a target.

53. A method of using multiple sensors in a tracking system comprising:

providing an estimation module;

coupling one or more sensor modules to the estimation module, each associated with a different set of one or more sensors;

configuring the tracking system, including

providing configuration information from each of the sensor modules to the estimation module regarding the characteristics of the sensors associated with the sensor module, and

configuring the estimation module using the provided configuration information;

maintaining estimates of tracking parameters in the estimation module, including repeatedly

passing data based on the estimates of the tracking parameters from the estimation module to one or more of the sensor modules,

receiving from said one or more sensor modules at the estimation module data based on measurements obtained from the associated sensors, and the data passed to the sensor modules, and

combining the data received from said one or more sensor modules and the estimates of the tracking parameters in the estimation module to update the tracking parameters.

54. The method of claim 53 wherein the data passed from the estimation module to one or more of the sensor modules includes an estimate of the pose of a target relative to a sensor that was calculated by the estimation module using an estimate of the pose of a tracked object relative to a frame of reference fixed to an environment.

55. The method of claim 54 wherein the data passed from the estimation module to one or more of the sensor modules does not include the estimate of the pose of the tracked object relative to the frame of reference fixed to the environment.

56. The method of claim 53 wherein providing the estimation module includes providing a module that is configurable to use different sets of sensor modules coupled to it.

57. The method of claim 53 wherein maintaining estimates of the tracking parameters in the estimation module includes using a stochastic model in the estimation module.

58. The method of claim 57 wherein using a stochastic model includes implementing some or all of a Kalman filter in the estimation module.

59. The method of claim 58 wherein implementing some or all of the Kalman filter includes updating error estimates using linearized models of the sensor system.

60. The method of claim 58 wherein implementing some or all of the Kalman filter includes implementing a distributed Kalman filter, wherein each of a plurality of components of the distributed Kalman filter is associated with a different subset of the sensor modules.

61. The method of claim 60 wherein one of the components of the distributed Kalman filter is associated with a subset of sensor modules consisting of sensor modules that are affixed to a tracked object.

62. The method of claim 60 wherein one of the components of the distributed Kalman filter is associated with a subset of sensor modules consisting of sensor modules which are affixed to an environment.

63. The method of claim 60 wherein one of the components of the distributed Kalman filter is not associated with any sensor modules.

64. The method of claim 60 wherein implementing the distributed Kalman filter includes implementing a Federated Kalman Filter.

65. The method of claim 53 wherein providing configuration information from the sensor modules includes providing information characterizing a type of a sensor associated with a sensor module.

66. The method of claim 53 wherein providing configuration information from the sensor modules includes providing information characterizing a position or an orientation of a sensor associated with a sensor module.

67. The method of claim 53 wherein providing configuration information from the sensor modules includes providing information characterizing one or more calibration parameters of a sensor associated with a sensor module.

68. A machine-accessible medium, which when accessed results in a tracking or navigation system that tracks or navigates, respectively, an object, performing operations comprising:

enumerating a set of sensing elements available to the tracking or navigation system, the sensing elements available to the tracking or navigation system including at least one of an inside-out sensor and an outside-in sensor, the inside-out sensor being fixed to the object and makes measurements with respect to a target fixed to an environment, the outside-in sensor being fixed to the environment and makes measurements with respect to a target fixed to the object;

configuring an estimation module of the tracking or navigation system based on an enumeration of the set of sensing elements available to the tracking or navigation system so that the estimation module can process measurement information from either inside-out sensors, outside-in sensors, or a combination of inside-out and outside-in sensors depending on the sensors available; and

repeatedly updating an estimated pose of an object based on measurements from the set of sensing elements available to the tracking or navigation system.

69. The machine-accessible medium of claim 68 in which the sensing elements available to the tracking or navigation system include range sensors, and configuring the estimation module includes configuring the estimation module so that the estimation module can process measurement information from either inside-out sensors, outside-in sensors, range sensors, or any combination of the above sensors.

70. The machine-accessible medium of claim 68 in which the sensing elements available to the tracking or navigation system include inertial sensors, and configuring the estimation module includes configuring the estimation module so that the estimation module can process measurement information from either inside-out sensors, outside-in sensors, inertial sensors, or any combination of the above sensors.

71. The machine-accessible medium of claim 68 in which the sensing elements available to the tracking or navigation system include dead reckoning sensors, and configuring the estimation module includes configuring the estimation module so that the estimation module can process measurement information from either inside-out sensors, outside-in sensors, dead reckoning sensors, or any combination of the above sensors.

72. A method comprising:

receiving sensor configuration information indicating a set of sensing elements available to a tracking or navigation system;

configuring a data processing module of the tracking or navigation system based on the sensor configuration information to selectively perform one of

(a) receiving data from at least one inside-out bearing sensor, and updating an estimated pose of an object based on data received from the inside-out bearing sensor,

(b) receiving data from at least one outside-in bearing sensor, and updating an estimated pose of an object based on data received from the outside-in bearing sensor, and

(c) receiving data from at least one inside-out bearing sensor and at least one outside-in bearing sensor, and updating an estimated pose of an object based on data received from the outside-in bearing sensor and the inside-out bearing sensor.

73. The method of claim 72 further comprising configuring the data processing module to selectively perform one of

(d) receiving data from at least one range sensor, and updating an estimated pose of an object based on data received from the range sensor,

(e) receiving data from at least one range sensor and at least one inside-out bearing sensor, and updating an estimated pose of an object based on data received from the range sensor and the inside-out bearing sensor,

(f) receiving data from at least one range sensor and at least one outside-in bearing sensor, and updating an estimated pose of an object based on data received from the range sensor and the outside-in bearing sensor, and

(g) receiving data from at least one range sensor, at least one outside-in bearing sensor, and at least one inside-out bearing sensor, and updating an estimated pose of an object based on data received from the range sensor, the inside-out bearing sensor, and the outside-in bearing sensor.

74. An apparatus comprising:

an estimation module to estimate a pose of an object based on measurement data from sensing elements, the estimation module configured to enable selective performance of

(a) receiving data from at least one inside-out bearing sensor, and updating an estimated pose of an object based on the data received from the inside-out bearing sensor,

(b) receiving data from at least one outside-in bearing sensor, and updating an estimated pose of an object based on the data received from the outside-in bearing sensor, and

(c) receiving data from at least one inside-out bearing sensor and at least one outside-in bearing sensor, and updating an estimated pose of an object based on the data received from the outside-in bearing sensor and the inside-out bearing sensor.

75. An apparatus comprising:

an estimation module to estimate a pose of an object based on measurement data from sensing elements, the estimation module configured to enable selective performance of one of;

(a) updating an estimate of the position or orientation of the object relative to an environment,

(b) updating an estimate of the position or orientation, relative to the object, of at least one sensing element fixed to the object, and

(c) updating an estimate of the position or orientation, relative to the environment, of at least one sensing element fixed in the environment.