Rapid resolving method and system for strapdown attitude of ultra-short-range guided missile

Abstract

The invention discloses an ultra-short-range guided missile strapdown attitude rapid resolving method and system, and the method specifically comprises the steps: firstly, carrying out the lightweight design of an algorithm according to the characteristics that the flight time of an ultra-short-range guided missile is short, and the requirement for the real-time performance of an attitude resolving algorithm is high; a ground system is adopted as a navigation coordinate system to simplify a navigation calculation method under a conventional geographic coordinate system, and the influence of earth rotation angular velocity and earth curvature is ignored; secondly, in order to simulate irregular disturbance generated in the initial aiming process of the shoulder-carried weapon of the human body, a human body shaking model is established; then, in order to further accelerate the initial alignment convergence speed, a shaking scene rapid initial alignment method based on a sliding window is designed, and algorithm verification is carried out based on a human body shaking model; and finally, based on navigation coordinate system simplification and a rapid alignment method based on a sliding window, realizing rapid and high-precision calculation of ultra-short-range missile manufacturing whole-process strapdown inertial navigation.

Description

Technical Field

[0001] The invention belongs to the technical field of inertial navigation, in particular to a strapdown attitude fast solution method and system for an ultra-short-range guided missile. Background Art

[0002] The Strapdown Inertial Navigation System (SINS) is the guarantee for the rapid response of ultra-short-range guided missiles. It uses the motion information of the missile body sensed by the Inertial Measurement Unit (IMU) to achieve navigation and positioning through attitude solution, velocity solution and position solution. Among them, attitude solution is the key link of navigation. Its solution accuracy and speed directly affect the overall navigation effect. Therefore, how to improve the speed of attitude solution is the key to ensure that SINS completes the task on time.

[0003] At present, it is difficult to ensure rapidity in the attitude solution process of ultra-short-range guided missiles because 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, when solving the velocity, the gravity acceleration vector will change with the position, so the gravity acceleration needs to be solved 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 dynamic base problem caused by the irregular motion of the human body in direct-aiming and 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 the present invention is to provide a method and system for rapid strapdown attitude calculation of an ultra-short-range guided missile, which can be applied to the rapid attitude calculation of the ultra-short-range guided missile, and solve the problem that the prior art cannot meet the rapidity and high precision requirements of the initial alignment of the ultra-short-range guided missile under the condition of a shaking base, as well as the problem of rapid attitude acquisition when the flight time is short.

[0006] The present invention is achieved through the following technical solutions. The present invention proposes a method for quickly calculating the strapdown attitude of an ultra-short-range guided missile, and the method specifically includes:

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

[0008] Step 2: Establishing the human body shaking model in the scenario of rapid aiming of shoulder-mounted weapons;

[0009] Step 3: Rapid initial alignment of shoulder-fired ultra-short-range guided missiles in shaking scenarios based on sliding windows;

[0010] Step 4: Rapid attitude solution of strapdown inertial navigation for ultra-short-range guided missile based on simplified navigation coordinate system.

[0011] A strapdown attitude rapid solution system for an ultra-short-range guided missile, used to implement the above method, the system comprising:

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

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

[0014] The third module is the rapid initial alignment of shoulder-fired ultra-short-range guided missiles in shaking scenarios based on sliding windows;

[0015] The fourth module realizes the rapid attitude solution of the strapdown inertial navigation of ultra-short-range guided missiles based on the simplification of the navigation coordinate system.

[0016] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the steps of the above method are implemented when the processor executes the program.

[0017] A computer-readable storage medium stores a computer program, which implements the steps of the above method when executed by a processor.

[0018] A computer program product comprises a computer program, which implements the steps of the above method when executed by a processor.

[0019] Compared with the prior art, the invention has the following beneficial effects: the invention has a lightweight design for an attitude solution algorithm, simplifies the original navigation solution process under the geographic coordinate system into a strapdown inertial navigation solution process based on the ground system according to the characteristics of short flight time and short flight distance of an ultra-short-range guided missile, and effectively improves the real-time performance and reliability of the algorithm; the established human body shaking model can effectively simulate the irregular disturbance generated by a human shoulder-carried weapon in the aiming process; the designed fast initial alignment method for a shaking scene based on a sliding window uses the sliding window to solve the gravity component error in the initial alignment, so that the convergence of the algorithm is judged not by reaching a fixed convergence time, but by using the error mean obtained by the rolling window, thereby greatly improving the convergence speed of the algorithm under the premise of ensuring the convergence accuracy; finally, the designed strapdown inertial navigation attitude solution method for an ultra-short-range guided missile based on the simplification of the navigation coordinate system meets the rapidity requirement of the strapdown attitude solution under the condition of short flight time. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] Figure 1 Schematic diagram of the posture angle curve of the human body swaying model.

[0021] Figure 2 Schematic diagram of the angular velocity curve of the human body sway model.

[0022] Figure 3 Schematic diagram of the acceleration curve of the human body sway model.

[0023] Figure 4 Schematic diagram of the initial alignment process.

[0024] Figure 5 Schematic diagram of the pitch angle curve during the initial alignment process.

[0025] Figure 6 Schematic diagram of the roll angle curve during the initial alignment process.

[0026] Figure 7 Schematic diagram of the yaw angle curve during the initial alignment process.

[0027] Figure 8 Schematic diagram of attitude angle error curve during the initial alignment process.

[0028] Fig. 9 This is a schematic diagram of the strapdown navigation attitude angle solution results.

[0029] Fig.10 This is a schematic diagram of the strapdown navigation speed solution results.

[0030] Fig.11 This is a schematic diagram of the strapdown navigation position solution results.

[0031] Fig.12 Schematic diagram of strapdown navigation attitude angle solution error.

[0032] Fig.13 This is a schematic diagram of speed solution error.

[0033] Fig.14 Schematic diagram of position solution error. DETAILED DESCRIPTION

[0034] In order to ensure the strike accuracy and speed of ultra-short-range guided missiles, the present invention provides a method and system for fast strapdown attitude solution of ultra-short-range guided missiles. The innovation of the present invention is that, firstly, for the ultra-short-range guided missiles, the complexity of solving the angular velocity of the missile system relative to the geographic system in the attitude solution is high, and the gravity acceleration vector changes with the position when solving the speed is complex, and the algorithm is designed to be lightweight: the influence of the earth's rotation angular velocity is ignored, the navigation coordinate system of the commonly used guided missile is changed from the geographic coordinate system to the ground coordinate system, and the angular velocity processing process of the missile system relative to the geographic system is cancelled; the gravity acceleration vector decomposition process caused by the change of position coordinates is ignored, and the gravity acceleration is considered to be unchanged, so as to simplify the navigation coordinate system; secondly, for the strong interference problem caused by the irregular movement of the human body during the initial alignment of the direct-aiming ultra-short-range guided missile mobile base, a human body shaking model is established, and a fast alignment method based on a sliding window is designed; finally, based on the simplification of the navigation coordinate system and the fast alignment method based on the sliding window, the fast and high-precision solution of the strapdown inertial navigation of the ultra-short-range guided missile throughout the process is realized.

[0035] The technical solutions in the embodiments of the present invention will be described clearly and completely below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0036] A method for quickly calculating the strapdown attitude of an ultra-short-range guided missile according to an embodiment of the present invention specifically comprises the following steps:

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

[0038] 1. Definition of coordinate system in strapdown solution process; First, the missile body coordinate system, navigation coordinate system, geographic coordinate system, ground coordinate system, platform coordinate system and initial platform coordinate system involved in the calculation process are defined and explained:

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

[0040] The guided missile mass center O is the coordinate origin, OX b The axis points to the head along the longitudinal axis of the projectile, OY b The axis is in the longitudinal symmetry plane of the guided missile and is aligned with 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] The navigation coordinate system is the coordinate system used by the inertial navigation system when solving navigation parameters. The geographic coordinate system is often 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 axis horizontally points to true north, OX l Horizontal pointing east, OZ l Axis and OX l , OY l The axes form a right-handed coordinate system, with the direction perpendicular to the Earth 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 the 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] The center of mass of the projectile is taken as the coordinate origin, which coincides with the projectile coordinate system before initial alignment. Ideally, the initial platform coordinate system is stable in the inertial space.

[0049] 2. Simplification of navigation coordinate system; navigation coordinate system OX n Y n Z n It is the coordinate system used by the inertial navigation system when solving navigation parameters. The geographic coordinate system is usually used as the navigation coordinate system. In view of the high complexity of solving the angular velocity of the missile system relative to the geographic system when solving the attitude in the geographic coordinate system, and the complex problem of solving the gravity acceleration vector with position changes when solving the velocity, considering the characteristics of short flight time and short flight distance of ultra-short-range guided missiles, the conventional navigation coordinate system is simplified. The calculation equation of the strapdown inertial navigation in the geographic coordinate system (taking the Northeast Sky coordinate system as an example) is as follows:

[0050]

[0051] in, λ,h are latitude, longitude, and altitude respectively; v n =[v e ,v n ,v u ] T , v e ,v n ,v u They are eastward speed, northward speed, and celestial speed, respectively; is the rotation matrix of the missile system relative to the geographic system; D -1 is the velocity vector transformation matrix; f b is the relative strength of the elastic system; g n is the local gravity acceleration vector, which changes with the position of the projectile; is the projection vector of the earth's rotation angular velocity in the geographic coordinate system; It is the angular velocity vector generated by the rotation of the geographic coordinate system relative to the earth; Angular velocity vector for attitude calculation The corresponding antisymmetric matrix is is the angular velocity vector sensed by the gyroscope The corresponding antisymmetric matrix; is the angular velocity vector caused by the rotation of the geographic coordinate system relative to the inertial system The corresponding antisymmetric matrix.

[0052] In attitude solution, the angular velocity vector generated by the rotation of the geographic coordinate system relative to the inertial system Cause angular velocity The complex solution process is the main factor affecting the rapidity of the strapdown inertial navigation solution of ultra-short-range guided missiles. For the angular velocity vector It consists of two parts, namely

[0053] (2) Among them, is the projection of the earth's rotation angular velocity relative to the inertial coordinate system in the projectile coordinate system; is the projection of the direction change rate of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the elastic system. It can be expressed in the form of an antisymmetric matrix:

[0054]

[0055] in, for The corresponding antisymmetric matrix is, ω e is the angular velocity of the Earth's rotation; for The corresponding antisymmetric matrix is, In the formula, R M is the meridian curvature radius, R N is the radius of curvature of the convex circle.

[0056] The above calculation process involves a variety of parameter calculation formulas and matrix operations, which are complex. Due to the requirement of rapid strapdown attitude solution of ultra-short-range guided missiles, it is necessary to simplify the design of the above angular velocity pre-calculation process while ensuring accuracy. First, consider the angular velocity term caused by the change of direction due to the rotation of the coordinate system. Since the meridian radius R M The radius of the circle R N The magnitude is much larger than the eastward velocity component v of the projectile e With the north velocity component v n ,but is a minimum value close to zero, and this term can be ignored. The term describes the influence of the earth's rotation angular velocity on attitude solution. Since the earth's rotation angular velocity is small, its influence on attitude solution is reflected in the flight process with a longer flight time. The flight time of ultra-short-range guided missiles is less than 30 seconds, and the attitude error caused by the change of the earth's rotation angular velocity is extremely small. At the same time, the existing ultra-short-range guided missiles are mostly equipped with micro-small low-cost MEMS sensors, and their device errors are close to the order of magnitude of the earth's rotation angular velocity. The earth's angular velocity component that they are sensitive to is mixed in the measurement error. Therefore, the influence of the earth's rotation angular velocity on ultra-short-range guided missiles can be ignored. Therefore, the attitude solution equation for ultra-short-range guided missiles can be simplified to:

[0057]

[0058] Based on the above assumptions, the velocity update formula in the geographic system is simplified to:

[0059]

[0060] In the formula, g n =[0,0,-g] T is the projection of the gravity acceleration vector in the navigation system, It's about location As a function of , the ultra-short-range guided missile has a short flight distance and a very small range of longitude and latitude, so the change of its gravity acceleration can be ignored. It is considered that the gravity acceleration vector during its flight is a constant vector, and g is the local gravity acceleration.

[0061] The position calculation process of the ultra-short-range guided missile adopts the Cartesian rectangular coordinate system, not the geodetic coordinate system, so its position calculation formula is simplified as follows:

[0062]

[0063] Among them, r car is the expression of the projectile position in the Cartesian coordinate system.

[0064] The simplified calculation equation of the strapdown inertial navigation in the geographic coordinate system (taking the northeast celestial coordinate system as an example) is as follows:

[0065]

[0066] In summary, considering the flight characteristics of ultra-short-range guided missiles and the requirements for navigation solution accuracy, the following simplified assumptions are proposed for the navigation coordinate system under the original geographic system: 1) Ignore the influence of the earth's rotation angular velocity, and assume that the ultra-short-range guided missile strapdown inertial navigation solution process is located in the inertial coordinate system, that is, the earth's rotation angular velocity is not considered. 2) The earth's surface is regarded as a plane, and the gravity acceleration is considered to be a constant during the flight, and finally the navigation coordinate system simplification work is completed.

[0067] Taking into account the direct-aiming launch characteristics of ultra-short-range guided missiles, the following navigation coordinate system suitable for ultra-short-range guided missiles is established in combination with the above assumptions, denoted as the ground coordinate system O d X d Y d Z d , the origin of the coordinate system is O d is the center of mass of the instantaneous guided missile when it is launched, O d X d The axis points to the intersection of the trajectory plane (track plane) and the horizontal plane. The axis points to the target as positive. d Y d The axis is vertically upward, O d Z d Axis and O d X d Y d The plane is perpendicular to the right-hand coordinate system, which is fixed to the earth's surface, stationary relative to the earth, and is approximately an inertial coordinate system during flight. In short-range guided missiles, the earth's surface is regarded as a plane within the range, and the gravity field is a parallel force field, which is parallel to O d Y d Axis parallel, direction along O d Y d Negative axis.

[0068] S2. Establishment of human body sway model in the scenario of rapid aiming of shoulder-mounted weapons

[0069] When a person aims a shoulder-held weapon, there will be a large shake at the beginning. As the shooter's body stabilizes, the amplitude of the shake gradually decreases until it stabilizes, but there is still a slight shake due to factors such as breathing and muscle tremors. In order to simulate a series of irregular disturbances generated during the aiming process, this patent uses a sine function whose amplitude slows down over time and a sine function with a constant amplitude to describe this process. First, a sine function whose amplitude slows down over time is used to describe the process of the shooter gradually stabilizing from the initial large shaking amplitude. Secondly, a sine function with a constant amplitude is used to describe the slight shaking process that still exists after the shooter's body stabilizes. Finally, the above-mentioned sine functions are superimposed to approximate the entire process of irregular shaking of the human body during the initial alignment process.

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

[0071]

[0072] Among them, θ, γ, ψ are the pitch angle, roll angle and yaw angle of the missile in the ground coordinate system respectively; θ Δ , γ Δ , Δ Respectively represent the pitch angle, roll angle and yaw angle after stabilization during shoulder-carrying; θ A1 , γ A1 , A1 are the maximum amplitudes of each posture motion under the sway attenuation motion; α θ , α γ , α ψ is the attenuation coefficient of each attitude angle under the shaking attenuation motion; T θ1 , T γ1 , T ψ1 is the period of change of each attitude angle under the shaking attenuation motion; θ A2 , γ A2 , A2 are the angular amplitudes of each attitude of sinusoidal motion; T θ2 , T γ2 , T ψ2 is the angular period of each posture of sinusoidal motion; t represents the current moment.

[0073] The above model can simulate the missile attitude angle change curve under shaking, and the results are as follows: Figure 1-Figure 3 ,It can be seen from the curve that with the increase of time, the disturbance gradually decreases, and finally basically presents a stable shaking. ,The simulation curve shows that the model can approximately simulate the disturbance process of the human shoulder-mounted weapon during the aiming process, and serve as the ,simulation input of the initial alignment process to verify the performance of the algorithm.

[0074] S3. Rapid initial alignment of shoulder-fired ultra-short-range guided missiles in shaking scenarios based on sliding windows

[0075] Since the ultra-short-range guided missile adopts direct aiming to ensure rapid launch, that is, the guided missile points directly to the target, the initial heading angle is taken as zero, and only the initial pitch angle and initial roll angle are aligned and estimated.

[0076] In the shoulder-carrying shaking scenario, the specific force measured by the accelerometer is is the gravitational acceleration vector g n and disturbance acceleration The resultant force, the disturbance acceleration Mainly comes from the irregular shaking of the human body. In order to effectively deal with the interference acceleration caused by the shaking of the human body during the initial alignment process Influence, first establish the initial platform coordinate system at the start time of initial alignment The relative force measured at each time In the initial platform coordinate system The average value is taken under the condition of human body shaking gradually tending to a stable state, and the interference acceleration is calculated by taking the average value over a period of time. To suppress.

[0077] The specific force in the initial platform coordinate system at time t is The measured specific force is The conversion relationship between the two is:

[0078]

[0079] in, is 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 quaternion method is used to solve the attitude matrix at any time. The rotation of the missile body coordinate system relative to the initial platform coordinate system is expressed using quaternions, that is,

[0080]

[0081] Among them, i b ,j b ,k b is the unit vector of the three axes of the b system. At the beginning of the initial alignment, i.e. t = 0, since the initial platform coordinate system coincides with the missile body coordinate system, the initial quaternion Q(0) = [1, 0, 0, 0] T , the differential equation of the variable Q is:

[0082]

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

[0084] Rewritten in matrix form:

[0085]

[0086] Using the fourth-order Runge-Kutta update of the above differential equation, the calculation form is as follows:

[0087]

[0088] Where Δt is the discrete time interval, and the updated quaternion is used to solve the current attitude matrix.

[0089]

[0090] Then find the rotation matrix of the projectile system relative to the initial platform coordinate system at each moment

[0091] Using the rotation matrix at each moment The calculated N wd +1 acceleration projection in the initial platform coordinates Perform arithmetic averaging to obtain the average specific force in the initial platform coordinate system:

[0092]

[0093] Among them, N wd is the number of data frames in the sliding window, is the acceleration projection at time k, Ideally, if there is no shaking interference acceleration, the specific force in the initial platform coordinate system should be the local gravity acceleration vector g n , let the gravity acceleration vector g in the ground coordinate system n =[0,-g0,0] T , g0 is the local gravitational acceleration (constant value). Then With g n There are the following relationships:

[0094]

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

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

[0097]

[0098] According to the definition of the platform coordinate system, Ox p y pThe plane is parallel to the Axy plane, Axis in Ox p y p Projection on the coordinate plane and Ox p The axis coincides. Using the relationship between the rotation matrix and the attitude angle, the rotation matrix from the initial platform coordinate system to the platform coordinate system is obtained. for:

[0099]

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

[0101]

[0102] In order to speed up the convergence of transfer alignment, the posture matrix at each moment During the solution process, a sliding window is established and the acceleration error within the window is calculated:

[0103]

[0104] Among them, N wd is the number of data frames in the sliding window; M wd is the number of data frames between two adjacent sliding windows; k is the current time. wd +2N wd , since the time does not reach the sliding window size, the error is not calculated.

[0105] When the above error value continues N sum The windows are all smaller than the set error value S err ,Right now

[0106]

[0107] It is considered that the error has converged, and thus according to equation (7), the strapdown matrix from b system to p system is calculated: At this point, the initial alignment is basically completed. The overall flow chart of the initial alignment is as follows: Figure 4 The initial alignment result is as follows: Figure 5-Figure 8 .

[0108] S4. Fast attitude calculation of strapdown inertial navigation system for ultra-short-range guided missiles with simplified navigation coordinate system

[0109] After the guided missile is launched, the simplified ground coordinate system (7) in step S1 is used as the navigation coordinate system for strapdown inertial navigation solution. In terms of the solution method, the gyro measurement value directly participates in the quaternion solution attitude, without considering the angular velocity such as the earth's rotation speed. The processing of the accelerometer data considers eliminating harmful acceleration, and then performs coordinate transformation and eliminates the influence of gravity to obtain the position vector of the missile body. The initial solution is to use the alignment result of the initial alignment algorithm of the shaking scene introduced in step S3 as the initial value of the inertial navigation solution.

[0110] The strapdown inertial navigation solution process of ultra-short-range guided missiles is divided into three stages: attitude matrix update, velocity update, and position update. The strapdown inertial navigation attitude update method of ultra-short-range guided missiles adopts the quaternion method. Figure 6 The detailed steps are described below.

[0111] (1) Quaternion initialization

[0112]

[0113] in, is the quaternion component calculated according to the initial attitude angle, θ0, γ0, ψ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] There are rounding errors in the floating-point calculation process of the navigation computer, which makes the updated quaternion norm non-unique. In order to ensure the orthogonality of the attitude matrix, the quaternion is normalized as follows:

[0116]

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

[0118]

[0119] (3) Constructing the initial posture matrix

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

[0121]

[0122] (4) Obtaining angular velocity

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

[0124]

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

[0126] (5) Quaternion update

[0127] Quaternion differential equations The fourth-order Runge-Kutta method is used to solve:

[0128]

[0129] Among them, Δt is the solution step size; Q(t-Δt) is the quaternion at the previous moment; Ω(t-Δt) is the antisymmetric matrix of angular velocity at the previous moment; is the average value of the angular velocity at the previous moment and the current moment,

[0130]

[0131] in, It is the angular velocity of three axes of the inertial navigation sensitive projectile system.

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

[0133] (6) Posture angle extraction

[0134] Use the attitude matrix to get the attitude angle:

[0135]

[0136] Among them, the pitch angle value range is [-90°, 90°], the roll angle value range is [-90°, 90°], and the heading angle value range is [-180°, 180°].

[0137] (7) Speed ​​update

[0138] The velocity differential equation is:

[0139]

[0140] in, is the velocity differential vector of the navigation system; f b g is the specific force that the missile-borne accelerometer is sensitive to; n is the gravitational acceleration vector in the ground coordinate system. n =[0,-g0,0] T , g0 is the local gravitational acceleration, which is considered to be a constant during flight.

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

[0142]

[0143] Among them, Δt is the speed solution step, f n (t) is the specific force 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 is the velocity vector.

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

[0150]

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

[0152] (9) Update of apparent velocity of missile system

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

[0154]

[0155] in, is the apparent velocity of the projectile system, f b is the acceleration of the missile system sensed by the missile-loaded accelerometer. The apparent velocity of the missile system is solved using the trapezoidal method, and the formula is as follows:

[0156]

[0157] The present invention is described in detail below in conjunction with embodiments and drawings.

[0158] Based on the same inventive concept, the present invention also provides a strapdown attitude rapid solution system for an ultra-short-range guided missile, which is used to implement the above method. The system comprises:

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

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

[0161] The third module is the rapid initial alignment of shoulder-fired ultra-short-range guided missiles in shaking scenarios based on sliding windows;

[0162] The fourth module realizes the rapid attitude solution of the strapdown inertial navigation of ultra-short-range guided missiles based on the simplification of the navigation coordinate system.

[0163] The specific implementation methods of the above modules are the same as the specific steps of the aforementioned ultra-short-range guided missile strapdown attitude rapid solution method, which will not be repeated here.

[0164] Example

[0165] The present invention is further described below in conjunction with specific embodiments, but the present invention is not limited by the embodiments.

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

[0167] Assume that the latitude, longitude and altitude of the launch point are (32.0615513°, 118.7915619°, 54m), the alignment simulation period is 100 seconds, and the simulation experiment parameters are given as shown in Table 1 according to the three-axis attitude angle human body sway formula. The generated human body sway simulation image is shown in Figure 1-Figure 3 shown.

[0168] Depend on Figure 1-Figure 3 It can be seen that the human body shaking simulation curve oscillates first and then gradually stabilizes. This process is close to the real human body shaking posture aiming process.

[0169] Table 1 Human body sway simulation parameters

[0170]

[0171] 2) Initial alignment simulation

[0172] The data after the human body sway model simulation is used as the true value of the initial alignment input data. Considering that the MEMS inertial group sensitive components carried by the ultra-short-range guided missile have large noise, it is necessary to add the corresponding device noise to the simulated true value as the initial alignment measurement value data. The device noise parameters are shown in the following table.

[0173] Table 2 Initial alignment results

[0174]

[0175] The data after adding device noise is used for initial posture alignment, where the sliding window size is N wd =50, the alignment end period is N sum =10, the error accuracy is set to S err =4×10 -8 m / s 2 The initial alignment result under this simulation setting is as follows Figure 5-Figure 8 As shown in Table 3. The initial alignment time is 1.335 seconds, which meets the requirements of initial alignment speed. From Table 3, it can be seen that the overall error is less than 0.07°, the alignment accuracy is high, and it meets the guidance requirements of ultra-short-range strapdown missiles. Figure 5-Figure 7 It can be seen that the predicted value and the standard value of the attitude angle maintain the same change trend. Figure 8 It can be seen intuitively that the error curve shows a trend of small-range fluctuations and remains basically stable.

[0176] Table 3 Initial alignment results

[0177]

[0178]

[0179] To further verify the reliability of the method, 1000 Monte Carlo test simulations were carried out under the above test parameter settings, and the simulation results are shown in Table 4. The simulation results show that the mean of the three-axis attitude angle alignment error is basically consistent with the alignment error in Table 3. The mean square error of the multiple simulation test results shows that the alignment errors of multiple tests are basically around the mean error, which illustrates the stability of the initial alignment algorithm. The mean alignment time of multiple tests is less than 2s, and the mean square error of the alignment time is small, indicating that under this parameter setting, 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 shaking conditions.

[0180] Table 4 Initial alignment results (Monte Carlo test)

[0181]

[0182] 3) Rapid posture calculation and simulation

[0183] Based on the initial alignment algorithm solution, the ballistic simulation data is used to verify the ultra-short-range guided missile strapdown inertial navigation fast attitude solution algorithm. The fast attitude solution algorithm verification uses the same device noise parameters as the initial alignment algorithm verification. The strapdown navigation solution cycle is 0.005s. The initial alignment algorithm results are considered in the solution process, and the initial attitude angle is set to γ0=0°+Δγ, ψ0=0°+Δψ. in Δγ and Δψ are the alignment posture errors after initial alignment. According to the mean value of the initial alignment error, Δγ=0.06407°, Δψ=0.00415°.

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

[0185] Table 5 Simulation hardware parameters

[0186]

[0187] The strapdown attitude solution result of the ultra-short-range guided missile is as follows Figure 9-14 As shown. Fig. 9 It can be seen that the three-axis attitude solution error is basically around the initial alignment error. Except for the roll angle error showing a decreasing divergence trend, the pitch angle and yaw angle solution errors have both shown error divergence, but overall the attitude error is small, the pitch angle solution error is less than 0.046°, the roll angle solution error is less than 0.065°, and the yaw angle solution error is less than 0.025°, which meets the attitude solution accuracy requirements during the flight process; the speed solution errors of the X-axis and Y-axis are small, with the maximum error less than 0.05m / s, and the Y-axis speed diverges, with the maximum error less than 0.2m / s; from Fig.11 It can be seen that the position solution errors of the X-axis and Y-axis are small, with the maximum error less than 0.4m, and the Y-axis position diverges, with the maximum error less than 1.5m.

[0188] During the simulation process, the algorithm solved 1822 steps in total, with a total solution time of 0.05707 seconds and a single-step solution time of 0.00003 seconds. Compared with the conventional geographic strapdown attitude solution algorithm, this algorithm greatly shortens the navigation solution time while ensuring the solution accuracy, providing a better algorithm environment for the algorithm to be deployed in low-cost and lightweight components, and greatly meeting the requirements for rapid attitude solution of ultra-short-range guided missiles.

Claims

1. A method for fast solving the strapdown attitude of an ultra-short-range guided missile, characterized in that: The following steps are involved: Step 1: Define the coordinate system of the strapdown solution process for ultra-short-range guided missiles and simplify the navigation coordinate system; Step 2: Establish a human body shaking model in the scenario of rapid aiming of shoulder-mounted weapons; Step 3: Rapid initial alignment of shoulder-fired ultra-short-range guided missiles in shaking scenarios based on sliding windows; Step 4: Based on the simplification of the navigation coordinate system, the fast attitude solution of the strapdown inertial navigation of the ultra-short-range guided missile is realized.

2. The strapdown attitude rapid solution method for an ultra-short-range guided missile according to claim 1 is characterized in that: Define the coordinate systems in the strapdown solution process, including the missile body coordinate system, navigation coordinate system, geographic coordinate system, ground coordinate system, platform coordinate system and initial platform coordinate system: (1) Projectile coordinate system OX b Y b Z b The guided missile mass center O is the coordinate origin, OX b The axis points to the head along the longitudinal axis of the projectile, OY b The axis is in the longitudinal symmetry plane of the guided missile and is aligned with OX b Vertical, upward is positive, OZ b With OX b Y b The faces are perpendicular and form a right-handed coordinate system; (2) Navigation coordinate system OX n Y n Z n The navigation coordinate system is the coordinate system used by the inertial navigation system when solving navigation parameters; the geographic coordinate system is used as the navigation coordinate system; (3) Geographic coordinate system OX l Y l Z l The origin of the geographic coordinate system is the center of mass of the projectile, OY l The axis horizontally points to true north, OX l Horizontal pointing east, OZ l Axis and OX l , OY l The axes form a right-handed coordinate system, with the direction perpendicular to the Earth ellipsoid; (4) Platform coordinate system OX p Y p Z p 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; (5) Initial platform coordinate system OX p Y p Z p The center of mass of the projectile is taken as the coordinate origin, which coincides with the coordinate system of the projectile before initial alignment.

3. The strapdown attitude rapid solution method for an ultra-short-range guided missile according to claim 2 is characterized in that: Simplify the navigation coordinate system as follows: Navigation coordinate system OX n Y n Z n It is the coordinate system used by the inertial navigation system when solving navigation parameters. The strapdown inertial navigation calculation equation in the geographic coordinate system is as follows: in, λ,h are latitude, longitude, and altitude respectively; v n =[v e ,v n ,v u ] T , v e ,v n ,v u They are eastward speed, northward speed, and celestial speed, respectively; is the rotation matrix of the missile system relative to the geographic system; D -1 is the velocity vector transformation matrix; f b is the relative strength of the elastic system; g n is the local gravity acceleration vector, which changes with the position of the projectile; is the projection vector of the earth's rotation angular velocity in the geographic coordinate system; It is the angular velocity vector generated by the rotation of the geographic coordinate system relative to the earth; Angular velocity vector for attitude calculation The corresponding antisymmetric matrix is is the angular velocity vector sensed by the gyroscope The corresponding antisymmetric matrix; is the angular velocity vector caused by the rotation of the geographic coordinate system relative to the inertial system The corresponding antisymmetric matrix; because For the angular velocity vector It consists of two parts, namely in, is the projection of the earth's rotation angular velocity relative to the inertial coordinate system in the projectile coordinate system; is the projection of the direction change rate of the geographic coordinate system relative to the Earth-centered Earth-fixed coordinate system in the projectile system; it can be expressed in the form of an antisymmetric matrix: in, for The corresponding antisymmetric matrix is, ω e is the angular velocity of the Earth's rotation; for The corresponding antisymmetric matrix is, In the formula, R M is the meridian curvature radius, R N is the radius of curvature of the Maoyou circle; The attitude solution equation for ultra-short-range guided missiles is simplified to: Based on the above assumptions, the velocity update formula in the geographic system is simplified to: In the formula, g n =[0,0,-g] T is the projection of the gravity acceleration vector in the navigation system, It's about location function, g is the local gravitational acceleration; The position calculation process of the ultra-short-range guided missile adopts the Cartesian rectangular coordinate system, not the geodetic coordinate system, so its position calculation formula is simplified as follows: Among them, r car is the expression of the projectile position in the Cartesian coordinate system; The simplified calculation equation of strapdown inertial navigation in geographic coordinate system is as follows: The following simplified assumptions are proposed for the navigation coordinate system under the original geographic system: 1) Ignore the influence of the earth's rotation angular velocity, and assume that the strapdown inertial navigation solution process 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) The earth's surface is regarded as a plane, and the gravity acceleration is considered to be a constant during the flight, and finally the navigation coordinate system simplification work is completed; Combined with 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 of the coordinate system is O d is the center of mass of the instantaneous guided missile when it is launched, O d X d The axis points to the intersection of the ballistic plane and the horizontal plane, pointing to the target is positive, O d Y d The axis is vertically upward, O d Z d Axis and O d X d Y d The plane is perpendicular to the right-hand coordinate system, which is fixed to the surface of the earth, is stationary relative to the earth, and is approximately an inertial coordinate system during flight. In short-range guided missiles, the surface of the earth is regarded as a plane within the range, and the gravity field is a parallel force field, which is parallel to O. d Y d Axis parallel, direction along O d Y d Negative axis.

4. The strapdown attitude rapid solution method for an ultra-short-range guided missile according to claim 3 is characterized in that: Establish a human body shaking model in the scenario of rapid aiming of shoulder-mounted weapons, specifically: A sine function with a decreasing amplitude over time is used to describe the process of the shooter gradually stabilizing from the initial shaking, and a sine function with a constant amplitude is used to describe the shaking process that still exists after the shooter's body is stable. Finally, the above sine functions are superimposed to approximate the entire process of irregular shaking of the human body during the initial alignment process. The established three-axis posture angle human body sway model is as follows: Among them, θ, γ, ψ are the pitch angle, roll angle and yaw angle of the missile in the ground coordinate system respectively; θ Δ , γ Δ , Δ Respectively represent the pitch angle, roll angle and yaw angle after stabilization during shoulder-carrying; θ A1 , γ A1 , A1 are the maximum amplitudes of each posture motion under the sway attenuation motion; α θ , α γ , α ψ is the attenuation coefficient of each attitude angle under the shaking attenuation motion; T θ1 , T γ1 , T ψ1 is the period of change of each attitude angle under the shaking attenuation motion; θ A2 , γ A2 , A2 are the angular amplitudes of each attitude of sinusoidal motion; T θ2 , T γ2 , T ψ2 is the angular period of each posture of sinusoidal motion; t represents the current moment.

5. The strapdown attitude rapid solution method for an ultra-short-range guided missile according to claim 4 is characterized in that: The fast initial alignment of shoulder-fired ultra-short-range guided missiles in shaking scenarios based on sliding windows is as follows: The initial heading angle is taken as zero, and only the initial pitch angle and initial roll angle are used for alignment estimation; In the shoulder-carrying shaking scenario, the specific force measured by the accelerometer is is the gravitational acceleration vector g n and disturbance acceleration The resultant force, the disturbance acceleration From the irregular shaking process of the human body; the initial platform coordinate system is established at the beginning of the initial alignment The relative force measured at each time In the initial platform coordinate system The average value is taken under the condition of human body shaking gradually tending to a stable state, and the interference acceleration is calculated by taking the average value over a period of time. To suppress; The specific force in the initial platform coordinate system at time t is The measured specific force is The conversion relationship between the two is: in, is 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 quaternion method is used to solve the attitude matrix at any time; the rotation of the missile coordinate system relative to the initial platform coordinate system is expressed using quaternions, that is, Among them, i b ,j b ,k b is the unit vector of the three axes of the b system; at the beginning of the initial alignment, i.e. t = 0, since the initial platform coordinate system coincides with the missile body coordinate system, the initial quaternion Q(0) = [1, 0, 0, 0] T , the differential equation of the variable Q is: in, is the angular velocity vector of the carrier coordinate system; Rewritten in matrix form: Using the fourth-order Runge-Kutta update of the above differential equation, the calculation form is as follows: Where Δt is the discrete time interval, and the updated quaternion is used to solve the current attitude matrix. Then find the rotation matrix of the projectile system relative to the initial platform coordinate system at each moment Using the rotation matrix at each moment The calculated N wd +1 acceleration projection in the initial platform coordinates Perform arithmetic averaging to obtain the average specific force in the initial platform coordinate system: Among them, N wd is the number of data frames in the sliding window, is the acceleration projection at time k, Ideally, if there is no shaking interference acceleration, the specific force in the initial platform coordinate system should be the local gravity acceleration vector g n , let the gravity acceleration vector g in the ground coordinate system n =[0,-g0,0] T , g0 is the local gravitational acceleration; then With g n There are the following relationships: In the formula is the rotation matrix of the ground coordinate system relative to the initial platform coordinate system; θ, γ, ψ are the pitch angle, roll angle and yaw angle of the initial platform coordinate system relative to the platform coordinate system respectively; Assuming that the initial heading angle ψ = 0, the initial pitch angle and the initial roll angle are: According to the definition of the platform coordinate system, Ox p y p The plane is parallel to the Axy plane, Axis in Ox p y p Projection on the coordinate plane and Ox p Axis coincidence; using the relationship between the rotation matrix and the attitude angle, the rotation matrix from the initial platform coordinate system to the platform coordinate system is obtained for: Then use and the launch time t n of Establishing the initial strapdown matrix At each moment, the posture matrix During the solution process, a sliding window is established and the acceleration error within the window is calculated: Among them, N wd is the number of data frames in the sliding window; M wd is the number of data frames between two adjacent sliding windows; k is the current time; when k≤M wd +2N wd When , the error is not calculated because the time does not reach the sliding window size; When the above error value continues N sum The windows are all smaller than the set error value S err ,Right now It is considered that the error has converged, and thus the strapdown matrix from b system to p system is calculated according to equation (7): At this point, the initial alignment is completed.

6. The strapdown attitude rapid solution method for an ultra-short-range guided missile according to claim 5 is characterized in that: The fast attitude solution of strapdown inertial navigation for ultra-short-range guided missiles with simplified navigation coordinate system is as follows: After the guided missile is launched, the simplified ground coordinate system (7) is used as the navigation coordinate system for strapdown inertial navigation solution; the gyro measurement value is directly involved in the quaternion solution of attitude; At the beginning of the solution, the alignment result of the initial alignment algorithm of the step-shaking scene is used as the initial value of the inertial navigation solution; The strapdown inertial navigation solution process of ultra-short-range guided missiles is divided into three stages: attitude matrix update, velocity update, and position update; The strapdown inertial navigation attitude update method for ultra-short-range guided missiles adopts the quaternion method; (1) Quaternion initialization in, is the quaternion component calculated according to the initial attitude angle, θ0, γ0, ψ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; (2) Quaternion Normalization There are rounding errors in the floating-point calculation process of the navigation computer, which makes the updated quaternion norm non-unique. In order to ensure the orthogonality of the attitude matrix, the quaternion is normalized as follows: Where Q = [q0, q1, q2, q3] T ,Right now (3) Constructing the initial posture matrix Calculate the attitude matrix using the initial quaternion: (4) Obtaining angular velocity The angular velocity required to calculate the attitude angle is: in, The angular velocity to which the missile-borne gyro is sensitive; (5) Quaternion update Quaternion differential equations The fourth-order Runge-Kutta method is used to solve: Among them, Δt is the solution step size; Q(t-Δt) is the quaternion at the previous moment; Ω(t-Δt) is the antisymmetric matrix of angular velocity at the previous moment; is the average value of the angular velocity at the previous moment and the current moment, in, is the angular velocity of the three axes of the inertial navigation sensitive projectile system; After the solution is completed, the quaternion is normalized using formula (23), and the updated attitude matrix of the quaternion is obtained using formula (25); (6) Posture angle extraction Use the attitude matrix to get the attitude angle: Among them, the pitch angle value range is [-90°, 90°], the roll angle value range is [-90°, 90°], and the heading angle value range is [-180°, 180°]; (7) Speed ​​update The velocity differential equation is: in, is the velocity differential vector of the navigation system; f b g is the specific force that the missile-borne accelerometer is sensitive to; n is the gravitational acceleration vector in the ground coordinate system; where g n =[0,-g0,0] T , g0 is the local gravitational acceleration; The velocity differential equation is solved using the trapezoidal method: Among them, Δt is the speed solution step, f n (t) is the specific force in the navigation coordinate system at the current moment, where (8) Location Update The position differential equation is: in, V is the position differential vector; n =[V x ,V y ,V z ] T is the velocity vector; The position differential equation is also solved using the trapezoidal method: Where Δt is the solution step size, X(t), Y(t) and Z(t) are the positions of the X-axis, Y-axis and Z-axis of the ground-based guided missile at the current moment respectively; X(t-Δt), Y(t-Δt) and Z(t-Δt) are the positions of the X-axis, Y-axis and Z-axis of the ground-based guided missile at the previous moment respectively; V x (t), V y (t) and V z (t) are the speeds of the ground-based guided missile on the X, Y and Z axes at the current moment; V x (t-Δt), V y (t-Δt) and V z (t-Δt) are the speeds of the X-axis, Y-axis and Z-axis of the ground-based guided missile at the last moment; (9) Update of apparent velocity of missile system The differential equation of the apparent velocity of the projectile system is as follows: in, is the apparent velocity of the projectile system, f b is the acceleration of the missile system sensed by the missile-loaded accelerometer; the apparent velocity of the missile system is solved by the trapezoidal method, and the formula is as follows:

7. A strapdown attitude rapid solution system for ultra-short-range guided missiles, characterized in that: For implementing the method described in any one of claims 1 to 6, the system comprises: The first module is used to define the coordinate system of the strapdown solution process of the ultra-short-range guided missile and simplify the navigation coordinate system; The second module is used to establish a human body shaking model in the scenario of rapid aiming of shoulder-fired weapons; The third module is the rapid initial alignment of shoulder-fired ultra-short-range guided missiles in shaking scenarios based on sliding windows; The fourth module realizes the rapid attitude solution of the strapdown inertial navigation of ultra-short-range guided missiles based on the simplification of the navigation coordinate system.

8. 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, the steps of the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the method described in any one of claims 1 to 6 are implemented.

10. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images