An automatic driving laser inertial odometer method and system under a complex scene

Abstract

The application belongs to the technical field of automatic driving environment perception and positioning navigation, and discloses a kind of automatic driving laser inertial odometry method and system under complex scene, its method includes constructing point cloud saliency prediction network for automatic driving, fusing depth, semantic and other information to predict laser point saliency value and calculate weight;Prior state is obtained by using inertial measurement unit data forward propagation, state update is carried out by iteration error state Kalman filtering in combination with saliency weight, and dynamic interference is inhibited;An adaptive planar voxel map based on hash index is constructed, and coarse-to-fine plane fitting is realized according to environmental characteristics;Based on sliding window, multiple frames of pose are maintained, point-surface residual is constructed using adaptive fitting plane, and low-frequency joint optimization is carried out.The method can effectively improve the positioning accuracy and robustness of the automatic driving vehicle in complex and degraded scenarios.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving environmental perception and positioning navigation technology. Specifically, it relates to a method and system for laser inertial odometry in complex scenarios for autonomous driving. Background Technology

[0002] Laser inertial odometry (LIO) is a key technology in the environmental perception and localization navigation of autonomous vehicles. By fusing data from lidar and inertial measurement units (IMUs), it enables continuous pose estimation of vehicles in urban roads, tunnels, underground parking lots, and environments where satellite signals are blocked or denied. In autonomous driving applications, the positioning accuracy and stability of LIO directly affect the reliability of vehicle path planning, motion control, and safety decisions.

[0003] Existing laser inertial odometry methods typically utilize line features, surface features, or local geometric structures in lidar point clouds for point cloud matching, and combine this with high-frequency motion information from the inertial measurement unit to complete state prediction and updates. Represented by the LOAM series algorithms and tightly coupled lidar-inertial odometry methods based on iterative error state Kalman filtering, existing technologies can improve the real-time performance and accuracy of vehicle pose estimation to a certain extent.

[0004] However, existing methods still have significant shortcomings in complex autonomous driving scenarios. First, urban road environments contain a large number of dynamic traffic participants such as vehicles and pedestrians. Traditional point cloud screening methods based on geometric features struggle to effectively distinguish between static structural features and dynamic target features, easily incorporating dynamic point cloud errors into residual calculations and state update processes, thus causing pose estimation drift. Second, in geometrically degraded scenarios such as tunnels, long straight corridors, and underground passages, there are insufficient effective features for constraining pose. Traditional point cloud matching methods are prone to constraint degradation, leading to increased positioning errors in the longitudinal or vertical directions of the vehicle.

[0005] Furthermore, existing laser inertial odometry methods often employ fixed-size voxel grids or fixed-neighborhood point cloud fitting when constructing local maps. Since point cloud distribution in autonomous driving scenarios is significantly non-uniform, fixed-scale map structures struggle to adapt to both sparse and complex dense regions simultaneously, easily leading to insufficient planar fitting accuracy and consequently affecting the quality of point-to-surface residual constraints. Simultaneously, some existing methods rely heavily on recursive state updates between the current frame and the local map, lacking consistency constraints on poses across multiple historical frames. This can easily lead to accumulated errors during long-distance driving, particularly noticeable trajectory drift in degraded scenarios. Summary of the Invention

[0006] To address the problems existing in the prior art, this invention provides a method and system for laser inertial odometry in complex scenarios for autonomous driving.

[0007] To achieve the above-mentioned technical objectives, the technical solution adopted by the present invention is as follows: A laser inertial odometry method for autonomous driving in complex scenarios includes the following steps: S1. Acquire LiDAR point cloud data and inertial measurement unit data in autonomous driving scenarios, perform projection processing on the LiDAR point cloud, and extract multimodal features from the LiDAR point cloud. S2. Input the multimodal features extracted from the laser point cloud into a pre-constructed point cloud saliency prediction network for autonomous driving scenarios to perform saliency prediction, generate a saliency map, and filter out dynamic objects by combining the point cloud dynamic segmentation network to obtain saliency weights for pose estimation. S3. Based on the input data of the inertial measurement unit, the state is forward propagated to obtain the prior state and the corresponding covariance. The point and surface residuals are constructed by combining the significance weights. The state is updated by iterative error state Kalman filtering to obtain the laser inertial odometry pose result of the current frame. S4. Construct an adaptive planar voxel map based on hash index, and calculate the centroid and covariance matrix of points within the voxels. S5. Determine whether the voxel satisfies the planar characteristic condition based on the eigenvalue relationship of the covariance matrix. If it does not satisfy the condition, proceed to step S6; if it does satisfy the condition, proceed to step S7. S6. Recursively segment voxels that do not meet the conditions to achieve adaptive plane fitting from coarse to fine. S7. Treat the voxel as a planar voxel and store its planar information. Maintain the pose state of multiple consecutive frames based on the sliding window. Construct multi-frame point and surface constraints using the planar voxel. Perform joint optimization of the pose of multiple frames within the sliding window in a low-frequency manner. Output the optimized laser inertial odometry pose result.

[0008] Compared with the prior art, the present invention has the following advantages: By combining point cloud saliency prediction, dynamic object filtering, iterative error state Kalman filtering, adaptive planar voxel mapping, and sliding window joint optimization, autonomous vehicles can more stably complete laser inertial odometry positioning in dynamic or degraded scenarios such as complex urban roads, tunnels, and long straight corridors.

[0009] By constructing a point cloud saliency prediction network for autonomous driving scenarios and combining it with a dynamic object filtering mechanism, high saliency weights are assigned to high-quality static point cloud regions, reducing the impact of dynamic vehicles, pedestrians and noise points on pose estimation, thereby improving the robustness of localization results in complex dynamic environments.

[0010] By incorporating saliency weights into the point-to-surface residual calculation and the iterative error state Kalman filter state update process, pose estimation is made more inclined towards stable and reliable environmental feature regions, thus improving the problem of insufficient confidence differentiation of different point cloud features in traditional laser inertial odometry.

[0011] By constructing an adaptive planar voxel map based on hash indexes and performing recursive segmentation and planar fitting based on the eigenvalue relationships of the covariance matrix of point clouds within voxels, the map can be adaptively adjusted according to the local point cloud distribution and geometric complexity, thereby improving the accuracy of planar fitting and the quality of point-plane constraints.

[0012] By maintaining poses across multiple frames using a sliding window and constructing multi-frame point-surface constraints based on an adaptive fitting plane for low-frequency joint optimization, the cumulative error of autonomous vehicles during long-distance operation can be reduced, positioning drift in degraded scenarios can be suppressed, and overall positioning accuracy and stability can be improved. Attached Figure Description

[0013] Figure 1 This is an overall flowchart of a laser inertial odometry method for autonomous driving in a complex scenario, as described in an embodiment of the present invention. Figure 2 This is a detailed flowchart of the laser inertial odometry method for autonomous driving in complex scenarios in an embodiment of the present invention; Figure 3 This is a schematic diagram of the point cloud saliency prediction network framework in an embodiment of the present invention; Figure 4 Schematic diagram of the nearest neighbor fitting plane search in this embodiment of the invention; Figure 5 A schematic diagram of planar voxel segmentation in an embodiment of the present invention; Figure 6 A schematic diagram of planar features corresponding to multi-frame poses in an embodiment of the present invention; Figure 7 A visualization diagram of point cloud saliency prediction in an embodiment of the present invention; Figure 8 A visual comparison diagram of adaptive plane fitting in an embodiment of the present invention; Figure 9 A schematic diagram of the positioning accuracy of SAP-LIO on some test sequences in an embodiment of the present invention; Figure 10 A schematic diagram comparing the longitudinal positioning of different algorithms in a tunnel in this embodiment of the invention. Detailed Implementation

[0014] To facilitate understanding by those skilled in the art, the present invention will be further described below with reference to embodiments and accompanying drawings. The content mentioned in the embodiments is not intended to limit the present invention.

[0015] like Figure 1and 2 As shown, this embodiment provides a laser inertial odometry method for autonomous driving in complex scenarios, including the following steps: S1. Acquire LiDAR point cloud data and inertial measurement unit data in autonomous driving scenarios, perform projection processing on the LiDAR point cloud, and extract multimodal features from the LiDAR point cloud. S2. Input the multimodal features extracted from the laser point cloud into a pre-constructed point cloud saliency prediction network for autonomous driving scenarios to perform saliency prediction, generate a saliency map, and filter out dynamic objects by combining the point cloud dynamic segmentation network to obtain saliency weights for pose estimation. S3. Based on the input data of the inertial measurement unit, the state is forward propagated to obtain the prior state and the corresponding covariance. The point and surface residuals are constructed by combining the significance weights. The state is updated by iterative error state Kalman filtering to obtain the laser inertial odometry pose result of the current frame. S4. Construct an adaptive planar voxel map based on hash index, and calculate the centroid and covariance matrix of points within the voxels. S5. Determine whether the voxel satisfies the planar characteristic condition based on the eigenvalue relationship of the covariance matrix. If it does not satisfy the condition, proceed to step S6; if it does satisfy the condition, proceed to step S7. S6. Recursively segment voxels that do not meet the conditions to achieve adaptive plane fitting from coarse to fine. S7. Treat the voxel as a planar voxel and store its planar information. Maintain the pose state of multiple consecutive frames based on the sliding window. Construct multi-frame point and surface constraints using the planar voxel. Perform joint optimization of the pose of multiple frames within the sliding window in a low-frequency manner. Output the optimized laser inertial odometry pose result.

[0016] Multimodal features in laser point clouds include local curvature, depth, intensity, and semantic features.

[0017] The detailed steps of step S1 include: The three-dimensional laser point cloud is converted into a two-dimensional image through cylindrical projection, and local curvature map, depth map and intensity map are generated based on the two-dimensional image respectively; The depth map is processed using a semantic segmentation network to obtain a semantic information map; The local curvature map, depth map, intensity map, and semantic information map are used as multimodal features input into a point cloud saliency prediction network for autonomous driving scenarios.

[0018] We employ the local curvature calculation method from LOAM, combined with point cloud depth information, to uniformly transform it into a two-dimensional projection form as the saliency screening criterion. Simultaneously, we utilize cylindrical projection of the point cloud to map the three-dimensional point cloud into a two-dimensional image, and introduce a mature semantic segmentation network to extract semantic information from the point cloud, thereby enhancing the ability to distinguish targets in different environments. Furthermore, point cloud intensity information is affected by factors such as object surface characteristics, incident angle, and laser energy, which can also serve as important references for saliency assessment.

[0019] In summary, based on the four dimensions of local curvature, depth, semantics, and intensity, a unified cylindrical two-dimensional projection method is used to calculate saliency, and finally a saliency map is generated. For multi-line mechanical lidar, after completing one scan, the point cloud data is projected into a two-dimensional image of size 𝑊×𝐻, and converted to the Cartesian coordinate system according to the depth, yaw angle, and pitch angle of each laser point using formula (1).

[0020] Laser points in Cartesian coordinate system Converted to a two-dimensional image coordinate system The conversion relationship between the two is shown in equation (2): in, The vertical field of view. The field of view is vertically upward. This represents the depth value corresponding to the laser point.

[0021] Based on formula (2), the local curvature, depth, and intensity information of the laser point cloud are projected into a two-dimensional image. Subsequently, based on the obtained depth information map, the mature semantic segmentation network RangeNet++ is used to process it to obtain a semantic information map.

[0022] In step S2, the point cloud saliency prediction network models the saliency of laser points based on multimodal features of local curvature, depth, semantics and intensity, and generates a saliency map by weighted fusion of multimodal features. The point cloud saliency prediction network adopts an encoder-decoder structure and uses the generated saliency map as a supervision signal for training.

[0023] In step S2, the point cloud dynamic segmentation network is used to identify dynamic objects in the laser point cloud, and the dynamic segmentation result is fused with the saliency map output by the point cloud saliency prediction network to filter the dynamic object information and generate the final saliency map for subsequent pose estimation.

[0024] The four types of information maps are processed to obtain corresponding feature maps. First, the distance and intensity information are normalized and sparse regions are filled. Then, they are input into the improved ITTI saliency model, which uses an adaptive Gaussian pyramid (standard deviation of each layer). The external filtering is replaced by a scaling-increasing method, and the geometric gradient contrast, which integrates depth and intensity abrupt changes, replaces color contrast calculation to generate depth and intensity feature maps. In semantic information processing, categories with regular shapes and rich information are assigned higher weights, while categories with irregular shapes or low information content are assigned lower weights. Furthermore, the processing method for dynamic objects will be further elaborated in subsequent chapters. In the processing of local curvature, the threshold setting method used in LOAM to distinguish between line and surface features is adopted. Specifically, portions above the line feature threshold are assigned higher weights, and portions below the surface feature threshold are also assigned higher weights.

[0025] The depth was obtained through the above processing. ,strength curvature and semantics The feature map is then concatenated into a four-channel tensor based on these four modal information types: Then, the spatial-channel joint weights of each mode are further calculated: in, Map the 4 channels to an intermediate dimension and extract cross-modal correlations. It is the activation function that enhances nonlinearity. Used to capture local spatial context. Normalize along the channel dimension to make the weight sum equal to 1.

[0026] Finally, the features of each modality are weighted and fused using formula (5) to obtain a saliency map, where the region with higher brightness has a greater saliency value. This method can be used to generate a saliency point cloud dataset and to train the subsequent saliency network.

[0027] in For the first Attention weight map for each modality This is pixel-by-pixel multiplication.

[0028] Point cloud saliency prediction framework: In the aforementioned saliency calculation process, the failure to filter out dynamic objects in the autonomous driving scenario significantly impacts the algorithm's localization accuracy. Therefore, as... Figure 3As shown in (b), a dynamic point cloud segmentation network is introduced into the final point cloud saliency prediction network framework to effectively identify and filter out dynamic objects. To facilitate subsequent network training, the SemanticKITTI dataset is reorganized. By utilizing the existing annotation information of mobile traffic participants in the dataset, the original labels are integrated into two classes: "mobile" and "static," with "mobile" set to 1 and "static" set to 0. Based on the design of RangeNet++, a classic encoder-decoder architecture is adopted. The residual module structure used in the encoder is as follows: Compared to ordinary convolutional layers, it is more capable of aggregating features. Since the point cloud projection map is elongated, to avoid loss of height information, the downsampling module only reduces the dimension in the width direction, with a stride of 2. The upsampling module uses deconvolution, which is used in the decoder. The convolutional layers are also set to a stride of 2. Finally, the output layer of the decoder is adjusted, reducing its number of channels from the original number of semantic categories to 2, corresponding to both dynamic and static classification tasks. Ultimately, the network can output the "moving" or "static" classification probability for each LiDAR point.

[0029] In the specific training of the dynamic segmentation network, a weighted cross-entropy loss is used to alleviate the class imbalance between dynamic and static objects, as shown in formula (6): in, This represents the total number of pixels in a single batch of samples. For the first The real label of each pixel To predict the probability that the pixel belongs to the dynamic class for the model. and These are the dynamic and static class weight coefficients, respectively, which need to be adjusted based on the specific training dataset (in this invention). , In general, when the training dataset has few dynamic classes, increasing the weights of the dynamic classes can help. This allows the model to focus more on sparse, dynamic targets.

[0030] Similarly, such as Figure 3 (a) In the design of the saliency network, an encoder-decoder structure is also adopted. First, the previously generated saliency map is regarded as the probability distribution of salient regions in the whole image, and the L1 norm is used as the loss function for optimization, as shown in Equation (7). The softmax normalization method is used to convert the saliency predicted by the model and the corresponding ground truth values ​​of point cloud saliency into probability distributions, as shown in Equation (8):

[0031] in, The significance probability distribution for the prediction. It is the probability distribution of the significance of the true value. and These are the saliency values ​​of pixels in the predicted saliency map and the true saliency map, respectively.

[0032] Finally, through the analysis of Figure 3 (a) Saliency network and Figure 3 (b) The point cloud dynamic segmentation network is trained under supervision. The saliency map predicted by the saliency network is fused with the output of the dynamic segmentation network to filter information of dynamic objects, thereby generating the final saliency map. This saliency map can be used for pose estimation in subsequent laser inertial odometry calculations.

[0033] In step S2, the saliency map assigns a saliency weight to each laser point, and this saliency weight is used in the residual calculation for subsequent pose estimation. The definition formula for the saliency weight is as follows: After saliency prediction processing, the laser point cloud is assigned saliency values, generating point cloud data with weighted information. This result will be used for subsequent point cloud matching and residual calculation. Here, we first define the saliency weights. As shown in formula (9): in This represents the significance value of the corresponding pixel in the saliency map. and It is a constant. Regions with better point cloud quality... It is also relatively large. As a coefficient for residual calculation, It can guide the optimization direction to be more inclined towards high-quality point cloud regions, thereby accelerating the convergence of the optimization solution process.

[0034] The detailed steps of step S3 include: State forward propagation is performed using inertial measurement unit data to obtain prior states and corresponding covariances; Perform distortion correction processing on the laser point cloud of the current frame, and then transform the distortion-corrected point cloud of the current frame to the global coordinate system; Search for the neighboring points of the current laser point and perform plane fitting, and construct the point-plane residual measurement equation by combining the significance weights; The state is updated by iterative error state Kalman filtering to obtain the optimal state estimate and its corresponding covariance matrix; The laser inertial odometry pose result for the current frame is obtained based on the attitude and position in the optimal state estimate.

[0035] IMU motion model and forward propagation: If the system's IMU and LiDAR are rigidly connected and their external parameters are known... Based on this, using the IMU as the vehicle body reference frame, the kinematic model describing the system's motion state can be derived as follows:

[0036] (10) in, As a gesture, For speed, For location, and The acceleration and angular velocity measured by the IMU and It is random walk noise with a derivative of a Gaussian process. and To measure noise. Furthermore, the continuous motion model of equation (10) can be transformed into a discrete motion model, as shown in equation (11):

[0037] Among them, state variables ,function Input quantity ,noise The definition is as follows: in This represents a manifold defined on the state space. Each time IMU data is received, a forward propagation is performed to transmit the IMU state. This process includes noise terms. Setting it to 0, we get equation (14) as follows: in, The state variables passed during the filtering process, and This represents the optimal estimate obtained after filtering. Furthermore, to represent the error change between the predicted and true values ​​during forward propagation, formula (15) is used for calculation. Indicates the change in error:

[0038] in, State variables For state variables The Jacobian matrix has a size of 24. twenty four, State variables For error quantity The Jacobian matrix has a size of 24. 12. Here, the final matrix representation is further derived and obtained, as shown in formulas (17) and (18).

[0039] White noise propagating in the forward direction The covariance is defined as Furthermore, by substituting the two sparse matrices obtained from equations (17) and (18) into the filter transfer formula, the covariance transfer equation is derived as shown in equation (19). Thus, the complete derivation of the state and error transfer process is completed.

[0040] Significance-based residual calculation and pose determination: Suppose that in the iterative update of IESKF, the estimation of the state variables is as follows: , Refers to the first The end time of the frame point cloud is before the first iteration. ), These are the predicted values ​​from the forward propagation. Here, we first use backpropagation to correct the distortion of the point cloud and then transform the corrected point cloud to the global coordinate system.

[0041] like Figure 4 As shown, after the point cloud undergoes saliency prediction, the normal vector of the resulting plane is calculated by searching for the 5 nearest neighbors in the ikd-Tree and performing plane fitting. This allows the construction of a residual model. in, It is a plane normal vector. To fit a point in the plane, This refers to noise during the radar scanning process. Let be the significance weight defined in equation (21). Further, by... The measurement equation is simplified by using a first-order Taylor approximation:

[0042] in, Represents the point-to-surface residual. , express about Jacobian matrix, Indicates by The introduced Gaussian noise term, i.e. For ease of calculation, Set it to a constant. It is important to note that... The specific representation is shown in formula (22):

[0043] Since the forward propagation of the state and error variables has already been completed, for the unknown state... The above results provide a prior Gaussian hypothesis for it: in, for Relative to the zero point position The partial derivatives. Furthermore, in addition to the prior distribution, there exists a state distribution induced by the measurement equation (Equation 21):

[0044] By combining the prior distribution in equation (23) with the measurement model in equation (24), the state can be solved. Maximum a posteriori estimate (MAP): definition , For a vector of the corresponding dimension, Let be an invertible matrix. To further simplify the calculation, let be... If the number of laser points is , then , , , Then, substituting the result of equation (23) into equation (25), we can solve for the result based on the Iterative Error Kalman Filter (IESKF). Here, we define the Kalman gain. The calculations were optimized to improve the Kalman gain. The calculation results no longer depend on the dimension of the laser point measurement, but are related to the transpose dimension of the state matrix. This improvement significantly reduces computational complexity. Kalman gain The iterative formula is defined as follows:

[0045] The updated state quantity after iteration It will continue to be used in the residual calculation in equation (20). When the Kalman iteration reaches the maximum number of iterations, or when the change of state variables during the iteration process is less than the preset threshold, the optimal state estimate and its corresponding covariance matrix can be obtained.

[0046] At this point, the state update process based on IESKF has been completed. The calculated point cloud pose can be transformed into the global coordinate system through the transformation matrix. The specific pose estimation algorithm based on saliency is shown in Table 1.

[0047] Table 1. Flowchart of the pose estimation algorithm based on saliency In step S4, the adaptive planar voxel map is managed using a hash index. The hash key is formed by the voxel hierarchical index and its normalized spatial coordinates. Voxels are queried, inserted, and updated according to the hash key. Large voxels are retained in sparse regions, while small voxels are generated in dense regions or geometrically complex regions, so that the planar fitting scale is adaptively adjusted according to the local point cloud distribution.

[0048] Adaptive plane fitting strategy: Drawing inspiration from voxel mapping, an adaptive plane fitting (AP) strategy based on voxel mapping is introduced. Compared with traditional voxel segmentation methods, this strategy can extract geometric structures from point clouds more efficiently.

[0049] To improve the accuracy of plane fitting, a local map composed of dense points is constructed by accumulating multiple frames of continuous point clouds, and plane fitting is performed on this local map from coarse to fine. Specifically, for the local point cloud... First, calculate the coordinate centroid and the covariance matrix of the point cloud after centroid removal:

[0050] Then, methods such as Jacobi iteration or QR decomposition are used to analyze the covariance matrix. Perform eigenvalue decomposition: The calculation formula for determining whether a voxel satisfies the planar characteristic condition based on the eigenvalue relationship of the covariance matrix in step S5 is as follows: For eigenvalues, This represents the corresponding orthogonal feature vector. Here, a threshold is set. And a Boolean function was defined. Used to distinguish surface features:

[0051] If the current voxel satisfies the planar feature condition, then the planar information corresponding to the voxel is stored; if the current voxel does not satisfy the planar feature condition, then the voxel is recursively segmented.

[0052] As can be seen from this Boolean function, a lower degree of local surface variation indicates that the region has relatively flat and uniform characteristics, while a higher degree of variation suggests that the region may have curved or irregular geometric features. Based on the above analysis, this invention will construct an adaptive planar voxel map based on hash indexing, such as... Figure 3 As shown. First, based on equations (28) and (29), the coordinate centroid of the root voxel and the covariance matrix of the point cloud are calculated, and then the corresponding eigenvalues ​​are solved. Then, equation (30) is used to determine whether the voxel is a planar voxel. If the current voxel meets the planar threshold condition, the normal vector of the voxel is stored in a hash table. and its centroid coordinates If the planar threshold is not met, the current voxel is divided into eight sub-voxels (octree structure), and the covariance matrix and eigenvalues ​​are repeatedly calculated for each sub-voxel to perform planar feature discrimination. The recursive segmentation process continues until the preset maximum recursion depth is reached or the sub-voxels meet the planar feature conditions. After the above process, each voxel has a different size and contains a planar feature fitted based on the laser point cloud.

[0053] Given a laser point with a prior attitude located in the global coordinate system Starting from the root voxel, the corresponding voxel is queried layer by layer based on the hash key. This hash key is uniquely determined by the voxel's hierarchical index and its normalized spatial coordinates.

[0054] Firstly, for the hierarchical index, the root voxel is... Each time the daughter element is divided, Normalized coordinates convert physical coordinates into integer indices to avoid floating-point errors, as shown in formula (31): in The root voxel size is used, and then a hash key is calculated using prime number weighting to avoid coordinate conflicts between different levels, as shown in formula (32). It is a large prime number.

[0055] hash table The value stores planar information, specifically including the normal vectors of the voxels. and center of mass When performing a hash table lookup, if the corresponding key-value pair already exists, the point is added to the point cloud list of the corresponding voxel; if the key-value pair does not exist, a new voxel is created, its point cloud list is initialized, and the point is added to it. At the same time, the newly generated key-value pair is registered in the hash table.

[0056] In summary, adaptive voxel segmentation achieves coarse-to-fine plane fitting, preserving large voxels in sparse regions and small voxels in dense regions. This method lays the foundation for subsequent pose optimization based on plane fitting. Furthermore, the use of a hash table data structure ensures that the time complexity of insertion and query operations is O(n log n). .

[0057] Pose optimization based on adaptive plane fitting: To further improve the accuracy of pose estimation, this section builds upon the previously employed adaptive plane fitting strategy by utilizing a sliding window to maintain and optimize multi-frame pose data. Specifically, this sliding window refines the output pose through comprehensive processing of poses from consecutive frames. To ensure real-time performance, the sliding window optimization is performed at a relatively low frequency (2 Hz).

[0058] Through the aforementioned adaptive plane fitting, the local point cloud map will be fitted with planar features of varying sizes. Since the LiDAR is constantly moving, the point cloud data in each planar feature comes from consecutive and different LiDAR scan frames. For example... Figure 4 As shown, assuming there exists Each LiDAR pose is defined by a LiDAR pose. It indicates that it was observed simultaneously. Each planar feature is defined by parameters. express.

[0059] Here, we define Indicates that due to the first The building caused by the feature Figure 1 Consistency constraints, based on which the optimization equation can be constructed as follows: In this optimization equation, the objective is to minimize the distance between each scanning laser point and its corresponding planar feature. Specifically, let... Represented as the first The first lidar pose observation The number of laser points contained in a planar feature. Each laser point is specifically represented as... It can be transformed to the global coordinate system using a transformation matrix:

[0060] (34) Planar parameters are stored for each planar feature. , It is a plane normal vector. Let be any point in the plane. Then the observation point... The distance to the corresponding plane can be expressed as: Subsequently, the laser points scanned under all lidar poses are accumulated, and a corresponding loss function is constructed:

[0061] (35) in , representing the total number of laser points observed by all radar poses regarding the planar features. Analyzing this loss function reveals the loss term. It relates only to a single planar feature and corresponds to a unique planar parameter. Therefore, the planar parameter can be... Separate it from the loss term, so that the loss term is only related to the pose. Based on this, and considering all planar features, the final loss function is constructed as follows:

[0062] (36) Then, substituting equation (35) into equation (36) yields: (37) Referring to the idea in equation (28), the covariance matrix of the point cloud within a single planar voxel is defined. With local centroid as follows: (38) Further analysis of equations (36) and (27) reveals that adjusting the pose... This allows the point cloud covariance matrix to satisfy specific geometric constraints (i.e., the minimum eigenvalue tends to zero), thereby minimizing the distance from the point to the plane. Therefore, equation (37) can be simplified to solving the covariance matrix. The corresponding minimum eigenvalue :

[0063] (39) In step S7, a sliding window local map is constructed using consecutive multi-frame point clouds. Multi-frame point-surface constraints are built based on the adaptive plane fitting results. The joint optimization problem is transformed into an optimization problem that minimizes the sum of the minimum eigenvalues ​​corresponding to each plane voxel. The loss function can ultimately be simplified to minimizing the sum of the minimum eigenvalues ​​corresponding to all plane voxels. The optimization objective formula is: (40) in Represents the covariance matrix. Represents the covariance matrix The third eigenvalue; the low-frequency method is used to jointly optimize the pose of multiple frames of LiDAR within the sliding window, and the optimized LiDAR inertial odometry pose result is output.

[0064] Due to the covariance matrix Variables involved in the calculation Depends on lidar pose Therefore, in the calculation of the loss function, the result is only related to the lidar pose. Related. To solve for this loss function, the second-order Newton method or the Levenberg-Marquardt method can be used for iterative solution. Thus, simultaneous optimization of radar pose across multiple consecutive frames in a local area is achieved, improving the system's positioning accuracy.

[0065] The local map maintained in the sliding window is formed by accumulating 5 to 10 consecutive frames of point cloud data. When the sliding window is full, the old point cloud frames are removed, and the planar information such as the centroid and normal vector of the corresponding voxels are merged into the global container. In the process of a new point cloud frame entering the sliding window, the corresponding voxel is first searched using a hash index. When the sliding window optimization frequency requirement (2Hz) is met, optimization is triggered, utilizing a global container. The planar information is used to construct point-plane residuals, and the pose of the lidar maintained within the sliding window is jointly optimized.

[0066] A laser inertial odometry system for autonomous driving in complex scenarios includes a data acquisition and preprocessing module, a pose estimation module, an initial laser inertial odometry pose module, a voxel point cloud centroid and covariance matrix calculation module, a plane feature condition judgment module, a voxel recursive segmentation module, and an optimized laser inertial odometry pose output module. The data acquisition and preprocessing module is used to acquire lidar point cloud data and inertial measurement unit data in autonomous driving scenarios, perform projection processing on the lidar point cloud, and extract multimodal features from the lidar point cloud. The pose estimation module communicates with the data acquisition and preprocessing module to input the multimodal features extracted from the laser point cloud into a pre-constructed point cloud saliency prediction network for autonomous driving scenarios to perform saliency prediction, generate a saliency map, and filter out dynamic objects by combining the point cloud dynamic segmentation network to obtain saliency weights for pose estimation. The initial laser inertial odometry pose module is connected to the pose estimation module for forward propagation of state based on the input data of the inertial measurement unit, to obtain the prior state and corresponding covariance, and to construct point-surface residuals by combining significance weights. Iterative error state Kalman filtering is used to update the state and obtain the laser inertial odometry pose result of the current frame. The module for calculating the centroid and covariance matrix of point clouds within voxels is connected to the initial laser inertial odometry pose module to construct an adaptive planar voxel map based on hash index and to calculate the centroid and covariance matrix of point clouds within voxels. The planar feature condition judgment module is connected to the voxel point cloud centroid and covariance matrix calculation module. It is used to judge whether the voxel meets the planar feature condition based on the eigenvalue relationship of the covariance matrix. If it does not meet the condition, it enters the voxel recursive segmentation module. If it does meet the condition, it enters the optimized laser inertial odometry pose output module. The voxel recursive segmentation module is connected to the planar feature condition judgment module to recursively segment voxels that do not meet the conditions, thereby achieving adaptive planar fitting from coarse to fine. The optimized laser inertial odometry pose output module is connected to the planar feature condition judgment module. It is used to treat the voxel as a planar voxel and store its planar information. It maintains the pose state of multiple consecutive frames based on a sliding window, constructs multi-frame point and surface constraints using planar voxels, and performs joint optimization of the pose of multiple frames within the sliding window in a low-frequency manner, outputting the optimized laser inertial odometry pose result.

[0067] Compared with the prior art, the present invention has the following advantages: By combining point cloud saliency prediction, dynamic object filtering, iterative error state Kalman filtering, adaptive planar voxel mapping, and sliding window joint optimization, autonomous vehicles can more stably complete laser inertial odometry positioning in dynamic or degraded scenarios such as complex urban roads, tunnels, and long straight corridors.

[0068] By constructing a point cloud saliency prediction network for autonomous driving scenarios and combining it with a dynamic object filtering mechanism, high saliency weights are assigned to high-quality static point cloud regions, reducing the impact of dynamic vehicles, pedestrians and noise points on pose estimation, thereby improving the robustness of localization results in complex dynamic environments.

[0069] By incorporating saliency weights into the point-to-surface residual calculation and the iterative error state Kalman filter state update process, pose estimation is made more inclined towards stable and reliable environmental feature regions, thus improving the problem of insufficient confidence differentiation of different point cloud features in traditional laser inertial odometry.

[0070] By constructing an adaptive planar voxel map based on hash indexes and performing recursive segmentation and planar fitting based on the eigenvalue relationships of the covariance matrix of point clouds within voxels, the map can be adaptively adjusted according to the local point cloud distribution and geometric complexity, thereby improving the accuracy of planar fitting and the quality of point-plane constraints.

[0071] By maintaining poses across multiple frames using a sliding window and constructing multi-frame point-surface constraints based on an adaptive fitting plane for low-frequency joint optimization, the cumulative error of autonomous vehicles during long-distance operation can be reduced, positioning drift in degraded scenarios can be suppressed, and overall positioning accuracy and stability can be improved.

[0072] Experimental Design: To evaluate the algorithm's performance, this invention underwent comprehensive testing on public datasets and the Chongqing University campus environment. First, the effectiveness of the saliency module (S) was validated on the KITTI dataset, and the processing time of S-LIO for a single frame of point cloud was recorded. Next, the performance of the adaptive plane fitting (AP) module was verified through visualization results and the improvement effect of AP-based sliding window optimization on Z-axis drift. Subsequently, AP-LIO's performance was compared with other state-of-the-art algorithms on the M2DGR dataset. The final SAP-LIO algorithm was validated on the UTBM and KAIST datasets, and its localization accuracy was compared with other LO and LIO algorithms. Finally, testing was conducted in a campus setting. All experiments were performed on a Linux platform running Ubuntu 18.04, with hardware including an NVIDIA RTX 2060 graphics card, an Intel i7-11700F processor, and 32 GB of RAM.

[0073] Analysis of the effectiveness and real-time nature of saliency prediction: To verify the effectiveness of the saliency prediction network, the original point cloud was first processed using the network, and the results were visualized and analyzed. For further evaluation, ablation experiments were conducted on the KITTI dataset, and the localization accuracy was quantitatively analyzed by calculating the absolute trajectory error (ATE) and relative pose error (RPE). Experimental results show that the proposed saliency prediction network can accurately extract high-quality point cloud regions and improve the algorithm's localization accuracy. To evaluate the algorithm's running efficiency, the time taken to process a single frame of point cloud using the odometry method after adding the saliency module is presented, and it is compared with other state-of-the-art algorithms.

[0074] like Figure 7 (a) shows the original point cloud, which is processed by a point cloud saliency prediction network to obtain the final saliency map. Subsequently, this saliency map is back-projected into a saliency point cloud. For example... Figure 7 In (b), the highlighted areas are identified as salient regions during the saliency prediction process, including sharp corners and edges, distinctive buildings, isolated pillars, and vehicles. Conversely, flat and continuous surfaces, haphazardly distributed vegetation, and areas far from the sensor and with unstable scanning are classified as non-salient regions. Based on the saliency classification of different regions in the point cloud, each point is assigned a corresponding saliency weight. These weights will be used in subsequent residual calculations and pose estimation to increase the emphasis on key features and accelerate the algorithm's convergence process.

[0075] To specifically evaluate the optimization effect of saliency prediction on the LIO algorithm, this invention was tested on sequences 00 to 10 of the KITTI dataset. The test compared the complete algorithm including saliency prediction with a version of the LIO algorithm that does not use saliency prediction. To ensure the reliability of the results, 10 tests were performed for each sequence and the average value was taken. Tables 2 and 3 show the test results for absolute trajectory error (ATE) and relative pose error (RPE), respectively.

[0076] Table 2. Comparison of ablation prediction results based on the significance of absolute trajectory error. Table 3. Significance prediction and ablation comparison results based on relative pose error According to the data in Tables 2 and 3, both the absolute trajectory error (ATE) and the relative pose error (RPE) decreased after the addition of the saliency prediction module. Using RSME as the main evaluation metric, on the KITTI dataset, the addition of this module reduced ATE by an average of 43.7%, while RPE decreased by 72.1%. These results demonstrate the effectiveness of the saliency prediction module in improving localization accuracy. For example, in dynamic urban environments (such as sequences 00 and 02), the saliency prediction module can effectively suppress potential dynamic disturbances (such as vehicles and pedestrians) and cope with occlusion caused by complex structures, thereby reducing localization errors. On low-texture structured roads (such as sequences 03 and 07), this module can assign higher weights to stable features. However, in simple static scenes (such as sequence 04), the ATE-RMSE only decreased slightly from 0.41 meters to 0.39 meters, showing limited improvement, indicating that the algorithm may have computational redundancy in low-dynamic scenes. Future work could focus on optimizing the saliency model for such scenarios to achieve a lightweight design. For urban expressway scenarios (such as Sequence 01), due to the limited information that can be extracted from the laser point cloud, ATE and RPE are still relatively high.

[0077] Table 4. Comparison of processing time for each algorithm on the KITTI dataset for a single frame of point cloud. Given that the saliency prediction model proposed in this invention is based on deep learning, and considering the importance of lightweight algorithms for autonomous vehicles, this invention compares the odometry algorithm with an added saliency module (S-LIO) with other advanced laser SLAM algorithms, and the processing time for a single frame of point cloud is shown in Table 4. The average processing time for a single frame of point cloud using S-LIO is 46.56 milliseconds, demonstrating higher efficiency and better real-time performance compared to other advanced deep learning-based algorithms such as KDD-LOAM and SAGE-ICP.

[0078] Validity analysis of adaptive plane fitting: This section primarily verifies the effectiveness of the Adaptive Plane Fitting (AP) strategy and its corresponding odometry pose optimization algorithm (AP-LIO). First, the AP strategy is applied to a laser point cloud map, and a comparative analysis is performed through visualization. Then, the effectiveness of AP-LIO in addressing the trajectory Z-axis drift problem is verified. Finally, AP-LIO is evaluated on the M2DGR dataset to quantify its improvement in positioning accuracy.

[0079] like Figure 8 The image shows a visual comparison of adaptive plane fitting in point cloud map processing. Figure 8 (a) is the original point cloud map. Figure 8 (b) is the processed point cloud map. From Figure 8 As shown in (b), adaptive plane fitting can identify coplanar point sets, effectively extract planar features from the environment, and improve the clarity of structural contours such as walls and floors. Compared with traditional fixed-threshold plane fitting, this method exhibits stronger robustness in complex scenes. For example, at the junction of columns and walls, this method can maintain the continuity of features; in areas where the point cloud is relatively sparse, it can still effectively extract planar features.

[0080] To verify the performance of AP-LIO, this invention was tested on 10 street sequences in the M2DGR dataset. For each sequence, A-LOAM, LeGO-LOAM, LIO-SAM, FAST-LIO2, Faster-LIO, and AP-LIO were run 10 times, and the root mean square value (RMSE) of the absolute trajectory error (ATE) was calculated. The average value was then taken as the result of each algorithm on that sequence.

[0081] Table 5 Comparison of algorithm accuracy based on absolute trajectory error on the M2DGR dataset Table 5 shows the test results of different algorithms on the M2DGR dataset. It can be seen that AP-LIO exhibits smaller trajectory errors in half of the sequences and has the smallest average error across all 10 sequences. Specifically, compared to A-LOAM, AP-LIO reduces the average trajectory error by 73.4%, and compared to FAST-LIO2, it reduces it by 26.1%.

[0082] Comparative analysis of different positioning algorithms: This section combines the previously introduced saliency prediction module with the adaptive plane fitting module to form the SAP-LIO algorithm. To verify the algorithm's performance in complex urban environments, eight representative sequences were selected from the UTBM and KAIST-Urban datasets for testing, and compared with current state-of-the-art laser and laser inertial odometry methods. The algorithms tested included A-LOAM, Point-LIO, D-LIO, FAST-LIO2, and Faster-LIO, and the root mean square error (RMSE) of the absolute trajectory error (ATE) was used as the evaluation metric. Table 6 provides a brief overview of the corresponding test sequences in the original datasets. This comprehensive comparative test aims to deeply evaluate the performance of SAP-LIO in urban environments of varying complexity and its advantages and disadvantages compared to other algorithms.

[0083] Table 6. Details of the test sequences Table 7 Comparison of Algorithm Accuracy Based on Absolute Trajectory Error Table 7 shows the comparison results of positioning accuracy of different algorithms. Among all test sequences, SAP-LIO achieved the lowest root mean square error (RMSE) on multiple sequences, especially on the UTBM-1, UTBM-3 and KAIST-3 sequences, with errors of 9.069 meters, 9.262 meters and 3.581 meters, respectively, which is better than other algorithms.

[0084] Figure 9 The visualization results of the absolute pose error of the proposed SAP-LIO algorithm in partial sequences are presented. The dashed lines in the figure represent the ground truth trajectory, while the color heatmap shows the overall error distribution of the estimated trajectory. As can be seen from the figure, the estimated trajectory is mainly concentrated in the blue area, indicating a smaller error, thus verifying the effectiveness of the algorithm of this invention. Furthermore, Table 8 shows the ablation comparison results of the S-LIO, AP-LIO, and SAP-LIO algorithms in partial sequences. In general, SAP-LIO outperforms S-LIO and AP-LIO. Experimental results also show that the saliency module (S) and the adaptive plane fitting module (AP) are functionally complementary. A single module is insufficient to cover the needs of multiple scenarios, and multi-module collaboration can improve the robustness of the algorithm in complex environments.

[0085] Table 8. Comparison of ablation methods (ATE) for S-LIO, AP-LIO, and SAP-LIO [m] Algorithm verification and analysis in complex real-world environments: This section uses the Chongqing University dataset A collection platform to test and verify the algorithm proposed in this invention in multiple scenarios across campuses A and B. The test sequences include 10 sequences from School_01 to School_10. The SAP-LIO proposed in this invention is compared and analyzed with A-LOAM and FAST-LIO2 to evaluate the performance of each algorithm in terms of absolute trajectory error (ATE) and relative pose error (RPE).

[0086] Campus testing scenarios include densely populated areas with tall buildings, large-scale turning sections, dynamic objects, and degenerate scenarios with insignificant feature changes. These scenarios pose significant challenges to the positioning of intelligent vehicles. Figure 10 A comparison of different algorithms for longitudinal positioning reveals that A-LOAM, as a pure laser odometry system, estimates a distance of only 90 meters, a significant deviation from the reference distance (190 meters). This is because A-LOAM uses frame-to-frame matching, and since the geometric features within a channel are highly similar, the algorithm may misjudge that there is no motion between two frames. FAST-LIO2 combines high-frequency information from the IMU with low-frequency point cloud registration from the lidar, improving the accuracy of motion estimation; however, its longitudinal positioning distance is 170 meters, still showing some deviation from the reference value.

[0087] In contrast, the SAP-LIO proposed in this invention uses an adaptive plane fitting method based on voxel maps to model the inner wall of the channel as continuous planar elements of different sizes. By optimizing the pose consistency of these elements, the accuracy of longitudinal positioning is improved. In the School_04 sequence, SAP-LIO estimated a distance of 181 meters, which is better than A-LOAM (90 meters) and FAST-LIO2 (170 meters), indicating that SAP-LIO can handle simple degenerate environments.

[0088] Table 10 Comparison of accuracy of various algorithms on the campus dataset Finally, Table 10 summarizes the algorithm accuracy comparison results for all 10 sequences in the campus scenario. Overall, SAP-LIO performs better in most sequences in terms of both absolute trajectory error (ATE) and relative pose error (RPE). This further verifies the advancement and robustness of the algorithm proposed in this invention, demonstrating its applicability to complex environments.

[0089] The foregoing has provided a detailed description of a laser inertial odometry method and system for autonomous driving in complex scenarios, as provided in this application. The specific embodiments are described only to aid in understanding the method and its core concepts. It should be noted that those skilled in the art can make various improvements and modifications to this application without departing from its principles, and these improvements and modifications also fall within the scope of protection of the claims.

Claims

1. A laser inertial odometry method for autonomous driving in complex scenarios, characterized in that, Including the following steps: S1. Acquire LiDAR point cloud data and inertial measurement unit data in autonomous driving scenarios, perform projection processing on the LiDAR point cloud, and extract multimodal features from the LiDAR point cloud. S2. Input the multimodal features extracted from the laser point cloud into a pre-constructed point cloud saliency prediction network for autonomous driving scenarios to perform saliency prediction, generate a saliency map, and filter out dynamic objects by combining the point cloud dynamic segmentation network to obtain saliency weights for pose estimation. S3. Based on the input data of the inertial measurement unit, the state is forward propagated to obtain the prior state and the corresponding covariance. The point and surface residuals are constructed by combining the significance weights. The state is updated by iterative error state Kalman filtering to obtain the laser inertial odometry pose result of the current frame. S4. Construct an adaptive planar voxel map based on hash index, and calculate the centroid and covariance matrix of points within the voxels. S5. Determine whether the voxel satisfies the planar characteristic condition based on the eigenvalue relationship of the covariance matrix. If it does not satisfy the condition, proceed to step S6; if it does satisfy the condition, proceed to step S7. S6. Recursively segment voxels that do not meet the conditions to achieve adaptive plane fitting from coarse to fine. S7. Treat the voxel as a planar voxel and store its planar information. Maintain the pose state of multiple consecutive frames based on the sliding window. Construct multi-frame point and surface constraints using the planar voxel. Perform joint optimization of the pose of multiple frames within the sliding window in a low-frequency manner. Output the optimized laser inertial odometry pose result.

2. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 1, characterized in that, Multimodal features in laser point clouds include local curvature, depth, intensity, and semantic features.

3. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 2, characterized in that, The detailed steps of step S1 include: The three-dimensional laser point cloud is converted into a two-dimensional image through cylindrical projection, and local curvature map, depth map and intensity map are generated based on the two-dimensional image respectively; The depth map is processed using a semantic segmentation network to obtain a semantic information map; The local curvature map, depth map, intensity map, and semantic information map are used as multimodal features input into a point cloud saliency prediction network for autonomous driving scenarios.

4. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 3, characterized in that, In step S2, the point cloud saliency prediction network models the saliency of laser points based on multimodal features of local curvature, depth, semantics and intensity, and generates a saliency map by weighted fusion of multimodal features. The point cloud saliency prediction network adopts an encoder-decoder structure and uses the generated saliency map as a supervision signal for training.

5. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 4, characterized in that, In step S2, the point cloud dynamic segmentation network is used to identify dynamic objects in the laser point cloud, and the dynamic segmentation result is fused with the saliency map output by the point cloud saliency prediction network to filter the dynamic object information and generate the final saliency map for subsequent pose estimation. The saliency map assigns a saliency weight to each laser point, and this weight is used in the residual calculation for subsequent pose estimation. The formula for the saliency weight is as follows: in This represents the significance value of the corresponding pixel in the saliency map. and It is a constant.

6. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 5, characterized in that, The detailed steps of step S3 include: State forward propagation is performed using inertial measurement unit data to obtain prior states and corresponding covariances; Perform distortion correction processing on the laser point cloud of the current frame, and then transform the distortion-corrected point cloud of the current frame to the global coordinate system; Search for the neighboring points of the current laser point and perform plane fitting, and construct the point-plane residual measurement equation by combining the significance weights; The state is updated by iterative error state Kalman filtering to obtain the optimal state estimate and its corresponding covariance matrix; The laser inertial odometry pose result for the current frame is obtained based on the attitude and position in the optimal state estimate.

7. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 6, characterized in that, In step S4, the adaptive planar voxel map is managed using a hash index method. The hash key is formed by the voxel level index and its normalized spatial coordinates. The voxels are then queried, inserted, and updated based on the hash key. Large voxels are retained in sparse regions, while small voxels are generated in dense or geometrically complex regions, so that the plane fitting scale is adaptively adjusted according to the local point cloud distribution.

8. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 7, characterized in that, The calculation formula for determining whether a voxel satisfies the planar characteristic condition based on the eigenvalue relationship of the covariance matrix in step S5 is as follows: in, , , Let be the eigenvalues ​​of the covariance matrix, and , This represents the threshold for determining planar voxels.

9. The laser inertial odometry method for autonomous driving in complex scenarios according to claim 8, characterized in that, In step S7, a sliding window local map is constructed using consecutive multi-frame point clouds. Multi-frame point-surface constraints are built based on the adaptive plane fitting results. The joint optimization problem is then transformed into an optimization problem that minimizes the sum of the minimum eigenvalues ​​corresponding to each plane voxel. The optimization objective formula is: in Represents the covariance matrix. Represents the covariance matrix The third eigenvalue; the low-frequency method is used to jointly optimize the pose of multiple frames of LiDAR within the sliding window, and the optimized LiDAR inertial odometry pose result is output.

10. A laser inertial odometry system for autonomous driving in complex scenarios, characterized in that, It includes a data acquisition and preprocessing module, a pose estimation module, an initial laser inertial odometry pose module, a voxel in-point cloud centroid and covariance matrix calculation module, a plane feature condition judgment module, a voxel recursive segmentation module, and an optimized laser inertial odometry pose output module. The data acquisition and preprocessing module is used to acquire lidar point cloud data and inertial measurement unit data in autonomous driving scenarios, perform projection processing on the lidar point cloud, and extract multimodal features from the lidar point cloud. The pose estimation module communicates with the data acquisition and preprocessing module to input the multimodal features extracted from the laser point cloud into a pre-constructed point cloud saliency prediction network for autonomous driving scenarios to perform saliency prediction, generate a saliency map, and filter out dynamic objects by combining the point cloud dynamic segmentation network to obtain saliency weights for pose estimation. The initial laser inertial odometry pose module is connected to the pose estimation module for forward propagation of state based on the input data of the inertial measurement unit, to obtain the prior state and corresponding covariance, and to construct point-surface residuals by combining significance weights. Iterative error state Kalman filtering is used to update the state and obtain the laser inertial odometry pose result of the current frame. The module for calculating the centroid and covariance matrix of point clouds within voxels is connected to the initial laser inertial odometry pose module to construct an adaptive planar voxel map based on hash index and to calculate the centroid and covariance matrix of point clouds within voxels. The planar feature condition judgment module is connected to the voxel point cloud centroid and covariance matrix calculation module. It is used to judge whether the voxel meets the planar feature condition based on the eigenvalue relationship of the covariance matrix. If it does not meet the condition, it enters the voxel recursive segmentation module. If it does meet the condition, it enters the optimized laser inertial odometry pose output module. The voxel recursive segmentation module is connected to the planar feature condition judgment module to recursively segment voxels that do not meet the conditions, thereby achieving adaptive planar fitting from coarse to fine. The optimized laser inertial odometry pose output module is connected to the planar feature condition judgment module. It is used to treat the voxel as a planar voxel and store its planar information. It maintains the pose state of multiple consecutive frames based on a sliding window, constructs multi-frame point and surface constraints using planar voxels, and performs joint optimization of the pose of multiple frames within the sliding window in a low-frequency manner, outputting the optimized laser inertial odometry pose result.

Images