Micro azimuth-level detector based on micro electro-mechanical systems and a method for determination of attitude

• US 6,813,584 B2
• Filed: 03/12/2002
• Issued: 11/02/2004
• Est. Priority Date: 03/30/2001
• Status: Expired due to Fees

• Current Assignee: Tsinghua University
• Original Assignee: Tsinghua University
• Inventors: Sun, Xuefeng, Zhu, Rong, Wang, Xiaohao, Xiong, Shenshu, Wang, Guanglong, Zhu, Junhua, Wei, Qiang, Zhou, Zhaoying, Ye, Xiongying
• Primary Examiner(s): Ngheim, Michael
• Application Number:
  US10/094,991
• Publication Number:
  US 20020188416A1
• Time in Patent Office: 966 Days
• Field of Search: 702/38, 702/94, 702/95, 702/141, 702/150, 702/151, 702/153, 701/11, 701/200, 701/220
• US Class Current: 702/151
• CPC Class Codes: G01C 17/28 Electromagnetic compasses with north seeking magnetic elements and having electric pick-offs G01C17/26
• CPC Class Codes: G01C 17/38 Testing, calibrating, or compensating of compasses

Abstract

A system and method for determining attitude using triaxial micro electro-mechanical accelerometer-magnetometer sensors. The triaxial accelerometer-magnetometer sensors measure the triaxial components of the local gravity and magnetic field along three sensing axes of the sensors. The misalignment of the components due to nonorthogonal triaxial assembling of the sensors is estimated via performing three optical alignments and is further compensated in the sensing outputs. A micro-cube with three orthogonal coated mirrors is fixed on the circuit board of the system for implementing the optical alignments. The compensated sensing components are processed to figure out the attitude using Orientation Cosine Conversion of the local gravity and magnetic field. The computation for determining the attitude is based on the inverse tangent of the ratio of the processed components, which is more precise than the computation using the absolute values of the components.

Claims

1. A Micro Electro-mechanical System (MEMS) based micro azimuth-level detector comprising:

multi-sensors,

an analog/digital (A/D) converter,

a microprocessor for signal processing,

an RS-232 electrical interface, and

a PC having an Operation-Display software,

wherein the multi-sensors comprise three accelerometers and three magnetometers, the three accelerometers are assembled along three orthogonal axes and having a processing circuit, and the three magnetometers are assembled along three orthogonal axes and have a processing circuit,

a micro-cube with three orthogonal coated mirrors for optical alignment, and

wherein analogue signals from the multi-sensors are converted into digital signals by the A/D converter, sent to and processed in the microprocessor, and further transmitted via the RS-232 electrical interface to be shown in the PC.

2. The micro azimuth-level detector of claim 1, wherein the digital signals are processed in the microprocessor by signal syncretizing and attitude computation, and sent via the RS232 to be shown in a graphical Operation-Display Software in the PC.

3. A method for determining attitude comprising

obtaining electrical/analog signals from sensors,

converting the electrical/analog signals into digital signals by an A/D converter,

filtering the digital signals and processing the digital signals by electric/physical conversion,

eliminating misalignment in the processed digital signals by misalignment compensation based on optical alignment,

computing attitude angles including pitch, roll and heading from the processed signals by Orientation Cosine Conversion of the earth'"'"'s gravity and magnetic field and inverse tangent method,

conditionally smooth-processing the computed attitude angles by applying a large amount of smoothing to small variations and little or no smoothing for large change in attitude outputs,

displaying the conditionally smooth-processed attitude on an Operation-Display interface.

4. The method for determining attitude according to claim 3, wherein the processed triaxial gravity signals x_g, y_g, and z_g, and the processed triaxial magnetic signals x_m, y_m, and z_m are obtained through the sensors and processed, and the attitude angles pitch, roll and heading are computed by

(1) calculating roll γ

according to Table 1 and value of γ

_ref

5. The method for determining attitude according to claim 3, wherein the misalignment is eliminated by

(1) defining the orthogonal beam adjustment X-Y-Z of micro-cube as a datum orthogonal frame of groupware,

(2) defining X′

-Y′

-Z′

as a frame of the accelerometers in the orthogonal directions, and X″

-Y″

-Z″

as a frame of the magnetometers in the orthogonal directions,

(3) Aligning the datum frame X-Y-Z to North(magnetic north)-East-Ground by beam-based alignment, a relation between three accelerometers'"'"' readings [X′

_a,Y′

_a,Z′

_a]^T and independent components [0,0, f_g]^T of the gravity field along N(X)-E(Y)-G(Z) being $\{\begin{array}{c}{X}_a^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(X^{\mathrm{\prime <br><br>}},Z\right)\\ Y_a^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(Y^{\mathrm{\prime <br><br>}},Z\right)\\ Z_a^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(Z^{\mathrm{\prime <br><br>}},Z\right);<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{and}\end{array}<br><br>$

 

a relation between three magnetometers'"'"' readings [X″

_a,Y″

_a,Z″

_a]^T and independent components [H_N,0,H_G]^T of the earth'"'"'s magnetic field along N(X)-E(Y)-G(Z) being $\{\begin{array}{c}{X}_a^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(X^{\mathrm{″<br><br>}},X\right)+H_G·<br><br>\cos <br><br>\left(X^{\mathrm{″<br><br>}},Z\right)\\ Y_a^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(Y^{\mathrm{″<br><br>}},X\right)+H_G·<br><br>\cos <br><br>\left(Y^{\mathrm{″<br><br>}},Z\right)<br><br>\text{\hspace{1em}<br><br>}\\ Z_a^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(Z^{\mathrm{″<br><br>}},X\right)+H_G·<br><br>\cos <br><br>\left(Z^{\mathrm{″<br><br>}},Z\right)<br><br>\text{\hspace{1em}<br><br>};<br><br>\end{array}<br><br>\text{\hspace{1em}<br><br>}<br><br>$

(4) Aligning X-Y-Z to East-Ground-North(magnetic north);

A relation between three accelerometers'"'"' readings [X′

_b,Y′

_b,Z′

_b]^T and gravity XYZ-stage components [0, f_g,0]^T being $\{\begin{array}{c}{X}_b^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(X^{\mathrm{\prime <br><br>}},Y\right)\\ Y_b^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(Y^{\mathrm{\prime <br><br>}},Y\right)\\ Z_b^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(Z^{\mathrm{\prime <br><br>}},Y\right);<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{and}\end{array}<br><br>$

 

a relation between three magnetometers'"'"' readings [X″

_b,Y″

_b,Z″

_b]^T and magnetic XYZ-stage components [0, H_G,H_N]^T being $\{\begin{array}{c}{X}_b^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(X^{\mathrm{″<br><br>}},Z\right)+H_G·<br><br>\cos <br><br>\left(X^{\mathrm{″<br><br>}},Y\right)\\ Y_b^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(Y^{\mathrm{″<br><br>}},Z\right)+H_G·<br><br>\cos <br><br>\left(Y^{\mathrm{″<br><br>}},Y\right)<br><br>\text{\hspace{1em}<br><br>}\\ Z_b^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(Z^{\mathrm{″<br><br>}},Z\right)+H_G·<br><br>\cos <br><br>\left(Z^{\mathrm{″<br><br>}},Y\right);<br><br>\end{array}<br><br>$

(5) Aligning X-Y-Z to Ground-North(magnetic north)-East;

a relation between three accelerometers'"'"' readings [X′

_c,Y′

_c,Z′

_c]^T and gravity XYZ-stage components [f_g,0,0]^T being $\{\begin{array}{c}{X}_c^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(X^{\mathrm{\prime <br><br>}},X\right)\\ Y_c^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(Y^{\mathrm{\prime <br><br>}},X\right)\\ Z_c^{\mathrm{\prime <br><br>}}=f_g·<br><br>\cos <br><br>\left(Z^{\mathrm{\prime <br><br>}},X\right);<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{and}\end{array}<br><br>$

 

a relation between three magnetometers'"'"' readings [X″

_c,Y″

_c,Z″

_c]^T and magnetic XYZ-stage components [H_G,H_N,0]^T being $\{\begin{array}{c}{X}_c^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(X^{\mathrm{″<br><br>}},Y\right)+H_G·<br><br>\cos <br><br>\left(X^{\mathrm{″<br><br>}},X\right)\\ Y_c^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(Y^{\mathrm{″<br><br>}},Y\right)+H_G·<br><br>\cos <br><br>\left(Y^{\mathrm{″<br><br>}},X\right)\\ Z_c^{\mathrm{″<br><br>}}=H_N·<br><br>\cos <br><br>\left(Z^{\mathrm{″<br><br>}},Y\right)+H_G·<br><br>\cos <br><br>\left(Z^{\mathrm{″<br><br>}},X\right);<br><br>\end{array}<br><br>$

(6) computing Orientation Cosine Matrix Mg between the datum frame X-Y-Z and the acceleration frame X′

-Y′

-Z′

, $\mathrm{Mg}=\left[\begin{array}{ccc}\cos <br><br>\left(X^{\mathrm{\prime <br><br>}},X\right)& \cos <br><br>\left(X^{\mathrm{\prime <br><br>}},Y\right)& \cos <br><br>\left(X^{\mathrm{\prime <br><br>}},Z\right)\\ \cos <br><br>\left(Y^{\mathrm{\prime <br><br>}},X\right)& \cos <br><br>\left(Y^{\mathrm{\prime <br><br>}},Y\right)& \cos <br><br>\left(Y^{\mathrm{\prime <br><br>}},Z\right)\\ \cos <br><br>\left(Z^{\mathrm{\prime <br><br>}},X\right)& \cos <br><br>\left(Z^{\mathrm{\prime <br><br>}},Y\right)& \cos <br><br>\left(Z^{\mathrm{\prime <br><br>}},Z\right)\end{array}\right],$

(7) computing Orientation Cosine Matrix Mm between X-Y-Z and the magnetic frame X″

-Y″

-Z″

, $\mathrm{Mm}=\left[\begin{array}{ccc}\cos <br><br>\left(X^{\mathrm{″<br><br>}},X\right)& \cos <br><br>\left(X^{\mathrm{″<br><br>}},Y\right)& \cos <br><br>\left(X^{\mathrm{″<br><br>}},Z\right)\\ \cos <br><br>\left(Y^{\mathrm{″<br><br>}},X\right)& \cos <br><br>\left(Y^{\mathrm{″<br><br>}},Y\right)& \cos <br><br>\left(Y^{\mathrm{″<br><br>}},Z\right)\\ \cos <br><br>\left(Z^{\mathrm{″<br><br>}},X\right)& \cos <br><br>\left(Z^{\mathrm{″<br><br>}},Y\right)& \cos <br><br>\left(Z^{\mathrm{″<br><br>}},Z\right)\end{array}\right];<br><br>$

(8) switching three accelerometers'"'"' readings [x′

_g,y′

_g,z′

_g]^T and three magnetometers'"'"' readings'"'"' [x′

_M,y′

_M,z′

_M]^T into the orthogonal datum XYZ-stage components [x_g,y_g,z_g]^T and [x_M,y_M,z_M]^T using Mg and Mm when detecting attitude $\left[\begin{array}{c}{x}_g\\ y_g\\ z_g\end{array}\right]={\mathrm{Mg}}^{-1}<br><br>\left[\begin{array}{c}{x}_g^{\mathrm{\prime <br><br>}}\\ y_g^{\mathrm{\prime <br><br>}}\\ z_g^{\mathrm{\prime <br><br>}}\end{array}\right],<br><br>\left[\begin{array}{c}{x}_M\\ y_M\\ z_M\end{array}\right]={\mathrm{Mm}}^{-1}<br><br>\left[\begin{array}{c}{x}_M^{\mathrm{\prime <br><br>}}\\ y_M^{\mathrm{\prime <br><br>}}\\ z_M^{\mathrm{\prime <br><br>}}\end{array}\right].$

6. The method for determining attitude according to claim 3, wherein conditionally smooth-processing to the computed attitude angles is performed by applying smoothing to small variations and little or no smoothing for large change in attitude outputs as:

If, |angle(n)−

angle(n−

1)|<

ss

Then, attitude(n)=a₀·

angle(n)+a₁·

angle(n−

1)

Where, angle(n), angle(n−

1) are the current attitude outputs and their backward shift, attitude(n) is the smoothed attitude, ss is threshold value, a₀, a₁ are constant coefficients.