CN107063254B - Gesture resolving method for gyros and geomagnetic combination - Google Patents

Abstract

The application discloses a gyroscope geomagnetic combination attitude resolving method, which comprises the following steps: step 1, calculating attitude information by using triaxial gyro angular rate information and adopting a rotation vector method optimization algorithm; calculating by using geomagnetic information, and solving roll angle information by an integral ratio method; step 2, calculating quaternion by taking a roll angle calculated by geomagnetic signals and a pitch angle and a yaw angle calculated by gyroscopic signals as the next moment; and 3, repeating the step 1 and the step 2 to realize gesture updating. According to the application, the attitude information is calculated by using the current angular rate and the angular increment of the gyroscope and the angular increment information of the last moment, and the roll angle calculated by using the two-axis geomagnetic signals is corrected to obtain higher precision.

Description

An Attitude Calculation Method of Gyro Geomagnetic Combination

technical field

The invention belongs to the field of attitude measurement, and in particular relates to an attitude calculation method of a gyro-geomagnetism combination.

Background technique

The attitude solution of strapdown inertial navigation system in high dynamic environment is the key technology to improve the system accuracy. Attitude calculation refers to the calculation and analysis of the output of the carrier sensor to obtain attitude information, including heading angle, pitch angle, and roll angle. For carriers such as missiles and shells in high dynamic environments, attitude measurement accuracy is a key factor to determine whether the strapdown inertial navigation system can work normally.

The rotating projectile rotates around its own longitudinal axis at high speed, and the gyroscope has high precision, but the error will accumulate over time, and it needs to be combined with other inertial devices or sensors to measure the attitude angle. Compared with gyro devices, the short-term accuracy of geomagnetism is not high, but its error will not accumulate over time, and its ability to resist high overload is also strong. If the two are used in combination, the accuracy and stability can be greatly improved.

Therefore, a new attitude calculation method of gyro-geomagnetic combination is needed to solve the above problems.

Contents of the invention

The purpose of the present invention is to solve the problems existing in the prior art, and provide a gyro-geomagnetic combination attitude calculation method capable of obtaining higher-precision attitude information.

In order to achieve the above-mentioned purpose of the invention, the attitude solution method of the gyro-geomagnetic combination of the present invention can adopt the following technical solutions:

A method for calculating the attitude of a gyro-geomagnetic combination, using a gyroscope and a geomagnetic sensor for data acquisition to obtain a gyro signal and a geomagnetic signal, comprising the following steps:

Step 1, using the gyro angular rate information to solve the attitude information by using the rotation vector method optimization algorithm, and using the geomagnetic information to solve the roll angle by the integral ratio method;

Step 2, using the roll angle obtained by solving the geomagnetic signal and the pitch angle and the yaw angle obtained by solving the gyro signal in step 1 as the quaternion to be solved at the next moment;

Step 3: Repeat steps 1 and 2 to update the attitude.

Furthermore, in step 1), using the gyroscope angular rate information and adopting the rotation vector method optimization algorithm to solve the attitude information includes the following steps:

1.1. Calculate the rotation vector Φ(T) through the gyro signal;

1.2, according to the rotation vector Φ (T) that step 1.1 obtains, calculate and obtain the rotation quaternion;

1.3. Calculate the posture information of the posture according to the rotation quaternion in step 1.2. The rotation vector method uses the angular rate and angular increment at the previous moment to solve the attitude angle.

Furthermore, in step 1), the use of geomagnetic information to solve the roll angle information by the integral ratio method includes the following steps:

2.1. Calculate the value of the model function f(θ);

2.2. The geomagnetic sensor includes a magnetic sensor S1 and a magnetic sensor S2. The measured values of the magnetic sensor S1 and the magnetic sensor S2 are H _S1 and H _S2 respectively, and the roll angle γ when the measured values H _S1 and H _S2 are zero is calculated, wherein, H _S1 is the measured value of the magnetic sensor S1, and H _S2 is the measured value of the magnetic sensor S2.

Furthermore, in step 1.1, the rotation vector Φ(T) is calculated by the following formula:

In the formula, ω ₁ , ω ₂ , ω ₃ are the gyroscope at t, The output angular rate at time t+T, Δθ ₁ is the angular increment at the current moment, and Δθ ₀ is the angular increment signal at the previous moment.

Furthermore, the rotation quaternions q ₀ , q ₁ , q ₂ , and q ₃ in step 1.2 are:

In the formula, ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle.

Furthermore, the attitude information described in step 1.3 is calculated by the following formula to obtain the projectile pitch angle θ, roll angle γ and heading angle ψ:

Among them, the yaw angle is the angle between the actual route and the planned route,

In the above formula,

ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle.

Furthermore, the model function f(θ) in step 2 is calculated by the following formula:

Among them, ψ represents the heading angle, θ represents the pitch angle, γ represents the roll angle, h is the scalar magnitude of the geomagnetic field vector H, and γ represents the angle between the magnetic sensor S2 and the Ox ₁ axis in the projectile coordinate system.

Furthermore, the relationship between the measured values of the magnetic sensor S1 and the magnetic sensor S2 and the attitude angle in step 2.2 is:

H _S1 ＝h(cosγsinψcosθ+sinγsinθ)

H _S2 ＝h(cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ)

Among them, ψ represents the heading angle, θ represents the pitch angle, γ represents the roll angle, h is the scalar magnitude of the geomagnetic field vector H, and γ represents the angle between the magnetic sensor S2 and the Ox ₁ axis in the projectile coordinate system.

Beneficial effects: the attitude calculation method of the gyro-geomagnetic combination of the present invention uses the gyro magnetic sensor combination to calculate the attitude, uses the rotation vector method to solve the attitude angle, and uses the integral ratio method to solve the roll angle of the geomagnetic sensor to correct the cumulative error of the gyro, and the gyro The output is dynamically corrected to improve the accuracy of calculating the attitude angle. Through experimental verification, it can be obtained that the present invention uses a combination of gyro magnetic sensors to solve the attitude method, which is equivalent to a single gyro or geomagnetic sensor, and has better accuracy and stability.

Description of drawings

Fig. 1 is a schematic diagram of the principle of the present invention;

Fig. 2 is the installation schematic diagram of the gyro magnetic sensor of the embodiment of the present invention;

Fig. 3 is a schematic diagram of the algorithm flow of gesture settlement implemented in the present invention.

Detailed ways

The present invention will be further described below in conjunction with the accompanying drawings. Below in conjunction with accompanying drawing and specific embodiment, further illustrate the present invention, should be understood that these embodiments are only the preferred embodiment of the present invention, after having read the present invention, those skilled in the art will understand various aspects of the present invention that do not depart from the principle of the present invention All modifications fall within the scope defined by the appended claims of this application.

Below in conjunction with embodiment the present invention will be further described.

Please refer to Figure 1 and Figure 2, the three-axis gyroscope is installed on the body so that the three-axis gyroscope is aligned with the three axes of the carrier coordinate system, and two non-orthogonal single-axis magnetic sensors S1 and S2 are respectively installed on At the origin of Ox ₁ y ₁ z ₁ of the projectile coordinate system, the Ox ₁ axis coincides with the longitudinal axis of the projectile, and both sensitive axes are in the Ox ₁ z ₁ plane, S1 is installed along the Oz ₁ axis, and S2 is installed along the Ox ₁ axis Installed at an angle of lambda.

1. The principle of the rotation vector optimization two-sample pose update algorithm:

Assume that from time t _k to time t _k+1 , the update period T=t _k+1 -t _k , the q(T) update quaternion has the following relationship with the Q(t _k+1 ) rotation quaternion:

The relationship between the rotation vector Φ and q(T) is as follows

Quaternions can be expressed in terms of attitude angles

There is the following relationship between the quaternion and the attitude matrix:

set up

The projectile pitch angle θ, roll angle γ, and heading angle ψ can be obtained through the attitude matrix

Therefore, by calculating the rotation vector Φ every cycle, the quaternion information can be updated to obtain the attitude information. The calculation algorithm principle of the rotation vector Φ used in this paper is as follows:

When the rotation vector is used to determine the attitude motion of the carrier, there is the following simplified differential equation

In an angular rate calculation cycle [t,t+T] is fitted with a parabola to the angular rate:

ω＝a+2bτ+3cτ ² , 0≤τ≤T

Let the gyro be at t, The output of the angular rate at time t+T is ω ₁ , ω ₂ , ω ₃ respectively, expand Φ with Taylor series at T=0, and express the derivatives of Φ with ω ₁ , ω ₂ , ω ₃ , you can get the rotation vector expression based on the angular rate

Add the angle increment Δθ ₁ at the current moment and the angle increment Δθ ₀ at the previous moment to optimize the conical motion of the above formula, and set

Among them, X, Y, Z, K are undetermined coefficients, so that the algorithm drift in the conical motion environment is the smallest.

Let Ω be the cone motion frequency, α be the cone angle, and the rotation vector within an attitude update cycle can be deduced from the properties of quaternions:

The angular increment and angular rate of the gyroscope in an attitude update period are:

Substituting into formula (1) can obtain the rotation vector increment of one period, and the error of the rotation vector can be obtained by making a difference with formula (2). When we only consider the DC component that causes platform drift, and consider that α is a small amount, then we have :

Carry out Taylor expansion series expansion on the above formula, and set the coefficient of the high-order term of ΩT to zero, and the solution is

make get the algorithm

The angular increment is then approximated by interpolating the integral

2. Geomagnetic sensor attitude algorithm

The expressions of the measured values H _S1 and H _{S2 of the magnetic sensors S1 and S2} and the projectile pitch angle θ, roll angle γ, and heading angle ψ

H _S1 ＝h(cosγsinψcosθ+sinγsinθ)

H _S2 ＝h(cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ)

In the formula, h is the scalar magnitude of the geomagnetic field vector H.

Integral mathematical model:

Among them, N represents the total sampling times of the projectile rotating one circle, and k represents the sampling time; and Indicates the integral operation with respect to the roll angle γ;/> and /> Indicates the square sum operation of the discrete sampling points of the two sensor sampling values H _S1 and H _S2 in one revolution of the projectile;

According to the expression of the magnetic sensor, it can be deduced that

When the projectile rotates one circle, assuming that the angle λ and the heading angle ψ remain unchanged, the value of f(θ) can be obtained by the integral operation of the sum of the squares of the sampling values of the two sensors. The solution of f(θ) can be obtained to obtain the angle value of the roll angle γ of the projectile at a specific moment, and the roll angle γ of all moments within one revolution of the projectile can be calculated.

The algorithm of the geomagnetic sensor used in this paper to solve the roll angle γ is as follows:

Assuming that the specific moment is the zero point of _HS1 or _HS2 , when the measured value _HS1 or _HS2 is zero, the unknown magnetic field strength scalar h can be eliminated, and the influence of the surrounding interference magnetic field environment on the calculation results can be reduced.

(1) When the measured value H _S1 of the magnetic sensor S1 =0:

cosγsinψcosθ+sinγsinθ＝0

Tidy up

In the formula, the two parameters ψ and θ of the function are not zero at the same time;

The roll angle γ _K of the current K-th cycle should have two values, positive and negative, corresponding to quadrants 1 and 3 or quadrants 2 and 4 respectively. At this time, the roll angle γ _K-1 and projectile body of the previous cycle should be used Roughly estimate the positive and negative status of the roll angle in the current cycle with the rotation speed ω, and select the value of the roll angle γ _K.

For each revolution _of the projectile, the maximum value H _S1max and the minimum value H _{S1min of} the magnetic sensor output value H _S1 (or H _S2 ) can be obtained, and the time points t(H _S1max ) and t(H _S1min ), ω can be roughly calculated, that is, ω=2(t(H _S1max )-t( _HS1min )).

make Where T is the sampling cycle time. γ _K positive and negative conditions and /> Positive and negative are the same.

(2) When the measured value H _s2 of the magnetic sensor S2 =0:

cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ=0

In the formula,

When |sinλ|＞|cosθcosψ|, the measured value H _s2 of the magnetic sensor S2 has two zero points, and the roll angle γ _K of the current K-th cycle has two solutions γ _1,K and γ _2,K , at this time it should be calculated The approximate value of the roll angle of the current cycle obtained by adding the roll angle γ _K-1 of the K-1 cycle to the projectile speed ω roll angle /> with /> γ corresponding to the smaller one;

When |sinλ|=|cosθcosψ|, the measured value H _S2 of the magnetic sensor S2 has a zero point;

When |sinλ|<|cosθcosψ|, the measurement value H _S2 of the magnetic sensor S2 does not necessarily have a zero point, and the roll angle γ obtained at this time is not accurate.

3. Use the roll angle at each moment calculated by the geomagnetic signal and the pitch angle and yaw angle at each time calculated by the gyro signal as the quaternion to be solved in the next step, and then use the optimized two-sample algorithm to solve the attitude, and the gyro The output is dynamically corrected to improve the accuracy of the attitude angle and reduce the error.

Claims (6)

1. A gyroscope geomagnetic combination attitude resolving method is characterized in that a gyroscope and a geomagnetic sensor are adopted for data acquisition to obtain a gyroscope signal and a geomagnetic signal, and the method comprises the following steps:

firstly, solving attitude information by using gyro angular rate information and adopting a rotation vector method optimization algorithm, and solving roll angles by using geomagnetic information through an integral ratio method;

the step 1) of calculating attitude information by using gyro angular rate information and adopting a rotation vector method optimization algorithm comprises the following steps of:

1.1, calculating a rotation vector phi (T) through a gyro signal;

in step 1.1, the rotation vector Φ (T) is calculated by:

wherein omega is ₁ ,ω ₂ ,ω ₃ For gyroscopes atOutput angular rate of time, Δθ ₁ Delta theta is the current time angle increment ₀ An angular increment signal for a previous time;

1.2, calculating to obtain a rotation quaternion according to the rotation vector phi (T) obtained in the step 1.1;

1.3, calculating attitude information of the attitude according to the rotation quaternion in the step 1.2;

step two, using the roll angle obtained by the geomagnetic signal calculation in the step one and the pitch angle and yaw angle obtained by the gyro signal calculation as quaternions calculated at the next moment;

and thirdly, repeating the first step and the second step to update the gesture.

2. The gyros-geomagnetic combination pose solving method according to claim 1, wherein the solving of roll angle information by an integral-ratio method using geomagnetic information in step 1) includes the steps of:

2.1, calculating the value of a model function f (theta);

2.2 the geomagnetic sensor includes a magnetic sensor S1 and a magnetic sensor S2, and the measured values of the magnetic sensor S1 and the magnetic sensor S2 are H respectively _S1 And H _S2 Calculate the measured value H _S1 And H _S2 Roll angle γ of zero, where H _S1 H, which is the measurement value of the magnetic sensor S1 _S2 Is a measurement value of the magnetic sensor S2.

3. The gyros-geomagnetic combination pose solving method according to claim 1, wherein the quaternion q is rotated in step 1.2 ₀ ,q ₁ ,q ₂ ,q ₃ The method comprises the following steps:

where ψ represents a heading angle, θ represents a pitch angle, and γ represents a roll angle.

4. The gyros-geomagnetic combination attitude calculation method of claim 1, wherein the attitude information in step 1.3 is calculated to obtain an projectile pitch angle θ, a roll angle γ and a heading angle ψ by the following formula:

wherein the yaw angle is the included angle between the actual course and the planned course,

in the above-mentioned method, the step of,

ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle.

5. The gyros-geomagnetic combination pose solving method according to claim 2, wherein the model function f (θ) in step 2 is calculated by:

wherein, psi represents course angle, theta represents pitch angle, gamma represents roll angle, H is scalar magnitude of geomagnetic field vector H, gamma is magnetic sensors S2 and Ox in an elastomer coordinate system ₁ And an included angle of the axes.

6. The gyros-geomagnetic combination attitude calculation method according to claim 2, wherein the relationship between the measured values of the magnetic sensor S1 and the magnetic sensor S2 and the attitude angle in step 2.2 is:

H _S1 ＝h(cosγsinψcosθ+sinγsinθ)

H _S2 ＝h(cosθcosψcosλ+cosγsinψcosθsinλ+sinγsinθsinλ)

wherein, psi represents course angle, theta represents pitch angle, gamma represents roll angle, H is scalar magnitude of geomagnetic field vector H, gamma is magnetic sensors S2 and Ox in an elastomer coordinate system ₁ And an included angle of the axes.