An adaptive anti-degradation laser inertial vision fusion positioning method

Abstract

The application provides an adaptive anti-degradation laser inertial vision fusion positioning method, and belongs to the technical field of simultaneous localization and mapping. The method comprises the following steps: S1, data preprocessing; S2, fine degradation analysis; S3, continuous adaptive weight calculation; S4, direction selectivity factor graph optimization; S5, time smoothing and output: low-pass filtering is performed on the calculated weight sequence in the time domain, and the optimized pose is output after smoothing, as the final positioning result. By using a continuous weight function instead of discrete switch logic, the method enables the sensor weight to continuously and smoothly change with the change of the environmental constraint strength, so as to eliminate trajectory sawtooth, changes the extensive mode of “overall judgment and overall processing”, can independently evaluate and distribute the weight of six degrees of freedom of translation and rotation, fully utilizes the sensor information, and outputs smooth and high-precision pose estimation in various degradation scenes.

Description

Technical Field

[0001] This invention relates to the field of synchronous positioning and mapping technology, specifically to an adaptive and degrade-resistant laser inertial vision fusion positioning method. Background Technology

[0002] With the rapid development of artificial intelligence and robotics, autonomous navigation capabilities of mobile terminals have become a core requirement for autonomous vehicles, intelligent drones, inspection robots, and augmented reality (AR) devices. Simultaneous Localization and Mapping (SLAM), as a key technology enabling machines to "perceive their environment and locate themselves," directly determines the autonomy and safety of mobile platforms. To adapt to complex and ever-changing real-world environments, single-sensor solutions are insufficient to meet the requirements of all-weather, high-precision operations. While LiDAR provides accurate ranging, it is prone to positioning drift or even failure in "degraded scenarios" with simple geometric structures, such as long corridors, tunnels, or open squares, due to insufficient constraints. Visual sensors, while providing rich texture information, are severely limited by drastic changes in lighting, motion blur, and weakly textured environments. Inertial Measurement Units (IMUs), while offering good short-term dynamic response and unaffected by external environmental interference, suffer from inherent bias drift characteristics that prevent them from operating independently for extended periods. Therefore, multi-sensor fusion systems integrating laser, visual, and inertial information have become the mainstream solution in academia and industry. However, existing fusion frameworks still face significant technical bottlenecks in addressing sensor degradation issues. The LVI-SAM method proposed by T. Shan, B. Englot, C. Ratti and D. Rus in "LVI-SAM: Tightly-coupled Lidar-Visual-Inertial Odometry via Smoothing and Mapping" is the mainstream approach. This method often employs a hard-switching strategy based on a fixed threshold, that is, deciding whether to trust laser data by judging whether the feature value is below a certain set value. This discontinuous "hard logic" ignores the gradual characteristics of environmental degradation, causing the system to oscillate violently between different modes in the critical state, resulting in severe "sawtooth effect" and jumps in the output trajectory. This not only reduces positioning accuracy, but also brings great uncertainty and safety hazards to downstream path planning and motion control.

[0003] J. Lee et al. proposed the Switch-SLAM scheme in "Switch-SLAM: Switching-Based LiDAR-Inertial-Visual SLAM for Degenerate Environments." While it can identify specific degradation directions (such as the direction of travel in a long corridor) through Hessian matrix eigenvalue analysis, its processing strategy is holistic. Once a degradation direction is determined, the system switches to visual odometry as the initial guess in all six degrees of freedom, or completely removes the laser constraint in the degradation direction during optimization. Essentially, it employs a binary "switch" logic: for the degradation direction, the weight of the laser constraint is reduced to zero, relying entirely on vision. This approach fails to fully utilize the characteristics of directional degradation, completely discarding the high-precision absolute measurement information of the laser in the degradation direction, resulting in information waste.

[0004] In summary, existing solutions for lidar in near-degradable environments result in high-frequency switching between degradable and non-degradable states, leading to persistent high-frequency jitter in the output pose trajectory, known as the "trajectory jaggedness" effect. This is extremely detrimental to downstream path planning and motion control modules. Therefore, developing a robust fusion method capable of continuously sensing the degree of environmental degradation and adaptively and smoothly adjusting sensor weights has become a key challenge in improving the environmental adaptability of mobile robots. Summary of the Invention

[0005] To address the shortcomings of existing technologies, this invention provides an adaptive and anti-degradation laser-inertial-visual fusion positioning method. It innovatively introduces an adaptive anti-degradation module and conducts performance verification, which can effectively alleviate the problem of positioning accuracy attenuation caused by sensor degradation, significantly improve the mapping accuracy and environmental adaptability of the laser-visual SLAM system, and enhance the robustness of the system in complex scenarios.

[0006] To solve the above problems, the present invention adopts the following technical solution: An adaptive, degradation-resistant laser-inertial-visual fusion localization method is described, comprising the following steps: S1: Data preprocessing: Preprocess the camera images, IMU data, and LiDAR point clouds respectively to generate preliminary relative pose estimation; S2: Refined Degradation Analysis: The preprocessed lidar point cloud data is input into the lidar odometry submodule. The lidar odometry submodule uses Scan-to-Map matching. During the matching process, the Hessian matrix H is extracted. The matrix H is divided into blocks to separate the translation submatrix H_trans and the rotation submatrix H_rot. The constraint strength of each submatrix in the x, y, and z coordinate axes is calculated to obtain the constraint strength score S_i for each degree of freedom. S3: Continuous adaptive weight calculation: Normalize the constraint strength score S_i of each degree of freedom in S2 and input a continuous weight mapping function; S4: Direction Selectivity Factor Graph Optimization: Construct a factor graph, where the state nodes in the factor graph represent the robot's pose. Construct a block diagonal covariance matrix, where each element on the diagonal is adaptively adjusted according to the laser weight in that direction. S5: Time Smoothing and Output: The calculated weight sequence is subjected to low-pass filtering in the time domain, and the optimized pose is output after smoothing as the final localization result.

[0007] Furthermore, in S1, the lidar point cloud is subjected to surface and corner feature extraction; the camera image is subjected to feature extraction and tracking; the IMU data is pre-integrated; the pre-processed lidar point cloud data is input into the lidar odometry submodule; and the pre-processed camera image and IMU data are input into the visual inertial odometry submodule to generate a preliminary relative pose estimate.

[0008] Furthermore, in S2, both the translation submatrix H_trans and the rotation submatrix H_rot are 3×3 matrices. H_trans uses the eigenvalue projection method to calculate its constraint strength in the x, y, and z coordinate axes. By calculating the projection components of each eigenvector on the coordinate axis, weighting them with the corresponding eigenvalues, and summing them, the total constraint strength score S_x, S_y, and S_z for that coordinate axis is obtained. H_rot uses the eigenvalue projection calculation method to calculate its constraint strength in the roll, pitch, and yaw rotational degrees of freedom, obtaining constraint strength scores S_roll, S_pitch, and S_yaw.

[0009] Furthermore, in S3, the weight mapping function adopts the Sigmoid function, as shown below: ; Where k is the kurtosis factor, which controls the speed of the transition; The degradation threshold; This refers to the fusion weight of the lidar in the i-th degree of freedom, and its value varies continuously between 0 and 1.

[0010] Furthermore, the complementary weights of the visual sensors in the visual-inertial odometry submodule on this degree of freedom are represented as follows: ; Set a lower limit w_min for the weights to ensure that laser information is not completely discarded even in cases of severe degradation.

[0011] Furthermore, in S4, the laser radar point cloud is preprocessed and output as a laser factor, and the camera image and IMU data are preprocessed and output as a visual inertial factor. The block diagonal covariance matrix constructed by the laser factor is Σ_lidar, and the block diagonal covariance matrix constructed by the visual inertial factor is Σ_vision. The laser factor and the visual inertial factor are added to the factor graph and solved using the iSAM2 optimizer.

[0012] Furthermore, for the diagonal covariance matrix Σ_lidar of the laser factor, each element on the diagonal is adaptively adjusted according to the laser weight in that direction, as shown below: ; Among them, the larger the weight, the smaller the allocated covariance, and the greater its influence on the optimization; For the adaptive covariance matrix Σ_vision of the visual inertia factor, its diagonal elements correspond to the visual weights, as shown below: ; in, The diagonal of the fundamental covariance matrix represents the first... i One element, This represents extremely small positive numbers to prevent the denominator from being 0.

[0013] Furthermore, in S5, the low-pass filter calculation method in the time domain is as follows: ; in, Indicates the current time t The weight values ​​after filtering Represents the smoothing factor. This represents the original weight value.

[0014] This invention can effectively alleviate the problem of positioning accuracy degradation caused by sensor degradation, significantly improve the mapping accuracy and environmental adaptability of the laser-vision SLAM system, and enhance the robustness of the system in complex scenarios, as detailed below: 1. This invention patent changes the crude "overall judgment and overall processing" mode of existing technologies, enabling independent degradation assessment and weight allocation for six degrees of freedom: translation (x, y, z) and rotation (roll, pitch, yaw). It achieves a positioning method that can continuously and precisely sense the geometric constraint strength on each degree of freedom, and accordingly perform directional selective and continuous adaptive weighted fusion of sensor information. This fully utilizes sensor information and outputs smooth and high-precision pose estimation under various degradation scenarios.

[0015] 2. This invention replaces discrete switching logic with a continuous weighting function. This method enables the sensor weights to change continuously and smoothly with the change of environmental constraint intensity, thereby eliminating the jaggedness of the trajectory.

[0016] 3. This invention abandons the simplistic logic of "complete trust" or "complete discard". Even for severely degraded directions, it retains a minimum weight for laser measurement and makes full use of the absolute scale information provided by the laser to suppress visual scale drift. Attached Figure Description

[0017] To more clearly illustrate the specific embodiments of the present invention, the accompanying drawings used in the description of the specific embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are some embodiments of the present invention. For those skilled in the art, other drawings can be obtained from these drawings without creative effort.

[0018] Figure 1 This is a flowchart of an embodiment of the present invention; Figure 2 The laser mapping effect of adding an adaptive anti-degradation module in an embodiment of the present invention; Figure 3 A comparison diagram of the true trajectory before and after adding the adaptive anti-degradation module in an embodiment of the present invention; Figure 4 A magnified view of the true value trajectory before and after adding the adaptive anti-degradation module in an embodiment of the present invention; Figure 5 A comparison chart of error indicators before and after adding the adaptive anti-degradation module in an embodiment of the present invention; Figure 6 The box topographic diagrams are shown before and after the addition of the adaptive anti-degradation module in an embodiment of the present invention. Detailed Implementation

[0019] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0020] like Figure 1As shown, this invention provides an adaptive anti-degradation laser inertial vision fusion localization method. Based on the LVI-SAM algorithm, this method innovatively introduces an adaptive anti-degradation module and conducts performance verification. Specifically, the method includes the following steps: S1: Data Preprocessing Feature extraction (face points and corner points) is performed on the lidar point cloud; feature extraction and tracking are performed on the camera images; and pre-integration is performed on the IMU data. The preprocessed data are then input into the lidar odometry submodule and the visual inertial odometry (VINS) submodule to generate a preliminary relative pose estimate.

[0021] S2: Refined Degradation Analysis From the Scan-to-Map matching process of the laser odometry, the Hessian matrix H is extracted. The H matrix is ​​then divided into blocks, separating the translation submatrix H_trans (3×3) and the rotation submatrix H_rot (3×3). For H_trans, its constraint strength along the x, y, and z coordinate axes is calculated. The strength index is not limited to diagonal elements; the eigenvalue projection method can be used: calculate the projection components of each eigenvector onto the coordinate axis, weight them with the corresponding eigenvalues, and sum them to obtain the total constraint strength score S_x, S_y, S_z for that coordinate axis. A similar process is performed on H_rot to obtain the constraint strength scores S_roll, S_pitch, and S_yaw for the three rotational degrees of freedom: roll, pitch, and yaw.

[0022] S3: Continuous Adaptive Weight Calculation Normalize the constraint strength score S_i for each degree of freedom and input it into a continuous weight mapping function. The preferred function is the Sigmoid function.

[0023] Where k is the kurtosis factor, which controls the speed of transition; θ is the degradation threshold. w_lidar_i is the fusion weight of the lidar in the i-th degree of freedom, and its value varies continuously between 0 and 1.

[0024] The complementary weights of the visual sensor in this degree of freedom are: w_vision_i = 1 - w_lidar_i.

[0025] Set a lower limit for the weight w_min (e.g., 0.1) to ensure that laser information is not completely discarded even in cases of severe degradation.

[0026] S4: Directional Selectivity Factor Map Optimization Construct a factor graph, where the state nodes represent the robot's pose.

[0027] Laser factor: Instead of using a single covariance matrix, a block diagonal covariance matrix Σ_lidar is constructed, where each element on the diagonal (corresponding to a degree of freedom) is adaptively adjusted according to the laser weight in that direction: Σ_lidar[i,i] = Σ_base[i,i] / (w_lidar_i+ε). Larger weights result in smaller assigned covariance (noise) and greater influence on the optimization.

[0028] Visual inertia factor: Similarly, construct an adaptive covariance matrix Σ_vision, whose diagonal elements correspond to visual weights: Σ_vision[i,i] = Σ_base[i,i] / (w_vision_i+ε).

[0029] These two factors are added to the factor graph, and the solution is obtained using optimizers such as iSAM2. Since the covariance matrix accurately reflects the instantaneous confidence of each sensor in different directions, the optimizer can automatically achieve fine-grained direction-selective fusion.

[0030] S5: Time Smoothing and Output: The calculated weight sequence is then subjected to a low-pass filter in the time domain, such as w_filtered(t) = α. w_filtered(t-1)+(1-α) w_raw(t) is used to further ensure the smoothness of weight changes. The optimized pose is then output after smoothing, serving as the final localization result.

[0031] The method will now be described in detail with reference to specific embodiments.

[0032] The experiment selected the Gate01 sequence from the M2DGR dataset, which contains a large number of tree-occluded scenes. As can be seen from the visualization results in Figure 2, the system's laser mapping effect is good.

[0033] In the Gate01 sequence of the M2DGR dataset, the complete process of localization performed by this system is as follows: After system startup, the system first completes time synchronization and spatial calibration between the LiDAR, camera, and IMU to ensure that all sensor data are within a unified spatiotemporal framework. Subsequently, the system begins processing the Gate01 sequence data stream, which contains challenging scenes due to frequent tree occlusion.

[0034] The first step in the process is data preprocessing. The raw point cloud received by the LiDAR is subjected to feature extraction, separating corner points and facet points; simultaneously acquired camera images are processed using visual algorithms to extract and track key point features; concurrently, the raw angular velocity and acceleration data from the IMU are pre-integrated to estimate relative motion over a short period. The preprocessed data are then fed into two parallel sub-modules: a LiDAR odometry module and a visual-inertial odometry module, each generating preliminary relative pose estimates.

[0035] The core component is refined degradation analysis. During the scan-to-map matching process from the laser odometry, the system extracts the Hessian matrix H in real time. This matrix is ​​decomposed into a translational component H_trans and a rotational component H_rot. By performing eigenvalue projection analysis on these two 3x3 sub-matrices, the system independently calculates the geometric constraint strength score S_i for the robot in the x, y, and z translational degrees of freedom and the roll, pitch, and yaw rotational degrees of freedom. In the tree-occluded scene of the Gate01 sequence, where the laser beam is partially blocked, the system can accurately identify a significant decrease in constraint strength in specific directions (e.g., the vertical direction or certain horizontal directions), indicating "directional degradation."

[0036] Based on the above analysis, the system enters the continuous adaptive weight calculation stage. After the constraint strength score S_i for each degree of freedom is normalized, it is input into a continuous sigmoid function to dynamically calculate the fusion weight w_lidar_i of the LiDAR in that direction. This weight value changes smoothly between 0 and 1, rather than jumping. Simultaneously, the system sets a minimum weight value to ensure that even in severely degraded directions, the ability of the laser to provide absolute scale information is not completely discarded. The weights of the visual sensor in each direction are complementary, set as w_vision_i = 1 - w_lidar_i. In degrees of freedom where tree occlusion weakens the laser constraint, the laser weight decreases gradually, while the visual weight increases accordingly.

[0037] Next is the optimization of the direction-selective factor graph. The system constructs a factor graph, with nodes representing the robot pose to be estimated. The key innovation lies in the fact that the laser constraint factor and the visual-inertial constraint factor no longer use fixed noise models. Instead, the system constructs a block diagonal covariance matrix for each factor. The noise value of each element on the diagonal of the matrix (corresponding to a degree of freedom) is adaptively adjusted based on the sensor weights calculated in real time: the higher the weight, the smaller the noise value assigned to that measurement in that direction, meaning its greater influence during the optimization process. Subsequently, an optimizer (such as iSAM2) solves this factor graph, thereby achieving a refined, on-demand weighted fusion of laser and visual information at each degree of freedom.

[0038] To further ensure output stability, the system performs a low-pass filter on the calculated weight sequence in the time domain to eliminate instantaneous fluctuations and ensure smooth weight changes. Finally, the optimized and smoothed pose is output as the system's high-precision, degradation-resistant positioning result at that moment.

[0039] Throughout the Gate01 sequence, the system continuously performs the aforementioned real-time analysis and fusion. Experimental results show that even in environments with incomplete and directional degradation caused by trees, the system can effectively suppress high-frequency trajectory jitter ("sawtooth effect") and significant drift that occur in traditional methods, outputting a smooth and accurate positioning trajectory that highly matches the real trajectory, comprehensively improving robustness and reliability in complex occlusion scenarios.

[0040] like Figure 3 As shown, the red trajectory represents the improved scheme integrating the adaptive anti-degradation module, and the blue trajectory corresponds to the original LVI-SAM system; combined with Figure 4 The details of the trajectory can be clearly seen when magnified. The improved system's trajectory is closer to the true value, while the original system's positioning trajectory deviates significantly from the true value. After adding the adaptive anti-degradation module, the new system's positioning effect is significantly better than the original system.

[0041] like Figure 5 The trajectory error quantification comparison results show that the proposed solution, presented in blue bars, is superior to the original system, presented in green bars, in all evaluation metrics. Specific quantification results are as follows:

[0042] It is evident that the present invention outperforms the original system in all error metrics, demonstrating the effectiveness of the added module.

[0043] Figure 6 The box diagram further confirms that the present invention not only surpasses the original system in positioning accuracy with smaller errors, but also completely eliminates the abnormal error points that exist in the original system, namely the serious drift or positioning error caused by sensor degradation.

[0044] In summary, this invention can effectively alleviate the problem of positioning accuracy decay caused by sensor degradation, significantly improve the mapping accuracy and environmental adaptability of the laser-visual SLAM system, and enhance the robustness of the system in complex scenarios.

[0045] The present invention has been described in detail above through embodiments, but the content is only a preferred embodiment of the present invention and should not be considered as limiting the scope of the present invention. All equivalent changes and improvements made within the scope of the present invention should still fall within the patent coverage of the present invention.

Claims

1. An adaptive and degradation-resistant laser inertial vision fusion positioning method, characterized in that: The method includes the following steps: S1: Data preprocessing: Preprocess the camera images, IMU data, and LiDAR point clouds respectively to generate preliminary relative pose estimation; S2: Refined Degradation Analysis: The preprocessed lidar point cloud data is input into the lidar odometry submodule. The lidar odometry submodule uses Scan-to-Map matching. During the matching process, the Hessian matrix H is extracted. The matrix H is divided into blocks to separate the translation submatrix H_trans and the rotation submatrix H_rot. The constraint strength of each submatrix in the x, y, and z coordinate axes is calculated to obtain the constraint strength score S_i for each degree of freedom. S3: Continuous adaptive weight calculation: Normalize the constraint strength score S_i of each degree of freedom in S2 and input a continuous weight mapping function; S4: Direction Selectivity Factor Graph Optimization: Construct a factor graph, where the state nodes in the factor graph represent the robot's pose. Construct a block diagonal covariance matrix, where each element on the diagonal is adaptively adjusted according to the laser weight in that direction. S5: Time Smoothing and Output: The calculated weight sequence is subjected to low-pass filtering in the time domain, and the optimized pose is output after smoothing as the final localization result.

2. The adaptive anti-degradation laser inertial vision fusion positioning method according to claim 1 is characterized in that: In S1, the lidar point cloud is subjected to surface and corner feature extraction; the camera image is subjected to feature extraction and tracking; the IMU data is pre-integrated; the pre-processed lidar point cloud data is input into the lidar odometry submodule; the pre-processed camera image and IMU data are input into the visual inertial odometry submodule to generate a preliminary relative pose estimate.

3. The adaptive anti-degradation laser inertial vision fusion positioning method according to claim 1 is characterized in that: In S2, both the translation submatrix H_trans and the rotation submatrix H_rot are 3×3 matrices. H_trans uses the eigenvalue projection method to calculate its constraint strength in the x, y, and z coordinate axes. By calculating the projection components of each eigenvector on the coordinate axis, weighting them with the corresponding eigenvalues, and summing them, the total constraint strength score S_x, S_y, and S_z for that coordinate axis is obtained. H_rot uses the eigenvalue projection calculation method to calculate its constraint strength in the roll, pitch, and yaw rotational degrees of freedom, obtaining constraint strength scores S_roll, S_pitch, and S_yaw.

4. The adaptive anti-degradation laser inertial vision fusion positioning method according to claim 3 is characterized in that: In S3, the weight mapping function adopts the Sigmoid function, as shown below: ； Where k is the kurtosis factor, which controls the speed of the transition; The degradation threshold; This refers to the fusion weight of the lidar in the i-th degree of freedom, and its value varies continuously between 0 and 1.

5. The adaptive anti-degradation laser inertial vision fusion positioning method according to claim 4 is characterized in that: The complementary weights of the visual sensors in this degree of freedom of the visual inertial odometry submodule are represented as follows: ； Set a lower limit w_min for the weights to ensure that laser information is not completely discarded even in cases of severe degradation.

6. The adaptive anti-degradation laser inertial vision fusion positioning method according to claim 1 is characterized in that: In S4, the laser radar point cloud is preprocessed and output as a laser factor, and the camera image and IMU data are preprocessed and output as a visual inertial factor. The block diagonal covariance matrix constructed by the laser factor is Σ_lidar, and the block diagonal covariance matrix constructed by the visual inertial factor is Σ_vision. The laser factor and the visual inertial factor are added to the factor graph and solved using the iSAM2 optimizer.

7. The adaptive anti-degradation laser inertial vision fusion positioning method according to claim 6 is characterized in that: For the diagonal covariance matrix Σ_lidar of the laser factor, each element on the diagonal is adaptively adjusted according to the laser weight in that direction, as shown below: ； Among them, the larger the weight, the smaller the allocated covariance, and the greater its influence on the optimization; For the adaptive covariance matrix Σ_vision of the visual inertia factor, its diagonal elements correspond to the visual weights, as shown below: ； in, The diagonal of the fundamental covariance matrix represents the first... i One element, This represents extremely small positive numbers to prevent the denominator from being 0.

8. The adaptive anti-degradation laser inertial vision fusion positioning method according to claim 1 is characterized in that: In S5, the low-pass filter in the time domain is calculated as follows: ； in, Indicates the current time t The weight values ​​after filtering Represents the smoothing factor. This represents the original weight value.

Images