Map Localization and Construction Method Based on Laser SLAM

Abstract

This application proposes a map localization and construction method based on laser SLAM. This method proposes a point cloud registration algorithm that fuses semantic and distribution information. This algorithm can use the semantic label information of the point cloud to optimize the point cloud association process, and can assign different downsampling parameters to the point cloud through the semantic label information, thereby reducing the information loss of the point cloud and reducing the probability of registration mis-matching. This application starts from the point cloud registration link of the SLAM method for optimization, which can improve the positioning accuracy and mapping quality of the SLAM method.

Description

Technical Field

[0001] This application relates to the field of autonomous driving technology, and more specifically, to a map positioning and construction method based on laser SLAM. Background Art

[0002] With the continuous development of related technologies in the field of autonomous driving, vehicles with autonomous driving capabilities are expected to become an important part of the transportation field in the future, bringing great changes to our way of life. Among them, achieving accurate self-positioning is the premise for supporting downstream applications of autonomous driving, and the higher the level of autonomous driving, the higher the requirement for positioning accuracy. However, traditional positioning solutions based on high-precision differential global positioning system (DGPS) and inertial navigation system (INS) will have problems such as poor accuracy and positioning drift in scenarios such as under overpasses, inside dense high-rise buildings, and underground parking lots, and cannot meet the needs of continuous accurate positioning of autonomous driving. SLAM (Simultaneous Localization and Mapping), with its characteristics of achieving stable and accurate self-positioning by carrying sensors that sense environmental information and being unaffected by signal occlusion interference, and being able to assist in constructing high-precision maps, has been widely used in the field of autonomous driving.

[0003] However, there are still some problems with traditional SLAM algorithms. First, traditional SLAM algorithms rely only on the geometric information of point clouds for registration, and are prone to false matches; second, the method of using only the geometric information of point clouds for loop detection is prone to false detections when facing similar scenarios; in addition, the constructed point cloud map has poor expression ability, which is not conducive to later development and maintenance; finally, false matches of point clouds and false detections of loops will introduce incorrect constraint information into the SLAM system, thereby affecting the overall positioning accuracy and mapping quality of the SLAM system. Summary of the Invention

[0004] In view of at least one defect or improvement requirement of the prior art, this application provides a map positioning and construction method based on laser SLAM, which is used to improve the positioning accuracy and mapping quality of traditional SLAM algorithms.

[0005] To achieve the above object, this application provides a map positioning and construction method based on laser SLAM, including:

[0006] Obtain original point cloud data;

[0007] Perform semantic segmentation preprocessing on the original point cloud data, extract the semantic label information of each point in each frame of the point cloud, and obtain the semantic point cloud;

[0008] Register adjacent scan frames of the semantic point cloud through a point cloud registration algorithm that fuses semantic and distribution information to obtain the relative pose of the point cloud between adjacent frames;

[0009] Extract the local map based on the relative pose of the point cloud between adjacent frames;

[0010] Based on the point cloud registration algorithm that fuses semantic and distribution information, perform registration of scan frames to the local map on the local map to screen out key frames;

[0011] Perform loop detection on the key frames to obtain loop frames;

[0012] Based on the point cloud registration algorithm that fuses semantic and distribution information, perform registration on the loop frames to obtain the relative pose between frames of the loop frames;

[0013] Jointly optimize the relative pose between frames of the loop frames and the poses of the key frames and the local map to obtain the final semantic point cloud map.

[0014] Further, the point cloud registration algorithm that fuses semantic and distribution information includes:

[0015] Perform semantic downsampling processing on adjacent scan frames of the semantic point cloud respectively;

[0016] Construct a semantic association loss function between the source point cloud and the target point cloud according to the pose of the source point cloud;

[0017] Solve the semantic association loss function iteratively through the Levenberg-Marquardt nonlinear optimization algorithm to obtain the relative pose transformation matrix from the source point cloud to the target point cloud.

[0018] Further, the semantic downsampling processing includes:

[0019] Input adjacent scan frames of the semantic point cloud into a semantic downsampler, and different types of point clouds are distinguished by different flags;

[0020] Extract different types of point clouds respectively according to the semantic label information of the semantic point cloud to form sub-point clouds;

[0021] Blocks with different flags in the semantic downsampler represent different types of point cloud data, and each type of point cloud is followed by a corresponding voxel downsampler.

[0022] Further, the method for constructing the semantic association loss function includes:

[0023] The nearest neighbor search algorithm is used to find the nearest neighbor of each point in the source point cloud in the target point cloud and form a set of nearest neighbors. Based on the set of nearest neighbors, the Euclidean nearest neighbor set of each point is found in the target point cloud. Then, the calculation formula for the covariance matrix of each point in the Euclidean nearest neighbor set is as follows:

[0024]

[0025] where p i represents the point numbered i in the Euclidean nearest neighbor set; represents the mean of the Euclidean nearest neighbor set; C i represents the covariance matrix of p i ;

[0026] The semantic label information is introduced to optimize the association process of the point cloud. Let p a = {x a , y a , z a , l a} and q b = {x b , y b , z b , l b} be individual points in the source point cloud and the target point cloud respectively, and q b is the corresponding point found for p a through the nearest neighbor search algorithm. Then, the Euclidean distance between these two points is d pq = ||p a - q b ||2. A semantic correlation coefficient ζ is defined to evaluate the effectiveness of the association. ζ can be expressed as:

[0027]

[0028] where τ is a constant less than 1, used to measure the correlation coefficient of semantic label information mismatch;

[0029] The expression of the semantic association loss function is:

[0030]

[0031] where T represents the optimal relative pose to be solved; represents the covariance matrix of p i in the source point cloud; represents the covariance matrix of the nearest neighbor of p i in the corresponding target point cloud; d i represents the point p i and the point p iThe Euclidean distance between the nearest neighbor points in the corresponding target point cloud.

[0032] Further, the extraction of the local map based on the relative pose between the point clouds of adjacent frames includes:

[0033] Using iKd-Tree to maintain the local map;

[0034] The relative pose transformation matrix from the source point cloud to the target point cloud provides the initial pose for maintaining the local map;

[0035] Extract the local map from the area of the initial pose.

[0036] Further, the point cloud registration algorithm based on the fused semantic and distribution information performs registration of the scan frame to the local map on the local map to screen out key frames, including:

[0037] The point cloud registration algorithm based on the fused semantic and distribution information performs registration of the scan frame to the local map on the local map to obtain the pose estimation of the current frame of the semantic point cloud;

[0038] If the displacement and / or rotation of the current frame of the semantic point cloud exceeds the first preset threshold, then mark the current frame as a key frame and extract it.

[0039] Further, the loop detection of the key frames to obtain loop frames includes:

[0040] According to the semantic label information of the key frames of the semantic point cloud, extract the point cloud with continuous invariance;

[0041] Encode the point cloud with continuous invariance based on the loop descriptor that fuses semantic and geometric information to obtain an encoding matrix;

[0042] Calculate the similarity of the encoding matrix based on the evaluation algorithm of the similarity of the encoding matrix, and obtain the highest similarity between the encoding matrix of the current key frame and the encoding matrix of the historical key frame;

[0043] If the highest similarity is less than the second preset threshold, then the point clouds corresponding to the current key frame and the historical key frame with the highest similarity are mutually the loop frames.

[0044] Further, the construction method of the loop descriptor that fuses semantic and geometric information includes:

[0045] Using the partitioning method of Scan-Context, partition the point cloud with continuous invariance For each point p(x, y, z, l), perform the conversion from Cartesian coordinates to polar coordinates to obtain p(d, α, β); where d represents the horizontal distance from point p to the lidar, α represents the azimuth angle in the horizontal direction, and β represents the pitch angle in the vertical direction;

[0046] According to d and α, divide the point cloud into ring-sector blocks, obtaining a total of N s ×N r ring-sector blocks; The expression for each ring-sector block Block ij is:

[0047]

[0048] where i and j respectively represent the numbers of each ring-sector block; the subscript k represents the sequence number of the point cloud , h k represents the height information of the point with sequence number k, l k represents the semantic label information of the point with sequence number k, d k represents the horizontal distance from the point with sequence number k to the lidar, α k represents the azimuth angle in the horizontal direction of the point with sequence number k; R max represents the maximum scanning radius of the lidar;

[0049] Traverse the height values of the point cloud in each ring-sector block Block ij to find the maximum height value h max in the point cloud, and assign values according to the height encoding matrix Ω h (i, h) = h max ; if no points fall into the current ring-sector block, let the height encoding matrix Ω h (i, j) = 0, and obtain the height encoding matrix Ω h in this way;

[0050] Traverse the semantic label information of the point cloud in each ring-sector block Block ij to find the label l max with the highest priority, and assign values according to the height encoding matrix Ω h (i, h) = l max ; if no points fall into the current ring-sector block, let the semantic encoding matrix Ω s (i, h) = 0, and obtain the semantic encoding matrix Ω s in this way;

[0051] Create a three-dimensional matrix Ω s ×N s ×2 with dimensions, and store the height encoding matrix Ω in one layer of Ω hs , in Ω hs ​h , store the semantic encoding matrix Ω in another layer s , and finally obtain the joint encoding matrix Ω that fuses height information and semantic information hs , and its expression is:

[0052]

[0053] Further, the joint optimization of the inter-frame relative pose of the loopback frame and the poses of the key frame and the local map to obtain the final semantic point cloud map includes:

[0054] Use GTSAM to construct a factor graph;

[0055] Based on the factor graph, perform pose joint optimization on the inter-frame relative pose constraint information of the loopback frame and the pose constraint information of the key frame and the local map to obtain the final optimized pose;

[0056] Use the final optimized pose to perform coordinate transformation on the point cloud to obtain the final semantic point cloud map.

[0057] Generally speaking, compared with the prior art through the above technical solutions conceived by the present application, the following beneficial effects can be achieved:

[0058] (1) The present application proposes a map positioning and construction method based on laser SLAM. This method proposes a point cloud registration algorithm that fuses semantic and distribution information. This algorithm can use the semantic label information of the point cloud to optimize the point cloud association process, and can assign different downsampling parameters to the point cloud through the semantic label information, thereby reducing the information loss of the point cloud and reducing the probability of registration mis-matching. The present application optimizes from the point cloud registration link of the SLAM method, which can improve the positioning accuracy and mapping quality of the SLAM method.

[0059] (2) The present application designs a semantic downsampling method to perform custom downsampling on the point cloud, assigns different downsampling parameters to the point cloud through the semantic label information, thereby reducing the information loss of the point cloud, and defines a new set of semantic registration loss functions, thereby reducing the probability of mis-matching and optimizing the construction of the loss function.

[0060] (3) This application proposes a loop descriptor that fuses semantic and geometric information. By jointly encoding the high-dimensional semantic information and geometric information of the point cloud, the representativeness of the encoding matrix is improved, the expressive ability of the constructed point cloud map is enhanced, which is more conducive to later development and maintenance. At the same time, due to the fusion of the high-dimensional semantic information and geometric information of the point cloud for loop detection, the probability of false detection in loop detection is reduced. This application proposes a calculation method for evaluating the similarity of encoding matrices, which is used to detect whether there is a loop relationship between point clouds corresponding to similar encoding matrices, thereby further reducing the probability of false detection in loop detection.

[0061] (4) This application integrates the above improvements into the SLAM framework to form a complete new semantic SLAM method. Starting from the point cloud registration link and loop detection link of the SLAM method, this application proposes the above targeted optimization solutions, thereby improving the positioning accuracy and mapping quality of the SLAM method at the overall level. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] To more clearly illustrate the technical solutions in the embodiments of this application, the following will briefly introduce the drawings required in the embodiments. Obviously, the drawings described below are only some embodiments of this application. For those of ordinary skill in the art, without creative efforts, other drawings can be obtained based on these drawings.

[0063] Figure 1 It is the core flowchart of a map positioning and construction method based on laser SLAM provided by the embodiment of this application;

[0064] Figure 2 It is the complete system framework diagram of a new semantic SLAM method provided by the embodiment of this application;

[0065] Figure 3 It is the schematic diagram of the principle of a point cloud registration algorithm that fuses semantic and distribution information provided by the embodiment of this application;

[0066] Figure 4 It is the process schematic and effect comparison diagram of a semantic downsampling process provided by the embodiment of this application;

[0067] Figure 5 It is the loop encoding schematic diagram that fuses semantic and geometric information provided by the embodiment of this application;

[0068] Figure 6 It is the schematic diagram of the shift transformation of the encoding vector provided by the embodiment of this application;

[0069] Figure 7Schematic diagram of joint coding matrix shift transformation provided by an embodiment of this application. Detailed implementation manners

[0070] In order to make the objectives, technical solutions and advantages of this application clearer, the following further describes this application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not used to limit this application. In addition, the technical features involved in the various implementation manners of this application described below can be combined with each other as long as they do not conflict with each other.

[0071] Terms such as "first", "second" or "nth" in the specification, claims or drawings of this application are used to distinguish different objects rather than to describe a specific order. In addition, the terms "include" or "have" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device that includes a series of steps or units is not limited to the listed steps or units, but may optionally further include steps or units not listed, or may optionally further include other steps or units inherent to these processes, methods, products or devices.

[0072] As described in the background art section of the specification, traditional SLAM algorithms rely only on the geometric information of point clouds for registration, which is prone to false matching; secondly, the method of using only the geometric information of point clouds for loop detection is prone to false detection when facing similar scenes; in addition, the constructed point cloud map has poor expression ability, which is not conducive to later development and maintenance; finally, false matching of point clouds and false detection of loops will introduce incorrect constraint information in the SLAM system, thereby affecting the overall positioning accuracy and mapping quality of the SLAM system. In view of this, this application proposes a map positioning and construction method based on laser SLAM to improve the positioning accuracy and mapping quality of traditional SLAM algorithms.

[0073] Refer to Figure 1 and Figure 2 , an embodiment of this application proposes a map positioning and construction method based on laser SLAM, and this method mainly includes the following steps.

[0074] Step 1: Obtain the original point cloud data.

[0075] In some embodiments, specifically, install a lidar on the test vehicle, perform coordinate system calibration, and collect the original point cloud data obtained by the on-vehicle lidar during the test process.

[0076] Step 2: Perform semantic segmentation preprocessing on the original point cloud data, extract the semantic label information of each point in each frame of the point cloud, and obtain the semantic point cloud.

[0077] In some embodiments, specifically, the original point cloud data is input into the point cloud preprocessing module. The point cloud preprocessing module contains a pre-trained semantic segmentation network model, which performs forward inference on the point cloud to extract the semantic label information of each point in each frame of the point cloud, obtaining a semantic point cloud.

[0078] Step 3: Register adjacent scan frames of the semantic point cloud through a point cloud registration algorithm that fuses semantic and distribution information to obtain the relative pose between the point clouds of adjacent frames.

[0079] In some embodiments, specifically, first, the adjacent frame semantic point clouds passed in are subjected to semantic downsampling processing. The semantic point cloud is classified and downsampled according to preset downsampling parameters, reducing the consumption of computing resources while retaining more valuable points. Then, the downsampled adjacent frame point clouds are registered through the SD-GICP algorithm (a point cloud registration algorithm that fuses semantic and distribution information) proposed in this application to obtain the relative pose transformation matrix between these two frames of point clouds.

[0080] For the flow schematic and effect comparison diagram of the semantic downsampling process, refer to Figure 4 , the input of the semantic downsampler is the semantic point cloud data. In the semantic point cloud, each point carries a label information, which characterizes the category information of the point. Point clouds of different categories are distinguished by different colors. According to the label information of the semantic point cloud, point clouds of different categories are respectively extracted to form sub-point clouds. The different colored squares in the semantic downsampler represent point cloud data of different categories, and each category of point cloud is followed by a voxel downsampler, and the downsampling parameters of different voxel downsamplers are different. From the detailed comparison between the semantic downsampled point cloud and the traditional downsampled point cloud, it can be seen that the point cloud after sampling by the semantic downsampler retains more points with structured features (the yellow points in the figure), while the traditional downsampled point cloud retains more cluttered points (the green points in the figure).

[0081] The principle of the SD-GICP algorithm is as Figure 3 shown. After the semantic point cloud is downsampled, a semantic association loss function between the source point cloud and the target point cloud is constructed according to the pose of the source point cloud, and then it is solved through the Levenberg-Marquardt nonlinear optimization algorithm. Through iterative solution, the relative pose transformation matrix from the source point cloud to the target point cloud is finally obtained.

[0082] An embodiment of this application constructs a new semantic association loss function, and the specific principle of this semantic association loss function is as follows.

[0083] Let the target point cloud and the source point cloud be adjacent frame semantic point clouds (assuming and The numbers of the midpoints are m and n respectively), and the nearest neighbor search algorithm is used to find in the nearest neighbor of each point in and form a point set. Based on this nearest neighbor point set, in k the Euclidean nearest neighbor point set {p1, p2,... p i}(assuming the number of this point set is fixed as k) of each point is found, then the covariance matrix C

[0084]

[0085] where, p i represents the point with the number i in the Euclidean nearest neighbor point set {p1, p2,... p k}; represents the mean value of the point set {p1, p2,..., p k}; C i represents the covariance matrix of p i .

[0086] Since the nearest neighbor search algorithm can only perform nearest neighbor search based on the Euclidean minimum distance, there will be a situation where the source point and the target point do not belong to the same category, and this mis - matching will reduce the accuracy of the registration algorithm. Therefore, in this embodiment, semantic label information is introduced here to optimize the association process of the point cloud.

[0087] First, assume that p a ={x a , y a , z a , l a} and q b ={x b , y b , z b , l b} are respectively single points in the point clouds and , and q b is the corresponding point of p a found by the nearest neighbor search algorithm, then the Euclidean distance between the two points can be expressed as d pq =‖p a - q b ‖2.

[0088] Then, a semantic correlation coefficient ζ is defined to evaluate the effectiveness of the association, and ζ can be expressed as Equation (2).

[0089]

[0090] Among them, τ is a constant less than 1, which is used to measure the correlation coefficient of the mismatch of semantic label information. Then, the semantic association loss function representing semantic distribution point cloud registration can be expressed as Equation (3).

[0091]

[0092] Among them, T represents the optimal relative pose to be solved; represents the source point cloud the covariance matrix of p i in; represents p i the covariance matrix of the nearest neighbor point in the corresponding target point cloud; d i represents the point p i in the source point cloud i and the Euclidean distance between the nearest neighbor point in the corresponding target point cloud of the point p

[0093] The SD-GICP algorithm incorporates semantic association information and the distribution information of the point cloud into the construction of the loss function, comprehensively considering the high-dimensional semantic information and geometric information of the point cloud. Therefore, the SD-GICP algorithm can be described as a "semantic plane - semantic plane" registration algorithm.

[0094] Step 4: Extract the local map based on the relative pose of the point cloud between adjacent frames.

[0095] In some embodiments, specifically, an iKd-Tree is used to maintain the local map, and the relative pose transformation matrix from the source point cloud to the target point cloud obtained by the adjacent scan frame registration module provides the initial pose for the local map, facilitating the extraction of the local map from the initial pose area.

[0096] Since the registration between frames is a local-to-local registration, there are some point clouds that cannot find true corresponding points during the registration process. And adopting a local-to-global registration method can solve this problem and achieve higher accuracy during the registration process. Therefore, it is necessary to maintain a local map for frame-to-local map registration.

[0097] Step 5: Perform frame-to-local map registration on the local map based on the point cloud registration algorithm that fuses semantic and distribution information to screen out key frames.

[0098] In some embodiments, specifically, the SD-GICP algorithm is used to perform local map registration on the scan frame to obtain the pose estimation of the current frame semantic point cloud. If the displacement or rotation between the current frame and the previous frame exceeds the first preset threshold, it is marked as a key frame.

[0099] Step 6: Perform loop detection on the key frames to obtain loop frames.

[0100] In some embodiments, specifically, the key-frame semantic point cloud is input into a loop closure detection module that fuses semantic and geometric information. First, based on the semantic label information of the key-frame semantic point cloud, the loop closure detection module extracts the point cloud with persistent invariance; then, it encodes the point cloud with persistent invariance based on a loop closure descriptor that fuses semantic and geometric information to obtain an encoding matrix; based on an evaluation algorithm for the similarity of the encoding matrix, the similarity of the encoding matrix is calculated. If the current key frame is the first key frame, the corresponding encoding matrix is directly stored in the key-frame encoding matrix library. Otherwise, in the encoding matrix library, a historical key-frame encoding matrix with the highest similarity to the encoding matrix of the current key frame is searched through traversal. If the similarity between the two is less than the second preset threshold, these two frames of point cloud are considered loop closure frames for each other.

[0101] In some embodiments, more specifically, the method for constructing a loop closure descriptor that fuses semantic and geometric information includes:

[0102] According to the semantic label information of the semantic point cloud data, the points with persistent invariance in the point cloud (such as points like buildings, trees, and traffic signs, etc.) are extracted to obtain the point cloud

[0103] Using the partitioning method of Scan-Context, first, each point p(x, y, z, l) in the point cloud is converted from Cartesian coordinates to polar coordinates to obtain p(d, α, β); where d represents the horizontal distance from point p to the lidar, α represents the azimuth angle in the horizontal direction, and β represents the pitch angle in the vertical direction. Assuming the maximum scanning radius of the lidar is R max , according to d and α, the point cloud is divided into ring-sector blocks, resulting in a total of N s ×N r ring-sector blocks, as shown in Figure 5 . Each ring-sector block can be represented by Equation (4).

[0104]

[0105] Among them, i and j respectively represent the numbers of each ring-sector block; the subscript k represents the serial number of the point cloud , h k represents the height information of the point with serial number k, l k represents the semantic label information of the point with serial number k, d k represents the horizontal distance from the point with serial number k to the lidar, and α k represents the azimuth angle of the point with serial number k in the horizontal direction.

[0106] After the ring-sector block division, first, a height encoding matrix Ω h and a semantic encoding matrix Ω sMapping calculation

[0107] Ω h The calculation method is as follows: traverse each annular sector block Block ij in the point cloud to find the maximum height value h of the point cloud, and assign values according to Ω h (i, h) = h. If no points fall into the current annular sector block, then let Ω h (i, j) = 0. In this way, the height encoding matrix Ω h is obtained.

[0108] Ω s The calculation method is as follows: traverse each annular sector block Block ij in the semantic label information of the point cloud to find the label l with the highest priority (the semantic priority order can be given artificially and sorted according to the occurrence probability of the semantic label in the scene. The lower the occurrence probability of the current semantic label, the higher the priority), and assign values according to Ω h (i, h) = l. If no points fall into the current annular sector block, then let Ω s (i, j) = 0. In this way, the semantic encoding matrix Ω s is obtained.

[0109] Create a three-dimensional matrix Ω s ×N s ×2, and store the height encoding matrix Ω hs in the first layer of Ω hs , and store the semantic encoding matrix Ω h in the second layer. Finally, the joint encoding matrix Ω s that fuses height information and semantic information is obtained, and its expression can refer to Equation (5). hs The above mapping method divides the annular sector blocks according to the X-axis and Y-axis coordinate values of the points, and the Z-axis coordinate participates in the encoding of the annular sector blocks. For Ω

[0110]

[0111] and Ω h and Ω s , the same column in the matrix belongs to the same ring, and each row belongs to the same sector.

[0112] In some embodiments, more specifically, the algorithm for evaluating the similarity of the encoding matrix specifically includes the following steps.

[0113] After the loop descriptor that fuses semantic and geometric information encodes the point cloud into a matrix, a method for measuring the similarity of the encoding matrix is needed to associate the point clouds with similar encoding matrix similarities, so as to achieve the purpose of loop detection.

[0114] Assume there is a source point cloud and target point cloud The corresponding joint encoding matrix is ​​obtained by fusing the loop descriptor with semantic and geometric information. and Respectively and Proceed as Figure 6 The vector construction process shown in (a) is in the matrix Ω h and Ω s to vector ζ h and s In the conversion process, h and s Each number in is represented by the Ω h and Ω s The mean of all elements in the corresponding column of h and s To hs In the fusion step, formula (6) is used for weighted fusion.

[0115] ζ hs (i) = ηζ h (i)+(1-η)ζ s (i) (6)

[0116] Among them, η represents the weight coefficient in the weighting process. The vector ζ calculated in this way hs Each element in represents the fusion feature of a fan-shaped area, and the entire vector represents all the features of the 360° scan, thereby constructing a detection vector that can be used to detect the horizontal rotation between the source point cloud and the target point cloud.

[0117] Methods for detecting rotation are as follows Figure 6 (b) shows that and According to the above method, we can calculate and Then the distance r between these two vectors can be expressed as:

[0118]

[0119] Formula (7) represents the distance relationship between two vectors. and They are loop frames, then and The distance between them will be small.

[0120] Source point cloud The corresponding vector Perform a shift operation, that is, move the first element of the vector to the end of the vector. Each time a move is made, calculate the distance between the vectors according to Equation (7) until a minimum r is found. At this time, record the number of moves as n, and then calculate the and The horizontal rotation angle ω existing between them refers to Equation (8).

[0121]

[0122] While shifting the vector, it is also necessary to shift the joint coding matrix by the same rule, as shown in Figure 7 .

[0123] After the shift is completed, by comparing each column of and , that is, by the similarity of the column vectors of the matrix, it is judged whether and are loopback frames to each other. For this purpose, a method for measuring the similarity d of the joint coding matrix is given, referring to Equation (9).

[0124]

[0125] Among them, d represents the similarity of the two joint coding matrices; and respectively represent the joint coding matrices corresponding to the source point cloud and the target point cloud; and respectively represent the height coding matrices corresponding to the source point cloud and the target point cloud; and respectively represent the semantic coding matrices corresponding to the source point cloud and the target point cloud; F h and F s respectively represent the similarity between the height coding matrices and the similarity between the semantic coding matrices; λ represents the similarity weight coefficient. F h and F s The specific expressions of can refer to Equation (10).

[0126]

[0127] Among them, H is the semantic correlation function, which is used to evaluate the relationship between two loop fan blocks, and its expression is:

[0128]

[0129] Since the loop descriptors that integrate semantic and geometric information encode the semantic point cloud data, each encoding matrix becomes a feature representation of the semantic point cloud. By calculating the similarity of these encoding matrices, it is possible to determine whether there is a similarity relationship between the corresponding point clouds. If the similarity of the encoding matrices corresponding to two frames of point clouds is less than the set threshold, these two frames of point clouds are considered loop frames for each other.

[0130] Step 7: Register the loop frames based on the point cloud registration algorithm that integrates semantic and distribution information to obtain the relative pose between the loop frames.

[0131] In some embodiments, specifically, the SD-GICP algorithm is used for registration between loop frames to obtain an accurate relative pose transformation matrix between frames, and this matrix will be transmitted to the backend optimization module for optimization.

[0132] Step 8: Jointly optimize the relative pose between the loop frames and the poses of the key frames and the local map to obtain the final semantic point cloud map.

[0133] In some embodiments, specifically, in the backend optimization module, a factor graph will be constructed using GTSAM, and the pose constraints of the key frames and the local map and the constraint information of the loop frames will be sent to this backend optimization module for joint pose optimization. Using the finally optimized pose (i.e., trajectory), coordinate transformation is performed on the point cloud to obtain the final semantic point cloud map.

[0134] First, the present application proposes a point cloud registration algorithm (SD-GICP algorithm) that integrates semantic and distribution information, uses the semantic labels of the point cloud to optimize the point cloud association process, designs a semantic downsampling method to perform custom downsampling on the point cloud, assigns different downsampling parameters to the point cloud through the semantic label information, reduces the information loss of the point cloud, and defines a new set of semantic registration loss functions, reducing the probability of false matching and optimizing the construction of the loss function; Secondly, the present application proposes a loop detection method that integrates semantic and geometric information. By jointly encoding the high-dimensional semantic information of the point cloud and the geometric information of the point cloud, the representativeness of the encoding matrix is improved, the expression ability of the constructed point cloud map is enhanced, and it is more conducive to later development and maintenance. A calculation method for evaluating the similarity of the encoding matrix is also proposed, which is used to detect whether there is a loop relationship between point clouds with similar encoding matrices, thereby further reducing the probability of false detection during loop detection. Finally, the present application integrates the above improvements into the SLAM framework to form a complete new semantic SLAM method. The present application starts from the point cloud registration link and the loop detection link of the SLAM method, and proposes the above targeted optimization solutions, thereby improving the positioning accuracy and mapping quality of the SLAM method at the overall level.

[0135] To verify the superiority of a map localization and construction method based on laser SLAM proposed in this application, an embodiment of this application selected the KITTI dataset for experimental verification. The KITTI dataset uses a Velodyne HDL-64E-3D lidar with a scanning frequency of 10 Hz. The collected scenarios include cities, villages, and highways. The dataset content only includes raw lidar point cloud data, and there are 11 publicly available sequences with ground truth trajectories (KITTI-00, KITTI-01, …, KITTI-10). The trained Cylinder3D semantic segmentation model was used to perform semantic segmentation on the KITTI dataset, obtaining the class label information of each point in the point cloud sequence. A total of 19 semantic classes were output, and the same class was represented by the same color, while different classes had different colors. All experiments were run on a laptop equipped with an AMD R5-5600H (3.3 GHz) processor, an Nvidia RTX 3050 graphics card, and 32 GB (3200 MHz) of memory.

[0136] Through experiments, it was found that the point cloud registration algorithm that fuses semantic and geometric information and the loop closure detection method that fuses semantic and geometric information proposed in this application, when applied to the SLAM system, can effectively improve the positioning accuracy and the quality of map construction.

[0137] In summary, in response to the technical problem of high requirements for positioning accuracy and mapping quality in the road scenarios of the autonomous driving scenario, this application proposes a map localization and construction method based on laser SLAM, which can significantly improve the positioning accuracy and mapping quality. The proposed method combines the technologies in the traditional point cloud data processing field and the deep learning field, and has certain promotional value.

[0138] The flowcharts and / or block diagrams in the accompanying drawings illustrate the possible architectures, functions, and operations of systems, methods, or computer program products according to various embodiments of this application. In this regard, each block in the flowchart and / or block diagram may represent a module, a program segment, or a part of code, and the above-mentioned module, program segment, or part of code contains one or more executable instructions for implementing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the blocks may occur in a different order than marked in the accompanying drawings. It should also be noted that each block in the block diagram or flowchart, and the combination of blocks in the block diagram or flowchart, can be implemented by a dedicated hardware-based system for performing the specified functions or operations, or can be implemented by a combination of dedicated hardware and computer instructions.

[0139] Those skilled in the art will understand that the technical features recited in the various embodiments and / or claims of the present application can be combined and / or combined in various ways, even if such combinations or combinations are not explicitly recited in the present application. In particular, without departing from the spirit and teachings of the present application, the technical features recited in the various embodiments and / or claims of the present application can be combined and / or combined in various ways, and all such combinations and / or combinations fall within the scope of the present application.

[0140] Although the present application has been shown and described with reference to specific exemplary embodiments thereof, those skilled in the art should understand that various changes in form and detail can be made therein without departing from the spirit and scope of the present application as defined by the appended claims and their equivalents. Therefore, the scope of the present application should not be limited to the above embodiments, but should be determined not only by the appended claims but also by the equivalents of the appended claims.

Claims

1. A map positioning and construction method based on laser SLAM, characterized in that: include: Get the original point cloud data; Performing semantic segmentation preprocessing on the original point cloud data, extracting semantic label information of each point in each frame of point cloud, and obtaining a semantic point cloud; Registering adjacent scanned frames of the semantic point cloud by using a point cloud registration algorithm that integrates semantics and distribution information to obtain relative poses of point clouds between adjacent frames; Extracting a local map based on the relative poses of the point clouds between adjacent frames; Based on the point cloud registration algorithm of the fusion semantics and distribution information, the local map is registered from the scanned frame to the local map to screen out key frames; Performing loop detection on the key frame to obtain a loop frame; Registering the loop frames based on the point cloud registration algorithm that integrates semantics and distribution information to obtain the relative pose between frames of the loop frames; Jointly optimizing the relative poses between the loop frames and the poses of the key frames and the local map to obtain a final semantic point cloud map; The point cloud registration algorithm integrating semantic and distribution information includes: Performing semantic downsampling processing on adjacent scan frames of the semantic point cloud respectively; Construct a semantic association loss function between the source point cloud and the target point cloud according to the pose of the source point cloud; The semantic association loss function is cyclically solved by the Levenberg-Marquardt nonlinear optimization algorithm to obtain a relative pose transformation matrix from the source point cloud to the target point cloud; The semantic downsampling process includes: Inputting adjacent scan frames of the semantic point cloud into a semantic downsampler, and different categories of point clouds are distinguished using different marks; Extracting point clouds of different categories respectively according to the semantic label information of the semantic point cloud to form sub-point clouds; The blocks with different marks in the semantic downsampler represent different categories of point cloud data, and each category of point cloud is followed by a corresponding voxel downsampler.

2. The map positioning and construction method according to claim 1, characterized in that: The method for constructing the semantic association loss function includes: The nearest neighbor search algorithm is used to find the nearest neighbor point of each point in the source point cloud in the target point cloud and form a nearest neighbor point set. Based on the nearest neighbor point set, the Euclidean nearest neighbor point set of each point in the target point cloud is found. The calculation formula of the covariance matrix of each point in the Euclidean nearest neighbor point set is: Among them, p i represents the point numbered i in the Euclidean nearest neighbor point set; represents the mean of the Euclidean nearest neighbor point set; C i Indicates p i The covariance matrix of The semantic label information is introduced to optimize the point cloud association process, and p a ={x a ,y a ,z a ,l a } and q b ={x b ,y b ,z b ,l b } are single points in the source point cloud and the target point cloud respectively, and q b Yes a The corresponding points found by the nearest neighbor search algorithm are then the Euclidean distance between the two points is d pq =||p a -q b ||2, define a semantic correlation coefficient ζ to evaluate the effectiveness of the association, ζ can be expressed as: Among them, τ is a constant less than 1, which is used to measure the correlation coefficient of semantic label information mismatch; The expression of the semantic association loss function is: Where T represents the optimal relative posture to be solved; represents p in the source point cloud i The covariance matrix of Indicates p i The covariance matrix of the nearest neighbor point in the corresponding target point cloud; d i represents the point p in the source point cloud i and point p i The Euclidean distance between the nearest neighbor points in the corresponding target point cloud.

3. The map positioning and construction method according to claim 1, characterized in that: The extracting of the local map based on the relative pose of the point clouds between adjacent frames includes: Use iKd-Tree to maintain local maps; The relative pose transformation matrix from the source point cloud to the target point cloud provides an initial pose for maintaining the local map; The local map is extracted from a region of the initial pose.

4. The map positioning and construction method according to claim 3, characterized in that: The point cloud registration algorithm based on the fusion semantics and distribution information performs registration of the scanned frame to the local map on the local map to screen out key frames, including: Based on the point cloud registration algorithm integrating semantics and distribution information, the local map is registered from the scanned frame to the local map to obtain a pose estimate of the current frame of the semantic point cloud; If the displacement and / or rotation between the current frame and the previous frame of the semantic point cloud exceeds a first preset threshold, the current frame is marked as a key frame and extracted.

5. The map positioning and construction method according to claim 4, characterized in that: The performing loop detection on the key frame to obtain the loop frame comprises: Extracting a point cloud with persistence and invariance according to the semantic label information of the key frame of the semantic point cloud; Encoding the point cloud with continuous invariance based on a loop descriptor that integrates semantic and geometric information to obtain a coding matrix; Based on the coding matrix similarity evaluation algorithm, similarity calculation is performed on the coding matrix to obtain the highest similarity between the coding matrix of the current key frame and the coding matrix of the historical key frame; If the highest similarity is less than a second preset threshold, the point clouds corresponding to the current key frame and the historical key frame corresponding to the highest similarity are each other's loop frames.

6. The map positioning and construction method according to claim 5, characterized in that: The step of jointly optimizing the relative inter-frame poses of the loop-closed frames and the poses of the key frames and the local map to obtain a final semantic point cloud map includes: Use GTSAM to construct factor graphs; Based on the factor graph, performing joint pose optimization on the relative pose constraint information between frames of the loop frame and the pose constraint information of the key frame and the local map to obtain a final optimized pose; The point cloud is coordinate-transformed using the final optimized pose to obtain the final semantic point cloud map.

Images