Null space dimension reduction covariance estimation method and system for high-dimensional sequential graph optimization

Abstract

The present invention discloses a high-dimensional sequential graph optimized null space dimension reduction covariance estimation method and system, the null space dimension reduction covariance estimation method comprises: obtaining measurement data of a sliding window, the measurement data comprises data in the sliding window collected by a number of sensors; establishing a factor graph optimization estimation of the measurement data, the factor graph optimization estimation comprises parameters to be estimated; performing dimension reduction processing on the parameters to be estimated; and solving the covariance of the factor graph optimization estimation after dimension reduction. The present invention linearizes the high-dimensional space and maps the covariance solution problem to a low-dimensional space by obtaining the null space of the high-dimensional parameters, thereby greatly reducing the covariance solution time while ensuring the optimality of the covariance estimation.

Description

Technical Field

[0001] The present invention relates to a high-dimensional sequential graph optimized null space dimension reduction covariance estimation method and system. Background Art

[0002] In recent years, inclination measurement based on inertial measurement units has been gradually promoted and applied. Through the inertial orientation positioning navigation system (INS) built into the GNSS receiver, the GNSS receiver attitude data is output in real time, and then the direction angle, tilt angle and tilt direction angle of the centering pole in the tilted state are calculated. Combined with the obtained GNSS receiver antenna phase center coordinates, the coordinates of the ground point at the bottom of the tilt centering pole can be calculated.

[0003] With the development and application of technologies such as high-precision engineering measurement and high-precision navigation, the accuracy consistency and reliability of sensor estimation systems are receiving more and more attention. In this industry, GNSS / INS combined navigation algorithms, visual VIO algorithms, and lidar LIO algorithms, as representative fusion navigation technologies, have been widely used. In recent years, in order to further improve the reliability of the system, the tight combination of GNSS, INS, vision, and lidar has gradually become a mainstream trend. Scientific research and engineering applications have proved that this type of multi-sensor tight combination system can provide the system with robust pose estimation output in complex environments to meet a wider range of application conditions.

[0004] To solve this multi-sensor fusion problem, two estimators can be used: Extended Kalman Filter (EKF) and Factor Graph Optimization (FGO). Among them, the graph optimization estimator has gradually become the mainstream today due to its better estimation performance. However, for high-dimensional estimation problems such as the above-mentioned multi-sensor tight combination problem, it is difficult to estimate the covariance of its parameters using the graph optimization estimator. In the prior art, since it takes a long time to directly estimate the covariance of the graph optimization estimator, its covariance cannot be estimated in practical applications. Instead, its covariance is directly set according to experience, which obviously cannot meet the growing demand for system reliability. Summary of the invention

[0005] The technical problem to be solved by the present invention is to overcome the defects in the prior art that the graph optimization estimator takes a long time to perform covariance estimation and is unable to estimate its covariance, and to provide a high-dimensional sequential graph optimization null space dimensionality reduction covariance estimation method and system which can significantly reduce the covariance solution time while ensuring the optimality of the covariance estimation.

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

[0007] A high-dimensional sequential graph optimized null space dimensionality reduction covariance estimation method, characterized in that the null space dimensionality reduction covariance estimation method comprises:

[0008] Acquire measurement data of the sliding window, wherein the measurement data includes data in the sliding window collected by a plurality of sensors;

[0009] Establishing a factor graph optimization estimate of the measurement data, wherein the factor graph optimization estimate includes parameters to be estimated;

[0010] Performing dimensionality reduction processing on the parameters to be estimated;

[0011] The covariance of the optimized estimate of the factor graph after dimensionality reduction is solved.

[0012] Preferably, the null space dimensionality reduction covariance estimation method comprises:

[0013] Get the covariance part to be solved in the estimated parameters;

[0014] Solve the covariance for the part to be solved.

[0015] Preferably, the factor graph optimization estimation is expressed using a nonlinear equation, and the dimension reduction processing of the parameters to be estimated includes:

[0016] Using a transformation matrix to linearize the nonlinear equation to obtain a linearized equation;

[0017] The linearized equation is divided into the covariance part to be solved and the covariance part that does not need to be solved;

[0018] Get the left null space of the transformation matrix corresponding to the part that does not need to be solved for the covariance;

[0019] The left null space is used to perform dimensionality reduction processing on the parameters to be estimated in the factor graph optimization estimation.

[0020] Preferably, the nonlinear equation is:

[0021]

[0022] in, For all parameters to be estimated, represent The state at the moment updates the sensor-related parameters to be estimated, Pass sensor related parameters for the state, for The state at time instant transmits the sensor-related parameters to be estimated, Update the sensor-related parameters to be estimated for the state at time i, The state transfer sensor related parameters to be estimated for time i, Update the sensor-related parameters to be estimated for the state at time i-1, represents the robust kernel function, The robust kernel function of the marginalization parameter, Z is the measurement of the sliding window, To marginalize pseudo-measurements, is the sensor measurement at time i, is the measurement from the state transfer sensor, The model is used to transfer parameters that cannot be measured by the state transfer sensor and express, is the covariance of the measurement, which is used to give the weight of the measurement in the system within a given sliding window.

[0023] Preferably, the null space dimensionality reduction covariance estimation method comprises:

[0024] For a target parameter to be estimated, it is determined whether there is a measurement of a state transfer sensor between the target parameter to be estimated. If not, a dynamic model corresponding to the target parameter to be estimated is used as the parameter transfer model instead.

[0025] Preferably, the solution formula for the covariance of the nonlinear equation parameters is: .in, and is the global Jacobian matrix and covariance matrix constructed from the Jacobian matrix and covariance matrix of all residuals, and T is the transposed symbol.

[0026] Preferably, the nonlinear equation is linearized to obtain , where Z is a vector composed of all measurements, is a vector consisting of all parameters to be estimated, is the Jacobian matrix.

[0027] Preferably, the formula Divided into two parts , where a is the covariance part to be solved, and b is the covariance part that does not need to be solved;

[0028] Find the Jacobian matrix The left null space of .

[0029] Preferably, using The solution formula for obtaining the covariance of the nonlinear equation parameters is: .

[0030] The present invention also provides a high-dimensional sequential graph optimized null space dimensionality reduction covariance estimation system, the high-dimensional sequential graph optimized null space dimensionality reduction covariance estimation system includes a GNSS receiver, and its characteristic is that the null space dimensionality reduction covariance estimation system is used to implement the high-dimensional sequential graph optimized null space dimensionality reduction covariance estimation method as described above.

[0031] 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.

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

[0033] The present invention linearizes the high-dimensional space and maps the covariance solution problem to a low-dimensional space by obtaining the high-dimensional parameter null space, thereby greatly reducing the covariance solution time while ensuring the optimality of the covariance estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 Schematic diagram of the principle of the null space dimensionality reduction covariance estimation system of Example 1 of the present invention.

[0035] Figure 2 Schematic diagram of the effect of the null space dimensionality reduction covariance estimation system of Example 1 of the present invention.

[0036] Figure 3 This is another schematic diagram of the effect of the null space dimensionality reduction covariance estimation system of Example 1 of the present invention.

[0037] Figure 4 This is a flow chart of the null space dimensionality reduction covariance estimation method of embodiment 1 of the present invention. DETAILED DESCRIPTION

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

[0039] Example 1

[0040] This embodiment provides a null-space dimensionality reduction covariance estimation system for high-dimensional sequential graph optimization, wherein the null-space dimensionality reduction covariance estimation system includes a GNSS receiver, a processing module, and a plurality of measurement sensors, wherein the measurement sensors include but are not limited to an inertial measurement unit (for realizing a tilt measurement function), a visual measurement unit, and a driving detection unit.

[0041] The processing module may be a separate processing terminal, such as a server, a desktop computer, a laptop computer, a tablet computer, a mobile phone, etc.

[0042] The processing module may also be a computing module integrated in the GNSS receiver, such as a central processing unit.

[0043] The measurement sensor is used to obtain measurement data of the sliding window, and the measurement data includes data in the sliding window collected by a plurality of sensors;

[0044] The processing module is used to establish a factor graph optimization estimate of the measurement data, wherein the factor graph optimization estimate includes parameters to be estimated;

[0045] The processing module is also used to perform dimensionality reduction processing on the parameters to be estimated;

[0046] The processing module is also used to solve the covariance of the factor graph optimization estimate after dimensionality reduction.

[0047] The processing module is also used for:

[0048] Get the covariance part to be solved in the estimated parameters;

[0049] Solve the covariance for the part to be solved.

[0050] The factor graph optimization estimation is expressed using a nonlinear equation, and the processing module is further used for:

[0051] Using a transformation matrix to linearize the nonlinear equation to obtain a linearized equation;

[0052] The linearized equation is divided into the covariance part to be solved and the covariance part that does not need to be solved;

[0053] Get the left null space of the transformation matrix corresponding to the part that does not need to be solved for the covariance;

[0054] The left null space is used to perform dimensionality reduction processing on the parameters to be estimated in the factor graph optimization estimation.

[0055] The nonlinear equation is:

[0056]

[0057] in, For all parameters to be estimated, represent The state at the moment updates the sensor-related parameters to be estimated, Pass sensor related parameters for the state, for The state at time instant transmits the sensor-related parameters to be estimated, Update the sensor-related parameters to be estimated for the state at time i, The state transfer sensor related parameters to be estimated for time i, Update the sensor-related parameters to be estimated for the state at time i-1, represents the robust kernel function, The robust kernel function of the marginalization parameter, Z is the measurement of the sliding window, To marginalize pseudo-measurements, is the sensor measurement at time i, is the measurement from the state transfer sensor, The model is used to transfer parameters that cannot be measured by the state transfer sensor and express, is the covariance of the measurement, which is used to give the weight of the measurement in the system within a given sliding window.

[0058] For all parameters to be estimated, represents the parameters to be estimated at time k, Pass sensor related parameters for the state, represents the robust kernel function, Z is the measurement of the sliding window, To marginalize pseudo-measurements, is the sensor measurement at time i, is the measurement from the state transfer sensor, The model is used to transfer parameters that cannot be measured by the state transfer sensor and express, is the covariance of the measurement, which is used to give the weight of the measurement in the system within a given sliding window.

[0059] For the sequential estimation problem, its optimization-based equation construction can be expressed as the nonlinear equation. The graphical optimization expression of the nonlinear equation is as follows: Figure 1 shown.

[0060] For all parameters to be estimated, is an estimated value, represents the parameters to be estimated at time k, The state update sensor is a sensor that can be used to connect two states at a time, such as an INS sensor. represents the robust kernel function. The subscript m represents the marginalization factor, which contains the prior information outside the sequential sliding window. z represents the measurement, where is the marginalized pseudo-measurement, corresponding to Figure 1 The m in is the sensor measurement at time i, corresponding to Figure 1 In , is the measurement from the state transfer sensor, corresponding to Figure 1 In , The parameter transfer model that the state transfer sensor cannot measure is usually replaced by the dynamic model of the parameter itself, which corresponds to Figure 1 In . is the covariance of the measurement, which gives the weight of the measurement in the system within the sliding window.

[0061] Specifically, the processing module is also used for:

[0062] For a target parameter to be estimated, it is determined whether there is a measurement of a state transfer sensor between the target parameter to be estimated. If not, a dynamic model corresponding to the target parameter to be estimated is used as the parameter transfer model instead.

[0063] Specifically, taking the VIO system as an example, Figure 1 In the VIO system, the state transfer sensor is INS, which contributes Figure 1 In Measurement, its corresponding parameters are the position, velocity, attitude, acceleration deviation and angular velocity deviation; correspondingly, Provided by the visual sensor, it is generally the reprojection error of the feature points, and its corresponding parameters represents a landmark point (the 2D point in the photo becomes a feature point, and its corresponding point in the 3D world becomes a landmark point). The state transfer model of the landmark point is For the VIO system, the number of landmark points is large (usually 100 to 1000), and the huge number of feature points makes the dimension of the parameters to be estimated extremely high.

[0064] The solution formula for the covariance of the nonlinear equation parameters is: .

[0065] T is the transpose symbol.

[0066] in, and is the global Jacobian matrix and covariance matrix constructed from the Jacobian matrix and covariance matrix of all residuals. Each of the nonlinear equations are all called residuals, and the nonlinear equation is linearized and becomes a linear equation Jx, corresponding to is the Jacobian matrix.

[0067] The nonlinear equation is linearized to obtain , where Z is a vector composed of all measurements, is a vector consisting of all parameters to be estimated, is the Jacobian matrix.

[0068] The formula Divided into two parts , where a is the covariance part to be solved, and b is the covariance part that does not need to be solved;

[0069] For example, for the above-mentioned VIO system, users generally only care about the covariance of position and posture, but not the covariance of landmark points.

[0070] Find the Jacobian matrix The left null space of , using the left null space to obtain .

[0071] Among them, the left zero matrix It can be obtained by Givens transformation. Since this process is parameter mapping, the optimality of the system will not change before and after the change occurs.

[0072] use The solution formula for obtaining the covariance of the nonlinear equation parameters is: .

[0073] In the above formula, the dimension of the matrix inversion is It can be seen that its dimension has been greatly reduced, and its solution efficiency will also be greatly improved.

[0074] See also Figure 2 , Figure 3 , the position and attitude covariance estimation accuracy and computational time of the GNSS / INS / vision tightly integrated system are given. Figure 2 This is a performance comparison of the dimensionality reduction method proposed in this embodiment compared to covariance estimation directly based on high-dimensional graphs, where sub-graph is the method in this paper.

[0075] Figure 3 The time consumption comparison of the two methods shows that the method proposed in this paper greatly reduces the amount of calculation. At the same time, its covariance estimation performance is basically consistent with the performance of directly using high-dimensional graphs for estimation.

[0076] See also Figure 4 , using the above-mentioned high-dimensional sequential graph optimized null space dimensionality reduction covariance estimation system, this embodiment also provides a null space dimensionality reduction covariance estimation method, including:

[0077] Step 100: Acquire measurement data of a sliding window, wherein the measurement data includes data in the sliding window collected by a plurality of sensors;

[0078] Step 101, establishing a factor graph optimization estimation of the measurement data, wherein the factor graph optimization estimation includes parameters to be estimated;

[0079] Step 102: performing dimensionality reduction processing on the parameters to be estimated;

[0080] Step 103: Solve the covariance of the optimized estimate of the factor graph after dimensionality reduction.

[0081] Step 102 includes obtaining the covariance part to be solved in the parameter to be estimated.

[0082] Step 102 includes solving the covariance of the part to be solved for.

[0083] Specifically, the factor graph optimization estimation is expressed using a nonlinear equation, and step 102 specifically includes:

[0084] Step 1021, linearizing the nonlinear equation using a conversion matrix to obtain a linearized equation;

[0085] Step 1022, dividing the linearized equation into a covariance part to be solved and a covariance part that does not need to be solved;

[0086] Step 1023, obtaining the left null space of the transformation matrix corresponding to the part that does not need to be solved for the covariance;

[0087] Step 1024: Use the left null space to perform dimensionality reduction processing on the parameters to be estimated in the factor graph optimization estimation.

[0088] The nonlinear equation is:

[0089]

[0090] in, For all parameters to be estimated, represents the parameters to be estimated at time k, Pass sensor related parameters for the state, represents the robust kernel function, Z is the measurement of the sliding window, To marginalize pseudo-measurements, is the sensor measurement at time i, is the measurement from the state transfer sensor, The model is used to transfer parameters that cannot be measured by the state transfer sensor and express, is the covariance of the measurement, which is used to give the weight of the measurement in the system within a given sliding window.

[0091] The null space dimension reduction covariance estimation method comprises:

[0092] For a target parameter to be estimated, it is determined whether there is a measurement of a state transfer sensor between the target parameter to be estimated. If not, a dynamic model corresponding to the target parameter to be estimated is used as the parameter transfer model instead.

[0093] The solution formula for the covariance of the nonlinear equation parameters is: .

[0094] The nonlinear equation is linearized to obtain , where Z is a vector composed of all measurements, is a vector consisting of all parameters to be estimated, and J is the Jacobian matrix.

[0095] The formula Divided into two parts , where a is the covariance part to be solved, and b is the covariance part that does not need to be solved;

[0096] Find the Jacobian matrix The left null space of .

[0097] use The solution formula for obtaining the covariance of the nonlinear equation parameters is: .

[0098] 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 high-dimensional sequential graph optimized null space dimension reduction covariance estimation method, characterized in that: The null space dimension reduction covariance estimation method comprises: Acquire measurement data of the sliding window, wherein the measurement data includes data in the sliding window collected by a plurality of sensors; Establishing a factor graph optimization estimate of the measurement data, wherein the factor graph optimization estimate includes parameters to be estimated; Performing dimensionality reduction processing on the parameters to be estimated; Solve the covariance of the optimized estimate of the factor graph after dimensionality reduction; The factor graph optimization estimation is expressed by a nonlinear equation, and the dimension reduction process of the parameter to be estimated includes: Using a transformation matrix to linearize the nonlinear equation to obtain a linearized equation; The linearized equation is divided into the covariance part to be solved and the covariance part that does not need to be solved; Get the left null space of the transformation matrix corresponding to the part that does not need to be solved for the covariance; The left null space is used to perform dimensionality reduction processing on the parameters to be estimated in the factor graph optimization estimation.

2. The high-dimensional sequential graph optimization null space dimensionality reduction covariance estimation method according to claim 1, characterized in that: The null space dimension reduction covariance estimation method comprises: Get the covariance part to be solved in the estimated parameters; Solve the covariance for the part to be solved.

3. The high-dimensional sequential graph optimized null space dimension reduction covariance estimation method according to claim 1, characterized in that: The nonlinear equation is: ， in, For all parameters to be estimated, represent The state at the moment updates the sensor-related parameters to be estimated, The sensor-related parameters to be estimated are passed to the state, for The state at time instant transmits the sensor-related parameters to be estimated, Update the sensor-related parameters to be estimated for the state at time i, The state transfer sensor related parameters to be estimated for time i, Update the sensor-related parameters to be estimated for the state at time i-1, represents the robust kernel function, The robust kernel function of the marginalization parameter, Z is the measurement of the sliding window, To marginalize pseudo-measurements, is the sensor measurement at time i, is the measurement from the state transfer sensor, The model is used to transfer parameters that cannot be measured by the state transfer sensor and express, is the covariance of the measurement, which is used to give the weight of the measurement in the system within a given sliding window.

4. The high-dimensional sequential graph optimization null space dimensionality reduction covariance estimation method according to claim 3, characterized in that: The null space dimension reduction covariance estimation method comprises: For a target parameter to be estimated, it is determined whether there is a measurement of a state transfer sensor between the target parameter to be estimated. If not, a dynamic model corresponding to the target parameter to be estimated is used as the parameter transfer model instead.

5. The high-dimensional sequential graph optimization null space dimension reduction covariance estimation method according to claim 3, characterized in that: The solution formula for the covariance of the nonlinear equation parameters is: ,in, and is the global Jacobian matrix and covariance matrix constructed from the Jacobian matrix and covariance matrix of all residuals, and T is the transposed symbol.

6. The high-dimensional sequential graph optimized null space dimension reduction covariance estimation method according to claim 5, characterized in that: The nonlinear equation is linearized to obtain , where Z is a vector composed of all measurements, is a vector consisting of all parameters to be estimated, is the Jacobian matrix.

7. The high-dimensional sequential graph optimized null space dimension reduction covariance estimation method according to claim 6, characterized in that: The formula Divided into two parts , where a is the covariance part to be solved, and b is the covariance part that does not need to be solved; Find the Jacobian matrix The left null space of .

8. The high-dimensional sequential graph optimized null space dimension reduction covariance estimation method according to claim 7, characterized in that: use The solution formula for obtaining the covariance of the nonlinear equation parameters is: .

9. A high-dimensional sequential graph optimized null space dimensionality reduction covariance estimation system, the high-dimensional sequential graph optimized null space dimensionality reduction covariance estimation system comprising a GNSS receiver, characterized in that: The null space dimensionality reduction covariance estimation system is used to implement the null space dimensionality reduction covariance estimation method for high-dimensional sequential graph optimization as described in any one of claims 1 to 8.

Images