METHOD FOR ESTIMATING THE DIRECTION OF A MOVING SOLID

• US 20120185204A1
• Filed: 07/26/2010
• Published: 07/19/2012
• Est. Priority Date: 07/31/2009
• Status: Abandoned Application

• Current Assignee: Commissariat a L'Energie Atomique (AES Corporation), Movea
• Original Assignee: Commissariat a L'Energie Atomique (AES Corporation), Movea
• Inventors: Jallon, Pierre, Bonnet, Stéphane
• Application Number:
  US13/388,216
• Publication Number:
  US 20120185204A1
• Time in Patent Office: Days
• US Class Current: 702/141
• CPC Class Codes: G01C 21/16 by integrating acceleration or speed, i.e. inertial navigation

Abstract

The invention relates to a method for estimating the orientation in an inertial reference frame of a solid in motion equipped with an accelerometer and a magnetometer which are mounted on said solid.

According to this method, an orientation of the solid is measured at a reference instant, said orientation being defined by a rotation matrix for rotating from the mobile reference frame of the solid at the reference instant to the inertial reference frame. A rotation matrix for rotating between the orientation of the solid at a subsequent instant n and said orientation of the solid at the reference instant is thereafter estimated. The orientation of the solid at the instant n is thereafter determined with the aid of the previously estimated rotation matrix and of the known orientation of the solid at the reference instant.

Claims

1. A method for estimating orientation in an inertial reference frame of a solid in motion equipped with an accelerometer and a magnetometer which are coupled to said solid, said method comprising the steps of:

measuring, at a reference instant n₀ for which the solid is devoid of inherent acceleration, gravitational field and magnetic field vectors in a reference frame of the solid, said measured vectors usable to determine an orientation of said solid in the inertial reference frame at the reference instant n₀;

measuring, at successive instants, acceleration vectors ({a(k)}) and magnetic field vectors ({m(k)}) in said reference frame of the solid;

estimating a matrix U(n,n₀) usable to ensure rotation of said orientation of the solid, previously determined at the reference instant n₀, to an orientation at a subsequent instant n,

said matrix U(n,n₀) being expressible in the form of a product of a first and of a second rotation matrix,

said first matrix being defined by a first angle of rotation α

(n,n₀) of the magnetic field vector m(n₀) measured at the reference instant n₀ to the magnetic field vector m(n) measured at the instant n, about a first rotation vector oriented along a vector product of the magnetic field vectors measured at the instants n and n₀;

said second matrix being defined by a second angle of rotation θ

(n,n₀) about a second rotation vector chosen from among the magnetic field vectors measured at the instant n₀and n, said second angle being estimated using the gravitational field vector a_g(n₀), measured at the reference instant n₀ and of a gravitational field vector extrapolated at the instant n (â

_g(n)) using a plurality of acceleration vectors measured at instants prior to the instant n;

estimating the orientation of the solid at the instant n using the matrix U(n,n₀) previously estimated and of said orientation at the reference instant n₀.

2. The method of claim 1, wherein the orientation of said solid in the inertial reference frame at an arbitrary instant k is defined by a conversion matrix R^(k) for passing from the reference frame of the solid at the instant k to the inertial reference frame, said orientation of the solid at the instant n being obtained through the relation R⁽ⁿ⁾=U(n,n₀)R⁽ⁿ⁰⁾.

3. The method of claim 1, wherein said first angle of rotation α

(n,n₀) is estimated using a projection into an orthonormal basis of the magnetic field vectors measured at the instants n₀ and n, said basis being defined by first and second basis vectors oriented, respectively, along said first and second rotation vectors and by a third basis vector orthogonal to the first two.)

4. The method of claim 3, wherein, said second basis vector is the magnetic field vector measured at the instant n, said first angle of rotation is estimated through a relation:

α

(n,n₀)=arctan(m_z/m_y), where [0 m_y m_z] are components of a projection in said basis of the magnetic field vector measured at the instant n₀.

5. The method of claim 3, wherein, said second basis vector is the magnetic field vector measured at the instant n₀, said first angle of rotation is estimated through the relation:

α

(n,n₀)=arctan(m_z/m_y), where [0 m_y m_z] are components of a projection in said basis of the magnetic field vector measured at the instant n.

6. The method of claim 1, wherein said first angle of rotation α

(n,n₀) is estimated through a scalar product of said magnetic field vector m(n₀) measured at the instant n₀ and magnetic field vector m(n) measured at the instant n, said vectors being previously normed.

7. The method of claim 1, wherein, said first angle of rotation α

(n,n₀) being previously estimated, the second angle of rotation θ

(n,n₀) is estimated by comparison of a product of the matrix U(n,n₀) and of the gravitational field vector measured at the instant n₀ with said gravitational field vector extrapolated at the instant n.

8. The method of claim 1, wherein the second angle of rotation θ

(n,n₀) is estimated using a scalar product of said gravitational field vector a_g(n₀) measured at the instant n₀ and of the gravitational field vector â

_g(n) extrapolated at the instant n, said vectors being previously normed and projected into a plane orthogonal to said second rotation vector.

9. The method of claim 1, wherein the reference instant n₀ is determined using measurements of the acceleration vectors.

10. The method of claim 1, wherein the reference instant n₀ is determined prior to the step of measuring the acceleration vectors.

11. A method for estimating inherent acceleration of a solid in motion equipped with an accelerometer and a magnetometer which are coupled to said solid, said method comprising the steps of:

measuring, at a reference instant n₀ for which the solid is devoid of inherent acceleration, gravitational field and magnetic field vectors in a reference frame of the solid, said measured vectors usable to determine an orientation of said solid in the inertial reference frame at the reference instant n₀;

measuring, at successive instants, acceleration vectors ({a(k)}) and magnetic field vectors ({m(k)}) in said reference frame of the solid;

estimating a matrix U(n,n₀) usable to ensure rotation of said orientation of the solid, previously determined at the reference instant n₀, to an orientation at a subsequent instant n,

said matrix U(n,n₀) being expressible as a product of a first and of a second rotation matrix,

said first matrix being defined by a first angle of rotation α

(n,n₀) of the magnetic field vector m(n₀) measured at the reference instant n₀ to the magnetic field vector m(n) measured at the instant n, about a first rotation vector oriented along a vector product of the magnetic field vectors measured at the instants n and n₀;

said second matrix being defined by a second angle of rotation θ

(n,n₀) about a second rotation vector chosen from among the magnetic field vectors measured at the instant n₀ and n, said second angle being estimated using the gravitational field vector a_g(n₀), measured at the reference instant n₀ and of a gravitational field vector extrapolated at the instant n (â

_g(n)) using a plurality of acceleration vectors measured at instants prior to the instant n;

estimating the orientation of the solid at the instant n using the matrix U(n,n₀) previously estimated and of said orientation at the reference instant n₀;

calculating the gravitational field vector at the instant n (a_g(n)) as a product of the matrix U(n,n₀) estimated previously with the gravitational field vector measured at the reference instant n₀ (a_g(n₀));

deducing an inherent acceleration vector of the solid at the instant n using the acceleration vector measured at the instant n and of the gravitational field vector at the instant n calculated previously.