Online removal system and method for dynamic target in laser SLAM (Simultaneous Localization and Mapping) process

Abstract

The invention provides an online removal system and method for a dynamic target in a laser SLAM process, and relates to the technical field of laser three-dimensional reconstruction. The system comprises a laser radar used for acquiring three-dimensional structure information of objects and space in a surrounding environment and generating a plurality of single-frame laser point cloud initial data; the SLAM module is used for removing ground points in all the single-frame laser point cloud initial data to obtain single-frame laser point cloud data and construct a local map, and calculating the position and attitude of the laser radar when each single-frame laser point cloud initial data is generated as the collection pose of the single-frame laser point cloud data; and the dynamic removal module is used for identifying the dynamic object point cloud from the received single-frame laser point cloud data, and splicing the local map after the dynamic object point cloud is removed by using the collection pose of the single-frame laser point cloud data so as to construct a global map. According to the method, the dynamic target in the point cloud map can be accurately removed in real time, and the defect of serious wrong removal of an existing algorithm is overcome.

Description

Technical Field

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

[0002] Simultaneous localization and mapping (SLAM) technology is a key technology for 3D reconstruction, unmanned driving, and unmanned warehousing. Among SLAM sensors, LiDAR has the characteristics of high stability and high applicability, and is the mainstream sensor of SLAM technology. However, laser SLAM technology is often used in environments where dynamic and static objects coexist, such as environments consisting of buildings and moving vehicles. At this time, there will be a large number of dynamic points in the scene map generated by laser SLAM technology, forming "ghosts". This "ghost" will not only affect the positioning accuracy of laser SLAM technology, but also damage the mapping effect of laser SLAM technology. At present, 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] In the laser SLAM process, the method of performing target detection based on deep learning and then analyzing the possibility of dynamic targets in the map constructed by laser SLAM has the problems of large computational complexity, high computing hardware cost, and weak practicality for complex scenes. The Chinese patent application "Dynamic Environment Laser SLAM Method Based on Semantic Constraints" with document number CN113671522 discloses a method of using environmental elements for semantic definition, giving semantic definitions to objects in the environment through deep learning, and dividing the targets in the environment into dynamic elements, static elements, and pending elements to remove dynamic objects in the laser SLAM process. The method predefines the categories of environmental elements according to the map element classification and grading method, and performs semantic segmentation according to the predefined environmental element categories; establishes weight values ​​for each environmental element category and pre-judges the targets in the environment, and divides the targets in the environment into dynamic elements, static elements, and pending elements; adds the semantic identification map as a semantic constraint to the ICP to generate a semantic map. However, this method needs to establish a semantic library on the algorithm platform and use RangeNet++ for target detection. It has certain requirements on hardware storage space and computing power. It cannot be migrated and used on platforms such as drones and AGVs. When facing objects that are not included in the semantic library, it will produce wrong target detection results. After target detection, it is also necessary to judge the dynamics of the target. The program workload is large and real-time analysis cannot be completed on most platforms.

[0004] In the laser SLAM process, the dynamic removal algorithm based on geometric consistency has the problems of low accuracy, erroneous removal and incomplete removal, and is greatly affected by parameters, environment, sensor, geometric model, and algorithm operation cycle in dynamic removal. At the same time, some algorithms also have the problems of large calculation amount and high memory requirements for the device using the algorithm. The Chinese patent application "A method and system for indoor dynamic scene SLAM based on point cloud" with document number CN106056643 discloses a method for generating point cloud maps in dynamic scenes based on probability maps. 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 laser frame and probability map registration based on the geometric consistency of the space, updates each layer of historical probability map, and converts the current frame to the coordinate system where the historical probability map is located; obtains the converted laser frame, generates the corresponding probability map; combines the current frame probability map and the historical probability map, updates the historical probability map, and obtains the current historical probability map; finally, calculates the operation trajectory of the laser and the two-dimensional map of the indoor dynamic environment. However, this method has the problem of erroneous removal of static targets and non-removal of dynamic targets due to inaccurate point cloud estimation. This method establishes a multi-scale probability map, whose probability value is related to the Gaussian kernel size along the x-axis and y-axis and the distance from the grid to the edge of the dynamic target along the x-axis and y-axis. For different environments such as roads, forests, and indoors, due to the large difference in field of view and different scale heights, it is easy to cause inaccurate probability calculations, thereby producing erroneous dynamic removal results. At the same time, the number of points collected by general laser sensors per second can reach more than 100,000. This method needs to calculate the weight of each point and save the adjacent multi-frame probability map in memory, so the method has high computational and memory requirements. Summary of the invention

[0005] In view of the above-mentioned deficiencies in the prior art, the present invention provides a system and method for online removal of dynamic targets in the laser SLAM process. By voxelizing the local map generated in the laser SLAM process and extracting voxel features to match multiple local maps, the effect of real-time and accurate removal of dynamic targets in the point cloud map can be achieved, overcoming the defects of the existing algorithms such as high computational complexity, high memory requirements, serious erroneous removal and single usage scenario.

[0006] A first aspect of the present invention provides an online removal system for dynamic targets in a laser SLAM process, the system comprising: a laser radar, a SLAM module and a dynamic removal module;

[0007] The laser radar 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 them to the SLAM module;

[0008] The SLAM module is used to remove the ground points in all single-frame laser point cloud initial data, obtain single-frame laser point cloud data and construct a local map, and calculate the position and posture of the laser radar in the world coordinate system when generating each single-frame laser point cloud initial data as the acquisition posture of the single-frame laser point cloud data, and transmit all single-frame laser point cloud data, the local map and the acquisition posture of the single-frame laser 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 laser point cloud data, and to construct a global map by splicing the local map after removing the dynamic object point cloud using the collected posture of the single-frame laser point cloud data;

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

[0011] The ground removal unit is used to calculate the pitch angle of each point in each single-frame laser point cloud initial data, and divide all points in the single-frame laser point cloud initial data into ground points and non-ground points according to the calculated pitch angle, and obtain the single-frame laser point cloud data by removing all ground points and transmitting it to the laser odometer unit and the dynamic removal module;

[0012] The laser odometer unit is used to construct a local map by performing frame-to-frame matching on all single-frame laser point cloud data; at the same time, the acquisition posture of the single-frame laser point cloud data is calculated, and the acquisition posture of all local maps and single-frame laser point cloud data is transmitted 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 laser point cloud data into a plurality of voxels according to the position, sort the voxels according to the generation time, 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 laser point cloud data, generate the shape feature and distribution feature of the voxel for each voxel, and perform dynamic judgment on the voxel according to the shape feature and distribution feature of each voxel; transmit the dynamic judgment result of each voxel in all single-frame laser point cloud data to the dynamic voxel association unit; wherein the dynamic judgment result of the voxel is: static voxel, low dynamic voxel or high dynamic voxel;

[0016] The dynamic voxel association unit is used to set the vicinity of the high dynamic voxel, and for any high dynamic voxel, use the principal component analysis method to associate the low dynamic voxels in the vicinity of the high dynamic voxel, and determine whether the low dynamic voxel in the vicinity of the high dynamic voxel is a static voxel or a high dynamic voxel according to the association result; all high dynamic voxels in the single-frame laser point cloud data are regarded as dynamic voxels, and all dynamic voxels and static voxels in the single-frame laser 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 references and use the region growing algorithm to extend the point cloud of the dynamic voxels, generate the dynamic object point cloud in all single-frame laser point cloud data, and transmit it to the dynamic point removal unit;

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

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

[0020] A second aspect of the present invention provides a method for online removal of dynamic targets in a laser SLAM process, which is implemented using the system for online removal of dynamic targets in a laser SLAM process, and the method comprises the following steps:

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

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

[0023] Step 3: Use the region growing algorithm to extend the adjacent point cloud of the dynamic voxel to generate the dynamic object point cloud in the single frame laser point cloud data;

[0024] Step 4: Repeat steps 2-3 to generate point clouds of dynamic objects in all single-frame laser point cloud data, mark all dynamic object point clouds on the local map and remove them, and obtain a local map that only retains the static part;

[0025] Step 5: splice all local maps that only retain the static part according to the acquisition posture of each single frame of laser point cloud data to obtain a global map that only retains the static part;

[0026] The specific content of step 1 is: obtaining a number of single-frame laser point cloud initial data;

[0027] For any single frame of laser point cloud initial data, the pitch angle of each laser point in the single frame of laser point cloud initial data is calculated respectively;

[0028] According to the pitch angle of each laser point in the single-frame laser point cloud initial data and the laser line number characteristics, the line number of each laser point is calculated, and the laser points are classified according to the line numbers of all laser points in the single-frame laser point cloud initial data, so as to generate lines with pitch angles from low to high;

[0029] Calculate the yaw angle of each laser point in the initial data of a single-frame laser point cloud, and traverse each laser point in the line with the lowest pitch angle in turn, wherein 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, and judge 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 point according to the judgment result, and then remove the ground point in the initial data of the single-frame laser point cloud to obtain the single-frame laser point cloud data;

[0030] The method for determining 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 determined that the target point P1 changes from a ground point to a non-ground point, and the starting point P on the line with the lowest elevation angle is set to low The laser point between the target point P1 and the laser point P2 is marked as the ground point, and the end point P on the line with the highest pitch angle is marked as the ground point. high The laser points between 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;

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

[0032] The step 2 further comprises:

[0033] Step 2.1: For any single frame of laser point cloud data, perform two-dimensional rasterization on the single frame of laser point cloud data, and take 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 using a hash table as an index;

[0034] 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 use 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 laser point cloud data, the two single-frame laser point cloud data before and after the current single-frame laser point cloud data are used as reference frames, and the voxel is dynamically determined by comparing the shape characteristics and distribution characteristics between the voxel and the voxel at the same position in the reference frame in the world coordinate system, and the voxel is classified 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 high-dynamic voxels. For any high-dynamic voxel, use the principal component analysis method to make an association judgment on the low-dynamic voxels in the proximity range of the high-dynamic voxel, and determine whether the low-dynamic voxels in the proximity range of the high-dynamic voxel are static voxels or high-dynamic voxels based on the association result. All high-dynamic voxels in a single frame of laser point cloud data are regarded as dynamic voxels.

[0037] The specific content of step 2.3 is: for any voxel in the current single-frame laser point cloud data, respectively determine the reference voxel in each reference frame that has the same position as the voxel in the world coordinate system, and calculate the difference in the number of points, the difference in the average height of the points and the difference in the point dispersion between the voxel and the reference voxel, respectively obtain the difference error between the voxel and each reference voxel;

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

[0039] ,in, Representing voxels With reference voxel The difference error of Indicates The first single-frame laser point cloud data Individual voxels; Indicates The first single-frame laser point cloud data Voxel, and for The reference voxel, ; The weight representing the number of points; Representing voxels The number of points; Represents the reference voxel The number of points; Representing voxels And the sum of the number of points of the voxel adjacent to the voxel; The weight representing the average height of the points; Representing voxels The points are all high; Represents the reference voxel The points are all high; Representing voxels And the average height of the points of the voxel adjacent to the voxel; The weight representing the point dispersion; Representing voxels The point dispersion of Represents the reference voxel The point dispersion of Representing voxels and the point discreteness of the voxel's neighboring voxels;

[0040] The voxel is dynamically judged 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 with a difference error greater than the set dynamic threshold, , the dynamic judgment result of the voxel is a high dynamic voxel; if there is a reference voxel corresponding to a difference error greater than the set dynamic threshold , the dynamic judgment 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;

[0041] The specific content of step 2.4 is: for any high-dynamic voxel in the current single-frame laser point cloud data, each low-dynamic voxel in the vicinity of the high-dynamic voxel is taken as a neighboring voxel of the high-dynamic voxel, and the center of gravity of the high-dynamic voxel and the centers of gravity of all neighboring voxels of the high-dynamic voxel are calculated respectively;

[0042] The high-dynamic voxel is centralized by using the centroid of the high-dynamic voxel, and the covariance matrix of the high-dynamic voxel is calculated by using the centralized high-dynamic voxel; for any neighboring voxel of the high-dynamic voxel, the neighboring voxel is centralized by using the centroid of the neighboring voxel, and the covariance matrix of the neighboring voxel is calculated by using the centralized neighboring voxel;

[0043] For the high-dynamic voxel or any neighboring voxel of the high-dynamic voxel, the eigenvalue of the covariance matrix is ​​solved according to the matrix property of the covariance matrix of the voxel , and using the eigenvalues ​​of the covariance matrix Solve and The corresponding non-zero eigenvectors will be As the main direction of the voxel in the distribution, As the secondary main direction of the voxel in the distribution, and As the primary and secondary distribution vector of the point cloud of this voxel;

[0044] Then, according to the point cloud major and minor distribution vectors of the high dynamic voxel and the point cloud major and minor distribution vectors of all neighboring voxels of the high dynamic voxel, respectively calculate the geometric consistency between the high dynamic voxel and each neighboring voxel of the high dynamic voxel;

[0045] For any neighboring voxel of the high dynamic voxel, if the geometric consistency between the high dynamic voxel and the neighboring voxel exceeds the set geometric consistency threshold , then the adjacent voxel is marked 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] ,in, Representing voxels and voxels The geometric consistency between represents the yaw angle weight; represents the pitch angle weight; represents the voxel The center in the world coordinate system; Representing voxels The center in the world coordinate system;

[0048] The step 3 further comprises:

[0049] Step 3.1: For any dynamic voxel in the current single-frame laser point cloud data, determine the neighboring voxels of the dynamic voxel, and establish a KDTree to store the laser points in the dynamic voxel and the laser points in the neighboring voxels of the dynamic voxel;

[0050] Step 3.2: Use KDTree to search for all laser points in each neighboring voxel whose distance from the center point of the dynamic voxel is within the set threshold, and filter out all laser points belonging to static voxels from the found laser points to construct a static voxel point collection. ;

[0051] Step 3.3: Use KDTree to find quickly The three nearest static voxel points in the neighborhood of each laser point in , and the normal vector of the three-point fitting plane is calculated , and calculate the normal vectors of the fitted plane of the three nearest dynamic voxel points in the neighborhood of the laser point ;

[0052] Step 3.4: Calculate the normal vector and the normal vector Angle ;

[0053] Step 3.5: When A laser point When the laser point is considered to be on the same plane as the laser point in the dynamic voxel, the laser point is used as the extension point of the dynamic voxel; then the extension point set is constructed using all the extension points of the dynamic voxel. ; Mark at the same time All unsatisfied Laser points and construct a static point set ;

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

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

[0056] The beneficial effects of adopting the above technical solution are:

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

[0058] Taking into account the characteristics of large data volume of laser sensors and a certain proportion of ground points, the method of the present invention uses a ground extraction method based on laser viewing angle and a uniform downsampling method based on a grid map to reduce the number of low-significance point clouds and improve the system speed and stability.

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

[0060] Since the volume, shape, movement speed and relative position of dynamic targets in different environments are quite different from those of laser sensors, dynamic removal in the SLAM process is prone to mis-extraction. Therefore, after voxelizing the single-frame laser point cloud data, the method of the present invention extracts three types of voxel features for inter-frame matching to extract dynamic targets, thereby improving accuracy.

[0061] Due to the complexity of the shape of dynamic targets, dynamic removal based solely on voxels, clustering, and probability can easily produce unclear dynamic and static boundaries. Therefore, the method of the present invention uses a dynamic voxel extension method to extend the boundaries of voxels confirmed to be dynamic, and can also accurately extract dynamic objects with complex shapes.

[0062] The experimental results show that in a variety of scenarios, the method of the present invention can effectively remove dynamic objects, while overcoming the defects of high computational complexity, high memory requirements, and serious erroneous removal in existing algorithms. For SLAM algorithms using commonly used sensors such as Velodyne 16 / 32 / 64-line lidar, Hesai lidar, and Leishen lidar, the effect of real-time dynamic removal can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0064] Figure 2 Flow chart of a method for online removal of dynamic targets in a laser SLAM process in this embodiment;

[0065] Figure 3 Schematic diagram of ground points and non-ground points scanned by the laser radar in this embodiment;

[0066] Figure 4 The overall point cloud map of a street including the dynamic target point in this implementation mode;

[0067] Figure 5 This is a schematic diagram of the point cloud ground extraction effect of the overall point cloud map of a street in this implementation mode;

[0068] Figure 6 This is a schematic diagram of the point cloud ground removal effect of the overall point cloud map of a street in this implementation mode;

[0069] Figure 7This is a point cloud map of a portion of a street before point cloud voxelization is performed on the point cloud map in this implementation manner;

[0070] Figure 8 The point cloud map is a point cloud map obtained by voxelizing a point cloud map of a portion of a street in this implementation manner;

[0071] Fig. 9 It is a point cloud map after dynamic voxel discrimination is performed on a point cloud map of a part of a street in this implementation mode;

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

[0073] Fig.11 1 is a comparison diagram of the point cloud map of Street 1 before and after dynamic target removal in this embodiment; (a) is a schematic diagram of the point cloud map of Street 1 before dynamic target removal; (b) is a schematic diagram of the point cloud map of Street 1 after dynamic target removal;

[0074] Fig.12 1 is a comparison diagram of the point cloud map of Street 2 before and after dynamic target removal in this embodiment; (a) is a schematic diagram of the point cloud map of Street 2 before dynamic target removal; (b) is a schematic diagram of the point cloud map of Street 2 after dynamic target removal. DETAILED DESCRIPTION

[0075] The specific implementation of the present invention is further described in detail below in conjunction with the accompanying drawings and implementation examples. The following implementation examples are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0076] In this embodiment, a system for removing dynamic targets online in a laser SLAM process is provided. Figure 1 As shown, the system includes: a laser radar, a SLAM module and a dynamic removal module.

[0077] The laser radar 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 them to the SLAM module.

[0078] In this embodiment, three-dimensional structural information of all objects and spaces within the sensing range of the laser radar is obtained, such as the outlines, edges and surfaces of objects such as buildings, roads, trees, etc. in the environment around the laser radar, as well as the terrain and spatial layout of the surrounding environment.

[0079] The SLAM module is used to remove ground points from all single-frame laser point cloud initial data, obtain single-frame laser point cloud data and construct a local map, and simultaneously calculate the position and posture of the laser radar in the world coordinate system when generating each single-frame laser point cloud initial data as the acquisition posture of the single-frame laser point cloud data, and transmit all single-frame laser point cloud data, local maps and the acquisition posture of the single-frame laser point cloud data to the dynamic removal module.

[0080] In this embodiment, a single frame of laser point cloud data is used to generate a local map, and the acquisition posture of the single frame of laser point cloud data is used to subsequently stitch the local map into an overall map.

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

[0082] The ground removal unit is used to calculate the pitch angle of each point in each single-frame laser point cloud initial data respectively, and divide all points in the single-frame laser point cloud initial data into ground points and non-ground points according to the calculated pitch angle, and obtain the single-frame laser point cloud data by removing all ground points and transmitting it to the laser odometer unit and the dynamic removal module.

[0083] The laser odometer unit is used to construct a local map by performing frame-to-frame matching on all single-frame laser point cloud data; at the same time, the acquisition posture of the single-frame laser point cloud data is calculated, and the acquisition posture of all local maps and single-frame laser point cloud data is transmitted to the dynamic removal module.

[0084] The dynamic removal module is used to identify dynamic object point clouds from the received single-frame laser point cloud data, and use the acquisition posture of the single-frame laser point cloud data to splice the local map after removing the dynamic object point cloud 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 laser point cloud data into a plurality of voxels according to the position, and 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 laser point cloud data, generate the shape characteristics and distribution characteristics of the voxel for each voxel, and dynamically determine the voxel according to the shape characteristics and distribution characteristics of each voxel; and transmit the dynamic determination results of each voxel in all single-frame laser point cloud data to the dynamic voxel association unit.

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

[0089] The dynamic voxel association unit is used to set the proximity range of the high dynamic voxel. For any high dynamic voxel, the principal component analysis method is used to associate the low dynamic voxels in the proximity range of the high dynamic voxel, and the low dynamic voxels in the proximity range of the high dynamic voxel are determined to be static voxels or high dynamic voxels according to the association result; all high dynamic voxels in a single frame of laser point cloud data are regarded as dynamic voxels, and all dynamic voxels and static voxels in all single frame of laser 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 references and use the region growing algorithm to extend the point cloud of the dynamic voxels, generate the dynamic object point cloud in all single-frame laser point cloud data and transmit it to the dynamic point removal unit.

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

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

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

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

[0095] The online removal method of a dynamic target in a laser SLAM process of this embodiment is implemented by using the online removal system of a dynamic target in a laser SLAM process, such as Figure 2 As shown, the method comprises the following steps:

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

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

[0098] For any single frame of laser point cloud initial data, the pitch angle of each laser point in the single frame of laser point cloud initial data is calculated respectively.

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

[0100] (1);

[0101] in, Indicates the elevation angle of the laser point; Represents the optical center coordinates of the laser radar in the world coordinate system; Represents the abscissa of the optical center of the laser radar in the world coordinate system; Represents the optical center ordinate of the laser radar in the world coordinate system; Represents the vertical coordinate of the optical center of the laser radar in the world coordinate system; Represents the coordinates of the laser point in the world coordinate system; Represents the horizontal coordinate of the laser point in the world coordinate system; Indicates the ordinate of the laser point in the world coordinate system; Indicates the vertical coordinate of the laser point in the world coordinate system.

[0102] In this embodiment, the pitch angle calculation formula is used to calculate the pitch angle of each laser point when collecting the initial data of a single frame of laser point cloud. If the pitch angle is positive, the laser emission angle of this point is upward relative to the horizontal. If the pitch angle is negative, the laser emission angle of this point is downward relative to the horizontal.

[0103] According to the pitch angle of each laser point in the single-frame laser point cloud initial data and the line number characteristics of the laser, the line number of each laser point is calculated, and the laser points are classified according to the line numbers of all laser points in the single-frame laser point cloud initial data, thereby generating lines with pitch angles from low to high.

[0104] In this embodiment, the line numbers corresponding to all laser points are calculated according to the pitch angle and the line number characteristics of the laser itself. scanID , classify all laser points in a single frame according to the line sequence to form a certain number of lines with pitch angles from low to high. Since the number of classification lines is consistent with the number of laser lines, different lasers have different numbers of classification lines. Common laser radars can be divided into 16-line lasers, 32-line lasers, 64-line lasers and solid-state lasers. The number of laser lines of a 64-line laser radar is 64, and the number of laser lines of a 32-line laser radar is 32. For example, XT32 is a laser radar produced by Hesai, which will form 32 classification lines here.

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

[0106] (2);

[0107] in, scanID Indicates the line number of the laser point; represents the ceiling function; It indicates the initial pitch angle of the laser and is determined by the characteristics of the laser itself; It indicates the line spacing angle of the laser and is determined by the characteristics of the laser itself.

[0108] In this embodiment, since the number of laser lines, initial pitch angle and laser interval angle are all determined by laser characteristics, the number of laser lines, initial pitch angle and laser interval angle of different lasers are not necessarily exactly the same. For example, for lasers with 32 lines, Oster and Hesai have different initial pitch angles and laser interval angles, 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, and 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. According to 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, mark the ground point according to the determination result, and then remove the ground point in 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:

[0111] (3);

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

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

[0114] (4);

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

[0116] In this embodiment, if Figure 3 As shown, the points with the same yaw angle are divided into a certain number of lines from low to high pitch angles, and the point P with the lowest pitch angle is low Start traversing each laser point until the point P with the highest pitch angle high , the traversed laser point P1 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, and determines whether the laser point changes from a ground point to a non-ground point based on the degree of the elevation angle.

[0117] The method for determining 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 determined that the target point P1 changes from a ground point to a non-ground point, and the starting point P on the line with the lowest elevation angle is set to low The laser point between the target point P1 and the laser point P2 is marked as the ground point, and the end point P on the line with the highest pitch angle is marked as the ground point. high The laser points between 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.

[0118] All single-frame laser point cloud data are matched frame by frame, the acquisition pose of the single-frame laser point cloud data is calculated and a local map is constructed.

[0119] In this embodiment, the single-frame laser point cloud data after the ground points are removed is passed through the laser odometer 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 posture of the single-frame laser point cloud data. In the laser odometer unit, the acquisition position and attitude information of the single-frame laser point cloud data is calculated by iteratively solving the point cloud data of multiple consecutive frames without ground points. The input of the iterative closest point is the point cloud data of the previous and next two frames without ground points, and the output result is the position transformation T and attitude transformation R between the two frames. The Iterative Closest Point (ICP) algorithm is a classic point cloud registration method used to calculate the optimal rigid body transformation between two point clouds, including rotation and translation. When processing the point cloud data of the previous and next two frames without ground points, the ICP algorithm can effectively find the position transformation T and attitude transformation R between them.

[0120] The method for performing frame-to-frame matching on all single-frame laser point cloud data is as follows: for any single-frame laser point cloud data, an initial rigid body transformation is selected for the single-frame laser point cloud data, the single-frame laser point cloud data of the previous and next two frames are used as reference frames, the nearest point pair is found in the reference frame and the optimal rigid body transformation is calculated, so that the sum of the distances from the points in the single-frame laser point cloud data to the corresponding points in the reference frame is minimized, the single-frame laser point cloud data is updated using the optimal rigid body transformation, the position transformation T and the posture transformation R of the single-frame laser point cloud data are obtained, and then the acquisition posture of the single-frame laser point cloud data is calculated.

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

[0122] In this embodiment, the point cloud map of the overall map of a street is as follows: Figure 4 As shown in the figure, the color depth of the point reflects the height information of the point. The lower the height, the darker it is, and the higher the height, the lighter it is. The point cloud map is composed of multiple frames of point clouds merged through laser SLAM calculation. The three white circles in the point cloud map are continuous point clouds formed by dynamic pedestrians, dynamic motor vehicles, and dynamic electric vehicles from left to right. 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 point cloud map is judged whether the laser point is a dynamic point, such as Figure 5 As shown in the figure, the light-colored part is the extracted ground point cloud, and the dark-colored part is the non-ground area. The dynamic points in the point cloud map are removed according to the discrimination results, that is, Figure 4 The point cloud after removing the ground points on the street is as follows Figure 6 shown.

[0123] Step 2: For any single frame of laser point cloud data, generate a number of voxels by two-dimensionally rasterizing the single frame of laser point cloud data, and perform dynamic judgment on each voxel, and then perform dynamic voxel association based on the dynamic judgment result, so as to divide each voxel into dynamic voxels and static voxels.

[0124] The step 2 further comprises:

[0125] Step 2.1: For any single frame of laser point cloud data, perform two-dimensional rasterization on the single frame of laser point cloud data, and take the point set formed by the laser points in each grid as a voxel. Determine the voxel number according to the order in which the voxels are generated, and store each voxel using a hash table as an index.

[0126] In this embodiment, a single frame of laser point cloud data is two-dimensionally rasterized, specifically: according to the horizontal and vertical coordinate axes of the world coordinate system, the point cloud is divided into 0.1m long and wide squares, wherein the rasterized coordinate systems of different frames are consistent, which are all world coordinate systems, thereby establishing a 0.01 square meter grid, and the collection of all laser points in each grid is called a voxel, and the voxels are sorted by generation time, and the voxel sequence number is determined. For each voxel, a hash table is established for storage for fast reading; wherein the hash table is a three-dimensional index hash table, and the input of the hash table is is the center coordinate of the voxel, and the output is the voxel number , establish a data structure with a query time complexity of 1, which is a multi-element array, in which the single element structure is .

[0127] In this embodiment, a portion of a street in the overall point cloud map of a street after removing the ground points is intercepted, such as Figure 7 As shown in the figure, the black circle area is the dynamic point cloud area formed by the dynamic vehicle. The point cloud map is two-dimensionally rasterized, that is, point cloud voxelized, as shown in the figure. Figure 8 As shown, the points in 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 use the number of points and point dispersion of each voxel as the distribution feature of the voxel.

[0129] The specific content of step 2.2 is: for any voxel, count the number of points in the voxel ; Get the height of all points in the voxel and calculate the average height of all points in the voxel And take it as the average height of the points in the voxel; obtain the coordinates of all points in the voxel in the world coordinate system and the geometric center of the voxel, and calculate the point discreteness of the voxel.

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

[0131] (5);

[0132] in, Represents the point discreteness of voxels; The number of points representing voxels; represents the point index in the voxel; Represents the number of voxels in the world coordinate system The coordinates of the points; Represents the geometric center of the voxel.

[0133] Step 2.3: For any voxel in the current single-frame laser point cloud data, the two single-frame laser point cloud data before and after the current single-frame laser point cloud data are used as reference frames, and the voxel is dynamically determined by comparing the shape characteristics and distribution characteristics between the voxel and the voxel in the same position in the world coordinate system in the reference frame, and the voxel is divided into a static voxel, a low-dynamic voxel or a high-dynamic voxel according to the dynamic determination result.

[0134] The specific content of step 2.3 is: for any voxel in the current single-frame laser point cloud data, determine the reference voxel in each reference frame that has the same position as the voxel in the world coordinate system, and calculate the difference in the number of points, the difference in the average height of the points, and the difference in the point dispersion between the voxel and the reference voxel to obtain the difference error between the voxel and each reference voxel.

[0135] In this embodiment, the number of points, average height of points and point dispersion of voxels are compared respectively to obtain the difference error. Express the difference of voxel features between different frames.

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

[0137] (6);

[0138] in, Representing voxels With reference voxel The difference error of Indicates The first single-frame laser point cloud data Individual voxels; Indicates The first single-frame laser point cloud data voxel; The geometric center of and The geometric center of The same, and for The reference voxel, ; The weight representing the number of points; Representing voxels The number of points; Represents the reference voxel The number of points; Representing voxels and the sum of the number of points of the neighboring voxels of the voxel. In this embodiment, the neighboring voxels are set to be 8 voxels around the voxel. The weight representing the average height of the points; Representing voxels The points are all high; Represents the reference voxel The points are all high; Representing voxels And the average height of the points of the voxel adjacent to the voxel; The weight representing the point dispersion; Representing voxels The point dispersion of Represents the reference voxel The point dispersion of Representing voxels And the point discreteness sum of the voxel's neighboring voxels.

[0139] The voxel is dynamically judged 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 with a difference error greater than the set dynamic threshold, , the dynamic judgment result of the voxel is a high dynamic voxel; if there is a reference voxel corresponding to a difference error greater than the set dynamic threshold , the dynamic judgment 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 judgment result of the voxel is a static voxel.

[0140] In this embodiment, the dynamic threshold is set Obtained through a large number of experimental parameter adjustments; since each grid is independent of each other, parallel processing can be used to speed up the calculation and comparison of grid shape and distribution characteristics.

[0141] Step 2.4: Set the proximity range of high-dynamic voxels. For any high-dynamic voxel, use the principal component analysis method to make an association judgment on the low-dynamic voxels in the proximity range of the high-dynamic voxel, and determine whether the low-dynamic voxels in the proximity range of the high-dynamic voxel are static voxels or high-dynamic voxels based on the association results. All high-dynamic voxels in a single frame of laser point cloud data are regarded as dynamic voxels.

[0142] In this embodiment, the low-dynamic voxels adjacent to the high-dynamic voxels are associated with each other through principal component analysis, with the purpose of performing secondary detection on the low-dynamic voxels, traversing and dynamically removing each high-dynamic voxel in the current frame, and judging whether the 8 voxels around these high-dynamic voxels are low-dynamic voxels; if there are low-dynamic voxels in the 8 voxels around any high-dynamic voxel, the principal component analysis is used to calculate the primary and secondary distribution vectors of the point clouds in the central high-dynamic voxel and the adjacent low-dynamic voxel, and judge whether the two voxels have geometric consistency based on the primary and secondary distribution vectors of the point clouds of the above two voxels. If there is geometric consistency, the two voxels are associated, and the dynamic information of the low-dynamic voxel is modified to be a high-dynamic voxel.

[0143] The specific content of step 2.4 is: for any high-dynamic voxel in the current single-frame laser point cloud data, each low-dynamic voxel in the vicinity of the high-dynamic voxel is regarded as a neighboring voxel of the high-dynamic voxel, and the center of gravity of the high-dynamic voxel and the center of gravity of all neighboring voxels of the high-dynamic voxel are calculated respectively.

[0144] The method of respectively calculating the center of gravity of the high dynamic voxel and the center of gravity of all neighboring voxels of the high dynamic voxel is:

[0145] (7);

[0146] in, Represents the center of gravity of the voxel.

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

[0148] The covariance matrix is ​​expressed as:

[0149] (8);

[0150] in, represents the covariance matrix; Indicates the voxel in the world coordinate system Covariance in direction; Indicates the voxel in the world coordinate system and Covariance in direction; Indicates the voxel in the world coordinate system and Covariance in direction; Indicates the voxel in the world coordinate system and Covariance in direction; Indicates the voxel in the world coordinate system Covariance in direction; Indicates the voxel in the world coordinate system and Covariance in direction; Indicates the voxel in the world coordinate system and Covariance in direction; Indicates the voxel in the world coordinate system and Covariance in direction; Indicates the voxel in the world coordinate system Covariance in the direction. And:

[0151] (9);

[0152] in, Indicates the voxel in the world coordinate system and Covariance in direction; and Indicates the direction of the world coordinate system, which can be , , One of and It can be the same or different; Represents the voxel Points in The value in the direction; Represents the voxel Points in The value in the direction; Represents all points in the voxel The average value in the direction; Represents all points in the voxel The average value in the direction.

[0153] For the high-dynamic voxel or any neighboring voxel of the high-dynamic voxel, the eigenvalue of the covariance matrix is ​​solved according to the matrix property of the covariance matrix of the voxel , and using the eigenvalues ​​of the covariance matrix Solve and The corresponding non-zero eigenvectors , will be used as the main direction of the voxel in the distribution, As the secondary main direction of the voxel in the distribution, and As the primary and secondary distribution vector of the point cloud of this voxel.

[0154] In this embodiment, the eigenvalues ​​and eigenvectors of the covariance matrix C are solved. Since the covariance matrix C is a three-dimensional matrix, the eigenvalues ​​of the covariance are calculated. and the corresponding eigenvector ,in ; The main direction of the point cloud distribution is , the secondary main direction is , the eigenvector expresses the spatial distribution of the midpoints of the voxels. The specific method is to firstly calculate the spatial distribution of the midpoints of the voxels according to the matrix properties. Find ,in EAs the unit matrix, Substitution Finding non-zero solutions .

[0155] In this embodiment, since the laser radar can only scan one side of an object at the same time, for multiple adjacent voxels containing dynamic objects, the distribution of the dynamic objects in the point cloud space formed by the laser radar scanning can only be one side. The geometric consistency can be determined based on the primary and secondary distribution vectors of the point clouds of the two voxels.

[0156] Then, according to the point cloud major and minor distribution vectors of the high dynamic voxel and the point cloud major and minor distribution vectors of all neighboring voxels of the high dynamic voxel, the geometric consistency between the high dynamic voxel and each neighboring voxel of the high dynamic voxel is calculated respectively.

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

[0158] (10);

[0159] in, Representing voxels and voxels The geometric consistency between represents the yaw angle weight; represents the pitch angle weight; Representing voxels The center in the world coordinate system; Representing voxels The center of the world in coordinates.

[0160] In this embodiment, the trigonometric function in formula (10) calculates 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 primary and secondary distribution vectors of the point cloud data on the geometric consistency.

[0161] For any neighboring voxel of the high dynamic voxel, if the geometric consistency between the high dynamic voxel and the neighboring voxel exceeds the set geometric consistency threshold , then the adjacent voxel is marked 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 the neighboring voxel is marked as a static voxel.

[0162] In this embodiment, the geometric consistency threshold is set It is obtained by adjusting parameters through multiple experiments. Fig. 9 As shown, dynamic voxel discrimination is performed on a portion of the point cloud map of a street after the point cloud is voxelized, where the vehicle shadow in the black circle is the grid voxel formed by the dynamic target.

[0163] Step 3: Use the region growing algorithm to extend the adjacent point cloud of the dynamic voxels to generate the dynamic object point cloud in the single frame laser point cloud data.

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

[0165] The step 3 further comprises:

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

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

[0168] Step 3.2: Use KDTree to search for all laser points in each neighboring voxel whose distance from the center point of the dynamic voxel is within the set threshold, and filter out all laser points belonging to static voxels from the found laser points to construct a static voxel point collection. .

[0169] In this embodiment, a KDTree is established for a dynamic voxel and its neighboring voxels, and a collection of static voxel points within 3 cm of the neighboring dynamic voxel is obtained using KDTree. .

[0170] Step 3.3: Use KDTree to find quickly The three nearest static voxel points in the neighborhood of each laser point in , and the normal vector of the three-point fitting plane is calculated , and calculate the normal vectors of the fitted plane of the three nearest dynamic voxel points in the neighborhood of the laser point .

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

[0172] (11);

[0173] The three points in the neighborhood used to calculate the normal vector are , , , is the normal vector corresponding to the three-point fitting plane.

[0174] Step 3.4: Calculate the normal vector and the normal vector Angle .

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

[0176] (12);

[0177] in, Representation vector and the normal vector Angle.

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

[0179] Step 3.5: When A laser point When the laser point is considered to be on the same plane as the laser point in the dynamic voxel, the laser point is used as the extension point of the dynamic voxel; then the extension point set is constructed using all the extension points of the dynamic voxel. ; Mark at the same time All unsatisfied Laser points and construct a static point set .

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

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

[0182] In this embodiment, if Fig.10 As shown, the dark part is the point set in the original dynamic voxel, and the light part is the point set in the original static voxel. Since the normal vector angle 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 area can be marked as a dynamic voxel by extension.

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

[0184] In this embodiment, the classification results of the voxels and the results of the extension points are recorded in the current local map, and the dynamic voxels, associated voxels of the dynamic voxels, and dynamic points extended by the dynamic voxels of each local map are deleted, leaving only the static part. When the next local map is generated, steps 2-3 are repeated to obtain the corresponding local map that only retains the static part.

[0185] Step 5: According to the acquisition posture of each single frame of laser point cloud data, all local maps that only retain the static part are spliced ​​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 merge the point clouds, and completing the overall map means completing the three-dimensional reconstruction.

[0187] In this embodiment, if Fig.11 As shown in the figure, the point cloud map of a street is subjected to dynamic target removal. 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 the dynamic point clouds including pedestrians, vehicles, and electric vehicles are removed. Fig.12 As shown, the point cloud map of another street is subjected to dynamic target removal. Figure (a) is the overall point cloud map before dynamic removal, including the vehicle dynamic point cloud, and Figure (b) is the overall point cloud map after the vehicle dynamic point cloud is removed.

[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 aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some or all of the technical features therein. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope defined by the claims of the present invention.

Claims

1. An online removal system for dynamic targets in the laser SLAM process, characterized in that: The system includes: laser radar, SLAM module and dynamic removal module; The laser radar 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 them to the SLAM module; The SLAM module is used to remove the ground points in all single-frame laser point cloud initial data, obtain single-frame laser point cloud data and construct a local map, and calculate the position and posture of the laser radar in the world coordinate system when generating each single-frame laser point cloud initial data as the acquisition posture of the single-frame laser point cloud data, and transmit all single-frame laser point cloud data, the local map and the acquisition posture of the single-frame laser point cloud data to the dynamic removal module; The SLAM module includes: a ground removal unit and a laser odometer unit; The ground removal unit is used to calculate the pitch angle of each point in each single-frame laser point cloud initial data, and divide all points in the single-frame laser point cloud initial data into ground points and non-ground points according to the calculated pitch angle, and obtain the single-frame laser point cloud data by removing all ground points and transmitting it to the laser odometer unit and the dynamic removal module; The laser odometer unit is used to construct a local map by performing frame-to-frame matching on all single-frame laser point cloud data; at the same time, the acquisition posture of the single-frame laser point cloud data is calculated, and the acquisition posture of all local maps and single-frame laser point cloud data is transmitted to the dynamic removal module; The dynamic removal module is used to identify dynamic object point clouds from the received single-frame laser point cloud data, and use the acquisition posture of the single-frame laser point cloud data to splice the local map after removing the dynamic object point cloud to construct a global map.

2. According to the online removal system of a dynamic target in the laser SLAM process of claim 1, it is characterized in that: 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 laser point cloud data into a plurality of voxels according to the position, sort the voxels according to the generation time, 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 laser point cloud data, generate the shape feature and distribution feature of the voxel for each voxel, and perform dynamic judgment on the voxel according to the shape feature and distribution feature of each voxel; transmit the dynamic judgment result of each voxel in all single-frame laser point cloud data to the dynamic voxel association unit; wherein the dynamic judgment result of the voxel is: static voxel, low dynamic voxel or high dynamic voxel; The dynamic voxel association unit is used to set the vicinity of the high dynamic voxel, and for any high dynamic voxel, use the principal component analysis method to associate the low dynamic voxels in the vicinity of the high dynamic voxel, and determine whether the low dynamic voxel in the vicinity of the high dynamic voxel is a static voxel or a high dynamic voxel according to the association result; all high dynamic voxels in the single-frame laser point cloud data are regarded as dynamic voxels, and all dynamic voxels and static voxels in the single-frame laser point cloud data are transmitted to the dynamic point extension unit; The dynamic point extension unit is used to use the static voxels as references and use the region growing algorithm to extend the point cloud of the dynamic voxels, generate the dynamic object point cloud in all single-frame laser point cloud data, and transmit it to the dynamic point removal unit; The dynamic point removal unit is used to mark the dynamic object point clouds in all single-frame laser point cloud data in the local map and remove them, and then splice the local map after removing the dynamic point clouds in the world coordinate system according to the acquisition posture of the single-frame laser point cloud data to obtain a global map that only retains the static part; The local map data storage space is used to store all voxels stored in each single frame of laser point cloud data using a hash table as an index, as well as dynamic determination results and geometric information of each voxel; the geometric information includes the average height, centroid and covariance of the voxels.

3. A method for removing dynamic targets online in a laser SLAM process, implemented by using a system for removing dynamic targets online in a laser SLAM process as claimed in any one of claims 1 to 2, characterized in that: The method comprises the following steps: Step 1: Obtain several single-frame laser point cloud initial data, and generate several single-frame laser point cloud data by removing the ground points in each single-frame laser point cloud initial data, calculate the acquisition pose of each single-frame laser point cloud data by frame-to-frame matching method, and construct a local map; Step 2: For any single frame of laser point cloud data, generate a number of voxels by two-dimensionally rasterizing the single frame of laser point cloud data, and perform dynamic judgment on each voxel, and then perform dynamic voxel association according to the dynamic judgment result, so as to divide each voxel into dynamic voxel and static voxel; Step 3: Use the region growing algorithm to extend the adjacent point cloud of the dynamic voxel to generate the dynamic object point cloud in the single frame laser point cloud data; Step 4: Repeat steps 2-3 to generate point clouds of dynamic objects in all single-frame laser point cloud data, mark all dynamic object point clouds on the local map and remove them, and obtain a local map that only retains the static part; Step 5: According to the acquisition posture of each single frame of laser point cloud data, all local maps that only retain the static part are spliced ​​to obtain a global map that only retains the static part.

4. The method for online removal of dynamic targets in a laser SLAM process according to claim 3, characterized in that: The specific content of step 1 is: obtaining a number of single-frame laser point cloud initial data; For any single frame of laser point cloud initial data, the pitch angle of each laser point in the single frame of laser point cloud initial data is calculated respectively; According to the pitch angle of each laser point in the single-frame laser point cloud initial data and the laser line number characteristics, the line number of each laser point is calculated, and the laser points are classified according to the line numbers of all laser points in the single-frame laser point cloud initial data, so as to generate lines with pitch angles from low to high; Calculate the yaw angle of each laser point in the initial data of a single-frame laser point cloud, and traverse each laser point in the line with the lowest pitch angle in turn, wherein 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, and judge 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 point according to the judgment result, and then remove the ground point in the initial data of the single-frame laser point cloud to obtain the single-frame laser point cloud data; The method for determining 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 determined that the target point P1 changes from a ground point to a non-ground point, and the starting point P on the line with the lowest elevation angle is set to low The laser point between the target point P1 and the laser point P2 is marked as the ground point, and the end point P on the line with the highest pitch angle is marked as the ground point. high The laser points between 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; All single-frame laser point cloud data are matched frame by frame, the acquisition pose of the single-frame laser point cloud data is calculated and a local map is constructed.

5. The method for online removal of dynamic targets in a laser SLAM process according to claim 4, characterized in that: The step 2 further comprises: Step 2.1: For any single frame of laser point cloud data, perform two-dimensional rasterization on the single frame of laser point cloud data, and take 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 using a hash table as an index; 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 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, the two single-frame laser point cloud data before and after the current single-frame laser point cloud data are used as reference frames, and the voxel is dynamically determined by comparing the shape characteristics and distribution characteristics between the voxel and the voxel at the same position in the reference frame in the world coordinate system, and the voxel is classified 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 high-dynamic voxels. For any high-dynamic voxel, use the principal component analysis method to make an association judgment on the low-dynamic voxels in the proximity range of the high-dynamic voxel, and determine whether the low-dynamic voxels in the proximity range of the high-dynamic voxel are static voxels or high-dynamic voxels based on the association results. All high-dynamic voxels in a single frame of laser point cloud data are regarded as dynamic voxels.

6. The method for online removal of dynamic targets in a laser SLAM process according to claim 5, characterized in that: The specific content of step 2.3 is: for any voxel in the current single-frame laser point cloud data, respectively determine the reference voxel in each reference frame that has the same position as the voxel in the world coordinate system, and calculate the difference in the number of points, the difference in the average height of the points and the difference in the point dispersion between the voxel and the reference voxel, respectively obtain the difference error between the voxel and each reference voxel; The calculation method of the difference error is: ,in, Representing voxels With reference voxel The difference error of Indicates The first single-frame laser point cloud data Individual voxels; Indicates The first single-frame laser point cloud data Voxel, and for The reference voxel, ; The weight representing the number of points; Representing voxels The number of points; Represents the reference voxel The number of points; Representing voxels And the sum of the number of points of the voxel adjacent to the voxel; The weight representing the average height of the points; Representing voxels The points are all high; Represents the reference voxel The points are all high; Representing voxels And the average height of the points of the voxel adjacent to the voxel; The weight representing the point dispersion; Representing voxels The point dispersion of Represents the reference voxel The point dispersion of Representing voxels and the point discreteness sum of the voxel's neighboring voxels; The voxel is dynamically judged 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 with a difference error greater than the set dynamic threshold, , the dynamic judgment result of the voxel is a high dynamic voxel; if there is a reference voxel corresponding to a difference error greater than the set dynamic threshold , the dynamic judgment 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 judgment result of the voxel is a static voxel.

7. The method for online removal of dynamic targets in the laser SLAM process according to claim 6, characterized in that: The specific content of step 2.4 is: for any high-dynamic voxel in the current single-frame laser point cloud data, each low-dynamic voxel in the vicinity of the high-dynamic voxel is taken as a neighboring voxel of the high-dynamic voxel, and the center of gravity of the high-dynamic voxel and the centers of gravity of all neighboring voxels of the high-dynamic voxel are calculated respectively; Centralizing the high-dynamic voxel using the center of gravity of the high-dynamic voxel, and calculating the covariance matrix of the high-dynamic voxel using the centralized high-dynamic voxel; For any neighboring voxel of the high dynamic voxel, the neighboring voxel is centralized using the centroid of the neighboring voxel, and the covariance matrix of the neighboring voxel is calculated using the centralized neighboring voxel; For the high-dynamic voxel or any neighboring voxel of the high-dynamic voxel, the eigenvalue of the covariance matrix is ​​solved according to the matrix property of the covariance matrix of the voxel , and using the eigenvalues ​​of the covariance matrix Solve and The corresponding non-zero eigenvectors ,Will As the main direction of the voxel in the distribution, As the secondary main direction of the voxel in the distribution, and As the primary and secondary distribution vector of the point cloud of this voxel; Then, according to the point cloud major and minor distribution vectors of the high dynamic voxel and the point cloud major and minor distribution vectors of all neighboring voxels of the high dynamic voxel, respectively calculate the geometric consistency between the high dynamic voxel and each neighboring voxel of the high dynamic voxel; For any neighboring voxel of the high dynamic voxel, if the geometric consistency between the high dynamic voxel and the neighboring voxel exceeds the set geometric consistency threshold , then the adjacent voxel is marked 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 the neighboring voxel is marked as a static voxel.

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

9. The method for online removal of dynamic targets in the laser SLAM process according to claim 8, characterized in that: The step 3 further comprises: The step 3 further comprises: Step 3.1: For any dynamic voxel in the current single-frame laser point cloud data, determine the neighboring voxels of the dynamic voxel, and establish a KDTree to store the laser points in the dynamic voxel and the laser points in the neighboring voxels of the dynamic voxel; Step 3.2: Use KDTree to search for all laser points in each neighboring voxel whose distance from the center point of the dynamic voxel is within the set threshold, and filter out all laser points belonging to static voxels from the found laser points to construct a static voxel point collection. ; Step 3.3: Use KDTree to find quickly The three nearest static voxel points in the neighborhood of each laser point in , and the normal vector of the three-point fitting plane is calculated , and calculate the normal vectors of the fitted plane of the three nearest dynamic voxel points in the neighborhood of the laser point ; Step 3.4: Calculate the normal vector and the normal vector Angle ; Step 3.5: When A laser point When the laser point is considered to be on the same plane as the laser point in the dynamic voxel, the laser point is used as the extension point of the dynamic voxel; then the extension point set is constructed using all the extension points of the dynamic voxel. ; Mark at the same time All unsatisfied Laser points and construct a static point set ; Step 3.6: For the extended point set For any extension point in the voxel, use KDTree to search for all laser points in each adjacent voxel whose distance from the center point of the voxel where the extension point is located is within the set threshold in the adjacent voxels of the voxel where the extension point is located, and filter out all laser points belonging to static voxels from the found laser points to construct a new static voxel point collection, and repeat steps 3.3-3.5 until there are no new extension points; Step 3.7: Generate a point cloud of the dynamic object in the single frame of laser point cloud data using all dynamic voxels and the extension points of each dynamic voxel.

Images