CN108050999A - A kind of infrared and earth magnetism composite rotating body of new breath orthogonality surveys attitude positioning method - Google Patents
A kind of infrared and earth magnetism composite rotating body of new breath orthogonality surveys attitude positioning method Download PDFInfo
- Publication number
- CN108050999A CN108050999A CN201711214682.6A CN201711214682A CN108050999A CN 108050999 A CN108050999 A CN 108050999A CN 201711214682 A CN201711214682 A CN 201711214682A CN 108050999 A CN108050999 A CN 108050999A
- Authority
- CN
- China
- Prior art keywords
- infrared
- angle
- geomagnetic
- projectile
- attitude
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
- G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/87—Combinations of systems using electromagnetic waves other than radio waves
- G01S17/875—Combinations of systems using electromagnetic waves other than radio waves for determining attitude
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Electromagnetism (AREA)
- General Physics & Mathematics (AREA)
- General Life Sciences & Earth Sciences (AREA)
- Automation & Control Theory (AREA)
- Geology (AREA)
- Life Sciences & Earth Sciences (AREA)
- Environmental & Geological Engineering (AREA)
- Computer Networks & Wireless Communication (AREA)
- Navigation (AREA)
- Medicines Containing Antibodies Or Antigens For Use As Internal Diagnostic Agents (AREA)
Abstract
The invention discloses a kind of infrared and earth magnetism composite rotating bodies of new breath orthogonality to survey attitude positioning method, including establishing three axis earth magnetism component measurement model of body；On the basis of the basic theories of earth's magnetic field, the theoretical formula of the integration ratio and body pitch angle between Magnetic Sensor output signal is derived, posture angular dimensions is independently solved using the method for integrating ratio；According to the difference of infrared sensor observed direction, the theoretical measurement model of infrared radiation field is obtained；Posture angular dimensions is resolved using three axis infrared sensor pose refinement algorithms of segmentation interaction；To newly breath is analyzed in Kalman filter, the adaptive-filtering handoff algorithms and threshold range of new breath orthogonality are designed；According to new breath orthogon theory, outlier is differentiated；To the judgement that infrared sensor, geomagnetic sensor measurement data are carried out with threshold value, attitude positioning method is surveyed using the infrared and earth magnetism composite rotating body of new breath orthogonality and is handled with geomagnetic sensor output infrared；This method is conducive to improve body attitude calculation accuracy.
Description
Technical Field
The invention belongs to the technical field of navigation control of a rotating body, and particularly relates to an infrared and geomagnetic composite rotating projectile attitude measurement method with innovation orthogonality.
Background
Precision guided munitions are favored by countries in the world because of their ability to efficiently target enemy targets, and their very high killing and deterrence. However, in the current guidance technology, the production cost of the traditional accurate guidance equipment is too high, a large number of troops cannot be equipped, and the research on a high-precision, high-interference-resistance and high-overload-resistance attitude test technology suitable for conventional ammunition becomes the key point and difficulty of research of relevant scientific researchers in all countries in the world.
At present, various methods for acquiring flight attitude parameters of a rotator are available, and in the application of adaptive kalman filtering in geomagnetic attitude detection in the paper "application of adaptive kalman filtering in military science" published by longli et al, an extended kalman filtering model is established for the problem of insufficient measurement accuracy of a geomagnetic attitude detection system under the action of various noises, and through adaptive estimation of system noise, measurement noise and steering engine noise, the adaptation of the whole filtering process is realized, but when geomagnetic anomalies are encountered, abnormal geomagnetic signals obtained by resolving the adaptive extended kalman filtering model cannot be used for attitude testing, and the system detection accuracy is sharply reduced. The infrared sensing principle is analyzed in a paper ' unmanned aerial vehicle attitude measurement system design based on the infrared sensing principle ' published by Lexuan Yu et al sensor and microsystem ', and an infrared sensor is designed to be applied to the unmanned aerial vehicle attitude measurement direction, so that the attitude information of the unmanned aerial vehicle in the flight process can be effectively reflected, but the system attitude angle detection error is larger under the influence of infrared radiation interference.
Disclosure of Invention
The invention aims to provide an infrared and geomagnetic composite rotating projectile attitude measurement method with innovation orthogonality, and aims to solve the problems that an infrared and geomagnetic independent attitude calculation method is weak in anti-interference capacity and low in attitude calculation accuracy under the influence of interference.
The technical solution for realizing the purpose of the invention is as follows:
an innovation orthogonality infrared and geomagnetic composite rotating projectile attitude measurement method comprises the following steps:
step 1, solving attitude angle parameters: establishing a three-axis geomagnetic component measurement model of the projectile according to the relation between a projectile coordinate system and a geographic coordinate system in the flying process of the projectile; on the basis of the basic theory of the geomagnetic field, deducing a theoretical formula of an integral ratio between output signals of the magnetic sensors and a pitch angle of the projectile body, and independently solving attitude angle parameters by using an integral ratio method;
step 2, resolving attitude angle parameters: obtaining a theoretical measurement model of an infrared radiation field according to different observation directions of the infrared sensors; resolving attitude angle parameters by using a theoretical measurement model of the missile-borne infrared sensor and adopting a three-axis infrared sensor attitude optimization algorithm of segmented interaction;
step 3, distinguishing the outlier points: organically combining a measurement model of triaxial geomagnetic components of the projectile body with a theoretical measurement model of the missile-borne infrared sensor, analyzing information in a Kalman filter, and designing a self-adaptive filtering switching algorithm and a threshold range of orthogonality of the information; according to an innovation orthogonal theory, under the condition of infrared or geomagnetic anomaly, discriminating outlier points;
and 4, judging the measurement data and the threshold value of the infrared sensor and the geomagnetic sensor according to the step 3, and processing the output of the infrared sensor and the geomagnetic sensor by adopting an infrared and geomagnetic composite rotating projectile attitude measurement method with innovation orthogonality.
Compared with the prior art, the invention has the following remarkable advantages:
(1) The method utilizes an integral ratio method to calculate the attitude angle parameter, wherein the integral method is adopted to calculate the statistical characteristic ratio of a plurality of points to replace the single-point characteristic ratio, the projectile attitude calculation precision is still high under the condition of low signal-to-noise ratio, and the method is favorable for improving the projectile geomagnetic independent attitude calculation precision;
(2) The attitude angle parameters are solved by utilizing a three-axis infrared sensor attitude optimization algorithm of sectional interaction, wherein the sectional interaction uses the output of infrared sensors of Y and Z axes, and the output value of a sensor with small error transfer coefficient is used for solving the roll angle, so that the attitude solving precision of the roll angle is improved;
(3) According to the method, an infrared and geomagnetic composite rotary projectile body full-attitude resolving model is established, an innovation orthogonality self-adaptive filtering switching estimation algorithm is provided, a measured data field value is eliminated by using an innovation orthogonality theory, filtering parameters are self-adaptively estimated by adopting an innovation windowing method, and compared with a conventional estimation algorithm, the adaptability of an infrared and geomagnetic composite attitude measurement system to a complex environment is improved, and the anti-jamming capability is greatly enhanced.
The present invention is described in further detail below with reference to the attached drawing figures.
Drawings
FIG. 1 is a schematic flow diagram of the process of the present invention.
Fig. 2 is a schematic diagram of a measurement model of triaxial geomagnetic and infrared components of a projectile.
Fig. 3 is a graph of integral ratio versus pitch angle.
FIG. 4 is a graph of estimated error for a projectile attitude angle using a conventional extended Kalman filter algorithm.
FIG. 5 is a graph of an attitude angle estimation error obtained by the adaptive filter switching algorithm using innovation orthogonality according to the present invention.
Detailed Description
For the purpose of illustrating the technical solutions and technical objects of the present invention, the present invention will be further described with reference to the accompanying drawings and specific embodiments.
With reference to fig. 1, the innovation orthogonality infrared and geomagnetic composite rotating projectile attitude measurement method comprises the following steps:
step 1, solving attitude angle parameters: on the basis of the basic theory of the geomagnetic field, deducing a theoretical formula of an integral ratio between output signals of the magnetic sensors and a pitch angle of the projectile body, and independently solving attitude angle parameters by using an integral ratio method;
step 1.1, establishing a three-axis geomagnetic component measurement model of the projectile: establishing a three-axis geomagnetic component measurement model of the projectile according to the relation between a projectile coordinate system and a geographic coordinate system in the flying process of the projectile;
as shown in FIG. 2, the triaxial geomagnetic sensor is located at the center of gravity, O-x, of the projectile b y b z b The missile coordinate system is a navigation reference coordinate system, and the navigation reference coordinate system is an NED geographic coordinate system. The axis of rotation of the projectile being x b And the sensitive direction of the triaxial geomagnetic sensor points to the forward direction of each axis along the carrier coordinate system to meet the right-hand rectangular coordinate system. When the projectile body moves in a posture in space, the projectile body coordinate system is not overlapped with the geographic coordinate system, and the measured value of the geomagnetic sensor is related to the magnetic yaw angle psi, the pitch angle theta, the roll angle gamma and local geomagnetic elements of the projectile body at the moment; establishing a three-axis geomagnetic component measurement model of the projectile body:
wherein M is x Is x under the missile body coordinate system b Geomagnetic component of the axis, M y Is y under the missile coordinate system b Geomagnetic component of the axis, M z Is z under the missile body coordinate system b The geomagnetic component of the axis, M is the local geomagnetic field, and I is the local geomagnetic inclination. And obtaining the relation between the measured value of the triaxial geomagnetic sensor and the attitude angle of the projectile body according to the measurement model.
Step 1.2, solving a theoretical formula of an integral ratio between output signals of the magnetic sensor and pitch angles and roll angles of the projectile body:
acquiring the full attitude information of the carrier by using the assumption that the magnetic yaw angle psi is unchanged in the flight process, and deducing a theoretical formula of an integral ratio according to a three-axis geomagnetic component measurement model of the projectile:
wherein A = cos ψ sin θ + tanIcos θ, W upxy Is M x And M y Integral ratio of the first half period, W downxy Is M x And M y The integral ratio of the next half period can obtain M x And M z W is the integral ratio of the upper half period to the lower half period, and psi and I are constant upxy And W downxy Only related to theta, corresponding to the pitch angle theta of the projectile body one by one, according to the formula (2), in theta epsilon-50 degrees and 130 degrees]The integral ratio is plotted against the pitch angle over the range as shown in figure 3.
In the resolving process, M in each half period can be calculated firstly x And M y 、M z The integral ratio is divided by the roll angle rotating speed in each half period to respectively obtain the integral ratio W upxy 、W downxy And M x And M z Integral ratio W of half period upxz 、M x And M z Integral ratio W of the next half cycle downxz And solving the pitch angle value by using the formula (2). Then, M is solved by the calculated pitch angle theta of each half period and the known yaw angle psi y ＝0、M z The roll angle γ at time = 0.
Step 1.3, correcting the magnetic yaw angle in the flight process:
using output three-axis component M of magnetic sensor x 、M y 、M z And step 1.2, correcting the magnetic yaw angle of the projectile body in the flying process according to the calculated values of the pitch angle and the roll angle, wherein the correction formula is shown as a formula (3).
Step 2, resolving attitude angle parameters: obtaining a theoretical measurement model of an infrared radiation field according to the observation direction of the infrared sensor; resolving attitude angle parameters by utilizing a theoretical measurement model of the missile-borne infrared sensor and adopting a three-axis infrared sensor attitude optimization algorithm with segmented interaction;
step 2.1, deducing a theoretical measurement model of the missile-borne infrared sensor: obtaining a theoretical measurement model of an infrared radiation field according to different observation directions of the infrared sensors;
the three-axis infrared sensor is arranged on the surface of the shell of the projectile body, and the sensitive direction of the three-axis infrared sensor is along the O-x coordinate system of the projectile body b y b z b Pointing in the forward direction of the respective axis. When the observation direction of the infrared sensor points to sky, namely the inclination angle to the ground is 0<β&In the range of lt and pi, the sensing is the descending infrared radiation of the atmosphere; when the observation direction of the infrared sensor points to the earth, namely the dip angle to the earth is pi<β&And in the range of lt 2 pi, the sum of the upward radiation of the atmosphere, the infrared radiation of the ground and the downward infrared radiation of the atmosphere reflected by the ground is sensed. Finally, a theoretical measurement model of the infrared radiation field of the missile-borne infrared sensor with the wave band of 8-14 mu m is obtained, as shown in a formula (4):
wherein T (beta) is the theoretical measurement output of the infrared sensor, lambda represents the infrared radiation wavelength, and lambda 1 ＝8μm，λ 2 =14 μm, beta is the inclination angle to the ground of the observation direction of the infrared sensor, k is the sensor sensitivity coefficient, beta 0 The angle of inclination of the bisector of the angle of field of the infrared sensor is opposite to the ground, and alpha is the angle of field of the infrared sensor; dividing the atmosphere into n layers in the calculation process of atmospheric infrared radiation, wherein i represents the atmosphere of the ith layer, and epsilon s (λ, β) represents the atmospheric infrared emissivity, C 1 Is a first radiation constant, C 2 Is the second radiation constant, T s Is the temperature of the atmosphere, and τ (λ, β) represents the atmospheric transmittance, ε g (λ, β) is the surface emissivity, T g Representing the surface temperature.
Through the coordinate transformation matrix, the relation between the ground inclination angle and the projectile attitude angle can be obtained:
substituting formula (5) into formula (4) to obtain the theoretical formula of infrared sensor output along with to projectile body pitch angle and roll angle, and through data analysis and fitting, can obtain the simplified measurement model approximate fitting of missile-borne infrared sensor as follows:
T(θ,γ)＝Kcosθsin(γ+δ)+B (6)
in the formula, K is the amplitude of the output signal of the infrared sensor, B is the offset of the output signal of the infrared sensor, and delta is the phase angle of the infrared sensor.
Step 2.2, resolving a pitch angle and a roll angle of the projectile body by adopting a three-axis infrared sensor attitude optimization algorithm with sectional interaction:
x in the actual measurement process b Output value T of axial infrared sensor x 、y b Output value T of axial infrared sensor y 、z b Output value T of axial infrared sensor z All with errors, the following analysis can be made according to the principle of error propagation:
the pitch angle can be solved as follows:
θ＝arcsin(T x ) (7)
the roll angle can be calculated as follows:
γ y ＝arcsin(T y /cosθ) (8)
γ z ＝arcsin(T z /cosθ)-0.5π (9)
the pitch angle is resolved only on the basis of a measurement factor T x And the roll angle is calculated in relation to a number of measurement factors, including T y Or T z And pitch angle solution. Therefore, various factors must be reasonably utilized to minimize the error of the solution result.
According to the formula (8) < gamma > y Is about T y And a function of theta, denoted as gamma y ＝f(T y θ), the standard deviation of the function is:
as can be seen from the functional error formula, if the error transfer coefficient of each measured value to the function is made 0 or minimum, the functional error can be reduced accordingly. Since theta can only pass through T x Solved, whereas γ can be passed through T y Or T z Resolving, so let T be y Or T z The smaller the error transfer coefficient of (a), the better.
According to the above formula, T y The smaller the absolute value of (a), the smaller the error transfer coefficient thereof, and the smaller the solution error of the roll angle. In the same way, T z The smaller the absolute value of (a), the smaller the solution error of the roll angle. In the actual measurement process, alternative use of T is adopted y Or T z The roll angle is calculated, so that the calculation error of the roll angle is reduced as much as possible, namely: when | T y |≤|T z L, using T y Resolving roll angle using T z Judging a roll angle quadrant; when | T y |>|T z L. using T z Resolving the roll angle by T y And judging a roll angle quadrant.
Step 3, distinguishing outlier points: organically combining a measurement model of triaxial geomagnetic components of the projectile body with a theoretical measurement model of the missile-borne infrared sensor, analyzing innovation in a Kalman filter, and designing a self-adaptive filtering switching algorithm and a threshold range of the orthogonality of the innovation; according to an innovation orthogonal theory, under the condition of infrared or geomagnetic anomaly, distinguishing outlier points;
step 3.1, setting a threshold range of an adaptive filtering switching algorithm of information orthogonality:
analyzing the innovation in the Kalman filter according to an innovation sequence e k The orthogonal property can be used for judging whether abnormal data exists in the measured data of the sensor, distinguishing and correcting outlier points and updating the sequence e k Expressed as:
wherein Z k Representing the true measurement of the sensor at time k, H represents the linearized output matrix,representing a state matrix X k|k-1 Due to the optimal estimation ofIs Z k Then according to the orthogonal theory:
wherein R is k Is a matrix of n × n, P k Represents the variance matrix of the observation noise at the time k, and can determine abnormal data by using the above formula (13) during the filtering process to set oneThe threshold ranges of (c) are as follows:
(1) When the temperature is higher than the set temperatureThe measured data is normal value;
(2) When in useWhen the range is out of the range in (1), the measured data is abnormal.
Wherein D is a set normal value, and the influence of measurement noise and calculation error is considered to allowHas a fluctuation range of + -epsilon.
Step 3.2, outlier point discrimination under abnormal conditions:
when sensingWhen the measured data of the device is abnormal data, the data of the sensor needs to be corrected. According to the characteristic that the output of the missile-borne infrared and geomagnetic sensors changes into sine along with the rotation of the projectile, judging abnormal data Z k Abandon, adopt Z k Is estimated byAnd substituting the measurement value as a new measurement value into a filtering algorithm so as to eliminate the influence of abnormal interference data on the attitude estimation precision.
The method comprises the following steps of designing a filtering switching algorithm under an abnormal condition to improve the robustness and reliability of an infrared and geomagnetic compounded attitude testing system and improve the environmental adaptability of the system, wherein the core idea is that measurement data of an infrared or geomagnetic sensor in one rotation period of a projectile is obtained, and the measurement data is fitted into a sine function T (T) according to the principle of least square method:
T(t)＝k'sin(w't+δ')+B' (14)
wherein k 'is the amplitude value of the sine function T (T) fitted by the measurement data of the infrared or geomagnetic sensor, δ' is the phase of the sine function T (T) fitted by the measurement data of the infrared or geomagnetic sensor, B 'is the offset of the sine function T (T) fitted by the measurement data of the infrared or geomagnetic sensor, and w' is the rotating speed of the sine function T (T) fitted by the measurement data of the infrared or geomagnetic sensor.
The expression of the sine ρ of the output signal of the infrared or geomagnetic sensor in one rotation period is:
where N is the number of sampling points in a rotation period, T j Is the value of the jth sample point.
Therefore, the sine degree rho of the output signal of the missile-borne infrared and geomagnetic sensor under the condition of no interference can be obtained by pre-calibration 0 It is set as a judgment threshold value and an allowable fluctuation range is given. By detecting sine of infrared sensor or geomagnetic sensor in one rotation period of projectile in real timeAnd comparing the measured data with a set threshold value so as to judge whether the measured data of the infrared and geomagnetic sensors are effective or not, wherein the expression is as follows:
wherein f (T) is a judging function for the validity of the measured data of the infrared sensor and the geomagnetic sensor. A "1" indicates that the data is valid, and a "0" indicates that the data is invalid, indicating a outlier.
And 4, judging the measurement data and the threshold value of the infrared sensor and the geomagnetic sensor according to the step 3, and processing the output of the infrared sensor and the geomagnetic sensor by adopting an infrared and geomagnetic composite rotating projectile attitude measurement method with innovation orthogonality.
Step 4.1, if the sine degree rho detected in real time does not exceed the threshold range, selecting the extended Kalman filtering which adopts the infrared and geomagnetic sensor combination:
the method for estimating the attitude angle and the rotation angular rate of the rotating flying body by using the extended Kalman filtering mainly comprises the following steps.
First, the state at time k is predicted from the state estimate at the previous time:
wherein the content of the first and second substances,representing the state prediction of the state matrix X at time k, X = [ γ ω θ ψ] T Representing a state matrix, where γ represents roll angle, ω represents rotational angular rate, θ represents pitch angle, ψ represents heading angle,represents the state transition matrix, t s Representing an estimated time step.
Prediction of noise covariance matrix P for extended Kalman filtering
P k|k-1 ＝FP k-1 F T +Q (18)
Wherein, P k|k-1 For prediction of the noise covariance matrix, P k-1 And Q is the collected noise variance matrix, and represents the uncertainty of the model.
Observation matrix is Z = [ T = x T y T z M x M y M z ] T ，T x 、T y 、T z Respectively, output voltage, M, of a three-axis infrared sensor x 、M y 、M z Respectively, the output voltage of the triaxial geomagnetic sensor, and the evaluation value of the attitude parameter is used for calculating a linearized output matrix by combining the triaxial infrared sensor measurement model of step 2 and the geomagnetic sensor measurement model of step 1
Predicting P according to the linearized output matrix H by combining the noise covariance matrix k|k-1 The gain K of the Kalman filter at the moment K can be obtained k
K k ＝P k|k-1 H T (HP k|k-1 H T +R k ) -1 (20)
Wherein R is k Is an n × n matrix and represents an observation noise variance matrix. Gain matrix K k Is used to correct the estimated value of the attitude parameter for state updating
Wherein the content of the first and second substances,represents the optimal estimate of X at time k, Z k Representing the true measurement of the sensor at time k;the estimated measurement value at time k can be obtained by the following calculation
Finally, the noise covariance matrix P of the extended Kalman Filter k Update with the following equation
P k ＝(I n -K k H)P k|k-1 (I n -K k H) T +K k RK k T (23)
The estimated value of the attitude parameter of the rotating flying object obtained according to the formula (21) can be used as the input value of the formula (22) for the next filtering estimation.
Step 4.2, if the shot encounters strong infrared radiation interference in the flying process, the output signal of the infrared sensor is seriously influenced, the sine degree rho detected in real time exceeds the threshold range, at the moment, the output signal of the three-axis infrared sensor is rejected, and the magnetic yaw angle psi obtained by estimation in the previous period is used k-1 And (4) substituting the method in the step 4.1 into an extended Kalman filter as a known invariant, and outputting and solving attitude angle parameters of the projectile by using a triaxial geomagnetic sensor.
Step 4.3, when the shot encounters an geomagnetic abnormal condition in the flying process, the output signal of the geomagnetic sensor is seriously influenced, the sine rho detected in real time exceeds a threshold range, at the moment, the output signal of the triaxial geomagnetic sensor is rejected, the method introduced in the step 4.1 is adopted to be substituted into an extended Kalman filter, and the magnetic yaw angle psi obtained by estimation in the previous period is substituted into the extended Kalman filter k-1 And (3) as a known invariant, utilizing the output of the three-axis infrared sensor to solve the pitch angle and roll angle parameters of the projectile body.
Example 1
For an infrared and geomagnetic composite attitude measurement system, under the condition of encountering severe environment interference, strong infrared radiation interference is added into a local area of an output signal of a triaxial infrared sensor to carry out a semi-physical experiment, and the geomagnetism and the output result of the infrared sensor acquired by the experiment are subjected to attitude estimation by respectively adopting a conventional extended Kalman filtering algorithm and an innovation orthogonality self-adaptive filtering switching algorithm designed by the invention. Fig. 4 is an estimation error of a projectile attitude angle by using a conventional extended kalman filter algorithm, and fig. 5 is an estimation error of an attitude angle by using an adaptive filter switching algorithm based on innovation orthogonality. As can be seen from the figure, within the time of 1 to 1.2 seconds, the infrared sensor is interfered by a severe environment, the estimation precision of the attitude angle is seriously influenced by the conventional extended Kalman filtering algorithm within the time, and the estimation error is about +/-10 degrees; and a self-adaptive filtering switching algorithm is adopted, and a filtering model is switched according to the sine degree judgment output by the infrared sensor, so that the estimation error of the attitude angle is controlled within +/-0.5 degrees.
Those skilled in the art will appreciate that the invention may be practiced without these specific details.
Claims (4)
1. An innovation orthogonality infrared and geomagnetic composite rotating projectile attitude measurement method is characterized by comprising the following steps of:
step 1, solving attitude angle parameters: establishing a three-axis geomagnetic component measurement model of the projectile according to the relation between a projectile coordinate system and a geographic coordinate system in the flying process of the projectile; on the basis of the basic theory of the geomagnetic field, a theoretical formula of an integral ratio between output signals of the magnetic sensors and a pitching angle of the projectile body is deduced, and an attitude angle parameter is independently solved by using an integral ratio method;
step 2, resolving attitude angle parameters: obtaining a theoretical measurement model of an infrared radiation field according to different observation directions of the infrared sensors; resolving attitude angle parameters by using a theoretical measurement model of the missile-borne infrared sensor and adopting a three-axis infrared sensor attitude optimization algorithm of segmented interaction;
step 3, distinguishing outlier points: organically combining a measurement model of triaxial geomagnetic components of the projectile body with a theoretical measurement model of the missile-borne infrared sensor, analyzing innovation in a Kalman filter, and designing a self-adaptive filtering switching algorithm and a threshold range of the orthogonality of the innovation; according to an innovation orthogonal theory, under the condition of infrared or geomagnetic anomaly, discriminating outlier points;
and 4, judging the measurement data and the threshold value of the infrared sensor and the geomagnetic sensor according to the step 3, and processing the output of the infrared sensor and the geomagnetic sensor by adopting an infrared and geomagnetic composite rotating projectile attitude measurement method with innovation orthogonality.
2. The innovation orthogonality-based infrared and geomagnetic composite rotating projectile attitude measurement method according to claim 1, wherein the step 1 of solving attitude angle parameters specifically comprises the following steps:
step 1.1, establishing a three-axis geomagnetic component measurement model of the projectile:
establishing a projectile coordinate system O-x b y b z b The three-axis geomagnetic component measurement model of the projectile body is as follows:
wherein, M x Is x under the missile body coordinate system b Geomagnetic component of the axis, M y Is y under the missile body coordinate system b Geomagnetic component of the axis, M z Is z under the missile body coordinate system b The geomagnetic component of the axis, M is the local geomagnetic field, and I is the local geomagnetic inclination angle; psi is the projectile magnetic yaw angle, theta is the pitch angle, and gamma is the roll angle;
step 1.2, solving a theoretical formula of an integral ratio between output signals of the magnetic sensor and pitch angles and roll angles of the projectile body:
according to the three-axis geomagnetic component measurement model of the projectile body, a theoretical formula of an integral ratio is deduced:
wherein, A = cos ψ sin θ + tanIcos θ, W upxy Is M x And M y Integral ratio of first half period, W downxy Is M x And M y Integral ratio of the next half cycle; by the same method, M can be obtained x And M z Integral ratio W of upper half period to lower half period upxz 、W downxz (ii) a Solving the pitch angle value by using a formula (2), and solving M by using the solved pitch angle theta of each half period and the known yaw angle psi y ＝0、M z The roll angle γ at time = 0;
step 1.3, correcting the magnetic yaw angle in the flight process:
3. the innovation orthogonality type infrared and geomagnetic composite rotating projectile attitude measurement method according to claim 2, wherein the step 2 of calculating attitude angle parameters specifically comprises the following steps:
step 2.1, deducing a theoretical measurement model of the missile-borne infrared sensor:
obtaining a theoretical measurement model of an infrared radiation field of a missile-borne infrared sensor with a wave band of 8-14 mu m according to the range of the ground inclination angle of the infrared sensor:
wherein T (beta) is the theoretical measurement output of the infrared sensor, λ represents the infrared radiation wavelength, λ 1 ＝8μm，λ 2 =14 μm, beta is the inclination angle to the ground of the observation direction of the infrared sensor, k is the sensor sensitivity coefficient, beta 0 The angle of field of the infrared sensor is the angle of inclination to the ground of the angular bisector of the angle of field of the infrared sensor, and alpha is the angle of field of the infrared sensor; dividing the atmosphere into n layers, i represents the ith atmosphere, epsilon s (λ, β) represents the atmospheric infrared emissivity, C 1 Is a first radiation constant, C 2 Is the second radiation constant，T s Is the temperature of the atmosphere, and τ (λ, β) represents the atmospheric transmittance, ε g (λ, β) is the surface emissivity, T g Representing the surface temperature;
through the coordinate transformation matrix, the relation between the ground inclination angle and the projectile attitude angle can be obtained:
through data analysis and fitting, the simplified measurement model of the missile-borne infrared sensor is obtained by approximate fitting as follows:
T(θ,γ)＝Kcosθsin(γ+δ)+B (6)
in the formula, K is the amplitude of the output signal of the infrared sensor, B is the offset of the output signal of the infrared sensor, and delta is the phase angle of the infrared sensor;
step 2.2, resolving a pitch angle and a roll angle of the projectile body by adopting a three-axis infrared sensor attitude optimization algorithm with sectional interaction:
according to the error transfer principle:
the pitch angle can be solved as follows:
θ＝arcsin(T x ) (7)
the roll angle can be calculated as follows:
γ y ＝arcsin(T y /cosθ) (8)
γ z ＝arcsin(T z /cosθ)-0.5π (9)
when | T y |≤|T z L. using T y Resolving the roll angle by T z Judging a roll angle quadrant; when | T y |>|T z L. using T z Resolving the roll angle by T y And judging a roll angle quadrant.
4. The innovation orthogonality-based infrared and geomagnetic composite rotating projectile attitude measurement method according to claim 3, wherein the step 3 of distinguishing outlier points specifically comprises the following steps:
step 4.1, if the sine degree rho detected in real time does not exceed the threshold range, adopting extended Kalman filtering of infrared and geomagnetic sensor composition:
first, the state at time k is predicted from the state estimate at the previous time:
wherein the content of the first and second substances,representing the state prediction of the state matrix X at time k, X = [ γ ω θ ψ] T Representing a state matrix, ω representing a rotation angular rate,represents the state transition matrix, t s Representing an estimated time step;
prediction of noise covariance matrix P for extended kalman filtering
P k|k-1 ＝FP k-1 F T +Q (18)
Wherein, P k|k-1 For prediction of the noise covariance matrix, P k-1 The covariance matrix of the noise at the moment of k-1 is obtained, and Q is the collected covariance matrix of the noise;
observation matrix is Z = [ T = [) x T y T z M x M y M z ] T ，T x 、T y 、T z Respectively, output voltage, M, of a three-axis infrared sensor x 、M y 、M z The output voltages of the triaxial geomagnetic sensors, respectively, and the estimated values of the attitude parameters are used to calculate a linearized output matrix
Predicting P according to the linearized output matrix H by combining the noise covariance matrix k|k-1 The gain K of the Kalman filter at the moment K can be obtained k
K k ＝P k|k-1 H T (HP k|k-1 H T +R k ) -1 (20)
Wherein R is k For observing the noise variance matrix, the gain matrix K k Used to correct the estimated values of the attitude parameters, the state update is performed:
wherein, the first and the second end of the pipe are connected with each other,represents the optimal estimate of X at time k, Z k Representing the true measurement of the sensor at time k;the estimated measurement value at time k can be obtained by the following calculation
Finally, the noise covariance matrix P of the extended Kalman Filter k Update with the following equation
P k ＝(I n -K k H)P k|k-1 (I n -K k H) T +K k RK k T (23)
Step 4.2, if the projectile encounters strong infrared radiation interference in the flying process, estimating the magnetic yaw angle psi obtained in the previous period k-1 Substituting the method in the step 4.1 into an extended Kalman filter as a known invariant, and outputting and solving attitude angle parameters of the projectile by using a triaxial geomagnetic sensor;
step 4.3, when the projectile encounters geomagnetic abnormal conditions in the flight process, substituting the method in the step 4.1 into an extended Kalman filter, and performing magnetic navigation on the projectile obtained by estimation in the previous periodAngle psi k-1 And (3) as a known invariant, utilizing the output of a triaxial infrared sensor to solve pitch angle and roll angle parameters of the projectile body.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711214682.6A CN108050999B (en) | 2017-11-28 | 2017-11-28 | Innovative orthogonality infrared and geomagnetic composite rotating projectile attitude measurement method |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711214682.6A CN108050999B (en) | 2017-11-28 | 2017-11-28 | Innovative orthogonality infrared and geomagnetic composite rotating projectile attitude measurement method |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108050999A true CN108050999A (en) | 2018-05-18 |
| CN108050999B CN108050999B (en) | 2021-05-04 |
Family
ID=62120786
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711214682.6A Active CN108050999B (en) | 2017-11-28 | 2017-11-28 | Innovative orthogonality infrared and geomagnetic composite rotating projectile attitude measurement method |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108050999B (en) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109373998A (en) * | 2018-10-11 | 2019-02-22 | 重庆天箭惯性科技股份有限公司 | Posture of rotator measurement method based on multi-sensor data |
| CN109538208A (en) * | 2018-12-21 | 2019-03-29 | 冀中能源峰峰集团有限公司 | A kind of compound positioning system of cutting head of roadheader and method |
| CN110672078A (en) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | High spin projectile attitude estimation method based on geomagnetic information |
| CN112362048A (en) * | 2020-11-12 | 2021-02-12 | 中国电子科技集团公司第三研究所 | Practical magnetic gradient tensor high-precision single-point positioning method |
| CN113418499A (en) * | 2021-05-13 | 2021-09-21 | 青岛杰瑞自动化有限公司 | Method and system for resolving roll angle of rotary aircraft |
| CN115900770A (en) * | 2023-02-14 | 2023-04-04 | 北京理工大学前沿技术研究院 | Online correction method and system for magnetic sensor in airborne environment |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS61161415A (en) * | 1985-01-11 | 1986-07-22 | Mitsubishi Electric Corp | Attitude detector of yaw axis of artificial satellite |
| CN103196445A (en) * | 2013-02-07 | 2013-07-10 | 哈尔滨工业大学 | Geomagnetism-assisted inertial carrier attitude measurement method based on matching technology |
| CN103712621A (en) * | 2013-12-23 | 2014-04-09 | 大连理工大学 | Method for determining attitude of polarized light and infrared sensor auxiliary inertial navigation system |
| CN106767789A (en) * | 2017-01-12 | 2017-05-31 | 南京航空航天大学 | A kind of pedestrian course Optimal Fusion based on adaptive Kalman filter |
| CN107314718A (en) * | 2017-05-31 | 2017-11-03 | 中北大学 | High speed rotating missile Attitude estimation method based on magnetic survey rolling angular rate information |
-
2017
- 2017-11-28 CN CN201711214682.6A patent/CN108050999B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JPS61161415A (en) * | 1985-01-11 | 1986-07-22 | Mitsubishi Electric Corp | Attitude detector of yaw axis of artificial satellite |
| CN103196445A (en) * | 2013-02-07 | 2013-07-10 | 哈尔滨工业大学 | Geomagnetism-assisted inertial carrier attitude measurement method based on matching technology |
| CN103712621A (en) * | 2013-12-23 | 2014-04-09 | 大连理工大学 | Method for determining attitude of polarized light and infrared sensor auxiliary inertial navigation system |
| CN106767789A (en) * | 2017-01-12 | 2017-05-31 | 南京航空航天大学 | A kind of pedestrian course Optimal Fusion based on adaptive Kalman filter |
| CN107314718A (en) * | 2017-05-31 | 2017-11-03 | 中北大学 | High speed rotating missile Attitude estimation method based on magnetic survey rolling angular rate information |
Non-Patent Citations (3)
| Title |
|---|
| JING YU 等: ""Spinning projectile’s attitude measurement using background magnetic field compensation"", 《JOURNAL OF APPLIED REMOTE SENSING》 * |
| 于靖 等: ""利用地球红外辐射的旋转飞行体姿态估计方法"", 《物理学报》 * |
| 朱占龙 等: ""基于新息正交性自适应滤波的惯性/地磁组合导航方法"", 《中国惯性技术学报》 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109373998A (en) * | 2018-10-11 | 2019-02-22 | 重庆天箭惯性科技股份有限公司 | Posture of rotator measurement method based on multi-sensor data |
| CN109538208A (en) * | 2018-12-21 | 2019-03-29 | 冀中能源峰峰集团有限公司 | A kind of compound positioning system of cutting head of roadheader and method |
| CN110672078A (en) * | 2019-10-12 | 2020-01-10 | 南京理工大学 | High spin projectile attitude estimation method based on geomagnetic information |
| CN110672078B (en) * | 2019-10-12 | 2021-07-06 | 南京理工大学 | High spin projectile attitude estimation method based on geomagnetic information |
| CN112362048A (en) * | 2020-11-12 | 2021-02-12 | 中国电子科技集团公司第三研究所 | Practical magnetic gradient tensor high-precision single-point positioning method |
| CN113418499A (en) * | 2021-05-13 | 2021-09-21 | 青岛杰瑞自动化有限公司 | Method and system for resolving roll angle of rotary aircraft |
| CN115900770A (en) * | 2023-02-14 | 2023-04-04 | 北京理工大学前沿技术研究院 | Online correction method and system for magnetic sensor in airborne environment |
| CN115900770B (en) * | 2023-02-14 | 2023-05-23 | 北京理工大学前沿技术研究院 | Online correction method and system for magnetic sensor in airborne environment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108050999B (en) | 2021-05-04 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108050999B (en) | Innovative orthogonality infrared and geomagnetic composite rotating projectile attitude measurement method | |
| CN109001787B (en) | Attitude angle resolving and positioning method and fusion sensor thereof | |
| US6779752B1 (en) | Projectile guidance with accelerometers and a GPS receiver | |
| US6573486B1 (en) | Projectile guidance with accelerometers and a GPS receiver | |
| US6398155B1 (en) | Method and system for determining the pointing direction of a body in flight | |
| CN109373998B (en) | Rotator attitude measurement method based on multi-sensor data | |
| JP2006515055A (en) | Projectile guidance using accelerometer and GPS receiver | |
| CN112611380B (en) | Attitude detection method based on multi-IMU fusion and attitude detection device thereof | |
| CN110672078B (en) | High spin projectile attitude estimation method based on geomagnetic information | |
| CN112985421B (en) | Spacecraft autonomous astronomical navigation method based on angle constraint auxiliary measurement | |
| CN106403934B (en) | A kind of missile-borne earth magnetism attitude measurement processing method | |
| CN105737793B (en) | Rolling angle measurement unit and measuring method | |
| CN114001602A (en) | Rocket gun disturbance detection method based on quaternion Kalman filtering denoising fusion | |
| Xu et al. | Dual-band infrared and geomagnetic fusion attitude estimation algorithm based on IMMEKF | |
| CN110017808A (en) | The method for resolving attitude of flight vehicle using Geomagnetism Information and accelerometer | |
| CN104748734B (en) | A kind of vehicle electronics height above sea level compass of compensation with angle | |
| CN115628714A (en) | Rotating projectile attitude measurement method based on uniaxial infrared focal plane array | |
| CN114578857A (en) | Guidance aircraft autonomous control method, device and system based on full trajectory information | |
| CN110954096B (en) | Method for measuring course attitude based on MEMS device | |
| CN110017809A (en) | The method for resolving attitude of flight vehicle using Geomagnetism Information and light stream sensor | |
| Zhu et al. | Attitude measurement method of geomagnetic/gyro combination based on difference-quotient | |
| CN115752119B (en) | Ballistic correction fuze missile-borne computer semi-physical simulation system and method | |
| CN114858004B (en) | Explosion control method for anti-unmanned aerial vehicle based on weak magnetic detection technology | |
| Zhang et al. | Attitude Measurement of Special Aircraft Based on Geomagnetic and Angular Velocity Sensors | |
| Pingan et al. | Research Article Attitude Measurement of Special Aircraft Based on Geomagnetic and Angular Velocity Sensors |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |