Low-cost embedded GNSS / INS integrated navigation time synchronization algorithm

Abstract

The application relates to a low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, which comprises the following steps: at an integral second moment, mechanically arranging and calculating state covariance matrix of received inertial navigation data to obtain first data; starting from second sampling interval IMU data, saving subsequent IMU data and odometer data in a buffer to obtain second data; according to whether GNSS data is received within a set time, selecting to perform Kalman filtering prediction and update on the second data alone or performing Kalman filtering prediction and update after combining the GNSS data and the second data. The application can solve the problem of time asynchronization of GNSS and INS data reception in engineering application, and can guarantee that the GNSS data and the INS data can be combined at the same correct time through effective methods such as setting a buffer, so that the positioning precision of the system is further improved.

Description

Technical Field

[0001] This invention relates to a low-cost embedded GNSS / INS integrated navigation time synchronization algorithm. Background Technology

[0002] Time synchronization is a key technology for achieving real-time integrated navigation, and the magnitude of its error determines the positioning accuracy of the integrated navigation. Real-time GNSS / INS integrated navigation requires the fusion of GNSS data at 1PPS (1 second interval) and inertial navigation (INS) data. However, due to electronic circuit delays, data from both systems cannot be received at exact moments, preventing them from performing integrated navigation at precisely the last second. Typically, the 1PPS time does not coincide with the time when the INS and GNSS data are received. Transmission time exists between the INS and MCU communication and within the INS' internal circuitry, resulting in a time difference between the 1PPS time and the time of IMU data reception. Furthermore, delays in GNSS chip positioning calculations and serial communication between the GNSS chip and the MCU also create a time gap between the 1PPS time and the complete reception of GNSS data. Therefore, waiting for the data to be received before fusion will result in significant positioning errors. To address this issue, a novel time synchronization algorithm is proposed. Summary of the Invention

[0003] To address the time synchronization issue between GNSS and INS data in GNSS / INS integrated navigation during engineering applications, this invention aims to develop a low-cost embedded GNSS / INS integrated navigation time synchronization algorithm. This algorithm solves the time synchronization problem between GNSS and INS data, enabling high-precision and reliable remote real-time positioning.

[0004] A low-cost embedded GNSS / INS integrated navigation time synchronization algorithm.

[0005] At the exact second, the received inertial navigation data is mechanically arranged and the state covariance matrix is ​​calculated to obtain the first data.

[0006] Starting with the second sampling interval of IMU data, subsequent IMU data and odometer data are stored in a buffer to obtain the second data.

[0007] Depending on whether GNSS data is received within a set time period, the system can choose to perform Kalman filter prediction and update on the second data alone, or combine the GNSS data and the second data before performing Kalman filter prediction and update.

[0008] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, if GNSS data has not been received within a set time, the second data is sequentially mechanically arranged, and the state covariance matrix and odometry Kalman filter are predicted and updated to obtain new data. This new data is then used to correct errors in the integrated navigation results.

[0009] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, if GNSS data is received within a set time, the GNSS data at this time is compared with the first data using Kalman filtering prediction and update to obtain the third data. The third data is then used to correct the error in the integrated navigation result. The second data is then mechanically arranged, and the state covariance matrix and odometer Kalman filtering prediction and update are used to obtain the fourth data. The fourth data is then used to correct the error in the integrated navigation result.

[0010] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, when processing the second data, the result of mechanically arranging and calculating the state covariance matrix of the IMU data stored in the second data is used to perform Kalman filtering prediction and update with the odometry data stored in the second data.

[0011] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, the Kalman filter prediction and update include...

[0012] Kalman filter prediction steps: Based on the system model, predict the current state based on the previous state of the system, as shown in formula (1). Then, multiply the state transition matrix of the previous time step with the state transition matrix of the current time step to obtain the estimated value of the state transition matrix of the current time step. In the prediction process, first calculate the error state vector X, and then calculate the state transition matrix Φ, as shown in the following formula:

[0013] X k,k-1 =Φ k,k-1 X k (1)

[0014]

[0015] Kalman filter update steps: After obtaining the state prediction result according to the formula in the prediction step, the current state is optimally estimated using the measured value and the predicted value to obtain the optimal state estimate. In the update process, the state prediction covariance matrix P is calculated first, then the scaling factor and the adaptive factor are calculated respectively, and then the scaling factor and the adaptive factor are substituted into the formula for calculating the gain K. Finally, the error state vector X and the state estimation covariance matrix P are updated using the gain matrix.

[0016] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, the Kalman filter update step uses the following formula:

[0017]

[0018]

[0019]

[0020]

[0021] X k =X k,k-1 +K k (Z k -H k X k,k-1 (7)

[0022] P k =(IK k H k )P k,k-1 (8)

[0023] Where X is the error state vector; Φ represents the state transition matrix; P is the state prediction covariance matrix; Q is the system noise covariance matrix; K is the Kalman filter gain matrix, which determines the weight of the observation information; H represents the design matrix; R represents the observation noise covariance matrix; Z represents the observation vector; α is the scaling factor; β is the adaptive factor; and trace() represents the trace function of the matrix. Represents the chi-square distribution; This is the predicted residual vector; the subscripts above represent state changes, for example, X k,k-1 This represents the error state vector from epoch k-1 to epoch k.

[0024] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, when the GNSS RTK is a fixed solution, the scaling factor is calculated according to the following formula.

[0025]

[0026] in, It is the covariance matrix of the observation vectors of a fixed solution.

[0027] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, when the GNSS RTK is a floating-point solution, the scaling factor is calculated according to the following formula.

[0028]

[0029] in, It is the observation vector covariance matrix of the floating-point solution.

[0030] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, when GNSS uses pseudorange difference decomposition, the scaling factor is calculated according to the following formula.

[0031]

[0032] in, It is the observation vector covariance matrix of the floating-point solution.

[0033] In the aforementioned low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, the adaptive factor is calculated according to the following formula.

[0034]

[0035] β k (i) initial value β k (0) = 1.

[0036] This invention improves the positioning accuracy of the system by using effective methods such as setting buffers to ensure that GNSS and INS data can be combined simultaneously. This GNSS / INS integrated navigation time synchronization method can be applied to various platforms, including vehicle-mounted and aircraft-based systems. To meet the needs of the general public, this time synchronization method is a low-overhead algorithm that runs well on inexpensive embedded devices. Attached Figure Description

[0037] Figure 1 Flowchart of a low-cost embedded GNSS / INS integrated navigation time synchronization algorithm

[0038] Figure 2 Time synchronization data processing sequence diagram

[0039] Figure 3 Horizontal position, horizontal velocity, and heading error in onboard data post-processing

[0040] Figure 4 Real-time processing of horizontal position, horizontal velocity, and heading error by embedded vehicle data

[0041] Figure 5 This is a flowchart of the improved adaptive robust Kalman filter algorithm.

[0042] Figure 6 It represents the number of operations performed for each operation type in the standard extended Kalman filter and one-step predictive Kalman filter tests.

[0043] Figure 7The positioning errors of the standard extended Kalman filter and the one-step predictive Kalman filter in the north, east, and down directions during the test are: (a) curve overlap; (b) curve separation.

[0044] Figure 8 The test results show the positioning errors of traditional adaptive robust Kalman filtering, Sage filtering, standard Kalman filtering, improved adaptive robust Kalman filtering, and GNSS in a tree-lined road scene.

[0045] Figure 9 It measures the real-time position, velocity, and attitude errors of traditional adaptive robust Kalman filtering, Sage filtering, standard Kalman filtering, and improved adaptive robust Kalman filtering during testing.

[0046] Figure 10 This refers to the real-time power consumption of the improved adaptive robust Kalman filter, the traditional adaptive robust Kalman filter, the standard Kalman filter, and the Sage filter during testing. Detailed Implementation

[0047] The technical solution of the present invention will be further described in detail below through embodiments and in conjunction with the accompanying drawings.

[0048] Example:

[0049] I. The flowchart of this time synchronization algorithm is as follows: Figure 1 As shown, the process can be divided into the following steps:

[0050] 1. At 1PPS (1 second interval), the MCU external interrupt is triggered to collect IMU data and mechanically arrange the data. At this time, GNSS data has not been received yet, so GNSS measurement update cannot be performed. Therefore, mechanical arrangement is only performed on the IMU data at the whole second interval during this IMU sampling interval.

[0051] 2. When the microcontroller's internal clock count reaches 10ms, it triggers an MCU interrupt to collect IMU data. This data is not IMU data at whole seconds and time synchronization has not yet been completed, so the data at this time is saved. Since no GNSS data is received during this period, only the received IMU data is saved during this period.

[0052] 3. Since no GNSS data was received in the subsequent period, the received IMU data was continuously saved.

[0053] 4. After a period of time, if GNSS data is suddenly received, the result of the mechanical arrangement in the first step is combined with the GNSS positioning result at this time and then subjected to a Kalman filter for navigation. Conversely, if no GNSS data is received within 240ms after 1PPS, it can be understood that there is a problem with the GNSS signal observation quality or the receiver has malfunctioned in the current second, and it is assumed that there is no GNSS positioning result output in the current second.

[0054] 5. The next step is to mechanically arrange the previously saved IMU data and calculate the state transition matrix.

[0055] 6. Once time synchronization is complete, the IMU data collected every 10ms will be processed normally. If odometer data is encountered during processing, Kalman filtering will be performed between the odometer and the data.

[0056] This time synchronization algorithm first saves the inertial navigation data, and then combines the two for navigation after receiving GNSS data. The specific method is as follows:

[0057] Data processing sequence such as Figure 2 As shown in the figure, Δt IMU Δt represents the IMU sampling interval. GNSS Δt represents the time from the triggering of the MCU external interrupt at 1PPS to the receipt of the GNSS positioning result. GNSS / INS The time Δt represents the time taken to run the Kalman filter (including Kalman prediction and update) for the GNSS / INS integrated navigation system. SAVE_IMU Δt represents the runtime for mechanically orchestrating the saved IMU data. ODO / INS This indicates the time taken for the odometer / INS integrated navigation Kalman filter to run.

[0058] The key to this method is ensuring that the GNSS / INS integrated navigation Kalman filtering (including Kalman prediction and update), mechanical orchestration of the saved IMU data, and odometry / INS integrated navigation calculations are completed within the IMU sampling interval (10ms). This paper refers to the time consumed by these three data processing operations as the time synchronization data processing time. Based on the above requirements, a simplified mechanical orchestration algorithm and a one-step predictive Kalman filtering algorithm are combined. This allows the algorithm to only calculate the mechanical orchestration and state transition matrix during the IMU sampling interval, without performing Kalman prediction calculations. This approach significantly reduces the computational load and ensures that data processing is completed within the IMU sampling interval.

[0059] To verify the impact of the time synchronization algorithm on the positioning accuracy of integrated navigation, the collected vehicle navigation data was post-processed and then processed in real-time using embedded systems. The position, velocity, and attitude errors output by both methods were then compared to analyze the performance of the time synchronization algorithm. Since the post-processing integrated navigation algorithm does not need to consider time synchronization and directly combines GNSS and IMU data at whole-second intervals, its positioning accuracy is relatively high. The horizontal position, forward velocity, and heading error obtained from MATLAB post-processing are shown below. Figure 3 As shown in the figure, the post-processed integrated navigation results have relatively small errors.

[0060] Embedded real-time processing must consider time synchronization, because the higher the vehicle speed, the greater the positioning error caused by time asynchrony. The embedded platform reads raw in-vehicle navigation data from the SD card. This data is stored based on the reception time; therefore, the method of combining navigation data from the SD card by the embedded platform is exactly the same as the method of processing real-time in-vehicle navigation data. The error result of embedded real-time processing is as follows: Figure 4 As shown in the figure, the real-time integrated navigation results using the proposed time synchronization algorithm have smaller errors. However, the positioning error within the red circle is significantly larger compared to other time periods. This is because the first red circle represents the transition from a moving state to a stationary state, which does not consider that the stationary state does not occur at exact seconds. Ideally, the vehicle stops at exact seconds, allowing for zero-speed correction. However, in reality, vehicles rarely stop at exact seconds. If the vehicle stops at the middle of an exact second, the IMU data saved during the period from the exact second to the vehicle's stop will be lost, resulting in positioning errors during zero-speed correction. The second red circle is due to the GNSS arrival time being longer than the manually set time, causing GNSS data to be discarded during this period. In this case, only the odometry-assisted inertial navigation is used for integrated navigation, resulting in a sawtooth-shaped error during this time period.

[0061] Will Figure 3 and Figure 4 The horizontal position error, horizontal velocity error, and heading error obtained from post-processing and embedded real-time processing, respectively, were quantitatively analyzed, and the root mean square and maximum values ​​of the errors were statistically analyzed as shown in Table 1.

[0062] Table 1. Horizontal position, horizontal velocity, and heading error (RMS and MAX) for onboard data post-processing and embedded real-time processing.

[0063]

[0064] As can be seen from the table, due to the influence of zero-velocity correction, the maximum position error of embedded real-time processing is significantly greater than that of post-processing. Furthermore, both the heading error (RMS) and the maximum heading error (MAX) of embedded real-time processing are greater than those of post-processing. The main reason for this is that post-processing uses a directly given initial heading value, while embedded real-time processing obtains the initial heading using dynamic alignment.

[0065] II. The Kalman update and prediction method involved in this invention will be described below.

[0066] (I) Mainly includes

[0067] Kalman filter prediction steps:

[0068] Based on the system model, the current state is predicted based on the previous state, as shown in formula (4-19). Then, the estimated value of the current state transition matrix is ​​obtained by multiplying the state transition matrix from the previous time step with the current time step. In the state prediction process, the error state vector X is calculated first, followed by the state transition matrix Φ, as shown in the following formula:

[0069] X k,k-1 =Φ k,k-1 X k (4-19)

[0070]

[0071] Kalman filter update steps:

[0072] After obtaining the state prediction result according to the formula in the prediction step, the measured value and the predicted value are used to make an optimal estimate of the current state, thus obtaining the optimal state estimate. During the update process, the state prediction covariance matrix P is first calculated, then the scaling factor and adaptive factor are calculated respectively. The scaling factor and adaptive factor are then substituted into the formula for calculating the gain K. Finally, the gain matrix is ​​used to update the error state vector X and the state estimation covariance matrix P, as shown in the following formula:

[0073]

[0074]

[0075]

[0076]

[0077] X k =X k,k-1 +K k (Z k -H k X k,k-1 (4-23)

[0078] P k =(IK k H k )P k,k-1 (4-24)

[0079] Where X is the error state vector; Φ represents the state transition matrix; P is the state prediction covariance matrix; Q is the system noise covariance matrix; K is the Kalman filter gain matrix, which determines the weight of the observation information; H represents the design matrix; R represents the observation noise covariance matrix; Z represents the observation vector; α is the scaling factor; β is the adaptive factor; and trace() represents the trace function of the matrix. Represents the chi-square distribution; This is the predicted residual vector. The subscripts above represent state changes, for example, X. k,k-1 This represents the error state vector from epoch k-1 to epoch k.

[0080] Compared to the traditional Kalman filter algorithm, this invention improves the Kalman filter by incorporating the state prediction vector covariance matrix into the measurement update process, allowing it to be executed concurrently with the measurement update. During the prediction process, only the state vector and the state transition matrix are calculated. To verify the feasibility of this approach, a detailed derivation of the improved adaptive robust Kalman filter algorithm is provided, as follows:

[0081] The state transition matrix Φ in the above equation is expanded as follows:

[0082]

[0083] in, I3 is a three-dimensional identity matrix, f n It is a comparison of forces in the n-system, t gb and t ab These are the zero bias correlation times of the gyroscope and the accelerometer, respectively.

[0084] In formula (4-21), the update accuracy of the P matrix mainly depends on the state transition matrix. Taking the prediction of the P matrix in three adjacent epochs (k, k+1, and k+2) in the extended Kalman filter as an example, the derivation of the update of the P matrix in the simplified Kalman filter is as follows:

[0085] epoch k:

[0086]

[0087]

[0088] Epoch k+1:

[0089]

[0090]

[0091] Epoch k+2:

[0092]

[0093]

[0094] Where Q is the constant spectral density matrix, It is the system noise covariance matrix.

[0095] Comparing equations (4-27), (4-29), and (4-31), we can see that the structures of the three formulas are particularly similar. To incorporate the predicted state covariance matrix P into the measurement update process, the state transition matrix needs to be utilized. Therefore, the feasibility of this idea needs to be proven through theoretical reasoning. Equation (4-31) is the predicted state covariance matrix from epoch k to k+3. Expanding it...

[0096]

[0097] Observing the above equation, the first and second terms on the right side of the equation are relatively easy to calculate, while the third and fourth terms are relatively difficult. To calculate the third and fourth terms, it is necessary to save the previous time step Φ. k+2,k+1 The value of the matrix is ​​stored, but as time increases, the number of stored Φ matrices also increases, which seriously wastes hardware memory resources. Therefore, for ease of calculation, the third and fourth terms are constructed into the form of the first term, and it is written in the following form:

[0098]

[0099]

[0100] Therefore, by replacing the last two terms in equation (4-32) with equations (4-33) and (4-34), we can obtain the following:

[0101]

[0102] To verify the feasibility of replacing the third and fourth terms on the right-hand side of equation (4-32) with (4-33) and (4-34), subtract (4-32) from (4-35). If the result of subtracting the two equations approaches zero, then the construction method is feasible. The expanded equation (4-35)-(4-32) is shown below.

[0103]

[0104] Observing the state transition matrix (4-25), it can be assumed to be in the following form:

[0105] Φk ≈I+F k Δt (4-37)

[0106] The sampling time interval Δt = 0.01s. Substituting the assumed state transition matrix into equation (4-36), as shown below,

[0107]

[0108] Observing the above equation, we find that the higher-order small quantities of Δt approach zero. Therefore, the third and fourth terms on the right-hand side of equation (4-31) can be replaced by (4-33) and (4-34), and written in the following form:

[0109]

[0110] Among them, P k+3,k Let P be the covariance matrix of the predicted states from epoch k to k+3. From the above equation, it can be seen that the P matrix can be implemented in one step from epoch k to k+3. The state transition matrix from epoch k to k+3 can be written in the following form.

[0111] Φ k+3,k =Φ k+3,k+2 Φ k+2,k+1 Φ k+1,k (4-40)

[0112] Similarly, extending P and Φ to n epochs, i.e. from k to k+n, is as follows:

[0113]

[0114] Φ k+n,k =Φ k+n,k+n-1 Φ k+n-1,k+n-2 …Φ k+2,k+1 Φ k+1,k (4-42)

[0115] Therefore, the predicted state covariance matrix can be generated during the Kalman filter update process. Furthermore, during the Kalman filter prediction process, the updated state transition matrix for the current epoch can be obtained by multiplying the calculated value of the state transition matrix in each epoch by the value calculated in the previous epoch. Φ and P, running in the embedded chip code, can be represented as follows:

[0116]

[0117]

[0118] As can be seen from the above derivation process, the computational cost of calculating only the Φ matrix in the Kalman filter prediction process is much less than the computational cost of calculating the P matrix in the traditional Kalman filter algorithm.

[0119] (II) The detailed steps for calculating the two factors in the Kalman filter update step are as follows:

[0120] Calculate the scaling factor:

[0121] According to the covariance matrix propagation law, the theoretical covariance of the prediction residual is:

[0122]

[0123] Based on hardware limitations, a single-epoch prediction residual estimation method with low computational cost is selected to estimate the covariance of the prediction residuals. It can be obtained The covariance matrix is:

[0124]

[0125] Since the dynamic model error is relatively small in a short period of time, the error of the measurement values ​​can be observed by comparing the theoretical covariance and the estimated covariance of the predicted residuals. In adaptive filtering, if the observation vector error is large, the covariance matrix R of the measurement vector... k It should become α k R k (α k >1) means that the measurement vector Z k The weighting of the factor is weakened. However, we hope that the theoretical and estimated values ​​of the predicted residual covariance after the adaptive factor are basically consistent. Therefore, we assume that formulas (5-9) and (5-10) are equal, that is:

[0126]

[0127] Based on the above formula, the estimated value of the measurement vector covariance matrix can be obtained as follows:

[0128]

[0129] Will With R k After trace extraction, the scaling factor is

[0130]

[0131] Therefore, when the GNSS RTK is a fixed solution, the scaling factor is:

[0132]

[0133] in, It is the covariance matrix of the observation vectors of a fixed solution.

[0134] When the GNSS RTK is a floating-point solution, the scaling factor is:

[0135]

[0136] in, It is the observation vector covariance matrix of the floating-point solution.

[0137] When GNSS uses pseudorange difference decomposition, the scaling factor is:

[0138]

[0139] in, It is the observation vector covariance matrix of the floating-point solution.

[0140] Calculate the adaptive factor:

[0141] State-space model of stochastic systems

[0142]

[0143] For the above equation, the observation vector follows a mean of . variance is The probability density function is a Gaussian distribution with the following probability density function ρ(Z). k )for,

[0144]

[0145] Here, m is the dimension of the observation vector. If gross errors exist, the probability density function in the above equation will not hold. Therefore, this characteristic can be used to determine whether outliers exist in the system state.

[0146] The test statistic is constructed according to equation (5-19) to detect model error. Therefore, from the observed value Z k to the mean The squared Mahalanobis distance is used as the test statistic.

[0147]

[0148] in, It is the Mahalanobis distance. According to the definition of Mahalanobis distance above, the prediction residual between the observation vector and the state prediction vector is the Mahalanobis distance. Therefore, the squared Mahalanobis distance of the prediction residual is...

[0149]

[0150] Since the prediction residuals follow a standard normal distribution, the squared Mahalanobis distance based on the prediction residuals follows a chi-square distribution with m degrees of freedom.

[0151]

[0152] Where α represents the significance level, which is very small; here we set it to 0.01; m is the dimension of the predicted residuals; P{} represents γ. k Greater than The probability of γ is α. k Greater than This is a low-probability event. If this low-probability event occurs, it indicates that there are outliers in the system state; otherwise, there are no outliers. Based on this characteristic, an adaptive factor can be obtained, and then the adaptive factor can be used to expand the state prediction covariance matrix.

[0153]

[0154] Therefore, based on formula (5-22), we assume the following equation holds true.

[0155]

[0156] After transformation, it is as follows:

[0157]

[0158] The adaptive factor can be obtained by using the Newton-Gaussian interpolation method on the above equation.

[0159]

[0160] Where i represents the i-th iteration; based on the reciprocal of the inverse matrix From formula (5.25), we can obtain f′(β) k )as follows

[0161]

[0162] Substituting equations (5-25) and (5-27) into equation (5-26), we get

[0163]

[0164] β k (i) initial value β k (0) = 1.

[0165] (III) The following are specific examples of the application of the present invention.

[0166] (1) Analysis and comparison of the number of operations

[0167] To verify the effectiveness of the simplified one-step predictive Kalman filter in the improved robust adaptive Kalman filter algorithm, its computational complexity and positioning performance were analyzed and compared with the standard extended Kalman filter. The improved robust adaptive Kalman filter (IRAKF) algorithm uses a simplified one-step predictive Kalman filter, where the predicted state covariance matrix is ​​calculated only once during the measurement update cycle. Therefore, this algorithm significantly reduces computational complexity and can operate in real-time on low-cost, low-frequency MCUs. It ensures low-power operation of the system. To evaluate the computational complexity of the one-step predictive Kalman filter and the standard extended Kalman filter in the IRAKF algorithm, the number of operations for multiplication (M), addition and subtraction (A&S), division (D), square root (SR), and trigonometric functions (T) were calculated. (Appendix) Figure 10 The number of operations in each of the two Kalman filters is shown. Ultimately, in the standard Extended Kalman Filter (EPF), the number of operations per second for M, A&S, D, SR, and T are 215,959, 203,912, 247, 132, and 35, respectively. In the one-step predictive Kalman Filter (BRF), the number of operations for M, A&S, D, SR, and T are 52,747, 39,216, 143, 132, and 35, respectively. Compared to the standard EPF algorithm, the one-step predictive Kalman Filter reduces the number of operations for M by 75.6%, A&S by 80.8%, and D by 42.1%.

[0168] To verify whether the simplified one-step predictive Kalman filter in the IRAKF algorithm affects the positioning performance of the integrated navigation system, a real-time vehicle-mounted experiment was conducted in a challenging urban environment. Simultaneously, the simplified one-step predictive Kalman filter was replaced with a standard extended Kalman filter, and the positioning accuracy of the two algorithms was compared. (Appendix) Figure 7 The positioning errors of the two algorithms in the north, east, and down directions are shown. The results show that although the simplified one-step predictive Kalman filter reduces the number of operations, its positioning accuracy is almost not reduced.

[0169] (2) Performance verification

[0170] Positioning, velocity, and attitude accuracy are crucial indicators of the performance of an integrated navigation system. To verify the performance of the improved robust adaptive Kalman filter algorithm in complex scenarios, based on real-time vehicle-mounted experiments, the horizontal positioning error, forward velocity error, and heading error of the GNSS / MEMS-IMU / odometer were analyzed. Furthermore, the positioning performance in special scenarios, namely tree-lined roads and tunnels, was also analyzed and compared.

[0171] The results show that the horizontal position error of the standard Kalman filter exhibits large values ​​between 281,989 s (280,000 + 1989) and 282,344 s (280,000 + 2344). Compared with CRAKF, Sage filter, and standard Kalman filter, the IRAKF algorithm proposed in this invention has the highest positioning accuracy and the best robustness. However, it is limited to most road scenarios, mainly because GNSS receivers cannot output the observation vector covariance matrix, and manually set values ​​cannot accurately represent the current positioning error. In terms of horizontal position error, IRAKF can provide better performance than the other three methods. The heading error of IRAKF is smaller than that of the other three methods in most periods, and the error curve is smoother.

[0172] The statistical values ​​of the root mean square (RMS) and maximum error of horizontal position, forward speed, and heading from CRAKF, Sage filter, standard Kalman filter, and IRAKF show that the IRAKF proposed in this invention has smaller RMS values ​​and maximum errors compared to the other three methods. Specific comparative analysis results are as follows: Compared to CRAKF, the RMS errors of horizontal position, forward speed, and heading of IRAKF increased by 3.8%, 7.9%, and 2.3%, respectively; compared to Sage filter, they improved by 10.7%, 10.3%, and 6.5%, respectively; and compared to standard Kalman filter, they increased by 80.7%, 84.1%, and 44.2%, respectively. In terms of maximum error, compared with CRAKF, IRAKF improves horizontal position, forward speed, and heading by 1.2%, 22.2%, and 0%, respectively; compared with Sage filter, it improves by 2.9%, 22.2%, and 1%, respectively; and compared with standard Kalman filter, it increases by 90.4%, 95.9%, and 47.9%, respectively.

[0173] Single-frequency GNSS signals are easily blocked on tree-lined roads, leading to poor positioning accuracy. Results showed that the GNSS signal was blocked at epoch 282,126 seconds, resulting in an abnormal observation. At this time, the GNSS positioning error reached 4.12 meters. The positioning error of the single-frequency GNSS / MEMS-IMU / odometer integrated navigation system using the standard Kalman filter algorithm was also 3.8 meters. Although CRAKF can suppress abnormal GNSS positioning errors, it is still relatively poor in reducing error divergence. (Appendix) Figure 8 IARKF demonstrates optimal accuracy, prevents anomalous GNSS observations, and effectively mitigates the divergence of positioning errors.

[0174] Based on the horizontal position errors of CRAKF, Sage filtering, standard Kalman filtering, and IRAKF at the time of GNSS anomalous observations in the tree-lined avenue scenario, it can be concluded that the integrated navigation system with IRAKF eliminates GNSS gross errors and improves positioning accuracy. Compared with the other three methods, its positioning accuracy is improved by 20%, 14.3%, and 96.8% respectively when dealing with GNSS gross errors.

[0175] Tunnel scenarios significantly impact positioning accuracy, leading to substantial errors without proper handling. When a vehicle enters a tunnel, GNSS signals are blocked, with only the odometer assisting INS. However, GNSS RTK operates with a pseudo-fixed solution when the vehicle is inside the tunnel. Without sufficient measures, this erroneous GNSS solution can interfere with a stable integrated navigation system, resulting in significant positioning errors in subsequent results. When GNSS outliers occur, the fixed-solution positioning error using standard Kalman filtering for integrated navigation reaches 2.1m, while IRAKF's positioning error is only 0.14m. This is also superior to Sage filtering and CRAKF's 0.165m and 0.16m, respectively. Furthermore, INS diverges more slowly in tunnels than the other three methods. Therefore, IRAKF eliminates the RTK pseudo-fixed solution, improves system stability, and reduces positioning errors.

[0176] From the horizontal position errors of CRAKF, Sage filtering, standard Kalman filtering, and IRAKF in the GNSS RTK pseudo-fixed epoch, it can be concluded that the integrated navigation system using IRAKF eliminates GNSS anomalous observations and improves positioning accuracy. Compared with the other three methods, its positioning accuracy in the RTK pseudo-fixed epoch is improved by 12.5%, 15.2%, and 93.3%, respectively.

[0177] (3) Comparison with other filtering algorithms

[0178] The IRAKF algorithm was validated in a single-frequency GNSS / MEMS-IMU / odometer integrated navigation module and compared with CRAKF, Sage filtering, and standard Kalman filtering. Throughout the tests, the real-time errors of position, velocity, and attitude of the four algorithms were within acceptable limits. Figure 9 The results show that in complex scenarios such as tunnels, position, velocity, and attitude errors are relatively large. In these scenarios, integrated navigation systems using the standard Kalman filter algorithm struggle to reduce positioning errors caused by GNSS gross errors. While integrated navigation systems using Sage filtering and CRAKF algorithms can achieve some results, their performance remains unsatisfactory in some harsh environments. However, the proposed IRAKF can effectively eliminate GNSS gross errors, mitigate system divergence errors in complex environments, and improve the position, velocity, and attitude accuracy of single-frequency GNSS / MEMS-IMU / odometer integrated navigation.

[0179] Because the standard Kalman filter has poor robustness, its root mean square errors for position, velocity, and attitude are relatively large. CRAKF, Sage filtering, and IRAKF can eliminate gross errors, and their performance indicators are superior to the standard Kalman filter. Among them, test results show that IRAKF has the best performance. In terms of positioning accuracy, compared with the CRAKF algorithm, IRAKF improves accuracy by 14.3%, 12.5%, and 24.3% in the north, east, and down directions, respectively; compared with the Sage filter, it improves accuracy by 10%, 26.3%, and 17.6%, respectively; and compared with the standard Kalman filter, it improves accuracy by 83.7%, 78.7%, and 45.1%, respectively.

[0180] (4) Real-time power consumption verification and comparison

[0181] During real-time vehicle navigation, the power consumption of the multi-sensor fusion navigation and positioning module was tested using a power meter and power supply, and the data was saved. The real-time power consumption curve is attached. Figure 10 The test results show that the MCU operating frequency in the module using the IRAKF algorithm can be reduced to 50MHz, with an average power consumption of 143mW. Since CRAKF, Sage filtering, and standard Kalman filtering do not employ the Kalman one-step prediction strategy, the MCU operating frequency can only be reduced to 200MHz, and the average power consumption of these methods is relatively higher, at 296mW, 300mW, and 304mW respectively. The results demonstrate that IRAKF can reduce the module's power consumption by reducing computational complexity.

[0182] The specific embodiments described herein are merely illustrative of the spirit of the invention. Those skilled in the art to which this invention pertains may make various modifications or additions to the described specific embodiments or use similar methods to substitute them, without departing from the spirit of the invention or exceeding the scope defined by the appended claims.

Claims

1. A low-cost embedded GNSS / INS integrated navigation time synchronization algorithm, characterized in that: At the exact second, the received inertial navigation data is mechanically arranged and the state covariance matrix is ​​calculated to obtain the first data. Starting with the second sampling interval of IMU data, subsequent IMU data and odometer data are stored in a buffer to obtain the second data. Depending on whether GNSS data is received within a set time, the system can choose to perform Kalman filter prediction and update on the second data separately, or combine the GNSS data and the second data before performing Kalman filter prediction and update. If GNSS data is not received within the set time, the second data will be mechanically arranged, and the state covariance matrix and odometer Kalman filter will be predicted and updated in sequence to obtain new data. This new data will then be used to correct the error of the integrated navigation results. If GNSS data is received within the set time, perform Kalman filtering prediction and update on the current GNSS data and the first data. The third data is obtained and used to correct errors in the integrated navigation results. The second data is then mechanically arranged, and the state covariance matrix and odometer Kalman filter are used to predict and update the fourth data. The fourth data is then used to correct errors in the integrated navigation results.

2. The low-cost embedded GNSS / INS integrated navigation time synchronization algorithm according to claim 1, characterized in that: When processing the second data, the result of mechanically arranging and calculating the state covariance matrix of the IMU data stored in the second data is used to perform Kalman filtering prediction and update with the odometer data stored in the second data.

3. The low-cost embedded GNSS / INS integrated navigation time synchronization algorithm according to claim 1, characterized in that: The Kalman filter prediction and update include Kalman filter prediction steps: Based on the system model, predict the current state based on the previous state of the system, as shown in formula (1), and then multiply the state transition matrix of the previous time step with the state transition matrix of the current time step to obtain the estimated value of the state transition matrix of the current time step. In the prediction process, the error state vector is calculated first. Then calculate the state transition matrix. The formula is as follows: (1) (2) Kalman filter update steps: After obtaining the state prediction result according to the formula in the prediction step, the measured value and the predicted value are used to make the optimal estimate of the current state to obtain the optimal state estimate. The update process first calculates the state prediction covariance matrix P, then calculates the scaling factor and adaptive factor, and then substitutes the scaling factor and adaptive factor into the formula for calculating the gain K. Finally, the error state vector is updated using the gain matrix. and state estimation covariance matrix The Kalman filter update step uses the following formula. (3) (4) (5) (6) (7) (8) in, It is the error state vector; Represents the state transition matrix; It is the state prediction covariance matrix; It is a constant spectral density matrix; It is the Kalman filter gain matrix, which determines the weights of the observed information; Represents the design matrix; Represents the observation noise covariance matrix; Represents the observation vector; It is a scaling factor; It is an adaptive factor; This indicates finding the trace of a matrix; Represents the chi-square distribution; It is the predicted residual vector; the subscripts above represent the state changes, and the subscripts k, k-1, k-2 represent the epoch numbers; for The error state vector of an epoch. Indicates from Era to Error state vector of an epoch; This is an estimate of the state transition matrix from the previous time step. Let be the state transition matrix at the current moment. This is an estimate of the state transition matrix at the current moment; Here is the system noise covariance matrix; is the sampling time interval; n is the number of epochs in the expansion of the state transition matrix and the state prediction covariance matrix; The theoretical covariance for predicting the residuals; i is the ordinal number of the iteration number; This is an estimate of the covariance matrix of the observation vectors.

4. The low-cost embedded GNSS / INS integrated navigation time synchronization algorithm according to claim 3, characterized in that: When the GNSS RTK solution is fixed, the scale factor is calculated according to the following formula. (5-14) in, It is the covariance matrix of the observation vectors of a fixed solution.

5. The low-cost embedded GNSS / INS integrated navigation time synchronization algorithm according to claim 3, characterized in that: When the GNSS RTK is a floating-point solution, the scaling factor is calculated according to the following formula. (5-15) in, It is the observation vector covariance matrix of the floating-point solution.

6. The low-cost embedded GNSS / INS integrated navigation time synchronization algorithm according to claim 3, characterized in that: When GNSS uses pseudorange difference decomposition, the scale factor is calculated according to the following formula. (5-16) in, It is the covariance matrix of the observation vectors in the pseudorange-difference decomposition.

7. The low-cost embedded GNSS / INS integrated navigation time synchronization algorithm according to claim 3, characterized in that: The adaptive factor is calculated according to the following formula (9) initial value .

Images