A combined navigation method for guided projectiles based on launching coordinate system

Abstract

The invention discloses a guided artillery shell integrated navigation method based on a launch coordinate system, relates to the field of guided artillery shell integrated navigation, improves the guided artillery shell integrated navigation method from the launch coordinate system strapdown inertial navigation / satellite measurement equation, solves the problem of weak observation of the integrated navigation yaw angle, and meets the requirements of the guided artillery shell integrated navigation. The difference between the yaw angle output by the strapdown inertial navigation and the satellite is introduced as a quantity measurement, and the traditional measurement equation is expanded to reduce the yaw angle error.

Description

Technical Field

[0001] The invention relates to the field of guided projectile integrated navigation, and in particular to a guided projectile integrated navigation method based on a launching coordinate system. Background Art

[0002] Strapdown inertial navigation / satellite integrated navigation mostly adopts the position + velocity combination mode, selects the position and velocity difference between the strapdown inertial navigation and satellite output as the measurement quantity, and uses Kalman filtering to design the integrated navigation algorithm. The yaw angle in the integrated navigation system has weak observability. At the same time, due to the large error of inertial devices, the yaw angle error is usually large and may even diverge. Therefore, when studying the integrated navigation of guided artillery shells, solving the problem of weak observability of the integrated navigation yaw angle is a technical difficulty in this field.

[0003] In order to solve the problem of weak yaw angle observability, some guided shells introduce the yaw angle information of the magnetometer, but the magnetometer has a large error, is easily disturbed by the external environment, and has low accuracy. Some guided shells use dual antennas to provide yaw angle information, but it is difficult to install two antennas with a long distance between them on the guided shells. In order to solve the problem of weak yaw angle observability of combined navigation and meet the requirements of combined navigation of guided shells, a guided shell combined navigation method based on the launch coordinate system proposed in the present invention is improved from the strapdown inertial navigation / satellite measurement equation of the launch coordinate system. The difference between the yaw angles output by the strapdown inertial navigation and the satellite is introduced as a quantitative measurement, and the traditional measurement equation is expanded to reduce the yaw angle error. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the present invention provides a combined navigation method for guided projectiles based on a launching coordinate system.

[0005] In order to achieve the above-mentioned object of the invention, the technical solution adopted by the present invention is:

[0006] A guided projectile integrated navigation method based on a launch coordinate system comprises the following steps:

[0007] S1. After the guided projectile is launched and the attitude recognition is completed, the state equation and measurement equation of the guided projectile are discretized;

[0008] S2, using the filtering recursive algorithm to calculate the state of the guided projectile at the next moment, and repeatedly filtering until the convergence state is obtained to obtain the estimated error value;

[0009] S3. Correct the calculated estimated error value to the navigation solution value to complete the combined navigation of the guided projectile.

[0010] Furthermore, the discretization of the state equation in S1 is expressed as:

[0011] X k =Φk / k-1 X k-1 +Γ k / k-1 W k-1

[0012] Among them, X k is the system state at time k; X k-1 is the system state at time k-1; W k-1 is the noise at time k-1; Φ k,k-1 With Γ k,k-1 Discretization of the state equation and the noise driving matrix.

[0013] Furthermore, the discretization of the measurement equation in S1 is expressed as:

[0014]

[0015] Among them, X is the system state of the guided projectile, H vpψ is the measurement matrix, V vpψ is the output error, V I is the velocity of the guided projectile with inertial navigation, V S is the guided projectile velocity output by the satellite, P I is the position of the guided projectile with inertial navigation, P S is the position of the guided projectile output by the satellite, ψ I is the yaw angle of the inertial navigation guided projectile, ψ S It is the yaw angle of the guided projectile output by the satellite.

[0016] Furthermore, the specific method of calculating the state of the guided projectile at the next moment by using the filtering recursive algorithm in S2 is:

[0017]

[0018] in, Updated state estimate, is the one-step state prediction, H k is the measurement matrix at time k, Z k is the discretized measurement equation at time k, K k is the filter gain matrix.

[0019] Furthermore, the estimated error value in S2 includes the error values ​​of the attitude, speed, position, gyro drift and accelerometer drift of the guided projectile.

[0020] Furthermore, the S3 specifically includes:

[0021] S31. Obtaining attitude information through navigation algorithm calculation Speed and location

[0022] S32, correct the attitude information, speed and position respectively through the estimated error value obtained in S2 to complete the combined navigation.

[0023] Furthermore, in S32, the posture information is corrected as follows:

[0024]

[0025] in, is the calculated attitude quaternion, is the corrected attitude quaternion, Q k is the attitude error φ g The corresponding quaternion;

[0026] The speed correction method is:

[0027]

[0028] Among them, V g is the corrected speed, is the calculated velocity, δV g is the speed estimation error value.

[0029] The position correction method is:

[0030]

[0031] P g is the corrected position, is the calculated position, δP g is the position estimation error value.

[0032] The present invention has the following beneficial effects:

[0033] The difference between the yaw angle of strapdown inertial navigation and satellite output is introduced as the measurement quantity to expand the traditional measurement equation, reduce the yaw angle error, and solve the problem of weak observability of the yaw angle of integrated navigation, while meeting the requirements of integrated navigation of guided artillery shells. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 The figure is a flow chart of the combined navigation method of guided projectiles based on the launch coordinate system of the present invention.

[0035] Figure 2 Schematic diagram of the relationship between the launch coordinate system and the earth in an embodiment of the present invention. DETAILED DESCRIPTION

[0036] The specific implementation modes of the present invention are described below so that those skilled in the art can understand the present invention. However, it should be clear that the present invention is not limited to the scope of the specific implementation modes. For those of ordinary skill in the art, as long as various changes are within the spirit and scope of the present invention as defined and determined by the attached claims, these changes are obvious, and all inventions and creations utilizing the concept of the present invention are protected.

[0037] In order to achieve the above object, the present invention proposes the following scheme:

[0038] (1) The present invention uses the following coordinate system and coordinate system conversion

[0039] 1) Geocentric inertial coordinate system (i system)

[0040] Earth-Centered-Inertial frame, the origin of the coordinate system is O i is the Earth's center of mass, z i The axis points to the Earth's pole along the Earth's rotation axis, x i The axis lies in the equatorial plane and points to the vernal equinox, y i The axis satisfies the right-hand rule.

[0041] 2) Earth-centered Earth-fixed coordinate system (e system)

[0042] Earth-Centered Earth-Fixed frame, with the origin O e is the center of the Earth, x e The axis lies in the equatorial plane and points toward the prime meridian, z e The y axis is the Earth's rotation axis and points to the North Pole. e The axis lies in the equatorial plane and is parallel to the x e Axis, z e The axes form a right-handed rectangular coordinate system.

[0043] 3) Emission coordinate system (g system)

[0044] Launch-Centered Earth-Fixed frame, origin of the coordinate system O g is the launch point, x g The axis is in the horizontal plane of the launch point and points in the launch aiming direction, y g The axis is perpendicular to the launch point horizontal plane and points upward, z g Axis and x g Axis, y g The axes form a right-hand rectangular coordinate system, and the launch coordinate system is fixed to the earth. The geographic latitude B0, longitude λ0, altitude H0 and launch azimuth A0 of the launch point determine the relationship between the launch coordinate system and the earth, such as Figure 2 shown.

[0045] 4) Launch inertial coordinate system (a system)

[0046] The launch inertial frame coincides with the launch coordinate frame at the moment of launch, and then the launch inertial frame remains unchanged in the inertial space and does not rotate with the earth. The launch inertial frame is defined by the launch coordinate frame at the moment of launch, and its origin remains unchanged relative to the geocentric inertial frame.

[0047] 5) Projectile body coordinate system (b system)

[0048] Body frame, origin of coordinate system O b is the center of mass of the projectile, x b The y axis points to the front of the projectile along the longitudinal axis of the projectile. b The axis points upwards in the plane of the main symmetry axis of the projectile, z b Axis and x b Axis, y b The axes form a right-handed rectangular coordinate system pointing to the right of the projectile.

[0049] 6) Transformation relationship between the launch coordinate system and the Earth-centered Earth-fixed coordinate system

[0050] The direction cosine matrix from the Earth-centered Earth-fixed coordinate system to the launch coordinate system is: The Earth-centered Earth-fixed coordinate system is rotated to the launch coordinate system, which is obtained by three rotations, involving the initial longitude λ0, geographic latitude B0 and heading A0 of the aircraft, and the transformation matrix is ​​obtained. as follows:

[0051]

[0052] 7) Conversion relationship between launch coordinate system and projectile coordinate system

[0053] The attitude angle of the guided projectile in the launch coordinate system relative to the projectile body coordinate system is determined by the pitch angle The three Euler angles of yaw angle ψ and roll angle γ describe the launch coordinate system to the missile body coordinate system, and rotate the pitch angle around the z axis first. Then rotate the yaw angle ψ around the y-axis and then the roll angle γ around the x-axis in a 3-2-1 rotation sequence to get the transformation matrix as follows

[0054]

[0055] Transformation Matrix

[0056] Based on the above principle, the present invention provides a combined navigation method for guided projectiles based on a launch coordinate system, such as Figure 1 As shown, the following steps are included:

[0057] S1. After the guided projectile is launched and the attitude recognition is completed, the state equation and measurement equation of the guided projectile are discretized;

[0058] Launch coordinate system strapdown inertial navigation / satellite loosely coupled state equation

[0059] The Kalman filter state equation is:

[0060]

[0061] 1) The state vector X is

[0062]

[0063] The state vector includes the attitude vector error φ in the launch coordinate system g 、Velocity error vector δV g 、Position error vector δP g , gyro error vector δε b and the accelerometer error vector

[0064] 2) The state transfer matrix F is

[0065]

[0066] In formula (5), is the projection of the Earth's rotational angular velocity in the launch coordinate system The antisymmetric matrix of . They are

[0067]

[0068]

[0069] In formula (6), ω ie is the angular velocity of system e relative to system i caused by the rotation of the Earth, is the corresponding angular velocity vector

[0070]

[0071] 3) The noise driving matrix G is

[0072]

[0073] 4) The process noise vector W is

[0074]

[0075] In formula (10), w g is the gyroscope white noise, w a is the accelerometer white noise.

[0076] The traditional launch coordinate system strapdown inertial navigation / satellite loosely coupled measurement equation uses velocity and position as observation quantities, and the Kalman filter measurement equation is:

[0077]

[0078] In formula (11), V I , P I , V S , P S are the speed and position output by the inertial navigation and satellite respectively, and the measurement matrix is is the speed and position output error.

[0079] The yaw angle ψ output by the satellite S

[0080] The satellite receiver can directly obtain the velocity vector V of the northeast sky coordinate system l =[V E V N V U ] T According to the characteristics of guided artillery shells, the angle of attack and the angle of sideslip are very small when the projectile is in an uncontrolled static stable state. The ballistic deviation angle can be used as an approximate value of the yaw angle. Therefore, the northeast celestial velocity component V provided by the satellite receiver is used. E 、V N To calculate the yaw angle of the guided projectile, the calculation formula is as follows

[0081] ψ S ≈ψ v =arctan 2(-V E ,V N ) (12)

[0082] The yaw angle ψ output by the inertial navigation I

[0083] The calculated transformation matrix It can be written as

[0084]

[0085] in,

[0086]

[0087] Ψ g is φ g The antisymmetric matrix, φ g =[φ x φ y φz ] T is the attitude error vector,

[0088]

[0089] From formula (13), we get

[0090]

[0091] In formula (16), is the theoretical value of the measurement matrix, and is obtained from formula (2):

[0092]

[0093] In formula (17), ψ, γ are the theoretical values ​​of yaw angle, pitch angle and roll angle, respectively, and

[0094]

[0095] The calculated transformation matrix for

[0096]

[0097] In formula (18), are the measured values ​​of yaw angle, pitch angle and roll angle respectively. And

[0098]

[0099] The relationship between the measured values ​​and theoretical values ​​of yaw angle, pitch angle and roll angle is:

[0100]

[0101] In formula (19), δψ, δγ are the measurement errors of yaw angle, pitch angle and roll angle respectively.

[0102] Substituting equation (17) and equation (19) into equation (16), we get

[0103]

[0104] From equation (15) and equation (17), we can get

[0105]

[0106] From equations (19), (20) and (21), we know that the element in the third row and first column on the left side of equation (14) is -sin(ψ I +δψ I )+sinψ I, the element in the third row and first column on the right side of formula (14) is Therefore, there is

[0107]

[0108] For small amounts of X, we have

[0109] sin X=X (23)

[0110] ψ I is a small amount, formula (22) can be transformed into

[0111]

[0112] The measurement matrix of the yaw angle is

[0113] Z ψ =ψ I -ψ S =δψ I -δψ s (25)

[0114] In formula (25), ψ I , S ,δψ I ,δψ S They are the yaw angle and yaw angle error output by the inertial navigation and satellite respectively.

[0115] Substituting equation (24) into equation (25), we get

[0116]

[0117] in

[0118]

[0119] remember

[0120]

[0121] Then there is

[0122] Z ψ =H ψ X+V ψ (29)

[0123] Taking speed, position and yaw angle as observation quantities, the Kalman filter measurement equation is:

[0124]

[0125] In formula (11), V I , P I , I , V S, P S , S are the speed, position and yaw angle output by the inertial navigation and satellite respectively. The measurement matrix is is the speed and position output error.

[0126] Based on this, the discretization of the state equation and the measurement equation are

[0127] X k =Φ k / k-1 X k-1 +Γ k / k-1 W k-1 (31)

[0128] Z k =H k X k +V k (32)

[0129] Where, X k is the system state at time k; Φ k / k-1 With Γ k / k-1 Discretization of the state equation and the noise driving matrix.

[0130]

[0131]

[0132] S2, using the filtering recursive algorithm to calculate the state of the guided projectile at the next moment, and repeat filtering until the convergence state to obtain the estimated error values ​​of attitude, speed, position, gyroscope drift and accelerometer drift;

[0133] In this embodiment, the state is predicted in one step:

[0134] X k / k-1 =Φ k / k-1 X k-1 (35)

[0135] State Estimation

[0136]

[0137] In the formula, K k It is called the filter gain matrix, which is the weight of the observation information when the state is updated.

[0138]

[0139] Where P k / k-1 It is called the one-step prediction mean square error matrix, whose diagonal elements are the variances of each state estimate, which can represent the uncertainty of the estimate

[0140]

[0141] The estimated mean square error matrix is ​​P k

[0142]

[0143] Based on the above principle, given the initial values ​​X0 and P0, according to the measured value Z at time k k , recursively obtain the state estimate at time k After repeating the calculation n times, we can get The convergence value, that is, the error value of attitude, velocity, position, gyro drift, and accelerometer drift.

[0144] S3. Correct the calculated estimated error value to the navigation solution value to complete the combined navigation of the guided projectile.

[0145] The attitude information is calculated by the navigation algorithm Speed

[0146] (1) Posture correction:

[0147] Calculated by quaternion

[0148]

[0149] In the formula, is the calculated attitude quaternion, is the corrected attitude quaternion, Q k is the attitude error φ g The corresponding quaternion.

[0150] (2) Speed ​​correction:

[0151]

[0152] (3) Position correction:

[0153]

[0154] The present invention uses specific embodiments to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on the present invention.

[0155] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and should be understood that the protection scope of the present invention is not limited to such specific statements and embodiments. Those skilled in the art can make various other specific variations and combinations that do not deviate from the essence of the present invention based on the technical revelations disclosed by the present invention, and these variations and combinations are still within the protection scope of the present invention.

Claims

1. A guided projectile integrated navigation method based on a launch coordinate system, characterized in that: The steps include: S1. After the guided projectile is launched and the attitude recognition is completed, the state equation and measurement equation of the guided projectile are discretized; S2. Calculate the state of the guided projectile at the next moment by using the filtering recursive algorithm, and repeat filtering until convergence to obtain the estimated error values ​​of attitude, speed, position, gyroscope drift and accelerometer drift. The specific method of calculating the state of the guided projectile at the next moment by using the filtering recursive algorithm is as follows: in, Updated state estimate, is the one-step state prediction, for k The measurement matrix of time, for k The discretized measurement equation at time, Filter gain matrix; S3, correcting the calculated estimated error value to the navigation solution value to complete the combined navigation of the guided projectile, specifically including: S31. Obtaining attitude information through navigation algorithm calculation ,speed and location S32, respectively correct the attitude information, speed and position through the estimated error value obtained in S2 to complete the combined navigation, wherein: The correction method for posture information is: in, is the calculated attitude quaternion, is the corrected attitude quaternion, is the attitude error The corresponding quaternion; The speed correction method is: in, is the corrected speed, is the calculated speed, is the speed estimation error value; The position correction method is: is the corrected position, is the calculated position, is the position estimation error value.

2. The guided artillery shell integrated navigation method based on the launch coordinate system according to claim 1 is characterized in that: The discretization of the state equation in S1 is expressed as: in, yes k System status at the moment; yes System status at the moment; for The noise of the moment; and Discretization of the state equation and the noise driving matrix.

3. The guided artillery shell integrated navigation method based on the launch coordinate system according to claim 1 is characterized in that: The discretization of the measurement equation in S1 is expressed as: in, is the system status of the guided projectile, is the measurement matrix, is the output error, is the velocity of the guided projectile with inertial navigation, is the guided projectile velocity output by the satellite, is the position of the inertial navigation guided projectile, The position of the guided shell output by the satellite, is the yaw angle of the inertial navigation guided projectile, It is the yaw angle of the guided projectile output by the satellite.

4. The guided projectile integrated navigation method based on the launch coordinate system according to claim 1 is characterized in that: The estimated error values ​​in S2 include the error values ​​of the attitude, speed, position, gyro drift and accelerometer drift of the guided projectile.

Images