A three self-inertia group rapid alignment and inertia error parameter calibration integrated method

Abstract

The application relates to a three-self-inertial group rapid alignment and inertial error parameter calibration integrated method, and belongs to the technical field of inertial navigation, which comprises the following steps: S1, performing coarse alignment on the three-self-inertial group based on an initial carrier solidification coordinate system to obtain an initial alignment attitude matrix; S2, constructing a closed-loop Kalman filter based on a random closed-loop control system; S3, inputting the inertial device output of the three-self-inertial group and the initial alignment attitude matrix obtained in the step S1 into the closed-loop Kalman filter constructed in the step S2 to perform fine alignment on the three-self-inertial group, and to calibrate and compensate the inertial error parameters. The application realizes high-precision initial alignment of the three-self-inertial group, and can calibrate and compensate the inertial error parameters during alignment, integrates the alignment and calibration processes, reduces the alignment and calibration time of the three-self-inertial group, and improves the navigation precision of the three-self-inertial group.

Description

Technical Field

[0001] This invention belongs to the field of inertial navigation technology, and specifically relates to an integrated method for rapid alignment of three-autoinertial groups and calibration of inertial error parameters. Background Technology

[0002] Three-auto inertial navigation systems (IASIs) come with a built-in dual-axis turntable, enabling "self-testing," "self-alignment," and "self-calibration" without disassembly, significantly reducing maintenance costs. As a safe, reliable, passive, and high-precision attitude and position measurement device, its outstanding advantages have led to its rapid and widespread application, particularly in aviation, aerospace, and maritime fields. For example, invention patent CN110995083A provides a highly reliable locking control method and control system for three-auto inertial navigation system products; invention patent CN113503895A discloses a three-auto inertial navigation system accelerometer size estimation method based on Kalman filtering; invention patent CN114285343A provides a multi-turn rotation control method and system for the rotating mechanism of three-auto inertial navigation system products; and invention patent CN118092873A discloses a three-auto inertial navigation system navigation software architecture design method based on a multi-core processor.

[0003] However, the existing three-autoinertial-systems separate the "self-alignment" and "self-calibration" processes, resulting in long alignment and calibration times and low accuracy. Summary of the Invention

[0004] In view of the shortcomings of the prior art, the purpose of this invention is to provide a simple and high-precision integrated method for rapid alignment and inertial error parameter calibration of three autoinertial groups, so as to solve or improve the defects existing in the prior art.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: an integrated method for rapid alignment and inertial error parameter calibration of three self-inertial navigation systems, comprising the following steps:

[0006] S1. Based on the initial carrier solidification coordinate system, perform coarse alignment of the three inertial navigation systems to obtain the initial alignment attitude matrix;

[0007] S2. Construct a closed-loop Kalman filter based on a stochastic closed-loop control system;

[0008] S3. Input the output of the inertial devices of the three-autoinertial-group and the initial alignment attitude matrix obtained in step S1 into the closed-loop Kalman filter constructed in step S2 to perform fine alignment of the three-autoinertial-group and calibrate and compensate the inertial error parameters.

[0009] Preferably, the specific steps of step S1 are as follows:

[0010] S11. Construct the coordinate system as follows:

[0011] The navigation coordinate system is n, the vehicle coordinate system is b, the geocentric inertial coordinate system is i, the Earth coordinate system is e, the initial navigation coordinate system is n0, the initial inertial coordinate system is i0, the initial Earth coordinate system is e0, and the initial solidified vehicle coordinate system is i. b0 Tie;

[0012] S12. Based on the constructed coordinate system, perform coarse alignment of the three-autoinertial navigation system based on the initial carrier solidified coordinate system to obtain the initial alignment attitude matrix, as follows:

[0013]

[0014] In the formula, The initial alignment attitude matrix, Let be the attitude transformation matrix from the i0 system to the n system. For i b0 The attitude transformation matrix from the i0 system to the i0 system. For b system to i b0 The attitude transformation matrix of the system;

[0015] in,

[0016] In the formula, ω ie L is the Earth's rotational angular rate, t is the alignment time, and L is the rotational speed. t Let be the local latitude at time t;

[0017] in,

[0018] In the formula, Let g be the integral of gravitational acceleration in the i0 system from t0 to t1, where t0 is the initial time of alignment, t1 is the midpoint between the initial and current times of alignment, and t2 is the current time of alignment. n Let T represent the acceleration due to gravity, and T denote the transpose of the matrix.

[0019] UI 0 (t2) is the integral of gravitational acceleration in the i0 system from t0 to t2, where t0 is the initial time of alignment and t2 is the current time of alignment;

[0020] For gravitational acceleration in i b0 The integral from t0 to t1;

[0021] For gravitational acceleration in i b0 The integral from t0 to t2;

[0022] in, The solution is obtained using the following formula:

[0023]

[0024] In the formula, yes The derivative with respect to time, This is the three-dimensional angular rate vector of the carrier relative to the inertial frame, measured by the gyroscopes within the three-autoinertial navigation system. The symbol × denotes the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix.

[0025] Preferably, the specific steps of step S2 are as follows:

[0026] S21. The state variables of the 33D closed-loop Kalman filter are constructed as follows:

[0027]

[0028] In the formula, X represents the state variables of the 33D closed-loop Kalman filter. For the three-dimensional attitude error, δv n δP is the three-dimensional velocity error, δP is the three-dimensional position error, and X is the three-dimensional position error. g X represents the nine-dimensional inertial error parameter of the gyroscope. a δl is the twelve-dimensional inertial error parameter of the accelerometer. b The error of the three-dimensional outer lever arm is due to the misalignment between the rotation center of the indexing mechanism and the sensitive center of the inertial measurement unit.

[0029] S22. The state equations for a 33D closed-loop Kalman filter are constructed as follows:

[0030]

[0031] In the formula, Let X be the derivative of X with respect to time, F' be the state transition matrix of the 3D closed-loop Kalman filter, G' be the control input matrix of the 3D closed-loop Kalman filter, and u be the control input vector.

[0032]

[0033] In the formula, F is the state transition matrix of the 30-dimensional closed-loop Kalman filter, 0 30×3 A 30x3 matrix of zeros, 0 3×30 A 3x30 matrix of zeros, 0 3×3 G is a 3x3 zero matrix; G is the control input matrix of the 30-dimensional closed-loop Kalman filter, 0 3×6 It is a zero matrix with 3 rows and 6 columns;

[0034]

[0035] In the formula, F 11 Let F be the first block matrix of F. 12 Let F be the second block matrix of F. 13 Let F be the third block matrix of F. 14 Let F be the fourth block matrix of F. 21 Let F be the fifth block matrix of F. 22 Let F be the sixth block matrix of F. 23 Let F be the seventh block matrix of F. 25 Let F be the eighth block matrix of F. 32 Let F be the ninth block matrix of F. 33 Let F be the tenth block matrix, 0 3×12 It is a 3x12 matrix of zeros, 0 3×9 It is a 3x9 matrix of zeros, 0 9×3 It is a 9x3 matrix with zeros, 0 9×9 It is a 9x9 matrix of zeros, 0 9×12 It is a 9x12 matrix of zeros, 0 12×3 It is a 12x3 matrix of zeros, 0 12×9 It is a 12x9 matrix with zeros, 0 12×12 It is a zero matrix with 12 rows and 12 columns;

[0036]

[0037] In the formula, Let be the three-dimensional angular rate vector of the navigation frame relative to the inertial frame, and let × denote the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix;

[0038]

[0039] In the formula, L is the local latitude, h is the local altitude, and R is the local altitude. M R is the local radius of the Earth's meridian. N The radius of the Earth's geocentric circle in the local area;

[0040]

[0041] In the formula, v E Let v be the eastward velocity. N The speed is northbound;

[0042]

[0043] In the formula, The transformation matrix from the carrier system to the navigation system. Let x be the component of the gravity vector along the x-axis of the loading system. Let be the component of the gravity vector along the y-axis of the loaded system. Let I2 be the component of the gravity vector along the z-axis of the loaded system, I3 be a 2x2 identity matrix, and I4 be a 3x3 identity matrix. 1×2 A 1x2 matrix of zeros, 0 2×1 It is a zero matrix with 2 rows and 1 column;

[0044]

[0045] In the formula, f b This is the three-dimensional specific force vector of the carrier relative to the inertial frame, measured by the accelerometers within the three-autoinertial navigation system. The symbol × indicates the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix;

[0046]

[0047] In the formula, Let be the vector of the Earth's angular rate of rotation relative to the inertial frame. Let × be the angular rate vector of the navigation frame relative to the Earth, and let × denote the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix, v n v is the velocity vector of the carrier. n × represents vector v n The corresponding antisymmetric matrix;

[0048]

[0049]

[0050] In the formula, 0 3×3 It is a 3x3 matrix of zeros, 0 24×3 It is a 24-row, 3-column zero matrix;

[0051] S23. The observation equations for the 33D closed-loop Kalman filter are as follows:

[0052] Z = HX + V;

[0053] In the formula, Z is the measurement vector, H is the observation matrix, and H = [0 3×3 I3 0 3×3 0 3×21 ], 0 3×21 It is a 3x21 matrix of zeros, and V is the observation noise;

[0054] S24. The feedback compensation form of the filtering estimation result of the 33D closed-loop Kalman filter is as follows:

[0055]

[0056] In the formula, The filtered result of the transformation matrix from the carrier system to the navigation system. The filtered result of the carrier velocity vector. The filtered result represents latitude, where δL is the latitude measurement error, λ is the local geographical longitude, and δλ is the longitude measurement error. The filtered result is for altitude, δh is the altitude measurement error, and K... g Here is the scaling factor matrix of the gyroscope. The filtered result of the gyroscope's scaling factor matrix, δK g Let ε be the measurement error of the gyroscope's scaling factor matrix, and ε be the gyroscope's zero bias. The filter result for the gyroscope's zero bias is given, where δε is the measurement error of the gyroscope's zero bias, and K... a Here is the scaling factor matrix of the accelerometer. δK is the filtered result of the accelerometer's scaling factor matrix. a The measurement error of the accelerometer's scaling factor matrix, For zero bias of the accelerometer, The result of zero bias filtering for the accelerometer. This refers to the measurement error of the accelerometer's zero bias.

[0057] Preferably, the specific steps of step S3 are as follows:

[0058] The output of the inertial devices of the three-auto inertial group and the initial alignment attitude matrix obtained in step S1 are input into the closed-loop Kalman filter constructed in step S2 for Kalman filtering solution to achieve fine alignment of the three-auto inertial group and obtain the inertial error parameter calibration result; the inertial error parameter calibration result is used to compensate the navigation result through the feedback compensation form of the filter estimation result constructed in step S24.

[0059] Compared with the prior art, the present invention has the following beneficial effects: The integrated method of rapid alignment and inertial error parameter calibration of the three-auto inertial group of the present invention realizes high-precision initial alignment of the three-auto inertial group, and can also calibrate and compensate inertial error parameters during the alignment period. By integrating the alignment and calibration processes, the alignment and calibration time of the three-auto inertial group is reduced, the navigation accuracy of the three-auto inertial group is improved, and the problem of long alignment and calibration time and low accuracy of the existing three-auto inertial group with separate "self-alignment" and "self-calibration" processes is solved.

[0060] Using the method of this invention, the alignment time of the three-auto inertial group (IAIG) is 3 minutes, and the alignment results begin to be displayed at 40 seconds. The two horizontal angles converge quickly with relatively small oscillation amplitudes. The heading angle reaches within 1.2′ of the theoretical accuracy of the IIG at approximately 80 seconds and continues to converge within this range. Simultaneously, the zero bias, gyroscope component installation error, and accelerator installation error converge quickly, while gyroscope drift, gyroscope scaling factor, and accelerator scaling factor converge after approximately 1 hour. Navigation takes 3.5 hours, with a maximum velocity error of 0.3 m / s and a maximum position error of 0.5 nm / 3.5 hours. This demonstrates the correctness and accuracy of the method provided by this invention, which can effectively shorten the alignment and calibration time of the IIG, improve the navigation accuracy of the IIG, and has good practicality. Attached Figure Description

[0061] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, those skilled in the art can obtain other drawings based on the drawings described below without creative effort.

[0062] Figure 1 This is a flowchart illustrating an integrated method for rapid alignment and inertial error parameter calibration of three self-inertial navigation systems according to the present invention.

[0063] Figure 2 This is a schematic diagram of the roll angle obtained after coarse alignment in step S1 of the three-autoinertial group verification in Experiment Example 1 of the present invention.

[0064] Figure 3 This is a schematic diagram of the coarse alignment pitch angle obtained by the three-autoinertial navigation system in step S1 of Experiment Example 1 of the present invention.

[0065] Figure 4 This is a schematic diagram of the heading angle obtained after coarse alignment in step S1 of the three-autoinertial navigation system in Experiment Example 1 of the present invention.

[0066] Figure 5 This is a schematic diagram of the gyroscope drift estimation curve obtained by verifying the method of the present invention using a three-autoinertial navigation system in Experiment Example 2 of the present invention.

[0067] Figure 6 This is a schematic diagram of the zero bias estimation curve obtained by the three-autoinertial navigation system in Experiment Example 2 of this invention to verify the method of this invention.

[0068] Figure 7 This is a schematic diagram of the gyroscope scaling factor estimation curve obtained by verifying the method of the present invention using the three-autoinertial navigation system in Experiment Example 2 of the present invention.

[0069] Figure 8This is a schematic diagram of the accelerometer scaling factor estimation curve obtained by verifying the method of the present invention using the three-autoinertial navigation system in Experiment Example 2 of the present invention.

[0070] Figure 9 This is a schematic diagram of the gyroscope component installation error estimation curve obtained by verifying the method of the present invention using the three-autoinertial navigation system in Experiment Example 2 of the present invention.

[0071] Figure 10 This is a schematic diagram of the installation error estimation curve of the accelerometer component obtained by verifying the method of the present invention using the three-autoinertial navigation system in Experiment Example 2 of the present invention.

[0072] Figure 11 This is a schematic diagram of the heading angle curve obtained after fine alignment in Experiment Example 2 of this invention, obtained by verifying the method of this invention using the three-autoinertial navigation system.

[0073] Figure 12 This is a schematic diagram of the velocity error curve obtained by verifying the method of the present invention using the three-autoinertial navigation system in Experiment Example 2 of the present invention.

[0074] Figure 13 This is a schematic diagram of the position error curve obtained by the three-autoinertial navigation system in Experiment Example 2 of this invention to verify the method of this invention. Detailed Implementation

[0075] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of this invention. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention. To make the above features and advantages of this invention more apparent and understandable, specific embodiments are provided below with reference to the accompanying drawings for detailed description.

[0076] like Figure 1 As shown, an embodiment of the present invention provides an integrated method for rapid alignment and inertial error parameter calibration of three self-inertial groups, comprising the following steps:

[0077] S1. Based on the initial carrier solidification coordinate system, perform coarse alignment of the three inertial navigation systems to obtain the initial alignment attitude matrix;

[0078] S2. Construct a closed-loop Kalman filter based on a stochastic closed-loop control system;

[0079] S3. Input the output of the inertial devices of the three-autoinertial-group and the initial alignment attitude matrix obtained in step S1 into the closed-loop Kalman filter constructed in step S2 to perform fine alignment of the three-autoinertial-group and calibrate and compensate the inertial error parameters.

[0080] In this embodiment, the specific steps of step S1 are as follows:

[0081] S11. Construct the coordinate system as follows:

[0082] The navigation coordinate system is n, the vehicle coordinate system is b, the geocentric inertial coordinate system is i, the Earth coordinate system is e, the initial navigation coordinate system is n0, the initial inertial coordinate system is i0, the initial Earth coordinate system is e0, and the initial solidified vehicle coordinate system is i. b0 Tie;

[0083] S12. Based on the constructed coordinate system, perform coarse alignment of the three-autoinertial navigation system based on the initial carrier solidified coordinate system to obtain the initial alignment attitude matrix, as follows:

[0084]

[0085] In the formula, The initial alignment attitude matrix, Let be the attitude transformation matrix from the i0 system to the n system. For i b0 The attitude transformation matrix from the i0 system to the i0 system. For b system to i b0 The attitude transformation matrix of the system;

[0086] in,

[0087] In the formula, ω ie L is the Earth's rotational angular rate, t is the alignment time, and L is the rotational speed. t Let be the local latitude at time t;

[0088] in,

[0089] In the formula, Let g be the integral of gravitational acceleration in the i0 system from t0 to t1, where t0 is the initial time of alignment, t1 is the midpoint between the initial and current times of alignment, and t2 is the current time of alignment. n Let T represent the acceleration due to gravity, and T denote the transpose of the matrix.

[0090] Let t0 be the integral of gravitational acceleration in the i0 system from t0 to t2, where t0 is the initial moment of alignment and t2 is the current moment of alignment.

[0091] For gravitational acceleration in i b0 The integral from t0 to t1;

[0092] For gravitational acceleration in i b0 The integral from t0 to t2;

[0093] in, The solution is obtained using the following formula:

[0094]

[0095] In the formula, yes The derivative with respect to time, This is the three-dimensional angular rate vector of the carrier relative to the inertial frame, measured by the gyroscopes within the three-autoinertial navigation system. The symbol × denotes the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix.

[0096] In this embodiment, step S2 specifically involves the following steps:

[0097] S21. The state variables of the 33D closed-loop Kalman filter are constructed as follows:

[0098]

[0099] In the formula, X represents the state variables of the 33D closed-loop Kalman filter. For the three-dimensional attitude error, δv n δP is the three-dimensional velocity error, δP is the three-dimensional position error, and X is the three-dimensional position error. g X represents the nine-dimensional inertial error parameter of the gyroscope. a δl is the twelve-dimensional inertial error parameter of the accelerometer. b The error of the three-dimensional outer lever arm is due to the misalignment between the rotation center of the indexing mechanism and the sensitive center of the inertial measurement unit.

[0100] S22. The state equations for a 33D closed-loop Kalman filter are constructed as follows:

[0101]

[0102] In the formula, Let X be the derivative of X with respect to time, F' be the state transition matrix of the 3D closed-loop Kalman filter, G' be the control input matrix of the 3D closed-loop Kalman filter, and u be the control input vector.

[0103]

[0104] In the formula, F is the state transition matrix of the 30-dimensional closed-loop Kalman filter, 0 30×3 A 30x3 matrix of zeros, 0 3×30 A 3x30 matrix of zeros, 0 3×3G is a 3x3 zero matrix; G is the control input matrix of the 30-dimensional closed-loop Kalman filter, 0 3×6 It is a zero matrix with 3 rows and 6 columns;

[0105]

[0106] In the formula, F 11 Let F be the first block matrix of F. 12 Let F be the second block matrix of F. 13 Let F be the third block matrix of F. 14 Let F be the fourth block matrix of F. 21 Let F be the fifth block matrix of F. 22 Let F be the sixth block matrix of F. 23 Let F be the seventh block matrix of F. 25 Let F be the eighth block matrix of F. 32 Let F be the ninth block matrix of F. 33 Let F be the tenth block matrix, 0 3×12 It is a 3x12 matrix of zeros, 0 3×9 It is a 3x9 matrix of zeros, 0 9×3 It is a 9x3 matrix with zeros, 0 9×9 It is a 9x9 matrix of zeros, 0 9×12 It is a 9x12 matrix of zeros, 0 12×3 It is a 12x3 matrix of zeros, 0 12×9 It is a 12x9 matrix with zeros, 0 12×12 It is a zero matrix with 12 rows and 12 columns;

[0107]

[0108] In the formula, Let be the three-dimensional angular rate vector of the navigation frame relative to the inertial frame, and let × denote the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix;

[0109]

[0110] In the formula, L is the local latitude, h is the local altitude, and R is the local altitude. M R is the local radius of the Earth's meridian. N The radius of the Earth's geocentric circle in the local area;

[0111]

[0112] In the formula, v E Let v be the eastward velocity. N The speed is northbound;

[0113]

[0114] In the formula, The transformation matrix from the carrier system to the navigation system. Let x be the component of the gravity vector along the x-axis of the loading system. Let be the component of the gravity vector along the y-axis of the loaded system. Let I2 be the component of the gravity vector along the z-axis of the loaded system, I3 be a 2x2 identity matrix, and I4 be a 3x3 identity matrix. 1×2 A 1x2 matrix of zeros, 0 2×1 It is a zero matrix with 2 rows and 1 column;

[0115]

[0116] In the formula, f b This is the three-dimensional specific force vector of the carrier relative to the inertial frame, measured by the accelerometers within the three-autoinertial navigation system. The symbol × indicates the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix;

[0117]

[0118] In the formula, Let be the vector of the Earth's angular rate of rotation relative to the inertial frame. Let × be the angular rate vector of the navigation frame relative to the Earth, and let × denote the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix, v n v is the velocity vector of the carrier. n × represents vector v n The corresponding antisymmetric matrix;

[0119]

[0120] In the formula, 0 3×3 It is a 3x3 matrix of zeros, 0 24×3 It is a 24-row, 3-column zero matrix;

[0121] S23. The observation equations for the 33D closed-loop Kalman filter are as follows:

[0122] Z = HX + V;

[0123] In the formula, Z is the measurement vector, H is the observation matrix, and H = [0 3×3 I3 0 3×3 0 3×21 ], 0 3×21 It is a 3x21 matrix of zeros, and V is the observation noise;

[0124] S24. The feedback compensation form of the filtering estimation result of the 33D closed-loop Kalman filter is as follows:

[0125]

[0126] In the formula, The filtered result of the transformation matrix from the carrier system to the navigation system. The filtered result of the carrier velocity vector. The filtered result represents latitude, where δL is the latitude measurement error, λ is the local geographical longitude, and δλ is the longitude measurement error. The filtered result is for altitude, δh is the altitude measurement error, and K... g Here is the scaling factor matrix of the gyroscope. The filtered result of the gyroscope's scaling factor matrix, δK g Let ε be the measurement error of the gyroscope's scaling factor matrix, and ε be the gyroscope's zero bias. The filter result for the gyroscope's zero bias is given, where δε is the measurement error of the gyroscope's zero bias, and K... a Here is the scaling factor matrix of the accelerometer. δK is the filtered result of the accelerometer's scaling factor matrix. a The measurement error of the accelerometer's scaling factor matrix, For zero bias of the accelerometer, The result of zero bias filtering for the accelerometer. This refers to the measurement error of the accelerometer's zero bias.

[0127] In this embodiment, step S3 specifically involves the following steps:

[0128] The output of the inertial devices of the three-auto inertial group and the initial alignment attitude matrix obtained in step S1 are input into the closed-loop Kalman filter constructed in step S2 for Kalman filtering solution to achieve fine alignment of the three-auto inertial group and obtain the inertial error parameter calibration result; the inertial error parameter calibration result is used to compensate the navigation result through the feedback compensation form of the filter estimation result constructed in step S24.

[0129] This embodiment presents an integrated method for rapid alignment and inertial error parameter calibration of a three-auto inertial group (SAIG). It achieves high-precision initial alignment of the SAIG ​​and also allows for the calibration and compensation of inertial error parameters during alignment. By integrating the alignment and calibration processes, the alignment and calibration time of the SAIG ​​is reduced, and the navigation accuracy of the SAIG ​​is improved. This method solves the problem of long alignment and calibration times and low accuracy in existing SAIG ​​systems where the "self-alignment" and "self-calibration" processes are separate.

[0130] Experimental Example 1:

[0131] A certain three-auto inertial navigation system was selected to verify the coarse alignment in step S1 of the method of the present invention. The three-auto inertial navigation system was placed on a stationary marble platform, and coarse alignment was performed according to step S1 to obtain the following results. Figures 2 to 4 The attitude after coarse alignment is shown. The alignment time is 3 minutes. The alignment results are displayed after 40 seconds. The two horizontal angles converge quickly and the oscillation amplitude is relatively small. The heading angle reaches the theoretical accuracy of the three-autoinertial system within 1.2′ at about 80 seconds and continues to converge within this range. The alignment process can be completed in 2 minutes, which verifies the effectiveness of step S1 of the method of the present invention.

[0132] Experimental Example 2:

[0133] A specific three-auto inertial navigation system was selected to verify the method of this invention. The system was placed on a stationary marble platform, and integrated alignment and calibration were performed according to the method of this invention, yielding the following results: Figures 5 to 10 The calibration results shown indicate that the error curves are estimated. The zero bias, gyroscope component installation error, and accelerator installation error converge quickly, while the gyroscope drift, gyroscope scaling factor, and accelerator scaling factor converge after about 1 hour. Next, to verify the accuracy of the method in parameter estimation, the experimental data from this experiment of 3.8 hours were processed offline. After 10 minutes of alignment, navigation was initiated, including 3 minutes of coarse alignment and 6 minutes of closed-loop fine alignment. Figure 11 It is the heading angle curve after fine alignment. After 2 minutes of fine alignment, the heading angle converges within the oscillation range of 2′. Figure 12 and Figure 13 The curves show the speed error and position error during 3.5 hours of navigation, respectively. The maximum speed error is 0.3 m / s, and the maximum position error is 0.5 nm / 3.5 hours. This demonstrates that the method of the present invention can accurately estimate the parameters, thus verifying the effectiveness of the method.

[0134] The parts of this invention not disclosed in detail are well-known technologies in the field.

[0135] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for rapid alignment and inertial error parameter calibration of three self-autoinertial navigation systems, characterized in that, Includes the following steps: S1. Based on the initial carrier solidification coordinate system, perform coarse alignment of the three inertial navigation systems to obtain the initial alignment attitude matrix; S2. Construct a closed-loop Kalman filter based on a stochastic closed-loop control system. The specific steps are as follows: S21. The state variables of the 33D closed-loop Kalman filter are constructed as follows: ； In the formula, X These are the state variables of a 33D closed-loop Kalman filter. φ For three-dimensional attitude error, δv n For three-dimensional velocity error, δP For three-dimensional position error, X g These are the nine-dimensional inertial error parameters of the gyroscope. X a These are the twelve-dimensional inertial error parameters of the accelerometer. The error of the three-dimensional outer lever arm is due to the misalignment between the rotation center of the indexing mechanism and the sensitive center of the inertial measurement unit. S22. The state equations for a 33D closed-loop Kalman filter are constructed as follows: ； In the formula, for X The derivative with respect to time, F’ This is the state transition matrix of a 33D closed-loop Kalman filter. G’ This is the control input matrix of a 3D closed-loop Kalman filter. u To control the input vector; ， ； In the formula, F Here is the state transition matrix of a 30-dimensional closed-loop Kalman filter, 0 30×3 A 30x3 matrix of zeros, 0 3×30 A 3x30 matrix of zeros, 0 3×3 It is a 3x3 zero matrix; G The control input matrix of a 30-dimensional closed-loop Kalman filter is 0. 3×6 It is a zero matrix with 3 rows and 6 columns; ； In the formula, F 11 for F The first block matrix, F 12 for F The second block matrix, F 13 for F The third block matrix, F 14 for F The fourth block matrix, F 21 for F The fifth block matrix, F 22 for F The sixth block matrix, F 23 for F The seventh block matrix, F 25 for F The eighth block matrix, F 32 for F The ninth block matrix, F 33 for F The tenth block matrix, 0 3×12 It is a 3x12 matrix of zeros, 0 3×9 It is a 3x9 matrix of zeros, 0 9×3 It is a 9x3 matrix with zeros, 0 9×9 It is a 9x9 matrix of zeros, 0 9×12 It is a 9x12 matrix of zeros, 0 12×3 It is a 12x3 matrix of zeros, 0 12×9 It is a 12x9 matrix with zeros, 0 12×12 It is a zero matrix with 12 rows and 12 columns; ； In the formula, Let be the three-dimensional angular rate vector of the navigation frame relative to the inertial frame, and let × denote the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix; ； In the formula, L The local geographical latitude, h For local geographical altitude, R M This is the local radius of the Earth's meridian. R N The radius of the Earth's geocentric circle in the local area; ； In the formula, v E For eastward speed, v N The speed is northbound; ； In the formula, The transformation matrix from the carrier system to the navigation system. Let x be the component of the gravity vector along the x-axis of the loading system. Let be the component of the gravity vector along the y-axis of the loaded system. Let be the component of the gravity vector along the z-axis of the loaded system. I 2 is a 2x2 identity matrix. I 3 is a 3x3 identity matrix, 0 1×2 A 1x2 matrix of zeros, 0 2×1 It is a zero matrix with 2 rows and 1 column; ； In the formula, This is the three-dimensional specific force vector of the carrier relative to the inertial frame, measured by the accelerometers within the three-autoinertial navigation system. The symbol × indicates the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix; ； In the formula, Let be the vector of the Earth's angular rate of rotation relative to the inertial frame. Let × be the angular rate vector of the navigation frame relative to the Earth, and let × denote the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix, v n For the carrier velocity vector, v n × represents a vector v n The corresponding antisymmetric matrix; ； ； ； ； ； In the formula, 0 3×3 It is a 3x3 matrix of zeros, 0 24×3 It is a 24-row, 3-column zero matrix; S23. The observation equations for the 33D closed-loop Kalman filter are as follows: ； In the formula, Z For measurement vectors, H For the observation matrix, H = [0 3×3 I 30 3×3 0 3×21 ], 0 3×21 It is a 3x21 matrix of zero. V To observe noise; S24. The feedback compensation form of the filtering estimation result of the 33D closed-loop Kalman filter is as follows: ； In the formula, The filtered result of the transformation matrix from the carrier system to the navigation system. The filtered result of the carrier velocity vector. The result of latitude filtering. δL This is due to the measurement error of latitude. λ The local geographical longitude, δλ Due to the measurement error of longitude, For the high-resolution filtering results, δh Due to the measurement error of height, K g Here is the scaling factor matrix of the gyroscope. This is the filtered result of the gyroscope's scaling factor matrix. δK g The measurement error is the scaling factor matrix of the gyroscope. ε For zero bias of the gyroscope, This is the filtered result of the gyroscope's zero bias. δε This refers to the measurement error of the gyroscope's zero bias. K a Here is the scaling factor matrix of the accelerometer. The filtered result of the accelerometer's scaling factor matrix. δK a The measurement error of the accelerometer's scaling factor matrix, For zero bias of the accelerometer, The result of zero bias filtering for the accelerometer. This refers to the measurement error of the accelerometer's zero bias. S3. Input the output of the inertial devices of the three-autoinertial-station system and the initial alignment attitude matrix obtained in step S1 into the closed-loop Kalman filter constructed in step S2 to perform fine alignment of the three-autoinertial-station system and calibrate and compensate the inertial error parameters. The specific steps are as follows: The output of the inertial devices of the three-autoinertial-group system and the initial alignment attitude matrix obtained in step S1 are input into the closed-loop Kalman filter constructed in step S2 for Kalman filtering solution, so as to achieve fine alignment of the three-autoinertial-group system and obtain the inertial error parameter calibration results. The navigation results are compensated by the feedback compensation form of the filter estimation results constructed in step S24, based on the calibration results of the inertial error parameters.

2. The integrated method for rapid alignment and inertial error parameter calibration of a three-autoinertial navigation system according to claim 1, characterized in that, The specific steps of step S1 are as follows: S11. Construct the coordinate system as follows: The navigation coordinate system is n The coordinate system of the carrier is b The geocentric inertial coordinate system is i The Earth coordinate system is e The initial navigation coordinate system is n 0 system, initial inertial coordinate system is i 0 system, initial Earth coordinate system is e 0 series, initial carrier solidification coordinate system is i b0 Tie; S12. Based on the constructed coordinate system, perform coarse alignment of the three-autoinertial navigation system based on the initial carrier solidified coordinate system to obtain the initial alignment attitude matrix, as follows: ； In the formula, The initial alignment attitude matrix, for i 0 series to n The attitude transformation matrix of the system. for i b0 Tie i The attitude transformation matrix of the 0 series, for b Tie i b0 The attitude transformation matrix of the system; in, ; In the formula, ω ie This is the Earth's rotational angular rate. t To ensure the timing is accurate, L t for t The local latitude at that moment; in, ; In the formula, , For gravitational acceleration in i 0 series from t 0 to t The integral of 1 t 0 represents the initial moment of alignment. t 1 represents the midpoint between the initial time and the current time. t 2 represents the current time of alignment. g n It is the acceleration due to gravity. T Represents the transpose of a matrix; , For gravitational acceleration in i 0 series from t 0 to t The integral of 2, t 0 represents the initial moment of alignment. t 2 represents the current moment of alignment; , For gravitational acceleration in i b0 From t 0 to t The integral of 1; , For gravitational acceleration in i b0 From t 0 to t The integral of 2; in, The solution is obtained using the following formula: ； In the formula, yes The derivative with respect to time, This is the three-dimensional angular rate vector of the carrier relative to the inertial frame, measured by the gyroscopes within the three-autoinertial navigation system. The symbol × denotes the antisymmetric matrix of the orientation quantity. Representing vectors The corresponding antisymmetric matrix.

Images