Multi-mode GNSS / inertia tight combination factor graph optimization method based on variance component estimation

Abstract

The invention discloses a multimode GNSS / inertia tight combination factor graph optimization method based on variance component estimation, and belongs to the technical field of satellite / inertia combination navigation. The implementation method comprises the following steps: fusing observed quantities of a multimode GNSS and an inertial navigation system on the basis of a factor graph optimization method under the condition of distributing prior weights among multiple GNSS systems; on the basis of a posteriori state estimation result obtained after factor graph optimization, combining a multi-mode GNSS observation equation, calculating a posteriori residual error, and utilizing a variance component estimation method to adaptively adjust weights among the multiple GNSS systems in real time according to the signal quality of satellites of the satellite navigation systems; on the basis of gross error detection and a weight reduction algorithm, weight reduction processing is carried out on the satellite observed quantity containing gross errors before the weight between the systems is adjusted in a self-adaptive mode. According to the invention, the robust variance component estimation is used to determine the weight among multiple GNSS systems, the reliability of satellite observation information of different satellite navigation systems is adjusted in real time, and the precision and robustness of the multimode GNSS / inertial tight integration navigation system are improved.

Description

Technical Field

[0001] The invention belongs to the technical field of satellite / inertial combined navigation and relates to a multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation. Background Art

[0002] With the opening of global satellite navigation systems (GNSS) such as the Global Positioning System (GPS), Galileo, and BeiDou Navigation Satellite System (BDS), the demand for high-precision and robust GNSS positioning services in multiple industries, especially in the field of motion state estimation of autonomous intelligent unmanned systems, has grown rapidly. However, in scenes with severe signal obstruction such as urban centers, transportation hubs, and mountainous areas, or in harsh environmental conditions such as thunderstorms and magnetic storms, the number of available GNSS satellites is reduced, and it is difficult for carriers to obtain accurate state estimation information by relying on observation information provided by a single GNSS.

[0003] In the case of a limited number of available satellites, the observation information provided by multiple global satellite navigation systems that have been opened is used, and the original GNSS observation information is combined with the continuous and stable state estimation information provided by the inertial navigation system. The factor graph is used to optimize this type of sliding window state estimation algorithm, making full use of multi-mode GNSS observation information and inertial navigation state estimation information at multiple times to provide high-precision navigation and positioning results for unmanned systems.

[0004] Since the satellites of multiple global satellite navigation systems are designed, distributed, and use different frequency bands, and the number and signal quality of satellites of different systems observed in different regions at the same time are also different, when integrating the observation information provided by satellites of different systems, determining the trust weight of observation information from different satellite navigation systems, detecting and reducing the impact of gross errors in observation information are the key to further improving the positioning accuracy and robustness of multi-mode GNSS / inertial integrated navigation. However, the current GNSS / inertial integrated navigation system based on factor graph optimization method mainly focuses on using multiple different types of GNSS observation information, such as pseudorange, Doppler, carrier, etc., and only considers the weights between different types of observation information of the same system or different altitude satellite observation information of the same observation type in the same system, and lacks consideration of the weights between observation information provided by different satellite navigation systems. Once a satellite navigation system is affected by human attacks or natural causes and causes erroneous interference in observation information, if the trust weight of the observation information of the satellite navigation system cannot be adaptively adjusted, it will seriously affect the overall navigation and positioning performance of the multi-mode GNSS / inertial integrated navigation, resulting in an erroneous estimation of the navigation state of the unmanned system. Therefore, the present invention proposes a multi-mode GNSS / inertial tight combination factor graph optimization method based on robust variance component estimation, considering multiple groups of observation information and state information of multiple epochs, and iteratively determining the weights between multiple GNSS using the variance component estimation method. At the same time, in order to reduce the impact of satellite observation information containing gross errors on weight estimation and state estimation, a gross error detection and robustness algorithm for GNSS pseudorange and Doppler observations is designed, thereby improving the accuracy and robustness of multi-mode GNSS / inertial tight combination navigation positioning based on factor graph optimization. Summary of the invention

[0005] When combining the original observation information of multiple different global satellite navigation systems with the dead reckoning information of inertial navigation, the different constellation designs of different GNSS systems and the differences in satellite distribution and signal quality of different GNSS systems at the same time are taken into consideration. The purpose of the present invention is to provide a multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation, which can not only further improve the accuracy of the tight combination of multi-mode GNSS and inertial navigation, but also make full use of the advantages of factor graph optimization considering multiple epochs to be estimated and observation information, cyclically adaptively adjust the weights of multiple GNSS systems, further improve the overall robustness of the combined navigation system, and enhance the accuracy of the combined navigation system under GNSS interference or deception.

[0006] The purpose of the present invention is achieved through the following technical solutions.

[0007] The present invention discloses a multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation. Under the condition of allocating a priori weights between multi-GNSS systems, the observations of multi-mode GNSS and inertial navigation systems are integrated based on the factor graph optimization method to obtain accurate navigation state estimation results; based on the posterior state estimation results obtained after factor graph optimization, the posterior residual is calculated in combination with the multi-mode GNSS observation equation, and the weights between the multi-GNSS systems are adaptively adjusted in real time according to the signal quality of satellites of each satellite navigation system by using the variance component estimation method; based on the gross error detection and weight reduction algorithm, before the weights between the systems are adaptively adjusted, the satellite observations containing gross errors are weighted down to avoid the influence of the gross error observations on the variance component estimation algorithm. The present invention determines the weights between the multi-GNSS systems by using the robust variance component estimation for the multi-mode GNSS / inertial tight combination navigation system based on factor graph optimization. The adaptive weighting by the robust variance component estimation method adjusts the trust in the satellite observation information of different satellite navigation systems in real time, thereby improving the accuracy and robustness of the multi-mode GNSS / inertial tight combination navigation system.

[0008] The multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation disclosed in the present invention comprises the following steps:

[0009] Step 1: From the original observation information provided by satellites of multiple satellite navigation systems, obtain the pseudorange and Doppler observation data of each satellite. According to the satellite navigation pseudorange observation equation and Doppler observation equation, for each satellite's pseudorange and Doppler observation, construct a residual function containing the estimated states such as position, velocity, attitude, and sensor error and the original satellite observations, which are called pseudorange residual blocks and Doppler residual blocks respectively; according to the basic equation of inertial navigation, take the result of the state estimation at the previous moment as the reference starting point, and continuously integrate the accelerometer and gyroscope data between the arrival times of two adjacent satellite observation information to the current moment according to the inertial navigation data output frequency, and obtain the position, velocity, attitude and sensor error. The error increment between two moments is used to construct the inertial navigation pre-integration residual block using the difference between the increment and the states at these two adjacent moments. After constructing the pseudorange residual block, Doppler residual block and pre-integration residual block of inertial navigation integration based on satellite observation, a sliding window estimator based on factor graph optimization is used to sum all the residual blocks in the time window according to the set sliding window size, and an optimization objective function is constructed. The Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window. When the new moment observation residual block and the state to be estimated are added to the window, if the window size has reached the set window size, the Schur elimination method is used to marginalize the premature observation residual block and the state to be estimated.

[0010] Step 2: Before each sliding window factor graph optimization, assign a priori weights to the pseudorange observations and Doppler observations provided by each satellite in each operating system. This weight includes the weights of different systems estimated in the last time, the constant weights between different observation types in the same system, and the weights between different satellites in the same system determined according to the real-time satellite elevation angle. The three types of weights are multiplied. After determining the prior weights, perform the first factor graph optimization to obtain the posterior state estimation results after the first optimization. Based on the results of the first round of optimized state estimation, use the robustness algorithm to detect gross errors and assign gross error weights to the observations containing gross errors to reduce the impact of gross errors on multi-system weight estimation.

[0011] Step 3: After the first optimization and weight reduction for gross error observations, a second round of optimization is performed until the optimization structure converges to obtain the joint optimal estimation result of the state at multiple moments in the current time window, that is, the posterior estimation result of the state. The posterior observation residual is calculated using the posterior estimation result, the observation equation design matrix and the observation equation. Then, according to the cost function minimum theory, the posterior observation residual and the prior weight are substituted into the variance component estimation variance to obtain the unit weight variance estimation of the observation quantities of multiple GNSS systems. According to the variance estimation, the prior weight is adjusted to the posterior estimation weight, so that while completing the state estimation of the multi-mode GNSS / inertial system, the posterior trust weights between different GNSS systems at each moment are determined, and the degree of trust in the satellite observation information of different satellite navigation systems is adjusted in real time according to the posterior trust weight, so as to improve the accuracy and robustness of the multi-mode GNSS / inertial tightly integrated navigation system.

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

[0013] S11. Obtain the pseudorange and Doppler observation data of each satellite from the satellite navigation message, satellite observation file and satellite broadcast ephemeris, and construct the pseudorange residual block and Doppler residual block with the state to be estimated as the independent variable according to the pseudorange and Doppler observation equations. The state to be estimated is defined as:

[0014]

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

[0016] There is a fixed arm distance l between the inertial navigation center and the GNSS receiver center. b , l b Defined in the carrier coordinate system (b system), it points 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 Represents the receiver clock error and clock drift of the three global satellite navigation systems: GPS, Beidou and Galileo. k+m Represents the estimated state of all epochs in the sliding window when the window size is m at the k+mth moment. The initial state and initial state variance are set before the factor graph optimization begins.

[0018] Construct pseudorange and Doppler observation equations. Since the navigation state of pseudorange and Doppler constraints is defined in the Earth-centered Earth-fixed coordinate system (ECEF system, e system), the coordinate system transformation is performed on the estimated state during the construction of the observation equation:

[0019]

[0020] in, and are the rotation matrices from n to e and from b to n, respectively. and are the angular velocity of the earth's rotation and the angular velocity measured by the inertial navigation at time k respectively.

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

[0022]

[0023] in and are the pseudorange and Doppler raw observations of the ith satellite of the Sth satellite navigation system at time k, and are the position and velocity of the i-th satellite of the S-th satellite navigation system at time k, D sagnac and λ S are the Sagnac effect term and the wavelength of the corresponding frequency band signal of the Sth satellite navigation system. S，i , and are the clock error, clock drift, tropospheric delay and ionospheric delay of the i-th satellite of the S-th satellite navigation system at time k, are the observation noise of pseudorange observation and Doppler observation 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 represent the pseudorange observation residual and Doppler observation residual, respectively.

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

[0028]

[0029] Where X contains the states of two adjacent moments, They are the inertial navigation pre-integrated position, velocity, and attitude increments, is the increment of gravity and Coriolis force correction terms.

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

[0031]

[0032] Among them, ∑ ρ ,∑ D and∑ Pre are the information matrix of pseudo-range observation, Doppler observation and inertial navigation pre-integration, i.e., the inverse of each observation, ∑ ρ or D The composition form is:

[0033]

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

[0035] Preferably, step 2 comprises the following steps:

[0036] S21, each observation weight Includes: weights of different systems estimated from the last optimization The constant weight P between different observation types in the same system obs , according to the real-time satellite altitude angle, and then determine the weights between different satellites in the same system The three types of weights are multiplied to obtain the prior weight matrix of all observations;

[0037] According to 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 weight of each observation is further obtained as:

[0040]

[0041] S22. On the basis of determining the above a priori weights, perform the first round of optimization according to formula (8) to obtain the state estimation result after the first round of optimization, calculate the posterior residual according to formula (5) (6), use the robustness algorithm to detect gross errors and assign gross error weights to the observations containing gross errors to reduce the impact of gross errors on the multi-system weight estimation, and obtain the gross error adjustment weight of each observation

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

[0043] S31. After determining the gross error weight, update the prior weight matrix in combination with the prior weight before robustness:

[0044]

[0045] Then, according to formula (8), a second round of optimization is performed until the optimized structure converges, and the joint optimal estimation result of the states at multiple moments in the current time window is obtained.

[0046] S32. Using the posterior estimation results The observation equation design matrix and the observation equation calculate the posterior observation residual, and then the posterior weights are adaptively adjusted according to the cost function minimum theory;

[0047] By designing the matrix through the observation equation, the posterior residuals of the GPS, Beidou and Galileo systems are approximately expressed as:

[0048]

[0049] in To design the matrix, it can be obtained by calculating the Jacobian matrix of the residual block relative to the state to be estimated.

[0050] Substitute the posterior observation residuals and prior weights into the variance component estimation variance to obtain the unit weight variance estimates of the observations of multiple GNSS systems.

[0051]

[0052] Among them, the S matrix is ​​determined by each design matrix and the prior weight matrix, and its specific form varies according to the variance component estimation method used. is the weight matrix of satellite observations of each system, and the distribution of this matrix is ​​consistent with formula (9).

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

[0054]

[0055] The posterior system weights after variance component estimation are used as the prior weights for the next optimization and factor graph optimization is continued until the navigation task is completed. 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 and the prior system weights of the residual blocks that are not marginalized are selected as the previous a posteriori system weights, thereby achieving repeated iterations of the a posteriori multi-GNSS system weights with the help of sliding window factor graph optimization, and obtaining the multi-system weights that are adaptively adjusted at each moment and the optimal navigation state estimation.

[0056] Beneficial effects:

[0057] 1. The multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation disclosed in the present invention increases the number of visible satellites in complex environments, improves the geometric distribution of satellites, augments the observation matrix, and integrates the information provided by an inertial navigation system that can work autonomously and stably, thereby improving the state estimation accuracy and robustness of the GNSS / inertial tight combination navigation system.

[0058] 2. The multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation disclosed in the present invention forms a tight combination system of multi-mode GNSS and inertial navigation based on the sliding window factor graph optimization method, integrates multi-mode GNSS pseudorange observations, Doppler observations and pre-integrated quantities of inertial navigation, and obtains accurate multi-epoch joint optimal state estimation results at low cost and low computational consumption by considering the estimated state and observation information at multiple moments in the time window. The tight combination method complements the advantages of the two navigation methods by integrating the observations of inertial navigation and satellite navigation, and improves the overall performance of the GNSS system in complex environments.

[0059] 3. The multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation disclosed in the present invention calculates the posterior residuals by using the optimized state, pseudorange and Doppler observation equations, makes full use of the redundant contribution of observation information, obtains the unit weight variance of the multi-system, and performs a posteriori weighting on the multi-GNSS system by calculating the unit weight variance through variance component estimation. It can make full use of the results of state estimation to adaptively improve the degree of trust in different GNSS systems in real time, and can improve the positioning accuracy of the entire tight combination system when the satellite distribution of the a posteriori multi-GNSS system is poor, the quality is poor, or some system signals are interfered. Reduce the impact of the GNSS system with large errors on the overall positioning accuracy of the entire tight combination system.

[0060] 4. The present invention discloses a multi-mode GNSS / inertial tight combination factor graph optimization method based on robust variance component estimation. In view of the problem that the original observation information provided by satellites of multiple GNSS systems in complex environments may have large gross errors, and the observations with gross errors may cause distortion of the variance component estimation model, which in turn leads to misjudgment of multi-system weights, a GNSS original observation robust model is introduced before determining the posterior weights. The accuracy and reliability of variance component estimation are improved by downgrading the observation information containing gross errors. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0062] Figure 2 The results of an on-vehicle experiment conducted to verify the actual effect of the present invention are compared with the position, speed, and attitude error curves of the method proposed in the present invention and those optimized by a single factor graph.

[0063] Figure 3 It is the cumulative distribution function curve of the positioning error of the method proposed in the present invention and the existing single robustness method, single variance component estimation method, single factor graph optimization method and extended Kalman filtering method. DETAILED DESCRIPTION

[0064] In order to make the purpose, technical scheme and advantages disclosed in the examples of the present invention clearer, the embodiments of the present invention are further described in detail in combination with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the embodiments of the present invention and are not used to limit the embodiments of the present invention. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application. Examples of the embodiments are shown in the accompanying drawings, where the same or similar reference numerals throughout represent the same or similar elements or elements with the same or similar functions.

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

[0066] Like reference numerals and letters denote similar items in the following drawings, and thus, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0067] This embodiment takes an unmanned vehicle with a receiver and an inertial navigation module capable of receiving three different types of constellation signals of GPS / Galileo / Beidou as an example, and the vehicle needs to inspect according to a specified track in a complex environment. In a complex environment, the satellite signal is obviously blocked, and the distribution and quality of the visible satellites will be weakened by environmental influences such as blocking and reflection. The number of available satellites is significantly reduced, and it is seriously insufficient to rely on a single system to observe satellites. It is necessary to introduce the GPS / Galileo / Beidou three systems to increase the number of available observations, thereby increasing the effective information amount of the multi-mode GNSS / inertial combined navigation system state estimation. However, although the introduction of multi-system observations has expanded the observation equation, the quality and distribution of satellites in different systems are different, and the trust weights of each system should be different. It is necessary to use a suitable variance component estimation method to estimate the weights. In this embodiment, the Helmert variance component estimation method is selected to estimate the weights of observations of different GNSS systems based on the prior weights. In view of the possible impact of some observations containing gross errors on weight estimation, it is also necessary to use gross error detection and weight reduction methods to reduce the impact of observations containing gross errors. In this embodiment, the commonly used IGG-III gross error detection and weight reduction method is selected to divide the observations of each satellite into three parts: no gross errors, suspicious gross errors and serious gross errors for segmented weight reduction. The following example uses the Helmert variance component estimation method under IGG-III anti-error to adaptively estimate the state of multi-mode GNSS / inertial tight integrated navigation based on factor graph optimization method and the weights between different systems in real time.

[0068] like Figure 1 As shown, multi-system weight estimation and vehicle navigation state estimation are performed on the tightly combined system carried on the unmanned vehicle. The multi-mode GNSS / inertial tightly combined factor graph optimization method based on robust variance component estimation disclosed in this embodiment is specifically implemented as follows:

[0069] Step 1. Obtain the pseudorange and Doppler observation data of each satellite from the original observation information provided by the satellites of multiple satellite navigation systems received by the on-board receiver of the car. According to the satellite navigation pseudorange observation equation and the Doppler observation equation, for the pseudorange and Doppler observations of each satellite, construct a residual function containing the estimated states such as position, velocity, attitude, and sensor error and the original observations of the satellite, which are called pseudorange residual blocks and Doppler residual blocks respectively; according to the basic equation of inertial navigation, take the result of the state estimation at the previous moment as the reference starting point, continuously integrate the accelerometer and gyroscope data between the arrival times of two adjacent satellite observation information to the current moment according to the output frequency of the inertial navigation data, and obtain the position, velocity, attitude and sensor error. The increment of sensor error between two moments is used, and the difference between the increment and the state of these two adjacent moments is used to construct the inertial navigation pre-integration residual block; after constructing the pseudorange residual block from satellite observation, the Doppler residual block and the pre-integration residual block from the inertial navigation integration, a sliding window estimator based on factor graph optimization is used to add all the residual blocks in the time window according to the set sliding window size to form an optimization objective function, and the Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window; when the new moment observation residual block and the state to be estimated are added to the window, if the window size has reached the set window size, the Schur elimination method is used to marginalize the premature observation residual block and the state to be estimated;

[0070] Step 1 specifically includes the following steps:

[0071] S11. The status to be estimated is defined as:

[0072]

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

[0074] There is a fixed arm distance l between the inertial navigation center and the GNSS receiver center. b , l b Defined in the carrier coordinate system (b system), it points 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 Represents the receiver clock error and clock drift of the three global satellite navigation systems: GPS, Beidou and Galileo. k+m Represents the estimated state of all epochs in the sliding window when the window size is m at the k+mth moment. The initial state and initial state variance are set before the factor graph optimization begins.

[0076] Construct pseudorange and Doppler observation equations. Since the navigation state of pseudorange and Doppler constraints is defined in the Earth-centered Earth-fixed coordinate system (ECEF system, e system), it is necessary to transform the coordinate system of the estimated state in the process of constructing the observation equation:

[0077]

[0078] in, and are the rotation matrices from n to e and from b to n, respectively. and are the angular velocity of the earth's rotation and the angular velocity measured by the inertial navigation at time k respectively.

[0079] The pseudorange and Doppler observation data of each satellite are obtained from the satellite navigation message, satellite observation file and satellite broadcast ephemeris. According to the pseudorange and Doppler observation equations, the pseudorange residual block and Doppler residual block with the estimated state as the independent variable are constructed:

[0080]

[0081] S12, taking 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-integrated position, velocity, attitude and sensor error increment, and use the difference between the increment and the state to construct the inertial navigation pre-integrated residual block:

[0082]

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

[0084]

[0085] Step 2. After completing the construction of the multi-mode GNSS / inertial tight integration navigation based on the 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 downgrade the weights of observations containing gross errors; before each sliding window factor graph optimization, assign prior weights to the pseudorange observations and Doppler observations provided by each satellite in each operating system. This weight includes the weights of different systems estimated in the last time, the constant weights between different observation types in the same system, and the weights between different satellites in the same system determined according to the real-time satellite elevation angle. The three types of weights are multiplied together. After determining the prior weights, perform the first factor graph optimization to obtain the posterior state estimation results after the first optimization. Based on the results of the first round of optimized state estimation, use an anti-error algorithm to detect gross errors and assign gross error weights to observations containing gross errors to reduce the impact of gross errors on multi-system weight estimation;

[0086] Step 2 specifically includes the following steps:

[0087] S21, each observation weight Includes: weights of different systems estimated from the last optimization The constant weight P between different observation types in the same system obs , according to the real-time satellite altitude angle, and then determine the weights between different satellites in the same system The three types of weights are multiplied to obtain the prior weight matrix of all observations;

[0088] According to 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] We can further obtain the prior weight of each observation as:

[0091]

[0092] S22. Perform the first round of optimization according to formula (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 to perform gross error detection and weight reduction. The impact of gross errors on multi-system weight estimation is nonlinearly reduced in three stages: no gross errors, suspicious gross errors, and serious gross errors. The robust estimation weight function of IGG-III is:

[0094]

[0095] Among them, v iis the a posteriori residual of the pseudorange or Doppler observation of a single satellite in a single system. σ is generally taken as the median of all pseudorange or Doppler a posteriori residuals observed at the current moment. med / 0.6745, k 1 and k 2 are constants, generally ranging from 1.5 to 2.0, 3.0 to 8.5, n is the number of all observations at the current moment, m is the dimension of the state to be estimated, γ i is the gross error reduction coefficient of the current observation.

[0096] Step 3: Figure 1 As shown in the figure, after the first optimization and using the IGG-III method to reduce the weight of the gross error observations, the prior weights are multiplied by the gross error reduction coefficients, and the second round of optimization is performed until the optimization structure converges to obtain the joint optimal estimation results of the states at multiple moments in the current time window, that is, the posterior estimation results of the states. The posterior observation residuals are calculated using the posterior estimation results, the observation equation design matrix and the observation equation, and then the Helmert variance component estimation equation is constructed based on the Helmert variance component estimation cost function minimum theory. The posterior observation residuals and the prior weights are substituted into the variance component estimation variance to obtain the unit weight variance estimates of the observations of multiple GNSS systems. According to the unit weight variance estimates, the prior weights are adjusted to the 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 moment are determined.

[0097] Step three specifically includes the following steps:

[0098] S31. After using IGG-III to reduce the gross error weight according to (25), the prior weight matrix is ​​updated in combination with the prior weight before robustness:

[0099]

[0100] The second round of optimization is performed according to formula (22) until the optimization structure converges and the joint optimal estimation result of the states at multiple moments in the current time window is obtained.

[0101] S32. According to the Helmert 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 to obtain the unit weight variance estimates of the observation quantities of the multiple GNSS systems;

[0102] By designing the matrix through the observation equation, the posterior residuals of the GPS, Beidou and Galileo systems can be approximately expressed as:

[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] Substitute the posterior observation residuals and the prior weights into the variance component estimation variance, and according to the least squares error minimization theory, obtain the unit weight variance estimates of the observations of multiple GNSS systems estimated by the Helmert variance component And the S matrix:

[0106]

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

[0108] In the Helmert variance component estimation, the unit weight variance estimate When a component may have a negative value, the unit weight variance estimate may not meet the non-negative variance setting, and it is necessary to improve the misestimation of the unit weight variance estimate through reasonable non-negative constraints.

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

[0110]

[0111] According to the final Helmert unit weight variance estimate adjusted by the non-negative constraint, the prior weights are adjusted to the posterior estimated weights:

[0112]

[0113] The posterior system weights estimated by the Helmert variance components are used as the prior weights for the next optimization and factor graph optimization is continued until the navigation task is completed. 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 and the prior system weights of the residual blocks that are not marginalized are selected as the previous posterior system weights, thereby achieving repeated iterations of multi-system weights with the help of sliding window factor graph optimization, and obtaining the adaptively adjusted multi-system weights at each moment and the joint optimal navigation state estimation.

[0114] This embodiment adopts the above-mentioned multi-mode GNSS / inertial tight combination factor graph optimization method based on robust variance component estimation, uses the IGG-III method to detect and reduce the weight of observations containing gross errors, and on this basis uses the Helmert variance component estimation method to calculate the posterior multi-GNSS system weights, further adjusts the trust level of multi-system information in complex environments, and thus improves the overall performance of the multi-mode GNSS / inertial tight combination navigation system. In the case of complex multi-system signal quality, the trust weights between multi-GNSS systems are adjusted in real time and adaptively, so that the multi-system satellite observation information is more effectively utilized, and the accuracy and robustness of the unmanned vehicle state estimation under complex observation conditions are improved.

[0115] The actual data set is collected by using low-cost multi-GNSS satellite navigation receivers and consumer-grade inertial navigation systems on vehicles, and then verified by vehicle-mounted experiments, such as Figure 2 As shown, through the algorithm proposed in the present invention, the absolute positioning accuracy is further improved on the basis of the existing multi-mode GNSS / inertial tight combination navigation positioning based on factor graph optimization. Facing the complex urban environment, the maximum positioning error of the method proposed in the present invention is significantly reduced, and the root mean square error of the north, east, and vertical positions are respectively improved by 46.4%, 38.3%, and 26.6% compared with the single factor graph optimization. Furthermore, the cumulative distribution function graph is used to analyze the statistical distribution of the positioning results of the method proposed in the present invention, the positioning results of only anti-error, the positioning results of only variance component estimation, the positioning results of only factor graph optimization, and the positioning errors of the extended Kalman filter. As shown Figure 3 As shown, the positioning result obtained by the method proposed in the present invention has a positioning error cumulative distribution function that rises quickly and is biased to the left. The positioning error of this method is mostly within 5 meters, and the positioning error is within 2 meters at 90% of the time. The overall accuracy is the highest among the compared methods. The vehicle-mounted experimental results can fully illustrate the accurate and robust positioning performance of the multi-mode GNSS / inertial tight combination factor graph optimization method based on robust variance component estimation proposed in the present invention in complex urban environments.

[0116] The above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that they can still modify or replace the technical solution of the present invention with equivalents, and these modifications or equivalent replacements cannot cause the modified technical solution to deviate from the spirit and scope of the technical solution of the present invention.

Claims

1. A multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation, characterized by: The following steps are included: Step 1: From the original observation information provided by satellites of multiple satellite navigation systems, obtain the pseudorange and Doppler observation data of each satellite. According to the satellite navigation pseudorange observation equation and Doppler observation equation, for each satellite's pseudorange and Doppler observation, construct a residual function containing the estimated states such as position, velocity, attitude, and sensor error and the original satellite observations, which are called pseudorange residual blocks and Doppler residual blocks respectively; according to the basic equation of inertial navigation, take the result of the state estimation at the previous moment as the reference starting point, and continuously integrate the accelerometer and gyroscope data between the arrival times of two adjacent satellite observation information to the current moment according to the inertial navigation data output frequency, and obtain the position, velocity, attitude and sensor error. The error increment between two moments is used to construct the inertial navigation pre-integration residual block by using the difference between the increment and the state of the two adjacent moments; after constructing the pseudorange residual block, Doppler residual block and pre-integration residual block of inertial navigation integration based on satellite observation, a sliding window estimator based on factor graph optimization is used to add all residual blocks in the time window according to the set sliding window size, construct the optimization objective function, and use the Gauss-Newton method to solve the joint optimal estimation result of multiple states in the time window; when the new moment observation residual block and the state to be estimated are added to the window, if the window size has reached the set window size, the Schur elimination method is used to marginalize the premature observation residual block and the state to be estimated; Step 2: Before each sliding window factor graph optimization, assign a priori weights to the pseudorange observations and Doppler observations provided by each satellite in each operating system. This weight includes the weights of different systems estimated in the last time, the constant weights between different observation types in the same system, and the weights between different satellites in the same system determined according to the real-time satellite altitude angle. The three types of weights are multiplied together. After determining the prior weights, the first factor graph optimization is performed to obtain the posterior state estimation results after the first round of optimization. According to the state estimation results of the first round of optimization, the robust algorithm is used to detect gross errors and re-assign gross error weights to the observations containing gross errors to reduce the impact of gross errors on the multi-system weight estimation. Step 3: After the first optimization and weight reduction for gross error observations, a second round of optimization is performed until the optimization structure converges to obtain the joint optimal estimation result of the state at multiple moments in the current time window, that is, the posterior estimation result of the state. The posterior observation residual is calculated using the posterior estimation result, the observation equation design matrix and the observation equation. Then, according to the cost function minimum theory, the posterior observation residual and the prior weight are substituted into the variance component estimation variance to obtain the unit weight variance estimation of the observation quantities of multiple GNSS systems. According to the variance estimation, the prior weight is adjusted to the posterior estimation weight, so that while completing the state estimation of the multi-mode GNSS / inertial system, the posterior trust weights between different GNSS systems at each moment are determined, and the degree of trust in the satellite observation information of different satellite navigation systems is adjusted in real time according to the posterior trust weight, so as to improve the accuracy and robustness of the multi-mode GNSS / inertial tightly integrated navigation system.

2. The multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation according to claim 1, characterized in that: Step 1 includes the following steps, S11. Obtain the pseudorange and Doppler observation data of each satellite from the satellite navigation message, satellite observation file and satellite broadcast ephemeris, and construct a pseudorange residual block and a Doppler residual block with the state to be estimated as the independent variable according to the pseudorange and Doppler observation equations; the state to be estimated is defined as: in, and represents the position, velocity and attitude of the inertial navigation center of the unmanned system in the navigation coordinate system at time k, b g，k and b a，k Represent the random zero bias of the gyroscope and accelerometer of the inertial navigation system at time k; There is a fixed arm distance l between the inertial navigation center and the GNSS receiver center. b , l b Defined in the carrier coordinate system, from the inertial navigation center to the GNSS antenna phase center; δt k =[δt G，k δt B，k δt E，k ] T and Represents the receiver clock error and clock drift of the three global satellite navigation systems: GPS, Beidou and Galileo; x k+m represents the estimated state of all epochs in the sliding window when the window size is m at the k+mth moment; the initial state and initial state variance are set before the factor graph optimization starts; Construct pseudorange and Doppler observation equations. Since the navigation state of pseudorange and Doppler constraints is defined in the Earth-centered Earth-fixed coordinate system, the coordinate system transformation is performed on the estimated state during the construction of the observation equation: in, and are the rotation matrices from n to e and from b to n, respectively. and are the angular velocity of the earth's rotation and the angular velocity measured by the inertial navigation at time k respectively; Construct the relationship between pseudorange, Doppler original observation and the state to be estimated at the current moment: in and are the pseudorange and Doppler raw observations of the i-th satellite of the S-th satellite navigation system at time k, and are the position and velocity of the i-th satellite of the S-th satellite navigation system at time k, D sagnac and λ s are the Sagnac effect term and the wavelength of the corresponding frequency band signal of the Sth satellite navigation system; δt S，i , and are the clock error, clock drift, tropospheric delay and ionospheric delay of the i-th satellite of the S-th satellite navigation system at time k, are the observation noise of pseudorange observation and Doppler observation respectively; Construct the pseudorange residual block as shown in equation (5) and the Doppler residual block as shown in equation (6): in, and represent pseudorange observation residuals and Doppler observation residuals respectively; S12, taking the state at the previous moment as a reference, integrate the inertial navigation data between the two observation moments, obtain the inertial navigation pre-integrated position, velocity, attitude and sensor error increment, and construct the inertial navigation pre-integrated residual block shown in formula (7): Where X contains the states of two adjacent moments, They are the inertial navigation pre-integrated position, velocity, and attitude increments, is the increment of gravity and Coriolis force correction term; S13. Using a sliding window estimator based on factor graph optimization, according to the set sliding window size, all residual blocks in the time window are summed to form an optimization objective function, and the Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window: Among them, ∑ ρ ,∑ D and∑ Pre are the information matrix of pseudo-range observation, Doppler observation and inertial navigation pre-integration, i.e., the inverse of each observation, ∑ ρ or D The composition form is: in represents the weight of the observation of the i-th satellite of the S-th system at time k, n S Represents the number of available satellites observed by each system at time k.

3. The multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation according to claim 2, characterized in that: Step 2 includes the following steps: S21, weight of each observation Includes: weights of different systems estimated from the last optimization The constant weight P between different observation types in the same system obs , according to the real-time satellite altitude angle, and then determine the weights between different satellites in the same system The three types of weights are multiplied to obtain the prior weight matrix of all observations; According to 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 weight of each observation is further obtained as: S22. On the basis of determining the above a priori weights, perform the first round of optimization according to formula (8) to obtain the state estimation result after the first round of optimization, calculate the posterior residual according to formula (5) (6), use the robustness algorithm to detect gross errors and assign gross error weights to the observations containing gross errors to reduce the impact of gross errors on the multi-system weight estimation, and obtain the gross error adjustment weight of each observation 4. The multi-mode GNSS / inertial tight combination factor graph optimization method based on variance component estimation according to claim 3, characterized in that: Step three includes the following steps, S31. After determining the gross error weight, update the prior weight matrix in combination with the prior weight before robustness: Then, according to formula (8), a second round of optimization is performed until the optimized structure converges, and the joint optimal estimation result of the states at multiple moments in the current time window is obtained. S32. Using the posterior estimation results The observation equation design matrix and the observation equation calculate the posterior observation residual, and then the posterior weights are adaptively adjusted according to the cost function minimum theory; By designing the matrix through the observation equation, the posterior residuals of the GPS, Beidou and Galileo systems are approximately expressed as: in is the design matrix, which can be obtained by finding the Jacobian matrix of the residual block relative to the state to be estimated; Substitute the posterior observation residuals and prior weights into the variance component estimation variance to obtain the unit weight variance estimates of the observations of multiple GNSS systems. Among them, the S matrix is ​​determined by each design matrix and the prior weight matrix, and its specific form varies according to the variance component estimation method used. is the weight matrix of satellite observations of each system, and the distribution of this matrix is ​​consistent with formula (9); According to the adjusted unit weight variance estimate, the prior weights are adjusted to the posterior estimated weights: The posterior system weights after variance component estimation are used as the prior weights for the next optimization and factor graph optimization is continued until the navigation task is completed. 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 and the prior system weights of the residual blocks that are not marginalized are selected as the previous a posteriori system weights. Thus, the sliding window factor graph optimization is used to achieve repeated iteration of the a posteriori multi-GNSS system weights, and the multi-system weights that are adaptively adjusted at each moment and the optimal navigation state estimation are obtained.

Images