Multi-sensor pose estimation method and apparatus considering perception degradation

Abstract

This application discloses a multi-sensor pose estimation method and apparatus considering perceptual degradation. The method includes: performing distortion correction on LiDAR point cloud data using initial IMU pose variables; constructing a point-to-plane loss function based on the distorted point cloud data and the initial IMU pose variables, and performing singular value decomposition on the initial Hessian matrix to obtain rotation and translation feature vectors; calculating a joint contribution vector and a strong shared vector based on the two feature vectors and a preset information matrix; using a decision tree to perform perceptual degradation classification prediction based on the two vector parameters to obtain degradation detection results; and solving the point-to-plane loss function using an iterative Kalman filter method based on additional constraints constructed from the degradation detection results to obtain optimized pose variables. This application addresses the technical problem that existing technologies require parameter adjustment for specific environments and ignore system influences, resulting in limited applicability and poor performance of pose estimation considering perceptual degradation.

Description

Technical Field

[0001] This application relates to the field of localization and mapping technology, and in particular to a multi-sensor pose estimation method and apparatus that takes into account perceptual degradation. Background Technology

[0002] Compared to visual sensors, LiDAR offers advantages such as longer sensing range, higher reliability, and immunity to illumination changes. LiDAR odometry typically uses the Iterative Closest Point (ICP) algorithm for point cloud registration to accurately estimate pose. While ICP and its variants are among the most popular registration methods to date, their limitations must be considered in practical applications. ICP suffers from four distinct sources of error: the risk of convergence to local minima, sensitivity to sensor bias and noise, inaccurate initial pose transformations, and a lack of sufficient geometric constraints in the underlying optimization problem. Although methods and systems have been developed to mitigate the first three sources of error, the last source can still lead to odometry failure in complex environments. For example, in near-self-symmetric environments, geometric constraints along the axis of symmetry are almost indistinguishable from noise. Therefore, the ICP algorithm may converge to a noise-induced optimal solution during optimization; such environments are called degenerate environments. To enable robots to operate in various scenarios, including degenerate environments, a two-tiered approach is needed: first, to examine the degenerate condition, and second, to mitigate its negative impact on optimization.

[0003] Degradation detection methods mainly include geometry-based methods, optimization-based methods, and learning-based methods. Geometry-based methods analyze the quality of pose estimation by leveraging the relationship between the loss function of point cloud registration and the environment. Optimization-based methods construct a metric based on the inherent properties of the optimization problem, measuring whether degradation has occurred by comparing it to a set threshold. Learning-based methods provide an alternative to traditional degradation detection approaches through data-driven methods.

[0004] Existing degradation detection methods require parameter adjustments for specific environments, making it difficult to adapt to the perception degradation detection needs of different scenarios. Moreover, most current pose estimation methods only analyze radar odometry, ignoring the overall system impact, resulting in poor pose estimation performance that actually considers perception degradation. Summary of the Invention

[0005] This application provides a multi-sensor pose estimation method and apparatus that takes into account perceptual degradation, in order to solve the technical problem that the prior art requires parameter adjustment for specific environments and ignores the influence of the system, resulting in limited applicability and poor performance of pose estimation that takes into account perceptual degradation.

[0006] In view of this, the first aspect of this application provides a multi-sensor pose estimation method that considers perceptual degradation, including:

[0007] Distortion-free point cloud data is obtained by performing distortion removal operation on LiDAR point cloud data using IMU initial pose variables, wherein the IMU initial pose variables are obtained by optimization based on backend measurement parameters;

[0008] Based on the distortion-reduced point cloud data and the initial pose variables of the IMU, a point-to-plane loss function is constructed, and singular value decomposition is performed on the initial Hessian matrix in the point-to-plane loss function to obtain rotation feature vectors and translation feature vectors.

[0009] Calculate the joint contribution vector and the strong sharing vector based on the rotation feature vector, the translation feature vector, and the preset information matrix, respectively.

[0010] A decision tree is used to perform perceptual degradation classification prediction based on the joint contribution vector and the strong shared vector to obtain degradation detection results, which include partial degradation and complete degradation.

[0011] Under the constraints of additional constraints constructed based on the degradation detection results, the point-to-plane loss function is solved by the iterative Kalman filter method to obtain the optimized pose variables.

[0012] Preferably, the step of performing distortion correction on the LiDAR point cloud data using the initial pose variables of the IMU to obtain distorted point cloud data further includes:

[0013] The backend GNC attitude measurement parameters are used to adjust the frontend IMU pre-integration calculated based on the discrete kinematics model, thereby obtaining the initial pose variables and IMU pre-integration factors of the IMU.

[0014] Preferably, the step of calculating the joint contribution vector and the strong sharing vector based on the rotation feature vector, the translation feature vector, and the preset information matrix respectively includes:

[0015] The preset information matrix is ​​projected onto the feature space to which the rotation feature vector and the translation feature vector belong to obtain the localizable contribution parameter;

[0016] The joint contribution vector and the strong shared vector are calculated based on the locationable contribution parameters and the preset radar parameters, respectively.

[0017] Preferably, the step of calculating the joint contribution vector and the strong sharing vector based on the rotation feature vector, the translation feature vector, and the preset information matrix respectively further includes:

[0018] The rotation information matrix and translation information matrix are calculated based on the distortion-reduced point cloud data using the point-normal vector method to obtain the preset information matrix.

[0019] Preferably, the step of solving the point-to-plane loss function using an iterative Kalman filter method to obtain the optimized pose variables under the constraints of additional constraints constructed based on the degradation detection results includes:

[0020] A constraint Hessian matrix is ​​generated based on the category of the degradation detection result, and a constraint matrix equation is constructed based on the constraint Hessian matrix to obtain additional constraint conditions.

[0021] The point-to-plane loss function and the constraint matrix equation are combined using an iterative Kalman filter method, and the optimized pose variables under the additional constraints are solved.

[0022] Preferably, the step of solving the point-to-plane loss function using an iterative Kalman filter method to obtain the optimized pose variables under the constraints of additional constraints constructed based on the degradation detection results further includes:

[0023] The target pose graph is obtained by constructing a pose graph on the backend based on the optimized pose variables and various sensor parameters.

[0024] A second aspect of this application provides a multi-sensor pose estimation apparatus that takes into account perceptual degradation, including:

[0025] The data distortion optimization unit is used to perform distortion removal operation on LiDAR point cloud data through IMU initial pose variables to obtain distorted point cloud data. The IMU initial pose variables are optimized based on backend measurement parameters.

[0026] The matrix decomposition calculation unit is used to construct a point-to-plane loss function based on the distortion-reduced point cloud data and the initial pose variables of the IMU, and to perform singular value decomposition on the initial Hessian matrix in the point-to-plane loss function to obtain rotation feature vectors and translation feature vectors.

[0027] The feature parameter calculation unit is used to calculate the joint contribution vector and the strong sharing vector based on the rotation feature vector, the translation feature vector and the preset information matrix, respectively.

[0028] The degradation classification prediction unit is used to perform perceptual degradation classification prediction using a decision tree based on the joint contribution vector and the strong shared vector to obtain degradation detection results, which include partial degradation and complete degradation.

[0029] The optimized pose solving unit is used to solve the point-to-plane loss function through an iterative Kalman filter method under the constraints of additional constraints constructed based on the degradation detection results, so as to obtain the optimized pose variables.

[0030] Preferably, it further includes:

[0031] The pre-integration calculation unit is used to adjust the IMU pre-integration calculated by the front end based on the discrete kinematics model using the back-end GNC attitude measurement parameters, so as to obtain the initial pose variables of the IMU and the IMU pre-integration factor.

[0032] Preferably, the feature parameter calculation unit is specifically used for:

[0033] The preset information matrix is ​​projected onto the feature space to which the rotation feature vector and the translation feature vector belong to obtain the localizable contribution parameter;

[0034] The joint contribution vector and the strong shared vector are calculated based on the locationable contribution parameters and the preset radar parameters, respectively.

[0035] Preferably, the optimized pose solving unit is specifically used for:

[0036] A constraint Hessian matrix is ​​generated based on the category of the degradation detection result, and a constraint matrix equation is constructed based on the constraint Hessian matrix to obtain additional constraint conditions.

[0037] The point-to-plane loss function and the constraint matrix equation are combined using an iterative Kalman filter method, and the optimized pose variables under the additional constraints are solved.

[0038] As can be seen from the above technical solutions, the embodiments of this application have the following advantages:

[0039] This application provides a multi-sensor pose estimation method considering perceptual degradation, comprising: performing distortion correction on LiDAR point cloud data using initial IMU pose variables to obtain distorted point cloud data, wherein the initial IMU pose variables are optimized based on backend measurement parameters; constructing a point-to-plane loss function based on the distorted point cloud data and the initial IMU pose variables, and performing singular value decomposition on the initial Hessian matrix in the point-to-plane loss function to obtain rotation feature vectors and translation feature vectors; calculating a joint contribution vector and a strong shared vector based on the rotation feature vectors, translation feature vectors, and a preset information matrix, respectively; using a decision tree to perform perceptual degradation classification prediction based on the joint contribution vectors and the strong shared vectors to obtain degradation detection results, which include partial degradation and complete degradation; and solving the point-to-plane loss function using an iterative Kalman filter method under the constraints of additional constraints constructed based on the degradation detection results to obtain optimized pose variables.

[0040] The multi-sensor pose estimation method considering perceptual degradation provided in this application optimizes the results of the front-end IMU pre-integration calculation using back-end measurement parameters to obtain the initial IMU pose variables. This process considers the parameter influence between the front and back ends, ensuring that subsequent degradation detection analysis is approached from a system perspective and avoiding local limitations in the analysis process. Moreover, degradation analysis based on the calculated feature parameters is unaffected by the environment, allowing parameters to be pre-adjusted to their optimal state, avoiding the impact of environmental changes. Furthermore, additional constraints are constructed based on the degradation detection results, thereby constraining the solution process of the point-to-plane loss function in the registration process. This ensures that the registration results consider the impact of perceptual degradation, making the predicted and optimized pose variables more accurate and reliable, and more robust to perceptual degradation. Therefore, this application addresses the technical problem that existing technologies require parameter adjustment for specific environments and ignore system influences, resulting in limited applicability and poor performance of pose estimation methods considering perceptual degradation. Attached Figure Description

[0041] Figure 1 A flowchart illustrating the multi-sensor pose estimation method considering perceptual degradation provided in an embodiment of this application;

[0042] Figure 2 A schematic diagram of the structure of a multi-sensor pose estimation device considering perceptual degradation provided in an embodiment of this application;

[0043] Figure 3 Example diagram of IMU pre-integration and lidar data acquisition time nodes provided in the embodiments of this application;

[0044] Figure 4 A schematic diagram of the degradation classification prediction process based on decision trees provided in an embodiment of this application;

[0045] Figure 5 This is a schematic diagram illustrating the overall working mechanism of the multi-sensor pose graph construction system considering perceptual degradation, as provided in the embodiments of this application. Detailed Implementation

[0046] To enable those skilled in the art to better understand the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present application, and not all embodiments. Based on the embodiments in the present application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of the present application.

[0047] For easier understanding, please refer to Figure 1 The embodiments of the multi-sensor pose estimation method considering perceptual degradation provided in this application include:

[0048] Step 101: Perform distortion correction on the LiDAR point cloud data using the IMU initial pose variables to obtain distorted point cloud data. The IMU initial pose variables are obtained by optimizing the backend measurement parameters.

[0049] Furthermore, step 101, preceding the following, also includes:

[0050] The backend GNC attitude measurement parameters are used to adjust the frontend IMU pre-integration calculated based on the discrete kinematics model, thereby obtaining the initial pose variables and IMU pre-integration factors of the IMU.

[0051] It should be noted that, in order to study and analyze perception degradation from the perspective of the entire localization and mapping system, this embodiment considers the influence of backend measurement parameters. Therefore, by adjusting the backend GNC attitude measurement parameters to adjust the frontend IMU pre-integration based on the discrete kinematics model, the optimized pose variables, i.e., the initial IMU pose variables, can be obtained. This process also yields the IMU pre-integration factor, which can also be input into the backend to optimize the backend pose map construction process, thereby realizing parameter interaction optimization between the frontend and backend and ensuring the reliability of the system.

[0052] LiDAR point cloud data is the data acquired by LiDAR. Please refer to [link / reference]. Figure 3 LiDAR inertial odometry typically processes data in real time at the acquisition frequency of 3D LiDAR. Since the acquisition frequency of IMU is much higher than that of 3D LiDAR, IMU pre-integration can be used to describe the motion between keyframes as the initial value for point cloud registration and to perform point cloud distortion correction to obtain distorted point cloud data.

[0053] It is understood that in this embodiment, "front-end" and "back-end" refer to the concepts in Simultaneous Localization and Mapping (SLAM). SLAM refers to a robot moving from an unknown location in an unknown environment, performing self-localization based on its position and a map during movement, and simultaneously building an incremental map based on its self-localization to achieve autonomous localization and navigation. SLAM is typically divided into a front-end and a back-end. The front-end includes odometry and loop closure, ensuring real-time localization output from SLAM. Odometry performs real-time pose tracking by matching continuous sensor data and studying the transformation relationships between sensor observations, thus obtaining its relative localization, i.e., odometer measurement.

[0054] Step 102: Construct a point-to-plane loss function based on the distorted point cloud data and the initial pose variables of the IMU, and perform singular value decomposition on the initial Hessian matrix in the point-to-plane loss function to obtain rotation feature vectors and translation feature vectors.

[0055] It should be noted that the point-to-plane loss function constructed based on the distorted point cloud data and the initial pose variables of the IMU is expressed as:

[0056]

[0057] in, The initial pose variables for the IMU include the rotation vector. Translation vector Let represent the initial Hessian matrix of the optimization problem, and This represents the constraints between the contained point clouds, where Const is a constant and q is a variable. i p i n i These are the normal vectors of the plane containing the i-th point cloud corresponding to the local map used for registration, the i-th point cloud of the distorted point cloud data, and the plane containing the i-th point cloud, respectively.

[0058] The initial Hessian matrix can be determined by minimizing the variables. The relationships between them are divided into submatrices:

[0059]

[0060] Among them, A' rr A' rt A′ tr A′ tt These are all submatrices of the initial Hessian matrix. Performing singular value decomposition on them, the rotation eigenvectors and translation eigenvectors, from the perspective of rotation and translation components, can be obtained from the decomposition:

[0061]

[0062] Among them, V r V t Let Σ represent the rotation eigenvector and the translation eigenvector, respectively. r , Σ t These represent the eigenvalues ​​corresponding to the rotation eigenvectors and translation eigenvectors, respectively.

[0063] Step 103: Calculate the joint contribution vector and strong sharing vector based on the rotation feature vector, translation feature vector and preset information matrix respectively.

[0064] Further, step 103 includes:

[0065] Projecting the preset information matrix onto the feature space to which the rotated and translated feature vectors belong, we obtain the localizable contribution parameters;

[0066] The joint contribution vector and the strong shared vector are calculated based on the locationable contribution parameters and the preset radar parameters, respectively.

[0067] Furthermore, step 103, preceding the following, also includes:

[0068] The point-normal vector method is used to calculate the rotation information matrix and translation information matrix based on the distorted point cloud data to obtain the preset information matrix.

[0069] It should be noted that before calculating the relevant parameters used for degradation analysis, a pre-defined information matrix needs to be constructed to verify the reliability of the feature space. Therefore, this embodiment uses the point-normal vector method to calculate the rotation information matrix and translation information matrix based on the distorted point cloud data.

[0070]

[0071] F t =[n1 … n N ] T

[0072] in, For rotating information matrices, To obtain the translation information matrix, degradation detection is required in the feature space of each eigenvalue. Therefore, the localizability of each eigenvector needs to be obtained. By projecting the two pre-set information matrices onto the Hessian feature spaces of the corresponding rotation and translation eigenvectors, we obtain:

[0073] J r =(F r ·V r ) |·|

[0074] J t =(F t ·V t ) |·|

[0075] in, For projection information pairs based on rotation information matrix ( L p, L The localizable contribution parameters obtained from n) For projection information pairs based on translation information matrix ( L p, L n) is the localizable contribution parameter; (...)|·| represents the calculation of the absolute value of the principal element of the vector.

[0076] in addition, The scalar value directly indicates the degree of localizability in a specific direction. A scalar value of 0 indicates that the information does not contribute to the constraint of that direction; a scalar value of 1 indicates that the information completely constrains localization in that direction. To remove redundant information interference and enhance the interpretability of the current information, this embodiment is based on... The two locationable contribution parameters are used to calculate the joint contribution vector and the strong sharing vector, respectively:

[0077]

[0078]

[0079] in,

[0080]

[0081]

[0082] Where α can take the values ​​r and t, respectively, to obtain J r and J t K f These are preset radar parameters, set according to different radar models; L c (j), L s (j) are the joint contribution vector and strong sharing vector, respectively.

[0083] Step 104: Use a decision tree to perform perceptual degradation classification prediction based on the joint contribution vector and strong shared vector to obtain degradation detection results, which include partial degradation and complete degradation.

[0084] It should be noted that the perception degradation in this embodiment refers to the sensor's perception degradation in the measurement environment. For example, radar degradation means that when the radar is measuring, the geometric constraints in a certain direction are too few, making it almost impossible to distinguish the geometric constraints from the noise in that direction, causing the radar odometer to drift.

[0085] Please see Figure 4 Decision trees are used to analyze the joint contribution vector L. c (j) and the strongly shared vector L s (j) A judgment analysis is performed. The threshold parameters used in the judgment analysis include K1, K2, and K3, where K1 > K2 > K3. These threshold parameters are only related to the sensor's relevant parameters and are independent of the environment. Therefore, the degradation classification prediction based on this is not affected by environmental changes and does not require parameter adjustment as the environment changes. The degradation classification prediction results include not only partial degradation and complete degradation, but also the case of no degradation, i.e., L. c (j)≥K1, or L s(j)≥K2; In addition, this indicates that perception is degenerating, either partially or completely. c (j)≥K2, or L s (j)≥K3, if not, then perception has completely degenerated.

[0086] Step 105: Under the constraints of the additional constraints constructed based on the degradation detection results, the point-to-plane loss function is solved by the iterative Kalman filter method to obtain the optimized pose variables.

[0087] Further, step 105 includes:

[0088] Generate a constraint Hessian matrix based on the category of degradation detection results, and construct a constraint matrix equation based on the constraint Hessian matrix to obtain additional constraint conditions.

[0089] The point-to-plane loss function and constraint matrix equations are combined using the iterative Kalman filter method, and the optimal pose variables under additional constraints are solved.

[0090] Different additional constraints can be constructed for different degradation detection results to improve the accuracy of pose registration; this embodiment is based on the feature vector v j ∈V r / v j ∈V t Constraint formulas defined for rotational or translational properties:

[0091]

[0092]

[0093] If the degradation detection result indicates no degradation in that feature direction, no additional constraints are needed; registration can be performed based on the current state.

[0094] If the feature is completely degenerate in that direction, then directly set t0 or r0 in the corresponding direction to 0. 3×1 That's all.

[0095] If there is partial degradation in this feature direction, a new constrained Hessian matrix needs to be calculated based on resampling. The resampling operation is performed on the partially distorted point cloud data after sorting the localizable contribution parameters. There are two different cases for partial degradation, namely L... c When (j)≥K2, and L s When (j)≥K3, that is, when one of the two cases occurs, if L c If (j) ≥ K2, then the joint contribution vector L is used. c Calculate relevant constraint parameters for the sorted point cloud data, if Ls (j)≥K3, then use the strong shared vector L s The relevant constraint parameters are calculated from the sorted point cloud data. The calculation process for the new constraint Hessian matrix is ​​expressed as follows:

[0096] re A t =[ re n][ re n] T , re b t =[ re n] T ( re q- re p)

[0097]

[0098] re A r =[ re p× re n][ re p× re n] T , re b r =[ re p× re n] T ( re q- re p)

[0099]

[0100] in, re A t , re A r , re b t , re b r These are all constraint-related parameters used to constrain the direction of degradation. re p represents the resampled portion of the point cloud data, r eq To register the point cloud data corresponding to the local map, re n is the normal vector of the plane corresponding to the point cloud data. Based on the constraint Hessian matrix obtained above, constraint matrix equations can be constructed.

[0101] re Ax = re b

[0102] in, re A can take the value of re A t , re Ar Corresponding re b takes the value of re b t , re b r ;x can be the initial pose variable of the IMU in the first registration optimization solution process, or the updated pose variable in multiple iteration optimization processes, or the finally solved optimized pose variable, which is the variable that needs to be continuously iterated and optimized in this embodiment.

[0103] Solving the combined equations of the simultaneous constraint matrix equations and the point-to-plane loss function constitutes the registration optimization process considering additional constraints. The point-to-plane loss function is optimized based on different additional constraints constructed from the degradation detection results until the error of the point-to-plane loss function is reduced to a preset range. This yields the registration pose variables considering degradation detection, i.e., the optimized pose variables, which include the rotation variable r and the translation variable t.

[0104] Furthermore, step 105, followed by:

[0105] The target pose graph is obtained by constructing a pose graph based on optimized pose variables and multiple sensor parameters in the backend.

[0106] It should be noted that you should refer to [link / reference]. Figure 5 In this embodiment, the front-end IMU pre-integration receives GNC attitude measurement parameters sent by the back-end to correct the initial pose prediction. The back-end also collects the optimized pose variables obtained from the front-end and the pre-integration factor obtained during the IMU pre-integration calculation, fusing data from various other sensors to construct the pose map, resulting in a more accurate and reliable target pose map. Pose map optimization is a nonlinear optimization process that can be used to solve nonlinear least squares problems. This method assumes that the landmarks in the SLAM problem are fixed and only optimizes the pose, thus reducing the overall system error. The target position map can then be used to build a corresponding surrounding environment map based on the estimated robot trajectory.

[0107] The backend GNC attitude measurement parameters are also calculated using IMU pre-integration based on the known pose at a given time, and the relative factors of various sensors are connected through the IMU pre-integration factor. Employing an IMU-centric sensor fusion method effectively handles transient data loss and fuses various sensor types. As long as not all sensor modes fail simultaneously, the IMU noise can be constrained by any relative factor, ensuring continuous and accurate state estimation. This approach also better considers the overall system impact to obtain more accurate pose estimation results.

[0108] To address the issue of varying latency among different sensors, the backend of this embodiment establishes a new state node in the factor graph for each IMU measurement, allowing each lower-rate incoming relative factor to be linked to a specific node created at a higher rate. Within this framework, this embodiment fundamentally eliminates the processing of delayed measurements; by precisely updating the state at the sampling time of each IMU, the backend factor graph avoids the problem of missing anchor nodes for measurements.

[0109] Due to sensor degradation or environmental changes, sensor fusion architectures are vulnerable to outliers. Therefore, this embodiment employs the Gross Non-Convexity (GNC) method to eliminate outliers. IMU factors and non-degraded LiDAR relative factors are considered known interior points, while relative factors of other modalities and degraded LiDAR relative factors are considered potential outliers. The GNC method better initializes the non-convex version of the problem, thereby reducing the likelihood of convergence to local optima. To better solve the GNC-based graph optimization problem, this embodiment uses incremental smoothing to solve the least squares problem, while adding marginalization operations to control the problem size and ensure the real-time performance of the backend solution. Graduated Non-Convexity (GNC) is a technique designed to alleviate the difficulties observed when optimizing robust cost functions; it better initializes the non-convex version of the problem, thereby reducing the likelihood of convergence to local optima.

[0110] The multi-sensor pose estimation method considering perceptual degradation provided in this application optimizes the results of the front-end IMU pre-integration calculation using back-end measurement parameters to obtain the initial IMU pose variables. This process considers the parameter influence between the front and back ends, ensuring that subsequent degradation detection analysis is performed from a system perspective and avoiding local limitations in the analysis process. Moreover, degradation analysis based on the calculated feature parameters is unaffected by the environment, allowing parameters to be pre-adjusted to their optimal state, avoiding the impact of environmental changes. Furthermore, additional constraints are constructed based on the degradation detection results, thereby constraining the solution process of the point-to-plane loss function in the registration process. This ensures that the registration results consider the impact of perceptual degradation, making the predicted and optimized pose variables more accurate and reliable, and more robust to perceptual degradation. Therefore, this application addresses the technical problem that existing technologies require parameter adjustments for specific environments and ignore system influences, resulting in limited applicability and poor performance of pose estimation considering perceptual degradation.

[0111] For easier understanding, please refer to Figure 2 This application provides embodiments of a multi-sensor pose estimation device that takes into account perceptual degradation, including:

[0112] The data distortion optimization unit 201 is used to perform distortion removal operation on LiDAR point cloud data through IMU initial pose variables to obtain distorted point cloud data. The IMU initial pose variables are optimized based on backend measurement parameters.

[0113] The matrix decomposition calculation unit 202 is used to construct a point-to-plane loss function based on the distorted point cloud data and the initial pose variables of the IMU, and to perform singular value decomposition on the initial Hessian matrix in the point-to-plane loss function to obtain rotation feature vectors and translation feature vectors.

[0114] The feature parameter calculation unit 203 is used to calculate the joint contribution vector and the strong sharing vector based on the rotation feature vector, the translation feature vector and the preset information matrix, respectively.

[0115] The degradation classification prediction unit 204 is used to perform perceptual degradation classification prediction using a decision tree based on the joint contribution vector and strong shared vector to obtain degradation detection results, which include partial degradation and complete degradation.

[0116] The optimized pose solving unit 205 is used to solve the point-to-plane loss function through the iterative Kalman filtering method under the constraints of additional constraints constructed based on the degradation detection results, so as to obtain the optimized pose variables.

[0117] Furthermore, it also includes:

[0118] The pre-integration calculation unit 206 is used to adjust the IMU pre-integration calculated by the front end based on the discrete kinematics model using the back-end GNC attitude measurement parameters, so as to obtain the initial pose variables of the IMU and the IMU pre-integration factor.

[0119] Furthermore, the feature parameter calculation unit 203 is specifically used for:

[0120] Projecting the preset information matrix onto the feature space to which the rotated and translated feature vectors belong, we obtain the localizable contribution parameters;

[0121] The joint contribution vector and the strong shared vector are calculated based on the locationable contribution parameters and the preset radar parameters, respectively.

[0122] Furthermore, the pose solving unit 205 is optimized, specifically for:

[0123] Generate a constraint Hessian matrix based on the category of degradation detection results, and construct a constraint matrix equation based on the constraint Hessian matrix to obtain additional constraint conditions.

[0124] The point-to-plane loss function and constraint matrix equations are combined using the iterative Kalman filter method, and the optimal pose variables under additional constraints are solved.

[0125] In the several embodiments provided in this application, it should be understood that the disclosed apparatus and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0126] The units described as separate components may or may not be physically separate. The components shown as units may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.

[0127] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0128] If the integrated unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for executing all or part of the steps of the methods described in the various embodiments of this application through a computer device (which may be a personal computer, server, or network device, etc.). The aforementioned storage medium includes: USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, optical disks, and other media capable of storing program code.

[0129] The above-described embodiments are only used to illustrate the technical solutions of this application, and are not intended to limit them. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application.

Claims

1. A multi-sensor pose estimation method considering sensor degradation, characterized in that, include: Distortion-free point cloud data is obtained by performing distortion removal operation on LiDAR point cloud data using IMU initial pose variables, wherein the IMU initial pose variables are obtained by optimization based on backend measurement parameters; Based on the distortion-reduced point cloud data and the initial pose variables of the IMU, a point-to-plane loss function is constructed, and singular value decomposition is performed on the initial Hessian matrix in the point-to-plane loss function to obtain rotation feature vectors and translation feature vectors. The rotation information matrix and translation information matrix are calculated based on the distortion-reduced point cloud data using the point-normal vector method to obtain the preset information matrix; The joint contribution vector and the strong sharing vector are calculated based on the rotation feature vector, the translation feature vector, and the preset information matrix, respectively. The specific process is as follows: Projecting the preset information matrix onto the feature space to which the rotation feature vector and the translation feature vector belong, we obtain the localizable contribution parameters; The joint contribution vector and the strong shared vector are calculated based on the locationable contribution parameters and the preset radar parameters, respectively. A decision tree is used to perform perceptual degradation classification prediction based on the joint contribution vector and the strong shared vector to obtain degradation detection results, which include partial degradation and complete degradation. Under the constraints of additional constraints constructed based on the degradation detection results, the point-to-plane loss function is solved by the iterative Kalman filter method to obtain the optimized pose variables.

2. The multi-sensor pose estimation method considering sensor degradation according to claim 1, characterized in that, The step of performing distortion correction on LiDAR point cloud data using the initial pose variables of the IMU to obtain distorted point cloud data includes, prior to: The backend GNC attitude measurement parameters are used to adjust the frontend IMU pre-integration calculated based on the discrete kinematics model, thereby obtaining the initial pose variables and IMU pre-integration factors of the IMU.

3. The multi-sensor pose estimation method considering sensor degradation according to claim 1, characterized in that, Under the constraints of the additional constraints constructed based on the degradation detection results, the point-to-plane loss function is solved by the iterative Kalman filter method to obtain the optimized pose variables, including: A constraint Hessian matrix is ​​generated based on the category of the degradation detection result, and a constraint matrix equation is constructed based on the constraint Hessian matrix to obtain additional constraint conditions. The point-to-plane loss function and the constraint matrix equation are combined using an iterative Kalman filter method, and the optimized pose variables under the additional constraints are solved.

4. The multi-sensor pose estimation method considering sensor degradation according to claim 1, characterized in that, Under the constraints of the additional constraints constructed based on the degradation detection results, the point-to-plane loss function is solved by the iterative Kalman filter method to obtain the optimized pose variables, and then the process further includes: The target pose graph is obtained by constructing a pose graph on the backend based on the optimized pose variables and various sensor parameters.

5. A multi-sensor pose estimation device considering sensor degradation, characterized in that, include: The data distortion optimization unit is used to perform distortion removal operation on LiDAR point cloud data through IMU initial pose variables to obtain distorted point cloud data. The IMU initial pose variables are optimized based on backend measurement parameters. The matrix decomposition calculation unit is used to construct a point-to-plane loss function based on the distortion-reduced point cloud data and the initial pose variables of the IMU, and to perform singular value decomposition on the initial Hessian matrix in the point-to-plane loss function to obtain rotation feature vectors and translation feature vectors. The rotation information matrix and translation information matrix are calculated based on the distortion-reduced point cloud data using the point-normal vector method to obtain the preset information matrix; The feature parameter calculation unit is used to calculate the joint contribution vector and the strong sharing vector based on the rotation feature vector, the translation feature vector, and the preset information matrix, respectively. Specifically, the feature parameter calculation unit is used for: Projecting the preset information matrix onto the feature space to which the rotation feature vector and the translation feature vector belong, we obtain the localizable contribution parameters; The joint contribution vector and the strong shared vector are calculated based on the locationable contribution parameters and the preset radar parameters, respectively. The degradation classification prediction unit is used to perform perceptual degradation classification prediction using a decision tree based on the joint contribution vector and the strong shared vector to obtain degradation detection results, which include partial degradation and complete degradation. The optimized pose solving unit is used to solve the point-to-plane loss function through an iterative Kalman filter method under the constraints of additional constraints constructed based on the degradation detection results, so as to obtain the optimized pose variables.

6. The multi-sensor pose estimation device considering perceptual degradation according to claim 5, characterized in that, Also includes: The pre-integration calculation unit is used to adjust the IMU pre-integration calculated by the front end based on the discrete kinematics model using the back-end GNC attitude measurement parameters, so as to obtain the initial pose variables of the IMU and the IMU pre-integration factor.

7. The multi-sensor pose estimation device considering perceptual degradation according to claim 5, characterized in that, The optimized pose solving unit is specifically used for: A constraint Hessian matrix is ​​generated based on the category of the degradation detection result, and a constraint matrix equation is constructed based on the constraint Hessian matrix to obtain additional constraint conditions. The point-to-plane loss function and the constraint matrix equation are combined using an iterative Kalman filter method, and the optimized pose variables under the additional constraints are solved.

Images