Graph optimization model design method for system time-varying parameters and integrated navigation system

Abstract

The invention discloses a graph optimization model design method of system time-varying parameters and an integrated navigation system, the graph optimization model design method comprises: acquiring sensor parameters of a sliding window estimation system, the sensor parameters comprising time-varying parameters and non-time-varying parameters; performing time-varying parameter expression on the non-time-varying parameters; and obtaining a graph optimization model of the sliding window estimation system by using the non-time-varying parameters expressed by the time-varying parameters and the time-varying parameters. According to the method, the design and development process of the multi-sensor fusion algorithm based on graph optimization can be accelerated, and the unification of the designed graph optimization model and the traditional EKF model is ensured, so that the migration design process of the algorithm to graph optimization is simplified.

Description

Technical Field

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

[0002] GNSS-based engineering surveying technology has developed over decades, from centering measurement to the widely used tilt measurement today. In recent years, the release of GNSS integrated navigation system has provided a new direction 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 localization and mapping), graph optimization estimation technology has been proven to provide higher accuracy and robustness than 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) combined navigation, GNSS / INS / vision fusion navigation, etc.

[0004] The factor graph optimization algorithm is essentially a graphical expression 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 the graph optimization algorithm is applied to the state estimation problem in continuous time represented by multi-sensor fusion, its feasible design methods are diverse, which makes the certainty of its algorithm design worse than that of the Kalman filter algorithm.

[0005] The graph optimization model in the prior art has the defect that the time-invariant 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 of chaotic graph structure caused by non-time-varying parameters of graph optimization model in the prior art, and to provide a graph optimization model design method and integrated navigation system with system time-varying parameters that can accelerate the design and development process of multi-sensor fusion algorithm based on graph optimization, ensure the unity of the designed graph optimization model with the traditional EKF (Extended Kalman Filter) model, thereby simplifying the algorithm migration design process to graph optimization.

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

[0008] A method for designing a graph optimization model of a system time-varying parameter, wherein the method comprises:

[0009] Acquire sensor parameters of a sliding window estimation system, wherein the sensor parameters include time-varying parameters and time-invariant parameters;

[0010] Expressing the time-invariant parameters as time-varying parameters;

[0011] The graph optimization model of the sliding window estimation system is obtained by using the time-invariant parameters expressed by the time-varying parameters and the time-varying parameters.

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

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

[0014] Determine all 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 time-invariant parameters as time-varying parameters;

[0017] A nonlinear least squares problem is constructed using a graph optimization model, in which time-varying parameters with zero-value state transfer are reduced to non-time-varying parameters.

[0018] The nonlinear least squares problem is solved.

[0019] Preferably, expressing the time-invariant parameter as a time-variant parameter comprises:

[0020] Adding state transfer factors to time-invariant parameters to obtain time-varying parameter expressions;

[0021] Set several values ​​of state transfer factors;

[0022] The state estimation problem of time-invariant parameters is constructed using the set value of the state transfer factor;

[0023] Obtaining an error estimation result of the state estimation problem;

[0024] Obtaining an optimal value of a state transfer factor using the error estimation result;

[0025] The preferred value is used to express the time-invariant parameter as a time-variant parameter.

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

[0027] ,

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

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

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

[0031] Obtain estimation results for the state estimation problem;

[0032] An estimation error graph is drawn using the estimation result, the number of iterations, and the iteration time as the error estimation result.

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

[0034] Adding state transfer factors to time-invariant parameters to obtain time-varying parameter expressions;

[0035] Set the state transfer factor to zero;

[0036] The graphical optimization model of the sliding window estimation system is obtained by using time-invariant parameters with zero state transfer factor and time-varying parameters.

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

[0038] Based on the value of the state transfer factor covariance, dynamically adjusting the properties of the parameters to meet the numerical stability requirements of solving the nonlinear optimization problem;

[0039] The graph optimization model is solved.

[0040] Preferably, a factor of a target sensor of the sliding window estimation system at each moment is related to a time-invariant parameter, and a factor of the target sensor at each moment is related to a time-varying parameter at a corresponding moment, and the graph optimization model design method comprises:

[0041] Obtaining the graph optimization model;

[0042] Associating the factor of the target sensor at each time instant with the time-varying parameter expression of the time-invariant parameter at the corresponding time instant;

[0043] The graph optimization model is solved.

[0044] The present invention also provides an integrated navigation system, which is characterized in that the integrated navigation system is used to implement the above-mentioned graph optimization model design method for system time-varying parameters.

[0045] On the basis of being in accordance with the common sense in the art, the above-mentioned preferred conditions can be arbitrarily combined to obtain the preferred embodiments of the present invention.

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

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

[0048] Figure 1 A schematic diagram of a graph structure optimized for a sliding window graph in the prior art.

[0049] Figure 2 Another diagram structure schematic diagram of the sliding window diagram optimization of embodiment 1 of the present invention.

[0050] Figure 3 It is an experimental effect diagram of transmission errors in three states of Example 1 of the present invention.

[0051] Figure 4 This is another experimental effect diagram of the transmission errors in three states of Example 1 of the present invention.

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

[0053] Figure 6 This is a flowchart of the graph optimization model design method of embodiment 1 of the present invention. DETAILED DESCRIPTION

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

[0055] Example 1

[0056] This embodiment provides a GNSS integrated 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 integrated navigation system is a receiver including a GNSS module, an INS module and a vision module.

[0057] The processing module is used for:

[0058] Acquire sensor parameters of a sliding window estimation system, wherein the sensor parameters include time-varying parameters and time-invariant parameters;

[0059] Expressing the time-invariant parameters as time-varying parameters;

[0060] The graph optimization model of the sliding window estimation system is obtained by using the time-invariant parameters expressed by the time-varying parameters and the time-varying parameters.

[0061] The graph structure is Figure 1 As shown, the parameters in the graph can be divided into two categories: time-varying parameters and non-time-varying parameters. A graph structure of sliding window graph optimization is composed of these two types of parameters. Figure 1 middle, and Represents the sensor and sensors exist All measurements and constraints involved at the moment, subscript represents the starting time of the sliding window, and the length of the sliding window is The figure describes a sliding window estimation system in which the sensor The factors at each moment are related to the time-invariant parameters and the time-varying parameters at the corresponding moments Related, while the sensor The factor at each moment is only related to the time-varying parameter at the corresponding moment For example, for a GNSS / INS combined system, represents the position, velocity, attitude and other errors related to INS, and represents the GNSS-related centering lever arm error, that is, the translation vector between the GNSS antenna and the INS. All factors are This makes the expression of the graph more complicated, which will affect the design of the graph structure on the one hand, and also affect the structural arrangement when optimizing the graph on the other hand.

[0062] In order to avoid the confusion of graph structure caused by time-invariant parameters, the present application adds state transfer functions to the time-invariant parameters to express the time-varying parameters.

[0063] The processing module is used to:

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

[0065] Determine all 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 time-invariant parameters as time-varying parameters;

[0068] A nonlinear least squares problem is constructed using a graph optimization model, in which time-varying parameters with zero-value state transfer are reduced to non-time-varying parameters.

[0069] The nonlinear least squares problem is solved.

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

[0071] Adding state transfer factors to time-invariant parameters to obtain time-varying parameter expressions;

[0072] Set several values ​​of state transfer factors;

[0073] The state estimation problem of time-invariant parameters is constructed using the set value of the state transfer factor;

[0074] Obtaining an error estimation result of the state estimation problem;

[0075] Obtaining an optimal value of a state transfer factor using the error estimation result;

[0076] The preferred value is used to express the time-invariant parameter as a time-variant parameter.

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

[0078] ,

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

[0080] See also Figure 2 , is a state transfer factor, and the values ​​of the two parameters connected by this factor are the same. To connect the states, for the EKF algorithm, this process can be described by a state transfer process, that is, .

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

[0082]

[0083] in, For the matrix The Cholesky decomposition matrix of . In practice, 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 easy to cause the divergence of the iterator.

[0084] The processing module is also used for:

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

[0086] Obtain estimation results for the state estimation problem;

[0087] An estimation error graph is drawn using the estimation result, the number of iterations, and the iteration time as the error estimation result.

[0088] In order to obtain the value of the state transfer factor, the state transfer error covariance matrix is ​​explained The relationship with the optimizer is illustrated by taking the lever arm estimation problem in GNSS / INS integrated navigation as an example, using a set of vehicle data for experimental explanation. This application divides the processing methods into three types:

[0089] S1) The lever arm error is modeled as a time-varying parameter, and the state transfer error between time instants is set as ;

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

[0091] S3) The lever arm error is calculated according to Figure 2 The equivalence relationship shown is modeled as a time-invariant parameter.

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

[0093] Depend on Figure 3 It can be seen that when the state transfer noise of the lever arm error is set to a small value (i.e., S2), its estimated value is difficult to converge to the normal error range. Therefore, compared with the EKF, As a state model, the state transfer noise is set to zero. The graph optimization algorithm cannot strictly set the state transfer noise to zero. Accordingly, it is more feasible to set it to 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. Figure 4 It can be seen that the number of iterations will still increase sharply, and accordingly, the time consumption will also increase sharply, which makes this operation very unprofitable. Therefore, in graph optimization, it is very necessary to model the variables with zero state transfer noise as time-invariant parameters (i.e., S3).

[0094] Further, 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] The graphical optimization model of the sliding window estimation system is obtained by using time-invariant parameters with zero state transfer factor and time-varying parameters.

[0097] The processing module is also used for:

[0098] Based on the value of the state transfer factor covariance, dynamically adjusting the properties of the parameters to meet the numerical stability requirements of solving the nonlinear optimization problem;

[0099] The graph optimization model is solved.

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

[0101] Obtaining the graph optimization model;

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

[0103] The graph optimization model is solved.

[0104] Therefore, this application can Figure 1 The graph optimization framework shown in the figure is transformed into a more concise form, as Figure 5 As shown. Among them, and Figure 2 middle has the same meaning, i.e. is a time-invariant parameter .

[0105] In the actual graphical model solution, the properties of the parameters are dynamically adjusted based on the value of the state transfer factor covariance to meet the numerical stability requirements of the nonlinear optimization problem. Through this conversion, the designed graphical optimization model can be unified with the traditional EKF model to the greatest extent, thereby simplifying the design process of migrating the algorithm to graphical optimization.

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

[0107] Acquire sensor parameters of a sliding window estimation system, wherein the sensor parameters include time-varying parameters and time-invariant parameters;

[0108] Expressing the time-invariant parameters as time-varying parameters;

[0109] The graph optimization model of the sliding window estimation system is obtained by using the time-invariant parameters expressed by the 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, determining all parameters to be estimated for the estimation problem;

[0113] Step 102, classifying the parameters to be estimated into time-varying and time-invariant parameters;

[0114] Step 103: Establish a graph optimization model and express the time-invariant parameters as time-varying parameters;

[0115] Step 104: construct a nonlinear least squares problem using the graph optimization model, wherein the time-varying parameters with zero-value state transfer are restored to non-time-varying parameters;

[0116] Step 105: Solve the nonlinear least squares problem.

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

[0118] Adding state transfer factors to time-invariant parameters to obtain time-varying parameter expressions;

[0119] Set several values ​​of state transfer factors;

[0120] The state estimation problem of time-invariant parameters is constructed using the set value of the state transfer factor;

[0121] Obtaining an error estimation result of the state estimation problem;

[0122] Obtaining an optimal value of a state transfer factor using the error estimation result;

[0123] The preferred value is used to express the time-invariant parameter as a time-variant parameter.

[0124] The time-varying parameter expression is described by a state transfer process, and the state transfer process is: ,in is the sensor parameter at the sliding window k moment, is the state transfer error covariance matrix, which expresses the state transfer process in residual form:

[0125]

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

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

[0128] Setting the number of iterations and the iteration time of the state estimation problem according to several values ​​of the state transfer factor;

[0129] Obtain estimation results for the state estimation problem;

[0130] An estimation error graph is drawn using the estimation result, the number of iterations, and the iteration time as the error estimation result.

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

[0132] Adding state transfer factors to time-invariant parameters to obtain time-varying parameter expressions;

[0133] Set the state transfer factor to zero;

[0134] The graphical optimization model of the sliding window estimation system is obtained by using time-invariant parameters with zero state transfer factor and time-varying parameters.

[0135] The graph optimization model design method comprises:

[0136] Based on the value of the state transfer factor covariance, dynamically adjusting the properties of the parameters to meet the numerical stability requirements of solving the nonlinear optimization problem;

[0137] The graph optimization model is solved.

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

[0139] Obtaining the graph optimization model;

[0140] Associating the factor of the target sensor at each time instant with the time-varying parameter expression of the time-invariant parameter at the corresponding time instant;

[0141] The graph optimization model is solved.

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

Claims

1. A graphical optimization model design method for system time-varying parameters, characterized in that: The graph optimization model design method comprises: Acquire sensor parameters of a sliding window estimation system, wherein the sensor parameters include time-varying parameters and time-invariant parameters; Expressing the time-invariant parameters as time-varying parameters; The graph optimization model of the sliding window estimation system is obtained by using the time-invariant parameters expressed by the time-varying parameters and the time-varying parameters.

2. The method for designing a graph optimization model of a system time-varying parameter according to claim 1, characterized in that: The graph optimization model design method comprises: Given a multi-sensor fusion estimation problem containing multiple sensors to be fused; Determine all parameters to be estimated for the estimation problem; Classify the parameters to be estimated into time-varying and non-time-varying parameters; Establish a graph optimization model and express time-invariant parameters as time-varying parameters; A nonlinear least squares problem is constructed using a graph optimization model, in which time-varying parameters with zero-value state transfer are reduced to non-time-varying parameters. The nonlinear least squares problem is solved.

3. The method for designing a graph optimization model of a system time-varying parameter according to claim 1, characterized in that: The step of expressing the non-time-varying parameter as a time-varying parameter comprises: Adding state transfer factors to time-invariant parameters to obtain time-varying parameter expressions; Set several values ​​of state transfer factors; The state estimation problem of time-invariant parameters is constructed using the set value of the state transfer factor; Obtaining an error estimation result of the state estimation problem; Obtaining an optimal value of a state transfer factor using the error estimation result; The preferred value is used to express the time-invariant parameter as a time-variant parameter.

4. The method for designing a graph optimization model of a system time-varying parameter according to claim 3, characterized in that: The time-varying parameter expression is described by a state transfer process, and the state transfer process is: ,in is the sensor parameter at the sliding window k moment, for The estimated value of for The prediction from time to time k, is the state transfer error covariance matrix, yes The covariance matrix of , expresses the state transfer process in residual form: ， in, is the residual of the state transfer factor, and several values ​​of the state transfer factor are set using the residual form.

5. The method for designing a graph optimization model of a system time-varying parameter according to claim 3, characterized in that: The obtaining of the error estimation result of the state estimation problem includes: Setting the number of iterations and the iteration time of the state estimation problem according to several values ​​of the state transfer factor; Obtain estimation results for the state estimation problem; An estimation error graph is drawn using the estimation result, the number of iterations, and the iteration time as the error estimation result.

6. The method for designing a graph optimization model of a system time-varying parameter according to claim 1, characterized in that: The graph optimization model design method comprises: Adding state transfer factors to time-invariant parameters to obtain time-varying parameter expressions; Set the state transfer factor to zero; The graphical optimization model of the sliding window estimation system is obtained by using time-invariant parameters with zero state transfer factor and time-varying parameters.

7. The method for designing a graph optimization model of a system time-varying parameter according to claim 6, characterized in that: The graph optimization model design method comprises: Based on the value of the state transfer factor covariance, dynamically adjusting the properties of the parameters to meet the numerical stability requirements of solving the nonlinear optimization problem; The graph optimization model is solved.

8. The method for designing a graph optimization model of a system time-varying parameter according to claim 1, characterized in that: A factor of a target sensor of the sliding window estimation system at each moment is related to a time-invariant parameter, and a factor of the target sensor at each moment is related to a time-varying parameter at a corresponding moment. The graph optimization model design method includes: Obtaining the graph optimization model; Associating the factor of the target sensor at each time instant with the time-varying parameter expression of the time-invariant parameter at the corresponding time instant; The graph optimization model is solved.

9. An integrated navigation system, characterized in that: The integrated navigation system is used to implement the graph optimization model design method of system time-varying parameters as described in any one of claims 1 to 8.

Images