A method for retrospective correction and alignment of a swaying base of a launch system

Abstract

The invention discloses a method for backtracking correction alignment of a swaying base of a launch system, which relates to the field of aircraft navigation. The invention adopts a forward calculation-reverse backtracking-forward pursuit strategy for backtracking calculation, and each calculation adopts different algorithms and constraints. The initial calculation adopts constraints with stronger effects but not strictly satisfied, so that the system converges quickly. Subsequent backtracking and forward pursuit calculations adopt constraints with weaker effects but more strictly satisfied, so that the system achieves higher convergence accuracy. Finally, the shaking interference is effectively isolated in a shorter alignment time, so that the system fully converges.

Description

Technical Field

[0001] The invention relates to the field of aircraft navigation, and in particular to a method for backtracking correction and alignment of a rocking base of a launch system. Background Art

[0002] Initial alignment is one of the important links in the work of inertial navigation system. Inertial navigation system usually needs to obtain accurate and reliable initial attitude value in the initial alignment link to ensure that the system can maintain high accuracy after entering the inertial navigation state. The inertial navigation system has the ability of self-alignment. Its principle is to measure the angular velocity of the earth's rotation through a gyroscope, measure the earth's gravity through an accelerometer, use the earth's gravity to indicate the horizontal direction, and use the earth's rotation angular velocity to indicate the north direction to determine its horizontal attitude and orientation. The initial alignment measurement value of the inertial device is disturbed, which will affect the accuracy of the alignment. Among them, the shaking from the outside makes it impossible to separate the effective earth rotation and gravity observations from the gyroscope and accelerometer, so that the alignment is disturbed. For vehicle-mounted inertial navigation or vehicle-launched aircraft inertial navigation, gusts, personnel activities on the vehicle, low-frequency vibration of the engine idling, and wave shaking of ship-mounted inertial navigation are all shaking interference. The shaking interference can be divided into angular shaking and linear shaking in terms of manifestation. Angular shaking is the change of posture, while linear shaking is the movement of the center of mass. Angular shaking causes changes in the angular velocity measurement value, while linear shaking causes changes in the acceleration measurement value. When angular shaking and linear shaking exist at the same time, the value read by the gyroscope is the superposition of the angular velocity of the earth's rotation and the shaking interference, and the shaking angular velocity is much greater than the angular velocity of the earth's rotation. The value read by the gyroscope is the superposition of the linear motion acceleration and the earth's gravity. The two are often at the same order of magnitude, and there is considerable interference.

[0003] Due to the randomness of external interference, sway base alignment has always been one of the research directions of inertial navigation alignment. Filtering or integrating the output signal of the inertial measurement unit to isolate the sway interference is a common method for sway base alignment. These methods use some constraints, such as zero speed, fixed position, wireless acceleration, etc. Although these constraints are often not strictly met, alignment cannot be performed without these constraints. Summary of the invention

[0004] In view of the above-mentioned deficiencies in the prior art, the present invention provides a method for retrospective correction and alignment of a rocking base of a launch system.

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

[0006] A method for backtracking correction alignment of a rocking base of a launch system comprises the following steps:

[0007] S1, start the inertial navigation system, wait for the inertial navigation data to be output after initialization is completed, and record an output stable time t0;

[0008] S2, use the rocking base coarse alignment method to perform real-time alignment, and record the alignment end time t1;

[0009] S3, binding the rough alignment result as the initial attitude value of the reverse inertial navigation and establishing a reverse backtracking Kalman filter, using the reverse inertial navigation and Kalman filter algorithm model, adopting the reverse zero speed correction method to correct from t1 to t0 and calculate the first correction parameter;

[0010] S4, binding the reverse zero speed correction result as the initial attitude value of the forward pursuit inertial navigation and establishing a forward pursuit Kalman filter, using the forward inertial navigation and Kalman filter algorithm model, adopting the fixed position correction method to perform correction from t0 to t1 and calculate the second correction parameter;

[0011] S5. Perform catch-up calculation based on the first correction parameter and the second correction parameter until the Kalman filter time is synchronized with the inertial navigation system and the filter correction is completed.

[0012] Furthermore, the S2 specifically includes the following steps:

[0013] S21, integrating the accelerometer measurement value and the gravity vector in the solidification coordinate system at the initial moment of alignment;

[0014] S22, updating the time-varying matrix each time the gyroscope outputs an angle increment, and taking the accelerometer measurement values ​​at different times and the integral of the gravity vector in the solidified coordinate system to calculate the attitude matrix estimate;

[0015] S23. Calculate the attitude matrix in the transmission coordinate system according to the calculated attitude matrix estimate.

[0016] Furthermore, the specific method of updating the time-varying matrix in S22 is:

[0017]

[0018] In the formula, is a time-varying matrix, It is the three-axis components of the angular increment output by the gyroscope.

[0019] Furthermore, the specific calculation method of the attitude matrix estimation in S23 is:

[0020]

[0021] In the formula, is the estimator of the attitude matrix, is the integral of the gravity vector in the solidification coordinate system at the first moment, is the integral of the accelerometer measurement value at the first moment; is the integral of the gravity vector in the solidification coordinate system at the second moment, is the integral of the accelerometer measurement value at the second moment, and T is the matrix transpose.

[0022] Further, the first correction parameter in S3 includes attitude reverse update, velocity reverse update and position reverse update, wherein:

[0023] The specific method of posture reverse update is:

[0024]

[0025] in, , are the attitude quaternions of the launch system at time k and k-1 respectively, is the rotation quaternion of the earth's rotation from time k-1 to time k, is the rotation of the carrier relative to the system at time k between time k and time k-1;

[0026] The specific method of speed reverse update is:

[0027]

[0028] in, , are the velocities of the launch system at time k and k-1 respectively, is the specific velocity increment, is the gravity velocity increment;

[0029] The specific method of reverse position update is:

[0030]

[0031] in , are the positions of the transmitting system at time k and k-1 respectively, is the position increment.

[0032] Furthermore, the second correction parameter in S4 includes attitude forward update, velocity forward update and position forward update, wherein:

[0033] The specific method of attitude positive update is:

[0034]

[0035] In the formula, , are the attitude quaternions of the launch system at time k and k-1 respectively, is the rotation quaternion of the earth's rotation from time k-1 to time k, is the rotation of the carrier relative to the system at time k between time k and time k-1;

[0036] The specific method of speed positive update is:

[0037]

[0038] in, , are the velocities of the launch system at time k and k-1 respectively, is the specific velocity increment, is the gravity velocity increment;

[0039] The specific method of positive position update is:

[0040]

[0041] in , are the positions of the transmitting system at time k and k-1 respectively, is the position increment.

[0042] Furthermore, the specific method of the catch-up calculation in S5 is:

[0043] S51, using the Kalman filter algorithm model in steps S3 and S4 as the correction model, and adding the position variable to its state quantity, expressed as:

[0044]

[0045] in is the state variable, is the misalignment angle, is the speed error, is the position error, is the gyro bias, To add zero bias;

[0046] S52, rewrite the state equation, measurement equation and observation quantity of the correction model with position variables added in S51, where:

[0047] The state equation is rewritten as:

[0048]

[0049] is the attitude matrix, is the angular velocity of the Earth's rotation in the transmitting system, is the attitude matrix, is the angular velocity of the Earth's rotation in the transmitting system, is the gyroscope white noise, is the accelerometer white noise; Output by the meter Calculated, expressed as:

[0050]

[0051] is the accelerometer measurement value;

[0052] The measurement equation is rewritten as:

[0053]

[0054] In the formula ， For size The zero matrix, for The zero matrix, For size The unit array of

[0055] The observed quantity is rewritten as:

[0056]

[0057] is the launch position output by the inertial navigation system;

[0058] S53, based on the revised model rewritten in S52, forward calculation is performed to catch up with the real-time output of the inertial navigation system, and the inertial navigation system is corrected to complete the alignment.

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

[0060] The present invention proposes a retrospective alignment method for a sway base of a launch system. Through a retrospective filtering framework, three basic alignment methods of a sway base of a launch system, namely, coarse alignment, zero-speed correction, and fixed-position correction, are integrated. The algorithms are executed sequentially in a manner of decreasing convergence speed and increasing convergence accuracy. At the same time, the alignment result of the previous algorithm is used as prior information of the next alignment algorithm, and the alignment result is corrected twice to achieve the purpose of improving the alignment accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 It is a schematic flow chart of the swing base backtracking correction alignment method of the launch system of the present invention. DETAILED DESCRIPTION

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

[0063] A method for tracing back and correcting the alignment of a rocking base of a launch system, such as Figure 1 As shown, the following steps are included:

[0064] S1, start the inertial navigation system, wait for the inertial navigation data to be output after initialization is completed, and record an output stable time t1;

[0065] The algorithm structure of calculation + backtracking + catching up determines that the alignment process needs to be processed three times, ①, ②, and ③, and different alignment algorithms can be used in the processing. For the collected data information, first use algorithm ① to perform a forward calculation, and use the reverse navigation algorithm of algorithm ② to reversely solve the system state from the end time t1 of the rough alignment to the start time t0 of the alignment in a short time. At the same time, the attitude information estimated by the reverse Kalman filter backtracking is combined with the known speed and position information at the starting point to correct the inertial navigation, complete the reverse fine alignment process and obtain attitude information with a certain accuracy. Then use algorithm ③ to perform fast forward combined navigation to make the system state catch up with the normal real-time calculation until the end time t2 of the alignment, complete the alignment process and obtain high-precision attitude and position information.

[0066] Considering that the coarse alignment of the solidified coordinate system swaying base adopts the assumption of wireless acceleration, the zero speed correction adopts the assumption of zero speed, and the fixed position correction adopts the assumption of fixed position, the acceleration is integrated to obtain the speed, and then the position is integrated, then the solidified coordinate system method has the fastest convergence speed, and is most affected by the first motion interference, followed by zero speed, and then fixed position. Relaxing the corrections in sequence can ensure rapid convergence in the early stage and isolation of interference in the later stage. Therefore, processes ①, ②, and ③ use coarse alignment of the solidified coordinate system, zero speed correction, and fixed position correction in sequence.

[0067] S2, use the rocking base coarse alignment method to perform real-time alignment, and record the alignment end time t0;

[0068] Process ① uses the launch system swing rough alignment method and the wireless acceleration assumption to perform the first forward calculation.

[0069] The initial moment of the rocking base alignment process is , the end time is , and record the intermediate time as Integrate the equation during the initial alignment process to obtain the following formula:

[0070] (1)

[0071] in is the added measurement value, is the gravity vector in the solidification coordinate system The calculation method is

[0072] (2)

[0073] in is the local gravity.

[0074] is a time-varying matrix, which is a unit matrix at t=0 , each time the gyroscope outputs an angular increment , Make an update:

[0075]

[0076] Pick get Corresponding , ,Pick get Corresponding , According to the dual vector pose determination algorithm shown in Eq. ,have

[0077] (3)

[0078] Now calculate the attitude matrix in the launch coordinate system .

[0079] (4)

[0080] in for The corresponding value at the moment, Given by (3), The calculation formula is as follows:

[0081] (5)

[0082] in is the launch azimuth, is the local latitude.

[0083] S3, binding the rough alignment result as the initial attitude value of the reverse inertial navigation and establishing a reverse backtracking Kalman filter, using the reverse inertial navigation and Kalman filter algorithm model, adopting the reverse zero speed correction method to correct from t1 to t0 and calculate the first correction parameter;

[0084] S4, binding the reverse zero speed correction result as the initial attitude value of the forward pursuit inertial navigation and establishing a forward pursuit Kalman filter, using the forward inertial navigation and Kalman filter algorithm model, adopting the fixed position correction method to perform correction from t0 to t1 and calculate the second correction parameter;

[0085] When the alignment ends, the launch system swings the coarse alignment to output the alignment result, and the navigation computer performs reverse zero-speed correction. The reverse zero-speed correction uses the Kalman filter method, which uses the swing coarse alignment output alignment result as the Kalman filter initial value, and calculates the prior attitude error of the fine alignment filter based on the probability distribution of the coarse alignment, and uses the Kalman filter zero-speed correction method to correct the coarse alignment result.

[0086] The attitude, velocity and position update equations of the reverse strapdown inertial navigation can generally be expressed as follows, and the attitude forward update formula is:

[0087] (6)

[0088] The inverse update formula of the posture is:

[0089] (7)

[0090] in

[0091] (8)

[0092] (9)

[0093] (10)

[0094] (11)

[0095] , For the angular increment output of the gyroscope, the gyroscope data is collected twice in each step of the recursive calculation.

[0096] The velocity forward update formula is:

[0097] (12)

[0098] The speed reverse update formula is:

[0099] (13)

[0100] in:

[0101] (14)

[0102] (15)

[0103] (16)

[0104] (17)

[0105] (18)

[0106] (19)

[0107] (20)

[0108] , To output the speed increment of the adder, the adder data is collected twice in each step of the recursive calculation.

[0109] The position forward update formula is:

[0110] (twenty one)

[0111] The reverse position update formula is:

[0112] (twenty two)

[0113] in

[0114] (twenty three)

[0115] S5. Perform catch-up calculation based on the first correction parameter and the second correction parameter until the Kalman filter time is synchronized with the inertial navigation system and the filter correction is completed.

[0116] Next, the error equation of the reverse navigation system is derived based on the strapdown inertial navigation error equation.

[0117] Zero-speed correction uses speed as the observed quantity, and the state space model is shown in the following formula:

[0118] (twenty four)

[0119] In the formula, the state vector .

[0120] The state equation of the inverse filter is as follows:

[0121] (25)

[0122] The above model is used for backtracking zero speed correction. When the system is backtracked to the initial moment, a forward filter fixed position correction is performed again. The correction model still uses Kalman filtering, but the position needs to be added to the state variable:

[0123] (26)

[0124] At the same time, the state equation is rewritten as:

[0125] (27)

[0126] The measurement equation is rewritten as:

[0127] (28)

[0128] The observed quantity becomes:

[0129] (29)

[0130] The above model is used to complete the forward calculation and catch up with the real-time output of the inertial navigation system, then the inertial navigation system is corrected to complete the alignment.

[0131] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 A process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0132] These computer program instructions may also be stored in a computer-readable memory capable of directing a computer or other programmable data processing device to operate in a specific manner, so that the instructions stored in the computer-readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 A process or multiple processes and / or boxes Figure 1 A function specified in one or more boxes.

[0133] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operating steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing instructions for implementing the process. Figure 1 A process or multiple processes and / or boxes Figure 1 The steps for the functions specified in one or more boxes.

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

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

Claims

1. A method for backtracking correction alignment of a rocking base of a launch system, characterized in that: The steps include: S1, start the inertial navigation system, wait for the inertial navigation data to be output after initialization is completed, and record an output stable time t0; S2, use the rocking base coarse alignment method to perform real-time alignment, and record the alignment end time t1; S3, binding the rough alignment result as the initial attitude value of the reverse inertial navigation and establishing a reverse backtracking Kalman filter, using the reverse inertial navigation and Kalman filter algorithm model, adopting the reverse zero speed correction method to correct from t1 to t0 and calculate the first correction parameter; S4, binding the reverse zero speed correction result as the initial attitude value of the forward pursuit inertial navigation and establishing a forward pursuit Kalman filter, using the forward inertial navigation and Kalman filter algorithm model, adopting the fixed position correction method to perform correction from t0 to t1 and calculate the second correction parameter; S5, performing catch-up calculation based on the first correction parameter and the second correction parameter until the Kalman filter time is synchronized with the inertial navigation system and the filter correction is completed. The specific method of the catch-up calculation is: S51, using the Kalman filter algorithm model in steps S3 and S4 as the correction model, and adding the position variable to its state quantity, expressed as: in is the state variable, is the misalignment angle, is the speed error, is the position error, is the gyro bias, To add zero bias; S52, rewrite the state equation, measurement equation and observation quantity of the correction model with position variables added in S51, where: The state equation is rewritten as: is the posture matrix, is the angular velocity of the Earth's rotation in the transmitting system, is the posture matrix, is the angular velocity of the Earth's rotation in the transmitting system, is the gyroscope white noise, is the accelerometer white noise; Output by adding Calculated, expressed as: is the accelerometer measurement value; The measurement equation is rewritten as: In the formula ， For size The zero matrix, for The zero matrix, For size The unit array of The observed quantity is rewritten as: is the launch position output by the inertial navigation system; S53, based on the revised model rewritten in S52, forward calculation is performed to catch up with the real-time output of the inertial navigation system, and the inertial navigation system is corrected to complete the alignment.

2. The method for backtracking correction alignment of a rocking base of a launch system according to claim 1, characterized in that: The S2 specifically includes the following steps: S21, integrating the accelerometer measurement value and the gravity vector in the solidification coordinate system at the initial moment of alignment; S22, updating the time-varying matrix each time the gyroscope outputs an angle increment, and taking the accelerometer measurement values ​​at different times and the integral of the gravity vector in the solidified coordinate system to calculate the attitude matrix estimate; S23. Calculate the attitude matrix in the transmission coordinate system according to the calculated attitude matrix estimate.

3. The method for backtracking correction alignment of a rocking base of a launch system according to claim 2, characterized in that: The specific method of updating the time-varying matrix in S22 is: In the formula, is a time-varying matrix, It is the three-axis components of the angular increment output by the gyroscope.

4. The method for backtracking correction alignment of a rocking base of a launch system according to claim 2, characterized in that: The specific calculation method of the attitude matrix estimation in S23 is: In the formula, is the estimator of the attitude matrix, is the integral of the gravity vector in the solidification coordinate system at the first moment, is the integral of the accelerometer measurement value at the first moment; is the integral of the gravity vector in the solidification coordinate system at the second moment, is the integral of the accelerometer measurement value at the second moment, and T is the matrix transpose.

5. The method for backtracking correction alignment of a rocking base of a launch system according to claim 1, characterized in that: The first correction parameter in S3 includes attitude reverse update, velocity reverse update and position reverse update, wherein: The specific method of updating the posture in reverse is: in, , are the attitude quaternions of the launch system at time k and k-1 respectively, is the rotation quaternion of the earth's rotation from time k-1 to time k, is the rotation of the carrier relative to the system at time k between time k and time k-1; The specific method of speed reverse update is: in, , are the velocities of the launch system at time k and k-1 respectively, is the specific velocity increment, is the gravity velocity increment; The specific method of reverse position update is: in , are the positions of the transmitting system at time k and k-1 respectively, is the position increment.

6. The method for retrospective correction and alignment of a rocking base of a launch system according to claim 1, characterized in that: The second correction parameter in S4 includes attitude forward update, velocity forward update and position forward update, wherein: The specific method of attitude positive update is: In the formula, , are the attitude quaternions of the launch system at time k and k-1 respectively, is the rotation quaternion of the earth's rotation from time k-1 to time k, is the rotation of the carrier relative to the system at time k between time k and time k-1; The specific method of speed positive update is: in, , are the velocities of the launch system at time k and k-1 respectively, is the specific velocity increment, is the gravity velocity increment; The specific method of positive position update is: in , are the positions of the transmitting system at time k and k-1 respectively, is the position increment.

Images