Anti-alternating magnetic field roll angle calculation method based on geomagnetic sensor

Abstract

The invention discloses an anti-alternating magnetic field roll angle calculation method based on a geomagnetic sensor, and belongs to the technical field of attitude measurement. The method comprises the following steps: firstly, calculating the direction of a geomagnetic field vector in a quasi-projectile coordinate system based on a small attack angle hypothesis by utilizing speed information measured by a satellite receiver arranged on a two-dimensional correction assembly; and then a two-axis geomagnetic sensor installed in the two-dimensional correction assembly is used for measuring the magnetic field intensity in the axial direction of the projectile body coordinate system. And finally, taking the roll angle phase, the roll angle rate, the roll angle acceleration rate and the geomagnetic signal amplitude as state quantities of extended Kalman filtering, taking the magnetic field intensity in the axial direction of the projectile body coordinate system as a measured value, and performing an extended Kalman filtering algorithm to obtain an optimal estimated value of the state quantities. According to the method, an attitude reference is constructed through satellite receiver information, and a roll angle measurement model is constructed by using extended Kalman filter (EKF), so that high-precision roll angle estimation under alternating magnetic field interference is realized.

Description

Technical Field

[0001] This invention belongs to the field of attitude measurement technology, specifically relating to a method for calculating the roll angle of a two-dimensional correction component based on a geomagnetic sensor to resist alternating magnetic fields. Background Technology

[0002] As a core component of modern precision-guided artillery projectiles, the two-dimensional correction module's guidance accuracy directly determines the weapon system's hit effectiveness. This module integrates key modules such as an onboard computer, satellite receiver, roll angle measurement sensor, and servo motor. By replacing the fuze of a traditional projectile, it enables ordinary projectiles to possess precision guidance capabilities. In this system, accurate roll angle measurement is the primary technical prerequisite for effective trajectory correction. The roll angle parameter defines the absolute azimuth of the projectile's rotation around its longitudinal axis, and its measurement accuracy directly affects the accuracy of guidance and control. In practical applications, roll angle measurement errors will lead to phase deviations in the trajectory correction command, resulting in a significant increase in the miss distance. Therefore, achieving high-precision roll angle measurement is the core technical challenge facing the two-dimensional correction module.

[0003] Currently, there are two main methods for measuring roll angle in the field of rotating projectiles;

[0004] I. Inertial Measurement Unit (IMU) Scheme: The IMU directly measures the projectile's rotational angular velocity using a gyroscope and obtains the roll angle information through integration. While this scheme offers high measurement accuracy, it has significant limitations: 1. Poor adaptability to high overload environments: The overload experienced during projectile launch can exceed 10,000g, making it difficult for ordinary commercial-grade MEMS gyroscopes to function properly under such conditions; 2. Low cost-effectiveness: Military-grade IMUs capable of withstanding high overloads are expensive, contradicting the low-cost design philosophy of two-dimensional correction components; 3. Cumulative error problem: The zero-bias instability inherent in gyroscopes leads to the accumulation of integration errors over time.

[0005] II. Traditional Geomagnetic Sensor Solution: Geomagnetic sensors calculate the roll angle by sensing the projection change of the Earth's magnetic field vector in the projectile's coordinate system, offering advantages such as low cost and no accumulated error. However, in specific application scenarios of the two-dimensional correction component, this solution faces significant challenges: 1. Installation Location Limitations: The two-dimensional correction component employs a highly integrated design, with the control compartment and servo compartment arranged adjacently. This compact layout prevents the geomagnetic sensor from being installed far from servo interference sources. 2. Alternating Magnetic Field Interference: The strong alternating magnetic field generated during servo operation (such as...) Figure 6 As shown, this severely interferes with normal measurements of the Earth's magnetic field. Figure 5 and Figure 6 The comparison shows that when there is no interference, the output of the geomagnetic sensor is a regular sine signal, while when there is alternating magnetic field interference, the signal is severely distorted and has burrs and noise.

[0006] Existing methods for calculating the roll angle of geomagnetic sensors suffer from the following technical shortcomings: 1. Limitations of the arctangent algorithm: Traditional methods directly perform four-quadrant arctangent calculations on the Y and Z axis measurements of the geomagnetic sensor. This method performs poorly under alternating magnetic field interference: it is sensitive to measurement noise and has weak anti-interference capabilities; it cannot effectively filter out high-frequency interference components generated by the servo motor; furthermore, the calculation results fluctuate greatly and cannot meet the guidance and control accuracy requirements. 2. Insufficient filter design: While conventional filtering methods (such as low-pass filters) can suppress noise to some extent, they also introduce phase lag, affecting the real-time performance of roll angle measurements. In addition, traditional methods lack modeling of the roll angle motion characteristics and cannot fully utilize the dynamic constraint information of the projectile's rotation.

[0007] Current spinning projectile guidance technology is developing towards higher precision and stronger anti-interference capabilities. For the specific application scenarios of two-dimensional correction components, there is an urgent need for a novel method for calculating the roll angle of a geomagnetic sensor to address the problem of alternating magnetic fields generated during servo motor operation interfering with the measurement of the geomagnetic field, leading to significant errors and noise in roll angle measurements. Summary of the Invention

[0008] In view of this, the present invention provides a method for calculating the roll angle under alternating magnetic field interference based on a geomagnetic sensor. This method constructs an attitude reference through satellite receiver information and uses an extended Kalman filter (EKF) to construct a roll angle measurement model, thereby achieving high-precision roll angle estimation under alternating magnetic field interference.

[0009] A method for calculating the roll angle against alternating magnetic fields based on a geomagnetic sensor, the steps of which are as follows:

[0010] Step 1: Construct the attitude reference of the quasi-projectile coordinate system.

[0011] The navigation coordinate system is obtained using a satellite receiver mounted on a two-dimensional correction assembly. The velocity vector in the system. Based on the small angle of attack assumption (the angle of attack and sideslip angle during projectile flight are small angles, which can be approximated as zero), the velocity inclination angle is... and velocity deflection angle Approximately the pitch angle of the projectile and yaw angle According to the pitch angle and yaw angle Constructing from the navigation coordinate system ( Transform to the quasi-projectile coordinate system ( Transformation matrix The quasi-projectile coordinate system ( It does not roll with the projectile.

[0012] Step 2: Calculate the reference projection of the geomagnetic vector.

[0013] Based on the location information (longitude, latitude, and altitude) measured by a satellite receiver, the characteristics of the local magnetic field, including magnetic field strength, are calculated using a geomagnetic model (such as the World Magnetic Model (WMM) or the International Geomagnetic Reference Field (IGRF)). Magnetic tilt angle And magnetic declination Based on these factors, calculate the projection of the geomagnetic vector in the navigation coordinate system. .

[0014] Step 3: Calculate the offset angle of the geomagnetic field in the quasi-ballistic coordinate system.

[0015] Using the transformation matrix The geomagnetic vector in the navigation coordinate system Transform to the quasi-projectile coordinate system to obtain the geomagnetic vector. .calculate Projection amplitude in the YZ plane of the quasi-projectile coordinate system and the projection vector and the quasi-projectile coordinate system Angle between axes included angle This indicates the reference direction of the geomagnetic vector in the quasi-projectile coordinate system.

[0016] Step 4: Establish a geomagnetic measurement model.

[0017] The geomagnetic sensor is fixed to the projectile coordinate system and rotates together with the two-dimensional correction component. The projectile coordinate system is defined relative to the geomagnetic vector in the quasi-projectile coordinate system. The included angle of the planar projection is the relative roll angle. Geomagnetic sensors , The axial measurement reflects the rotational state of the projectile relative to the geomagnetic vector.

[0018] Step 5: Construct the extended Kalman filter.

[0019] Construct an extended Kalman filter (EKF) and define the state vector. To adapt to the interference of the alternating magnetic field and effectively track changes in roll angle and roll angular velocity, the state vector... Selected as ,in Indicates relative roll angle, Indicates roll angular velocity, Indicates roll acceleration. This represents the amplitude of the geomagnetic sensor signal. The state transition equation is constructed based on a uniformly accelerated rotation model, utilizing the rotational kinematics of the projectile to predict the state.

[0020] Step Six: Measurement Update and Signal Preprocessing.

[0021] Real-time measurements from the geomagnetic sensor are acquired, and its output signal is debiased (e.g., using a high-pass filter) to eliminate zero bias and temperature drift, reducing the performance requirements of the sensor. The processed data is then used as the measurement vector for the EKF. Constructing nonlinear measurement equations Establish the relationship equation between the state vector and the measurement vector.

[0022] Step 7: Noise characteristic matching and optimal estimation.

[0023] The process noise covariance matrix of the extended Kalman filter is set according to the noise characteristics of the alternating magnetic field. Measurement noise covariance matrix To address the alternating magnetic field interference generated during servo motor operation, the measurement noise covariance matrix is ​​increased. The magnitude of the values ​​is adjusted to reduce the weight of disturbed measurement data in state updates. Simultaneously, based on the control bandwidth of the roll channel of the two-dimensional correction component, the process noise variance of roll angular velocity and roll angular acceleration is slightly increased, enabling the state vector to quickly track changes in the projectile's roll motion. The filter is then applied to output the optimal estimates of the relative roll angle and roll angular velocity. Finally, the roll angle of the two-dimensional correction component relative to the ground is calculated. .

[0024] Beneficial effects:

[0025] 1. The invention has strong anti-interference capability: The solution method of the invention utilizes the principle of extended Kalman filtering and selects the relative roll angle. Roll angular velocity Roll acceleration Geomagnetic sensor signal amplitude as a state vector Meanwhile, unlike traditional methods that directly calculate the roll angle by performing four-quadrant arctangent operations on the y and z axis data of geomagnetic sensors, this invention uses the y and z axis data as the measurement vectors of the extended Kalman filter, thereby simultaneously predicting and estimating four state vectors. It has strong resistance to alternating magnetic field interference and the calculated roll angular velocity and roll angular velocity accuracy are high.

[0026] 2. The parameter tuning method of this invention is simple: when tuning parameters, the main adjustments are made to four parameters in the process noise covariance matrix. , , , And two parameters in the measurement noise covariance matrix , . , , , These represent the relative roll angles. Roll angular velocity Roll acceleration Geomagnetic sensor signal amplitude The desired tracking performance and noise suppression capabilities can be rapidly adjusted in conjunction with the guidance and control requirements of the two-dimensional correction component. , It is the standard deviation of the alternating magnetic field interference noise generated when the servo motor is working. It can be quickly calculated by simply recording the output data of the geomagnetic sensor when the servo motor is working. The parameter tuning method is simple and intuitive, which reduces the professional requirements of engineers in terms of algorithm understanding and signal processing knowledge, and improves the efficiency of algorithm application and implementation.

[0027] 3. The present invention has a high cost-effectiveness ratio: Since it does not require expensive high-performance MEMS gyroscopes, it can achieve high-precision roll angle calculation by relying only on low-cost geomagnetic sensors combined with satellite receivers equipped with two-dimensional correction components, which meets the low-cost requirements of two-dimensional correction components. Attached Figure Description

[0028] Figure 1 This is a schematic diagram of the roll angle calculation method based on a geomagnetic sensor for resisting alternating magnetic fields according to the present invention.

[0029] Figure 2 This is a schematic diagram illustrating the transformation relationship between the navigation coordinate system and the quasi-missile coordinate system of the present invention;

[0030] Figure 3 This is a schematic diagram illustrating the transformation relationship between the navigation coordinate system and the geomagnetic coordinate system of this invention;

[0031] Figure 4 This invention is based on the quasi-projectile coordinate system. A schematic diagram showing the relationship between the in-plane magnetic vector projection and the relative roll angle;

[0032] Figure 5 This is the output curve of the geomagnetic sensor when there is no alternating magnetic field interference (the servo motor is not working);

[0033] Figure 6 This is the output curve of the geomagnetic sensor when there is alternating magnetic field interference (servo motor reciprocating motion);

[0034] Figure 7 This invention demonstrates the calculation results of roll angle and roll angular velocity under alternating magnetic field interference. Detailed Implementation

[0035] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0036] 1. Reference roll angle Calculation principle:

[0037] See Figure 2 As shown, the navigation coordinate system is defined as North, Sky, and East. During the projectile's flight, the angle of attack and sideslip angle are small angles, and the velocity vector can be considered to coincide with the projectile axis vector, thus:

[0038]

[0039] Therefore, the pitch angle and sideslip angle can be determined based on the northward velocity measured by the satellite receiver. Horizontal speed Eastward speed calculate:

[0040]

[0041] like Figure 2 As shown, the navigation coordinate system is transformed into the quasi-missile coordinate system through two rotations. The first rotation is a clockwise rotation about the celestial axis. Angles are calculated to obtain an intermediate coordinate system. Then, the angle is revolved around the intermediate coordinate system. Rotate the axis clockwise Angle, to obtain the quasi-projectile coordinate system ( Based on this, the rotation matrix The expression is:

[0042]

[0043] like Figure 3 As shown, after the satellite receiver measures the coordinate system information of the projectile, the local geomagnetic vector can be obtained through the geomagnetic model. The orientation of this geomagnetic vector is determined by magnetic declination. and magnetic inclination The method for transforming from the navigation coordinate system to the quasi-projectile coordinate system is determined by the rotation matrix. The expression is:

[0044]

[0045] Therefore, it can be calculated Figure 4 Central reference geomagnetic vector and reference roll angle .

[0046] First, calculate the projection of the geomagnetic vector onto the quasi-projectile coordinate system:

[0047]

[0048] Then the calculation yields... .

[0049] 2. Relative roll angle Calculation principle:

[0050] The first step is to initialize the process noise covariance matrix of the extended Kalman filter. Measurement noise covariance matrix .

[0051] The second step is to collect data from geomagnetic sensors. shaft and The sensor output value of the axis is preprocessed and debiased. In this embodiment, a second-order high-pass filter is used to eliminate the bias. The basic transfer function of the second-order high-pass filter is as follows: Represents complex frequency. and It is a coefficient that adjusts the bandwidth of the filter:

[0052]

[0053] The third step, as Figure 4 As shown, the geomagnetic sensor is fixed to the projectile's coordinate system. Under ideal conditions, the geomagnetic sensor at any given time... shaft and The measured values ​​of the shaft are: .like Figure 5 As shown, when there is no alternating magnetic field interference (i.e., the servo motor is not working), the two-dimensional correction component rotates ( Figure 5 When the rotational speed is 4 revolutions per second, the geomagnetic sensor outputs two sinusoidal signals with different phases and frequencies equal to the rotational speed. For example... Figure 6 As shown, under alternating magnetic field interference (servo motor reciprocating motion), the output value of the geomagnetic sensor will produce distortion, burrs and other noise interference, and conventional methods cannot accurately calculate the roll angle.

[0054] The fourth step is to construct the state vector of the extended Kalman filter. ,in Indicates relative roll angle, Indicates roll angular velocity, Indicates roll acceleration. This represents the amplitude of the geomagnetic sensor signal. Based on the constant angular acceleration model, the state transition equation is obtained. ,in Sampling period:

[0055]

[0056] Fifth step: Construct the nonlinear measurement equations for the extended Kalman filter:

[0057]

[0058] Step 6: Derive the measurement Jacobian matrix based on the measurement equation:

[0059]

[0060] Step 7: Perform initial estimation and state prediction. The initial state can be estimated as follows: Initial state error covariance matrix Then, state prediction is performed to obtain , .

[0061] Step nine: Proceed to the extended Kalman filter iterative calculation process.

[0062] a) Obtain the geomagnetic sensor measurement values ​​for the new cycle as the measurement vector. .

[0063] b) Update the measurement equation And measurement Jacobian matrix .

[0064] c) Calculate the Kalman filter gain

[0065] .

[0066] d) Calculate the current state estimate Estimates of the process noise covariance matrix Output the current relative roll angle. and roll angular velocity And calculate the roll angle. .

[0067] e) Perform state prediction: , After the extended Kalman filter algorithm has finished executing, return to a) to wait for the next geomagnetic sensor measurement.

[0068] Figure 7 For this method to Figure 6 A diagram illustrating the effect of calculating the roll angle from geomagnetic sensor measurement data under alternating magnetic field interference. The roll angle error calculated using this method is... Standard deviation Roll angular velocity error standard deviation of roll angular velocity It has high precision, low noise and jitter, and can be used for guidance and control in two-dimensional correction components.

[0069] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. A method for calculating the roll angle against alternating magnetic fields based on a geomagnetic sensor, characterized in that, Step 1: Use a satellite receiver to obtain the coordinates of the two-dimensional correction component in the navigation system ( Calculate the velocity tilt angle under the following conditions: and velocity deflection angle Based on the small angle of attack assumption, the velocity inclination angle and velocity deflection angle are approximated as the pitch angle. and yaw angle Therefore, the matrix that transforms the vectors in the navigation coordinate system to the quasi-projectile coordinate system is calculated. ; Step 2: Based on the location information measured by the satellite receiver, calculate the local geomagnetic elements, including the geomagnetic field strength, using a geomagnetic model. Magnetic tilt angle And magnetic declination This yields the matrix that transforms vectors in the navigation coordinate system to their representation in the geomagnetic coordinate system. ; Step 3: Using the matrix and The geomagnetic vector in the quasi-ballistic coordinate system was calculated. Projection in a plane and the vector and Angle between axes ; Step 4: Establish the measurement model of the geomagnetic sensor, fix the geomagnetic sensor to the projectile coordinate system, and define the projectile coordinate system relative to the geomagnetic vector. The angle between projections in the plane is the relative roll angle. ; Step 5: Construct an extended Kalman filter (EKF), setting the state vector to the relative roll angle. Roll angular velocity Roll acceleration and the amplitude of the geomagnetic sensor signal ; Step Six: Based on the full-scale time-varying characteristics of the pitch and yaw angles during ballistic flight, the geomagnetic sensor measurements, after bias removal processing, are used as the measurements for the extended Kalman filter. And construct nonlinear measurement equations ; Step 7: Set the process noise covariance matrix based on the noise characteristics of the alternating magnetic field. Measurement noise covariance matrix Then run the extended Kalman filter to output the relative roll angle. and roll angular velocity The optimal estimate; Step 8: Based on the relative roll angle in Step 7 and roll angular velocity The roll angle of the two-dimensional correction component was calculated. .

2. The method for calculating the roll angle of an alternating magnetic field based on a geomagnetic sensor as described in claim 1, characterized in that, The matrix used in step one to transform the vectors in the navigation coordinate system to the quasi-projectile coordinate system The expression is: 。 3. The method for calculating the roll angle against alternating magnetic fields based on a geomagnetic sensor according to claim 1, characterized in that, The matrix described in step two for transforming vectors in the navigation coordinate system to their representation in the geomagnetic coordinate system. The expression is: 。 4. The method for calculating the roll angle of an alternating magnetic field based on a geomagnetic sensor according to claim 3, characterized in that, In step three, the geomagnetic vector in the quasi-projectile coordinate system The following components are denoted as Then the projection amplitude and included angle The calculation formulas are as follows: 。 5. The method for calculating the roll angle of an alternating magnetic field based on a geomagnetic sensor according to claim 4, characterized in that, In step five, the state vector The state transition equation is constructed based on a uniformly accelerated rotation model, assuming... Given the sensor sampling period, the state transition equation is... for: 。 6. The method for calculating the roll angle of an alternating magnetic field based on a geomagnetic sensor according to claim 5, characterized in that, In step six, the measurement equation The specific form is as follows: ； in, This is the measurement value of the Y-axis of the geomagnetic sensor. It is the negative of the Z-axis measurement. For measuring noise.

7. The method for calculating the roll angle of an alternating magnetic field based on a geomagnetic sensor according to claim 6, characterized in that, In step seven, the process noise covariance matrix is ​​set. Measurement noise covariance matrix The expression is: ； To counter alternating magnetic field interference, a larger setting is required. and The numerical values ​​are used to reduce the weight of measurements on state updates; at the same time, a larger value is set. and The numerical values ​​improve the tracking performance of roll velocity and angular acceleration while suppressing noise caused by alternating magnetic field interference.

8. The method for calculating the roll angle of an alternating magnetic field based on a geomagnetic sensor according to claim 7, characterized in that, The final output of the two-dimensional correction component's roll angle Calculated using the following formula: 。

Images