An online removal system and method for dynamic targets during laser SLAM

Abstract

The present invention provides an online removal system and method for dynamic targets during laser SLAM, which relates to the field of laser three-dimensional reconstruction technology. The system includes: a lidar for obtaining three-dimensional structure information of objects and spaces in the surrounding environment and generating a plurality of initial single-frame laser point cloud data; a SLAM module for removing ground points from all the initial single-frame laser point cloud data to obtain single-frame laser point cloud data and construct a local map, and simultaneously calculating the position and pose of the lidar when each initial single-frame laser point cloud data is generated as the acquisition pose of the single-frame laser point cloud data; a dynamic removal module for identifying dynamic object point clouds from the received single-frame laser point cloud data and splicing the local maps after removing the dynamic object point clouds by using the acquisition poses of the single-frame laser point cloud data to construct a global map. The present invention can accurately remove dynamic targets in the point cloud map in real time, overcoming the serious defect of misremoval in existing algorithms.

Description

Technical Field

[0001] The present invention relates to the technical field of laser three-dimensional reconstruction, and particularly to an online removal system and method for dynamic targets during the laser SLAM process. Background Art

[0002] Simultaneous localization and mapping (SLAM) technology is a key technology for current three-dimensional reconstruction, unmanned driving, and unmanned warehousing. Among SLAM sensors, lidar has the characteristics of high stability and high applicability, and is the mainstream sensor for current SLAM technology. However, laser SLAM technology is often applied to environments where dynamic and static objects coexist, such as an environment composed of a building and a moving vehicle. At this time, there will be a large number of dynamic points in the scene map generated by the laser SLAM technology, forming "ghost images". Such "ghost images" will not only affect the positioning accuracy of the laser SLAM technology, but also damage the mapping effect of the laser SLAM technology. Currently, the methods for removing dynamic targets in laser SLAM technology mainly include target detection based on deep learning and dynamic removal algorithms based on geometric consistency.

[0003] The method of performing target detection based on deep learning during the laser SLAM process and then analyzing the possibility of dynamic targets existing in the map constructed by laser SLAM has problems such as large computational workload, high computational hardware cost, and weak applicability to complex scenarios. The Chinese patent application "Laser SLAM method for dynamic environment based on semantic constraints" with the document number CN113671522 discloses a method of using environmental elements for semantic definition, endowing semantic definitions to objects in the environment through deep learning, and classifying the targets in the environment into dynamic elements, static elements, and undetermined elements to remove dynamic objects during the laser SLAM process. This method predefines the environmental element categories according to the map element classification and grading method, and performs semantic segmentation according to the predefined environmental element categories; establishes the weight values of each environmental element category and performs pre-judgment of the targets in the environment, and classifies the targets in the environment into dynamic elements, static elements, and undetermined elements; adds the semantic identification map as a semantic constraint to the ICP to generate a semantic map. However, this method requires establishing a semantic library on the algorithm platform and using RangeNet++ for target detection, which has certain requirements for the hardware storage space and computing power, and cannot be migrated and used on platforms such as drones and AGVs. When facing objects not included in the semantic library, incorrect target detection results will be generated, and after target detection, it is also necessary to judge the dynamicity of the target, with a large program workload and unable to complete real-time analysis on most platforms.

[0004] During the laser SLAM process, the dynamic removal algorithm based on geometric consistency has problems such as low accuracy, incorrect removal and incomplete removal, and is greatly affected by parameters, environment, sensors, geometric models, and algorithm operation cycles in dynamic removal. At the same time, some algorithms also have problems of large computational complexity and high memory requirements for the devices using the algorithms. The Chinese patent application "A Method and System for Indoor Dynamic Scene SLAM Based on Point Cloud" with the literature number CN106056643 discloses a method for generating a point cloud map in a dynamic scene according to a probability map. Since the geometric structure of the static space is consistent at different times, this method uses a probability method and a dynamic removal method of laser occlusion to perform multi-scale registration of laser frames and probability maps based on the geometric consistency of the space, updates each layer of the historical probability map, and converts the current frame to the coordinate system where the historical probability map is located; obtains the converted laser frame and generates a corresponding probability map; combines the current frame probability map and the historical probability map to update the historical probability map to obtain the current historical probability map; finally calculates the running trajectory of the laser and the two-dimensional map of the indoor dynamic environment. However, this method has problems of incorrect removal of static targets and non-removal of dynamic targets caused by inaccurate point cloud estimation. This method establishes a multi-scale probability map, and its probability value is related to the size of the Gaussian kernel along the x-axis and y-axis and the distance from the grid along the x-axis and y-axis to the edge of the dynamic target. For different environments such as roads, forests, and indoors, due to large differences in the field of view and different scale heights, it is easy to cause inaccurate probability calculation, resulting in incorrect dynamic removal results. At the same time, a general laser sensor can collect more than 100,000 points per second. This method needs to calculate the weight of each point and needs to save multiple adjacent frames of probability maps in the memory. Therefore, this method has high computational complexity and high memory requirements. Summary of the Invention

[0005] Aiming at the deficiencies of the above-mentioned prior art, the present invention provides an online removal system and method for dynamic targets during the laser SLAM process. By voxelizing the local maps generated during the laser SLAM process and extracting voxel features to match multiple local maps, the effect of real-time and accurately removing dynamic targets from the point cloud map is achieved, overcoming the defects of high computational complexity, high memory requirements, serious incorrect removal, and single use scenario of existing algorithms.

[0006] An online removal system for dynamic targets during the laser SLAM process provided by the first aspect of the present invention includes: a lidar, a SLAM module, and a dynamic removal module;

[0007] The lidar is used to obtain the three-dimensional structure information of objects and spaces in the surrounding environment, generate several single-frame laser point cloud initial data, and send them to the SLAM module;

[0008] The SLAM module is used to remove the ground points in all the initial single-frame lidar point cloud data, obtain the single-frame lidar point cloud data and construct a local map. At the same time, it calculates the position and attitude of the lidar in the world coordinate system when generating each initial single-frame lidar point cloud data, which is used as the acquisition pose of the single-frame lidar point cloud data, and transmits all the single-frame lidar point cloud data, the local map and the acquisition pose of the single-frame lidar point cloud data to the dynamic removal module;

[0009] The dynamic removal module is used to identify the dynamic object point cloud from the received single-frame lidar point cloud data, and use the acquisition pose of the single-frame lidar point cloud data to splice and remove the local map after the dynamic object point cloud, and construct a global map;

[0010] Further, the SLAM module includes: a ground removal unit and a laser odometry unit;

[0011] The ground removal unit is used to calculate the pitch angle of each point in each initial single-frame lidar point cloud data respectively, and divide all the points in the initial single-frame lidar point cloud data into ground points and non-ground points according to the calculated pitch angle. The single-frame lidar point cloud data is obtained by removing all the ground points and transmitted to the laser odometry unit and the dynamic removal module;

[0012] The laser odometry unit is used to construct a local map by performing frame-to-frame matching on all the single-frame lidar point cloud data; at the same time, it calculates the acquisition pose of the single-frame lidar point cloud data, and transmits all the local maps and the acquisition poses of the single-frame lidar point cloud data to the dynamic removal module;

[0013] Further, the dynamic removal module includes: a point cloud voxelization unit, a multi-frame voxel comparison unit, a dynamic voxel association unit, a dynamic point extension unit, a dynamic point removal unit and a local map data storage space;

[0014] The point cloud voxelization unit is used to divide the single-frame lidar point cloud data into several voxels according to the position, sort the voxels according to the generation time, and store all the voxels by establishing a hash table and transmit them to the multi-frame voxel comparison unit;

[0015] The multi-frame voxel comparison unit is used to traverse each voxel of the current single-frame lidar point cloud data, generate the shape feature and distribution feature of each voxel, and perform dynamic determination on each voxel according to the shape feature and distribution feature of each voxel; transmit the dynamic determination results of the voxels in all the single-frame lidar point cloud data to the dynamic voxel association unit; where the dynamic determination result of the voxel is: a static voxel, a low-dynamic voxel or a high-dynamic voxel;

[0016] The dynamic voxel association unit is used to set the proximity range of high-dynamic voxels. For any high-dynamic voxel, the principal component analysis method is used to associate the low-dynamic voxels within the proximity range of the high-dynamic voxel, and based on the association result, the low-dynamic voxels within the proximity range of the high-dynamic voxel are determined as static voxels or high-dynamic voxels; all the high-dynamic voxels in the single-frame lidar point cloud data are used as dynamic voxels, and both the dynamic voxels and static voxels in all the single-frame lidar point cloud data are transmitted to the dynamic point extension unit;

[0017] The dynamic point extension unit is used to use the static voxels as a reference and adopt a region growing algorithm to extend the point cloud of the dynamic voxels, generate the point cloud of the dynamic objects in all the single-frame lidar point cloud data, and transmit it to the dynamic point removal unit;

[0018] The dynamic point removal unit is used to mark and remove the point cloud of the dynamic objects in all the single-frame lidar point cloud data in the local map, and then splice the local map after removing the dynamic point cloud in the world coordinate system according to the acquisition pose of the single-frame lidar point cloud data to obtain a global map that only retains the static part;

[0019] The local map data storage space is used to save all the voxels stored with a hash table index in each single-frame lidar point cloud data, as well as the dynamic determination results and geometric information of each voxel; the geometric information includes the average height, centroid, and covariance of the voxel;

[0020] An online removal method for dynamic targets during laser SLAM provided in the second aspect of the present invention is implemented by using the above-mentioned online removal system for dynamic targets during laser SLAM. The method includes the following steps:

[0021] Step 1: Obtain a plurality of initial single-frame lidar point cloud data, generate a plurality of single-frame lidar point cloud data by removing the ground points in each initial single-frame lidar point cloud data, calculate the acquisition pose of each single-frame lidar point cloud data by using the frame-to-frame matching method, and construct a local map;

[0022] Step 2: For any single-frame lidar point cloud data, generate a plurality of voxels by two-dimensional rasterization of the single-frame lidar point cloud data, respectively perform dynamic determination on each voxel, and then perform dynamic voxel association according to the dynamic determination result, so as to divide each voxel into dynamic voxels and static voxels;

[0023] Step 3: Adopt a region growing algorithm to extend the adjacent point cloud of the dynamic voxels to generate the point cloud of the dynamic objects in the single-frame lidar point cloud data;

[0024] Step 4: Repeat Steps 2 - 3 to generate the point clouds of dynamic objects in all single - frame lidar point clouds. Mark all the point clouds of dynamic objects on the local map and remove them to obtain a local map that only retains the static part.

[0025] Step 5: Stitch all the local maps that only retain the static part according to the acquisition poses of each single - frame lidar point cloud to obtain a global map that only retains the static part.

[0026] The specific content of Step 1 is: Obtain several initial single - frame lidar point cloud data.

[0027] For any initial single - frame lidar point cloud data, calculate the pitch angles of each lidar point in this single - frame lidar point cloud data respectively.

[0028] According to the pitch angles of each lidar point and the line number characteristics of the lidar in the initial single - frame lidar point cloud data, calculate the line number of each lidar point, and classify the lidar points according to the line numbers of all lidar points in the initial single - frame lidar point cloud data, so as to generate lines with increasing pitch angles from low to high.

[0029] Calculate the yaw angles of each lidar point in the initial single - frame lidar point cloud data. Traverse each lidar point in the line with the lowest pitch angle in turn. The traversal process is as follows: Take the currently traversed lidar point as the target point P1, find the lidar point P2 with the same yaw angle as the target point P1 in the line with the second - lowest pitch angle, and calculate the elevation angle of the lidar point P2 relative to the target point P1. Judge whether the target point P1 has changed from a ground point to a non - ground point according to the elevation angle of the lidar point P2 relative to the target point P1, and mark the ground points according to the judgment result, and then remove the ground points in the initial single - frame lidar point cloud data to obtain the single - frame lidar point cloud data.

[0030] The method for judging whether the target point P1 has changed from a ground point to a non - ground point is: If the elevation angle of the lidar point P2 relative to the target point P1 is greater than 10 degrees, it is judged that the target point P1 has changed from a ground point to a non - ground point, and mark the lidar points between the starting point P low on the line with the lowest pitch angle and the target point P1 as ground points, and mark the lidar points between the lidar point P2 and the end point P high on the line with the highest pitch angle as non - ground points; otherwise, it is judged that the target point P1 has not changed from a ground point to a non - ground point.

[0031] Perform frame - to - frame matching on all single - frame lidar point cloud data, calculate the acquisition poses of the single - frame lidar point cloud data, and construct a local map.

[0032] Step 2 further includes:

[0033] Step 2.1: For any single-frame lidar point cloud data, perform two-dimensional rasterization on the single-frame lidar point cloud data, and take the point set formed by the lidar points in each grid as a voxel. Determine the voxel number according to the generation order of the voxels, and store each voxel by using the method indexed by a hash table;

[0034] Step 2.2: Calculate the number of points, average height of points, and point dispersion in each voxel respectively, take the average height of points in each voxel as the shape feature of the voxel, and take the number of points and point dispersion of each voxel as the distribution feature of the voxel;

[0035] Step 2.3: For any voxel in the current single-frame lidar point cloud data, take the two single-frame lidar point cloud data before and after the current single-frame lidar point cloud data as reference frames. Dynamically determine the voxel by comparing the shape features and distribution features of the voxel with the voxels at the same position in the reference frames in the world coordinate system, and classify the voxel into a static voxel, a low-dynamic voxel, or a high-dynamic voxel according to the dynamic determination result;

[0036] Step 2.4: Set the proximity range of the high-dynamic voxels. For any high-dynamic voxel, use the principal component analysis method to perform an association judgment on the low-dynamic voxels within the proximity range of the high-dynamic voxel, and determine whether the low-dynamic voxels within the proximity range of the high-dynamic voxel are static voxels or high-dynamic voxels according to the association result. Take all the high-dynamic voxels in the single-frame lidar point cloud data as dynamic voxels;

[0037] The specific content of Step 2.3 is: For any voxel in the current single-frame lidar point cloud data, respectively determine the reference voxels in each reference frame that have the same position as the voxel in the world coordinate system. By calculating the difference in the number of points, the difference in the average height of points, and the difference in point dispersion between the voxel and the reference voxels, respectively obtain the difference errors between the voxel and each reference voxel;

[0038] The calculation method of the difference error is:

[0039] , where, represents the difference error between voxel and reference voxel ; represents the th voxel in the th single-frame lidar point cloud data; represents the th voxel in the th single-frame lidar point cloud data, and is the reference voxel of , ; represents the weight of the number of points; represents voxel The number of points; Indicates the reference voxel The number of points; Indicates the voxel And the sum of the number of points of the voxels adjacent to this voxel; Indicates the weight of the average height of the points; Indicates the voxel The average height of the points; Indicates the reference voxel The average height of the points; Indicates the voxel And the sum of the average heights of the voxels adjacent to this voxel; Indicates the weight of the point dispersion; Indicates the voxel The point dispersion; Indicates the reference voxel The point dispersion; Indicates the voxel And the point dispersion of the voxels adjacent to this voxel;

[0040] Dynamically determine this voxel according to the difference error between this voxel and each reference voxel. For the four reference voxels of this voxel, if there are at least two reference voxels corresponding to the difference error greater than the set dynamic threshold , then the dynamic determination result of this voxel is a high-dynamic voxel; if there is a reference voxel corresponding to the difference error greater than the set dynamic threshold , then the dynamic determination result of this voxel is a low-dynamic voxel; if there is no reference voxel corresponding to the difference error greater than the set dynamic threshold , then the dynamic determination result of this voxel is a static voxel;

[0041] The specific content of step 2.4 is: for any high-dynamic voxel in the current single-frame lidar point cloud data, each low-dynamic voxel within the adjacent range of this high-dynamic voxel is used as an adjacent voxel of this high-dynamic voxel, and the centroid of this high-dynamic voxel and the centroids of all adjacent voxels of this high-dynamic voxel are calculated respectively;

[0042] Centering process this high-dynamic voxel using the centroid of this high-dynamic voxel, and calculating the covariance matrix of this high-dynamic voxel using the centered high-dynamic voxel; for any adjacent voxel of this high-dynamic voxel, centering process this adjacent voxel using the centroid of this adjacent voxel, and calculating the covariance matrix of this adjacent voxel using the centered adjacent voxel;

[0043] For this high-dynamic voxel or any adjacent voxel of this high-dynamic voxel, solve the eigenvalues of the covariance matrix according to the matrix properties of the covariance matrix of the voxel , and using the eigenvalues of the covariance matrix Solve separately the corresponding non-zero eigenvectors, and use as the main direction of the voxel in the distribution, and use as the secondary main direction of the voxel in the distribution, and use and as the primary and secondary distribution vectors of the point cloud of the voxel;

[0044] Then, according to the primary and secondary distribution vectors of the point cloud of the high-dynamic voxel and the primary and secondary distribution vectors of the point cloud of all adjacent voxels of the high-dynamic voxel, calculate the geometric consistency between the high-dynamic voxel and each adjacent voxel of the high-dynamic voxel respectively;

[0045] For any adjacent voxel of the high-dynamic voxel, if the geometric consistency between the high-dynamic voxel and the adjacent voxel exceeds the set geometric consistency threshold , then mark the adjacent voxel as a high-dynamic voxel; if the geometric consistency between the high-dynamic voxel and the adjacent voxel does not exceed the set geometric consistency threshold , then mark the adjacent voxel as a static voxel;

[0046] Furthermore, the calculation method of the geometric consistency is:

[0047] , where represents the geometric consistency between voxel and voxel ; represents the yaw angle weight; represents the pitch angle weight; represents the center of voxel in the world coordinate system; represents the center of voxel in the world coordinate system;

[0048] Step 3 further includes:

[0049] Step 3.1: For any dynamic voxel in the current single-frame lidar point cloud data, determine the adjacent voxels of the dynamic voxel, and store the lidar points in the dynamic voxel and the lidar points in the adjacent voxels of the dynamic voxel by building a KDTree;

[0050] Step 3.2: Inside the adjacent voxels of the dynamic voxel, use the KDTree to separately find all the lidar points within the set threshold distance from the center point of the dynamic voxel in each adjacent voxel, and screen out all the lidar points belonging to static voxels from the found lidar points to construct a static voxel point set ;

[0051] Step 3.3: Use the KDTree to quickly find The three nearest static voxel points within the neighborhood of each laser point, and calculate the normal vector of the plane fitted by the three points ; at the same time, calculate the normal vector of the plane fitted by the three nearest dynamic voxel points within the neighborhood of this laser point ;

[0052] Step 3.4: Calculate the normal vectors and the normal vector of the included angle ;

[0053] Step 3.5: When for a certain laser point in , it is considered that this laser point and the laser point in this dynamic voxel are on the same plane, and this laser point is used as the extension point of this dynamic voxel; then use all the extension points of this dynamic voxel to construct an extension point set ; at the same time, mark all the laser points in that do not meet and construct a static point set ;

[0054] Step 3.6: For any extension point in the extension point set , within the neighboring voxels of the voxel where this extension point is located, use KDTree to separately find all the laser points within each neighboring voxel whose distance from the center point of the voxel where this extension point is located is within the set threshold, and screen out all the laser points belonging to static voxels from the found laser points to construct a new set of static voxel points, and repeat steps 3.3 - 3.5 until there are no new extension points;

[0055] Step 3.7: Use all the dynamic voxels and the extension points of each dynamic voxel to generate the dynamic object point cloud in this single - frame laser point cloud data.

[0056] The beneficial effects produced by adopting the above - mentioned technical solutions are as follows:

[0057] The method of the present invention can achieve the removal of dynamic targets in the laser SLAM process by means of voxelized point cloud, inter - frame voxel feature matching and comparison, and dynamic voxel region extension while meeting the requirements of on - line and low cost.

[0058] Combined with the characteristics of large amount of laser sensor data and a certain proportion of ground points, the method of the present invention uses a ground extraction method based on the laser view and a uniform down - sampling method based on the grid map, reducing the number of low - significance point clouds and improving the system speed and stability.

[0059] The method of the present invention first uses the method of point cloud voxelization to reduce the memory load, then uses a grid map to reduce the computational amount and time loss when saving voxels and searching for adjacent points, and finally uses a hash table to quickly locate the specific position of the voxel in the world coordinate system, exponentially compressing the time loss.

[0060] Due to the large differences in the volume, shape, motion speed, and relative position to the laser sensor of dynamic targets in different environments, false extraction is likely to occur during the dynamic removal in the SLAM process. Therefore, after voxelizing the single-frame laser point cloud data, the method of the present invention extracts three voxel features for inter-frame matching to extract dynamic targets, improving the accuracy.

[0061] Due to the complexity of the shape of dynamic targets, the dynamic removal based solely on voxels, clustering, and probability is likely to produce unclear dynamic and static boundaries. Therefore, the method of the present invention uses the method of dynamic voxel extension to extend the boundaries of the voxels confirmed as dynamic, enabling accurate extraction even for dynamic objects with complex shapes.

[0062] The experimental results show that in various scenarios, the method of the present invention can effectively remove dynamic objects, while overcoming the defects of high computational amount, high memory requirements, and serious false removal of existing algorithms. It can achieve real-time dynamic removal for SLAM algorithms using common lidars in the industry, such as velodyne16 / 32 / 64-line lidars, Hesai lidars, and Raytron lidars, etc. Description of the Drawings

[0063] Figure 1 It is a structural diagram of an online removal system for dynamic targets during a laser SLAM process in this embodiment;

[0064] Figure 2 It is a flowchart of an online removal method for dynamic targets during a laser SLAM process in this embodiment;

[0065] Figure 3 It is a schematic diagram of ground points and non-ground points scanned by a lidar in this embodiment;

[0066] Figure 4 It is an overall point cloud map of a certain street containing dynamic target points in this embodiment;

[0067] Figure 5 It is a schematic diagram of the point cloud ground extraction effect of an overall point cloud map of a certain street in this embodiment;

[0068] Figure 6 It is a schematic diagram of the point cloud ground removal effect of an overall point cloud map of a certain street in this embodiment;

[0069] Figure 7This is the point cloud map before voxelizing a partial point cloud map of a certain street in this embodiment;

[0070] Figure 8 This is the point cloud map after voxelizing a partial point cloud map of a certain street in this embodiment;

[0071] Figure 9 This is the point cloud map after dynamically discriminating voxels for a partial point cloud map of a certain street in this embodiment;

[0072] Figure 10 This is a schematic diagram of dynamic voxels and static voxels in the point cloud map before extending dynamic voxels using the region growing algorithm in this embodiment;

[0073] Figure 11 This is a comparison diagram before and after removing dynamic objects from the point cloud map of Street 1 in this embodiment; where (a) is a schematic diagram before removing dynamic objects from the point cloud map of Street 1; (b) is a schematic diagram after removing dynamic objects from the point cloud map of Street 1;

[0074] Figure 12 This is a comparison diagram before and after removing dynamic objects from the point cloud map of Street 2 in this embodiment; where (a) is a schematic diagram before removing dynamic objects from the point cloud map of Street 2; (b) is a schematic diagram after removing dynamic objects from the point cloud map of Street 2. Specific Embodiment

[0075] The following further describes in detail the specific embodiments of the present invention in conjunction with the drawings and embodiments. The following embodiments are used to illustrate the present invention, but are not used to limit the scope of the present invention.

[0076] An online removal system for dynamic objects during the laser SLAM process in this embodiment, as Figure 1 shown, the system includes: a lidar, a SLAM module, and a dynamic removal module.

[0077] The lidar is used to obtain the three-dimensional structure information of objects and space in the surrounding environment, generate a number of single-frame laser point cloud initial data, and send it to the SLAM module.

[0078] In this embodiment, the three-dimensional structure information of all objects and space within the sensing range of the lidar is obtained, such as the contours, edges, and surfaces of objects such as buildings, roads, and trees in the surrounding environment of the lidar, as well as the terrain and spatial layout of the surrounding environment.

[0079] The SLAM module is used to remove the ground points from all the initial data of single-frame lidar point clouds, obtain the single-frame lidar point cloud data and construct a local map. At the same time, it calculates the position and attitude of the lidar in the world coordinate system when generating each initial data of single-frame lidar point clouds, which is used as the acquisition pose of the single-frame lidar point cloud data, and transmits all the single-frame lidar point cloud data, the local map and the acquisition pose of the single-frame lidar point cloud data to the dynamic removal module.

[0080] In this embodiment, the single-frame lidar point cloud data is used to generate a local map, while the acquisition pose of the single-frame lidar point cloud data is used to splice the local maps into a global map later.

[0081] The SLAM module includes: a ground removal unit and a laser odometry unit.

[0082] The ground removal unit is used to calculate the pitch angle of each point in each initial data of single-frame lidar point clouds respectively, and classify all the points in the initial data of single-frame lidar point clouds into ground points and non-ground points according to the calculated pitch angle. The single-frame lidar point cloud data is obtained by removing all the ground points and transmitted to the laser odometry unit and the dynamic removal module.

[0083] The laser odometry unit is used to construct a local map by performing frame-to-frame matching on all the single-frame lidar point cloud data; at the same time, it calculates the acquisition pose of the single-frame lidar point cloud data, and transmits all the local maps and the acquisition poses of the single-frame lidar point cloud data to the dynamic removal module.

[0084] The dynamic removal module is used to identify the dynamic object point clouds from the received single-frame lidar point cloud data, and splice the local maps after removing the dynamic object point clouds by using the acquisition pose of the single-frame lidar point cloud data to construct a global map.

[0085] The dynamic removal module includes: a point cloud voxelization unit, a multi-frame voxel comparison unit, a dynamic voxel association unit, a dynamic point extension unit, a dynamic point removal unit and a local map data storage space.

[0086] The point cloud voxelization unit is used to divide the single-frame lidar point cloud data into several voxels according to the position, sort the voxels according to the generation time, and store all the voxels by establishing a hash table and transmit them to the multi-frame voxel comparison unit.

[0087] The multi-frame voxel comparison unit is used to traverse each voxel of the current single-frame lidar point cloud data, generate the shape feature and distribution feature of each voxel, and perform dynamic determination on each voxel according to the shape feature and distribution feature of each voxel; transmit the dynamic determination results of each voxel in all the single-frame lidar point cloud data to the dynamic voxel association unit.

[0088] The dynamic determination result of the voxels is: static voxels, low-dynamic voxels or high-dynamic voxels.

[0089] The dynamic voxel association unit is used to set the proximity range of high-dynamic voxels. For any high-dynamic voxel, the principal component analysis method is used to associate the low-dynamic voxels within the proximity range of the high-dynamic voxel, and based on the association result, determine whether the low-dynamic voxels within the proximity range of the high-dynamic voxel are static voxels or high-dynamic voxels; all high-dynamic voxels in a single-frame LiDAR point cloud data are used as dynamic voxels, and all dynamic and static voxels in all single-frame LiDAR point cloud data are transmitted to the dynamic point extension unit.

[0090] The dynamic point extension unit is used to use the static voxels as a reference and adopt the region growing algorithm to extend the point cloud of the dynamic voxels, generate the point cloud of the dynamic objects in all single-frame LiDAR point cloud data and transmit it to the dynamic point removal part unit.

[0091] In this embodiment, it is determined whether the edge points of the static voxels can grow according to the approximation degree of the normal vectors of the edge points of the dynamic voxels and the static voxels, and the dynamic points are iteratively grown towards the static region according to the newly grown dynamic points to finally determine all the point clouds of the entire dynamic target, that is, the point cloud of the dynamic object.

[0092] The dynamic point removal unit is used to mark and remove the point cloud of the dynamic objects in all single-frame LiDAR point cloud data in the local map, and then splice the local map after removing the dynamic point cloud in the world coordinate system according to the acquisition pose of the single-frame LiDAR point cloud data to obtain a global map that only retains the static part.

[0093] The local map data storage space is used to save all voxels stored with a hash table as an index in each single-frame LiDAR point cloud data, as well as the dynamic determination results and geometric information of each voxel; the geometric information includes the average height, centroid and covariance of the voxel.

[0094] In this embodiment, the original points of each frame of voxels, the dynamic determination information and geometric information of the voxels are stored with a hash table as an index.

[0095] An online removal method for dynamic targets in a laser SLAM process in this embodiment is implemented by using the online removal system for dynamic targets in a laser SLAM process, as Figure 2 shown. This method includes the following steps:

[0096] Step 1: Obtain a plurality of initial single-frame LiDAR point cloud data, generate a plurality of single-frame LiDAR point cloud data by removing the ground points in each initial single-frame LiDAR point cloud data, calculate the acquisition pose of each single-frame LiDAR point cloud data by using the frame-to-frame matching method, and construct a local map.

[0097] The specific content of step 1 is: Obtain a number of initial single-frame lidar point cloud data.

[0098] For any initial single-frame lidar point cloud data, calculate the pitch angle of each lidar point in the initial single-frame lidar point cloud data respectively.

[0099] The calculation formula for the pitch angle is:

[0100] (1);

[0101] Wherein, represents the pitch angle of the lidar point; represents the optical center coordinates of the lidar in the world coordinate system; represents the abscissa of the optical center of the lidar in the world coordinate system; represents the ordinate of the optical center of the lidar in the world coordinate system; represents the vertical coordinate of the optical center of the lidar in the world coordinate system; represents the lidar point coordinates in the world coordinate system; represents the abscissa of the lidar point in the world coordinate system; represents the ordinate of the lidar point in the world coordinate system; represents the vertical coordinate of the lidar point in the world coordinate system.

[0102] In this embodiment, calculate the pitch angle of each lidar point during acquisition in the initial single-frame lidar point cloud data according to the pitch angle calculation formula , if the angle of the pitch angle is positive, the laser emission angle of this point is upward relative to the horizontal, and if the angle of the pitch angle is negative, the laser emission angle of this lidar point is downward relative to the horizontal.

[0103] According to the pitch angle of each lidar point in the initial single-frame lidar point cloud data and the line number characteristics of the laser, calculate the line serial number of each lidar point, and classify the lidar points according to the line serial numbers of all lidar points in the initial single-frame lidar point cloud data, so as to generate lines with pitch angles from low to high.

[0104] In this embodiment, calculate the line serial numbers corresponding to all lidar points according to the pitch angle value and the line number characteristics of the laser itself scanID , classify all lidar points in a single frame according to the line order, and form a certain number of lines with pitch angles from low to high. Since the number of classified lines is the same as the line number of the laser itself, the number of classified lines is different for different lasers. The line number characteristics of common lidars can be divided into 16-line lasers, 32-line lasers, 64-line lasers and solid-state lasers. Among them, the line number of the laser of a 64-line lidar is 64, and the line number of the laser of a 32-line lidar is 32; for example, XT32 is a lidar of Hesai, and it will form 32 classified lines here.

[0105] The calculation method of the line serial number is as follows:

[0106] (2);

[0107] Wherein, scanID represents the line serial number of the laser point; represents the ceiling function; represents the initial pitch angle of the laser, which is determined by the characteristics of the laser itself; represents the line interval angle of the laser, which is determined by the characteristics of the laser itself.

[0108] In this embodiment, since the number of laser lines, the initial pitch angle, and the laser interval angle are all determined by the laser characteristics, the number of laser lines, the initial pitch angle, and the laser interval angle of different lasers are not necessarily exactly the same. For example, for lasers with 32 lines, the initial pitch angles and laser interval angles of oster and Hesai are different, but the number of lines is the same.

[0109] Calculate the yaw angle of each laser point in the initial data of a single-frame laser point cloud. Traverse each laser point in the line with the lowest pitch angle in turn. The traversal process is as follows: Take the currently traversed laser point as the target point P1, find the laser point P2 with the same yaw angle as the target point P1 in the line with the second lowest pitch angle, and calculate the elevation angle of the laser point P2 relative to the target point P1. Determine whether the target point P1 changes from a ground point to a non-ground point according to the elevation angle of the laser point P2 relative to the target point P1, and mark the ground points according to the judgment result. Then remove the ground points from the initial data of the single-frame laser point cloud to obtain the single-frame laser point cloud data.

[0110] The calculation method of the yaw angle is as follows:

[0111] (3);

[0112] Wherein, represents the yaw angle of the laser point, and the range of is from -180 degrees to +180 degrees, and this angle is recorded along with the laser point.

[0113] The calculation method of the elevation angle of the laser point P2 relative to the target point P1 is as follows:

[0114] (4);

[0115] Wherein, represents the elevation angle of the laser point P2 relative to the target point P1; represents the coordinates of the target point P1; represents the abscissa of the target point P1; represents the ordinate of the target point P1; represents the vertical coordinate of the target point P1; Represents the coordinates of the laser point P2; Represents the abscissa of the laser point P2; Represents the ordinate of the laser point P2; Represents the vertical coordinate of the laser point P2; Represents the square root function.

[0116] In this embodiment, as Figure 3 shown, the points with the same yaw angle are sorted according to the pitch angle from low to high. Among a certain number of lines, the point P with the lowest pitch angle low starts to traverse each laser point until the point P with the highest pitch angle high , and the target point P1 to be traversed searches for the laser point P2 with the same yaw angle as P1 in the line with the second lowest pitch angle, and calculates the elevation angle of P2 relative to P1. According to the degree of the elevation angle, it is judged whether the laser point changes from a ground point to a non-ground point.

[0117] The method for judging whether the target point P1 changes from a ground point to a non-ground point is as follows: If the elevation angle of the laser point P2 relative to the target point P1 is greater than 10 degrees, it is judged that the target point P1 has changed from a ground point to a non-ground point, and the laser points between the starting point P low on the line with the lowest pitch angle and the target point P1 are marked as ground points, and the laser points between the laser point P2 and the end point P high on the line with the highest pitch angle are marked as non-ground points; otherwise, it is judged that the target point P1 has not changed from a ground point to a non-ground point.

[0118] Perform frame-to-frame matching on all single-frame laser point cloud data, calculate the acquisition pose of the single-frame laser point cloud data, and construct a local map.

[0119] In this embodiment, the single-frame laser point cloud data after removing the ground points is passed through the laser odometry unit to calculate the acquisition position and attitude information of the single-frame laser point cloud data in the world coordinate system, that is, to calculate the acquisition pose of the single-frame laser point cloud data. In the laser odometry unit, by performing iterative closest point solution on consecutive multi-frame point cloud data after removing the ground points, the acquisition position and attitude information of the single-frame laser point cloud data are calculated. The input of the iterative closest point is the point cloud data after removing the ground points in the previous and subsequent two frames, and the output result is the position transformation T and the attitude transformation R between the two frames. The Iterative Closest Point (ICP) algorithm is a classic point cloud registration method used to calculate the best rigid body transformation between two point clouds, including rotation and translation. When processing the point cloud data after removing the ground points in the previous and subsequent two frames, the ICP algorithm can effectively find the position transformation T and the attitude transformation R between them.

[0120] The method for frame-to-frame matching of all single-frame lidar point cloud data is as follows: For any single-frame lidar point cloud data, an initial rigid body transformation is selected for this single-frame lidar point cloud data. The single-frame lidar point cloud data of the previous and next frames are used as reference frames. The nearest point pairs are searched in the reference frames, and the optimal rigid body transformation is calculated to minimize the sum of the distances from the points in this single-frame lidar point cloud data to the corresponding points in the reference frames. The single-frame lidar point cloud data is updated using the optimal rigid body transformation to obtain the position transformation T and attitude transformation R of this single-frame lidar point cloud data, and then the acquisition pose of the single-frame lidar point cloud data is calculated.

[0121] In this embodiment, the local map is composed of lidar point cloud data within 3 seconds. The storage information of each point in the lidar point cloud includes position information and a discriminant item indicating whether the point is a ground point, and the ground points in the local map are removed.

[0122] In this embodiment, the point cloud map of the overall map of a certain street is as Figure 4 shown. The color depth of the points reflects the height information of the points. The lower the height, the darker the color; the higher the height, the lighter the color. This point cloud map is formed by merging multiple frames of point clouds through laser SLAM calculation. The contents of the three white circles in the point cloud map from left to right are the continuous point clouds formed by dynamic pedestrians, dynamic motor vehicles, and dynamic electric vehicles in sequence. These points affect the calculation accuracy of SLAM and the visual effect of the three-dimensional point cloud map, and are dynamic points that need to be removed. The discrimination of whether the laser points in this point cloud map are dynamic points is as Figure 5 shown. The light-colored part is the extracted ground point cloud, and the dark-colored part is the non-ground area. And according to the discrimination result, the dynamic points in the point cloud map are removed, that is, Figure 4 the point cloud after removing the ground points in the street in Figure 6 is shown.

[0123] Step 2: For any single-frame lidar point cloud data, a number of voxels are generated by two-dimensional rasterization of this single-frame lidar point cloud data, and each voxel is dynamically judged respectively, and then dynamic voxel association is performed according to the dynamic judgment result, so as to divide each voxel into dynamic voxels and static voxels.

[0124] The said Step 2 further includes:

[0125] Step 2.1: For any single-frame lidar point cloud data, two-dimensional rasterization is performed on this single-frame lidar point cloud data, and the point set formed by the laser points in each grid is used as a voxel. The voxel numbers are determined according to the generation order of the voxels, and each voxel is stored by using the method with a hash table as an index.

[0126] In this embodiment, two-dimensional rasterization of single-frame laser point cloud data is performed as follows: According to the horizontal and vertical axis directions of the world coordinate system, the point cloud is divided into squares with a side length of 0.1 m. The rasterization coordinate systems of different frames are the same, all being the world coordinate system, thereby establishing grids of 0.01 square meters. The set of all laser points within each grid is called a voxel, and the voxels are sorted according to the generation time to determine the voxel numbers. For each voxel, a hash table is established for storage for fast reading; where the hash table is a three-dimensional indexed hash table, and the input of the hash table is the central coordinate of the voxel, and the output is the voxel number , establishing a data structure with a query time complexity of 1. This data structure is a multi-element array, where the structure of a single element is .

[0127] In this embodiment, a part of a street in the overall point cloud map of a certain street after removing ground points is intercepted. As Figure 7 shown, the area circled in black is the dynamic point cloud area formed by dynamic vehicles. Two-dimensional rasterization of the point cloud map, that is, point cloud voxelization, is performed. As Figure 8 shown, the points within the same grid are merged into one voxel for display.

[0128] Step 2.2: Calculate the number of points, average point height, and point dispersion in each voxel respectively, and use the average point height in each voxel as the shape feature of the voxel, and the number of points and point dispersion in each voxel as the distribution feature of the voxel.

[0129] The specific content of step 2.2 is as follows: For any voxel, count the number of points in this voxel ; obtain the heights of all points in this voxel, and calculate the average height of all points in this voxel and use it as the average point height in this voxel; obtain the coordinates of all points in this voxel in the world coordinate system and the geometric center of this voxel, and calculate the point dispersion of this voxel.

[0130] The calculation method of the point dispersion of this voxel is:

[0131] (5);

[0132] Among them, represents the point dispersion of the voxel; represents the number of points in the voxel; represents the point index in the voxel; represents the th point coordinate in the voxel in the world coordinate system; represents the geometric center of the voxel.

[0133] Step 2.3: For any voxel in the current single-frame LiDAR point cloud data, use the two single-frame LiDAR point cloud data before and after the current single-frame LiDAR point cloud data as reference frames. By comparing the voxel with the voxels at the same position in the reference frames in the world coordinate system, perform dynamic determination on the voxel based on its shape features and distribution features, and classify the voxel into a static voxel, a low-dynamic voxel, or a high-dynamic voxel according to the dynamic determination result.

[0134] The specific content of the said Step 2.3 is: For any voxel in the current single-frame LiDAR point cloud data, respectively determine the reference voxels in each reference frame that have the same position as the voxel in the world coordinate system. By calculating the differences in the number of points, the average height of points, and the dispersion of points between the voxel and the reference voxels, respectively obtain the difference errors between the voxel and each reference voxel.

[0135] In this embodiment, compare the number of points, the average height of points, and the dispersion of points for the voxels respectively, with the difference errors expressing the voxel feature differences between different frames.

[0136] The calculation method of the said difference error is:

[0137] (6);

[0138] Wherein, represents the difference error between the voxel and the reference voxel ; represents the th voxel in the th single-frame LiDAR point cloud data; represents the th voxel in the th single-frame LiDAR point cloud data; wherein the geometric center of and of the geometric center are the same, and is the reference voxel of ; ; represents the weight of the number of points; represents the number of points of the voxel ; represents the number of points of the reference voxel ; represents the sum of the number of points of the voxel and its neighboring voxels. In this embodiment, the neighboring voxels are set as the 8 voxels around the voxel; represents the weight of the average height of points; represents the average height of points of the voxel ; Indicates the reference voxel The points are all at a high level; Indicates the voxel And the sum of the heights of the points of the voxel and its neighboring voxels; Indicates the weight of the point dispersion; Indicates the voxel The point dispersion of; Indicates the reference voxel The point dispersion of; Indicates the voxel And the sum of the point dispersions of the voxel and its neighboring voxels.

[0139] Based on the difference error between the voxel and each reference voxel, the voxel is dynamically determined. For the four reference voxels of the voxel, if there are at least two reference voxels corresponding to the difference error greater than the set dynamic threshold , then the dynamic determination result of the voxel is a high-dynamic voxel; if there is a reference voxel corresponding to the difference error greater than the set dynamic threshold , then the dynamic determination result of the voxel is a low-dynamic voxel; if there is no reference voxel corresponding to the difference error greater than the set dynamic threshold , then the dynamic determination result of the voxel is a static voxel.

[0140] In this embodiment, the set dynamic threshold Is obtained by adjusting parameters through a large number of experiments; since the grids are independent of each other, parallel processing can be used to improve the speed of calculating and comparing the grid shape and distribution characteristics.

[0141] Step 2.4: Set the neighboring range of the high-dynamic voxels. For any high-dynamic voxel, the principal component analysis method is used to perform an association judgment on the low-dynamic voxels within the neighboring range of the high-dynamic voxel, and the low-dynamic voxels within the neighboring range of the high-dynamic voxel are determined to be static voxels or high-dynamic voxels according to the association result. All the high-dynamic voxels in the single-frame lidar point cloud data are used as dynamic voxels.

[0142] In this embodiment, the principal component analysis method is used to perform an association judgment on the low-dynamic voxels adjacent to the high-dynamic voxels. The purpose is to perform a secondary detection on the low-dynamic voxels. Traverse each high-dynamic voxel in the current frame dynamically, and judge whether the 8 voxels around these high-dynamic voxels are low-dynamic voxels; if there are low-dynamic voxels among the 8 voxels around any high-dynamic voxel, the principal component analysis method is used to calculate the main and secondary distribution vectors of the point clouds in the central high-dynamic voxel and the adjacent low-dynamic voxel respectively, and judge whether the two voxels have geometric consistency according to the main and secondary distribution vectors of the point clouds of the two voxels. If there is geometric consistency, the two voxels are associated, and the dynamic information of the low-dynamic voxel is modified to a high-dynamic voxel.

[0143] The specific content of step 2.4 is as follows: For any high-dynamic voxel in the current single-frame lidar point cloud data, each low-dynamic voxel within the adjacent range of the high-dynamic voxel is regarded as an adjacent voxel of the high-dynamic voxel, and the centroid of the high-dynamic voxel and the centroids of all adjacent voxels of the high-dynamic voxel are calculated respectively.

[0144] The method for calculating the centroid of the high-dynamic voxel and the centroids of all adjacent voxels of the high-dynamic voxel respectively is:

[0145] (7);

[0146] where represents the centroid of the voxel.

[0147] The high-dynamic voxel is centered using the centroid of the high-dynamic voxel, and the covariance matrix of the high-dynamic voxel is calculated using the centered high-dynamic voxel; for any adjacent voxel of the high-dynamic voxel, the adjacent voxel is centered using the centroid of the adjacent voxel, and the covariance matrix of the adjacent voxel is calculated using the centered adjacent voxel.

[0148] The covariance matrix is expressed as:

[0149] (8);

[0150] where represents the covariance matrix; represents the covariance of the voxel in the direction in the world coordinate system; represents the covariance of the voxel in the and direction in the world coordinate system; represents the covariance of the voxel in the and direction in the world coordinate system; represents the covariance of the voxel in the and direction in the world coordinate system; represents the covariance of the voxel in the direction in the world coordinate system; represents the covariance of the voxel in the and direction in the world coordinate system; represents the covariance of the voxel in the and direction in the world coordinate system; represents the covariance of the voxel in the and direction in the world coordinate system; represents the covariance of the voxel in the world coordinate system in the direction. And there is:

[0151] (9);

[0152] Among them, represents the covariance of the voxel in the world coordinate system in the and direction; and represent the directions in the world coordinate system. In this embodiment, it can be , , one of them, and and can be the same or different; represents the value of the th point in the voxel in the direction; represents the value of the th point in the voxel in the direction; represents the average value of all points in the voxel in the direction; represents the average value of all points in the voxel in the direction.

[0153] For this high-dynamic voxel or any adjacent voxel of this high-dynamic voxel, solve the eigenvalues of the covariance matrix according to the matrix properties of the covariance matrix of the voxel, and use the eigenvalues of the covariance matrix to solve the non-zero eigenvectors corresponding to respectively. will be used as the main direction of the voxel in the distribution, and will be used as the secondary main direction of the voxel in the distribution, and

[0154] In this embodiment, solve the eigenvalues and eigenvectors of the matrix for the covariance matrix C. Since the covariance matrix C is a three-dimensional matrix, calculate the eigenvalues of the covariance and the corresponding eigenvectors , where ; the main direction of the point cloud in the distribution is , the secondary main direction is , and the eigenvector expresses the spatial distribution of the points in the voxel. The specific method is to first calculate according to the matrix property , where Eis the identity matrix, and then substitute the obtained into to solve for the non - zero solution .

[0155] In this embodiment, since the lidar can only scan one side of an object at a time, for multiple adjacent voxels containing dynamic objects, the distribution of the dynamic objects in the point - cloud space formed by the lidar scan can only be one side. The geometric consistency can be judged according to the principal - secondary distribution vectors of the point clouds of two voxels.

[0156] Then, according to the principal - secondary distribution vector of the point cloud of the high - dynamic voxel and the principal - secondary distribution vectors of the point clouds of all adjacent voxels of this high - dynamic voxel, calculate the geometric consistency between this high - dynamic voxel and each adjacent voxel of this high - dynamic voxel respectively.

[0157] The calculation method of the geometric consistency is as follows:

[0158] (10);

[0159] Among them, represents the geometric consistency between voxel and voxel ; represents the yaw - angle weight; represents the pitch - angle weight; represents the center of voxel in the world coordinate system; represents the center of voxel in the world coordinate system.

[0160] In this embodiment, the trigonometric functions in formula (10) calculate the relative angle between the centers of two voxels, which can reflect the distribution direction of the point - cloud data in space, and further infer the influence of the principal - secondary distribution vectors of the point - cloud data on the geometric consistency.

[0161] For any adjacent voxel of this high - dynamic voxel, if the geometric consistency between this high - dynamic voxel and this adjacent voxel exceeds the set geometric - consistency threshold , then mark this adjacent voxel as a high - dynamic voxel; if the geometric consistency between this high - dynamic voxel and this adjacent voxel does not exceed the set geometric - consistency threshold , then mark this adjacent voxel as a static voxel.

[0162] In this embodiment, the set geometric - consistency threshold is obtained by adjusting parameters through multiple experiments. As Figure 9 shown, perform dynamic - voxel discrimination on a partial point - cloud map of a certain street after voxelizing the point cloud, where the vehicle ghosting within the black circle is the grid voxel formed by the dynamic target.

[0163] Step 3: Use the region growing algorithm to extend the neighboring point cloud of the dynamic voxel to generate the point cloud of the dynamic object in the single-frame lidar point cloud data.

[0164] In this embodiment, a KDTree of the three-dimensional point cloud is established for the dynamic voxel and its neighboring voxels. The region growing algorithm is used to extend the neighboring point cloud of the dynamic voxel points. The purpose of extending the neighboring point cloud of the dynamic voxel points is to identify a small amount of point cloud of the dynamic target in the static voxel. For each neighboring voxel of the dynamic voxel, a KDTree of the point cloud with respect to the world coordinate system is established, and through the region growing method, it grows from the dynamic voxel to the static voxel, and the points that meet the growth conditions are classified as dynamic points to determine the complete dynamic region.

[0165] The said Step 3 further includes:

[0166] Step 3.1: For any dynamic voxel in the current single-frame lidar point cloud data, determine the neighboring voxels of the dynamic voxel, and store the lidar points in the dynamic voxel and the lidar points in the neighboring voxels of the dynamic voxel by establishing a KDTree.

[0167] In this embodiment, the purpose of establishing a three-dimensional KDTree of the point cloud is to use the tree-shaped data structure based on the KDTree, which can utilize this structure to achieve efficient searching of the point cloud and quickly find all points within a set threshold distance from any point in the point cloud.

[0168] Step 3.2: Inside the neighboring voxels of the dynamic voxel, use the KDTree to respectively search for all lidar points within a set threshold distance from the center point of the dynamic voxel in each neighboring voxel, and screen out all lidar points belonging to the static voxel from the searched lidar points to construct a set of static voxel points .

[0169] In this embodiment, a KDTree is established for the dynamic voxel and its neighboring voxels, and the KDTree is used to obtain a set of static voxel points within 3 cm of the neighboring dynamic voxels .

[0170] Step 3.3: Use the KDTree to quickly find the three nearest static voxel points in the neighborhood of each lidar point in , and calculate the normal vector of the plane fitted by the three points, .

[0171] In this embodiment, the normal vector calculation formula is as shown in formula (11):

[0172] (11);

[0173] Among them, the three points in the neighborhood used to calculate the normal vector are respectively , , , is the normal vector corresponding to the plane fitted by the three points.

[0174] Step 3.4: Calculate the normal vector and the normal vector of the included angle .

[0175] Calculate the included angle of the two normal vectors according to the vector included angle formula:

[0176] (12);

[0177] Among them, represents the included angle between the normal vector and the normal vector .

[0178] In this embodiment, the content of the region growing algorithm is as follows: Determine whether the dynamic voxel can grow according to the approximation degree of the normal vectors of the edge points of the dynamic voxel and the static voxel. The growth will select appropriate points in the static voxel as the growing dynamic points, and iteratively grow towards the static region according to the newly grown dynamic points to finally determine all the point clouds of the entire dynamic target.

[0179] Step 3.5: When for a certain laser point in , it is considered that the laser point and the laser points in the dynamic voxel are on the same plane, and this laser point is used as the extension point of the dynamic voxel; then use all the extension points of the dynamic voxel to construct an extension point set ; At the same time, mark all the laser points in that do not meet and construct a static point set .

[0180] Step 3.6: For any extension point in the extension point set , in the neighboring voxels of the voxel where the extension point is located, use KDTree to respectively search for all the laser points within the set threshold distance from the center point of the voxel where the extension point is located in each neighboring voxel, and screen out all the laser points belonging to the static voxel from the searched laser points to construct a new set of static voxel points, and repeat steps 3.3 - 3.5 until there are no new extension points.

[0181] Step 3.7: Generate the point cloud of the dynamic object in the single-frame laser point cloud data by using all the dynamic voxels and the extension points of each dynamic voxel.

[0182] In this embodiment, as Figure 10 shown, the dark part is the point set in the original dynamic voxel, and the light part is the point set of the original static voxel. Since the included angle of the normal vectors of the point cloud at the connection between the dynamic voxel and the static voxel calculated by the region growing algorithm is less than 10°, the static voxel region can be marked as a dynamic voxel by extension.

[0183] Step 4: Repeat Steps 2 - 3 to generate the point cloud of dynamic objects in all single-frame lidar point clouds, mark all the point clouds of dynamic objects in the local map and remove them, and obtain a local map that only retains the static part.

[0184] In this embodiment, the classification result of the voxel and the result of the extended points are recorded in the current local map. The dynamic voxels, associated voxels of the dynamic voxels, and dynamic points extended from the dynamic voxels in each local map are deleted, and only the static part is retained. When generating the next local map, repeat Steps 2 - 3 to obtain the corresponding local map that only retains the static part.

[0185] Step 5: Stitch all the local maps that only retain the static part according to the acquisition poses of each single-frame lidar point cloud to obtain a global map that only retains the static part.

[0186] In this embodiment, since each local map is composed of point clouds in the world coordinate system, the stitching process is to directly find the union of the point clouds, and the completion of the overall map means the completion of 3D reconstruction.

[0187] In this embodiment, as Figure 11 shown, perform dynamic target removal on the point cloud map of a certain street. Figure (a) is the overall point cloud map before dynamic removal, including the dynamic point clouds of pedestrians, vehicles, and electric vehicles; Figure (b) is the overall point cloud map after removing the dynamic point clouds of pedestrians, vehicles, and electric vehicles. As Figure 12 shown, perform dynamic target removal on the point cloud map of another street. Figure (a) is the overall point cloud map before dynamic removal, including the dynamic point cloud of vehicles, and Figure (b) is the overall point cloud map after removing the dynamic point cloud of vehicles.

[0188] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it; 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 recorded in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope defined by the claims of the present invention.

Claims

1. An online removal system for dynamic targets during the laser SLAM process, characterized in that, The system includes: a lidar, a SLAM module, and a dynamic removal module; The lidar is used to obtain the three-dimensional structure information of objects and space in the surrounding environment, generate a number of initial single-frame lidar point cloud data, and send it to the SLAM module; The SLAM module is used to remove the ground points in all the initial single-frame lidar point cloud data, obtain the single-frame lidar point cloud data and construct a local map. At the same time, calculate the position and pose of the lidar in the world coordinate system when generating each initial single-frame lidar point cloud data as the acquisition pose of the single-frame lidar point cloud data, and transmit all the single-frame lidar point cloud data, the local map, and the acquisition pose of the single-frame lidar point cloud data to the dynamic removal module; The SLAM module includes: a ground removal unit and a laser odometry unit; The ground removal unit is used to calculate the pitch angle of each point in each initial single-frame lidar point cloud data respectively, and divide all the points in the initial single-frame lidar point cloud data into ground points and non-ground points according to the calculated pitch angle. Obtain the single-frame lidar point cloud data by removing all the ground points and transmit it to the laser odometry unit and the dynamic removal module; The laser odometry unit is used to construct a local map by performing frame-to-frame matching on all the single-frame lidar point cloud data; at the same time, calculate the acquisition pose of the single-frame lidar point cloud data, and transmit all the local maps and the acquisition poses of the single-frame lidar point cloud data to the dynamic removal module; The dynamic removal module is used to identify the dynamic object point cloud from the received single-frame lidar point cloud data, and use the acquisition pose of the single-frame lidar point cloud data to splice and remove the local map after the dynamic object point cloud, and construct a global map.

2. The online removal system for dynamic targets in the laser SLAM process according to claim 1, wherein The dynamic removal module includes: a point cloud voxelization unit, a multi-frame voxel comparison unit, a dynamic voxel association unit, a dynamic point extension unit, a dynamic point removal unit, and a local map data storage space; The point cloud voxelization unit is used to divide the single-frame lidar point cloud data into a number of voxels according to the position, sort the voxels according to the generation time, and store all the voxels by establishing a hash table and transmit them to the multi-frame voxel comparison unit; The multi-frame voxel comparison unit is used to traverse each voxel of the current single-frame lidar point cloud data, generate the shape feature and distribution feature of each voxel, and perform dynamic determination on each voxel according to the shape feature and distribution feature of each voxel; transmit the dynamic determination results of each voxel in all the single-frame lidar point cloud data to the dynamic voxel association unit; where the dynamic determination result of the voxel is: static voxel, low-dynamic voxel or high-dynamic voxel; The dynamic voxel association unit is used to set the adjacent range of the high-dynamic voxel. For any high-dynamic voxel, use the principal component analysis method to associate the low-dynamic voxels within the adjacent range of the high-dynamic voxel, and determine whether the low-dynamic voxels within the adjacent range of the high-dynamic voxel are static voxels or high-dynamic voxels according to the association result; regard all the high-dynamic voxels in the single-frame lidar point cloud data as dynamic voxels, and transmit all the dynamic voxels and static voxels in the single-frame lidar point cloud data to the dynamic point extension unit; The dynamic point extension unit is used to take static voxels as a reference and perform point cloud extension on dynamic voxels using a region growing algorithm, generate the point cloud of dynamic objects in all single-frame lidar point cloud data, and transmit it to the dynamic point removal unit; The dynamic point removal unit is used to mark and remove the point cloud of dynamic objects in all single-frame lidar point cloud data in the local map, and then stitch the local map after removing the dynamic point cloud in the world coordinate system according to the acquisition pose of the single-frame lidar point cloud data to obtain a global map that only retains the static part; The local map data storage space is used to save all voxels stored with a hash table as an index in each single-frame lidar point cloud data, as well as the dynamic determination results and geometric information of each voxel; the geometric information includes the average height, centroid, and covariance of the voxel.

3. An online removal method for dynamic targets in the laser SLAM process, which is implemented by using the online removal system for dynamic targets in the laser SLAM process described in any one of claims 1-2, characterized in that, This method includes the following steps: Step 1: Obtain several initial single-frame lidar point cloud data, generate several single-frame lidar point cloud data by removing ground points in each initial single-frame lidar point cloud data, calculate the acquisition pose of each single-frame lidar point cloud data using a frame-to-frame matching method, and construct a local map; Step 2: For any single-frame lidar point cloud data, generate several voxels by performing two-dimensional rasterization on the single-frame lidar point cloud data, perform dynamic determination on each voxel respectively, and then perform dynamic voxel association according to the dynamic determination results, so as to divide each voxel into dynamic voxels and static voxels; Step 3: Use the region growing algorithm to perform adjacent point cloud extension on the dynamic voxels to generate the point cloud of dynamic objects in the single-frame lidar point cloud data; Step 4: Repeat steps 2-3 to generate the point cloud of dynamic objects in all single-frame lidar point cloud data, mark and remove all the point clouds of dynamic objects in the local map to obtain a local map that only retains the static part; Step 5: Stitch all the local maps that only retain the static part according to the acquisition pose of each single-frame lidar point cloud data to obtain a global map that only retains the static part.

4. The online removal method for dynamic targets in a laser SLAM process according to claim 3, characterized in that, The specific content of step 1 is: Obtain several initial single-frame lidar point cloud data; For any initial single-frame lidar point cloud data, calculate the pitch angle of each laser point in the single-frame lidar point cloud data respectively; According to the pitch angle of each laser point in the initial single-frame lidar point cloud data and the line number characteristics of the laser, calculate the line number of each laser point, and classify the laser points according to the line numbers of all laser points in the initial single-frame lidar point cloud data, so as to generate lines with increasing pitch angle; Calculate the yaw angle of each laser point in the initial single-frame lidar point cloud data, and sequentially traverse each laser point in the line with the lowest pitch angle. The traversal process is: Take the currently traversed laser point as the target point P1, find the laser point P2 with the same yaw angle as the target point P1 in the line with the second lowest pitch angle, and calculate the elevation angle of the laser point P2 relative to the target point P1. According to the elevation angle of the laser point P2 relative to the target point P1, judge whether the target point P1 changes from a ground point to a non-ground point, and mark the ground points according to the judgment result, and then remove the ground points in the initial single-frame lidar point cloud data to obtain the single-frame lidar point cloud data; The method for determining whether the target point P1 has changed from a ground point to a non-ground point is as follows: If the elevation angle of the laser point P2 relative to the target point P1 is greater than 10 degrees, it is determined that the target point P1 has changed from a ground point to a non-ground point, and the starting point P on the line with the lowest pitch angle low The laser points between the target point P1 are marked as ground points, and the laser points between the laser point P2 and the end point P on the line with the highest pitch angle high are marked as non-ground points; otherwise, it is determined that the target point P1 has not changed from a ground point to a non-ground point; Perform frame-to-frame matching on all single-frame laser point cloud data, calculate the acquisition pose of the single-frame laser point cloud data, and construct a local map.

5. The online removal method of dynamic targets in a laser SLAM process according to claim 4, wherein, The step 2 further includes: Step 2.1: For any single-frame laser point cloud data, rasterize the single-frame laser point cloud data into two dimensions, and use the point set formed by the laser points in each grid as a voxel. Determine the voxel number according to the generation order of the voxels, and store each voxel by means of a hash table as an index; Step 2.2: Calculate the number of points, average point height, and point dispersion in each voxel respectively, use the average point height in each voxel as the shape feature of the voxel, and use the number of points and point dispersion of each voxel as the distribution feature of the voxel; Step 2.3: For any voxel in the current single-frame laser point cloud data, use the two single-frame laser point cloud data before and after the current single-frame laser point cloud data as reference frames, and dynamically determine the voxel by comparing the shape features and distribution features between the voxel and the voxels with the same position in the world coordinate system in the reference frames, and classify the voxel into a static voxel, a low-dynamic voxel, or a high-dynamic voxel according to the dynamic determination result; Step 2.4: Set the proximity range of the high-dynamic voxels. For any high-dynamic voxel, use the principal component analysis method to perform an association judgment on the low-dynamic voxels within the proximity range of the high-dynamic voxel, and determine whether the low-dynamic voxels within the proximity range of the high-dynamic voxel are static voxels or high-dynamic voxels according to the association result, and regard all the high-dynamic voxels in the single-frame laser point cloud data as dynamic voxels.

6. The online removal method for dynamic targets during laser SLAM according to claim 5, characterized in that The specific content of the step 2.3 is: For any voxel in the current single-frame laser point cloud data, respectively determine the reference voxels with the same position as the voxel in the world coordinate system in each reference frame, and obtain the difference errors between the voxel and each reference voxel by calculating the difference in the number of points, the difference in average point height, and the difference in point dispersion between the voxel and the reference voxel; The calculation method of the difference error is: , where represents a voxel and the difference error from the reference voxel ; represents the -th voxel in the -th single-frame lidar point cloud data; represents the -th voxel in the -th single-frame lidar point cloud data, and is the reference voxel of ; represents the weight of the number of points; represents the number of points of the voxel ; represents the number of points of the reference voxel ; represents the sum of the number of points of the voxel and its neighboring voxels; represents the weight of the average height of points; represents the average height of points of the voxel ; represents the average height of points of the reference voxel ; represents the sum of the average heights of points of the voxel and its neighboring voxels; represents the weight of the point dispersion; represents the point dispersion of the voxel ; represents the point dispersion of the reference voxel ; represents the sum of the point dispersions of the voxel and its neighboring voxels; Dynamically determine the voxel based on the difference error between the voxel and each reference voxel. For the four reference voxels of the voxel, if there are at least two reference voxels corresponding to difference errors greater than the set dynamic threshold , then the dynamic determination result of the voxel is a high-dynamic voxel; if there is one reference voxel corresponding to a difference error greater than the set dynamic threshold , then the dynamic determination result of the voxel is a low-dynamic voxel; if there is no reference voxel corresponding to a difference error greater than the set dynamic threshold , then the dynamic determination result of the voxel is a static voxel.

7. The online removal method for dynamic targets in the laser SLAM process according to claim 6, characterized in that The specific content of the step 2.4 is: For any high-dynamic voxel in the current single-frame laser point cloud data, regard each low-dynamic voxel within the proximity range of the high-dynamic voxel as a neighboring voxel of the high-dynamic voxel, and calculate the centroid of the high-dynamic voxel and the centroids of all the neighboring voxels of the high-dynamic voxel respectively; Perform centering processing on the high-dynamic voxel by using the centroid of the high-dynamic voxel, and calculate the covariance matrix of the high-dynamic voxel by using the centered high-dynamic voxel; For any neighboring voxel of the high-dynamic voxel, perform centering processing on the neighboring voxel by using the centroid of the neighboring voxel, and calculate the covariance matrix of the neighboring voxel by using the centered neighboring voxel; For the high-dynamic voxel or any adjacent voxel of the high-dynamic voxel, solve the eigenvalues of the covariance matrix according to the matrix properties of the covariance matrix of the voxel , and use the eigenvalues of the covariance matrix to solve the non-zero eigenvectors corresponding to respectively. Take as the main direction of the voxel in terms of distribution, take as the secondary main direction of the voxel in terms of distribution, and take and and as the primary and secondary distribution vectors of the point cloud of the voxel; Then, according to the main and secondary distribution vectors of the point cloud of the high-dynamic voxel and the main and secondary distribution vectors of the point cloud of all the neighboring voxels of the high-dynamic voxel, calculate the geometric consistency between the high-dynamic voxel and each neighboring voxel of the high-dynamic voxel respectively; For any neighboring voxel of the high-dynamic voxel, if the geometric consistency between the high-dynamic voxel and the neighboring voxel exceeds a set geometric consistency threshold , then mark the neighboring voxel as a high-dynamic voxel; if the geometric consistency between the high-dynamic voxel and the neighboring voxel does not exceed the set geometric consistency threshold , then mark the neighboring voxel as a static voxel.

8. The online removal method for dynamic targets during laser SLAM according to claim 7, characterized in that, The calculation method of the geometric consistency is: , wherein, represents the geometric consistency between voxels and voxels ; represents the yaw angle weight; represents the pitch angle weight; represents the center of the voxel in the world coordinate system; represents the center of the voxel in the world coordinate system.

9. The online removal method for dynamic targets in a laser SLAM process according to claim 8, characterized in that, The step 3 further includes: The step 3 further includes: Step 3.1: For any dynamic voxel in the current single-frame LiDAR point cloud data, determine the adjacent voxels of this dynamic voxel, and store the LiDAR points in this dynamic voxel and the LiDAR points in the adjacent voxels of this dynamic voxel by building a KDTree; Step 3.2: Within the neighboring voxels of the dynamic voxel, use KDTree to separately find all the laser points within each neighboring voxel whose distances from the center point of the dynamic voxel are within the set threshold, and screen out all the laser points belonging to the static voxels from the found laser points to construct a static voxel point set ; Step 3.3: Use KDTree to quickly find the three nearest static voxel points within the neighborhood of each laser point, and calculate the normal vector of the plane fitted by the three points , and at the same time calculate the normal vector of the plane fitted by the three nearest dynamic voxel points within the neighborhood of the laser point ; Step 3.4: Calculate the normal vector and the normal vector to calculate the included angle ; Step 3.5: When the of a certain laser point in, it is considered that the laser point is on the same plane as the laser points in the dynamic voxel, and this laser point is used as the extension point of the dynamic voxel; then, an extension point set is constructed using all the extension points of the dynamic voxel; at the same time, all the laser points in that do not meet are marked and a static point set is constructed; Step 3.6: For any extension point in the set of extension points use a KDTree to separately search for all the laser points within each neighboring voxel of the voxel where the extension point is located, whose distances from the center point of the voxel where the extension point is located are within a set threshold. Then, screen out all the laser points belonging to static voxels from the found laser points to construct a new set of static voxel points. Repeat steps 3.3 - 3.5 until there are no new extension points; Step 3.7: Generate the point cloud of the dynamic object in this single-frame LiDAR point cloud data by using all dynamic voxels and the extended points of each dynamic voxel.

Images