A laser inertial odometer method based on point-distribution factor in deteriorating environment
By constructing degradation factors through adaptive voxelization modeling and point-to-distribution factors, the positioning drift problem of lidar in degraded environments is solved, improving positioning accuracy and stability and adapting to dynamic environmental changes.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- HOHAI UNIV
- Filing Date
- 2026-04-07
- Publication Date
- 2026-06-16
Smart Images
Figure CN121977580B_ABST
Abstract
Description
Technical Field
[0001] This invention belongs to the field of multi-sensor fusion and robot autonomous positioning and navigation technology, specifically relating to a laser inertial odometry method based on point-distribution factor in degraded environments. Background Technology
[0002] In environments where global satellite navigation system signals are absent or interfered with (such as complex underground spaces like underground mines, urban tunnels, and air-raid shelters), unmanned equipment must rely on autonomous perception of its surroundings to complete positioning and navigation tasks. To address the degradation of visual sensors caused by low light conditions, lidar, because it does not depend on external lighting conditions and can construct dense 3D point cloud maps, has been widely adopted as a core sensor in SLAM systems for underground scenarios.
[0003] However, lidar also encounters perception degradation in underground environments such as tunnels and corridors. These environments typically have simple geometric structures and strong self-similarity, resulting in insufficient geometric constraints for pose estimation, which in turn leads to positioning drift and even system divergence. Existing traditional methods based on fixed observation noise models are difficult to adapt to dynamic environmental degradation, and the front-end degradation perception and back-end state estimation lack fine-grained coupling. To enable the system to actively adapt to environmental changes, some existing studies adjust map resolution by monitoring motion modes or construct degradation factors using point-to-distribution matching, but these often lack fine-grained evaluation of the quality of individual feature points and fail to achieve tight integration with the existing mainstream iterative error state Kalman filter framework. At the same time, in degraded environments with sparse geometric features, the challenges of degradation due to insufficient feature constraints, the limitations of traditional fixed observation noise models in adapting to dynamic environmental degradation, and the lack of fine-grained coupling between front-end degradation perception and back-end state estimation remain. Summary of the Invention
[0004] To address the aforementioned problems, this invention proposes a laser inertial odometry method based on a point-distribution factor in degraded environments. First, the input point cloud is voxelized and local geometrically modeled. Adaptive voxel segmentation is then used to dynamically segment the point cloud within each voxel, driven by geometric distribution, making the local distribution more consistent with the Gaussian assumption. Second, based on the point-to-distribution idea, a degradation factor is constructed using the probability changes of points within a local Gaussian distribution to more robustly identify degraded states under noise interference. Finally, a degradation optimization strategy is introduced: the degradation factor is normalized to a degradation confidence level and converted into weights. High-degraded observations are reduced-weighted and suppressed during state estimation updates, thereby improving the stability and accuracy of pose estimation. This solves the problem of positioning drift that easily occurs in real-time localization and map building algorithms under degraded environments.
[0005] The above objectives are achieved through the following technical solutions:
[0006] The present invention provides a laser inertial odometry method based on point-distribution factor in degraded environments, the method comprising the following steps:
[0007] S1. Adaptive modeling of local map point cloud: First, the input local map point cloud is initially spatially divided by constructing a two-layer voxel structure; then, the mean and covariance matrix statistics of the point cloud within each voxel are calculated based on this; finally, the covariance matrix of these statistics is used to perform eigenvalue decomposition to complete adaptive voxel segmentation, thereby outputting a set of voxels that satisfy geometric consistency.
[0008] S2. Based on the set of voxels that satisfy geometric consistency obtained in S1, calculate the response of local geometry to pose changes: First, calculate the point distribution probability cost function of the local point cloud; then, artificially introduce a small pose perturbation in the neighborhood of the current estimated pose; finally, combine the perturbation to perform a second-order Taylor expansion on the aforementioned probability cost function, thereby extracting the key observation Jacobian matrix.
[0009] S3. Using the observation Jacobian matrix extracted in S2, the degree of single-point degradation is first quantified by solving its largest eigenvalue, generating a set of single-point degradation factors; then, all single-point degradation factors in the set are accumulated to calculate the system degradation state quantity, and compared with a preset threshold to determine the system degradation state.
[0010] S4. For the degradation state determined in step S3, firstly, the set of single-point degradation factors output in step S3 is normalized, and the observation weight of each feature point is calculated and mapped; then, the observation weight is used to reduce the nominal noise variance to complete the dynamic reconstruction of the observation noise covariance matrix; finally, the reconstructed observation noise covariance matrix is substituted into the Kalman filter to solve for the gain, and the final filter state update is completed.
[0011] Furthermore, the specific steps of step S1 are as follows:
[0012] S1.1 Construct a two-layer voxel structure and use the two-layer voxel structure to model the local map point cloud: The first layer of the two-layer voxel structure is the root voxel layer, which divides the space into voxel units of fixed size and stores them using a hash table; the second layer is the voxel octree layer, which constructs an octree inside each root voxel.
[0013] S1.2 Calculate the statistics of the point cloud within the voxels: Calculate the mean vector of the point set based on the geometric properties of the point cloud within the voxels. With covariance matrix As shown in formula (1):
[0014]
[0015] in, This indicates the total number of point clouds contained within the current voxel; The first voxel represents the first voxel. Three-dimensional point coordinates are used to characterize the center position of the point cloud within the voxel; The symbol for the transpose of a matrix;
[0016] S1.3 Adaptive Voxel Segmentation: Eigenvalue decomposition is performed on the covariance matrix obtained in S1.2. When the proportion of covariance eigenvalues meets the empirical threshold condition, the point cloud geometry within the voxel is considered to be consistent and no further subdivision is performed; otherwise, it is recursively divided into eight sub-voxels along the three coordinate axes, and finally a set of voxels that meet the geometric consistency is obtained.
[0017] Furthermore, the specific steps of step S2 are as follows:
[0018] S2.1 Cost of Calculating Point Distribution Probability: Approximate the local point cloud within the voxel satisfying geometric consistency in S1.3 as a 3D Gaussian distribution. Given the current inter-frame pose increment, calculate the likelihood probability density of the point under the corresponding Gaussian distribution. As shown in formula (2):
[0019]
[0020] in, This represents the natural exponential function. Pi is a constant.
[0021] Taking the negative logarithm of formula (2) and removing the constant term that is irrelevant to optimization, we obtain the equivalent cost function. As shown in formula (3):
[0022] .
[0023] S2.2 Introducing a small pose perturbation: A six-DOF small pose perturbation, incorporating translation and Euler angle variations, is introduced into the neighborhood of the current estimated pose. As shown in formula (4):
[0024]
[0025] in, These correspond to minute displacement perturbations along the X, Y, and Z axes in a three-dimensional Cartesian coordinate system, respectively. These represent the minute angular changes in rotation about each axis, i.e., Euler angle perturbations;
[0026] S2.3 Extracting the observation Jacobian matrix: After introducing the perturbation of S2.2, the probability cost function of S2.1 is expanded by a second-order Taylor, as shown in formula (5):
[0027]
[0028] in, This represents the generalized addition operator on the manifold space, indicating that the perturbation... Mapping onto manifold states, This represents the current estimated pose state before the perturbation is introduced. This represents the first-order Jacobian matrix of the error term with respect to the pose perturbation, from which the observation Jacobian matrix is extracted to characterize the intensity of the second-order response of the local geometry to the pose perturbation. .
[0029] Furthermore, the specific steps of step S3 are as follows:
[0030] S3.1 Extracting Single-Point Degradation Factor: Define the change in probability cost of a point before and after the perturbation as... Ignoring the influence of the first-order linear term, the change in cost is approximated as a quadratic response determined by the observed Jacobian matrix in S2.3, as shown in formula (6):
[0031] (6)
[0032] Define single-point degradation factor The maximum value of the cost change with respect to the direction of the disturbance is shown in formula (7):
[0033] (7)
[0034] in, This indicates the solution for the direction of the disturbance. The maximum value operation, according to Rayleigh's quotient theorem, is equivalent to solving formula (7) for finding the maximum eigenvalue of the observed Jacobian matrix, as shown in formula (8):
[0035] (8)
[0036] Represents the observation Jacobian matrix The largest eigenvalue; thereby obtaining the set of single-point degradation factors for all points at the current time;
[0037] S3.2 Determine the system degradation state: Accumulate the set of single-point degradation factors obtained in S3.1 to obtain the system degradation state quantity. As shown in formula (9):
[0038] (9)
[0039] The degradation state of the system is compared with a preset threshold. If the degradation standard is reached, the system is determined to have entered a geometric degradation state, providing a trigger basis for subsequent observation weighting.
[0040] Furthermore, the specific steps of step S4 are as follows:
[0041] S4.1 Calculate the degradation confidence score and observation weights: Normalize the set of single-point degradation factors extracted in S3.1 and calculate the degradation confidence score. As shown in formula (10):
[0042] (10)
[0043] in, and These are the minimum and maximum values in the set of single-point degradation factors within the current frame, respectively; subsequently, the degradation confidence is inversely mapped to the observation weight of that feature point. As shown in formula (11):
[0044] (11)
[0045] S4.2 Adaptive Reconstruction of Observation Noise: Under the framework of manifold error state iterative Kalman filter, the observation residual model after first-order Taylor expansion is established as shown in Equation (12):
[0046] (12)
[0047] in, Indicates the first The current observed residual value at the next iteration This represents the initial baseline residual value at the expansion point. Indicates the first The first-order Jacobian matrix at the next iteration Indicates the first The small pose perturbation during the next iteration. The observed noise term follows a Gaussian distribution;
[0048] Observation weights obtained using S4.1 The nominal noise variance constant for lidar ranging Dynamic reconstruction is performed to obtain the adaptively adjusted observation noise covariance matrix, i.e., the dynamic observation noise covariance matrix. As shown in formula (13):
[0049] (13).
[0050] S4.3 Filtering State Update: Reconstruct the dynamic observation noise covariance matrix from S4.2. Substituting into the filtering system, the corrected Kalman gain is obtained. As shown in formula (14):
[0051] (14)
[0052] in, Let represent the prior covariance matrix. Finally, the error vector is mapped onto the manifold to complete the latest pose state vector update, as shown in formula (15):
[0053] (15)
[0054] in, This represents the latest pose state vector after the state update. This represents the initial manifold state vector before the state update. This mechanism adaptively reduces the weight of highly degenerate feature points, thus achieving robust localization estimation in degenerate environments.
[0055] The advantages of this invention compared to the prior art are:
[0056] (1) This invention addresses the localization drift problem caused by single features in degraded environments by proposing an adaptive voxel octree modeling method. This method dynamically adjusts the voxel resolution based on the geometric consistency of the point cloud, ensuring the reliability of the local statistical model while taking into account computational efficiency, thus providing a stable geometric basis for degradation detection.
[0057] (2) To address the problem of traditional algorithms lacking refined evaluation of feature point quality, this invention proposes a degradation detection method based on voxel covariance analysis. By calculating the change in the probability cost of point distribution under small pose perturbations, degradation factors are extracted, thereby accurately quantifying the degree of environmental degradation and achieving robust identification of degradation states.
[0058] (3) To address the problem that traditional fixed noise models are difficult to adapt to dynamic environmental degradation, this invention designs an adaptive reconstruction mechanism based on observation noise covariance. By constructing a mapping from degradation factors to observation weights, the weights of highly degraded feature points are adaptively reduced during filter updates, effectively suppressing erroneous gradient guidance and significantly improving the positioning accuracy and stability of the system in extreme environments. Attached Figure Description
[0059] Figure 1 This is a flowchart of a laser inertial odometry method based on point-distribution factor in a degraded environment, as described in this invention.
[0060] Figure 2The results show the transformation of the degradation factor at different locations in the self-collected UAV dataset Cage2 during the experiment.
[0061] Figure 3 The curves show the comparison of global localization trajectories of different methods on the 01 sequence of the GEODE-Offroad dataset during the experiment.
[0062] Figure 4 This is a comparison curve of the local localization trajectories of the start and end points of different methods on the 01 sequence of the GEODE-Offroad dataset during the experiment. Detailed Implementation
[0063] The method of the present invention will be further described below with reference to the accompanying drawings.
[0064] like Figure 1 As shown, a laser inertial odometry method based on point-distribution factor in a degraded environment according to the present invention includes the following steps:
[0065] S1. To obtain a reliable local statistical model in a degenerate environment with simple geometric features, this step adaptively models the local map point cloud. First, a two-layer voxel structure is constructed (S1.1) to perform preliminary spatial partitioning of the input local map point cloud. Then, based on this, statistical measures such as the mean and covariance matrix of the point cloud within each voxel are calculated (S1.2). Finally, eigenvalue decomposition is performed using the covariance matrix of these statistics to complete adaptive voxel segmentation, thereby outputting a set of voxels that satisfy geometric consistency (S1.3). This step provides a reliable geometric modeling foundation for subsequent probability calculations. The specific steps are as follows:
[0066] S1.1 Constructing a Two-Layer Voxel Structure: A two-layer voxel structure is used to model the local map point cloud. The first layer is the root voxel layer, which uniformly divides the space into voxel units of fixed size and stores them using a hash table; the second layer is the voxel octree layer, which constructs an octree within each root voxel.
[0067] S1.2 Calculate the statistics of the point cloud within the voxels: Calculate the mean vector of the point set based on the geometric properties of the point cloud within the voxels. With covariance matrix As shown in formula (1):
[0068]
[0069] in, This indicates the total number of point clouds contained within the current voxel; The first voxel represents the first voxel. Three-dimensional point coordinates are used to characterize the center position of the point cloud within the voxel; The symbol for the transpose of a matrix;
[0070] S1.3 Adaptive Voxel Segmentation: Eigenvalue decomposition is performed on the covariance matrix obtained in S1.2. When the proportion of covariance eigenvalues meets the empirical threshold condition, the point cloud geometry within the voxel is considered to be consistent and no further subdivision is performed; otherwise, it is recursively divided into eight sub-voxels along the three coordinate axes, ultimately obtaining a set of voxels that satisfy geometric consistency.
[0071] S2. Based on the set of voxels that satisfy geometric consistency obtained in S1, calculate the local geometry's response to pose changes. First, calculate the point distribution probability cost function of the local point cloud (S2.1); then, artificially introduce a small pose perturbation in the neighborhood of the current estimated pose (S2.2); finally, combine this perturbation with a second-order Taylor expansion of the aforementioned probability cost function to extract the key observation Jacobian matrix (S2.3). This step bridges the gap between geometric features and state estimation sensitivity analysis. The specific steps are as follows:
[0072] S2.1 Cost of Calculating Point Distribution Probability: Approximate the local point cloud within the voxels satisfying geometric consistency in S1.3 as a 3D Gaussian distribution. Given the current inter-frame pose increment, calculate the likelihood probability density of the points under the corresponding Gaussian distribution. As shown in formula (2):
[0073]
[0074] Taking the negative logarithm of formula (2) and removing the constant term that is irrelevant to optimization, we obtain the equivalent cost function. As shown in formula (3):
[0075] .
[0076] S2.2 Introducing a small pose perturbation: A six-DOF small pose perturbation, incorporating translation and Euler angle variations, is introduced into the neighborhood of the current estimated pose. As shown in formula (4):
[0077]
[0078] in, These correspond to minute displacement disturbances along the X, Y, and Z axes in a three-dimensional Cartesian coordinate system, respectively. These represent the minute angular changes in rotation about each axis, i.e., Euler angle perturbations.
[0079] S2.3 Extracting the observation Jacobian matrix: After introducing the perturbation of S2.2, the probability cost function of S2.1 is expanded by a second-order Taylor, as shown in formula (5):
[0080]
[0081] in, This represents the generalized addition operator on the manifold space, indicating that the perturbation... Mapping onto manifold states, This represents the current estimated pose state before the perturbation is introduced. Let represent the first-order Jacobian matrix of the error term with respect to the pose perturbation. From this, the observation Jacobian matrix is extracted to characterize the second-order response intensity of the local geometry to the pose perturbation. .
[0082] S3. Using the observed Jacobian matrix extracted in S2, the degree of single-point degradation is first quantified by solving its largest eigenvalue, generating a set of single-point degradation factors (S3.1). Then, all single-point degradation factors in this set are accumulated to calculate the system degradation state variable, which is compared with a preset threshold to determine the system degradation state (S3.2). This step clarifies whether the system has entered a degradation environment and identifies the specific characteristic points that trigger degradation. The specific steps are as follows:
[0083] S3.1 Extracting Single-Point Degradation Factor: Define the change in probability cost of a point before and after the perturbation as... Ignoring the influence of the first-order linear term, the change in cost is approximated as a quadratic response determined by the observed Jacobian matrix in S2.3, as shown in Equation (6):
[0084] (6).
[0085] Define single-point degradation factor The maximum value of the cost change with respect to the direction of the disturbance is shown in formula (7):
[0086] (7)
[0087] in, This indicates the solution for the direction of the disturbance. The maximum value operation. According to Rayleigh's quotient theorem, solving formula (7) is equivalent to solving for the maximum eigenvalue of the observed Jacobian matrix, as shown in formula (8):
[0088] (8)
[0089] This yields the set of single-point degradation factors for all points at the current moment.
[0090] S3.2 Determine the system degradation state: Accumulate the set of single-point degradation factors obtained in S3.1 to obtain the system degradation state quantity. As shown in formula (9):
[0091] (9)
[0092] The degradation state of the system is compared with a preset threshold. If the degradation standard is reached, the system is determined to have entered a geometric degradation state, providing a trigger basis for subsequent observation weighting.
[0093] S4. For the degradation state determined in the previous step, firstly, the set of single-point degradation factors output in the previous step is normalized, and the observation weights of each feature point are calculated and mapped (S4.1); then, the nominal noise variance is reduced using these observation weights to complete the dynamic reconstruction of the observation noise covariance matrix (S4.2); finally, the reconstructed observation noise covariance matrix is substituted into the Kalman filter to solve for the gain, completing the final filter state update (S4.3). This step achieves the ultimate goal of correcting positioning drift using degradation information. The specific steps are as follows:
[0094] S4.1 Calculate the degradation confidence score and observation weights: Normalize the set of single-point degradation factors extracted in S3.1 and calculate the degradation confidence score. As shown in formula (10):
[0095] (10)
[0096] in, and These are the minimum and maximum values in the set of single-point degradation factors within the current frame, respectively. Subsequently, the degradation confidence is inversely mapped to the observation weight of that feature point. As shown in formula (11):
[0097] (11).
[0098] S4.2 Adaptive Reconstruction of Observation Noise: Under the framework of manifold error state iterative Kalman filter, the observation residual model after first-order Taylor expansion is established as shown in Equation (12):
[0099] (12)
[0100] in, Indicates the first The current observation residual value at the next iteration. This represents the initial baseline residual value at the expansion point. Indicates the first The first-order Jacobian matrix at the next iteration. Indicates the first The small pose perturbation during the next iteration. This represents the observed noise term, which follows a Gaussian distribution.
[0101] Observation weights obtained using S4.1 The nominal noise variance constant for lidar ranging Dynamic reconstruction is performed to obtain the adaptively adjusted observation noise covariance matrix, i.e., the dynamic observation noise covariance matrix. As shown in formula (13):
[0102] (13).
[0103] S4.3 Filtering State Update: Reconstruct the dynamic observation noise covariance matrix from S4.2. Substituting into the filtering system, the corrected Kalman gain is obtained. As shown in formula (14):
[0104] (14)
[0105] in, Let represent the prior covariance matrix. Finally, the error vector is mapped onto the manifold to complete the latest pose state vector update, as shown in equation (15):
[0106] (15)
[0107] in, This represents the latest pose state vector after the state update. This represents the initial manifold state vector before the state update. This mechanism adaptively suppresses the weights of highly degenerate feature points during the update phase, significantly improving the system's localization accuracy and convergence stability in scenarios with missing geometric features.
[0108] To verify the effectiveness of the method of this invention, robust laser inertial positioning experiments based on point-distribution were conducted. Experiments were performed on the GEODE public dataset and a self-acquired UAV dataset. The GEODE dataset integrates multiple types of LiDAR, stereo cameras, and IMU sensors, realistically depicting geometric degradation and diverse motion states through refined acquisition processes, providing a challenging and representative benchmark for the algorithm. This embodiment uses a self-acquired UAV dataset, and the experimental platform is a quadcopter equipped with an Intel-RealSense-D435i vision camera, a Livox-Mid-360 LiDAR, and an internally integrated inertial sensor.
[0109] Figure 2The diagram shows the transformation of the degradation factor at different locations within a cave during the experiment. It can be seen that in the cage2 sequence of the self-collected dataset, after the UAV enters the main passage of the long, straight tunnel / cave, the environmental structure is highly repetitive and observable features are scarce. The degradation factor shifts downwards overall, approaching the threshold of 0.2, and remains consistently near this level in multiple intervals, indicating that the system is in a state of significant geometric degradation. Conversely, when side passages or open areas appear on the side of the passage during flight (as shown by the arrow in the local point cloud example), the available geometric constraints in the scene increase significantly, the degradation state is alleviated, and the degradation factor rises significantly, corresponding to a switch from "degraded" to "non-degraded".
[0110] Figure 3 The graph shows the comparison curves of global localization trajectories of different methods on the GEODE-Offroad dataset 01 sequence during the experiment. It can be seen that both algorithms showed excellent performance on this dataset, and the trajectories were close to the true values, but there are still some differences. Figure 4 The curves showing the comparison of the local localization trajectories of the start and end points of different methods on the GEODE-Offroad dataset 01 sequence during the experiment can be seen as follows: at the position indicated by the arrow, the FAST-LIO2 algorithm estimation is disordered, while the algorithm in this paper remains stable.
[0111] In summary, the method proposed in this invention significantly improves the positioning accuracy and stability in degraded environments compared to traditional methods.
Claims
1. A laser inertial odometry method based on point-distribution factor in degraded environments, characterized in that, The method includes the following steps: S1. Adaptive modeling of local map point cloud: First, the input local map point cloud is initially spatially divided by constructing a two-layer voxel structure; then, the mean and covariance matrix statistics of the point cloud within each voxel are calculated based on this; finally, the covariance matrix of these statistics is used to perform eigenvalue decomposition to complete adaptive voxel segmentation, thereby outputting a set of voxels that satisfy geometric consistency. S2. Based on the set of voxels that satisfy geometric consistency obtained in S1, calculate the response of local geometry to pose changes: First, calculate the point distribution probability cost function of the local point cloud; then, artificially introduce a small pose perturbation in the neighborhood of the current estimated pose; finally, combine the perturbation to perform a second-order Taylor expansion on the aforementioned probability cost function, thereby extracting the key observation Jacobian matrix. S3. Using the observed Jacobian matrix extracted in S2, the degree of single-point degradation is first quantified by solving its largest eigenvalue, generating a set of single-point degradation factors; then, all single-point degradation factors in this set are accumulated to calculate the system degradation state quantity, and compared with a preset threshold to determine the system degradation state; the specific steps are as follows: S3.1 Extracting Single-Point Degradation Factor: Define the change in probability cost of a point before and after the perturbation as... Ignoring the influence of the first-order linear term, the change in cost is approximated as a quadratic response determined by the observed Jacobian matrix in S2.3, as shown in formula (6): （6） Define single-point degradation factor The maximum value of the cost change with respect to the direction of the disturbance is shown in formula (7): （7） in, This indicates the solution for the direction of the disturbance. The maximum value operation, according to Rayleigh's quotient theorem, is equivalent to solving formula (7) for finding the maximum eigenvalue of the observed Jacobian matrix, as shown in formula (8): （8） Represents the observation Jacobian matrix The largest eigenvalue; thereby obtaining the set of single-point degradation factors for all points at the current time; S3.2 Determine the system degradation state: Accumulate the set of single-point degradation factors obtained in S3.1 to obtain the system degradation state quantity. As shown in formula (9): （9） The degradation state of the system is compared with a preset threshold. If the degradation standard is reached, the system is determined to have entered a geometric degradation state, providing a trigger basis for subsequent observation weighting. S4. For the degradation state determined in step S3, firstly, the set of single-point degradation factors output in step S3 is normalized, and the observation weights of each feature point are calculated and mapped; then, the nominal noise variance is reduced using these observation weights to complete the dynamic reconstruction of the observation noise covariance matrix; finally, the reconstructed observation noise covariance matrix is substituted into the Kalman filter to solve for the gain, completing the final filter state update. The specific steps are as follows: S4.1 Calculate the degradation confidence score and observation weights: Normalize the set of single-point degradation factors extracted in S3.1 and calculate the degradation confidence score. As shown in formula (10): （10） in, and These are the minimum and maximum values in the set of single-point degradation factors within the current frame, respectively; subsequently, the degradation confidence is inversely mapped to the observation weight of that feature point. As shown in formula (11): （11） S4.2 Adaptive Reconstruction of Observation Noise: Under the framework of manifold error state iterative Kalman filter, the observation residual model after first-order Taylor expansion is established as shown in Equation (12): （12） in, Indicates the first The current observed residual value at the next iteration This represents the initial baseline residual value at the expansion point. Indicates the first The first-order Jacobian matrix at the next iteration Indicates the first The small pose perturbation during the next iteration. The observed noise term follows a Gaussian distribution; Observation weights obtained using S4.1 The nominal noise variance constant for lidar ranging Dynamic reconstruction is performed to obtain the adaptively adjusted observation noise covariance matrix, i.e., the dynamic observation noise covariance matrix. As shown in formula (13): （13） S4.3 Filtering State Update: Reconstruct the dynamic observation noise covariance matrix from S4.
2. Substituting into the filtering system, the corrected Kalman gain is obtained. As shown in formula (14): （14） in, Let represent the prior covariance matrix. Finally, the error vector is mapped onto the manifold to complete the latest pose state vector update, as shown in formula (15): （15） in, This represents the latest pose state vector after the state update. This represents the initial manifold state vector before the state update. This represents a generalized addition operator on the manifold space. This mechanism adaptively reduces the weight of highly degenerate feature points, thus achieving robust localization estimation in degenerate environments.
2. The laser inertial odometry method based on point-distribution factor in a degraded environment according to claim 1, characterized in that, The specific steps of step S1 are as follows: S1.1 Construct a two-layer voxel structure and use the two-layer voxel structure to model the local map point cloud: The first layer of the two-layer voxel structure is the root voxel layer, which divides the space into voxel units of fixed size and stores them using a hash table; the second layer is the voxel octree layer, which constructs an octree inside each root voxel. S1.2 Calculate the statistics of the point cloud within the voxels: Calculate the mean vector of the point set based on the geometric properties of the point cloud within the voxels. With covariance matrix As shown in formula (1): in, This indicates the total number of point clouds contained within the current voxel; The first voxel represents the first voxel. Three-dimensional point coordinates are used to characterize the center position of the point cloud within the voxel; The symbol for the transpose of a matrix; S1.3 Adaptive Voxel Segmentation: Eigenvalue decomposition is performed on the covariance matrix obtained in S1.
2. When the proportion of covariance eigenvalues meets the empirical threshold condition, the point cloud geometry within the voxel is considered to be consistent and no further subdivision is performed; otherwise, it is recursively divided into eight sub-voxels along the three coordinate axes, and finally a set of voxels that meet the geometric consistency is obtained.
3. The laser inertial odometry method based on point-distribution factor in a degraded environment according to claim 2, characterized in that, The specific steps of step S2 are as follows: S2.1 Cost of Calculating Point Distribution Probability: Approximate the local point cloud within the voxel satisfying geometric consistency in S1.3 as a 3D Gaussian distribution. Given the current inter-frame pose increment, calculate the likelihood probability density of the point under the corresponding Gaussian distribution. As shown in formula (2): in, This represents the natural exponential function. Pi is a constant. Taking the negative logarithm of formula (2) and removing the constant term that is irrelevant to optimization, we obtain the equivalent cost function. As shown in formula (3): S2.2 Introducing a small pose perturbation: A six-DOF small pose perturbation, incorporating translation and Euler angle variations, is introduced into the neighborhood of the current estimated pose. As shown in formula (4): in, These correspond to minute displacement perturbations along the X, Y, and Z axes in a three-dimensional Cartesian coordinate system, respectively. These represent the minute angular changes in rotation about each axis, i.e., Euler angle perturbations; S2.3 Extracting the observation Jacobian matrix: After introducing the perturbation of S2.2, the probability cost function of S2.1 is expanded by a second-order Taylor, as shown in formula (5): in, This represents the generalized addition operator on the manifold space, indicating that the perturbation... Mapping onto manifold states, This represents the current estimated pose state before the perturbation is introduced. This represents the first-order Jacobian matrix of the error term with respect to the pose perturbation, from which the observation Jacobian matrix is extracted to characterize the intensity of the second-order response of the local geometry to the pose perturbation. .