Garden Map Construction Method Based on Multi-Scale Distance Map and Point Cloud Semantic Segmentation

Abstract

The present invention provides a method for constructing a garden map based on multi-scale distance maps and point cloud semantic segmentation. First, dynamic / static objects in the garden environment are defined. A convolutional neural network is used to project the scanned objects onto the distance map to extract semantic information, and the distance map with semantic labels is processed by morphological closing operation in graphics to optimize the dynamic area. Then, the semantic information is used to refine the detection sensitivity of the dynamic / static areas through weight adjustment. The classification task of dynamic and static points is completed by checking the visibility of map points in the plane of the projected distance image. Finally, the tasks of removing and restoring the point cloud are realized under multi-scale distance maps, and the construction and optimization of the static map are completed. The method for constructing a garden map of the present invention improves the probability of misdeleting and wrongly deleting static objects in the traditional method, improves the point cloud registration accuracy when the map is reused, and can better retain the complete information of the vegetation appearance in the garden environment, providing rich prior information for the subsequent work of the garden pruning robot.

Description

Technical Field

[0001] The present invention relates to the technical field of map construction, and particularly relates to a method for constructing a static map of a garden environment based on a multi-scale distance map and point cloud semantic segmentation. Background Art

[0002] Establishing a rich and accurate environmental map is an extremely important link in the autonomous operation process of a garden hedge trimming robot. When using sensors such as lidar and cameras to perform environmental modeling, it is inevitable to also model some pedestrians on the roadside and passing vehicles. These dynamic objects leave more or less traces on the map, which are not conducive to subsequent autonomous navigation tasks and affect the realization of the overall function. For a garden trimming robot, the garden environment not only contains a large number of dynamic objects, but also contains a large amount of vegetation model information for precise positioning. Most of the branches and leaves of the vegetation will sway with the wind and thus also exist as dynamic objects in the environment. At the same time, inaccurate pose estimation will also cause a large number of static points to be misjudged as dynamic targets. If traditional dynamic filtering methods are used, not only will the dynamic objects in the environment be filtered out, but most of the appearance information of the vegetation in the garden environment will also be damaged. In subsequent trimming work, it may be difficult to achieve the expected trimming expectation due to information loss. Therefore, removing dynamic objects and ensuring that the vegetation model is not damaged has become a major problem in garden environment modeling.

[0003] In order to remove dynamic objects in the environment, there are currently two mainstream methods. One is in the sensor scanning stage, continuously querying the consistency with the historical world model to detect dynamic objects in the current scan and filtering them out. This method aims to improve the robustness of the Simultaneous Localization and Mapping (SLAM) system in positioning and reduce the impact of dynamic objects on environmental mapping and self-positioning. Another method is to perform post-processing on the map that meets the requirements after scanning is completed, aiming to construct a map without potential errors and capable of retaining effective environmental information.

[0004] The establishment of a laser static map is a classic topic. Traditional methods mainly rely on the constructed point cloud map to complete the removal of dynamic points. A common method is to use voxel rays for casting. This method requires dense laser scanning and very accurate pose information. Therefore, it also brings a large amount of computation to the system. To solve the problem of large computation, a visibility-based method has been proposed. This type of method associates a query point with a mapped point within a narrow field of view. In recent years, the distance map-based method has also shown good performance. This method uses the difference between the query scan and the known sub-map as a way to detect dynamic objects, and optimizes the map by using multi-resolution to restore static points. With the development of learning-based methods and semantic SLAM in dynamic scenarios, mapping in an environment containing dynamic objects has shown good results. This method usually uses a neural network to predict the probability of potential moving objects, or efficiently and accurately separates dynamic and static targets by using semantic information.

[0005] The above dynamic filtering algorithms have achieved good results in urban environments. However, in garden environments, the branches and leaves of vegetation often appear as dynamic objects in the environment. Most existing dynamic removal algorithms only consider whether an object is moving, without considering whether it should be removed. This leads to the removal of some slightly moving vegetation, which is not beneficial for garden maps. Since the appearance shape of vegetation is an important information for garden pruning robots, it cannot be removed as a dynamic object when building a static map. Existing algorithms cannot be well adapted to garden environments. Protecting key information in the garden environment while filtering out dynamic objects is a key problem that needs to be solved.

[0006] Therefore, there is an urgent need to optimize the method for constructing a static map of the garden environment to solve the above problems. Summary of the Invention

[0007] The purpose of the present invention is to provide a method for constructing a garden map based on a multi-scale distance map and point cloud semantic segmentation, including the following steps:

[0008] Step S1, define moving / static objects in the garden environment: perform spherical projection on the point cloud in the form of a distance map, then use a convolutional neural network on the distance map to extract semantic information of the scanned objects, and perform semantic segmentation on the distance map to obtain a distance map containing semantic labels and depth information;

[0009] Step S2, perform a closing operation in the image processing algorithm on the distance map containing semantic labels and depth information to optimize the dynamic area in the entire distance map, and further fill the phenomenon that the target image coverage is incomplete due to incomplete semantic segmentation, to obtain the semantic labels and label probabilities of each frame of point cloud;

[0010] Step S3: Use semantic information to assign different weights to the point clouds in the dynamic / static regions on the distance map. Increase the weight for the region to be removed to improve the detection sensitivity, and decrease the weight for the region to be retained to reduce the detection sensitivity, thereby adjusting the detection sensitivity of the point clouds in the dynamic / static regions.

[0011] Step S4: Add the distance map to the dynamic detection model to establish a multi-time dimension matching model, and initially complete the classification task of dynamic and static points based on the residual map.

[0012] Step S5: Continuously change the pixel resolution of the distance map, input the distances with different pixel resolutions into the multi-time dimension matching model in Step S4, perform the classification of dynamic and static points according to the method in Step S4, then integrate the number of times the point clouds are classified as dynamic / static at each pixel resolution, conduct comprehensive calculations based on the classification results at different resolutions, finally determine the classification of dynamic / static points, and then based on the classification of dynamic / static points in each frame and the pose situation of each frame, construct a static point cloud map from the static point clouds and the corresponding poses in each frame, and finally establish a static map.

[0013] Further, in Step S1, the pixel coordinates of the distance map are obtained using the following method: Through the mapping ∏: R 3 →R 2 Convert each point P(x, y, z) in the point cloud of each frame into spherical coordinates and finally into pixel coordinates. The conversion formula is as follows:

[0014]

[0015] In the formula, (u, v) represents the position corresponding to the laser point in the image coordinates, (h, w) are the height and width of the distance map, f = f up +f down represents the vertical field of view of the sensor, r = ||p i || 2 represents the distance information of the laser point to the sensor; in this process, each point p i corresponds to a list of tuples of a pair of image coordinates (u, v). The laser points in the same pixel can be represented by different indices, and finally the point cloud information is stored in an image with a resolution of 64×900. Additionally, extra information can be added as an extra channel to the image storage.

[0016] Further, in step S1, semantic segmentation uses the RangeNet++ convolutional neural network to predict the point cloud and generate semantic information. Using the kitti dataset as the training set, the dynamic objects (such as cars, bicycles, and people) on the road that affect the autonomous navigation and path planning of the garden pruning robot are trained for the network. Then, the RangeNet++ convolutional neural network projects each scan frame into a distance map for segmentation, classifying the segmented dynamic objects (at this time, there is no distinction between moving and stationary objects, only the classes that are more likely to be dynamic objects are segmented) into one category, and the static objects that should not be filtered out (except for the defined dynamic objects) into another category; the semantic labels and the probabilities of the corresponding labels in the segmentation are inferred from the distance map of each frame, and further processing will be done on the labels with different probabilities in the subsequent steps.

[0017] Further, in step S2, a closing operation of dilation + erosion is used to optimize the dynamic region in the entire distance map. The method is as follows:

[0018] First, the definition of the value of a single pixel point is as follows:

[0019]

[0020] where p represents the point cloud within the pixel point (i, j), r(p) represents the distance value of the point cloud, and r k represents the distance value of the point closest to the distance sensor in a single pixel;

[0021] The entire closing operation is carried out in two steps in total;

[0022] In the first step, the original semantic mask S output by the RangeNet++ convolutional neural network sem is dilated. A structuring element of Size(3×3) and a cross structure is selected for expansion, and the definition is as follows:

[0023]

[0024] represents the dilation operation. The dilation operation of image A is to generate an image that can completely contain structure B, and then it is associated with the r value in the distance map; the regions where the distance difference between the dilated region and the adjacent region (the central pixel of the structuring element) exceeds the threshold θ will not be considered as dilation objects, which is expressed as:

[0025] ||r s d -r s || < θ

[0026] r s , respectively represent the original semantic mask S sem and the dilated semantic mask The pixel value of

[0027] In the second step, the dilated semantic mask is eroded. The structuring element of Size(3×3) and cross structure is still selected for erosion, and the definition is as follows:

[0028]

[0029] Θ represents the erosion operation. The erosion operation of image A is to find the pixel points in the image that can completely contain the structuring element B; the θ region where the distance difference between the eroded region and the adjacent region (the central pixel of the structuring element) is less than the threshold will not be considered as the erosion object, which is expressed as:

[0030] ||r s e -r s d ||<θ

[0031] They are respectively the pixel values of the dilated semantic mask and the eroded semantic mask .

[0032] Among them, the method of assigning different weights to the point clouds in the dynamic / static regions on the distance map in step S3 is as follows:

[0033] In step S1, the defined "dynamic object" has been segmented and optimized on the distance map. In step S2, the label probability of each point cloud has been obtained through the RangeNet++ convolutional neural network. The greater the probability, the more likely the point is to be a real dynamic point. A semantic label channel, a label probability channel, and a semantic weight channel are added to each point cloud of the dynamic object defined in step S1, which is expressed as where l Dynamic =1, l static =0, (x, y, z) is the position of the point cloud relative to the lidar coordinate system, l Dynamic , l static represent the dynamic label and the static label respectively, and are respectively the weights of the dynamic and static points in a single scan; n is the total number of point clouds in a single scan, and p is the label probability of the point cloud;

[0034] The pose information of each frame of point cloud obtained by using the SLAM odometer (visual odometer) is used to calculate the residual value of each pixel between the current frame and the transformed frame, and the definition is as follows:

[0035]

[0036] Among them represents frame S A The relative positional relationship with frame S B represents the difference between two pixel points represents S A and S B The corresponding pixel value represents the value of a single pixel point in the B frame. The specific definition of the introduced semantic weight is as follows:

[0037]

[0038] r represents the original distance from the laser point to the sensor, r s represents the distance value after adding the semantic weight represents the weight information

[0039] By increasing the weight value in the area to be removed to improve the detection sensitivity and reducing the weight value in the area to be retained to reduce the detection sensitivity, the detection sensitivity of the dynamic / static area point cloud can be adjusted.

[0040] Furthermore, in step S4, a method of checking the visibility of map points in the projection distance image plane is used to perform the classification task of dynamic and static points. In the offline mode, a distance image with multiple time dimensions is added to the matching model of the query point and the mapped point to establish a multi-time dimension matching model; specifically as follows:

[0041] First, define a set of sequential scans along the movement of the garden pruning robot, where the current scan is S j , the query sequence is …, S j-3 , S j-2 , S j-1 , S j+1 , S j+2 , S j+3 , …, using the pose information obtained by SALM, calculate the pose relationship between the query sequence and the current scan, and the definition is as follows:

[0042]

[0043]

[0044] Among them, is the pose change relationship between the query sequence S j+n and the current scan frame S j ​​It represents the pose change relationship between two adjacent frames; when querying and scanning the sequence, not only the historical query sequence is used, but also the future query sequence is added to the detection model to overcome problems such as occlusion and shadow in a single scan, and it is more likely to detect some dynamic objects with similar speeds;

[0045] Then calculate the residual value between each sequence point in the current distance map and the mapped points in other distance maps, and mark the sequence points as dynamic / static according to the threshold, obtain the number of times each pixel point is marked with different states, and then obtain the number of times each point cloud is marked as dynamic / static points. And an adaptive threshold method based on distance is used to better classify dynamic / static points, which is defined as follows,

[0046]

[0047] where τ is the threshold for marking dynamic points, τ D is a fixed threshold, α is an adjustment coefficient, and r is the distance from the laser point to the sensor.

[0048] Furthermore, in step S5, the method for constructing the static point cloud map is as follows:

[0049] Gradually reduce the resolution of the distance map, and input the distances with different pixel resolutions into the multi-time dimension matching model in step S4, classify dynamic / static points according to the method in step S4, then restore the dynamic / static points to the three-dimensional point cloud space, and then calculate the number of times each point P is marked as a dynamic point n Dynamic and a static point n Static (the number of mapped sequence scans is n Dynamic ) in total, and reclassify the dynamic / static points by calculating the score of each sequence point. The specific formula is as follows:

[0050] S(·) = αn Dynamic + βn Static

[0051] where α is the positive weight and β is the negative weight; update and iterate the classification of dynamic / static points by reducing the resolution of the distance map, and finally complete the construction of the static point cloud map by integrating the results of dynamic / static classification under multiple-scale distance maps.

[0052] Furthermore, the method for restoring the dynamic / static points to the three-dimensional point cloud space is as follows:

[0053] Adopt the method of secondary mapping of the original point cloud. If the pixel point corresponding to a certain point is marked as a dynamic point, then delete the point in the original point cloud, and if it is a static point, keep it. Specifically as follows:

[0054]

[0055]

[0056] Among them, F(·) represents the mapping function of Π: R 3 →R 2 .

[0057] Furthermore, in step S5, the method for establishing the static map is as follows:

[0058] According to the pose information {T i , T i+1 , …, T n} saved by SLAM and the processed scan frames perform point cloud stitching as follows:

[0059] M = {M D , M S}

[0060]

[0061] In the formula, M is the original map; M D is the dynamic map; M S is the static map; represents associating the scan frame with the corresponding pose information.

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

[0063] The present invention provides a method for constructing a static map of a garden environment based on a multi-scale distance map and point cloud semantic segmentation, which improves the probability of misdeleting and wrongly deleting static objects in the traditional method, improves the point cloud registration accuracy when the map is reused, and can better retain the complete information of the vegetation appearance in the garden environment according to requirements, providing rich prior information for the subsequent work of the garden pruning robot. Description of the Drawings

[0064] Figure 1 is a flowchart of a method for constructing a garden map based on a multi-scale distance map and point cloud semantic segmentation;

[0065] In Figure 1 , the parts numbered ①②③ represent the point cloud semantic segmentation and optimization processing part of the distance image; the part numbered ④ represents the query point and mapping point matching model part; the part numbered ⑤ represents the dynamic point removal and static point recovery part, and S n represents that at the nth iteration, the point cloud mainly consists of static points, and the size of each part of the box represents the number of points;

[0066] Figure 2 is a visualization process diagram of generating a distance map from the original point cloud;

[0067] Figure 3For the visualization process of semantic segmentation and the dynamic region in the optimized distance map;

[0068] Figure 4 For the visualization process of generating a residual map that assigns different weights to the point clouds of the dynamic / static regions on the distance map using semantic information;

[0069] Figure 5 For the comparative visualization graph of the residual maps at different resolutions;

[0070] Figure 6 For the visualization graph of deleting and restoring the point cloud;

[0071] Figure 7 For the comparative visualization graph of the method of the present invention and other methods. Detailed implementation manners

[0072] The following combines the accompanying drawings to describe in detail the specific implementation manners of the present invention, but it should be understood that the protection scope of the present invention is not limited by the specific implementation manners.

[0073] Unless otherwise clearly stated, throughout the specification and claims, the term "comprising" or its variations such as "including" or "having" etc. will be understood to include the stated elements or components, without excluding other elements or other components.

[0074] Embodiment 1

[0075] Please refer to Figure 1 , a method for constructing a garden map based on multi-scale distance map and point cloud semantic segmentation, comprising the following steps:

[0076] Step S1, defining the dynamic / static objects in the garden environment: performing spherical projection on the point cloud in the form of a distance map, then using a convolutional neural network on the distance map to extract semantic information from the scanned objects, and performing semantic segmentation on the distance map to obtain a distance map containing semantic labels and depth information;

[0077] Among them, in step S1, the pixel coordinates of the distance map are obtained by the following method: through the mapping Π: R 3 →R 2 Converting each point P(x, y, z) in each frame of the point cloud into spherical coordinates, and finally converting it into pixel coordinates. The conversion formula is as follows:

[0078]

[0079] In the formula, (u, v) represents the position corresponding to the laser point in the image coordinates, (h, w) are the height and width of the distance map, f = f up +f down represents the vertical field of view of the sensor, r = ||pi || 2 represents the distance information from the laser point to the sensor; in this process, each point p i corresponds to a list of tuples of a pair of image coordinates (u, v). The laser points in the same pixel can be represented by different indices, and finally the point cloud information is stored in an image with a resolution of 64×900. At the same time, additional information can also be added as an additional channel to the image storage;

[0080] Please refer to Figure 2 , Figure 2 shows the visualization process of generating a distance map from the original point cloud in step S1. First, each point cloud coordinate point is spherically projected, converted into spherical coordinates, and finally unfolded into a distance map. Eventually, the point cloud is converted into pixel coordinates. For a mechanical lidar, such as Velodyne HDL-64E, according to the vertical angular resolution FOV_Up (upper field of view angle) and FOV_Down (lower field of view angle), each of the 64 lasers inside the lidar is oriented at a fixed angle. When rotating one week, a point cloud is formed by calculating the flight time of each laser after reflecting from an object (as shown in Figure 2 -(b)). Taking the lidar as the center of the coordinate system, the point cloud is projected onto a hollow cylinder through spherical mapping (as shown in Figure 2 -(a)), and finally converted into a 2D image through a base coordinate system transformation (as shown in Figure 2 -(c))

[0081] Among them, in step S1, semantic segmentation uses the RangeNet++ convolutional neural network to predict the point cloud and generate semantic information. Using the kitti dataset as the training set, the dynamic objects (such as cars, bicycles, and people) on the road that affect the autonomous navigation and path planning of the garden pruning robot are trained in the network; then, the RangeNet++ convolutional neural network projects each scan frame into the distance map for segmentation, classifying the segmented dynamic objects (at this time, there is no distinction between moving and stationary objects, only the classes that are more likely to be dynamic objects are segmented) into one category, and the static objects that should not be filtered out (except for the defined dynamic objects) into another category; the semantic labels and the probabilities of the corresponding labels in the segmentation are inferred from the distance map of each frame, and further processing will be done on the labels with different probabilities in the subsequent steps.

[0082] Step S2, perform a closing operation in the image processing algorithm on the distance map containing semantic labels and depth information to optimize the dynamic region in the entire distance map, further filling the phenomenon that the target image coverage is incomplete due to incomplete semantic segmentation, and obtaining the semantic labels and label probabilities of each frame of point cloud;

[0083] Among them, in step S2, a closing operation of dilation + erosion is used to optimize the dynamic region in the entire distance map. The method is as follows:

[0084] First, the value definition of a single pixel point is as follows:

[0085]

[0086] Where p represents the point cloud within the pixel point (i, j), r(p) represents the distance value of the point cloud, and r k represents the distance value of the point closest to the distance sensor in a single pixel;

[0087] The entire closing operation process is divided into two steps;

[0088] In the first step, the original semantic mask S output by the RangeNet++ convolutional neural network sem is dilated. A structuring element of Size(3×3) and a cross structure is selected for expansion, and the definition is as follows:

[0089]

[0090] represents the dilation operation. The dilation operation of image A is to generate an image that can completely contain the structure B, and then associate it with the r value in the distance map; the region where the distance difference between the dilated region and the adjacent region (the central pixel of the structuring element) exceeds the threshold θ will not be considered as a dilation object, which is expressed as:

[0091] ||r s d -r s || < θ

[0092] r s , respectively represent the pixel point values of the original semantic mask S sem and the dilated semantic mask ;

[0093] In the second step, the dilated semantic mask is eroded. A structuring element of Size(3×3) and a cross structure is still selected for erosion, and the definition is as follows:

[0094]

[0095] Θ represents the erosion operation. The erosion operation of image A is to find the pixel points in the image that can completely contain the structuring element B; the region where the distance difference between the eroded region and the adjacent region (the central pixel of the structuring element) is less than the threshold θ will not be considered as an erosion object, which is expressed as:

[0096] ||r se -r s d || < θ

[0097] are the pixel values of the dilated semantic mask and the eroded semantic mask respectively.

[0098] Please refer to Figure 3 , Figure 3 which shows the visualization process of semantic segmentation in step S1 and the dynamic region in the optimized distance map in step S2. S raw represents the distance map obtained by spherical mapping from the initial point cloud map, and S sem represents the distance map obtained after semantic segmentation. In this embodiment, only people, cars, motorcycles, and bicycles are trained and predicted. Then, a dilation operation is used to expand the semantic region as much as possible to obtain Finally, an erosion operation is used to remove small regions of boundary labels and incorrect labels to obtain Figure 3 The right side of shows the detailed information of semantic segmentation and optimization processing. The erosion operation can limit the area growth of the connected domain caused by the dilation operation, but there is still a certain amount of area expansion here, which is of course the result desired by the inventor. Compared with the original prediction, after the closing operation, the parts that were not completely segmented are re-included, and the unconnected regions of an object are connected, and a region larger than the target object is taken as the dynamic region.

[0099] Step S3: Use semantic information to assign different weights to the point clouds in the dynamic / static regions of the distance map, increase the weight for the regions to be removed to improve the detection sensitivity, and decrease the weight for the regions to be retained to reduce the detection sensitivity, and adjust the detection sensitivity of the point clouds in the dynamic / static regions. The method is as follows:

[0100] In step S1, the defined "dynamic objects" have been segmented and optimized on the distance map. In step S2, through the RangeNet++ convolutional neural network, the label probability of each point cloud is obtained. The greater the probability, the more likely the point is to be a real dynamic point. For each point cloud of the dynamic objects defined in step S1, a semantic label channel, a label probability channel, and a semantic weight channel are added, denoted as where l Dynamic = 1, l static = 0, (x, y, z) is the position of the point cloud relative to the lidar coordinate system, and l Dynamic , l static represent the dynamic label and the static label respectively, and They are the weights of moving and static points in a single scan; n is the total number of point clouds in a single scan, and p is the label probability of the point cloud.

[0101] The pose information of each frame of point cloud obtained by using SLAM odometer (visual odometer) Calculate the residual value of each pixel between the current frame and the transformed frame, which is defined as follows:

[0102]

[0103] where represents frame S A and frame S B 's relative position relationship, represents the difference between two pixel points, represents S A and S B 's corresponding pixel point value, represents the value of a single pixel point in frame B. The specific definition of introducing semantic weight is as follows:

[0104]

[0105] r represents the original distance from the laser point to the sensor, and r s represents the distance value after adding semantic weight, represents the weight information;

[0106] By increasing the weight in the area to be removed to improve the detection sensitivity and decreasing the weight in the area to be retained to reduce the detection sensitivity, the detection sensitivity of the point cloud in the moving / static area can be adjusted.

[0107] Please refer to Figure 4 , Figure 4 which shows the visualization process of generating the residual map by using semantic information to assign different weights to the point cloud in the moving / static area on the distance map in step S3. By increasing the weight in the area to be removed to improve the detection sensitivity and decreasing the weight in the area to be retained to reduce the detection sensitivity, in the Figure 4 shown residual map, for example, a moving car (box 1), due to its movement trend, the residual pattern is more obvious in the residual map with semantic weight, which also applies to a slowly moving car. For slightly moving branches and leaves (box 2), the residual pattern is less obvious in the residual map with semantic weight, while there are some blurred residual patterns in the original residual pattern. Therefore, semantic information provides greater flexibility for the segmentation of moving objects in the garden environment.

[0108] Step S4, add the distance map to the dynamic detection model, establish a multi-time dimension matching model, and initially complete the classification task of moving and static points according to the residual map;

[0109] Among them, in step S4, the method of checking the visibility of map points in the projection distance image plane is adopted to perform the classification task of dynamic and static points. In the offline mode, a distance image of multiple dimensions of time is added to the matching model of the query point and the mapped point to establish a multi-time dimension matching model. Specifically as follows:

[0110] First, define a set of sequential scans along the movement of the garden pruning robot, where the current scan is S j , the query sequence is …, S j-3 , S j-2 , S j-1 , S j+1 , S j+2 , S j+3 , …, Using the pose information obtained by SALM, calculate the pose relationship between the query sequence and the current scan accordingly, and define as follows:

[0111]

[0112]

[0113] Among them, is the pose change relationship between the query sequence S j+n and the current scan frame S j , represents the pose change relationship between two adjacent frames; when querying and scanning the sequence, not only the historical query sequence is used, but also the future query sequence is added to the detection model to overcome some problems such as occlusion and shadow in a single scan, and it is more likely to be detected when facing some dynamic objects with similar speeds;

[0114] Then calculate the residual value between each sequence point in the current distance map and the mapped point in other distance maps, and mark the sequence points as dynamic / static states according to the threshold, obtain the number of times each pixel point is marked with different states, and then obtain the number of times each point cloud is marked as dynamic and static points. And adopt an adaptive threshold method based on distance to better classify dynamic / static points, and define as follows,

[0115] τ = τ D + α * r

[0116] Among them, τ is the threshold for whether to mark dynamic points, τ D is the fixed threshold, α is the adjustment coefficient, and r is the distance from the laser point to the sensor.

[0117] Please refer to Figure 5 , Figure 5Shows the visualization of the difference map between the current frame and other frames at different resolutions. On the left is the visualization of the difference map between the current frame and other frames at high resolution (0.4° per pixel); on the right is the visualization of the difference map between the current frame and other frames at low resolution (1° per pixel). In box 1 is a cyclist; in box 2 is a building; in box 3 are plants; in box 4 is a slowly moving car. In the distance map, light-colored pixel points represent long distances; in the residual map, the number of yellow points represents the number of dynamic points, and the brightness of the pixel points represents the magnitude of the distance difference. Figure 4 In, the slowly moving car in box 1 shows more residual patterns in the residual map with semantic information, and in the case of low pixel resolution ( Figure 5 box 4 in), the residual patterns are still obvious, and this part will be processed as dynamic points. The slightly moving branches ( Figure 4 box 2 in) also show fewer blurred residual patterns, and in the case of low resolution ( Figure 5 box 3 in), the residual patterns are almost non-existent, and this part will be processed as static points.

[0118] Step S5: Continuously change the pixel resolution of the distance map, input the distances at different pixel resolutions into the multi-time dimension matching model in step S4, perform dynamic and static point classification according to the method in step S4, then integrate the number of times the point cloud is classified as dynamic / static at each pixel resolution, perform comprehensive calculation based on the classification results at different resolutions, finally determine the classification of dynamic / static points, and then according to the classification of dynamic / static points in each frame and the pose situation of each frame, construct a static point cloud map from the static point cloud and the corresponding pose in each frame, and finally establish a static map.

[0119] Among them, in step S5, the method for constructing the static point cloud map is as follows:

[0120] Gradually reduce the resolution of the distance map, input the distances at different pixel resolutions into the multi-time dimension matching model in step S4, perform dynamic and static point classification according to the method in step S4, then restore the dynamic / static points to the three-dimensional point cloud space, and then calculate the total number of times each point P is marked as a dynamic point n Dynamic and a static point n Static (the number of mapped sequence scans is n Dynamic ), and re-classify the dynamic / static points by calculating the score of each sequence point. The specific formula is as follows:

[0121] S(·) = αn Dynamic +βn Static

[0122] In the formula, α is the positive weight and β is the negative weight; the dynamic / static point classification is updated iteratively by reducing the resolution of the distance map, and finally the construction of the static point cloud map is completed by synthesizing the dynamic / static classification results under multiple-scale distance maps.

[0123] Among them, the method for restoring the dynamic / static points to the 3D point cloud space is as follows:

[0124] The method of performing a secondary mapping on the original point cloud is adopted. If the pixel point corresponding to a certain point is marked as a dynamic point, then this point is deleted from the original point cloud; if it is a static point, it is retained. Specifically as follows:

[0125]

[0126]

[0127] In the formula, F(·) represents the mapping function of Π: R 3 →R 2 .

[0128] Among them, in step S5, the method for establishing the static map is as follows:

[0129] According to the pose information {T i , T i+1 , …, T n} saved by SLAM and the processed scan frames perform point cloud stitching as follows:

[0130] M = {M D , M S}

[0131]

[0132] In the formula, M is the original map; M D is the dynamic map; M S is the static map; represents associating the scan frame with the corresponding pose information.

[0133] Please refer to Figure 6 , Figure 6 which shows the visualization diagram of deleting and restoring the point cloud in step S5. The figure is a segment of the 08 sequence of the KITTI dataset. The dynamic points and static points in the original map are separated and respectively form a visualized map. It can be seen here that the number of dynamic points is continuously restored to static points as the resolution decreases, while the number of points in the static map continuously increases and is continuously restored. The static points and dynamic points are generally in a complementary state. The dark points in the original map are the estimated dynamic points, and the point clouds generated by plants are in the two boxes, box 1 and box 2.

[0134] Please refer to Figure 7 , Figure 7 which shows the visualization comparison of the method proposed by the present invention with other methods. In the figure, for the Rellis-3d dataset sequences 01 (from frame 1800 to 2000) and 02 (from 1200 to 1500), the original map is on the top, and the static comparison maps of multiple methods and the method of the present invention are on the bottom. Among them, the traces on the road are the traces left by dynamic objects on the map (the right side is the partial enlarged view). At low pixel resolution, the correspondence between the query point and the mapping point is relatively easy, which reduces the motion blur problem caused by inaccurate motion estimation ( Figure 5 the bottom of the bicycle in box 1, the edge of the pole in box 2), and the dynamic points marked at higher pixel resolution ( Figure 6 the plant in box 1) are re-marked as static points at low resolution ( Figure 6 the plant in box 2), and it can be seen that the dynamic points decrease correspondingly, showing a complementary state overall.

[0135] Through the above steps, the targeted point cloud filtering work can be realized, and the construction and enhancement optimization of the static map of the garden environment can be completed.

[0136] The method for constructing a static map of a garden environment based on the combination of multi-scale distance maps and point cloud semantic segmentation provided by the present invention improves the probability of misdeleting and wrongly deleting static objects in traditional methods, improves the point cloud registration accuracy when the map is reused, and can better retain the complete information of the vegetation appearance in the garden environment according to requirements, providing rich prior information for the subsequent work of garden pruning robots.

Claims

1. A method for constructing a garden map based on multi-scale distance maps and point cloud semantic segmentation, characterized in that, it includes the following steps: Step S1, define moving / static objects in the garden environment: perform spherical projection on the point cloud in the form of a distance map, then use a convolutional neural network on the distance map to extract semantic information from the scanned objects, perform semantic segmentation on the distance map to obtain a distance map containing semantic labels and depth information, and define moving / static objects in the garden environment according to the semantic labels; Step S2, perform a closing operation in the image processing algorithm on the distance map containing semantic labels and depth information to optimize the dynamic region in the entire distance map, and further fill the phenomenon that the target image coverage is incomplete due to incomplete semantic segmentation, to obtain the semantic labels and label probabilities of each frame of point cloud; Step S3, assign different weights to the point clouds in the moving / static regions on the distance map, increase the weight of the region to be removed to improve the detection sensitivity, and reduce the weight of the region to be retained to reduce the detection sensitivity, so as to adjust the detection sensitivity of the point clouds in the moving / static regions; among them, the region to be removed is the dynamic region, and the region to be retained is the static region; Step S4, establish a multi-time dimension matching model using the distance map, and initially complete the classification task of moving and static points according to the residual map of the distance map; Step S5, continuously change the pixel resolution of the distance map, input the distances with different pixel resolutions into the multi-time dimension matching model in Step S4, perform moving / static point classification according to the method in Step S4, then integrate the number of times the point cloud is classified as moving / static at each pixel resolution, perform comprehensive calculation according to the classification results at different resolutions, finally determine the classification of moving / static points, and then according to the classification of moving / static points in each frame and the pose situation of each frame, construct a static point cloud map from the static point cloud and the corresponding pose in each frame, and finally establish a static map.

2. The method for constructing a garden map according to claim 1, characterized in that: In step S1, the pixel coordinates of the distance map are obtained using the following method: By mapping Π: R 3 →R 2 Each point p(x, y, z) in each frame of the point cloud is converted into spherical coordinates and finally into pixel coordinates. The conversion formula is as follows: In the formula, (u, v) represents the position corresponding to the laser point in the image coordinates, (h, w) are the height and width of the distance map, f = f up + f down represents the vertical field of view of the sensor, r = ||p i || 2 represents the distance information from the laser point to the sensor; in this process, each point p i corresponds to a list of tuples of a pair of image coordinates (u, v). The laser points in the same pixel can be represented by different indices, and finally the point cloud information is stored in an image with a resolution of 64×900.

3. The method for constructing a garden map according to claim 1, characterized in that: In Step S1, semantic segmentation uses the RangeNet++ convolutional neural network to predict the point cloud and generate semantic information. Using the kitti dataset as the training set, train the network for dynamic objects that affect the autonomous navigation and path planning of the garden pruning robot on the road. Among them, the dynamic objects include cars, bicycles, and people; then, the RangeNet++ convolutional neural network projects each scan frame into the distance map for segmentation, classifies the segmented dynamic objects into one category, and classifies the static objects that should not be filtered out into another category; infer the semantic labels and the probabilities of the corresponding labels in the segmentation for each frame of the distance map, and further processing will be performed on the labels with different probabilities in the subsequent steps.

4. The method for constructing a garden map according to claim 1, characterized in that, In Step S2, an erosion + dilation closing operation is used to optimize the dynamic region in the entire distance map. The method is as follows: First, the definition of the value of a single pixel point is as follows: where p represents the point cloud within the pixel point (i, j), r(p) represents the distance value of the point cloud, and r k represents the distance value of the point closest to the distance sensor in a single pixel; The entire closing operation is divided into two steps in total; In the first step, the original semantic mask S output by the RangeNet++ convolutional neural network sem is subjected to a dilation operation. A structural element with a size of (3×3) and a cross structure is selected for expansion, and it is defined as follows: Denotes dilation operation. The dilation operation of image A is to generate an image that can completely contain structure B, and then associate it with the r value in the distance map; the area where the distance difference between the dilated area and the area adjacent to the central pixel of the structuring element exceeds the threshold θ will not be considered as a dilation object, which is expressed as: r s , respectively represent the pixel values of the original semantic mask S sem and the dilated semantic mask ; The second step is to perform erosion on the dilated semantic mask Perform erosion operation. Still select the structural element with Size(3×3) and cross structure for erosion, which is defined as follows: Θ represents the erosion operation. The erosion operation of image A is to find the pixel points in the image that can completely contain the structural element B. The erosion area and the θ area where the structural element value is less than the threshold will not be considered as erosion objects, which is expressed as: The pixel values of the dilated semantic mask and the eroded semantic mask respectively.

5. The method for constructing a garden map according to claim 1, characterized in that, the method for assigning different weights to the point clouds of the dynamic / static regions on the distance map in step S3 is: In step S1, the defined "dynamic objects" have been segmented and optimized on the distance map. In step S2, the label probabilities of each point cloud are obtained through the RangeNet++ convolutional neural network. The greater the probability, the more likely the point is to be a real dynamic point. For each point cloud of the dynamic objects defined in step S1, a semantic label channel, a label probability channel, and a semantic weight channel are added, denoted as where l Dynamic = 1, l static = 0, is the position of the point cloud relative to the lidar coordinate system, l Dynamic , l static represent the dynamic label and the static label respectively, and are the weights of the dynamic and static points in a single scan respectively; n is the total number of point clouds in a single scan, and p is the label probability of the point cloud; The pose information of each frame of point cloud obtained by the SLAM odometer Calculate the residual value of each pixel between the current frame and the transformed frame, defined as follows: Among them represents frame S A The relative position relationship with frame S B is represents the difference between two pixel points represents S A The pixel point value corresponding to S B is represents the value of a single pixel point in the B frame. The specific definition of introducing the semantic weight is as follows: r represents the original distance from the laser point to the sensor, r s represents the distance value after adding semantic weights, represents the weight information; By increasing the weight of the area to be removed to improve the detection sensitivity, and reducing the weight of the area to be retained to reduce the detection sensitivity, the detection sensitivity of the point clouds in the dynamic / static regions can be adjusted; among them, the area to be removed is the dynamic region, and the area to be retained is the static region.

6. The method for constructing a garden map according to claim 1, characterized in that, in step S4, the method of checking the visibility of the map points in the projection distance image plane is used to perform the classification task of dynamic and static points. In the offline mode, a multi-dimensional time distance image is added to the matching model of the query point and the mapped point to establish a multi-time dimension matching model. The method is: First, define a set of sequential scans along the movement of the garden pruning robot, where the current scan is S j , the query sequence is …, S j-3 , S j-2 , S j-1 , S j+1 , S j+2 , S j+3 , …, Using the pose information obtained by SALM, calculate the pose relationship between the query sequence and the current scan, defined as follows: Among them, is the query sequence S j+n and the pose change relationship with the current scanned frame S j ; represents the pose change relationship between two adjacent frames; when querying and scanning the sequence, not only the historical query sequence is used, but also the future query sequence is added to the detection model to overcome some problems such as occlusion and shadow in a single scan, and it is more likely to be detected when facing some dynamic objects with similar speeds.​​​ Then calculate the residual value between each sequence point in the current distance map and the mapped point in other distance maps, and mark the sequence points as dynamic / static states according to the threshold, obtain the number of times each pixel point is marked with different states, and then obtain the number of times each point cloud is marked as dynamic / static points; and adopt an adaptive threshold method based on distance to better classify dynamic / static points, which is defined as follows, where τ is the threshold for marking dynamic points, and τ D is a fixed threshold, α is an adjustment coefficient, and r is the distance from the laser point to the sensor.

7. The method for constructing a garden map according to claim 1, characterized in that, in step S5, the method for constructing the static point cloud map is: Gradually reduce the resolution of the distance map, input the distances with different pixel resolutions into the multi-time dimension matching model in step S4, classify moving and static points according to the method in step S4, then restore the moving / static points to the three-dimensional point cloud space, and then calculate the total number of times each point P is marked as a dynamic point n Dynamic and a static point n Static at different resolutions, and re-classify the moving / static points by calculating the score of each sequence point. The specific formula is as follows: S(·) = αn Dynamic + βn Static where α is the positive weight and β is the negative weight; by reducing the resolution of the distance map, the dynamic / static point classification is updated iteratively, and finally the static point cloud map is constructed by integrating the results of dynamic / static classification under multiple scale distance maps.

8. The method for constructing a garden map according to claim 7, characterized in that, the method for restoring the dynamic / static points to the three-dimensional point cloud space is: Adopt the method of secondary mapping of the original point cloud. If the pixel point corresponding to a certain point is marked as a dynamic point, then delete the point in the original point cloud. If it is a static point, then retain it, specifically as follows: where F(·) represents the mapping function of Π: R 3 →R 2 ​ 9. The method for constructing a garden map according to claim 1, characterized in that, in step S5, the method for establishing the static map is: According to the pose information {T saved by SLAM i , T i+1 , …, T n}, perform point cloud stitching with the processed scan frames as follows: M = {M D , M S} Wherein, M is the original map; M D is the dynamic map; M S is the static map; represents the associated scan frame and the corresponding pose information.

Images