Vehicle spatial position uncertainty modeling method based on factor graph

Abstract

The invention discloses a vehicle spatial position uncertainty modeling method based on a factor graph, and relates to the technical field of automatic driving. Establishing a factor graph model, and constructing state variable nodes and factor nodes; an objective function of the factor graph optimization problem is converted into a nonlinear least square problem, and optimal state estimation is obtained through iterative solution by adopting a Gauss-Newton method; on the basis of an objective function under a Gaussian Newton method framework, a posterior covariance matrix is obtained through calculation of a Hesse matrix and inversion; performing incremental optimization on the factor graph, and updating a global Jacobian matrix through matrix decomposition and Givens transformation; and carrying out confidence ellipse geometric quantization, and representing the positioning uncertainty in a geometric form. Based on a factor graph and a confidence ellipse theory, through multi-sensor space-time constraint fusion and geometric interpretable representation, a vehicle positioning uncertainty modeling framework is constructed, and the safety and robustness of an automatic driving system downstream decision planning algorithm are improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, specifically a method for modeling vehicle spatial location uncertainty based on factor graphs. Background Technology

[0002] In recent years, with the rapid development of autonomous driving technology, the impact of positioning uncertainty on vehicle decision-making and planning has become increasingly prominent. Factors such as error coupling of multi-source sensors (such as Global Navigation Satellite System (GNSS) and Inertial Measurement Unit (IMU), environmental interference, and long-term accumulated drift cause positioning errors to exhibit nonlinear propagation characteristics, leading to covariance matrix expansion and state estimation distortion, which seriously threatens driving safety in complex dynamic scenarios.

[0003] To address this problem, the mainstream methods for estimating positioning uncertainty currently include: linearization methods based on Kalman filtering (such as Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF)), nonparametric methods based on particles (such as particle filtering), and probabilistic models based on grid maps. However, these approaches share the following common drawbacks:

[0004] 1. The error propagation mechanism is inadequate, and the spatiotemporal fusion characteristics of multi-sensor errors are not fully modeled;

[0005] 2. Limited dynamic adaptability, weak incremental update capability, difficulty in coping with long-term operation scenarios, insufficient compatibility of results with the planning layer, and lack of geometric interpretability support for downstream planning modules.

[0006] Therefore, there is an urgent need for an efficient modeling method that can accurately quantify the nonlinear propagation of positioning uncertainty in order to support robust decision-making in autonomous driving systems in dynamic environments. Summary of the Invention

[0007] To address the shortcomings of the prior art, this invention provides a method for modeling vehicle spatial location uncertainty based on factor graphs. It is based on factor graphs and confidence ellipse theory, and is applicable to the estimation of positioning uncertainty for both the vehicle itself and other vehicles in the environment. By fusing multi-sensor spatiotemporal constraints and using geometrically interpretable representation, a vehicle positioning uncertainty modeling framework is constructed to solve the problems of uncontrolled error propagation and fragmentation of the planning layer, thereby improving the safety and robustness of downstream decision-making and planning algorithms in autonomous driving systems.

[0008] To achieve the above objectives, the present invention adopts the following technical solution: a method for modeling vehicle spatial position uncertainty based on factor graphs, comprising the following steps:

[0009] Step 1: Establish a factor graph model, construct a temporal pose node chain as state variable nodes, and construct factor nodes containing prior factors and observation factors. The observation factors are based on observation data from multiple sources of sensors, and the uncertainty in the observations of the prior factors and observation factors follows a Gaussian distribution.

[0010] Step 2: Solving for the optimal estimate. The objective function of the factor graph optimization problem is transformed into a nonlinear least squares problem, and the optimal state estimate is obtained by iteratively solving the problem using the Gauss-Newton method.

[0011] Step 3: Solving the covariance matrix. Based on the objective function under the Gauss-Newton method framework, the posterior covariance matrix is ​​obtained by calculating the Hess matrix and inverting it. The diagonal elements of this matrix represent the position variance, and the off-diagonal elements represent the error correlation between pose components.

[0012] Step 4: Incremental factor graph optimization. When new sensor data is input, incremental smoothing and mapping algorithms are used to incrementally optimize the factor graph. The global Jacobian matrix is ​​updated through matrix decomposition and Givens transformation.

[0013] Step 5: Confidence Ellipse Geometric Quantization. Eigenvalue decomposition is performed on the covariance matrix corresponding to the current position component. The parameters of the confidence ellipse, including the rotation angle, semi-major axis, and semi-minor axis, are determined based on the eigenvalues ​​and eigenvectors to geometrically characterize the positioning uncertainty.

[0014] Furthermore, in step one, the temporal pose node chain ,in, The total number of state variables. The index represents the position of an autonomous vehicle at a given moment. Indicates a time index;

[0015] The definition of a factor node is:

[0016]

[0017]

[0018] In the formula, Represents factor nodes, For factor index, Represents a subset of states associated with factor nodes, satisfying , Represents the error function. The function representing the mapping from state to observation. This represents the actual observations associated with the factor nodes. This represents the covariance.

[0019] Furthermore, in step two, when the objective function is transformed into a nonlinear least squares problem, taking the negative log-likelihood form, the objective function becomes:

[0020]

[0021] The Gauss-Newton method obtains the state increment by iteratively linearizing the error function and solving a local linear least squares problem until the convergence condition is met.

[0022] Furthermore, in step three, the Hessian matrix ignores error functions. The complex term of the second derivative is expressed as:

[0023]

[0024] In the formula, Represents the global Jacobian matrix. Let the block diagonal weight matrix and the posterior covariance matrix represent the values ​​of the two matrices. .

[0025] Furthermore, in step four, the update process of the global Jacobian matrix is ​​as follows: when a new factor node is added to the factor graph, the global Jacobian matrix... Append new line Forming an augmented matrix: The matrix is ​​transformed into a new upper triangular matrix through QR decomposition and Givens transformation, thus achieving incremental update.

[0026] Furthermore, in step five, the eigenvalue decomposition of the covariance matrix is ​​expressed as: ,in, It is an eigenvalue diagonal matrix. Let be a rotation matrix composed of orthogonal eigenvectors. and These are the eigenvalues ​​of the covariance matrix. and These are the eigenvectors of the covariance matrix;

[0027] Rotation angle of the ellipse From the largest eigenvalue Corresponding main feature vector It is confirmed that the major and minor semi-axes of the ellipse are respectively... and ,in, The chi-square distribution at a significance level of 1 The critical value below.

[0028] Compared with existing technologies, the beneficial effects of this invention are as follows: By constructing a factor graph model that integrates spatiotemporal constraints from multiple sensors, this invention effectively solves the problem of insufficient estimation of vehicle multi-sensor positioning uncertainty in traditional modeling methods. It utilizes an incremental optimization algorithm to dynamically update the pose covariance matrix, significantly reducing computational complexity while maintaining accuracy and improving the real-time performance of high-frequency asynchronous fusion of multi-source heterogeneous sensor data. This invention also employs confidence ellipse theory to achieve geometric quantification of uncertainty: by performing eigenvalue decomposition on the positioning covariance matrix, the abstract probability distribution is transformed into a two-dimensional geometric boundary with clear physical meaning through a confidence ellipse. This representation method breaks through the limitations of traditional covariance matrices or probability grids. The major axis of the ellipse intuitively reflects the direction of maximum error propagation, the minor axis represents the dimension of minimum uncertainty, and the rotation angle reflects the direction of maximum uncertainty in real time. In dynamic operating scenarios where sensors are interfered with, the system characterizes positioning reliability in real time through the morphological changes of the confidence ellipse. This quantification capability provides an adaptive uncertainty perception basis for autonomous driving systems, enabling downstream modules to dynamically evaluate positioning reliability based on ellipse parameters, and establishing mathematically interpretable input conditions for highly robust decision-making. Attached Figure Description

[0029] Figure 1 This is a flowchart of the method of the present invention;

[0030] Figure 2 This is the factor graph model in the embodiment. Detailed Implementation

[0031] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the invention, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention.

[0032] In multi-sensor fusion localization for autonomous driving, problems exist such as strong spatiotemporal fusion of localization errors leading to covariance expansion, difficulty in directly supporting planning and decision-making with uncertainty representation, and low efficiency in fusing multi-source heterogeneous data. This invention, based on a factor graph optimization method, constructs an incremental factor graph model with spatiotemporal constraints for multi-sensor fusion to estimate the localization uncertainty of the vehicle and other vehicles in the environment. Furthermore, it combines confidence ellipse theory to transform the covariance matrix into a geometric representation, quantifying the statistical distribution characteristics of the localization error. This provides an interpretable spatial representation of uncertainty for the downstream decision-making and planning layer of the autonomous driving system, supporting dynamic adjustment of safety margins and robust trajectory optimization.

[0033] like Figure 1 As shown, a method for modeling vehicle spatial location uncertainty based on factor graphs includes the following steps:

[0034] Step 1: Establish a factor graph model;

[0035] The factor graph mainly consists of state variable nodes and factor nodes. A factor graph model fusing multi-sensor data is constructed to address the spatiotemporal coupling problem of errors. First, state variable nodes are constructed, representing the state variables that need to be optimized. Then, a temporal pose node chain is established. ,in, The total number of state variables. The index represents the position of an autonomous vehicle at a given moment. This represents the time index. Next, factor nodes are constructed, representing observational or prior information related to the state variable. Prior factors. Prior information representing the initial pose of an autonomous vehicle, observation factors This represents observation data obtained from multiple sensors.

[0036] Assuming that the uncertainties in the observations of both the prior and observed factors follow a Gaussian distribution, each factor can be defined using an exponential function, we obtain:

[0037]

[0038]

[0039] In the formula, The term "factor node" has a physical meaning determined by the type of sensor observation. This serves as a factor index, used to identify unique sensor constraints. Represents a subset of states associated with factor nodes, satisfying , This represents the error function, which is the error between the observed value and the true value. The function representing the mapping from state to observation. This represents the actual observations associated with the factor nodes. This represents the covariance.

[0040] Step 2: Solving for the optimal estimate;

[0041] For the factor graph optimization problem, the objective function is expressed as:

[0042]

[0043] In the formula, This represents the joint probability density of the factors in the factor graph.

[0044] To facilitate numerical solutions, we adopt the negative log-likelihood form, transforming the objective function into a nonlinear least squares problem:

[0045]

[0046] This solution process transforms the joint probability maximization problem into a nonlinear least squares optimization problem. The Gauss-Newton method handles the nonlinear problem through an iterative linearization strategy, locally approximating the complex nonlinear problem as a linear least squares problem to gradually approach the optimal estimate. Set initial state estimation Typically, the optimal estimate obtained at the previous time step is used. The error function is adjusted at the current estimation point. Perform a first-order Taylor expansion:

[0047]

[0048] In the formula, Indicates the first State estimation for the next iteration. Indicates the state increment. This represents the Jacobian matrix of the error function with respect to the state variables, where the row indices correspond to the factor indices. , This represents the higher-order remainder term.

[0049] Substituting the objective function and ignoring higher-order terms, we obtain the local linear least squares problem:

[0050]

[0051] Rewritten in matrix form:

[0052]

[0053] In the formula, Represents the global Jacobian matrix. This represents the error vector.

[0054] Calculate the state increment using the normal equation:

[0055]

[0056] A Jacobian matrix with full column rank can be decomposed using Cholesky's method:

[0057]

[0058] In the formula, It is an upper triangular matrix.

[0059] Solve the lower trigonometric system of equations It can be obtained Substitute into the upper triangular equation system The state increment can be obtained. Update the state variables. Repeat the above process until the convergence condition is met or the maximum number of iterations is reached.

[0060] Step 3: Solve for the covariance matrix;

[0061] Since the factors in the factor graph are assumed to be Gaussian distributed, the objective function is defined as the negative logarithm of the joint probability:

[0062]

[0063] In the formula, Represents a constant.

[0064] In solving the least squares problem using the Gauss-Newton method, the gradient of the objective function is expressed as:

[0065]

[0066] The Hessian matrix is ​​obtained by differentiating the gradient:

[0067]

[0068] In the formula, .

[0069] Within the framework of Gauss-Newton's method, for computational efficiency, the error function involved in the Hessian matrix is ​​usually ignored. The complex term of the second derivative is obtained as follows:

[0070]

[0071] In the formula, This represents the block diagonal weight matrix.

[0072] Through the and By performing partial differentiation, we obtain the Hessian matrix. Quantity:

[0073]

[0074] In the formula, Let be the posterior covariance matrix.

[0075] Since the higher-order derivatives of a quadratic function are constants, the Hessian matrix as a whole satisfies:

[0076]

[0077] Depend on The inversion yields the posterior covariance matrix. Its diagonal elements represent position variance, and off-diagonal elements represent error correlation between pose components.

[0078] Step 4: Incremental factor graph optimization;

[0079] The core of the Incremental Smoothing And Mapping (iSAM) algorithm lies in leveraging the properties of matrix factorization (usually QR factorization) to achieve efficient incremental updates. It avoids redundant computations through a local update strategy and eliminates the need to reconstruct the global Jacobian matrix. And then decompose it again.

[0080] During the operation of an autonomous vehicle, when sensors receive new data and new factor nodes are added to the factor graph, the global Jacobian matrix... Additional bank accounts are required. This forms an augmented matrix:

[0081]

[0082] To reduce computational cost, the iSAM method is used for incremental optimization of the factor graph:

[0083]

[0084] The Givens transform can be used to append new rows to a matrix. Transform into a new upper triangular matrix. Because the matrix... Since it's an upper triangular matrix, only the new rows need to be processed. ,right Every non-zero element ( Construct Givens rotation matrix The goal is to Reset to zero. Let the position to be eliminated be... ( The (column diagonal elements) and Calculate the rotation angle:

[0085]

[0086]

[0087] Generate a 2×2 Givens matrix:

[0088]

[0089] Givens rotation matrix Acting on contained matrices No. Bank and New Bank 2×n submatrix:

[0090]

[0091] Each rotation only affects the matrix The rows and new rows After n rotations, Converted to zero rows, the matrix form is updated as follows:

[0092]

[0093] in, This is the updated upper triangular matrix.

[0094] Accumulate all Givens rotations to obtain the total orthogonal transformation. :

[0095]

[0096] Final Incremental QR Decomposition Results .

[0097] Step 5: Confidence Elliptic Geometric Quantization;

[0098] Based on the optimal estimation covariance matrix Extract the covariance matrix corresponding to the current position component. Eigenvalue decomposition is performed on it:

[0099]

[0100] in, It is an eigenvalue diagonal matrix. Let be a rotation matrix composed of orthogonal eigenvectors. and These are the eigenvalues ​​of the covariance matrix. and These are the eigenvectors of the covariance matrix. The largest eigenvalue. Corresponding feature vector Principal eigenvectors, principal eigenvectors The direction determines the rotation angle of the ellipse. , , and for exist The directional component.

[0101] The confidence ellipse is an abstract representation of the covariance matrix. A key tool for transforming into a concrete, understandable two-dimensional geometric boundary. For the covariance matrix obtained from factor graph optimization, confidence ellipses are used to geometrically quantify the positioning uncertainty. A confidence ellipse can be represented as:

[0102]

[0103] in, Let the coordinates of a point on the confidence ellipse be given. The optimal estimate of the current position. The chi-square distribution at a significance level of 1 The critical value below.

[0104] The calculation of the semi-axis length of the ellipse needs to be combined with a preset confidence probability. For a two-dimensional Gaussian distribution, the probability of the true value falling within the elliptical region is determined. Determined by the chi-square distribution, i.e., satisfying The major and minor axes of the ellipse are respectively and For example, when choosing a 95% confidence level, , At this time, the major semi-axis and the minor semi-axis are respectively and Different confidence values ​​can be selected to meet security requirements depending on the scenario.

[0105] Example

[0106] This embodiment selects an urban road simulation test of an autonomous vehicle. The vehicle is equipped with GNSS and IMU sensors. The total test mileage is 1.2 kilometers long, with a total of 3615 simulation frames. The sensor response frequency is 10 Hz (10 sensor data outputs per second, i.e., 10 frames). Frame 1180 is selected as an example for illustration, which can effectively demonstrate the superiority of the method of the present invention.

[0107] Combination Figure 1 As shown, the method of the present invention is implemented according to the following process:

[0108] Step 1: Establish a factor graph model;

[0109] Based on the types of sensors equipped in autonomous vehicles, a factor graph model is first established, combined with... Figure 2 As shown.

[0110] Define state variables and construct a temporal pose node chain. , The index represents the position of an autonomous vehicle at a given moment. Indicates a time index. .

[0111] Construct factor nodes for different sensor data types, prior factors Initial pose of autonomous vehicle covariance IMU factor IMU accelerometer covariance Displacement ,in The velocity is obtained by integrating the IMU acceleration. The acceleration obtained from the IMU With an inter-frame time interval of 0.1s, the displacement increment was calculated. GNSS factor Due to the obstruction of buildings, the observed value is covariance .

[0112] Step 2: Solving for the optimal estimate;

[0113] Based on the established factor graph model, the objective function is constructed, and the joint probability density in negative logarithmic form is transformed into a least squares problem:

[0114]

[0115] The error functions are as follows:

[0116]

[0117]

[0118]

[0119] The initial value for the Gauss-Newton method iteration uses the best estimate from the previous frame. .

[0120] Calculate the Jacobian matrix. , , The partial derivatives of the factor nodes are added to the global Jacobian matrix and updated.

[0121] Convergence was achieved after 3 iterations. Optimal estimate. The deviation from the true value is .

[0122] Step 3: Solve for the covariance matrix;

[0123] Add the covariance of the factors to the weight matrix. In the above, calculate the Hessian matrix:

[0124] The posterior covariance is calculated based on the Hessian matrix. ,from extract covariance matrix :

[0125]

[0126]

[0127] Step 4: Incremental factor graph optimization;

[0128] When the new sensor data arrives in frame 1181 , , Augmented Jacobian matrix:

[0129]

[0130] Using iSAM and Givens rotational updates, for Construct a rotation matrix for each column. Update the upper triangular matrix .

[0131] Step 5: Confidence Elliptic Geometric Quantization;

[0132] For the covariance matrix Perform eigenvalue decomposition, and the eigenvalues ​​obtained after eigenvalue decomposition are as follows: , .

[0133] With a confidence level of 95% and a chi-square distribution critical value of 5.991, calculate the parameters of the confidence ellipse:

[0134] Long half shaft short half-shaft .

[0135] By incorporating the parameters of the confidence ellipse into the downstream decision-making and planning system, security and robustness support can be provided for decision-making and planning.

[0136] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above, and that the invention can be implemented in other forms without departing from its spirit or essential characteristics. Therefore, the embodiments should be considered illustrative and non-limiting in all respects, and the scope of the invention is defined by the appended claims rather than the foregoing description. Thus, all variations falling within the meaning and scope of the equivalents of the claims are intended to be included within the present invention. No reference numerals in the claims should be construed as limiting the scope of the claims.

[0137] Furthermore, it should be understood that although this specification describes embodiments, not every embodiment contains only one independent technical solution. This narrative style is merely for clarity. Those skilled in the art should consider the specification as a whole, and the technical solutions in each embodiment can also be appropriately combined to form other embodiments that can be understood by those skilled in the art.

Claims

1. A method for modeling vehicle spatial location uncertainty based on factor graphs, characterized in that: Includes the following steps: Step 1: Establish a factor graph model, construct a temporal pose node chain as state variable nodes, and simultaneously construct factor nodes containing prior factors and observed factors. The observation factor is based on observation data from multiple sensors, and the uncertainty in the observations of the prior factor and the observation factor follows a Gaussian distribution. Step 2: Solving for the optimal estimate. The objective function of the factor graph optimization problem is transformed into a nonlinear least squares problem, and the optimal state estimate is obtained by iteratively solving the problem using the Gauss-Newton method. Step 3: Solving the covariance matrix. Based on the objective function under the Gauss-Newton method framework, the posterior covariance matrix is ​​obtained by calculating the Hess matrix and inverting it. The diagonal elements of this matrix represent the position variance, and the off-diagonal elements represent the error correlation between pose components. Step 4: Incremental factor graph optimization. When new sensor data is input, incremental smoothing and mapping algorithms are used to incrementally optimize the factor graph. The global Jacobian matrix is ​​updated through matrix decomposition and Givens transformation. Step 5: Confidence Ellipse Geometric Quantization. Eigenvalue decomposition is performed on the covariance matrix corresponding to the current position component. The parameters of the confidence ellipse, including the rotation angle, semi-major axis, and semi-minor axis, are determined based on the eigenvalues ​​and eigenvectors to geometrically characterize the positioning uncertainty.

2. The method for modeling vehicle spatial position uncertainty based on factor graphs according to claim 1, characterized in that: In step one, the temporal pose node chain ,in, The total number of state variables. The index represents the position of an autonomous vehicle at a given moment. Indicates a time index; The definition of a factor node is: In the formula, Represents factor nodes, For factor index, Represents a subset of states associated with factor nodes, satisfying , Represents the error function. The function representing the mapping from state to observation. This represents the actual observations associated with the factor nodes. This represents the covariance.

3. The method for modeling vehicle spatial position uncertainty based on factor graphs according to claim 2, characterized in that: In step two, when the objective function is transformed into a nonlinear least squares problem, taking the negative log-likelihood form, the objective function becomes: The Gauss-Newton method obtains the state increment by iteratively linearizing the error function and solving a local linear least squares problem until the convergence condition is met.

4. The method for modeling vehicle spatial position uncertainty based on factor graphs according to claim 3, characterized in that: In step three, the Hessian matrix ignores error functions. The complex term of the second derivative is expressed as: In the formula, Represents the global Jacobian matrix. Let the block diagonal weight matrix and the posterior covariance matrix represent the values ​​of the two matrices. .

5. The method for modeling vehicle spatial position uncertainty based on factor graphs according to claim 4, characterized in that: In step four, the global Jacobian matrix is ​​updated as follows: when a new factor node is added to the factor graph, the global Jacobian matrix... Append new line Forming an augmented matrix: The matrix is ​​transformed into a new upper triangular matrix through QR decomposition and Givens transformation, thus achieving incremental update.

6. The method for modeling vehicle spatial position uncertainty based on factor graphs according to claim 5, characterized in that: In step five, the eigenvalue decomposition of the covariance matrix is ​​expressed as: ,in, It is an eigenvalue diagonal matrix. Let be a rotation matrix composed of orthogonal eigenvectors. and These are the eigenvalues ​​of the covariance matrix. and These are the eigenvectors of the covariance matrix; Rotation angle of the ellipse From the largest eigenvalue Corresponding main feature vector It is confirmed that the major and minor semi-axes of the ellipse are respectively... and ,in, The chi-square distribution at a significance level of 1 The critical value below.

Images