Method and system for measuring flight parameters of rotating projectile based on prior knowledge of trajectory

Abstract

The present invention discloses a method and system for filtering the flight parameters of a rotating missile based on prior knowledge of trajectory, which selects the system state equation with the rotating missile attitude quaternion q as the filter state variable and solves the system quaternion q by magnetic measurement. MR The system observation equation as the filter observation variable is solved by the missile body attitude quaternion matrix #imgabs0# to solve the magnetic measurement system quaternion q MR ; Then construct the missile attitude filtering model, use the Kalman filter algorithm to complete the optimal filtering estimation of the state equation X(k) #imgabs1#, calculate the optimal estimation of the rotating missile attitude quaternion #imgabs2# and substitute it into the missile attitude quaternion matrix #imgabs3#, calculate the optimal missile attitude quaternion matrix #imgabs4# and calculate the missile velocity V n and the missile flight position P n , completing the real-time parameter measurement of the rotating missile flight, thereby effectively improving the accuracy and reliability of testing the rotating missile status.

Description

Technical Field

[0001] The present invention relates to the technical field of methods for measuring the flight attitude, speed and position of an aircraft or a rotating missile, and in particular to a method and system for measuring the flight parameters of a rotating missile based on prior knowledge of trajectory. Background Art

[0002] Due to the special application environment of high overload, high self-rotation and high speed of rotating missiles, the existing mature missile-borne measurement system cannot be directly transplanted and applied to the test application of rotating missiles, and there are problems such as difficult survival, incomplete measurement parameters, low accuracy and poor reliability. Therefore, the high-precision attitude measurement technology of high-speed rotating missiles is the difficulty of its guided transformation. It is urgent to seek a low-cost, high-precision technology suitable for rotating missile flight attitude measurement, which has important theoretical value and practical significance for solving the problem of aerial flight attitude measurement in the guided transformation of rotating missiles. Summary of the invention

[0003] In view of the above problems, the present invention provides a method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory, comprising the following steps:

[0004] S1 selects the system state equation with the rotating missile attitude quaternion q as the filter state variable and the system quaternion q calculated by magnetic measurement MR The system observation equation as the filter observation variable is obtained by the missile body attitude quaternion matrix Solve the magnetic measurement solution system quaternion q MR ;

[0005] S2: Construct the missile attitude filtering model and use the Kalman filter algorithm to complete the optimal filtering estimation of the state equation X(k) The optimal estimate of the quaternion of the rotating missile attitude is calculated And substitute into the missile body attitude quaternion matrix The optimal missile body attitude quaternion matrix is ​​calculated

[0006] S3: Calculate the projectile velocity V n and the missile flight position P n ;

[0007] S4: Complete the real-time parameter measurement of the rotating missile flight.

[0008] The relationship between the geomagnetic sensor measurement output and the geomagnetic vector is defined as:

[0009]

[0010] According to the prior knowledge of trajectory, Equal to the firing angle α, calculate the pitch angle θ of the high-speed rotating projectile mand the roll angle γ m , the formula is:

[0011]

[0012] Calculate the missile body attitude transformation matrix Simplified to:

[0013]

[0014] In the above formula, It is the geomagnetic field vector information measured and output by the three-axis geomagnetic sensor. are the three components of the geomagnetic field in the navigation coordinate system; is the missile body attitude transformation matrix; θ, γ represent the yaw angle, pitch angle and roll angle of the projectile respectively.

[0015] Among them, the magnetic measurement and solution system quaternion q MR , specifically:

[0016] Let q be the attitude quaternion of the missile body coordinate system relative to the navigation coordinate system, then the missile body attitude quaternion matrix It can be expressed as:

[0017]

[0018] Then the quaternion of the magnetic measurement system is The solution formula is:

[0019]

[0020] In the above formula The symbol is defined as The sign of is determined by:

[0021]

[0022] In the above formula, q = q 0 +q 1 i+q 2 j+q 3 k, q 0 ,q 1 ,q 2 ,q 3 is a real number, i, j, k are mutually orthogonal imaginary units, expressed as q = [q 0 ,q 1 ,q 2 ,q 3 ] T ; and Both represent the missile body attitude transformation matrix,

[0023] The system state equation includes:

[0024] Select the attitude quaternion q as the filter state variable X(t)=q=[q 0 ,q 1 ,q 2 ,q 3 ] T , the state equation of the system is:

[0025]

[0026]

[0027] In the above formula, w(t) is the system process noise, its mean is E[w(t)]=0, and its variance is E[w(t),w T (τ)] = Q(t), Q(t) is the variance matrix of the system noise sequence, ω i (i=x,y,z) represents the angular velocity component of the rotational projectile.

[0028] The system observation equation is: Select the magnetic measurement to solve the system quaternion q MR , as the observed variable Z(t) of the filter, the system observation measurement equation is:

[0029]

[0030] In the above formula, v(t) is the measurement noise of the system, its mean is E[v(t)]=0, and its variance is E[v(t),v T (τ)] = R(t), where R(t) is the variance matrix of the measurement noise sequence.

[0031] The projectile posture filtering model is constructed as follows:

[0032] The system state equation and system observation measurement equation are simplified and discretized to obtain:

[0033]

[0034] In the above formula, is the identity matrix, Φ k,k-1 is the state transfer matrix calculation formula, X(k-1) is the next state variable, w(k) is the system process noise, and v(k) is the system measurement noise.

[0035] Furthermore, the Kalman filter algorithm is used to complete the optimal filtering estimation of the state equation X(k) It includes: time update process and measurement update process; among which,

[0036] Time update process, the formula is:

[0037]

[0038] The measurement update process is as follows:

[0039]

[0040] The optimal estimate of the quaternion of the rotating missile attitude is calculated And substitute into the missile body attitude quaternion matrix The optimal missile body attitude quaternion matrix is ​​calculated As the rotating projectile attitude parameter A;

[0041] Among them, in the above formula, is the state one-step prediction; P (k,k-1) is the one-step prediction mean square error; K k represents the filter gain; R(k) is the measurement noise matrix; Q(k-1) is the system noise variance matrix at time k-1; I is the unit matrix; P k is the estimated mean square error.

[0042] Furthermore, it is characterized in that said S3 further includes:

[0043] Calculate the missile body velocity Vn and the missile body flight position Pn, the formula is:

[0044]

[0045] In the above formula, the missile body attitude quaternion matrix is the output component measured by the missile-borne triaxial accelerometer, g n is the gravity component in the navigation coordinate system; V n =[v x ,v y ,v z ] T is the velocity component of the projectile;

[0046] P n =[x,y,z] T is the flight position component of the rotating projectile.

[0047] As another preferred embodiment, the present invention also provides a system for measuring the flight parameters of a rotating projectile based on prior knowledge of trajectory, comprising at least: a signal acquisition module and a signal processing module, wherein:

[0048] The signal acquisition module includes a three-axis geomagnetic sensor, a three-axis MEMS gyroscope and a three-axis accelerometer which are sequentially connected to the signal conditioning module for communication; the signal conditioning module sends the acquired data to the ADC data acquisition module to complete the data acquisition;

[0049] The signal processing module includes an FPGA configuration module, a DSP digital signal processor and a FLASH which are sequentially connected to the FPGA unit for communication; a computer program is stored in the FLASH, and when the computer program is called and executed by the DSP digital signal processor, it is used to implement the method for filtering the flight parameters of a rotating missile based on a priori knowledge of the trajectory as claimed in any one of claims 1 to 8;

[0050] Also includes:

[0051] The missile-borne flight control computer connected to the DSP digital signal processor controls the flight of the rotating missile according to the rotating missile attitude parameter A, the missile body speed Vn and the missile body flight position Pn;

[0052] The host computer connected to the FPGA unit is used to read the data collected by the signal acquisition module and read the data in the FLASH.

[0053] The three-axis geomagnetic sensor, the three-axis MEMS gyroscope and the three-axis accelerometer are respectively installed at the Xb axis, the Zb axis and the axis center 0 point position in the high-speed rotating missile carrier coordinate system.

[0054] In summary, the present invention discloses a method and system for filtering the flight parameters of a rotating missile based on prior knowledge of trajectory, by selecting the system state equation with the rotating missile attitude quaternion q as the filter state variable and solving the system quaternion q by magnetic measurement. MR The system observation equation as the filter observation variable is obtained by the missile body attitude quaternion matrix Solve the magnetic measurement solution system quaternion q MR ; Then, the missile attitude filtering model is constructed, and the Kalman filtering algorithm is used to complete the optimal filtering estimation of the state equation X(k) The optimal estimate of the quaternion of the rotating missile attitude is calculated And substitute into the missile body attitude quaternion matrix The optimal missile body attitude quaternion matrix is ​​calculated And calculate the projectile velocity V n and the missile flight position P n , completing the real-time parameter measurement of the rotating missile flight, thereby effectively improving the accuracy and reliability of testing the rotating missile status. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 Schematic diagram of a system for measuring flight parameters of a rotating projectile based on prior knowledge of trajectory according to an embodiment.

[0056] Figure 2 A schematic diagram of the installation and coordinate system of a rotating missile navigation sensor according to an embodiment.

[0057] Figure 3 The schematic diagram of a method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory according to an embodiment of the present invention is shown. DETAILED DESCRIPTION

[0058] The method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory of the present invention will be further described in detail below in conjunction with specific embodiments and drawings.

[0059] like Figure 1 As shown, the rotating projectile flight parameter measurement system based on trajectory prior knowledge of the present invention at least includes: a signal acquisition module and a signal processing module, wherein:

[0060] The signal acquisition module includes a three-axis geomagnetic sensor, a three-axis MEMS gyroscope and a three-axis accelerometer which are sequentially connected to the signal conditioning module for communication; the signal conditioning module sends the acquired data to the ADC data acquisition module to complete the data acquisition;

[0061] The signal processing module includes an FPGA configuration module, a DSP digital signal processor and a FLASH which are sequentially connected to the FPGA unit for communication; a computer program is stored in the FLASH, and when the computer program is called and executed by the DSP digital signal processor, it is used to implement the method for filtering the flight parameters of a rotating missile based on a priori knowledge of the trajectory as claimed in any one of claims 1 to 8;

[0062] Also includes:

[0063] The missile-borne flight control computer connected to the DSP digital signal processor determines the missile body speed V according to the rotating missile attitude parameters. n and the missile flight position P n Control the flight of the spinning projectile.

[0064] The host computer connected to the FPGA unit is used to read the data collected by the signal acquisition module and read the data in the FLASH.

[0065] like Figure 2 As shown, the three-axis geomagnetic sensor, the three-axis MEMS gyroscope and the three-axis accelerometer are respectively installed in the X b Axis, Z b Axis and axis center 0 point position.

[0066] like Figure 3 As shown, the method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory of the present invention comprises the following steps:

[0067] S1 selects the system state equation with the rotating missile attitude quaternion q as the filter state variable and the system quaternion q calculated by magnetic measurementMR The system observation equation as the filter observation variable is obtained by the missile body attitude quaternion matrix Solve the magnetic measurement solution system quaternion q MR ;

[0068] S2: Construct the missile attitude filtering model and use the Kalman filter algorithm to complete the optimal filtering estimation of the state equation X(k) The optimal estimate of the quaternion of the rotating missile attitude is calculated And substitute into the missile body attitude quaternion matrix The optimal missile body attitude quaternion matrix is ​​calculated

[0069] S3: Calculate the projectile velocity V n and the missile flight position P n ;

[0070] S4: Complete the real-time parameter measurement of the rotating missile flight.

[0071] The relationship between the geomagnetic sensor measurement output and the geomagnetic vector is defined as:

[0072]

[0073] In the above formula (1), It is the measurement output of the three-axis geomagnetic sensor. are the three components of the geomagnetic field in the navigation coordinate system; is the missile attitude transformation matrix, abbreviated as θ, γ represent the yaw angle, pitch angle and roll angle of the projectile respectively.

[0074] According to the prior knowledge of trajectory, the rotating bullet relies on high-speed rotation to achieve stable flight. According to the prior knowledge of trajectory, it can be known that its change in the launch navigation coordinate system is not large. The yaw angle can be considered as a known angle ( Equal to the incident angle α), to simplify the calculation usually Treat it as zero degree. Therefore, let Equal to the firing angle α, calculate the pitch angle θ of the high-speed rotating projectile m and the roll angle γ m , the formula is:

[0075]

[0076] Calculate the missile body attitude transformation matrix Simplified to:

[0077]

[0078] If q is used to represent the attitude quaternion of the missile body coordinate system relative to the navigation coordinate system, the missile body attitude quaternion matrix can be obtained from the strapdown inertial navigation: It can be expressed as:

[0079]

[0080] In the above formula, q = q 0 +q 1 i+q 2 j+q 3 k, q 0 ,q 1 ,q 2 ,q 3 is a real number, i, j, k are mutually orthogonal imaginary units, which can also be expressed as q = [q 0 ,q 1 ,q 2 ,q 3 ] T . and Both represent the missile body attitude transformation matrix, From this, we can deduce the quaternion of the rotating projectile attitude The solution formula is:

[0081]

[0082] In the above formula (5) symbol Optional, The sign of is determined by the following formula (6):

[0083]

[0084] Among them, the system state equation using the rotating missile attitude quaternion q as the filter state variable is selected, and the system state equation includes:

[0085] Select the attitude quaternion q as the filter state variable X(t)=q=[q 0 ,q 1 ,q 2 ,q 3 ] T , if ω i (i=x,y,z) is expressed as the angular velocity component of the rotating missile. According to the strapdown inertial navigation theory, the missile attitude equation described by quaternion is:

[0086]

[0087] The filter state variable X(t) of the present invention = q = [q 0 ,q 1 ,q 2 ,q3 ] T , the corresponding system state equation is:

[0088]

[0089] In the above formula (8), w(t) is the system process noise, which is assumed to be zero-mean white noise, with a mean of E[w(t)] = 0 and a variance of E[w(t),w T (τ)] = Q(t), Q(t) is the variance matrix of the system noise sequence, ω i (i=x,y,z) represents the angular velocity component of the rotational projectile.

[0090] Select the quaternion q of the magnetic measurement solution system MR , as the observed variable Z(t) of the filter, the system observation measurement equation is:

[0091]

[0092] In the above formula (9), v(t) is the measurement noise of the system, which is assumed to be zero-mean white noise, with a mean of E[v(t)] = 0 and a variance of E[v(t),v T (τ)] = R(t), where R(t) is the variance matrix of the measurement noise sequence.

[0093] Therefore, the missile attitude filtering model of the system is composed of the state equation (8) and the observation equation (9), and the Kalman filtering method is used to achieve the optimal estimation of the missile attitude quaternion q:

[0094] Simplify the rotating missile attitude filter equations (8) and (9) into a general form:

[0095]

[0096] The system state equation and system observation measurement equation are simplified and discretized to obtain:

[0097]

[0098] In the above formula (11), is the identity matrix, Φ k,k-1 The state transfer matrix calculation formula is:

[0099]

[0100] In the above formula (12), is the angular velocity component of the rotating projectile; X(k-1) is the next state variable, w(k) is the system process noise, and v(k) is the system measurement noise.

[0101] The present invention adopts the discrete Kalman filter algorithm to complete the optimal filtering estimation of the state variable X(k) The filtering algorithm includes a time update process and a measurement update process, where:

[0102] (1) Time update process:

[0103]

[0104] (2) Measurement update process:

[0105]

[0106] The optimal estimate of the quaternion of the rotating missile attitude is calculated Substitute the missile attitude quaternion matrix into formula (4): The optimal missile body attitude quaternion matrix is ​​calculated As the rotating projectile attitude parameter A;

[0107] In the above formula, is the state one-step prediction; P (k,k-1) is the one-step prediction mean square error; K k represents the filter gain; R(k) is the measurement noise matrix; Q(k-1) is the system noise variance matrix at time k-1; I is the unit matrix; P k is the estimated mean square error.

[0108] The S3 further includes:

[0109] Calculate the projectile velocity V n and the missile flight position P n , the formula is:

[0110]

[0111] In the above formula, the missile body attitude quaternion matrix is the output component measured by the missile-borne triaxial accelerometer, g n is the gravity component in the navigation coordinate system; V n =[v x ,v y ,v z ] T is the velocity component of the projectile; P n =[x,y,z] T is the flight position component of the rotating projectile.

[0112] Although the present invention is described in conjunction with the above specific embodiments, it is obvious that those skilled in the art can make many substitutions, modifications and changes according to the above content. Therefore, all such substitutions, improvements and changes are included in the spirit and scope of the appended claims.

Claims

1. A method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory, characterized in that: include: S1 selects the system state equation with the rotating missile attitude quaternion q as the filter state variable and the system quaternion q calculated by magnetic measurement MR The system observation equation as the filter observation variable is obtained by the missile body attitude quaternion matrix Solve the magnetic measurement solution system quaternion q MR ; S2: Construct the missile attitude filtering model and use the Kalman filter algorithm to complete the optimal filtering estimation of the state equation X(k) The optimal estimate of the quaternion of the rotating missile attitude is calculated And substitute into the missile body attitude quaternion matrix The optimal missile body attitude quaternion matrix is ​​calculated S3: Calculate the projectile velocity V n and the missile flight position P n ; S4: Complete the real-time parameter measurement of the rotating missile flight; It also includes: defining the relationship between the geomagnetic sensor measurement output and the geomagnetic vector as: According to the prior knowledge of trajectory, Equal to the firing angle α, calculate the pitch angle θ of the high-speed rotating projectile m and the roll angle γ m , the formula is: Calculate the missile body attitude transformation matrix Simplified to: In the above formula, It is the geomagnetic field vector information measured and output by the three-axis geomagnetic sensor. are the three components of the geomagnetic field in the navigation coordinate system; is the missile body attitude transformation matrix; θ, γ represent the yaw angle, pitch angle and roll angle of the projectile respectively.

2. A method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory according to claim 1, characterized in that: The magnetic measurement solves the system quaternion q MR , specifically: Let q be the attitude quaternion of the missile body coordinate system relative to the navigation coordinate system, then the missile body attitude quaternion matrix It can be expressed as: Then the quaternion of the magnetic measurement system is The solution formula is: In the above formula The symbol is defined as The sign of is determined by: In the above formula, q=q0+q1i+q2j+q3k, q0,q1,q2,q3 are real numbers, i,j,k are mutually orthogonal imaginary units, expressed as q=[q0,q1,q2,q3] T ; and Both represent the missile body attitude transformation matrix, 3. The method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory according to claim 1, characterized in that: The system state equation includes: Select the attitude quaternion q as the filter state variable X(t)=q=[q0,q1,q2,q3] T , the state equation of the system is: In the above formula, w(t) is the system process noise, its mean is E[w(t)]=0, and its variance is E[w(t),w T (τ)] = Q(t), Q(t) is the variance matrix of the system noise sequence, ω i (i=x,y,z) represents the angular velocity component of the rotational projectile.

4. The method for filtering the flight parameters of a rotating projectile based on trajectory prior knowledge according to claim 1, characterized in that: The system observation equation is: Select the magnetic measurement to solve the system quaternion q MR , as the observed variable Z(t) of the filter, the system observation measurement equation is: In the above formula, v(t) is the measurement noise of the system, its mean is E[v(t)]=0, and its variance is E[v(t),v T (τ)] = R(t), where R(t) is the variance matrix of the measurement noise sequence.

5. The method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory according to claim 1, characterized in that: The projectile posture filtering model is constructed as follows: The system state equation and system observation measurement equation are simplified and discretized to obtain: In the above formula, is the identity matrix, Φ k,k-1 is the state transfer matrix calculation formula, X(k-1) is the next state variable, w(k) is the system process noise, and v(k) is the system measurement noise.

6. The method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory according to claim 1, characterized in that: The Kalman filter algorithm is used to complete the optimal filtering estimation of the state equation X(k) It includes: time update process and measurement update process; among which, Time update process, the formula is: The measurement update process is as follows: The optimal estimate of the quaternion of the rotating missile attitude is calculated And substitute into the missile body attitude quaternion matrix The optimal missile body attitude quaternion matrix is ​​calculated As the rotating projectile attitude parameter A; In the above formula, is the state one-step prediction; P (k,k-1) is the one-step prediction mean square error; K k represents the filter gain; R(k) is the measurement noise matrix, Q(k-1) is the system noise variance matrix at time k-1; I is the unit matrix; P k is the estimated mean square error.

7. The method for filtering the flight parameters of a rotating projectile based on prior knowledge of trajectory according to claim 1, characterized in that: The S3 further includes: Calculate the projectile velocity V n and the missile flight position P n , the formula is: In the above formula, the missile body attitude quaternion matrix is the output component measured by the missile-borne triaxial accelerometer, g n is the gravity component in the navigation coordinate system; V n =[v x ,v y ,v z ] T is the velocity component of the projectile; P n =[x,y,z] T is the flight position component of the rotating projectile.

8. A system for measuring flight parameters of a rotating projectile based on prior knowledge of trajectory, characterized in that: It includes: a signal acquisition module and a signal processing module, wherein: The signal acquisition module includes a three-axis geomagnetic sensor, a three-axis MEMS gyroscope and a three-axis accelerometer which are sequentially connected to the signal conditioning module for communication; the signal conditioning module sends the acquired data to the ADC data acquisition module to complete the data acquisition; The signal processing module includes an FPGA configuration module, a DSP digital signal processor and a FLASH which are sequentially connected to the FPGA unit for communication; a computer program is stored in the FLASH, and when the computer program is called and executed by the DSP digital signal processor, it is used to implement the method for filtering the flight parameters of a rotating missile based on a priori knowledge of the trajectory as claimed in any one of claims 1 to 7; Also includes: The missile-borne flight control computer connected to the DSP digital signal processor controls the missile according to the rotating missile attitude parameter A, the missile body speed V n and the missile flight position P n Control the flight of the spinning projectile; The host computer connected to the FPGA unit is used to read the data collected by the signal acquisition module and read the data in the FLASH.

9. A system for measuring flight parameters of a rotating projectile based on prior knowledge of trajectory according to claim 8, characterized in that: The three-axis geomagnetic sensor, the three-axis MEMS gyroscope and the three-axis accelerometer are respectively installed at the Xb axis, the Zb axis and the axis center 0 point position in the high-speed rotating missile carrier coordinate system.

Images