Method and System for Constructing a Two-Dimensional Grid Map Based on a Three-Dimensional Laser SLAM Point Cloud Map

Abstract

The present invention discloses a method and system for constructing a two-dimensional grid map based on a three-dimensional laser SLAM point cloud map, belonging to the technical field of laser point cloud rasterization, and solving the problem of low accuracy in converting a three-dimensional laser SLAM point cloud map into a two-dimensional grid map. The method includes: subscribing to three-dimensional point cloud data in the LOAM framework for simultaneous localization and mapping to obtain a three-dimensional laser SLAM point cloud map; performing ground segmentation and two-dimensional plane projection on the three-dimensional laser SLAM point cloud map to obtain a first two-dimensional grid map; performing loop detection and backend optimization on the environmental three-dimensional laser SLAM point cloud map in sequence, and projecting the result after backend optimization onto a two-dimensional plane to obtain a second two-dimensional grid map; fitting the first and second two-dimensional grid maps to construct a final two-dimensional grid map.

Description

Technical Field

[0001] The present invention relates to the technical field of laser point cloud rasterization, and in particular, to a method and system for constructing a two-dimensional grid map based on a three-dimensional laser SLAM point cloud map. Background Art

[0002] With the development of technology and the progress of society, the requirements for robots are getting higher and higher. Robots not only need to achieve remote control driving, but also need to operate autonomously in some fields such as military and scientific research. Among them, autonomous operation involves the problem of how robots should determine their positions in the surrounding environment. First, humans comprehensively judge their positions in the environment through their eyes and brains. By analogy, the SLAM technology came into being.

[0003] SLAM is the abbreviation of Simultaneous Localization And Mapping, which was first proposed by Hugh Durrant-Whyte and John J. Leonard. Its appearance has completely solved the problem of "where am I" in the field of robotics, making it possible for robots to move autonomously in unknown environments. By inputting various external and internal sensor data, an accurate robot pose (i.e., position and orientation) is solved using algorithms. At the same time, the sensor data obtained from each pose is stitched together to form a complete map.

[0004] After years of development, the laser SLAM technology has been subdivided into multiple directions. According to the different dimensions and purposes of using the laser SLAM technology, the laser SLAM algorithms can be divided into three-dimensional laser SLAM algorithms and three-dimensional laser SLAM algorithms; among them, the two-dimensional laser SLAM algorithm can be used for mapping and positioning and subsequent path planning and navigation positioning, while the three-dimensional laser SLAM algorithm can only be used as an odometer. After a grid map is established by the three-dimensional laser SLAM, the grid map can be used for path planning and navigation positioning. However, after a point cloud map is established by the three-dimensional laser SLAM, due to the different types of maps, it cannot be used for path planning and navigation positioning. However, due to the rich information, diverse point clouds, and a large amount of odometer information of the three-dimensional laser SLAM algorithm, it is very necessary to accurately convert the point cloud map established by the three-dimensional laser SLAM algorithm into a grid map. However, how to ensure the accuracy of the grid map obtained by this method is a problem that needs to be considered. Summary of the Invention

[0005] In view of the above analysis, the embodiments of the present invention aim to provide a method and system for constructing a two-dimensional grid map based on a three-dimensional laser SLAM point cloud map, so as to solve the problem of low accuracy in the existing conversion of a three-dimensional laser SLAM point cloud map into a two-dimensional grid map.

[0006] On the one hand, the embodiment of the present invention discloses a method for constructing a two-dimensional grid map based on a three-dimensional laser SLAM point cloud map, including:

[0007] Subscribe to three-dimensional point cloud data under the LOAM framework for simultaneous localization and mapping to obtain a three-dimensional laser SLAM point cloud map;

[0008] Perform ground segmentation and two-dimensional plane projection on the three-dimensional laser SLAM point cloud map to obtain a first two-dimensional grid map;

[0009] Perform loop detection and backend optimization on the environmental three-dimensional laser SLAM point cloud map in sequence, and project the result after backend optimization onto a two-dimensional plane to obtain a second two-dimensional grid map;

[0010] Fit the first and second two-dimensional grid maps to construct the final two-dimensional grid map.

[0011] On the basis of the above solution, the present invention also makes the following improvements:

[0012] Based on the further improvement of the above solution, obtain a three-dimensional laser SLAM point cloud map by performing the following operations:

[0013] Obtain three-dimensional point cloud data by subscribing to the frameID of the lidar;

[0014] Obtain the actual angle turned by the lidar in each frame;

[0015] Based on the three-dimensional coordinates of the three-dimensional point cloud data and the actual angle turned by the lidar in each frame, perform ordering processing on the three-dimensional point cloud data;

[0016] Perform NDT matching on the ordered three-dimensional laser point cloud data to generate a three-dimensional laser SLAM point cloud map.

[0017] Based on the further improvement of the above solution, obtain the first two-dimensional grid map by performing the following operations:

[0018] Perform ground segmentation on the three-dimensional laser SLAM point cloud map to obtain an environmental three-dimensional laser SLAM point cloud map;

[0019] Perform point cloud projection on the environmental three-dimensional laser SLAM point cloud map to obtain a first two-dimensional grid map.

[0020] Based on the further improvement of the above solution, obtain the second two-dimensional grid map by performing the following operations:

[0021] Perform loop detection on the environmental three-dimensional laser SLAM point cloud map;

[0022] Perform backend optimization on the closed-loop detection results;

[0023] Based on the backend optimization results, update the environmental 3D lidar SLAM point cloud map, and project the updated point cloud map onto a 2D plane to obtain a second 2D grid map.

[0024] Based on a further improvement of the above solution, the closed-loop detection includes:

[0025] Use the pose of the latest point cloud data frame in the environmental 3D lidar SLAM point cloud map as the current pose, and use the KDtree method to find the historical pose closest to the current pose. If the found historical pose meets the time threshold requirement or the position threshold requirement, use the point cloud data corresponding to the historical pose as the source point cloud and the point cloud corresponding to the current pose as the target point cloud.

[0026] Based on a further improvement of the above solution, the time threshold requirement is that the historical pose satisfies a time interval greater than 30 ms from the current pose;

[0027] The position threshold requirement is that the distance between the historical pose and the current pose is greater than 2 m.

[0028] Based on a further improvement of the above solution, obtain the final 2D grid map by performing the following operations:

[0029] Align the first and second 2D grid maps, and for each corresponding grid, perform the following judgment:

[0030] If the grid is full in any of the maps, the grid in the final 2D grid map is full;

[0031] If the grid is empty in both maps, the grid in the final 2D grid map is empty.

[0032] On the other hand, the present invention also discloses a 2D grid map construction system based on a 3D lidar SLAM point cloud map, including:

[0033] A 3D lidar SLAM point cloud map acquisition module, configured to subscribe to 3D point cloud data in the LOAM framework for simultaneous localization and mapping to obtain a 3D lidar SLAM point cloud map;

[0034] A first 2D grid map acquisition module, configured to perform ground segmentation and 2D plane projection on the 3D lidar SLAM point cloud map to obtain a first 2D grid map;

[0035] A second 2D grid map acquisition module, configured to perform closed-loop detection and backend optimization on the environmental 3D lidar SLAM point cloud map in sequence, and project the result after backend optimization onto a 2D plane to obtain a second 2D grid map;

[0036] The final 2D grid map acquisition module is used to fit the first and second 2D grid maps to construct the final 2D grid map.

[0037] Compared with the prior art, the present invention can at least achieve one of the following beneficial effects:

[0038] The method and system for constructing a 2D grid map based on a 3D laser SLAM point cloud map provided by the present invention improve the process of constructing the 2D grid map, and establish the 2D grid map while establishing the 3D point cloud map. The 2D grid map established in this way has richer information and more accurate mapping.

[0039] In the present invention, the above technical solutions can also be combined with each other to achieve more preferred combination schemes. Other features and advantages of the present invention will be described in the following specification, and some advantages can be obvious from the specification, or can be understood by implementing the present invention. The purpose and other advantages of the present invention can be realized and obtained through the content specifically pointed out in the specification and the drawings. Description of the Drawings

[0040] The drawings are only for the purpose of showing specific embodiments and are not considered to be a limitation of the present invention. Throughout the drawings, the same reference signs denote the same components.

[0041] Figure 1 It is a flowchart of the method for constructing a 2D grid map based on a 3D laser SLAM point cloud map in Embodiment 1 of the present invention;

[0042] Figure 2 It is a schematic structural diagram of the system for constructing a 2D grid map based on a 3D laser SLAM point cloud map in Embodiment 2 of the present invention. Detailed Embodiments

[0043] The following will specifically describe the preferred embodiments of the present invention in conjunction with the drawings, wherein the drawings form a part of the present application and are used together with the embodiments of the present invention to explain the principle of the present invention, and are not used to limit the scope of the present invention.

[0044] Embodiment 1

[0045] A specific embodiment of the present invention discloses a method for constructing a 2D grid map based on a 3D laser SLAM point cloud map. The flowchart is as Figure 1 shown, including the following steps:

[0046] Step S1: Subscribe to 3D point cloud data in the LOAM framework for simultaneous localization and mapping to obtain a 3D laser SLAM point cloud map;

[0047] In this step, the following operations are specifically performed:

[0048] Step S11: Obtain 3D point cloud data by subscribing to the frame ID of the lidar;

[0049] Specifically, in the LOAM framework, by subscribing to the frame ID of the lidar, the point cloud data of the lidar can be received. Since the point cloud data received from the network port is of the PointCloud2 type in sensor_msgs (included in ROS), without processing, it appears as binary numbers and has no distance information. Therefore, after receiving the point cloud data from the network port, it is necessary to use the existing PCL library to convert the point cloud data of the lidar into PointCloud <pcl::pointxyz>For data of this type format, the converted point cloud data contains x, y, and z coordinate information, but does not have the angle information of each specific point.

[0050] Step S12: Obtain the actual angle turned by the lidar in each frame;

[0051] Exemplarily, according to the scanning method and data manual of the lidar, find the coordinate definition of VLP16, and combine with the arctangent function to calculate the start angle and end angle of the lidar turning in each data frame; where,

[0052] The start angle startaAngle = -arctan(y / x),

[0053] The end angle endAngle = -arctan(y / x) + 2π;

[0054] Based on the start angle and end angle, the actual angle turned by the lidar in the corresponding data frame can be obtained; specifically,

[0055] The actual angle actualAngle = endAngle - startaAngle;

[0056] where x and y are PointCloud obtained after being processed by the PCL library <pcl::pointxyz>Data of type format.

[0057] Step S13: Based on the three-dimensional coordinates of the three-dimensional point cloud data and the actual angle turned by the lidar in each frame, perform an ordering process on the three-dimensional point cloud data;

[0058] Specifically, since the x, y, and z coordinate information of each point cloud in the 0-360 rotation range is stored in the current frame of point cloud data, the mutual relationship between adjacent frames of point cloud data cannot be obtained. Therefore, given the beam number of the used lidar sensor, according to the beam number information and the horizontal resolution, the lidar data of adjacent frames can be divided into fixed beam groups according to the vertical beam information, and then stored in the corresponding vector container (including the three-dimensional coordinates of the three-dimensional point cloud data in the current data frame and the actual angle turned by the current data frame), which is convenient for subsequent direct calls. For example, if it is the data of the first line of the lidar, it is stored in laser_data[1], and the lidar data of other beams is stored in the corresponding vector container.

[0059] Step S14: Perform NDT matching on the ordered three-dimensional lidar point cloud data to generate a three-dimensional lidar SLAM point cloud map;

[0060] Perform NDT registration on the ordered current frame of point cloud data and the historical three-dimensional lidar SLAM point cloud map formed by the historical frame of point cloud data to generate an updated three-dimensional lidar SLAM point cloud map. Among them, if there is only one frame of three-dimensional lidar point cloud data of the lidar, a three-dimensional lidar SLAM point cloud map is directly generated based on this frame of point cloud data for use in the next frame of matching.

[0061] The process of NDT matching (NDT inter-frame matching algorithm) is as follows:

[0062] First, define an object of the NDT matching method; then, set the matching parameters of NDT; next, match the input three-dimensional lidar point cloud data of the current frame with the existing three-dimensional lidar SLAM point cloud map (historical) to obtain an updated three-dimensional lidar SLAM point cloud map.

[0063] It should be noted that since the amount of point cloud data matched in the current frame is particularly large, point cloud filtering is first performed before establishing the point cloud map according to the pose.

[0064] The voxel_filter in the PCL library is directly used for point cloud filtering. Its basic principle is to divide the three-dimensional space into cube grids of equal size, and at most only one point is retained within a cube grid, thus achieving a sparsification effect. It should be noted that the pose of the lidar movement has been obtained before point cloud filtering, and then the matched point cloud can be placed at the specified position on the map according to this pose. In this way, a three-dimensional lidar SLAM point cloud map is established based on the ordered three-dimensional lidar point cloud data.

[0065] Step S2: Perform ground segmentation and two-dimensional plane projection on the three-dimensional lidar SLAM point cloud map to obtain the first two-dimensional grid map;

[0066] Step S21: Perform ground segmentation on the three-dimensional lidar SLAM point cloud map to obtain the environmental three-dimensional lidar SLAM point cloud map;

[0067] Preferably, the process of ground segmentation can be implemented in various ways. To facilitate those skilled in the art to better understand the solution in this embodiment, this embodiment exemplifies a ground segmentation method, which is introduced as follows:

[0068] Through the ordering of the unordered point cloud in step S1, the points of different beam lines have been saved into the corresponding vector containers. Then, according to the specific beam line schematic diagram of the lidar, the beam lines in the lower half of the three-dimensional point cloud map are determined. Based on the angle between the front and rear lidar point clouds and the ground, and setting relevant thresholds, the ground point cloud is segmented from the entire point cloud map, and only the environmental map is retained, thereby obtaining the environmental three-dimensional lidar SLAM point cloud map.

[0069] Step S22: Perform point cloud projection on the environmental three-dimensional lidar SLAM point cloud map to obtain the first two-dimensional grid map;

[0070] According to the established environmental three-dimensional lidar SLAM point cloud map, the three-dimensional space is discretized into cubic grids of a set size, and the number of environmental point clouds in each grid is counted;

[0071] For each grid, if the number of environmental point clouds exceeds the set threshold, the two-dimensional plane grid projected by this grid is filled; otherwise, the two-dimensional plane grid projected by this grid is empty.

[0072] Exemplarily, according to the established environmental three-dimensional lidar SLAM point cloud map, the three-dimensional space is discretized into cubic grids of 5cm * 5cm * 5cm. Then, the point clouds above the ground in each small grid are counted. The set specific threshold is 35. As long as the number of point clouds in each small grid is larger than this 35 threshold, finally projected onto the two-dimensional plane grid, the two-dimensional plane grid is filled, and if the number of point clouds is smaller than this 35 threshold, the two-dimensional plane grid is set to be empty.

[0073] Step S3: Perform loop closure detection and backend optimization on the environmental 3D lidar SLAM point cloud map in sequence, and project the result after backend optimization onto a two-dimensional plane to obtain a second two-dimensional grid map;

[0074] Step S31: Perform loop closure detection on the environmental 3D lidar SLAM point cloud map

[0075] Take the pose of the latest point cloud data frame in the environmental 3D lidar SLAM point cloud map as the current pose, use the KDtree method to find the historical pose closest to the current pose. If the found historical pose meets the time threshold requirement or the position threshold requirement, then take the point cloud data corresponding to this historical pose as the source point cloud, and take the point cloud corresponding to the current pose as the target point cloud. Store and publish the source point cloud and the target point cloud.

[0076] Preferably, in this embodiment,

[0077] The time threshold requirement is that the historical pose meets the condition that the time interval from the current pose is greater than 30 ms;

[0078] The position threshold requirement is that the distance between the historical pose and the current pose is greater than 2 m;

[0079] The reason for such processing is to prevent the problem of memory explosion caused by excessive data volume.

[0080] Step S32: Perform backend optimization on the loop closure detection result

[0081] Subscribe to the stored point cloud data of the loop closure detection, detect the loop of the entire historical position of the lidar scan, and store the source and target point clouds and position data; use the NDT algorithm of the PCL function library to perform matching calculations on the lidar point cloud, and at the same time design the maximum correlation distance and the maximum number of iterations to solve the relative pose between the source and the target again.

[0082] Step S33: Based on the result of the backend optimization, update the environmental 3D lidar SLAM point cloud map, and project the updated point cloud map to obtain a second two-dimensional grid map;

[0083] In this step, combine the Eigen library to project the relative pose between the source and the target onto a two-dimensional plane and record it in a container, thereby updating the two-dimensional grid projection to obtain a second two-dimensional grid map.

[0084] Step S4: Fit the first and second two-dimensional grid maps to construct the final two-dimensional grid map.

[0085] Align the first and second two-dimensional grid maps, and for each corresponding grid, perform the following judgment:

[0086] If the grid is full in any map, then the grid in the final 2D grid map is full;

[0087] If the grid is empty in both maps, then the grid in the final 2D grid map is empty.

[0088] Compared with the prior art, the method and system for constructing a 2D grid map based on a 3D laser SLAM point cloud map provided in this embodiment improve the construction process of the 2D grid map and establish the 2D grid map while establishing the 3D point cloud map. The 2D grid map established in this way has richer information and more accurate mapping.

[0089] Embodiment 2

[0090] Embodiment 2 of the present invention discloses a system for constructing a 2D grid map based on a 3D laser SLAM point cloud map. The structural schematic diagram is as Figure 2 shown, including:

[0091] A 3D laser SLAM point cloud map acquisition module, configured to subscribe to 3D point cloud data in the LOAM framework for simultaneous localization and mapping to obtain a 3D laser SLAM point cloud map;

[0092] A first 2D grid map acquisition module, configured to perform ground segmentation and 2D plane projection on the 3D laser SLAM point cloud map to obtain a first 2D grid map;

[0093] A second 2D grid map acquisition module, configured to perform loop detection and backend optimization on the environmental 3D laser SLAM point cloud map in sequence, and project the result after backend optimization onto a 2D plane to obtain a second 2D grid map;

[0094] A final 2D grid map acquisition module, configured to fit the first and second 2D grid maps to construct a final 2D grid map.

[0095] For the specific implementation process of the embodiments of the present invention, refer to the above method embodiments, and details are not described herein again. Since the principles of this embodiment and the above method embodiments are the same, this system also has the corresponding technical effects of the above method embodiments.

[0096] Those skilled in the art can understand that all or part of the processes for implementing the methods of the above embodiments can be completed by instructing relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium. Among them, the computer-readable storage medium is a disk, an optical disc, a read-only memory, or a random access memory, etc.

[0097] The above are only the preferred specific embodiments of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.< / pcl::pointxyz> < / pcl::pointxyz>

Claims

1. A method for constructing a two-dimensional grid map based on a three-dimensional lidar SLAM point cloud map, characterized in that, Including: Subscribe to 3D point cloud data under the LOAM framework for simultaneous localization and mapping to obtain a 3D lidar SLAM point cloud map; Perform ground segmentation on the 3D lidar SLAM point cloud map to obtain an environmental 3D lidar SLAM point cloud map; perform point cloud projection on the environmental 3D lidar SLAM point cloud map to obtain a first 2D grid map; Perform loop closure detection on the environmental 3D lidar SLAM point cloud map; perform backend optimization on the loop closure detection result; based on the backend optimization result, update the environmental 3D lidar SLAM point cloud map, and project the updated point cloud map to obtain a second 2D grid map; Fit the first and second 2D grid maps to construct a final 2D grid map; Obtain the final 2D grid map by performing the following operations: align the first and second 2D grid maps, and for each corresponding grid, perform the following judgment: if the grid is full in any map, then the grid in the final 2D grid map is full; if the grid is empty in both maps, then the grid in the final 2D grid map is empty.

2. The method for constructing a two-dimensional grid map based on a three-dimensional laser SLAM point cloud map according to claim 1, wherein Obtain the 3D lidar SLAM point cloud map by performing the following operations: Obtain 3D point cloud data by subscribing to the frameID of the lidar; Obtain the actual angle turned by the lidar in each frame; Based on the 3D coordinates of the 3D point cloud data and the actual angle turned by the lidar in each frame, perform ordering processing on the 3D point cloud data; Perform NDT matching on the ordered 3D lidar point cloud data to generate a 3D lidar SLAM point cloud map.

3. The method for constructing a two-dimensional grid map based on a three-dimensional laser SLAM point cloud map according to claim 2, wherein The loop closure detection includes: Use the pose of the latest point cloud data frame in the environmental 3D lidar SLAM point cloud map as the current pose; Use the KDtree method to find the historical pose closest to the current pose. If the found historical pose meets the time threshold requirement or the position threshold requirement, then use the point cloud data corresponding to the historical pose as the source point cloud and the point cloud corresponding to the current pose as the target point cloud.

4. The method for constructing a 2D grid map based on a 3D lidar SLAM point cloud map according to claim 3, wherein The time threshold requirement is: the historical pose satisfies that the time interval from the current pose is greater than 30 ms; The position threshold requirement is: the distance between the historical pose and the current pose is greater than 2 m.

5. A two-dimensional grid map construction system based on a three-dimensional laser SLAM point cloud map, characterized in that, Including: A 3D lidar SLAM point cloud map acquisition module, configured to subscribe to 3D point cloud data under the LOAM framework for simultaneous localization and mapping to obtain a 3D lidar SLAM point cloud map; A first 2D grid map acquisition module, configured to perform ground segmentation on the 3D lidar SLAM point cloud map to obtain an environmental 3D lidar SLAM point cloud map; perform point cloud projection on the environmental 3D lidar SLAM point cloud map to obtain a first 2D grid map; A second 2D grid map acquisition module, configured to perform loop closure detection on the environmental 3D lidar SLAM point cloud map; perform backend optimization on the loop closure detection result; based on the backend optimization result, update the environmental 3D lidar SLAM point cloud map, and project the updated point cloud map to obtain a second 2D grid map; The final 2D grid map acquisition module is used to fit the first and second 2D grid maps to construct the final 2D grid map; The final 2D grid map is obtained by performing the following operations: Align the first and second 2D grid maps. For each corresponding grid, perform the following judgment: If the grid is full in any of the maps, the grid in the final 2D grid map is full; If the grid is empty in both maps, the grid in the final 2D grid map is empty.

Images