CN105937911A

CN105937911A - Magnetic sensor attitude calculation method

Info

Publication number: CN105937911A
Application number: CN201610517475.7A
Authority: CN
Inventors: 吴盘龙; 朱建良; 刘佳乐; 李星秀; 周洋; 薄煜明; 邹卫军
Original assignee: Nanjing University of Science and Technology
Current assignee: Nanjing University of Science and Technology
Priority date: 2016-07-01
Filing date: 2016-07-01
Publication date: 2016-09-14

Abstract

The invention relates to a method for calculating the attitude of a magnetic sensor. function; solve the pitch angle according to the model function; calculate the roll angle when the non-orthogonal magnetic sensor measurement data is zero; repeat the above steps to solve the magnetic sensor attitude in real time. The present invention utilizes all the data of the magnetic sensor within the scope of one rotation of the projectile to calculate the pitch angle, and the calculation result is more accurate.

Description

A magnetic sensor attitude calculation method

技术领域technical field

本发明涉及姿态测量领域，具体涉及一种磁传感器姿态解算方法。The invention relates to the field of attitude measurement, in particular to a magnetic sensor attitude calculation method.

背景技术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 high-precision gyroscope has a limited range, making it difficult to apply it to the high-speed and high-spinning missile-borne environment. The detection of the geomagnetic field has the advantages of fast response, small size, strong resistance to high overload, and no accumulation of errors. It is suitable for attitude measurement of conventional ammunition with high speed and high rotation.

磁传感器测量弹体姿态的方法有多种方式，其中，两个传感器的非正交磁传感器组合测量法采用两个非正交安装的磁传感器采集弹体旋转一圈的地磁场数据，根据弹体旋转一圈时俯仰角和航向角几乎不变，求解弹体横滚角。非正交磁传感器组合测量法解算载体俯仰角有两种算法，零点交叉法和极值比值法。两种算法都能获得对弹体某一时刻的姿态角估计值，但零点交叉法和极值比值法都是利用了弹体旋转一圈时采样数据中的特殊点进行运算，采样值误差较小时，姿态角估计值的误差也较小，采样值受到随机干扰误差增大时，姿态角误差也随之增加，算法易受到随机干扰的影响。There are many ways for the magnetic sensor to measure the attitude of the projectile. Among them, the non-orthogonal magnetic sensor combination measurement method of two sensors uses two non-orthogonal magnetic sensors to collect the geomagnetic field data of the projectile’s rotation. When the body rotates a circle, the pitch angle and heading angle are almost unchanged, and the roll angle of the projectile is solved. Non-orthogonal magnetic sensor combination measurement method has two algorithms for calculating carrier pitch angle, zero point crossing method and extreme value ratio method. Both algorithms can obtain the estimated value of the attitude angle of the projectile at a certain moment, but both the zero-point crossing method and the extreme value ratio method use special points in the sampling data when the projectile rotates a circle, and the error of the sampling value is relatively small. When the error of the estimated value of the attitude angle is small, the error of the estimated value of the attitude angle is also small. When the error of the sampling value is increased by the random disturbance, the error of the attitude angle also increases, and the algorithm is easily affected by the random disturbance.

发明内容Contents of the invention

本发明的目的在于克服现有零点交叉法和极值比值法的不足，提出一种磁传感器姿态解算方法。The purpose of the present invention is to overcome the deficiencies of the existing zero-point crossing method and extreme value ratio method, and propose a method for calculating the attitude of a magnetic sensor.

实现本发明目的的技术方案为：一种磁传感器姿态解算方法，括如下步骤：The technical solution for realizing the object of the present invention is: a method for calculating the attitude of a magnetic sensor, comprising the following steps:

步骤1、采用最小二乘法对弹体旋转一周的非正交磁传感器测量数据进行滤波处理；Step 1, using the least squares method to filter the non-orthogonal magnetic sensor measurement data of the missile body for one revolution;

步骤2、将滤波后的磁传感器测量数据输入到积分模型中计算模型函数；Step 2, input the filtered magnetic sensor measurement data into the integral model to calculate the model function;

步骤3、根据模型函数求解俯仰角；Step 3, solving the pitch angle according to the model function;

步骤4、计算非正交磁传感器测量数据为零时的横滚角；Step 4, calculating the roll angle when the non-orthogonal magnetic sensor measurement data is zero;

步骤5、重复步骤1至步骤4实时解算磁传感器姿态。Step 5. Repeat steps 1 to 4 to calculate the attitude of the magnetic sensor in real time.

与现有技术相比，本发明的有益效果为：本发明的积分比值法是利用弹丸旋转一圈范围内磁传感器的所有数据进行计算俯仰角；相对于极值比值法在每个旋转周期仅取一个数据，积分模型在每个旋转周期取得多组数据，在噪声方差越大的情况下，本发明俯仰角的解算误差相对于极值比值法越小。Compared with the prior art, the beneficial effects of the present invention are: the integral ratio method of the present invention uses all the data of the magnetic sensor within the range of the projectile to rotate to calculate the pitch angle; Taking one piece of data, the integral model obtains multiple sets of data in each rotation period, and the greater the noise variance is, the smaller the calculation error of the pitch angle of the present invention is compared with the extreme value ratio method.

附图说明Description of drawings

图1是本发明的磁传感器姿态解算方法流程图。Fig. 1 is a flow chart of the magnetic sensor attitude calculation method of the present invention.

图2是本发明实施例中两轴磁传感器安装示意图。Fig. 2 is a schematic diagram of the installation of the two-axis magnetic sensor in the embodiment of the present invention.

具体实施方式detailed description

结合图1，本发明的一种磁传感器姿态解算方法，包括如下步骤：In conjunction with Fig. 1, a kind of magnetic sensor attitude calculation method of the present invention comprises the following steps:

进一步的，步骤2所述的模型函数f(θ_m)是将两个非正交磁传感器测量数据H_S1和H_S2在旋转过程中的所有采样点进行积分计算，然后再做比例运算，具体公式为：Further, the model function f(θ _m ) described in step 2 is to integrate and calculate all the sampling points of the two non-orthogonal magnetic sensor measurement data H _S1 and H _S2 during the rotation process, and then perform a proportional operation, specifically The formula is:

$f f (({θ θ}_{m m})) = = \frac{{&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 22} {((γ γ))}^{22} d d γ γ}{{&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 11} {((γ γ))}^{22} d d γ γ} = = \frac{22 {cos cos}^{22} {θ θ}_{m m} {cos cos}^{22} {ψcos ψcos}^{22} λ λ + + {sin sin}^{22} {ψcos ψcos}^{22} {θ θ}_{m m} {sin sin}^{22} λ λ + + {sin sin}^{22} {θ θ}_{m m} {sin sin}^{22} λ λ}{{sin sin}^{22} {ψcos ψcos}^{22} {θ θ}_{m m} + + {sin sin}^{22} {θ θ}_{m m}}$

其中，ψ表示航向角，θ_m表示俯仰角，γ表示横滚角，h为地磁场矢量H的标量大小，λ为弹体坐标系Ox₁y₁z₁中磁传感器S2与Ox₁轴夹角。Among them, ψ represents the heading angle, θ _m represents the pitch angle, γ represents the roll angle, h represents the scalar magnitude of the geomagnetic field vector H, and λ represents the axis between the magnetic sensor S2 and Ox ₁ in the projectile coordinate system Ox ₁ y ₁ z ₁ horn.

进一步的，步骤3所述根据模型函数f(θ_m)求解俯仰角θ_m的具体过程为：Further, the specific process of solving the pitch angle θ _m according to the model function f(θ _m ) described in step 3 is:

${θ θ}_{m m} = = arctan arctan 22 ((\sqrt{| | u u ((u u - - v v)) | |},, &PlusMinus; &PlusMinus; \sqrt{| | u u v v | |})),, {θ θ}_{m m} &Element; &Element; [[- - π π / / 22,, π π / / 22]]$

式中， In the formula,

两个参数u和v不能同时为零。Both parameters u and v cannot be zero at the same time.

下面结合附图和具体实施例对本发明的方法作进一步说明。The method of the present invention will be further described below in conjunction with the accompanying drawings and specific embodiments.

实施例Example

磁场强度矢量H在弹体坐标系Ox₁y₁z₁中的表达式H_b表示为：The expression H _b of the magnetic field intensity vector H in the projectile coordinate system Ox ₁ y ₁ z ₁ is expressed as:

${H h}_{b b} = = [\begin{matrix} h h {cosθ cosθ}_{m m} c c o o s the s ψ ψ \\ h h (({sinγsinψcosθ sinγsinψcosθ}_{m m} - - {cosγsinθ cosγsinθ}_{m m})) \\ h h (({cosγsinψcosθ cosγsinψcosθ}_{m m} + + {sinγsinθ sinγsinθ}_{m m})) \end{matrix}]$

其中，ψ表示航向角，θ_m表示俯仰角，γ表示横滚角，h为地磁场矢量H的标量大小。Among them, ψ represents the heading angle, θ _m represents the pitch angle, γ represents the roll angle, and h is the scalar magnitude of the geomagnetic field vector H.

如图2所示，两个非正交的第一单轴磁传感器S1和第二单轴磁传感器S2分别安装在弹体坐标系的Ox₁y₁z₁的原点上，Ox₁轴与弹体纵轴重合，两个敏感轴都在Ox₁z₁平面内，S1沿Oz₁轴安装，S2与Ox₁轴呈λ角安装。As shown in Figure 2, two non-orthogonal first uniaxial magnetic sensors S1 and second uniaxial magnetic sensors S2 are respectively installed on the origin of Ox ₁ y ₁ z ₁ in the projectile coordinate system, and the Ox ₁ axis and the projectile The body longitudinal axis coincides, the two sensitive axes are in the Ox ₁ z ₁ plane, S1 is installed along the Oz ₁ axis, and S2 is installed at a λ angle with the Ox ₁ axis.

第一单轴磁传感器S1和第二单轴磁传感器S2的测量值H_S1和H_S2与弹体姿态角θ_m、γ、h的表达式为：The expressions of the measurement values H _S1 and H _S2 of the first uniaxial magnetic sensor S1 and the second uniaxial magnetic sensor S2 and the missile attitude angles θ _m , γ, and h are:

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

H_S2＝h(cosθ_mcosψcosλ+cosγsinψcosθ_msinλ+sinγsinθ_msinλ)H _S2 ＝h(cosθ _m cosψcosλ+cosγsinψcosθ _m sinλ+sinγsinθ _m sinλ)

其中，λ为S2与Ox₁轴夹角；Wherein, λ is the angle between S2 and Ox ₁ axis;

积分数学模型：Integral mathematical model:

$f f (({θ θ}_{m m})) = = \frac{{&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 22} {((γ γ))}^{22} d d γ γ}{{&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 11} {((γ γ))}^{22} d d γ γ} = = \frac{\frac{11}{N N} {Σ Σ}_{k k = = 11}^{N N} {H h}_{S S 22} {((k k))}^{22}}{\frac{11}{N N} {Σ Σ}_{k k = = 11}^{N N} {H h}_{S S 11} {((k k))}^{22}}$

其中，N表示弹丸旋转一圈的总采样次数，k表示采样时刻；和表示关于横滚角γ的积分运算；和表示弹丸旋转一圈两个传感器采样值H_S1和H_S2的离散采样点的平方和运算；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;

$\frac{11}{N N} {Σ Σ}_{k k = = 11}^{N N} {H h}_{S S 11} {((k k))}^{22} = = \frac{11}{N N} (({H h}_{S S 11} {((11))}^{22} + + {H h}_{S S 11} {((22))}^{22} + + ... ... + + {H h}_{S S 11} {((N N))}^{22}))$

$\frac{11}{N N} {Σ Σ}_{k k = = 11}^{N N} {H h}_{S S 22} {((k k))}^{22} = = \frac{11}{N N} (({H h}_{S S 22} {((11))}^{22} + + {H h}_{S S 22} {((22))}^{22} + + ... ... + + {H h}_{S S 22} {((N N))}^{22}))$

弹丸旋转一圈时，假设夹角λ和航向角ψ不变，通过两个传感器采样值平方和的积分运算，得到一个f(θ_m)的值；求出f(θ_m)的解，即可得到俯仰角θ_m的值。H_S1或H_S2为零时，消去未知数磁场强度标量h，可获得横滚角γ的估计值。When the projectile rotates one circle, assuming that the angle λ and the heading angle ψ remain unchanged, a value of f(θ _m ) is obtained through the integral operation of the sum of the squares of the sampling values of the two sensors; the solution of f(θ _m ) is obtained, namely The value of pitch angle θ _m can be obtained. When H _S1 or H _S2 is zero, the estimated value of roll angle γ can be obtained by eliminating the unknown magnetic field strength scalar h.

如图1所示，本发明采用积分比值法计算弹体姿态角具体包括如下步骤：As shown in Figure 1, the present invention adopts the integral ratio method to calculate the projectile attitude angle and specifically includes the following steps:

步骤1、将采集的一圈磁传感器数据进行最小二乘法滤波Step 1. Perform least squares filtering on the collected data of a circle of magnetic sensors

(1)当滤波次数小于等于指定值p时，本实施例中p取10，采用增长记忆、逐次引入滤波：(1) When the number of times of filtering is less than or equal to the specified value p, p is taken as 10 in this embodiment, and the memory is increased and filtering is introduced successively:

$\overset{^^}{r r} ((k k)) = = \frac{22 ((11 - - k k))}{((k k + + 11)) ((k k + + 22))} {Σ Σ}_{i i = = 00}^{k k} r r ((k k - - i i)) + + \frac{66}{((k k + + 11)) ((k k + + 22))} {Σ Σ}_{i i = = 00}^{k k} ((k k - - i i)) r r ((k k - - i i))$

(2)当滤波次数大于p时，采用固定记忆滤波：(2) When the number of filtering is greater than p, use fixed memory filtering:

$\overset{^^}{r r} ((k k)) = = \frac{22 ((11 - - p p))}{((p p + + 11)) ((p p + + 22))} {Σ Σ}_{j j = = k k - - p p}^{k k} r r ((k k - - j j)) + + \frac{66}{((p p + + 11)) ((p p + + 22))} {Σ Σ}_{j j = = k k - - p p}^{k k} ((k k - - j j)) r r ((k k - - j j))$

式中，r(k)表示k时刻采样值，表示k时刻的最小二乘滤波值。In the formula, r(k) represents the sampling value at time k, Indicates the least squares filter value at time k.

磁传感器的采样值先经过滤波器进行滤波，滤除相应的噪声误差后，再输入积分模型中计算模型函数f(θ_m)的值。The sampling value of the magnetic sensor is first filtered by a filter to filter out the corresponding noise error, and then input into the integral model to calculate the value of the model function f(θ _m ).

步骤2、计算模型函数f(θ_m)Step 2. Calculate the model function f(θ _m )

令：H_S1＝h(d₂cosγ+e₂sinγ)Order: H _S1 ＝h(d ₂ cosγ+e ₂ sinγ)

H_S2＝h(a₂+b₂cosγ+c₂sinγ)H _S2 ＝h(a ₂ +b ₂ cosγ+c ₂ sinγ)

其中， in,

根据滤波器输出计算H_S1关于θ_m的积分表达式Calculate the integral expression of H _S1 with respect to θ _m from the filter output

$\begin{matrix} {&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 11} {((γ γ))}^{22} d d γ γ = = {&Integral; &Integral;}_{00}^{22 π π} {((h h (({d d}_{22} cos cos γ γ + + {e e}_{22} sin sin γ γ))))}^{22} d d γ γ \\ = = {h h}^{22} {&Integral; &Integral;}_{00}^{22 π π} (({d d}_{22}^{22} {cos cos}^{22} γ γ + + {e e}_{22}^{22} {sin sin}^{22} γ γ + + 22 {d d}_{22} {e e}_{22} sin sin γ γ cos cos γ γ)) d d γ γ \\ = = {h h}_{22}^{22} (({πd πd}_{22}^{22} + + {πe πe}_{22}^{22})) \end{matrix}$

$\begin{matrix} {&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 22} {((γ γ))}^{22} d d γ γ = = {&Integral; &Integral;}_{00}^{22 π π} {((h h (({a a}_{22} + + {b b}_{22} cos cos γ γ + + {c c}_{22} sin sin γ γ))))}^{22} d d γ γ \\ = = {h h}^{22} {&Integral; &Integral;}_{00}^{22 π π} (({a a}_{22}^{22} + + {b b}_{22}^{22} {cos cos}^{22} γ γ + + {c c}_{22}^{22} {sin sin}^{22} γ γ + + 22 {b b}_{22} {c c}_{22} sin sin γ γ cos cos γ γ + + 22 {a a}_{22} {b b}_{22} cos cos γ γ + + 22 {a a}_{22} {c c}_{22} sin sin γ γ)) d d γ γ \\ = = {h h}^{22} ((22 {πa πa}_{22}^{22} + + {πb πb}_{22}^{22} + + {πc πc}_{22}^{22})) \end{matrix}$

得到get

步骤3、根据模型函数f(θ_m)求解俯仰角θ_m：Step 3. Solve the pitch angle θ _m according to the model function f(θ _m ):

将sin²θ_m＝1-cos²θ_m将代入f(θ_m)表达式，得到关于cos²θ_m的表达式：Substitute sin ² θ _m ＝1-cos ² θ _m into the expression of f(θ _m ), and get the expression about cos ² θ _m :

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

假设分母不等于零，解得cosθ_m和sinθ_m，为得到俯仰角θ_m更通用的表达式，同时避免引入不等于零的假设，做如下变换：Assuming that the denominator is not equal to zero, cosθ _m and sinθ _m are solved. In order to obtain a more general expression for the pitch angle θ _m and avoid introducing the assumption that it is not equal to zero, the following transformation is performed:

$\{\begin{matrix} u u = = 22 {cos cos}^{22} {ψcos ψcos}^{22} λ λ + + {sin sin}^{22} {ψsin ψsin}^{22} λ λ - - {sin sin}^{22} λ λ - - f f (({θ θ}_{m m})) {sin sin}^{22} ψ ψ + + f f (({θ θ}_{m m})) \\ v v = = f f (({θ θ}_{m m})) - - {sin sin}^{22} λ λ \end{matrix}$

整理可得：Organized to get:

uvsin²θ_m＝u(u-v)cos²θ_m uvsin ² θ _m ＝u(uv)cos ² θ _m

最后，得到俯仰角θ_m的表达式：Finally, the expression for the pitch angle θ _m is obtained:

式中，两个参数u和v不能同时为零；In the formula, the two parameters u and v cannot be zero at the same time;

俯仰角θ_m初始值取值与射角象限相同，随后取值与f(θ_m)的极值点有关，f(θ_m)每经过其极值点后，当前周期俯仰角θ_m取值应改变前一周期的俯仰角象限。The initial value of the pitch angle θ _m is the same as that of the _shooting angle quadrant, and the subsequent value is related to the extreme point of _{f(θ m} ₎ . The pitch angle quadrant of the previous cycle should be changed.

步骤4、计算非正交磁传感器测量数据为零时的横滚角Step 4. Calculate the roll angle when the non-orthogonal magnetic sensor measurement data is zero

弹体旋转的一周内，可以近似为匀速转动，而且航向角ψ和俯仰角θ几乎不变，可获得弹体某一特定时刻的横滚角γ角度值，即可计算出弹体旋转一周内所有时刻的横滚角γ。假设特定时刻为H_S1或H_S2的零点，测量值H_S1或H_S2为零时，可以消去未知数磁场强度标量h，减少周围干扰磁场环境对计算结果的影响。During one round of the projectile’s rotation, it can be approximated as a constant speed rotation, and the heading angle ψ and pitch angle θ are almost constant, and the roll angle γ angle value of the projectile at a specific moment can be obtained, and the calculation of The roll angle γ at all times. 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)第一磁传感器S1的测量值H_S1＝0时：(1) When the measured value H _{S1 of the first magnetic sensor S1} =0:

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

整理得γ∈[-π，π]Tidy up γ∈[-π,π]

式中，函数的两个参数ψ和θ_m不同时为零；In the formula, the two parameters of the function ψ and θ _m are not zero at the same time;

求得当前第K周期的横滚角γ_K应当有正负两个取值，分别对应1,3象限或者2,4象限，此时应根据前一周期横滚角γ_K-1和弹体的转速ω估算当前周期的横滚角正负情况，来选取横滚角γ_K的值。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 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.

弹丸每旋转一圈，得到磁传感器输出值H_S1的最大值H_S1max和最小值H_S1min，利用取得H_S1max和H_S1min的时刻点t(H_S1max)和t(H_S1min)，计算出ω＝2(t(H_S1max)-t(H_S1min))。For each revolution of the projectile, the maximum value H _S1max and the minimum _value H _S1min _of the output value H _S1 of the magnetic sensor _are _obtained , and ω= 2(t( _HS1max )-t( _HS1min )).

令其中T为采样周期时间；γ_K正负情况与正负相同。make Where T is the sampling cycle time; the positive and negative conditions of γ _K are the same as Positive and negative are the same.

(2)第二磁传感器S2的测量值H_S2＝0时：(2) When the measured value H _{S2 of the second magnetic sensor S2} =0:

cosθ_mcosψcosλ+cosγsinψcosθ_msinλ+sinγsinθ_msinλ＝0cosθ _m cosψcosλ+cosγsinψcosθm _sinλ + _sinγsinθm sinλ＝0

$c c o o s the s γ γ = = \frac{- - {a a}_{11} {b b}_{11} &PlusMinus; &PlusMinus; \sqrt{{c c}_{11}^{22} (({b b}_{11}^{22} + + {c c}_{11}^{22} - - {a a}_{11}^{22}))}}{{b b}_{11}^{22} + + {c c}_{11}^{22}}$

式中， In the formula,

当|sinλ|＞|cosθ_m cosψ|时，第二磁传感器S2的测量值H_S2有两个零点，当前第K周期横滚角γ_K有两个解γ_1,K和γ_2,K，此时应计算由第K-1周期的横滚角γ_K-1加上弹体转速ω得到的当前周期的滚转角近似值横滚角取与较小者对应的γ；When |sinλ|＞|cosθ _m cosψ|, the measured value H _S2 of the second magnetic sensor S2 has two zero points, and the roll angle γ _K of the current Kth period has two solutions γ _1,K and γ _2,K , At this time, 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 rotational speed ω of the projectile should be calculated roll angle and γ corresponding to the smaller one;

至此，完成一次更新；At this point, an update is completed;

步骤5、重复步骤1至步骤4即实现传感器姿态更新。Step 5. Repeat steps 1 to 4 to update the sensor attitude.

Claims

1. A magnetic sensor attitude solution method, is characterized in that, comprises the steps:

Step 1, using the least squares method to filter the non-orthogonal magnetic sensor measurement data of the missile body for one revolution;

Step 2, input the filtered magnetic sensor measurement data into the integral model to calculate the model function;

Step 3, solving the pitch angle according to the model function;

Step 4, calculating the roll angle when the non-orthogonal magnetic sensor measurement data is zero;

Step 5. Repeat steps 1 to 4 to calculate the attitude of the magnetic sensor in real time.

2. The magnetic sensor attitude solution method according to claim 1, characterized in that, the model function f (θ _m ) described in step 2 is to rotate two non-orthogonal magnetic sensor measurement data H _S1 and H _S2 All sampling points in the process are integrally calculated and then proportionally calculated. The specific formula is:

f f (({θ θ}_{m m})) = = \frac{{&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 22} {((γ γ))}^{22} d d γ γ}{{&Integral; &Integral;}_{00}^{22 π π} {H h}_{S S 11} {((γ γ))}^{22} d d γ γ} = = \frac{22 {cos cos}^{22} {θ θ}_{m m} {cos cos}^{22} {ψcos ψcos}^{22} λ λ + + {sin sin}^{22} {ψcos ψcos}^{22} {θ θ}_{m m} {sin sin}^{22} λ λ + + {sin sin}^{22} {θ θ}_{m m} {sin sin}^{22} λ λ}{{sin sin}^{22} {ψcos ψcos}^{22} {θ θ}_{m m} + + {sin sin}^{22} {θ θ}_{m m}}

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

3. magnetic sensor attitude solution method according to claim 1, is characterized in that, the concrete process of solving pitch angle θ _m described in step 3 according to model function f (θ _m ) is:

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

In the formula,

The parameters u and v cannot be zero at the same time.