Laser Semantic Simultaneous Localization and Mapping Method Based on Multi-Corresponding Point Collaborative Registration

Abstract

This application belongs to the field of computer vision technology and discloses a laser semantic simultaneous localization and mapping method based on multi-corresponding point collaborative registration, including: mapping lidar point cloud data onto a bird's-eye polar coordinate system and extracting per-point high-dimensional features; inputting the high-dimensional features into a U-shaped network to predict corresponding class labels for each lidar point cloud, and dividing the point cloud into static point cloud, dynamic point cloud, and ground point cloud according to the labels; using keypoint convolution and attention mechanism to synchronously capture node features within and between static point cloud frames; using the node features of intra-frame and inter-frame nodes to construct a relaxation similarity matrix and solve the optimal matching relationship of cross-frame nodes; searching for precise corresponding relationships of local point clouds and resolving pose estimation, and simultaneously optimizing the pose in collaboration with the ground point cloud registration method. This application realizes high-precision three-dimensional motion estimation and map construction in dynamic complex scenarios through a semantic-guided multi-level corresponding point registration mechanism.

Description

Technical Field

[0001] The present invention belongs to the technical field of computer vision, and specifically relates to a method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration. Background Art

[0002] In recent years, with the rapid development of autonomous driving and mobile robot technologies, simultaneous localization and mapping (SLAM) based on light detection and ranging (LiDAR) has become a core technology for realizing environmental perception and autonomous navigation. LiDAR point clouds, with their high-precision three-dimensional spatial representation capabilities, provide key data support for vehicle pose estimation and map construction. Among them, the LiDAR odometry and mapping (LOAM) method significantly reduces the cumulative error of the inertial navigation system through feature matching and pose optimization, and has become an important research direction for three-dimensional reconstruction of dynamic scenes.

[0003] Existing LOAM methods are mainly divided into two categories: traditional optimization-based schemes and deep learning-based improved models. The former relies on geometric features designed manually, such as edge points and plane points, for inter-frame registration, but is prone to matching deviations under the interference of dynamic objects and has difficulty effectively using ground structure information; the latter extracts global features through neural networks. Although the feature expression ability is improved, there are still many challenges: when most methods use low weights to suppress the influence of dynamic objects, conventional segmentation networks are prone to misjudgment in complex scenes, resulting in residual dynamic point clouds; ignoring the correction effect of dense ground point clouds on the pitch angle and vertical displacement, and only screening rough ground planes through thresholds causes pose drift; the density of far-distance point clouds decreases sharply, resulting in sparse inter-frame correspondence relationships, and the global feature projection is vulnerable to interference from locally similar regions; at the same time, the supervision dependence on the true value of the inertial navigation unit pose limits the model generalization ability, and the lack of semantic information fusion mechanism makes it difficult for existing frameworks to construct three-dimensional semantic maps to support tasks such as path planning.

[0004] In large-scale dynamic scenes, LiDAR point clouds show a distribution characteristic of being dense near and sparse far away, and the number of effective corresponding point pairs decreases sharply with the increase of distance. Interference points introduced by dynamic objects such as vehicles and pedestrians further exacerbate the data complexity. Traditional methods face a double dilemma in such scenes: the registration algorithm based on global features fails due to sparse correspondence relationships, and the pose solution model that ignores the constraints of dynamic objects and the ground leads to a significant increase in cumulative error. In addition, the separate utilization of ground and non-ground structure information results in insufficient pitch angle estimation accuracy, and the problem of mis-matching of dynamic point clouds continuously restricts the robustness of three-dimensional reconstruction. Summary of the Invention

[0005] In view of the problems in the prior art such as positioning drift caused by the residue of dynamic objects and the susceptibility of the correspondence relationship to local similarity interference, the present application proposes a laser semantic simultaneous localization and mapping method based on multi-corresponding point collaborative registration, which extracts local semantic features from an aerial view to effectively eliminate the interference of dynamic areas and improve the accuracy and robustness of pose estimation and three-dimensional reconstruction.

[0006] To achieve the above object, the present application is implemented through the following technical solutions:

[0007] The present application is a laser semantic simultaneous localization and mapping method based on multi-corresponding point collaborative registration, which includes the following steps:

[0008] Step 1: Frame by frame, map the lidar point cloud to the aerial polar coordinate system, and extract the per-point high-dimensional features through a multi-layer perceptron.

[0009] Step 2: Input the per-point high-dimensional features of each frame into a U-shaped network in the lidar scanning order, perform per-point high-dimensional feature calculation using multi-layer circular convolution, predict the one-hot encoded semantic labels for each point of the lidar point cloud, and divide the lidar point cloud into static point cloud, dynamic point cloud, and ground point cloud according to the one-hot encoded semantic labels.

[0010] Step 3: Based on the static point cloud, synchronously capture the node features within the static point cloud frame and the cross-frame node association features through the multi-layer rigid kernel point convolution layer and the attention mechanism of the U-shaped network.

[0011] Step 4: Use the node features within the frame calculated in Step 3 to construct an inter-frame relaxation similarity matrix, solve the alignment node relationship of the cross-frame node association features using the Sinkhorn algorithm, and establish an accurate point correspondence relationship based on the local point cloud matching guided by the alignment node relationship.

[0012] Step 5: Perform pose calculation on the established accurate point correspondence relationship through the MultiCorrICP algorithm, optimize the pose, and construct a three-dimensional semantic map with point-level semantic annotations.

[0013] A further improvement of the present application lies in: In Step 1, frame by frame, map the lidar point cloud to the aerial polar coordinate system, and extract the per-point high-dimensional features through a multi-layer perceptron, which specifically includes the following steps:

[0014] Step 1.1: Map each frame of lidar point cloud to the aerial polar coordinate system;

[0015] Step 1.2: Divide the lidar point cloud into polar coordinate grids indexed by the angular dimension and the radial dimension , and for each polar coordinate grid ​ the lidar point cloud within adopt a PointNet network with shared weights extract polar coordinate grids of local features;

[0016] Step 1.3. Perform global max pooling on the local features of the polar coordinate grids extracted in Step 1.2 to generate point-wise high-dimensional features :

[0017] ;

[0018] wherein, represents the points within the polar coordinate grid ;

[0019] A further improvement of this application is that: Step 2 includes the following steps

[0020] Step 2.1. Input the point-wise high-dimensional features output in Step 1 in the order of lidar scans, taking the polar coordinate grid as a unit into the U-shaped network;

[0021] Step 2.2. Perform feature fusion through multi-layer circular convolutions in the encoder-decoder architecture of the U-shaped network, and perform hierarchical convolution calculations through circular convolution kernels in the bird's-eye polar coordinate system:

[0022] ;

[0023] wherein, represents the level of the convolutional layer, and are the index parameters of the circular convolution kernel in the polar coordinate system, is the convolution kernel;

[0024] The circular convolution kernel forms a closed-loop convolution path in the angular dimension, and the circular convolution kernel forms a linear convolution path in the radial dimension. In the decoder stage, an upsampling operation is used to restore the feature resolution, and the feature maps at the corresponding levels of the encoder are concatenated through skip connections;

[0025] Step 2.3. After multi-layer circular convolution calculations, output the one-hot encoded semantic labels of each polar coordinate grid of the current lidar point cloud , and complete the division of the lidar point cloud according to the one-hot encoded semantic labels , divided into ground point cloud , dynamic point cloud , and static point cloud , and static point cloud , static point cloud For calculating the corresponding relationship of spatial nodes, ground point cloud For optimizing the pitch angle parameter.

[0026] A further improvement of the present application lies in that: step 3 specifically includes the following steps:

[0027] Step 3.1: Perform neighborhood sampling on each frame of static point cloud and calculate the neighborhood position deviation of the neighborhood point cloud with the center point as the reference, and use a multi-layer rigid kernel point convolution layer to perform feature aggregation on the neighborhood point cloud : wherein, is the kernel point,

[0028] ;

[0029] is the threshold, is the neighborhood point cloud;

[0030] Each multi-layer rigid kernel point convolution layer contains learnable kernel points and calculates the geometric correlation between the learnable kernel points and the neighborhood position deviation through a linear function . The linear function outputs a weight negatively correlated with the Euclidean distance when the Euclidean distance between the neighborhood position deviation and the learnable kernel point is less than the threshold , and outputs a zero weight when the Euclidean distance between the neighborhood position deviation and the learnable kernel point is greater than or equal to the threshold . Through the multi-layer rigid kernel point convolution layer and the downsampling operation, the static point cloud is aggregated into multiple groups of spatial features ;

[0031] Step 3.2: Adopt a self-attention-cross-attention-self-attention three-level serialization processing architecture to gradually extract the node features within the static point cloud frame, cross-frame node association features, and finally strengthen the node features within the frame.

[0032] A further improvement of the present application lies in that: step 3.2 specifically includes the following steps:

[0033] Step 3.2.1: Calculate the query matrix for the single-frame spatial feature , the key matrix , and the value matrix through the feature dimension ​​Weighted calculation of the normalized similarity matrix for node features within a static point cloud frame :

[0034] ;

[0035] Among them, is the feature dimension, represents the transpose of the matrix ;

[0036] Step 3.2.2, Based on the node features within the static point cloud frame , use the cross-frame cross-attention mechanism to use the node features of the current frame as the query, and the node features of the adjacent frame as the key and value to calculate the cross-frame node association features :

[0037] ;

[0038] Step 3.2.3, Use the self-attention reinforcement mechanism within the static point cloud frame to perform the calculation in Step 3.2.1 again on the cross-frame node association features to strengthen the node features within the frame.

[0039] A further improvement of this application is that: The said step 4 includes the following steps:

[0040] Step 4.1, Based on the node features within the strengthened frame obtained in Step 3.2.3, calculate the similarity matrix , and construct the inter-frame relaxation similarity matrix of the current frame and the adjacent frame : :

[0041] ;

[0042] Among them, is the relaxation column, is the relaxation row;

[0043] Step 4.2, Use the relaxation similarity matrix as the cost matrix, and use the Sinkhorn algorithm to iteratively calculate the transport matrix , and add the entropy regularization term to prevent the elements of the transport matrix from being overly concentrated:

[0044] ;

[0045] Among them, is a regularization coefficient used to balance cost minimization and entropy maximization;

[0046] Step 4.3: Through the alternating normalization transfer matrix All rows and columns satisfy the constraint conditions: the sum of the elements in each row is equal to the current frame node distribution vector , and the sum of the elements in each column is equal to the adjacent frame node distribution vector ;

[0047] Step 4.4: Obtain the optimal transfer matrix by minimizing the total transfer cost :

[0048] ;

[0049] where , are the number of nodes in the current frame and the adjacent frame respectively, is the current frame node distribution vector, is the adjacent frame node distribution vector;

[0050] Step 4.5: Align the current frame with the adjacent frame based on the alignment node relationship obtained in Step 4.4, and establish an accurate point correspondence based on the local point cloud matching guided by the alignment node relationship for pose solution, where , , are the aligned nodes of the previous frame and the adjacent frame respectively.

[0051] A further improvement of this application is that in the above Step 5, based on the accurate point correspondence , the following pose solution is performed:

[0052] Step 5.1: Ground point cloud pose solution: Use the ground point cloud to calculate the pitch angle and the vertical displacement through the MultiCorrICP algorithm;

[0053] Step 5.2: Static point cloud pose solution: Use the non-ground static point cloud to calculate the heading angle , the roll angle and the horizontal displacement in the direction and the horizontal displacement in the direction through the MultiCorrICP algorithm;

[0054] Step 5.3, Pose Joint Optimization: The pitch angle and vertical displacement calculated from the ground point cloud are fused with the heading angle and roll angle calculated from the static point cloud, as well as the horizontal displacement in the direction and the horizontal displacement in the

[0055] direction to perform parameter fusion, and a complete pose transformation matrix including the three-dimensional rotation matrix and the translation vector

[0056] is constructed; Step 5.4, Semantic Map Construction: Apply the complete pose transformation matrix calculated in Step 5.3 to the coordinate transformation of each frame of lidar point cloud, and combine it with the one-hot encoded semantic labels

[0057] generated in Step 2.3 to construct a three-dimensional semantic map with point-level semantic annotations.

[0058] Furthermore, in the present application: Step 5.1 is specifically: Establish a normal vector constraint on the ground point cloud correspondence, and solve the optimal rotation matrix and translation vector by minimizing the projection error of adjacent frame ground point clouds in the normal vector direction: where is the number of points in the ground point cloud , is the index of the cross-frame corresponding point pair, is the normal vector of the point cloud , is the three-dimensional coordinate of the th corresponding point in the current frame point cloud, and

[0059] is the rigid body transformation matrix from the current frame point cloud to the adjacent frame point cloud.

[0060] ;

[0061] where , is the normal vector of the point cloud , is the current frame the rigid body transformation displacement vector from the point cloud of the current frame to the point cloud of the adjacent frame is the current frame the rigid body transformation rotation matrix from the point cloud of the current frame to the point cloud of the adjacent frame

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

[0063] Aiming at the problem of positioning drift caused by the residue of dynamic objects, this application proposes to adopt a bird's-eye view semantic module based on circular convolution to extract local semantic features from the bird's-eye view perspective, effectively eliminating the interference of dynamic regions.

[0064] Aiming at the problem that the correspondence is easily interfered by local similarity, this application proposes a point-by-point feature extraction method to achieve robust matching through local node search.

[0065] This application designs an unsupervised rigid transformation calculation based on SVD, a method of using corresponding point clouds to calculate poses, getting rid of the strong dependence on IMU data. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] Figure 1 is a schematic flow chart of the method of this application.

[0067] Figure 2 is a schematic diagram of the U-shaped network of this application.

[0068] Figure 3 is a schematic diagram of the model of this application. DETAILED DESCRIPTION OF THE INVENTION

[0069] As Figure 1 shown, this application is a laser semantic simultaneous localization and mapping method based on multi-corresponding point collaborative registration, including the following steps:

[0070] Step 1: Map the lidar point cloud to the bird's-eye view polar coordinate system frame by frame, and extract the high-dimensional features of each point through a multi-layer perceptron. The following operations are included:

[0071] Step 1.1: Map each frame of lidar point cloud to the bird's-eye view polar coordinate system, and divide the lidar point cloud into polar coordinate grids indexed by the angular dimension and the radial dimension . For the lidar point cloud in each polar coordinate grid , use the PointNet network with shared weights ​ Extract the polar coordinate grid of local features;

[0072] Step 1.2. Perform global max pooling on the local features of the extracted polar coordinate grid to generate point-wise high-dimensional features :

[0073] ;

[0074] wherein, represents the points within the polar coordinate grid ;

[0075] Step 2. Input the point-wise high-dimensional features of each frame into the U-shaped network in the order of lidar scans, perform point-wise high-dimensional feature calculation using multi-layer circular convolutions, predict the one-hot encoded semantic labels for each point of the lidar point cloud of each frame, and divide the lidar point cloud into static point cloud, dynamic point cloud, and ground point cloud according to the one-hot encoded semantic labels, as Figure 2 shown;

[0076] The said Step 2 includes the following steps

[0077] Step 2.1. Input the point-wise high-dimensional features output from Step 1 into the U-shaped network in the order of lidar scans, with the polar coordinate grid as the unit; perform feature fusion through multi-layer circular convolutions in the encoder-decoder architecture, and perform hierarchical convolution calculation through the circular convolution kernel in the bird's-eye polar coordinate system:

[0078] ;

[0079] wherein, represents the level of the convolutional layer, and are the index parameters of the circular convolution kernel in the polar coordinate system, is the convolution kernel;

[0080] The circular convolution kernel forms a closed-loop convolution path in the angular dimension and a linear convolution path in the radial dimension. In the decoder stage, an upsampling operation is used to restore the feature resolution, and channel splicing is performed with the feature map of the corresponding level of the encoder through skip connections;

[0081] Step 2.2. After multi-layer circular convolution calculation, output the one-hot encoded semantic labels of each polar coordinate grid of the current lidar point cloud, and complete the division of the lidar point cloud according to the one-hot encoded semantic labels into ground point cloud and dynamic point cloud and static point cloud , the static point cloud is used for calculating the spatial node correspondence, and the ground point cloud is used for optimizing the pitch angle parameter.

[0082] Step 3. Based on the static point cloud, synchronously capture the node features within the static point cloud frame and the cross-frame node association features through the multi-layer rigid kernel point convolution layer and the attention mechanism of the U-shaped network. Based on the divided static point cloud , perform the following node feature extraction and matching operations:

[0083] Step 3.1. For each frame of static point cloud , perform neighborhood sampling, calculate the neighborhood position deviation of the neighborhood point cloud with the center point as the reference, and use the multi-layer rigid kernel point convolution layer to perform feature aggregation on the neighborhood point cloud : ;

[0084] ;

[0085] Among them, is the kernel point, is the threshold, is the neighborhood point cloud;

[0086] Each multi-layer rigid kernel point convolution layer contains learnable kernel points , calculate the geometric correlation between the learnable kernel point and the neighborhood position deviation through the linear function . The linear function outputs a weight negatively correlated with the Euclidean distance when the Euclidean distance between the neighborhood position deviation and the learnable kernel point is less than the threshold , and outputs a zero weight when the Euclidean distance between the neighborhood position deviation and the learnable kernel point is greater than or equal to the threshold . Through the multi-layer rigid kernel point convolution layer and the downsampling operation, aggregate the static point cloud into multiple groups of spatial features ;

[0087] Step 3.2. Adopt a three-level serial processing architecture of self-attention - cross-attention - self-attention to gradually extract the node features within the static point cloud frame and the cross-frame node association features, and finally strengthen the node features within the frame, specifically including the following steps:

[0088] Step 3.2.1. Calculate the query matrix for the single-frame spatial feature ​ , key matrix , value matrix , weighted calculation of node features within the static point cloud frame through the feature dimension Normalized similarity matrix: :

[0089] ;

[0090] Among them, is the feature dimension, represents the transpose of matrix ;

[0091] Step 3.2.2, based on the node features within the static point cloud frame , use the cross-frame cross-attention mechanism to use the node features of the current frame as the query, and the node features of the adjacent frame as the key and value, and calculate the cross-frame node association features : :

[0092] ;

[0093] Step 3.2.3, use the self-attention reinforcement mechanism within the static point cloud frame to perform the calculation in Step 3.2.1 on the cross-frame node association features again to strengthen the node features within the frame.

[0094] Step 4: Use the node features within the frame calculated in Step 3 to construct an inter-frame relaxed similarity matrix, use the Sinkhorn algorithm to solve the alignment node relationship of the cross-frame node association features, and establish an accurate point correspondence based on the local point cloud matching guided by the alignment node relationship; including the following steps:

[0095] Step 4.1, based on the strengthened node features within the frame obtained in Step 3.2.3, calculate the similarity matrix , and construct the inter-frame relaxed similarity matrix of the current frame and the adjacent frame : :

[0096] ;

[0097] Among them, is the relaxation column, is the relaxation row;

[0098] Step 4.2, use the relaxed similarity matrix as the cost matrix, and use the Sinkhorn algorithm to iteratively calculate the transport matrix , add an entropy regularization term , to prevent the elements of the transmission matrix from being overly concentrated:

[0099] ;

[0100] Among them, is the regularization coefficient, used to balance cost minimization and entropy maximization;

[0101] Step 4.3, by alternately normalizing the transmission matrix All rows and columns satisfy the constraint conditions: the sum of the elements in each row is equal to the current frame node distribution vector , and the sum of the elements in each column is equal to the adjacent frame node distribution vector ;

[0102] Step 4.4, obtain the optimal transmission matrix by minimizing the total transmission cost :

[0103] ;

[0104] Among them, , are the number of nodes in the current frame and the adjacent frame respectively, is the current frame node distribution vector, is the adjacent frame node distribution vector;

[0105] Step 4.5, according to the optimal transmission matrix obtained in Step 4.4 align the current frame with the adjacent frame 's alignment node relationship , and establish an accurate point correspondence based on the local point cloud matching guided by the alignment node relationship , for pose solution, where , are the alignment nodes of the previous frame and the adjacent frame respectively.

[0106] Step 5, under the accurate point correspondence, perform pose solution on the established accurate point correspondence through the MultiCorrICP algorithm, and at the same time jointly optimize the pose by combining the ground point cloud registration method to construct a three-dimensional semantic map with point-level semantic annotations.

[0107] Based on the accurate point correspondence , perform the following pose solution:

[0108] Ground point cloud pose solution: Use the ground point cloud to calculate the pitch angle through the MultiCorrICP algorithm With vertical displacement , specifically: Establish a normal vector constraint for the corresponding relationship of the ground point cloud, and solve the optimal rotation matrix by minimizing the projection error of adjacent frame ground point clouds in the normal vector direction And translation vector :

[0109] ;

[0110] Among them, Is the number of points of the ground point cloud , Is the index of the cross-frame corresponding point pair, Is the point cloud Of the normal vector, Is the current frame In the Three-dimensional coordinates of the corresponding point, Is the current frame Point cloud to adjacent frame Point cloud rigid body transformation matrix.

[0111] Static point cloud pose solution: Use the non-ground static point cloud Calculate the heading angle through the MultiCorrICP algorithm , roll angle And To horizontal displacement And To horizontal displacement . Specifically include: Establish a geometric residual constraint for the corresponding relationship of the static point cloud, and solve the optimal rotation matrix by minimizing the projection error of adjacent frame point clouds under the normal vector constraint And translation vector :

[0112] ;

[0113] Among them, , Is the point cloud Of the normal vector, Is the current frame Point cloud to adjacent frame Point cloud rigid body transformation displacement vector, Is the current frame Point cloud to adjacent frame Point cloud rigid body transformation rotation matrix.

[0114] Pose joint optimization: Combine the pitch angle , vertical displacement Calculated from the ground point cloud with the heading angle , roll angle And Horizontal displacement and Horizontal displacement Perform parameter fusion and construct a three-dimensional rotation matrix with translation vector The complete pose transformation matrix ;

[0115] Semantic map construction: Apply the complete pose transformation matrix T solved in step 1 to the complete point cloud coordinate transformation of each frame, combined with the generated one-hot encoded semantic label , construct a three-dimensional semantic map with point-level semantic annotations.

[0116] In order to verify this application, the KITTI odometry standard dataset was used to carry out the test, using sequences 00 to 08 as training sets, and sequences 09 and 10 as validation sets to verify the generalization ability of the algorithm in complex dynamic scenes. As shown in Table 1, the translation error and rotation error are defined as the root mean square error (RMSE) of the trajectory relative to the true value, in units of percentage (%) and degrees per hundred meters (° / 100m), respectively.

[0117] Table 1

[0118]

[0119] Experimental results show that among the traditional point cloud registration methods, point-to-point ICP (ICP-po2po) performs poorly in dynamic scenes, with an average translation error of up to 54.79%; although point-to-plane ICP (ICP-po2pl) optimizes part of the matching problem through plane constraints, the translation error is still 18.69%. The GICP method based on the probability model reduces the translation error to 3.53% in sequence 09, but the error in sequence 10 rises to 7.21% due to complex geometric interference; although the DeLORA method introduces a residual neural network to optimize local features, the average translation error is still 4.11%. In contrast, the method of the present application achieves translation errors of 3.24% and 2.98% and rotation errors of 1.95° and 1.28° in sequence 09 and sequence 10 respectively through multimodal feature coupling. The comprehensive average error is reduced by 40.9%, i.e. 2.43% vs 4.11%, compared with the optimal baseline (DeLORA), and the rotation error is optimized to 36.9%, i.e. 1.38° vs 2.19°, which significantly improves the registration accuracy and robustness in complex scenarios and verifies the advanced nature of the technical solution.

[0120] Figure 3The model of this application takes the complete multi-frame point cloud of radar with time-series input as the processing object. First, two adjacent frames of point clouds are projected onto the bird's-eye polar coordinate grid, and a U-shaped deep neural network is used to extract features and predict semantic labels for the point clouds within the grid cells, and the static point cloud and the ground point cloud are segmented through semantic labels. For the static point cloud, the model combines the attention mechanism and the similarity matrix, and adopts an improved MultiCorrICP algorithm to calculate the rigid body transformation matrix and decompose it to obtain the horizontal displacement, heading angle and roll angle parameters; for the ground point cloud, the vertical displacement and pitch angle are independently solved through MultiCorrICP. Subsequently, the model couples the above-mentioned sub-modal parameters to generate a six-degree-of-freedom rigid body transformation matrix, and finally realizes the construction of a three-dimensional map with semantic label fusion and the high-precision drawing of the carrier's motion trajectory through iterative accumulation of the transformation matrix. Figure 3 The full-process data interaction relationship of point cloud input, semantic segmentation, sub-modal registration, parameter coupling and map output is clearly presented, highlighting the technical features of multi-modal feature collaborative optimization and dynamic interference suppression.

[0121] The above is only the implementation mode of the present invention and is not used to limit the present invention. For those skilled in the art, various changes and modifications can be made to the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the scope of the claims of the present invention.

Claims

1. A laser semantic simultaneous localization and mapping method based on multi - corresponding point collaborative registration, characterized in that: The laser semantic simultaneous localization and mapping method includes the following steps: Step 1: Map the lidar point cloud frame by frame onto the bird's-eye polar coordinate system, and extract the per-point high-dimensional features through a multi-layer perceptron; Step 2: Input the per-point high-dimensional features of each frame into a U-shaped network in the lidar scanning order, perform per-point high-dimensional feature calculation using multi-layer circular convolutions, predict the one-hot encoded semantic labels for each point of the lidar point cloud, and divide the lidar point cloud into static point clouds, dynamic point clouds, and ground point clouds according to the one-hot encoded semantic labels; Step 3: Based on the static point cloud, synchronously capture the intra-frame node features and cross-frame node correlation features of the static point cloud through the multi-layer rigid kernel point convolution layer and attention mechanism of the U-shaped network; Step 4: Use the intra-frame node features calculated in Step 3 to construct an inter-frame relaxation similarity matrix, solve the alignment node relationship of the cross-frame node correlation features using the Sinkhorn algorithm, and establish an accurate point correspondence based on the local point cloud matching guided by the alignment node relationship; Step 5: Perform pose calculation on the established accurate point correspondence through the MultiCorrICP algorithm, optimize the pose, and construct a three-dimensional semantic map with point-level semantic annotations.

2. The method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration according to claim 1, wherein: Step 1 maps the lidar point cloud frame by frame onto the bird's-eye polar coordinate system, and extracts the per-point high-dimensional features through a multi-layer perceptron, which specifically includes the following steps: Step 1.

1. Map each frame of lidar point cloud to the bird's-eye polar coordinate system; Step 1.2: Divide the lidar point cloud into a polar coordinate grid indexed by the angular dimension and the radial dimension . For the lidar point cloud within each polar coordinate grid , use a PointNet network with shared weights to extract the local features of the polar coordinate grid ; Step 1.

3. Perform global max pooling on the local features of the polar coordinate grid extracted in Step 1.2 to generate pointwise high-dimensional features : ； Among them, represents the points within the polar coordinate grid .

3. The method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration according to claim 2, wherein: Step 2 includes the following steps Step 2.1: Input the point-by-point high-dimensional features output in Step 1 into the U-shaped network in the order of lidar scanning, with the polar coordinate grid as the unit; Step 2.2: Perform feature fusion through multi-layer circular convolutions in the encoder-decoder architecture of the U-shaped network, and perform hierarchical convolution calculations through circular convolution kernels in the bird's-eye polar coordinate system: ； Among them, represents the level of the convolutional layer, and are the index parameters of the circular convolution kernel in the polar coordinate system, is the convolution kernel; The circular convolution kernel forms a closed-loop convolution path in the angular dimension, and the circular convolution kernel forms a linear convolution path in the radial dimension. In the decoder stage, an upsampling operation is used to restore the feature resolution, and the feature maps of the corresponding levels of the encoder are spliced by channels through skip connections; Step 2.3: After multi-layer circular convolution calculation, output the one-hot encoded semantic labels of each polar coordinate grid of the current lidar point cloud and, based on the one-hot encoded semantic labels , complete the division of the lidar point cloud into ground point cloud , dynamic point cloud , and static point cloud . .

4. The method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration according to claim 3, wherein: Step 3 specifically includes the following steps: Step 3.1: For each frame of static point cloud perform neighborhood sampling, and calculate the neighborhood position deviation of the neighborhood point cloud with the center point as the reference, and use multiple rigid kernel point convolutional layers to perform feature aggregation on the neighborhood point cloud : ​ ； Among them, is the core point, is the threshold, is the neighborhood point cloud; Each multi-layer rigid kernel point convolution layer contains learnable kernel points , and calculates the geometric correlation between the learnable kernel points and the neighborhood position deviation through a linear function. The linear function outputs weights negatively correlated with the Euclidean distance when the Euclidean distance between the neighborhood position deviation and the learnable kernel points is less than a threshold , and outputs zero weights when the Euclidean distance between the neighborhood position deviation and the learnable kernel points is greater than or equal to the threshold . Through the multi-layer rigid kernel point convolution layer and the downsampling operation, the static point cloud is aggregated into multiple groups of spatial features ; Step 3.2: Adopt a three-level serialization processing architecture of self-attention - cross-attention - self-attention to gradually extract the intra-frame node features and cross-frame node correlation features of the static point cloud, and finally strengthen the intra-frame node features.

5. The method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration according to claim 4, wherein: Step 3.2 specifically includes the following steps: Step 3.2.1, for the single-frame spatial feature Calculate the query matrix , the key matrix , the value matrix , and calculate the node features within the static point cloud frame by weighted calculation using the similarity matrix normalized by the feature dimension : : ； Among them, is the feature dimension, represents the transpose of the matrix ; Step 3.2.2, based on the node features within the static point cloud frame , use the cross-frame cross-attention mechanism to take the node features of the current frame as queries, the node features of adjacent frames as keys and values, and calculate the cross-frame node association features : : ； Step 3.2.

3. Use the intra-frame self-attention enhancement mechanism of static point clouds to process the cross-frame node association features Perform the calculation in Step 3.2.1 again to enhance the node features within the frame.

6. The method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration according to claim 5, wherein: Step 4 includes the following steps: Step 4.1: Calculate the similarity matrix based on the node features in the enhanced frame obtained in Step 3.2.3 , according to the similarity matrix Construct the inter-frame relaxation similarity matrix of the current frame and the adjacent frame : : ； Among them, is a slack column, is a slack row; Step 4.

2. Use the relaxation similarity matrix as the cost matrix, and iteratively calculate the transport matrix using the Sinkhorn algorithm, and add the entropy regularization term : ； Among them, is a regularization coefficient used to balance cost minimization and entropy maximization; Step 4.3: Through the alternating normalization transfer matrix All rows and columns satisfy the constraint condition: the sum of the elements in each row is equal to the current frame node distribution vector , and the sum of the elements in each column is equal to the adjacent frame node distribution vector ; Step 4.4, by minimizing the total transmission cost obtain the optimal transmission matrix : ； Among them, , are the number of nodes in the current frame and the adjacent frame respectively, is the node distribution vector of the current frame, is the node distribution vector of the adjacent frame; Step 4.

5. According to the optimal transmission matrix obtained in Step 4.4 align the current frame with the adjacent frame for the alignment node relationship , and establish an accurate point correspondence based on the local point cloud matching guided by the alignment node relationship for pose solution, where , are the alignment nodes of the previous frame and the adjacent frame respectively.

7. The method for laser semantic simultaneous localization and mapping based on multi - corresponding - point collaborative registration according to claim 6, wherein: In step 5, based on the exact point correspondence , perform the following pose calculation: Step 5.1, Ground point cloud pose calculation: Using the ground point cloud Calculate the pitch angle through the MultiCorrICP algorithm ; With vertical displacement ; Step 5.2, Static Point Cloud Pose Solution: Using the non-ground static point cloud Calculate the heading angle through the MultiCorrICP algorithm , roll angle and lateral horizontal displacement and longitudinal horizontal displacement ; Step 5.3, Pose Joint Optimization: The pitch angle and vertical displacement calculated from the ground point cloud, together with the heading angle , roll angle and horizontal displacement in the direction and horizontal displacement in the direction, are used for parameter fusion to construct a complete pose transformation matrix containing the three-dimensional rotation matrix and the translation vector ; Step 5.4, Semantic Map Construction: Apply the complete pose transformation matrix solved in Step 5.3 to the coordinate transformation of each frame of lidar point cloud, and combine with the one-hot encoded semantic labels generated in Step 2.3 , to construct a three-dimensional semantic map with point-level semantic annotations.

8. The method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration according to claim 7, wherein: Step 5.1 specifically is: establishing a normal vector constraint for the corresponding relationship of the ground point cloud, and solving the optimal rotation matrix by minimizing the projection error of adjacent-frame ground point clouds in the direction of the normal vector and the translation vector : ； Among them, is the number of points in the ground point cloud , is the index of the cross-frame corresponding point pairs is the normal vector of the point cloud , is the current frame in the th three-dimensional coordinates of the corresponding point is the current frame point cloud to the adjacent frame rigid body transformation matrix of the point cloud 9. The method for laser semantic simultaneous localization and mapping based on multi-corresponding point collaborative registration according to claim 7, wherein: Step 5.2 specifically includes: establishing a geometric residual constraint for the static point cloud correspondence, and solving for the optimal rotation matrix by minimizing the projection error of adjacent frame point clouds under the normal vector constraint and the translation vector : ； Among them, , is the normal vector of the point cloud , is the rigid body transformation displacement vector from the current frame point cloud to the adjacent frame point cloud, is the rigid body transformation rotation matrix from the current frame point cloud to the adjacent frame point cloud.

Images