MOTION SMOOTHING IN 3-D POSITION SENSING APPARATUS

• US 20110071785A1
• Filed: 09/13/2010
• Published: 03/24/2011
• Est. Priority Date: 05/02/2008
• Status: Active Grant

• Current Assignee: Performance Designed Products LLC
• Original Assignee: Performance Designed Products LLC
• Inventors: HEATH, THOMAS PETER
• Granted Patent: US 8,868,368 B2
• Time in Patent Office: Days
• US Class Current: 702/96
• CPC Class Codes: G01S 15/66 Sonar tracking systems
• CPC Class Codes: G01S 5/18 using ultrasonic, sonic, or infrasonic waves

Abstract

A method of estimating and extrapolating the position of an article is provided. The article'"'"'s position is detected by relatively infrequent ultrasonic ranging, and provides more frequent reports from internal accelerometers, gyroscopes and optional magnetometers. In a first instance, the method includes calculating new position, velocity and orientation vectors by linearly interpolating between the readings of the sensors at two times. In a second instance, the method includes estimating the orientation of the article by calculating the duration of a timeslice, making a rotational increment matrix and taking the product of the initial orientation and the increment matrix for the appropriate number of timeslices. In a third instance, the method includes calculating the acceleration of an article between three ultrasonically determined locations and recalibrating the accelerometers to align the measured acceleration with the calculated acceleration.

Claims

1. A method of estimating position and orientation of an article, comprising:

reading the orientation of the article from at least three gyroscopes mounted on the article;

reading the acceleration of the article from three accelerometers mounted on the article;

at time T estimating a position vector velocity vector V, and orientation matrix O of an article, between T and an initial time T₀ where;

an initial position vector=P₀,

an initial velocity vector=V₀,

an initial orientation matrix=O₀,

an initial rotational velocity, relative to the article, vector=R₀, and

an initial acceleration, relative to the article, vector=A₀; and

re-calculating the position vector P, velocity vector V, and orientation matrix O at a subsequent time T₁ using new information from the accelerometers and gyroscopes where;

new acceleration relative to the object vector=A₁,

new rotational velocity relative to the object vector=R₁,and

the re-calculation is carried out by, linearly interpolating from R₀ to R₁ to discover the local rotational velocity at any time between T₀ and T₁, and by linearly interpolating from A₀ to A₁ to discover the local acceleration.

2. A method according to claim 1, the article further comprising three magnetic sensors mounted therein, and the method further comprising:

at time T estimating the position vector P, velocity vector V, and orientation matrix O of an article, between T and an initial time T₀ where;

initial direction of fixed magnetic field, relative to the article, vector=F₀ (in angular units); and

re-calculating the position vector P, velocity vector V, and orientation matrix O at a subsequent time T using new information from the accelerometers, gyroscopes and magnetometers where;

new direction of fixed magnetic field, relative to the article, vector=F₁ (in angular units), and

the re-calculation is carried out by linearly interpolating from F₀ to F₁ to discover the local direction of fixed magnetic field.

3. A method according to claim 1, further comprising using ultrasonic readings to correct the velocity estimates and thereby estimated position, by modifying the historical velocity estimate when each new ultrasonic reading is received, to make the estimated position match the position given by the new ultrasonic measurement.

4. A method of estimating position and orientation of an article, comprising:

extrapolating a position vector P, and orientation matrix O of the article at time T, moveable from an initial state at time T₀, described by;

an initial position vector=P₀,

an initial velocity vector=V₀,

an initial orientation matrix=O₀, (in angular units), and

initial rotational velocity, relative to the article, vector=R₀;

calculating the duration of a timeslice T_Slice=(T−

T₀)/N, where N tends to zero; and

making a rotational increment matrix M to create a rotation matrix from a rotation expressed as a vector of components representing x, y, and z axes of rotation ;

M=MatrixCreate(T_Slice*R₀), where the estimated orientation and estimated position are given as O=M₁×

M₂×

M₃ ×

. . . ×

M_N×

O₀ and P=P₀+V(T−

T₀) respectively.

5. A method according to claim 4, further comprising using ultrasonic readings to correct the velocity estimates and thereby estimated position, by modifying the historical velocity estimate when each new ultrasonic reading is received, to make the estimated position match the position given by the new ultrasonic measurement.

6. A method of estimating position and orientation of an article, the method comprising correcting orientation, and optionally velocity, by:

taking a plurality of readings from the output of at least three accelerometers mounted on the article;

using at least three positions given by ultrasonic time-of-flight measurements, during transit between at least the first, second and third ultrasonic-derived positions, assuming that acceleration of the article between the first to the third the positions was substantially constant and verifying this against the plurality of accelerometer readings;

calculating the acceleration vector at the second position using the position and time information about the at least three known positions and comparing that calculated acceleration vector with one obtained from the accelerometers; and

thereby re-calibrating the measurements taken from the accelerometers.

7. A computer program product embedded on non-transitory computer-readable medium, the computer program product carrying program steps which when executed on suitable hardware causes the hardware to carry out the steps of:

reading the orientation of the article from at least three gyroscopes mounted on the article;

reading the acceleration of the article from at least three accelerometers mounted on the article;

at time T estimating a position vector P, velocity vector V, and orientation matrix O of an article, between T and an initial time T₀ where;

an initial position vector=P₀,

an initial velocity vector=V₀,

an initial orientation matrix=O₀),

an initial rotational velocity, relative to the article, vector=R₀, and

an initial acceleration, relative to the article, vector=A₀; and

re-calculating the position vector P, velocity vector V, and orientation matrix O at a subsequent time T₁ using new information from the accelerometers and gyroscopes where;

new acceleration relative to the object vector=A₁,

new rotational velocity relative to the object vector=R₁,and

the re-calculation is carried out by, linearly interpolating from R₀ to R₁ to discover the local rotational velocity at any time between T₀ and T₁, and by linearly interpolating from A₀ to A₁ to discover the local acceleration.

8. A computer program product according to claim 7, the article further comprising three magnetic sensors mounted therein, and the steps further comprising:

at time T estimating a position vector P, velocity vector V, and orientation matrix O of an article, between T and an initial time T₀ where ;

an initial direction of fixed magnetic field, relative to the article, vector=F₀ (in angular units); and

re-calculating the position vector P, velocity vector V, and orientation matrix O at a subsequent time T₁ using new information from the accelerometers, gyroscopes and magnetometers where;

a new direction of fixed magnetic field, relative to the article, vector=F₁ (in angular units), and

the re-calculation is carried out by linearly interpolating from F₀ to F₁ to discover the local direction of fixed magnetic field.

9. A computer program product according to claim 7, the steps further comprising using ultrasonic readings to correct the velocity estimates and thereby estimated position, by modifying the historical velocity estimate when each new ultrasonic reading is received, to make the estimated position match the position given by the new ultrasonic measurement.

10. A computer program product embedded on non-transitory computer-readable medium, the computer program product carrying program steps which when executed on suitable hardware causes the hardware to carry out the steps of:

extrapolating a position vector P, and orientation matrix O of the article at time T, moveable from an initial state at time T₀, described by;

an initial position vector=P₀,

an initial velocity vector=V₀,

an initial orientation matrix=O₀, (in angular units), and

an initial rotational velocity, relative to the article, vector=R₀;

calculating the duration of a timeslice T_Slice=(T−

T₀)/N, where N tends to zero; and

making a rotational increment matrix M to create a rotation matrix from a rotation expressed as a vector of components representing x, y, and z axes of rotation ;

M=MatrixCreate(T_Slice*R₀), where the estimated orientation and estimated position are given as O=M₁×

M₂×

M₃ ×

. . . ×

M_N×

O₀ and P=P₀+V(T−

T₀) respectively.

11. A computer program product according to claim 10, the steps further comprising using ultrasonic readings to correct the velocity estimates and thereby estimated position, by modifying the historical velocity estimate when each new ultrasonic reading is received, to make the estimated position match the position given by the new ultrasonic measurement.

12. A computer program product embedded on non-transitory computer-readable medium, the computer program product carrying program steps which when executed on suitable hardware causes the hardware to carry out the steps of:

taking a plurality of readings from the output of at least three accelerometers mounted on the article;

using at least three positions given by ultrasonic time-of-flight measurements, during transit between at least the first, second and third ultrasonic-derived positions, assuming that acceleration of the article between the first to the third the positions was substantially constant and verifying this against the plurality of accelerometer readings;

calculating the acceleration vector at the second the position using the position and time information about the at least three known positions and comparing that calculated acceleration vector with one obtained from the accelerometers; and

thereby re-calibrating the measurements taken from the accelerometers.

13. A method of controlling an on-screen pointer, comprising:

reading the orientation of the article from at least three gyroscopes mounted on the article;

reading the acceleration of the article from at least three accelerometers mounted on the article;

at time T estimating a position vector P, velocity vector V, and orientation matrix O of an article, between T and an initial time T₀ where;

an initial position vector=P₀,

an initial velocity vector=V₀,

an initial orientation matrix=O₀,

an initial rotational velocity, relative to the article, vector=R₀, and

an initial acceleration, relative to the article, vector=A₀;

re-calculating the position vector P, velocity vector V, and orientation matrix O at a subsequent time T₁ using new information from the accelerometers and gyroscopes where;

new acceleration relative to the object vector=A₁,

new rotational velocity relative to the object vector=R₁,and

the re-calculation is carried out by, linearly interpolating from R₀ to R₁ to discover the local rotational velocity at any time between T₀ and T₁, and by linearly interpolating from A₀ to A₁ to discover the local acceleration; and

using the position vector P, velocity vector V, and orientation matrix O of the article to determine where a line through a predetermined one of the article'"'"'s axes intersects a plane.

14. A method according to claim 13, the article further comprising three magnetic sensors mounted therein, and the method further comprising:

at time T estimating the position vector P, velocity vector V, and orientation matrix O of the article, between T and an initial time T₀ where;

an initial direction of fixed magnetic field, relative to the article, vector=F₀ (in angular units); and

re-calculating the position vector P, velocity vector V, and orientation matrix O at a subsequent time T₁ using new information from the accelerometers, gyroscopes and magnetometers where;

a new direction of fixed magnetic field, relative to the article, vector=F₁ (in angular units), and

the re-calculation is carried out by linearly interpolating from F₀ to F₁ to discover the local direction of fixed magnetic field.

15. A method according to claim 13, further comprising using ultrasonic readings to correct the velocity estimates and thereby estimated position, by modifying the historical velocity estimate when each new ultrasonic reading is received, to make the estimated position match the position given by the new ultrasonic measurement.

16. A method of controlling an on-screen pointer, comprising:

extrapolating a position vector P, and orientation matrix O of the article at time T, moveable from an initial state at time T₀, described by;

an initial position vector=P₀,

an initial velocity vector=V₀,

an initial orientation matrix=O₀, (in angular units), and

initial rotational velocity, relative to the article, vector=R₀;

calculating the duration of a timeslice T_Slice=(T−

T₀)/N, where N tends to zero; and

making a rotational increment matrix M to create a rotation matrix from a rotation expressed as a vector of components representing x, v, and z axes of rotation ;

M=MatrixCreate(T_Slice*R₀), where the estimated orientation and estimated position are given as O=M₁×

M₂×

M₃ ×

. . . ×

M_N×

O₀ and P=P₀+V(T−

T₀) respectively; and

using the position vector P, and orientation matrix O of the article to determine where a line through a predetermined one of the article'"'"'s axes intersects a plane.

17. A method according to claim 16, further comprising using ultrasonic readings to correct the velocity estimates and thereby estimated position, by modifying the historical velocity estimate when each new ultrasonic reading is received, to make the estimated position match the position given by the new ultrasonic measurement.