Laser positioning method and system based on building wall feature extraction and matching

Abstract

The present invention provides a laser positioning method and system based on building wall feature extraction and matching, including: step 1: collecting real-time point cloud and posture; step 2: filtering the real-time point cloud to obtain real-time wall point cloud; step 3: converting the real-time wall point cloud to the UTM coordinate system through the posture, filtering the ground point cloud, and obtaining the map wall point cloud after clustering and segmentation; step 4: using the local neighborhood search method and the normal vector to match the map wall point cloud and the real-time wall point cloud features; step 5: using the three-point method to construct a nonlinear optimization target according to the distance from the point to the surface, and obtain the wall posture that meets the preset requirements. The present invention uses the wall features for rapid laser positioning, which improves the stability and accuracy of positioning.

Description

Technical Field

[0001] The present invention relates to the field of laser positioning technology, and in particular to a laser positioning method and system based on building wall feature extraction and matching. Background Art

[0002] High-precision positioning for autonomous driving is an extremely important part of autonomous driving, and in scenes where GPS signals are blocked, LiDAR point cloud features can play a good auxiliary positioning role. Features commonly used in positioning methods based on LiDAR point clouds include line and surface features of the entire point cloud in the scene, manually placed features, and features extracted based on deep learning methods. When the positioning performance of the full point cloud line and surface features is weakened, it is difficult to track the problem to which type of feature, which increases the difficulty of problem solving. Manually placed features require a lot of manpower and material resources in the early stage, and deep learning methods rely on hardware and models, which increases the complexity of positioning.

[0003] Patent document CN110926474A (application number: CN201911188082.6) discloses a satellite / vision / laser combined urban canyon environment UAV positioning and navigation method, which belongs to the technical field of measurement and testing. The method uses an omnidirectional infrared camera to obtain sky images, extracts part of the data in the 3D city information to build an ideal urban skyline database, obtains the horizontal position information of the drone by comparing the building skyline information extracted from the infrared image with the ideal urban skyline database, uses laser ranging to determine the height information of the drone, and constructs a building boundary sky map based on the initial position information of the drone and the building boundary information. The building boundary sky map and the satellite sky map are superimposed to mine the multi-path judgment rule, and the final position information of the drone is solved, avoiding interference caused by light such as the weakening of light at night, reducing the complexity of calculation, and image processing feature extraction is easy to implement, which can reduce the error and calculation amount caused by direct scene matching, and can meet the needs of drone all-weather flight and operation. However, this patent cannot solve the current technical problems.

[0004] Therefore, positioning different features separately and assigning different weights to them in the filter according to the scene is a simple and easy-to-adjust and analyze method. For example, lane lines and curbs can be applied to most road scenes, and tree trunks and other rod-shaped objects can be applied to scenes where GPS signals are blocked by trees. There are also many scenes where GPS signals are blocked by buildings on roads under high-rise buildings or viaducts in cities. In this scene, using building wall features to participate in positioning can improve positioning performance. Therefore, the present invention proposes a method for laser positioning using building wall features. Summary of the invention

[0005] In view of the defects in the prior art, the object of the present invention is to provide a laser positioning method and system based on building wall feature extraction and matching.

[0006] In a first aspect, the present invention provides a laser positioning method based on building wall feature extraction and matching, comprising:

[0007] Step 1: Collect real-time point cloud and pose;

[0008] Step 2: Filter the real-time point cloud to obtain the real-time wall point cloud;

[0009] Step 3: Convert the real-time wall point cloud to the UTM coordinate system through pose, filter the ground point cloud, and obtain the map wall point cloud after clustering and segmentation;

[0010] Step 4: Use the local neighborhood search method and normal vector to match the map wall point cloud and the real-time wall point cloud features;

[0011] Step 5: Use the three-point method to construct a nonlinear optimization target based on the distance from the point to the surface to obtain the wall posture that meets the preset requirements.

[0012] Preferably, step 2 comprises:

[0013] Step 2.1: Filter out distant point clouds and some ground point clouds in the real-time point cloud by presetting the height and distance thresholds;

[0014] Step 2.2: Sort the single Ring point cloud according to the scanning order through the column feature of the point cloud, put the points with adjacent point distance less than the threshold in the same cluster, put the points greater than the threshold in another cluster, cluster the single Ring point cloud into multiple point cloud clusters, remove the point cloud with less than 5 cluster points and the distance from the cluster start to the end point less than 50cm, and calculate the curvature of a single point cloud cluster. The calculation method is: take the cluster center point P as the center point of the cluster. c With the starting point P a , end point P b The sine of the angle between<AC,BC> Value, calculate curvature Curvity = 2*sin<AC,BC> / ||BC||, if the curvature is less than the threshold, the point cloud cluster tends to be linear and is used as a candidate point cloud for the wall point cloud;

[0015] Step 2.3: Eliminate the ground point cloud by neighborhood height difference, convert the real-time point cloud from a 3D point cloud into a 2D image point cloud, save the height information in the image pixel intensity by downsampling, insert the 2D image point cloud into the kd-tree, traverse the 2D image point cloud, search for neighborhood points within 40 cm in the kd-tree, and calculate the maximum height difference in the neighborhood. If the height difference is less than the threshold, there is only a ground point within 40 cm of the point, and it is eliminated;

[0016] Step 2.4: Based on the point cloud features and neighborhood scanning, segment the two-dimensional line point cloud. First, downsample the filtered two-dimensional image point cloud in Step 2.3, then perform Euclidean clustering, and restore the two-dimensional points of each cluster to their original resolution. For each cluster, perform SVD decomposition, calculate the ratio R of the maximum eigenvalue to the second-largest eigenvalue. According to the R value, divide the two-dimensional point cloud into: pure line point cloud with R > 1000, secondary line point cloud with 200 < R < 1000, possible line point cloud, broken line point cloud, or line point cloud doped with a large number of miscellaneous points with R < 200. Use neighborhood scanning to segment the broken line point cloud and the line point cloud after filtering out the miscellaneous points. Insert the entire point cloud cluster with R < 1000 into the kd-tree. For each point cloud in the entire point cloud cluster, search for neighborhood points within 70 cm in the kd-tree. Perform SVD decomposition on the found neighborhood point cloud. If the neighborhood is a line point cloud and the ratio R of its maximum eigenvalue to the second-largest eigenvalue is > 200, obtain the normal vector of the neighborhood line point cloud, and scan the real-time point cloud cluster in the direction of this neighborhood line. If the distance from each scanned point to the line is less than the threshold, insert the real-time point cloud point into the line point cloud cluster and record the id of this point;

[0017] Step 2.5: Restore the three-dimensional point cloud through the 3D points within the neighborhood of the straight line two-dimensional image point cloud cluster, and remove the non-wall point cloud to obtain the wall three-dimensional point cloud. Put the original three-dimensional point cloud into a two-dimensional grid of 64 * 32. Search for the three-dimensional points within 20 cm in the corresponding grid for each point of the two-dimensional point cloud to obtain the corresponding three-dimensional point cloud. If the normal vector of the cluster is perpendicular to the ground normal vector G, it is determined to be a wall. For each cluster, perform SVD decomposition, and the eigenvector V corresponding to the minimum eigenvalue is the normal vector of the cluster. The cluster with the included angle cos<V, G> greater than the threshold is recognized as the wall three-dimensional point cloud, which is the obtained real-time wall point cloud.

[0018] Preferably, the said Step 3 includes:

[0019] Step 3.1: Transform the real-time wall point cloud to the utm coordinate system through pose transformation, and splice the map wall point clouds of all frames into a wall map;

[0020] Step 3.2: Perform hierarchical extraction on the overall wall map;

[0021] Step 3.3: Perform Euclidean clustering on the real-time wall point cloud of each layer, and remove the clusters with the number of points less than the threshold and the maximum length less than the threshold;

[0022] Step 3.4: For each cluster, use neighborhood scanning to segment out a single line point cloud cluster;

[0023] Step 3.5: For each line point cloud cluster, restore it to a wall point cloud;

[0024] Step 3.6: Merge multiple layers of wall point clouds belonging to the same wall to obtain the map wall point cloud.

[0025] Preferably, step 4 comprises:

[0026] Step 4.1: Downsample all map wall point clouds by 0.65m, then store them in Kd-tree, and save the map id in the intensity value of the point;

[0027] Step 4.2: For the real-time frame point cloud in the UTM coordinate system, determine a search point every 0.7m, search all map points within the 1m neighborhood threshold in the Kd-tree, and obtain the wall map ID based on the intensity value, and finally obtain the matching pairs of the real-time frame and multiple map walls;

[0028] Step 4.3: Calculate the real-time wall direction vector V in the utm coordinate system 0 The vector angle cos with multiple matching wall direction vectors Vi <V 0 ,Vi>, where the direction vector is calculated as the eigenvector corresponding to the maximum eigenvalue after SVD decomposition of the point cloud cluster;

[0029] Step 4.4: Select the matching pair corresponding to the minimum value of the angle cosine absolute value less than 0.1 as the wall feature matching pair.

[0030] Preferably, step 5 comprises:

[0031] Step 5.1: Transform the matching map wall point cloud into the vehicle coordinate system through pose, and insert it into the kd-tree. In the first iteration, the input pose is noisy. In the second and subsequent iterations, the pose is the result of the last nonlinear optimization.

[0032] Step 5.2: For each point cloud P of the real-time frame wall 0 , projected onto the matching map wall in the vehicle coordinate system, and obtain the projection point P t ;

[0033] Step 5.3: Projection point P t Search all neighboring threshold points within 1m in the kd-tree and get point P t The matching pair P with the local surface point D t -;

[0034] Step 5.4: Randomly select three non-collinear points within the local surface point D and at least 40 cm apart {P 1 ,P 2 ,P 3} baselink represents the D plane;

[0035] Step 5.5: Match the pair P 0 -{P1 ,P 2 ,P 3} utm Construct the Factor of the point-to-surface error in Ceres, and convert {P1,P2,P3} utm Transform the pose to be optimized T[R|t] to {P1, P2, P3} baselink , calculate the normal vector n of the map plane = (P 1 -P 2 )×(P 2 -P 3 ), calculate the distance d from the point in the real-time frame to the map plane = (P 0 -P 1 )×n;

[0036] Step 5.6: Ceres optimizes the point-to-surface error and iterates multiple times to obtain the pose for wall positioning.

[0037] In a second aspect, the present invention provides a laser positioning system based on building wall feature extraction and matching, comprising:

[0038] Module M1: collect real-time point cloud and pose;

[0039] Module M2: Filter the real-time point cloud to obtain the real-time wall point cloud;

[0040] Module M3: transform the real-time wall point cloud into the UTM coordinate system through posture, filter the ground point cloud, and obtain the map wall point cloud after clustering and segmentation;

[0041] Module M4: Use local neighborhood search method and normal vector to match the map wall point cloud and real-time wall point cloud features;

[0042] Module M5: Use the three-point method to construct a nonlinear optimization target based on the distance from the point to the surface to obtain the wall posture that meets the preset requirements.

[0043] Preferably, the module M2 comprises:

[0044] Module M2.1: Filter out distant point clouds and some ground point clouds in the real-time point cloud by presetting the height and distance thresholds;

[0045] Module M2.2: Sort a single Ring point cloud according to the scanning order through the column feature of the point cloud, put the points with adjacent point distances less than the threshold in the same cluster, put the points greater than the threshold in another cluster, cluster a single Ring point cloud into multiple point cloud clusters, remove the point cloud with less than 5 cluster points and the distance from the cluster start to the end point less than 50cm, and calculate the curvature of a single point cloud cluster. The calculation method is: take the cluster center point P as the center point of the cluster. c With the starting point Pa and the endpoint P b calculate the sine value of the included angle between them, sin<AC,BC>, and calculate the curvature Curvity = 2 * sin<AC,BC> / ||BC||. If the curvature is less than the threshold, the point cloud cluster tends to be linear and is used as a candidate point cloud for the wall point cloud;

[0046] Module M2.3: Remove the ground point cloud through the neighborhood height difference. Convert the real-time point cloud from a three-dimensional point cloud into a two-dimensional image point cloud. Save the height information in the intensity of the image pixel points through downsampling. Insert the two-dimensional image point cloud into the kd-tree. Traverse the two-dimensional image point cloud, search for neighborhood points within 40 cm in the kd-tree, and calculate the maximum height difference within this neighborhood. If the height difference is less than the threshold, there are only ground points within 40 cm of this point, and it is removed;

[0047] Module M2.4: Segment the two-dimensional line point cloud based on point cloud features and neighborhood scanning. First, downsample the filtered two-dimensional image point cloud in Module M2.3, then perform Euclidean clustering, and restore the two-dimensional points of each cluster to their original resolution. Perform SVD decomposition on each cluster, calculate the ratio R of the maximum eigenvalue to the second-largest eigenvalue. According to the R value, the two-dimensional point cloud is divided into: pure line point cloud with R > 1000, secondary line point cloud with 200 < R < 1000, possible line point cloud, broken line point cloud, or line point cloud doped with a large number of miscellaneous points with R < 200. Use neighborhood scanning to segment the broken line point cloud and the line point cloud after filtering out the miscellaneous points. Insert the entire point cloud cluster with R < 1000 into the kd-tree. For each point cloud in the entire point cloud cluster, search for neighborhood points within 70 cm in the kd-tree. Perform SVD decomposition on the found neighborhood point cloud. If the neighborhood is a line point cloud and the ratio of its maximum eigenvalue to the second-largest eigenvalue R > 200, obtain the normal vector of the neighborhood line point cloud, and scan the real-time point cloud cluster in the direction of this neighborhood line. If the distance from each scanned point to the line is less than the threshold, insert the real-time point cloud point into the line point cloud cluster and record the id of this point;

[0048] Module M2.5: Restore the three-dimensional point cloud through the 3D points within the neighborhood of the straight line two-dimensional image point cloud cluster, and remove the non-wall point cloud to obtain the wall three-dimensional point cloud. Put the original three-dimensional point cloud into a two-dimensional 64 * 32 grid. Search for 3D points within 20 cm in the corresponding grid for each point of the two-dimensional point cloud to obtain the corresponding three-dimensional point cloud. If the normal vector of the cluster is perpendicular to the ground normal vector G, it is determined to be a wall. Perform SVD decomposition on each cluster, and the eigenvector V corresponding to the minimum eigenvalue is the normal vector of the cluster. The cluster with the included angle cos<V,G> greater than the threshold is recognized as the wall three-dimensional point cloud, which is the obtained real-time wall point cloud.

[0049] Preferably, the module M3 includes:

[0050] Module M3.1: Convert the real-time wall point cloud to the UTM coordinate system through pose, and stitch the wall point clouds of all frames into a wall map;

[0051] Module M3.2: Layered extraction of the overall wall map;

[0052] Module M3.3: Euclidean clustering of each layer of real-time wall point cloud, and remove clusters with less than the threshold number of points and less than the threshold maximum length;

[0053] Module M3.4: For each cluster, use neighborhood scanning to segment a single line point cloud cluster;

[0054] Module M3.5: Cluster each line point cloud and restore it to wall point cloud;

[0055] Module M3.6: Merge multiple layers of wall point clouds belonging to the same wall to obtain a map wall point cloud.

[0056] Preferably, the module M4 comprises:

[0057] Module M4.1: downsample all map wall point clouds by 0.65m, then store them in Kd-tree, and save the map id in the intensity value of the point;

[0058] Module M4.2: For the real-time frame point cloud in the UTM coordinate system, a search point is determined every 0.7m, and all map points within the 1m neighborhood threshold are searched in the Kd-tree. The wall map ID is obtained based on the intensity value, and finally the matching pairs of the real-time frame and multiple map walls are obtained;

[0059] Module M4.3: Calculate the real-time wall direction vector V in the utm coordinate system 0 The vector angle cos with multiple matching wall direction vectors Vi <V 0 ,Vi>, where the direction vector is calculated as the eigenvector corresponding to the maximum eigenvalue after SVD decomposition of the point cloud cluster;

[0060] Module M4.4: Select the matching pair corresponding to the minimum value of the angle cosine absolute value less than 0.1 as the wall feature matching pair.

[0061] Preferably, the module M5 comprises:

[0062] Module M5.1: Transform the matching map wall point cloud into the vehicle coordinate system through pose, and insert it into the kd-tree. In the first iteration, the input pose is noisy. In the second and subsequent iterations, the pose is the result of the last nonlinear optimization.

[0063] Module M5.2: For each point cloud P of the real-time frame wall 0, projected onto the matching map wall in the vehicle coordinate system, and obtain the projection point P t ;

[0064] Module M5.3: Projection point P t Search all neighboring threshold points within 1m in the kd-tree and get point P t The matching pair P with the local surface point D t -;

[0065] Module M5.4: Randomly select three non-collinear points within the local surface point D and at least 40 cm apart {P 1 ,P 2 ,P 3} baselink represents the D plane;

[0066] Module M5.5: Match the P 0 -{P 1 ,P 2 ,P 3} utm Construct the Factor of the point-to-surface error in Ceres, and convert {P1,P2,P3} utm Transform the pose to be optimized T[R|t] to {P1, P2, P3} baselink , calculate the normal vector n of the map plane = (P 1 -P 2 )×(P 2 -P 3 ), calculate the distance d from the point in the real-time frame to the map plane = (P 0 -P 1 )×n;

[0067] Module M5.6: Ceres optimizes the point-to-surface error and iterates multiple times to obtain the pose for wall positioning.

[0068] Compared with the prior art, the present invention has the following beneficial effects:

[0069] The present invention proposes a method for laser positioning using building wall features. In urban roads under high-rise buildings or viaducts, there are many scenes where GPS signals are blocked by buildings. In such scenes, using building wall features to participate in positioning can improve positioning performance; using wall features for rapid laser positioning improves positioning stability and accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Other features, objects and advantages of the present invention will become more apparent from the detailed description of non-limiting embodiments made with reference to the following drawings:

[0071] Figure 1 This is an overall flow chart of the laser positioning method of the present invention;

[0072] Figure 2 It is a flowchart for real-time wall feature extraction;

[0073] Figure 3 This is a schematic diagram of preliminary screening based on radar characteristic point cloud;

[0074] Figure 4 Provide a flowchart for line point cloud extraction;

[0075] Figure 5 Provide a flowchart for line point cloud extraction;

[0076] Figure 6 It is the feature matching flow chart;

[0077] Figure 7 It is a nonlinear optimization flow chart;

[0078] Figure 8 It is a point cloud map of the wall surface;

[0079] Fig. 9 Comparison diagram of surface and real-time point cloud;

[0080] Fig.10 It is a comparison chart between the real-time frame wall point cloud and the real-time point cloud;

[0081] Fig.11 This is the error comparison diagram of wall positioning;

[0082] Fig.12 Trajectory comparison diagram for wall positioning. DETAILED DESCRIPTION

[0083] The present invention is described in detail below in conjunction with specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but are not intended to limit the present invention in any form. It should be noted that, for those of ordinary skill in the art, several changes and improvements can also be made without departing from the concept of the present invention. These all belong to the protection scope of the present invention.

[0084] Embodiment 1:

[0085] The present invention provides a laser positioning method based on building wall feature extraction and matching, such as Figure 1 As shown, the following steps are included:

[0086] Step 1: Collect real-time point cloud and pose;

[0087] Step 2: Filter the real-time point cloud to obtain the real-time wall point cloud;

[0088] Step 3: Convert the real-time wall point cloud to the UTM coordinate system through pose, filter the ground point cloud, and obtain the map wall point cloud after clustering and segmentation;

[0089] Step 4: Use the local neighborhood search method and normal vector to match the map wall point cloud and the real-time wall point cloud features;

[0090] Step 5: Use the three-point method to construct a nonlinear optimization target based on the distance from the point to the surface to obtain the wall posture that meets the preset requirements.

[0091] The input of the present invention is a real-time dedistorted point cloud and a pose with noise (within 70cm), a map wall point cloud generated offline by the Hesai radar point cloud and the mapping pose, and the output is the optimized vehicle pose. The present invention has four parts: map wall extraction, real-time wall extraction, wall feature matching and nonlinear optimization. The real-time point cloud is filtered by Ring and Column, ground point filtering, line point cloud separation and wall point filtering to obtain a real-time wall point cloud. The real-time wall point cloud (collected by the mapping vehicle) is converted to the UTM coordinate system through the mapping pose, and then the ground is filtered and clustered to obtain an offline map wall point cloud. After wall feature matching and nonlinear optimization, a high-precision vehicle pose can be obtained, in which the present invention only optimizes the three degrees of freedom of x, y and yaw. Each part is explained in detail below.

[0092] Part 1: Real-time wall automatic extraction process Figure 2 shown.

[0093] Step 1: The real-time point cloud is filtered out of distant points and some ground points through height and distance thresholds;

[0094] Step 2: The process and corresponding schematic diagram of the preliminary screening of point clouds using the Ring and Column characteristics of the LiDAR are as follows: Figure 3 .

[0095] Step 2.1: Use the column feature of the point cloud to sort the single Ring point cloud according to the order of scanning;

[0096] Step 2.2: Set the distance Δd between adjacent points to be smaller than the threshold (such as Figure 3 Δd 1 ) are placed in the same cluster S 1 , and is greater than the threshold (such as Figure 3 Δd 2 ) points in another cluster, a single Ring can be clustered into multiple point cloud clusters. This method is more time-saving than Euclidean clustering;

[0097] Step 2.3: Eliminate clusters with less than 5 points and a length (the distance from the cluster start point to the end point) less than 50 cm;

[0098] Step 2.4: Calculate the curvature of a single point cloud cluster. The specific calculation method is: take the cluster center point P cWith the starting point P a , the ending point P b , calculate the curvature using the sine value of the included angle sin<AC,BC>:

[0099] Curvity = 2 * sin<AC,BC> / ||BC||

[0100] If the curvature is less than the threshold, it means that the point cloud cluster tends to be linear and can be used as a candidate point cloud for the wall point cloud.

[0101] By the above steps, at least 45% of the miscellaneous points can be removed, saving a large amount of subsequent processing time.

[0102] Step 3: In Step 1, 90% of the ground points can be filtered by height, but a small number of distant ground points cannot be filtered. Among them, the ground points close to the wall in the distance have the greatest impact on wall extraction and need to be removed separately. The specific method of using the neighborhood height difference method in the present invention to remove the ground point cloud is as follows:

[0103] Step 3.1: Convert the point cloud from three-dimensional points to two-dimensional image points (z = 0), and after downsampling, save the height information in the intensity of the image pixel points;

[0104] Step 3.2: Insert the two-dimensional point cloud into the kd-tree;

[0105] Step 3.3: Traverse each two-dimensional point, search for neighboring threshold points within 40 cm in the kd-tree, and calculate the maximum height difference within this neighboring threshold;

[0106] Step 3.4: If the height difference is less than the threshold, then there are only ground points within the 40 cm neighboring threshold of this point, and it can be removed.

[0107] Step 4: The specific flowchart of using the principal component (point cloud feature) and point neighborhood scanning method in the present invention to segment the two-dimensional line point cloud is as Figure 4 shown.

[0108] Step 4.1: Since the Euclidean clustering of the original two-dimensional point cloud is extremely time-consuming, the present invention first downsamples the two-dimensional point cloud (preferably to 70 cm), then performs Euclidean clustering, and restores the two-dimensional points of each cluster to the original resolution. Compared with directly performing Euclidean clustering on the original two-dimensional point cloud, 80% of the time can be saved.

[0109] Step 4.2: Perform SVD decomposition for each cluster, and calculate the ratio R of the maximum eigenvalue to the second largest eigenvalue;

[0110] Step 4.3: Divide the two-dimensional point cloud into three categories according to the R value: pure line point cloud (R > 1000), secondary line point cloud (200 < R < 1000), and possible line point cloud (R < 200);

[0111] Step 4.4: For point clouds with R ≥ 1000, since the direction of the principal component is much larger than the secondary component (the maximum eigenvalue is much larger than the second largest eigenvalue), it can be directly used as a linear point cloud;

[0112] Step 4.5: For point clouds with R < 200, the point cloud cluster may be a non-line point cloud, a broken line point cloud, or a line point cloud mixed with a large number of clutter point clouds;

[0113] Step 4.6: The point cloud with 200≤R<1000 can be considered as a line point cloud with a small amount of miscellaneous point clouds;

[0114] Step 4.7: The specific extraction process of segmenting the polyline point cloud using the point neighborhood scanning method and filtering out the line point cloud after the noise is as follows:

[0115] Step 4.7.1: Insert the entire R < 1000 point cloud cluster into the kd-tree;

[0116] Step 4.7.2: For each point in the entire point cloud cluster (excluding points with recorded IDs), find the neighboring points within 70cm in the kd-tree;

[0117] Step 4.7.3: Perform SVD decomposition on the found neighborhood point cloud. If the neighborhood is a line point cloud, the ratio of its maximum eigenvalue to the second largest eigenvalue is R>200.

[0118] Step 4.7.4: Get the normal vector of the neighborhood line point cloud, and scan the real-time point cloud cluster with the neighborhood line as the direction. If the distance from each scanned point (not scanning points with recorded IDs) to the line is less than the threshold, the real-time point cloud point can be inserted into the line point cloud cluster and the ID of the point is recorded.

[0119] Through the above method, each point cloud cluster can be divided into multiple straight line point cloud clusters, and each straight line point cloud cluster does not contain interfering noise points.

[0120] Step 5: The present invention restores the 3D point cloud through the 3D points in the neighborhood of the linear 2D point cloud cluster, and removes the non-wall point cloud to obtain the 3D point of the wall. The specific process is as follows:

[0121] Step 5.1: To save search time, put the original 3D point cloud into a 2D 64*32 grid;

[0122] Step 5.2: Search for a 3D point within 20 cm for each point in the 2D point cloud in the corresponding grid to obtain the corresponding 3D point cloud;

[0123] Step 5.3: There are some noise points in the 3D point cloud that need to be removed. If the normal vector of the cluster is perpendicular to the ground normal vector G, it can be determined to be a wall. For each cluster SVD decomposition, the eigenvector V corresponding to the minimum eigenvalue is the normal vector of the cluster, and the angle cos<V,G> The clusters with a value greater than the threshold th (0.98) are identified as the three-dimensional wall point cloud, which is the real-time wall point cloud obtained.

[0124] Part II: Flowchart of automatic extraction and clustering of offline map wall point clouds using point neighborhood scanning method Figure 5 shown.

[0125] The specific steps are:

[0126] Step 1: Convert the wall point cloud extracted by the mapping vehicle in real time into the UTM coordinate system through the mapping pose, and splice the wall point clouds of all frames into a wall map;

[0127] Step 2: Since part of the wall surface may be a non-wall surface in a certain layer, it is necessary to extract the entire wall surface in layers;

[0128] Step 3: Euclidean clustering is performed on each layer of point cloud, and clusters with less points than the threshold and with a maximum length less than the threshold are eliminated;

[0129] Step 4: For each two-dimensional cluster, use the point neighbor threshold scanning method (the specific method is the same as Figure 4 ) Segment out single line point cloud clusters;

[0130] Step 5: Cluster each line point cloud and restore it to wall points;

[0131] Step 6: Merge multiple layers of wall points belonging to the same wall to obtain a separate cluster for each wall in the point cloud map.

[0132] The above method can be used to segment the offline wall point cloud map into a single wall cluster without clutter.

[0133] Part III: The flow chart of the present invention using local neighborhood search and normal vector method to match map wall and real-time frame wall features is as follows Figure 6 .

[0134] The specific steps are as follows:

[0135] Step 1: Feature matching only needs to obtain the matching map ID. To save time, all map walls are downsampled by 0.65m and inserted into the Kd-Tree, and the map ID is saved in the intensity value of the point.

[0136] Step 2: For the real-time frame W in the UTM coordinate system 0, determine a search point every 0.7m, search all map points within the 1m neighborhood threshold in the Kd-Tree, and obtain the wall map ID based on the intensity value. Finally, the real-time frame W is obtained. 0 With multiple map walls {W i} matching pairs;

[0137] Step 3: Calculate the real-time wall (UTM coordinate system) direction vector V 0 Match multiple wall direction vectors V i The vector angle cos <V 0 ,V i >, where the direction vector is calculated as the eigenvector corresponding to the maximum eigenvalue after SVD decomposition of the point cloud cluster;

[0138] Step 4: Select the matching pair (most parallel) with the minimum angle cosine absolute value less than 0.1 as the wall feature matching pair W 0 —W i .

[0139] Part 4: The present invention uses the three-point method to construct a flow chart of the nonlinear optimization target based on the distance from the point to the surface. Figure 7 shown.

[0140] The specific process is as follows:

[0141] Step 1: Convert the matching map wall to the baselink (vehicle coordinate system) coordinate system through the input pose and insert it into the kd-tree. If it is the first iteration, the pose is the pose with noise input by the positioning system. For the second and subsequent iterations, the pose is the result of the last nonlinear optimization.

[0142] Step 2: For each point P on the wall of the real-time frame 0 , projected onto the matching map wall (baselink system) to obtain the projection point pt;

[0143] Step 3: The projection point pt searches for all neighboring threshold points within 1m in the kd-tree to obtain a matching pair pt-D between the point pt and the local surface point D;

[0144] Step 4: Randomly select three non-collinear points in the local surface point D that are at least 40 cm apart {P 1 ,P 2 ,P 3} baselink represents the D plane;

[0145] Step 5: Match the pair P 0 —{P 1 ,P 2 ,P 3} utmConstruct the Factor of the point-to-surface error in Ceres. The specific construction process is:

[0146] Step 5.1: {P 1 ,P 2 ,P 3} utm By transforming the pose to be optimized T[R|t] into {P 1 ,P 2 ,P 3} baselink ;

[0147] Step 5.2: Calculate the normal vector n of the map plane = (P 1 -P 2 )×(P 2 -P 3 );

[0148] Step 5.3: Calculate the distance d from the point in the real-time frame to the map plane = (P 0 -P 1 )×n;

[0149] Step 6: Ceres optimizes the point-to-surface error and iterates multiple times to obtain the pose of the wall.

[0150] The wall point cloud map extracted by the present invention is as follows: Figure 8 As shown in the figure, the comparison with the real-time point cloud map is as follows Fig. 9 shown.

[0151] The comparison between the wall point cloud extracted by real-time frame and the real-time point cloud is shown in the figure below. Fig.10 shown.

[0152] The error comparison chart using wall positioning is as follows Fig.11 As shown in the figure. The error in the x direction (the input error is InputError_x, and the error of wall positioning is Wall_Location error_x) is the forward error of the vehicle, and the error in the y direction (the input error is InputError_y, and the error of wall positioning is Wall_Location error_y) is the error on both sides of the vehicle. The true value of the evaluation error is the pose of the map.

[0153] Depend on Fig.11 It can be seen that except for a few places with large positioning errors due to the lack of x-direction constraints (few and short walls), the x-positioning errors of other positions are mostly within 10 cm and smaller than the input error. The y-direction positioning error is controlled within 15 cm, and the actual test single frame time is also controlled within 75 ms.

[0154] The trajectory comparison of a test on a certain section of road is as follows Fig.12 As shown. Fig.12It can be seen that the poses of the overall positioning frame and wall positioning are close to those of RTK.

[0155] Embodiment 2:

[0156] The present invention also provides a laser positioning system based on building wall feature extraction and matching. The laser positioning system based on building wall feature extraction and matching can be realized by executing the process steps of the laser positioning method based on building wall feature extraction and matching, that is, those skilled in the art can understand the laser positioning method based on building wall feature extraction and matching as a preferred implementation of the laser positioning system based on building wall feature extraction and matching.

[0157] The laser positioning system based on building wall feature extraction and matching provided by the present invention comprises: module M1: collecting real-time point cloud and posture; module M2: filtering the real-time point cloud to obtain real-time wall point cloud; module M3: converting the real-time wall point cloud into a UTM coordinate system through posture, filtering the ground point cloud, and obtaining a map wall point cloud after clustering and segmentation; module M4: using a local neighborhood search method and a normal vector to match the map wall point cloud and the real-time wall point cloud features; module M5: using a three-point method to construct a nonlinear optimization target according to the distance from the point to the surface, and obtaining a wall posture that meets the preset requirements.

[0158] The module M2 includes: module M2.1: filtering out distant point clouds and some ground point clouds in the real-time point cloud by presetting the height and distance thresholds; module M2.2: sorting a single Ring point cloud according to the scanning order by using the column characteristics of the point cloud, placing points with adjacent point distances less than the threshold in the same cluster, placing points with distances greater than the threshold in another cluster, clustering a single Ring point cloud into multiple point cloud clusters, eliminating point clouds with less than 5 cluster points and a distance from the cluster start point to the end point less than 50 cm, and calculating the curvature Curvity of a single point cloud cluster. The calculation method is: taking the cluster center point P c With the starting point P a , end point P bThe sine value of the included angle, sin<AC,BC>, is calculated, and the curvature Curvity = 2 * sin<AC,BC> / ||BC||. If the curvature is less than the threshold, the point cloud cluster tends to be linear and is used as a candidate point cloud for the wall point cloud; Module M2.3: Remove the ground point cloud through the height difference in the neighborhood. Convert the real-time point cloud from a three-dimensional point cloud into a two-dimensional image point cloud. Preserve the height information in the intensity of the image pixel points through downsampling. Insert the two-dimensional image point cloud into the kd-tree. Traverse the two-dimensional image point cloud, search for neighborhood points within 40 cm in the kd-tree, and calculate the maximum height difference within this neighborhood. If the height difference is less than the threshold, there are only ground points within 40 cm of this point, and it is removed; Module M2.4: Segment the two-dimensional line point cloud based on point cloud features and neighborhood scanning. First, downsample the filtered two-dimensional image point cloud in Module M2.3, then perform Euclidean clustering, and restore the two-dimensional points of each cluster to their original resolution. Perform SVD decomposition for each cluster, calculate the ratio R of the maximum eigenvalue to the second-largest eigenvalue. According to the R value, the two-dimensional point cloud is divided into: pure line point cloud with R > 1000, secondary line point cloud with 200 < R < 1000, possible line point cloud, broken line point cloud, or line point cloud doped with a large number of miscellaneous points with R < 200. Use neighborhood scanning to segment the broken line point cloud and the line point cloud after filtering out the miscellaneous points. Insert the entire point cloud cluster with R < 1000 into the kd-tree. For each point cloud in the entire point cloud cluster, search for neighborhood points within 70 cm in the kd-tree. Perform SVD decomposition on the obtained neighborhood point cloud. If the neighborhood is a line point cloud and the ratio R of its maximum eigenvalue to the second-largest eigenvalue is > 200, obtain the normal vector of the neighborhood line point cloud, and scan the real-time point cloud cluster in the direction of this neighborhood line. If the distance from each scanned point to the line is less than the threshold, insert the real-time point cloud point into the line point cloud cluster and record the id of this point; Module M2.5: Restore the three-dimensional point cloud through the 3D points within the neighborhood of the straight line two-dimensional image point cloud cluster, and remove non-wall point clouds to obtain the wall three-dimensional point cloud. Put the original three-dimensional point cloud into a two-dimensional 64 * 32 grid. Search for 3D points within 20 cm in the corresponding grid for each point of the two-dimensional point cloud to obtain the corresponding three-dimensional point cloud. If the normal vector of the cluster is perpendicular to the ground normal vector G, it is determined to be a wall. Perform SVD decomposition for each cluster, and the eigenvector V corresponding to the minimum eigenvalue is the normal vector of the cluster. Clusters with the included angle cos<V,G> greater than the threshold are identified as wall three-dimensional point clouds, which are the obtained real-time wall point clouds.

[0159] The module M3 includes: module M3.1: converting the real-time wall point cloud into the UTM coordinate system through posture, and splicing the map wall point clouds of all frames into a wall map; module M3.2: performing layered extraction on the overall wall map; module M3.3: Euclidean clustering of each layer of real-time wall point cloud, and eliminating clusters with less than a threshold number of points and less than a threshold maximum length; module M3.4: for each cluster, using neighborhood scanning to segment a single line point cloud cluster; module M3.5: for each line point cloud cluster, restoring it to a wall point cloud; module M3.6: merging multiple layers of wall point clouds belonging to the same wall to obtain a map wall point cloud.

[0160] The module M4 includes: module M4.1: downsampling all map wall point clouds by 0.65m, then in Kd-tree, and saving the map ID in the intensity value of the point; module M4.2: for the real-time frame point cloud in the utm coordinate system, determining a search point every 0.7m, searching all map points within the 1m neighborhood threshold in the Kd-tree, and obtaining the wall map ID according to the intensity value, and finally obtaining a matching pair of the real-time frame and multiple map walls; module M4.3: calculating the real-time wall direction vector V in the utm coordinate system 0 The vector angle cos with multiple matching wall direction vectors Vi <V 0 ,Vi>, where the direction vector is calculated as the eigenvector corresponding to the maximum eigenvalue after SVD decomposition of the point cloud cluster; Module M4.4: Select the matching pair corresponding to the minimum value of the angle cosine absolute value less than 0.1 as the wall feature matching pair.

[0161] The module M5 includes: module M5.1: transforming the matching map wall point cloud into the vehicle coordinate system through the pose, and inserting it into the kd-tree. For the first iteration, the input pose is noisy. For the second and subsequent iterations, the pose is the result of the last nonlinear optimization. module M5.2: for each point cloud P of the real-time frame wall 0 , projected onto the matching map wall in the vehicle coordinate system, and obtain the projection point P t ; Module M5.3: Projection point P t Search all neighboring threshold points within 1m in the kd-tree and get point P t The matching pair P with the local surface point D t -; Module M5.4: Randomly select three non-collinear points within the local surface point D and at least 40 cm apart {P 1 ,P 2 ,P 3} baselink Represents D plane; module M5.5: will match P 0 -{P 1 ,P 2 ,P 3}utm Construct the Factor of the point-to-surface error in Ceres, and convert {P1,P2,P3} utm Transform the pose to be optimized T[R|t] to {P1, P2, P3} baselink , calculate the normal vector n of the map plane = (P 1 -P 2 )×(P 2 -P 3 ), calculate the distance d from the point in the real-time frame to the map plane = (P 0 -P 1 )×n; Module M5.6: Ceres optimizes the point-to-surface error and iterates multiple times to obtain the pose of the wall positioning.

[0162] Those skilled in the art know that, in addition to implementing the system, device and its various modules provided by the present invention in a purely computer-readable program code, it is entirely possible to implement the same program in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers and embedded microcontrollers by logically programming the method steps. Therefore, the system, device and its various modules provided by the present invention can be considered as a hardware component, and the modules included therein for implementing various programs can also be considered as structures within the hardware component; the modules for implementing various functions can also be considered as both software programs for implementing the method and structures within the hardware component.

[0163] The above describes the specific embodiments of the present invention. It should be understood that the present invention is not limited to the above specific embodiments, and those skilled in the art can make various changes or modifications within the scope of the claims, which does not affect the essence of the present invention. In the absence of conflict, the embodiments of the present application and the features in the embodiments can be combined with each other arbitrarily.

Claims

1. A laser positioning method based on building wall feature extraction and matching, It is characterized in that include: Step 1: Collect real-time point cloud and pose; Step 2: Filter the real-time point cloud to obtain the real-time wall point cloud; Step 3: Convert the real-time wall point cloud to the UTM coordinate system through pose, filter the ground point cloud, and obtain the map wall point cloud after clustering and segmentation; Step 4: Use the local neighborhood search method and normal vector to match the map wall point cloud and the real-time wall point cloud features; Step 5: Use the three-point method to construct a nonlinear optimization target based on the distance from the point to the surface, and obtain the wall posture that meets the preset requirements; The step 3 comprises: Step 3.1: Convert the real-time wall point cloud to the UTM coordinate system through pose, and stitch the wall point clouds of all frames into a wall map; Step 3.2: Extract the overall wall map in layers; Step 3.3: Euclidean clustering is performed on each layer of real-time wall point cloud, and clusters with less points than a threshold and a maximum length less than a threshold are eliminated; Step 3.4: For each cluster, use neighborhood scanning to segment a single line point cloud cluster; Step 3.5: Cluster each line point cloud and restore it to the wall point cloud; Step 3.6: Merge multiple wall point clouds belonging to the same wall to obtain the map wall point cloud; The step 4 comprises: Step 4.1: Downsample all map wall point clouds by 0.65m, then store them in Kd-tree, and save the map id in the intensity value of the point; Step 4.2: For the real-time frame point cloud in the UTM coordinate system, determine a search point every 0.7m, search all map points within the 1m neighborhood threshold in the Kd-tree, and obtain the wall map ID based on the intensity value, and finally obtain the matching pairs of the real-time frame and multiple map walls; Step 4.3: Calculate the real-time wall direction vector V in the utm coordinate system 0 The vector angle cos with multiple matching wall direction vectors Vi <V 0 ,Vi>, where the direction vector is calculated as the eigenvector corresponding to the maximum eigenvalue after SVD decomposition of the point cloud cluster; Step 4.4: Select the matching pair corresponding to the minimum value of the angle cosine absolute value less than 0.1 as the wall feature matching pair.

2. The laser positioning method based on building wall feature extraction and matching according to claim 1, It is characterized in that The step 2 comprises: Step 2.1: Filter out distant point clouds and some ground point clouds in the real-time point cloud by presetting the height and distance thresholds; Step 2.2: Sort the single Ring point cloud according to the scanning order through the column feature of the point cloud, put the points with adjacent point distance less than the threshold in the same cluster, put the points greater than the threshold in another cluster, cluster the single Ring point cloud into multiple point cloud clusters, remove the point cloud with less than 5 cluster points and the distance from the cluster start to the end point less than 50cm, and calculate the curvature of a single point cloud cluster. The calculation method is: take the cluster center point P as the center point of the cluster. c With the starting point P a , end point P b The sine of the angle between<AC,BC> Value, calculate curvature Curvity = 2*sin<AC,BC> / ||BC||, if the curvature is less than the threshold, the point cloud cluster tends to be linear and is used as a candidate point cloud for the wall point cloud; Step 2.3: Eliminate the ground point cloud by neighborhood height difference, convert the real-time point cloud from a 3D point cloud into a 2D image point cloud, save the height information in the image pixel intensity by downsampling, insert the 2D image point cloud into the kd-tree, traverse the 2D image point cloud, search for neighborhood points within 40 cm in the kd-tree, and calculate the maximum height difference in the neighborhood. If the height difference is less than the threshold, there is only a ground point within 40 cm of the point, and it is eliminated; Step 2.4: Segment the two-dimensional line point cloud based on point cloud features and neighborhood scanning. First, downsample the two-dimensional image point cloud filtered in Step 2.3, then perform Euclidean clustering, and restore the two-dimensional points of each cluster to their original resolution. Perform SVD decomposition on each cluster, calculate the ratio R of the maximum eigenvalue to the second-largest eigenvalue. According to the R value, the two-dimensional point cloud is divided into: pure line point cloud with R > 1000, secondary line point cloud with 200 < R < 1000, possible line point cloud, broken line point cloud, or line point cloud doped with a large number of miscellaneous points with R < 200. Use neighborhood scanning to segment the broken line point cloud and the line point cloud after filtering out miscellaneous points. Insert the entire point cloud cluster with R < 1000 into the kd-tree. For each point cloud in the entire point cloud cluster, search for neighborhood points within 70 cm in the kd-tree. Perform SVD decomposition on the obtained neighborhood point cloud. If the neighborhood is a line point cloud and the ratio R of its maximum eigenvalue to the second-largest eigenvalue is > 200, obtain the normal vector of the neighborhood line point cloud, and scan the real-time point cloud cluster in the direction of this neighborhood line. If the distance from each scanned point to the line is less than the threshold, insert the real-time point cloud point into the line point cloud cluster and record the id of this point; Step 2.5: Restore the three-dimensional point cloud through the 3D points within the neighborhood of the straight-line two-dimensional image point cloud cluster, and remove non-wall point clouds to obtain the wall three-dimensional point cloud. Place the original three-dimensional point cloud into a two-dimensional 64 * 32 grid. Search for three-dimensional points within 20 cm in the corresponding grid for each point of the two-dimensional point cloud to obtain the corresponding three-dimensional point cloud. If the normal vector of the cluster is perpendicular to the ground normal vector G, it is determined to be a wall. Perform SVD decomposition on each cluster, and the eigenvector V corresponding to the minimum eigenvalue is the normal vector of the cluster. Clusters with the included angle cos<V, G> greater than the threshold are identified as wall three-dimensional point clouds, which are the obtained real-time wall point clouds.

3. The laser positioning method based on building wall feature extraction and matching according to claim 1, characterized in that, the said Step 5 includes: Step 5.1: Transform the matching map wall point cloud to the vehicle body coordinate system through pose and insert it into the kd-tree. In the initial iteration, the input pose is with noise. In the second and subsequent iterations, the pose is the result of the previous non-linear optimization; Step 5.2: For each point cloud P of the real-time frame wall 0 , projected onto the matching map wall in the vehicle coordinate system, and obtain the projection point P t ; Step 5.3: Projection point P t Search all neighboring threshold points within 1m in the kd-tree and get point P t The matching pair P with the local surface point D t -D; Step 5.4: Randomly select three non-collinear points within the local surface point D and at least 40 cm apart {P 1 ,P 2 ,P 3 } baselink represents the D plane; Step 5.5: Match the pair P 0 -{P 1 ,P 2 ,P 3 } utm Construct the Factor of the point-to-surface error in Ceres, and convert {P 1 ,P 2 ,P 3 } utm By transforming the pose to be optimized into {P 1 ,P 2 ,P 3 } baselink , calculate the normal vector n of the map plane = (P 1 -P 2 )×(P 2 -P 3 ), calculate the distance d from the point in the real-time frame to the map plane = (P 0 -P 1 )×n; Step 5.6: Ceres optimizes the point-to-plane error and iterates multiple times to obtain the pose of wall positioning.

4. A laser positioning system based on building wall feature extraction and matching, characterized in that, it includes: Module M1: Collect real-time point cloud and pose; Module M2: Filter the real-time point cloud to obtain the real-time wall point cloud; Module M3: Transform the real-time wall point cloud to the utm coordinate system through pose, filter the ground point cloud, and after clustering and segmentation, obtain the map wall point cloud; Module M4: Use the local neighborhood search method and normal vector to match the features of the map wall point cloud and the real-time wall point cloud; Module M5: Use the three-point method to construct a non-linear optimization objective based on the distance from the point to the plane to obtain the wall pose that meets the preset requirements; the said Module M3 includes: Module M3.1: Convert the real-time wall point cloud to the UTM coordinate system through pose, and stitch the wall point clouds of all frames into a wall map; Module M3.2: Layered extraction of the overall wall map; Module M3.3: Euclidean clustering of each layer of real-time wall point cloud, and remove clusters with less than the threshold number of points and less than the threshold maximum length; Module M3.4: For each cluster, use neighborhood scanning to segment a single line point cloud cluster; Module M3.5: Cluster each line point cloud and restore it to wall point cloud; Module M3.6: Merge multiple wall point clouds belonging to the same wall to obtain the map wall point cloud; The module M4 comprises: Module M4.1: downsample all map wall point clouds by 0.65m, then store them in Kd-tree, and save the map id in the intensity value of the point; Module M4.2: For the real-time frame point cloud in the UTM coordinate system, a search point is determined every 0.7m, and all map points within the 1m neighborhood threshold are searched in the Kd-tree. The wall map ID is obtained based on the intensity value, and finally the matching pairs of the real-time frame and multiple map walls are obtained; Module M4.3: Calculate the real-time wall direction vector V in the utm coordinate system 0 The vector angle cos with multiple matching wall direction vectors Vi <V 0 ,Vi>, where the direction vector is calculated as the eigenvector corresponding to the maximum eigenvalue after SVD decomposition of the point cloud cluster; Module M4.4: Select the matching pair corresponding to the minimum value of the angle cosine absolute value less than 0.1 as the wall feature matching pair.

5. The laser positioning system based on building wall feature extraction and matching according to claim 4, It is characterized in that The module M2 comprises: Module M2.1: Filter out distant point clouds and some ground point clouds in the real-time point cloud by presetting the height and distance thresholds; Module M2.2: Sort a single Ring point cloud according to the scanning order through the column feature of the point cloud, put the points with adjacent point distances less than the threshold in the same cluster, put the points greater than the threshold in another cluster, cluster a single Ring point cloud into multiple point cloud clusters, remove the point cloud with less than 5 cluster points and the distance from the cluster start to the end point less than 50cm, and calculate the curvature of a single point cloud cluster. The calculation method is: take the cluster center point P as the center point of the cluster. c With the starting point P a , end point P b The sine of the angle between<AC,BC> Value, calculate curvature Curvity = 2*sin<AC,BC> / ||BC||, if the curvature is less than the threshold, the point cloud cluster tends to be linear and is used as a candidate point cloud for the wall point cloud; Module M2.3: Eliminate ground point clouds by neighborhood height difference, convert real-time point clouds from 3D point clouds into 2D image point clouds, save height information in image pixel intensity by downsampling, insert 2D image point clouds into kd-tree, traverse 2D image point clouds, search for neighborhood points within 40cm in kd-tree, and calculate the maximum height difference in the neighborhood. If the height difference is less than the threshold, there are only ground points within 40cm of the point, which will be eliminated. Module M2.4: Segment the two-dimensional line point cloud based on point cloud features and neighborhood scanning. First, downsample the filtered two-dimensional image point cloud in Module M2.3, then perform Euclidean clustering, and restore the two-dimensional points of each cluster to their original resolution. Perform SVD decomposition on each cluster, calculate the ratio R of the largest eigenvalue to the second largest eigenvalue, and classify the two-dimensional point cloud according to the R value: pure line point cloud with R > 1000, secondary line point cloud with 200 < R < 1000, possible line point cloud, broken line point cloud, or line point cloud doped with a large number of miscellaneous points with R < 200. Use neighborhood scanning to segment the broken line point cloud and the line point cloud after filtering out miscellaneous points. Insert the entire point cloud cluster with R < 1000 into the kd-tree. For each point cloud in the entire point cloud cluster, search for neighborhood points within 70 cm in the kd-tree. Perform SVD decomposition on the obtained neighborhood point cloud. If the neighborhood is a line point cloud and the ratio R of its largest eigenvalue to the second largest eigenvalue is > 200, obtain the normal vector of the neighborhood line point cloud, and scan the real-time point cloud cluster in the direction of this neighborhood line. If the distance from each scanned point to the line is less than the threshold, insert the real-time point cloud point into the line point cloud cluster and record the id of this point; Module M2.5: Restore the three-dimensional point cloud through the 3D points within the neighborhood of the straight line two-dimensional image point cloud cluster, and remove non-wall point clouds to obtain the wall three-dimensional point cloud. Place the original three-dimensional point cloud into a two-dimensional 64 * 32 grid. Search for three-dimensional points within 20 cm in the corresponding grid for each point of the two-dimensional point cloud to obtain the corresponding three-dimensional point cloud. If the normal vector of the cluster is perpendicular to the ground normal vector G, it is determined to be a wall. Perform SVD decomposition on each cluster, and the eigenvector V corresponding to the smallest eigenvalue is the normal vector of the cluster. Clusters with the included angle cos<V, G> greater than the threshold are identified as wall three-dimensional point clouds, which are the obtained real-time wall point clouds.

6. The laser positioning system based on building wall feature extraction and matching according to claim 4, characterized in that, the module M5 includes: Module M5.1: Transform the matching map wall point cloud to the vehicle body coordinate system through pose and insert it into the kd-tree. In the first iteration, the input pose is noisy, and in the second and subsequent iterations, the pose is the result of the previous non-linear optimization; Module M5.2: For each point cloud P of the real-time frame wall 0 , projected onto the matching map wall in the vehicle coordinate system, and obtain the projection point P t ; Module M5.3: Projection point P t Search all neighboring threshold points within 1m in the kd-tree and get point P t The matching pair P with the local surface point D t -D; Module M5.4: Randomly select three non-collinear points within the local surface point D and at least 40 cm apart {P 1 ,P 2 ,P 3 } baselink represents the D plane; Module M5.5: Match the P 0 -{P 1 ,P 2 ,P 3 } utm Construct the Factor of the point-to-surface error in Ceres, and convert {P 1 ,P 2 ,P 3 } utm By transforming the pose to be optimized into {P 1 ,P 2 ,P 3 } baselink , calculate the normal vector n of the map plane = (P 1 -P 2 )×(P 2 -P 3 ), calculate the distance d from the point in the real-time frame to the map plane = (P 0 -P 1 )×n; Module M5.6: Ceres optimizes the point-to-plane error and iterates multiple times to obtain the pose of wall positioning.

Images