A robust adaptive integrated navigation method based on variational inference factor graph

Abstract

The application discloses a robust adaptive integrated navigation method based on variational inference factor graph, belongs to the field of multi-sensor information fusion and integrated navigation, and particularly relates to a state estimation algorithm part with robustness and adaptivity. The method comprises IMU one-step prior updating, inter-frame innovation gross error detection and GNSS iterative posterior updating. In the prior stage, a pre-integration is utilized to predict a prior state, an IMU factor is added to marginalize a prior state covariance, and an IW distribution parameter is propagated according to a forgetting factor. Covariance prediction values are calculated based on average innovation between adjacent GNSS frames to perform gross error detection. In the posterior stage, a GNSS factor is further added, nonlinear optimization and marginalization are iteratively performed to update a posterior state and its covariance, a measurement covariance and other parameters. The method can effectively estimate time-varying measurement noise in the case of gross error interference, so that the system has both robustness and estimation accuracy, and has good adaptability to complex environments.

Description

Technical Field

[0001] This invention relates to a robust adaptive integrated navigation method based on variational inference factor graphs, belonging to the fields of multi-sensor information fusion and integrated navigation, particularly the robustness and adaptive state estimation algorithm. Background Technology

[0002] Inertial navigation systems (INS) recursively integrate to solve for comprehensive navigation parameters such as attitude, velocity, and position, offering advantages such as strong autonomy and good dynamic performance. However, low-cost inertial measurement units (IMUs) suffer from high noise levels and rapid error accumulation. Global navigation satellite systems (GNSS) provide non-divergent, high-precision position and time information, but their reliability is poor in obstructed environments. Integrated navigation systems combining INS and GNSS are widely used due to their complementary advantages. Factor graph optimization (FGO) state estimation uses graphical models to encode the conditional probabilities between states and measurements to represent the batch estimation problem. Multiple iterative linearizations over longer time scales yield better consistency results and are increasingly being applied to information fusion in integrated navigation systems.

[0003] The optimal state estimation of FGO relies on accurate measurement models. However, in diverse and complex scenarios such as urban canyons, the gross errors in GNSS measurements and the time-varying statistical characteristics of noise significantly reduce the accuracy and reliability of state estimation. Various robust adaptive algorithms have been extensively studied. Robust estimation, which relies solely on single-point residual correction of measurement weights, lacks the ability to estimate measurement noise. Adaptive estimation has been extensively studied in filtering, with Bayesian methods being the most general and comprehensive. Adaptive methods based on variational Bayesian (VB) inference have good integration with various nonlinear filtering-based navigation methods. However, research on adaptive estimation based on FGO is relatively limited. Generally, it relies on innovation sequence estimation of covariance. To obtain reliable estimates, a large data window is required, resulting in high processing load and poor performance in tracking rapidly changing noise. Furthermore, these methods lack robustness against measurement gross errors. Therefore, researching a method that combines both accuracy and robustness in adaptive estimation is of practical significance. Summary of the Invention

[0004] The purpose of this invention is to address the shortcomings of the prior art by providing a robust adaptive integrated navigation method based on variational inference factor graphs. This method extends variational Bayesian inference to the factor graph optimization state estimation framework and combines it with an inter-frame innovation robustness strategy to achieve effective estimation of time-varying measurement noise under gross interference. It can provide reliable and accurate state estimation results in complex environments and has good adaptability.

[0005] This invention provides a robust adaptive integrated navigation method based on variational inference factor graphs, the method comprising the following steps:

[0006] Step 1 uses the IMU to perform a prior update, predicting the prior state and its covariance, and propagating the IW distribution parameters;

[0007] Step 2 calculates the predicted measurement covariance based on the average innovation of GNSS measurements between adjacent frames to identify gross errors;

[0008] Step 3 uses GNSS for iterative posterior update, corrects the posterior state and its covariance, and updates the IW distribution parameters.

[0009] As a further improvement of the present invention, step 1 includes:

[0010] (11) Predicting prior states using IMU measurement equations

[0011] In factor graph optimization, pre-integration techniques are used to decouple the attitude integral dependence between adjacent states in the INS. Multiple IMU sampled data over a period of time are integrated to generate equivalent relative constraints to adapt to the nonlinear optimization algorithm, based on the state at the previous time step. and pre-integral quantity Able to predict the current prior state

[0012]

[0013] In the formula, h IMU The measurement equations representing the pre-integration of the IMU;

[0014] (12) Add IMU factors to the factor plot to marginalize the solution of prior state covariance.

[0015] The sliding window of the factor plot retains multiple states. Let its window size be s+1, and the state covariance P k Using the Jacobian matrix J and covariance matrix W constructed from multiple factors within the sliding window, a marginal solution is performed based on the Schur complement:

[0016]

[0017]

[0018] In the formula, Λ represents the information matrix, and the subscript m = ks:k-1 indicates that the marginalized state involves t. k-s To t k-1 At time t, the subscript r = k indicates that the state being retained is t. k Time; prior state covariance P k|k-1 Obtained from IMU pre-integration factor prediction, the required J and W are defined as follows:

[0019]

[0020]

[0021] In the formula, and Let represent the residuals and covariance of the IMU pre-integration, respectively. The residuals are differentiated column-wise with respect to the state within the sliding window, and the Jacobian matrix is ​​expanded by row vectors. The covariance is directly expanded along the diagonal.

[0022] (13) IW distribution parameters of the covariance of the forgetting factor propagation measurement

[0023] Time-varying measurement covariance R k It is typically modeled as an inverse Vichter IW distribution, and the prior distribution parameters are propagated using a forgetting factor ρ:

[0024] v k|k-1 =ρ(v k-1 -n-1)+n+1

[0025] V k|k-1 =ρV k-1

[0026] In the formula, v k and V k These are the degrees of freedom parameters and inverse scaling matrix of the IW distribution, respectively. k It is a real number, n represents R k The measurement dimension.

[0027] As a further improvement of the present invention, step 2 includes:

[0028] (21) Construct the new information and its covariance based on measurements and predictions:

[0029]

[0030]

[0031] In the formula, s k and S k Let them represent the new information and its covariance, respectively. and h GNSS These are the measured values ​​and measurement functions of the GNSS factor, H. k It is h GNSS The corresponding residual Jacobian matrix;

[0032] (22) Calculate the predicted value of measurement covariance based on the average innovation between two adjacent frames.

[0033]

[0034]

[0035] In the formula, gross errors The value is relatively large and easy to determine; it can be estimated by averaging the adjacent times of the new information. Greater than the threshold R max At that time, the variational hindsight update is abandoned.

[0036] As a further improvement of the present invention, step 3 includes:

[0037] (31) Add GNSS factors to the factor plot to expand the Jacobian and covariance matrices.

[0038] The definitions of J and W after adding GNSS factors are as follows:

[0039]

[0040]

[0041] In the formula, the subscript ks:k|k-1 indicates that the factor graph is in t k IMU factors were added for prediction at time t, and the subscript ks:k indicates that the factor plot was at time t. k GNSS factors are constantly being added and updated;

[0042] (32) Nonlinear optimization solution of posterior state

[0043] Posterior state After iterative convergence using a nonlinear optimization algorithm, the following results were obtained:

[0044]

[0045]

[0046] In the formula, x represents all states within the sliding window of the factor graph, Δx is the corresponding state increment, [·] k Indicates the selection of t k The state at any given moment;

[0047] (33) Solve for the posterior state covariance P using the marginalization method in step (12). k

[0048] (34) Update the measurement covariance and its distribution parameters

[0049] The measurement covariance R is obtained by minimizing the Kullback-Leibler (KL) divergence between the true posterior distribution and the approximate posterior distribution. k and its distribution parameter V k Iterative update solution:

[0050] R k =V k / (v k -n-1)

[0051]

[0052] (35) Repeat steps (31) to (34) 3 to 5 times.

[0053] The beneficial effects of this invention are as follows: variational Bayesian inference is introduced into the factor graph-based integrated navigation algorithm, which can effectively estimate time-varying measurement noise in complex environments. The measurement covariance prediction value constructed based on inter-frame innovation can effectively identify gross errors and ensure the robustness of state estimation. The reasonable application of both improves the estimation accuracy and robustness of the system in complex environments. Attached Figure Description

[0054] Figure 1 Robust adaptive factor graph model based on variational Bayesian inference.

[0055] Figure 2 GNSS measurements with gross errors and time-varying noise.

[0056] Figure 3 Comparison of standard deviations of measurement noise.

[0057] Figure 4 Comparison of positional errors. Detailed Implementation

[0058] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the invention.

[0059] like Figure 1 As shown, the variational inference factor graph model proposed in this invention is roughly divided into three stages: prior prediction, gross error detection, and posterior update. After adding the IMU pre-integration factor, a prior factor graph within a small frame is formed, predicting various prior parameters in one step. Gross errors are eliminated using the IMU prior, GNSS measurements, and a threshold. Further addition of the GNSS factor forms a posterior factor graph within a larger frame, and iterative nonlinear optimization and marginalization are performed to update various posterior parameters.

[0060] Step 1 uses the IMU to perform a prior update, predicting the prior state and its covariance, and propagating the IW distribution parameters.

[0061] a. Predicting prior states using IMU measurement equations

[0062] In factor graph optimization, pre-integration techniques are used to decouple the attitude integral dependence between adjacent states in the INS. Multiple IMU sampled data over a period of time are integrated to generate equivalent relative constraints to adapt to the nonlinear optimization algorithm, based on the state at the previous time step. and pre-integral quantity Able to predict the current prior state

[0063]

[0064] h IMU The measurement equations represent the pre-integration equations of the IMU.

[0065] b. Add an IMU factor to the factor plot to marginalize the solution of the prior state covariance.

[0066] The sliding window of the factor plot retains multiple states. Let its window size be s+1, and the state covariance P k Using the Jacobian matrix J and covariance matrix W constructed from multiple factors within the sliding window, a marginal solution is performed based on the Schur complement:

[0067]

[0068]

[0069] Λ represents the information matrix, and the subscript m = ks:k-1 indicates that the marginalized state involves t. k-s To t k-1 At time t, the subscript r = k indicates that the state being retained is t. k Time. Prior state covariance P k|k-1 Obtained from IMU pre-integration factor prediction, the required J and W are defined as follows:

[0070]

[0071]

[0072] and Let represent the residuals and covariance of the IMU pre-integration, respectively. The residuals are differentiated column-wise with respect to the state within the sliding window, and the Jacobian matrix is ​​expanded by row vectors. The covariance is directly expanded along the diagonal.

[0073] c. IW distribution parameters of the covariance of the forgetting factor propagation measurement

[0074] Time-varying measurement covariance R k It is typically modeled as an inverse Vichter (IW) distribution, and the prior distribution parameters are propagated using a forgetting factor ρ:

[0075] v k|k-1 =ρ(v k-1 -n-1)+n+1

[0076] V k|k-1 =ρV k-1

[0077] v k and V kThese are the degrees of freedom parameters and inverse scaling matrix of the IW distribution, respectively. k It is a real number, n represents R k The measurement dimension.

[0078] Step 2: Calculate the predicted measurement covariance based on the average innovation of GNSS measurements between adjacent frames to identify gross errors.

[0079] a. Constructing new information and its covariance based on measurements and predictions:

[0080]

[0081]

[0082] s k and S k Let them represent the new information and its covariance, respectively. and h GNSS These are the measured values ​​and measurement functions of the GNSS factor, H. k It is h GNSS The corresponding residual Jacobian matrix.

[0083] b. Calculate the predicted measurement covariance based on the average innovation between two adjacent frames.

[0084]

[0085]

[0086] Gross The value is relatively large and easy to determine; it can be estimated by averaging the adjacent times of the new information. Greater than the threshold R max At that time, the variational hindsight update is abandoned.

[0087] Step 3 uses GNSS for iterative posterior update, corrects the posterior state and its covariance, and updates the IW distribution parameters.

[0088] a. Add GNSS factors to the factor plot to expand the Jacobian and covariance matrices.

[0089] The definitions of J and W after adding GNSS factors are as follows:

[0090]

[0091]

[0092] The subscript ks:k|k-1 indicates that the factor plot is in t k IMU factors were added for prediction at time t, and the subscript ks:k indicates that the factor plot was at time t. k GNSS factors are constantly being added and updated.

[0093] b. Nonlinear optimization solution of posterior state

[0094] Posterior state After iterative convergence using a nonlinear optimization algorithm, the following results were obtained:

[0095]

[0096]

[0097] x represents all states within the sliding window of the factor graph, Δx is the corresponding state increment, [·] k Indicates the selection of t k The state at any given moment.

[0098] c. Solve for the posterior state covariance P using the marginalization method from step 1.b. k

[0099] d. Update the measurement covariance and its distribution parameters

[0100] The measurement covariance R is obtained by minimizing the Kullback-Leibler (KL) divergence between the true posterior distribution and the approximate posterior distribution. k and its distribution parameter V k Iterative update solution:

[0101] R k =V k / (v k -n-1)

[0102]

[0103] e. Repeat steps 3.a to 3.d 3 to 5 times.

[0104] Finally, simulation experiments were conducted to verify the effectiveness of the method proposed in this invention:

[0105] The sensor simulation parameters are set as follows: the zero bias and random walk of the IMU gyroscope are 10 deg / h and... The zero bias and random walk of the IMU accelerometer were 40 μg and , respectively. The IMU outputs a frequency of 100Hz; the GNSS provides position measurements with a standard deviation of 1m and an output frequency of 1Hz. The M-estimation adaptive algorithm (MAFG) based on the Huber kernel function, the residual sliding window adaptive algorithm (SWAFG), and the variational Bayesian inference adaptive algorithm (VBAFG) proposed in this patent are compared.

[0106] like Figure 2As shown, a GNSS position measurement system with both time-varying noise and gross error was designed. The standard deviation of the GNSS noise changed abruptly to 10m between 400s and 800s, and was 1m in other time periods. A gross error with a standard deviation of 100m appeared with a 10% probability between 450s and 750s.

[0107] like Figure 3 As shown, a comparison of the measurement standard deviations of several algorithms is presented, where REF is the true value of the measurement standard deviation. MAFG has some discriminative power for outliers, which appear as spikes in the data. Figure 3 The results are presented in the table. SWAFG lacks robustness; gross errors differ significantly from the residual sequence within the sliding window, severely impacting the accuracy of its noise estimation. VBAFG combines variational inference with a robust mechanism, eliminating gross errors before the variational posterior update, thus possessing both robustness and adaptability.

[0108] like Figure 4 As shown, the position error curves of several algorithms are presented. SWAFG is most affected by gross errors and has obvious error fluctuations. MAFG does not have the ability to estimate measurement noise and its overall position accuracy is also poor. VBAFG has the smallest position error compared to the other two algorithms, and its estimation accuracy and stability are significantly improved.

[0109] The technical means disclosed in this invention are not limited to those disclosed in the above embodiments, but also include technical solutions composed of any combination of the above technical features. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of this invention, and these improvements and modifications are also considered within the scope of protection of this invention.

Claims

1. A robust adaptive integrated navigation method based on variational inference factor graphs, characterized in that, Includes the following steps: Step 1: Use the IMU to perform a prior update, predict the prior state and its covariance, and propagate the IW distribution parameters; Step 2: Calculate the predicted measurement covariance based on the average innovation of GNSS measurements between adjacent frames to identify gross errors; Step 3: Use GNSS to perform iterative posterior update, correct the posterior state and its covariance, and update the IW distribution parameters; Step 1 includes: (11) Predicting prior states using IMU measurement equations In factor graph optimization, pre-integration techniques are used to decouple the attitude integral dependence between adjacent states in the INS. Multiple IMU sampled data over a period of time are integrated to generate equivalent relative constraints to adapt to the nonlinear optimization algorithm, based on the state at the previous time step. and pre-integral quantity Able to predict the current prior state : In the formula, The measurement equations representing the IMU pre-integration; (12) Add IMU factors to the factor plot to marginalize the solution of prior state covariance. The sliding window of the factor graph retains multiple states; let its window size be... State covariance Jacobian matrix constructed using multiple factors within a sliding window Covariance Matrix Marginalization solution based on Shur complement: In the formula, Information matrix, subscript The state of marginalization involves to Time, Subscript The state is reserved. Time; prior state covariance Obtained by IMU pre-integration factor prediction, its required and Defined as: In the formula, and Let represent the residuals and covariance of the IMU pre-integration, respectively. The residuals are differentiated column-wise with respect to the state within the sliding window, and the Jacobian matrix is ​​expanded by row vectors. The covariance is directly expanded along the diagonal. (13) IW distribution parameters of the covariance of the forgetting factor propagation measurement Time-varying measurement covariance It is typically modeled as an inverse Vichter IW distribution and uses a forgetting factor. Propagation prior distribution parameters: In the formula, and These are the degrees of freedom parameters and inverse scaling matrix of the IW distribution, respectively. It is a real number. express The measurement dimension; Step 2 includes: (21) Construct the innovation and its covariance based on the measurement and prediction: In the formula, and Let them represent the new information and its covariance, respectively. and These are the measured values ​​and measurement functions of the GNSS factor, respectively. yes The corresponding residual Jacobian matrix; (22) Calculate the measurement covariance prediction value based on the average innovation between two adjacent frames. In the formula, gross errors The value is relatively large and easy to determine; it can be estimated by averaging the adjacent times of the new information. Greater than the threshold At that time, the variational hindsight update is abandoned.

2. The robust adaptive integrated navigation method based on variational inference factor graphs according to claim 1, characterized in that, Step 3 includes: (31) Add GNSS factors to the factor plot to expand the Jacobian and covariance matrices. After adding GNSS factors and Defined as: In the formula, the subscript Representing factor plots in IMU factors were added for prediction at all times, subscript Representing factor plots in GNSS factors are constantly being added and updated; (32) Nonlinear optimization solution of posterior state Posterior state After iterative convergence using a nonlinear optimization algorithm, the following results were obtained: In the formula, It represents all states within the sliding window of the factor graph. It is the corresponding state increment. Indicates selection The state at any given moment; (33) Solve for the posterior state covariance using the marginalization method in step (12). (34) Update the measurement covariance and its distribution parameters The measurement covariance is obtained by minimizing the KL divergence between the true posterior distribution and the approximate posterior distribution. and its distribution parameters Iterative update solution: (35) Repeat steps (31) to (34) 3 to 5 times.

Images