Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)

• US 20090089001A1
• Filed: 08/14/2008
• Published: 04/02/2009
• Est. Priority Date: 08/14/2007
• Status: Active Grant

• Current Assignee: American GNC Corporation
• Original Assignee: American GNC Corporation
• Inventors: Lin, Ching-Fang
• Granted Patent: US 8,005,635 B2
• Time in Patent Office: Days
• US Class Current: 702/92
• CPC Class Codes: G01C 21/188 for accumulated errors, e.g. by coupling inertial systems with absolute positioning systems
• CPC Class Codes: G01C 25/005 initial alignment, calibration or starting-up of inertial devices

Abstract

A method and system for Self-calibrated Azimuth and Attitude Accuracy Enhancing are disclosed, wherein SAAAEMS approach is based on fully auto-calibration self-contained INS principles, not depending on magnetometers for azimuth/heading determination, and thus the system outputs and performance are not affected by the environmental magnetic fields. In order to reduce the system size and cost, this new innovative methods and algorithms are used for SAAAEMS system configuration and integration. Compared to a conventional INS for gyrocompassing, AGNC'"'"'s approach uses a smaller number of high accuracy sensors: SAAAEMS uses only one 2-axis high accuracy gyro (for example, one DTG) instead of 3-axis; the third axis gyro is a MEMS gyro. It uses only 2 high accuracy accelerometers instead of 3, since the two accelerometers are used only for gyrocompassing not for navigation. These two changes to the conventional INS system configuration remarkably reduce the whole system size and cost. SAAAEMS, uses dynamic gyrocompassing processing for isolation of Base motion disturbance/interference and vibration. SAAAEMS provides a method and system for using automatic methods for system calibration.

Claims

1. A self-calibrated azimuth and attitude accuracy enhancing system, comprising:

a first one-axis gyro measuring an inertial angular rate;

a second one-axis gyro measuring an inertial angular rate wherein said first one-axis gyro and said second one-axis gyro are not identical in direction;

a first accelerometer measuring an acceleration;

a second accelerometer measuring an acceleration wherein said first accelerometer and said second accelerometer are not identical in direction; and

a system processor processing said measurements of said first one-axis gyro, said second one-axis gyro, said first accelerometer, and said second accelerometer for determining an azimuth and attitude wherein said system is located.

2. The system, as recited in claim 1, wherein said system further comprises a third gyro for said system to work in dynamic state.

3. The system, as recited in claim 2, wherein said third gyro is a MEMS gyro.

4. The system, as recited in claim 3, wherein said first one-axis gyro, said second one-axis gyro, said first accelerometer, and said second accelerometer are measuring an earth'"'"'s angular velocity vector and a local gravity force vector.

5. The system, as recited in claim 4, further comprising:

one or more sensor signal filters for filtering signals of said first one-axis gyro, said second one-axis gyro, said first accelerometer, and said second accelerometer; and

an ADC (analog/digital convertor) circuit converting analog signals of said first one-axis gyro, said second one-axis gyro, said first accelerometer, and said second accelerometer into digital signals and feeding said digital signals into said system processor.

6. The system, as recited in claim 1, wherein said first one-axis gyro and said second one-axis gyro is a two-axis gyro.

7. The system, as recited in claim 3, wherein said first one-axis gyro and said second one-axis gyro is a two-axis gyro.

8. The system, as recited in claim 5, wherein said first one-axis gyro and said second one-axis gyro is a two-axis gyro.

9. A self-calibrated azimuth and attitude accuracy enhancing method, comprising the steps of:

(a) powering on a SAAAEMS (self-calibrated azimuth and attitude accurate enhancing method and system) system and positioning said system on a stationary base;

(b) waiting for a predetermined period of time for gyros to fully start;

(c) getting two channel gyro and two channel accelerometer measurement data;

(d) performing sensor calibration and error compensation for both two channel gyro and two channel accelerometer measurement data;

(e) performing low pass filtering for both two channel gyro and two channel accelerometer measurement data;

(f) calculating the initial SAAAEMS unit attitude and heading/azimuth using said filtered two channel gyro and two channel accelerometer measurement data; and

(g) repeating from step (c) and obtaining real-time continuous SAAAEMS unit initial attitude and heading/azimuth, until said system enters into a dynamic gyrocompassing operation mode.

10. The method as recited in claim 9, wherein the step (f) further comprises the steps of:

(f1) constructing an intermediate vector Iⁿ using local gravity and local earth velocity vectors;

(f2) construct an intermediate vector I^b using said two channel gyro and said two channel accelerometer measurement data;

(f3) constructing an intermediate matrix in the N frame using the local gravity and local earth velocity vectors and constructed intermediate vector Iⁿ;

(f4) constructing an intermediate matrix in the B frame using said two channel gyro and said two channel accelerometer measurement data and constructed intermediate vector I^b;

(f5) testing non-singularity of said constructed intermediate matrix in said N frame;

(f6) calculating DCM (Direction Cosine Matrix) of said B frame with respect to said N frame C_n^b using said constructed intermediate matrix in said N frame and the constructed intermediate matrix in said B frame; and

(f7) calculating the initial quaternion for said SAAAEMS attitude updating algorithms using said DCM of said B frame with respect to said N frame.

11. The method as recited in claim 9, wherein the step (f) further comprises the steps of:

(f1′

) scaling an accelerometer X data;

(f2′

) obtaining a sine function of pitch angle using said scaled accelerometer X data;

(f3′

) testing if the absolute value of said scaled accelerometer X data is less than 1;

(f4′

) discarding said scaled accelerometer X data if it is grater than 1;

(f5′

) obtaining a pitch angle value using said sine function of said pitch angle if said test is passed;

(f6′

) calculating a cosine valued of said pitch angle;

(f7′

) scaling an accelerometer Y data;

(f8′

) obtaining a sine value of a roll angle using said scaled accelerometer Y data and the cosine value of said pitch angle;

(f9′

) testing if the absolute value of said sine value of the roll angle is less than 1;

(f10′

) discarding said absolute value of said sine value if it is grater than 1;

(f11′

) obtaining a roll angle value using the sine function of roll angle if said test is passed;

(f12′

) obtaining a cosine value of said cosine heading/azimuth value using said gyro X measurement data, said local geographical data, and said calculated cosine value and said sine value of said pitch angle;

(f13′

) obtaining a sine value of said cosine heading/azimuth value using said gyro Y measurement data, said local geographical data, said calculated cosine value and said sine value of the roll angle, and said calculated cosine value and said sine value of said pitch angle;

(f14′

) testing if the absolute value of said sine value and said cosine of said heading/azimuth angle is less than 1;

(f15′

) discarding said the data If it is grater than 1;

(f16′

) performing normalization of said sine value and said cosine of said heading/azimuth angle, to reduce the error caused by sensor error or computation if said test is passed;

(f17′

) obtaining a full range (0,

     360) degrees heading/azimuth angle using said sine value and said cosine value of said heading/azimuth angle;

(f18′

) calculating a DCM of said B frame with respect to said N frame;

C_n^b, using said calculated heading/azimuth angle, said roll angle and said pitch; and

(f19′

) calculating an initial quaternion for said SAAAEMS attitude updating algorithms using said DCM of said B frame with respect to said N frame.

12. The method as recited in claim 10, wherein the step (d) further comprises the steps of:

(d1) positioning said SAAAEMS system on a stationary base;

(d2) waiting for a predetermined period of time for gyros to fully start;

(d3) performing ADC to get two channel gyro measurement data;

(d4) processing said gyro measurement data by two (X, Y) low-pass filters;

(d5) averaging said gyro data over a predetermined time period;

(d6) using said standard deviation of said gyro data over a predetermined time period to verify if said SAAAEMS system is really on a stationary base;

(d7) saving said obtained data as group 1;

(d8) moving said SAAAEMS system to a second angular position and on a stationary base;

(d9) repeating step (d2) to step (d6), saving said obtained data as group 2;

(d10) moving said SAAAEMS system to a third angular position and on a stationary base;

(d11) repeating step (d2) to step (d6), saving said obtained data as group 3;

(d12) moving said SAAAEMS system to a forth angular position and on a stationary base;

(d13) repeating step (d2) to step (d6), saving said obtained data as group 4;

(d14) constructing an initial vector r which includes an initial estimates of a gyro bias;

(d15) using said obtained 4 groups data to construct initial F(r) matrix;

(d16) using said obtained 4 groups data to construct an initial

$\frac{\partial <br><br>f}{\partial <br><br>r}$

matrix;

(d17) determining a non-singularity of said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r};<br><br>$

(d18) calculating an inverse matrix thereof if said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r}$

is non-singular;

(d19) obtaining updated vector r using said inverse matrix and F(r);

(d20) using said updated vector r and said obtained 4 groups data to construct said updated F(r) matrix;

(d21) using said updated vector r and said obtained 4 groups data to construct said updated

$\frac{\partial <br><br>f}{\partial <br><br>r}$

matrix;

(d22) determining said non-singularity of said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r};<br><br>$

(d23) calculating an inverse matrix thereof if said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r}$

is non-singular;

(d24) using said inverse matrix and said F(r) matrix to get a new updated vector r; and

(d25) repeating said step (d20) to said step (d24) a predetermined number of times, until r converges to a near constant vector, which are the gyro biases from the calibration procedure.

13. The method as recited in claim 11, wherein the step (d) further comprises the steps of:

(d1) positioning said SAAAEMS system on a stationary base;

(d2) waiting for a predetermined period of time for gyros to fully start;

(d3) performing ADC to get two channel gyro measurement data;

(d4) processing said gyro measurement data by two (X, Y) low-pass filters;

(d5) averaging said gyro data over a predetermined time period;

(d6) using said standard deviation of said gyro data over a predetermined time period to verify if said SAAAEMS system is really on a stationary base;

(d7) saving said obtained data as group 1;

(d8) moving said SAAAEMS system to a second angular position and on a stationary base;

(d9) repeating step (d2) to step (d6), saving said obtained data as group 2;

(d10) moving said SAAAEMS system to a third angular position and on a stationary base;

(d11) repeating step (d2) to step (d6), saving said obtained data as group 3;

(d12) moving said SAAAEMS system to a forth angular position and on a stationary base;

(d13) repeating step (d2) to step (d6), saving said obtained data as group 4;

(d14) constructing an initial vector r which includes an initial estimates of a gyro bias;

(d15) using said obtained 4 groups data to construct initial F(r) matrix;

(d16) using said obtained 4 groups data to construct an initial

$\frac{\partial <br><br>f}{\partial <br><br>r}$

matrix;

(d17) determining a non-singularity of said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r};<br><br>$

(d18) calculating an inverse matrix thereof if said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r}$

is non-singular;

(d19) obtaining updated vector r using said inverse matrix and F(r);

(d20) using said updated vector r and said obtained 4 groups data to construct said updated F(r) matrix;

(d21) using said updated vector r and said obtained 4 groups data to construct said updated

$\frac{\partial <br><br>f}{\partial <br><br>r}$

matrix;

(d22) determining said non-singularity of said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r};<br><br>$

(d23) calculating an inverse matrix thereof if said matrix

$\frac{\partial <br><br>f}{\partial <br><br>r}$

is non-singular;

(d24) using said inverse matrix and said F(r) matrix to get a new updated vector r; and

(d25) repeating said step (d20) to said step (d24) a predetermined number of times, until r converges to a near constant vector, which are the gyro biases from the calibration procedure.

14. The method as recited in claim 9, further comprising the steps of:

(h) using said calculated Ĉ

_bⁿ(t) as an initial value of said C_bⁿ¹(t) DCM based on said static gyrocompasing process as said SAAAEMS system initialization method;

(i) using said initial C_bⁿ¹(t) to transform said calibrated and compensated gyro input data from said B frame to a N1 frame;

(i) using said initial C_bⁿ¹(t) to transform said calibrated and compensated accelerometer input data from said B frame to said N1 frame;

(k) sending X and Y components of said transformed accelerometer input data to SAAAEMS estimator and controller to form a closed-loop control command;

(l) removing an earth angular velocity component from said transformed gyro input data;

(m) adding closed-loop control command from said SAAAEMS estimator and controller to said transformed gyro input data, to form a total SAAAEMS attitude update command;

(n) using said total SAAAEMS attitude update command to update said system attitude by an quaternion attitude update equation;

(o) transforming said updated quaternion attitude to DMC representation and get an updated DMC C_bⁿ¹(t);

(p) looping back to the step (i), using said updated C_bⁿ¹(t) to transform said calibrated and compensated gyro input data from said B frame to said N1 frame;

(q) using said updated C_bⁿ¹(t) to transform said calibrated and compensated accelerometer input data from said B frame to said N1 frame;

(r) repeating said step (i) to said step (q) to obtain real-time continuous C_bⁿ¹(t) updates; and

(s) calculating a real-time continuous SAAAEMS pitch/elevation, roll and azimuth/heading angles for system output during the loop.

15. The method as recited in claim 12, further comprising the steps of:

(h) using said calculated Ĉ

_bⁿ(t) as an initial value of said C_bⁿ¹(t) DCM based on said static gyrocompasing process as said SAAAEMS system initialization method;

(i) using said initial C_bⁿ¹(t) to transform said calibrated and compensated gyro input data from said B frame to a N1 frame;

(j) using said initial C_bⁿ¹(t) to transform said calibrated and compensated accelerometer input data from said B frame to said N1 frame;

(k) sending X and Y components of said transformed accelerometer input data to SAAAEMS estimator and controller to form a closed-loop control command;

(l) removing an earth angular velocity component from said transformed gyro input data;

(m) adding closed-loop control command from said SAAAEMS estimator and controller to said transformed gyro input data, to form a total SAAAEMS attitude update command;

(n) using said total SAAAEMS attitude update command to update said system attitude by an quaternion attitude update equation;

(o) transforming said updated quaternion attitude to DMC representation and get an updated DMC C_bⁿ¹(t);

(p) looping back to the step (i), using said updated C_bⁿ¹(t) to transform said calibrated and compensated gyro input data from said B frame to said N1 frame;

(q) using said updated C_bⁿ¹(t) to transform said calibrated and compensated accelerometer input data from said B frame to said N1 frame;

(r) repeating said step (i) to said step (q) to obtain real-time continuous C_bⁿ¹(t) updates; and

(s) calculating a real-time continuous SAAAEMS pitch/elevation, roll and azimuth/heading angles for system output during the loop.

16. The method as recited in claim 13, further comprising the steps of:

(h) using said calculated Ĉ

_bⁿ(t) as an initial value of said C_bⁿ¹(t) DCM based on said static gyrocompasing process as said SAAAEMS system initialization method;

(i) using said initial C_bⁿ¹(t) to transform said calibrated and compensated gyro input data from said B frame to a N1 frame;

A) using said initial C_bⁿ¹(t) to transform said calibrated and compensated accelerometer input data from said B frame to said N1 frame;

(k) sending X and Y components of said transformed accelerometer input data to SAAAEMS estimator and controller to form a closed-loop control command;

(l) removing an earth angular velocity component from said transformed gyro input data;

(m) adding closed-loop control command from said SAAAEMS estimator and controller to said transformed gyro input data, to form a total SAAAEMS attitude update command;

(n) using said total SAAAEMS attitude update command to update said system attitude by an quatemion attitude update equation;

(o) transforming said updated quaternion attitude to DMC representation and get an updated DMC C_bⁿ¹(t);

(p) looping back to the step (i), using said updated C_bⁿ¹(t) to transform said calibrated and compensated gyro input data from said B frame to said N1 frame;

(q) using said updated C_bⁿ¹(t) to transform said calibrated and compensated accelerometer input data from said B frame to said N1 frame;

(r) repeating said step (i) to said step (q) to obtain real-time continuous C_bⁿ¹(t) updates; and

(s) calculating a real-time continuous SAAAEMS pitch/elevation, roll and azimuth/heading angles for system output during the loop.