Method and system for rapid solution of strapdown attitude of super-short-range guided missile

Abstract

The application discloses a kind of strapdown attitude quick solution method and system of super short-range guided missile, and the method specifically includes: first, for the characteristics that super short-range guided missile flight time is short, the real-time requirement of attitude solution algorithm is higher, algorithm lightweight design is carried out, ground system is used as navigation coordinate system to simplify the method of navigation solution under conventional geographic coordinate system, the influence of earth rotation angular velocity and earth curvature is ignored;Second, in order to simulate the irregular disturbance produced in the aiming initial alignment process of human body shoulder carrying weapon, a human body shaking model is established;Then, to further speed up the convergence speed of initial alignment, a shaking scene fast initial alignment method based on sliding window is designed, and the algorithm verification is carried out based on human body shaking model;Finally, based on navigation coordinate system simplification and fast alignment method based on sliding window, the fast and high-precision solution of strapdown inertial navigation of super short-range guided missile in whole process is realized.

Description

Technical Field

[0001] This invention belongs to the field of inertial navigation technology, and in particular to a method and system for rapid attitude calculation of ultra-short-range guided missiles. Background Technology

[0002] Strapdown Inertial Navigation System (SINS) is crucial for the rapid response of ultra-short-range guided missiles. It utilizes the motion information of the projectile sensed by inertial measurement units (IMUs) to achieve navigation and positioning through attitude, velocity, and position calculations. Attitude calculation is a critical step in navigation; its accuracy and speed directly impact the overall navigation performance. Therefore, improving the speed of attitude calculation is essential to ensuring that SINS completes its mission on time.

[0003] Currently, the reason why it is difficult to guarantee speed in the attitude calculation of ultra-short-range guided missiles is that the angular velocity vector generated by the rotation of the geographic coordinate system relative to the inertial system leads to a large complexity in solving the angular velocity. At the same time, since the gravitational acceleration vector changes with position during velocity calculation, it is necessary to solve the gravitational acceleration in real time, which further increases the solution time of the algorithm.

[0004] In addition, the navigation accuracy of SINS is greatly affected by the initial alignment accuracy, especially when facing the problem of moving base caused by the irregular movement of the human body when facing direct-fire ultra-short-range guided missiles. The initial alignment method is required to have the ability to resist strong interference. Summary of the Invention

[0005] The purpose of this invention is to provide a method and system for rapid attitude calculation of ultra-short-range guided missiles, which can be applied to the rapid attitude calculation of ultra-short-range guided missiles. This solves the problem that existing technologies cannot meet the requirements of speed and high precision for initial alignment of ultra-short-range guided missiles under swaying base conditions, as well as the problem of rapid attitude acquisition when the flight time is short.

[0006] This invention is achieved through the following technical solution: This invention proposes a fast attitude calculation method for ultra-short-range guided missiles, the method specifically including:

[0007] Step 1: Definition of the coordinate system and simplification of the navigation coordinate system in the strapdown solution process for ultra-short-range guided missiles;

[0008] Step 2: Establishing a human body swaying model in a scenario of rapid aiming with shoulder-fired weapons;

[0009] Step 3: Rapid initial alignment of a shoulder-launched ultra-short-range guided missile in a swaying scenario based on a sliding window;

[0010] Step 4: Fast attitude calculation of ultra-short-range guided missiles using a simplified navigation coordinate system and strapdown inertial navigation.

[0011] A rapid attitude calculation system for ultra-short-range guided missiles, used to implement the above method, the system includes:

[0012] The first module is used to define the coordinate system for the strapdown solution process of ultra-short-range guided missiles and to simplify the navigation coordinate system;

[0013] The second module is used to establish a human body sway model in the scenario of rapid aiming of shoulder-fired weapons.

[0014] The third module is a rapid initial alignment of a shoulder-launched ultra-short-range guided missile in a swaying scenario based on a sliding window.

[0015] The fourth module, based on a simplified navigation coordinate system, enables rapid attitude calculation for strapdown inertial navigation systems used in ultra-short-range guided missiles.

[0016] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the program to implement the steps of the method described above.

[0017] A computer-readable storage medium having a computer program stored thereon that, when executed by a processor, implements the steps of the above-described method.

[0018] A computer program product includes a computer program that, when executed by a processor, implements the steps of the above-described method.

[0019] Compared with existing technologies, the beneficial effects of this invention are as follows: The lightweight design of the attitude calculation algorithm simplifies the original navigation calculation process under the geographic coordinate system to a strapdown inertial navigation calculation process based on the characteristics of short flight time and short flight distance of ultra-short-range guided missiles, effectively improving the real-time performance and reliability of the algorithm. The established human body sway model can effectively simulate the irregular disturbances generated by a human shoulder-fired weapon during aiming. The designed fast initial alignment method based on a sliding window for swaying scenarios uses the sliding window to solve for the gravity component error in the initial alignment. This means that the algorithm's convergence is judged not by reaching a fixed convergence time, but by using the average error obtained from the rolling window, thus greatly improving the convergence speed while ensuring convergence accuracy. Finally, the designed strapdown inertial navigation attitude calculation method for ultra-short-range guided missiles based on a simplified navigation coordinate system meets the requirement of speed in strapdown attitude calculation under short flight time conditions. Attached Figure Description

[0020] Figure 1 This is a schematic diagram of the posture angle curve of a human body swaying model.

[0021] Figure 2 This is a schematic diagram of the angular velocity curve of a human body swaying model.

[0022] Figure 3 This is a schematic diagram of the acceleration curve of a human body swaying model.

[0023] Figure 4 This is a schematic diagram of the initial alignment process.

[0024] Figure 5 This is a schematic diagram of the pitch angle curve during the initial alignment process.

[0025] Figure 6 This is a schematic diagram of the roll angle curve during the initial alignment process.

[0026] Figure 7 This is a schematic diagram of the yaw angle curve during the initial alignment process.

[0027] Figure 8 This is a schematic diagram of the attitude angle error curve during the initial alignment process.

[0028] Figure 9 This is a schematic diagram of the attitude angle calculation results for strapdown navigation.

[0029] Figure 10 This is a schematic diagram of the speed calculation results for strapdown navigation.

[0030] Figure 11 This is a schematic diagram of the position calculation results for strapdown navigation.

[0031] Figure 12 This is a schematic diagram illustrating the error in attitude angle calculation for strapdown navigation.

[0032] Figure 13 This is a schematic diagram of the velocity calculation error.

[0033] Figure 14 This is a schematic diagram of the position calculation error. Detailed Implementation

[0034] To ensure the accuracy and speed of ultra-short-range guided missiles, this invention provides a method and system for rapid attitude calculation using strapdown inertial navigation systems (SINS). The innovations of this invention are as follows: First, addressing the high complexity of calculating the missile's rotational angular velocity relative to the geographic coordinate system during attitude calculation in ultra-short-range guided missiles, and the complex calculation of the gravitational acceleration vector as the position changes during velocity calculation, a lightweight algorithm design is implemented: The influence of the Earth's rotational angular velocity is ignored, and the navigation coordinate system of commonly used guided missiles is changed from the geographic coordinate system to a ground coordinate system, eliminating the process of processing the missile's rotational angular velocity relative to the geographic coordinate system; the decomposition process of the gravitational acceleration vector caused by changes in position coordinates is ignored, assuming that gravitational acceleration is constant, thus simplifying the navigation coordinate system. Second, addressing the strong interference caused by irregular human movement during the initial alignment of the moving base of a direct-fire ultra-short-range guided missile, a human sway model is established, and a rapid alignment method based on a sliding window is designed. Finally, based on the simplified navigation coordinate system and the rapid alignment method based on the sliding window, rapid and high-precision calculation of the entire strapdown inertial navigation system for ultra-short-range guided missiles is achieved.

[0035] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0036] An embodiment of the present invention provides a method for rapid attitude calculation of a strapdown guided missile, which specifically includes the following steps:

[0037] S1. Definition of coordinate system and simplification of navigation coordinate system in the strapdown solution process of ultra-short-range guided missiles

[0038] 1. Coordinate system definition in the strapdown calculation process; First, the coordinate systems involved in the calculation process, such as the missile body coordinate system, navigation coordinate system, geographic coordinate system, ground coordinate system, platform coordinate system, and initial platform coordinate system, are defined and explained:

[0039] (1) Projectile coordinate system OX b Y b Z b

[0040] With the missile's center of mass O as the origin of the coordinate system, OX b The axis points along the longitudinal axis of the projectile towards the head, OY b The axis is in the longitudinal symmetry plane of the guided missile and is related to OX. b Vertical, upward is positive, OZ b With OX b Y b The planes are perpendicular and form a right-handed coordinate system.

[0041] (2) Navigation coordinate system OX n Y n Z n

[0042] A navigation coordinate system is the coordinate system used by an inertial navigation system when solving for navigation parameters. The geographic coordinate system is commonly used as the navigation coordinate system.

[0043] (3) Geographic coordinate system OX l Y l Z l

[0044] The origin of the geographic coordinate system is the center of mass of the projectile, OY l The horizontal axis points to true north, OX l Horizontal direction east, OZ l Axis and OX l OY l The axes form a right-handed coordinate system, with the direction perpendicular to the Earth's ellipsoid.

[0045] (4) Platform coordinate system OX p Y p Z p

[0046] The origin of the platform coordinate system is the center of mass of the projectile, OX. p OY p The two axes are in the horizontal plane and perpendicular to each other, OZ p Perpendicular to the horizontal plane, the platform coordinate system is stable in inertial space. The platform coordinate system may coincide with the projectile coordinate system or may form a small angle with the projectile coordinate system.

[0047] (5) Initial platform coordinate system OX p Y p Z p

[0048] Take the center of mass of the projectile as the origin of the coordinate system, which coincides with the coordinate system of the projectile before initial alignment. Ideally, the initial platform coordinate system is stable in inertial space.

[0049] 2. Simplification of the navigation coordinate system; Navigation coordinate system OX n Y n Z n The coordinate system used by inertial navigation systems to solve for navigation parameters is the geographic coordinate system. Typically, the geographic coordinate system is used. However, considering the high complexity of calculating the missile's rotational angular velocity relative to the geographic coordinate system during attitude calculations and the complexity of calculating the gravitational acceleration vector as a function of position during velocity calculations, a simplified design of the conventional navigation coordinate system is implemented, taking into account the short flight time and short distance of ultra-short-range guided missiles. The calculation equations for strapdown inertial navigation in the geographic coordinate system (taking the Northeast-Eastern Sky coordinate system as an example) are as follows:

[0050]

[0051] in, λ, h represent latitude, longitude, and altitude, respectively; v n =[v e ,v n ,v u ] T v e ,v n ,v u In order, the speeds are: eastward speed, northward speed, and upward speed. D is the rotation matrix of the projectile system relative to the geographic system. -1 f is the velocity vector transformation matrix; b For comparison of missile systems; g n This is the local gravitational acceleration vector, which varies with the position of the projectile; This is the projection vector of the Earth's rotational angular velocity onto the geographic coordinate system; This is the angular velocity vector generated by the rotation of the geographic coordinate system relative to the Earth's direction. Angular velocity vector for attitude calculation The corresponding antisymmetric matrix, and The angular velocity vector sensed by the gyroscope The corresponding antisymmetric matrix; The angular velocity vector caused by the rotation of the geographic coordinate system relative to the inertial frame. The corresponding antisymmetric matrix.

[0052] In attitude calculation, the angular velocity vector is generated by the rotation of the geographic coordinate system relative to the inertial frame. Cause angular velocity The complex solution process is a major factor affecting the speed of calculation in strapdown inertial navigation systems for ultra-short-range guided missiles. Due to... For angular velocity vector In fact, it consists of two parts, namely

[0053] (2) Among them, This is the projection of the Earth's rotational angular rate relative to the inertial coordinate system onto the projectile's coordinate system. Let be the projection of the rate of change of orientation relative to the geographic coordinate system (relative to the geocentric coordinate system) onto the elastic system. It can be expressed in antisymmetric matrix form as:

[0054]

[0055] in, for The corresponding antisymmetric matrix, ω e This is the Earth's rotational angular velocity; for The corresponding antisymmetric matrix, In the formula, R M R is the radius of curvature of the meridian. N Let be the radius of curvature of the y-axis.

[0056] The above calculation process involves multiple parameter calculation formulas and matrix operations, making it computationally complex. Due to the speed requirements of strapdown attitude calculation for ultra-short-range guided missiles, the above angular velocity pre-calculation process needs to be simplified while ensuring accuracy. First, the angular velocity term caused by the change in direction due to coordinate system rotation is considered. Due to the radius R of the meridian circle M The radius R of the circle between the east and west N The order of magnitude is much larger than the eastward velocity component v of the projectile. e With the northward velocity component v n ,but Since this is a near-zero minimum, this term can be ignored. Secondly, This section describes the impact of Earth's rotation angular velocity on attitude calculation. Since Earth's rotation angular velocity is relatively small, its impact on attitude calculation is only significant during long-duration flights. For ultra-short-range guided missiles (USTOLs), the flight time is less than 30 seconds, and the attitude error caused by changes in Earth's rotation angular velocity is minimal. Furthermore, most existing USTOLs are equipped with miniaturized, low-cost MEMS sensors, whose device errors are on the order of magnitude of Earth's rotation angular velocity. The Earth's angular velocity component they detect is mixed into the measurement error; therefore, the impact of Earth's rotation angular velocity on USTOLs is negligible. Thus, the attitude calculation equation for USTOLs can be simplified to:

[0057]

[0058] Based on the above assumptions, the velocity update formula under the geographical system is simplified as follows:

[0059]

[0060] In the formula, g n =[0,0,-g] T This is the projection of the gravitational acceleration vector onto the navigation frame. It's about location. The function is given. The ultra-short-range guided missile has a short flight distance and a very small range of latitude and longitude changes. Its gravitational acceleration change can be ignored. It is assumed that the gravitational acceleration vector is a constant vector during its flight, and g is the local gravitational acceleration.

[0061] The position calculation process for ultra-short-range guided missiles uses a Cartesian coordinate system, not a geodetic coordinate system. Therefore, the position calculation formula is simplified as follows:

[0062]

[0063] Where, r car This represents the position of the projectile in Cartesian coordinates.

[0064] The simplified calculation equations for strapdown inertial navigation in the geographic coordinate system (taking the Northeast-Sky coordinate system as an example) are as follows:

[0065]

[0066] In summary, considering the flight characteristics of ultra-short-range guided missiles and the accuracy requirements of navigation calculations, the following simplified assumptions are proposed for the original geographic navigation coordinate system: 1) The influence of the Earth's rotational angular velocity is ignored, and it is assumed that the ultra-short-range guided missile strapdown inertial navigation calculation process is conducted in an inertial coordinate system, i.e., the Earth's rotational angular velocity is not considered. 2) The Earth's surface is considered to be a plane, and the gravitational acceleration during flight is considered to be constant, thus completing the simplification of the navigation coordinate system.

[0067] Considering the direct-fire characteristics of ultra-short-range guided missiles, and based on the above assumptions, the following navigation coordinate system suitable for ultra-short-range guided missiles is established, denoted as the ground coordinate system O. d X d Y d Z d The origin O of this coordinate system d For the center of mass of the instantaneously guided missile, O d X d The axis points to the intersection of the ballistic plane (track plane) and the horizontal plane, with the direction pointing towards the target being positive. d Y d The axis is perpendicular to the upward line, O d Z d Shaft and O d X d Y d The planes are perpendicular and form a right-handed coordinate system. This coordinate system is fixed to the Earth's surface, stationary relative to the Earth, and approximates as an inertial coordinate system during flight. In short-range guided missiles, within the range, the Earth's surface is considered a plane, and the gravitational field is a parallel force field, perpendicular to the Earth's surface. d Y d The axis is parallel, and the direction is along O. d Y d Negative axis.

[0068] S2. Establishment of a human body sway model in a scenario of rapid aiming with shoulder-fired weapons

[0069] The aiming of a shoulder-fired weapon initially produces significant swaying. As the shooter's body stabilizes, the swaying gradually decreases until it becomes stable, but slight swaying still exists due to factors such as breathing and muscle tremors. To simulate the series of irregular disturbances generated during aiming, this patent uses the superposition of a sine function whose amplitude decreases over time and a sine function whose amplitude remains constant to describe this process. First, a sine function whose amplitude decreases over time is used to describe the shooter's gradual stabilization from the initial large swaying amplitude. Second, a sine function with a constant amplitude is used to describe the slight swaying process that still exists after the shooter's body stabilizes. Finally, the above sine functions are superimposed to approximate the entire process of irregular swaying of the human body during the initial aiming process.

[0070] The established three-axis attitude angle human body sway model is as follows:

[0071]

[0072] Where θ, γ, and ψ are the missile's pitch angle, roll angle, and yaw angle in the ground coordinate system, respectively; θ Δ γ Δ ψ Δ These represent the pitch, roll, and yaw angles after stabilization during shoulder-loaded operations; θ A1 γ A1 ψ A1 These represent the maximum amplitudes of motion in each posture under swaying decay motion; α θ α γ α ψ T represents the attenuation coefficient of each attitude angle under swaying and decaying motion; θ1 T γ1 T ψ1 θ represents the period of change of various attitude angles under swaying decay motion; A2 γ A2 ψ A2 These represent the amplitude values ​​of the angular inclinations at various attitudes during sinusoidal motion; T θ2 T γ2 T ψ2 t represents the period of the angular attitude of the sinusoidal motion; t represents the current time.

[0073] The above model can simulate the change curve of the missile's attitude angle under shaking, and the results are as follows: Figures 1-3 As can be seen from the curve, the disturbance gradually decreases with time and eventually becomes a stable sway. The simulation curve shows that the model can approximately simulate the disturbance process of a human shoulder-fired weapon during aiming. This can be used as the initial simulation input for the aiming process to verify the algorithm's performance.

[0074] S3. Rapid initial alignment of shoulder-launched ultra-short-range missiles in swaying scenarios based on a sliding window.

[0075] Since ultra-short-range guided missiles adopt a direct-fire approach to ensure rapid launch, meaning the guided missile points directly at the target, the initial heading angle is set to zero, and alignment estimation is performed only for the initial pitch and roll angles.

[0076] In a shoulder-mounted swaying scenario, the specific force measured by the accelerometer is: It is the gravitational acceleration vector g n and interference acceleration The combined force, the disturbance acceleration The main source is the irregular shaking of the human body. To effectively handle the interference acceleration caused by human body shaking during the initial alignment process... The impact is first addressed by establishing the initial platform coordinate system at the start of the initial alignment. The ratio measured at each time point In the initial platform coordinate system The average value is calculated over a period of time to measure the acceleration caused by the disturbance, taking advantage of the characteristic that human body swaying gradually tends to a stable state. To suppress.

[0077] Let the ratio in the initial platform coordinate system at time t be... The measured specific force is The conversion relationship between the two is as follows:

[0078]

[0079] in, Let be the rotation matrix of the projectile system relative to the initial platform coordinate system at time t. The solution steps are as follows: Assume that the angular velocity vector sensed by the gyroscope at time t is... The attitude matrix at any given time is solved using the quaternion method. The rotation of the projectile coordinate system relative to the initial platform coordinate system is represented using quaternions, i.e.

[0080]

[0081] Among them, i b ,j b ,k b It is the unit vector of the three axes of the b-system. At the initial alignment start time, i.e., t=0, since the initial platform coordinate system coincides with the projectile coordinate system, the initial quaternion Q(0) = [1,0,0,0]. T The differential equation for the element Q is:

[0082]

[0083] in, The angular velocity vector in the carrier coordinate system;

[0084] Rewritten in matrix form:

[0085]

[0086] The above differential equation is updated using fourth-order Runge-Kutta, and its computational form is as follows:

[0087]

[0088] In the formula, Δt is the discrete time interval, and the attitude matrix at the current time is solved using the updated quaternion.

[0089]

[0090] Then, the rotation matrix of the projectile system relative to the initial platform coordinate system at each moment can be calculated.

[0091] Using the rotation matrix at each time step The calculated N wd Acceleration projection within +1 initial platform coordinates The arithmetic mean was used to obtain the average specific force in the initial platform coordinate system.

[0092]

[0093] Where, N wd The number of data frames in the sliding window. Let k be the projection of the acceleration. Ideally, without the influence of swaying acceleration, the specific force in the initial platform coordinate system should be the local gravitational acceleration vector g. n Let g be the acceleration vector of gravity in the ground coordinate system. n =[0,-g0,0] T Let g0 be the local gravitational acceleration (constant). Then... With g n The following relationship exists:

[0094]

[0095] In the formula θ is the rotation matrix of the ground coordinate system relative to the initial platform coordinate system; θ, γ, and ψ are the pitch, roll, and yaw angles of the initial platform coordinate system relative to the platform coordinate system, respectively.

[0096] Assuming the initial heading angle ψ = 0, the initial pitch angle and initial roll angle are:

[0097]

[0098] According to the definition of the platform coordinate system, Ox should be made p y pThe plane is parallel to the Axy plane. The axis at Ox p y p Projection on the coordinate plane and Ox p The axes coincide. Using the relationship between the rotation matrix and the attitude angles, the rotation matrix from the initial platform coordinate system to the platform coordinate system is obtained. for:

[0099]

[0100] Then use and launch time t n of Establish the initial strapdown matrix

[0101]

[0102] To accelerate the convergence speed of the transfer alignment, the pose matrix at each time step is... During the solution process, a sliding window is established, and the acceleration error within the window is calculated:

[0103]

[0104] Where, N wd M is the number of data frames in the sliding window; wd The interval between two adjacent sliding windows is the number of data frames; k is the current time. When k ≤ M wd +2N wd Since the time has not reached the sliding window size, the error is not calculated.

[0105] When the above error values ​​are consecutive N sum All windows are smaller than the set error value S. err ,Right now

[0106]

[0107] Then it is assumed that the error has converged, and the strapdown matrix from the b system to the p system is calculated according to equation (7). At this point, the initial alignment is basically complete. The overall flowchart of the initial alignment is as follows: Figure 4 The initial alignment result is as follows Figures 5-8 .

[0108] S4. Simplified navigation coordinate system for rapid attitude calculation using strapdown inertial navigation in ultra-short-range missiles.

[0109] After the missile is launched, the simplified ground coordinate system (7) from step S1 is used as the navigation coordinate system for strapdown inertial navigation calculation. Regarding the calculation method, gyroscope measurements are directly used in quaternion attitude calculation, without considering the Earth's rotation speed or angular velocity. Accelerometer data processing considers eliminating harmful accelerations, then coordinate transformation is performed and the influence of gravity is eliminated before integration to obtain the missile's position vector. The initial calculation uses the alignment result from the initial alignment algorithm for the swaying scenario introduced in step S3 as the initial value for the inertial navigation calculation.

[0110] The strapdown inertial navigation (INS) process for ultra-short-range guided missiles consists of three stages: attitude matrix update, velocity update, and position update. The attitude update method for ultra-short-range guided missiles uses the quaternion method. A preliminary strapdown INS navigation algorithm is as follows: Figure 6 As shown below. The detailed steps are described in detail.

[0111] (1) Quaternion initialization

[0112]

[0113] in, The quaternion components are calculated based on the initial attitude angles. θ0, γ0, and ψ0 are the initial attitude angles of the projectile in the ground coordinate system, which are provided by the initial alignment process in step S2.

[0114] (2) Quaternion normalization

[0115] The navigation computer has rounding errors during floating-point calculations, which makes the updated quaternion norm non-unique. To ensure the orthogonality of the attitude matrix, the quaternions are normalized as follows:

[0116]

[0117] Where Q = [q0, q1, q2, q3] T ,Right now

[0118]

[0119] (3) Construct the initial attitude matrix

[0120] Calculate the attitude matrix using the initial quaternion:

[0121]

[0122] (4) Obtain angular velocity

[0123] The angular velocity required to calculate the attitude angle is:

[0124]

[0125] in, The angular velocity that the missile-borne gyroscope is sensitive to.

[0126] (5) Quaternion Update

[0127] Quaternion differential equations Solve using the fourth-order Runge-Kutta method:

[0128]

[0129] Where Δt is the solution step size; Q(t-Δt) is the quaternion of the previous time step; Ω(t-Δt) is the antisymmetric matrix of the angular velocity of the previous time step; This is the average of the angular velocities at the previous and current moments.

[0130]

[0131] in, The angular velocities along the three axes of the inertial navigation-sensitive missile system.

[0132] After the solution is completed, the quaternion is normalized using equation (23), and the attitude matrix after the quaternion is updated is obtained using equation (25).

[0133] (6) Attitude angle extraction

[0134] Obtaining attitude angles using the attitude matrix:

[0135]

[0136] The pitch angle ranges from -90° to 90°, the roll angle ranges from -90° to 90°, and the heading angle ranges from -180° to 180°.

[0137] (7) Speed ​​Update

[0138] The velocity differential equation is:

[0139]

[0140] in, f is the velocity differential vector of the navigation system; b The specific force sensed by the onboard accelerometer; g n Let g be the gravitational acceleration vector in the ground coordinate system. In the formula, g... n =[0,-g0,0] T g0 is the local gravitational acceleration, which is considered constant during flight.

[0141] The velocity differential equation is solved using the trapezoidal rule:

[0142]

[0143] Where Δt is the velocity calculation step size, f n (t) represents the ratio in the navigation coordinate system at the current moment, where

[0144]

[0145] (8) Location update

[0146] The position differential equation is:

[0147]

[0148] in, V is the position differential vector; n =[V x V y V z ] T This is the velocity vector.

[0149] The position differential equation is also solved using the trapezoidal rule:

[0150]

[0151] Where Δt is the solution step size, X(t), Y(t), and Z(t) are the positions of the ground-based guided missile on the X, Y, and Z axes at the current moment, respectively; X(t-Δt), Y(t-Δt), and Z(t-Δt) are the positions of the ground-based guided missile on the X, Y, and Z axes at the previous moment, respectively; V x (t), V y (t) and V z (t) represents the velocity of the ground-guided missile along the X, Y, and Z axes at the current moment; V x (t-Δt), V y (t-Δt) and V z (t-Δt) represents the velocities of the missile along the X, Y, and Z axes at the previous moment in the ground-based system.

[0152] (9) Missile system apparent velocity update

[0153] The differential equation for the apparent velocity of the projectile is as follows:

[0154]

[0155] in, For the projectile's apparent velocity, f b This represents the projectile acceleration sensed by the onboard accelerometer. The apparent velocity of the projectile is solved using the trapezoidal rule, as shown in the following formula:

[0156]

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

[0158] Based on the same inventive concept, the present invention also provides a rapid attitude calculation system for ultra-short-range guided missiles, used to implement the above-mentioned method, the system comprising:

[0159] The first module is used to define the coordinate system for the strapdown solution process of ultra-short-range guided missiles and to simplify the navigation coordinate system;

[0160] The second module is used to establish a human body sway model in the scenario of rapid aiming of shoulder-fired weapons.

[0161] The third module is a rapid initial alignment of a shoulder-launched ultra-short-range guided missile in a swaying scenario based on a sliding window.

[0162] The fourth module, based on a simplified navigation coordinate system, enables rapid attitude calculation for strapdown inertial navigation systems used in ultra-short-range guided missiles.

[0163] The specific implementation methods of the above modules are the same as the specific steps of the aforementioned method for rapid attitude calculation of ultra-short-range guided missiles, and will not be repeated here.

[0164] Example

[0165] The present invention will be further described below with reference to specific embodiments, but the present invention is not limited to the embodiments.

[0166] 1) Generation of human body sway model data

[0167] The latitude, longitude, and altitude of the launch point are set to (32.0615513°, 118.7915619°, 54m). The alignment simulation period is 100 seconds. Referring to the three-axis attitude angle human body sway formula, the simulation experimental parameters are given in Table 1. The generated human body sway simulation image is shown below. Figures 1-3 As shown.

[0168] Depend on Figures 1-3 It can be seen that the human body swaying simulation curve first oscillates and then gradually becomes stable, a process that closely resembles the actual human body swaying posture aiming process.

[0169] Table 1 Simulation parameters of human body swaying

[0170]

[0171] 2) Initial Alignment Simulation

[0172] The data simulated using a human body sway model is used as the initial alignment input data. Considering the significant noise of the MEMS inertial navigation system onboard the ultra-short-range guided missile, corresponding device noise needs to be added to the simulated ground truth as the initial alignment measurement data. The device noise parameters are shown in the table below.

[0173] Table 2 Initial Alignment Results

[0174]

[0175] Initial attitude alignment is performed using data with added device noise, where the sliding window size is N. wd =50, alignment end period is N sum =10, error precision set to S err =4×10 -8 m / s 2 The initial alignment result under this simulation setting is as follows: Figures 5-8 As shown in Table 3, the initial alignment time is 1.335 seconds, meeting the requirement for rapid initial alignment. Table 3 shows that the overall error is less than 0.07°, indicating high alignment accuracy, which meets the guidance requirements for ultra-short-range strapdown missiles. Figures 5-7 It can be seen that the predicted and standard values ​​of the attitude angles maintain the same trend of change. Figure 8 It can be clearly seen that the error curve shows a trend of small fluctuations, but remains basically stable.

[0176] Table 3 Initial Alignment Results

[0177]

[0178]

[0179] To further verify the reliability of the method, 1000 Monte Carlo simulations were conducted under the above experimental parameters. The simulation results are shown in Table 4. The simulation results show that the mean value of the three-axis attitude angle alignment error is basically consistent with the alignment error in Table 3. The root mean square error of the multiple simulation results indicates that the alignment error of the multiple tests is basically around the mean value, which demonstrates the stability of the initial alignment algorithm. The mean alignment time of the multiple tests is less than 2 seconds, and the root mean square error of the alignment time is small, indicating that under these parameter settings, the initial alignment algorithm can quickly and stably converge to a good accuracy, and the algorithm meets the requirements for rapid alignment of ultra-short-range guided missiles under swaying conditions.

[0180] Table 4 Initial Alignment Results (Monte Carlo Test)

[0181]

[0182] 3) Fast attitude calculation simulation

[0183] Based on the initial alignment algorithm calculation, a fast attitude calculation algorithm for a strapdown inertial navigation system (SINS) of an ultra-short-range guided missile was verified using ballistic simulation data. The fast attitude calculation algorithm verification used the same device noise parameters as the initial alignment algorithm verification. The strapdown navigation calculation cycle was 0.005 s. The initial alignment algorithm results were considered during the calculation process, and the initial attitude angle was set to... γ0=0°+Δγ, ψ0=0°+Δψ. in Δγ and Δψ are the alignment attitude errors after initial alignment. Based on the mean of the initial alignment error, the values ​​used in the simulation are... Δγ=0.06407°, Δψ=0.00415°.

[0184] The following hardware environment was used for simulation verification.

[0185] Table 5 Simulation Hardware Parameters

[0186]

[0187] The attitude calculation results of the ultra-short-range guided missile strapdown are as follows: Figures 9-14 As shown. By Figure 9 It can be seen that the three-axis attitude calculation errors are basically related to the initial alignment error. Except for the roll angle error, which shows a decreasing divergence trend, the pitch and yaw angle calculation errors both exhibit error divergence. However, overall, the attitude errors are relatively small: the pitch angle calculation error is less than 0.046°, the roll angle calculation error is less than 0.065°, and the yaw angle calculation error is less than 0.025°, meeting the accuracy requirements for attitude calculation during flight. The velocity calculation errors on the X and Y axes are small, with a maximum error of less than 0.05 m / s. The Y-axis velocity diverges, with a maximum error of less than 0.2 m / s. Figure 11 It can be seen that the position calculation errors of the X-axis and Y-axis are small, with a maximum error of less than 0.4m, while the Y-axis position diverges, with a maximum error of less than 1.5m.

[0188] During the simulation, the algorithm completed 1822 steps with a total calculation time of 0.05707 seconds. The single-step calculation time was 0.00003 seconds. Compared with conventional geo-based strapdown attitude calculation algorithms, this algorithm significantly shortens the navigation calculation time while ensuring calculation accuracy. It provides a better algorithm environment for low-cost, lightweight component deployment and greatly meets the requirements of rapid attitude calculation for ultra-short-range guided missiles.

Claims

1. A method for rapid attitude calculation of a strapdown guided missile for ultra-short-range applications, characterized in that, Includes the following steps: Step 1: Define the coordinate system for the strapdown solution process of the ultra-short-range guided missile, including the missile body coordinate system, navigation coordinate system, geographic coordinate system, ground coordinate system, platform coordinate system, and initial platform coordinate system, and simplify the navigation coordinate system; (1) Projectile coordinate system ; Missile center of gravity Let O be the origin of the coordinate system. The axis points towards the head along the longitudinal axis of the projectile. The axis is in the longitudinal symmetry plane of the guided missile and Vertical, upward is positive. and The planes are perpendicular and form a right-handed coordinate system; (2) Navigation coordinate system ; The navigation coordinate system is the coordinate system used by an inertial navigation system when solving for navigation parameters; the geographic coordinate system is used as the navigation coordinate system. (3) Geographic coordinate system ; The origin of the geographic coordinate system is the center of mass of the projectile. The horizontal axis points to true north. The horizontal direction points east. shaft and , The axes form a right-handed coordinate system, with the direction perpendicular to the Earth's ellipsoid. (4) Platform coordinate system ; The origin of the platform coordinate system is the center of mass of the projectile. , The two axes lie in the horizontal plane and are perpendicular to each other. Perpendicular to the horizontal plane; (5) Initial platform coordinate system ; Take the center of mass of the projectile as the origin of the coordinate system, which coincides with the coordinate system of the projectile before the initial alignment. Step 2: Establish a human body sway model for a rapid aiming scenario with shoulder-fired weapons, specifically: A sine function with amplitude decreasing over time is used to describe the shooter's gradual stabilization process from initial swaying. A sine function with constant amplitude is used to describe the swaying process that still exists after the shooter's body has stabilized. Finally, the above sine functions are superimposed to approximate the entire process of irregular swaying of the human body during the initial aiming process. The established three-axis attitude angle human body sway model is as follows: （8） in, These are the missile's pitch angle, roll angle, and yaw angle in the ground coordinate system; These represent the pitch angle, roll angle, and yaw angle after stabilization during shoulder-loaded operation, respectively. These represent the maximum amplitudes of motion in each posture under swaying decay motion; The attenuation coefficients for each attitude angle under swaying and decaying motion; The period of change of each attitude angle under swaying decay motion; These represent the angular amplitudes of various attitudes during sinusoidal motion; The period of each attitude angle of sinusoidal motion; Indicates the current time; Step 3: Rapid initial alignment of a shoulder-launched ultra-short-range guided missile in a swaying scenario based on a sliding window; For the pose matrix at each time step During the solution process, a sliding window is established, and the acceleration error within the window is calculated: （20） in, It represents the number of data frames in the sliding window; The number of data frames between two adjacent sliding windows; For the current moment; when When the time has not reached the sliding window size, the error is not calculated; for Comparison in the initial platform coordinate system at time; To calculate the step size; When the above error values ​​are continuous All windows are smaller than the set error value. ,Right now （21） Then it is assumed that the error has converged, and the strapdown matrix from the projectile coordinate system to the platform coordinate system is calculated according to equation (7). At this point, the initial alignment is complete; The simplified calculation equations for strapdown inertial navigation in geographic coordinate system are as follows: （7） in, It is the position differential vector; It is a velocity vector. The velocity differential vector in the navigation coordinate system; This is the rotation matrix of the projectile coordinate system relative to the geographic coordinate system; The force is the ratio in the projectile coordinate system; This is the local gravitational acceleration vector, which varies with the position of the projectile; The angular velocity vector sensed by the gyroscope The corresponding antisymmetric matrix; Step 4: Based on the simplification of the navigation coordinate system, realize the fast attitude calculation of the strapdown inertial navigation system for ultra-short-range guided missiles.

2. The method for rapid attitude calculation of ultra-short-range guided missiles according to claim 1, characterized in that, The simplified navigation coordinate system is as follows: Navigation coordinate system The coordinate system used by the inertial navigation system when solving for navigation parameters is the geographic coordinate system. The calculation equations for strapdown inertial navigation in the geographic coordinate system are as follows: （1） in, , In order: latitude, longitude, altitude. , In order, the speeds are: eastward speed, northward speed, and upward speed. The velocity vector transformation matrix; This is the projection vector of the Earth's rotational angular velocity onto the geographic coordinate system; This is the angular velocity vector generated by the rotation of the geographic coordinate system relative to the Earth's direction. Angular velocity vector for attitude calculation The corresponding antisymmetric matrix, and ; The angular velocity vector caused by the rotation of the geographic coordinate system relative to the inertial coordinate system. The corresponding antisymmetric matrix; because For angular velocity vector It consists of two parts, namely （2） in, This is the projection of the Earth's rotational angular rate relative to the inertial coordinate system onto the projectile's coordinate system. The projection of the rate of change of orientation relative to the geographic coordinate system (relative to the geocentric coordinate system) onto the projectile coordinate system; expressed in antisymmetric matrix form as: （3） wherein, , is the corresponding antisymmetric matrix, , is the Earth's rotation angular velocity; , is the corresponding antisymmetric matrix, , in the formula, is the meridian radius of curvature, is the prime vertical radius of curvature; The attitude calculation equation for ultra-short-range guided missiles simplifies to: （4） The velocity update formula in the geographic coordinate system is simplified as follows: （5） In the formula, This is the projection of the gravitational acceleration vector onto the navigation coordinate system. It's about location. The function, This refers to the local gravitational acceleration. The position calculation process for ultra-short-range guided missiles uses a Cartesian coordinate system, not a geodetic coordinate system. Therefore, the position calculation formula is simplified as follows: （6） in, This represents the position of the projectile in Cartesian coordinates. The following simplification assumptions are proposed for the navigation coordinate system under the original geographic coordinate system: 1) Ignore the influence of the Earth's rotation angular velocity, and assume that the calculation process of the strapdown inertial navigation of the ultra-short-range guided missile is located in the inertial coordinate system, that is, the Earth's rotation angular velocity is not considered; 2) Treat the Earth's surface as a plane, and assume that the gravitational acceleration during the flight is constant, and finally complete the simplification of the navigation coordinate system. Based on the above assumptions, the following navigation coordinate system suitable for ultra-short-range guided missiles is established, denoted as the ground coordinate system. The origin of this coordinate system For the center of gravity of the instantaneous guided missile, The axis points to the intersection of the ballistic plane and the horizontal plane; pointing towards the target is considered positive. The axis points upward along the vertical line. shaft and The planes are perpendicular and form a right-handed coordinate system. This coordinate system is fixed to the Earth's surface, stationary relative to the Earth, and approximates an inertial coordinate system during flight. In short-range guided missiles, within the range, the Earth's surface is considered a plane, and the gravitational field is a parallel force field, parallel to the Earth's surface. The axis is parallel, and the direction is along Negative axis.

3. The method for rapid attitude calculation of ultra-short-range guided missiles according to claim 2, characterized in that, Rapid initial alignment of a shoulder-launched ultra-short-range guided missile in a swaying scenario based on a sliding window is as follows: The initial heading angle is set to zero, and alignment estimation is performed only for the initial pitch angle and initial roll angle. In a shoulder-mounted swaying scenario, the specific force measured by the accelerometer is: It is the gravitational acceleration vector. and interference acceleration The combined force, the disturbance acceleration Originates from the irregular swaying process of the human body; Establish the initial platform coordinate system at the start of the initial alignment. The ratio measured at each time point In the initial platform coordinate system The average value is calculated over a period of time to measure the acceleration caused by the disturbance, taking advantage of the characteristic that human body swaying gradually tends to a stable state. To suppress; remember The ratio in the initial platform coordinate system at time is The measured specific force is The conversion relationship between the two is as follows: （9） in, for The rotation matrix of the projectile coordinate system relative to the initial platform coordinate system at any given time. The solution steps are as follows: Assume The angular velocity vector sensed by the gyroscope at any given moment is The attitude matrix at any given time is solved using the quaternion method; the rotation of the projectile coordinate system relative to the initial platform coordinate system is represented using quaternions, i.e. （10） in, It is the unit vector of the three axes of the projectile coordinate system; the initial alignment start time, i.e. Since the initial platform coordinate system coincides with the projectile coordinate system, the initial quaternion... , element The differential equation is: （11） in, The angular velocity vector in the carrier coordinate system; Rewritten in matrix form: （12） The above differential equation is updated using fourth-order Runge-Kutta, and its computational form is as follows: （13） In the formula For discrete time intervals, the attitude matrix at the current time moment is solved using the updated quaternions. （14） Then, the rotation matrix of the projectile coordinate system relative to the initial platform coordinate system at each moment is calculated. ; Using the rotation matrix at each time step The calculated results Acceleration projection within the initial platform coordinates The arithmetic mean was used to obtain the average specific force in the initial platform coordinate system. （15） in, The number of data frames in the sliding window. for Projection of acceleration at time, Ideally, without the influence of swaying acceleration, the specific force in the initial platform coordinate system should be the local gravitational acceleration vector. Let the gravitational acceleration vector in the ground coordinate system be... , Given the local gravitational acceleration; then and The following relationship exists: （16） In the formula This is the rotation matrix of the ground coordinate system relative to the initial platform coordinate system; The pitch angle, roll angle, and yaw angle of the initial platform coordinate system relative to the platform coordinate system are, in order. Based on the assumed initial heading angle Then the initial pitch angle and the initial roll angle are: （17） According to the definition of the platform coordinate system, it should be possible to make... plane and Plane parallel Axis in Projection on the coordinate plane and Axis coincidence; using the relationship between the rotation matrix and attitude angles, obtain the rotation matrix from the initial platform coordinate system to the platform coordinate system. for: （18） Then use and launch time of Establish the initial strapdown matrix : （19）。 4. The method for rapid attitude calculation of ultra-short-range guided missiles according to claim 3, characterized in that, The simplified navigation coordinate system for fast attitude calculation of ultra-short-range guided missiles using strapdown inertial navigation is as follows: After the missile is launched, the simplified ground coordinate system (7) is used as the navigation coordinate system for strapdown inertial navigation calculation; the gyroscope measurement values ​​are directly used in the quaternion attitude calculation. The initial values ​​for the inertial navigation system calculation are the alignment results of the initial alignment algorithm for the step sway scene. The calculation process of the strapdown inertial navigation system for ultra-short-range guided missiles is divided into three stages: attitude matrix update, velocity update, and position update. The attitude update method for strapdown inertial navigation systems of ultra-short-range guided missiles adopts the quaternion method. (1) Quaternion initialization （22） in, , , , These are the quaternion components calculated based on the initial attitude angles. , , The initial attitude angle of the projectile in the ground coordinate system is provided by the initial alignment process in step S2; (2) Quaternion normalization The navigation computer has rounding errors during floating-point calculations, which makes the updated quaternion norm non-unique. To ensure the orthogonality of the attitude matrix, the quaternions are normalized as follows: （23） in, ,Right now （24） (3) Construct the initial attitude matrix Calculate the attitude matrix using the initial quaternion: （25） (4) Obtain angular velocity The angular velocity required to calculate the attitude angle is: （26） in, The angular velocity that the missile-borne gyroscope is sensitive to; (5) Quaternion update Quaternion differential equations Solve using the fourth-order Runge-Kutta method: ； in, To calculate the step size; It is the quaternion from the previous time step; This is the antisymmetric matrix of the angular velocity at the previous moment; This is the average of the angular velocities at the previous and current moments. : （27） in, , , The angular velocities along the three axes in the inertial navigation-sensitive projectile coordinate system; After the solution is completed, use the formula Normalize the quaternions and use the formula Obtain the pose matrix updated by quaternions; (6) Attitude angle extraction Obtaining attitude angles using the attitude matrix: （28） The pitch angle range is as follows: The roll angle range is The range of heading angle values ​​is ; (7) Speed ​​update The velocity differential equation is: （29） in, The velocity differential vector in the navigation coordinate system; The specific force sensed by the onboard accelerometer; Let be the gravitational acceleration vector in the ground coordinate system; where, , This refers to the local gravitational acceleration. The velocity differential equation is solved using the trapezoidal rule: （30） in, To calculate the step size for speed, The ratio is the ratio in the navigation coordinate system at the current moment, where... ; (8) Location update The position differential equation is: （31） in, It is the position differential vector; It is a velocity vector; The position differential equation is also solved using the trapezoidal rule: （32） in, To calculate the step size, , and The following are the current moments of missile guidance in the ground coordinate system. axis, shaft and The position of the axis; , and These are the previous moments of the guided missile in the ground coordinate system. axis, shaft and The position of the axis; , and These are the current moments of the guided missile in the ground coordinate system. axis, shaft and The speed of the shaft; , and The previous moment of the guided missile in the ground coordinate system is shown in the following order. axis, shaft and The speed of the shaft; (9) Apparent velocity update of projectile coordinate system The apparent velocity differential equation in the projectile coordinate system is as follows: （33） in, The apparent velocity in the projectile coordinate system. Let be the acceleration in the projectile coordinate system as sensed by the onboard accelerometer; the apparent velocity in the projectile coordinate system is solved using the trapezoidal rule, as shown in the following formula: （34）。 5. A rapid attitude calculation system for ultra-short-range guided missiles, characterized in that, The system for implementing the method according to any one of claims 1 to 4 comprises: The first module is used to define the coordinate system for the strapdown solution process of ultra-short-range guided missiles and to simplify the navigation coordinate system; The second module is used to establish a human body sway model in the scenario of rapid aiming of shoulder-fired weapons. The third module is a rapid initial alignment of a shoulder-launched ultra-short-range guided missile in a swaying scenario based on a sliding window. The fourth module, based on a simplified navigation coordinate system, enables rapid attitude calculation for strapdown inertial navigation systems used in ultra-short-range guided missiles.

6. A computer device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that, When the processor executes the program, it implements the steps of the method according to any one of claims 1-4.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that, When executed by a processor, the program implements the steps of the method described in any one of claims 1-4.

8. A computer program product, comprising a computer program, characterized in that, When executed by a processor, the computer program implements the steps of the method described in any one of claims 1-4.

Images