Method and apparatus for motion tracking of an articulated rigid body

• US 7,089,148 B1
• Filed: 09/30/2004
• Issued: 08/08/2006
• Est. Priority Date: 10/30/2000
• Status: Expired due to Term

• Current Assignee: the united states of america as represented by the secretary of the navy
• Original Assignee: the united states of america as represented by the secretary of the navy
• Inventors: McGhee, Robert B., Bachmann, Eric R., McKinney, Douglas L., Yun, Xiaoping, Zyda, Michael J.
• Primary Examiner(s): Barlow, John
• Assistant Examiner(s): Le, Toan M.
• Application Number:
  US10/963,007
• Time in Patent Office: 677 Days
• Field of Search: 702/151, 702/150, 702/141, 702/142, 702/94, 702/53, 600/595, 345/473
• US Class Current: 702/151
• 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 includes 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 includes 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 includes 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.

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;

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 without calculating the criterion function 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.

2. 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;

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 using time weighted filtering 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. 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;

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 estimated sensor orientation quaternion using the local magnetic field vector and local gravity vector, 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 using time weighted filtering 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.

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

5. 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, wherein each new estimate of sensor orientation is output as a sensor orientation signal; and

j) repeating steps a)–

i), wherein the new estimate of sensor orientation is used for e), calculating a computed measurement vector.

6. The method of claim 5 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.

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

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

9. The method of claim 5 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.

10. 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;

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

1 quaternion error estimate without calculating the criterion function;

h) integrating the quaternion error estimate;

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

j) repeating steps a)–

i), wherein the new estimated sensor orientation quaternion is used for e), calculating a computed measurement vector.

11. The method of claim 10 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.

12. The method of claim 10 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.

13. The method of claim 10 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.

14. The method of claim 10 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.