Guidance cartridge model / inertial autonomous navigation method and system

Abstract

The invention provides a guided projectile model / inertial autonomous navigation method and system, which are suitable for guided projectile whole-process autonomous navigation positioning, the framework mainly comprises two parts of filtering model construction and filtering method design, a system state equation is constructed according to a trajectory dynamics model, and the system state equation is calculated based on a parameter coupling relationship between an inertial device and a trajectory model. Constructing a measurement equation by taking the output angular rate of the gyroscope and the output specific force of the accelerometer as observed quantities; on the basis of filter innovation information and historical data, an innovation UKF based on data driving is designed, a measurement variance matrix can be adjusted in real time, the model / inertial parameter fusion precision is improved, and high-precision guided projectile autonomous navigation is achieved. The method is suitable for guided projectile navigation in a high-dynamic environment.

Description

Technical Field

[0001] The invention belongs to the field of guided projectile navigation in a high dynamic environment, and relates to a guided projectile model / inertial autonomous navigation method and system. Background Art

[0002] The important position of guided artillery shells in national security and the development of weapons and equipment is becoming increasingly prominent. How to obtain navigation information with high precision is the key technology for guided artillery shells to achieve precise strikes on targets, and navigation accuracy directly affects the accuracy of artillery shell guidance in hitting targets. Since GNSS is a weak signal of wireless communication, in natural environments, satellite signals are easily interfered by complex environments such as dark clouds, lightning, and solar storms; in actual battlefield environments, the security of satellite constellations is threatened by anti-satellite missiles, laser weapons, and electromagnetic shocks; satellite receivers are easily subject to electronic interference or deception and cannot be accurately positioned. Therefore, in view of satellite denial conditions, the research and development of guided artillery shells with inertial guidance as the core has become an urgent need for the battlefield environment in the new era. It is of great significance to carry out research on inertial autonomous navigation methods for guided artillery shells in high dynamic environments.

[0003] Under satellite denial conditions, the whole process of autonomous navigation of guided artillery shells mainly focuses on the research of navigation methods based on the fusion of information from various multi-source sensors. The Army Research Laboratory of a certain country proposed a multi-source fusion scheme combining accelerometers, gyroscopes and magnetometers to achieve the calculation and motion navigation of projectile attitude. In 2015, a certain country launched a precision robust inertial guided munitions project to develop advanced inertial microsensors in order to provide stable navigation performance under extreme conditions. Alicia Roux and others from the Franco-German Institute of Saint-Louis in France used EKF and incomplete right-invariant EKF to effectively estimate the trajectory of artillery shells. This method focuses on the research of filtering estimation algorithms, and less analysis of the characteristics of the artillery shells themselves and their environment. Recently, Alicia Roux et al. proposed a deep Kalman filtering method, which only uses the reference magnetic field and an inertial measurement unit composed of an accelerometer, a gyroscope and a magnetometer to estimate the trajectory of artillery shells in a satellite denial environment and achieved good results. This method focuses on the application of deep learning in the field of Kalman filtering, lacks correlation with the actual projectile motion characteristics and environmental characteristics, and does not meet the problem of inertial navigation accuracy in complex disturbance environments. Summary of the invention

[0004] In view of the problem of insufficient inertial navigation accuracy in complex disturbance environments, the present invention proposes a guided projectile model / inertial autonomous navigation method and system to improve navigation autonomy.

[0005] The technical solution to achieve the purpose of the present invention is:

[0006] A guided artillery shell model / inertial autonomous navigation method, comprising:

[0007] Step 1, constructing the system state equation according to the six-degree-of-freedom rigid body trajectory model of the guided projectile;

[0008] Step 2: Analyze the coupling relationship between the trajectory parameters and the inertial device measurement information based on the system state equation, and construct the system measurement equation;

[0009] Step 3, constructing a guided projectile model / inertial autonomous navigation filter model according to the system state equation and the system measurement equation;

[0010] Step 4: Based on the filter innovation information and historical data, a data-driven innovation UKF is designed to estimate the guided projectile model / inertial autonomous navigation filter model to achieve autonomous navigation of the guided projectile.

[0011] Furthermore, the six-degree-of-freedom rigid body trajectory model of the guided projectile in step 1 is:

[0012]

[0013] In the formula, v, θ a , ψ2, ω ξ ,ω η ,ω ξ , γ, x, y, z represent the velocity of the projectile's center of mass, the velocity elevation angle, the velocity direction angle, the three-axis angular velocity of rotation around the center of mass, the projectile axis elevation angle, the projectile axis azimuth angle, the projectile body roll angle, and the projectile's center of mass's three-axis position, respectively; M ξ 、M η 、M ζ It represents the three-axis components of the external torque in the elastic axis coordinate system, represents the three-axis components of the external force in the ballistic coordinate system, m, C, and A represent the mass of the projectile, the polar moment of inertia, and the equatorial moment of inertia, respectively;

[0014] The system state equation is:

[0015]

[0016] In the formula,

[0017]

[0018] Furthermore, in step 2, the system measurement equation is constructed using the gyroscope output angular velocity and the accelerometer output specific force as observation quantities.

[0019] Furthermore, the gyroscope output angular rate is:

[0020]

[0021] In the formula, h G(*) represents the nonlinear equation of the relationship between the trajectory parameters and the gyroscope output angular rate, X(i), i = 1, 2, ..., 12 represents the i-th element in X, G b Output angular rate for the gyroscope;

[0022] The accelerometer output specific force is:

[0023]

[0024] In the formula, h f (*) represents the nonlinear equation of the relationship between the trajectory parameters and the specific force output by the accelerometer, a=[a z a x a y ] T , is the triaxial acceleration, is the east, north and celestial velocity, g n is the local gravitational acceleration, the state matrix

[0025]

[0026] Furthermore, the system measurement equation is:

[0027]

[0028] Among them, G b is the angular rate output by the gyroscope; f b is the accelerometer output specific force, h G (*) represents the nonlinear equation of the relationship between the trajectory parameters and the angular rate output by the gyroscope, X(i), i=1,2,...,12 represents the i-th element in X, h f (*) represents the nonlinear equation that represents the relationship between the trajectory parameters and the specific force output by the accelerometer.

[0029] Furthermore, the step 4 specifically includes:

[0030] First, initialize the state estimate And the state error covariance matrix P0, through UT transformation, calculate the sigma point;

[0031] The 4RK method is used to update the system state and state error covariance matrix in time, and the UT transformation is performed again to generate a new sigma point set;

[0032] Based on the guided missile model / inertial autonomous navigation filter model, measurement updates are performed to calculate new information;

[0033] Based on the new information, evaluate the deviation between the system measurement prediction and the new measurement information and determine the degree of deviation;

[0034] Based on the deviation degree, the mean of the angular velocity and specific force deviation degree is calculated, and a data-driven adaptive measurement mean square error matrix is ​​designed;

[0035] Calculate the Kalman gain matrix and update the system state and covariance to achieve autonomous navigation of the guided projectile.

[0036] Furthermore, the 4RK method is used to update the system status in time as follows:

[0037]

[0038]

[0039] In the formula, the weight coefficient is calculated as follows:

[0040]

[0041] Where i = 1, 2, ..., 2n, coefficient λ′ = α 2 (n+κ)-n, the coefficient α ranges from [10 -4 ,1], β and κ are setting coefficients.

[0042] Furthermore, the degree of deviation is:

[0043]

[0044] Where, T G It is a measure of the degree of deviation of the angular rate measured by the gyroscope, T f It is a measure of the deviation of the accelerometer in measuring specific force, e k (1~3) represents the first three elements of the new information, e k (4~6) represent the last 3 elements of the new information.

[0045] Furthermore, the adaptive measurement mean square error matrix is:

[0046]

[0047] In the formula, Represents the elements on the diagonal of the measurement mean square error matrix, α 1s ,α 1w ,α 2s ,α 2w Indicates the coefficient of strong or weak deviation.

[0048] A guided artillery shell model / inertial autonomous navigation system, comprising:

[0049] A system state equation building unit, used to build the system state equation according to a six-degree-of-freedom rigid body trajectory model of the guided projectile;

[0050] System measurement equation construction unit, which analyzes the coupling relationship between trajectory parameters and inertial device measurement information based on the system state equation and constructs the system measurement equation;

[0051] A filter model building unit, which builds a guided projectile model / inertial autonomous navigation filter model according to the system state equation and the system measurement equation;

[0052] The navigation control unit designs a data-driven innovation UKF based on the filter innovation information and historical data, estimates the guided projectile model / inertial autonomous navigation filter model, and realizes autonomous navigation of the guided projectile.

[0053] Compared with the prior art, the present invention has the following beneficial effects:

[0054] The invention comprises two parts, namely, filter model construction and filter method design. The system state equation is constructed according to the ballistic dynamics model. Based on the coupling relationship between the inertial device and the ballistic model parameters, the measurement equation is constructed with the gyroscope output angular rate and the accelerometer output specific force as the observed quantities. Based on the filter innovation information and historical data, a data-driven innovation UKF is designed, which can adjust the measurement variance matrix in real time, improve the model / inertial parameter fusion accuracy, and realize high-precision guided projectile autonomous navigation. The method of the invention is suitable for guided projectile navigation in a high dynamic environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 This is the guided projectile model / inertial autonomous navigation framework diagram proposed by the present invention.

[0056] Figure 2 Construct a mind map of the measurement equations for systems based on inertial measurements.

[0057] Figure 3 Schematic diagram of the mean square error matrix of data-driven adaptive measurement. DETAILED DESCRIPTION

[0058] Next, the implementation of the present invention will be described in detail with reference to the accompanying drawings.

[0059] Combination Figure 1 This embodiment provides a guided projectile model / inertial autonomous navigation method, including:

[0060] Combination Figure 2 The present invention constructs a system measurement equation based on inertial measurement. According to the angular velocity, attitude information and velocity of the projectile output by the high-precision six-degree-of-freedom trajectory model, the angular velocity and specific force information based on the inertial coordinate system are inverted through the coordinate transformation relationship, and the coupling relationship between the inertial measurement and the trajectory model parameters is constructed to realize the establishment of the measurement equation.

[0061] The established six-degree-of-freedom rigid-body ballistic model of the guided projectile is shown below.

[0062]

[0063] In the formula, They represent respectively the velocity of the center of mass of the projectile, the velocity elevation angle, the velocity direction angle, the three-axis angular velocity of rotation around the center of mass, the elevation angle of the projectile axis, the azimuth angle of the projectile axis, the roll angle of the projectile body, and the three-axis position of the center of mass of the projectile. [M ξ M η M ζ ] represents the three-axis components of the external torque in the elastic axis coordinate system. Represents the three-axis components of the external force in the ballistic coordinate system. [m CA] represents the mass of the projectile, the polar moment of inertia, and the equatorial moment of inertia.

[0064] The coordinate system transformation relationship coupled with equation (1) is shown below.

[0065]

[0066] Where [δ2 δ1 β] represents the directional attack angle, the high and low attack angles, and the rotation angle from the second missile axis coordinate system to the missile axis coordinate system, respectively.

[0067] make Formula (1) can be written as:

[0068]

[0069] In the formula,

[0070] According to formula (3), X is used as the system state variable and formula (3) is used as the system state equation, where the system state update process is completed using the 4th-order Runge-Kutta method.

[0071] According to the guided projectile trajectory model, the relationship between its trajectory parameters and the inertial device measurement information is constructed, so as to obtain the system measurement equation based on inertial measurement. The idea is as shown in the attached figure. Figure 1 shown.

[0072] According to formula (1), the angular velocity of the projectile measured by the gyroscope can be inverted to obtain

[0073]

[0074] In the formula, h G (*) represents the nonlinear equation of the relationship between the trajectory parameters and the angular rate output by the gyroscope, and X(i), i=1,2,...,12 represents the i-th element in X. b Output angular rate for the gyroscope.

[0075] According to formula (1), taking the derivative of the last three formulas, we can get

[0076]

[0077] According to the velocity differential equation of SINS:

[0078]

[0079] In the formula, a=[a z a x a y ] T , is the triaxial acceleration; are speeds, representing the east, north and celestial speeds respectively; R e is the average radius of the Earth, ω ie is the Earth's rotation angular velocity; [L λ h] T is the location, representing latitude, longitude and altitude respectively. b is the triaxial specific force measured by the accelerometer, g n is the local gravitational acceleration.

[0080]

[0081] Combining equations (1), (5) and (6), the accelerometer output specific force can be inverted to obtain

[0082]

[0083] In the formula, h f (*) represents the nonlinear equation that represents the relationship between the trajectory parameters and the specific force output by the accelerometer.

[0084] Combining equations (4) and (7), we can obtain the inertia-based system measurement equation:

[0085]

[0086] In the formula, G b is the angular rate output by the gyroscope; f b is the specific force output by the accelerometer.

[0087] See attached Figure 3 The present invention designs a data-driven adaptive adjustment method for the measurement variance matrix, defines the degree of deviation according to the new information, divides the degree of deviation into strong deviation and weak deviation based on the data distribution characteristics of the degree of deviation of historical data statistics, realizes real-time adjustment of the measurement variance matrix, and improves the estimation accuracy of the model / inertial filter fusion.

[0088] Combining equations (3) and (8), we can get the guided projectile model / inertial autonomous navigation filter model:

[0089]

[0090] In the formula,

[0091] For the filtering model (9), UKF is usually used for state estimation. The setting of the measurement covariance matrix is ​​directly related to the estimation accuracy. According to the observation equation, there are strong nonlinear characteristics. Therefore, based on historical data, a data-driven innovation UKF (DIUKF) is proposed.

[0092] First, initialize the state estimate and the state error covariance matrix P0.

[0093]

[0094] UT transformation, calculate sigma point.

[0095]

[0096] In the formula, the value range of α is [10 -4 ,1]; is the state error covariance matrix The i-th column of , i = 1, 2, …, n, is obtained by Cholesky decomposition.

[0097] According to formula (3), the 4RK method is used to update the time.

[0098]

[0099]

[0100] In the formula, Q k is the system variance matrix, and the weight coefficient is calculated as follows

[0101]

[0102] In the formula, i=1,2,…,2n. λ′=α 2 (n+κ)-n, β and κ are coefficients, generally β=2.

[0103] According to equations (12) and (13), UT transformation is performed again to generate a new sigma point set.

[0104]

[0105] According to equations (4), (7), (8) and (15), the measurement is updated.

[0106]

[0107] Calculate the new information based on equation (16) and the inertial device output.

[0108]

[0109] In the formula, Z k =[G b,k f b,k ] T , is the angular rate and specific force information output by the inertial device at time k.

[0110] Based on the new information, evaluate the deviation between the system measurement prediction and the measurement new information, and define the degree of deviation:

[0111]

[0112] Where, T G It is a measure of the degree of deviation of the angular rate measured by the gyroscope, T f It is a measure of the degree of deviation in the accelerometer's measurement of specific force. k (1~3) represents the first three elements of the new information, e k (4~6) represent the last 3 elements of the new information.

[0113] According to formula (18), the mean values ​​of angular velocity and specific force deviation are calculated respectively.

[0114]

[0115] Record the degree of deviation at every moment and design a data-driven adaptive measurement mean square error matrix. The idea is as follows Figure 2 shown.

[0116] Combining formula (19) and Figure 2 , design a data-driven adaptive measurement mean square error matrix.

[0117]

[0118] In the formula, Represents the elements on the diagonal of the measurement mean square error matrix. 1s ,α 1w ,α 2s ,α 2w It indicates the coefficient of strong or weak deviation, and the specific value is determined according to the actual situation.

[0119] The system covariance is calculated according to equations (16) and (20).

[0120]

[0121] Calculate the Kalman gain matrix and update the system state and covariance.

[0122]

[0123] DIUKF is used to realize the fusion of model and inertia, and to achieve autonomous navigation of guided artillery shells.

[0124] The present invention also provides a guided artillery shell model / inertial autonomous navigation system, comprising:

[0125] A system state equation building unit, used to build the system state equation according to a six-degree-of-freedom rigid body trajectory model of the guided projectile;

[0126] System measurement equation construction unit, which analyzes the coupling relationship between trajectory parameters and inertial device measurement information based on the system state equation and constructs the system measurement equation;

[0127] A filter model building unit, which builds a guided projectile model / inertial autonomous navigation filter model according to the system state equation and the system measurement equation;

[0128] The navigation control unit designs a data-driven innovation UKF based on the filter innovation information and historical data, estimates the guided projectile model / inertial autonomous navigation filter model, and realizes autonomous navigation of the guided projectile.

[0129] Although the embodiments disclosed in the present invention are as above, the above contents are only embodiments adopted for facilitating the understanding of the present invention and are not intended to limit the present invention. Any technician in the technical field to which the present invention belongs can make any modifications and changes in the form and details of the implementation without departing from the spirit and scope disclosed in the present invention, but the patent protection scope of the present invention shall still be subject to the scope defined in the attached claims.

Claims

1. A guided projectile model / inertial autonomous navigation method, characterized in that: include: Step 1, constructing the system state equation according to the six-degree-of-freedom rigid body trajectory model of the guided projectile; Step 2: Analyze the coupling relationship between the trajectory parameters and the inertial device measurement information based on the system state equation, and construct the system measurement equation; Step 3, constructing a guided projectile model / inertial autonomous navigation filter model according to the system state equation and the system measurement equation; Step 4: Based on the filter innovation information and historical data, a data-driven innovation UKF is designed to estimate the guided projectile model / inertial autonomous navigation filter model to achieve autonomous navigation of the guided projectile.

2. A guided projectile model / inertial autonomous navigation method according to claim 1, characterized in that: The six-degree-of-freedom rigid body trajectory model of the guided projectile described in step 1 is: In the formula, v, θ a , ψ2, ω ξ ,ω η ,ω ζ , γ, x, y, z represent the velocity of the projectile's center of mass, the velocity elevation angle, the velocity direction angle, the three-axis angular velocity of rotation around the center of mass, the projectile axis elevation angle, the projectile axis azimuth angle, the projectile body roll angle, and the projectile's center of mass's three-axis position, respectively; M ξ 、M η 、M ζ It represents the three-axis components of the external torque in the elastic axis coordinate system, represents the three-axis components of the external force in the ballistic coordinate system, m, C, and A represent the mass of the projectile, the polar moment of inertia, and the equatorial moment of inertia, respectively; The system state equation is: In the formula, 3. A guided projectile model / inertial autonomous navigation method according to claim 1, characterized in that: The step 2 constructs a system measurement equation using the gyroscope output angular rate and the accelerometer output specific force as observation quantities.

4. A guided projectile model / inertial autonomous navigation method according to claim 3, characterized in that: The gyroscope output angular rate is: In the formula, h G (*) represents the nonlinear equation of the relationship between the trajectory parameters and the gyroscope output angular rate, X(i), i = 1, 2, ..., 12 represents the i-th element in X, G b Output angular rate for the gyroscope; The accelerometer output specific force is: In the formula, h f (*) represents the nonlinear equation of the relationship between the trajectory parameters and the specific force output by the accelerometer, a=[a z a x a y ] T , is the triaxial acceleration, is the east, north and celestial velocity, g n is the local gravitational acceleration, the state matrix 5. A guided projectile model / inertial autonomous navigation method according to claim 4, characterized in that: The system measurement equation is: Among them, G b is the angular rate output by the gyroscope; f b is the accelerometer output specific force, h G (*) represents the nonlinear equation of the relationship between the trajectory parameters and the angular rate output by the gyroscope, X(i), i=1,2,...,12 represents the i-th element in X, h f (*) represents the nonlinear equation that represents the relationship between the trajectory parameters and the specific force output by the accelerometer.

6. A guided projectile model / inertial autonomous navigation method according to claim 1, characterized in that: The step 4 specifically includes: First, initialize the state estimate And the state error covariance matrix P0, through UT transformation, calculate the sigma point; The 4RK method is used to update the system state and state error covariance matrix in time, and the UT transformation is performed again to generate a new sigma point set; Based on the guided missile model / inertial autonomous navigation filter model, measurement updates are performed to calculate new information; Based on the new information, evaluate the deviation between the system measurement prediction and the new measurement information and determine the degree of deviation; Based on the deviation degree, the mean of the angular velocity and specific force deviation degree is calculated, and a data-driven adaptive measurement mean square error matrix is ​​designed; Calculate the Kalman gain matrix and update the system state and covariance to achieve autonomous navigation of the guided projectile.

7. A guided projectile model / inertial autonomous navigation method according to claim 6, characterized in that: The 4RK method is used to update the system state and state error covariance matrix in time: In the formula, Q k is the system variance matrix, and the variables The weight coefficient is calculated as follows: In the formula, the coefficient α ranges from [10 -4 ,1], β and κ are coefficients.

8. A guided projectile model / inertial autonomous navigation method according to claim 6, characterized in that: The degree of deviation is: Where, T G It is a measure of the degree of deviation of the angular rate measured by the gyroscope, T f It is a measure of the degree of deviation in the accelerometer's measurement of specific force, e k (1~3) represents the first three elements of the new information, e k (4~6) represent the last 3 elements of the new information.

9. A guided projectile model / inertial autonomous navigation method according to claim 6, characterized in that: The adaptive measurement mean square error matrix is: In the formula, Represents the elements on the diagonal of the measurement mean square error matrix, α 1s ,α 1w ,α 2s ,α 2w Indicates the coefficient of strong or weak deviation.

10. A guided projectile model / inertial autonomous navigation system for implementing the method described in any one of claims 1 to 9, characterized in that: include: A system state equation building unit, used to build the system state equation according to a six-degree-of-freedom rigid body trajectory model of the guided projectile; System measurement equation construction unit, which analyzes the coupling relationship between trajectory parameters and inertial device measurement information based on the system state equation and constructs the system measurement equation; A filter model building unit, which builds a guided projectile model / inertial autonomous navigation filter model according to the system state equation and the system measurement equation; The navigation control unit designs a data-driven innovation UKF based on the filter innovation information and historical data, estimates the guided projectile model / inertial autonomous navigation filter model, and realizes autonomous navigation of the guided projectile.

Images