A Fast Coarse Alignment Method for Strapdown Inertial Navigation Systems via Segmented Integration Closed-Loop Backtracking
By employing a piecewise integral closed-loop backtracking fast coarse alignment method, and utilizing Kalman filtering and multiple backtracking to construct non-collinear vectors, the problem of the strapdown inertial navigation system's inability to converge quickly in a short time is solved, achieving high-precision fast initial alignment.
Patent Information
- Application Number
- CN202311047059.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-08-21
- Publication Date
- 2026-11-10
- Estimated Expiration
- 2043-08-21
AI Technical Summary
Existing coarse alignment methods for strapdown inertial navigation systems cannot converge quickly in a short time, thus failing to meet the requirements for rapid initial alignment of the carrier.
A piecewise integral closed-loop backtracking fast coarse alignment method is adopted. Based on the closed-loop coarse alignment of Kalman filtering, more non-collinear vectors are constructed through multiple backtracking. Combined with the piecewise integral method, the amount of data is reduced and the alignment accuracy is improved.
It significantly improves the coarse alignment accuracy of strapdown inertial navigation systems in a short period of time, shortens the alignment time, and meets the needs of rapid initial alignment of the carrier.
Smart Images

Figure CN117053836B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of strapdown inertial navigation technology, specifically to a rapid coarse alignment method for strapdown inertial navigation systems using segmented integration closed-loop backtracking. This method can improve the coarse alignment accuracy of strapdown inertial navigation systems in a short time. Background Technology
[0002] In the initial alignment algorithm of a strapdown inertial navigation system (SINS), the attitude relationship between the vehicle coordinate system and the local navigation coordinate system can be obtained by utilizing the outputs of the gyroscope and accelerometer, thus completing the initial alignment. Initial alignment technology is one of the key technologies of a strapdown SINS, and the quality of the initial alignment directly affects the accuracy of the inertial navigation calculation. At the same time, reducing alignment time is also necessary to improve the vehicle's maneuverability and rapid response capability.
[0003] Initial alignment is generally divided into two parts: coarse alignment and fine alignment. Coarse alignment is suitable for cases with large misalignment angles and can narrow the alignment error to a smaller range. However, since it does not consider the zero bias error of the device, it cannot obtain an accurate carrier attitude and requires further processing by fine alignment.
[0004] Currently, in engineering applications of strapdown inertial navigation systems, the optimization-based alignment (OBA) method is often used for coarse alignment. It adopts the idea of determining the attitude of the inertial frame and can be applied to coarse alignment when the carrier is shaking. However, it requires at least 2 minutes of coarse alignment time to achieve high alignment accuracy. Otherwise, it will increase the convergence time of fine alignment and cannot meet the needs of rapid initial alignment of the carrier. As an improvement, reference 1 [Huang Yulong, Zhang Yonggang, Wang Xiaodong. Kalman-Filtering-Based In-Motion Coarse Alignment for Odometer-Aided SINS.[J].IEEE Transactions on Instrumentation & Measurement, 2017, 66(12): 3364-3377.] proposes a Kalman-Filtering-Based Coarse Alignment (KFBCA) method. By modeling the misalignment angle of the attitude matrix during coarse alignment and using Kalman filtering for estimation and compensation, it can achieve better alignment speed and accuracy than the OBA method. However, it still cannot converge within a short alignment time and has room for further improvement. Summary of the Invention
[0005] This invention proposes a fast coarse alignment method for strapdown inertial navigation systems using segmented integration and closed-loop backtracking. Based on closed-loop coarse alignment using Kalman filtering, a fast coarse alignment method using closed-loop backtracking is proposed, and a method for constructing segmented integration vectors is designed. This method can construct more non-collinear vectors during multiple backtracking processes, further reducing the amount of data required for coarse alignment and improving alignment accuracy.
[0006] The technical solution adopted in this invention is a fast coarse alignment method for a strapdown inertial navigation system using segmented integration and closed-loop backtracking, which consists of the following steps:
[0007] S1: Connect the Inertial Measurement Unit (IMU) to the navigation computer and determine the navigation coordinate system, vehicle coordinate system, geocentric inertial coordinate system, and Earth coordinate system:
[0008] Select "East-North-Sky" The geographic coordinate system is the navigation coordinate system, denoted as . The coordinate system is selected as the "right-front-up" coordinate system, denoted as [reference needed]. The geocentric inertial coordinate system is denoted as... The Earth coordinate system is denoted as... Tie.
[0009] S2: Alignment algorithm initialization settings:
[0010] S2.1 Set IMU sampling period Earth's rotational angular velocity Length of Earth's semi-major axis Gravitational constant ; Gyroscope zero bias matrix ,in , , They represent axis, Shaft and Zero bias of axial gyroscope, superscript This indicates the matrix transpose.
[0011] S2.2 Set the carrier location, including longitude. ,latitude and height .
[0012] S2.3 Calculate local gravity using equation (1) :
[0013] (1),
[0014] S2.4 Set coarse alignment time Number of backtracking steps Initial window length for piecewise integration Initial interval With control parameters ,in , , The unit is seconds. , are positive integers and , .
[0015] S2.5 Setting the input coefficient matrix of the Kalman filter system ,in Represents a 3×3 identity matrix; noise variance matrix of a Kalman filter system. and measurement noise variance matrix All are 3×3 diagonal matrices, and their specific values are determined by the characteristics of the IMU.
[0016] S3 performs the first forward solution. In this solution, piecewise integration is not used. The details are as follows:
[0017] S3.1 Execute the navigation computer timer, initialization time. .
[0018] S3.2 Record The gyroscope output angular velocity at any given time is The accelerometer output ratio is The above output is then stored in the navigation computer.
[0019] S3.3 Calculate using formula (2) time System and time 0 Inter-system attitude matrix :
[0020] (2),
[0021] S3.4 Calculate using formula (3) First observation vector at time :
[0022] (3),
[0023] In equation (3), j represents time j. express The antisymmetric matrix, and so on. express System and Inter-system angular velocity System projection:
[0024] (4),
[0025] In formula (3) Indicates the local gravity vector at System projection:
[0026] (5),
[0027] S3.5 Calculated using formula (6) time The estimation of the system at time 0 Inter-system attitude matrix “ " indicates the modulo operation:
[0028] (6),
[0029] S3.6 Calculate using equations (7) and (8) respectively Second observation vector at time and Third observation vector at time :
[0030] (7),
[0031] (8),
[0032] In formula (7) express time Relationship with the initial time Inter-system attitude matrix estimation, when That is, at the initial moment, let the initial value be... ;
[0033] S3.7 Calculate using formula (9) Time recursion matrix :
[0034] (9),
[0035] “ " denotes matrix multiplication; if That is, at the initial moment, let the initial value be... That is, a 4×4 0 matrix; if ,storage To navigation computer;
[0036] S3.8 Calculation Given the eigenvalues and eigenvectors, select the eigenvector corresponding to the smallest eigenvalue. , , , , They represent the eigenvectors respectively. The first to fourth components are calculated using equation (10) to determine the initial time. Relationship with the initial time Inter-system attitude matrix :
[0037] (10),
[0038] S3.9 Perform one-step prediction update of the Kalman filter state:
[0039] (11),
[0040] In equation (11), express Kalman filter state vector at time step The estimate, Represents the Kalman filter state vector from Time to One-step prediction at time step; Kalman filter state vector for 3D vector, representing Its truth value The error between them is shown in equation (12):
[0041] (12),
[0042] In formula (12) , , They represent At the initial moment Tie axis, Shaft and On the axis and Error between; if That is, at the initial moment, let the initial value be... ,in Represents a 3×1 dimensional zero matrix;
[0043] In formula (11) for Time's up The state prediction matrix at time t is calculated using the following formula:
[0044] (13),
[0045] S3.10 Calculate from Time's up The mean square error matrix of the state at time step one prediction :
[0046] (14),
[0047] In formula (14) for The mean square error matrix of the Kalman filter estimation at time t is, if That is, at the initial time, set the initial value. ,in Represents the diagonal elements of a matrix;
[0048] S3.11 Calculation Kalman filter gain matrix at time 1 :
[0049] (15),
[0050] In formula (15) for Time-mapping Kalman filter measurement matrix:
[0051] (16),
[0052] S3.12 Calculation Kalman filter estimation mean square error matrix at time 1 :
[0053] (17),
[0054] S3.13 Perform Kalman filter state estimation:
[0055] (18),
[0056] In formula (18) express The time measurement value is expressed as shown in equation (19):
[0057] (19),
[0058] S3.14 Using Kalman Filter State Estimation Compensation The algorithm loop is completed, as shown in equation (20):
[0059] (20),
[0060] like ,storage To navigation computer;
[0061] S3.15 usage Reassignment of zero-dimensional matrix ,Right now:
[0062] (twenty one),
[0063] S3.16 Order ;
[0064] S3.17 Repeat steps S3.2-S3.16 until... Exit the loop and complete the first forward solution;
[0065] S4 performs the first backtracking, which includes the first reverse calculation and the second forward calculation, as detailed below:
[0066] S4.1 Perform the first reverse calculation. In this calculation, piecewise integration is not required to construct an observation vector that is not collinear with the observation vector constructed in step S3:
[0067] S4.1.1 To distinguish it from the forward solution, the reverse solution step uses... Represent a certain moment, and initialize it. ;
[0068] S4.1.2 Read from navigation computer The time gyroscope output is then inverted, and the value is used... This represents the inverted output of the gyroscope, i.e. ;
[0069] S4.1.3 Calculate using equation (22) time System and time 0 Inter-system attitude matrix :
[0070] (twenty two),
[0071] S4.1.4 Recalculate using equation (23) First observation vector at time :
[0072] (twenty three),
[0073] S4.1.5 Calculate using formula (24) time The estimation of the system at time 0 Inter-system attitude matrix :
[0074] (twenty four),
[0075] S4.1.6 Recalculate using equations (25) and (26) respectively. Second observation vector at time and Third observation vector at time :
[0076] (25),
[0077] (26),
[0078] In equation (25), when season Equal to the value stored in step S3.14 ;
[0079] S4.1.7 Calculate using formula (27) Time recursion matrix :
[0080] (27),
[0081] when season Equal to the value stored in step S3.7 ;when At that time, store To navigation computer;
[0082] S4.1.8 Calculation Given the eigenvalues and eigenvectors, select the eigenvector corresponding to the smallest eigenvalue. Calculate using formula (28) :
[0083] (28),
[0084] S4.1.9 Perform one-step prediction update of the Kalman filter state:
[0085] (29),
[0086] In equation (29), if ,make ; for Time's up The state prediction matrix at time t is calculated using the following formula:
[0087] (30),
[0088] S4.1.10 Calculate from Time's up The mean square error matrix of the state at time step one prediction :
[0089] (31),
[0090] In equation (31), if ,make ;
[0091] S4.1.11 Calculation Kalman filter gain matrix at time 1 :
[0092] (32),
[0093] In equation (32):
[0094] (33),
[0095] S4.1.12 Calculation Kalman filter estimation mean square error matrix at time 1 :
[0096] (34),
[0097] S4.1.13 Perform Kalman filter state estimation:
[0098] (35),
[0099] In formula (35):
[0100] (36),
[0101] S4.1.14 Using Kalman filtering for state estimation compensation Complete the algorithm loop:
[0102] (37),
[0103] S4.1.15 usage Reassignment of zero-dimensional matrix ,Right now:
[0104] (38),
[0105] S4.1.16 Order ;
[0106] S4.1.17 Repeat steps S4.1.2-S4.1.16 until... Exit the loop and complete the first reverse calculation.
[0107] S4.2 Perform the second forward solution. Unlike the first forward solution in step S3, this solution requires piecewise integration to construct the observation vector:
[0108] S4.2.1 Order ;
[0109] S4.2.2 Calculate using formula (2) ;
[0110] S4.2.3 Recalculate using equation (39) First observation vector at time Here, a piecewise integration method is used to construct observation vectors that are not collinear with the observation vectors constructed in steps S3 and S4.1, thus enabling the algorithm to converge further.
[0111] (39),
[0112] In equation (39) For a natural number that satisfies the condition shown in equation (40), where Represent the set of natural numbers:
[0113] (40),
[0114] S4.2.4 Calculate using formula (6) ;
[0115] S4.2.5 Recalculate using equations (41) and (42) respectively. Second observation vector at time and Third observation vector at time The same piecewise integration method as in S4.2.3 is also used here:
[0116] (41),
[0117] (42),
[0118] In equation (41), if ,make ;
[0119] S4.2.6 Calculate the recursive matrix using equation (9) ,like ,make It equals the value stored in step S4.1.7 ;like ,storage The data is sent to the navigation computer and used to update existing data.
[0120] S4.2.7 Calculation Given the eigenvalues and eigenvectors of , select the eigenvector corresponding to the smallest eigenvalue, denoted as . Calculate using formula (10) ;
[0121] S4.2.8 Use equation (11) to perform a one-step Kalman filter state update, if ,make The state one-step prediction matrix in equation (11) The calculation formula is shown in equation (13);
[0122] S4.2.9 Calculate the mean square error matrix of the one-step prediction of the Kalman filter using equation (14). ,like ,make ;
[0123] S4.2.10 Calculate the Kalman filter gain matrix using equation (15) In the formula, the Kalman filter measurement matrix The calculation formula is shown in equation (16);
[0124] S4.2.11 Use equation (17) to calculate the mean square error matrix of the Kalman filter estimation. ;
[0125] S4.2.12 Use equation (18) to perform Kalman filter state estimation, where the measured values are... The calculation formula is shown in equation (19);
[0126] S4.2.13 Compensation using Kalman filter state estimates Complete the algorithm loop:
[0127] (43),
[0128] like ,storage The data is sent to the navigation computer and used to update existing data.
[0129] S4.2.14 Usage Reassignment of zero-dimensional matrix ,Right now:
[0130] (44),
[0131] S4.2.15 Order ;
[0132] S4.2.16 Repeat steps S4.2.2-S4.2.15 until... Exit the loop and complete the second forward solution;
[0133] S5 uses variables Represents the number of backtracking iterations, and is initialized. .
[0134] S6 Execution The backtracking includes the first... The second reverse solution and the first The next forward solution is as follows:
[0135] S6.1 Execute the first This reverse calculation differs from the first reverse calculation in step S4.1. This calculation uses piecewise integration to construct the observation vector:
[0136] S6.1.1 Order ;
[0137] S6.1.2 Read from navigation computer The time gyroscope output is then inverted, and the value is used... This represents the inverted output of the gyroscope, i.e. ;
[0138] S6.1.3 Calculate using formula (22) ;
[0139] S6.1.4 Recalculate the first observation vector using equation (45) :
[0140] (45),
[0141] In equation (45) For natural numbers that satisfy the conditions in equation (46):
[0142] (46),
[0143] In equations (45) and (46) and These represent the segmented integration window length and the interval time, which decrease with each backtracking iteration. This design enables the algorithm to construct more observation vectors that are not collinear with the already constructed observation vectors during multiple backtracking iterations. The calculation formulas are shown in equations (47) and (48):
[0144] (47),
[0145] (48),
[0146] S6.1.5 Calculate using formula (24) ;
[0147] S6.1.6 Recalculate using equations (49) and (50) respectively. and The same piecewise integration method as in step S6.1.4 is also used here:
[0148] (49),
[0149] (50),
[0150] In equation (49), when , season It equals the value stored in step S4.2.13. ;when , season It equals the value stored in step S6.2.13. ;
[0151] S6.1.7 Calculate using formula (27) ,when , At that time, take It equals the value stored in step S4.2.6 ;when , At that time, take It equals the value stored in step S6.2.6 ;when At that time, store The data is sent to the navigation computer and used to update existing data.
[0152] S6.1.8 Calculation Given the eigenvalues and eigenvectors, select the eigenvector corresponding to the smallest eigenvalue. Calculate using formula (28) ;
[0153] S6.1.9 Use equation (29) to perform one-step Kalman filter state prediction update, if ,make The state one-step prediction matrix in equation (29) The calculation formula is shown in equation (30);
[0154] S6.1.10 Use equation (31) to calculate the mean square error matrix of the one-step prediction of the state. ,like ,make ;
[0155] S6.1.11 Calculate the Kalman filter gain matrix using equation (32) In the formula, the Kalman filter measurement matrix The calculation formula is shown in equation (33);
[0156] S6.1.12 Use equation (34) to calculate the mean square error matrix of the Kalman filter estimation. ;
[0157] S6.1.13 Use equation (35) to perform Kalman filter state estimation, where the measured values are... The calculation formula is shown in equation (36);
[0158] S6.1.14 Using Kalman filtering for state estimation compensation Complete the algorithm loop:
[0159] (51),
[0160] S6.1.15 usage Reassignment of zero-dimensional matrix ,Right now:
[0161] (52),
[0162] S6.1.16 Order ;
[0163] S6.1.17 Repeat steps S6.1.2-S6.1.16 until... Exit the loop at time and complete the first step. Second reverse solution;
[0164] S6.2 Execute the first The second forward solution uses piecewise integration to construct a vector, but unlike the second forward solution in step S4.2, this solution uses... and replace and Perform piecewise integration to construct more observation vectors that are not collinear with the already constructed observation vectors. and The details are explained in step S6.2.3:
[0165] S6.2.1 Order ;
[0166] S6.2.2 Calculate using formula (2) ,when At that time, Stored in the navigation computer; if the same data already exists, the newly acquired data will be used. Update existing data;
[0167] S6.2.3 Recalculate using equation (53) :
[0168] (53),
[0169] In formula (53) The calculation process is similar to that in step S4.2.3, but note the application here. and Replace initial window length and initial interval time Therefore For natural numbers that satisfy the conditions in equation (54):
[0170] (54),
[0171] In equations (53) and (54) and The segmented integration window length and interval time decrease with the number of backtracking iterations, respectively, and are calculated using formulas (55) and (56):
[0172] (55),
[0173] (56),
[0174] S6.2.4 Calculate using formula (6) ;
[0175] S6.2.5 Recalculate using equations (57) and (58) respectively. and , here it is applied replace :
[0176] (57),
[0177] (58),
[0178] In equation (57), if ,make ;
[0179] S6.2.6 Calculate the recursive matrix using equation (9) ,like ,make It equals the value stored in step S6.1.7 ;like ,storage The data is sent to the navigation computer and used to update existing data.
[0180] S6.2.7 Calculation Given the eigenvalues and eigenvectors of , select the eigenvector corresponding to the smallest eigenvalue, denoted as . Calculate using formula (10) and will Stored in the navigation computer; if the same data already exists, the newly acquired data will be used. Update existing data;
[0181] S6.2.8 Use equation (11) to perform a one-step Kalman filter state update, if ,make The state one-step prediction matrix in equation (11) The calculation formula is shown in equation (13).
[0182] S6.2.9 Calculate the mean square error matrix of the one-step prediction of the Kalman filter using equation (14). ,like ,make ;
[0183] S6.2.10 Calculate the Kalman filter gain matrix using equation (15) In the formula, the Kalman filter measurement matrix The calculation formula is shown in equation (16);
[0184] S6.2.11 Use equation (17) to calculate the mean square error matrix of the Kalman filter estimation. ;
[0185] S6.2.12 Use equation (18) to perform Kalman filtering state estimation, where the measured values are... The calculation formula is shown in equation (19);
[0186] S6.2.13 Compensation using Kalman filter state estimates Complete the algorithm loop:
[0187] (59),
[0188] like ,storage The data is sent to the navigation computer and used to update existing data.
[0189] S6.2.14 usage Reassignment of zero-dimensional matrix ,Right now:
[0190] (60),
[0191] S6.2.15 Order ;
[0192] S6.2.16 Repeat steps S6.2.2-S6.2.15 until... Exit the loop at time and complete the first step. Second forward solution;
[0193] S6.3 Order
[0194] S7 repeats step S6 until... The loop exits at this point, and a total of [number] operations have been completed. Second backtracking operation.
[0195] S8 Reads the data stored in step S6.2.2. The data stored in step 6.2.7 The data stored in step S6.2.13 The carrier attitude matrix is calculated using equation (61). Complete the coarse alignment process:
[0196] (61).
[0197] The present invention has the following technical effects:
[0198] This invention addresses the problem that commonly used coarse alignment algorithms cannot converge quickly within a short time. It proposes a fast coarse alignment method based on Kalman filtering and piecewise integration with closed-loop backtracking. Building upon the KFBCA method, a backtracking algorithm is further introduced, and a piecewise integration method is proposed. This method can construct more non-collinear vectors during multiple backtracking processes, enabling the algorithm to converge further over multiple backtracking steps. Using this method for initial alignment of strapdown inertial navigation systems can shorten the time required for coarse alignment and improve the alignment accuracy of fast initial alignment. Attached Figure Description
[0199] Figure 1 The implementation process of the method described in this invention;
[0200] Figure 2 The heading angle alignment error obtained by the OBA method after 30 seconds of alignment;
[0201] Figure 3 The heading angle alignment error obtained by the KFBCA method after 30 seconds of alignment;
[0202] Figure 4 The method proposed in this invention aligns the heading angle alignment error obtained after 30 seconds.
[0203] Figure 5 The heading angle alignment error obtained by the OBA method after 50 seconds of alignment.
[0204] Figure 6 The heading angle alignment error obtained by the KFBCA method after 50 seconds of alignment.
[0205] Figure 7 The method proposed in this invention aligns the heading angle alignment error obtained after 50 seconds.
[0206] Specific implementation methods
[0207] To make the technical methods and advantages of this application clearer, the present invention will be further described below with reference to the accompanying drawings. The specific embodiments described herein are only used to explain this application and are not intended to limit this application.
[0208] The specific implementation process of the coarse alignment method proposed in this invention is as follows: Figure 1 As shown, it is important to note that during the alignment process, the carrier must be kept approximately stationary relative to the ground (a certain degree of swaying or other adverse factors are permissible). The method described in this invention includes the following steps:
[0209] S1 connects the IMU to the navigation computer and establishes the coordinate system;
[0210] S2 performs initialization settings;
[0211] S3 performs the first forward solution calculation, without using the piecewise integration method;
[0212] S4 performs the first backtracking. The first inverse calculation does not use piecewise integration; the second forward calculation uses piecewise integration and the initial window length. and initial integration interval Perform piecewise integration;
[0213] S5 uses variables Represents the number of backtracking iterations, and is initialized. ;
[0214] S6 Execution Backtrack once, and set the initial value Among them, the first The reverse computation employs a piecewise integration method, using a window length and integration interval that decrease with the number of backtracking iterations. and Perform segmented integration exercises; The forward solution employs a piecewise integration method, using a window length and integration interval that decrease with the number of backtracking iterations. and Perform piecewise integration;
[0215] S7 repeats step S6 in a loop, setting the... after each backtracking operation during the loop. ,when The total time completed The loop exits after the second backtracking operation.
[0216] S8 calculates the attitude matrix It then outputs the result, which can be used for the next step.
[0217] To illustrate the advantages of the proposed method in rapid coarse alignment, it was compared with the traditional OBA and KFBCA methods in an alignment experiment. The experiment used a Type 90 laser inertial navigation system, and the parameters are shown in Table 1.
[0218] Table 1 Parameters of the Type 90 Laser Inertial Navigation System
[0219]
[0220] The above-mentioned IMU was used in the experiment on a three-axis turntable. Approximately 1.2 hours of static base IMU output data were collected. About 300 seconds after the data acquisition began, the turntable rotated at a speed of 10... The angular velocity rotates 180° around the celestial axis and then remains stationary.
[0221] Based on the obtained IMU output data, we first use OBA coarse alignment for 180 s and dual-position Kalman filtering fine alignment for 3820 s. Since the fine alignment time is relatively long, its result can be considered as the true alignment value.
[0222] From 400 s to 4050 s, a total of 73 segments of IMU output data, each 50 s long, were extracted. First, the OBA method and KFBCA method were used for 30 s (i.e.,...) (The same applies below) and 50 s coarse alignment; then, 30 s coarse alignment is performed using the method proposed in this invention, with piecewise integration of the initial window length. Initial interval time Control variables Backtrack 5 times; finally, use this method for 50 seconds of coarse alignment, and set... , , The heading angle alignment error was determined by backtracking five times. Figures 2-7 As shown.
[0223] For the method proposed in this invention, the specific values of the remaining parameters in step S2 are shown in Table 2:
[0224] Table 2. Other parameter settings in step S2
[0225] The root mean square error (RMSE) of the heading angle alignment results for each method is calculated using equation (62), where... The total number of samples, and These represent the measured value and the true value, respectively. The results are shown in Table 3:
[0226] (62),
[0227] Table 3 Root Mean Square Error of Alignment Results
[0228]
[0229] The data above shows that, with an alignment time of 30 seconds, the alignment error of the method proposed in this invention is approximately 65% and 66% of that of the OBA and KFBCA methods, respectively; with an alignment time of 50 seconds, the alignment error of the method proposed in this invention is approximately 66% and 66% of that of the OBA and KFBCA methods, respectively. This indicates that the alignment accuracy of the method proposed in this invention is superior to that of the OBA and KFBCA methods in a short time, and can meet the requirements for rapid initial alignment.
Claims
1. A fast coarse alignment method for a strapdown inertial navigation system using segmented integration and closed-loop backtracking, characterized in that, This method consists of the following steps: S1: Connect the inertial measurement unit to the navigation computer and determine the navigation coordinate system, vehicle coordinate system, geocentric inertial coordinate system, and Earth coordinate system: The "East-North-Sky" geographic coordinate system is selected as the navigation coordinate system, denoted as... Tie; We select the "right-front-up" coordinate system as the carrier coordinate system, denoted as The geocentric inertial coordinate system is denoted as... The Earth coordinate system is denoted as... Tie; S2: Alignment algorithm initialization settings: S2.1 Set the sampling period of the inertial measurement unit Earth's rotational angular velocity Length of Earth's semi-major axis Gravitational constant ; Gyroscope zero bias matrix ,in , , They represent axis, Shaft and Zero bias of axial gyroscope, superscript Indicates matrix transpose; S2.2 Set the carrier location, including longitude. ,latitude and height ; S2.3 Calculate local gravity using equation (1) : ;(1), S2.4 Set coarse alignment time Number of backtracking steps ; Initial window length for piecewise integration Initial interval With control parameters ,in , , The unit is seconds. , are positive integers and , ; S2.5 Setting the input coefficient matrix of the Kalman filter system ,in Represents a 3×3 identity matrix; Kalman filter system noise variance matrix and measurement noise variance matrix All are 3×3 diagonal matrices, and their specific values are determined by the characteristics of the IMU; S3 performs the first forward solution, and does not use piecewise integration in this solution; S4 performs the first backtracking, which includes the first reverse calculation and the second forward calculation, as detailed below: S4.1 Perform the first reverse calculation. In this calculation, piecewise integration is not required to construct an observation vector that is not collinear with the observation vector constructed in step S3. S4.2 Perform the second forward solution. Unlike the first forward solution in step S3, this solution requires piecewise integration to construct the observation vector. S5 uses variables Represents the number of backtracking iterations, and is initialized. ; S6 Execution The backtracking includes the first... The second reverse solution and the first The next forward solution is as follows: S6.1 Execute the first The second reverse calculation differs from the first reverse calculation in step S4.
1. This calculation uses piecewise integration to construct the observation vector. S6.2 Execute the first The second forward solution uses piecewise integration to construct a vector, but unlike the second forward solution in step S4.2, this solution uses... and replace and Perform piecewise integration to construct more observation vectors that are not collinear with the already constructed observation vectors. and These represent the length of the segmented integration window and the interval time, which decrease with the number of backtracking iterations, respectively. S6.3 Order ; S7 repeats step S6 until... The loop exits at this point, and a total of [number] operations have been completed. Second backtracking operation; S8 completes the coarse alignment process.
2. The fast coarse alignment method for segmented integration closed-loop backtracking in a strapdown inertial navigation system according to claim 1, characterized in that: The specific steps for S3 are as follows: S3.1 Execute the navigation computer timer, initialization time. ; S3.2 Record The gyroscope output angular velocity at any given time is The accelerometer output ratio is The above output is then stored in the navigation computer. S3.3 Calculate using formula (2) time System and time 0 Inter-system attitude matrix : (2), S3.4 Calculate using formula (3) First observation vector at time : (3), In equation (3), j represents time j. express The antisymmetric matrix, and so on. express System and Inter-system angular velocity System projection: (4), In formula (3) Indicates the local gravity vector at System projection: (5), S3.5 Calculated using formula (6) time Estimation of the system at time 0 Inter-system attitude matrix " " indicates the modulo operation: (6), S3.6 Calculate using equations (7) and (8) respectively Second observation vector at time and Third observation vector at time : (7), (8), In formula (7) express time System and initial time Inter-system attitude matrix estimation, when That is, at the initial moment, let the initial value be... ; S3.7 Calculate using formula (9) Time recursion matrix : (9), " " denotes matrix multiplication; if That is, at the initial moment, let the initial value be... That is, a 4×4 0 matrix; if ,storage To navigation computer; S3.8 Calculation Given the eigenvalues and eigenvectors, select the eigenvector corresponding to the smallest eigenvalue. , , , , They represent the eigenvectors respectively. The first to fourth components are calculated using equation (10) to determine the initial time. System and initial time Inter-system attitude matrix : (10), S3.9 Perform one-step prediction update of the Kalman filter state: (11), In equation (11), express Kalman filter state vector at time step The estimate, Represents the Kalman filter state vector from Time to A one-step prediction of a moment; Kalman filter state vector for 3D vector, representing Its truth value The error between them is shown in equation (12): (12), In formula (12) , , They represent At the initial moment Tie axis, Shaft and On the axis and Error between; if That is, at the initial moment, let the initial value be... ,in Represents a 3×1 dimensional zero matrix; In formula (11) for Time's up The state prediction matrix at time t is calculated using the following formula: (13), S3.10 Calculate from Time's up The mean square error matrix of the state at time step one prediction : (14), In formula (14) for The mean square error matrix of the Kalman filter estimation at time t is, if That is, at the initial time, set the initial value. , where diag represents the diagonal element of the matrix; S3.11 Calculation Kalman filter gain matrix at time 1 : (15), In formula (15) for Time-mapping Kalman filter measurement matrix: (16), S3.12 Calculation Kalman filter estimation mean square error matrix at time 1 : (17), S3.13 Perform Kalman filter state estimation: (18), In formula (18) express The time measurement value is expressed as shown in equation (19): (19), S3.14 Using Kalman Filter State Estimation Compensation The algorithm loop is completed, as shown in equation (20): (20), like ,storage To navigation computer; S3.15 usage Reassignment of zero-dimensional matrix ,Right now: (21), S3.16 Order ; S3.17 Repeat steps S3.2-S3.16 until... Exit the loop when the time is right, and complete the first forward solution.
3. The fast coarse alignment method for segmented integration closed-loop backtracking in a strapdown inertial navigation system according to claim 2, characterized in that: The specific steps of S4.1 are as follows: S4.1.1 To distinguish it from the forward solution, the reverse solution step uses... Represent a certain moment, and initialize it. ; S4.1.2 Read from navigation computer The time gyroscope output is then inverted, and the value is used... This represents the inverted output of the gyroscope, i.e. ; S4.1.3 Calculate using equation (22) time System and time 0 Inter-system attitude matrix : (22), S4.1.4 Recalculate using equation (23) First observation vector at time : (23), S4.1.5 Calculate using formula (24) time Estimation of the system at time 0 Inter-system attitude matrix : (24), S4.1.6 Recalculate using equations (25) and (26) respectively. Second observation vector at time and Third observation vector at time : (25), (26), In equation (25), when season Equal to the value stored in step S3.14 ; S4.1.7 Calculate using formula (27) Time recursion matrix : (27), when season Equal to the value stored in step S3.7 ;when At that time, store To navigation computer; S4.1.8 Calculation Given the eigenvalues and eigenvectors, select the eigenvector corresponding to the smallest eigenvalue. Calculate using formula (28) : (28), S4.1.9 Perform one-step prediction update of the Kalman filter state: (29), In equation (29), if ,make ; for Time's up The state prediction matrix at time t is calculated using the following formula: (30), S4.1.10 Calculate from Time's up The mean square error matrix of the state at time step one prediction : (31), In equation (31), if ,make ; S4.1.11 Calculation Kalman filter gain matrix at time 1 : (32), In equation (32): (33), S4.1.12 Calculation Kalman filter estimation mean square error matrix at time 1 : (34), S4.1.13 Perform Kalman filter state estimation: (35), In equation (35): (36), S4.1.14 Using Kalman filtering for state estimation compensation Complete the algorithm loop: (37), S4.1.15 usage Reassignment of zero-dimensional matrix ,Right now: (38), S4.1.16 Order ; S4.1.17 Repeat steps S4.1.2-S4.1.16 until... Exit the loop and complete the first reverse calculation.
4. The fast coarse alignment method for segmented integration closed-loop backtracking in a strapdown inertial navigation system according to claim 3, characterized in that: The specific steps of S4.2 are as follows: S4.2.1 Order ; S4.2.2 Calculate using formula (2) ; S4.2.3 Recalculate using equation (39) First observation vector at time Here, a piecewise integration method is used to construct observation vectors that are not collinear with the observation vectors constructed in steps S3 and S4.1, thus enabling the algorithm to converge further. (39), In equation (39) For a natural number that satisfies the condition shown in equation (40), where Represent the set of natural numbers: (40), S4.2.4 Calculate using formula (6) ; S4.2.5 Recalculate using equations (41) and (42) respectively. Second observation vector at time and Third observation vector at time The same piecewise integration method as in S4.2.3 is also used here: (41), (42), In equation (41), if ,make ; S4.2.6 Calculate the recursive matrix using equation (9) ,like ,make It equals the value stored in step S4.1.7 ;like ,storage The data is sent to the navigation computer and used to update existing data. S4.2.7 Calculation Given the eigenvalues and eigenvectors of , select the eigenvector corresponding to the smallest eigenvalue, denoted as . Calculate using formula (10) ; S4.2.8 Use equation (11) to perform a one-step Kalman filter state update, if ,make The state one-step prediction matrix in equation (11) The calculation formula is shown in equation (13); S4.2.9 Calculate the mean square error matrix of the one-step prediction of the Kalman filter using equation (14). ,like ,make ; S4.2.10 Calculate the Kalman filter gain matrix using equation (15) In the formula, the Kalman filter measurement matrix The calculation formula is shown in equation (16); S4.2.11 Calculate the mean square error matrix of the Kalman filter estimation using equation (17). ; S4.2.12 Use equation (18) to perform Kalman filter state estimation, where the measured values are... The calculation formula is shown in equation (19); S4.2.13 Compensation using Kalman filter state estimates Complete the algorithm loop: (43), like ,storage The data is sent to the navigation computer and used to update existing data. S4.2.14 Usage Reassignment of zero-dimensional matrix ,Right now: (44), S4.2.15 Order ; S4.2.16 Repeat steps S4.2.2-S4.2.15 until... Exit the loop and complete the second forward solution.
5. The fast coarse alignment method for segmented integration closed-loop backtracking in a strapdown inertial navigation system according to claim 4, characterized in that: The specific steps in S6.1 are as follows: S6.1.1 Order ; S6.1.2 Read from navigation computer The time gyroscope output is then inverted, and the value is used... This represents the inverted output of the gyroscope, i.e. ; S6.1.3 Calculate using formula (22) ; S6.1.4 Recalculate the first observation vector using equation (45) : (45), In equation (45) For natural numbers that satisfy the conditions in equation (46): (46), In equations (45) and (46) and The length of the piecewise integration window and the interval time are respectively the lengths of the piecewise integration window that decrease with the number of backtracking iterations. Their calculation formulas are shown in equations (47) and (48): (47), (48), S6.1.5 Calculate using formula (24) ; S6.1.6 Recalculate using equations (49) and (50) respectively. and The same piecewise integration method as in step S6.1.4 is also used here: (49), (50), In equation (49), when , season It equals the value stored in step S4.2.
13. ;when , season It equals the value stored in step S6.2.
13. ; S6.1.7 Calculate using formula (27) ,when , At that time, take It equals the value stored in step S4.2.6 ;when , At that time, take It equals the value stored in step S6.2.6 ;when At that time, store The data is sent to the navigation computer and used to update existing data. S6.1.8 Calculation Given the eigenvalues and eigenvectors, select the eigenvector corresponding to the smallest eigenvalue. Calculate using formula (28) ; S6.1.9 Use equation (29) to perform a one-step prediction update of the Kalman filter state. ,make The state one-step prediction matrix in equation (29) The calculation formula is shown in equation (30); S6.1.10 Use equation (31) to calculate the mean square error matrix of the one-step prediction of the state. ,like ,make ; S6.1.11 Calculate the Kalman filter gain matrix using equation (32) In the formula, the Kalman filter measurement matrix The calculation formula is shown in equation (33); S6.1.12 Use equation (34) to calculate the mean square error matrix of the Kalman filter estimation. ; S6.1.13 Use equation (35) to perform Kalman filter state estimation, where the measured values are... The calculation formula is shown in equation (36); S6.1.14 Using Kalman filtering for state estimation compensation Complete the algorithm loop: (51), S6.1.15 usage Reassignment of zero-dimensional matrix ,Right now: (52), S6.1.16 Order ; S6.1.17 Repeat steps S6.1.2-S6.1.16 until... Exit the loop at the specified time to complete the first iteration. Secondary reverse solution.
6. The fast coarse alignment method for segmented integration closed-loop backtracking in a strapdown inertial navigation system according to claim 5, characterized in that: The specific steps in S6.2 are as follows: S6.2.1 Order ; S6.2.2 Calculate using formula (2) ,when At that time, Stored in the navigation computer; if the same data already exists, the newly acquired data will be used. Update existing data; S6.2.3 Recalculate using equation (53) : (53), In formula (53) The calculation process is similar to that in step S4.2.3, but note the application here. and Replace initial window length and initial interval time Therefore For natural numbers that satisfy the conditions in equation (54): (54), In equations (53) and (54) and The length of the piecewise integration window and the interval time are respectively the lengths of the piecewise integration window that decrease with the number of backtracking iterations. The calculation formulas are shown in equations (55) and (56): (55), (56), S6.2.4 Calculate using formula (6) ; S6.2.5 Recalculate using equations (57) and (58) respectively. and , here it is applied replace : (57), (58), In equation (57), if ,make ; S6.2.6 Calculate the recursive matrix using equation (9) ,like ,make It equals the value stored in step S6.1.7 ;like ,storage The data is sent to the navigation computer and used to update existing data. S6.2.7 Calculation Given the eigenvalues and eigenvectors of , select the eigenvector corresponding to the smallest eigenvalue, denoted as . Calculate using formula (10) and will Stored in the navigation computer; if the same data already exists, the newly acquired data will be used. Update existing data; S6.2.8 Use equation (11) to perform a one-step Kalman filter state update, if ,make The state one-step prediction matrix in equation (11) The calculation formula is shown in equation (13); S6.2.9 Calculate the mean square error matrix of the one-step prediction of the Kalman filter using equation (14). ,like ,make ; S6.2.10 Calculate the Kalman filter gain matrix using equation (15) In the formula, the Kalman filter measurement matrix The calculation formula is shown in equation (16); S6.2.11 Use equation (17) to calculate the mean square error matrix of the Kalman filter estimation. ; S6.2.12 Use equation (18) to perform Kalman filtering state estimation, where the measured values are... The calculation formula is shown in equation (19); S6.2.13 Compensation using Kalman filter state estimates Complete the algorithm loop: (59), like ,storage The data is sent to the navigation computer and used to update existing data. S6.2.14 usage Reassignment of zero-dimensional matrix ,Right now: (60), S6.2.15 Order ; S6.2.16 Repeat steps S6.2.2-S6.2.15 until... Exit the loop at the specified time to complete the first iteration. The next forward solution.
7. The fast coarse alignment method for segmented integration closed-loop backtracking in a strapdown inertial navigation system according to claim 6, characterized in that: S8 completes the coarse alignment process as follows: Read the data stored in step S6.2.
2. The data stored in step 6.2.7 The data stored in step S6.2.13 The carrier attitude matrix is calculated using equation (61). : (61) 。
