Method for constructing two-dimensional grid map based on three-dimensional point cloud map

Abstract

The application discloses a two-dimensional grid map construction method based on a three-dimensional point cloud map, comprising the following steps: preprocessing the three-dimensional point cloud map; mapping the point cloud in the point cloud map to a two-dimensional grid map, and obtaining a point cloud set falling into each grid in the two-dimensional grid map; extracting height distribution features, density features and normal vector features from the point cloud set corresponding to each grid; and using the height distribution features, the density features and the normal vector features for grid state determination, and outputting a two-dimensional grid map, wherein the state of the grid comprises a passable grid and an obstacle grid. By extracting the height distribution features, the density features and the normal vector features of the point cloud set corresponding to the grid map, the state of all the point clouds in the grid is determined based on the features, the state of the point in the point cloud does not need to be determined one by one, and the conversion efficiency of the two-dimensional grid map is greatly improved.

Description

Technical Field

[0001] This invention belongs to the field of environmental perception technology, and more specifically, this invention relates to a method for constructing a two-dimensional raster map based on a three-dimensional point cloud map. Background Technology

[0002] The autonomous navigation capability of mobile robots relies on accurate environmental modeling. Sensors such as LiDAR and depth cameras are widely used to construct point cloud maps that describe the three-dimensional structure of the environment. However, the data volume of three-dimensional point cloud maps is huge, and directly using them for path planning and real-time obstacle avoidance will bring a huge computational burden, making it difficult to meet the real-time requirements of dynamic environments.

[0003] Currently, most ground mobile robots perform navigation planning on a two-dimensional plane, making the height information in the three-dimensional map redundant. Existing technologies typically use a simple height slicing method to project the three-dimensional point cloud into a two-dimensional grid map. This involves setting a fixed height threshold and projecting points above that threshold as obstacles. While this two-dimensional grid map conversion method is simple, it is prone to misjudging overhanging tree branches, lampposts, and low curbs as obstacles, increasing the risk of navigation failure. Summary of the Invention

[0004] This invention provides a method for constructing a two-dimensional raster map based on a three-dimensional point cloud map, aiming to solve at least one of the above-mentioned problems.

[0005] This invention is implemented as follows: a method for constructing a two-dimensional raster map based on a three-dimensional point cloud map, the method being as follows:

[0006] Step 1: Preprocess the 3D point cloud map;

[0007] Step 2: Map the point cloud in the 3D point cloud map to the 2D grid map to obtain the point cloud set that falls into each grid cell of the 2D grid map;

[0008] Step 3: Extract height distribution features, density features, and normal vector features from the point cloud set corresponding to each raster;

[0009] Step 4: Use the height distribution features, density features, and normal vector features to determine the state of the grid and output a two-dimensional grid map. The grid state includes: passable grids and obstacle grids.

[0010] Furthermore, the extraction process for the point cloud set corresponding to each raster is as follows:

[0011] Determine the x-axis and y-axis directions of the raster map, in order to The corresponding point is used as the origin of the two-dimensional raster map. A two-dimensional xy plane coordinate system is established, and the xy plane is divided into M×N grids.

[0012] Each point in the 3D point cloud map Mapped to the corresponding 2D raster In the middle, the mapping rule is: , ,in, This represents the floor function, which rounds down to the nearest integer, resulting in each grid cell. Corresponding point cloud set .

[0013] Furthermore, the extraction of height distribution features is as follows:

[0014] Obtain point cloud collection The maximum value of all point clouds in the z-axis direction It will have the maximum value As a grid The height distribution characteristics.

[0015] Furthermore, the density feature extraction process is as follows:

[0016] Computational Raster Point cloud density within Point cloud density As a grid The density characteristics, among which, Represents a set of point clouds The number of point clouds in the cloud. Represents the grid area.

[0017] Furthermore, the extraction process of the normal vector features is as follows:

[0018] Point cloud set calculated using principal component analysis. The normal vector of each point in the point cloud is used to calculate the point cloud set. The average value of the projection of the normal vector of all points onto the z-axis. , average As a grid The characteristics of the normal vector.

[0019] Furthermore, the state determination process of the raster includes:

[0020] If point cloud set If condition 1 is met, the grid is marked as a passable grid. Condition 1 is as follows:

[0021] Condition 1: ;

[0022] If point cloud set If condition 2 is met, the grid state is marked as an obstacle grid. Condition 2 is as follows:

[0023] Condition 2: ;

[0024] If point cloud set If conditions 1 and 2 above are not met, the grid status can be marked as a grid to be confirmed.

[0025] in, The first height threshold, The second height threshold, , The first density threshold, The second density threshold, , The threshold value for the first normal vector. The second normal vector threshold, .

[0026] Furthermore, the second height threshold With the height of the robot Positive correlation ,in, For a high safety margin; first density threshold Second density threshold Average number of point clouds across all rasters Positive correlation , ,and .

[0027] Furthermore, a second determination of the state is performed on the grates marked as pending confirmation. The second determination process is as follows:

[0028] Centered on the grid whose state is "Unconfirmed Grid", count the states of other grids within a 3×3 neighborhood. If the number of grids marked as "Obstacle Grid" within the 3×3 neighborhood reaches a first threshold N1, then the grid whose state is "Unconfirmed Grid" is marked as an obstacle grid. If the number of grids marked as "Passable Grid" within the 3×3 neighborhood reaches a second threshold N2, then the grid whose state is "Unconfirmed Grid" is marked as a passable grid. If the number of grids marked as "Obstacle Grid" within the 3×3 neighborhood does not reach the first threshold N1, and the number of grids marked as "Passable Grid" also does not reach the second threshold N2, then query the historical navigation data of the mobile robot. If the grid whose state is "Unconfirmed Grid" has a successful passage record, then mark its state as a passable grid. If the grid whose state is "Unconfirmed Grid" has no successful passage record, then mark its state as an obstacle grid.

[0029] Furthermore, preprocessing includes using a combination of statistical filtering and radius filtering on the 3D point cloud map. Noise reduction is performed.

[0030] Furthermore, the statistical filtering process is as follows:

[0031] Calculation points The distance between the point and its k nearest neighbors will be used to determine the distance between the points. The average distance between a point and its k nearest neighbors is used as the point. average distance 3D point cloud map Calculate the average distance between all points in the given set. and standard deviation Eliminate those that meet the requirements The point, among which, Indicates the scale parameter;

[0032] The radius filtering process is as follows:

[0033] By setting a search radius r and a minimum neighborhood threshold t, points with fewer than t neighborhood points within the radius r are removed to obtain a denoised 3D point cloud map. .

[0034] The grid state is determined by comprehensively considering three features: height, density, and normal vector of the point cloud within the grid. The feature thresholds for grid state determination are dynamically adjusted according to robot size and navigation scenario, greatly improving the accuracy of grid state recognition. In addition, for grids with uncertain states, a secondary determination is made by combining the states of their neighboring grids and the robot's historical navigation data, further improving the accuracy of grid state determination. By extracting the height distribution features, density features, and normal vector features of the corresponding point cloud set in the grid map, the state of all point clouds within the grid is determined based on these features, eliminating the need to determine the state of each point in the point cloud individually, greatly improving the conversion efficiency of the two-dimensional grid map. Furthermore, by determining the state of the grid as a whole and performing feature integration analysis on the point cloud within the region, the interference of single-point noise and abnormal point clouds can be effectively filtered out, avoiding misjudgments caused by single-point data deviations. Attached Figure Description

[0035] Figure 1 A flowchart illustrating a method for constructing a two-dimensional raster map based on a three-dimensional point cloud map, as provided in an embodiment of the present invention. Detailed Implementation

[0036] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0037] Figure 1 This invention provides a flowchart of a method for constructing a two-dimensional raster map based on a three-dimensional point cloud map, which includes:

[0038] Step 1: Preprocess the 3D point cloud map;

[0039] Obtain the 3D point cloud map already created by the mobile robot. in, Represents a 3D point cloud map The i-th point 3D coordinates ,point , Represents a 3D point cloud map The total number of point clouds in the 3D point cloud map Preprocessing is performed, which includes two processes: noise reduction and completion. Statistical filtering combined with radius filtering is used to process the 3D point cloud map. To perform noise reduction, statistical filtering and radius filtering will be explained below:

[0040] The statistical filtering process is as follows: Set the number of neighboring points k for each point, and calculate the number of neighboring points k. The distance between the point and its k nearest neighbors will be used to determine the distance between the points. The average distance between a point and its k nearest neighbors is used as the point. average distance 3D point cloud map Calculate the average distance between all points in the given set. and standard deviation Eliminate those that meet the requirements The point, among which, Indicates the scale parameter;

[0041] The radius filtering process is as follows: Set the search radius r and the minimum neighbor number threshold t, and remove points with fewer than t neighbor points within the radius r to obtain the denoised 3D point cloud map. .

[0042] For the denoised 3D point cloud map There may be localized data gaps. Kriging interpolation is used to fill in the missing points within these areas. Using point clouds within a certain radius around the missing areas as samples, a spatial interpolation model is constructed to estimate the coordinates of the missing locations, generating a complete 3D point cloud map. .

[0043] Step 2: Map the point cloud in the 3D point cloud map to the 2D grid map to obtain the point cloud set that falls into each grid cell of the 2D grid map;

[0044] In this embodiment of the invention, the process of obtaining the point cloud set within each grid cell of a two-dimensional grid map is as follows:

[0045] Based on the navigation accuracy requirements of the mobile robot, the grid resolution of the two-dimensional grid map is set. ,

[0046] Determine the x-axis and y-axis directions of the raster map, in order to The corresponding point is used as the origin of the two-dimensional raster map. A two-dimensional xy-plane coordinate system is established, and the xy-plane is divided into M×N grids. The coordinates of each grid are: , , , , These represent 3D point cloud maps. Maximum and minimum values ​​in the x-axis direction , These represent 3D point cloud maps. Maximum and minimum values ​​in the y-axis direction This represents the function for rounding up.

[0047] 3D point cloud map is generated through parallel computing. Each point cloud Mapped to the corresponding 2D raster In the middle, the mapping rule is: , ,in, This represents the floor function, which ultimately yields each grid cell. Corresponding point cloud set .

[0048] Step 3: Extract height distribution features, density features, and normal vector features from the point cloud set corresponding to each raster;

[0049] In this embodiment of the invention, for each grid Point cloud collection Three core features—height distribution, density, and normal vector—are extracted to achieve a comprehensive representation of the environmental information of the raster region. The specific extraction process for these features is as follows:

[0050] (31) Extraction of height distribution features: obtaining point cloud set The maximum value of all point clouds in the z-axis direction It will have the maximum value As a grid The height distribution characteristics;

[0051] (32) Extraction of density features: calculation of raster Point cloud density within Point cloud density Reflecting grid The density of point cloud distribution in the region, among which, Represents a set of point clouds The number of point clouds in the cloud. Represents the raster area. Point cloud density. As a grid The density characteristics of the point cloud are as follows: if the point cloud density is too low, it indicates that the area is an open area or an area with missing data; if the density is too high, it indicates that there are dense obstacles in the area.

[0052] (33) Extraction of normal vector features: The point cloud set is calculated by principal component analysis. The normal vector of each point in the point cloud is used to calculate the point cloud set. The average value of the projection of the normal vector of all points onto the z-axis. , average As a grid The characteristics of the normal vector.

[0053] Step 4: Use height distribution features, density features, and normal vector features to determine the state of the grid and output a two-dimensional grid map. The grid state includes: passable grids and obstacle grids.

[0054] In this embodiment of the invention, the state of the raster is determined by means of feature thresholds, which include: height threshold, density threshold, and normal vector threshold. The method for determining the feature thresholds is as follows:

[0055] Height threshold: including the first height threshold and the second height threshold , ,in, A height threshold set to ignore minor ground undulations, its height value being close to the ground height; a second height threshold. With the height of the robot Positive correlation, set as ,in, It provides a high safety margin.

[0056] Density threshold: including the first density threshold Second density threshold First density threshold Second density threshold Average number of point clouds across all rasters Positive correlation, among which, , , , This is the proportionality coefficient. .

[0057] Normal vector threshold: includes: a first normal vector threshold used to identify the horizontal plane. And the second normal vector threshold for identifying non-horizontal structures ,in, .

[0058] The process of determining the grid state based on feature thresholds is as follows:

[0059] If point cloud set Condition 1 is satisfied, and the specific details of Condition 1 are as follows:

[0060] Condition 1: ;

[0061] Indicate grid If there are no high-protruding obstacles in the corresponding area, the point cloud density is within the normal range, and the area is a horizontal plane, the grid status is marked as a passable grid and marked as 0.

[0062] If point cloud set Condition 2 is satisfied, and condition 2 is as follows:

[0063] Condition 2: ;

[0064] This indicates the grid If the corresponding area contains high-protruding obstacles, dense obstacle areas, or non-horizontal obstacle structures, the grid state is marked as an obstacle grid and marked as 1;

[0065] If point cloud set If conditions 1 and 2 above are not met, there may be low obstacles such as small protrusions. The grid status will be marked as grid to be confirmed, marked as 2.

[0066] Traverse all rasters in the 2D raster map. For rasters marked as pending confirmation, perform a secondary determination of the raster status based on neighboring rasters. The secondary determination process is as follows:

[0067] Centered on the grid with the status of "unconfirmed grid", count the status of other grids in a 3×3 neighborhood;

[0068] If the number of grid cells in a 3×3 neighborhood that are marked as obstacle grid cells reaches the first quantity threshold N1, then the grid cells that are marked as unconfirmed grid cells will be marked as obstacle grid cells, and the marking will change from 2 to 1.

[0069] If the number of grids marked as passable within a 3×3 neighborhood reaches the second threshold N2, then the grids in the pending confirmation state will be marked as passable grids, and the marking will change from 2 to 0.

[0070] If the number of grids marked as obstacle grids in a 3×3 neighborhood does not reach the first threshold N1, and the number of grids marked as passable grids does not reach the second threshold N2, then the historical navigation data of the mobile robot is queried. If there is a successful passage record for a grid in the state of pending confirmation, then its state is marked as passable grid and marked as 0. If there is no successful passage record for a grid in the state of pending confirmation, then its state is marked as obstacle grid and marked as 1.

[0071] The grid map is arranged in row and column order, and all grids in the grid map are traversed sequentially based on the above method. The grid state is calibrated, a standardized two-dimensional grid map is generated, and it is stored in a format that can be directly recognized by the mobile robot navigation algorithm, thus completing the rapid conversion from a three-dimensional point cloud map to a two-dimensional grid map.

[0072] The two-dimensional grid map construction method based on three-dimensional point cloud map provided by the present invention has the following beneficial technical effects: (1) The grid state is determined by comprehensively considering the three types of features of point cloud height, density and normal vector in the grid. The feature thresholds involved in the grid state determination are dynamically adjusted according to the robot size and navigation scenario, which greatly improves the recognition accuracy of grid state. In addition, for grids with uncertain states, a secondary determination is made by combining the status of its neighboring grids and the robot's historical navigation data, which further improves the accuracy of grid state determination. (2) By extracting the height distribution features, density features and normal vector features of the corresponding point cloud set of the grid map, the state of all point clouds in the grid is determined based on the above features. It is not necessary to determine the state of each point in the point cloud one by one, which greatly improves the conversion efficiency of two-dimensional grid map. In addition, the grid is used as the whole to determine the state, and the feature integration analysis of the point cloud in the region can effectively filter the interference of single point noise and abnormal point clouds, and avoid misjudgment caused by single point data deviation.

[0073] The present invention has been described by way of example. Obviously, the specific implementation of the present invention is not limited to the above-described manner. Any non-substantial improvements made using the inventive concept and technical solution of the present invention, or the direct application of the inventive concept and technical solution of the present invention to other occasions without modification, are all within the protection scope of the present invention.

Claims

1. A method for constructing a two-dimensional grid map based on a three-dimensional point cloud map, characterized by, The method is as follows: Step 1: Preprocess the 3D point cloud map; Step 2: Map the point cloud in the 3D point cloud map to the 2D grid map to obtain the point cloud set that falls into each grid cell of the 2D grid map; Step 3: Extract height distribution features, density features, and normal vector features from the point cloud set corresponding to each raster; Step 4: Use the height distribution features, density features, and normal vector features to determine the state of the grid and output a two-dimensional grid map. The grid state includes: passable grids and obstacle grids. 2.The method of claim 1, wherein, The specific process for extracting the point cloud set corresponding to each raster is as follows: Determine the x-axis and y-axis directions of the grid map, so that Corresponding points as the origin of the two-dimensional grid map, establish a two-dimensional x-y plane coordinate system, divide the x-y plane into MxN grids; mapping each point in a three-dimensional point cloud map to a corresponding two-dimensional grid mapping rule is: , wherein, denotes a floor function, obtaining a corresponding point cloud set for each grid .​ 3.The method of claim 2, wherein, The extraction of height distribution features is as follows: Acquiring a point cloud set The maximum value of all point clouds in the z-axis direction The maximum value As a height distribution feature of the grid . 4.The method of claim 2, wherein, The process of extracting density features is as follows: Computing a grid Point cloud density within Point cloud density As a density feature of the grid Wherein, Denotes the number of point clouds in the set of point clouds Denotes the number of point clouds in the set of point clouds Denotes the grid area. 5.The method of claim 2, wherein, The process of extracting normal vector features is as follows: Point cloud set calculated using principal component analysis. The normal vector of each point in the point cloud is used to calculate the point cloud set. The average value of the projection of the normal vector of all points onto the z-axis. , average As a grid The characteristics of the normal vector.

6. The method for constructing a two-dimensional raster map based on a three-dimensional point cloud map as described in claim 2, characterized in that, The grid state determination process includes: If the point cloud set If condition 1 is met, the state of the grid is marked as a passable grid, and condition 1 is specifically as follows: Condition 1: ; If the point cloud set If condition 2 is satisfied, the state of the grid is marked as an obstacle grid, and condition 2 is specifically as follows: Condition 2: ; If the point cloud set If the above conditions 1 and 2 are not met, the grid state can be marked as a to-be-confirmed grid. in, The first height threshold, The second height threshold, , The first density threshold, The second density threshold, , The threshold value for the first normal vector. The second normal vector threshold, . 7.The method of claim 6, wherein, Second height threshold With the height of the robot Positive correlation ,in, For a high safety margin; first density threshold Second density threshold Average number of point clouds across all rasters Positive correlation , ,and .

8. The method for constructing a two-dimensional raster map based on a three-dimensional point cloud map as described in claim 6, characterized in that, For grates marked as pending confirmation, a secondary status determination is performed. The secondary determination process is as follows: Centered on the grid whose state is "Unconfirmed Grid", count the states of other grids within a 3×3 neighborhood. If the number of grids marked as "Obstacle Grid" within the 3×3 neighborhood reaches a first threshold N1, then the grid whose state is "Unconfirmed Grid" is marked as an obstacle grid. If the number of grids marked as "Passable Grid" within the 3×3 neighborhood reaches a second threshold N2, then the grid whose state is "Unconfirmed Grid" is marked as a passable grid. If the number of grids marked as "Obstacle Grid" within the 3×3 neighborhood does not reach the first threshold N1, and the number of grids marked as "Passable Grid" also does not reach the second threshold N2, then query the historical navigation data of the mobile robot. If the grid whose state is "Unconfirmed Grid" has a successful passage record, then mark its state as a passable grid. If the grid whose state is "Unconfirmed Grid" has no successful passage record, then mark its state as an obstacle grid. 9.The method of claim 1, wherein, The preprocessing includes denoising the three-dimensional point cloud map by combining statistical filtering and radius filtering . 10.The method of claim 9, wherein, The statistical filtering process is as follows: Calculation points The distance between the point and its k nearest neighbors will be used to determine the distance between the points. The average distance between a point and its k nearest neighbors is used as the point. average distance 3D point cloud map Calculate the average distance between all points in the given set. and standard deviation Eliminate those that meet the requirements The point, among which, Indicates the scale parameter; The radius filtering process is as follows: A search radius r and a minimum neighborhood point number threshold t are set, points with a neighborhood point number less than t within the radius r are removed, and a denoised three-dimensional point cloud map is obtained .

Images