Static map generation method in indoor dynamic environment

Abstract

The invention discloses a static map generation method in an indoor dynamic environment, and the method comprises the steps: carrying out the mapping through employing an SLAM algorithm, and obtaining a point cloud map containing a dynamic object; preprocessing point clouds in the point cloud map, removing point cloud noise, and meanwhile, performing ground segmentation to obtain ground points and non-ground points; on the basis of an octree structure, updating the point cloud map with the point cloud noise removed in real time to obtain an updated map; performing superposition scanning on non-ground points by using a ray casting method, and identifying a potential dynamic region through a preset confidence threshold; performing multi-frame consistency detection on the potential dynamic area, and if the occupancy probability of the node in the potential dynamic area changes, judging the point cloud corresponding to the node as a dynamic point cloud; and removing the dynamic point cloud in the updated map to generate a static map. According to the method, superposition scanning and multi-frame consistency detection are performed on the point clouds in the indoor dynamic environment, so that the dynamic point clouds can be quickly identified, and the dynamic point clouds are rejected to generate the static map.

Description

Technical Field

[0001] The present invention relates to the technical field of map generation, and particularly relates to a method for generating a static map in an indoor dynamic environment. Background Art

[0002] Point cloud is a data structure used to represent three-dimensional information in space in lidar generation, which is divided into static point cloud and dynamic point cloud. Static point cloud refers to the point cloud that does not change in a certain time dimension, that is, static objects, such as a stationary table, trees, etc. Dynamic point cloud refers to the point cloud that changes in a certain time dimension, that is, dynamic objects, such as a moving car, a moving pedestrian, etc. In point cloud positioning, too many dynamic obstacles will affect the accuracy and robustness of point cloud registration. In extreme cases, if most of the point cloud in the previous frame of lidar data is data returned from dynamic obstacles, then the point cloud registration under this frame of point cloud will fail. Therefore, how to filter out dynamic point cloud is one of the key research directions in current point cloud positioning.

[0003] In an indoor dynamic environment, lidar will inevitably detect dynamic objects, such as pedestrians, etc., which will generate ghosting in the 3D point cloud map. During the positioning process, the matching of the current frame and the map point cloud may misidentify these ghostings as part of the map, resulting in inaccurate positioning. Therefore, removing dynamic point cloud and ghosting in the point cloud map is the key to constructing a high-quality static map for accurate positioning and subsequent human-computer interaction and other functions. Summary of the Invention

[0004] Aiming at the problems that dynamic ghosting generated in map generation in an indoor dynamic environment will cause subsequent point cloud matching failures, etc., the present invention provides a method for generating a static map in an indoor dynamic environment.

[0005] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0006] A method for generating a static map in an indoor dynamic environment, comprising the following steps:

[0007] S1. Use the SLAM algorithm to build a map to obtain a point cloud map containing dynamic objects;

[0008] S2. Preprocess the point cloud in the point cloud map to remove point cloud noise and simultaneously perform ground segmentation to obtain ground points and non-ground points;

[0009] S3. Based on the octree structure, perform real-time update on the point cloud map after removing point cloud noise to obtain an updated map;

[0010] S4. Use the ray casting method to perform superposition scanning on non-ground points, and identify potential dynamic regions through a preset confidence threshold;

[0011] S5. Perform multi-frame consistency detection on the potential dynamic area. If the occupancy probability of the nodes in the potential dynamic area changes, the point cloud corresponding to the nodes is determined as the dynamic point cloud.

[0012] S6. Remove the dynamic point cloud in the updated map to generate a static map.

[0013] Further, in S1, the NDT point cloud matching algorithm of the SLAM algorithm is used for mapping.

[0014] Further, in S2, a denoising algorithm based on spatial distribution is used to remove the point cloud noise.

[0015] Further, in S2, the point cloud is divided into different rays and concentric circles according to the horizontal angle and horizontal distance, and the slope and height of the point cloud on each ray are calculated; a slope threshold and a height threshold are set: if the slope of a point cloud is less than or equal to the slope threshold, it is a ground point, otherwise it is a non-ground point; if the height of a point cloud is less than or equal to the height threshold, it is a ground point, otherwise it is a non-ground point.

[0016] Further, S3 includes:

[0017] S31. Divide the three-dimensional space of the point cloud map into eight nodes of an octree, and each node is further subdivided or merged into different nodes;

[0018] S32. Use the floating-point number X to describe the nodes of the octree, X ∈ [0, 1], where 0 represents absolute free space, 1 represents absolute occupancy, and values between 0 and 1 represent uncertain or noisy data; obtain the measurement value of a certain cloud point in the environment through radar scanning, locate the octree node corresponding to the cloud point, and obtain the Bayesian formula for each node:

[0019]

[0020] Among them, P(O|Z) represents the occupancy probability after radar observation, that is, the original probability value, P(Z|O) represents the possibility of the radar detecting an obstacle, P(O) represents the prior occupancy probability, and P(Z) represents the total probability of sensor observation;

[0021] S33. Take the log-odds transformation on both sides of the Bayesian formula to obtain the update formula:

[0022] L(O|Z) = L(Z|O) + L(O);

[0023] Among them,

[0024] S34. For Z1, Z2, …, Z4 obtained from multiple independent observations, the log-odds update can be recursively expressed as:

[0025] L(O|z 1:t ) = L(O|z 1:t-1 ) + L(z t |O);

[0026] S35. Generalize the formula in S34 to the time series to obtain the map update formula:

[0027] L(M∣z 1:t+1 ) = L(M∣z 1:t-1 ) + L(M∣z t )

[0028] S36. Use the map update formula to obtain the updated map.

[0029] Furthermore, the S4 includes: S41. Use the ray casting method for detection, mark the ground points as free voxels without ray casting, and only perform superimposed scanning on non-ground points; S42. Divide the non-ground points into multiple regions, perform superimposed scanning on the nodes in each region to obtain the occupancy probability of each node; S42. Preset a confidence threshold. When the occupancy probability of any node in a certain region is greater than the confidence threshold, mark the corresponding region as an occupied region, otherwise mark it as an idle region, and record the occupied region as a potential dynamic region.

[0030] Furthermore, the confidence threshold is preset to 0.8.

[0031] Furthermore, in the S5, count the probability changes of the nodes in the potential dynamic region, calculate the variance of the probability changes. If the variance is greater than the preset variance threshold, it is considered that the corresponding node has changed, and the point cloud corresponding to the node is determined to be a dynamic point cloud.

[0032] Furthermore, the preset variance threshold is 0.1.

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

[0034] In an indoor dynamic environment, the radar will inevitably detect dynamic objects such as pedestrians, which will cause ghosting in the 3D point cloud map. During the positioning process, the matching of the current frame and the map point cloud may misidentify these ghosts as part of the map, resulting in inaccurate positioning. Therefore, removing the dynamic points and ghosts in the point cloud map is the key to constructing a high-quality static map for accurate positioning and subsequent human-computer interaction and other functions. The present invention can quickly identify the dynamic point cloud through superimposed scanning and multi-frame consistency detection of the point cloud in the indoor dynamic environment, and remove it to generate a static map containing only static point clouds. Brief Description of the Drawings

[0035] Figure 1 This is the flowchart of the method of the present invention. Detailed Description of the Embodiments

[0036] In order to make the objectives, technical solutions and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the scope of protection of the present invention.

[0037] Currently, the filtering of dynamic point clouds mainly falls into two major directions. One is the method based on deep learning, which directly detects dynamic objects from point clouds for real-time 3D dynamic object detection. Although this method has good effects, deep learning models usually require a large amount of high-quality labeled data for training, which requires a lot of manpower and time. And this method usually requires a large amount of computing resources for training and inference, requires high-performance chip computing power, increases the cost of application in industrial production, and is not conducive to industrial production. Moreover, a common problem of deep learning algorithms is limited generalization ability, and they may perform poorly when different environments, weather conditions, and different dynamic objects appear, which limits the applicability of the algorithms. The other method is to filter dynamic point clouds based on traditional methods. This method usually relies on geometric models or statistical-based filtering methods. These methods have relatively small computational amounts, can be applied in resource-constrained embedded systems, do not require high-performance hardware resources, and can quickly adapt to most mobile robot devices on the market. The present invention optimizes the traditional method to remove the afterimages of dynamic objects in the map and establish a static point cloud map that can reflect real environmental information.

[0038] As Figure 1 shown, a method for generating a static map in an indoor dynamic environment provided by the present invention includes the following steps:

[0039] S1. Use the SLAM algorithm to build a map to obtain a point cloud map containing dynamic objects;

[0040] S2. Preprocess the point clouds in the point cloud map to remove point cloud noise and perform ground segmentation at the same time to obtain ground points and non-ground points;

[0041] S3. Based on the octree structure, perform real-time update on the point cloud map after removing point cloud noise to obtain an updated map;

[0042] S4. Use the ray casting method to perform superposition scanning on non-ground points, and identify potential dynamic regions through a preset confidence threshold;

[0043] S5. Perform multi-frame consistency detection on the potential dynamic area. If the occupancy probability of the nodes in the potential dynamic area changes, the point cloud corresponding to the nodes is determined as the dynamic point cloud.

[0044] S6. Remove the dynamic point cloud in the updated map to generate a static map.

[0045] The present invention first constructs a point cloud map containing dynamic objects; then removes the point cloud noise in the point cloud map, optimizes the point cloud data, and simultaneously performs ground segmentation to obtain ground points and non-ground points; then performs real-time update of the point cloud map based on the octree structure to achieve dynamic map update; then quickly identifies the dynamic point cloud through overlay scanning and multi-frame consistency detection of the point cloud in the indoor dynamic environment, and removes it to generate a static map containing only static point clouds. The present invention can remove the dynamic point cloud and ghosting in the point cloud map and construct a high-quality static map.

[0046] In S1 of the present invention, the NDT point cloud matching algorithm of the SLAM algorithm is used for map construction to obtain a point cloud map containing dynamic objects. The constructed point cloud map includes a prior map and a local map.

[0047] In S2 of the present invention, a denoising algorithm based on spatial distribution is used to remove the point cloud noise. Specifically, the geometric structure or neighborhood relationship of the data in space is utilized to improve the denoising effect, and this method can retain the important geometric structure and remove the point cloud noise.

[0048] In S2 of the present invention, the point cloud is divided into different rays and concentric circles according to the horizontal angle and horizontal distance, and the slope and height of the point cloud on each ray are calculated; a slope threshold and a height threshold are set: if the slope of a point cloud is less than or equal to the slope threshold, it is a ground point, otherwise it is a non-ground point; if the height of a point cloud is less than or equal to the height threshold, it is a ground point, otherwise it is a non-ground point.

[0049] In the present invention, S3 is to perform real-time update on the point cloud map based on the octree structure. The octree nodes are described by using floating-point number X based on a probability model. The three-dimensional space of the point cloud map is divided into eight nodes of the octree, and each node is further subdivided or merged into different nodes. The floating-point number X ∈ [0, 1], where 0 represents absolute idle, 1 represents absolute occupancy, and the value between 0 and 1 represents uncertain or noisy data. The prior occupancy probability of each node is initially 0.5, and the measurement values of each point cloud in the point cloud map are obtained through lidar scanning. Through the correspondence between the octree nodes and the point cloud, the measurement value of each point cloud obtained by lidar scanning is the occupancy probability of the corresponding node. At this time, the new occupancy probability of each node can be calculated through the Bayesian formula. According to the new occupancy probability and the resolution requirement, the octree nodes are dynamically segmented or merged to update the point cloud map in real time. The Bayesian formula is as follows:

[0050]

[0051] Among them, P(O|Z) represents the new occupancy probability of the node after lidar scanning, P(Z|O) represents the occupancy probability of each node through lidar scanning, P(O) represents the prior occupancy probability of the node, and P(Z) represents the total probability of sensor observation.

[0052] To exclude the situation where the nodes in the constructed map are observed by the sensor multiple times, beyond the range of [0, 1], or the calculation becomes unstable. The present invention optimizes the Bayesian formula by using the log-odds transformation, and the optimization is as follows:

[0053] Set P(O|Z) = x', P(O) = x, substitute P(O|Z) = x' and P(O) = x into the Bayesian formula and simplify to obtain the following first formula:

[0054]

[0055] After taking the logarithm of the first formula, the second formula is obtained:

[0056]

[0057] That is, the update formula is obtained:

[0058] L(O|Z) = L(Z|O) + L(O);

[0059] For Z1, Z2,..., Z obtained from multiple independent observations t , the log-odds update can be recursively expressed as:

[0060] L(O|z 1:t ) = L(O|z 1:t-1 ) + L(z t |O);

[0061] Generalize the recursive formula to a time series to obtain a map update formula:

[0062] L(M|z 1:t+1 ) = L(M|z 1:t-1 ) + L(M|z t );

[0063] Finally, use the map update formula to obtain the updated map.

[0064] The present invention uses the logit function y = log(x / (1 - x)) of the log-odds transformation to convert the probability x into a real number y. In this way, the range of y is from negative infinity to positive infinity, and the inverse transformation is to use the sigmoid function to convert y back to x. In this case, no matter how y is updated, x always remains between 0 and 1, solving the problem of exceeding the range of [0, 1].

[0065] S4 in the present invention is to identify potential dynamic regions with dynamic point clouds. S4 includes: S41. Use the ray casting method for detection, mark the ground points as free voxels and do not perform ray casting, and only perform superposition scanning on non-ground points (i.e., voxels); S42. Divide the non-ground points into multiple regions, perform superposition scanning on the nodes in each region to obtain the occupancy probability of each node; S42. Preset a confidence threshold of 0.8. If the ray passes through the same voxel multiple times, its occupancy probability will increase. When the occupancy probability of any node in a certain region is greater than the confidence threshold, it indicates that this region may contain dynamic objects, mark the corresponding region as an occupied region, otherwise mark it as an idle region, and record the occupied region as a potential dynamic region.

[0066] S5 in the present invention is to perform multi-frame consistency detection on the potential dynamic regions, count the probability changes of the nodes in the potential dynamic regions, calculate the variance of the probability changes. If the variance is greater than the preset variance threshold of 0.1, it is considered that the corresponding node has changed, and it is determined that the point cloud corresponding to the node is a dynamic point cloud.

[0067] Finally, it should be noted that the above embodiments are only preferred embodiments of the present invention to illustrate the technical solutions of the present invention, rather than limiting it, and certainly not limiting the patent scope of the present invention; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention; that is to say, any meaningless changes or polishing made in the main design concept and spirit of the present invention, as long as the technical problems solved by it are still consistent with the present invention, should be included in the protection scope of the present invention; in addition, directly or indirectly applying the technical solutions of the present invention to other related technical fields shall similarly be included in the patent protection scope of the present invention.

Claims

1. A method for generating a static map in an indoor dynamic environment, characterized in that, It includes the following steps: S1. Use the SLAM algorithm to build a map to obtain a point cloud map containing dynamic objects; S2. Preprocess the point cloud in the point cloud map to remove point cloud noise and perform ground segmentation simultaneously to obtain ground points and non-ground points; S3. Based on the octree structure, perform real-time update on the point cloud map after removing point cloud noise to obtain an updated map; S4. Use the ray casting method to perform superposition scanning on non-ground points and identify potential dynamic areas through a preset confidence threshold; S5. Perform multi-frame consistency detection on potential dynamic areas. If the occupancy probability of nodes in the potential dynamic area changes, the point cloud corresponding to the nodes is determined as dynamic point cloud; S6. Remove the dynamic point cloud in the updated map to generate a static map.

2. The method for generating a static map in an indoor dynamic environment according to claim 1, wherein In S1, the NDT point cloud matching algorithm of the SLAM algorithm is used for map building.

3. A method for generating a static map in an indoor dynamic environment according to claim 1, wherein In S2, a denoising algorithm based on spatial distribution is used to remove point cloud noise.

4. A method for generating a static map in an indoor dynamic environment according to claim 1, wherein, In S2, the point cloud is divided into different rays and concentric circles according to the horizontal angle and horizontal distance, and the slope and height of the point cloud on each ray are calculated; a slope threshold and a height threshold are set: if the slope threshold of a point cloud is less than or equal to the slope threshold, it is a ground point, otherwise it is a non-ground point; if the height of a point cloud is less than or equal to the height threshold, it is a ground point, otherwise it is a non-ground point.

5. A method for generating a static map in an indoor dynamic environment according to claim 1, characterized in that S3 includes: S31. Divide the three-dimensional space of the point cloud map into eight nodes of the octree, and each node is further subdivided or merged into different nodes; S32. Use the floating-point number X to describe the nodes of the octree, X ∈ [0, 1], where 0 represents absolute free space, 1 represents absolute occupancy, and a value between 0 and 1 represents uncertain or noisy data; obtain the measurement value of a certain cloud point in the environment through radar scanning, locate the octree node corresponding to the cloud point, and obtain the Bayesian formula for each node: Among them, P(O|Z) represents the occupancy probability after radar observation, that is, the original probability value, P(Z|O) represents the possibility of detecting an obstacle by radar, P(O) represents the prior occupancy probability, and P(Z) represents the total probability of sensor observation; S33. Take the log-odds transformation on both sides of the Bayesian formula to obtain the update formula: L(O|Z) = L(Z|O) + L(O); Among them, S34. For Z1, Z2,..., Z4 obtained from multiple independent observations, the log-odds update can be recursively expressed as: L(O|z 1:t ) = L(O|z 1:t-1 ) + L(z t |O); S35. Generalize the formula in S34 to the time series to obtain the map update formula: L(M|z 1:t+1 ) = L(M|z 1:t-1 ) + L(M|z t ) S36. Use the map update formula to obtain the updated map.

6. A method for generating a static map in an indoor dynamic environment according to claim 1, characterized in that, S4 includes: S41. Use the ray casting method for detection, mark the ground points as free voxels and do not perform ray casting, and only perform superposition scanning on non-ground points; S42. Divide the non-ground points into multiple regions, perform superposition scanning on the nodes in each region to obtain the occupancy probability of each node; S42. Preset a confidence threshold. When the occupancy probability of any node in a certain region is greater than the confidence threshold, mark the corresponding region as an occupied region, otherwise mark it as a free region, and record the occupied region as a potential dynamic region.

7. A method for generating a static map in an indoor dynamic environment according to claim 6, characterized in that, The confidence threshold is preset to 0.

8.

8. A method for generating a static map in an indoor dynamic environment according to claim 6, characterized in that, In S5, the probability changes of the nodes in the potential dynamic area are statistically analyzed, and the variance of the probability changes is calculated. If the variance is greater than the preset variance threshold, it is considered that the corresponding node has changed, and the point cloud corresponding to the node is determined to be a dynamic point cloud.

9. A method for generating a static map in an indoor dynamic environment according to claim 8, characterized in that The preset variance threshold is 0.1.

Images