CN-121977580-A - Point-distribution factor-based laser inertial odometer method in degradation environment

Abstract

The invention discloses a laser inertial odometer method based on point-distribution factors in a degradation environment, and belongs to the technical field of multi-sensor fusion and autonomous robot positioning navigation. The method comprises the steps of carrying out self-adaptive modeling on a local map point cloud, outputting a group of voxel sets meeting geometric consistency, calculating response of local geometric pose change, extracting a key observation jacobian matrix, calculating a system degradation state quantity, judging a system degradation state, finally completing dynamic reconstruction of an observation noise covariance matrix according to the judged degradation state, and finally substituting the reconstructed observation noise covariance matrix into a Kalman filter to solve gain to complete final filter state updating. Compared with the traditional method, the method can effectively inhibit unreliable geometric constraint and remarkably improve the positioning accuracy and robustness in a long-distance weak texture environment.

Inventors

• LIU FENGYU
• WANG BING
• HUANG HAOQIAN

Assignees

• 河海大学

Dates

Publication Date

20260505

Application Date

20260407

Claims (5)

1. 1. A laser inertial odometer method based on a point-distribution factor in a degenerate environment, comprising the steps of: S1, carrying out self-adaptive modeling on local map point clouds, namely firstly carrying out preliminary space division on the input local map point clouds by constructing a double-layer voxel structure, then calculating the statistic of the mean value and covariance matrix of the point clouds in each voxel on the basis, and finally carrying out eigenvalue decomposition by utilizing the covariance matrix of the statistic to complete self-adaptive voxel segmentation so as to output a group of voxel sets meeting geometric consistency; S2, calculating the response of local geometry to pose change based on the voxel set meeting the geometric consistency, which is obtained in the S1, wherein the response comprises the steps of firstly calculating a point distribution probability cost function of local point cloud, then artificially introducing small pose disturbance in the current estimated pose neighborhood, and finally carrying out second-order Taylor expansion on the probability cost function by combining the disturbance so as to extract a key observation jacobian matrix; S3, utilizing the observed jacobian matrix extracted in the S2, firstly quantifying single-point degradation degree by solving the maximum eigenvalue of the observed jacobian matrix to generate a single-point degradation factor set; S4, for the degradation state determined in the step S3, firstly, carrying out normalization processing on the single-point degradation factor set output in the step S3, calculating and mapping out the observation weight of each characteristic point, then carrying out weight reduction penalty on the nominal noise variance by using the observation weight, completing dynamic reconstruction of the observation noise covariance matrix, and finally substituting the reconstructed observation noise covariance matrix into a Kalman filter to solve the gain, thus completing final filter state updating.
2. 2. The method of laser inertial odometry in a degenerate environment based on point-distribution factors according to claim 1, characterized in that step S1 comprises the following specific steps: S1.1, constructing a double-layer voxel structure, and modeling a local map point cloud by adopting the double-layer voxel structure, wherein a first layer in the double-layer voxel structure is a root voxel layer, and space is uniformly divided into voxel units with fixed sizes and stored by using a hash table; s1.2 calculating the statistics of the point cloud in the voxel, namely calculating the mean vector of the point set according to the geometric characteristics of the point cloud in the voxel And covariance matrix As shown in formula (1): Wherein, the Representing a total number of point clouds contained within the current voxel; representing the first in a voxel The three-dimensional point coordinates are used for representing the central position of the point cloud in the voxel; transposed symbols representing a matrix; And S1.3, self-adaptive voxel segmentation, namely carrying out eigenvalue decomposition on the covariance matrix obtained in the step S1.2, when the covariance eigenvalue proportion meets the empirical threshold condition, considering that the point cloud geometric structure in the voxels has consistency and does not continue subdivision, otherwise, recursively dividing the point cloud geometric structure into eight sub-voxels along the directions of three coordinate axes, and finally obtaining a group of voxel sets meeting geometric consistency.
3. 3. The method of laser inertial odometry in a degenerate environment based on point-distribution factor according to claim 2, characterized in that step S2 comprises the following specific steps: s2.1 calculating the probability cost of point distribution, namely approximating the local point cloud in the voxels satisfying geometric consistency in S1.3 to three-dimensional Gaussian distribution, giving the pose increment of the current frame, and calculating the likelihood probability density of the point under the corresponding Gaussian distribution As shown in formula (2): Wherein, the Representing a natural exponential function of the sign, Representing the circumference ratio constant; taking the negative logarithm of the formula (2) and removing constant terms irrelevant to optimization to obtain an equivalent cost function As shown in formula (3): ; S2.2, introducing a six-degree-of-freedom micro-pose disturbance comprising translation and Euler angle change into the neighborhood of the current estimated pose As shown in formula (4): Wherein, the Respectively corresponding to the tiny displacement disturbance along each axis X, Y, Z in a three-dimensional Cartesian coordinate system; respectively representing small angle changes of rotation around each axis, namely Euler angle disturbance; s2.3, extracting an observation jacobian matrix, namely after the disturbance of S2.2 is introduced, performing second-order Taylor expansion on the probability cost function of S2.1, wherein the second-order Taylor expansion is shown as a formula (5): Wherein, the Representing generalized addition operators over manifold space, representing disturbances The mapping is performed on the manifold state, Representing the current estimated pose state before the disturbance is introduced, First-order jacobian matrix representing error terms about pose disturbance, thereby extracting an observation jacobian matrix for describing second-order response intensity of local geometric pose disturbance 。
4. 4. A laser inertial odometer method based on point-distribution factors in a degenerate environment according to claim 3, wherein the specific steps of step S3 are as follows: s3.1, extracting a single-point degradation factor, wherein the variation of probability cost of a definition point before and after disturbance is as follows Neglecting the effect of the first order linear term, approximating the cost variation to a quadratic response determined by the observed jacobian matrix in S2.3, as shown in equation (6): (6) Definition of Single Point degradation factors The maximum value of the cost variation with respect to the disturbance direction is shown in formula (7): (7) Wherein, the Representing solutions with respect to disturbance direction According to rayleigh quotient theory, solving equation (7) is equivalent to solving the maximum eigenvalue of the observed jacobian matrix, as shown in equation (8): (8) Representing an observed jacobian matrix Acquiring a single-point degradation factor set of all points at the current moment; s3.2, judging the system degradation state, namely accumulating the single-point degradation factor set obtained in the S3.1 to obtain the system degradation state quantity As shown in formula (9): (9) And comparing the degradation state quantity of the system with a preset threshold value, and if the degradation standard is reached, judging that the system enters a geometric degradation state, and providing a trigger basis for subsequent observation weighting.
5. 5. The method of laser inertial odometry in a degenerate environment based on point-distribution factors according to claim 4, wherein step S4 is defined as follows: S4.1, calculating degradation confidence and observation weight, namely normalizing the single-point degradation factor set extracted in the S3.1, and calculating the degradation confidence As shown in formula (10): (10) Wherein, the And (3) with Respectively minimum value and maximum value in single-point degradation factor set in current frame, then reversely mapping degradation confidence coefficient into observation weight of said feature point As shown in formula (11): (11) ; and S4.2, self-adaptive reconstruction of observation noise, namely under the framework of a manifold error state iterative Kalman filter, establishing an observation residual model after first-order Taylor expansion as shown in a formula (12): (12) Wherein, the Represent the first The current observed residual value at the time of the iteration, Representing an initial reference residual value at the deployment point, Represent the first A first order jacobian matrix at a time of iteration, Represent the first The tiny pose disturbance during the next iteration, Representing observation noise items, and obeying Gaussian distribution; Observation weight obtained by S4.1 Nominal noise variance constant for lidar ranging Performing dynamic reconstruction to obtain an adaptive adjusted observed noise covariance matrix, namely a dynamic observed noise covariance matrix As shown in formula (13): (13); s4.3 updating the filtering state, and reconstructing the dynamic observation noise covariance matrix after S4.2 Substituting the corrected Kalman gain into a filtering system to solve the corrected Kalman gain As shown in equation (14): (14) Wherein, the Representing the prior covariance matrix, and finally mapping the error vector to the manifold to finish the update of the latest pose state vector, as shown in a formula (15): (15) Wherein, the Representing the latest pose state vector after the state update, The initial manifold state vector before the state update is represented, the weight of the high degradation characteristic points is reduced in a self-adaptive mode through the mechanism, and the robust positioning estimation under the degradation environment is completed.

Description

Point-distribution factor-based laser inertial odometer method in degradation environment Technical Field The invention belongs to the technical field of multi-sensor fusion and robot autonomous positioning navigation, and particularly relates to a laser inertial odometer method based on point-distribution factors in a degradation environment. Background In environments where signals of the global satellite navigation system are missing or disturbed (such as underground mines, urban tunnels, air-raid shelters and other complex underground spaces), unmanned equipment must rely on autonomous perception of the surrounding environment to complete positioning and navigation tasks. In order to solve the problem of degradation of the visual sensor caused by low illumination, the laser radar is not dependent on external illumination conditions, can construct a dense three-dimensional point cloud map, and is widely used as a core sensor to be introduced into an SLAM system of an underground scene. However, lidar suffers from perceived degradation in underground environments such as tunnels, hallways, and the like. Such environments are typically single in geometry and strong in self-similarity, resulting in insufficient geometric constraints available for pose estimation, and thus, positioning drift and even systematic divergence. The traditional method based on the fixed observation noise model is difficult to adapt to dynamic environment degradation, and the front-end degradation sensing and the back-end state estimation lack fine coupling. In order to enable the system to actively adapt to environmental changes, existing partial researches adjust map resolution by monitoring motion modes or construct degradation factors by utilizing point-to-distribution matching, but the existing partial researches often lack fine evaluation on single characteristic point quality and cannot be tightly combined with the existing main stream iterative error state Kalman filtering framework. Meanwhile, the degradation problem caused by insufficient feature constraint, the limitation that the traditional fixed observation noise model is difficult to adapt to dynamic environment degradation and the challenges of lack of fine coupling of front-end degradation perception and back-end state estimation are still faced in the degradation environment with sparse geometric features. Disclosure of Invention Aiming at the problems, the invention provides a laser inertial odometer method based on a point-distribution factor in a degradation environment, which comprises the steps of voxelization and local geometric modeling of an input point cloud, and dynamic segmentation of geometric distribution driving of the point cloud in the voxelization through self-adaptive voxelization so as to enable the local distribution to be more in accordance with Gaussian assumption. Secondly, based on the point-to-distribution idea, the probability change of the point in the local Gaussian distribution is utilized to construct a degradation factor so as to more robustly identify the degradation state under noise interference. Finally, a degradation optimization strategy is introduced, namely degradation factors are normalized to degradation confidence and converted to weights, and the high degradation observation is subjected to weight reduction inhibition in the state estimation updating process, so that the pose estimation stability and precision are improved. The problem that positioning drift easily occurs in an immediate positioning and map construction algorithm in a degradation environment is solved. The above purpose is achieved by the following technical scheme: the invention discloses a laser inertial odometer method based on a point-distribution factor in a degradation environment, which comprises the following steps: S1, carrying out self-adaptive modeling on local map point clouds, namely firstly carrying out preliminary space division on the input local map point clouds by constructing a double-layer voxel structure, then calculating the statistic of the mean value and covariance matrix of the point clouds in each voxel on the basis, and finally carrying out eigenvalue decomposition by utilizing the covariance matrix of the statistic to complete self-adaptive voxel segmentation so as to output a group of voxel sets meeting geometric consistency; S2, calculating the response of local geometry to pose change based on the voxel set meeting the geometric consistency, which is obtained in the S1, wherein the response comprises the steps of firstly calculating a point distribution probability cost function of local point cloud, then artificially introducing small pose disturbance in the current estimated pose neighborhood, and finally carrying out second-order Taylor expansion on the probability cost function by combining the disturbance so as to extract a key observation jacobian matrix; S3, utilizing the observed jacobian matrix extracted in the S2,