Graph Optimization Model Design Method for System Time-Varying Parameters and Integrated Navigation System

Abstract

The present invention discloses a method for designing a graph optimization model of system time-varying parameters and an integrated navigation system. The method for designing the graph optimization model includes: obtaining sensor parameters of a sliding window estimation system, where the sensor parameters include time-varying parameters and non-time-varying parameters; expressing the non-time-varying parameters in terms of time-varying parameters; and obtaining a graph optimization model of the sliding window estimation system by using the non-time-varying parameters expressed in terms of time-varying parameters and the time-varying parameters. The present invention can accelerate the design and development process of a multi-sensor fusion algorithm based on graph optimization, ensure that the designed graph optimization model is unified with the traditional EKF model, and thus simplify the migration design process of the algorithm to graph optimization.

Description

Technical Field

[0001] The present invention relates to the field of GNSS integrated navigation systems, and particularly to a method for designing a graph optimization model for system time-varying parameters and an integrated navigation system. Background Art

[0002] Based on decades of development, GNSS-based engineering surveying technology has evolved from centering measurement to the widely used tilt measurement today. In recent years, the release of GNSS integrated navigation systems has provided a new vane for engineering surveying technology.

[0003] In recent years, with the development of navigation technologies such as visual SLAM (simultaneous localization and mapping) and LIDAR SLAM (radar simultaneous localization and mapping), graph optimization estimation technology has been proven to provide higher accuracy and robustness compared to traditional Kalman estimators in such problems. At the same time, graph optimization technology has also been increasingly applied to other multi-sensor estimation problems, such as GNSS positioning, GNSS / INS (inertial navigation system) integrated navigation, GNSS / INS / visual fusion navigation, etc.

[0004] The factor graph optimization algorithm is essentially a graphical representation of the least squares problem. Therefore, similar to the least squares algorithm, the graph optimization algorithm can handle various optimization problems in nature and has high flexibility. However, precisely because of its flexibility, when applying the graph optimization algorithm to state estimation problems under continuous time represented by multi-sensor fusion, there are various feasible design methods, which makes the determinacy of its algorithm design worse than that of the Kalman filtering algorithm.

[0005] In the prior art, the graph optimization model has the defect that non-time-varying parameters cause the graph structure to be chaotic. Summary of the Invention

[0006] The technical problem to be solved by the present invention is to overcome the defect that the non-time-varying parameters of the graph optimization model in the prior art cause the graph structure to be chaotic, and to provide a method for designing a graph optimization model for system time-varying parameters and an integrated navigation system, which can accelerate the design and development process of the multi-sensor fusion algorithm based on graph optimization, ensure that the designed graph optimization model is unified with the traditional EKF (extended Kalman filter) model, and thus simplify the migration design process of the algorithm to graph optimization.

[0007] The present invention solves the above technical problem through the following technical solutions:

[0008] A method for designing a graph optimization model for system time-varying parameters, characterized in that the graph optimization model design method includes:

[0009] Obtain the sensor parameters of a sliding window estimation system, where the sensor parameters include time-varying parameters and non-time-varying parameters;

[0010] Express the non-time-varying parameters in terms of time-varying parameters;

[0011] Use the non-time-varying parameters expressed in terms of time-varying parameters and the time-varying parameters to obtain the graph optimization model of the sliding window estimation system.

[0012] Preferably, the method for designing the graph optimization model includes:

[0013] Given a multi-sensor fusion estimation problem involving multiple sensors to be fused;

[0014] Determine all the parameters to be estimated for the estimation problem;

[0015] Classify the parameters to be estimated into time-varying and non-time-varying parameters;

[0016] Establish a graph optimization model and express the non-time-varying parameters in terms of time-varying parameters;

[0017] Use the graph optimization model to construct a non-linear least squares problem, where the time-varying parameters with zero-value state transfer are restored to non-time-varying parameters for implementation;

[0018] Solve the non-linear least squares problem.

[0019] Preferably, the expression of the non-time-varying parameters in terms of time-varying parameters includes:

[0020] Add a state transfer factor to the non-time-varying parameters to obtain the expression in terms of time-varying parameters;

[0021] Set several values of the state transfer factor;

[0022] Use the set values of the state transfer factor to construct the state estimation problem of the non-time-varying parameters;

[0023] Obtain the error estimation result of the state estimation problem;

[0024] Use the error estimation result to obtain the optimal value of the state transfer factor;

[0025] Use the optimal value to express the non-time-varying parameters in terms of time-varying parameters.

[0026] Preferably, the expression in terms of time-varying parameters is described by a state transfer process, and the state transfer process is , where is the sensor parameter at the k-th moment of the sliding window, is the estimated value of, is the prediction from the moment to the k-th moment, is the state transfer error covariance matrix, is the covariance matrix of, and express the state transfer process in the form of residuals:

[0027] ,

[0028] wherein, is the residual of the state transfer factor, and several values of the state transfer factor are set by using the residual form.

[0029] Preferably, the obtaining of the error estimation result of the state estimation problem includes:

[0030] Set the number of iterations and the iteration time of the state estimation problem according to several values of the state transfer factor;

[0031] Obtain the estimation result of the state estimation problem;

[0032] Use the estimation result, the number of iterations and the iteration time to draw an estimation error graph as the error estimation result.

[0033] Preferably, the method for designing the graph optimization model includes:

[0034] Add a state transfer factor to the non-time-varying parameter to obtain a time-varying parameter expression;

[0035] Set the state transfer factor to zero;

[0036] Use the non-time-varying parameter with the state transfer factor being zero and the time-varying parameter to obtain the graph optimization model of the sliding window estimation system.

[0037] Preferably, the method for designing the graph optimization model includes:

[0038] Based on the value of the covariance of the state transfer factor, dynamically adjust the nature of the parameter to meet the numerical stability requirement for solving the non-linear optimization problem;

[0039] Solve the graph optimization model.

[0040] Preferably, the factor of a target sensor of the sliding window estimation system at each moment is related to the non-time-varying parameter, and the factor of the target sensor at each moment is related to the time-varying parameter at the corresponding moment. The method for designing the graph optimization model includes:

[0041] Obtain the graph optimization model;

[0042] Associate the factor of the target sensor at each moment with the time-varying parameter expression at the corresponding moment of the non-time-varying parameter;

[0043] Solve the graph optimization model.

[0044] The present invention also provides a combined navigation system, characterized in that the combined navigation system is used to implement the method for designing a graph optimization model of the system time-varying parameters as described above.

[0045] On the basis of conforming to the common knowledge in the art, the above preferred conditions can be combined arbitrarily to obtain various preferred examples of the present invention.

[0046] The positive and progressive effects of the present invention are as follows:

[0047] The present invention can accelerate the design and development process of a multi-sensor fusion algorithm based on graph optimization, ensure that the designed graph optimization model is unified with the traditional EKF model, and thus simplify the migration design process of the algorithm to graph optimization. BRIEF DESCRIPTION OF THE DRAWINGS

[0048] Figure 1 FIG. is a schematic diagram of the graph structure of the sliding window graph optimization for the prior art.

[0049] Figure 2 FIG. is another schematic diagram of the graph structure of the sliding window graph optimization for Embodiment 1 of the present invention.

[0050] Figure 3 FIG. is the experimental effect diagram of the three state transfer errors for Embodiment 1 of the present invention.

[0051] Figure 4 FIG. is another experimental effect diagram of the three state transfer errors for Embodiment 1 of the present invention.

[0052] Figure 5 FIG. is a schematic diagram of the graph structure of the sliding window graph optimization for Embodiment 1 of the present invention.

[0053] Figure 6 FIG. is a flowchart of the method for designing the graph optimization model for Embodiment 1 of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0054] The present invention will be further described below by way of examples, but the present invention is not limited to the scope of the described examples.

[0055] Embodiment 1

[0056] This embodiment provides a GNSS combined navigation system, which includes a GNSS receiver, a centering rod, an INS module, a vision module, and a processing module. In other embodiments, the GNSS combined navigation system is a receiver including a GNSS module, an INS module, and a vision module.

[0057] The processing module is used for:

[0058] Obtain the sensor parameters of a sliding window estimation system, where the sensor parameters include time-varying parameters and non-time-varying parameters;

[0059] Make a time-varying parameter expression for the non-time-varying parameter;

[0060] Obtain the graph optimization model of the sliding window estimation system by using the non-time-varying parameter and the time-varying parameter expressed by the time-varying parameter.

[0061] The graph structure is as Figure 1 shown. The parameters in the graph can be divided into two categories: time-varying parameters and non-time-varying parameters, and a graph structure of sliding window graph optimization composed of these two types of parameters. Figure 1 In and respectively represent all the measurements and constraint factors involved by sensor and sensor at moment. The subscript represents the starting moment of the sliding window, and the length of the sliding window is . This graph describes a sliding window estimation system. In this system, the factors of sensor at each moment are all related to the non-time-varying parameter and the time-varying parameter at the corresponding moment, while the factors of sensor at each moment are only related to the time-varying parameter at the corresponding moment. For example, for the GNSS / INS loosely coupled system, represents the errors such as position, velocity, and attitude related to INS, while represents the pole arm error related to GNSS, that is, the translation vector between the GNSS antenna and INS. It can be seen that when representing this system by a graph, all the factors of sensor need to be connected to , which makes the expression of this graph relatively complex. On the one hand, it will affect the design of the graph structure, and on the other hand, it will also affect the structural arrangement during the optimization solution of the graph.

[0062] To avoid the graph structure confusion caused by non-time-varying parameters, this application adds a state transfer function to the non-time-varying parameter for time-varying parameter expression.

[0063] The processing module is used for:

[0064] Given a multi-sensor fusion estimation problem including multiple sensors to be fused;

[0065] Determine all the parameters to be estimated for the estimation problem;

[0066] Classify the parameters to be estimated into time-varying and non-time-varying parameters;

[0067] Establish a graph optimization model and express the non-time-varying parameter as a time-varying parameter;

[0068] Use the graph to optimize the model to construct a nonlinear least squares problem, where the time-varying parameters with zero-value state transfer are restored to non-time-varying parameters for implementation;

[0069] Solve the nonlinear least squares problem.

[0070] Specifically, the processing module is used for:

[0071] Add a state transfer factor to the non-time-varying parameters to obtain a time-varying parameter expression;

[0072] Set several values of the state transfer factor;

[0073] Use the set values of the state transfer factor to construct a state estimation problem for the non-time-varying parameters;

[0074] Obtain the error estimation result of the state estimation problem;

[0075] Use the error estimation result to obtain the optimal value of the state transfer factor;

[0076] Use the optimal value to make a time-varying parameter expression for the non-time-varying parameters.

[0077] The time-varying parameter expression is described by a state transfer process, and the state transfer process is , where is the sensor parameter at the k-th moment of the sliding window, is the estimated value of, is the prediction from the moment to the k-th moment, is the state transfer error covariance matrix, is the covariance matrix of, and the state transfer process is expressed in a residual form:

[0078] ,

[0079] where, is the residual of the state transfer factor, and several values of the state transfer factor are set using the residual form.

[0080] See Figure 2 , is the state transfer factor, and the values of the two parameters connected by this factor are the same value. Through to connect between states. For the EKF algorithm, this process can be described by a state transfer process, that is .

[0081] where, is the state transfer error covariance matrix. This process is natural for the EKF algorithm. However, for the graph optimization algorithm, this state transfer needs to be expressed in the form of residuals, that is,

[0082]

[0083] where is the Cholesky decomposition matrix of the matrix . In actual operation, is set to a matrix as close to zero as possible. The numerical setting of needs to consider the numerical stability of the optimization problem and whether it is likely to cause the divergence of the iterator.

[0084] The processing module is further configured to:

[0085] Set the number of iterations and the iteration time of the state estimation problem according to several values of the state transfer factor;

[0086] Obtain the estimation result of the state estimation problem;

[0087] Use the estimation result, the number of iterations, and the iteration time to draw an estimation error graph as the error estimation result.

[0088] In order to obtain the value of the state transfer factor, clarify the relationship between the state transfer error covariance matrix and the optimizer. Taking the lever arm estimation problem in GNSS / INS integrated navigation as an example, a set of vehicle data is used for experimental illustration. This application divides the processing methods into 3 types:

[0089] S1) Model the lever arm error as a time-varying parameter, and set the state transfer error between moments as ;

[0090] S2) Model the lever arm error as a time-varying parameter, and set the state transfer error between moments as ;

[0091] S3) Model the lever arm error as a non-time-varying parameter according to the equivalent relationship shown in Figure 2 .

[0092] In all strategies, the upper limit of the number of iterations per epoch is set to 30 times. The lever arm estimation results, the number of iterations, and the iteration time of the above 3 strategies are respectively plotted in Figure 3 and Figure 4 .

[0093] As can be seen from Figure 3 , when the state transfer noise of the lever arm error is set to be small (i.e., S2), its estimated value is difficult to converge to the normal error range. Therefore, compared with the EKF where As a state model, the state transfer noise is set to zero. However, the graph optimization algorithm cannot strictly set the state transfer noise to zero. Correspondingly, it is more feasible to set it as a time-invariant parameter. Even if the state transfer noise is set to a relatively small but non-zero value (i.e., S1), it will not have a significant impact on the estimation performance. However, as can be seen from Figure 4 it is known that the number of iterations will still increase sharply, and correspondingly, the time consumption will also increase sharply, making this operation very uneconomical. Therefore, in graph optimization, it is very necessary to model the variable with zero state transfer noise as a time-invariant parameter (i.e., S3).

[0094] Furthermore, the processing module is also used to: add a state transfer factor to the time-invariant parameter to obtain a time-varying parameter expression;

[0095] Set the state transfer factor to zero;

[0096] Use the time-invariant parameter with zero state transfer factor and the time-varying parameter to obtain the graph optimization model of the sliding window estimation system.

[0097] The processing module is also used to:

[0098] Dynamically adjust the nature of the parameter based on the value of the covariance of the state transfer factor to meet the numerical stability requirements for solving the non-linear optimization problem;

[0099] Solve the graph optimization model.

[0100] The factor of a target sensor of the sliding window estimation system at each moment is related to the time-invariant parameter, and the factor of the target sensor at each moment is related to the time-varying parameter at the corresponding moment. The method for designing the graph optimization model includes:

[0101] Obtain the graph optimization model;

[0102] Associate the factor of the target sensor at each moment with the time-varying parameter expression of the time-invariant parameter at the corresponding moment;

[0103] Solve the graph optimization model.

[0104] Thus, this application can transform the Figure 1 shown graph optimization framework into a more concise form, as shown in Figure 5 shown. Among them, is the same as Figure 2 in , that is, is the time-invariant parameter .

[0105] In the actual solution of the graph model, based on the value of the state transfer factor covariance, the nature of the parameters is dynamically adjusted to meet the numerical stability requirements for solving the non-linear optimization problem. Through this transformation, the designed graph optimization model can be maximally unified with the traditional EKF model, thus simplifying the migration design process of the algorithm to graph optimization.

[0106] Using the above GNSS integrated navigation system, this embodiment also provides a method for designing a graph optimization model of system time-varying parameters, and the graph optimization model design method includes:

[0107] Obtain the sensor parameters of a sliding window estimation system, where the sensor parameters include time-varying parameters and non-time-varying parameters;

[0108] Express the non-time-varying parameters in terms of time-varying parameters;

[0109] Obtain the graph optimization model of the sliding window estimation system by using the non-time-varying parameters expressed in terms of time-varying parameters and the time-varying parameters.

[0110] Specifically, the graph optimization model design method includes:

[0111] Step 100: Given a multi-sensor fusion estimation problem including multiple sensors to be fused;

[0112] Step 101: Determine all the parameters to be estimated for the estimation problem;

[0113] Step 102: Classify the parameters to be estimated into time-varying and non-time-varying parameters;

[0114] Step 103: Establish a graph optimization model and express the non-time-varying parameters in terms of time-varying parameters;

[0115] Step 104: Use the graph optimization model to construct a non-linear least squares problem, where the time-varying parameters with zero-valued state transfer are restored to non-time-varying parameters for implementation;

[0116] Step 105: Solve the non-linear least squares problem.

[0117] The step of expressing the non-time-varying parameters in terms of time-varying parameters specifically includes:

[0118] Add state transfer factors to the non-time-varying parameters to obtain the expression in terms of time-varying parameters;

[0119] Set a number of values for the state transfer factors;

[0120] Use the set values of the state transfer factors to construct a state estimation problem for the non-time-varying parameters;

[0121] Obtain the error estimation result of the state estimation problem;

[0122] Obtain the optimal value of the state transfer factor using the error estimation result;

[0123] Use the optimal value to express the non-time-varying parameter as a time-varying parameter.

[0124] The time-varying parameter expression is described by a state transfer process, and the state transfer process is , where is the sensor parameter at the k-th moment of the sliding window, is the state transfer error covariance matrix, and express the state transfer process in the form of residuals:

[0125]

[0126] where, is the residual of the state transfer factor, and set several values of the state transfer factor using the residual form.

[0127] The obtaining of the error estimation result of the state estimation problem specifically includes:

[0128] Set the iteration times and iteration time of the state estimation problem according to several values of the state transfer factor;

[0129] Obtain the estimation result of the state estimation problem;

[0130] Use the estimation result, iteration times, and iteration time to draw an estimation error graph as the error estimation result.

[0131] Furthermore, the method for designing the graph optimization model includes:

[0132] Add a state transfer factor to the non-time-varying parameter to obtain a time-varying parameter expression;

[0133] Set the state transfer factor to zero;

[0134] Use the non-time-varying parameter with the state transfer factor being zero and the time-varying parameter to obtain the graph optimization model of the sliding window estimation system.

[0135] The method for designing the graph optimization model includes:

[0136] Based on the value of the covariance of the state transfer factor, dynamically adjust the nature of the parameter to meet the numerical stability requirements for solving the non-linear optimization problem;

[0137] Solve the graph optimization model.

[0138] The factor of a target sensor of the sliding window estimation system at each moment is related to the non-time-varying parameter, and the factor of the target sensor at each moment is related to the time-varying parameter at the corresponding moment. The method for designing the graph optimization model includes:

[0139] Obtain the graph optimization model;

[0140] Associate the factor of the target sensor at each moment with the time-varying parameter expression of the corresponding moment of the non-time-varying parameter;

[0141] Solve the graph optimization model.

[0142] Although the specific embodiments of the present invention have been described above, those skilled in the art should understand that these are only examples. The protection scope of the present invention is defined by the appended claims. Without departing from the principles and essence of the present invention, those skilled in the art can make various changes or modifications to these embodiments, but these changes and modifications all fall within the protection scope of the present invention.

Claims

1. A method for designing a graph optimization model of system time-varying parameters, which is used for an integrated navigation system, is characterized in that, The method for designing the graph optimization model includes: Obtain the sensor parameters of a sliding window estimation system, where the sensor parameters include time-varying parameters and non-time-varying parameters. The non-time-varying parameter is the lever arm error of the alignment rod of the integrated navigation system, and the time-varying parameters are the position parameters, velocity parameters, or attitude parameters related to the INS of the integrated navigation system; Express the non-time-varying parameters in terms of time-varying parameters; Use the non-time-varying parameters expressed in terms of time-varying parameters and the time-varying parameters to obtain the graph optimization model of the sliding window estimation system; Among them, expressing the non-time-varying parameters in terms of time-varying parameters includes: Add a state transfer factor to the non-time-varying parameters to obtain an expression in terms of time-varying parameters; Set several values of the state transfer factor; Use the set values of the state transfer factor to construct a state estimation problem for the non-time-varying parameters; Obtain the error estimation result of the state estimation problem; Use the error estimation result to obtain the optimal value of the state transfer factor; Use the optimal value to express the non-time-varying parameters in terms of time-varying parameters; The time-varying parameter expression is described by a state transfer process, and the state transfer process is , where is the sensor parameter at the k-th moment of the sliding window, is 's estimated value, is 's prediction from the moment to the k-th moment, is the state transfer error covariance matrix, is 's covariance matrix. The state transfer process is expressed in a residual form: ， Among them, is the residual of the state transfer factor, and several values of the state transfer factor are set using the residual form.

2. The method for designing a graph optimization model of the time-varying parameters of the system according to claim 1, characterized in that, The method for designing the graph optimization model includes: Given a multi-sensor fusion estimation problem including multiple sensors to be fused; Determine all the parameters to be estimated in the estimation problem; Classify the parameters to be estimated into time-varying and non-time-varying parameters; Establish a graph optimization model and express the non-time-varying parameters in terms of time-varying parameters; Use the graph optimization model to construct a non-linear least squares problem, where the time-varying parameters with zero-valued state transfer are restored to non-time-varying parameters for implementation; Solve the non-linear least squares problem.

3. The method for designing a graph optimization model of time-varying parameters of the system according to claim 1, characterized in that, Obtaining the error estimation result of the state estimation problem includes: Set the iteration times and iteration time of the state estimation problem according to several values of the state transfer factor; Obtain the estimation result of the state estimation problem; Use the estimation result, iteration times, and iteration time to draw an estimation error graph as the error estimation result.

4. The method for designing a graph optimization model of time-varying parameters of the system according to claim 1, wherein The method for designing the graph optimization model includes: Add a state transfer factor to the non-time-varying parameters to obtain an expression in terms of time-varying parameters; Set the state transfer factor to zero; Use the non-time-varying parameters with the state transfer factor being zero and the time-varying parameters to obtain the graph optimization model of the sliding window estimation system.

5. The method for designing a graph optimization model of time-varying parameters of the system according to claim 4, characterized in that, The method for designing the graph optimization model includes: Based on the value of the covariance of the state transfer factor, dynamically adjust the nature of the parameters to meet the numerical stability requirements for solving the non-linear optimization problem; Solve the graph optimization model.

6. The method for designing a graph optimization model of the time-varying parameters of the system according to claim 1, characterized in that For a target sensor of the sliding window estimation system, the factors at each moment are related to the non-time-varying parameters, and the factors of the target sensor at each moment are related to the time-varying parameters at the corresponding moment. The method for designing the graph optimization model includes: Obtain the graph optimization model; Associate the factors of the target sensor at each moment with the expression of the time-varying parameters at the corresponding moment of the non-time-varying parameters; Solve the graph optimization model.

7. A combined navigation system, characterized in that, The integrated navigation system is used to implement the method for designing the graph optimization model of the system time-varying parameters as described in any one of claims 1 to 6.

Images