A method for detecting a degradation direction of a laser SLAM system and related devices

Abstract

The application relates to the technical field of mobile robot positioning and environment perception, and particularly relates to a laser SLAM system degeneration direction detection method and related equipment. The method first acquires three-dimensional point cloud data through a laser sensor, inputs the acquired three-dimensional point cloud data into a point-surface matching model for feature extraction and matching, and calculates a Jacobian matrix under a geometric degeneration scene based on a nonlinear optimization theory. An information matrix is constructed through the Jacobian matrix, and a covariance matrix of pose state estimation is further generated. The covariance matrix is innovatively decomposed into a translation sub-matrix and a rotation sub-matrix, and the uncertainty distribution of the translation degree of freedom and the rotation degree of freedom is quantified respectively. By analyzing the eigenvalues and variance contribution rates of the translation sub-matrix and the rotation sub-matrix, the key spatial direction in the degeneration environment can be identified.

Description

Technical Field

[0001] This invention relates to the field of mobile robot localization and environmental perception technology, specifically to a degradation direction detection method and related equipment for a laser SLAM system. Background Technology

[0002] With the development of technologies such as autonomous driving, robotic inspection, and the digitization of underground infrastructure, lidar, as a high-precision three-dimensional perception sensor, has been widely used in enclosed structures such as hydropower station water pipelines, urban underground tunnels, wind turbine towers, and cable corridors. Especially in scenarios lacking external navigation signal support, lidar-based SLAM (Simultaneous Localization and Mapping) systems have become a core means of achieving autonomous navigation.

[0003] Most current mainstream laser SLAM systems construct matching relationships based on point cloud geometric features. Typical methods, such as iterative nearest point or normal distribution transformation, rely on changes in the geometric structure of the environment to constrain the estimation of the robot's six degrees of freedom pose. However, in practical applications, many industrial and energy infrastructures have extremely regular structures, such as cylindrical tunnels, straight pipes, and regular shafts. In these scenarios, the geometric variations of the point cloud are insufficient, which can easily lead to the system failing to form effective constraints in certain directions, resulting in the so-called localization degradation phenomenon. This phenomenon directly affects localization accuracy and can even cause the system to diverge, severely restricting the stability and practicality of laser SLAM systems in engineering scenarios.

[0004] Common factors contributing to degradation in laser SLAM systems include: First, the environment's geometry is too regular, lacking distinctive features such as edges, corners, and curved surfaces. In regular structural environments like tunnels, pipe corridors, and towers, the point clouds acquired by lidar are often regular in shape, with a single directional distribution of geometric features. This fails to adequately constrain the pose of all six degrees of freedom, easily leading to localization degradation in certain directions. Existing SLAM systems often rely on optimization failures to indirectly determine degradation status, lacking a unified and interpretable degradation detection method. In particular, they cannot accurately identify the specific direction of degradation, limiting subsequent observation compensation and localization enhancement capabilities. Second, the lidar's limited field of view results in insufficient coverage of the acquired point cloud, making it difficult to construct complete 3D structural information. Summary of the Invention

[0005] The technical problem to be solved by the present invention is to provide a degradation direction detection method and related equipment for a laser SLAM system, which addresses the shortcomings of the prior art and solves the technical problem that the degradation direction of the existing SLAM system cannot be accurately identified.

[0006] The objective of this invention is achieved through the following technical solutions: In a first aspect, the present invention provides a method for detecting the degradation direction of a laser SLAM system, comprising: Acquire three-dimensional point cloud data of the laser SLAM system, input the three-dimensional point cloud data into the point-surface matching model, and calculate the Jacobian matrix under the geometric degradation environment based on the point-surface matching model; The information matrix is ​​calculated based on the Jacobian matrix, and the covariance matrix for pose state estimation is established based on the information matrix. The covariance matrix is ​​decomposed into translation submatrices and rotation submatrices; the existence of uncertainty in the translation degree of freedom of the laser SLAM system is determined based on the translation submatrices, and the existence of uncertainty in the rotation degree of freedom of the laser SLAM system is determined based on the rotation submatrices; the spatial direction corresponding to the degradation is obtained by detecting the translation and rotation submatrices under the uncertainty conditions.

[0007] As a further improvement of the present invention, the construction process of the point-surface matching model includes: Obtain all point cloud data of the current frame, fit a local neighborhood plane based on the neighborhood points of all point cloud data of the current frame, and determine the plane normal vector of the local neighborhood plane. The residuals corresponding to each point cloud data are calculated based on the orthogonal distance of the plane normal vectors. The residuals are linearized using Lie algebra to obtain the Jacobian matrix corresponding to each point cloud data. After stacking the residuals and Jacobian matrices corresponding to all point cloud data, a nonlinear least squares optimization objective function is constructed with the goal of minimizing the total sum of squared residuals, thus completing the construction of the point residual model.

[0008] As a further improvement of the present invention, the optimization objective function is:

[0009] In the formula, These are the pose perturbation parameters; for The total residual vector is formed by stacking the residuals of all point cloud data. The total Jacobian matrix is ​​formed by stacking the corresponding Jacobian matrices of all point cloud data. It is the square of the L2 norm.

[0010] As a further improvement of the present invention, the information matrix is:

[0011] In the formula, For information matrix; The total amount of data in the Jacobian matrix; For the first Transpose of a Jacobian matrix; For the first A Jacobian matrix; It is a Gram matrix.

[0012] As a further improvement of the present invention, detection is performed based on the translation submatrix and rotation submatrix under uncertain conditions to obtain the spatial direction corresponding to the degradation, including: The translation submatrix is ​​decomposed into eigenvalues ​​and eigenvectors. When the laser SLAM system has uncertainties and the eigenvalue of the translation submatrix is ​​greater than the set translation degradation judgment threshold, it is determined that the translation degree of freedom has degraded, and the eigenvector corresponding to the eigenvalue of the translation submatrix is ​​taken as the spatial direction corresponding to the translation degradation. The rotation submatrix is ​​decomposed into eigenvalues ​​and eigenvectors. When the laser SLAM system has uncertainties and the eigenvalue of the rotation submatrix is ​​greater than the set rotation degradation judgment threshold, it is determined that the rotational degree of freedom has degraded, and the eigenvector corresponding to the eigenvalue of the rotation submatrix is ​​taken as the spatial direction corresponding to the rotation degradation.

[0013] As a further improvement of the present invention, the setting of translational degradation judgment threshold and the setting of rotational degradation judgment threshold are dynamically adjusted horizontally according to the measurement accuracy of the lidar and the environmental noise.

[0014] As a further improvement of the present invention, the coupling relationship between rotation and translation is preserved in the process of obtaining translation submatrix and rotation submatrix by covariance matrix decomposition, and the translation-rotation coupling strength is obtained. When there are uncertainties in the translation covariance submatrix and the rotation covariance submatrix, the spatial directions corresponding to the degradation of the translation covariance submatrix and the rotation covariance submatrix are constrained by the translation-rotation coupling strength.

[0015] Secondly, the present invention provides a degradation direction detection system for a laser SLAM system, comprising: The data acquisition module acquires the three-dimensional point cloud data of the laser SLAM system, inputs the three-dimensional point cloud data into the point-surface matching model, and calculates the Jacobian matrix under the geometric degradation environment based on the point-surface matching model. The data processing module calculates the information matrix based on the Jacobian matrix and establishes the covariance matrix for pose state estimation based on the information matrix. The degradation direction detection module decomposes the covariance matrix into a translation submatrix and a rotation submatrix; it determines whether there is uncertainty in the translational degree of freedom of the laser SLAM system based on the translation submatrix, and determines whether there is uncertainty in the rotational degree of freedom of the laser SLAM system based on the rotation submatrix; it performs detection based on the translation submatrix and rotation submatrix under uncertainty conditions to obtain the spatial direction corresponding to degradation.

[0016] Thirdly, the present invention provides a computer-readable storage medium for storing one or more programs, the one or more programs including instructions that, when executed by a computing device, cause the computing device to perform the degradation direction detection method of the laser SLAM system described above.

[0017] Fourthly, the present invention provides a computing device, comprising: One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs including a degradation direction detection method for performing the laser SLAM system described above.

[0018] The beneficial effects of this invention are as follows: This invention provides a degradation direction detection method for laser SLAM systems. It acquires three-dimensional point cloud data and inputs it into a point-surface matching model. Through geometric matching processing of the three-dimensional point cloud data using the point-surface matching model, a Jacobian matrix reflecting the relationship between point cloud geometric constraints and pose is calculated. Based on the Jacobian matrix, an information matrix characterizing the environmental constraint strength on the six degrees of freedom pose is calculated. Utilizing the correlation between the information matrix and the uncertainty of pose state estimation, a covariance matrix quantifying the uncertainty of pose observation is established. The covariance matrix is ​​decomposed into translational and rotational sub-matrices corresponding to translational and rotational degrees of freedom, respectively. The uncertainty of translational degrees of freedom is obtained through translational sub-matrices analysis, and the uncertainty of rotational degrees of freedom is obtained through rotational sub-matrices analysis. Finally, based on the uncertainties of translational and rotational degrees of freedom, the degradation-corresponding spatial direction is detected in a geometrically degraded environment, thereby solving the technical problems of positioning degradation and difficulty in identifying degradation directions in laser SLAM systems under geometrically degraded environments. This invention, based on the 3D point cloud data acquired by the laser SLAM system itself, accurately obtains the uncertainties of translational and rotational degrees of freedom and detects the spatial direction corresponding to degradation through a series of processing steps including point-surface matching models, Jacobian matrices, information matrices, covariance matrices, and sub-matrices. This provides a clear basis for the laser SLAM system to take localization optimization measures to address degradation issues, and provides a real-time, interpretable, and highly robust observation state evaluation mechanism for the laser SLAM system. This effectively improves the stability and reliability of the laser SLAM system's localization in geometrically degraded environments, without relying on additional sensors or prior information, and has good engineering practicality and adaptability.

[0019] Furthermore, by effectively processing and analyzing 3D point cloud data and using mathematical tools such as point-surface matching models to extract key geometric information (such as the Jacobian matrix, information matrix, and covariance matrix), redundancy in data processing can be reduced. During the computation process, complex calculations on all raw point cloud data are unnecessary; instead, the focus is on key geometric relationships and uncertainty analysis. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0021] Figure 1 This is a schematic flowchart of the degradation direction detection method of the laser SLAM system of the present invention; Figure 2This is a schematic diagram of the vector distribution related to the Jacobian matrix in the geometric degradation environment of the translation degradation case of the present invention; Figure 3 This is a schematic diagram of the vector distribution related to the Jacobian matrix in the geometrically degenerate environment of the rotational degeneracy case of the present invention; Figure 4 This is a diagram showing the results of the translational degradation direction detection according to the present invention; Figure 5 This is a diagram showing the detection results of the rotational degradation direction of the present invention; Figure 6 This is a schematic diagram of the structure of an electronic device in an embodiment of the present invention. Detailed Implementation

[0022] To make the objectives and technical solutions of this invention clearer and easier to understand, the invention will be further described in detail below with reference to the accompanying drawings and embodiments. The specific embodiments described herein are for illustrative purposes only and are not intended to limit the invention.

[0023] The technical solution of the present invention will be clearly and completely described below with reference to the accompanying drawings and specific embodiments. The described embodiments are only some embodiments of the present invention, and not all embodiments.

[0024] Example 1 like Figure 1 This embodiment provides a degradation direction detection method for a laser SLAM system, aiming to solve the positioning degradation problem caused by the simple geometric features in environments such as tunnels, pipe corridors, and wind turbine towers. This method constructs an optimized model through point cloud point-surface feature matching, and combines it with covariance matrix analysis to detect degradation in translation and rotation directions in real time, providing a basis for improving the robustness of the laser SLAM system. The specific implementation method is as follows.

[0025] This embodiment provides a degradation direction detection method for a laser SLAM system, mainly including: acquiring three-dimensional point cloud data of the laser SLAM system; inputting the three-dimensional point cloud data into a point-surface matching model; calculating the Jacobian matrix under geometric degradation based on the point-surface matching model; calculating an information matrix based on the Jacobian matrix; establishing a covariance matrix for pose state estimation based on the information matrix; decomposing the covariance matrix into translation submatrices and rotation submatrices; determining whether there is uncertainty in the translational degrees of freedom of the laser SLAM system based on the translation submatrices; and determining whether there is uncertainty in the rotational degrees of freedom of the laser SLAM system based on the rotation submatrices; and performing detection based on the translation and rotation submatrices under uncertainty conditions to obtain the spatial direction corresponding to degradation. This embodiment performs direction detection based on the translation submatrices to obtain the translational degradation direction, and performs direction detection based on the rotation submatrices to obtain the rotational degradation direction.

[0026] In this embodiment, to address the problem of positioning degradation and difficulty in accurately identifying the degradation direction caused by insufficient geometric constraints in geometrically degraded environments, the solution is as follows: First, acquire the 3D point cloud data of the laser SLAM system. Input this 3D point cloud data into a point-plane matching model. Through geometric matching processing of the 3D point cloud data by the point-plane matching model, calculate the Jacobian matrix that reflects the relationship between point cloud geometric constraints and pose. Then, based on the Jacobian matrix, calculate the information matrix characterizing the strength of the environment's constraints on the six degrees of freedom pose. Utilize the correlation between the information matrix and the uncertainty of pose state estimation to establish a covariance matrix that quantifies the uncertainty of pose observation. Subsequently, decompose the covariance matrix into translational and rotational submatrices corresponding to translational and rotational degrees of freedom, respectively. Analyze the uncertainties of translational and rotational degrees of freedom using the translational submatrices and the rotational submatrices, respectively. Finally, based on the uncertainties of translational and rotational degrees of freedom, detect the corresponding spatial direction of degradation in geometrically degraded environments, thereby solving the technical problems of positioning degradation and difficulty in identifying the degradation direction in laser SLAM systems under geometrically degraded environments. Therefore, this embodiment can accurately obtain the uncertainty of translational and rotational degrees of freedom and detect the spatial direction corresponding to degradation based on the three-dimensional point cloud data acquired by the laser SLAM system itself, through a series of processing methods including point-surface matching model, Jacobian matrix, information matrix, covariance matrix and submatrix. This provides a clear basis for the laser SLAM system to take localization optimization measures for degradation problems, effectively improving the stability and reliability of the laser SLAM system in geometrically degraded environments, without relying on additional sensors or prior information, thus improving the accuracy and robustness of degradation direction localization in complex environments.

[0027] In this embodiment, the 3D point cloud data is generated based on the target environment scanned by the lidar system. Specifically, it includes: the lidar emitting a laser beam at a preset frequency, calculating the distance to the target point by measuring the round-trip time of the laser, and generating a single frame of local point cloud data. The 3D point cloud data is a set of 3D laser point clouds. After the single frame point cloud data is generated, statistical filtering is used to calculate the average distance between the laser point and its neighboring points, and noise points far exceeding the mean are removed; radius filtering is used to remove isolated points in the neighborhood with fewer than a threshold. A point-to-surface matching model is used to perform inter-frame or frame-to-frame matching of adjacent frame point clouds to estimate the relative pose state. The pose transformation matrix is ​​used to convert the local coordinates of all single frame point clouds into world coordinates to form a global point cloud.

[0028] Furthermore, the construction process of the point-plane matching model includes: acquiring all point cloud data of the current frame; fitting a local neighborhood plane based on the neighborhood points of the point cloud data of the current frame; determining the plane normal vector of the local neighborhood plane; calculating the residual corresponding to each point cloud based on the orthogonal distance of the plane normal vector; linearizing the residual using Lie algebra to obtain the Jacobian matrix corresponding to each point cloud; stacking the residuals and Jacobian matrices corresponding to all point cloud data respectively; constructing a nonlinear least squares optimization objective function with the goal of minimizing the sum of squared total residuals; and completing the construction of the point residual model.

[0029] Specifically, the construction and optimization process of the point-area matching model is as follows. First, the laser point cloud data in the current frame is set to... Set the plane normal vector of the neighborhood fitting of the target point cloud data (i.e., map point) for the laser point matching in the current frame. Set the inter-frame pose transformation as The residual term of the current frame point cloud data is defined as the orthogonal distance between the coordinates of the laser point after pose transformation and the target plane, i.e., the objective function is: (1) in, The objective function is... This represents the rotational pose transformation from the current frame point cloud to the target point cloud. For the point cloud in the current frame One point; The first matching of the target point cloud with the current frame point cloud One point; This represents the translational pose transformation from the current frame point cloud to the target point cloud. This is the projection of the position deviation vector onto the normal vector. This is the positional deviation vector.

[0030] This embodiment uses Lie algebras to represent pose perturbations, and considers pose perturbations as optimization variables when constructing the objective function. The current pose estimate is set as... The optimization variable is the perturbation. Through exponential mapping, it becomes: (2) In the formula, For the result of the exponential mapping, Transform a 6-dimensional Lie algebra vector into a 4×4 antisymmetric matrix. For the hat operator, This is the estimated current pose. After performing a first-order approximate expansion of the exponential mapping result: (3) In the formula, The identity matrix and the antisymmetric matrix Same dimension. Therefore, we get: (4) Using the properties of Lie algebras: (5) in, Let be the perturbation variable for pose. For the rotational disturbance component, For translational disturbance components, The symbol represents finding the antisymmetric matrix of the matrix within the parentheses, which is the matrix multiplication form of the cross product. Therefore, the final linearized residual is: (6) In the formula, To linearize the residual results, Let be the unit normal vector of the plane containing the target point cloud. For the first The projection of the position deviation vector corresponding to each point cloud onto the normal vector. This serves as the reference pose for the rotational transformation from the current frame point cloud to the target point cloud. For the point cloud in the current frame One point; The first matching of the target point cloud with the current frame point cloud One point, This serves as the reference pose for the translation transformation from the current frame point cloud to the target point cloud. For the rotational disturbance component, This represents the translational perturbation component. The linearized residual matrix is ​​in the form of: (7) in, To linearize the residual results, For the initial residual, For Jacobian matrices, Let be the perturbation variable for pose.

[0031] like Figure 2 and Figure 3 As shown, in the degenerate environment, the vectors related to the Jacobian matrix are distributed according to a specific directional pattern, and the Jacobian matrix cannot provide sufficient constraints for pose optimization in some directions.

[0032] To minimize the sum of squared residuals at points and surfaces, an optimization objective function is constructed as follows: (8) in, For pose perturbation variables; The total residual vector is formed by stacking the residuals of all point cloud data. The total Jacobian matrix is ​​formed by stacking the corresponding Jacobian matrices of all point cloud data. This represents the square of the L2 norm. The total residual vector is obtained by stacking the residuals at all points. Sum of Jacobian matrices They are respectively: (9) In the formula, Let be the normal distance of the target point cloud under the current pose transformation for the first point cloud. Let be the normal distance of the target point cloud under the current pose transformation for the second point cloud. For the first N The normal distance of the target point cloud to the point cloud under the current pose change; This is the projection of the position deviation vector corresponding to the first point cloud onto the normal vector. This is the projection of the position deviation vector corresponding to the second point cloud onto the normal vector. For the first N The projection of the position deviation vector corresponding to each point cloud onto the normal vector; This serves as the reference pose for the rotational transformation from the current frame point cloud to the target point cloud. The first point in the point cloud of the current frame. This is the second point in the point cloud of the current frame. For the Nth point in the point cloud of the current frame, This represents the total amount of data in the Jacobian matrix. M = N .

[0033] The Gauss-Newton method (an iterative optimization algorithm based on Gauss-Newton nonlinear least squares problems) is used to solve the objective function in equation (8) to obtain the perturbation variables. The optimal value. Specifically, the objective function is expanded as follows: (10) Further expansion yields: (11) For disturbance variables Taking the derivative and setting it to zero, we obtain the first-order optimal condition: (12) Thus, the iterative update equation based on the Gauss-Newton method is derived: (13) In the formula, solving for the disturbance variable requires... Performing the inverse operation, we get: (14) therefore, This is the Hessian matrix under the Gauss-Newton approximation, i.e., the information matrix. The information matrix is: (15) In the formula, For information matrix; The total amount of data in the Jacobian matrix; For the first Transpose of a Jacobian matrix; For the first A Jacobian matrix; In this embodiment, the matrix is ​​a Gram matrix. and They have the same meaning.

[0034] matrix This is crucial for degradation detection, reflecting the strength of point cloud geometric constraints on pose variables. Its dimension is 6×6 (corresponding to 6 degrees of freedom pose: 3 rotational degrees of freedom + 3 translational degrees of freedom). Thus, the residual construction and optimization modeling based on the point-surface ICP model are completed.

[0035] The information matrix is ​​then inverted to obtain the covariance matrix for pose state estimation. The covariance matrix not only quantifies the observation uncertainty of the system in each direction but also reflects the coupling relationship between rotation and translation. To more precisely assess the degradation state, the covariance matrix is ​​decomposed into translation and rotation sub-blocks. Eigenvalue decomposition is used to obtain the magnitude of uncertainty in each degree of freedom direction, and a threshold is set to identify whether degradation has occurred.

[0036] Specifically, in solving the objective function of equation (15), the objective function is equivalent to maximizing the posterior probability of the state perturbation. According to Bayesian inference, under the Gaussian assumption, its optimal estimate satisfies: (16) Therefore, the disturbance variable The covariance matrix corresponding to the optimal value is: (17) In the formula, the information matrix H It describes the constraints of observation on pose variables, and the information matrix. H The inverse of represents the degree of divergence of the pose variables, which is the covariance matrix of the pose state estimation.

[0037] The diagonal elements of the covariance matrix reflect the uncertainty in each degree of freedom, while the off-diagonal terms represent the coupling relationship between degrees of freedom. Therefore, the coupling relationship between rotation and translation is preserved during the covariance matrix decomposition process, thus obtaining the translation-rotation coupling strength. When the translation and rotation covariance submatrices are in a state of uncertainty, the translation-rotation coupling strength is used to constrain the spatial directions of the degeneracy of the translation and rotation covariance submatrices.

[0038] Considering that the estimated variables for laser SLAM are 6-dimensional Lie algebra vectors (including 3D rotation and 3D translation), the covariance matrix is ​​divided into four sub-blocks in a 6×6 dimension: (18) in: (19) (20) Translation submatrix, describing the system in X / Y / Z Uncertainty regarding directional translation; : Rotation submatrix, describing the system's rotation around the rotation matrix. Roll (Roll angle) / Pitch (Pitch angle) / Yaw (Yaw angle) The degree of constraint on the direction of rotation; and Cross-covariance between rotation and translation reflects the strength of their coupling.

[0039] In the formula, This refers to the sub-block corresponding to the rotational degrees of freedom in the information matrix; For each information matrix, there is a sub-block corresponding to the translational degrees of freedom. These are the cross sub-blocks in the information matrix that have been rotated to translation; For the cross sub-blocks in the information matrix that have been translated and rotated, and Both are used to characterize the coupling constraint relationship between translational and rotational degrees of freedom.

[0040] Therefore, this covariance construction method has two major advantages over the information matrix: firstly, it can provide a directional uncertainty measure; secondly, it naturally integrates the coupling effect between rotation and translation, avoiding misjudgment and missed judgment of the degenerate direction.

[0041] The process of detecting the degradation direction of translation is as follows: extract the translation sub-matrix from the covariance matrix, and decompose the translation sub-matrix into eigenvalues ​​and eigenvectors; when there is uncertainty in the laser SLAM system, and there is an eigenvalue of the translation sub-matrix that is greater than the set translation degradation judgment threshold, it is determined that the translation degree of freedom has degraded, and the eigenvector corresponding to the eigenvalue that is greater than the set translation degradation judgment threshold is taken as the spatial direction corresponding to the translation degradation.

[0042] Furthermore, extract the translation submatrix. Eigenvalue decomposition yields three eigenvalues. , , and their corresponding eigenvectors , , : (twenty one) in, for The eigenvector matrix of the time-shifted submatrix for The eigenvalues ​​of the diagonal matrix at time step 1. When the eigenvalues ​​of a translation submatrix exceed a set translation degradation threshold. When, if a certain characteristic value Greater than This indicates insufficient point cloud constraints in that direction, suggesting uncertainty in the translational degrees of freedom of the laser SLAM system, thus indicating translational degradation. The translational degradation threshold is dynamically adjusted horizontally based on the measurement accuracy of the lidar and environmental noise. For example, if increased dust concentration leads to increased noise in the received point cloud data, the threshold is raised; conversely, if the lidar's measurement accuracy is high (e.g., a 64-line lidar with a ranging accuracy of ±1cm and an angular resolution of 0.1°×0.2°), the measurement error is small, and the residual fluctuation of the corresponding point cloud matching is reduced, the threshold can be lowered. Eigenvalues ​​obtained from eigenvalue decomposition... , and It has a definite physical unit, namely the unit of the variance of the translation state. At this point, the direction of degradation can be determined by the corresponding eigenvector. Clear instructions. Figure 2 This demonstrates that in a section of a hydroelectric power station's water transmission pipeline, the axial degradation direction detected by the system exhibits a clear directionality, and its estimation uncertainty is significantly higher than that of other degrees of freedom. In this embodiment, Figure 2The question mark indicates that the axial position cannot be determined by the geometric information of the laser point cloud alone in a long, straight horizontal pipe. The red line represents the laser radar light, the yellow line represents the direction of the normal vector on the pipe surface, and the blue line represents the axial movement direction of the unmanned vehicle inspection. The left image is an axial section of the water pipeline, and the right image is a radial section of the pipeline.

[0043] Figure 3 The image on the left is an axial view of the water pipeline, and the image on the right is a radial view of the pipeline. The red line represents the laser radar beam, and the yellow line represents the direction of the normal vector on the pipeline surface. The direction of the vector nxp is obtained by cross-multiplying the laser radar point vector p by the pipeline surface normal vector n. The circle represents the nxp vector perpendicular to the paper and outwards, and the cross represents the nxp vector perpendicular to the paper and inwards.

[0044] Similarly, the rotational degradation direction detection process is as follows: extract the rotational submatrix from the covariance matrix, and perform eigenvalue and eigenvector decomposition on the rotational submatrix. When there are uncertainties in the laser SLAM system, and there is an eigenvalue of the rotational submatrix that is greater than a set rotational degradation threshold, then rotational degree of freedom degradation is determined to have occurred, and the eigenvector corresponding to the eigenvalue that is greater than the set rotational degradation threshold is taken as the spatial direction corresponding to the rotational degradation. For the rotational submatrix... Perform eigenvalue decomposition to obtain the uncertainty assessment results for the three rotational degrees of freedom: (twenty two) in, for The eigenvector matrix of the rotating submatrix at each time step. for The eigenvalues ​​of the diagonal matrix at any given time. When an eigenvalue of the rotational submatrix exceeds a set rotational degradation threshold, the laser SLAM system exhibits uncertainty in its rotational degrees of freedom, thus indicating rotational degradation, i.e., degradation occurs in the corresponding rotational degree of freedom. Similarly, the rotational degradation threshold is dynamically adjusted horizontally based on the measurement accuracy of the lidar and environmental noise. Rotational eigenvalues. , and It also has the same physical unit as the variance of the rotational state ( In actual testing, it was found that in vertical or inclined straight pipe sections, the point cloud changes are distributed in a ring around the axis, making it difficult to constrain rotation around the axis, which leads to degradation of the system in this degree of freedom.

[0045] Figure 4 This is the result of identifying the translational degradation direction of a certain inclined straight pipe section. The green, blue, and yellow arrows represent the body coordinate system of the UAV or unmanned vehicle, with red indicating the x-direction, blue the y-direction, and green the z-direction. The red dashed boxes represent the spatial directions corresponding to translational degradation. Figure 5This is the detection result for the rotational degradation direction of the inclined straight pipe section. The green, blue, and yellow arrows represent the body coordinate system of the UAV or unmanned vehicle; red indicates the x-direction, blue the y-direction, and green the z-direction. The red dashed box indicates the spatial direction corresponding to the rotational degradation.

[0046] The degradation direction results extracted by this method can not only serve as a supplementary judgment for the confidence of the state estimation of the SLAM system, but also be used in application scenarios such as point cloud feature selection, residual weighting, and degradation degree of freedom weighting in fusion optimization. Combined with existing intensity features, inertial observations, or prior structural information, the stability and accuracy of the lidar SLAM system in extremely regular structures can be further improved.

[0047] Example 2 This embodiment provides a degradation direction detection system for a laser SLAM system, used to implement the degradation direction detection method of the laser SLAM system in Embodiment 1. The system adopts a hierarchical architecture design, forming a complete closed loop from data acquisition to degradation direction output. The system includes three core modules: a data acquisition module, a data processing module, and a degradation direction detection module. Each module works collaboratively through a high-speed data bus.

[0048] The data acquisition module is used to acquire the three-dimensional point cloud data of the laser SLAM system, input the three-dimensional point cloud data into the point-surface matching model, and calculate the Jacobian matrix under the geometric degradation environment based on the point-surface matching model.

[0049] The data acquisition module execution process includes: acquiring 3D point cloud data from the laser SLAM system; inputting the 3D point cloud data into a point-surface matching model; calculating the Jacobian matrix under geometric degradation based on the point-surface matching model; calculating the information matrix based on the Jacobian matrix; establishing the covariance matrix for pose state estimation based on the information matrix; decomposing the covariance matrix into translation submatrices and rotation submatrices; determining whether there is uncertainty in the translational degree of freedom of the laser SLAM system based on the translation submatrices; determining whether there is uncertainty in the rotational degree of freedom of the laser SLAM system based on the rotation submatrices; and performing detection based on the translation and rotation submatrices under uncertainty conditions to obtain the spatial direction corresponding to degradation.

[0050] The 3D point cloud data is generated based on the target environment scanned by the lidar system. Specifically, it includes: the lidar emitting laser beams at a preset frequency; calculating the distance to the target point by measuring the laser's round-trip time; and generating a single-frame local point cloud data set, which is a set of 3D laser point clouds. After generating the single-frame point cloud data, statistical filtering is used to calculate the average distance between the laser point and its neighbors, eliminating noise points far exceeding the mean; radius filtering is used to remove isolated points with fewer than a threshold number of points in the neighborhood. A point-to-surface matching model is used to perform inter-frame or frame-to-frame matching of adjacent point clouds to estimate the relative pose state. A pose transformation matrix is ​​used to convert the local coordinates of all single-frame point clouds into world coordinates, forming a global point cloud.

[0051] The specific construction and optimization process of the point-area matching model is as follows. First, the laser point cloud data in the current frame is set to... Set the plane normal vector of the neighborhood fitting of the target point cloud (i.e., map point) for the laser point matching in the current frame. Set the inter-frame pose transformation as The residual term of the current frame point cloud data is defined as the orthogonal distance between the coordinates of the laser point after pose transformation and the target plane, i.e., the objective function is: (twenty three) in, The objective function is... This represents the rotational pose transformation from the current frame point cloud to the target point cloud. For the point cloud in the current frame i One point; The first matching of the target point cloud with the current frame point cloud i One point; This represents the translational pose transformation from the current frame point cloud to the target point cloud. This is the projection of the position deviation vector onto the normal vector. This is the positional deviation vector.

[0052] This embodiment uses Lie algebras to represent pose perturbations, and considers pose perturbations as optimization variables when constructing the objective function. The current pose estimate is set as... The optimization variable is the perturbation. Through exponential mapping, it can be expressed as: (twenty four) In the formula, For the result of the exponential mapping, Transform a 6-dimensional Lie algebra vector into a 4×4 antisymmetric matrix. For the hat operator, This is the estimated current pose. After performing a first-order approximate expansion of the exponential mapping result: (25) Therefore, we get: (26) Using the properties of Lie algebras, we have: (27) in, Let be the perturbation variable for pose. For the rotational disturbance component, For translational disturbance components, The symbol represents finding the antisymmetric matrix of the matrix within the parentheses, which is the matrix multiplication form of the cross product. Therefore, the final linearized residual is: (28) In the formula, To linearize the residual results, Let be the unit normal vector of the plane containing the target point cloud. Let be the projection of the position deviation vector corresponding to the i-th point cloud onto the normal vector. This serves as the reference pose for the rotational transformation from the current frame point cloud to the target point cloud. This refers to the i-th point in the point cloud of the current frame. For the i-th point that matches the target point cloud with the current frame point cloud, This serves as the reference pose for the translation transformation from the current frame point cloud to the target point cloud. For the rotational disturbance component, This represents the translational perturbation component. The linearized residual matrix is ​​in the form of: (29) in, To linearize the residual results, For the initial residual, For the first i Jacobian matrices Let be the perturbation variable for pose.

[0053] like Figure 2 and Figure 3 As shown, in the degenerate environment, the vectors related to the Jacobian matrix are distributed according to a specific directional pattern, and the Jacobian matrix cannot provide sufficient constraints for pose optimization in some directions.

[0054] To minimize the sum of squared residuals at points and surfaces, an optimization objective function is constructed as follows: (30) in, For pose perturbation variables; The total residual vector is formed by stacking the residuals of all point cloud data. The total Jacobian matrix is ​​formed by stacking the corresponding Jacobian matrices of all point cloud data. This represents the square of the L2 norm. The total residual vector is obtained by stacking the residuals at all points. Sum of Jacobian matrices They are respectively: (31) In the formula, Let be the normal distance of the target point cloud under the current pose transformation for the first point cloud. Let be the normal distance of the target point cloud under the current pose transformation for the second point cloud. For the first N The normal distance of the target point cloud to the point cloud under the current pose change; This is the projection of the position deviation vector corresponding to the first point cloud onto the normal vector. This is the projection of the position deviation vector corresponding to the second point cloud onto the normal vector. For the first N The projection of the position deviation vector corresponding to each point cloud onto the normal vector; This serves as the reference pose for the rotational transformation from the current frame point cloud to the target point cloud. The first point in the point cloud of the current frame. This is the second point in the point cloud of the current frame. For the Nth point in the point cloud of the current frame, This represents the total amount of data in the Jacobian matrix. In this embodiment... M = N .

[0055] The Gauss-Newton method (an iterative optimization algorithm based on Gauss-Newton nonlinear least squares problems) is used to solve the objective function in equation (30) to obtain the perturbation variables. The optimal value. Specifically, the objective function is expanded as follows: (32) Further expansion yields: (33) For disturbance variables Taking the derivative and setting it to zero, we obtain the first-order optimal condition: (34) Thus, the iterative update equation based on the Gauss-Newton method is derived: (35) In the formula, solving for the disturbance variable requires... Performing the inverse operation, we get: (36) therefore, This is the Hessian matrix under the Gauss-Newton approximation, i.e., the information matrix. The information matrix is: (37) In the formula, For information matrix; The total amount of data in the Jacobian matrix; For the first Transpose of a Jacobian matrix; For the first A Jacobian matrix; In this embodiment, the matrix is ​​a Gram matrix. and They have the same meaning.

[0056] matrix This is crucial for degradation detection, reflecting the strength of point cloud geometric constraints on pose variables. Its dimension is 6×6 (corresponding to 6 degrees of freedom pose: 3 rotational degrees of freedom + 3 translational degrees of freedom). Thus, the residual construction and optimization modeling based on the point-surface ICP model are completed.

[0057] The data processing module calculates the information matrix based on the Jacobian matrix and establishes the covariance matrix for pose state estimation based on the information matrix.

[0058] Disturbance variables The covariance matrix corresponding to the optimal value is: (38) In the formula, the information matrix H It describes the constraints of observation on pose variables, and the information matrix. H The inverse of represents the degree of divergence of the pose variables, which is the covariance matrix of the pose state estimation.

[0059] The degradation direction detection module decomposes the covariance matrix into translation submatrices and rotation submatrices.

[0060] The coupling relationship between rotation and translation is preserved during the covariance matrix decomposition process, thus obtaining the translation-rotation coupling strength. When the translation covariance submatrix and the rotation covariance submatrix are in an uncertain situation, the translation-rotation coupling strength is used to constrain the spatial direction of the degradation of the translation covariance submatrix and the spatial direction of the degradation of the rotation covariance submatrix.

[0061] Considering that the estimated variables for laser SLAM are 6-dimensional Lie algebra vectors (including 3D rotation and 3D translation), the covariance matrix is ​​divided into four sub-blocks in a 6×6 dimension: (39) in, (40) (41) Translation submatrix, describing the system in X / Y / Z Uncertainty regarding directional translation; : Rotation submatrix, describing the system's rotation around the rotation matrix. Roll / Pitch / Yaw The degree of constraint on the direction of rotation; and Cross-covariance between rotation and translation reflects the strength of their coupling.

[0062] In the formula, This refers to the sub-block corresponding to the rotational degrees of freedom in the information matrix; For each information matrix, there is a sub-block corresponding to the translational degrees of freedom. These are the cross sub-blocks in the information matrix that have been rotated to translation; For the cross sub-blocks in the information matrix that have been translated and rotated, and Both are used to characterize the coupling constraint relationship between translational and rotational degrees of freedom.

[0063] The process of detecting the degradation direction of translation is as follows: extract the translation sub-matrix from the covariance matrix, and decompose the translation sub-matrix into eigenvalues ​​and eigenvectors; when there is uncertainty in the laser SLAM system, and there is an eigenvalue of the translation sub-matrix that is greater than the set translation degradation judgment threshold, it is determined that the translation degree of freedom has degraded, and the eigenvector corresponding to the eigenvalue that is greater than the set translation degradation judgment threshold is taken as the spatial direction corresponding to the translation degradation.

[0064] Furthermore, extract the translation submatrix. Eigenvalue decomposition yields three eigenvalues. , , and their corresponding eigenvectors , , : (42) in, for The eigenvector matrix of the time-shifted submatrix for The diagonal matrix of eigenvalues ​​at time step. Setting the translation degradation criterion threshold. If a certain feature value Greater than This indicates insufficient point cloud constraints in this direction, resulting in significant system uncertainty, i.e., translational degradation. The eigenvalues ​​obtained from eigenvalue decomposition... , and It has a definite physical unit, namely the unit of the variance of the translation state. At this point, the direction of degradation can be determined by the corresponding eigenvector. Clear instructions. Figure 2 The system detects the axial degradation direction in a section of a hydroelectric power station's water transmission pipeline. The directionality is obvious, and the estimation uncertainty is much higher than that of other degrees of freedom.

[0065] Similarly, the rotational degradation direction detection process is as follows: extract the rotational submatrix from the covariance matrix, and perform eigenvalue and eigenvector decomposition on the rotational submatrix. When there are uncertainties in the laser SLAM system, and there is an eigenvalue of the rotational submatrix that is greater than a set rotational degradation threshold, then rotational degree of freedom degradation is determined to have occurred, and the eigenvector corresponding to the eigenvalue that is greater than the set rotational degradation threshold is taken as the spatial direction corresponding to the rotational degradation. For the rotational submatrix... Perform eigenvalue decomposition to obtain the uncertainty assessment results for the three rotational degrees of freedom: (43) in, for The eigenvector matrix of the rotating submatrix at each time step. for The diagonal matrix of eigenvalues ​​at each time step. If an eigenvalue is significantly greater than a set rotational degradation threshold, then degradation is determined to have occurred in the corresponding rotational degree of freedom. (Eigenvalues ​​of the rotational portion) , and It also has the same physical unit as the variance of the rotational state ( In actual testing, it was found that in vertical or inclined straight pipe sections, the point cloud changes are distributed in a ring around the axis, making it difficult to constrain rotation around the axis, which leads to degradation of the system in this degree of freedom. Figure 4 The result of identifying the rotational degradation direction of a certain inclined straight pipe section.

[0066] Example 3 In another embodiment of the present invention, a computer-readable storage medium is provided as a storage component within a terminal device, the function of which is to store programs and data. It should be noted that the computer-readable storage medium here encompasses not only the built-in storage components of the terminal device but also extended storage components supported by the device. Essentially, it is a tangible medium capable of containing or storing programs that can be invoked by or in conjunction with an instruction execution system, device, or apparatus. This storage medium provides storage areas for the terminal's operating system and stores one or more instructions suitable for processor loading and execution, which can constitute one or more computer programs containing program code.

[0067] Specifically, examples of computer-readable storage media (a non-exclusive list) include: electrical connections with one or more wires, portable disks, hard disks, random access memory, read-only memory, erasable programmable read-only memory, optical fibers, portable optical disc read-only memory, optical storage devices, magnetic storage devices, or any reasonable combination of the above types.

[0068] The storage medium may also include data signals propagated as part of a baseband portion or a carrier wave, carrying readable program code. Such propagated data signals may take various forms, including but not limited to electromagnetic signals, optical signals, or any reasonable combination of both. Furthermore, computer-readable storage medium may also refer to other readable media besides conventional readable storage media, capable of sending, propagating, or transmitting programs for use or operation by an instruction execution system, apparatus, or device. Program code on the storage medium can be transmitted via any suitable medium, including but not limited to wireless, wired, optical fiber, or any reasonable combination thereof.

[0069] The program code used to implement the operations of this invention can be written in any combination of one or more programming languages, including object-oriented programming languages ​​such as Java and C++, as well as conventional procedural programming languages ​​such as C. The execution modes of the program code include: running entirely on the user's computing device, running partially on the user's device as a standalone software package, running partially in a distributed manner on both the user's device and a remote computing device, or running entirely on a remote computing device or server. When a remote computing device is involved, the device can be connected to the user's computing device via any type of network such as a local area network (LAN) or a wide area network (WAN), or connected to an external computing device via the Internet through an Internet service provider.

[0070] The processor is capable of loading and executing one or more instructions stored in a computer-readable storage medium to implement the corresponding steps of the degradation direction detection method of the laser SLAM system described in Example 1.

[0071] Example 4 Figure 6 An internal structural diagram of a computer device is shown in one embodiment. This computer device may specifically be a terminal or a server. Figure 6As shown, the computer device includes a processor, memory, network interface, display, camera, and input device connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs in the non-volatile storage media. The network interface is used to communicate with external terminals via a network connection. When the computer program is executed by the processor, it implements a degradation direction detection method for a laser SLAM system. The display screen can be an LCD screen or an e-ink display screen. The input device can be a touch layer covering the display screen, buttons, a trackball, or a touchpad mounted on the computer device casing, or an external keyboard, touchpad, or mouse.

[0072] As will be understood by those skilled in the art, computer equipment Figure 6 The structure shown is merely a block diagram of a portion of the structure related to the present invention and does not constitute a limitation on the computer device to which the present invention is applied. Specific computing devices may include more or fewer components than those shown in the figure, or combine certain components, or have different component arrangements.

[0073] In one embodiment, a computer device is provided, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes the computer program to implement the steps of the method described above.

[0074] In one embodiment, a computer-readable storage medium is provided having a computer program stored thereon, which, when executed by a processor, implements the steps of the above-described method.

[0075] The various embodiments in this specification are described in a progressive manner. For directly identical or similar parts of the embodiments, refer to each other. Each embodiment focuses on its differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions in the method embodiments. It should be noted that the technical features of the above embodiments can be combined arbitrarily. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as the combination of these technical features does not contradict each other, it should be considered within the scope of this specification.

[0076] The above-described embodiments are merely preferred embodiments of the present invention, and while the descriptions are specific and detailed, they should not be construed as limiting the scope of the invention. It should be noted that those skilled in the art can make various improvements and substitutions without departing from the principles of the present invention, and these improvements and substitutions should also be considered within the scope of protection of the present invention. Therefore, the scope of protection of this invention should be determined by the scope of the claims.

Claims

1. A method for detecting the degradation direction in a laser SLAM system, characterized in that, include: Acquire three-dimensional point cloud data of the laser SLAM system, input the three-dimensional point cloud data into the point-surface matching model, and calculate the Jacobian matrix under the geometric degradation environment based on the point-surface matching model; The information matrix is ​​calculated based on the Jacobian matrix, and the covariance matrix for pose state estimation is established based on the information matrix. The covariance matrix is ​​decomposed into a translation submatrix and a rotation submatrix; The existence of uncertainties in the translational degree of freedom of the laser SLAM system is determined based on the translational submatrix, and the existence of uncertainties in the rotational degree of freedom of the laser SLAM system is determined based on the rotational submatrix. The spatial direction corresponding to the degradation is obtained by detecting the translational and rotational submatrixes under the uncertainty.

2. The degradation direction detection method for a laser SLAM system according to claim 1, characterized in that, The construction process of the point-surface matching model includes: Obtain all point cloud data of the current frame, fit a local neighborhood plane based on the neighborhood points of all point cloud data of the current frame, and determine the plane normal vector of the local neighborhood plane. The residuals corresponding to each point cloud data are calculated based on the orthogonal distance of the plane normal vectors. The residuals are linearized using Lie algebra to obtain the Jacobian matrix corresponding to each point cloud data. After stacking the residuals and Jacobian matrices corresponding to all point cloud data, a nonlinear least squares optimization objective function is constructed with the goal of minimizing the total sum of squared residuals, thus completing the construction of the point residual model.

3. The degradation direction detection method for a laser SLAM system according to claim 2, characterized in that, The optimization objective function is: In the formula, For pose perturbation variables; The total residual vector is formed by stacking the residuals of all point cloud data. The total Jacobian matrix is ​​formed by stacking the corresponding Jacobian matrices of all point cloud data. It is the square of the L2 norm.

4. The degradation direction detection method for a laser SLAM system according to claim 1, characterized in that, The information matrix is: In the formula, For information matrix; The total amount of data in the Jacobian matrix; For the first Transpose of a Jacobian matrix; For the first A Jacobian matrix; It is a Gram matrix.

5. The degradation direction detection method for a laser SLAM system according to claim 1, characterized in that, Based on the translation and rotation submatrices under uncertain conditions, the spatial direction corresponding to the degradation is obtained, including: The translation submatrix is ​​decomposed into eigenvalues ​​and eigenvectors. When the laser SLAM system has uncertainties and the eigenvalue of the translation submatrix is ​​greater than the set translation degradation judgment threshold, it is determined that the translation degree of freedom has degraded, and the eigenvector corresponding to the eigenvalue of the translation submatrix is ​​taken as the spatial direction corresponding to the translation degradation. The rotation submatrix is ​​decomposed into eigenvalues ​​and eigenvectors. When the laser SLAM system has uncertainties and the eigenvalue of the rotation submatrix is ​​greater than the set rotation degradation judgment threshold, it is determined that the rotational degree of freedom has degraded, and the eigenvector corresponding to the eigenvalue of the rotation submatrix is ​​taken as the spatial direction corresponding to the rotation degradation.

6. The degradation direction detection method for a laser SLAM system according to claim 5, characterized in that, The set translation degradation threshold and the set rotation degradation threshold are dynamically adjusted horizontally based on the measurement accuracy of the lidar and the environmental noise.

7. The degradation direction detection method for a laser SLAM system according to claim 5, characterized in that, The process of decomposing the covariance matrix to obtain the translation submatrix and the rotation submatrix also retains the coupling relationship between rotation and translation, thus obtaining the translation-rotation coupling strength; When there are uncertainties in the translation covariance submatrix and the rotation covariance submatrix, the spatial directions corresponding to the degradation of the translation covariance submatrix and the rotation covariance submatrix are constrained by the translation-rotation coupling strength.

8. A degradation direction detection system for a laser SLAM system, characterized in that, include: The data acquisition module acquires the three-dimensional point cloud data of the laser SLAM system, inputs the three-dimensional point cloud data into the point-surface matching model, and calculates the Jacobian matrix under the geometric degradation environment based on the point-surface matching model. The data processing module is used to calculate the information matrix based on the Jacobian matrix and to establish the covariance matrix for pose state estimation based on the information matrix. The degradation direction detection module is used to decompose the covariance matrix into a translation submatrix and a rotation submatrix; The existence of uncertainties in the translational degree of freedom of the laser SLAM system is determined based on the translational submatrix, and the existence of uncertainties in the rotational degree of freedom of the laser SLAM system is determined based on the rotational submatrix. The spatial direction corresponding to the degradation is obtained by detecting the translational and rotational submatrixes under the uncertainty.

9. A computer-readable storage medium for storing one or more programs, characterized in that, The one or more programs include instructions that, when executed by a computing device, cause the computing device to perform the degradation direction detection method of the laser SLAM system according to any one of claims 1 to 7.

10. A computing device, characterized in that, include: One or more processors, a memory, and one or more programs, wherein the one or more programs are stored in the memory and configured to be executed by the one or more processors, the one or more programs including a degradation direction detection method for performing any of the laser SLAM systems of claims 1 to 7.

Images