GNSS (Global Navigation Satellite System) / inertial navigation tight integrated navigation method and system based on robust self-speed constraint factor graph optimization

Abstract

The invention discloses a GNSS (Global Navigation Satellite System) / inertial navigation tightly integrated navigation method and system based on robust auto-velocity constraint factor graph optimization, and the method specifically comprises the following steps: extracting pseudo-range and Doppler observation data from original observation information of a GNSS satellite, and obtaining accelerometer and gyroscope data from an INS (Inertial Navigation System); constructing a pseudo-range residual block, a Doppler residual block, an inertial navigation pre-integration residual block and a speed constraint factor, and integrating into a sliding window estimator based on factor graph optimization; according to the size of a set sliding window, adding all residual blocks in the window to form an optimized objective function; performing a first round of factor graph optimization to obtain a preliminary state estimation result, and endowing the observed quantity containing gross error with a relatively low weight; and updating the prior weight matrix, and carrying out second round of optimization to obtain a joint optimal estimation result of a plurality of moment states in the current time window. According to the method, the influence of gross error-containing observation information on weight estimation and state estimation is suppressed, the positioning precision is high, the robustness is strong, and the reliability is high.

Description

Technical Field

[0001] The present invention relates to the field of satellite / inertial navigation integrated navigation technology, in particular to a GNSS / inertial navigation tightly integrated navigation method and system based on robust self-velocity constraint factor graph optimization. Background Art

[0002] Autonomous unmanned systems are systems that perform specific tasks or execute specific operations to achieve predetermined goals without human intervention. Accurate position, velocity, and direction are crucial for autonomous navigation. With the rapid advancement of inertial sensor performance and the development of global navigation satellite systems (GNSS), navigation and positioning technology is gradually evolving toward autonomous positioning, navigation, and timing (PNT) systems that utilize a flexible multi-sensor combination. Global navigation satellite systems (GNSS), including the US GPS, Europe's Galileo, and China's BeiDou, provide powerful support for global positioning and navigation. However, in complex environments such as urban canyons, mountainous areas, and inclement weather, GNSS signals are easily blocked or interfered with, resulting in a reduction in the number of available satellites and a significant decrease in positioning accuracy, making it difficult to meet the high-precision navigation requirements of autonomous unmanned systems.

[0003] To overcome the limitations of single GNSS systems in complex environments, integrated navigation technology, combining multi-mode GNSS with an inertial navigation system (INS), has emerged. By fusing observations from multiple GNSS systems with the continuous and stable dead reckoning provided by the INS, this approach can provide more reliable and accurate navigation and positioning results even in situations where GNSS signals are limited. However, current integrated navigation technology still faces numerous challenges. Differences in GNSS system design, frequency bands, satellite distribution, and observation environments, as well as potential errors in observations such as signal reflections and multipath effects, can severely impact navigation and positioning accuracy.

[0004] Although the existing GNSS / INS integrated navigation method based on factor graph optimization can utilize observation information at multiple times, when a GNSS system is interfered with or attacked, its observation information will be erroneous, which will seriously affect the performance of the entire integrated navigation system. Therefore, it is necessary to study a navigation method that can suppress the influence of erroneous information on positioning results, thereby effectively suppressing the influence of observation information containing gross errors on weight estimation and state estimation, improving the accuracy and robustness of navigation positioning, and providing high-precision and high-reliability navigation and positioning services for autonomous unmanned systems in complex environments. Summary of the Invention

[0005] The object of the present invention is to provide a GNSS / inertial navigation tightly integrated navigation method and system which can effectively suppress the influence of observation information containing gross errors on weight estimation and state estimation, and has high positioning accuracy, strong robustness and high reliability.

[0006] The technical solution to achieve the purpose of the present invention is: a GNSS / inertial tight integration navigation method based on robust self-velocity constraint factor graph optimization, comprising the following steps:

[0007] Step 1: Extract the pseudorange and Doppler observation data of each satellite from the raw observation information provided by the GNSS satellites, and obtain the accelerometer and gyroscope data from the INS;

[0008] Step 2: Based on the pseudorange and Doppler observation equations of satellite navigation, construct pseudorange residual blocks and Doppler residual blocks, and associate the satellite observation data with the position, velocity, attitude and sensor error information to be estimated;

[0009] Step 3: Using the state estimation result at the previous moment as a benchmark, integrate the inertial navigation data between two adjacent observation moments, calculate the increments of position, velocity, attitude and sensor error, and construct a pre-integration residual block based on them;

[0010] Step 4: Construct velocity constraint factors, including self-velocity recursive constraint and Doppler instantaneous velocity constraint;

[0011] Step 5: Integrate the pseudorange residual block, Doppler residual block, inertial navigation pre-integration residual block and velocity constraint factor into a sliding window estimator based on factor graph optimization;

[0012] Step 6: According to the set sliding window size, all residual blocks in the time window are summed to form the optimization objective function, and the Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window;

[0013] Step 7: When the new moment observation residual block and the state to be estimated are added to the window, if the window size reaches the set value, the Schur elimination method is used to marginalize the premature observation residual block and the state to be estimated;

[0014] Step 8: Perform the first round of factor graph optimization to obtain preliminary state estimation results;

[0015] Step 9: Use the robust algorithm to test the preliminary state estimation results. If there are observations with gross errors, assign lower weights to the observations with gross errors to reduce the impact of gross errors on the overall estimation.

[0016] Step 10: Combine the weights after robustness processing, update the prior weight matrix, and perform a second round of optimization until the optimization results converge to obtain the joint optimal estimation results of multiple time states in the current time window;

[0017] Step 11: Determine whether there are any states to be estimated. If yes, return to step 1 for the next state estimation; otherwise, end the state estimation.

[0018] A GNSS / INS tightly integrated navigation system based on robust self-velocity constraint factor graph optimization is disclosed. The system is used to implement the GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization. The system includes first to eleventh units, each of which has the following functions:

[0019] Unit 1: Extract pseudorange and Doppler observation data of each satellite from the raw observation information provided by GNSS satellites, and obtain accelerometer and gyroscope data from INS;

[0020] The second unit builds pseudorange residual blocks and Doppler residual blocks based on the pseudorange and Doppler observation equations of satellite navigation, and associates satellite observation data with the position, velocity, attitude and sensor error information to be estimated;

[0021] The third unit integrates the inertial navigation data between two adjacent observation moments, using the state estimation result of the previous moment as a benchmark, calculates the increments of position, velocity, attitude and sensor error, and constructs a pre-integration residual block based on this;

[0022] Unit 4, constructing velocity constraint factors, including self-velocity recursive constraints and Doppler instantaneous velocity constraints;

[0023] The fifth unit integrates the pseudorange residual block, Doppler residual block, inertial navigation pre-integration residual block and velocity constraint factor into a sliding window estimator based on factor graph optimization;

[0024] Unit 6, based on the set sliding window size, sums all residual blocks within the time window to form the optimization objective function, and uses the Gauss-Newton method to solve the joint optimal estimation result of multiple states within the time window;

[0025] In Unit 7, when a new moment observation residual block and state to be estimated are added to the window, if the window size reaches the set value, the Schur elimination method is used to marginalize the premature observation residual block and state to be estimated;

[0026] Unit 9: Use the robustness algorithm to test the preliminary state estimation results. If there are observations with gross errors, they are given a lower weight to reduce the impact of gross errors on the overall estimation.

[0027] Unit 10: Combine the weights after robustness processing, update the prior weight matrix, and perform a second round of optimization until the optimization results converge, obtaining the joint optimal estimation results of the states at multiple moments in the current time window;

[0028] Unit 11: Determine whether there are any states to be estimated. If yes, return to step 1 for the next state estimation; otherwise, end the state estimation.

[0029] A mobile terminal comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the program, the GNSS / inertial navigation tightly integrated navigation method based on robust self-velocity constraint factor graph optimization is implemented.

[0030] Compared with the existing technology, the present invention has the following significant advantages: (1) by integrating satellite observation information of multiple GNSS, the number of visible satellites in complex environments is effectively increased, the geometric distribution of satellites is optimized, and the observation matrix is expanded. In combination with the information of the inertial navigation system INS that can work autonomously and stably, the state estimation accuracy and robustness of the unmanned system are improved; (2) the use of a self-velocity constraint method including INS self-velocity recursive constraint, Doppler instantaneous velocity constraint and velocity constraint factor enhances the system's adaptability to dynamic environmental changes and improves the accuracy of velocity estimation. In particular, when the GNSS signal is weak or unavailable, the self-velocity constraint can provide stable and reliable velocity information, thereby enhancing the positioning continuity and reliability of the system; (3) based on the sliding window factor graph The optimization technology is used to build a tight combination system of GNSS and inertial navigation, integrating the pseudorange observations, Doppler observations and pre-integrated components of GNSS. At the same time, the estimated states and observation information at multiple moments in the time window are considered. It can achieve accurate multi-epoch joint optimal state estimation at low cost and low computational consumption, thereby improving the overall performance of the system in complex environments. (4) In order to solve the problem of large gross errors in the original observation information provided by satellites of multiple GNSS systems in complex environments, a GNSS original observation anti-error model is introduced. Before determining the posterior weights, the observation information containing gross errors is down-weighted, thereby improving the accuracy and reliability of the variance component estimation, thereby improving the performance of estimating the multi-mode GNSS posterior weights using the variance component estimation method. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 The present invention is a flow chart of a GNSS / inertial navigation tightly integrated navigation method based on the optimization of a robust self-velocity constraint factor graph.

[0032] Figure 2 The absolute error curve diagram is a graph showing the absolute error between the solution result of the vehicle-mounted integrated navigation using the method of the present invention and the reference true value in an embodiment of the present invention. DETAILED DESCRIPTION

[0033] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0034] In the integrated navigation of a multi-mode global satellite navigation system (GNSS) and an inertial navigation system (INS), traditional integrated navigation methods are unable to effectively cope with interference from complex environments on signal transmission. To improve the accuracy and robustness of navigation positioning, this paper proposes a GNSS / INS tightly integrated navigation method based on a robust self-velocity constraint factor graph optimization. This method aims to optimize the multi-mode GNSS / INS tightly integrated navigation system based on the factor graph through a multi-level velocity constraint mechanism and a robust adaptive weight adjustment strategy.

[0035] When combining the raw observation information of multiple different global satellite navigation systems with the dead reckoning information of inertial navigation, it is necessary to weight the information provided by different GNSS according to the quality of real-time satellite observations, and adaptively adjust the trust weight of the GNSS system under different observation conditions. The present invention further optimizes the accuracy and stability of speed estimation through a multi-level speed constraint mechanism, including the self-velocity recursive constraint of the inertial navigation system (INS), the instantaneous speed constraint of the GNSS Doppler observation, and the speed constraint factor. The INS self-velocity recursive constraint utilizes the continuity of inertial navigation to constrain the speed estimation in a recursive manner to reduce error accumulation; the Doppler instantaneous speed constraint utilizes the GNSS Doppler observation to provide instantaneous speed information to further correct the speed estimate; the speed constraint factor fuses the speed information of INS and GNSS to form a multi-level speed constraint system.

[0036] During the factor graph optimization process, the present invention adopts an anti-error algorithm to detect gross errors in GNSS pseudorange and Doppler observations, and reduces the weights of observations containing gross errors. The present invention adopts a sliding window method to limit the number of states to be estimated for multiple epochs to improve computational efficiency. Before each factor graph optimization, the observation information is first subjected to gross error detection and weight reduction processing, and then an optimization target residual function is constructed. The weights of the multi-system observation information of each epoch in the window are cyclically updated with the iteration of the state to be estimated for each epoch until the premature epoch is marginalized and removed from the sliding window. This process not only optimizes the state estimation results, but also dynamically adjusts the weights between different GNSS systems, further improving the robustness of the system. Through the above-mentioned innovative design, the present invention not only further improves the accuracy of the tight combination of multi-mode GNSS and inertial navigation, but also fully utilizes the advantages of factor graph optimization in considering the states to be estimated and observation information of multiple epochs, significantly enhancing the performance of the combined navigation system under GNSS interference or deception.

[0037] like Figure 1 As shown, the present invention provides a GNSS / INS tight integrated navigation method based on robust self-velocity constraint factor graph optimization, comprising the following steps:

[0038] Step 1: Extract the pseudorange and Doppler observation data of each satellite from the raw observation information provided by the GNSS satellites, and obtain the accelerometer and gyroscope data from the INS;

[0039] Step 2: Based on the pseudorange and Doppler observation equations of satellite navigation, construct pseudorange residual blocks and Doppler residual blocks, and associate the satellite observation data with the position, velocity, attitude and sensor error information to be estimated, as follows:

[0040] Step 2.1, define the state vector to be estimated as:

[0041] X=[x0,x1,...,x N ]...M∈Z (1)

[0042] Where X is all the states to be estimated in the sliding window with a fixed length of N, k is the epoch corresponding to the GNSS observation, Z represents an integer set, and x k is the estimated state at epoch k, defined as:

[0043]

[0044] in, and Define the position, attitude and velocity of the kth epoch in the nth frame, δt r，k is the receiver clock-related state at epoch k, defined as:

[0045]

[0046] Among them, δt G，k and are the receiver clock error and clock drift, δt B，k represents the inter-system bias of BDS relative to the GPS clock; the initial state and initial state variance are both set before the factor graph optimization begins;

[0047] Step 2.2: Construct the relationship between the pseudorange, Doppler raw observation and the current state to be estimated:

[0048]

[0049] in, and are the pseudorange observation and Doppler raw observation of the nth satellite of the k-epoch single-satellite system S=G,B, is the Euclidean distance between the receiver and the satellite; is the satellite clock error, calculated from the broadcast ephemeris; and represent the ionospheric delay error and the tropospheric delay error respectively; is the error caused by other unmodeled factors, including multipath error, hardware noise and observation noise; λ S is the corresponding GNSS signal wavelength, is the Doppler observation quantity of epoch k, where is the receiver clock drift, For satellite clock drift, is the error caused by the unmodeled part, is the satellite speed;

[0050] Step 2.3: Construct pseudorange residual block and Doppler residual block:

[0051]

[0052]

[0053] in, and represent the pseudorange observation residuals and Doppler observation residuals, respectively.

[0054] Step 3: Using the state estimation result of the previous moment as a benchmark, integrate the inertial navigation data between two adjacent observation moments, calculate the increments of position, velocity, attitude and sensor error, and construct a pre-integration residual block based on this, as follows:

[0055] Taking the state at the previous moment as a reference, the inertial navigation data between the two observation moments are integrated to obtain the inertial navigation pre-integration position, velocity, attitude and sensor error increment, and construct the inertial navigation pre-integration residual block:

[0056]

[0057] 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 the correction term for gravity and Coriolis force.

[0058] Step 4: Construct velocity constraint factors, including self-velocity recursive constraint and Doppler instantaneous velocity constraint, as follows:

[0059] According to the constraints of the velocity estimation results on the positions of adjacent epochs, the constraints between adjacent GNSS epochs are constructed, and the velocity constraint factor is obtained as follows:

[0060]

[0061] Where Δt is the difference between two consecutive GNSS observation epochs;

[0062] The information matrix of Doppler observations and velocity constraints is defined as:

[0063]

[0064] Where, is the standard deviation of the Doppler observation noise, is the Doppler observation weight of the satellite, is the noise standard deviation of the velocity constraint factor. Since the velocity constraint factor is independent of the original GNSS observation and depends on the velocity estimation result, its weight is not estimated; I is the three-dimensional unit matrix.

[0065] Step 5: Integrate the pseudorange residual block, Doppler residual block, inertial navigation pre-integration residual block and velocity constraint factor into a sliding window estimator based on factor graph optimization;

[0066] Step 6: According to the set sliding window size, all residual blocks in the time window are summed to form the optimization objective function, and the Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window, as follows:

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

[0068]

[0069] Step 7: When the new moment observation residual block and the state to be estimated are added to the window, if the window size reaches the set value, the Schur elimination method is used to marginalize the premature observation residual block and the state to be estimated;

[0070] Step 8: Perform the first round of factor graph optimization to obtain the preliminary state estimation results, as follows:

[0071] The first round of optimization is performed according to formula (12) to obtain the state estimation result after the first round of optimization.

[0072] Step 9: Use the robust algorithm to test the preliminary state estimation results. If there are observations with gross errors, assign lower weights to the observations with gross errors to reduce the impact of gross errors on the overall estimation. The details are as follows:

[0073] Using the posterior residuals calculated by equations (6) and (7), the IGG-III method is used to perform gross error detection and weight reduction. The impact of gross errors on the multi-system weight estimation is nonlinearly reduced in three stages: no gross errors, suspicious gross errors, and serious gross errors. The IGG-III robust estimation weight function is:

[0074]

[0075] Among them, vi is 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.6745k1 and k2 are constants, generally ranging from 1.5 to 2.0 and 3.0 to 8.5 respectively. 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.

[0076] Step 10: Combine the weights after robustness processing, update the prior weight matrix, and perform a second round of optimization until the optimization results converge. The joint optimal estimation results of the states at multiple moments in the current time window are obtained, as follows:

[0077] After determining the gross error weight, the second round of optimization is performed according to formula (12) until the optimization structure converges and the joint optimal estimation result of multiple time states in the current time window is obtained.

[0078] Step 11: Determine whether there are any states to be estimated. If yes, return to step 1 for the next state estimation; otherwise, end the state estimation.

[0079] The present invention also provides a GNSS / INS tightly integrated navigation system based on robust self-velocity constraint factor graph optimization, which is used to implement the GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization. The system includes first to eleventh units, and the functions of each unit are as follows:

[0080] Unit 1: Extract pseudorange and Doppler observation data of each satellite from the raw observation information provided by GNSS satellites, and obtain accelerometer and gyroscope data from INS;

[0081] The second unit builds pseudorange residual blocks and Doppler residual blocks based on the pseudorange and Doppler observation equations of satellite navigation, and associates satellite observation data with the position, velocity, attitude and sensor error information to be estimated;

[0082] The third unit integrates the inertial navigation data between two adjacent observation moments, using the state estimation result of the previous moment as a benchmark, calculates the increments of position, velocity, attitude and sensor error, and constructs a pre-integration residual block based on this;

[0083] Unit 4, constructing velocity constraint factors, including self-velocity recursive constraints and Doppler instantaneous velocity constraints;

[0084] The fifth unit integrates the pseudorange residual block, Doppler residual block, inertial navigation pre-integration residual block and velocity constraint factor into a sliding window estimator based on factor graph optimization;

[0085] Unit 6, based on the set sliding window size, sums all residual blocks within the time window to form the optimization objective function, and uses the Gauss-Newton method to solve the joint optimal estimation result of multiple states within the time window;

[0086] In Unit 7, when a new moment observation residual block and state to be estimated are added to the window, if the window size reaches the set value, the Schur elimination method is used to marginalize the premature observation residual block and state to be estimated;

[0087] Unit 9: Use the robustness algorithm to test the preliminary state estimation results. If there are observations with gross errors, they are given a lower weight to reduce the impact of gross errors on the overall estimation.

[0088] Unit 10: Combine the weights after robustness processing, update the prior weight matrix, and perform a second round of optimization until the optimization results converge, obtaining the joint optimal estimation results of the states at multiple moments in the current time window;

[0089] Unit 11: Determine whether there are any states to be estimated. If yes, return to step 1 for the next state estimation; otherwise, end the state estimation.

[0090] The present invention also provides a mobile terminal, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the program, the GNSS / inertial navigation tightly integrated navigation method based on the optimization of the robust self-velocity constraint factor graph is implemented.

[0091] Example

[0092] This embodiment uses an unmanned vehicle equipped with a receiver and inertial navigation module capable of receiving two different constellation signals, Beidou and GPS, as an example. The vehicle needs to patrol along a specified trajectory in a complex environment. In complex environments, satellite signals are significantly obscured, and the distribution and quality of visible satellites are weakened by environmental influences such as obscuration and reflection. The number of available satellites is significantly reduced, and relying solely on a single system to observe satellites is seriously insufficient. It is necessary to introduce a Beidou / GPS dual system to increase the number of available observations, thereby increasing the effective information content of the integrated navigation system state estimation. In order to address the potential impact of some observations containing gross errors on weight estimation, it is necessary to supplement the impact of observations containing gross errors with a gross error detection and weight reduction method. 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 severe gross errors, and then perform segmented weight reduction.

[0093] The actual data set was collected by using a low-cost multi-GNSS satellite navigation receiver and a low-cost inertial navigation system on a vehicle, and the vehicle-mounted experimental verification was carried out. The post-processing results of the combined navigation of the GNSS real-time precise positioning and the tactical-level inertial navigation system were used as the reference truth value. Figure 1 The algorithm is tested according to the process shown in the figure. The obtained vehicle integrated navigation solution result is compared with the reference true value to obtain the absolute error curve, as shown in Figure 2 As shown, it can be seen that the velocity error remains at a low level throughout the experiment, which directly reflects the significant effect of the velocity constraint factor and improves the accuracy of position, velocity and attitude estimation.

[0094] The above are only preferred embodiments of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization, characterized in that: The following steps are involved: Step 1: Extract the pseudorange and Doppler observation data of each satellite from the raw observation information provided by the GNSS satellites, and obtain the accelerometer and gyroscope data from the INS; Step 2: Based on the pseudorange and Doppler observation equations of satellite navigation, construct pseudorange residual blocks and Doppler residual blocks, and associate the satellite observation data with the position, velocity, attitude and sensor error information to be estimated; Step 3: Using the state estimation result at the previous moment as a benchmark, integrate the inertial navigation data between two adjacent observation moments, calculate the increments of position, velocity, attitude and sensor error, and construct a pre-integration residual block based on them; Step 4: Construct velocity constraint factors, including self-velocity recursive constraint and Doppler instantaneous velocity constraint; Step 5: Integrate the pseudorange residual block, Doppler residual block, inertial navigation pre-integration residual block and velocity constraint factor into a sliding window estimator based on factor graph optimization; Step 6: According to the set sliding window size, all residual blocks in the time window are summed to form the optimization objective function, and the Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window; Step 7: When the new moment observation residual block and the state to be estimated are added to the window, if the window size reaches the set value, the Schur elimination method is used to marginalize the premature observation residual block and the state to be estimated; Step 8: Perform the first round of factor graph optimization to obtain preliminary state estimation results; Step 9: Use the robust algorithm to test the preliminary state estimation results. If there are observations with gross errors, assign lower weights to the observations with gross errors to reduce the impact of gross errors on the overall estimation. Step 10: Combine the weights after robustness processing, update the prior weight matrix, and perform a second round of optimization until the optimization results converge to obtain the joint optimal estimation results of multiple time states in the current time window; Step 11: Determine whether there are any states to be estimated. If yes, return to step 1 for the next state estimation; otherwise, end the state estimation.

2. The GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to claim 1, characterized in that: According to the pseudorange and Doppler observation equations of satellite navigation described in step 2, a pseudorange residual block and a Doppler residual block are constructed to associate the satellite observation data with the position, velocity, attitude and sensor error information to be estimated, as follows: Step 2.1, define the state vector to be estimated as: X=[x0,x1,...,x N ]…M∈Z (1) Where X is all the states to be estimated in the sliding window with a fixed length of N, k is the epoch corresponding to the GNSS observation, Z represents an integer set, and x k is the estimated state at epoch k, defined as: in, and Define the position, attitude and velocity of the kth epoch in the nth frame, δt r，k is the receiver clock-related state at epoch k, defined as: Among them, δt G，k and are the receiver clock error and clock drift, δt B，k represents the inter-system bias of BDS relative to the GPS clock; the initial state and initial state variance are both set before the factor graph optimization begins; Step 2.2: Construct the relationship between the pseudorange, Doppler raw observation and the current state to be estimated: in, and are the pseudorange observation and Doppler raw observation of the nth satellite of the k-epoch single-satellite system S=G,B, is the Euclidean distance between the receiver and the satellite; is the satellite clock error, calculated from the broadcast ephemeris; and represent the ionospheric delay error and the tropospheric delay error respectively; is the error caused by other unmodeled factors, including multipath error, hardware noise and observation noise; λ S is the corresponding GNSS signal wavelength, is the Doppler observation quantity of epoch k, where is the receiver clock drift, For satellite clock drift, is the error caused by the unmodeled part, is the satellite speed; Step 2.3: Construct pseudorange residual block and Doppler residual block: in, and represent the pseudorange observation residuals and Doppler observation residuals, respectively.

3. The GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to claim 2, characterized in that: In step 3, the state estimation result of the previous moment is used as a benchmark to integrate the inertial navigation data between two adjacent observation moments, calculate the increments of position, velocity, attitude and sensor error, and construct a pre-integration residual block based on this, as follows: Taking the state at the previous moment as a reference, the inertial navigation data between the two observation moments are integrated to obtain the inertial navigation pre-integration position, velocity, attitude and sensor error increment, and construct the inertial navigation pre-integration residual block: 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 the correction term for gravity and Coriolis force.

4. The GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to claim 3, characterized in that: The velocity constraint factor constructed in step 4 includes the self-velocity recursive constraint and the Doppler instantaneous velocity constraint, as follows: According to the constraints of the velocity estimation results on the positions of adjacent epochs, the constraints between adjacent GNSS epochs are constructed, and the velocity constraint factor is obtained as follows: Where Δt is the difference between two consecutive GNSS observation epochs; The information matrix of Doppler observations and velocity constraints is defined as: Where, is the standard deviation of the Doppler observation noise, is the Doppler observation weight of the satellite, is the noise standard deviation of the velocity constraint factor. Since the velocity constraint factor is independent of the original GNSS observation and depends on the velocity estimation result, its weight is not estimated; I is the three-dimensional unit matrix.

5. The GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to claim 4, characterized in that: According to the set sliding window size described in step 6, all residual blocks in the time window are summed to form the optimization objective function, and the Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window, as follows: Using a sliding window estimator based on factor graph optimization, all residual blocks in the time window are summed according to the set sliding window size to form the optimization objective function, and the Gauss-Newton method is used to solve the joint optimal estimation result of multiple states in the time window:

6. The GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to claim 5, characterized in that: The first round of factor graph optimization described in step 8 is performed to obtain preliminary state estimation results, which are as follows: The first round of optimization is performed according to formula (12) to obtain the state estimation result after the first round of optimization.

7. The GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to claim 6, characterized in that: The robust algorithm described in step 9 is used to test the preliminary state estimation results. If there are observations containing gross errors, they are given lower weights to reduce the impact of gross errors on the overall estimation, as follows: Using the posterior residuals calculated by equations (6) and (7), the IGG-III method is used to perform gross error detection and weight reduction. The impact of gross errors on the multi-system weight estimation is nonlinearly reduced in three stages: no gross errors, suspicious gross errors, and serious gross errors. The IGG-III robust estimation weight function is: Among them, v i is 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, k1 and k2 are constants, generally ranging from 1.5 to 2.0 and 3.0 to 8.5 respectively. 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.

8. The GNSS / INS tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to claim 7, characterized in that: The weights after the robustness treatment described in step 10 are combined to update the prior weight matrix and perform a second round of optimization until the optimization results converge to obtain the joint optimal estimation results of the states at multiple moments in the current time window, as follows: After determining the gross error weight, the second round of optimization is performed according to formula (12) until the optimization structure converges and the joint optimal estimation result of multiple time states in the current time window is obtained.

9. A GNSS / INS tightly integrated navigation system based on robust self-velocity constraint factor graph optimization, characterized in that: The system is used to implement the GNSS / INS tight integrated navigation method based on robust self-velocity constraint factor graph optimization as described in any one of claims 1 to 8. The system includes the first to eleventh units, and the functions of each unit are as follows: Unit 1: Extract pseudorange and Doppler observation data of each satellite from the raw observation information provided by GNSS satellites, and obtain accelerometer and gyroscope data from INS; The second unit builds pseudorange residual blocks and Doppler residual blocks based on the pseudorange and Doppler observation equations of satellite navigation, and associates satellite observation data with the position, velocity, attitude and sensor error information to be estimated; The third unit integrates the inertial navigation data between two adjacent observation moments, using the state estimation result of the previous moment as a benchmark, calculates the increments of position, velocity, attitude and sensor error, and constructs a pre-integration residual block based on this; Unit 4, constructing velocity constraint factors, including self-velocity recursive constraints and Doppler instantaneous velocity constraints; Unit 5 integrates the pseudorange residual block, Doppler residual block, inertial navigation pre-integration residual block and velocity constraint factor into a sliding window estimator based on factor graph optimization; Unit 6, based on the set sliding window size, sums all residual blocks within the time window to form the optimization objective function, and uses the Gauss-Newton method to solve the joint optimal estimation result of multiple states within the time window; In Unit 7, when a new moment observation residual block and state to be estimated are added to the window, if the window size reaches the set value, the Schur elimination method is used to marginalize the premature observation residual block and state to be estimated; Unit 9: Use the robustness algorithm to test the preliminary state estimation results. If there are observations with gross errors, they are given a lower weight to reduce the impact of gross errors on the overall estimation. Unit 10: Combine the weights after robustness processing, update the prior weight matrix, and perform a second round of optimization until the optimization results converge, obtaining the joint optimal estimation results of the states at multiple moments in the current time window; Unit 11: Determine whether there are any states to be estimated. If yes, return to step 1 for the next state estimation; otherwise, end the state estimation.

10. A mobile terminal comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the program, the GNSS / inertial navigation tightly integrated navigation method based on robust self-velocity constraint factor graph optimization according to any one of claims 1 to 8 is implemented.

Images