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

• US 20020188416A1
• Filed: 03/12/2002
• Published: 12/12/2002
• Est. Priority Date: 03/30/2001
• Status: Active Grant

• Current Assignee: Tsinghua University
• Original Assignee: Tsinghua University
• Inventors: Wei, Qiang, Zhou, Zhaoying, Ye, Xiongying, Sun, Xuefeng, Zhu, Rong, Wang, Xiaohao, Xiong, Shenshu, Wang, Guanglong, Zhu, Junhua
• Granted Patent: US 6,813,584 B2
• Time in Patent Office: Days
• 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

This present invention is subjected to the field of instrument & measurement technology based on Micro Electro-mechanical Systems (MEMS). A micro azimuth-level detector comprises multi-sensors (triaxial silicon accelerometers with the processing circuit and triaxial silicon magnetometers with the processing circuit), A/D converter for converting the analog signals of the sensors to the digital signals, microprocessor in which the signal-processing and attitude computation are carried out, RS-232 for connecting the microprocessor with PC, and Operation-Display interface. An attitude computation method uses the Orientation Cosine Conversion of the earth'"'"'s gravity and magnetic field.

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, 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 RS-232 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 interface in the PC.

3. The micro azimuth-level detector of claim 1, further comprising a micro-cube with three orthogonal coated mirrors for optical alignment.

4. 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.

5. The method for determining attitude according to claim 4, 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

γ

_ref=arctg(y_g/z_g), TABLE 1z_gy_gγ

Quadrant→

0+ 

 

90°

→

0−

−

90°

++γ

_ref(0°

, 90°

)+−

γ

_ref(−

90°

, 0°

)−

+γ

_ref +180°

(90°

, 180°

)−

−

γ

_ref −

180°

(−

180°

, −

90°

)

(2) calculating pitch 0 according to

θ

=−

arctg(x_g·

cosγ

/z_g)

(3) and calculating heading ψ

according to Table 2, ψ

_ref, Xh and Yh,

$\mathrm{Xh}=x_m<br><br>\cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\theta <br><br>}+\left(y_m<br><br>\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}+z_m<br><br>\cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}\right)<br><br>\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\theta <br><br>}$$\mathrm{Yh}=z_m<br><br>\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}-y_m<br><br>\cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}$${\mathrm{\psi <br><br>}}_{\mathrm{ref}}=\mathrm{arc}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{tg}<br><br>\left(\frac{z_m<br><br>\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}-y_m<br><br>\cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}}{x_m<br><br>\cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\theta <br><br>}+\left(y_m<br><br>\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}+z_m<br><br>\cos <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\gamma <br><br>}\right)<br><br>\sin <br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{\theta <br><br>}}\right)=\mathrm{arc}<br><br>\text{\hspace{1em}<br><br>}<br><br>\mathrm{tg}<br><br>\left(\frac{\mathrm{Yh}}{\mathrm{Xh}}\right),$

TABLE 2XhYhψ

Quadrant→

0+ 

 

90°

→

0−

270°

++ψ

_ref(0°

, 90°

)+−

360°

+ψ

_ref(270°

, 360°

)−

+180°

+ψ

_ref(90°

, 180°

)−

−

180°

+ψ

_ref(180°

, 270°

)

6. The method for determining attitude according to claim 4, 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)\end{array};<br><br>\mathrm{and}$

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>}\end{array};<br><br><br><br>\text{\hspace{1em}<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)\end{array};<br><br>\mathrm{and}$

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>\text{\hspace{1em}<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)\end{array};<br><br>\mathrm{and}$

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)\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].$