Multi-mode gnss / inertial tight coupling factor graph optimization method based on variance component estimation

Abstract

The application discloses a multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation and belongs to the technical field of satellite / inertial combination navigation. The application realizes the method as follows: under the condition of assigning prior multi-GNSS inter-system weight, the observation of the multi-mode GNSS and the inertial navigation system is fused based on a factor graph optimization method; the posterior residual is calculated by combining the multi-mode GNSS observation equation and the posterior state estimation result obtained after the factor graph optimization; the weight between the multi-GNSS systems is adjusted in real time and adaptively according to the signal quality of each satellite navigation system satellite by using a variance component estimation method; and the satellite observation containing gross errors is subjected to weight reduction processing based on a gross error detection and weight reduction algorithm before the weight between the systems is adaptively adjusted. The application uses a robust variance component estimation to determine the weight between the multi-GNSS systems, adjusts the reliability degree of the observation information of different satellite navigation system satellites in real time, and improves the precision and robustness of the multi-mode GNSS / inertial tight combination navigation system.

Description

Technical Field

[0001] This invention belongs to the field of satellite / inertial integrated navigation technology, and relates to a multi-mode GNSS / inertial compact combination factor graph optimization method based on variance component estimation. Background Technology

[0002] With the successive launches of Global Navigation Satellite Systems (GNSS), such as GPS, Galileo, and BeiDou, the demand for high-precision, robust GNSS positioning services is rapidly increasing across various industries, particularly in the field of motion state estimation for autonomous intelligent unmanned systems. However, in scenarios with severe signal obstruction, such as urban centers, transportation hubs, and mountainous areas, or under adverse environmental conditions such as thunderstorms and geomagnetic storms, the number of available GNSS satellites decreases, making it difficult for carriers relying on observation information provided by a single GNSS system to obtain accurate state estimation information.

[0003] Given the limited number of available satellites, this method utilizes observation information from multiple operational global navigation satellite systems, combines raw GNSS observation information with continuous and stable state estimation information from inertial navigation systems, and employs factor graph optimization of this type of sliding window state estimation algorithm. By fully leveraging multi-mode GNSS observation information and inertial navigation state estimation information from multiple time points, it provides high-precision navigation and positioning results for unmanned systems.

[0004] Because multiple global navigation satellite systems (GNSS) have different designs, distributions, and frequency bands, and because the number and signal quality of satellites from different systems observed in different regions at the same time also vary, determining the reliability weights of observation information from different GNSS systems and detecting and reducing the impact of gross errors in observation information are crucial for improving the positioning accuracy and robustness of multi-mode GNSS / inertial integrated navigation systems when fusing observation information from different systems. However, current GNSS / inertial integrated navigation systems based on factor graph optimization methods mainly focus on utilizing multiple different types of GNSS observation information, such as pseudorange, Doppler, and carrier waves. They only consider the weights between different types of observation information from the same system or between observation information from satellites of the same type at different altitudes within the same system, lacking consideration for the weights between observation information provided by different GNSS systems. If a GNSS system's observation information is erroneously interfered with due to human attacks or natural causes, and the reliability weights of that system's observation information cannot be adaptively adjusted, the overall navigation and positioning performance of the multi-mode GNSS / inertial integrated navigation system will be severely affected, leading to incorrect estimations of the navigation state of the unmanned system. Therefore, this invention proposes a multi-mode GNSS / inertial compact combination factor graph optimization method based on robust variance component estimation. This method considers multiple sets of observation and state information from multiple epochs and iteratively determines the weights among multiple GNSS units using variance component estimation. Simultaneously, to mitigate the impact of satellite observation information containing gross errors on weight and state estimation, a gross error detection and robustness algorithm is designed for GNSS pseudorange and Doppler observations, thereby improving the accuracy and robustness of multi-mode GNSS / inertial compact combination navigation and positioning based on factor graph optimization. Summary of the Invention

[0005] When combining raw observation information from multiple different global navigation satellite systems (GNSS) with dead reckoning information from inertial navigation, the differences in constellation design and satellite distribution and signal quality across GNSS systems at the same time must be considered. The purpose of this invention is to provide a multi-mode GNSS / inertial navigation tight combination factor graph optimization method based on variance component estimation. This method not only further improves the accuracy of multi-mode GNSS and inertial navigation tight combination but also fully utilizes the advantages of factor graph optimization in considering multiple epochs of estimated states and observation information. It iteratively and adaptively adjusts the weights of multiple GNSS systems, further enhancing the overall robustness of the integrated navigation system and improving its accuracy under GNSS interference or deception conditions.

[0006] The objective of this invention is achieved through the following technical solution.

[0007] This invention discloses a multi-mode GNSS / inertial compact integration factor graph optimization method based on variance component estimation. Under the condition of allocating prior weights among multiple GNSS systems, it fuses observations from multiple GNSS and inertial navigation systems using a factor graph optimization method to obtain accurate navigation state estimation results. Based on the posterior state estimation results obtained after factor graph optimization, and combined with the multi-mode GNSS observation equations, the posterior residuals are calculated. Using variance component estimation, the weights among the multiple GNSS systems are adaptively adjusted in real time according to the signal quality of each satellite navigation system. Based on gross error detection and weight reduction algorithms, satellite observations containing gross errors are weighted before adaptively adjusting the weights among the systems to avoid the influence of gross error observations on the variance component estimation algorithm. This invention determines the weights among multiple GNSS systems using robust variance component estimation for a multi-mode GNSS / inertial compact integration navigation system based on factor graph optimization. Through adaptive weighting using robust variance component estimation, the reliability of satellite observation information from different satellite navigation systems is adjusted in real time, improving the accuracy and robustness of the multi-mode GNSS / inertial compact integration navigation system.

[0008] The multi-mode GNSS / inertial compact combination factor map optimization method disclosed in this invention includes the following steps:

[0009] Step 1: Obtain pseudorange and Doppler observation data for each satellite from the raw observation information provided by multiple satellite navigation systems. Based on the satellite navigation pseudorange and Doppler observation equations, construct residual functions between the estimated state (position, velocity, attitude, sensor error) and the raw satellite observations for each satellite, respectively called pseudorange residual blocks and Doppler residual blocks. Using the state estimation result from the previous moment as a reference starting point, continuously integrate the accelerometer and gyroscope data between the arrival times of observation information from two adjacent satellites according to the inertial navigation data output frequency, up to the current moment, to obtain the position, velocity, attitude, and sensor error data. The increment of error between two time points is used to construct an inertial navigation pre-integration residual block using the difference between this increment and the states at these two adjacent time points. After constructing the pseudorange residual block, Doppler residual block, and inertial navigation integration pre-integration residual block based on satellite observations, a sliding window estimator based on factor graph optimization is used to sum all residual blocks within the time window according to the set sliding window size, constructing an optimization objective function, and using the Gauss-Newton method to solve for the joint optimal estimation result of multiple states within the time window. When a new time observation residual block and the state to be estimated are added to the window, if the window size has already reached the set window size, the Schur elimination method is used to marginalize premature observation residual blocks and the state to be estimated.

[0010] Step 2: Before each sliding window factor map optimization, prior weights are assigned to the pseudorange and Doppler observations provided by each satellite for each operating system. These weights include the weights of different systems estimated in the previous round, constant weights between different observation types within the same system, and weights determined between different satellites within the same system based on the real-time satellite elevation angle. These three types of weights are then multiplied. After determining the prior weights, the first factor map optimization is performed to obtain the posterior state estimation results after the first optimization. Based on the state estimation results of the first round of optimization, a robust algorithm is used to detect outliers and assign outlier weights to observations containing outliers to reduce the impact of outliers on the multi-system weight estimation.

[0011] Step 3: After the first optimization and weight reduction of gross observations, a second round of optimization is performed until the optimization structure converges, obtaining the joint optimal estimation result of the state at multiple time points within the current time window, i.e., the posterior estimation result of the state. Using the posterior estimation result, the observation equation design matrix, and the observation equation, the posterior observation residuals are calculated. Then, according to the minimum cost function theory, the posterior observation residuals and prior weights are substituted into the variance component estimation variance to obtain the unit weight variance estimate of the observations of each of the multiple GNSS systems. Based on this variance estimate, the prior weights are adjusted to the posterior estimation weights. Thus, while completing the state estimation of the multi-mode GNSS / inertial system, the posterior trust weights between different GNSS systems at each time point are determined. Based on the posterior trust weights, the trust level of satellite observation information of different satellite navigation systems is adjusted in real time, improving the accuracy and robustness of the multi-mode GNSS / inertial tightly integrated navigation system.

[0012] Preferably, step one includes the following steps:

[0013] S11. Obtain pseudorange and Doppler observation data for each satellite from satellite navigation messages, satellite observation documents, and satellite broadcast ephemeris. Based on the pseudorange and Doppler observation equations, construct pseudorange residual blocks and Doppler residual blocks with the state to be estimated as the independent variable. The state to be estimated is defined as:

[0014]

[0015] in, and These represent the position, velocity, and attitude of the unmanned system's inertial navigation center in the navigation coordinate system (n-frame, north, east, and ground orientation) at time k, respectively. g，k and b a，k These represent the random zero bias of the gyroscope and accelerometer of the inertial navigation system at time k, respectively.

[0016] There is a fixed lever distance l between the inertial navigation center and the GNSS receiver center. b , l b Defined in the carrier coordinate system (b system), pointing from the inertial navigation center to the GNSS antenna phase center.

[0017] δt k =[δt G，k δt B，k δt E,k ] T and These represent the receiver clock bias and clock drift of the three global satellite navigation systems: GPS, BeiDou, and Galileo. k+m This represents the estimated states for all epochs within the sliding window when the window size is m at time k+m. The initial states and initial state variances are set before factor graph optimization begins.

[0018] The pseudorange and Doppler observation equations are constructed. Since the navigation state constrained by pseudorange and Doppler is defined in the geocentric geofixed coordinate system (ECEF system, e-system), a coordinate system transformation is performed on the state to be estimated during the construction of the observation equations.

[0019]

[0020] in, and Let be the rotation matrices from the n-system to the e-system and from the b-system to the n-system, respectively. and These are the Earth's rotational angular velocity and the angular velocity measured by the inertial navigation system at time k, respectively.

[0021] Construct the relationship between the pseudorange, the original Doppler observations, and the state to be estimated at the current moment:

[0022]

[0023] in and Let $k$ be the pseudorange and raw Doppler observations of the $i$-th satellite in the $S$-th satellite navigation system at time $k$. and Let D be the position and velocity of the i-th satellite of the S-th satellite navigation system at time k. sagnac and λ S δt represents the wavelength of the Sagnac effect term and the corresponding frequency band signal of the S-th satellite navigation system, respectively. S，i , and Let $k$ be the clock error, clock drift, tropospheric delay, and ionospheric delay of the $i$-th satellite in the $S$-th satellite navigation system at time $k$. These are the observation noises for pseudorange observations and Doppler observations, respectively.

[0024] Further construct the pseudorange residual block as shown in equation (5) and the Doppler residual block as shown in equation (6):

[0025]

[0026] in, and These represent pseudorange observation residuals and Doppler observation residuals, respectively.

[0027] S12. Using the state at the previous moment as a reference, integrate the inertial navigation data between the two observation moments to obtain the inertial navigation pre-integration position, velocity, attitude, and sensor error increment, and construct the inertial navigation pre-integration residual block shown in equation (7):

[0028]

[0029] X contains the states of two adjacent time points. These are the inertial navigation pre-integration position, velocity, and attitude increments, respectively. This is the increment for the correction terms for gravity and Coriolis force.

[0030] S13. Using a sliding window estimator based on factor graph optimization, sum all residual blocks within the time window according to the set sliding window size to form the optimization objective function, and use the Gauss-Newton method to solve for the joint optimal estimation result of multiple states within the time window:

[0031]

[0032] Where, ∑ ρ ,∑ D and ∑ Pre Let ∑ be the information matrix of pseudorange observation, Doppler observation, and inertial navigation pre-integral quantity, respectively, i.e., the inverse of each observation quantity. ρ or ∑ D The composition is as follows:

[0033]

[0034] in n represents the weight of the observations from the i-th satellite in the S-th system at time k. S This represents the number of available satellites observed by each system at time k.

[0035] Preferably, step two includes the following steps:

[0036] S21, Weight of each observation This includes: weights of different systems estimated based on the previous optimization. Constant weights P between different observation types within the same system obs Based on the real-time satellite elevation angle, the weights between different satellites in the same system are then determined. Multiply the three types of weights together to obtain the prior weight matrix for all observations;

[0037] Based on the real-time satellite elevation angle, the specific formula for determining the weights between different satellites in the same system is as follows:

[0038]

[0039] The prior weights for each observation are further obtained as follows:

[0040]

[0041] S22. Based on the above prior weights, perform the first round of optimization according to equation (8) to obtain the state estimation results after the first round of optimization. Calculate the posterior residuals according to equations (5) and (6), use a robust algorithm to detect gross errors, and assign gross error weights to observations containing gross errors to reduce the impact of gross errors on the weight estimation of the multi-system. Obtain the gross error adjustment weight for each observation.

[0042] Preferably, step three includes the following steps:

[0043] S31. After determining the gross error weights, update the prior weight matrix by combining it with the prior weights before robustness:

[0044]

[0045] Then, a second round of optimization is performed according to equation (8) until the optimization structure converges, obtaining the joint optimal estimation result of the states at multiple time points within the current time window.

[0046] S32. Utilizing posterior estimation results The observation equation design matrix and the observation equation are used to calculate the posterior observation residuals. Then, the posterior weights are adaptively adjusted according to the cost function minimization theory.

[0047] By designing matrices based on the observation equations, the posterior residuals of the GPS, BeiDou, and Galileo systems can be approximated as follows:

[0048]

[0049] in The design matrix can be obtained by finding the Jacobian matrix of the residual block relative to the state to be estimated.

[0050] Substituting the posterior observation residuals and prior weights into the variance component estimation, we obtain the unit-weighted variance estimates for the observations of multiple GNSS systems.

[0051]

[0052] The S matrix is ​​determined by the various design matrices and the prior weight matrix, and its specific form varies depending on the variance component estimation method used. This is the weight matrix of satellite observations for each system, and the distribution of this matrix is ​​consistent with that of equation (9).

[0053] Based on the adjusted unit weight variance estimate, the prior weights are adjusted to the posterior estimated weights:

[0054]

[0055] The posterior system weights estimated from the variance components are used as the prior weights for the next optimization step in factor graph optimization until the navigation task ends. When the number of state epochs to be estimated within the sliding window exceeds the window size, residual blocks that are marginalized prematurely are marginalized. The prior system weights of the residual blocks that are not marginalized are selected as the posterior system weights of the previous step. Thus, by using sliding window factor graph optimization, the posterior multi-GNSS system weights are iteratively adjusted to obtain the adaptively adjusted multi-system weights and the optimal navigation state estimate at each time step.

[0056] Beneficial effects:

[0057] 1. The multi-mode GNSS / inertial compact combination factor map optimization method disclosed in this invention increases the number of visible satellites in complex environments by fusing satellite observation information from multiple GNSS systems, improves the geometric distribution of satellites, broadens the observation matrix, and fuses with information provided by an inertial navigation system that can operate autonomously and stably, thereby improving the accuracy and robustness of the GNSS / inertial compact combination navigation system state estimation.

[0058] 2. The multi-mode GNSS / inertial compact combination factor graph optimization method disclosed in this invention, based on variance component estimation, constructs a compact combination system of multi-mode GNSS and inertial navigation using a sliding window factor graph optimization method. It integrates multi-mode GNSS pseudorange observations, Doppler observations, and pre-integrated quantities from inertial navigation. By considering the state to be estimated and observation information at multiple moments within a time window, it obtains accurate multi-epoch joint optimal state estimation results with low cost and low computational consumption. The compact combination method, by fusing observations from inertial navigation and satellite navigation, complements the advantages of the two navigation methods, improving the overall performance of the GNSS system in complex environments.

[0059] 3. The multi-mode GNSS / inertial compact combination factor map optimization method disclosed in this invention calculates the posterior residuals using the optimized state, pseudorange, and Doppler observation equations, making full use of the redundant contribution of observation information to obtain the unit weight variance of the multi-system. It then calculates the unit weight variance through variance component estimation to perform posterior weighting of the multi-GNSS system. This method can fully utilize the state estimation results to adaptively improve the reliability of different GNSS systems in real time. It can also reduce the impact of GNSS systems with large errors on the overall positioning accuracy of the entire compact combination system when the satellite distribution of the posterior multi-GNSS system is poor, the quality is low, or the signals of some systems are interfered with.

[0060] 4. The present invention discloses a multi-mode GNSS / inertial compact combination factor graph optimization method based on robust variance component estimation. It addresses the problem that the raw observation information provided by satellites of multiple GNSS systems in complex environments may contain large gross errors, and that the gross errors in the observations may lead to distortion of the variance component estimation model, resulting in misjudgment of the weights of multiple systems. Before determining the posterior weights, a robust model of the raw GNSS observations is introduced. By reducing the weights of the observation information containing gross errors, the accuracy and reliability of the variance component estimation are improved. Attached Figure Description

[0061] Figure 1 This is a flowchart of the multi-mode GNSS / inertial compact combination factor graph optimization method based on robust variance component estimation according to the present invention.

[0062] Figure 2 The figure shows the results of an on-vehicle experiment conducted to verify the actual effect of the present invention. The figure compares the position, velocity, and attitude error curves of the method proposed in this invention with those of the single-factor graph optimization.

[0063] Figure 3 The cumulative distribution function curves of the positioning error of the method proposed in this invention are compared with existing single robustness methods, single variance component estimation methods, single factor graph optimization methods, and extended Kalman filtering methods. Detailed Implementation

[0064] To make the objectives, technical solutions, and advantages of the present invention clearer, the embodiments of the present invention will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are only for explaining the embodiments of the present invention and are not intended to limit the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments in this application without creative effort are within the scope of protection of this application. Examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout.

[0065] It should be noted that the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product, or server that includes a series of steps or units is not necessarily limited to those steps or units that are explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to such process, method, product, or device.

[0066] Similar labels and letters in the following figures indicate similar items; therefore, once an item is defined in one figure, it does not need to be further defined and explained in subsequent figures.

[0067] This embodiment uses an unmanned vehicle (UAV) capable of receiving signals from three different constellations—GPS, Galileo, and BeiDou—and an inertial navigation module as an example. This UAV needs to perform inspections along a designated trajectory in a complex environment. In such an environment, satellite signals are significantly obstructed, and the distribution and quality of visible satellites are weakened due to obstruction, reflection, and other environmental factors. The number of usable satellites is significantly reduced, and relying solely on a single system is insufficient. Therefore, it is necessary to introduce GPS, Galileo, and BeiDou to increase the number of available observations, thereby increasing the effective information for state estimation of the multi-mode GNSS / inertial navigation system. However, while introducing multi-system observations broadens the observation equations, the quality and distribution of satellites from different systems vary, requiring different confidence weights for each system. A suitable variance component estimation method is needed to estimate these weights. In this embodiment, the Helmert variance component estimation method is chosen to estimate the weights of observations from different GNSS systems based on prior weights. To mitigate the potential impact of outlier-related observations on weight estimation, outlier detection and weight reduction methods are needed. In this embodiment, the commonly used IGG-III outlier detection and weight reduction method is selected, dividing the observations of each satellite into three parts: no outliers, suspected outliers, and severe outliers, and then performing segmented weight reduction. The following example uses the Helmert variance component estimation method under robust IGG-III to adaptively estimate the state of a multi-mode GNSS / inertial compact navigation system and the weights between different systems in real time based on the factor graph optimization method.

[0068] like Figure 1 As shown, this embodiment discloses a multi-system weight estimation and vehicle navigation state estimation method based on robust variance component estimation for a compactly integrated system mounted on an unmanned vehicle. The specific implementation steps are as follows:

[0069] Step 1: From the raw observation information received by the vehicle-mounted receiver from multiple satellite navigation systems, obtain the pseudorange and Doppler observation data for each satellite. Based on the satellite navigation pseudorange and Doppler observation equations, construct residual functions between the estimated state (position, velocity, attitude, sensor error, etc.) and the raw satellite observations for each satellite. These are called the pseudorange residual block and the Doppler residual block, respectively. Based on the fundamental equations of inertial navigation, using the state estimation result from the previous moment as a reference starting point, continuously integrate the accelerometer and gyroscope data between the arrival times of adjacent satellite observation information according to the inertial navigation data output frequency to the current moment to obtain the position, velocity, attitude, and transmission distance. The increment of sensor error between two time points is used to construct an inertial navigation pre-integration residual block using the difference between this increment and the states at these two adjacent time points. After constructing pseudorange residual blocks, Doppler residual blocks, and pre-integration residual blocks from inertial navigation integration, a sliding window estimator based on factor graph optimization is used. According to the set sliding window size, all residual blocks within the time window are summed to form the optimization objective function. The Gauss-Newton method is used to solve for the joint optimal estimation result of multiple states within the time window. When new observation residual blocks and states to be estimated are added to the window, if the window size has already reached the set window size, the Schur elimination method is used to marginalize premature observation residual blocks and states to be estimated.

[0070] Step one specifically includes the following steps:

[0071] S11, The state to be estimated is defined as:

[0072]

[0073] in, and These represent the position, velocity, and attitude of the unmanned system's inertial navigation center in the navigation coordinate system (n-frame, north, east, and ground orientation) at time k, respectively. g，k and b a,k These represent the random zero bias of the gyroscope and accelerometer of the inertial navigation system at time k, respectively.

[0074] There is a fixed lever distance l between the inertial navigation center and the GNSS receiver center. b , l b Defined in the carrier coordinate system (b system), pointing from the inertial navigation center to the GNSS antenna phase center.

[0075] δt k =[δt G,k δt B,k δt E，k ] T and These represent the receiver clock bias and clock drift of the three global satellite navigation systems: GPS, BeiDou, and Galileo. k+m This represents the estimated states for all epochs within the sliding window when the window size is m at time k+m. The initial states and initial state variances are set before factor graph optimization begins.

[0076] To construct the pseudorange and Doppler observation equations, since the navigation state constrained by pseudorange and Doppler is defined in the geocentric-ground-fixed coordinate system (ECEF system, e-system), a coordinate system transformation of the state to be estimated is required during the construction of the observation equations.

[0077]

[0078] in, and Let be the rotation matrices from the n-system to the e-system and from the b-system to the n-system, respectively. and These are the Earth's rotational angular velocity and the angular velocity measured by the inertial navigation system at time k, respectively.

[0079] Pseudorange and Doppler observation data for each satellite are obtained from satellite navigation messages, satellite observation documents, and satellite broadcast ephemeris. Based on the pseudorange and Doppler observation equations, pseudorange residual blocks and Doppler residual blocks are constructed with the state to be estimated as the independent variable.

[0080]

[0081] S12. Using the state at the previous moment as a reference, integrate the inertial navigation data between the two observation moments to obtain the inertial navigation pre-integration position, velocity, attitude, and sensor error increments. Using the difference between the increments and the state, construct the inertial navigation pre-integration residual block:

[0082]

[0083] S13. Using a sliding window estimator based on factor graph optimization, sum all residual blocks within the time window according to the set sliding window size to form the optimization objective function, and use the Gauss-Newton method to solve for the joint optimal estimation result of multiple states within the time window:

[0084]

[0085] Step 2: After completing the construction of a multi-mode GNSS / inertial tightly integrated navigation system based on sliding window factor graph optimization, it is necessary to determine the prior weights of each GNSS system and perform the first optimization to detect gross errors and reduce the weights of observations containing gross errors. Before each sliding window factor graph optimization, prior weights are assigned to the pseudorange and Doppler observations provided by each satellite for each operating system. These weights include the weights of different systems estimated in the previous round, constant weights between different observation types within the same system, and weights determined between different satellites within the same system based on the real-time satellite elevation angle. These three types of weights are then multiplied. After determining the prior weights, the first factor graph optimization is performed to obtain the posterior state estimation results after the first optimization. Based on the first round of optimized state estimation results, a robust algorithm is used to detect gross errors and assign gross error weights to observations containing gross errors to reduce the impact of gross errors on the multi-system weight estimation.

[0086] Step two specifically includes the following steps:

[0087] S21, Weight of each observation This includes: weights of different systems estimated based on the previous optimization. Constant weights P between different observation types within the same system obs Based on the real-time satellite elevation angle, the weights between different satellites in the same system are then determined. Multiply the three types of weights together to obtain the prior weight matrix for all observations;

[0088] Based on the real-time satellite elevation angle, the specific formula for determining the weights between different satellites in the same system is as follows:

[0089]

[0090] Furthermore, the prior weights for each observation can be obtained as follows:

[0091]

[0092] S22. Perform the first round of optimization according to equation (22) to obtain the state estimation result after the first round of optimization.

[0093] Using the posterior residuals calculated by equations (19) and (20), the IGG-III method is used for gross error detection and weight reduction. The impact of gross errors on the weight estimation of the multi-system is reduced nonlinearly in three stages: no gross errors, suspected gross errors, and serious gross errors. The robust estimation weight function of IGG-III is:

[0094]

[0095] Among them, v iFor a single system and a single satellite, σ represents the posterior residual of pseudorange or Doppler observations. σ is generally taken as the median of all pseudorange or Doppler posterior residuals observed at the current time. med / 0.6745, k1 and k2 are constants, typically taken as 1.5–2.0 and 3.0–8.5 respectively. n is the total number of observations at the current moment, m is the dimension of the state to be estimated, and γ i This represents the weighting coefficient for outliers in the current observations.

[0096] Step 3, as follows Figure 1 As shown, after the first optimization and weight reduction of gross observations using the IGG-III method, the prior weights are multiplied by the gross weight reduction coefficient, and a second round of optimization is performed until the optimization structure converges, obtaining the joint optimal estimation result of the state at multiple time points within the current time window, i.e., the posterior estimation result of the state. The posterior estimation result, the observation equation design matrix, and the observation equation are used to calculate the posterior observation residuals. Then, based on the Helmert variance component estimation cost function minimization theory, the Helmert variance component estimation equation is constructed. Substituting the posterior observation residuals and prior weights into the variance component estimation variance, the unit weight variance estimates of the observations of multiple GNSS systems are obtained. Based on the unit weight variance estimates, the prior weights are adjusted to posterior estimation weights. Finally, while completing the state estimation of the multi-mode GNSS / inertial system, the posterior trust weights between different GNSS systems at each time point are determined.

[0097] Step three specifically includes the following steps:

[0098] S31. After using IGG-III to reduce gross errors according to (25), update the prior weight matrix by combining the prior weights before robustness:

[0099]

[0100] The second round of optimization is performed according to equation (22) until the optimization structure converges, and the joint optimal estimation result of the state at multiple time points within the current time window is obtained.

[0101] S32. Based on Helmert's variance component estimation theory, the posterior estimation after the second optimization, the observation equation design matrix and the observation equation are used to calculate the posterior observation residuals, and the unit weighted variance estimates of the observations of multiple GNSS systems are obtained.

[0102] By designing matrices using the observation equations, the posterior residuals of the GPS, BeiDou, and Galileo systems can be approximately expressed as follows:

[0103]

[0104] in S = G, B, E is the design matrix, which can be obtained by calculating the Jacobian matrix of the residual block relative to the state to be estimated.

[0105] Substituting the posterior observation residuals and prior weights into the variance component estimation, and based on the least squares error minimization theory, the unit weighted variance estimates of the observations of multiple GNSS systems for the Helmert variance component estimation are obtained. And S matrix:

[0106]

[0107] in, S = G, B, E, N = N G +N B +N E , This is the weight matrix for satellite observations of each system.

[0108] In Helmert variance component estimation, unit weighted variance estimation When a component may have a negative value, the unit weight variance estimate may not meet the requirement of non-negativity of variance. It is necessary to improve the erroneous estimation of the unit weight variance estimate through reasonable non-negativity constraints.

[0109] for a certain non-negative component like Then update according to the following formula Each component:

[0110]

[0111] Based on the final Helmert unit weight variance estimate adjusted for nonnegativity constraints, the prior weights are adjusted to posterior estimated weights:

[0112]

[0113] The posterior system weights estimated by Helmert variance components are used as prior weights for the next optimization step in factor graph optimization until the navigation task ends. When the number of epochs of the state to be estimated within the sliding window exceeds the window size, residual blocks that are marginalized prematurely are marginalized, and the prior system weights of the residual blocks that are not marginalized are selected as the posterior system weights of the previous step. Thus, by leveraging sliding window factor graph optimization, iterative iteration of multi-system weights is achieved, resulting in adaptively adjusted multi-system weights at each time step and the jointly optimal navigation state estimate.

[0114] This embodiment employs the aforementioned multi-mode GNSS / inertial compact combination factor graph optimization method based on robust variance component estimation. The IGG-III method is used to detect and deweight observations containing gross errors. Based on this, the Helmert variance component estimation method is used to calculate the posterior weights of the multi-GNSS systems, further adjusting the reliability of multi-system information under complex environments. This improves the overall performance of the multi-mode GNSS / inertial compact combination navigation system. In cases of complex multi-system signal quality, real-time adaptive adjustment of the reliability weights between multi-GNSS systems more effectively utilizes multi-system satellite observation information, improving the accuracy and robustness of unmanned vehicle state estimation under complex observation conditions.

[0115] Real-world datasets were collected using a low-cost vehicle-mounted multi-GNSS satellite navigation receiver and a consumer-grade inertial navigation system for on-vehicle experimental verification, such as... Figure 2 As shown, the algorithm proposed in this invention further improves absolute positioning accuracy based on existing multi-mode GNSS / inertial compact combination navigation and positioning systems optimized by factor graphs. In complex urban environments, the method proposed in this invention significantly reduces the maximum positioning error, with root mean square errors in the north, east, and vertical directions improved by 46.4%, 38.3%, and 26.6% respectively compared to single factor graph optimization. Furthermore, the statistical distribution of positioning errors using the cumulative distribution function graph is analyzed for the proposed method, robust positioning results only, variance component estimation only, factor graph optimization only, and extended Kalman filtering. Figure 3 As shown, the positioning results obtained by the method proposed in this invention exhibit a rapid increase in the cumulative distribution function of positioning errors, which is biased to the left. The positioning errors of this method are mostly within 5 meters, and the positioning error is within 2 meters 90% of the time, demonstrating the highest overall accuracy among the compared methods. These vehicle-mounted experimental results fully illustrate the accurate and robust positioning performance of the multi-mode GNSS / inertial compact combination factor graph optimization method based on robust variance component estimation proposed in this invention in complex urban environments.

[0116] The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can still be made to the technical solutions of the present invention, and these modifications or equivalent substitutions cannot cause the modified technical solutions to deviate from the spirit and scope of the technical solutions of the present invention.

Claims

1. A multi-mode GNSS / inertial compact combination factor graph optimization method based on variance component estimation, characterized in that: Includes the following steps, Step 1: Obtain pseudorange and Doppler observation data for each satellite from the raw observation information provided by multiple satellite navigation systems. Based on the satellite navigation pseudorange and Doppler observation equations, construct residual functions between the estimated state (position, velocity, attitude, sensor error) and the raw satellite observations for each satellite, respectively called pseudorange residual blocks and Doppler residual blocks. Using the state estimation result from the previous moment as a reference starting point, continuously integrate the accelerometer and gyroscope data between the arrival times of observation information from two adjacent satellites according to the inertial navigation data output frequency, up to the current moment, to obtain the position, velocity, attitude, and sensor error data. The increment of error between two time points is used to construct an inertial navigation pre-integration residual block using the difference between this increment and the states at these two adjacent time points. After constructing the pseudorange residual block, Doppler residual block, and inertial navigation integration pre-integration residual block based on satellite observations, a sliding window estimator based on factor graph optimization is used to sum all residual blocks within the time window according to the set sliding window size, constructing an optimization objective function, and using the Gauss-Newton method to solve for the joint optimal estimation result of multiple states within the time window. When a new time observation residual block and the state to be estimated are added to the window, if the window size has already reached the set window size, the Schur elimination method is used to marginalize premature observation residual blocks and the state to be estimated. Step 2: Before each sliding window factor map optimization, assign prior weights to the pseudorange and Doppler observations provided by each satellite for each operating system. These weights include the weights of different systems estimated in the previous time, the constant weights between different observation types of the same system, and the weights determined between different satellites of the same system based on the real-time satellite elevation angle. Multiply the three types of weights together. After determining the prior weights, the first factor graph optimization is performed to obtain the posterior state estimation results after the first optimization. Based on the first round of optimization state estimation results, the robust algorithm is used to detect gross errors and assign gross error weights to the observations containing gross errors in order to reduce the impact of gross errors on the weight estimation of the multi-system. Step 3: After the first optimization and weight reduction of gross observations, a second round of optimization is performed until the optimization structure converges, obtaining the joint optimal estimation result of the state at multiple time points within the current time window, i.e., the posterior estimation result of the state. Using the posterior estimation result, the observation equation design matrix, and the observation equation, the posterior observation residuals are calculated. Then, according to the minimum cost function theory, the posterior observation residuals and prior weights are substituted into the variance component estimation variance to obtain the unit weight variance estimate of the observations of each of the multiple GNSS systems. Based on this variance estimate, the prior weights are adjusted to the posterior estimation weights. Thus, while completing the state estimation of the multi-mode GNSS / inertial system, the posterior trust weights between different GNSS systems at each time point are determined. Based on the posterior trust weights, the trust level of satellite observation information of different satellite navigation systems is adjusted in real time, improving the accuracy and robustness of the multi-mode GNSS / inertial tightly integrated navigation system.

2. The multi-mode GNSS / inertial compact combination factor graph optimization method based on variance component estimation as described in claim 1, characterized in that: Step one includes the following steps: S11. Obtain pseudorange and Doppler observation data for each satellite from satellite navigation messages, satellite observation documents, and satellite broadcast ephemeris. Based on the pseudorange and Doppler observation equations, construct pseudorange residual blocks and Doppler residual blocks with the state to be estimated as the independent variable. The state to be estimated is defined as: in, and b represents the position, velocity, and attitude of the unmanned system's inertial navigation center in the navigation coordinate system at time k, respectively. g，k and b a，k These represent the random zero bias of the gyroscope and accelerometer of the inertial navigation system at time k, respectively; There is a fixed lever distance l between the inertial navigation center and the GNSS receiver center. b , l b Defined in the carrier coordinate system, pointing from the inertial navigation center to the GNSS antenna phase center; δt k =[δt G，k δt B，k δt E，k ] T and These represent the receiver clock bias and clock drift of the three global satellite navigation systems: GPS, BeiDou, and Galileo; x k+m This represents the estimated state of all epochs within the sliding window when the window size is m at time k+m; the initial state and initial state variance are set before the factor graph optimization begins. The pseudorange and Doppler observation equations are constructed. Since the navigation state constrained by pseudorange and Doppler is defined in a geocentric coordinate system, a coordinate system transformation is performed on the state to be estimated during the construction of the observation equations. in, and Let be the rotation matrices from the n-system to the e-system and from the b-system to the n-system, respectively. and These are the Earth's rotational angular velocity and the angular velocity measured by the inertial navigation system at time k, respectively. Construct the relationship between the pseudorange, the original Doppler observations, and the state to be estimated at the current moment: in and Let $k$ be the pseudorange and raw Doppler observations of the $i$-th satellite in the $S$-th satellite navigation system at time $k$. and Let D be the position and velocity of the i-th satellite of the S-th satellite navigation system at time k. sagnac and λ s δt represents the wavelength of the Sagnac effect term and the corresponding frequency band signal of the S-th satellite navigation system, respectively. S，i , and Let $k$ be the clock error, clock drift, tropospheric delay, and ionospheric delay of the $i$-th satellite in the $S$-th satellite navigation system at time $k$. These are the observation noises for pseudorange observations and Doppler observations, respectively. Construct pseudorange residual blocks as shown in equation (5) and Doppler residual blocks as shown in equation (6): in, and These represent pseudorange observation residuals and Doppler observation residuals, respectively. S12. Using the state at the previous moment as a reference, integrate the inertial navigation data between the two observation moments to obtain the inertial navigation pre-integration position, velocity, attitude, and sensor error increment, and construct the inertial navigation pre-integration residual block shown in equation (7): Where X contains the states of two adjacent time points. These are the inertial navigation pre-integration position, velocity, and attitude increments, respectively. The increments are for the correction terms for gravity and Coriolis force; S13. Using a sliding window estimator based on factor graph optimization, sum all residual blocks within the time window according to the set sliding window size to form the optimization objective function, and use the Gauss-Newton method to solve for the joint optimal estimation result of multiple states within the time window: Where, ∑ ρ ,∑ D and ∑ Pre The information matrices for pseudorange observations, Doppler observations, and inertial navigation pre-integral quantities are respectively, i.e., the inverses of each observation quantity, ∑ ρ or ∑ D The composition is as follows: in n represents the weight of the observations from the i-th satellite in the S-th system at time k. S This represents the number of available satellites observed by each system at time k.

3. The multi-mode GNSS / inertial compact combination factor graph optimization method based on variance component estimation as described in claim 2, characterized in that: Step two includes the following steps: S21, Weight of each observation This includes: weights of different systems estimated based on the previous optimization. Constant weights P between different observation types within the same system obs Based on the real-time satellite elevation angle, the weights between different satellites in the same system are then determined. Multiply the three types of weights together to obtain the prior weight matrix for all observations; Based on the real-time satellite elevation angle, the specific formula for determining the weights between different satellites in the same system is as follows: The prior weights for each observation are further obtained as follows: S22. Based on the above prior weights, the first round of optimization is performed according to equation (8) to obtain the state estimation results after the first round of optimization. The posterior residuals are calculated according to equations (5) and (6). The robust algorithm is used to detect gross errors and assign gross error weights to the observations containing gross errors to reduce the impact of gross errors on the weight estimation of the multi-system. The gross error adjustment weights for each observation are obtained.

4. The multi-mode GNSS / inertial compact combination factor graph optimization method based on variance component estimation as described in claim 3, characterized in that: Step three includes the following steps: S31. After determining the gross error weights, update the prior weight matrix by combining it with the prior weights before robustness: Then, a second round of optimization is performed according to equation (8) until the optimization structure converges, obtaining the joint optimal estimation result of the states at multiple time points within the current time window. S32. Utilizing posterior estimation results The observation equation design matrix and the observation equation are used to calculate the posterior observation residuals. Then, the posterior weights are adaptively adjusted according to the cost function minimization theory. By designing matrices based on the observation equations, the posterior residuals of the GPS, BeiDou, and Galileo systems can be approximated as follows: in The design matrix can be obtained by finding the Jacobian matrix of the residual block relative to the state to be estimated. Substituting the posterior observation residuals and prior weights into the variance component estimation, we obtain the unit-weighted variance estimates for the observations of multiple GNSS systems. The S matrix is ​​determined by the various design matrices and the prior weight matrix, and its specific form varies depending on the variance component estimation method used. This is the weight matrix for the satellite observations of each system, and the distribution of this matrix is ​​consistent with that of equation (9); Based on the adjusted unit weight variance estimate, the prior weights are adjusted to the posterior estimated weights: The posterior system weights estimated from the variance components are used as the prior weights for the next optimization and the factor graph optimization continues until the navigation task ends. When the number of state epochs to be estimated in the sliding window is greater than the window size, the residual blocks that are marginalized too early are marginalized, and the prior system weights of the residual blocks that are not marginalized are selected as the posterior system weights of the previous time step. In this way, the posterior multi-GNSS system weights are iteratively adjusted through the sliding window factor graph optimization to obtain the adaptively adjusted multi-system weights and the optimal navigation state estimate at each time step.

Images