Robust factor graph optimization integrated navigation method based on adaptive MCMC

Abstract

The present invention provides a robust factor graph combined navigation method based on adaptive MCMC (Markov Chain Monte Carlo). In the INS / GNSS combined navigation system, factors such as multipath effect and non-line-of-sight will greatly reduce the positioning accuracy of the satellite, thereby affecting the effect of the entire combined navigation system. The presence of many canyon environments in urban traffic greatly affects the positioning accuracy of vehicles. Therefore, it is necessary to study a combined navigation method with both anti-error performance and estimation accuracy. A robust factor graph combined navigation method based on adaptive MCMC is proposed. First, adaptive MCMC is introduced into the factor graph optimization framework based on the prior and posterior stages to accurately estimate the GNSS time-varying measurement noise covariance; secondly, the new information χ is introduced 2 A detection algorithm is used to detect and eliminate gross errors. The proposed method can effectively estimate the GNSS time-varying measurement noise while reducing the interference of gross errors. Compared with the robust adaptive factor graph algorithm based on M estimation and the robust factor graph integrated navigation algorithm based on variational Bayesian, the positioning accuracy is significantly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of integrated navigation, and in particular to a robust factor graph integrated navigation method based on adaptive MCMC. Background Art

[0002] Integrated navigation generally refers to a technique where two or more navigation systems with complementary measurement characteristics measure the same information source to achieve higher navigation accuracy. Typically, one navigation system provides information with high short-term accuracy, while the other provides information with high long-term stability. A classic integrated navigation method is INS / GNSS integrated navigation. INS has a high frequency, but due to integration, errors accumulate. GNSS, on the other hand, has lower short-term accuracy and output frequency.

[0003] Integrated navigation methods typically include filtering-based approaches, such as the Kalman filter and its variants, and optimization-based approaches, such as factor graphs. While the Kalman filter has a simple formula and strong real-time performance, it is based on a first-order Markov model and does not consider historical data. Factor graph optimization (FGO) considers all historical information, thus improving estimation accuracy.

[0004] An accurate measurement model is crucial in factor graph optimization methods. However, in complex environments such as urban canyons, the presence of GNSS measurement errors and time-varying measurement noise significantly impacts the reliability of state estimation. Two main approaches address this issue: one is the use of robust cost functions, including the Huber cost function and dynamic covariance scaling. These robustness methods improve the system's interference tolerance at the expense of accuracy, but they do not estimate measurement noise, often resulting in suboptimal results. The other approach directly estimates the covariance matrix of the measurement noise, including the PROBE-based adaptive covariance estimation algorithm and the variational Bayesian robust estimation algorithm. However, these methods often rely on prior distributions of posterior probabilities or struggle with small measurement noise. Therefore, a method is urgently needed to predict time-varying measurement noise and enhance system robustness in complex urban canyon environments. Therefore, research on robust factor graph integrated navigation methods based on adaptive MCMC is of great significance. Summary of the Invention

[0005] In response to the current problems in modeling GNSS time-varying measurement noise, the present invention provides a robust factor graph integrated navigation method based on adaptive MCMC. This method introduces adaptive MCMC into the factor graph optimization framework based on a priori and posterior two-stage updates to estimate time-varying measurement noise. 2 Detection algorithms detect and eliminate gross errors by constructing hypothesis test statistics and measuring abnormal boundary conditions.

[0006] To achieve the above objectives, the technical solutions of the present invention are as follows:

[0007] A robust factor graph optimization combined navigation method based on adaptive MCMC, the method comprising the following steps:

[0008] Step 1: Install IMU, GNSS, RTK and other sensors on the vehicle to collect relevant data.

[0009] Step 2: Input the data obtained by IMU and GNSS into the robust factor graph integrated navigation algorithm based on adaptive MCMC, which includes the prior update of IMU prediction, the posterior update of GNSS measurement and the new information χ 2 Detection algorithm.

[0010] Step 3: The robust factor graph combined navigation algorithm based on adaptive MCMC outputs the combined results and compares them with the two-dimensional plane positioning data collected by RTK to verify the effectiveness and accuracy of the proposed algorithm. Evaluation metrics include RMSE (root mean square error) and STD (standard deviation). Specifically, the easting, northing, and skyward position data for all epochs after factor graph optimization are converted to the corresponding easting, northing, and skyward position data of RTK, and the RMSE and STD are calculated. The superiority of the proposed algorithm is verified by comparing the reduction in RMSE and STD indicators compared to other classic algorithms.

[0011] Among them, the specific steps of the adaptive MCMC-based robust factor graph optimization integrated navigation algorithm included in step 2 are:

[0012] ① Prior update of IMU prediction: update the prior probability p(x k |z 1:k-1 ) is modeled as a Gaussian distribution:

[0013]

[0014] Among them, x k represents the system state at time k, z 1:k-1 Represents t1 to t k-1 The measurement of Represents the prior state estimate at time k, P k|k-1 Represents the prior state covariance matrix at time k. Given by the system state at time k-1 and pre-integration:

[0015]

[0016] represents the posterior state estimate at time k-1, represents IMU pre-integration, h IMURepresents the IMU pre-integrated measurement function.

[0017] State covariance matrix P k The Jacobian matrix J and covariance matrix W in the factor graph sliding window are marginalized based on the Schur complement. Assuming the sliding window size is S+1, we have:

[0018]

[0019]

[0020] Where Λ is the information matrix, and the prior state covariance matrix P k|k-1 The required Jacobian matrix J is obtained by prediction based on the pre-integration factor k-s:k|k-1 and the covariance matrix W k-s:k|k-1 Defined as:

[0021]

[0022]

[0023] The subscript m=ks:k-1 indicates that the marginalized state is from t k-s Time to t k-1 At this moment, the subscript r=k means that t k The state is kept as is. k IMU Represents the residual of the IMU pre-integration factor, x k-s:k Represents the system state from time ks to time k. The residual of the IMU pre-integration factor is derived column by column with respect to the vector in the sliding window, and the Jacobian matrix J is expanded in the form of a row vector. The residual covariance of the IMU pre-integration factor is directly expanded in the form of a diagonal covariance matrix W, then Λ k|k-1 and P k|k-1 Can be expressed as:

[0024] Λ k|k-1 =J k-s:k|k-1 W k-s:k|k-1 -1 J k-s:k|k-1

[0025] P k|k-1 =(Λ k|k-1 22 -Λ k|k-1 21 (Λ k|k-1 11 ) -1 Λ k|k-1 12 ) -1

[0026] where Λ k|k-122 , Λ k|k-1 21 , Λ k|k-1 11 , Λ k|k-1 12 Represents Λ k|k-1 The element values ​​of the matrix with 2 rows and 2 columns, 2 rows and 1 column, 1 row and 1 column, and 1 row and 2 columns.

[0027] In order to estimate the time-varying measurement covariance R k , here we use the adaptive MCMC algorithm to sample the posterior probability, and estimate R by the sampled samples k The statistical characteristics of . The following expression is obtained from the Bayesian formula:

[0028]

[0029] p(R k |z 1:k-1 ) is the probability to be sought, that is, the posterior probability, which can be converted into the prior probability p(R k ) and the likelihood probability p(z 1:k-1 |R k ) and then divided by a constant ∫p(R k )p(z 1:k-1 |R k )dR k , due to the constant ∫p(R k )p(z 1:k-1 |R k )dR k The existence of only expands or reduces the probability of each point by a certain multiple, and does not affect the relative size of the probability of the sample point value. Therefore, we can k )p(z 1:k-1 |R k ) is sampled and p(R k |z 1:k-1 )’s statistical properties.

[0030] Assume that the measurement noise is Gaussian white noise and obeys the following normal distribution:

[0031] v k ~N(0,R k )

[0032] v k is the measurement noise, and the measurement noise covariance R k Decomposed into the following form:

[0033] R k =D k Σ k D k

[0034] Among them D k is a diagonal matrix containing the standard deviation of each variable, usually modeled as an inverse gamma distribution; k is a correlation matrix, usually modeled as a LKJ distribution (LKJ distribution, Lewandowski-Kurowicka-Joedistribution). k , has the following inverse gamma distribution:

[0035] D k ~IG(α,β)

[0036] α is the shape parameter that controls the shape of the distribution. The larger α is, the more concentrated the distribution is around the mean; when α is small, the distribution has a heavy-tail characteristic. β is the scale parameter that determines the scalability and mean of the distribution. When β is large, the distribution shifts to the right as a whole; when β is small, the distribution shifts to the left. For the inverse gamma distribution, p(D k ) is as follows:

[0037]

[0038] where Γ(α) is the gamma function and x is a random variable greater than 0.

[0039] For the correlation matrix Σ k , there is the following LKJ distribution:

[0040] Σ k ~LKJ(η)

[0041] Where η is another shape parameter used to control the concentration of the distribution, usually between 0.5 and 1.5. k ) is as follows:

[0042] p(Σ k )=c d |Σ k | η-1

[0043] c d is a normalizing constant that makes the distribution integral equal to 1, and its expression is as follows:

[0044]

[0045] d is the order of the matrix, K is the block matrix correlation coefficient, and B(a,b) is the Beta function.

[0046] Combining the above analysis, we can get the prior probability p(R k ) is:

[0047]

[0048] σ k It's D k The diagonal elements of the matrix are also the standard deviations of each variable.

[0049] For p(z 1:k-1 |R k ), which is usually modeled as a multivariate normal distribution, that is:

[0050] p(z 1:k-1 |R k )~N(μ 1:k-1 ,R 1:k-1 )

[0051] where μ 1:k-1 is the mean and R 1:k-1 is the covariance matrix and R 1:k-1 =diag(R1,R2,...,R k-1 ), assuming the measured value z 1:k-1 The noises of are independent and obey the normal distribution, then the joint likelihood probability p(z 1:k-1 |R k ) can be expressed as:

[0052]

[0053] Where n is the number of observations z t The dimension, R k is the measurement noise covariance matrix at the kth moment.

[0054] Since the prior probability does not depend on the probability distribution of the observed data and is given in advance, the prior probability and the likelihood probability are independent of each other. Therefore, a new distribution can be defined based on the product of the prior probability and the likelihood probability, which is here called the Mine distribution. Then the posterior probability p(R k |z 1:k-1 ) also obeys the Mine distribution, and has:

[0055]

[0056] The distribution that satisfies the above probability density function is called Mine distribution. The following steps are used to sample the Mine distribution using the adaptive MCMC algorithm.

[0057] Input: arbitrarily select the state transition matrix Q of the Markov chain, the target stationary distribution Mine distribution, set the state transition threshold to n1, and the required number of samples to n2.

[0058] 1. Sample the initial value from any simple probability distribution

[0059] 2. for t=0 to n1+n2-1:

[0060] (a) From the conditional probability distribution Samples are obtained by sampling

[0061] (b) Sample u~U[0,1] from the uniform distribution, where U[0,1] represents the uniform distribution on 0~1.

[0062] (c) If Accept the transfer Right now is the acceptance rate, meaning from the state by The probability of jumping to state When we The probability of accepting this transfer.

[0063] (d) Otherwise, the transfer will not be accepted.

[0064] Output: Sample set That is the sample set corresponding to the required target stationary distribution Mine distribution.

[0065] In order to avoid (c) In the case where most of the sample values ​​are rejected for transfer due to the small value, the MH sampling algorithm is used to solve the problem of low sampling acceptance rate in MCMC.

[0066] The improved MCMC sampling steps based on the MH sampling algorithm are as follows:

[0067] Input: arbitrarily select the state transition matrix Q of the Markov chain, the target stationary distribution Mine distribution, set the state transition threshold to n1, and the required number of samples to n2.

[0068] 1. Sample the initial value from any simple probability distribution

[0069] 2. for t=0 to n1+n2-1:

[0070] (a) From the conditional probability distribution Samples are obtained by sampling

[0071] (b) Sample u~U[0,1] from the uniform distribution, where U[0,1] represents the uniform distribution on 0~1.

[0072] (c) If Accept the transfer Right now

[0073] (d) Otherwise, the transfer will not be accepted.

[0074] Output: Sample set That is the sample set corresponding to the required target stationary distribution Mine distribution.

[0075] Based on the MH framework, the conditional probability distribution is dynamically adjusted using historical sampling information. To improve sampling efficiency. The specific adaptive strategies are as follows:

[0076] 1. Dynamically adjust the step size ε

[0077] According to the acceptance rate Adjusting the conditional probability distribution The step size ε.

[0078]

[0079] Among them, 0.23 to 0.44 is the common target acceptance rate range.

[0080] 2. Update the conditional probability distribution using the sampled covariance matrix:

[0081] (a) Calculate the covariance matrix C through historical samples t

[0082]

[0083] (b) The conditional probability distribution Adjusted to:

[0084]

[0085] (c) In the update C t , regularization is introduced to ensure numerical stability:

[0086] C t '=(1-λ)C t +λI

[0087] Among them, λ is a small positive value, generally λ = 0.01, and I is the unit matrix.

[0088] 3. After sampling is completed, calculate the autocorrelation of the Markov chain and the number of effective samples to verify the sampling quality.

[0089] By introducing the MH sampling algorithm and adaptive strategy, the sample set after MCMC sampling can be obtained By calculating the statistical characteristics of the sample set, the posterior probability p(R k |z1:k-1 ) satisfies the mathematical properties.

[0090] ②Posteriori update of GNSS measurements: By updating the prior of IMU predictions, the joint posterior probability p(x k ,R k |z 1:k ), assuming the joint posterior probability p(x k ,R k |z 1:k ) can be approximated as the product of Gaussian distribution and Mine distribution. Gaussian distribution is expressed as N(x k ), Mine distribution is expressed as Mine(R k ).

[0091] p(x k ,R k |z 1:k )≈N(x k )Mine(R k )

[0092] in Mine(R k )=Mine(R k |α,β,η). P k are the posterior state estimate and the posterior state covariance matrix at time k respectively. KL divergence is used to measure the true posterior p(x k ,R k |z 1:k ) and the approximate posterior N(x k )Mine(R k ) of the match.

[0093]

[0094] R can be obtained by minimizing the above KL divergence k The best estimate of .

[0095] Posterior state estimate After iterative convergence through nonlinear optimization algorithm, we can obtain:

[0096]

[0097]

[0098] Where x is the total state within the sliding window of the factor graph, Δx is the increment of the state, [x+Δx] k Indicates the selection of t k The state at the moment. The posterior state estimate and the posterior state covariance matrix P kIt is obtained by using the IMU factor for prior update and then adding the GNSS factor for optimization and solution. The above Jacobian matrix J and covariance matrix W are defined as:

[0099]

[0100] The subscript ks:k|k-1 indicates that the factor graph is in t k-1 The IMU factor is added for prediction at time t. The subscript ks:k indicates that the factor graph is at t k GNSS factors are added for updates at all times.

[0101] ③ Gross error detection and elimination: The existence of gross error in measurement will greatly affect the accuracy of the measured value. This will in turn affect the estimation of the system state. Therefore, it is necessary to use appropriate methods to detect and eliminate gross errors. Here we introduce the new information χ 2 The detection algorithm filters out gross errors. Construct new information V based on the measurement vector and prediction vector k and its covariance matrix

[0102]

[0103] ∑ Vk =H k P k|k-1 H k T +R k

[0104] Among them, Z k The measurement vector is the difference between the position output by GNSS and the position calculated by IMU mechanical arrangement. k is the observation matrix, is the prior error state vector at time k, P k|k-1 is the prior state covariance matrix, which has been obtained in the prior update stage of IMU prediction, R k is the covariance matrix of the measurement noise, V k is the innovation vector, is the covariance matrix of the innovation vector.

[0105] Generally speaking, the new information is a white noise sequence that follows a normal distribution with a mean of zero:

[0106]

[0107] When the measured value is normal, the new information is a white noise sequence with a mean of zero; when the measured value is abnormal, the mean of the new information is no longer zero. The above test conditions are transformed into a hypothesis test problem:

[0108]

[0109] Among them, H0 represents the null hypothesis, indicating that the measured value is not abnormal; H1 is the alternative hypothesis, indicating that the measured value is abnormal. The hypothesis test statistic is:

[0110]

[0111] T k Subject to χ with t degrees of freedom 2 Distribution, according to χ 2 The definition of the distribution has the following hypothesis tests:

[0112]

[0113] Where λ is the non-centrality parameter. If the significance level is α, the boundary condition for determining whether measurement anomaly occurs is:

[0114]

[0115] T D is the measurement abnormal boundary value, if T k ≤T D , then it is considered that there is no measurement abnormality. If T k >T D , it is considered that there is a measurement anomaly at this time, and this anomaly is determined as a measurement gross error and eliminated.

[0116] An electronic device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein the processor implements the adaptive MCMC-based robust factor graph optimization combined navigation method when executing the program.

[0117] A computer-readable storage medium having computer instructions stored thereon, which, when executed by a processor, implement the robust factor graph optimization combined navigation method based on adaptive MCMC.

[0118] Based on the above technical solution, the present invention has the following beneficial technical effects:

[0119] 1) Using the new information χ 2 The detection algorithm detects and eliminates gross errors, thereby improving the position accuracy of the integrated navigation results.

[0120] 2) Adaptive MCMC is used to sample the product of the prior probability and the likelihood probability, thereby indirectly obtaining samples of the distribution satisfied by the posterior probability, and then obtaining the mathematical statistical characteristics of the posterior probability, which is more accurate than directly modeling the posterior probability. k Modeling as R k =D k Σk D k The form of, and for the first time introduced LKJ distribution into the correlation matrix Σ k In the modeling of , it is more accurate than directly modeling the posterior probability as an inverse Wissart distribution.

[0121] 3) Use KL divergence to measure the degree of match between the true posterior and the approximate posterior, quantify the distribution difference, pay attention to whether the support area of ​​the approximate distribution covers the true distribution, and calculate the measurement noise covariance R k estimated value. BRIEF DESCRIPTION OF THE DRAWINGS

[0122] Figure 1 This is the framework diagram of the Adaptive MCMC-FGO algorithm;

[0123] Figure 2 This is the flow chart of the adaptive MCMC algorithm. DETAILED DESCRIPTION

[0124] The following will use the open source dataset UrbanNavDataset as an example, combined with the accompanying drawings in the embodiments of the present invention, to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of them. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0125] Example 1: Figure 1 As shown, a robust factor graph optimization combined navigation method based on adaptive MCMC includes the following steps:

[0126] Step 1: Install sensors such as IMU, GNSS, and RTK on the vehicle to collect relevant data. Here, we use IMU, GNSS, and RTK data from the Hong Kong dataset provided by the open-source UrbanNavDataset.

[0127] Step 2: Input the data obtained by IMU and GNSS into the robust factor graph integrated navigation algorithm based on adaptive MCMC, which includes the prior update of IMU prediction, the posterior update of GNSS measurement and the new information χ 2 Detection algorithm.

[0128] Step 3: The robust factor graph combined navigation algorithm based on adaptive MCMC outputs the combined results and compares them with the two-dimensional plane positioning data collected by RTK to verify the effectiveness and accuracy of the proposed algorithm. Evaluation metrics include RMSE (root mean square error) and STD (standard deviation). Specifically, the easting, northing, and skyward position data for all epochs after factor graph optimization are converted to the corresponding easting, northing, and skyward position data of RTK, and the RMSE and STD are calculated. The superiority of the proposed algorithm is verified by comparing the reduction in RMSE and STD indicators compared to other classic algorithms.

[0129] Among them, the specific steps of the adaptive MCMC-based robust factor graph optimization integrated navigation algorithm included in step 2 are:

[0130] ① Prior update of IMU prediction: update the prior probability p(x k |z 1:k-1 ) is modeled as a Gaussian distribution:

[0131]

[0132] Among them, x k represents the system state at time k, z 1:k-1 Represents t1 to t k-1 The measurement of Represents the prior state estimate at time k, P k|k-1 Represents the prior state covariance matrix at time k. Given by the system state at time k-1 and pre-integration:

[0133]

[0134] represents the posterior state estimate at time k-1, represents IMU pre-integration, h IMU Represents the IMU pre-integrated measurement function.

[0135] State covariance matrix P k The Jacobian matrix J and covariance matrix W in the factor graph sliding window are marginalized based on the Schur complement. Assuming the sliding window size is S+1, we have:

[0136]

[0137]

[0138] Where Λ is the information matrix, and the prior state covariance matrix P k|k-1 The required Jacobian matrix J is obtained by prediction based on the pre-integration factor k-s:k|k-1 and the covariance matrix W k-s:k|k-1Defined as:

[0139]

[0140]

[0141] The subscript m=ks:k-1 indicates that the marginalized state is from t k-s Time to t k-1 At this moment, the subscript r=k means that t k The state is kept as is. k IMU Represents the residual of the IMU pre-integration factor, x k-s:k Represents the system state from time ks to time k. The residual of the IMU pre-integration factor is derived column by column with respect to the vector in the sliding window, and the Jacobian matrix J is expanded in the form of a row vector. The residual covariance of the IMU pre-integration factor is directly expanded in the form of a diagonal covariance matrix W, then Λ k|k-1 and P k|k-1 Can be expressed as:

[0142] Λ k|k-1 =J k-s:k|k-1 W k-s:k|k-1 -1 J k-s:k|k-1

[0143] P k|k-1 =(Λ k|k-1 22 -Λ k|k-1 21 (Λ k|k-1 11 ) -1 Λ k|k-1 12 ) -1

[0144] where Λ k|k-1 22 , Λ k|k-1 21 , Λ k|k-1 11 , Λ k|k-1 12 Represents Λ k|k-1 The element values ​​of the matrix with 2 rows and 2 columns, 2 rows and 1 column, 1 row and 1 column, and 1 row and 2 columns.

[0145] In order to estimate the time-varying measurement covariance R k , here we use the adaptive MCMC algorithm to sample the posterior probability, and estimate R by the sampled samples k The statistical characteristics of . The following expression is obtained from the Bayesian formula:

[0146]

[0147] p(R k |z 1:k-1 ) is the probability to be sought, that is, the posterior probability, which can be converted into the prior probability p(R k ) and the likelihood probability p(z 1:k-1 |R k ) and then divided by a constant ∫p(R k )p(z 1:k-1 |R k )dR k , due to the constant ∫p(R k )p(z 1:k-1 |R k )dR k The existence of only expands or reduces the probability of each point by a certain multiple, and does not affect the relative size of the probability of the sample point value. Therefore, we can k )p(z 1:k-1 |R k ) is sampled and p(R k |z 1:k-1 )’s statistical properties.

[0148] Assume that the measurement noise is Gaussian white noise and obeys the following normal distribution:

[0149] v k ~N(0,R k )

[0150] v k is the measurement noise, and the measurement noise covariance R k Decomposed into the following form:

[0151] R k =D k Σ k D k

[0152] Among them D k is a diagonal matrix containing the standard deviation of each variable, usually modeled as an inverse gamma distribution; k is a correlation matrix, usually modeled as a LKJ distribution (LKJ distribution, Lewandowski-Kurowicka-Joedistribution). k , has the following inverse gamma distribution:

[0153] D k ~IG(α,β)

[0154] α is the shape parameter that controls the shape of the distribution. The larger α is, the more concentrated the distribution is around the mean; when α is small, the distribution has a heavy-tail characteristic. β is the scale parameter that determines the scalability and mean of the distribution. When β is large, the distribution shifts to the right as a whole; when β is small, the distribution shifts to the left. For the inverse gamma distribution, p(D k ) is as follows:

[0155]

[0156] where Γ(α) is the gamma function and x is a random variable greater than 0.

[0157] For the correlation matrix Σ k , there is the following LKJ distribution:

[0158] Σ k ~LKJ(η)

[0159] Where η is another shape parameter used to control the concentration of the distribution, usually between 0.5 and 1.5. k ) is as follows:

[0160] p(Σ k )=c d |Σ k | η-1

[0161] c d is a normalizing constant that makes the distribution integral equal to 1, and its expression is as follows:

[0162]

[0163] d is the order of the matrix, K is the block matrix correlation coefficient, and B(a,b) is the Beta function.

[0164] Combining the above analysis, we can get the prior probability p(R k ) is:

[0165]

[0166] σ k It's D k The diagonal elements of the matrix are also the standard deviations of each variable.

[0167] For p(z 1:k-1 |R k ), which is usually modeled as a multivariate normal distribution, that is:

[0168] p(z 1:k-1 |R k )~N(μ 1:k-1 ,R 1:k-1 )

[0169] where μ 1:k-1 is the mean and R 1:k-1 is the covariance matrix and R 1:k-1 =diag(R1,R2,...,R k-1 ), assuming the measured value z 1:k-1 The noises of are independent and obey the normal distribution, then the joint likelihood probability p(z 1:k-1 |R k ) can be expressed as:

[0170]

[0171] Where n is the number of observations z t The dimension, R k is the measurement noise covariance matrix at the kth moment.

[0172] Since the prior probability does not depend on the probability distribution of the observed data and is given in advance, the prior probability and the likelihood probability are independent of each other. Therefore, a new distribution can be defined based on the product of the prior probability and the likelihood probability, which is here called the Mine distribution. Then the posterior probability p(R k |z 1:k-1 ) also obeys the Mine distribution, and has:

[0173]

[0174] The distribution that satisfies the above probability density function is called Mine distribution. The following steps are used to sample the Mine distribution using the adaptive MCMC algorithm.

[0175] Input: arbitrarily select the state transition matrix Q of the Markov chain, the target stationary distribution Mine distribution, set the state transition threshold to n1, and the required number of samples to n2.

[0176] 1. Sample the initial value from any simple probability distribution

[0177] 2. for t=0 to n1+n2-1:

[0178] (a) From the conditional probability distribution Samples are obtained by sampling

[0179] (b) Sample u~U[0,1] from the uniform distribution, where U[0,1] represents the uniform distribution on 0~1.

[0180] (c) If Accept the transfer Right now is the acceptance rate, meaning from the state The probability of jumping to state When we The probability of accepting this transfer.

[0181] (d) Otherwise, the transfer will not be accepted.

[0182] Output: Sample set That is the sample set corresponding to the required target stationary distribution Mine distribution.

[0183] In order to avoid (c) In the case where most of the sample values ​​are rejected for transfer due to the small value, the MH sampling algorithm is used to solve the problem of low sampling acceptance rate in MCMC.

[0184] The improved MCMC sampling steps based on the MH sampling algorithm are as follows:

[0185] Input: arbitrarily select the state transition matrix Q of the Markov chain, the target stationary distribution Mine distribution, set the state transition threshold to n1, and the required number of samples to n2.

[0186] 1. Sample the initial value from any simple probability distribution

[0187] 2. for t=0 to n1+n2-1:

[0188] (a) From the conditional probability distribution Samples are obtained by sampling

[0189] (b) Sample u~U[0,1] from the uniform distribution, where U[0,1] represents the uniform distribution on 0~1.

[0190] (c) If Accept the transfer Right now

[0191] (d) Otherwise, the transfer will not be accepted.

[0192] Output: Sample set That is the sample set corresponding to the required target stationary distribution Mine distribution.

[0193] Based on the MH framework, the conditional probability distribution is dynamically adjusted using historical sampling information. To improve sampling efficiency. The specific adaptive strategies are as follows:

[0194] 1. Dynamically adjust the step size ε

[0195] According to the acceptance rate Adjusting the conditional probability distribution The step size ε.

[0196]

[0197] Among them, 0.23 to 0.44 is the common target acceptance rate range.

[0198] 2. Update the conditional probability distribution using the sampled covariance matrix:

[0199] (a) Calculate the covariance matrix C through historical samples t

[0200]

[0201] (b) The conditional probability distribution Adjusted to:

[0202]

[0203] (c) In the update C t , regularization is introduced to ensure numerical stability:

[0204] C t '=(1-λ)C t +λI

[0205] Among them, λ is a small positive value, generally λ = 0.01, and I is the unit matrix.

[0206] 3. After sampling is completed, calculate the autocorrelation of the Markov chain and the number of effective samples to verify the sampling quality.

[0207] By introducing the MH sampling algorithm and adaptive strategy, the sample set after MCMC sampling can be obtained By calculating the statistical characteristics of the sample set, the posterior probability p(R k |z 1:k-1 ) satisfies the mathematical properties, the flowchart of the improved adaptive MCMC algorithm based on MH sampling is as follows Figure 2 shown.

[0208] ②Posteriori update of GNSS measurements: By updating the prior of IMU predictions, the joint posterior probability p(x k ,R k |z 1:k ), assuming the joint posterior probability p(x k ,R k |z 1:k ) can be approximated as the product of Gaussian distribution and Mine distribution. Gaussian distribution is expressed as N(x k), Mine distribution is expressed as Mine(R k ).

[0209] p(x k ,R k |z 1:k )≈N(x k )Mine(R k )

[0210] in Mine(R k )=Mine(R k |α,β,η). P k are the posterior state estimate and the posterior state covariance matrix at time k respectively. KL divergence is used to measure the true posterior p(x k ,R k |z 1:k ) and the approximate posterior N(x k )Mine(R k ) of the match.

[0211]

[0212] R can be obtained by minimizing the above KL divergence k The best estimate of .

[0213] Posterior state estimate After iterative convergence through nonlinear optimization algorithm, we can obtain:

[0214]

[0215] Where x is the total state within the sliding window of the factor graph, Δx is the increment of the state, [x+Δx] k Indicates the selection of t k The state at the moment. The posterior state estimate and the posterior state covariance matrix P k It is obtained by using the IMU factor for prior update and then adding the GNSS factor for optimization and solution. The above Jacobian matrix J and covariance matrix W are defined as:

[0216]

[0217]

[0218] The subscript ks:k|k-1 indicates that the factor graph is in t k-1 The IMU factor is added for prediction at time t. The subscript ks:k indicates that the factor graph is at t k GNSS factors are added for updates at all times.

[0219] ③ Gross error detection and elimination: The existence of gross error in measurement will greatly affect the accuracy of the measured value. This will in turn affect the estimation of the system state. Therefore, it is necessary to use appropriate methods to detect and eliminate gross errors. Here we introduce the new information χ 2 The detection algorithm filters out gross errors. Construct new information V based on the measurement vector and prediction vector k and its covariance matrix

[0220]

[0221]

[0222] Among them, Z k The measurement vector is the difference between the position output by GNSS and the position calculated by IMU mechanical arrangement. k is the observation matrix, is the prior error state vector at time k, P k|k-1 is the prior state covariance matrix, which has been obtained in the prior update stage of IMU prediction, R k is the covariance matrix of the measurement noise, V k is the innovation vector, is the covariance matrix of the innovation vector.

[0223] Generally speaking, the new information is a white noise sequence that follows a normal distribution with a mean of zero:

[0224]

[0225] When the measured value is normal, the new information is a white noise sequence with a mean of zero; when the measured value is abnormal, the mean of the new information is no longer zero. The above test conditions are transformed into a hypothesis test problem:

[0226]

[0227] Among them, H0 represents the null hypothesis, indicating that the measured value is not abnormal; H1 is the alternative hypothesis, indicating that the measured value is abnormal. The hypothesis test statistic is:

[0228]

[0229] T k Subject to χ with t degrees of freedom 2 Distribution, according to χ 2 The definition of the distribution has the following hypothesis tests:

[0230]

[0231] Where λ is the non-centrality parameter. If the significance level is α, the boundary condition for determining whether measurement anomaly occurs is:

[0232]

[0233] T D is the measurement abnormal boundary value, if T k ≤T D , then it is considered that there is no measurement abnormality. If T k >T D , it is considered that there is a measurement anomaly at this time, and this anomaly is determined as a measurement gross error and eliminated.

[0234] In summary, this paper proposes a robust factor graph integrated navigation method based on adaptive MCMC, which effectively estimates the time-varying GNSS measurement noise while reducing gross errors. In a dynamically changing environment without prior knowledge, the proposed algorithm can relatively accurately estimate the GNSS measurement noise covariance matrix. Compared with robust adaptive factor graph algorithms based on M-estimation and robust factor graph integrated navigation algorithms based on variational Bayesian, this method achieves a better balance between robustness and accuracy.

Claims

1. A robust factor graph combined navigation method based on adaptive MCMC, characterized by: The method comprises the following steps: Step 1: Install IMU, GNSS, and RTK sensors on the vehicle to collect relevant data. Step 2: Input the data obtained by IMU and GNSS into the robust factor graph integrated navigation algorithm based on adaptive MCMC, which includes the prior update of IMU prediction, the posterior update of GNSS measurement and the new information χ 2 Detection algorithm, Step 3: The robust factor graph combined navigation algorithm based on adaptive MCMC outputs the combined results and compares them with the two-dimensional plane positioning data collected by RTK to verify the effectiveness and accuracy of the proposed algorithm. The evaluation indicators include RMSE and standard deviation (STD). The specific process is to convert the easting, northing, and sky position data of all epochs after factor graph optimization into the easting, northing, and sky position data corresponding to RTK and calculate the RMSE and STD. A new distribution is defined based on the product of the prior probability and the likelihood probability, which is called Mine distribution. Then the posterior probability p(R k |z 1:k-1 ) also obeys the Mine distribution, The improved MCMC sampling steps based on the MH sampling algorithm are as follows: Input: arbitrarily select the state transition matrix Q of the Markov chain, the target stationary distribution Mine distribution, set the state transition threshold to n1, and the required number of samples to n2.

1. Sample the initial value from any simple probability distribution 2. The value range of t is: [0, n1+n2-1], (a) From the conditional probability distribution Samples are obtained by sampling (b) Sample u~U[0,1] from uniform distribution, where U[0,1] represents the uniform distribution on 0~1. (c) If Accept the transfer Right now (d) Otherwise, the transfer will not be accepted. Output: Sample set That is the sample set corresponding to the target stable distribution Mine distribution, Based on the MH framework, the conditional probability distribution is dynamically adjusted using historical sampling information. In order to improve sampling efficiency, the specific adaptive strategies are as follows:

1. Dynamically adjust the step size ε According to the acceptance rate Adjusting the conditional probability distribution The step size ε, Among them, 0.23~0.44 is the target acceptance rate range, 2. Update the conditional probability distribution using the sampled covariance matrix: (a) Calculate the covariance matrix C through historical samples t (b) The conditional probability distribution Adjusted to: (c) In the update C t , regularization is introduced to ensure numerical stability: C t '=(1-λ)C t +λI Among them, λ is a small positive value, take λ = 0.01, I is the unit matrix, 3. After the sampling is completed, calculate the autocorrelation of the Markov chain and the number of effective samples to verify the sampling quality. By introducing the MH sampling algorithm and adaptive strategy, the sample set after MCMC sampling is obtained By calculating the statistical characteristics of the sample set, the posterior probability p(R k |z 1:k-1 ) satisfies the mathematical properties, Posterior update of GNSS measurements: By updating the prior of IMU predictions, the joint posterior probability p(x k ,R k |z 1:k ), joint posterior probability p(x k ,R k |z 1:k ) is approximately the product of Gaussian distribution and Mine distribution. Gaussian distribution is expressed as N(x k ), Mine distribution is expressed as Mine(R k ), p(x k ,R k |z 1:k )≈N(x k )Mine(R k ) in Mine(R k )=Mine(R k |α,β,η), P k They are the posterior state estimate and the posterior state covariance matrix at time k, and the KL divergence is used to measure the true posterior p(x k ,R k |z 1:k ) and the approximate posterior N(x k )Mine(R k ), By minimizing the above KL divergence, we can find R k The best estimate of Posterior state estimate After iterative convergence through nonlinear optimization algorithm, we can obtain: Where x is the total state within the sliding window of the factor graph, Δx is the increment of the state, [x+Δx] k Indicates the selection of t k The state at the moment, the posterior state estimate and the posterior state covariance matrix P k It is obtained by using the IMU factor for prior update and then adding the GNSS factor for optimization and solution. The above Jacobian matrix J and covariance matrix W are defined as: The subscript ks:k|k-1 indicates that the factor graph is in t k-1 The IMU factor is added for prediction at time t. The subscript ks:k indicates that the factor graph is at t k GNSS factors are added for updates at all times. Gross error detection and elimination: The existence of gross error in measurement will greatly affect the accuracy of the measured value, and thus affect the estimation of the system state. Therefore, it is necessary to select an appropriate method to detect and eliminate gross errors. Here we introduce the new information χ 2 The detection algorithm filters out gross errors and constructs new information V based on the measurement vector and prediction vector. k and its covariance matrix Among them, Z k is the measurement vector, which is the difference between the position output by GNSS and the position calculated by IMU mechanical arrangement, H k is the observation matrix, is the prior error state vector at time k, P k|k-1 is the prior state covariance matrix, which has been obtained in the prior update stage of IMU prediction, R k is the covariance matrix of the measurement noise, V k is the innovation vector, is the covariance matrix of the innovation vector, The new information is a white noise sequence that obeys a normal distribution with a mean of zero: When the measured value is normal, the new information is a white noise sequence with a mean of zero. When the measured value is abnormal, the mean of the new information is no longer zero. The above detection condition is transformed into a hypothesis testing problem: Among them, H0 represents the null hypothesis, indicating that the measured value is not abnormal; H1 is the alternative hypothesis, indicating that the measured value is abnormal. The hypothesis test statistic is: T k Subject to χ with t degrees of freedom 2 Distribution, according to χ 2 The definition of the distribution has the following hypothesis tests: Where λ is the non-centrality parameter and the significance level is α0. The boundary conditions for determining whether measurement anomalies occur are: T D is the measurement abnormal boundary value, if T k ≤T D , then it is considered that there is no measurement abnormality. If T k >T D , it is considered that there is a measurement anomaly at this time, and this anomaly is determined as a measurement gross error and eliminated.

2. The robust factor graph combined navigation method based on adaptive MCMC according to claim 1, characterized in that: In step 2, the robust factor graph integrated navigation algorithm based on adaptive MCMC includes the prior update of IMU prediction, the posterior update of GNSS measurement and the new information χ 2 Detection algorithm, ① Prior update of IMU prediction: update the prior probability p(x k |z 1:k-1 ) is modeled as a Gaussian distribution: Among them, x k represents the system state at time k, z 1:k-1 Represents t1 to t k-1 The measurement of Represents the prior state estimate at time k, P k|k-1 represents the prior state covariance matrix at time k, Given by the system state at time k-1 and pre-integration: represents the posterior state estimate at time k-1, represents IMU pre-integration, h IMU represents the IMU pre-integrated measurement function, State covariance matrix P k The Jacobian matrix J and covariance matrix W in the factor graph sliding window are marginalized based on the Schur complement. Assuming the sliding window size is S+1, we have: Where Λ is the information matrix, the prior state covariance matrix P k|k-1 The required Jacobian matrix J is obtained by prediction based on the pre-integration factor k-s:k|k-1 and the covariance matrix W k-s:k|k-1 Defined as: The subscript m=ks:k-1 indicates that the marginalized state is from t k-s Time to t k-1 At this moment, the subscript r=k means that t k As a reserved state, Represents the residual of the IMU pre-integration factor, x k-s:k Represents the system state from time ks to time k. The residual of the IMU pre-integration factor is derived from the vector in the sliding window by column and then the Jacobian matrix J is expanded in the form of a row vector. The residual covariance of the IMU pre-integration factor directly expands the covariance matrix W in the diagonal form. Then Λ k|k-1 and P k|k-1 Respectively expressed as: Λ k|k-1 =J k-s:k|k-1 W k-s:k|k-1 -1 I k-s:k|k-1 P k|k-1 =(Λ k|k-1 22 -L k|k-1 21 (L k|k-1 11 ) -1 L k|k-1 12 ) -1 where Λ k|k-1 22 , Λ k|k-1 21 , Λ k|k-1 11 , Λ k|k-1 12 Represents Λ k|k-1 The element values ​​of the matrix with 2 rows and 2 columns, 2 rows and 1 column, 1 row and 1 column, and 1 row and 2 columns, In order to estimate the time-varying measurement covariance R k , the adaptive MCMC algorithm is used to sample the posterior probability, and the sampled samples are used to estimate R k The statistical characteristics of , are expressed as follows by the Bayesian formula: p(R k |z 1:k-1 ) is the probability to be sought, that is, the posterior probability, which is converted into the prior probability p(R k ) and the likelihood probability p(z 1:k-1 |R k ) and then divided by a constant ∫p(R k )p(z 1:k-1 |R k )dR k , due to the constant ∫p(R k )p(z 1:k-1 |R k )dR k The existence of only expands or reduces the probability of each point by a certain multiple, and does not affect the relative size of the probability of the sample point value. Therefore, we can k )p(z 1:k-1 |R k ) is sampled and p(R k |z 1:k-1 )’s statistical properties, The measurement noise is Gaussian white noise and obeys the following normal distribution: v k ~N(0,R k ) v k is the measurement noise, and the measurement noise covariance R k Decomposed into the following form: R k =D k S k D k Among them D k is a diagonal matrix containing the standard deviation of each variable, usually modeled as an inverse gamma distribution; k is the correlation matrix, usually modeled as LKJ distribution, for the diagonal matrix D k , has the following inverse gamma distribution: D k ~IG(α,β) α is a shape parameter that controls the shape of the distribution. The larger α is, the more concentrated the distribution is near the mean. When α is small, the distribution has a heavy-tail characteristic. β is a scale parameter that determines the scalability and mean of the distribution. When β is large, the distribution shifts to the right as a whole. When β is small, the distribution moves to the left. For the inverse gamma distribution, p(D k ) is as follows: Where Γ(α) is the gamma function, x is a random variable greater than 0, For the correlation matrix Σ k , there is the following LKJ distribution: S k ~LKJ(h) Where η is another shape parameter used to control the concentration of the distribution, usually between 0.5 and 1.5, p(Σ k ) is as follows: p(Σ k )=c d |S k | η-1 c d is a normalizing constant that makes the distribution integral equal to 1, and its expression is as follows: d is the order of the matrix, K is the block matrix correlation coefficient, B(a,b) is the Beta function, Combining the above analysis, we can get the prior probability p(R k ) is: σ k It's D k The diagonal elements of the matrix are also the standard deviations of each variable, For p(z 1:k-1 |R k ), modeled as a multivariate normal distribution, that is: p(z 1:k-1 |R k )~N(μ 1:k-1 ,R 1:k-1 ) where μ 1:k-1 is the mean and R 1:k-1 is the covariance matrix and R 1:k-1 =diag(R1,R2,...,R k-1 ), measured value z 1:k-1 The noises of are independent and obey the normal distribution, then the joint likelihood probability p(z 1:k-1 |R k ) is expressed as: Where n is the number of observations z t The dimension, R k is the measurement noise covariance matrix at the kth moment, A new distribution is defined based on the product of the prior probability and the likelihood probability, which is called Mine distribution. Then the posterior probability p(R k |z 1:k-1 ) also obeys the Mine distribution, and has: The distribution that satisfies the above probability density function is called Mine distribution.

3. The adaptive MCMC-based robust factor graph combined navigation method according to claim 2, characterized in that: In step 2, the adaptive MCMC algorithm is used to sample the Mine distribution. The specific steps are as follows: Input: arbitrarily select the state transition matrix Q of the Markov chain, the target stationary distribution Mine distribution, set the state transition threshold to n1, and the required number of samples to n2.

1. Sample the initial value from any simple probability distribution 2. The value range of t is: [0, n1+n2-1], (a) From the conditional probability distribution Samples are obtained by sampling (b) Sample u~U[0,1] from uniform distribution, where U[0,1] represents the uniform distribution on 0~1. (c) If Accept the transfer Right now is the acceptance rate, meaning from the state by The probability of jumping to state When The probability of accepting this transfer is (d) Otherwise, the transfer will not be accepted. Output: Sample set That is the sample set corresponding to the target stable distribution Mine distribution, In order to avoid (c) In the case where most of the sample values ​​are rejected for transfer due to the small value, the MH sampling algorithm is used to solve the problem of low sampling acceptance rate in MCMC.

4. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the adaptive MCMC-based robust factor graph optimization combined navigation method according to any one of claims 1 to 3 is implemented.

5. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the computer instructions are executed by a processor, the adaptive MCMC-based robust factor graph optimization combined navigation method according to any one of claims 1 to 3 is implemented.

Images