A laser SLAM degradation processing method based on double threshold detection and adaptive compensation

Abstract

The application discloses a kind of based on dual threshold detection and adaptive compensation laser SLAM degradation processing method, applied to degradation detection technical field. Including the following steps: construct dual detection mechanism, static threshold ensures the detection stability under normal scene, dynamic threshold adjustment judgment standard, realize the quick identification to gradual and instantaneous degradation;Design degradation deviation driven adaptive two-stage compensation strategy: first compensation is based on vector projection principle;Second compensation is according to the influence of unreliable laser constraint that dynamic weakening according to the degree of degradation deviation.This application is based on the dual threshold detection mechanism of real-time perception of scene features, real-time quantification of the severity of degradation deviation, and drives the start and intensity adjustment of compensation strategy, links dual detection mechanism and two-stage compensation strategy, can improve scene adaptability and detection comprehensiveness, can also dynamically adjust compensation intensity, significantly improve the pose estimation accuracy and robustness of system.

Description

Technical Field

[0001] This invention relates to the field of degradation detection technology, and more specifically to a laser SLAM degradation processing method based on dual threshold detection and adaptive compensation. Background Technology

[0002] With the rapid development of artificial intelligence and automation technologies, the field of positioning and navigation is also undergoing continuous innovation. Simultaneous Localization and Mapping (SLAM), as a key technology that endows mobile robots with environmental perception and autonomous navigation capabilities, enables the vehicle to build a map based on incremental information collected by sensors, even when the initial pose and environment are unknown, and simultaneously determine its real-time position on the map. LiDAR-based SLAM technology is widely used due to its significant advantages such as rapid and accurate acquisition of environmental information, strong anti-interference ability, and insensitivity to changes in lighting. However, in scenarios with monotonous geometry, sparse or highly repetitive texture features, such as coal mine roadways, indoor corridors, and tunnels, LiDAR suffers from limited scanning range and highly repetitive environmental features. Differences in point cloud distribution cause constraint imbalance, and this lack of constraint leads to significant cumulative drift in pose estimation in specific directions. Over time, the positioning error continues to expand, eventually resulting in a sharp decline in positioning accuracy and the system falling into a degradation dilemma. To overcome the localization challenge of laser SLAM algorithms in degraded scenarios, multi-source sensor fusion technology has gradually become a research focus. Among them, tightly coupled LIO, which combines LiDAR and Inertial Measurement Unit (IMU) for joint processing and considers their inherent connections and mutual influences, has become the mainstream direction.

[0003] Existing mainstream tightly coupled methods (such as LIO-SAM and FAST-LIO) typically employ static or slowly adjusted weight allocation strategies to globally fuse the measurement residuals of the lidar and IMU. However, SLAM degradation is often transient. When the constraint of the lidar in a specific direction momentarily fails, the system cannot respond quickly enough. It cannot promptly reduce the weight of the unreliable lidar constraint, nor can it simultaneously increase the compensation weight of the IMU in that direction. This results in unreliable geometric constraints continuously generating pose estimation errors, ultimately leading to trajectory deviation.

[0004] In the degradation detection stage, most existing technologies rely on a single fixed threshold method. This method determines degradation occurs simply by setting a fixed numerical threshold; if the eigenvalues ​​of the information matrix fall below this threshold during laser SLAM optimization, degradation has significant limitations, directly leading to severely insufficient detection accuracy. Firstly, threshold setting is highly dependent on human experience and specific scenarios, making it difficult to adapt to the normal fluctuation range of eigenvalues ​​under different environments. In complex and ever-changing real-world scenarios, a "threshold mismatch" problem easily arises. Secondly, fixed thresholds only focus on the static values ​​of eigenvalues, ignoring the crucial information contained in the dynamic changes of eigenvalues. For gradual degradation (such as a robot gradually entering a long corridor from a feature-rich area), fixed thresholds often trigger detection only when the degradation is already severe, missing the opportunity for early intervention. Conversely, for instantaneous degradation (such as a sudden obstruction of the lidar due to drastic fluctuations or a precipitous drop in eigenvalues), the threshold may not cover the extreme fluctuation range, leading to misjudgment and failing to promptly identify the rapid deterioration of the observed data quality. In summary, single fixed threshold detection cannot achieve comprehensive and accurate perception of different types of degradation, becoming one of the core bottlenecks restricting system reliability. Furthermore, current research on targeted compensation for degradation detection results is severely lacking. Even when existing solutions achieve degradation detection, they fail to establish a correlation mechanism between "detection results" and "compensation strategies," remaining limited to simple fusion of IMU and SLAM data using fixed weights. They cannot adjust compensation strategies based on dynamic information such as the direction and degree of degradation. This indiscriminate compensation method not only fails to alleviate pose estimation bias caused by degradation but may even introduce new errors due to improper compensation, further reducing system performance. Degradation detection in long corridors currently suffers from the following problems: degradation detection is lagging and inaccurate, relying on fixed thresholds and focusing only on static feature values. This fails to capture the gradual degradation of slowly decreasing feature values ​​and is prone to missed detections and misjudgments due to poor threshold adaptability; it cannot pinpoint the specific direction of degradation and lacks effective compensation methods that match the detection results, making it difficult to specifically address pose estimation bias. Therefore, providing a technical solution that combines "accurate degradation detection" with "adaptive directional compensation" is a problem urgently needing to be solved by those skilled in the art. Summary of the Invention

[0005] In view of this, the present invention provides a laser SLAM degradation processing method based on dual threshold detection and adaptive compensation. The dual threshold detection mechanism based on real-time perception of scene features quantifies the severity of degradation deviation in real time and drives the activation and intensity adjustment of the compensation strategy. The dual detection mechanism and the two-level compensation strategy are linked to solve the problems existing in degradation scenarios such as corridors and tunnels.

[0006] To achieve the above objectives, the present invention provides the following technical solution:

[0007] A laser SLAM degradation processing method based on dual threshold detection and adaptive compensation includes the following steps:

[0008] S1. Perform eigenvalue decomposition on the Hessian matrix to obtain eigenvalues ​​and eigenvectors. Use the magnitude of the eigenvalues ​​as a quantitative indicator of the constraint strength of each degree of freedom, and use the direction of the eigenvectors as a criterion for the determination of the degradation direction.

[0009] S2. Design a dual threshold detection mechanism based on real-time scene feature perception. Perform statistical analysis on the set of eigenvalues ​​of the Hessian matrix to calculate the static threshold. Use a sliding window to store the eigenvalue sequence, perform statistical analysis on the eigenvalue sequence within the sliding window, and calculate the dynamic threshold.

[0010] S3. A dual-threshold detection mechanism based on real-time scene feature perception quantifies the severity of degradation deviation in real time and calculates the deviation rate by combining static and dynamic thresholds. Laser SLAM degradation direction is detected based on static and dynamic thresholds, and based on the deviation rate. Assess the degree of degradation;

[0011] S4. The compensation strategy is initiated and its intensity is adjusted based on a dual threshold detection mechanism. This is done according to the degradation direction and degree detected in S2, and based on the deviation rate. Determine the degree of degradation and initiate a two-stage progressive compensation strategy, linking the dual detection mechanism with the two-stage compensation strategy, including:

[0012] When the deviation rate δ < 0.2, it is judged as slight degradation, and the first-level compensation based on the vector projection principle is initiated;

[0013] When the deviation rate δ≥0.2, it is judged as moderate or above degradation, and secondary compensation based on IMU adaptive weighted fusion is initiated.

[0014] Optionally, the static threshold detection in S2 specifically includes:

[0015] Statistical analysis was performed on the eigenvalue set of the Hessian matrix, and the minimum value of the normal eigenvalues ​​was selected. Multiply by safety factor To obtain the static threshold :

[0016] ;

[0017] Since different degrees of freedom of motion have different sensitivities to constraints, a static threshold needs to be set independently for each degree of freedom. The feature values ​​obtained by optimizing the current frame and Compare, if If so, the system initially determines that there is degradation in that direction.

[0018] Optionally, the dynamic threshold detection in S2 specifically includes:

[0019] A sliding window of fixed size is used to store the recent eigenvalue sequence. The window update rule is as follows: when the amount of data in the window is less than SIZE, new feature values ​​are directly added to the window, and all historical data are retained; when the amount of data in the window reaches SIZE, new feature values ​​are added to the window, and the earliest added feature values ​​are removed from the window, so as to maintain a constant amount of data in the window.

[0020] Perform statistical analysis on the sequence of eigenvalues ​​within the sliding window and calculate its first quartile. and the third and fourth quartiles Set dynamic threshold As the lower bound of the normal range of eigenvalues, the formula is:

[0021] ;

[0022] In the formula, hyperparameters Control the detection sensitivity if the current frame feature value satisfy If so, it is determined that there is dynamic degradation in that direction.

[0023] Optionally, the detection of laser SLAM degradation direction and degradation degree in S3 is as follows:

[0024] Introducing deviation rate As a core quantitative indicator of the degree of degradation, the deviation rate It directly reflects the reliability of the laser odometer; to quantify the degree of degradation in the detection, a static deviation value is defined. Dynamic deviation value :

[0025] ;

[0026] ;

[0027] By combining static and dynamic detection results, a unified quantitative index for the degree of degradation is provided, and the comprehensive deviation value is defined as the deviation rate. :

[0028] ;

[0029] In the formula, For scene coefficients.

[0030] Optionally, the first-level compensation in S4 is as follows:

[0031] Based on the degradation direction accurately identified by the dual threshold mechanism, the constraint vector in the degradation direction is projected to other motion dimensions that have not undergone degradation. The remaining effective constraint part is used to offset the impact of degradation. By projecting the error component in the original degradation direction to the reliable dimension, the error is effectively suppressed.

[0032] Optionally, the secondary compensation in S4 is as follows:

[0033] The high-frequency pose data acquired by IMU integration is fused with the unreliable pose estimation from laser odometry in the degradation direction. The fusion weights are dynamically adjusted using the deviation rate δ output by a dual threshold mechanism to optimize the fusion effect. The fusion formula is as follows:

[0034] ;

[0035] In the formula, It is the compensated pose. The initial pose is provided by laser SLAM. It is the pose calculated by IMU integration. It is a weighting coefficient that is adaptively adjusted according to the degree of degradation.

[0036] Optional, weighting coefficient Dynamically calculated based on the deviation rate δ:

[0037] ;

[0038] in: The degradation deviation rate of the current frame; This represents the minimum value of the IMU weights, used for scenarios with no or slight degradation. This is the maximum value of the IMU weights, used to limit the maximum weights in cases of severe degradation, preventing the IMU from becoming overly dominant.

[0039] As can be seen from the above technical solution, compared with the prior art, the present invention provides a laser SLAM degradation processing method based on dual threshold detection and adaptive compensation, which has the following beneficial effects: The present invention covers both progressive and transient degradation through a dual threshold detection mechanism based on real-time scene feature perception, which can identify the degradation state in real time and accurately locate the degradation direction and quantify the degradation degree, solving the problems of traditional detection lag, misjudgment, and unclear direction; the deviation rate δ of the dual threshold detection output is used to dynamically adjust the fusion weight. The more severe the degradation degree (the larger the deviation rate δ), the higher the weight ratio of the IMU to strengthen its pose constraint effect. The weight of the laser odometry is reduced to minimize interference from unreliable information; conversely, the contribution of the laser odometry is moderately retained. Through adaptive weighted fusion, the directional correction of the laser odometry output is achieved, effectively suppressing positioning drift and ensuring high pose estimation accuracy even in severely degraded scenarios. By driving the activation and intensity adjustment of the compensation strategy through dual detection, the dual detection mechanism and the two-level compensation strategy are linked together. Based on the deep synergy between dual detection and adaptive compensation, full-scene coverage of degraded scenarios such as corridors and tunnels can be achieved, significantly improving the detection accuracy and compensation effectiveness of the laser SLAM system and greatly enhancing the robustness of the system in complex environments. Attached Figure Description

[0040] 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 only embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort.

[0041] Figure 1 This is a flowchart of the laser SLAM degradation treatment method based on dual threshold detection and adaptive compensation of the present invention;

[0042] Figure 2 This is a flowchart of the dual degradation detection mechanism of the present invention;

[0043] Figure 3 This is a schematic diagram of the dual degradation detection results in an embodiment of the present invention;

[0044] Figure 4 This is a schematic diagram of absolute error comparison in an embodiment of the present invention. Detailed Implementation

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

[0046] This invention discloses a laser SLAM degradation processing method based on dual threshold detection and adaptive compensation, such as... Figure 1 As shown, it includes the following steps:

[0047] S1. Perform eigenvalue decomposition on the Hessian matrix to obtain eigenvalues ​​and eigenvectors. Use the magnitude of the eigenvalues ​​as a quantitative indicator of the constraint strength of each degree of freedom, and use the direction of the eigenvectors as a criterion for the determination of the degradation direction.

[0048] S2. Design a dual threshold detection mechanism based on real-time scene feature perception. Perform statistical analysis on the Hessian matrix eigenvalue set to calculate the static threshold; use a sliding window to store the eigenvalue sequence, and perform statistical analysis on the eigenvalue sequence within the sliding window to calculate the dynamic threshold; the dual threshold detection mechanism is as follows: Figure 2 As shown;

[0049] S3. A dual-threshold detection mechanism based on real-time scene feature perception quantifies the severity of degradation deviation in real time and calculates the deviation rate by combining static and dynamic thresholds. Laser SLAM degradation direction is detected based on static and dynamic thresholds, and based on the deviation rate. Assess the degree of degradation;

[0050] S4. The compensation strategy is initiated and its intensity is adjusted based on a dual threshold detection mechanism. This is done according to the degradation direction and degree detected in S2, and based on the deviation rate. Determine the degree of degradation and initiate a two-stage progressive compensation strategy, linking the dual detection mechanism with the two-stage compensation strategy, including:

[0051] When the deviation rate δ < 0.2, it is judged as slight degradation, and the first-level compensation based on the vector projection principle is initiated;

[0052] When the deviation rate δ≥0.2, it is judged as moderate or above degradation, and secondary compensation based on IMU adaptive weighted fusion is initiated.

[0053] In this embodiment of the invention, S1 specifically refers to:

[0054] Calculate the Hessian matrix:

[0055] ;

[0056] ;

[0057] In the formula, It is the n×6 Jacobian matrix of the residual equation with respect to the optimization variables. Each row corresponds to the partial derivative of the residual function with respect to the six pose degrees of freedom (roll, pitch, yaw, x-axis translation, y-axis translation, z-axis translation). Let n be the residual vector. It is a 6-dimensional pose increment vector;

[0058] Eigenvalue decomposition of the Hessian matrix is ​​performed, and the magnitude of the eigenvalues ​​directly quantifies the reliability of the pose solution at the corresponding direction or rotation angle. By analyzing the eigenvalues ​​and eigenvectors of the Hessian matrix, a basis for degradation detection and compensation is provided. Specifically:

[0059] By using the Jacobian rotation method, a series of orthogonal rotation matrices are iteratively constructed to gradually eliminate the off-diagonal elements of the Hessian matrix, ultimately transforming the Hessian matrix into a diagonal matrix and obtaining the corresponding eigenvector matrix. The specific decomposition result satisfies:

[0060] ;

[0061] In the formula, These are orthogonal eigenvectors. yes The i 1 eigenvector It is a diagonal matrix. yes The i One eigenvalue;

[0062] The magnitude of eigenvalues ​​reflects the constraint strength of a given degree of freedom. In feature-rich, non-degenerate environments, point clouds provide strong constraints across all degrees of freedom, resulting in a full-rank Hessian matrix. When degradation occurs, the Hessian matrix exhibits singular or near-singular characteristics in the corresponding degree of freedom dimension. Specifically, the eigenvalue corresponding to that degree of freedom is significantly smaller than other eigenvalues, directly reflecting a sharp decrease in constraint strength in that direction. Therefore, the eigenvalues ​​and eigenvectors of the Hessian matrix have a clear mathematical relationship with constraints in laser SLAM systems: the magnitude of the eigenvalue directly maps to the strength of the constraint, while the direction of the eigenvector indicates the specific motion dimension under which the constraint applies. When a certain eigenvalue decreases significantly, its corresponding eigenvector can accurately identify the degree of freedom where the constraint is missing. Based on this characteristic, analyzing the eigenvalues ​​and eigenvectors of the Hessian matrix provides a more scientific and dynamic basis for degradation detection and compensation.

[0063] Furthermore, the static threshold detection in S2 specifically involves:

[0064] The core objective of static threshold detection is to quickly filter out degraded frames with severely lacking constraints. Its technical principle is based on the quantitative relationship between the eigenvalues ​​of the Hessian matrix and the constraint strength: when the eigenvalue corresponding to a certain degree of freedom of motion is too small, it indicates that the point cloud features in that direction cannot provide effective constraints, resulting in low pose calculation reliability, i.e., a risk of degradation. The static threshold needs to be determined through offline statistics to ensure that the eigenvalue is consistently higher than the threshold under normal operating conditions and significantly lower than the threshold during severe degradation.

[0065] Statistical analysis was performed on the eigenvalue set of the Hessian matrix, and the minimum value of the normal eigenvalues ​​was selected. Multiply by safety factor To obtain the static threshold :

[0066] ;

[0067] Since different degrees of freedom of motion have different sensitivities to constraints, a static threshold needs to be set independently for each degree of freedom. The feature values ​​obtained by optimizing the current frame and Compare, if If so, the system initially determines that there is degradation in that direction.

[0068] In this embodiment of the invention, under non-degradable conditions (such as structured environments or regions rich in point cloud features), pose calculation results of no less than 500 frames are collected, and the Hessian matrix eigenvalues ​​corresponding to each degree of freedom of motion in each frame are extracted to form a normal eigenvalue set. Among them, the safety factor The value ranges from 0.5 to 0.8. The stronger the scene interference, the better. The smaller the value, the lower the probability of misjudgment caused by normal fluctuations.

[0069] Furthermore, the dynamic threshold detection in S2 specifically involves:

[0070] To address the issues of static thresholding failing to capture gradual degradation and being prone to misjudgment, an interquartile range (IQR) algorithm based on a sliding window is introduced to construct an anomaly detection model for dynamically changing feature values. This model adaptively defines a "normal range" by analyzing the statistical distribution of historical feature value sequences, focusing on identifying both gradual degradation with slowly decreasing feature values ​​and instantaneous interference from sharp fluctuations, thus compensating for the scene adaptability limitations of static thresholding.

[0071] A sliding window of fixed size is used to store the recent eigenvalue sequence. The window update rule is as follows: when the amount of data in the window is less than SIZE, new feature values ​​are directly added to the window, and all historical data are retained; when the amount of data in the window reaches SIZE, new feature values ​​are added to the window, and the earliest added feature values ​​are removed from the window, so as to maintain a constant amount of data in the window.

[0072] Perform statistical analysis on the sequence of eigenvalues ​​within the sliding window and calculate its first quartile. (25th percentile of the sequence) and third quartile (75th percentile of sequence), set dynamic threshold As the lower bound of the normal range of eigenvalues, the formula is:

[0073] ;

[0074] In the formula, hyperparameters Control the detection sensitivity if the current frame feature value satisfy If so, it is determined that there is dynamic degradation in that direction.

[0075] Furthermore, the detection of laser SLAM degradation direction and degree in S3 is specifically as follows:

[0076] A dual detection mechanism combining static thresholding and a sliding window IQR algorithm is employed to achieve comprehensive determination of degradation events. In this mechanism, the static thresholding module provides a rapid response to significant degradation, while the IQR algorithm adaptively captures gradual or transient degradation trends by analyzing the historical distribution of feature values. The two mechanisms work together to ensure the system's robustness and timeliness in various degradation scenarios.

[0077] Introducing deviation rate As a core quantitative indicator of the degree of degradation, the deviation rate It directly reflects the reliability of the laser odometer; to quantify the degree of degradation in the detection, a static deviation value is defined. Dynamic deviation value :

[0078] ;

[0079] ;

[0080] By combining static and dynamic detection results, a unified quantitative index for the degree of degradation is provided, and the comprehensive deviation value is defined as the deviation rate. :

[0081] ;

[0082] In the formula, For scene coefficients.

[0083] In this embodiment of the invention, when the environment is stable, The value is set to 0.2~0.4, focusing on dynamic detection to adapt to gradual degradation; in the event of drastic environmental changes or sudden disturbances, The value is set to 0.6~0.8, focusing on static detection to quickly respond to significant degradation;

[0084] Through effective point cloud ratio To determine whether there are drastic changes or sudden disturbances in the environment, the effective point cloud ratio is... As a core quantitative indicator of laser odometry observation data quality, it directly reflects the integrity of the scene structure and the validity of sensor data. The dynamic adjustment provides an objective basis:

[0085] ;

[0086] In the formula, For the effective number of point clouds, This represents the total number of point clouds in the current frame; under normal conditions, It typically stabilizes between 0.6 and 0.9 (for structured indoor scenes, it's usually > 0.8; for cluttered outdoor scenes, it's usually 0.6 to 0.7); if This can be determined by drastic environmental changes (such as moving from an indoor structured scene to a cluttered outdoor scene) or sudden interference (such as contamination of the sensor lens leading to a sharp reduction in effective points).

[0087] Furthermore, the first-level compensation in S4 is specifically as follows:

[0088] Based on the degradation direction accurately identified by the dual threshold mechanism (based on Hessian matrix eigenvector analysis), the constraint vectors in the degradation direction are projected to other motion dimensions that have not undergone degradation. The remaining effective constraint parts are used to offset the effects of degradation. By projecting the error components in the original degradation direction to reliable dimensions, the error can be effectively suppressed.

[0089] Specifically: First, the degradation direction is extracted from the 6×6 Hessian matrix H. The original constraint vector :

[0090] ;

[0091] The reliable direction matrix is Construct orthogonal projection matrix To ensure that projection is only onto reliable dimensions, because Direct projection is zero; reliable observations (such as ground matching points) are extracted to construct... Reconstructing constraints Full rank, lack of compensation constraints.

[0092] Furthermore, the secondary compensation in S4 is specifically as follows:

[0093] The high-frequency pose data acquired by IMU integration is fused with the unreliable pose estimation from laser odometry in the degradation direction. The fusion weights are dynamically adjusted using the deviation rate δ output by a dual threshold mechanism to optimize the fusion effect. The fusion formula is as follows:

[0094] ;

[0095] In the formula, It is the compensated pose. The initial pose is provided by laser SLAM. It is the pose calculated by IMU integration. It is a weighting coefficient that is adaptively adjusted according to the degree of degradation.

[0096] Furthermore, weighting coefficients Dynamically calculated based on the deviation rate δ:

[0097] ;

[0098] in: The degradation deviation rate of the current frame; This represents the minimum value of the IMU weights, used for scenarios with no or slight degradation. This is the maximum value of the IMU weights, used to limit the maximum weights in cases of severe degradation, preventing the IMU from becoming overly dominant.

[0099] In an embodiment of the present invention, The value is 0.1. The value is set to 0.9; a larger δ indicates lower reliability of the laser odometry, and the IMU weight increases accordingly, thus making IMU data the dominant factor in severe degradation and improving the robustness of pose estimation. The compensation strategy uses the deviation rate δ as the quantitative basis to achieve on-demand compensation and avoid premature or late start-up; the compensation process is adaptive: the IMU weighting coefficients are dynamically adjusted according to the degree of degradation, improving the adaptability to the dynamic characteristics of complex scenes.

[0100] In one embodiment of the present invention, an experimental environment was built on a VMware virtual machine platform, and a dataset of an indoor long corridor with a total length of 616.45 meters was selected for testing. This dataset contains round-trip motion trajectories, and there are many corridor sections with sparse point cloud features and prone to degradation, which is suitable for validating the effectiveness of algorithms in degradation scenarios. The FAST-LIO2 algorithm, the LIO-SAM algorithm, and the algorithm proposed in this embodiment of the present invention were compared in experiments, and mapping and trajectory estimation were performed based on the same dataset.

[0101] The results of the dual degradation test are as follows Figure 3As shown in the figure. Due to the introduction of the IQR algorithm, the system can effectively identify anomalies in the feature value sequence that exhibit significant dynamic changes (such changes may originate from moving object interference or increased sensor noise), even if the values ​​are much higher than the static threshold, they can still be accurately detected. The absolute error comparison chart is shown below. Figure 4 As shown in the figure. Experimental results show that the absolute error generated by the algorithm of this invention is smaller than that of the LIO-SAM and FAST-LIO2 algorithms throughout the entire process, demonstrating higher estimation accuracy. At the same time, the deviation fluctuation amplitude is significantly reduced during the operation, indicating that its pose solution is more stable and the estimated trajectory fits the real path better.

[0102] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on the differences from other embodiments. The same or similar parts between the various embodiments can be referred to each other.

[0103] Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A laser SLAM degradation processing method based on dual threshold detection and adaptive compensation, characterized in that, Includes the following steps: S1. Perform eigenvalue decomposition on the Hessian matrix to obtain eigenvalues ​​and eigenvectors. Use the magnitude of the eigenvalues ​​as a quantitative indicator of the constraint strength of each degree of freedom, and use the direction of the eigenvectors as a criterion for the determination of the degradation direction. S2. Design a dual threshold detection mechanism based on real-time perception of scene features, perform statistical analysis on the set of Hessian matrix eigenvalues, and calculate the static threshold. A sliding window is used to store the feature value sequence. Statistical analysis of the feature value sequence within the sliding window is performed to calculate the dynamic threshold. S3. A dual-threshold detection mechanism based on real-time scene feature perception quantifies the severity of degradation deviation in real time and calculates the deviation rate by combining static and dynamic thresholds. Laser SLAM degradation direction is detected based on static and dynamic thresholds, and based on the deviation rate. Assess the degree of degradation; S4. The compensation strategy is initiated and its intensity is adjusted based on a dual threshold detection mechanism. This is done according to the degradation direction and degree detected in S3, and based on the deviation rate. Determine the degree of degradation and initiate a two-stage progressive compensation strategy, linking the dual detection mechanism with the two-stage compensation strategy, including: When the deviation rate δ < 0.2, it is judged as slight degradation, and the first-level compensation based on the vector projection principle is initiated; When the deviation rate δ≥0.2, it is judged as moderate or above degradation, and secondary compensation based on IMU adaptive weighted fusion is initiated; The static threshold detection in S2 is specifically as follows: Statistical analysis was performed on the eigenvalue set of the Hessian matrix, and the minimum value of the normal eigenvalues ​​was selected. Multiply by safety factor To obtain the static threshold : ； Since different degrees of freedom of motion have different sensitivities to constraints, a static threshold needs to be set independently for each degree of freedom. The feature values ​​obtained by optimizing the current frame and Compare, if If so, the system initially determines that there is degradation in that direction; The dynamic threshold detection in S2 is specifically as follows: A sliding window of fixed size is used to store the recent eigenvalue sequence. The window update rule is as follows: when the amount of data in the window is less than SIZE, new feature values ​​are directly added to the window, and all historical data are retained; when the amount of data in the window reaches SIZE, new feature values ​​are added to the window, and the earliest added feature values ​​are removed from the window, so as to maintain a constant amount of data in the window. Perform statistical analysis on the sequence of eigenvalues ​​within the sliding window and calculate its first quartile. and the third and fourth quartiles Set dynamic threshold As the lower bound of the normal range of eigenvalues, the formula is: ； In the formula, hyperparameters Control the detection sensitivity if the current frame feature value satisfy If so, it is determined that there is dynamic degradation in that direction.

2. The laser SLAM degradation processing method based on dual threshold detection and adaptive compensation according to claim 1, characterized in that, The specific steps for detecting the degradation direction and degree of laser SLAM in S3 are as follows: Introducing deviation rate As a core quantitative indicator of the degree of degradation, the deviation rate It directly reflects the reliability of the laser odometer; to quantify the degree of degradation in the detection, a static deviation value is defined. Dynamic deviation value : ； ； By combining static and dynamic detection results, a unified quantitative index for the degree of degradation is provided, and the comprehensive deviation value is defined as the deviation rate. : ； In the formula, For scene coefficients.

3. The laser SLAM degradation processing method based on dual threshold detection and adaptive compensation according to claim 1, characterized in that, The first-level compensation in S4 is as follows: Based on the degradation direction accurately identified by the dual threshold mechanism, the constraint vector in the degradation direction is projected to other motion dimensions that have not undergone degradation. The remaining effective constraint part is used to offset the impact of degradation. By projecting the error component in the original degradation direction to the reliable dimension, the error is effectively suppressed.

4. The laser SLAM degradation processing method based on dual threshold detection and adaptive compensation according to claim 1, characterized in that, The secondary compensation in S4 is specifically as follows: The high-frequency pose data obtained by IMU integration is fused with the unreliable pose estimation of the laser odometry in the degradation direction. The deviation rate δ output by the dual threshold mechanism is used to dynamically adjust the fusion weights to optimize the fusion effect. The fusion formula is as follows: ； In the formula, It is the compensated pose. The initial pose is provided by laser SLAM. It is the pose calculated by IMU integration. It is a weighting coefficient that is adaptively adjusted according to the degree of degradation.

5. The laser SLAM degradation processing method based on dual threshold detection and adaptive compensation according to claim 4, characterized in that, Weighting coefficient Dynamically calculated based on the deviation rate δ: ； in: The degradation deviation rate of the current frame; This represents the minimum value of the IMU weights, used for scenarios with no or slight degradation. This is the maximum value of the IMU weights, used to limit the maximum weights in cases of severe degradation, preventing the IMU from becoming overly dominant.

Images