CN105937911A - Magnetic sensor attitude calculation method - Google Patents

Abstract

The invention relates to a magnetic sensor attitude calculation method. The calculation method comprises the following steps: using a non-orthogonal magnetic sensor to measure an elastomer when the elastomer rotates in 360 degrees, subjecting measured data to a filtering treatment by adopting a least square method; substituting the filtered data measured by the magnetic sensor into an integral model to calculate the model function; according to the model function, calculating the pitch angle; calculating the roll angle when the data measure by the non-orthogonal magnetic sensor is zero, and repeating the steps mentioned above so as to calculate the magnetic sensor attitude in real time. The pitch angle is measured on the basis of all data that is measured by a magnetic sensor when an elastomer rotates in 360 degrees, and thus the results are more accurate.

Description

A kind of Magnetic Sensor attitude algorithm method

Technical field

The present invention relates to attitude measurement field, be specifically related to a kind of Magnetic Sensor attitude algorithm method.

Background technology

Under high dynamic environment, the attitude algorithm of SINS is the key technology improving system accuracy.Attitude algorithm refers to The output computational analysis utilizing carrier sensor obtains attitude information, including course angle, the angle of pitch, roll angle.To making height For dynamic motion is in the carriers such as the guided missile shell of high dynamic environment, attitude measurement accuracy is to determine its SINS The key factor that can normally work.

Rolling-airframe is around its longitudinal axis high speed rotating, and the range of high accuracy gyroscope instrument is limited, it is difficult to apply in high speed Gao Xuan Missile-borne environment.Earth's magnetic field detection has that fast response time, volume are little, anti high overload ability strong, without advantages such as accumulated errors, The conventional ammunition body attitude being suitable for high speed Gao Xuan is measured.

Magnetic Sensor measures the method for body attitude various ways, wherein, and the nonopiate Magnetic Sensor group of two sensors Closing measurement method uses the Magnetic Sensor of two nonopiate installations to gather the earth's magnetic field data of missile rotation one circle, revolves according to body When turning around, the angle of pitch and course angle are almost unchanged, solve body roll angle.Nonopiate magnetic sensor combination measurement method resolves The carrier angle of pitch has two kinds of algorithms, zero crossings method and extreme value ratio method.Two kinds of algorithms can obtain the moment a certain to body Pose estimation value, but when zero crossings method and extreme value ratio method all make use of missile rotation one to enclose in sampled data Particular point carries out computing, and when sampled value error is less, the error of pose estimation value is the least, and sampled value is done at random When disturbing error increase, attitude error is consequently increased, and algorithm is vulnerable to the impact of random disturbances.

Summary of the invention

It is an object of the invention to overcome existing zero crossings method and the deficiency of extreme value ratio method, propose a kind of Magnetic Sensor appearance State calculation method.

The technical scheme realizing the object of the invention is: a kind of Magnetic Sensor attitude algorithm method, includes following steps:

Step 1, employing method of least square are filtered place to the missile rotation nonopiate Magnetic Sensor measurement data of a week Reason；

Step 2, filtered Magnetic Sensor measurement data is input in integral model calculate pattern function；

Step 3, solve the angle of pitch according to pattern function；

Step 4, to calculate nonopiate Magnetic Sensor measurement data be roll angle when zero；

Step 5, repetition step 1 are to step 4 real-time resolving Magnetic Sensor attitude.

Compared with prior art, the invention have the benefit that the integration ratio method of the present invention is to utilize bullet rotation to turn around In the range of all data of Magnetic Sensor carry out calculating the angle of pitch；One is only taken at each swing circle relative to extreme value ratio method Individual data, integral model obtains multi-group data at each swing circle, and in the case of noise variance is the biggest, the present invention bows The resolution error at the elevation angle is the least relative to extreme value ratio method.

Accompanying drawing explanation

Fig. 1 is the Magnetic Sensor attitude algorithm method flow diagram of the present invention.

Fig. 2 is two axial magnetic sensor scheme of installation in the embodiment of the present invention.

Detailed description of the invention

In conjunction with Fig. 1, a kind of Magnetic Sensor attitude algorithm method of the present invention, comprise the steps:

Step 1, employing method of least square are filtered place to the missile rotation nonopiate Magnetic Sensor measurement data of a week Reason；

Step 2, filtered Magnetic Sensor measurement data is input in integral model calculate pattern function；

Step 3, solve the angle of pitch according to pattern function；

Step 4, to calculate nonopiate Magnetic Sensor measurement data be roll angle when zero；

Step 5, repetition step 1 are to step 4 real-time resolving Magnetic Sensor attitude.

Further, the pattern function f (θ described in step 2_m) it is by two nonopiate Magnetic Sensor measurement data H_S1With H_S2All sampled points in rotary course are integrated calculating, and do scale operation the most again, and concrete formula is:

$f\left({\mathrm{\θ}}_m\right)=\frac{{\∫}_0^{2\mathrm{\π}}{H}_{S2}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}{{\∫}_0^{2\mathrm{\π}}{H}_{S1}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}=\frac{2{\cos }^2{\mathrm{\θ}}_m{\cos }^2{\mathrm{\ψcos}}^2\mathrm{\λ}+{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}+{\sin }^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}}{{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m+{\sin }^2{\mathrm{\θ}}_m}$

Wherein, ψ represents course angle, θ_mRepresenting the angle of pitch, γ represents roll angle, and h is the scalar size of geomagnetic fieldvector H, λ is missile coordinate system Ox₁y₁z₁Middle Magnetic Sensor S2 and Ox₁Axle clamp angle.

Further, according to pattern function f (θ described in step 3_m) solve pitching angle theta_mDetailed process be:

${\mathrm{\θ}}_m=\arctan 2(\sqrt{\left|u(u-v)\right|},\±\sqrt{\left|uv\right|}),{\mathrm{\θ}}_m\∈\[-\mathrm{\π}/2,\mathrm{\π}/2\]$

In formula,

Two parameters u and v can not be zero simultaneously.

With specific embodiment, the method for the present invention is described further below in conjunction with the accompanying drawings.

Embodiment

Magnetic intensity vector H is in missile coordinate system Ox₁y₁z₁In expression formula H_bIt is expressed as:

$H_b=\left[\begin{array}{c}h{\mathrm{cos\θ}}_{m}cos\mathrm{\ψ}\\ h({\mathrm{sin\γsin\ψcos\θ}}_m-{\mathrm{cos\γsin\θ}}_m)\\ h({\mathrm{cos\γsin\ψcos\θ}}_m+{\mathrm{sin\γsin\θ}}_m)\end{array}\right]$

Wherein, ψ represents course angle, θ_mRepresenting the angle of pitch, γ represents roll angle, and h is that the scalar of geomagnetic fieldvector H is big Little.

As in figure 2 it is shown, two non-orthogonal first uniaxial magnetic quantity sensor S1 and the second uniaxial magnetic quantity sensor S2 are respectively mounted Ox in missile coordinate system₁y₁z₁Initial point on, Ox₁Axle overlaps with the body longitudinal axis, and two sensitive axes are all at Ox₁z₁In plane, S1 is along Oz₁Axle is installed, S2 and Ox₁Axle is that λ angle is installed.

First uniaxial magnetic quantity sensor S1 and the measured value H of the second uniaxial magnetic quantity sensor S2_S1And H_S2With body attitude angle θ_m、 The expression formula of γ, h is:

H_S1=h (cos γ sin ψ cos θ_m+sinγsinθ_m)

H_S2=h (cos θ_mcosψcosλ+cosγsinψcosθ_msinλ+sinγsinθ_msinλ)

Wherein, λ is S2 and Ox₁Axle clamp angle；

Integral mathematics model:

$f\left({\mathrm{\θ}}_m\right)=\frac{{\∫}_0^{2\mathrm{\π}}{H}_{S2}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}{{\∫}_0^{2\mathrm{\π}}{H}_{S1}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}=\frac{\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{S2}{\left(k\right)}^2}{\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{S1}{\left(k\right)}^2}$

Wherein, N represents that bullet revolves the total sampling number turned around, and k represents sampling instant；WithRepresent the integral operation about roll angle γ；WithRepresent that bullet rotation is turned around Two sensor sample values H_S1And H_S2The quadratic sum computing of discrete sampling point；

$\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{S1}{\left(k\right)}^2=\frac{1}{N}(H_{S1}{\left(1\right)}^2+H_{S1}{\left(2\right)}^2+...+H_{S1}{\left(N\right)}^2)$

$\frac{1}{N}\underset{k=1}{\overset{N}{\mathrm{\Σ}}}{H}_{S2}{\left(k\right)}^2=\frac{1}{N}(H_{S2}{\left(1\right)}^2+H_{S2}{\left(2\right)}^2+...+H_{S2}{\left(N\right)}^2)$

When bullet rotation is turned around, it is assumed that included angle X and course angle ψ are constant, amassing by two sensor sample value quadratic sums Partite transport is calculated, and obtains a f (θ_m) value；Obtain f (θ_m) solution, i.e. can get pitching angle theta_mValue.H_S1Or H_S2For When zero, eliminate unknown number magnetic field intensity scalar h, the estimated value of roll angle γ can be obtained.

As it is shown in figure 1, the present invention uses integration ratio method to calculate body attitude angle specifically includes following steps:

Step 1, by gather one circle magnetic sensor data carry out method of least square filtering

(1) when filter times is less than or equal to designated value p, in the present embodiment, p takes 10, uses to increase and remembers, gradually Introduce and filter:

$\hat{r}\left(k\right)=\frac{2(1-k)}{(k+1)(k+2)}\underset{i=0}{\overset{k}{\mathrm{\Σ}}}r(k-i)+\frac{6}{(k+1)(k+2)}\underset{i=0}{\overset{k}{\mathrm{\Σ}}}(k-i)r(k-i)$

(2) when filter times is more than p, fixed memory is used to filter:

$\hat{r}\left(k\right)=\frac{2(1-p)}{(p+1)(p+2)}\underset{j=k-p}{\overset{k}{\mathrm{\Σ}}}r(k-j)+\frac{6}{(p+1)(p+2)}\underset{j=k-p}{\overset{k}{\mathrm{\Σ}}}(k-j)r(k-j)$

In formula, r (k) represents k instance sample value,Represent the least squares filtering value in k moment.

The sampled value of Magnetic Sensor first passes through wave filter and is filtered, after filtering corresponding noise error, then input integral mould Type calculates pattern function f (θ_m) value.

Step 2, computation model function f (θ_m)

Order: H_S1=h (d₂cosγ+e₂sinγ)

H_S2=h (a₂+b₂cosγ+c₂sinγ)

Wherein,

H is calculated according to wave filter output_S1About θ_mIntegral expression

$\begin{array}{c}{\∫}_0^{2\mathrm{\π}}{H}_{S1}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}={\∫}_0^{2\mathrm{\π}}{\left(h\right(d_2\cos \mathrm{\γ}+e_2\sin \mathrm{\γ}\left)\right)}^{2}d\mathrm{\γ}\\ =h^2{\∫}_0^{2\mathrm{\π}}({d_2}^2{\cos }^2\mathrm{\γ}+{e_2}^2{\sin }^2\mathrm{\γ}+2d_2{e}_2\sin \mathrm{\γ}\cos \mathrm{\γ})d\mathrm{\γ}\\ ={h_2}^2({{\mathrm{\πd}}_2}^2+{{\mathrm{\πe}}_2}^2)\end{array}$

$\begin{array}{c}{\∫}_0^{2\mathrm{\π}}{H}_{S2}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}={\∫}_0^{2\mathrm{\π}}{\left(h\right(a_2+b_2\cos \mathrm{\γ}+c_2\sin \mathrm{\γ}\left)\right)}^{2}d\mathrm{\γ}\\ =h^2{\∫}_0^{2\mathrm{\π}}({a_2}^2+{b_2}^2{\cos }^2\mathrm{\γ}+{c_2}^2{\sin }^2\mathrm{\γ}+2b_2{c}_2\sin \mathrm{\γ}\cos \mathrm{\γ}+2a_2{b}_2\cos \mathrm{\γ}+2a_2{c}_2\sin \mathrm{\γ})d\mathrm{\γ}\\ =h^2(2{{\mathrm{\πa}}_2}^2+{{\mathrm{\πb}}_2}^2+{{\mathrm{\πc}}_2}^2)\end{array}$

Obtain

$f\left({\mathrm{\θ}}_m\right)=\frac{{\∫}_0^{2\mathrm{\π}}{H}_{S2}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}{{\∫}_0^{2\mathrm{\π}}{H}_{S1}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}=\frac{2{\cos }^2{\mathrm{\θ}}_m{\cos }^2{\mathrm{\ψcos}}^2\mathrm{\λ}+{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}+{\sin }^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}}{{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m+{\sin }^2{\mathrm{\θ}}_m}$

Step 3, according to pattern function f (θ_m) solve pitching angle theta_m:

By sin²θ_m=1-cos²θ_mF (θ will be substituted into_m) expression formula, obtain about cos²θ_mExpression formula:

cos²θ_m(2cos²ψcos²λ+sin²ψsin²λ-sin²λ-f(θ_m)sin²ψ+f(θ_m))=f (θ_m)-sin²λ

Assume that denominator is not equal to zero, solve cos θ_mAnd sin θ_m, for obtaining pitching angle theta_mMore general expression formula, avoids simultaneously Introduce be not equal to zero it is assumed that do such as down conversion:

$\left\{\begin{array}{c}u=2{\cos }^2{\mathrm{\ψcos}}^2\mathrm{\λ}+{\sin }^2{\mathrm{\ψsin}}^2\mathrm{\λ}-{\sin }^2\mathrm{\λ}-f\left({\mathrm{\θ}}_m\right){\sin }^2\mathrm{\ψ}+f\left({\mathrm{\θ}}_m\right)\\ v=f\left({\mathrm{\θ}}_m\right)-{\sin }^2\mathrm{\λ}\end{array}\right.$

Arrangement can obtain:

uvsin²θ_m=u (u-v) cos²θ_m

Finally, pitching angle theta is obtained_mExpression formula:

${\mathrm{\θ}}_m=\arctan 2(\sqrt{\left|u(u-v)\right|},\±\sqrt{\left|uv\right|}),{\mathrm{\θ}}_m\∈\[-\mathrm{\π}/2,\mathrm{\π}/2\]$

In formula, two parameters u and v can not be zero simultaneously；

Pitching angle theta_mInitial value value is identical with firing angle quadrant, subsequently value and f (θ_m) extreme point relevant, f (θ_m) every After its extreme point, current period pitching angle theta_mValue should change the angle of pitch quadrant in previous cycle.

Step 4, to calculate nonopiate Magnetic Sensor measurement data be roll angle when zero

In a week of missile rotation, can be approximated to be uniform rotation, and course angle ψ and pitching angle theta are almost unchanged, can Obtain the roll angle γ angle value of body a certain particular moment, the roll in one week interior all moment of missile rotation can be calculated Angle γ.Assume that particular moment is H_S1Or H_S2Zero point, measured value H_S1Or H_S2When being zero, unknown number magnetic can be eliminated Field intensity scalar h, reduces the interference magnetic field environment impact on result of calculation around.

The measured value H of (1) first Magnetic Sensor S1_S1When=0:

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

Arrangeγ ∈ [-π, π]

In formula, two parameters ψ of function and θ_mIt is asynchronously zero；

Try to achieve the roll angle γ in current K cycle_KShould have positive and negative two values, the most corresponding 1,3 quadrants or 2,4 as Limit, now should be according to previous cycle roll angle γ_K-1With the rotational speed omega of body estimation current period the positive and negative situation of roll angle, Choose roll angle γ_KValue.

Bullet often revolves and turns around, and obtains Magnetic Sensor output valve H_S1Maximum H_S1maxWith minima H_S1min, utilize Obtain H_S1maxAnd H_S1minMoment point t (H_S1max) and t (H_S1min), calculate ω=2 (t (H_S1max)-t(H_S1min))。

OrderWherein T is sample period time；γ_KPositive and negative situation withPositive and negative identical.

The measured value H of (2) second Magnetic Sensor S2_S2When=0:

cosθ_mcosψcosλ+cosγsinψcosθ_msinλ+sinγsinθ_mSin λ=0

$cos\mathrm{\γ}=\frac{-a_1{b}_1\±\sqrt{{c_1}^2({b_1}^2+{c_1}^2-{a_1}^2)}}{{b_1}^2+{c_1}^2}$

In formula,

When | sin λ | ＞ | cos θ_mCos ψ | time, the measured value H of the second Magnetic Sensor S2_S2There are two zero points, current K week Phase roll angle γ_KTwo are had to solve γ_1,KAnd γ_2,K, now should calculate by the roll angle γ in K-1 cycle_K-1Plus body rotational speed omega The roll angle approximation of the current period obtainedRoll angle takesWithThe γ that smaller is corresponding；

When | sin λ |=| cos θ_mCos ψ | time, the measured value H of the second Magnetic Sensor S2_S2There is a zero point；

When | sin λ | ＜ | cos θ_mCos ψ | time, the measured value H of the second Magnetic Sensor S2_S2Not necessarily there is zero point, now try to achieve Roll angle γ is inaccurate.

So far, complete once to update；

Step 5, the step 1 that repeats i.e. realize sensor attitude and update to step 4.

Claims (3)

1. a Magnetic Sensor attitude algorithm method, it is characterised in that comprise the steps:

The missile rotation nonopiate Magnetic Sensor measurement data of a week is filtered processing by step 1, employing method of least square；

Step 2, filtered Magnetic Sensor measurement data is input in integral model calculate pattern function；

Step 3, solve the angle of pitch according to pattern function；

Step 4, to calculate nonopiate Magnetic Sensor measurement data be roll angle when zero；

Step 5, repetition step 1 are to step 4 real-time resolving Magnetic Sensor attitude.

Magnetic Sensor attitude algorithm method the most according to claim 1, it is characterised in that the mould described in step 2 Type function f (θ_m) it is by two nonopiate Magnetic Sensor measurement data H_S1And H_S2All sampled points in rotary course Being integrated calculating, do scale operation the most again, concrete formula is:

$f\left({\mathrm{\θ}}_m\right)=\frac{{\∫}_0^{2\mathrm{\π}}{H}_{S2}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}{{\∫}_0^{2\mathrm{\π}}{H}_{S1}{\left(\mathrm{\γ}\right)}^{2}d\mathrm{\γ}}=\frac{2{\cos }^2{\mathrm{\θ}}_m{\cos }^2{\mathrm{\ψcos}}^2\mathrm{\λ}+{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}+{\sin }^2{\mathrm{\θ}}_m{\sin }^2\mathrm{\λ}}{{\sin }^2{\mathrm{\ψcos}}^2{\mathrm{\θ}}_m+{\sin }^2{\mathrm{\θ}}_m}$

Wherein, ψ represents course angle, θ_mRepresenting the angle of pitch, γ represents roll angle, and h is the scalar size of geomagnetic fieldvector H, λ is missile coordinate system Ox₁y₁z₁Middle Magnetic Sensor S2 and Ox₁Axle clamp angle.

Magnetic Sensor attitude algorithm method the most according to claim 1, it is characterised in that basis described in step 3 Pattern function f (θ_m) solve pitching angle theta_mDetailed process be:

θ_m∈[-π/2,π/2]

In formula,

Parameter u and v can not be zero simultaneously.