Method and apparatus for motion tracking of an articulated rigid body

• US 20020103610A1
• Filed: 10/30/2001
• Published: 08/01/2002
• Est. Priority Date: 10/30/2000
• Status: Active Grant

• Current Assignee: United States Secretary of the Navy
• Original Assignee: Government of the United States of America
• Inventors: Zyda, Michael J., McGhee, Robert B., Bachmann, Eric R., McKinney, Douglas L., Yun, Xiaoping
• Granted Patent: US 6,820,025 B2
• Time in Patent Office: Days
• US Class Current: 702/94
• CPC Class Codes: A61B 2562/0219 Inertial sensors, e.g. accelerometers, gyroscopes, tilt switches
• CPC Class Codes: A61B 5/1114 Tracking parts of the body
• CPC Class Codes: A61B 5/744 Displaying an avatar, e.g. an animated cartoon character
• CPC Class Codes: G01C 17/38 Testing, calibrating, or compensating of compasses
• CPC Class Codes: G01C 21/1654 with electromagnetic compass
• CPC Class Codes: G01C 9/00 Measuring inclination, e.g. by clinometers, by levels
• CPC Class Codes: G06F 3/011 Arrangements for interaction with the human body, e.g. for user immersion in virtual reality blind teaching G09B21/00

Abstract

One embodiment the invention comprises a method of determining an orientation of a sensor. The method includes measuring a local magnetic field vector and a local gravity vector and using those measurements to determine the orientation of the sensor. Embodiments can include measuring the magnetic field vector and the local gravity vector using quaternion coordinates.

Another embodiment comprises measuring a local magnetic field vector, a local gravity vector, and the angular velocity of the sensor. These three vectors are processed to determine the orientation of the sensor. In one embodiment the three vectors can all be measured in quaternion coordinates.

Another method embodiment comprises determining a local gravity vector by providing a acceleration detector, moving the detector from a start point to an end point over a time period, and summing acceleration measurements over the time period. The local gravity vector is calculated using the summed acceleration measurements.

A system embodiment of the present invention includes a body having mounted thereon at least one sensor. The at least one sensor is configured to output orientation information to at least one processing unit that inputs the orientation information into a synthetic environment. The system also can include a display for displaying the orientation of the body with respect to the synthetic environment.

Claims

1. A method of tracking the orientation of a sensor, the method comprising:

a) measuring an angular velocity of the sensor to generate angular rate values;

b) integrating the angular rate values;

c) normalizing the integrated angular rate values to produce an estimate of sensor orientation;

d) measuring a magnetic field vector to generate local magnetic field vector values;

e) measuring an acceleration vector to generate local gravity vector values; and

f) correcting the estimate of sensor orientation using the local magnetic field vector and local gravity vector.

2. A method of tracking as in claim 1 wherein correcting the estimate of sensor orientation using the local magnetic field vector and local gravity vector comprises:

g) determining a measurement vector from the local magnetic field vector values and the local gravity vector values;

h) calculating a computed measurement vector from the estimate of sensor orientation;

i) comparing the measurement vector with the computed measurement vector to generate an error vector that defines a criterion function;

j) performing a mathematical operation that results in the minimization of the criterion function and outputs an error estimate;

k) integrating the error estimate;

l) normalizing the integrated error estimate to produce a new estimate of sensor orientation; and

m) repeating steps a)-m), wherein the new estimate of sensor orientation is used for h), calculating a computed measurement vector until tracking is no longer desired.

3. The method of claim 2 wherein the operation of j), performing a mathematical operation that results in the minimization of the criterion function comprises minimizing the criterion function without calculating the criterion function.

4. The method of claim 2 wherein the operation of j), performing a mathematical operation that results in the minimization of the criterion function includes implementing a partial correction step to compensate for measurement error.

5. The method of claim 4 wherein implementing the partial correction step to compensate for measurement error is supplemented by using a weighted least squares regression to emphasize more reliable measurements with respect to less reliable measurements.

6. The method of claim 2 wherein the operation of j), performing a mathematical operation that results in the minimization of the criterion function comprises using time weighted filtering.

7. The method of claim 2 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function comprises using a Gauss-Newton iteration.

8. A method of tracking the orientation of a sensor, the method comprising:

a) measuring an angular velocity of the sensor to generate an angular rate quaternion;

b) integrating the angular rate quaternion;

c) normalizing the integrated angular rate quaternion to produce an estimated sensor orientation quaternion; and

d) measuring a magnetic field vector to generate local magnetic field vector values;

e) measuring an acceleration vector to generate local gravity vector values;

f) correcting the estimated sensor orientation quaternion using the local magnetic field vector and local gravity vector.

9. A method of tracking as in claim 8 wherein correcting the estimated sensor orientation quaternion using the local magnetic field vector and local gravity vector comprises:

g) determining a measurement vector from the local magnetic field vector values and the local gravity vector values;

h) calculating a computed measurement vector from the estimated sensor orientation quaternion;

i) comparing the measurement vector with the computed measurement vector to generate an error vector that defines a criterion function;

j) performing a mathematical operation that results in the minimization of the criterion function and outputs an error estimate quaternion;

k) integrating the error estimate quaternion;

l) normalizing the integrated error estimate quaternion to produce a new estimated sensor orientation quaternion; and

m) repeating steps a)-m), wherein the new estimated sensor orientation quaternion is used for h), calculating a computed measurement vector.

10. The method of claim 9 wherein the operation ofj), performing a mathematical operation that results in the minimization of the criterion function comprises minimizing the criterion function without calculating the criterion function.

11. The method of claim 9 wherein the operation of j), performing a mathematical operation that results in the minimization of the criterion function includes implementing a partial correction step to compensate for measurement error.

12. The method of claim 10 wherein implementing the partial correction step to compensate for measurement error is supplemented by using a weighted least squares regression to emphasize more reliable measurements with respect to less reliable measurements.

13. The method of claim 9 wherein the operation of j), performing a mathematical operation that results in the minimization of the criterion function comprises using time weighted filtering.

14. The method of claim 9 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function comprises using a Gauss-Newton iteration.

16. The method of claim 15 wherein each new estimate of sensor orientation is output as a sensor orientation signal.

17. The method of claim 15 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function comprises minimizing the criterion function without calculating the criterion function.

18. The method of claim 15 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function includes implementing a partial correction step to compensate for measurement error.

19. The method of claim 18 wherein implementing the partial correction step to compensate for measurement error is supplemented by using a weighted least squares regression to emphasize more reliable measurements with respect to less reliable measurements.

20. The method of claim 15 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function comprises using time weighted filtering.

21. The method of claim 15 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function comprises using a Gauss-Newton iteration.

22. The method of claim 15 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function and outputs an error estimate includes:

measuring an angular velocity of the sensor to generate angular rate values;

integrating the angular rate values;

normalizing the integrated angular rate values to produce an estimate of sensor orientation derived from the angular rate values; and

using the estimate of sensor orientation derived from the angular rate values to correct for time lag.

24. The method of claim 23 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function and outputs a 4×

1 quaternion error estimate comprises minimizing the criterion function without calculating the criterion function.

25. The method of claim 23 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function and outputs a 4×

1 quaternion error estimate comprises multiplying the 6×

1 error vector by the function [X^TX]<sup>−

1</sup>X^T.

26. The method of claim 23 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function and outputs a 4×

1 quaternion error estimate further includes implementing a partial correction step to compensate for measurement error.

27. The method of claim 23 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function and outputs a 4×

1 quaternion error estimate comprises using a time weighted filtering system.

28. The method of claim 23 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function and outputs a 4×

1 quaternion error estimate comprises using a Gauss-Newton iteration.

29. The method of claim 26 wherein implementing the partial correction step to compensate for measurement error is supplemented by using a weighted least squares regression to emphasize more reliable measurements with respect to less reliable measurements.

30. The method of claim 23 wherein the operation of g), performing a mathematical operation that results in the minimization of the criterion function and outputs a 4×

1 quaternion error estimate includes;

measuring an angular velocity of the sensor to generate an angular rate quaternion;

integrating the angular rate quaternion;

normalizing the integrated angular rate quaternion to produce an estimate of sensor orientation quaternion derived from the angular rate quaternion; and

using the estimate of sensor orientation quaternion derived from the angular rate quaternion to correct for time lag.

15. A method of tracking the orientation of a sensor, the method comprising:

a) providing a starting estimate of sensor orientation;

b) measuring a magnetic field vector to generate local magnetic field vector values;

c) measuring an acceleration vector to generate local gravity vector values;

d) determining a measurement vector from the local magnetic field vector values and the local gravity vector values;

e) calculating a computed measurement vector from the estimate of sensor orientation;

f) comparing the measurement vector with the computed measurement vector to generate an error vector that defines a criterion function;

g) performing a mathematical operation that results in the minimization of the criterion function and outputs an error estimate;

h) integrating the error estimate;

i) normalizing the integrated error estimate to produce a new estimate of sensor orientation; and

j) repeating steps a)-j), wherein the new estimate of sensor orientation is used for e), calculating a computed measurement vector.

23. A method of tracking the orientation of a sensor, the method comprising:

a) providing a starting estimate of sensor orientation quaternion;

b) measuring a magnetic field vector to generate local magnetic field vector values;

c) measuring an acceleration vector to generate local gravity vector values;

d) determining a measurement vector from the local magnetic field vector values and the local gravity vector values;

e) calculating a computed measurement vector from the estimate of sensor orientation, using quaternion mathematics;

f) comparing the measurement vector with the computed measurement vector to generate an 6×

1 error vector that defines a criterion function; and

g) performing a mathematical operation that results in the minimization of the criterion function and outputs a 4×

1 quaternion error estimate;

h) integrating the quaternion error estimate; and

i) normalizing the integrated quaternion error estimate to produce a new estimated sensor orientation quaternion;

j) repeating steps a)-j), wherein the new estimated sensor orientation quaternion is used for e), calculating a computed measurement vector.

31. A sensor apparatus comprising:

a magnetic field detector configured to measure a magnetic field vector and output a local magnetic field vector signal; and

an acceleration detector configured to detect a local gravitational field vector and output a local gravitational field vector signal.

32. The sensor of claim 31, further includes an angular velocity detector configured to detect an angular velocity vector of the sensor and output angular velocity signal.

33. The sensor of claim 32 wherein, the angular rate detector comprises a three-axis angular velocity detector;

the magnetic field detector comprises a three-axis magnetometer; and

the acceleration detector comprises a three-axis accelerometer.

34. The sensor of claim 31 wherein the sensor includes at least one processor that receives and processes the signals from the magnetic field detector and the acceleration detector to determine the orientation of the sensor apparatus.

35. The sensor of claim 32 wherein the sensor includes at least one processor that receives and processes the signals from the magnetic field detector, the acceleration detector, and the signal from the angular velocity detector, wherein the at least one processor is configured to determine the orientation of the sensor.

37. A system as in claim 36 wherein the at least one processor is configured input the body orientation information into a synthetic environment;

and

wherein the system further includes a display for displaying the position and orientation of the body with respect to the synthetic environment.

38. A system as in claim 37 wherein the at least one processor is configured to correct for the offset between sensor coordinates and body coordinates.

39. A system as in claim 36 wherein each sensor further includes an angular velocity detector for measuring a body angular velocity vector.

40. A system as in claim 39 wherein the at least one processing is configured input the body orientation information into a synthetic environment;

and

wherein the system further includes a display for displaying the position and orientation of the body with respect to the synthetic environment.

41. A system as in claim 40 wherein the at least one processor is configured to correct for the offset between sensor coordinates and body coordinates.

42. A system as in claim 36 wherein the body comprises an articulated rigid body having a plurality of segments interconnected by at least one joint and wherein each segment has mounted thereon at least one sensor.

43. A system as in claim 39 wherein the body comprises an articulated rigid body having a plurality of segments interconnected by at least one joint and wherein each segment has mounted thereon at least one sensor.

36. A system for tracking the posture and orientation of body, the system comprising:

the body having mounted thereon at least one sensor;

each sensor including a magnetometer for measuring a magnetic field vector and a acceleration detector for measuring a body acceleration vector, and

at least one processor for receiving input from the magnetometer and acceleration detector and using said input to calculate a local magnetic field vector and a local gravity vector and to determine the orientation of the body.

44. A method of determining the direction of a local gravity vector with an acceleration detector, the method comprising:

moving the acceleration detector from a start point to an end point over a time period;

taking measurements of the total acceleration vector during the time period;

weighted summing the measure ments of the total acceleration vector over the time period; and

calculating gravity vector values using the weighted sum of the total acceleration measurements.