Laser point cloud dynamic obstacle elimination method in low-speed motion scene

Abstract

The invention provides a laser point cloud dynamic obstacle elimination method in a low-speed motion scene, which is applied to an intelligent driving vehicle and comprises the following steps: step 1, acquiring original laser point cloud data, inertial measurement unit data and a local map; step 2, performing point-surface matching on the original laser point cloud data and a local map to estimate a vehicle pose; 3, according to the vehicle pose and the corresponding original laser point cloud data, the original laser point cloud data in a preset range are sequentially added into the voxel grid through a coordinate conversion function Pi = Rpi + t, and therefore a sub-map based on sparse voxels is constructed; and step 4, in the sub-map construction process, counting the collision times of each voxel, sorting the collision times, screening out the voxel where the dynamic obstacle point cloud is located, and then removing the voxel. The method solves the problems that in the prior art, the computing power requirement is too large, and long smear is likely to occur in the point cloud map.

Description

Technical Field

[0001] The present application relates to the field of intelligent driving technology, and in particular to a method for removing dynamic obstacles from laser point clouds in low-speed motion scenarios. Background Art

[0002] With the rapid development of science and technology, all walks of life have gradually introduced unmanned intelligent equipment to improve the quality and efficiency of enterprises. Among them, L4-level low-speed unmanned vehicles have begun to be widely used in the transportation process of various products. Unlike public road scenes, transportation operations in specific scenarios require extremely high accuracy of vehicle position and stability of vehicle movement, such as 24-hour continuous container transshipment at seaports and long-distance product transportation in industrial plants. When the vehicle is running in the aforementioned scenarios, its RTK antenna may be lost, such as entering an obstructed area or encountering a geomagnetic storm. In order to maintain a high level of positioning accuracy, other positioning sources are usually required to assist in achieving global positioning.

[0003] Generally speaking, the use of laser point cloud positioning assistance in low-speed unmanned driving technology is the mainstream solution. The real-time laser point cloud and the pre-collected point cloud map are matched and positioned to output the global positioning result. In this process, the main sources of error in the entire positioning include the calculation error in the matching process of the real-time point cloud and the map, and the matching error caused by the difference between the pre-built point cloud map and the actual scene. The pre-built point cloud map is different from the actual scene. This difference is mainly due to the movement of static objects in the actual scene after mapping (that is, scene changes) and the long trailing caused by the movement of dynamic obstacles during the mapping process. If the scene changes, this error is difficult to resolve, and the construction of the new map can only be completed by updating the point cloud map. The long trailing caused by dynamic obstacles during the mapping process can be eliminated by relevant methods. Summary of the invention

[0004] In view of this, the present invention provides a method for removing dynamic obstacles from laser point cloud in low-speed motion scenes to solve the technical problem that there are long trails in the point cloud map in the prior art, thereby causing dynamic obstacles to affect vehicle positioning.

[0005] The present invention provides a method for removing dynamic obstacles from laser point cloud in a low-speed motion scene, which is applied to intelligent driving vehicles. The method comprises: step 1, obtaining original laser point cloud data, inertial measurement unit data and a local map; step 2, performing point-surface matching between the original laser point cloud data and the local map to estimate the vehicle posture; step 3, according to the vehicle posture and its corresponding original laser point cloud data, converting the original laser point cloud data within a preset range into a coordinate conversion function P i =Rp i +t are added to the voxel grid in turn to construct a sparse voxel-based submap, where p iis the original laser point cloud data, R and t are the estimated vehicle poses, P i is the coordinate point in the sub-map coordinate system; step 4, during the sub-map construction process, the number of collisions of each voxel is counted, and the voxels where the dynamic obstacle point cloud is located are screened out by sorting the collision numbers, and then removed.

[0006] Furthermore, the method also includes: Step 0, the intelligent driving vehicle obtains original laser point cloud data through a distance sensor, obtains inertial measurement unit data through an inertial measurement unit, and obtains a local map through a network.

[0007] Furthermore, the sub-map is constructed using incremental voxels based on a sparse voxel-based neighbor structure.

[0008] Furthermore, the incremental voxel is composed of voxels sparsely distributed in space, and each voxel stores multiple point clouds, and the number of point clouds is determined by the set voxel size.

[0009] Furthermore, the voxel grid coordinates are mapped to hash key ids by a hash function. v A hash table is formed to construct a submap, and the hash function is: p = [p x ,p y ,p z ] T , id v = hash(v) = (v x n x )xor(v y n y )xor(v z n z )modN, where p is the original laser point cloud coordinate, id v Hash key value.

[0010] Furthermore, the step 4 includes: step 41, when the original laser point cloud data is added to the voxel grid during the construction of the sub-map, the voxel grid is accessed; step 42, the number of collisions of the accessed voxel grid is recorded once and stored in front of the sequence list; step 43, all voxels in the sequence list starting from the preset position are removed.

[0011] Furthermore, the method also includes: step 5, removing voxels corresponding to the original laser point cloud data outside a preset range.

[0012] Furthermore, the elimination is to delete all voxels and their internal point cloud data.

[0013] Furthermore, the method also includes: step 6, splicing the sub-maps into a global map according to the relative relationship between the initial positions of the sub-maps, and outputting the global map for use by the intelligent driving vehicle.

[0014] The present invention provides a method for removing dynamic obstacles from laser point clouds in low-speed motion scenes. The method estimates the vehicle posture in real time through the original laser point cloud and inertial measurement unit (IMU) data. According to the posture estimation result and its corresponding original point cloud, the original point cloud data within a certain range is converted into a dynamic obstacle removal method through the formula P. i =Rp i + t are added to the voxel grid in sequence to construct a sub-map. At the same time, the number of collisions of each voxel is counted. By sorting the number of collisions, the voxels where the dynamic obstacle point cloud is located are screened out and then removed. This technical solution solves the problem of excessive computing power requirements and long smears in point cloud maps in existing technical solutions. BRIEF DESCRIPTION OF THE DRAWINGS

[0015] Figure 1 A schematic flow chart of a method for removing dynamic obstacles from a laser point cloud in a low-speed motion scene provided by the present invention;

[0016] Figure 2 It is a schematic flow chart of another method for removing dynamic obstacles from laser point clouds in a low-speed motion scene provided by the present invention;

[0017] Figure 3 It is a schematic flow chart of another method for removing dynamic obstacles from laser point clouds in a low-speed motion scene provided by the present invention;

[0018] Figure 4 It is a flow chart of a method for screening and removing voxels where dynamic obstacle point clouds are located, provided by the present invention. DETAILED DESCRIPTION

[0019] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0020] Embodiment 1

[0021] The present invention provides a method for removing dynamic obstacles from laser point clouds in low-speed motion scenes, which is applied to intelligent driving vehicles, such as Figure 1 As shown, the method steps are as follows.

[0022] Step 1, obtaining raw laser point cloud data, inertial measurement unit data and local map;

[0023] Step 2: Match the original laser point cloud data with the local map to estimate the vehicle posture;

[0024] Step 3: According to the vehicle posture and its corresponding original laser point cloud data, the original laser point cloud data within the preset range is transformed through the coordinate conversion function P i =Rp i +t are added to the voxel grid in turn to construct a sparse voxel-based submap, where p i is the original laser point cloud data, R and t are the estimated vehicle poses, P i It is the coordinate point in the sub-map coordinate system;

[0025] Step 4: During the sub-map construction process, the number of collisions of each voxel is counted, and the voxels where the dynamic obstacle point cloud is located are screened out by sorting the collision numbers, and then removed.

[0026] The voxel is the abbreviation of volume pixel, and a solid body including voxels can be represented by stereo rendering or by extracting a polygonal isosurface with a given threshold contour.

[0027] The present invention provides a method for removing dynamic obstacles from laser point clouds in low-speed motion scenes. The method estimates the vehicle posture in real time through the original laser point cloud and inertial measurement unit (IMU) data. According to the posture estimation result and its corresponding original point cloud, the original point cloud data within a certain range is converted into a dynamic obstacle removal method through the formula P. i =Rp i + t are added to the voxel grid in sequence to construct a sub-map. At the same time, the number of collisions of each voxel is counted. By sorting the number of collisions, the voxels where the dynamic obstacle point cloud is located are screened out and then removed. This technical solution solves the problem of excessive computing power requirements and long smears in point cloud maps in existing technical solutions.

[0028] Embodiment 2

[0029] The present invention provides a method for removing dynamic obstacles from laser point clouds in low-speed motion scenes, which is applied to intelligent driving vehicles, such as Figure 2 As shown, the method steps are as follows.

[0030] Step 0: The intelligent driving vehicle obtains raw laser point cloud data through the distance sensor, obtains inertial measurement unit data through the inertial measurement unit, and obtains the local map through the network.

[0031] Step 1, obtaining raw laser point cloud data, inertial measurement unit data and local map;

[0032] Step 2: Match the original laser point cloud data with the local map to estimate the vehicle posture;

[0033] The original laser point cloud data and inertial measurement unit (IMU) data are used as inputs of the laser inertial odometry to estimate the vehicle posture in real time through relevant algorithms. The laser odometry module does not perform traditional point cloud feature extraction, but directly uses the original point cloud and the generated local map for point-to-surface matching to achieve accurate posture estimation.

[0034] Step 3: According to the vehicle posture and its corresponding original laser point cloud data, the original laser point cloud data within the preset range is transformed through the coordinate conversion function P i =Rp i +t are added to the voxel grid in turn to construct a sparse voxel-based submap, where p i is the original laser point cloud data, R and t are the estimated vehicle poses, P i It is the coordinate point in the sub-map coordinate system;

[0035] The pose estimation results of the key frames obtained in steps 1 and 2 and their corresponding original laser point clouds are used to convert the original point cloud data within a certain range into i =Rp i +t are added to the voxel grid in sequence, where p i is the original point cloud data, R and t are the pose results estimated by the laser inertial odometer in the previous step, and P i is the coordinate point in the submap coordinate system. The submap is constructed using a sparse voxel-based neighbor structure incremental voxels (iVox, Incremental Voxels). Incremental voxels are composed of voxels sparsely distributed in space. Each voxel can store a certain number of point clouds, and the number is determined by the set voxel size. The grid coordinates of the voxel itself are mapped to the hash key id by the spatial hash function (as shown below) v iVox can greatly speed up the voxel search rate and sub-map construction rate.

[0036] p=[p x ,p y ,p z ] T ,

[0037]

[0038] id v = hash(v) = (v x n x )xor(v y n y )xor(v z n z )modN,

[0039] Step 4: During the sub-map construction process, the number of collisions of each voxel is counted, and the voxels where the dynamic obstacle point cloud is located are screened out by sorting the collision numbers, and then removed.

[0040] The present invention provides a method for removing dynamic obstacles from laser point clouds in low-speed motion scenes. The method estimates the vehicle posture in real time through the original laser point cloud and inertial measurement unit (IMU) data. According to the posture estimation result and its corresponding original point cloud, the original point cloud data within a certain range is converted into a dynamic obstacle removal method through the formula P. i =Rp i + t are added to the voxel grid in sequence to construct a sub-map. At the same time, the number of collisions of each voxel is counted. By sorting the number of collisions, the voxels where the dynamic obstacle point cloud is located are screened out and then removed. This technical solution solves the problem of excessive computing power requirements and long smears in point cloud maps in existing technical solutions.

[0041] Embodiment 3

[0042] The present invention provides a method for removing dynamic obstacles from laser point clouds in low-speed motion scenes, which is applied to intelligent driving vehicles, such as Figure 2 and Figure 3 As shown, the method steps are as follows.

[0043] Step 1, obtaining raw laser point cloud data, inertial measurement unit data and local map;

[0044] Step 2: Match the original laser point cloud data with the local map to estimate the vehicle posture;

[0045] Step 3: According to the vehicle posture and its corresponding original laser point cloud data, the original laser point cloud data within the preset range is transformed through the coordinate conversion function P i =Rp i +t are added to the voxel grid in turn to construct a sparse voxel-based submap, where p i is the original laser point cloud data, R and t are the estimated vehicle poses, P i It is the coordinate point in the sub-map coordinate system;

[0046] Step 4: During the sub-map construction process, the number of collisions of each voxel is counted, and the voxels where the dynamic obstacle point cloud is located are screened out by sorting the collision numbers, and then removed.

[0047] like Figure 4 As shown, the specific steps of step 4 are as follows.

[0048] Step 41, when the original laser point cloud data is added to the voxel grid during the sub-map construction, the voxel grid is accessed;

[0049] Step 42, record the number of collisions of the visited voxel grid and store it in front of the sequence table;

[0050] Step 43: remove all voxels from the preset position onwards in the sequence list.

[0051] In the process of constructing the submap, the number of collisions of each voxel is counted at the same time. By sorting the number of collisions, the voxels where the dynamic obstacle point cloud is located are screened out and then eliminated. When constructing a submap with the original point cloud of a certain number of key frame sequences, an LRU cache (least recently used) is used to manage the voxels. When registering the point cloud to the voxel grid, the frequently accessed voxel grid will be stored in the front of the LRU cache sequence, and the voxels that are not frequently accessed will naturally be moved to the end of the queue. Voxels that are not frequently accessed indicate that they exist for a short period of time. Due to the movement of the object, they disappear, which directly shows that the voxel has a small number of collisions and can be eliminated.

[0052] Step 5: remove the voxels corresponding to the original laser point cloud data outside the preset range.

[0053] Since the measurement error of long-distance laser point clouds is relatively large, the voxel capacity and voxel distribution of the submap within a certain distance range are set to ensure that the submap only contains point clouds within the set range, and voxels in a farther range are discarded. In addition, the culling operation is for each complete voxel. If a voxel needs to be culled, the point cloud inside it will be completely deleted.

[0054] Step 6: According to the relative relationship between the initial positions of the sub-maps, the sub-maps are spliced ​​into a global map, and the global map is output for use by the intelligent driving vehicle.

[0055] The present invention provides a method for removing dynamic obstacles from laser point clouds in low-speed motion scenes. The method estimates the vehicle posture in real time through the original laser point cloud and inertial measurement unit (IMU) data. According to the posture estimation result and its corresponding original point cloud, the original point cloud data within a certain range is converted into a dynamic obstacle removal method through the formula P. i =Rp i + t are added to the voxel grid in sequence to construct a sub-map. At the same time, the number of collisions of each voxel is counted. By sorting the number of collisions, the voxels where the dynamic obstacle point cloud is located are screened out and then removed. This technical solution solves the problem of excessive computing power requirements and long smears in point cloud maps in existing technical solutions.

[0056] In summary, in order to reduce the long trails caused by dynamic obstacles in pre-built point cloud maps, existing technologies use emerging technologies such as convolutional neural networks (CNN) or graph neural networks (GNN) to pre-train detection models with a large amount of data; and use traditional (non-deep learning) methods to effectively distinguish dynamic points from static points by performing a series of operations such as clustering and segmentation on real-time input point clouds. This processing of real-time point clouds is usually used in Simulaneous Localization and Mapping (SLAM) to complete real-time positioning and global map construction in unknown environments. However, the use of deep learning methods to filter out dynamic obstacles requires pre-training of the model in advance, which is difficult to adapt to various types of scenarios. The pre-training cost and the calculation cost when the actual vehicle is installed are relatively high. Compared with the original direct use of the radar input point cloud, the deep learning point cloud processing and subsequent point cloud transmission are added, so there are greater requirements for the vehicle transmission bandwidth during actual operation; through the above SLAM method, the constructed global map still has long shadows caused by dynamic obstacles, which still has an adverse effect on subsequent matching. Multiple processing of the real-time input laser point cloud not only requires more computing power, but also increases the time delay. The technical solution proposed by the present invention effectively solves the above problems.

[0057] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for removing dynamic obstacles from laser point clouds in low-speed motion scenes, applied to intelligent driving vehicles, characterized in that: The method comprises: Step 1, obtaining raw laser point cloud data, inertial measurement unit data and local map; Step 2: Match the original laser point cloud data with the local map to estimate the vehicle posture; Step 3: According to the vehicle posture and its corresponding original laser point cloud data, the original laser point cloud data within the preset range is transformed through the coordinate conversion function P i =Rp i + t are added to the voxel grid in turn to construct a voxel-based submap, where p i is the original laser point cloud data, R and t are the estimated vehicle poses, P i It is the coordinate point in the sub-map coordinate system; Step 4: During the sub-map construction process, the number of collisions of each voxel is counted, and the voxels where the dynamic obstacle point cloud is located are screened out by sorting the collision numbers, and then removed.

2. According to claim 1, a method for removing dynamic obstacles from laser point cloud in a low-speed motion scene is characterized in that: The method further comprises: Step 0: The intelligent driving vehicle obtains raw laser point cloud data through the distance sensor, obtains inertial measurement unit data through the inertial measurement unit, and obtains the local map through the network.

3. According to claim 1, a method for removing dynamic obstacles from laser point cloud in a low-speed motion scene is characterized in that: The submap is constructed using incremental voxels based on a sparse voxel-based neighbor structure.

4. According to claim 3, a method for removing dynamic obstacles from laser point cloud in a low-speed motion scene is characterized in that: The incremental voxel is composed of voxels sparsely distributed in space, and each voxel stores multiple point clouds. The number of point clouds is determined by the set voxel size.

5. According to the method for removing dynamic obstacles from laser point cloud in a low-speed motion scene in claim 1, it is characterized in that: The voxel grid coordinates are mapped to hash key ids by a hash function v A hash table is formed to construct a submap, and the hash function is: p=[p x ,p y ,p z ] T , id v =hash(v)=(v x n x (xor(v) y n y (xor(v) z n z )modN, Among them, p is the original laser point cloud coordinate, id v Hash key value.

6. According to claim 1, a method for removing dynamic obstacles from laser point cloud in a low-speed motion scene is characterized in that: The step 4 comprises: Step 41, when the original laser point cloud data is added to the voxel grid during the sub-map construction, the voxel grid is accessed; Step 42, record the number of collisions of the visited voxel grid and store it in the front of the sequence table; Step 43: remove all voxels from the preset position onwards in the sequence list.

7. According to claim 1, a method for removing dynamic obstacles from laser point cloud in a low-speed motion scene is characterized in that: The method further comprises: Step 5: remove the voxels corresponding to the original laser point cloud data outside the preset range.

8. The method for removing dynamic obstacles from laser point cloud in a low-speed motion scene according to claim 1 or 7, characterized in that: The culling is to delete all voxels and their internal point cloud data.

9. According to claim 1, a method for removing dynamic obstacles from laser point cloud in a low-speed motion scene is characterized in that: The method further comprises: Step 6: According to the relative relationship between the initial positions of the sub-maps, the sub-maps are spliced ​​into a global map, and the global map is output for use by the intelligent driving vehicle.

Images