Map positioning and constructing method based on laser SLAM (Simultaneous Localization and Mapping)

Abstract

The invention provides a map positioning and constructing method based on laser SLAM (Simultaneous Localization and Mapping), the method proposes a point cloud registration algorithm fusing semantic and distribution information, the algorithm can utilize semantic label information of point cloud to optimize a correlation process of the point cloud, different down-sampling parameters can be distributed to the point cloud through the semantic label information, and the correlation process of the point cloud is optimized. Therefore, the information loss of the point cloud is reduced, and the probability of mismatching in registration is reduced. According to the method, optimization is carried out from the point cloud registration link of the SLAM method, and the positioning precision and the mapping quality of the SLAM method can be improved.

Description

Technical Field

[0001] The present 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 lifestyle. Among them, achieving accurate positioning is the premise for supporting downstream applications of autonomous driving. The higher the level of autonomous driving, the higher the positioning accuracy required. However, traditional positioning solutions based on high-precision differential global positioning system (DGPS) and inertial navigation system (INS) will have poor accuracy and positioning drift in scenes such as under viaducts, dense tall buildings and underground parking lots, and cannot meet the needs of continuous and accurate positioning of autonomous driving. SLAM (Simultaneous Localization and Mapping, SLAM, i.e.: simultaneous positioning and mapping) has achieved stable and accurate self-positioning with its sensors equipped with environmental information, and is not affected by signal occlusion and interference, as well as the ability to assist in the construction of high-precision maps, making it 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, which is prone to mismatching; second, the method of loop detection that only uses the geometric information of point clouds is prone to misdetection 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, point cloud mismatching and loop misdetection will introduce erroneous 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 response to at least one defect or improvement need in the prior art, the present 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 objectives, the present application provides a map positioning and construction method based on laser SLAM, including: 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; The relative poses between the loop frames and the poses of the key frames and the local map are jointly optimized to obtain a final semantic point cloud map.

[0006] Furthermore, 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 the relative pose transformation matrix from the source point cloud to the target point cloud.

[0007] Furthermore, 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.

[0008] Furthermore, 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:

[0009] in, Indicates that the number of the Euclidean nearest neighbor point set is point; represents the mean of the Euclidean nearest neighbor point set; express The covariance matrix of The semantic label information is introduced to optimize the association process of the point cloud. and are single points in the source and target point clouds, respectively, and yes The corresponding points found by the nearest neighbor search algorithm are then the Euclidean distance between the two points is , define a semantic correlation coefficient To evaluate the effectiveness of the association, It can be expressed as: ; in, is a constant less than 1, used to measure the correlation coefficient of semantic label information mismatch; The expression of the semantic association loss function is: ; in, represents the optimal relative pose to be solved; Represents the source point cloud The covariance matrix of express The covariance matrix of the nearest neighbor point in the corresponding target point cloud; Represents a point in the source point cloud and Point The Euclidean distance between the nearest neighbor points in the corresponding target point cloud.

[0010] Furthermore, extracting a local map based on the relative position 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.

[0011] Furthermore, 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.

[0012] Further, performing loop detection on the key frame to obtain a loop frame includes: Extracting a point cloud with continuous 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.

[0013] Furthermore, the method for constructing the loop descriptor integrating semantic and geometric information includes: Using the Scan-Context segmentation method, the point cloud with continuous invariance is Each point in Convert Cartesian coordinates to polar coordinates and get ;in, express The horizontal distance from the point to the laser radar, represents the horizontal azimuth, Indicates the pitch angle in the vertical direction; according to and Point Cloud Divide the ring into sectors and get the total Ring sector blocks; each ring sector block The expression is:

[0014] in, and Respectively represent the number of each ring sector block; subscript Representing point clouds The serial number, Indicates the serial number is The height information of the point, Indicates the serial number is The semantic label information of the point, Indicates the serial number is The horizontal distance from the point to the laser radar, Indicates the serial number is The azimuth of the point in the horizontal direction; Indicates the maximum scanning radius of the laser radar; Traverse each ring sector block The height value of the point cloud in the point cloud is found. , and encode the matrix according to the height Assign a value. If no point falls into the current ring sector block, let the height encoding matrix , according to this method, we can get the highly coded matrix ; Traverse each ring sector block Semantic label information of the point cloud in , find the label with the highest priority , and encode the matrix according to the height Assign a value. If no point falls into the current ring sector, let the semantic encoding matrix According to this method, the semantic coding matrix is ​​obtained ; Create a size The three-dimensional matrix ,exist One of the layers is stored in the highly encoded matrix , store the semantic encoding matrix in another layer , and finally obtain the joint encoding matrix that integrates height information and semantic information , whose expression is: .

[0015] Furthermore, the algorithm for evaluating the similarity of coding matrices includes: ; in, Represents the similarity of two joint coding matrices; and Represent the joint encoding matrices corresponding to the source point cloud and the target point cloud respectively; and Respectively represent the height encoding matrices corresponding to the source point cloud and the target point cloud; and Represent the semantic encoding matrices corresponding to the source point cloud and the target point cloud respectively; and Respectively represent the similarity between height encoding matrices and the similarity between semantic encoding matrices; Represents the similarity weight coefficient;

[0016] in, is a semantic relevance function used to evaluate the relationship between two ring sectors, and its expression is: .

[0017] Furthermore, the inter-frame relative pose of the loop-closed frame and the pose of the key frame and the local map are jointly optimized to obtain the final semantic point cloud map, including: 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.

[0018] In general, the above technical solutions conceived by the present application can achieve the following beneficial effects compared with the prior art: (1) This application proposes a map positioning and construction method based on laser SLAM. This method proposes a point cloud registration algorithm that integrates semantic and distribution information. This algorithm can use the semantic label information of the point cloud to optimize the point cloud association process. Different downsampling parameters can be assigned to the point cloud through the semantic label information, thereby reducing the information loss of the point cloud and reducing the probability of mismatching. This application starts with the optimization of the point cloud registration link of the SLAM method, which can improve the positioning accuracy and mapping quality of the SLAM method.

[0019] (2) This application designs a semantic downsampling method to perform customized downsampling on point clouds. Different downsampling parameters are assigned to point clouds through semantic label information, thereby reducing the information loss of point clouds. A new set of semantic registration loss functions is defined, thereby reducing the probability of mismatching and optimizing the construction of the loss function.

[0020] (3) This application proposes a loop descriptor that integrates semantic and geometric information. By combining the high-dimensional semantic information of the point cloud and the geometric information of the point cloud for joint encoding, the representativeness of the encoding matrix is ​​improved, the expressiveness of the constructed point cloud map is enhanced, and it is more conducive to later development and maintenance. At the same time, since the high-dimensional semantic information of the point cloud and the geometric information of the point cloud are integrated for loop detection, the probability of false detection during loop detection is reduced. This application proposes a calculation method for evaluating the similarity of encoding matrices, which is used to detect whether point clouds corresponding to similar encoding matrices have a loop relationship, thereby further reducing the probability of false detection during loop detection.

[0021] (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 scheme, thereby improving the positioning accuracy and mapping quality of the SLAM method at the overall level. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] In order to more clearly illustrate the technical solutions in the embodiments of the present application, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0023] Figure 1 A core flow chart of a map positioning and construction method based on laser SLAM provided in an embodiment of the present application; Figure 2 A complete system framework diagram of a novel semantic SLAM method provided in an embodiment of the present application; Figure 3 A schematic diagram of the principle of a point cloud registration algorithm that integrates semantic and distribution information provided in an embodiment of the present application; Figure 4 A flowchart of a semantic downsampling process and a comparison diagram of the effects provided in an embodiment of the present application; Figure 5 A schematic diagram of a loop coding method that integrates semantic and geometric information provided in an embodiment of the present application; Figure 6 A schematic diagram of a coding vector shift transformation provided in an embodiment of the present application; Figure 7 A schematic diagram of a joint coding matrix shift transformation provided in an embodiment of the present application. DETAILED DESCRIPTION

[0024] In order to make the purpose, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and are not intended to limit the present application. In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.

[0025] The terms "first", "second" or "third" in the specification, claims or drawings of this application are intended to refer to the following: n ” and the like 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 inclusions. For example, a process, method, system, product or device comprising a series of steps or units is not limited to the listed steps or units, but may optionally include steps or units that are not listed, or may optionally include other steps or units inherent to these processes, methods, products or devices.

[0026] As described in the background technology section of the specification, traditional SLAM algorithms rely only on the geometric information of point clouds for registration, which is prone to mismatching; secondly, methods that only use the geometric information of point clouds for loop detection are prone to misdetection 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, point cloud mismatching and loop misdetection will introduce erroneous constraint information into 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.

[0027] refer to Figure 1 and Figure 2 An embodiment of the present application proposes a map positioning and construction method based on laser SLAM, which mainly includes the following steps.

[0028] Step 1: Get the original point cloud data.

[0029] In some embodiments, specifically, a laser radar is installed on a test vehicle, and a coordinate system calibration is performed to collect original point cloud data obtained by the vehicle-mounted laser radar during the test.

[0030] 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 a semantic point cloud.

[0031] In some embodiments, specifically, the original point cloud data is passed into a point cloud preprocessing module, which includes a pre-trained semantic segmentation network model that performs forward reasoning on the point cloud, extracts semantic label information for each point in each frame of the point cloud, and obtains a semantic point cloud.

[0032] Step 3: The adjacent scanned frames of the semantic point cloud are registered by a point cloud registration algorithm that integrates semantic and distribution information to obtain the relative pose of the point cloud between adjacent frames.

[0033] In some embodiments, specifically, the semantic point clouds of adjacent frames transmitted are firstly processed by semantic downsampling, and the semantic point clouds are classified and downsampled by pre-set downsampling parameters to reduce the consumption of computing resources while retaining more valuable points. The downsampled point clouds of adjacent frames are then aligned through the SD-GICP algorithm (point cloud registration algorithm integrating semantics and distribution information) proposed in the present application to obtain the relative pose transformation matrix between the two frames of point clouds.

[0034] The process diagram of semantic downsampling and the effect comparison chart are referenced Figure 4, the input of the semantic downsampler is semantic point cloud data. In the semantic point cloud, each point carries a label information, which represents the category information of the point. Different categories of point clouds are distinguished by different colors. According to the label information of the semantic point cloud, different categories of point clouds are extracted separately to form sub-point clouds. The blocks of different colors in the semantic downsampler represent different categories of point cloud data. 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 of the semantic downsampled point cloud and the traditional downsampled point cloud, it can be seen that the point cloud sampled by the semantic downsampler retains more points with structured features (yellow points in the figure), while the traditional downsampled point cloud retains more messy points (green points in the figure).

[0035] The principle of SD-GICP algorithm is as follows Figure 3 As shown in the figure, after the semantic point cloud is downsampled, the 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 solved by the Levenberg-Marquardt nonlinear optimization algorithm. Through the cyclic solution, the relative pose transformation matrix from the source point cloud to the target point cloud is finally obtained.

[0036] An embodiment of the present application constructs a new set of semantic association loss functions, and the specific principle of the semantic association loss function is as follows.

[0037] Let the target point cloud and source point cloud For the semantic point cloud of adjacent frames (assuming and The number of midpoints is m and n ), and use the nearest neighbor search algorithm to Found The nearest neighbor points of each point in form a point set. Based on this nearest neighbor point set, Find the Euclidean nearest neighbor set of each point in (Assume that the number of points is fixed to ), then the covariance matrix of each point The calculation formula of refers to formula (1).

[0038]

[0039] in, Represents the Euclidean nearest neighbor point set The number in is point; Represents a point set The mean of express The covariance matrix of .

[0040] Since the nearest neighbor search algorithm can only perform the nearest neighbor search based on the Euclidean minimum distance, there may be a situation where the source point and the target point do not belong to the same category, and this mismatch will reduce the accuracy of the registration algorithm. For this reason, this embodiment introduces semantic label information here to optimize the point cloud association process.

[0041] First, assume Point cloud and A single point in yes The corresponding points found by the nearest neighbor search algorithm, then the Euclidean distance between two points can be expressed as .

[0042] Then, define a semantic correlation coefficient To evaluate the effectiveness of the association, It can be expressed as formula (2).

[0043] (2) in, 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 that represents the semantic distribution point cloud registration can be expressed as formula (3).

[0044] (3) in, represents the optimal relative pose to be solved; Represents the source point cloud In The covariance matrix of express The covariance matrix of the nearest neighbor point in the corresponding target point cloud; Represents a point in the source point cloud and Point The Euclidean distance between the nearest neighbor points in the corresponding target point cloud.

[0045] The SD-GICP algorithm incorporates semantic association information and point cloud distribution information into the construction of the loss function, and comprehensively considers 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.

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

[0047] In some embodiments, specifically, 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 scanning frame registration module provides an initial pose for the local map, so as to facilitate the extraction of the local map from the initial pose area.

[0048] Since the registration between frames is a local-to-local registration, some point clouds cannot find real corresponding points during the registration process. The local-to-global registration method can solve this problem and achieve higher accuracy in the registration process. Therefore, it is necessary to maintain a local map for frame-to-local map registration.

[0049] Step 5: Based on the point cloud registration algorithm that integrates semantics and distribution information, the scanned frame is registered to the local map to screen out key frames.

[0050] In some embodiments, specifically, the scanned frames are locally mapped using the SD-GICP algorithm to obtain a pose estimate of the semantic point cloud of the current frame. If the displacement or rotation of the current frame with the previous frame exceeds a first preset threshold, it is marked as a key frame.

[0051] Step 6: Perform loop detection on the key frame to obtain a loop frame.

[0052] In some embodiments, specifically, the keyframe semantic point cloud is input into a loop detection module that integrates semantic and geometric information. The loop detection module first extracts the point cloud with persistent invariance based on the semantic label information of the keyframe semantic point cloud; then encodes the point cloud with persistent invariance based on the loop descriptor that integrates semantic and geometric information to obtain a coding matrix; the coding matrix is ​​similarly calculated based on the coding matrix similarity evaluation algorithm. If the current keyframe is the first keyframe, its corresponding coding matrix is ​​directly stored in the keyframe coding matrix library. Otherwise, the coding matrix library is traversed to find a historical keyframe coding matrix with the highest similarity to the current keyframe coding matrix. If the similarity between the two is less than the second preset threshold, the two frames of point clouds are considered to be loop frames of each other.

[0053] In some embodiments, more specifically, a method for constructing a loop descriptor that integrates semantic and geometric information includes: According to the semantic label information of the semantic point cloud data, the points with constant invariance in the point cloud (such as buildings, trees, and traffic signs) are extracted to obtain the point cloud. Using the Scan-Context segmentation method, first divide the point cloud Each point in Convert Cartesian coordinates to polar coordinates and get ;in express The horizontal distance from the point to the laser radar, represents the horizontal azimuth, Indicates the vertical pitch angle. Assume that the maximum scanning radius of the laser radar is ,according to and Point Cloud Divide the ring into sectors and get the total Ring fan blocks, such as Figure 5 As shown, each ring sector block can be expressed by formula (4).

[0054]

[0055] in, and Respectively represent the number of each ring sector block; subscript Representing point clouds The serial number, Indicates the serial number is The height information of the point, Indicates the serial number is The semantic label information of the point, Indicates the serial number is The horizontal distance from the point to the laser radar, Indicates the serial number is The azimuth of the point in the horizontal direction.

[0056] After the ring sector blocks are divided, the height encoding matrix is ​​first and the semantic encoding matrix The mapping calculation.

[0057] The calculation method is: traverse each ring sector block The height value of the point cloud in the point cloud is found. , and follow Assign a value. If no point falls into the current ring sector, then , according to this method, we can get the highly coded matrix .

[0058] The calculation method is: traverse each ring sector block Semantic label information of the point cloud in , find the label with the highest priority (Semantic priority ranking can be given manually, and is sorted according to the probability of occurrence of semantic tags in the scene. The lower the probability of the current semantic tag appearing, the higher the priority), and according to Assign a value. If no point falls into the current ring sector, let According to this method, the semantic coding matrix is ​​obtained .

[0059] Create a size The three-dimensional matrix ,exist The first layer is stored in the highly encoded matrix , store the semantic encoding matrix in the second layer , and finally obtain the joint encoding matrix that integrates height information and semantic information , its expression can be referred to formula (5).

[0060]

[0061] The above mapping method divides the ring sector blocks according to the coordinate values ​​of the X-axis and Y-axis of the point, and the Z-axis coordinate participates in the encoding of the ring sector blocks. and , the same column in the matrix belongs to the same ring, and each row belongs to the same sector.

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

[0063] After the loop descriptor that integrates semantic and geometric information encodes the point cloud into a matrix, a measurement method is needed to describe the similarity of the encoding matrix, so as to associate point clouds with similar encoding matrices and achieve the purpose of loop detection.

[0064] Assume there is a source point cloud and target point cloud , the corresponding joint encoding matrix is ​​obtained by fusing the loop descriptor of semantic and geometric information and . and Proceed as Figure 6 (a) shows the vector construction process, in the matrix and To vector and In the process of conversion, and Each number in is represented by its and The mean of all elements in the corresponding column of and arrive In the fusion step, formula (6) is used for weighted fusion.

[0065]

[0066] in, Represents the weight coefficient in the weighting process. The vector calculated in this way 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.

[0067] 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 between these two vectors is It can be expressed as: (7) Formula (7) represents the distance relationship between two vectors. and They are loop frames, then and The distance between them will be small.

[0068] 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, and calculate the distance between the vectors according to formula (7) each time the vector is moved until a minimum value is found. , at this time, the number of moves is , then the calculated and There is a horizontal rotation angle between Refer to formula (8).

[0069] (8) When shifting the vector, the joint coding matrix needs to be shifted according to the same rule. Perform the same shift operation, such as Figure 7 shown.

[0070] After the shift is completed, and Compare each column of the matrix, that is, judge by the similarity of the column vectors of the matrix and Whether they are loop frames. For this purpose, the similarity of the joint coding matrix is ​​given Refer to formula (9) for the method.

[0071] (9) in, Represents the similarity of two joint coding matrices; and Represent the joint encoding matrices corresponding to the source point cloud and the target point cloud respectively; and Respectively represent the height encoding matrices corresponding to the source point cloud and the target point cloud; and Represent the semantic encoding matrices corresponding to the source point cloud and the target point cloud respectively; and Respectively represent the similarity between height encoding matrices and the similarity between semantic encoding matrices; Represents the similarity weight coefficient. and The specific expression of can refer to formula (10).

[0072] (10) in, is a semantic relevance function used to evaluate the relationship between two ring sectors, and its expression is:

[0073] Since the loop descriptor that combines semantic and geometric information encodes 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 can be determined whether there is a similar 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, the two frames of point clouds are considered to be loop frames.

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

[0075] In some embodiments, specifically, the loop frames are aligned using the SD-GICP algorithm to obtain an accurate relative pose transformation matrix between frames, which will be passed to the back-end optimization module for optimization.

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

[0077] In some embodiments, specifically, in the backend optimization module, a factor graph will be constructed using GTSAM, and the pose constraints of the key frame and the local map and the constraint information of the loop frame will be sent to the backend optimization module for pose joint optimization. The point cloud is transformed using the final optimized pose (i.e., trajectory) to obtain the final semantic point cloud map.

[0078] First, this application proposes a point cloud registration algorithm (SD-GICP algorithm) that integrates semantic and distribution information, optimizes the point cloud association process using point cloud semantic labels, designs a semantic downsampling method to perform custom downsampling on point clouds, assigns different downsampling parameters to point clouds through semantic label information, reduces the information loss of point clouds, and defines a new set of semantic registration loss functions, reduces the probability of mismatching and optimizes the construction of loss functions; secondly, this application proposes a loop detection method that integrates semantic and geometric information, and combines the high-dimensional semantic information of point clouds and the geometric information of point clouds for joint encoding, thereby improving the representativeness of the encoding matrix and the expression ability of the constructed point cloud map, which is more conducive to later development and maintenance. A calculation method for evaluating the similarity of encoding matrices is also proposed to detect whether there is a loop relationship between point clouds corresponding to similar encoding matrices, thereby further reducing the probability of misdetection during loop detection. Finally, this application integrates the above improvements into the SLAM framework to form a complete set of new semantic SLAM methods. This application starts from the point cloud registration link and loop detection link of the SLAM method, and proposes the above-mentioned targeted optimization scheme, thereby improving the positioning accuracy and mapping quality of the SLAM method at the overall level.

[0079] In order to verify the superiority of a map positioning 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 laser radar with a scanning frequency of 10 Hz. The collected scenes include cities, villages and highways. The dataset content only contains raw laser point cloud data. There are 11 public sequences with trajectory truth values ​​(KITTI-00, KITTI-01, ..., KITTI-10). The KITTI dataset is semantically segmented using the trained Cylinder3D semantic segmentation model to obtain the category label information of each point in the point cloud sequence. A total of 19 semantic classes are output. The same category is represented by the same color, and different categories have 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) memory.

[0080] Through experiments, it is found that the point cloud registration algorithm integrating semantic and geometric information and the loop detection method integrating semantic and geometric information proposed in this application are applied in the SLAM system, which can effectively improve the positioning accuracy and the quality of map construction.

[0081] In summary, this application proposes a map positioning and construction method based on laser SLAM to solve the technical problem of high positioning accuracy and mapping quality requirements in road scenes of autonomous driving scenes, which can significantly improve 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 promotion value.

[0082] The flowchart and / or block diagram in the accompanying drawings illustrate the possible architecture, functions and operations of the system, method or computer program product according to various embodiments of the present application. In this regard, each box in the flowchart and / or block diagram can represent a part of a module, program segment or code, and a part of the above-mentioned module, program segment or code includes one or more executable instructions for realizing the specified logical function. It should also be noted that in some alternative implementations, the functions marked in the box can also occur in a different order from that marked in the accompanying drawings. It should also be noted that each box in the block diagram or flowchart, and the combination of the boxes in the block diagram or flowchart can be implemented with a dedicated hardware-based system that performs a specified function or operation, or can be implemented with a combination of dedicated hardware and computer instructions.

[0083] Those skilled in the art will appreciate that the technical features described in the various embodiments and / or claims of the present application may be combined and / or coupled in a variety of ways, even if such combinations and / or couplings are not explicitly described in the present application. In particular, without departing from the spirit and teachings of the present application, the technical features described in the various embodiments and / or claims of the present application may be combined and / or coupled in a variety of ways, and all of these combinations and / or couplings fall within the scope of the present application.

[0084] Although the present application has been shown and described with reference to specific exemplary embodiments of the present application, it should be understood by those skilled in the art that various changes in form and details may be made to the present application 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-mentioned 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; The relative poses between the loop frames and the poses of the key frames and the local map are jointly optimized to obtain a final semantic point cloud map.

2. The map positioning and construction method according to claim 1, characterized in that: 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 the relative pose transformation matrix from the source point cloud to the target point cloud.

3. The map positioning and construction method according to claim 2, characterized in that: 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.

4. The map positioning and construction method according to claim 2, 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: ； in, Indicates that the number of the Euclidean nearest neighbor point set is point; represents the mean of the Euclidean nearest neighbor point set; express The covariance matrix of The semantic label information is introduced to optimize the association process of the point cloud. and are single points in the source and target point clouds, respectively, and yes The corresponding points found by the nearest neighbor search algorithm are then the Euclidean distance between the two points is , define a semantic correlation coefficient To evaluate the effectiveness of the association, It can be expressed as: ； in, is a constant less than 1, used to measure the correlation coefficient of semantic label information mismatch; The expression of the semantic association loss function is: ； in, represents the optimal relative pose to be solved; Represents the source point cloud The covariance matrix of express The covariance matrix of the nearest neighbor point in the corresponding target point cloud; Represents a point in the source point cloud and Point The Euclidean distance between the nearest neighbor points in the corresponding target point cloud.

5. The map positioning and construction method according to claim 2, 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.

6. The map positioning and construction method according to claim 5, 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.

7. The map positioning and construction method according to claim 6, characterized in that: The performing loop detection on the key frame to obtain the loop frame comprises: Extracting a point cloud with continuous 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.

8. The map positioning and construction method according to claim 7, characterized in that: The method for constructing a loop descriptor that integrates semantic and geometric information includes: Using the Scan-Context segmentation method, the point cloud with continuous invariance is Each point in Convert Cartesian coordinates to polar coordinates and get ;in, express The horizontal distance from the point to the laser radar, represents the horizontal azimuth, Indicates the pitch angle in the vertical direction; according to and Point Cloud Divide the ring into sectors and get the total Ring sector blocks; each ring sector block The expression is: ； in, and Respectively represent the number of each ring sector block; subscript Representing point clouds The serial number, Indicates the serial number is The height information of the point, Indicates the serial number is The semantic label information of the point, Indicates the serial number is The horizontal distance from the point to the laser radar, Indicates the serial number is The azimuth of the point in the horizontal direction; Indicates the maximum scanning radius of the laser radar; Traverse each ring sector block The height value of the point cloud in the point cloud is found. , and encode the matrix according to the height Assign a value. If no point falls into the current ring sector block, let the height encoding matrix , according to this method, we can get the highly coded matrix ; Traverse each ring sector block Semantic label information of the point cloud in , find the label with the highest priority , and encode the matrix according to the height Assign a value. If no point falls into the current ring fan block, let the semantic encoding matrix According to this method, the semantic coding matrix is ​​obtained ; Create a size The three-dimensional matrix ,exist One of the layers is stored in the highly encoded matrix , store the semantic encoding matrix in another layer , and finally obtain the joint encoding matrix that integrates height information and semantic information , whose expression is: 。 9. The map positioning and construction method according to claim 8, characterized in that: The algorithm for evaluating the similarity of coding matrices includes: ； in, Represents the similarity of two joint coding matrices; and Respectively represent the joint encoding matrices corresponding to the source point cloud and the target point cloud; and Respectively represent the height encoding matrices corresponding to the source point cloud and the target point cloud; and Represent the semantic encoding matrices corresponding to the source point cloud and the target point cloud respectively; and Respectively represent the similarity between height encoding matrices and the similarity between semantic encoding matrices; Represents the similarity weight coefficient; ； in, is a semantic relevance function used to evaluate the relationship between two ring sectors, and its expression is: 。 10. The map positioning and construction method according to claim 7, 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 the 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