Null-space dimensionality-reduction covariance estimation method and system for high-dimensional sequential graph optimization
By using a zero-space dimensionality reduction covariance estimation method to optimize high-dimensional sequential graphs, the problem of excessively long covariance estimation time in graph optimization estimators is solved, achieving efficient covariance estimation and improving the reliability and efficiency of the system.
Patent Information
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- TERSUS GNSS INC
- Filing Date
- 2024-11-15
- Publication Date
- 2026-05-07
AI Technical Summary
In existing technologies, graph optimization estimators take too long to perform covariance estimation, which cannot meet the system reliability requirements.
By acquiring measurement data from a sliding window, a factor graph optimization estimate is established, dimensionality reduction is performed, linearization is achieved using a transformation matrix, the covariance to be solved and the covariance that does not need to be solved are separated, and dimensionality reduction is performed using the left null space to achieve the solution of the covariance.
While ensuring the optimality of covariance estimation, the covariance solution time is significantly reduced, thus improving the efficiency of the system.
Smart Images
Figure CN2024132213_07052026_PF_FP_ABST
Abstract
Description
A method and system for zero-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization Technical Field
[0001] This invention relates to a method and system for null-space dimensionality reduction covariance estimation in high-dimensional sequential graph optimization. Background Technology
[0002] In recent years, tilt measurement based on inertial measurement units (INS) has been gradually promoted and applied. By using the inertial orientation and navigation system (INS) built into the GNSS receiver to output the GNSS receiver attitude data in real time, the azimuth angle, tilt angle, and tilt azimuth angle of the centering rod under tilted state can be calculated. Combined with the obtained phase center coordinates of the GNSS receiver antenna, the coordinates of the ground point at the bottom of the tilted centering rod can be calculated.
[0003] With the development and application of technologies such as high-precision engineering surveying and high-precision navigation, the accuracy consistency and reliability of sensor estimation systems are receiving increasing attention. In this industry, GNSS / INS integrated navigation algorithms, visual VIO algorithms, and LiDAR LIO algorithms, as representative fusion navigation technologies, have been widely used. In recent years, to further improve system reliability, tightly integrating GNSS, INS, visual, and LiDAR sensors has gradually become a mainstream trend. Scientific research and engineering applications have proven that such tightly integrated multi-sensor systems can provide robust pose estimation outputs in complex environments, meeting a wider range of application requirements.
[0004] Two estimators can be used to solve this multi-sensor fusion problem: the Extended Kalman Filter (EKF) estimator and the Factor Graph Optimization (FGO) estimator. Among them, the FGO estimator, with its superior estimation performance, has gradually become the mainstream. However, for high-dimensional estimation problems such as the aforementioned multi-sensor compact combination problem, it is difficult to estimate the covariance of its parameters using the FGO estimator. In existing technologies, directly estimating the covariance of the FGO estimator is very time-consuming. Therefore, in practical applications, the covariance cannot be estimated directly; instead, it is set empirically, which obviously cannot meet the growing demand for system reliability. Summary of the Invention
[0005] The technical problem to be solved by this invention is to overcome the shortcomings of existing graph optimization estimators, which take a long time to estimate covariance and cannot estimate its covariance. This invention provides a zero-space dimensionality reduction covariance estimation method and system for high-dimensional sequential graph optimization that significantly reduces the covariance solution time while ensuring the optimality of covariance estimation.
[0006] The present invention solves the above-mentioned technical problems through the following technical solution:
[0007] A zero-space dimensionality reduction covariance estimation method for high-dimensional sequential graph optimization is characterized in that the zero-space dimensionality reduction covariance estimation method includes:
[0008] Acquire measurement data of a sliding window, the measurement data including data collected by several sensors within the sliding window;
[0009] Establish a factor graph optimization estimate of the measurement data, wherein the factor graph optimization estimate includes the parameters to be estimated;
[0010] The parameters to be estimated are then subjected to dimensionality reduction processing;
[0011] The covariance of the optimized estimation of the dimensionality-reduced factor graph is solved.
[0012] Preferably, the zero-space dimensionality reduction covariance estimation method includes:
[0013] Obtain the covariance component to be solved from the parameters to be estimated;
[0014] The covariance is solved for the part that needs to be solved for.
[0015] Preferably, the factor graph optimization estimation is expressed using a nonlinear equation, and the dimensionality reduction processing of the parameters to be estimated includes:
[0016] The nonlinear equation is linearized using a transformation matrix to obtain a linearized equation.
[0017] The linearized equation is divided into a covariance part to be solved and a covariance part that does not need to be solved.
[0018] Obtain the left null space of the transformation matrix corresponding to the covariance component of the solution without requirements;
[0019] The left null space is used to reduce the dimensionality of the parameters to be estimated in the factor graph optimization estimation.
[0020] Preferably, the nonlinear equation is:
[0021]
[0022] Where χ represents all parameters to be estimated. χ represents the sensor-related parameters to be estimated at time k0, indicating the state update. p To transmit sensor-related parameters for status information, The sensor-related parameters to be estimated at time k0 are transferred to the state at k0, χ. i Update the sensor-related parameters to be estimated based on the state at time i. The sensor-related parameters to be estimated are passed to the state at time i, χ. i-1 To update the sensor-related parameters to be estimated at time i-1, ρ represents the robust kernel function. m Robust kernel function for marginalization parameters, Z is the measurement of the sliding window, Z m To marginalize pseudo-measurements, Z i For the sensor measurement at time i, Z p，i For measurements from state transfer sensors, z i|i-1 For parameter transfer models that cannot be measured by state transfer sensors and using R represents the covariance of the measurement, used to assign the weight of the measurement in the system within a given sliding window.
[0023] Preferably, the zero-space dimensionality reduction covariance estimation method includes:
[0024] For a target parameter to be estimated, determine whether there is a state transfer sensor measurement between the target parameters to be estimated. If not, use the dynamic model corresponding to the target parameter to be estimated as the parameter transfer model instead.
[0025] Preferably, the formula for solving the covariance of the parameters of the nonlinear equation is: Where J and R are the global Jacobian matrix and covariance matrix constructed from the Jacobian matrix and covariance matrix of all residuals, and T is the transpose symbol.
[0026] Preferably, the nonlinear equation is linearized to obtain z = Jχ, where Z is a vector composed of all measurements, χ is a vector composed of all parameters to be estimated, and J is the Jacobian matrix.
[0027] Preferably, the formula z = Jχ is divided into two parts: z = J a χa + J b χ b 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 J b The left null space is used to obtain L0z = L0J. a χ a .
[0029] Preferably, use L0z=L0J a χ a The formula for obtaining the covariance of the parameters of the nonlinear equation is as follows:
[0030] The present invention also provides a zero-space dimension reduction covariance estimation system for high-dimensional sequential graph optimization, the high-dimensional sequential graph optimization zero-space dimension reduction covariance estimation system including a GNSS receiver, characterized in that the zero-space dimension reduction covariance estimation system is used to implement the high-dimensional sequential graph optimization zero-space dimension reduction covariance estimation method as described above.
[0031] Based on common knowledge in the field, the above-mentioned preferred conditions can be combined arbitrarily to obtain various preferred embodiments of the present invention.
[0032] The positive and progressive effects of this invention are as follows:
[0033] This invention linearizes the high-dimensional space and maps the covariance problem to a low-dimensional space by obtaining the null space of the high-dimensional parameters, thereby significantly reducing the covariance solution time while ensuring the optimality of the covariance estimation. Attached Figure Description
[0034] Figure 1 is a schematic diagram of the zero-space dimensionality reduction covariance estimation system of Embodiment 1 of the present invention.
[0035] Figure 2 is a schematic diagram of the effect of the zero-space dimensionality reduction covariance estimation system of Embodiment 1 of the present invention.
[0036] Figure 3 is another schematic diagram of the effect of the zero-space dimensionality reduction covariance estimation system of Embodiment 1 of the present invention.
[0037] Figure 4 is a flowchart of the zero-space dimensionality reduction covariance estimation method of Embodiment 1 of the present invention. Detailed Implementation
[0038] The present invention will be further illustrated by way of embodiments below, but the present invention is not limited to the scope of the embodiments described herein.
[0039] Example 1
[0040] This embodiment provides a zero-space dimension reduction covariance estimation system for high-dimensional sequential graph optimization. The zero-space dimension reduction covariance estimation system includes a GNSS receiver, a processing module, and several measurement sensors, including but not limited to an inertial measurement unit (for tilt measurement function), a visual measurement unit, and a driving detection unit.
[0041] The processing module can be a standalone processing terminal, such as a server, desktop computer, laptop computer, tablet computer, or mobile phone.
[0042] The processing module can also be a computing module integrated into the GNSS receiver, such as a central processing unit.
[0043] The measurement sensor is used to acquire measurement data of the sliding window, and the measurement data includes data within the sliding window collected by several sensors;
[0044] The processing module is used to establish a factor graph optimization estimate of the measurement data, and 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 for the covariance of the optimized estimation of the dimension-reduced factor graph.
[0047] The processing module is also used for:
[0048] Obtain the covariance component to be solved from the parameters to be estimated;
[0049] The covariance is solved for the part that needs to be solved for.
[0050] The factor graph optimization estimation is expressed using a nonlinear equation, and the processing module is further used for:
[0051] The nonlinear equation is linearized using a transformation matrix to obtain a linearized equation.
[0052] The linearized equation is divided into a covariance part to be solved and a covariance part that does not need to be solved.
[0053] Obtain the left null space of the transformation matrix corresponding to the covariance component of the solution without requirements;
[0054] The left null space is used to reduce the dimensionality of the parameters to be estimated in the factor graph optimization estimation.
[0055] The nonlinear equation is:
[0056]
[0057] Where χ represents all parameters to be estimated. χ represents the sensor-related parameters to be estimated at time k0, indicating the state update. p To transmit sensor-related parameters for status information, The sensor-related parameters to be estimated at time k0 are transferred to the state at k0, χ. i Update the sensor-related parameters to be estimated based on the state at time i. The sensor-related parameters to be estimated are passed to the state at time i, χ. i-1 To update the sensor-related parameters to be estimated at time i-1, ρ represents the robust kernel function. m Robust kernel function for marginalization parameters, where Z is the measurement of the sliding window, z m To marginalize pseudo-measurements, Zi For the sensor measurement at time i, z p,i For measurements from state transfer sensors, z i|i-1 For parameter transfer models that cannot be measured by state transfer sensors and using R represents the covariance of the measurement, used to assign the weight of the measurement in the system within a given sliding window.
[0058] χ represents all parameters to be estimated. k χ represents the parameter to be estimated at time k. p The parameters related to the state transfer sensor are given, where ρ represents the robust kernel function, Z is the measurement of the sliding window, and Z0 is the value of the sensor. m To marginalize pseudo-measurements, Z i For the sensor measurement at time i, z p,i For measurements from state transfer sensors, z i|i-1 For parameter transfer models that cannot be measured by state transfer sensors and using R represents the covariance of the measurement, used to assign 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 representation of the nonlinear equation is shown in Figure 1.
[0060] χ represents all parameters to be estimated. χ is an estimated value. k χ represents the parameter to be estimated at time k. p The parameters related to the state-transfer sensor are used for state updates. The state-update sensor refers to a sensor that can be used to connect the states at two different times, such as an INS sensor. ρ represents the robust kernel function. The subscript m represents the marginalization factor, which contains prior information outside the sequential sliding window. z represents the measurement, where Z... m This is a marginalized pseudo-measurement, corresponding to m and Z in Figure 1. i For the sensor measurement at time i, corresponding to Figure 1 z p,i Measurements from the state transfer sensor, corresponding to z in Figure 1 p , z i|i-1 For parameter transfer models that cannot be measured by state transfer sensors, the dynamic model of the parameter itself is usually used instead, which corresponds to the one in Figure 1. R 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, determine whether there is a state transfer sensor measurement between the target parameters to be estimated. If not, use the dynamic model corresponding to the target parameter to be estimated as the parameter transfer model instead.
[0063] Specifically, taking the VIO system as an example, let's look at the correspondence between the model in Figure 1 and the actual system. In the VIO system, the state transfer sensor is INS, which contributes to the z-axis in Figure 1. p Measurement, and its corresponding parameter χ p For position, velocity, attitude, acceleration deviation, and angular velocity deviation; correspondingly, Provided by the vision sensor, it is generally the reprojection error of feature points, and its corresponding parameter χ k This represents a landmark (a 2D point in a photograph becomes a feature point, and its corresponding point in the 3D world becomes a landmark). The state transfer model for a landmark is as follows: For this VIO system, the number of landmarks 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 formula for solving the covariance of the parameters of the nonlinear equation is as follows:
[0065] T is the transpose symbol.
[0066] Wherein, J and R are the global Jacobian matrix and covariance matrix constructed from the Jacobian matrix and covariance matrix of all residuals. Each zf(χ) in the nonlinear equation is called a residual. The nonlinear equation is linearized into a linear equation Jx, and the corresponding J is the Jacobian matrix.
[0067] The nonlinear equation is linearized to obtain z = Jχ, where Z is a vector composed of all measurements, χ is a vector composed of all parameters to be estimated, and J is the Jacobian matrix.
[0068] Divide the formula z = Jχ into two parts: z = J a χ a +J b χ b 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, in the VIO system mentioned above, users are generally only concerned with the covariance of position and orientation, but not the covariance of landmarks.
[0070] Find the Jacobian matrix J b The left null space L0 is used to obtain L0z = L0J. a χ a .
[0071] The left zero matrix L0 can be obtained through the Givens transformation. Since this process is a parameter mapping, the optimality of the system will not change before and after the transformation.
[0072] Use L0z=L0J a χ a The formula for obtaining the covariance of the parameters of the nonlinear equation is as follows:
[0073] In the above formula, the dimension of the matrix inversion is χ. a As can be seen, its dimensionality has been greatly reduced, and its solution efficiency will also be greatly improved.
[0074] Referring to Figures 2 and 3, the accuracy and computation time of position and attitude covariance estimation for the GNSS / INS / visual tightly integrated system are presented. Figure 2 shows a performance comparison between the dimensionality reduction method proposed in this embodiment and covariance estimation directly based on high-dimensional graphs, where sub-graph represents the method proposed in this paper.
[0075] Figure 3 shows a comparison of the time consumption of the two methods. It can be seen that the method proposed in this paper significantly reduces the amount of computation. At the same time, its covariance estimation performance is basically consistent with that of estimation by directly using high-dimensional graphs.
[0076] Referring to Figure 4, this embodiment also provides a zero-space dimensionality reduction covariance estimation method using the above-mentioned high-dimensional sequential graph optimized zero-space dimensionality reduction covariance estimation system, including:
[0077] Step 100: Obtain measurement data of the sliding window, the measurement data including data collected by several sensors within the sliding window;
[0078] Step 101: Establish a factor graph optimization estimate of the measurement data, wherein the factor graph optimization estimate includes the parameters to be estimated;
[0079] Step 102: Perform dimensionality reduction processing on the parameters to be estimated;
[0080] Step 103: Solve for the covariance of the optimized estimation of the dimension-reduced factor graph.
[0081] Step 102 includes obtaining the covariance component to be solved from the parameters to be estimated.
[0082] Step 102 includes solving for the covariance of the part to be solved.
[0083] Specifically, the factor graph optimization estimation is expressed using a nonlinear equation, and step 102 specifically includes:
[0084] Step 1021: Linearize the nonlinear equation using a transformation matrix to obtain a linearized equation;
[0085] Step 1022: Divide the linearized equation into a covariance part to be solved and a covariance part that does not need to be solved;
[0086] Step 1023: Obtain the left null space of the transformation matrix corresponding to the covariance part of the solution without requirements;
[0087] Step 1024: Use the left null space to perform dimensionality reduction on the parameters to be estimated in the factor graph optimization estimation.
[0088] The nonlinear equation is:
[0089]
[0090] Where χ represents all parameters to be estimated, χ k χ represents the parameter to be estimated at time k. p The parameters related to the state transfer sensor are ρ, which represents the robust kernel function, Z, and z, respectively. m To marginalize pseudo-measurements, Z i For the sensor measurement at time i, z p,i For measurements from state transfer sensors, z i|i-1 For parameter transfer models that cannot be measured by state transfer sensors and using R represents the covariance of the measurement, used to assign the weight of the measurement in the system within a given sliding window.
[0091] The null-space dimensionality reduction covariance estimation method includes:
[0092] For a target parameter to be estimated, determine whether there is a state transfer sensor measurement between the target parameters to be estimated. If not, use the dynamic model corresponding to the target parameter to be estimated as the parameter transfer model instead.
[0093] The formula for solving the covariance of the parameters of the nonlinear equation is as follows:
[0094] The nonlinear equation is linearized to obtain z = Jχ, where Z is a vector composed of all measurements, χ is a vector composed of all parameters to be estimated, and J is the Jacobian matrix.
[0095] Divide the formula z = Jχ into two parts: z = J a χ a +J b χ b 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 J b The left null space is used to obtain L0z = L0J. a χ a .
[0097] Use L0z=L0J a χ a The formula for obtaining the covariance of the parameters of the nonlinear equation is as follows:
[0098] While specific embodiments of the present invention have been described above, those skilled in the art should understand that these are merely illustrative examples, and the scope of protection of the present invention is defined by the appended claims. Those skilled in the art can make various changes or modifications to these embodiments without departing from the principles and essence of the present invention, but all such changes and modifications fall within the scope of protection of the present invention.
Claims
1. A method for null-space dimensionality reduction and covariance estimation in high-dimensional sequential graph optimization, characterized in that, The null-space dimensionality reduction covariance estimation method includes: Acquire measurement data of a sliding window, the measurement data including data collected by several sensors within the sliding window; Establish a factor graph optimization estimate of the measurement data, wherein the factor graph optimization estimate includes the parameters to be estimated; The parameters to be estimated are then subjected to dimensionality reduction processing; The covariance of the optimized estimation of the dimensionality-reduced factor graph is solved.
2. The method for null-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization as described in claim 1, characterized in that, The null-space dimensionality reduction covariance estimation method includes: Obtain the covariance component to be solved from the parameters to be estimated; The covariance is solved for the part that needs to be solved for.
3. The method for null-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization as described in claim 1, characterized in that, The factor graph optimization estimation is expressed using a nonlinear equation, and the dimensionality reduction processing of the parameters to be estimated includes: The nonlinear equation is linearized using a transformation matrix to obtain a linearized equation. The linearized equation is divided into a covariance part to be solved and a covariance part that does not need to be solved. Obtain the left null space of the transformation matrix corresponding to the covariance component of the solution without requirements; The left null space is used to reduce the dimensionality of the parameters to be estimated in the factor graph optimization estimation.
4. The method for null-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization as described in claim 3, characterized in that, The nonlinear equation is: Where χ represents all parameters to be estimated. χ represents the sensor-related parameters to be estimated at time k0, indicating the state update. p To transmit the sensor-related parameters to be estimated for the state, The sensor-related parameters to be estimated at time k0 are transferred to the state at k0, χ. i Update the sensor-related parameters to be estimated based on the state at time i. The sensor-related parameters to be estimated are passed to the state at time i, χ. i-1 To update the sensor-related parameters to be estimated at time i-1, ρ represents the robust kernel function. m Robust kernel function for marginalization parameters, where Z is the measurement of the sliding window, z m To marginalize pseudo-measurements, z i For the sensor measurement at time i, z p,i For measurements from state transfer sensors, z i|i-1 For parameter transfer models that cannot be measured by state transfer sensors and using R represents the covariance of the measurement, used to assign the weight of the measurement in the system within a given sliding window.
5. The null-space dimensionality reduction covariance estimation method for high-dimensional sequential graph optimization as described in claim 4, characterized in that, The null-space dimensionality reduction covariance estimation method includes: For a target parameter to be estimated, determine whether there is a state transfer sensor measurement between the target parameters to be estimated. If not, use the dynamic model corresponding to the target parameter to be estimated as the parameter transfer model instead.
6. The method for null-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization as described in claim 4, characterized in that, The formula for solving the covariance of the parameters of the nonlinear equation is as follows: Where J and R are the global Jacobian matrix and covariance matrix constructed from the Jacobian matrix and covariance matrix of all residuals, and T is the transpose symbol.
7. The method for null-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization as described in claim 6, characterized in that, The nonlinear equation is linearized to obtain z = Jχ, where Z is a vector composed of all measurements, χ is a vector composed of all parameters to be estimated, and J is the Jacobian matrix.
8. The method for null-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization as described in claim 7, characterized in that, Divide the formula z = Jχ into two parts: z = J a χ a +J b χ b 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 J b The left null space is used to obtain L0z = L0J. a χ a .
9. The method for null-space dimensionality reduction and covariance estimation of high-dimensional sequential graph optimization as described in claim 8, characterized in that, Use L0z=L0J a χ a The formula for obtaining the covariance of the parameters of the nonlinear equation is as follows:
10. 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 high-dimensional sequential graph optimization null-space dimensionality reduction covariance estimation method as described in any one of claims 1 to 9.