A GNSS-INS factor graph optimization method with forward tight combination

Abstract

The application relates to a GNSS-INS factor graph optimization method using forward tight combination, which comprises the following steps: 1) collecting navigation data, and performing time and space synchronization alignment on the data; 2) establishing state equations and observation equations; 3) obtaining modified satellite data through tight combination filtering and weight correction; 4) acquiring incremental information, performing pre-integration operation and error analysis, constructing an IMU residual error equation and a cost function; 5) performing error analysis on the modified data, acquiring a GNSS residual error equation and a cost function, constructing marginalization information factors according to a Jacobian matrix obtained in the pre-integration process, and obtaining corresponding residual error equations and cost functions; and 6) optimizing the GNSS-INS factor graph according to the obtained cost functions, and obtaining optimal navigation solutions. The method can fully utilize historical navigation information to optimize navigation results, can repair satellite breakpoint data generated in a denial environment under the assistance of tight combination, can enhance the robustness of a combined navigation system, and can improve positioning precision.

Description

Technical Field

[0001] This invention relates to the field of integrated navigation data fusion technology, specifically, to a GNSS-INS factor graph optimization method employing forward compact combination. Background Technology

[0002] Data fusion in GNSS-INS integrated navigation primarily relies on traditional Kalman filtering, evolving into a series of combined algorithms based on extended Kalman filtering, adaptive Kalman filtering, etc. These strategies are based on the Markov assumption, assuming that the current state is only related to the previous state. The optimality of the fusion result depends entirely on the quality of the prior assumption about the covariance, which often fails in complex environments. Factor graph optimization in satellite / inertial data fusion improves upon these issues by fully utilizing historical sensor information, thus enhancing the accuracy of integrated navigation. However, in denied environments, graph optimization methods still exhibit significant errors. Summary of the Invention

[0003] The purpose of this invention is to address the shortcomings of existing algorithms by providing a GNSS-INS factor graph optimization method using forward compact combination. Unlike existing algorithms, this invention retains the advantages of factor graph optimization, fully utilizing historical navigation information to optimize navigation results, and can also repair satellite breakpoint data generated in denied environments with the assistance of compact combination. Furthermore, by incorporating the robust weighted factor function model IGG-III to process satellite data, this model demonstrates superior performance in complex urban environments compared to schemes such as Huber and Tukey, enhancing the robustness of the integrated navigation system and improving the positioning accuracy of integrated navigation.

[0004] To achieve the above objectives, the present invention provides the following technical solution:

[0005] A GNSS-INS factor graph optimization method employing forward compact combination includes the following steps:

[0006] Step S1: The inertial measurement unit (IMU) and the global positioning system (GNSS) collect navigation data at a certain frequency for a period of time, and synchronize the data in time and space.

[0007] Step S2: Model the satellite and inertial navigation data, and establish the state equations and observation equations required for the compact combination filtering process;

[0008] Step S3: Perform tight combination filtering based on the state equation and observation equation established in step S2. At the same time, use the IGG-III model to correct the weights of the covariance matrix of the observation equation, and then obtain the corrected satellite data after filtering.

[0009] Step S4: Obtain the incremental information of the gyroscope and accelerometer obtained by the IMU, perform pre-integration operation on it, consider the deviation caused by the Earth's rotation during the pre-integration process, perform error analysis on the pre-integration results, obtain the corresponding error equation, and then construct the IMU residual equation and cost function.

[0010] Step S5: Perform error analysis on the GNSS data corrected in step S3, obtain the GNSS residual equation and cost function, construct the marginalization information factor based on the Jacobian matrix obtained from the IMU pre-integration process, and obtain the residual equation and cost function corresponding to the marginalization information.

[0011] Step S6: Based on the cost function obtained in steps S4 and S5, optimize the GNSS-INS factor map to obtain the optimal navigation solution.

[0012] Furthermore, in step S1, the IMU provides gyroscope and accelerometer information, and the GNSS provides pseudorange and pseudorange rate information. The specific steps for synchronizing and aligning the data in time and space are as follows:

[0013] Step S11: Import IMU raw data and GNSS raw data. The IMU data includes sampling information from the gyroscope and accelerometer in the three directions of northeast, south, and east. The GNSS raw data includes base station information and corresponding navigation message information.

[0014] Step S12: For the GNSS raw data file, the open-source software Rtklib is used to perform preliminary calculations to obtain the corresponding carrier position and velocity information in the satellite data. The obtained data is then transformed into a coordinate system to achieve spatial synchronization between the satellite data and the inertial navigation data. Combined with the time of the data output, the data is then synchronized in time.

[0015] Furthermore, the state equations and observation equations required for establishing the compact combination filtering process described in step S2 are established as follows:

[0016] Step S21: Define each coordinate system for navigation. The state information required for the compact combination filtering process includes two parts: INS and GNSS. For the INS part, the state equation is established using inertial navigation error, which includes five state quantities: carrier attitude error, velocity error, position error, gyroscope zero bias error, and accelerometer zero bias error. For the GNSS part, since double-difference observations are used for calculation, it is not necessary to estimate the receiver clock error and drift parameters; only the double-difference ambiguity needs to be evaluated.

[0017] Step S22: Establish the state equations required for the compact combination filtering process based on the strapdown inertial navigation algorithm and the principle of integrated navigation;

[0018] Step S23: Subtract the satellite double-difference carrier phase observations and pseudorange observations obtained by differential technology from the double-difference distance calculated by INS to obtain the observation equations required for the compact combination filtering process.

[0019] Furthermore, the compact combination filtering and weight correction process described in step S3 is as follows:

[0020] Step S31: Let the state equation and observation equation established in step S2 be as follows:

[0021]

[0022] Z k =HX k +V k (2)

[0023] In the above formula, A represents the state transition matrix, and B represents the control matrix. and X k Let W represent the state estimate and the state, respectively. k Z represents process noise. k H represents the observed values, and V represents the observation matrix. k Indicates observation noise;

[0024] Step S32: The compactly combined data fusion process uses traditional Kalman filtering for processing.

[0025]

[0026]

[0027]

[0028]

[0029]

[0030] In the above formula, Q and R represent the process error covariance matrix and the observation error covariance matrix, respectively, and P represents the variance covariance matrix;

[0031] Step S32: To eliminate breakpoints and outliers in the GNSS data, the IGG-III strategy is used, and the concept of weights is introduced to constrain the measurement covariance matrix during the filtering process.

[0032]

[0033] In the above formula, γ is the weighting coefficient based on the IGG-III scheme.

[0034] Furthermore, the pre-integration operation and the processing of edge information in step S4 are as follows:

[0035] Step S41: Take any sampling interval, and this interval contains several IMU measurements. The differential equation of the carrier velocity can be obtained through the force equation. Then, the velocity pre-integration result of the IMU relative to the navigation system can be calculated by combining the equations. Considering the velocity compensation term caused by Coriolis acceleration and gravitational acceleration, the velocity differential equation is integrated once to obtain the position pre-integration result with compensation term.

[0036] Step S42: Select the same time interval as in step S41, and use this as the starting point to recursively solve the attitude pre-integration results. The IMU error model is the same as in step S22. In this way, the overall pre-integration error differential equation is calculated, and the corresponding error Jacobian matrix and its variance covariance matrix are obtained.

[0037] Furthermore, the construction process of the GNSS residual equation and the marginalization information residual equation in step S5 is as follows:

[0038] Step S51: The GNSS residual equation takes into account the coordinate system transformation process and the lever arm error between itself and the IMU;

[0039] Step S52: Using the variance-covariance matrix and error Jacobian matrix obtained in step S42, the marginal information residual equation is established by combining the Scher elimination method.

[0040] Furthermore, the optimization and comparative analysis process of the GNSS-INS factor map in step S6 is as follows:

[0041] Step S61: Combine the cost functions obtained in steps S4 and S5 to construct a least-squares problem, and use the LM iterative method to solve the nonlinear optimization problem.

[0042] Compared with the prior art, the beneficial effects of the present invention are:

[0043] The method of this invention retains the advantages of factor graph optimization, which can make full use of historical navigation information to optimize navigation results, and can repair satellite breakpoint data generated in denial environments with the assistance of tight combination. Furthermore, by adding the robust weighted factor function model IGG-III to process satellite data, this model has a greater performance advantage in complex urban environments than methods such as Huber and Tukey, which enhances the robustness of the integrated navigation system and improves the positioning accuracy of integrated navigation. Attached Figure Description

[0044] Figure 1 A flowchart of the GNSS-INS factor graph optimization method for forward compact combinations;

[0045] Figure 2 A comparison chart of the trajectories of each algorithm;

[0046] Figure 3 A comparison chart of the root mean square error of pose for each algorithm;

[0047] Figure 4 A comparison chart of the root mean square error of the speed of each algorithm;

[0048] Figure 5 This is a comparison chart of the root mean square error of each algorithm. Detailed Implementation

[0049] The technical solution of the present invention will now be described in detail with reference to the accompanying drawings.

[0050] This technology is intended for use by those skilled in the art to understand the technical terms contained herein. Unless otherwise defined, all terms (including technical and scientific terms) herein have meanings similar to those understood by a person skilled in the art. It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with those in the prior art, and unless specifically defined, they will not be interpreted in an idealized or overly formal sense.

[0051] A GNSS-INS factor graph optimization method employing forward compact combination includes the following steps:

[0052] Step S1: The inertial measurement unit (IMU) and the global positioning system (GNSS) collect navigation data at a certain frequency for a period of time, and synchronize the data in time and space.

[0053] Step S2: Model the satellite and inertial navigation data, and establish the state equations and observation equations required for the compact combination filtering process;

[0054] Step S3: Perform tight combination filtering based on the state equation and observation equation established in step S2. At the same time, use the IGG-III model to correct the weights of the covariance matrix of the observation equation, and then obtain the corrected satellite data after filtering.

[0055] Step S4: Obtain the incremental information of the gyroscope and accelerometer obtained by the IMU, perform pre-integration operation on it, consider the deviation caused by the Earth's rotation during the pre-integration process, perform error analysis on the pre-integration results, obtain the corresponding error equation, and then construct the IMU residual equation and cost function.

[0056] Step S5: Perform error analysis on the GNSS data corrected in step S3, obtain the GNSS residual equation and cost function, construct the marginalization information factor based on the Jacobian matrix obtained from the IMU pre-integration process, and obtain the residual equation and cost function corresponding to the marginalization information.

[0057] Step S6: Based on the cost function obtained in steps S4 and S5, optimize the GNSS-INS factor map to obtain the optimal navigation solution.

[0058] Furthermore, in step S1, the IMU provides gyroscope and accelerometer information, and the GNSS provides pseudorange and pseudorange rate information. The specific steps for synchronizing and aligning the data in time and space are as follows:

[0059] Step S11: Import IMU raw data and GNSS raw data. The IMU data includes sampling information from the gyroscope and accelerometer in the three directions of northeast, south, and east. The GNSS raw data includes base station information and corresponding navigation message information.

[0060] Step S12: For the GNSS raw data file, the open-source software Rtklib is used to perform preliminary calculations to obtain the corresponding carrier position and velocity information in the satellite data. The obtained data is then transformed into a coordinate system to achieve spatial synchronization between the satellite data and the inertial navigation data. Combined with the time of the data output, the data is then synchronized in time.

[0061] Furthermore, the state equations and observation equations required for establishing the compact combination filtering process described in step S2 are established as follows:

[0062] Step S21: Define each coordinate system for navigation. The state information required for the compact combination filtering process includes two parts: INS and GNSS. For the INS part, the state equation is established using inertial navigation error, which includes five state quantities: carrier attitude error, velocity error, position error, gyroscope zero bias error, and accelerometer zero bias error. For the GNSS part, since double-difference observations are used for calculation, it is not necessary to estimate the receiver clock error and drift parameters; only the double-difference ambiguity needs to be evaluated.

[0063] Step S22: Establish the state equations required for the compact combination filtering process based on the strapdown inertial navigation algorithm and the principle of integrated navigation;

[0064] Step S23: Subtract the satellite double-difference carrier phase observations and pseudorange observations obtained by differential technology from the double-difference distance calculated by INS to obtain the observation equations required for the compact combination filtering process.

[0065] Furthermore, the compact combination filtering and weight correction process described in step S3 is as follows:

[0066] Step S31: Let the state equation and observation equation established in step S2 be as follows:

[0067]

[0068] Z k=HX k +V k (2)

[0069] In the above formula, A represents the state transition matrix, and B represents the control matrix. and X k Let W represent the state estimate and the state, respectively. k Z represents process noise. k H represents the observed values, and V represents the observation matrix. k Indicates observation noise;

[0070] Step S32: The compactly combined data fusion process uses traditional Kalman filtering for processing.

[0071]

[0072]

[0073]

[0074]

[0075]

[0076] In the above formula, Q and R represent the process error covariance matrix and the observation error covariance matrix, respectively, and P represents the variance covariance matrix;

[0077] Step S32: To eliminate breakpoints and outliers in the GNSS data, the IGG-III strategy is used, and the concept of weights is introduced to constrain the measurement covariance matrix during the filtering process.

[0078]

[0079] In the above formula, γ is the weighting coefficient based on the IGG-III scheme.

[0080] Furthermore, the pre-integration operation and the processing of edge information in step S4 are as follows:

[0081] Step S41: Take any sampling interval, and this interval contains several IMU measurements. The differential equation of the carrier velocity can be obtained through the force equation. Then, the velocity pre-integration result of the IMU relative to the navigation system can be calculated by combining the equations. Considering the velocity compensation term caused by Coriolis acceleration and gravitational acceleration, the velocity differential equation is integrated once to obtain the position pre-integration result with compensation term.

[0082] Step S42: Select the same time interval as in step S41, and use this as the starting point to recursively solve the attitude pre-integration results. The IMU error model is the same as in step S22. In this way, the overall pre-integration error differential equation is calculated, and the corresponding error Jacobian matrix and its variance covariance matrix are obtained.

[0083] Furthermore, the construction process of the GNSS residual equation and the marginalization information residual equation in step S5 is as follows:

[0084] Step S51: The GNSS residual equation takes into account the coordinate system transformation process and the lever arm error between itself and the IMU;

[0085] Step S52: Using the variance-covariance matrix and error Jacobian matrix obtained in step S42, the marginal information residual equation is established by combining the Scher elimination method.

[0086] Furthermore, the optimization and comparative analysis process of the GNSS-INS factor map in step S6 is as follows:

[0087] Step S61: Combine the cost functions obtained in steps S4 and S5 to construct a least squares problem. Use the LM iterative method to solve the nonlinear optimization solution and obtain the optimal navigation solution.

[0088] Example 1

[0089] like Figure 1 As shown in the figure, this embodiment presents a forward compact combination GNSS-INS factor graph optimization method, which includes the following steps:

[0090] Step 1: Collect navigation data using IMU and GNSS. Since this embodiment mainly focuses on navigation calculation in denied and complex environments, human interruption can lead to ambiguity in the data. Therefore, an open-source dataset is selected for experimental verification (data source: https: / / github.com / IPNL-POLYU / UrbanNavDataset).

[0091] a) The IMU model is Tamagawa-seiki TAG264, with a sampling frequency of 50Hz; the GNSS receiver model is u-blox F9P, with a sampling frequency of 5Hz; the true value is mainly post-processed using an Applanix POS LV620 device, and the obtained position RMSE is approximately 5cm, with a sampling frequency of 10Hz.

[0092] b) The dataset provides satellite data including GNSS RINEX files and corresponding base station files. In this embodiment, the open-source software Rtklib is used for preliminary calculation (software source: https: / / www.rtklib.com / ).

[0093] c) The GNSS data after preliminary calculation is set with 273375 in GPS TOW as the starting frame and 273375.0093 in IMU data as the starting frame. This completes the synchronization initialization operation of time and space. Finally, the two are set with 274616 as the ending frame. In space, the GNSS data and IMU data are synchronously converted to the Northeast Sky navigation coordinate system for analysis.

[0094] Step 2: Based on the relevant files provided in the dataset, perform corresponding error modeling for IMU and GNSS. The IMU error equation is mainly modeled based on the attitude error, velocity error, position error, gyroscope zero bias and accelerometer zero bias during the strapdown calculation process. For the establishment of GNSS error, since this embodiment uses satellite double-difference observations for calculation, it is not necessary to estimate parameters such as receiver clock error and drift. Only the interference of double-difference ambiguity on the satellite system is considered.

[0095] a) In this embodiment, the state information consists of IMU error state and GNSS error state, totaling six items. The navigation coordinate system is defined as the n system, the geocentric coordinate system as the e system, and the carrier / IMU coordinate system as the b system. The discussion is carried out using the northeast-northeast coordinate system. The state quantities are shown in formula (9), and the noise quantities are shown in formula (10).

[0096]

[0097]

[0098] In the above formula, δq n ,δv n ,δp n These represent the attitude, velocity, and position errors in the navigation coordinate system, respectively. and ω b w represents the gyroscope zero bias and accelerometer zero bias in the IMU coordinate system, respectively. a and w g N represents the corresponding zero bias error. m This represents the double-difference ambiguity error parameter corresponding to m satellites;

[0099] In the state equation, the zero bias noise of the IMU gyroscope and accelerometer is modeled as a first-order Markov process, as shown in Equation (11);

[0100]

[0101] In the above formula, T a and T g Indicates the relevant time;

[0102] b) Based on the above system state variables and noise variables, the corresponding system state equations can be established, as shown in formula (12), where A represents the state transition matrix, and the specific expression is shown in formula (13), and B represents the system control matrix, and the specific expression is shown in formula (14).

[0103]

[0104]

[0105]

[0106] In the above formula, The coordinate transformation matrix represents the transformation from the navigation system to the carrier system. For detailed meanings and explanations of the parameters in the state transition matrix, please refer to the error equation section in the book "Stripdown Inertial Navigation Algorithm and Integrated Navigation Principle" by Yan Gongmin. This embodiment will not elaborate on this.

[0107] c) The GNSS error state quantity only needs to model the noise of the double difference ambiguity parameter. In this embodiment, it is modeled as a random walk process, as shown in formula (15), where m represents the number of satellites received by the carrier. The double difference ambiguity parameter can be written in the style of formula (16).

[0108]

[0109]

[0110] d) In this embodiment, the measurement equation in the compact combination filtering process is established by subtracting the satellite double-difference carrier phase observation value and pseudorange observation value obtained after differential technology from the double-difference geometric distance calculated by inertial navigation;

[0111] The distance between the carrier and the observation satellite calculated by inertial navigation is shown in formula (17); where This represents the actual distance between the carrier and the observed satellite i. These represent the cosine components of the observation satellite i relative to the carrier in three coordinate directions. When processing this vector, the lever arm error between GNSS and IMU is also included. The specific process is shown in formulas (18) to (19), where ζ represents the lever arm error parameter.

[0112] In the following formulas, the superscripts i and j represent the observed satellites, and their comma combinations indicate inter-satellite differences. The subscripts I, G, S, and R represent INS, GNSS, base stations, and rover stations, respectively, and their comma combinations indicate inter-station differences.

[0113]

[0114]

[0115]

[0116] According to formula (17), the double-difference geometric distance calculated by inertial navigation can be further obtained as shown in formula (20), where The value can be obtained from formula (21);

[0117]

[0118]

[0119] Errors related to ionospheric delay and tropospheric delay in satellite data can be eliminated using differential techniques. Equation (22) gives the equation for double-difference carrier phase observation under short baselines, where... It can be solved similarly to formula (21):

[0120]

[0121] In summary, the difference between formula (20) and formula (22) can be used to obtain the corresponding observation in the compact combination filter, as shown in formula (23);

[0122]

[0123] Therefore, the system observation equation is as shown in equation (24), where and It can be calculated by formula (25), and the system observation state transition matrix is ​​shown in formula (26);

[0124] Z k =HX k +V k (twenty four)

[0125]

[0126]

[0127] Step 3: As described above, the established observation equations and state equations are processed by Kalman filtering. The specific processing procedure is referred to formulas (3) to (7). Unlike traditional filtering methods, the GNSS information input by the system is not perfect and not all values ​​are completely within the error acceptance range. There are some outliers. Therefore, in this embodiment, a robust estimation method based on the IGG-III scheme is adopted for processing.

[0128] a) During the filtering process, the variance-covariance matrix, as a standard of accuracy, reliably reflects the accuracy of the observation results. If an observation is affected by outliers, its variance will be exaggerated, and its weight should be reduced. Therefore, another way to control the influence of peripheral related observations is to reduce the corresponding weight elements.

[0129] When updating the filtered measurement, the measurement covariance matrix can be updated by introducing weight coefficients. The specific process is shown in formula (27), and the evaluation criteria for the weight coefficients are shown in formulas (28) to (29).

[0130]

[0131]

[0132]

[0133] In the above formula, d represents the adaptive smoothing factor, and k0 and k1 are constants that control the magnitude of the weight coefficients of the robust model, which are 2 and 5 respectively in this invention. It is the standardized residual coefficient, with a range of -10 to 10m, and its specific expression for updating the weight matrix is ​​shown in formula (30);

[0134]

[0135] From this point on, the satellite data (time information, velocity information, and position information) obtained from the filtering result is taken as the optimized correction satellite data, and then optimized again with the IMU data to obtain the final navigation solution.

[0136] Step 4: After obtaining the repaired GNSS data, the backend optimization operation is performed using the data collected by the original IMU. In order to avoid the computational trouble caused by repeated integration of inertial navigation data during the optimization process, a pre-integration method is adopted in this embodiment.

[0137] a) The IMU measurement model in the back-end optimization process is shown in formula (31), where b k This represents the IMU / carrier coordinate system at frame k. f and f represent the actual measured value and the true value of the accelerometer, respectively. ω and ω represent the actual measured value and the true value of the gyroscope, respectively. The establishment of the IMU noise model and the filtering process are consistent during the optimization process, and the specific form is shown in formula (11).

[0138]

[0139] b) Integrate the IMU data between the k-th frame and the (k-1)-th frame. The corresponding IMU coordinate system is b. k and b k-1 The obtained data is the attitude, velocity, and position information between two frames, as shown in formula (32), where g represents the local gravitational acceleration. The meaning is the same as above, representing the coordinate system transformation matrix;

[0140]

[0141] c) Further, based on the pose obtained by integration, the pose pre-integration result between k and k-1 can be calculated, and the corresponding form is shown in formula (33), where α, β and γ represent the pose, velocity and position pre-integration results in frames k to k-1, respectively. Correspondingly, the pose, velocity and position in the time period h to h-1 can be recursively updated using formulas (34) to (35). If the pre-integration starts at k-1, but there is no IMU data sampling at time k, then the adjacent IMU data is used for linear interpolation calculation:

[0142]

[0143]

[0144]

[0145] d) By perturbing the error in equation (32), the error differential equations for attitude, velocity and position are obtained as shown in equation (36);

[0146]

[0147] Therefore, the overall differential equation of the pre-integration error is established, and its specific form is shown in formula (37). The transfer matrix and control matrix in the error differential equation are shown in formulas (38) and (40), and the error quantity and its noise discretization form are shown in formulas (41) and (43).

[0148]

[0149]

[0150]

[0151]

[0152]

[0153] e) To facilitate calculation, the error differential equation is discretized, and the discretization operation is shown in formula (42).

[0154]

[0155]

[0156] In the formula, Let k be the state transition matrix from k to k+1. This represents the driving response at time k+1 caused by the input white noise during the time interval k to k+1. Since W... t It is a random variable with zero mean and independent of time, and its variance-covariance matrix can be represented by formula (44), where

[0157]

[0158] Therefore, the IMU pre-integration error during the time interval h-1 to h variance covariance matrix It can be updated according to formula (45), and the iteration process of the Jacobian matrix is ​​shown in formula (46);

[0159]

[0160]

[0161] f) From this, the corresponding IMU pre-integration residual equation and the corresponding cost function can be derived, as shown in formulas (47) to (48), where These represent the predicted values ​​for attitude, velocity, and position, respectively. and P represents the difference in zero bias error between the accelerometer and gyroscope during the time interval from h-1 to h. t This represents the error covariance of the IMU at the corresponding time point;

[0162]

[0163]

[0164] Step 5: The establishment of the GNSS residual equation mainly considers the change of data from the Earth system to the navigation system and the lever arm factor. To ensure that the computational load does not increase with the increase of optimization variables, a sliding window method is introduced. This method saves certain historical information during the optimization process. When a new node is introduced during the optimization process, the system automatically discards the oldest node variable in the sliding window. However, if variables are directly discarded in this way, information loss is inevitable, and the discarded nodes may also have some connection with the nodes in the sliding window. Therefore, this invention adds marginalization constraints while removing historical variables through marginalization, thereby reducing information loss.

[0165] a) Step 3 yields the corrected latitude, longitude, and altitude information of the carrier. First, it is converted from the geographic coordinate system to the geocentric coordinate system. Then, the pose in the geocentric coordinate system is converted to the navigation coordinate system to establish the cost function. For example, formula (49) represents the GNSS residual equation, and formula (50) represents the corresponding GNSS cost function. Indicates the lever arm of the GNSS antenna. P represents the positioning result corrected by GNSS in the navigation system. g This represents the variance-covariance matrix provided by the positioning results;

[0166]

[0167]

[0168] b) Using the Jacobian matrix and variance-covariance matrix obtained in step 4, a sliding window is established and marginalization information is processed. The cost function in the optimization process is nonlinear, and the nonlinear least squares problem can be solved iteratively by formula (51).

[0169] Λδχ=g (51)

[0170]

[0171] In formula (52), A and b can be solved by the variance error term Jacobian matrix and variance covariance matrix obtained in step 4. The solution process is shown in formula (53), where x0 represents the initial linear point and H is defined as the first derivative of h(·) at the initial linear point.

[0172]

[0173] The variable χ that needs to be eliminated m Focusing on the upper part of the matrix, the variable χ needs to be retained. n Moving down, formula (51) can be rewritten as formula (54). Using Scher's complement to eliminate variables in formula (54), we obtain formula (55). The parameter Λ p The solution is shown in formula (56);

[0174]

[0175]

[0176]

[0177] Ultimately, we can obtain χ. n The residuals and the corresponding cost function are expressed in the form of formula (57). This indicates the χ used in formula (55) for elimination. n Estimation. In this process, while eliminating variables, information about marginal variables is also utilized. That is, constraints are not discarded, and the marginalization cost function of the following formula is added to the graph optimization to introduce constraints on marginal variables;

[0178]

[0179] Step 6: After obtaining the cost function required for the entire optimization process, establish a back-end nonlinear least squares problem and solve it using the LM method. Finally, compare the algorithm used in this invention (RFGO) with the traditional Extended Kalman Filter (EKF) algorithm and Factor Graph Optimization (FGO) algorithm. The trajectory comparison diagram is shown below. Figure 2 As shown, the root mean square error results for each algorithm are as follows: Figures 3-5 The detailed process is shown below;

[0180] a) After the backend preparation work, the GNSS cost function of the corrected satellite data is obtained. The IMU factor cost function is obtained by IMU pre-integration and the marginal information cost function is obtained by coordinate system transformation. The nonlinear least squares objective function is established as shown in formula (58).

[0181]

[0182] In the above formula, the detailed expression for the estimated vector is shown in formula (59), x ins The specific state expression is the same as that of the IMU in the front-end filtering process. For details, refer to the first five terms in formula (9), where f represents the corresponding cost function. The final navigation solution can be obtained by iteratively solving the least squares equation established by formula (58) using the LM method.

[0183]

[0184] b) such as Figure 2 As shown, the dashed path is the trajectory obtained by the method (RFGO) proposed in this invention, the dotted-line path is the trajectory obtained by the traditional extended Kalman filter algorithm (EKF) and the factor graph optimization algorithm (FGO) respectively, and the solid path represents the reference truth trajectory. It can be seen from the figure that the method proposed in this invention can effectively improve the navigation results in the denied environment.

[0185] c) such as Figures 3-5As shown in the figure, the root mean square errors of attitude, velocity, and position are represented by three different algorithms. The solid line represents the root mean square error between the proposed method (RFGO) and the reference ground truth, while the two dashed lines represent the root mean square errors between the traditional extended Kalman filter (EKF) algorithm, the factor graph optimization algorithm (FGO), and the reference ground truth, respectively. It can be seen from the figure that the proposed method improves the attitude, velocity, and position errors, with the position improvement being the best, indicating that the improved method can effectively improve the positioning accuracy of integrated navigation.

[0186] The preferred implementation of the present invention has been described in detail above, but the present invention is not limited to the described embodiments. Those skilled in the art can make various equivalent modifications or substitutions without departing from the spirit of the present invention, and these equivalent modifications or substitutions are all included within the scope defined by the claims of this application.

Claims

1. A GNSS-INS factor graph optimization method employing forward compact combination, characterized in that, Includes the following steps: Step S1: The inertial measurement unit (IMU) and the global positioning system (GNSS) collect navigation data at a certain frequency for a period of time, and synchronize the data in time and space. Step S2: Model the satellite and inertial navigation data, and establish the state equations and observation equations required for the compact combination filtering process; Step S3: Perform tight combination filtering based on the state equation and observation equation established in step S2. At the same time, use the IGG-III model to correct the weights of the covariance matrix of the observation equation, and then obtain the corrected satellite data after filtering. Step S4: Obtain the incremental information of the gyroscope and accelerometer obtained by the IMU, perform pre-integration operation on it, consider the deviation caused by the Earth's rotation during the pre-integration process, perform error analysis on the pre-integration results, obtain the corresponding error equation, and then construct the IMU residual equation and cost function. Step S5: Perform error analysis on the GNSS data corrected in step S3, obtain the GNSS residual equation and cost function, construct the marginalization information factor based on the Jacobian matrix obtained from the IMU pre-integration process, and obtain the residual equation and cost function corresponding to the marginalization information. Step S6: Based on the cost function obtained in steps S4 and S5, optimize the GNSS-INS factor map to obtain the optimal navigation solution.

2. The GNSS-INS factor graph optimization method using forward compact combination as described in claim 1, characterized in that, In step S1, the IMU provides gyroscope and accelerometer information, and the GNSS provides pseudorange and pseudorange rate information. The specific steps for synchronizing and aligning the data in time and space are as follows: Step S11: Import IMU raw data and GNSS raw data. The IMU data includes sampling information from the gyroscope and accelerometer in the three directions of northeast, south, and east. The GNSS raw data includes base station information and corresponding navigation message information. Step S12: For the GNSS raw data file, the open-source software Rtklib is used to perform preliminary calculations to obtain the corresponding carrier position and velocity information in the satellite data. The obtained data is then transformed into a coordinate system to achieve spatial synchronization between the satellite data and the inertial navigation data. Combined with the time of the data output, the data is then synchronized in time.

3. The GNSS-INS factor graph optimization method using forward compact combination as described in claim 1, characterized in that, The establishment of the state equations and observation equations required for the compact combination filtering process in step S2 is as follows: Step S21: Define each coordinate system for navigation. The state information required for the compact combination filtering process includes two parts: INS and GNSS. For the INS part, the state equation is established using inertial navigation error, which includes five state quantities: carrier attitude error, velocity error, position error, gyroscope zero bias error, and accelerometer zero bias error. For the GNSS part, since double-difference observations are used for calculation, it is not necessary to estimate the receiver clock error and drift parameters; only the double-difference ambiguity needs to be evaluated. Step S22: Establish the state equations required for the compact combination filtering process based on the strapdown inertial navigation algorithm and the principle of integrated navigation; Step S23: Subtract the satellite double-difference carrier phase observations and pseudorange observations obtained by differential technology from the double-difference distance calculated by INS to obtain the observation equations required for the compact combination filtering process.

4. The GNSS-INS factor graph optimization method using forward compact combination as described in claim 1, characterized in that, The compact combination filtering and weight correction process described in step S3 is as follows: Step S31: Let the state equation and observation equation established in step S2 be as follows: Z k =HX k +V k (2) In the above formula, A represents the state transition matrix, B represents the control matrix, and X represents the control matrix. k and X k Let W represent the state estimate and the state, respectively. k Z represents process noise. k H represents the observed values, and V represents the observation matrix. k Indicates observation noise; Step S32: The compactly combined data fusion process uses traditional Kalman filtering for processing. In the above formula, Q and R represent the process error covariance matrix and the observation error covariance matrix, respectively, and P represents the variance covariance matrix; Step S32: To eliminate breakpoints and outliers in the GNSS data, the IGG-III strategy is used, and the concept of weights is introduced to constrain the measurement covariance matrix during the filtering process. In the above formula, γ is the weighting coefficient based on the IGG-III scheme.

5. The GNSS-INS factor graph optimization method using forward compact combination as described in claim 1, characterized in that, The pre-integration operation and the processing of marginalization information in step S4 are as follows: Step S41: Take any sampling interval, and this interval contains several IMU measurements. The differential equation of the carrier velocity can be obtained through the force equation, and then the velocity pre-integration result of the IMU relative to the navigation system can be calculated by combining the equations. Considering the velocity compensation terms caused by Coriolis acceleration and gravitational acceleration, the velocity differential equation is integrated once to obtain the position pre-integration result with the compensation term. Step S42: Select the same time interval as in step S41, and use this as the starting point to recursively solve the attitude pre-integration results. The IMU error model is the same as in step S22. In this way, the overall pre-integration error differential equation is calculated, and the corresponding error Jacobian matrix and its variance covariance matrix are obtained.

6. The GNSS-INS factor graph optimization method using forward compact combination as described in claim 1, characterized in that, The construction process of the GNSS residual equation and the marginal information residual equation in step S5 is as follows: Step S51: The GNSS residual equation takes into account the coordinate system transformation process and the lever arm error between itself and the IMU; Step S52: Using the variance-covariance matrix and error Jacobian matrix obtained in step S42, the marginal information residual equation is established by combining the Scher elimination method.

7. The GNSS-INS factor graph optimization method using forward compact combination as described in claim 1, characterized in that, The optimization and comparative analysis process of the GNSS-INS factor map in step S6 is as follows: Step S61: Combine the cost functions obtained in steps S4 and S5 to construct a least squares problem and use the LM iterative method to solve the nonlinear optimization solution.

Images