CN107063254A - A kind of attitude algorithm method of gyro earth magnetism combination - Google Patents

Abstract

The invention discloses a method for calculating the attitude of a gyro-geomagnetic combination, which includes the following steps: Step 1. Using the angular rate information of the three-axis gyroscope to solve the attitude information by using the rotation vector method optimization algorithm; using the geomagnetic information for calculation, and solving it by the integral ratio method Roll angle information; step 2, use the roll angle calculated by the geomagnetic signal and the pitch angle and yaw angle calculated by the gyro signal as the next moment to solve the quaternion; step 3, repeat step 1 and step 2 to realize Posture update. The present invention uses the gyroscope's current angular velocity, angular increment, and last time angular increment information to calculate the attitude information, and uses the roll angle correction calculated by the two-axis geomagnetic signal 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, the accuracy of attitude measurement 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 solve 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 description

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:

Suppose the time t _k to 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:

Assume

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 get the rotation vector error by making a difference with formula (2). When we only consider the DC component that causes platform drift and consider α to be 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, 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; with Indicates the integral operation with respect to the roll angle γ; with 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 included angle λ and the heading angle ψ remain unchanged, the value of f(θ) can be obtained through 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 conditions of the roll angle in the current cycle, 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. The positive and negative conditions of γ _K 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 Kth 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 and γ 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 measured 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 for the next step, and then use the optimized two-sample algorithm to calculate the attitude. The output is dynamically corrected to improve the accuracy of the attitude angle and reduce the error.

Claims (8)

1. a kind of attitude solution method of gyro-geomagnetic combination, it is characterized in that, adopt gyroscope and geomagnetic sensor to carry out data acquisition and obtain gyro signal and geomagnetic signal, comprise the 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. 2. the attitude calculation method of gyro-geomagnetic combination according to claim 1, is characterized in that, utilizes gyro angular rate information to adopt rotation vector method optimization algorithm to solve attitude information in step 1) and comprises 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. 3. the attitude solution method of gyro-geomagnetic combination according to claim 1, is characterized in that, in step 1), utilizes geomagnetic information to solve roll angle information by integral ratio method and comprises 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. 4. the attitude solution method of gyro-geomagnetic combination according to claim 2, is characterized in that, calculates rotation vector Φ (T) by following formula in step 1.1: In the formula, ω ₁ , ω ₂ , ω ₃ are the gyro in The output angular rate at the moment, Δθ ₁ is the angular increment at the current moment, and Δθ ₀ is the angular increment signal at the previous moment. 5. The attitude calculation method of gyro-geomagnetic combination according to claim 2, characterized in that, in step 1.2, the rotation quaternions q ₀ , q ₁ , q ₂ , q ₂ are: In the formula, ψ represents the heading angle, θ represents the pitch angle, and γ represents the roll angle. 6. the attitude solution method of gyro-geomagnetic combination according to claim 2, is characterized in that, attitude information described in step 1.3 obtains projectile pitch angle θ, roll angle γ and heading angle ψ by following formula calculation: 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. 7. the attitude solution method of gyro-geomagnetic combination according to claim 3, is characterized in that, the model function f (θ) in the step 2 is calculated by 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. 8. the attitude solution method of gyro-geomagnetic combination according to claim 3, is characterized in that, in the step 2.2, the relationship between the measured value of magnetic sensor S1 and magnetic sensor S2 and the attitude angle 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.