Robust factor graph optimization combination navigation method based on adaptive MCMC

Abstract

The invention provides a robust factor graph integrated navigation method based on adaptive MCMC (Markov Chain Monte Carlo), and in an INS / GNSS (Inertial Navigation Satellite System / Global Navigation Satellite System) integrated navigation system, factors such as multipath effect and non-line-of-sight can greatly reduce the positioning accuracy of satellites, so that the effect of the whole integrated navigation system is influenced. In urban traffic, many canyon environments greatly influence the positioning precision of vehicles, so that a combined navigation method with robust performance and estimation precision needs to be researched. The invention provides a robust factor graph integrated navigation method based on the adaptive MCMC. The method comprises the following steps: firstly, introducing adaptive MCMC into a factor graph optimization framework based on a prior stage and a posterior stage so as to accurately estimate GNSS time variable measurement noise covariance; secondly, introducing an innovation x2 detection algorithm to detect and eliminate gross errors; the method can effectively estimate GNSS time-varying variable measurement noise while reducing gross error interference. Compared with a robust adaptive factor graph algorithm based on M estimation and a robust factor graph combined navigation algorithm based on variational Bayes, the method has the advantage that the positioning precision is obviously improved.

Description

Technical Field

[0001] The 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] Combined navigation generally refers to a technology that uses two or more navigation systems with complementary measurement characteristics to measure the same information source to obtain higher navigation accuracy. Usually, one navigation system provides information with high short-term accuracy, and the other provides information with high long-term stability. One of the classic combined navigation methods is INS / GNSS combined navigation. INS has a high frequency, but due to integration, errors will accumulate; while GNSS has low short-term accuracy and output frequency.

[0003] Integrated navigation methods usually include filtering-based methods, such as Kalman filtering and its variants, and optimization-based methods, such as factor graph-based methods. The Kalman filter formula is simple and has strong real-time performance, but it is based on a first-order Markov model and does not consider historical data; factor graph optimization (FGO) takes into account all historical information, so it can improve estimation accuracy.

[0004] In the factor graph optimization method, an accurate measurement model is very important. In complex environments such as urban canyons, the presence of GNSS measurement errors and time-varying measurement noise greatly affects the reliability of state estimation. There are two main solutions to this problem: one is to use a robust cost function, including the Huber cost function, dynamic covariance scaling method, etc. The robustness method improves the anti-interference ability of the system at the expense of a certain degree of accuracy, but does not estimate the measurement noise, and the results are often not optimal; the other is to directly estimate the covariance matrix of the measurement noise, including the adaptive covariance estimation algorithm based on PROBE and the robust estimation algorithm based on variational Bayes, etc., but such methods often rely on the prior distribution of the posterior probability or do not handle small measurement noise well. Therefore, there is an urgent need for a method that can predict the time-varying measurement noise and enhance the robustness of the system in complex urban canyon environments. Therefore, it is of great significance to study the robust factor graph combined navigation method based on adaptive MCMC. Summary of the invention

[0005] In view of the current problems related to modeling GNSS time-varying measurement noise, the present invention provides a robust factor graph integrated navigation method based on adaptive MCMC. The method introduces adaptive MCMC into the factor graph optimization framework based on a priori and a posteriori two-stage update to estimate the 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 purpose, the technical solution of the present invention is 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 a posteriori 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 result and compares it 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 (root mean square error) and STD (standard deviation). The specific process is to convert the east, north, and sky position data of all epochs after factor graph optimization into the east, north, and sky position data corresponding to RTK to calculate RMSE and STD. The superiority of the algorithm proposed in the present invention is verified by comparing the reduction of RMSE and STD indicators of the algorithm proposed in the present invention compared with other classical algorithms.

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

[0012] ①IMU prediction prior update: 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 from 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 time, the subscript r=k means that t k The state is kept as a reserve state. 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 directly expands the covariance matrix W in the form of a diagonal line, 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 Respectively represent Λ k|k-1 The element values ​​of the matrix 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, 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 . 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, it can be obtained by 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] Where 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 Lewandowski-Kurowicka-Joed distribution (LKJ distribution). k , has the following inverse gamma distribution:

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

[0036] α 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:

[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 Yes 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 follow a 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 denoted as 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 uses the adaptive MCMC algorithm to sample the Mine distribution. The specific steps are as follows:

[0057] Input: arbitrarily select the state transfer matrix Q of the Markov chain, the target stable distribution Mine distribution, set the state transition number 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 a uniform distribution, where U[0,1] represents a uniform distribution on the range 0 to 1.

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

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

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

[0065] In order to avoid (c) In the case where most of the sampled values ​​are rejected for transfer due to the small size, 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 transfer matrix Q of the Markov chain, the target stable distribution Mine distribution, set the state transition number 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 a uniform distribution, where U[0,1] represents a uniform distribution on the range 0 to 1.

[0072] (c) If Accept transfer Right now

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

[0074] Output: Sample set That is the sample set corresponding to the required target stable 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 length ε.

[0078]

[0079] Among them, 0.23~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 updating 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, we can get 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 that 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 estimated value of the posterior state at time k and the posterior state covariance matrix 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 )’s matching degree.

[0093]

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

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

[0096]

[0097]

[0098] Where x is the number of states in 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 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 all the time.

[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 screens 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 H is the measurement vector, which 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 detection conditions are transformed into hypothesis testing problems:

[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-centralized 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 gross measurement 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 In the form of, and for the first time introduced LKJ distribution into the correlation matrix Σ k It is more accurate than directly modeling the posterior probability as an inverse Wissart distribution.

[0121] 3) Use KL divergence to measure the matching degree 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 The estimated value of . 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 take the open source dataset UrbanNavDataset as an example, combined with the 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 described embodiments 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 creative work 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 IMU, GNSS, RTK and other sensors on the vehicle to collect relevant data. Here we use the IMU, GNSS and RTK data in the Hong Kong dataset provided by the open source dataset 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 a posteriori 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 result and compares it 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 (root mean square error) and STD (standard deviation). The specific process is to convert the east, north, and sky position data of all epochs after factor graph optimization into the east, north, and sky position data corresponding to RTK to calculate RMSE and STD. The superiority of the algorithm proposed in the present invention is verified by comparing the reduction of RMSE and STD indicators of the algorithm proposed in the present invention compared with other classical algorithms.

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

[0130] ①IMU prediction prior update: 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 from 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 time, the subscript r=k means that t k The state is kept as a reserve state. 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 directly expands the covariance matrix W in the form of a diagonal line, 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 Respectively represent Λ k|k-1 The element values ​​of the matrix 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, 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 . 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, it can be obtained by 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] Where 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 Lewandowski-Kurowicka-Joed distribution (LKJ distribution). k , has the following inverse gamma distribution:

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

[0154] α 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:

[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 Yes 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 follow a 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 denoted as 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 uses the adaptive MCMC algorithm to sample the Mine distribution. The specific steps are as follows:

[0175] Input: arbitrarily select the state transfer matrix Q of the Markov chain, the target stable distribution Mine distribution, set the state transition number 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 a uniform distribution, where U[0,1] represents a uniform distribution on the range 0 to 1.

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

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

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

[0183] In order to avoid (c) In the case where most of the sampled values ​​are rejected for transfer due to the small size, 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 transfer matrix Q of the Markov chain, the target stable distribution Mine distribution, set the state transition number 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 a uniform distribution, where U[0,1] represents a uniform distribution on the range 0 to 1.

[0190] (c) If Accept transfer Right now

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

[0192] Output: Sample set That is the sample set corresponding to the required target stable 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 length ε.

[0196]

[0197] Among them, 0.23~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 updating 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, we can get 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 that 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 estimated value of the posterior state at time k and the posterior state covariance matrix 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 )’s matching degree.

[0211]

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

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

[0214]

[0215] Where x is the number of states in 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 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 all the time.

[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 screens 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 H is the measurement vector, which 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 detection conditions are transformed into hypothesis testing problems:

[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-centralized 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 gross measurement error and eliminated.

[0234] In summary, the present invention proposes a robust factor graph integrated navigation method based on adaptive MCMC, which can effectively estimate the GNSS time-varying measurement noise while reducing gross error interference. In an environment without prior knowledge and in dynamic changes, the proposed algorithm can estimate the GNSS measurement noise covariance matrix more accurately. Compared with the robust adaptive factor graph algorithm based on M estimation and the robust factor graph integrated navigation algorithm based on variational Bayes, this method can better balance robustness and accuracy.

Claims

1. A robust factor graph combined navigation method based on adaptive MCMC, characterized in that: 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 a posteriori 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 result and compares it 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 (root mean square error) and STD (standard deviation). The specific process is to convert the east, north, and sky position data of all epochs after factor graph optimization and the east, north, and sky position data corresponding to RTK into RMSE and STD. The superiority of the algorithm proposed in the present invention is verified by comparing the reduction of RMSE and STD indicators compared with other classic algorithms.

2. The adaptive MCMC-based robust factor graph integrated navigation method 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 a posteriori update of GNSS measurement and the update of new information χ 2 Detection algorithm, ①IMU prediction prior update: 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 from 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 time, the subscript r=k means that t k The moment is kept as a 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 column by column with respect to the vector in the sliding window to expand the Jacobian matrix J 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 Respectively represent Λ k|k-1 The element values ​​of the matrix 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 , obtained by Bayesian formula are as follows: 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, it can be obtained by k )p(z 1:k-1 |R k ) is sampled and p(R k |z 1:k-1 ), Assume that 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 Where 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 the LKJ distribution (LKJ distribution, Lewandowski-Kurowicka-Joe 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 Yes D k The diagonal elements of the matrix are also the standard deviations of each variable, For p(z 1:k-1 |R k ), which is usually 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 ), assuming the measured value z 1:k-1 The noises of are independent and follow a normal distribution, then the joint likelihood probability p(z 1:k-1 |R k ) can be 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, Define a new distribution based on the product of prior probability and likelihood probability, here denoted as 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 method of robust factor graph optimization combined navigation based on adaptive MCMC 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 transfer matrix Q of the Markov chain, the target stable distribution Mine distribution, set the state transition number threshold to n1, and the required number of samples to n2.

1. Sample the initial value from any simple probability distribution 2. for t = 0 to n1 + n2 - 1, (a) From the conditional probability distribution Samples are obtained by sampling (b) Sample u~U[0,1] from a uniform distribution, where U[0,1] represents a uniform distribution between 0 and 1. (c) If Accept transfer Right now is the acceptance rate, meaning that from the state by The probability of jumping to the state When The probability of accepting this transfer is (d) Otherwise, the transfer is not accepted. Output: Sample set That is the sample set corresponding to the required target stable distribution Mine distribution, In order to avoid (c) The MH sampling algorithm is used to solve the problem of too low sampling acceptance rate in MCMC. The improved MCMC sampling steps based on the MH sampling algorithm are as follows: Input: arbitrarily select the state transfer matrix Q of the Markov chain, the target stable distribution Mine distribution, set the state transition number threshold to n1, and the required number of samples to n2.

1. Sample the initial value from any simple probability distribution 2. for t = 0 to n1 + n2 - 1, (a) From the conditional probability distribution Samples are obtained by sampling (b) Sample u~U[0,1] from a uniform distribution, where U[0,1] represents a uniform distribution between 0 and 1. (c) If Accept transfer Right now (d) Otherwise, the transfer is not accepted. Output: Sample set That is the sample set corresponding to the required 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 strategy is as follows:

1. Dynamically adjust the step size ε According to the acceptance rate Adjusting the conditional probability distribution The step length ε, Among them, 0.23 to 0.44 is the common 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 updating C t , regularization is introduced to ensure numerical stability: C t '=(1-λ)C t +λI Among them, λ is a small positive value, generally λ = 0.01, I is the unit matrix, 3. After sampling is completed, calculate the autocorrelation of the Markov chain and the number of valid 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, we can get the posterior probability p(R k |z 1:k-1 ) satisfies the mathematical properties, ②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 that 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 ), p(x k ,R k |z 1:k )≈N(x k )Mine(R k ) in Mine(R k )=Mine(R 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 ), R can be obtained by minimizing the above KL divergence k The best estimate of Posterior state estimate After iterative convergence of the nonlinear optimization algorithm, we can obtain: Where x is the number of states in 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 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 then 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 screens 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 follows 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, and the above detection conditions are 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, and 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: In the formula, λ is the non-centralized parameter. If the significance level is α, the boundary condition for determining whether measurement anomaly occurs is: 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 gross measurement error and eliminated.

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 as described in 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 instruction is executed by a processor, the adaptive MCMC-based robust factor graph optimization combined navigation method as described in any one of claims 1 to 3 is implemented.

Images