CN100405014C - Carrier attitude measurement method and system - Google Patents

Abstract

The present invention discloses a measurement method for carrier attitudes, and a system thereof. The system comprises a sensor and a data processing device, wherein the sensor comprises a triaxial accelerometer, a triaxial magnetometer and a triaxial rate gyro which are respectively use for measuring gravitational acceleration, earth induction strength, and the component of the angular speed of carrier movement on the three axials in a carrier coordinate. During signal processing, kalman filtration is carried out to the measured gravitational acceleration and the, earth induction strength by using the measured the component of carrier rotational angular speed, and the estimated values of component true values of the gravitational acceleration and the earth induction strength are obtained. Finally, the estimated values is used to resolve the carrier attitude. The present invention uses the stable gyro signals to reduce the influence of the movement acceleration to the system, and furthermore, the problem of integrator drift of the gyro does not exist, so the obtained dynamic accuracy and stability of the carrier attitude is better than that of the existing attitude measurement systems. The sensor in the present invention adopts an integrative MEMS chip compositive packaging technique, which makes the instrument more smaller.

Description

A kind of carrier posture measuring method

Technical field

The present invention relates to attitude measurement field, in particular, the present invention relates to a kind of attitude measurement that is used for carriers such as aircraft.

Background technology

Attitude measurement is meant that utilizing various sensors to carry out Measurement and analysis obtains the attitude data of carriers such as aircraft with respect to reference data, and said here attitude data is often referred to the attitude angle of carrier, comprises course angle ψ, pitching angle theta and roll angle γ.Existing attitude measurement method mainly comprises following two kinds:

(1) sensor is made up of three rate gyros, and what rate gyro measured is the angular velocity of carrier movement, must carry out integral operation to it and just can obtain attitude angle.But there is serious zero point drift problem in micro electronmechanical rate gyro, and therefore the attitude angle that obtains through integral and calculating will be no longer accurate, and this measuring system is influenced seriously by gyroscopic drift.

(2) the applicant provides a kind of method of the sensor measurement attitude of carrier of being made up of three axis magnetometer and three axis accelerometer in application number is 01110135.0 Chinese patent " based on the miniature navigation system of micro-electromechanical technology ", and this patent is incorporated herein by reference in this application.In the method, measure the component of gravity acceleration g three orthogonal axes on carrier coordinate system with three axis accelerometer, measure the component of earth induction intensity h three orthogonal axes on carrier coordinate system with three axis magnetometer, the expression in geographic coordinate system according to gravity acceleration g and earth induction intensity h, utilize the direction cosine matrix of mutual transformational relation between statement geographic coordinate system and the carrier coordinate system to set up system of equations, try to achieve the attitude angle of carrier at last.When carrier with certain attitude easy motion the time, the attitude angle precision of utilizing this method to measure is higher.But when carrier during with acceleration movement or when in motion process, being disturbed, for example aircraft is subjected to airflow influence and when having certain acceleration in flight course, then both comprised that acceleration of gravity also included the acceleration of motion of carrier in the acceleration that three axis accelerometer is measured, the attitude angle that utilize this method to calculate this moment just has sizable error.

Therefore just need a kind of method and can when doing accelerated motion, eliminate of the influence of the acceleration of motion of carrier attitude measurement.

Summary of the invention

The shortcoming and defect that is to overcome prior art of the present invention provides a kind of carrier posture measuring method and system thereof, with the influence to attitude measurement of the acceleration of motion of eliminating carrier.

To achieve these goals, the invention provides a kind of carrier posture measuring method, comprise and measure acceleration of gravity and the earth induction intensity component on three of carrier coordinate system; Also comprise and measure the component [ω of carrier rotational angular velocity on three of carrier coordinate system _xω _yω _z]; The carrier rotational angular velocity component that utilization records carries out Kalman filtering to acceleration of gravity and the earth induction strength component that records, and obtains the estimated value of acceleration of gravity and earth induction strength component true value; Carrying out attitude of carrier with described estimated value at last resolves; Wherein,

The state equation of described Kalman filtering is $\stackrel{\·}{S}=\mathrm{\ΩS}+W;$

The measurement equation of described Kalman filtering is Z=HS+V;

S and Z are respectively described acceleration of gravity and the earth induction intensity component [X on three of carrier coordinate system _gY _gZ _gX _mY _mZ _m] true value and measuring value,

Be the derivative of described true value to the time, H is for measuring battle array, and V is a measurement noise, and W is the system incentive noise,

$\mathrm{\Ω}=\left[\begin{array}{cc}-{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}& 0\\ 0& -{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right].$

The present invention also provides a kind of attitude of carrier measuring system, comprises sensor and data processing equipment, and the signal that described data processing equipment records according to described sensor carries out attitude of carrier and resolves; Described sensor comprises:

Along the three axis accelerometer of three layouts of carrier coordinate system, be used to measure the component of acceleration of gravity on three of carrier coordinate system;

Along the three axis magnetometer of three layouts of carrier coordinate system, be used to measure the component of earth induction intensity on three of carrier coordinate system;

Along three rate gyros of three layouts of carrier coordinate system, be used to measure the component of carrier acceleration on three of carrier coordinate system.

Described accelerometer is micro electronmechanical accelerometer, and described magnetic strength is counted micro electronmechanical magnetometer, and described rate gyro is micro electronmechanical rate gyro.

Described attitude of carrier measuring system also comprises first filtering circuit and first amplifying circuit, and the output signal of described three axis accelerometer enters described data processing equipment through A/D converter again after first filtering circuit and the first amplifying circuit filtering and amplifying.

Described attitude of carrier measuring system also comprises second filtering circuit and second amplifying circuit, and the output signal of described three axis magnetometer enters described data processing equipment through A/D converter again after second filtering circuit and the second amplifying circuit filtering and amplifying.

Described attitude of carrier measuring system also comprises the 3rd filtering circuit, the output signal of described three rate gyros through the 3rd filtering circuit filtering after A/D converter enters described data processing equipment.

Described data processing equipment comprises Kalman filtering module and attitude algorithm module; Wherein:

The Kalman filtering module, the component [ω of the carrier rotational angular velocity that utilization records _xω _yω _z] to the acceleration of gravity that records and the component [X of earth induction intensity _gY _gZ _gX _mY _mZ _m] carry out Kalman filtering, export the estimated value of acceleration of gravity and earth induction strength component true value; Wherein,

The state equation of described Kalman filtering is $\stackrel{\·}{S}=\mathrm{\ΩS}+W;$

The measurement equation of described Kalman filtering is Z=HS+V;

S and Z are respectively described acceleration of gravity and the earth induction intensity component [X on three of carrier coordinate system _gY _gZ _gX _mY _mZ _m] true value and measuring value,

Be the derivative of described true value to the time, H is for measuring battle array, and V is a measurement noise, and W is the system incentive noise,

$\mathrm{\Ω}=\left[\begin{array}{cc}-{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}& 0\\ 0& -{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right];$

The attitude algorithm module is used for carrying out resolving of attitude of carrier according to the estimated value of the acceleration of gravity of Kalman filtering module output and the earth induction intensity component true value on three of carrier coordinate system.

The present invention is merged existing attitude measurement system, has obtained a novel attitude measurement system that comprises nine sensors (three axis accelerometer, three axis magnetometer and three rate gyros).It adopts and comprises that the algorithm of Kalman filtering inherence carries out data processing, reduce the influence of acceleration of motion with stable gyro signal to system, and do not have the integrator drift problem of gyro, thereby attitude of carrier dynamic accuracy that obtains and stability are better than existing attitude measurement system.The present invention can realize the full attitude measurement of carrier, and promptly angle of pitch measurement range is-90 °～+ 90 °, and the rolling angle measurement scope is-180 °～+ 180 °, and the course angle measurement range is 0 °～360 °.Further, the sensor among the present invention adopts the integrated encapsulation technology of integrated MEMS chip, makes instrument miniaturization more.

Description of drawings

Fig. 1 is geographic coordinate system and carrier coordinate system synoptic diagram, and wherein (a) is geographic coordinate system, (b) is carrier coordinate system;

Fig. 2 is the structural representation of attitude of carrier measuring system of the present invention;

Fig. 3 is the distribution schematic diagram of the sensor in the attitude of carrier measuring system of Fig. 2;

Fig. 4 is the process flow diagram that the data processing equipment in the attitude of carrier measuring system of Fig. 2 carries out signal Processing.

Embodiment

Below in conjunction with the drawings and specific embodiments the present invention is described in further detail.

In the embodiment of an exemplary, the foundation of coordinate system as shown in Figure 1.Fig. 1 (a) is to be that initial point is set up geographic coordinate system north-Dong-ground (being N-E-D) that three orthogonal axes are formed with the carrier position, and wherein the N axle is to point to the positive north by carrier, and the E axle is to point to positive east by carrier, and the D axle is to point to the earth's core by carrier.Fig. 1 (b) is three quadrature carrier coordinate system X-Y-Z that are fixed on carrier 100, and wherein X-axis is in the carrier symmetrical plane, points to the carrier movement forward direction by the carrier barycenter, and Y-axis is perpendicular to the carrier symmetrical plane and point to right-hand; The Z axle is with the example of aircraft as carrier 100 among Fig. 1 (b) in the carrier symmetrical plane and perpendicular to the X-axis directed downwards.

Attitude measurement system of the present invention as shown in Figure 2, it is arranged in as on the carrier among Fig. 1 100.Sensor in this attitude measurement system comprises: the rate gyro 30 of the magnetometer of the accelerometer of three quadratures 10, three quadratures 20, three quadratures.The distribution of sensor on carrier 100 as shown in Figure 3, the accelerometer 10 of three quadratures comprises first accelerometer 11, second accelerometer 12 and the 3rd accelerometer 13, these three accelerometers 11,12 and 13 are used for measuring respectively the component of acceleration of gravity on three orthogonal axes X-Y-Z of carrier coordinate system respectively along three orthogonal axes X-Y-Z configured in parallel of carrier coordinate system.The magnetometer 20 of three quadratures comprises first magnetometer 21, second magnetometer 22 and the 3rd magnetometer 23, these three magnetometers 21,22 and 23 respectively with three orthogonal axes X-Y-Z configured in parallel of carrier coordinate system, be used for measuring respectively the component of earth induction intensity on three orthogonal axes X-Y-Z of carrier coordinate system.The rate gyro 30 of three quadratures comprises first rate gyro 31, second rate gyro 32 and third speed gyro 33, these three rate gyros 31,32 and 33 respectively with three orthogonal axes X-Y-Z configured in parallel of carrier coordinate system, be used for measuring respectively the component of carrier rotational angular velocity on three orthogonal axes X-Y-Z.

In order to allow attitude measurement system 200 be applicable to minute vehicle, in the present invention, sensor has adopted that volume is little, light weight, silicon microstructure sensor that power consumption is little, and this sensor is to use micromachining technologies such as photoetching, burn into deposition, ion injection, bonding to produce on electrooptical material chips such as monocrystalline silicon, quartz crystal, lithium niobate.Specifically, in the present invention, accelerometer 10 is the micro electronmechanical accelerometer of three quadratures, and magnetometer 20 is the micro electronmechanical magnetometer of three quadratures, and rate gyro 30 is the micro electronmechanical rate gyro of three quadratures, is commercially available product.

Return Fig. 2, accelerometer 10, magnetometer 20 and rate gyro 30 are output as the voltage signal of simulation, carry out filtering through first filtering circuit 14, second filtering circuit 24 and 34 pairs of signals of the 3rd filtering circuit respectively, be used for the high frequency noise of elimination sensor output signal.Be converted into digital signal from the signals of filtering circuit 14 and 24 outputs through being amplified into A/D converter 40 through first amplifying circuit 15 and second amplifying circuit 25 respectively.Because the voltage output range of rate gyro 30 has generally satisfied the input requirement of A/D converter 40, so the signal of filtering circuit 34 outputs directly enters A/D converter 40 and is converted into digital signal.

Digital data transmission to one a data treating apparatus 50 of A/D converter 40 outputs carries out the calculating of attitude of carrier.Wherein, data processing equipment 50 can be a little shape processor with embedded program, carries out the calculating of attitude of carrier by this embedded program, and this is applicable to that carrier 100 is the situation of minute vehicle; Data processing equipment 50 also can be a PC, carries out the calculating of attitude of carrier by the computer program of PC stored, and this is applicable to that the volume and weight to attitude measurement system 200 requires not tight carrier, for example large-scale aircraft.Particularly, when data processing equipment 50 is PC, A/D converter 40 can be by data transmission module (not shown) with digital data transmission to PC, this data transmission module can be USB transport module, serial ports transport module etc.

As previously mentioned, attitude measurement system carries out the attitude of carrier Program for Calculation and is loaded in the data processing equipment 50, and this program comprises data acquisition module, signal fused module, Kalman filtering module and attitude algorithm module, and is undertaken by process flow diagram shown in Figure 4.

As shown in Figure 4, in step 401, data acquisition module control data treating apparatus 50 receives the digital signal that comes from A/D converter 40, and this signal is the digital voltage signal that voltage signal that accelerometer 10, magnetometer 20 and rate gyro 30 record obtains after analog to digital conversion.

In step 402, be that the method for 01110135.0 patent is identical with the application number of being quoted, adopt a signal fused module that signal is handled.This signal fused module comprises signal filtering unit and signal conversion unit.Wherein the signal filtering unit carries out filtering to the digital signal that the data acquisition module obtains, and is used for the high frequency noise of further filtering sensor signal, reduces the influence of noise to attitude measurement.Digital signal filter adopts finite impulse corresponding (FIR) digital filtering, i.e. the output of digital filtering depend on limited in the past input and present input x (n), x (n-1) ..., x (n-N+1).Adopted secondary filtering in the present invention, the input and output relation of its filtering is:

y(n)＝h ₀x(n)+h ₁x(n-1)

H in the formula ₀And h ₁Be filter factor.

Signal conversion unit will be converted into physical quantity corresponding by digital voltage signal behind the signal filtering unit filtering, be about to the digital signal that the digital voltage signal corresponding with accelerometer 10 is converted into the expression acceleration, the digital signal that will the digital voltage signal corresponding with magnetometer 20 be converted into expression magnetic induction density will the digital voltage signal corresponding with rate gyro 30 be converted into the digital signal of expression angular velocity.It handles formula: u (n)=(v (n)-v ₀)/k _vV in the formula (n) is the voltage signal of input, and u (n) is a physical quantity corresponding, v ₀Be zero-point voltage, k _vFor changing amplitude, the zero point of each sensor and amplitude are determined by demarcating in advance.

In step 403, the acceleration of gravity that the angular velocity that the Kalman filtering module records with rate gyro 30 records accelerometer 10 and magnetometer 20 and the component of earth induction intensity carry out Kalman filtering.This step is core of the present invention, and for this step clearly is described, set: the component of the earth induction intensity that records of three axis magnetometer on carrier coordinate system X, Y and Z axle is respectively X _m, Y _mAnd Z _mThe gravitational acceleration component that record of three axis accelerometer on carrier coordinate system X, Y and Z axle is respectively X _g, Y _gAnd Z _gThe carrier rotational angular velocity component that record of three rate gyros on carrier coordinate system X, Y and Z axle is respectively ω _x, ω _yAnd ω _z

In the present invention, the state equation of Kalman filtering algorithm and measurement equation design at characteristics of the present invention, can calculate the attitude angle of compliance with system performance requirement.State equation and measurement equation are as follows respectively:

$\stackrel{\·}{S}=\mathrm{\ΩS}+W$ (state equation), Z=HS+V (measurement equation).

Wherein S and Z are respectively earth induction intensity and the component [X of acceleration of gravity on carrier coordinate system _gY _gZ _gX _mY _mZ _m] true value and measuring value,

Be the derivative of described true value to the time, H is for measuring battle array (getting office's battle array in the present embodiment), and V is a measurement noise, and W is system incentive noise (for convenience of calculation, setting W=0 in the present embodiment),

$\mathrm{\Ω}=\left[\begin{array}{cc}-{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}& 0\\ 0& -{\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}\end{array}\right],$ ${\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bk}}=\left[\begin{array}{ccc}0& -{\mathrm{\ω}}_z& {\mathrm{\ω}}_y\\ {\mathrm{\ω}}_z& 0& -{\mathrm{\ω}}_x\\ -{\mathrm{\ω}}_y& {\mathrm{\ω}}_x& 0\end{array}\right].$

In the present invention, the state equation of Kalman filtering algorithm has been described the time dependent rule of true value S of earth induction intensity and the component of acceleration of gravity on carrier coordinate system, and visible true value S will be subjected to by carrier rotation acceleration [ω _xω _yω _z] influence of the matrix Ω that forms.The measurement equation of Kalman filtering algorithm has been described the measuring value Z of earth induction intensity and the component of acceleration of gravity on carrier coordinate system and the relation of true value S.In the present invention, the input value of Kalman filtering process is the measuring value Z of current time actual measurement, and in conjunction with the estimation of previous moment to true value S, the estimation of finally exporting current time true value S

The state equation of the Kalman filtering algorithm that provides according to the present invention and measurement equation, those skilled in the art can carry out Kalman filtering to signal.Specific implementation with a Kalman filtering is that example is described in detail below:

Kalman filtering state equation need be carried out the discretize processing, obtains following discrete state equations:

S _k＝φ _k，k-1S _k-1

Wherein ${\mathrm{\φ}}_{k,k-1}=I+\mathrm{T\Ω}+\frac{{\mathrm{\ΩT}}^2}{2}+\·\·\·,$ Represent that with subscript k k constantly.

Carry out Kalman filtering, S according to universal card Kalman Filtering process _kEstimation

Press following equation solution: the state one-step prediction

${\hat{S}}_{k,k-1}={\mathrm{\φ}}_{k,k-1}{\hat{S}}_{k-1}$

According to current measuring value Z _kAnd predicted state Estimate the current time estimated value

${\hat{S}}_k={\hat{S}}_{k,k-1}+K_k(Z_k-H_k{\hat{S}}_{k,k-1})$

Filter gain $K_k=P_{k,k-1}{H}_k^T{(H_k{P}_{k,k-1}{H}_k^T+R_k)}^{-1},$ The one-step prediction square error $P_{k,k-1}={\mathrm{\φ}}_{k+1,k}{P}_{k-1}{\mathrm{\φ}}_{k+1,k}^T,$ Estimate square error $P_k=(I-K_k{H}_k)P_{k,k-1}{(1-K_k{H}_k)}^T+K_k{R}_k{K}_k^T,$ R _kVariance battle array for measurement noise V has relation $\mathrm{Cov}[V_k,V_j]=E\left[V_k{V_j}^T\right]=R_k{\mathrm{\δ}}_{\mathrm{kj}}.$

Kalman filtering process by above-mentioned can obtain [X _gY _gZ _gX _mY _mZ _m] at k true value S constantly _kEstimated value

One of ordinary skill in the art is appreciated that, behind the state equation of given Kalman filtering and measurement equation, can carry out signal filtering according to the thought of Kalman filtering, but can do various variations to detailed process in the specific implementation, the present invention is not limited to aforementioned concrete realization.

In step 404, the estimated value that the Attitude Calculation module obtains after according to Kalman filtering

Carry out attitude of carrier and resolve, here attitude to resolve with the application number of being quoted be that the method for 01110135.0 patent is identical, the difference part is to carry out in the present invention attitude algorithm and is the physical quantity through obtaining after the Kalman filtering.In the following description, still use [X under the situation about obscuring not causing _gY _gZ _gX _mY _mZ _m] k that after Kalman filtering, obtains of the expression estimated value of each physical quantity constantly.Specifically describe as follows:

The attitude available support coordinate system of carrier in the space represents that with respect to the motion of geographic coordinate system movement angle is called the attitude angle of carrier.Course angle ψ commonly used, pitching angle theta and roll angle γ were as the attitude angle of carrier during navigation was learned, two coordinate systems overlap (N is corresponding with X-axis, E and Y-axis, D and Z axle) when initial, carrier is around the capable partially ψ angle of D axle (Z), again around horizontal Y ' axle pitching θ angle, at last around X subsequently " axle lift-over γ angle.Vector in carrier coordinate system and the geographic coordinate system can be changed mutually by following direction cosine matrix:

$C_n^b=\left[\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}& \sin \mathrm{\ψ}\cos \mathrm{\θ}& \cos \mathrm{\ψ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}\\ \cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& \cos \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}\\ -\cos \mathrm{\θ\γ}\sin & \sin \mathrm{\θ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right]={\left[T_{\mathrm{ij}}\right]}_{3\×3}$

Its subscript b represents carrier coordinate system, and subscript n is represented geographic coordinate system, is matrix T _IjIn the capable j column element of i.

Terrestrial magnetic field h and the gravity acceleration g expression direction of passage cosine matrix C in geographic coordinate system and attitude frame of reference _n ^bChange, resolve formula thereby obtain following attitude angle, wherein h is an earth induction intensity, and β is an earth's magnetic dip angle.

$\left[\begin{array}{c}{X}_m\\ Y_m\\ Z_m\end{array}\right]={\mathrm{hC}}_n^b\left[\begin{array}{c}\cos \mathrm{\&beta;}\\ 0\\ \sin \mathrm{\&beta;}\end{array}\right]\&DoubleRightArrow;\left\{\begin{array}{c}h\&times;(T_{11}\cos \mathrm{\&beta;}+{\mathrm{<figure-callout\; id="13"\; label="T"\; filenames="C20041000466000121.png"\; state="\{\{state\}\}">T</figure-callout>}}_{13}\sin \mathrm{\&beta;})=X_m\\ h\&times;({\mathrm{<figure-callout\; id="21"\; label="T"\; filenames="C20041000466000121.png"\; state="\{\{state\}\}">T</figure-callout>}}_{21}\cos \mathrm{\&beta;}+{\mathrm{<figure-callout\; id="23"\; label="T"\; filenames="C20041000466000121.png"\; state="\{\{state\}\}">T</figure-callout>}}_{23}\sin \mathrm{\&beta;})=Y_m\\ h\&times;({\mathrm{<figure-callout\; id="31"\; label="T"\; filenames="C20041000466000121.png"\; state="\{\{state\}\}">T</figure-callout>}}_{31}\cos \mathrm{\&beta;}+{\mathrm{<figure-callout\; id="33"\; label="T"\; filenames="C20041000466000121.png"\; state="\{\{state\}\}">T</figure-callout>}}_{33}\sin \mathrm{\&beta;})=Z_m\end{array}\right.$

$\left[\begin{array}{c}{X}_g\\ Y_g\\ Z_g\end{array}\right]=C_n^b\left[\begin{array}{c}0\\ 0\\ g\end{array}\right]\&DoubleRightArrow;\left\{\begin{array}{c}{x}_g=T_{13}g\\ y_g=T_{23}g\\ z_g=T_{33}g\end{array}\right.$

Eliminate of the influence of carrier movement acceleration though adopted Kalman filtering in the present invention to attitude measurement, but by the attitude measurement system that has nine sensors (three axis accelerometer, three axis magnetometer and three rate gyros) provided by the invention, the attitude measurement algorithm that those skilled in the art also can design other carries out the high-accuracy posture measurement, therefore attitude measurement system of the present invention is not limited to use attitude measurement method provided by the invention, and other measuring method also goes for attitude measurement system of the present invention.

Claims (1)

1. carrier posture measuring method comprises and measures acceleration of gravity and the earth induction intensity component on three of carrier coordinate system; It is characterized in that, also comprise and measure the component [ω of carrier rotational angular velocity on three of carrier coordinate system _xω _yω _z]; The carrier rotational angular velocity component that utilization records carries out Kalman filtering to acceleration of gravity and the earth induction strength component that records, and obtains the estimated value of acceleration of gravity and earth induction strength component true value; Carrying out attitude of carrier with described estimated value at last resolves; Wherein,

The state equation of described Kalman filtering is $\stackrel{\&CenterDot;}{S}=\mathrm{\&Omega;S}+W;$

The measurement equation of described Kalman filtering is Z=HS+V;

S and Z are respectively described acceleration of gravity and the earth induction intensity component [X on three of carrier coordinate system _gY _gZ _gX _mY _mZ _m] true value and measuring value,

Be the derivative of described true value to the time, H is for measuring battle array, and V is a measurement noise, and W is the system incentive noise,

$\mathrm{\&Omega;}=\left[\begin{array}{cc}-{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{bk}}& 0\\ 0& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{bk}}\end{array}\right],$ ${\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{bk}}=\left[\begin{array}{ccc}0& -{\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z& 0& -{\mathrm{\&omega;}}_x\\ -{\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_x& 0\end{array}\right].$

Images