Degradation scene-oriented multi-residual fusion laser radar positioning method

Abstract

The invention discloses a degradation scene-oriented multi-residual fusion laser radar positioning method, which comprises the following steps of: firstly, performing state prediction by adopting an iterative extended Kalman filtering framework and an IMU (Inertial Measurement Unit), and constructing three complementary observation models of a global map matching residual, a local point-to-plane geometry residual and a luminosity residual; secondly, designing a degradation sensing mechanism based on a covariance ellipsoid, representing absolute and relative degradation degrees through a condition number and an information entropy respectively, realizing quantitative evaluation of system observability, and dynamically adjusting fusion weights of observation residuals; meanwhile, a self-adaptive weight strategy based on luminosity Jacobi intensity is introduced; and finally, performing anomaly detection through deviation comparison between the IMU predicted pose and the IEKF estimated pose, inhibiting pose jump, and ensuring continuity of a positioning time sequence. The method effectively overcomes the challenges of geometric constraint deficiency, positioning drift accumulation and the like of the LiDAR positioning system in the geometric degradation environment, does not need to adjust parameters for a specific scene, and improves the precision, robustness and real-time performance of global positioning in the degradation environment.

Description

Technical Field

[0001] This invention belongs to the field of lidar positioning technology, specifically relating to a multi-residual fusion lidar positioning method for degraded scenarios. Background Technology

[0002] High-precision real-time positioning plays a crucial role in the field of coal mine automation, providing essential technical support for intelligent equipment such as underground inspection robots, automated guided vehicles, and tunneling machines with autonomous navigation. However, coal mine roadways are characterized by narrow, enclosed spaces, monotonous geometry, and frequent dynamic interference. Traditional lidar positioning methods are prone to drift or failure in such geometrically degraded scenarios.

[0003] Existing lidar localization methods have certain limitations when dealing with degraded environments. While global map matching methods can provide absolute position constraints, they are prone to getting trapped in local optima in adjacent areas with similar geometric features. Odometry methods based on adjacent frame geometric constraints lack a global position reference, leading to continuous accumulation of errors along the motion direction. Traditional degradation detection methods mainly rely on threshold-based Hessian matrix eigenvalue analysis, requiring parameter tuning for specific robot platforms and environmental conditions, and have limited generalization ability in heterogeneous scenarios. Therefore, this paper proposes an adaptive weighted multi-residual fusion lidar localization method for degraded coal mine roadway environments to address the aforementioned problems. Summary of the Invention

[0004] To address the aforementioned issues, this invention discloses a multi-residual fusion lidar localization method for degraded scenarios. By fusing three complementary observation modes—global map matching, local geometric constraints, and photometric measurements—and employing a dual-index degradation detection mechanism based on covariance analysis to achieve adaptive weight adjustment, the method improves the accuracy, robustness, and real-time performance of global localization in geometrically degraded environments.

[0005] To achieve the above objectives, the technical solution of the present invention is as follows:

[0006] A multi-residual fusion lidar localization method for degraded scenarios includes the following steps:

[0007] 1. An iterative extended Kalman filter framework is adopted to perform state prediction using IMU measurement data and construct three complementary observation residuals: global residuals obtained by matching the current point cloud with the prior map, local residuals constructed based on the point-to-local planar geometric relationship, and photometric residuals constructed based on intensity information.

[0008] 2. The position state covariance matrix is ​​decomposed into eigenvalues. A dual-index degradation detection mechanism is adopted, which uses condition number to represent the absolute degree of degradation and information entropy to measure the relative degree of degradation. The observability of the system is quantified and the fusion weight of each observation residual is dynamically adjusted. At the same time, adaptive photometric weights are calculated based on the intensity of the photometric Jacobian matrix, and the confidence of photometric measurement is adjusted according to the richness of scene texture.

[0009] 3. Anomaly detection is performed by comparing the translational and rotational deviations between the IMU-predicted pose and the IEKF-estimated pose. When the deviation exceeds a set threshold, the current update is rejected and the pose is reverted to the IMU-predicted pose, thus suppressing pose jumps in degenerate scenarios and outputting a temporally continuous and stable 6-DOF pose estimation result.

[0010] The specific steps are as follows:

[0011] Step 1. The iterative extended Kalman filter framework is adopted, and state prediction is performed using IMU measurement data. Three complementary observation residuals are constructed: a global residual obtained by matching the current point cloud with the prior map, a local residual constructed based on the point-to-local planar geometric relationship, and a photometric residual constructed based on intensity information; as detailed below:

[0012] This invention uses an IMU coordinate system (denoted as ). The coordinate system is used as the body coordinate system, and the pre-constructed global point cloud map coordinate system is used as the global coordinate system (denoted as ). Hardware time synchronization has been completed between LiDAR and IMU, and all external parameters have been pre-calibrated.

[0013] System state is defined on manifold The state dimension is 24. The state vector is defined as:

[0014] ;

[0015] in , , These represent the rotation, position, and velocity of the IMU in the global coordinate system, respectively. and For biasing the gyroscope and accelerometer, The gravity vector The extrinsic parameters for the transition from the LiDAR coordinate system to the IMU coordinate system. Based on generalized addition and subtraction. The discrete state transition model is as follows:

[0016] ;

[0017] in This is the IMU sampling period. For input vector, For process noise and This is the state handling function.

[0018] Global observations are used to establish absolute positional constraints between the current state and the prior map. For each LiDAR measurement point, it is first transformed to the global coordinate system, then the nearest neighbor point is retrieved from the prior map, and a local plane is fitted. The global residual is defined as the signed distance from the point to the plane:

[0019] ;

[0020] in and These are the normal vector and centroid of the matching plane in the global prior map, respectively. This is the transformation matrix between the global coordinate system and the current frame IMU coordinate system. This is the transformation matrix from the IMU coordinate system to the LiDAR coordinate system. The residual represents the point-to-plane distance between the current LiDAR point after transformation to the global coordinate system and the corresponding plane in the prior map.

[0021] Local observations adopt the point-to-surface geometric residual form from FAST-LIO2 to maintain geometric consistency between adjacent frames. The residual is defined as:

[0022] ;

[0023] in and These are the normal vector and centroid of the matching plane in the local map, respectively.

[0024] Photometric observations borrow from the design principles of COIN-LIO, providing complementary constraints for the geometric degradation direction by minimizing the photometric error of feature blocks in the intensity image. The residual is defined as the difference between the current observed intensity and the reference intensity.

[0025] ;

[0026] in For the intensity image of the current frame, The intensity value stored for the feature block. This is a spherical projection function. These are LiDAR point cloud points with intensity information.

[0027] Combining the above three types of observations, the residual terms are weighted and stacked with their corresponding Jacobian matrices to construct the system's combined observation equation. The combined Jacobian matrix and the residual vector are defined as follows:

[0028] ;

[0029] ;

[0030] in , , These are the weighting factors for global observations, local observations, and photometric observations, respectively. , , These are observations from global observations, local observations, and photometric observations, respectively. , , These are the observation matrices for global observation, local observation, and photometric observation, respectively. Based on the above combined observation model, the system uses the iterative extended Kalman filter in FAST-LIO2 for state updates, achieving tight coupling and fusion of multimodal observation information.

[0031] Step 2. This invention proposes an adaptive weight fusion scheme, which dynamically adjusts the confidence level between local measurements and global constraints by analyzing the observability characteristics of the pose covariance matrix.

[0032] Pose-state covariance matrix in IEKF This represents the uncertainty in the output pose estimation, where... This represents the uncertainty of the translation component. We perform eigenvalue decomposition on it:

[0033] ;

[0034] in The eigenvalues ​​are arranged in ascending order. This is the corresponding feature vector.

[0035] To quantify the degree of system degradation, this invention employs two complementary theoretical indices. First, the condition number is considered. This metric measures the degree of anisotropy of the covariance matrix. A larger condition number indicates a more uneven distribution of uncertainty across different directions, suggesting severe degradation in some directions. The condition number is highly sensitive to extreme cases (such as severe degradation in one direction) and can quickly respond to observability deterioration in the system. However, this sensitivity also makes it susceptible to eigenvalue calculation errors and difficult to distinguish degradation patterns across different dimensions. For example, one-dimensional degradation (where one eigenvalue dominates) and two-dimensional degradation (where two eigenvalues ​​dominate) may produce similar condition numbers, but their degradation mechanisms and coping strategies are drastically different.

[0036] To comprehensively characterize the distribution of uncertainty in all directions, this invention introduces Shannon entropy as a complementary indicator. This method, derived from information theory, can quantitatively describe the degree of uncertainty in probability distributions. First, the eigenvalues ​​are normalized to a probability distribution:

[0037] ;

[0038] Make This satisfies the probability axiom. Based on this, we define the normalized entropy:

[0039] ;

[0040] in It has a clear physical meaning: when When the three eigenvalues ​​are nearly equal, the uncertainty is uniformly distributed along all principal directions, indicating that the system does not exhibit directional degradation; when When uncertainty is highly concentrated in one or two directions, it indicates significant directional degradation in the system. Normalization is achieved by dividing by the maximum possible entropy. This implementation ensures the comparability of the indicator across different dimensional spaces, making the quantification of degradation more intuitive. It is worth noting that condition number and information entropy characterize the degradation features of the covariance matrix from different perspectives, exhibiting a natural complementarity: information entropy reflects the distribution pattern of overall uncertainty, comprehensively considering the contribution of all eigenvalues ​​to the system's observability. Compared to condition number, which only focuses on extreme values, information entropy is more robust to noise disturbances of individual eigenvalues ​​and can effectively distinguish between different degradation modes, such as one-dimensional and two-dimensional degradation. Meanwhile, condition number is highly sensitive to extreme anisotropy, providing early warnings in the early stages of degradation—when uncertainty in a certain direction increases sharply, the condition number grows rapidly, triggering the system's adaptive adjustment mechanism. This complementary relationship provides a theoretical basis for constructing a robust adaptive weighting mechanism.

[0041] This invention designs two complementary degradation metrics: the entropy-based degradation score (reflecting the overall distribution) is defined as follows: The condition number degenerate fraction (reflecting extreme cases) is defined as follows: Logarithmic mapping is used because the condition number has a very large range of values ​​(usually between 1 and 100), and logarithmic transformation makes its distribution smoother; when hour, This indicates that a preset severe degradation threshold has been reached. The final adaptive weights... Calculated as ,in Weight Simultaneously, it is a global weight. and local weights The ratio can be defined as This mechanism can dynamically adjust the fusion strategy according to the degree of system degradation: when the degradation is severe, the weight of local constraints is increased to enhance system stability; when the degradation is mild, more reliance is placed on global measurements with stronger global structural information.

[0042] The calculation of photometric weights relies heavily on intensity texture variations within the scene. In regions with uniform intensity information, the extracted photometric feature points exhibit poor stability, potentially resulting in keypoints appearing at different locations in areas with significant gradient changes. Therefore, this invention proposes an adaptive photometric weight based on photometric residual information to quantify the reliability of photometric measurements.

[0043] Define the pose Jacobian matrix of photometric observations ,in This represents the number of photometric observation features. Calculate the average Jacobian intensity:

[0044] ;

[0045] in Representing the Frobenius norm. Introducing an adaptive reference value. Adaptive updates via exponential moving average (EMA):

[0046] ;

[0047] in This is the smoothing coefficient. The Jacobian ratio can be defined. The hyperbolic tangent function is used to map it to an information content index:

[0048] ;

[0049] This mapping exhibits good saturation properties: when hour, ;when hour, This effectively suppresses the luminance contribution from weakly textured regions. Furthermore, the information content is linearly mapped to the target weight range. :

[0050] ;

[0051] To eliminate transient disturbances, a time smoothing mechanism is introduced:

[0052] ;

[0053] in For time smoothing factor, This is the smoothed photometric weight. The invention sets... , This ensures that photometric constraints do not completely fail in weakly textured scenes, while also being fully effective in strongly textured regions. The final photometric blending weights are defined as follows:

[0054] ;

[0055] in This is a global photometric scaling factor, introduced to unify the feature scale of point clouds and photometric values. This adaptive mechanism can dynamically adjust the confidence level of photometric measurements based on the richness of scene textures, thereby improving the system's robustness in complex environments.

[0056] Step 3. To enhance system robustness, this invention introduces a motion constraint and pose anomaly detection mechanism based on IMU prediction, and performs a rationality check on the IEKF output before filtering and updating.

[0057] The IMU provides short-term, high-frequency pose predictions using integrated angular velocity and acceleration. Although drift exists, it maintains high reliability within a short time window. The IMU-predicted pose is defined as... The IEKF output pose is Calculate the pose deviation between the two:

[0058] ;

[0059] When filter updates introduce abnormal observations or degradation occurs, the IEKF output may deviate from the true motion trajectory. This invention sets a pose anomaly threshold. and rotational anomaly threshold If the following conditions are met, it is considered abnormal:

[0060] ;

[0061] When an anomaly is detected, the system rejects the current IEKF update result, reverts to the IMU predicted pose as the current output, and marks the frame as an anomalous frame. This mechanism effectively prevents filter divergence caused by anomalous observations in a single frame, maintaining the temporal continuity of pose estimation.

[0062] The beneficial effects of this invention are as follows:

[0063] This invention presents a multi-residual fusion lidar localization method for degraded scenarios. By fusing three complementary observation modes—global map matching, local geometric constraints, and photometric measurements—and employing a dual-index degradation detection mechanism based on covariance analysis to achieve adaptive weight adjustment, it improves the accuracy, robustness, and real-time performance of global localization in geometrically degraded environments. Validation results on multiple public datasets such as FusionPortable, Newer College, and GEODE, as well as in real coal mine roadway environments, demonstrate that the localization accuracy reaches 3.7cm–8.1cm in typical degraded environments such as long corridors, stairs, and dynamic scenes. It achieves stable real-time localization throughout a 2km coal mine tunnel test, with all indicators outperforming existing mainstream methods, providing an effective technical solution for the autonomous navigation of intelligent equipment in coal mines. Attached Figure Description

[0064] Figure 1 This is a flowchart illustrating the implementation of the multi-residual fusion lidar positioning method for degraded scenarios as described in this invention.

[0065] Figure 2 This is a trajectory diagram of the method described in this invention compared to other open-source methods in a degradation scenario;

[0066] Figure 3 This is a degradation score analysis graph based on the method described in this invention. Detailed Implementation

[0067] The present invention will be further illustrated below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are for illustrative purposes only and are not intended to limit the scope of the present invention.

[0068] As shown in the figure, the multi-residual fusion lidar localization method for degraded scenarios described in this invention comprises the following steps:

[0069] Step 1. An iterative extended Kalman filter framework is adopted to perform state prediction using IMU measurement data, and three complementary observation residuals are constructed: a global residual obtained by matching the current point cloud with the prior map, a local residual constructed based on the point-to-local planar geometric relationship, and a photometric residual constructed based on intensity information; as detailed below:

[0070] This invention uses an IMU coordinate system (denoted as ). The coordinate system is used as the body coordinate system, and the pre-constructed global point cloud map coordinate system is used as the global coordinate system (denoted as ). Hardware time synchronization has been completed between LiDAR and IMU, and all external parameters have been pre-calibrated.

[0071] System state is defined on manifold The state dimension is 24. The state vector is defined as:

[0072] ;

[0073] in , , These represent the rotation, position, and velocity of the IMU in the global coordinate system, respectively. and For biasing the gyroscope and accelerometer, The gravity vector The extrinsic parameters for the transition from the LiDAR coordinate system to the IMU coordinate system. Based on generalized addition and subtraction. The discrete state transition model is as follows:

[0074] ;

[0075] in This is the IMU sampling period. For input vector, For process noise and This is the state handling function.

[0076] Global observations are used to establish absolute positional constraints between the current state and the prior map. For each LiDAR measurement point, it is first transformed to the global coordinate system, then the nearest neighbor point is retrieved from the prior map, and a local plane is fitted. The global residual is defined as the signed distance from the point to the plane:

[0077] ;

[0078] in and These are the normal vector and centroid of the matching plane in the global prior map, respectively. This is the transformation matrix between the global coordinate system and the current frame IMU coordinate system. This is the transformation matrix from the IMU coordinate system to the LiDAR coordinate system. The residual represents the point-to-plane distance between the current LiDAR point after transformation to the global coordinate system and the corresponding plane in the prior map.

[0079] Local observations adopt the point-to-surface geometric residual form from FAST-LIO2 to maintain geometric consistency between adjacent frames. The residual is defined as:

[0080] ;

[0081] in and These are the normal vector and centroid of the matching plane in the local map, respectively.

[0082] Photometric observations borrow from the design principles of COIN-LIO, providing complementary constraints for the geometric degradation direction by minimizing the photometric error of feature blocks in the intensity image. The residual is defined as the difference between the current observed intensity and the reference intensity.

[0083] ;

[0084] in For the intensity image of the current frame, The intensity value stored for the feature block. For spherical projection functions, These are LiDAR point cloud points with intensity information.

[0085] Combining the above three types of observations, the residual terms are weighted and stacked with their corresponding Jacobian matrices to construct the system's combined observation equation. The combined Jacobian matrix and the residual vector are defined as follows:

[0086] ; ;

[0087] in , , These are the weighting factors for global observations, local observations, and photometric observations, respectively. , , These are observations from global observations, local observations, and photometric observations, respectively. , , These are the observation matrices for global observation, local observation, and photometric observation, respectively. Based on the above combined observation model, the system uses the iterative extended Kalman filter in FAST-LIO2 for state updates, achieving tight coupling and fusion of multimodal observation information.

[0088] Step 2. This invention proposes an adaptive weight fusion scheme, which dynamically adjusts the confidence level between local measurements and global constraints by analyzing the observability characteristics of the pose covariance matrix.

[0089] Pose-state covariance matrix in IEKF This represents the uncertainty in the output pose estimation, where... This represents the uncertainty of the translation component. We perform eigenvalue decomposition on it:

[0090] ;

[0091] in The eigenvalues ​​are arranged in ascending order. This is the corresponding feature vector.

[0092] To quantify the degree of system degradation, this invention employs two complementary theoretical indices. First, the condition number is considered. This metric measures the degree of anisotropy of the covariance matrix. A larger condition number indicates a more uneven distribution of uncertainty across different directions, suggesting severe degradation in some directions. The condition number is highly sensitive to extreme cases (such as severe degradation in one direction) and can quickly respond to observability deterioration in the system. However, this sensitivity also makes it susceptible to eigenvalue calculation errors and difficult to distinguish degradation patterns across different dimensions. For example, one-dimensional degradation (where one eigenvalue dominates) and two-dimensional degradation (where two eigenvalues ​​dominate) may produce similar condition numbers, but their degradation mechanisms and coping strategies are drastically different.

[0093] To comprehensively characterize the distribution of uncertainty in all directions, this invention introduces Shannon entropy as a complementary indicator. This method, derived from information theory, can quantitatively describe the degree of uncertainty in probability distributions. First, the eigenvalues ​​are normalized to a probability distribution:

[0094] ;

[0095] Make This satisfies the probability axiom. Based on this, we define the normalized entropy:

[0096] ;

[0097] in It has a clear physical meaning: when When the three eigenvalues ​​are nearly equal, the uncertainty is uniformly distributed along all principal directions, indicating that the system does not exhibit directional degradation; when When uncertainty is highly concentrated in one or two directions, it indicates significant directional degradation in the system. Normalization is achieved by dividing by the maximum possible entropy. This implementation ensures the comparability of the indicator across different dimensional spaces, making the quantification of degradation more intuitive. It is worth noting that condition number and information entropy characterize the degradation features of the covariance matrix from different perspectives, exhibiting a natural complementarity: information entropy reflects the distribution pattern of overall uncertainty, comprehensively considering the contribution of all eigenvalues ​​to the system's observability. Compared to condition number, which only focuses on extreme values, information entropy is more robust to noise disturbances of individual eigenvalues ​​and can effectively distinguish between different degradation modes, such as one-dimensional and two-dimensional degradation. Meanwhile, condition number is highly sensitive to extreme anisotropy, providing early warnings in the early stages of degradation—when uncertainty in a certain direction increases sharply, the condition number grows rapidly, triggering the system's adaptive adjustment mechanism. This complementary relationship provides a theoretical basis for constructing a robust adaptive weighting mechanism.

[0098] This invention designs two complementary degradation metrics: the entropy-based degradation score (reflecting the overall distribution) is defined as follows: The condition number degenerate fraction (reflecting extreme cases) is defined as follows: Logarithmic mapping is used because the condition number has a very large range of values ​​(usually between 1 and 100), and logarithmic transformation makes its distribution smoother; when hour, This indicates that a preset severe degradation threshold has been reached. The final adaptive weights... Calculated as ,in Weight Simultaneously, it is a global weight. and local weights The ratio can be defined as This mechanism can dynamically adjust the fusion strategy according to the degree of system degradation: when the degradation is severe, the weight of local constraints is increased to enhance system stability; when the degradation is mild, more reliance is placed on global measurements with stronger global structural information.

[0099] The calculation of photometric weights relies heavily on intensity texture variations within the scene. In regions with uniform intensity information, the extracted photometric feature points exhibit poor stability, potentially resulting in keypoints appearing at different locations in areas with significant gradient changes. Therefore, this invention proposes an adaptive photometric weight based on photometric residual information to quantify the reliability of photometric measurements.

[0100] Define the pose Jacobian matrix of photometric observations ,in This represents the number of photometric observation features. Calculate the average Jacobian intensity:

[0101] ;

[0102] in Representing the Frobenius norm. Introducing an adaptive reference value. Adaptive updates via exponential moving average (EMA):

[0103] ;

[0104] in This is the smoothing coefficient. The Jacobian ratio can be defined. The hyperbolic tangent function is used to map it to an information content index:

[0105] ;

[0106] This mapping exhibits good saturation properties: when hour, ;when hour, This effectively suppresses the luminance contribution from weakly textured regions. Furthermore, the information content is linearly mapped to the target weight range. :

[0107] ;

[0108] To eliminate transient disturbances, a time smoothing mechanism is introduced:

[0109] ;

[0110] in For time smoothing factor, This is the smoothed photometric weight. The invention sets... , This ensures that photometric constraints do not completely fail in weakly textured scenes, while also being fully effective in strongly textured regions. The final photometric blending weights are defined as follows:

[0111] ;

[0112] in This is a global photometric scaling factor, introduced to unify the feature scale of point clouds and photometric values. This adaptive mechanism can dynamically adjust the confidence level of photometric measurements based on the richness of scene textures, thereby improving the system's robustness in complex environments.

[0113] Step 3. To enhance system robustness, this invention introduces a motion constraint and pose anomaly detection mechanism based on IMU prediction, and performs a rationality check on the IEKF output before filtering and updating.

[0114] The IMU provides short-term, high-frequency pose predictions using integrated angular velocity and acceleration. Although drift exists, it maintains high reliability within a short time window. The IMU-predicted pose is defined as... The IEKF output pose is Calculate the pose deviation between the two:

[0115] ;

[0116] When filter updates introduce abnormal observations or degradation occurs, the IEKF output may deviate from the true motion trajectory. This invention sets a pose anomaly threshold. and rotational anomaly threshold If the following conditions are met, it is considered abnormal:

[0117] ;

[0118] When an anomaly is detected, the system rejects the current IEKF update result, reverts to the IMU predicted pose as the current output, and marks the frame as an anomalous frame. This mechanism effectively prevents filter divergence caused by anomalous observations in a single frame, maintaining the temporal continuity of pose estimation.

[0119] It should be noted that the above content merely illustrates the technical concept of the present invention and should not be construed as limiting the scope of protection of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and all such improvements and modifications fall within the scope of protection of the claims of the present invention.

Claims

1. A multi-residual fusion lidar localization method for degraded scenarios, characterized in that, include: Step 1: Using the iterative extended Kalman filter framework, state prediction is performed using IMU measurement data, and three complementary observation residuals are constructed: global residuals are obtained by matching the current point cloud with the prior map, local residuals are constructed based on the point-to-local planar geometric relationship, and photometric residuals are constructed based on intensity information. Step 2: Perform eigenvalue decomposition on the position state covariance matrix, and adopt a dual-index degradation detection mechanism that uses condition number to represent the absolute degree of degradation and information entropy to measure the relative degree of degradation. Quantify the observability of the system and dynamically adjust the fusion weight of each observation residual. At the same time, calculate the adaptive photometric weight based on the intensity of the photometric Jacobian matrix, and adjust the photometric measurement confidence based on the richness of scene texture. Step 3: Anomaly detection is performed by comparing the translational and rotational deviations between the IMU-predicted pose and the IEKF-estimated pose. When the deviation exceeds a set threshold, the current update is rejected and the pose is reverted to the IMU-predicted pose to suppress pose jumps in degenerate scenarios, thereby outputting a temporally continuous and stable 6-DOF pose estimation result.

2. The multi-residual fusion lidar localization method for degraded scenarios as described in claim 1, characterized in that, Step 1 employs an iterative extended Kalman filter framework, utilizes IMU measurement data for state prediction, and constructs three complementary observation residuals: a global residual obtained by matching the current point cloud with the prior map, a local residual constructed based on the point-to-local planar geometric relationship, and a photometric residual constructed based on intensity information; specifically as follows: Using the IMU coordinate system as the body coordinate system, denoted as... And using a pre-built global point cloud map coordinate system as the global coordinate system, denoted as Hardware time synchronization between LiDAR and IMU has been completed, and all external parameters have been pre-calibrated. System state is defined on manifold The state dimension is 24; the state vector is defined as: ； in , , These represent the rotation, position, and velocity of the IMU in the global coordinate system, respectively. and For biasing the gyroscope and accelerometer, The gravity vector The extrinsic parameters for the transition from the LiDAR coordinate system to the IMU coordinate system; based on generalized addition and subtraction. The discrete state transition model is as follows: ； in The IMU sampling period; For input vector, For process noise and This is a state handling function; Global observations are used to establish absolute positional constraints between the current state and the prior map; for each LiDAR measurement point, it is first transformed to the global coordinate system, then the nearest neighbor point is retrieved from the prior map and a local plane is fitted; the global residual is defined as the signed distance from the point to the plane. ； in and These are the normal vector and centroid of the matching plane in the global prior map, respectively; This is the transformation matrix between the global coordinate system and the current frame IMU coordinate system. This is the transformation matrix from the IMU coordinate system to the LiDAR coordinate system; the residual represents the point-to-plane distance between the current LiDAR point after transformation to the global coordinate system and the corresponding plane in the prior map. Local observations adopt the point-to-surface geometric residual form from FAST-LIO2 to maintain geometric consistency between adjacent frames; the residual is defined as: ； in and These are the normal vector and centroid of the matching plane in the local map, respectively; Photometric observations provide complementary constraints on the direction of geometric degradation by minimizing the photometric error of feature blocks in the intensity image; the residual is defined as the difference between the current observed intensity and the reference intensity. ； in For the intensity image of the current frame, The intensity value stored for the feature block. It is a spherical projection function; LiDAR point cloud points with intensity information; Combining the above three types of observations, the residual terms are weighted and stacked with their corresponding Jacobian matrices to construct the system's combined observation equation; the combined Jacobian matrix and the residual vector are defined as follows: ； ； in , , These are the weighting factors for global observations, local observations, and photometric observations, respectively. , , These are observations from global observations, local observations, and photometric observations, respectively. , , These are the observation matrices for global observation, local observation, and photometric observation, respectively. Based on the above combined observation model, the system uses the iterative extended Kalman filter in FAST-LIO2 for state updates, achieving tight coupling and fusion of multimodal observation information.

3. The multi-residual fusion lidar localization method for degraded scenarios as described in claim 2, characterized in that, Step 2 is as follows: An adaptive weight fusion scheme is proposed, which dynamically adjusts the confidence level between local measurements and global constraints by analyzing the observability characteristics of the pose covariance matrix. Pose-state covariance matrix in IEKF This represents the uncertainty in the output pose estimation, where... To represent the uncertainty of the translation component, we perform eigenvalue decomposition on it: ； in The eigenvalues ​​are arranged in ascending order. The corresponding feature vector; To quantify the degree of system degradation, two complementary theoretical indices are used; first, the condition number is considered. This metric measures the degree of anisotropy of the covariance matrix; The larger the value, the more uneven the distribution of uncertainty in different directions. To comprehensively characterize the distribution of uncertainty in all directions, Shannon entropy is introduced as a complementary indicator; firstly, the eigenvalues ​​are normalized to a probability distribution: ； Make It satisfies the probability axiom; based on this, we define normalized entropy: ； in It has a clear physical meaning: when When the three eigenvalues ​​are nearly equal, the uncertainty is uniformly distributed along all principal directions, indicating that the system does not exhibit directional degradation; when When uncertainty is highly concentrated in one or two directions, it indicates significant directional degradation in the system; normalization is achieved by dividing by the maximum possible entropy. This ensures the comparability of the indicator across different dimensional spaces, making the quantification of the degree of degradation more intuitive. Design two complementary degradation metrics: the entropy-based degradation score is defined as follows: The condition number degenerate fraction is defined as follows: Logarithmic mapping is used because the range of values ​​for the condition number is extremely large, and logarithmic transformation makes its distribution smoother; when hour, This indicates that a preset severe degradation threshold has been reached; the final adaptive weights Calculated as ,in ; Weight Simultaneously, it is a global weight. and local weights The ratio is defined as This mechanism can dynamically adjust the fusion strategy according to the degree of system degradation: when the degradation is severe, the weight of local constraints is increased to enhance system stability; when the degradation is mild, it relies more on global measurements with stronger global structural information. Define the pose Jacobian matrix of photometric observations ,in The number of photometric observation features; calculate the average Jacobian intensity: ； in Describe the Frobenius norm; introduce an adaptive reference value. Adaptive updates via exponential moving average (EMA): ； in The smoothing coefficient; the Jacobian ratio can be defined. The hyperbolic tangent function is used to map it to an information content index: ； This mapping exhibits good saturation properties: when hour, ;when hour, This effectively suppresses the luminance contribution from weakly textured regions; further, it linearly maps the information content to the target weight range. : ； To eliminate transient disturbances, a time smoothing mechanism is introduced: ； in For time smoothing factor, For smoothed photometric weights; set , To ensure that photometric constraints do not completely fail in weakly textured scenes, while also being fully effective in strongly textured regions, the final photometric fusion weights are defined as follows: ； in This is the global photometric scaling factor.

4. The multi-residual fusion lidar localization method for degraded scenarios as described in claim 3, characterized in that, Step 3 is as follows: A motion constraint and pose anomaly detection mechanism based on IMU prediction is adopted to verify the rationality of the IEKF output before filtering and updating; Define the IMU to predict the pose as The IEKF output pose is Calculate the pose deviation between the two: ； To prevent the IEKF output from deviating from the true motion trajectory when filter updates introduce abnormal observations or degradation occurs, a pose anomaly threshold is set. and rotational anomaly threshold If the following conditions are met, it is considered abnormal: ； When an anomaly is detected, the system rejects the current IEKF update result, reverts to the IMU predicted pose as the current state output, and marks the frame as an abnormal frame.

Images