CN103499348A - High-precision attitude data calculation method for AHRS (Attitude and Heading Reference System) - Google Patents

Abstract

The invention discloses a high-precision attitude data calculation method for an AHRS (Attitude and Heading Reference System), mainly solving the problems that as an accelerometer and a gyroscope are finite in precisions and have accumulative errors, the obtained attitude data errors are large and cannot meet technical requirements in the prior art. The high-precision attitude data calculation method comprises the following steps of initializing a sensor and a normalized coordinate system transfer matrix; calculating the attitude data and the coordinate system transfer matrix R1 of a carrier at the moment; calculating a coordinate system transfer matrix R2 at the moment; determining a final coordinate system transfer matrix at the moment, and normalizing the final coordinate system transfer matrix, so as to obtain the normalized coordinate system transfer matrix at the moment; solving the attitude data at the moment according to the normalized coordinate system transfer matrix obtained in the step (4); and returning to the step (2), and calculating the attitude data of the next moment. By utilizing the scheme, the high-precision attitude data calculation method is convenient to implement, is high in precision and has very high practical and promotional values.

Description

AHRS high-precision attitude method for computing data

Technical field

The present invention relates to a kind of attitude data computing method of navigation field, specifically, relate to a kind of AHRS high-precision attitude method for computing data.

Background technology

As everyone knows, inertial navigation relates generally to the content of two aspects: locate and determine appearance, location can complete by Global Positioning System (GPS) (as GPS, the Big Dipper etc.); Determine appearance mainly three axis angular rate information exchanges of the initial angle information of degree of will speed up meter output and gyroscope output cross integral and calculating and obtain current attitude data (mainly comprising deflection, the angle of pitch and roll angle).Due to accelerometer and gyrostatic precision limited, and have cumulative errors, thereby resulting attitude data error is larger, can not meet technical need.

Summary of the invention

The object of the present invention is to provide a kind of AHRS high-precision attitude method for computing data, mainly solve in prior art, exist due to accelerometer and gyrostatic precision limited, and have cumulative errors, thereby resulting attitude data error is larger, can not meet the technical need problem.

To achieve these goals, the technical solution used in the present invention is as follows:

AHRS high-precision attitude method for computing data comprises the following steps:

(1) sensor in AHRS and normalization Conversion Matrix of Coordinate are carried out to initialization process;

(2) read the data of accelerometer and magnetometer in AHRS, calculate the attitude data of carrier and Conversion Matrix of Coordinate R now ₁;

(3) read gyrostatic data in AHRS, according to the normalization Conversion Matrix of Coordinate in a upper moment, calculated the Conversion Matrix of Coordinate R in this moment ₂;

(4) according to Conversion Matrix of Coordinate R ₁with Conversion Matrix of Coordinate R ₂determine the Conversion Matrix of Coordinate that this moment is final, and final Conversion Matrix of Coordinate is carried out to normalization, obtain the normalization Conversion Matrix of Coordinate in this moment;

(5) solve final attitude data of this moment according to the normalization Conversion Matrix of Coordinate drawn in step (4);

(6) return to step (2), carry out next attitude data constantly and calculate.

Described step (1) specifically comprises:

(1a) parameters of sensors configured;

(1b) according to the output data initialization normalization Conversion Matrix of Coordinate R of accelerometer now and magnetometer _normalized.

Described step (1b) comprises the following steps:

(1b1) read carrier coordinate system at the acceleration accel_x of X axis, at the acceleration accel_y of Y-axis, at the acceleration accel_z of Z-axis direction from accelerometer; Read carrier coordinate system at the magnetic field intensity magnetom_x of X axis, at the magnetic field intensity magnetom_y of Y-axis, at the magnetic field intensity magnetom_z of Z-axis direction from magnetometer;

(1b2) according to formula calculate pitching angle theta; According to formula

calculate roll angle φ; According to formula

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ Calculated direction angle ψ;

Correspondingly, described normalization Conversion Matrix of Coordinate R _normalizedpass through formula

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ Complete initialization.

Described step (2) specifically comprises the following steps:

(2a) read carrier coordinate system at the acceleration accel_x of X axis, at the acceleration accel_y of Y-axis, at the acceleration accel_z of Z-axis direction from accelerometer; Read carrier coordinate system at the magnetic field intensity magnetom_x of X axis, at the magnetic field intensity magnetom_y of Y-axis, at the magnetic field intensity magnetom_z of Z-axis direction from magnetometer;

(2b) according to formula

calculate the pitching angle theta of current time; According to formula calculate the roll angle φ of current time; According to formula

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ Calculate the deflection ψ of current time, wherein, the angle of pitch that θ is current time; The roll angle that φ is current time;

According to formula $R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ Draw the Conversion Matrix of Coordinate R in this moment ₁, wherein, the angle of pitch that θ is current time; The roll angle that φ is current time; The deflection that ψ is current time.

Described step (3) specifically comprises the following steps:

(3a) read the angular velocity omega of carrier coordinate system at X axis from gyroscope _x, at the angular velocity omega of Y-axis _y, at the angular velocity omega of Z-axis direction _z;

(3b) according to formula $\begin{array}{c}{\mathrm{d\θ}}_x={\mathrm{\ω}}_x\mathrm{dt}\\ {\mathrm{d\θ}}_y={\mathrm{\ω}}_y\mathrm{dt}\\ {\mathrm{d\θ}}_z={\mathrm{\ω}}_z\mathrm{dt}\end{array}$ And formula $R_2(t+\mathrm{dt})=R_{\mathrm{normalized}}\left(t\right)\left[\begin{array}{ccc}1& -{\mathrm{d\θ}}_z& {\mathrm{d\θ}}_y\\ {\mathrm{d\θ}}_z& 1& -{\mathrm{d\θ}}_x\\ -{\mathrm{d\θ}}_y& {\mathrm{d\θ}}_x& 1\end{array}\right]$ Draw the Conversion Matrix of Coordinate R in this moment ₂.

Described step (4) specifically comprises the following steps:

(4a) according to the Conversion Matrix of Coordinate R calculated ₁with Conversion Matrix of Coordinate R ₂calculate final Conversion Matrix of Coordinate R=R of this moment ₁+ λ (R ₂-R ₁), wherein, λ is the Conversion Matrix of Coordinate correction factor, and 0≤λ≤1;

(4b) according to following formula:

$R^T=\left[\begin{array}{ccc}X& T& Z\end{array}\right]=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{yx}}& r_{\mathrm{zx}}\\ r_{\mathrm{xy}}& r_{\mathrm{yy}}& r_{\mathrm{zy}}\\ r_{\mathrm{xz}}& r_{\mathrm{yz}}& r_{\mathrm{zz}}\end{array}\right];$ $\mathrm{err}=X\·Y=X^{T}Y=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{xy}}& r_{\mathrm{xz}}\end{array}\right]\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right];$

${\left[\begin{array}{c}{r}_{\mathrm{xx}}\\ r_{\mathrm{xy}}\\ r_{\mathrm{xz}}\end{array}\right]}_{\mathrm{orth}}=X_{\mathrm{orth}}=X-\frac{\mathrm{err}}{2}Y;{\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right]}_{\mathrm{orth}}=Y_{\mathrm{orth}}=Y-\frac{\mathrm{err}}{2}X;{\left[\begin{array}{c}{r}_{\mathrm{zx}}\\ r_{\mathrm{zy}}\\ r_{\mathrm{zz}}\end{array}\right]}_{\mathrm{orth}}=Z_{\mathrm{orth}}=X_{\mathrm{orth}}\×Y_{\mathrm{orth}};$

$R_{\mathrm{normalized}}=\left[\begin{array}{ccc}{X}_{\mathrm{normalized}}& Y_{\mathrm{normalized}}& Z_{\mathrm{normalized}}\end{array}\right];X_{\mathrm{normalized}}=\frac{1}{2}(3-X_{\mathrm{orth}}\·X_{\mathrm{orth}})X_{\mathrm{orth}};$

$Y_{\mathrm{normalized}}=\frac{1}{2}(3-Y_{\mathrm{orth}}\·Y_{\mathrm{orth}})Y_{\mathrm{orth}};Z_{\mathrm{normalized}}=\frac{1}{2}(3-Z_{\mathrm{orth}}\·Z_{\mathrm{orth}})Z_{\mathrm{orth}};$ The Conversion Matrix of Coordinate R final to this moment carries out normalization, draws this moment normalization Conversion Matrix of Coordinate R _normalized.

In described step (5), this constantly final attitude data is determined by final pitching angle theta, roll angle φ and the deflection ψ in this moment, and this final pitching angle theta, roll angle φ and deflection ψ constantly is according to formula

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

And the normalization coordinate system that step (4b) calculates is changed square R _normalizedbattle array is tried to achieve.

Compared with prior art, the present invention has following beneficial effect:

(1) the present invention is with accelerometer, the data source that magnetometer and gyroscope obtain is basis, independently calculate respectively the coordinate system transition matrix of carrier, and to take the coordinate system transition matrix that accelerometer and magnetometer calculate be basis, the coordinate system transition matrix calculated by gyroscope is rectified a deviation to it, to promote the precision of coordinate transition matrix, and then greatly promoted the precision of final carriage data, and only use the coordinate system transition matrix calculated by gyroscope to assist correction can effectively reduce the impact that gyroscope cumulative errors etc. is brought, more contribute to the lifting of final carriage data precision, design very ingenious, meet technical need.

(2) the new A HRS high-precision attitude method for computing data that the present invention realizes can make the accuracy of attitude determination of inertial navigation be significantly improved, reach better inertial navigation effect, made major contribution to inertial navigation being applied in the fields that need accurate attitude data more, there is outstanding substantive distinguishing features and marked improvement, be applicable to large-scale promotion application.

The accompanying drawing explanation

Fig. 1 is earth axes and carrier coordinate system angle schematic diagram in embodiments of the invention.

Fig. 2 is schematic flow sheet of the present invention.

Embodiment

Below in conjunction with drawings and Examples, the invention will be further described, and embodiments of the present invention include but not limited to the following example.

Embodiment

For solve in prior art, exist due to accelerometer and gyrostatic precision limited, and there is cumulative errors, thereby resulting attitude data error is larger, can not meet the technical need problem, as shown in Figure 1, adopt two different coordinate systems in the present invention: earth axes (also claiming sky, northeast coordinate system) and carrier coordinate system.

Earth axes refers to the scope interior (in tens kilometers) of close together at the earth's surface;on the face of the globe, the earth surface level can be similar to and be considered as a plane, thereby can take the due east direction as the x positive axis, direct north is the y positive axis, perpendicular to ground, be the z positive axis facing up, form earth axes, namely sky, so-called northeast coordinate system;

Carrier coordinate system is based on a kind of coordinate system that meets the right-hand rule that carrier is set up, and carrier coordinate system be take the carrier barycenter as initial point, and the x axle overlaps with the carrier longitudinal axis, and the sensing carrier header is for just; The y axle overlaps with the carrier transverse axis, points to the carrier right side for just; The z axle is perpendicular to bearer plane, points to the carrier belly for just.

When attitude of carrier changes, carrier coordinate system will rotate with respect to earth axes.As shown in Figure 1, (xe, ye, ze) mean earth axes, (xb, yb, zb) mean carrier coordinate system, in Fig. 1, ground coordinate ties up to the xeOye plane internal rotation and turns the ψ degree, turn the θ degree at the xbOze plane internal rotation, after the ybOzb plane internal rotation turns the φ degree, can obtain the carrier coordinate system in Fig. 1, now, title ψ is deflection, and θ is the angle of pitch, and φ is roll angle.The present invention uses these 3 angles (deflection, the angle of pitch and roll angle) to measure the current attitude of carrier, and usings that this exports as attitude data.

If Q _b=[Q _bxq _byq _bz] ^t, Q _e=[Q _exq _eyq _ez] ^tmean respectively the coordinate vector under carrier coordinate system and earth axes, Q is arranged _b=RQ _e.Wherein R is called Conversion Matrix of Coordinate, and satisfied (formula 1): $R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ (formula 1)

In the present invention, [] ^tthe transposition of representing matrix; r _xxfor matrix R ^tthe element of the 1st row the 1st row, r _xyfor matrix R ^tthe element of the 2nd row the 1st row, by that analogy, r _zzfor matrix R ^tthe element of the 3rd row the 3rd row; X=[r _xxr _xyr _xz] ^t, Y=[r _yxr _yyr _yz] ^t, Z=[r _zxr _zyr _zz] ^t; X _orth, Y _orth, Z _orthrespectively X, Y, the orthogonal vector that Z is corresponding; X _normalized, Y _normalized, Z _normalizedrespectively X, Y, the normalized vector that Z is corresponding; XY means the dot product of vectorial X and vectorial Y, and X * Y means the multiplication cross of vectorial X and vectorial Y.

As shown in Figure 2, the invention discloses a kind of new A HRS high-precision attitude method for computing data, overall plan is as follows:

Step 1, by the sensors configured parameters, and according to the output data of accelerometer and magnetometer now, initialization normalization Conversion Matrix of Coordinate R _normalizedrealize the initialization of sensor and normalization Conversion Matrix of Coordinate;

Wherein, normalization Conversion Matrix of Coordinate R _normalizedinitialization is by reading acceleration accel_x, accel_y and the accel_z of 3 directions of carrier coordinate system x-axis, y-axis and z-axis from accelerometer, read magnetic field intensity magnetom_x, magnetom_y and the magnetom_z of three directions of carrier coordinate system x-axis, y-axis and z-axis from magnetometer, and calculate pitching angle theta, roll angle φ and deflection ψ according to (formula 2), (formula 3) and (formula 4), afterwards the pitching angle theta, roll angle φ and the deflection ψ substitution (formula 1) that calculate are completed.(formula 2), (formula 3) and (formula 4) are as follows:

$\tan \left(\mathrm{\θ}\right)=-\frac{\mathrm{accel}\_x}{\sqrt{\mathrm{accel}\_y^2+\mathrm{accel}\_z^2}}$ (formula 2)

$\tan \left(\mathrm{\φ}\right)=\frac{\mathrm{accel}\_y}{\mathrm{accel}\_z}$ (formula 3)

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ (formula 4)

Step 2, read acceleration accel_x, accel_y and the accel_z of 3 directions of carrier coordinate system x-axis, y-axis and z-axis from accelerometer; Read magnetic field intensity magnetom_x, magnetom_y and the magnetom_z of 3 directions of carrier coordinate system x-axis, y-axis and z-axis from magnetometer;

Pitching angle theta, roll angle φ and deflection ψ according to (formula 2), (formula 3) and (formula 4) primary Calculation current time.And, by the pitching angle theta, roll angle φ and the deflection ψ substitution (formula 1) that calculate, obtain the Conversion Matrix of Coordinate R in this moment ₁.

Step 3, read the angular velocity omega of 3 directions of carrier coordinate system x-axis, y-axis and z-axis from gyroscope _x, ω _yand ω _z; Obtain the Conversion Matrix of Coordinate R in this moment according to (formula 5) and (formula 6) ₂, (formula 5) and (formula 6) is as follows:

$\begin{array}{c}{\mathrm{d\θ}}_x={\mathrm{\ω}}_x\mathrm{dt}\\ {\mathrm{d\θ}}_y={\mathrm{\ω}}_y\mathrm{dy}\\ {\mathrm{d\θ}}_z={\mathrm{\ω}}_z\mathrm{dt}\end{array}$ (formula 5)

$R_2(t+\mathrm{dt})=R_{\mathrm{normalized}}\left(t\right)\left[\begin{array}{ccc}1& {-\mathrm{d\θ}}_z& {\mathrm{d\θ}}_y\\ {\mathrm{d\θ}}_z& 1& -{\mathrm{d\θ}}_x\\ -{\mathrm{d\θ}}_y& {\mathrm{d\θ}}_x& 1\end{array}\right]$ (formula 6)

The Conversion Matrix of Coordinate R that step 4, basis calculate ₁with Conversion Matrix of Coordinate R ₂, and (formula 7) calculate final Conversion Matrix of Coordinate R of this moment, (formula 7) is as follows:

R=R ₁+ λ (R ₂-R ₁) (formula 7)

Wherein, λ is the Conversion Matrix of Coordinate correction factor, and 0≤λ≤1.

According to (formula 8)～(formula 16), final Conversion Matrix of Coordinate R of this moment is carried out to normalization, obtain this moment normalization Conversion Matrix of Coordinate R _normalized, (formula 8)～(formula 16) is as follows:

$R^T=\left[\begin{array}{ccc}X& Y& Z\end{array}\right]=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{yx}}& r_{\mathrm{zx}}\\ r_{\mathrm{xy}}& r_{\mathrm{yy}}& r_{\mathrm{zy}}\\ r_{\mathrm{xz}}& r_{\mathrm{yz}}& r_{\mathrm{zz}}\end{array}\right]$ (formula 8)

$\mathrm{err}=X\•Y=X^{Y}Y=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{xy}}& r_{\mathrm{xz}}\end{array}\right]\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right]$ (formula 9)

${\left[\begin{array}{c}{r}_{\mathrm{xx}}\\ r_{\mathrm{xy}}\\ r_{\mathrm{xz}}\end{array}\right]}_{\mathrm{orth}}=X_{\mathrm{orth}}=X-\frac{\mathrm{err}}{2}Y$ (formula 10)

${\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right]}_{\mathrm{orth}}=Y_{\mathrm{orth}}=Y-\frac{\mathrm{err}}{2}X$ (formula 11)

${\left[\begin{array}{c}{r}_{\mathrm{zx}}\\ r_{\mathrm{zy}}\\ r_{\mathrm{zz}}\end{array}\right]}_{\mathrm{orth}}=Z_{\mathrm{orth}}=X_{\mathrm{orth}}\×Y_{\mathrm{orth}}$ (formula 12)

R _normalized=[X _normalizedy _normalizedz _normalized] (formula 13)

$X_{\mathrm{normalized}}=\frac{1}{2}(3-X_{\mathrm{orth}}\•X_{\mathrm{orth}})X_{\mathrm{orth}}$ (formula 14)

$Y_{\mathrm{normalized}}=\frac{1}{2}(3-Y_{\mathrm{orth}}\•Y_{\mathrm{orth}})Y_{\mathrm{orth}}$ (formula 15)

$Z_{\mathrm{normalized}}=\frac{1}{2}(3-Z_{\mathrm{orth}}\•Z_{\mathrm{orth}})Z_{\mathrm{orth}}$ (formula 16)

The definition to Conversion Matrix of Coordinate of step 5, basis (formula 1), and the normalization Conversion Matrix of Coordinate R calculated _normalizedcalculate final pitching angle theta, roll angle φ and the deflection ψ in this moment.

According to above-described embodiment, just can realize well the present invention.

Claims (7)

1.AHRS the high-precision attitude method for computing data, is characterized in that, comprises the following steps:

(1) sensor in AHRS and normalization Conversion Matrix of Coordinate are carried out to initialization process;

(2) read the data of accelerometer and magnetometer in AHRS, calculate the attitude data of carrier and Conversion Matrix of Coordinate R now ₁;

(3) read gyrostatic data in AHRS, according to the normalization Conversion Matrix of Coordinate in a upper moment, calculated the Conversion Matrix of Coordinate R in this moment ₂;

(4) according to Conversion Matrix of Coordinate R ₁with Conversion Matrix of Coordinate R ₂determine the Conversion Matrix of Coordinate that this moment is final, and final Conversion Matrix of Coordinate is carried out to normalization, obtain the normalization Conversion Matrix of Coordinate in this moment;

(5) solve final attitude data of this moment according to the normalization Conversion Matrix of Coordinate drawn in step (4);

(6) return to step (2), carry out next attitude data constantly and calculate.

2. AHRS high-precision attitude method for computing data according to claim 1, is characterized in that, described step (1) specifically comprises:

(1a) parameters of sensors configured;

(1b) according to the output data initialization normalization Conversion Matrix of Coordinate R of accelerometer now and magnetometer _normalized.

3. AHRS high-precision attitude method for computing data according to claim 2, is characterized in that, described step (1b) comprises the following steps:

(1b1) read carrier coordinate system at the acceleration accel_x of X axis, at the acceleration accel_y of Y-axis, at the acceleration accel_z of Z-axis direction from accelerometer; Read carrier coordinate system at the magnetic field intensity magnetom_x of X axis, at the magnetic field intensity magnetom_y of Y-axis, at the magnetic field intensity magnetom_z of Z-axis direction from magnetometer;

(1b2) according to formula

calculate pitching angle theta; According to formula

calculate roll angle φ; According to formula

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ Calculated direction angle ψ;

Correspondingly, described normalization Conversion Matrix of Coordinate R _normalizedpass through formula

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ Complete initialization.

4. AHRS high-precision attitude method for computing data according to claim 3, is characterized in that, described step (2) specifically comprises the following steps:

(2a) read carrier coordinate system at the acceleration accel_x of X axis, at the acceleration accel_y of Y-axis, at the acceleration accel_z of Z-axis direction from accelerometer; Read carrier coordinate system at the magnetic field intensity magnetom_x of X axis, at the magnetic field intensity magnetom_y of Y-axis, at the magnetic field intensity magnetom_z of Z-axis direction from magnetometer;

(2b) according to formula

calculate the pitching angle theta of current time; According to formula calculate the roll angle φ of current time; According to formula

$\begin{array}{c}\mathrm{mag}\_x=\mathrm{magnetom}\_x*\cos \left(\mathrm{\θ}\right)+\mathrm{magnetom}\_y*\sin \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ +\mathrm{magnetom}\_z*\cos \left(\mathrm{\φ}\right)*\sin \left(\mathrm{\θ}\right)\\ \mathrm{mag}\_y=\mathrm{magnetom}\_y*\cos \left(\mathrm{\φ}\right)-\mathrm{magnetom}\_z*\sin \left(\mathrm{\φ}\right)\\ \tan \left(\mathrm{\ψ}\right)=-\frac{\mathrm{mag}\_y}{\mathrm{mag}\_x}\end{array}$ Calculate the deflection ψ of current time

According to formula $R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$ Draw the Conversion Matrix of Coordinate R in this moment ₁, wherein, the angle of pitch that θ is current time; The roll angle that φ is current time; The deflection that ψ is current time.

5. AHRS high-precision attitude method for computing data according to claim 4, is characterized in that, described step (3) specifically comprises the following steps:

(3a) read the angular velocity omega of carrier coordinate system at X axis from gyroscope _x, at the angular velocity omega of Y-axis _y, at the angular velocity omega of Z-axis direction _z;

(3b) according to formula $\left[\begin{array}{c}{\mathrm{d\θ}}_x={\mathrm{\ω}}_x\mathrm{dt}\\ {\mathrm{d\θ}}_y={\mathrm{\ω}}_y\mathrm{dt}\\ {\mathrm{d\θ}}_z={\mathrm{\ω}}_z\mathrm{dt}\end{array}\right]$ And formula $R_2(t+\mathrm{dt})R_{\mathrm{normalized}}\left(t\right)\left[\begin{array}{ccc}1& -{\mathrm{d\θ}}_z& {\mathrm{d\θ}}_y\\ {\mathrm{d\θ}}_z& 1& -{\mathrm{d\θ}}_x\\ -{\mathrm{d\θ}}_y& {\mathrm{d\θ}}_x& 1\end{array}\right]$ Draw the Conversion Matrix of Coordinate R in this moment ₂.

6. AHRS high-precision attitude method for computing data according to claim 5, is characterized in that, described step (4) specifically comprises the following steps:

(4a) according to the Conversion Matrix of Coordinate R calculated ₁with Conversion Matrix of Coordinate R ₂calculate final Conversion Matrix of Coordinate R=R of this moment ₁+ λ (R ₂-R ₁), wherein, λ is the Conversion Matrix of Coordinate correction factor, and 0≤λ≤1;

(4b) according to following formula:

$R^T=\left[\begin{array}{ccc}X& T& Z\end{array}\right]=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{yx}}& r_{\mathrm{zx}}\\ r_{\mathrm{xy}}& r_{\mathrm{yy}}& r_{\mathrm{zy}}\\ r_{\mathrm{xz}}& r_{\mathrm{yz}}& r_{\mathrm{zz}}\end{array}\right];$ $\mathrm{err}=X\•Y=X^{T}Y=\left[\begin{array}{ccc}{r}_{\mathrm{xx}}& r_{\mathrm{xy}}& r_{\mathrm{xz}}\end{array}\right]\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right];$

${\left[\begin{array}{c}{r}_{\mathrm{xx}}\\ r_{\mathrm{xy}}\\ r_{\mathrm{xz}}\end{array}\right]}_{\mathrm{orth}}=X_{\mathrm{orth}}=X-\frac{\mathrm{err}}{2}Y;{\left[\begin{array}{c}{r}_{\mathrm{yx}}\\ r_{\mathrm{yy}}\\ r_{\mathrm{yz}}\end{array}\right]}_{\mathrm{orth}}=Y_{\mathrm{orth}}=Y-\frac{\mathrm{err}}{2}X;{\left[\begin{array}{c}{r}_{\mathrm{zx}}\\ r_{\mathrm{zy}}\\ r_{\mathrm{zz}}\end{array}\right]}_{\mathrm{orth}}=Z_{\mathrm{orth}}=X_{\mathrm{orth}}\×Y_{\mathrm{orth}};$

$R_{\mathrm{normalized}}=\left[\begin{array}{ccc}{X}_{\mathrm{normalized}}& Y_{\mathrm{normalized}}& Z_{\mathrm{normalized}}\end{array}\right];X_{\mathrm{normalized}}=\frac{1}{2}(3-X_{\mathrm{orth}}\•X_{\mathrm{orth}})X_{\mathrm{orth}};$

$Y_{\mathrm{normalized}}=\frac{1}{2}(3-Y_{\mathrm{orth}}\•Y_{\mathrm{orth}})Y_{\mathrm{orth}};Z_{\mathrm{normalized}}=\frac{1}{2}(3-Z_{\mathrm{orth}}\•Z_{\mathrm{orth}})Z_{\mathrm{orth}};$ The Conversion Matrix of Coordinate R final to this moment carries out normalization, draws this moment normalization Conversion Matrix of Coordinate R _normalized.

7. AHRS high-precision attitude method for computing data according to claim 6, it is characterized in that, in described step (5), this constantly final attitude data is determined by final pitching angle theta, roll angle φ and the deflection ψ in this moment, and this final pitching angle theta, roll angle φ and deflection ψ constantly is according to formula

$R=\left[\begin{array}{ccc}\cos \mathrm{\θ}\cos \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}-\cos \mathrm{\φ}\sin \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\cos \mathrm{\ψ}+\sin \mathrm{\φ}\sin \mathrm{\ψ}\\ \cos \mathrm{\θ}\sin \mathrm{\ψ}& \sin \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}+\cos \mathrm{\φ}\cos \mathrm{\ψ}& \cos \mathrm{\φ}\sin \mathrm{\θ}\sin \mathrm{\ψ}-\sin \mathrm{\φ}\cos \mathrm{\ψ}\\ -\sin \mathrm{\θ}& \sin \mathrm{\φ}\cos \mathrm{\θ}& \cos \mathrm{\φ}\cos \mathrm{\θ}\end{array}\right]$

And the normalization coordinate system that step (4b) calculates is changed square R _normalizedbattle array is tried to achieve.

Images