A multi-scale static map construction method based on multi-line lidar point cloud

Abstract

The present invention discloses a method for constructing a multi-scale static map based on a multi-line laser radar point cloud. The multi-line laser radar is used to obtain road driving data and output a laser point cloud with a timestamp for each frame. The posture data of each frame is output through a three-dimensional laser SLAM under an industrial computer. The dynamic target removal system reads the laser point cloud frame within the user-specified range according to the timestamp. An image pyramid is established at multiple scales, and each layer of the pyramid is iterated. For each layer of the image, the three-dimensional point cloud of each key frame in the local coordinate system and the original map are projected to a two-dimensional image according to the specified scale. The point cloud subset marked as dynamic points is used as all the point clouds of the next iteration of the image pyramid, and the cycle is repeated until all scales of the image pyramid are traversed, and the error recognition rate of dynamic points is reduced to a minimum, thereby constructing an accurate static point cloud map. The present invention can be further applied to unmanned system navigation, especially global path planning and scene re-identification.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned system positioning and mapping, and specifically relates to a multi-scale static map construction system based on multi-line laser radar point cloud Background Art

[0002] With the continuous development and progress of artificial intelligence and robotics, application scenarios are constantly enriched and diversified, especially the rapid commercialization of unmanned driving, the rapid industrialization of service robots, and the reconstruction of three-dimensional environments. There are more stringent requirements for related technologies, including high accuracy and high availability of maps. The establishment of maps and the positioning of unmanned systems are complementary, and SLAM (Simultaneous Localization And Mapping) technology is currently an effective way to achieve this goal. SLAM technology has undergone a long development, from the early two-dimensional map establishment that relied on single-line laser radar and used filtering algorithms to locate, to the current three-dimensional map establishment through multi-line laser radar and map optimization positioning. The unmanned system perceives the real environment that combines dynamic and static. The established map will inevitably have dynamic objects such as moving pedestrians and vehicles. The environmental map generated by the SLAM system will eventually leave a meaningless track in the static map. The dynamic object points in the point cloud map are rendered as "ghost" tracks in the point cloud map, which usually overlap and cover the views of road markings, traffic signs and other important static features, making it difficult to extract static features from them. The ultimate goal of building a map is to help people or machines to understand and use the environment, and the dynamic environment needs to be perceived in real time. Therefore, it is necessary to filter out the dynamic target ghost trajectory point cloud and rebuild the static map.

[0003] The sensors relied on for environmental perception mainly include LiDAR and visual cameras. As an important environmental perception sensor for unmanned systems, LiDAR has always played an irreplaceable role. It can accurately scan the environment at a high frequency without being affected by light, and has the characteristics of long detection distance and high reliability. Compared with depth cameras, the point cloud obtained has intensity information but no color information. Visual cameras, especially depth cameras, ToF cameras, and binocular cameras that contain depth information, have the advantages of low cost and good at capturing the texture of the scene, and thus effectively completing tasks such as target recognition and semantic segmentation; however, the perceived depth information is very limited, and they are sensitive to light, making it difficult to effectively perceive the environment in scenes such as darkness where light is severely disturbed.

[0004] The 3D laser SLAM system first processes the raw data of environmental perception to eliminate motion distortion, pre-integrates the IMU data, and uses the ICP inter-frame matching algorithm to perform front-end matching based on the pose of the original interpolated frame. It iterates all target points in the current frame based on the pose and target points calculated from the original points, and performs preliminary inter-frame point cloud registration. The back-end uses graph optimization to minimize the total cost of the target points after the pose transformation of the current frame and the corresponding points in the next frame, thereby obtaining the optimal pose and the best matching map. Although the back-end optimization can minimize the error as much as possible, as the error accumulates frame by frame, the error is gradually amplified. The closed-loop characteristics during the loop closure are used to perform secondary matching criteria to effectively eliminate the accumulated error and establish an accurate map. However, the motion trajectory of the dynamic target is inevitably retained in the static map.

[0005] Existing dynamic target removal includes online removal and offline removal. Online removal implants dynamic removal methods in the SLAM system, which obviously greatly increases the computational complexity and reduces the real-time performance of SLAM. It is inconsistent with the development direction of lightweight SLAM and it is difficult to maintain high-precision removal. Offline removal is to perform map post-processing based on the final accurate posture and map obtained by the SLAM system. Including dynamic object removal based on point cloud segmentation, through deep learning, this method relies on manually annotated training sets; dynamic object removal based on dynamic target detection first performs 3D target detection on each frame of point cloud and then reconstructs the static map.

[0006] The shortcomings of current technology are as follows:

[0007] 1. When using the original SLAM system, dynamic targets are completely ignored. In the process of scanning and building maps, dynamic objects such as moving pedestrians and vehicles will accumulate between frames to form ghost track point clouds in the map, which seriously affects the further use of the point cloud map. This is redundant information that is not needed to build a static map, which interferes with the human-machine cognition of unfamiliar environments and even blocks useful static information.

[0008] 2. The online dynamic removal algorithm embedded in SLAM will reduce the real-time performance of SLAM and affect the efficiency of SLAM positioning and mapping.

[0009] 3. Dynamic removal methods using semantic segmentation and 3D object detection generally use deep learning but cannot guarantee generalization across all scenarios

[0010] Therefore, researchers in this field are committed to designing a new static map construction system and overcoming the above defects. Summary of the invention

[0011] In view of the above-mentioned defects of the prior art, the purpose of the present invention is to provide a multi-scale static map construction system based on multi-line laser radar point cloud. The present invention uses the radar point cloud data collected by the multi-line laser radar and the pose corresponding to the point cloud frame output by the laser SLAM system to perform sampling and projection, and performs digital image processing on the projected image, and finally separates the dynamic target points from the static target points. At the same time, the image pyramid sequence is used to restore the incorrectly marked dynamic target points, and finally a static point cloud map with minimized errors is obtained, completing the construction of a multi-scale static point cloud map. It has the characteristics of fast computing speed, high precision in dynamic target removal, and maximum restoration of static maps.

[0012] The relevant terms are explained as follows:

[0013] Multi-line LiDAR: Different from the traditional single-line LiDAR, which emits one laser scanning line, multi-line LiDAR has 16, 32, 64 lines and other specifications, and emits multiple laser scanning lines, so it can perceive the three-dimensional environment.

[0014] Point cloud: A logical data structure described as a collection of points, each of which includes information such as x, y, z coordinates and laser intensity.

[0015] Image pyramid: An image pyramid is an effective but conceptually simple structure for interpreting images at multiple resolutions. It is also a multi-scale representation of images. The pyramid of an image is a set of images arranged in a pyramid shape with gradually decreasing resolutions and derived from the same original image. It is obtained by downsampling in stages until a certain termination condition is reached.

[0016] SLAM: (Simultaneous Localization And Mapping) simultaneous positioning and mapping, a technology that uses lidar or camera as absolute pose sensor, and IMU or wheel odometer as relative pose sensor; it achieves positioning and mapping at the same time by optimizing and complementing each other.

[0017] ICP: (Iterative Closest Point) Iterative Closest Point algorithm finds the nearest point (pi, qi) in the matching target point cloud P and source point cloud Q according to certain constraints, and then calculates the optimal matching parameters R and t to minimize the error function. The error function is E(R, t): Where n is the number of nearest point pairs, pi is a point in the target point cloud P, qi is the nearest point in the source point cloud Q corresponding to pi, R is the rotation matrix, and t is the translation vector.

[0018] KDTree: A tree-based data structure that can be used to efficiently search point clouds and find all points within a distance threshold x from a point.

[0019] OcTree: Octree, a tree-based data structure. The octree structure divides the geometric entities in the three-dimensional space into voxels. Each voxel has the same time and space complexity. It divides the geometric objects in the three-dimensional space of size (2nx2nx2n) through a cyclic recursive partitioning method to form a directional graph with a root node.

[0020] The technical solution of the present invention:

[0021] A multi-scale static map construction system based on multi-line laser radar point cloud, characterized in that it includes the following steps:

[0022] Step 1: Use a multi-line laser radar to obtain road driving data and output a laser point cloud with a timestamp for each frame, and output the posture data of each frame through three-dimensional laser SLAM under an industrial computer.

[0023] The preset parameters are:

[0024] v image_vfov Preset directional field of view for 2D images

[0025] v image_hfov is the horizontal field of view of the two-dimensional image

[0026] d downsample_size is the point cloud downsampling parameter of the local frame and the global map

[0027] S multi_resolution_list For each preset scale sequence of the image pyramid

[0028] n parallel_cores The number of concurrent threads for projecting 3D point clouds to 2D images

[0029] r keyframe_range Keyframe selection range

[0030] M extrinsic_lidar Radar's extrinsic matrix

[0031] Step 2: The dynamic target removal system reads the laser point cloud frames within the user-specified range according to the timestamp, and extracts the key frames and the posture data of the corresponding key frames.

[0032] Step 3: To speed up the calculation and improve memory utilization, the point cloud of the key frame is downsampled.

[0033] Step 4: In the global coordinate system, use the posture information of each frame point cloud to stitch all key frame point clouds into the original map.

[0034] Step 5: Establish a multi-scale image pyramid and iterate each layer of the pyramid.

[0035] Step 6: For each layer of the image, project each key frame 3D point cloud in the local coordinate system to a 2D image of a specified size according to a specified scale.

[0036] Step 7: At the pixel level, calculate the difference between the two frames and compare it with the threshold obtained from the experiment to determine the position of the dynamic target.

[0037] Step 8: Map the position of the dynamic target in the corresponding two-dimensional image back to the index of the point cloud in the three-dimensional point cloud, and mark and extract the dynamic points and static points respectively.

[0038] Step 9: Use the point cloud subset marked as dynamic points as the entire point cloud of the next iteration of the image pyramid, and repeat the process until all scales of the image pyramid are traversed, reducing the error recognition rate of dynamic points to a minimum, thereby constructing an accurate static point cloud map.

[0039] Furthermore, in step 1, the laser radar is located above the center of the robot or vehicle, and each frame of the laser point cloud includes the intensity information received after scanning multiple laser lines and the relative Cartesian coordinates relative to the laser receiver. The SLAM system used can be any three-dimensional multi-line laser SLAM system that takes multi-line laser radar data and nine-axis IMU data as input and can output the corresponding posture of each frame of laser point cloud.

[0040] Furthermore, step 2 includes:

[0041] Step 2.1, read or subscribe to the message queue scanlist of the asynchronous communication scanned in real time according to the persistent file path storing the laser point cloud scan frame, and keyframe_range Read the key frame and store it into the memory of the industrial computer.

[0042] Step 2.2: Read the iteratively optimized pose estimate corresponding to each laser point cloud scanning frame output by the laser SLAM system. The pose is the pose in the local coordinate system of the radar, and obtain the transformation matrix set P from the radar coordinate system to the current pose. direct .

[0043] Step 2.3: Invert all the poses read in 2.2 to obtain the transformation matrix set P from the current pose to the radar coordinate system. reverse

[0044] Furthermore, step 3 includes:

[0045] For each laser point cloud scan frame input in step 2, according to the preset d downsample_size Downsampling parameters, by creating a three-dimensional voxel grid in the point cloud data, and then using the center of gravity of each voxel to approximate the other points in the voxel, thereby downsampling the keyframes, which is convenient for subsequent processing speed and reduces memory usage. Finally, the downsampled keyframe set S is obtained local_after

[0046] Furthermore, step 4 includes:

[0047] Step 4.1: From the key frame point cloud and pose read in step 2, extract the corresponding key frame s in sequence according to the index local_after The pose T corresponding to the key frame direct

[0048] Step 4.2: Use the external parameter transformation matrix to transform s local_after Transform from the radar coordinate system to the main baselink coordinate system to obtain s local_after_then

[0049] Step 4.3: Use the T read in 4.1 direct Transform the pose from the main baselink coordinate system to the global coordinate system to obtain s global_i

[0050] Step 4.4, according to the index, iterate all s global_i Splice to get the original global map P global

[0051] Step 4.5: According to the preset downsampling parameter d downsample_size For the original global map P global Downsampling is performed. At this time, the map is too large and the point cloud data is too large to be downsampled using the traditional voxel grid in step 3. Instead, a more complex data structure, the octree, should be used to achieve downsampling through octree-based voxel filtering. Finally, the downsampled global map P is obtained. global_after

[0052] Furthermore, step 5 includes:

[0053] Image pyramid is a kind of multi-scale expression in an image. It is mainly used for image segmentation. It is an effective but conceptually simple structure to interpret images at multiple resolutions. Image pyramid was originally used for machine vision and image compression. The pyramid of an image is a set of images arranged in a pyramid shape with gradually decreasing resolutions and derived from the same original image. It is obtained by downsampling in stages until a certain termination condition is reached. The bottom of the pyramid is a high-resolution representation of the image to be processed, while the top is a low-resolution approximation. The layers of images can be likened to a pyramid. The higher the level, the smaller the image and the lower the resolution.

[0054] According to the preset S multi_resolution_list ,The preset parameters depend on the resolution of the radar, which is reflected in the number of lines of the multi-line laser radar. Multi-resolution is collected from top to bottom, and an image pyramid sequence is constructed, which contains image resolution parameters of different scales and is output to step 6 for separate iterations.

[0055] Furthermore, step 6 includes:

[0056] Step 6.1: Based on the image pyramid in step 5, we iterate through the image resolution from bottom to top. According to the vertical angle and horizontal field of view of the lidar, we preset the parameter v image_vfov , v image_hfov , the current scale r i *v image_vfov Get the number of image pixels in the vertical direction n num_row , and then r i *v image_hfov Get the number of image pixels in the horizontal direction n num_col

[0057] Step 6.2: Traverse S local_after Each key frame in n num_row and n num_col Initialize the image matrix M local_after

[0058] Step 6.3: For each key frame s local_after , then enable multithreading, the number of threads is determined by the number of cores of the industrial computer as n parallel_cores , each thread has its own non-shared variables, so there will be no interference between each other, and no thread safety issues will be caused. Here, we choose not to lock. At this time, the n threads will be allocated, scheduled and executed by the operating system.

[0059] Step 6.4: For each thread, the key is to calculate the precise position of each point in the point cloud mapped to the image. First, the vertical azimuth angle p of each point needs to be calculated. i_va =arctan(pi_y / p i_x )*(180 / pi), horizontal azimuth angle p i_ha =arctan(z / sqrt(x^2+z^2))*(180 / pi)

[0060] Step 6.5: The vertical index p of the point cloud in the image can be calculated index_x , and then map the three-dimensional point cloud to a two-dimensional image, the formula is

[0061]

[0062] Step 6.6: The horizontal position index p of the point cloud in the image can be calculated index_y , and then map the three-dimensional point cloud to a two-dimensional image, the formula is

[0063]

[0064] Step 6.7: According to the known index p of the point cloud in the image index_x 、p index_y , if the laser intensity p i <v x,y This indicates that the pixel is not occupied, so the laser intensity of each point is assigned to the image matrix M as the value of the single-channel image. local_after , otherwise it is occupied and no processing is performed. Then the projected point cloud is obtained, and the coordinate system is now located in the laser local coordinate system. The two-dimensional image obtained at this time is the original unprocessed image, that is, it has both static coordinate points and dynamic coordinate points.

[0065] Step 6.8: Downsample the global map point cloud P global_after , through the transformation matrix p reverse , radar external parameter matrix, transform the point cloud from the global coordinate system to the radar coordinate system to obtain P global_after_local , so that the coordinate system is consistent with the point cloud coordinate system of the current key frame, which can facilitate the comparison of the two images in the subsequent steps.

[0066] Step 6.9, initialize a size of n num_row and n num_col The single-channel image matrix M global_after_local , used to map s global_after_local ; Initialize a mapping matrix M for mapping point indices and image indices point_to_image

[0067] Step 6.10: For each s transformed to the same coordinate system as the current keyframe global_after_local , then enable multithreading, the number of threads is determined by the number of cores of the industrial computer as n parallel_coresEach thread has its own non-shared variables, so they will not interfere with each other and will not cause thread safety issues. Here, we choose not to lock. At this time, the n threads will be allocated and scheduled by the operating system and executed

[0068] Step 6.11: Similar to steps 6.5 and 6.6, replace s global_after_local Similarly, it is mapped to the two-dimensional image M global_after_local

[0069] Step 6.12: According to the known index p of the point cloud in the image index_x 、p index_y , if the laser intensity p i <v x,y This indicates that the pixel is not occupied, so the laser intensity of each point is assigned to the image matrix M as the value of the single-channel image. global_after_local At the same time, the most important thing is to index the point cloud p index Assign the mapping matrix M between the point cloud and the image point_to_image

[0070] Furthermore, step 7 includes:

[0071] Step 7.1: M obtained from steps 6.7 and 6.12 local_after and M global_after_local , calculate the difference M diff

[0072] Step 7.2: Traverse each pixel of the image, make high-precision judgments, and calculate the threshold t min =t adaptive *p intensity , that is, the adaptive parameters are combined with the original M local_after The corresponding pixel value is the laser intensity p intensity Multiply and set the upper limit threshold t according to the maximum detection distance of the laser max , in order to prevent the error of invalid points caused by noise disturbance.

[0073] Step 7.3, traverse M diff For each value in , if the value is greater than t min and less than t max , then the point can be marked as a dynamic point, and all dynamic point indexes are put into the set S dynamic_index

[0074] Step 7.4: According to the dynamic point index obtained above, all dynamic points in each frame point cloud are removed, and the remaining ones are static points S static_index

[0075] Furthermore, step 8 comprises:

[0076] Step 8.1: According to the dynamic point index set S dynamic_index , by using the filter ExtractIndices, which is a filtering algorithm for point cloud segmentation, it constructs point cloud indexes and extracts point cloud subsets by using index vectors. Therefore, through the index points we input, we can extract the dynamic point cloud subset P corresponding to the given index from the input point cloud. dynamic_i

[0077] Step 8.2: According to the static point index set S static_index , by using the filter ExtractIndices, which is a filtering algorithm for point cloud segmentation, it constructs point cloud indexes and extracts point cloud subsets by using index vectors. Therefore, through the index points we input, we can extract the static point cloud subset P corresponding to the given index from the input point cloud. static_i

[0078] Furthermore, step 9 includes:

[0079] Step 9.1, return to step 6.2, and start processing the next point cloud keyframe until all point cloud keyframes are calculated.

[0080] Step 9.2: Return to step 6.1 and start the transformation process of the image scale of the previous layer of the image pyramid until all point cloud key frames at all image scales are processed.

[0081] Step 9.3: Transform all point cloud subsets obtained into the global coordinate system and overlay them to obtain the final dynamic map. Final static map

[0082] Step 9.4: Export and save the complete static map

[0083] In the embodiments of the present invention, any type of multi-line laser radar and any multi-line laser SLAM system are allowed to be used, which can be input into the static map construction system and dynamic targets can be removed to construct a high-precision static point cloud map.

[0084] The multi-scale static map construction system based on multi-line laser radar point cloud provided by the present invention has the following beneficial effects:

[0085] The present invention is applied to the field of unmanned system positioning and mapping, and can be specifically used for multiple objects and scenes such as service robots, self-driving vehicles, and unknown three-dimensional environment modeling.

[0086] 1. It has excellent interpretability and is easy to process. The three-dimensional point cloud is accurately projected onto a two-dimensional image based on vertical and horizontal angles, taking into account errors such as noise, to achieve data dimensionality reduction. This enables the three-dimensional environment to be solved from a first-person perspective based on digital image processing technology and image features, making things simpler.

[0087] 2. The present invention minimizes the error marking rate and error, and provides a high-precision static map. By using an image pyramid, the image of a single scale obtained by projection is transformed into multiple scales. The dynamic point cloud set obtained at the bottom maximum scale must include a true set and an error set. The present invention inputs the first dynamic point cloud set into the upper scale of the image pyramid, transforms the scale, and then iterates and marks it again for the second time. In this way, it is effectively marked and a new dynamic point cloud set is obtained, which will filter and correct the erroneous dynamic point cloud in the first marking. And so on, it is continuously iterated to minimize the marking error rate.

[0088] 3. The present invention makes full use of memory and speeds up the operation: In the most time-consuming point cloud projection stage, since it is inevitable to traverse each point in the point cloud in order to ensure accuracy, we use OMP multi-threading to solve the problem of thread concurrency safety and give full play to the multi-core processor performance of the industrial computer. At the same time, we perform downsampling processing at the key frame and global map levels, and use different downsampling algorithms according to different point cloud scales. BRIEF DESCRIPTION OF THE DRAWINGS

[0089] Figure 1 This is a model structure diagram of a multi-scale static map construction system based on multi-line lidar point cloud;

[0090] Figure 2 is a schematic diagram of a multi-scale static map construction system according to a specific embodiment of the present invention;

[0091] Figure 3 Schematic diagram of a point cloud map projected two-dimensional image according to a specific embodiment of the present invention

[0092] Figure 4 It is the static construction result of the specific implementation mode of the present invention; DETAILED DESCRIPTION

[0093] In order to make the technical personnel in the technical field better understand the present invention, the technical scheme in the embodiment of the present invention will be clearly and completely described below in combination with the drawings in the embodiment of the present invention. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0094] This embodiment proposes a multi-scale static map construction system based on multi-line laser radar point cloud, and its model structure diagram is shown in Figure 1 , the flow chart is shown in Figure 2 The following steps are involved:

[0095] Step 1: The laser radar collects the point cloud frame sequence, and the SLAM system collects the pose sequence

[0096] Step 2: Extract the keyframes and the poses corresponding to the frames by timestamp

[0097] Step 3: Downsampling of keyframe point cloud 3D voxels

[0098] Step 4: Splice all keyframe point clouds into the original map in the global coordinate system.

[0099] Step 5: Build an image pyramid and determine whether the image pyramid has reached the top layer. Otherwise, iterate the previous layer of the pyramid.

[0100] Step 6: For each layer of image, project the keyframe 3D point cloud and the original map to a 2D image respectively.

[0101] Step 7: Compare the difference between the two images with the threshold to determine the index of the dynamic target

[0102] Step 8: Map the position of the dynamic target back to the index of the point cloud in the three-dimensional point cloud, and mark and extract the dynamic points and static points respectively.

[0103] Step 9: Determine whether the top layer of the image pyramid has been reached. If not, return to step 6. If yes, the static map is constructed and the static map and dynamic point set are output.

[0104] The preset parameters are:

[0105] v image_vfov Preset directional field of view for 2D images

[0106] v image_hfov is the horizontal field of view of the two-dimensional image

[0107] d downsample_size is the point cloud downsampling parameter of the local frame and the global map

[0108] S multi_resolution_list For each preset scale sequence of the image pyramid

[0109] n parallel_cores The number of concurrent threads for projecting 3D point clouds to 2D images

[0110] r keyframe_range Keyframe selection range

[0111] M extrinsic_lidar Radar's extrinsic matrix

[0112] Among them, step 1, use multi-line laser radar to obtain road driving data and output each frame of laser point cloud with a timestamp, and output the posture data of each frame through three-dimensional laser SLAM under the industrial computer. The laser radar is located above the center of the robot or vehicle, and each frame of laser point cloud includes the intensity information received after scanning multiple laser lines and the relative Cartesian coordinates relative to the laser receiver. The SLAM system used can be any three-dimensional multi-line laser SLAM system that takes multi-line laser radar data and nine-axis IMU data as input and can output the corresponding posture of each frame of laser point cloud.

[0113] Among them, step 2 of extracting key frames and poses corresponding to frames according to timestamps includes:

[0114] Step 2.1, read or subscribe to the message queue scanlist of the asynchronous communication scanned in real time according to the persistent file path storing the laser point cloud scan frame, and keyframe_range Read the key frame and store it into the memory of the industrial computer.

[0115] Step 2.2: Read the iteratively optimized pose estimate corresponding to each laser point cloud scanning frame output by the laser SLAM system. The pose is the pose in the local coordinate system of the radar, and obtain the transformation matrix set P from the radar coordinate system to the current pose. direct .

[0116] Step 2.3: Invert all the poses read in 2.2 to obtain the transformation matrix set P from the current pose to the radar coordinate system. reverse

[0117] In step 3, the key frame point cloud 3D voxel downsampling is performed by using each laser point cloud scan frame input in step 2, according to the preset d downsample_size Downsampling parameters, by creating a three-dimensional voxel grid in the point cloud data, and then using the center of gravity of each voxel to approximate the other points in the voxel, thereby downsampling the keyframes, which is convenient for subsequent processing speed and reduces memory usage. Finally, the downsampled keyframe set S is obtained local_after

[0118] Among them, the process of stitching all keyframe point clouds into the original map in the global coordinate system in step 4 includes:

[0119] Step 4.1: Use the key frame point cloud and pose read in step 3 to extract the corresponding key frame s in sequence according to the index local_after The pose T corresponding to the key frame direct

[0120] Step 4.2: Use the external parameter transformation matrix to transform s local_after Transform from the radar coordinate system to the main baselink coordinate system to obtain slocal_after_then

[0121] Step 4.3: Use the T read in 4.1 direct Transform the pose from the main baselink coordinate system to the global coordinate system to obtain s global_i

[0122] Step 4.4, according to the index, iterate all s global_i Splice to get the original global map P global

[0123] Step 4.5: According to the preset downsampling parameter d downsample_size For the original global map P global Downsampling is performed. At this time, the map is too large and the point cloud data is too large to be downsampled using the traditional voxel grid in step 3. Instead, a more complex data structure, the octree, should be used to achieve downsampling through octree-based voxel filtering. Finally, the downsampled global map P is obtained. global_after

[0124] Among them, step 5 is to build an image pyramid and determine whether the image pyramid has reached the top layer, otherwise iterate the previous layer of the pyramid. multi_resolution_list ,The preset parameters depend on the resolution of the radar, which is reflected in the number of lines of the multi-line laser radar. Multi-resolution is collected from top to bottom, and an image pyramid sequence is constructed, which contains image resolution parameters of different scales and is output to step 6 for separate iterations.

[0125] like Figure 3 As shown, step 6 of projecting the key frame three-dimensional point cloud and the original map to two-dimensional images for each layer of image includes:

[0126] Step 6.1: Based on the image pyramid in step 5, we iterate through the image resolution from bottom to top. According to the vertical angle and horizontal field of view of the lidar, we preset the parameter v image_vfov , v image_hfov , the current scale r i *v image_vfov Get the number of image pixels in the vertical direction n num_row , and then r i *v image_hfov Get the number of image pixels in the horizontal direction n num_col

[0127] Step 6.2: Traverse S local_after Each key frame in n num_row and n num_col Initialize the image matrix M local_after

[0128] Step 6.3: For each key frame s local_after , then enable multithreading, the number of threads is determined by the number of cores of the industrial computer as n parallel_cores , each thread has its own non-shared variables, so there will be no interference between each other, and no thread safety issues will be caused. Here, we choose not to lock. At this time, the n threads will be allocated, scheduled and executed by the operating system.

[0129] Step 6.4: For each thread, the key is to calculate the precise position of each point in the point cloud mapped to the image. First, the vertical azimuth angle p of each point needs to be calculated. i_va =arctan(p i_y / p i_x )*(180 / pi), horizontal azimuth angle p i_ha =arctan(z / sqrt(x^2+z^2))*(180 / pi)

[0130] Step 6.5: The vertical index p of the point cloud in the image can be calculated index_x , and then map the three-dimensional point cloud to a two-dimensional image, the formula is

[0131]

[0132] Step 6.6: The horizontal position index p of the point cloud in the image can be calculated index_y , and then map the three-dimensional point cloud to a two-dimensional image, the formula is

[0133]

[0134] Step 6.7: According to the known index p of the point cloud in the image index_x 、p index_y , if the laser intensity p i <v x,y This indicates that the pixel is not occupied, so the laser intensity of each point is assigned to the image matrix M as the value of the single-channel image. local_after , otherwise it is occupied and no processing is performed. Then the projected point cloud is obtained, and the coordinate system is now located in the laser local coordinate system. The two-dimensional image obtained at this time is the original unprocessed image, that is, it has both static coordinate points and dynamic coordinate points.

[0135] Step 6.8: Downsample the global map point cloud P global_after , through the transformation matrix p reverse , radar external parameter matrix, transform the point cloud from the global coordinate system to the radar coordinate system to obtain P global_after_local , so that the coordinate system is consistent with the point cloud coordinate system of the current key frame, which can facilitate the comparison of the two images in the subsequent steps.

[0136] Step 6.9, initialize a size of n num_row and n num_col The single-channel image matrix M global_after_local , used to map s global_after_local ; Initialize a mapping matrix M for mapping point indices and image indices point_to_image

[0137] Step 6.10: For each s transformed to the same coordinate system as the current keyframe global_after_local , then enable multithreading, the number of threads is determined by the number of cores of the industrial computer as n parallel_cores Each thread has its own non-shared variables, so they will not interfere with each other and will not cause thread safety issues. Here, we choose not to lock. At this time, the n threads will be allocated and scheduled by the operating system and executed

[0138] Step 6.11: Similar to steps 6.5 and 6.6, replace s global_after_local Similarly, it is mapped to the two-dimensional image M global_after_local

[0139] Step 6.12: According to the known index p of the point cloud in the image index_x 、p index_y , if the laser intensity p i <v x,y This indicates that the pixel is not occupied, so the laser intensity of each point is assigned to the image matrix M as the value of the single-channel image. global_after_local At the same time, the most important thing is to index the point cloud p index Assign the mapping matrix M between the point cloud and the image point_to_image

[0140] Wherein, step 7, comparing the difference between the two images with a threshold value to determine the index of the dynamic target, includes:

[0141] Step 7.1: M obtained from steps 6.7 and 6.12 local_after and M global_after_local , calculate the difference M diff

[0142] Step 7.2: Traverse each pixel of the image, make high-precision judgments, and calculate the threshold t min =t adaptive *p intensity , that is, the adaptive parameters are combined with the original M local_after The corresponding pixel value is the laser intensity p intensity Multiply and set the upper limit threshold t according to the maximum detection distance of the laser max , in order to prevent the error of invalid points caused by noise disturbance.

[0143] Step 7.3, traverse M diff For each value in , if the value is greater than t min and less than t max , then the point can be marked as a dynamic point, and all dynamic point indexes are put into the set S dynamic_index

[0144] Step 7.4: According to the dynamic point index obtained above, all dynamic points in each frame point cloud are removed, and the remaining ones are static points S static_index

[0145] Wherein, step 8 comprises:

[0146] Step 8.1: According to the dynamic point index set S dynamic_index , by using the filter ExtractIndices, which is a filtering algorithm for point cloud segmentation, it constructs point cloud indexes and extracts point cloud subsets by using index vectors. Therefore, through the index points we input, we can extract the dynamic point cloud subset P corresponding to the given index from the input point cloud. dynamic_i

[0147] Step 8.2: According to the static point index set S static_index , by using the filter ExtractIndices, which is a filtering algorithm for point cloud segmentation, it constructs point cloud indexes and extracts point cloud subsets by using index vectors. Therefore, through the index points we input, we can extract the static point cloud subset P corresponding to the given index from the input point cloud. static_i

[0148] Among them, step 9, judge whether to reach the top layer of the image pyramid, if not, return to step 6; if yes, the static map is built, and the static map and dynamic point set are output, including:

[0149] Step 9.1, return to step 6.2, and start processing the next point cloud keyframe until all point cloud keyframes are calculated.

[0150] Step 9.2: Return to step 6.1 and start the transformation process of the image scale of the previous layer of the image pyramid until all point cloud key frames at all image scales are processed.

[0151] Step 9.3: Transform all point cloud subsets obtained into the global coordinate system and overlay them to obtain the final dynamic map. Final static map

[0152] Step 9.4: Output and save the complete static map. The result is as follows: Figure 4 shown.

Claims

1. A multi-scale static map construction method based on multi-line laser radar point cloud, characterized in that: The steps to implement this method include: Step 1: Use multi-line laser radar to obtain road driving data and output laser point cloud with timestamp for each frame, and output the posture data of each frame through 3D laser SLAM system under industrial computer; The preset parameters are: v image_vfov is the preset directional field of view of the two-dimensional image; v image_hfov is the horizontal field of view of the two-dimensional image; d downsample_size is the point cloud downsampling parameter of the local frame and the global map; S multi_resolution_list Each preset scale sequence of the image pyramid; n parallel_cores The number of concurrent threads for projecting 3D point clouds to 2D images; r keyframe_range The selection range of the key frame; M extrinsic_lidar is the external parameter matrix of the radar; Step 2: The dynamic target removal system reads the laser point cloud frames within the user-specified range according to the timestamp, and extracts the key frames and the posture data of the corresponding key frames; Step 3: To speed up the calculation and improve memory utilization, the point cloud of the key frame is downsampled; Step 4: In the global coordinate system, use the posture information of each frame point cloud to stitch all key frame point clouds into the original map; Step 5: Establish a multi-scale image pyramid and iterate each layer of the pyramid; Step 6: For each layer of image, project each key frame 3D point cloud and the original map in the local coordinate system to a 2D image according to the specified scale; Step 7: At the pixel level, the difference between the two images is calculated and compared with the threshold obtained by the experiment to determine the position of the dynamic target; Step 8: Map the position of the dynamic target in the corresponding two-dimensional image back to the index of the point cloud in the three-dimensional point cloud, and mark and extract the dynamic points and static points respectively; Step 9: The point cloud subset marked as dynamic points is used as the entire point cloud of the next iteration of the image pyramid. This process is repeated until all scales of the image pyramid are traversed, and the error recognition rate of dynamic points is reduced to a minimum, thereby constructing an accurate static point cloud map. The step 4 comprises: Step 4.1: From the key frame point cloud and pose read in step 2, extract the corresponding key frame s in sequence according to the index local_after The pose T corresponding to the key frame direct ; Step 4.2: Use the external parameter transformation matrix to transform s local_after Transform from the radar coordinate system to the baselink coordinate system to obtain s local_after_then ; Step 4.3: Use the T read in 4.1 direct Transform the pose from the main baselink coordinate system to the global coordinate system to obtain s global_i ; Step 4.4, according to the index, iterate all s global_i Splice to get the original global map P global ; Step 4.5: According to the preset downsampling parameter d downsample_size For the original global map P global Downsampling is performed. At this time, the map is too large and the point cloud data is too large to be downsampled using the traditional voxel grid in step 3. Instead, a more complex data structure, the octree, should be used to achieve downsampling through octree-based voxel filtering. Finally, the downsampled global map P is obtained. global_after ; The step 5 comprises: According to the preset S multi_resolution_list , the preset parameters depend on the resolution of the radar, which is reflected in the number of lines of the multi-line laser radar; collect multi-resolution from top to bottom, build an image pyramid sequence, including image resolution parameters of different scales, and output them to step 6 for separate iterations; The step 6 comprises: Step 6.1: According to the image pyramid in step 5, traverse the image resolution from bottom to top and iterate. According to the angle perceived by the laser radar in the vertical direction and the field of view in the horizontal direction, preset the parameter v image_vfov , v image_hfov , according to the current scale r i *v image_vfov Get the number of image pixels in the vertical direction n num_row , and then according to r i *v image_hfov Get the number of image pixels in the horizontal direction n num_col ; Step 6.2: Traverse S local_after Each key frame in n num_row and n num_col Initialize the image matrix M local_after Step 6.3: For each key frame s local_after , then enable multithreading, the number of threads is determined by the number of cores of the industrial computer as n parallel_cores , each thread has its own non-shared variables, so there will be no interference between each other and no thread safety issues will be caused. Here, we choose not to lock; at this time, the n threads will be allocated, scheduled and executed by the operating system; Step 6.4: For each thread, calculate the precise position of each point in the point cloud mapped to the image; first, calculate the vertical azimuth angle p of each point i_va =arctan(p i_y / p i_x )*(180 / pi), horizontal azimuth angle p i_ha =arctan(z / sqrt(x^2+z^2))*(180 / pi); Step 6.5: Calculate the vertical index p of the point cloud in the image index_x , and then map the three-dimensional point cloud to a two-dimensional image, the formula is Step 6.6: The horizontal position index p of the point cloud in the image can be calculated index_y , and then map the three-dimensional point cloud to a two-dimensional image, the formula is Step 6.7: According to the known index p of the point cloud in the image index_x 、p index_y , if the laser intensity p i <v x,y This indicates that the pixel is not occupied, so the laser intensity of each point is assigned to the image matrix M as the value of the single-channel image. local_after , otherwise it is occupied and no processing is performed; then the projected point cloud is obtained, and the coordinate system is now located in the laser local coordinate system. The two-dimensional image obtained at this time is the original unprocessed image, that is, it has both static coordinate points and dynamic coordinate points; Step 6.8: Downsample the global map point cloud P global_after , through the transformation matrix p reverse , radar external parameter matrix, transform the point cloud from the global coordinate system to the radar coordinate system to obtain P global_after_local , so that the coordinate system is consistent with the point cloud coordinate system of the current key frame, which can facilitate the comparison of the two images in the subsequent steps; Step 6.9, initialize a size of n num_row and n num_col The single-channel image matrix M global_after_local , used to map s global_after_local ; Initialize a mapping matrix M for mapping point indices and image indices point_to_image ; Step 6.10: For each s transformed to the same coordinate system as the current keyframe global_after_local , then enable multithreading, the number of threads is determined by the number of cores of the industrial computer as n parallel_cores , each thread has its own non-shared variables, so there will be no interference between each other and no thread safety issues will be caused. Here, we choose not to lock; at this time, the n threads will be allocated, scheduled and executed by the operating system; Step 6.11: Similar to steps 6.5 and 6.6, replace s global_after_local Similarly, it is mapped to the two-dimensional image M global_after_local ; Step 6.12: According to the known index p of the point cloud in the image index_x 、p index_y , if the laser intensity p i <v x,y This indicates that the pixel is not occupied, so the laser intensity of each point is assigned to the image matrix M as the value of the single-channel image. global_after_local , and the point cloud index p index Assign the mapping matrix M between the point cloud and the image point_to_image .

2. The multi-scale static map construction method based on multi-line laser radar point cloud according to claim 1, characterized in that: The laser radar is located directly above the center of the robot or vehicle, and each frame of laser point cloud includes the intensity information received after scanning multiple laser lines and the relative Cartesian coordinates relative to the laser receiver; the SLAM system used can be any three-dimensional multi-line laser SLAM system that takes multi-line laser radar data and nine-axis IMU data as input and can output the corresponding posture of each frame of laser point cloud.

3. The multi-scale static map construction method based on multi-line laser radar point cloud according to claim 2, characterized in that: The step 2 comprises: Step 2.1, read or subscribe to the message queue scanlist of the asynchronous communication scanned in real time according to the persistent file path storing the laser point cloud scan frame, and keyframe_range Read the key frame and store it in the memory of the industrial computer; Step 2.2: Read the iteratively optimized pose estimate corresponding to each laser point cloud scanning frame output by the laser SLAM system. The pose is the pose in the local coordinate system of the radar, and obtain the transformation matrix set P from the radar coordinate system to the current pose. direct ; Step 2.3: Invert all the poses read in step 2.2 to obtain the transformation matrix set P from the current pose to the radar coordinate system. reverse .

4. The multi-scale static map construction method based on multi-line laser radar point cloud according to claim 3, characterized in that: The step 3 comprises: For each laser point cloud scan frame input in step 2, according to the preset d downsample_size Downsampling parameters, by creating a three-dimensional voxel grid in the point cloud data, and then using the center of gravity of each voxel to approximate the other points in the voxel, so as to achieve downsampling of the key frame, which is convenient for subsequent processing speed and reduces memory usage; finally, the downsampled key frame set S is obtained local_after .

5. The multi-scale static map construction method based on multi-line laser radar point cloud according to claim 1, characterized in that: The step 7 comprises: Step 7.1: M obtained from steps 6.7 and 6.12 local_after and M global_after_local , calculate the difference M diff ; Step 7.2: Traverse each pixel of the image, make high-precision judgments, and calculate the threshold t min =t adaptive *p intensity , that is, the adaptive parameters are combined with the original M local_after The corresponding pixel value is the laser intensity p intensity Multiply and set the upper limit threshold t according to the maximum detection distance of the laser max , to prevent the error of invalid points caused by noise disturbance; Step 7.3, traverse M diff For each value in , if the value is greater than t min and less than t max , then the point can be marked as a dynamic point, and all dynamic point indexes are put into the set S dynamic_index , set S dynamic_index is a dynamic point index set; Step 7.4: According to the dynamic point index obtained above, all dynamic points in each frame of point cloud are removed, and the remaining ones are static points, S static_index Is a static point index collection.

6. The method for constructing a multi-scale static map based on a multi-line laser radar point cloud according to claim 5, characterized in that: The step 8 comprises: Step 8.1: According to the dynamic point index set S dynamic_index , by using the filter ExtractIndices, which is a filtering algorithm for point cloud segmentation, it constructs point cloud indexes and extracts point cloud subsets by using index vectors. Therefore, through the index points we input, we can extract the dynamic point cloud subset P corresponding to the given index from the input point cloud. dynamic_i ; Step 8.2: According to the static point index set S static_index , by using the filter ExtractIndices, which is a filtering algorithm for point cloud segmentation, it constructs point cloud indexes and extracts point cloud subsets by using index vectors. Therefore, through the index points we input, we can extract the static point cloud subset P corresponding to the given index from the input point cloud. static_i .

7. The method for constructing a multi-scale static map based on a multi-line laser radar point cloud according to claim 6, characterized in that: The step 9 comprises: Step 9.1, return to step 6.2, and start processing the next point cloud keyframe until all point cloud keyframes are calculated. Step 9.2, return to step 6.1, and start the transformation process of the image scale of the previous layer of the image pyramid until all point cloud key frames at all image scales are processed; Step 9.3: Transform all point cloud subsets obtained into the global coordinate system and overlay them to obtain the final dynamic map. Final static map Step 9.4: Export and save the complete static map.

Images