A method for removing dynamic object point clouds in a point cloud map

Abstract

The application discloses a kind of methods of rejecting dynamic object point cloud in point cloud map, vehicle normal driving, the point cloud data of the surrounding environment information obtained by laser radar scanning is collected, and the 3D pose information of integrated navigation;According to the surrounding environment information obtained by laser radar scanning, the motion of dynamic target point cloud is identified and tracked and dynamic target is rejected;The data of the time dimension of map key frame is used to track the dynamic target point cloud cluster remaining in the map, and the point cloud quantity attribute is introduced in the target association stage of dynamic target tracking.The application uses the information of point cloud frame in time and space two dimensions, tracks the motion of dynamic target point cloud, reduces the false deletion to static target.The application introduces the ratio of the number of points of target clustering cluster in adjacent point cloud frame as a new attribute when using data association to track the motion of the point cloud where the moving target is located, improves the effect of tracking moving target using only the point cloud output by laser radar.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of assisted driving, in particular to a method for removing dynamic object point clouds in a point cloud map. BACKGROUND

[0002] With the rapid development of the automobile industry in recent years, assisted driving technology, especially advanced assisted driving technology, has become a technology commanding point that major car companies are competing to occupy. Major car companies have launched car models equipped with L2+ and L2.99 level assisted driving technology.

[0003] Currently, the main technical problems in realizing advanced assisted driving are in the subfields of positioning, perception, planning and control. The positioning technology not only solves the simple problem of which road the vehicle is on and which direction the vehicle is driving in, but also is responsible for the current lane of the vehicle and the accurate lateral and longitudinal position of the vehicle in the lane.

[0004] The most common method to solve the positioning problem at present is to use a priori map as a reference and combine the environmental information scanned by the vehicle sensors around the vehicle to match and obtain the position of the vehicle in the priori map. The correctness of the map directly affects the positioning accuracy, which requires that as much static scene information as possible be restored during the mapping process and the influence of dynamic objects on the map be minimized.

[0005] The methods currently used to reduce the influence of dynamic objects on the map include the following: using a grid map to filter dynamic objects; filtering dynamic objects by calculating the occupancy probability; filtering dynamic objects by the iterative closest point algorithm; and using a model trained by deep learning to filter dynamic objects in recent years.

[0006] Unlike the above algorithms, the present application proposes a method for removing dynamic object point clouds in a point cloud map, which more thoroughly removes dynamic target point clouds in the map without mistakenly deleting static points in the map. SUMMARY

[0007] To solve the problems in the prior art, the purpose of the present application is to provide a method for removing dynamic object point clouds in a point cloud map, which uses the time dimension data of the key frames of the map to track the residual dynamic target point cloud clusters in the map, and introduces the point cloud quantity attribute in the target association stage of dynamic target tracking.

[0008] To achieve the purpose of the present application, the technical solution adopted by the present application is:

[0009] A method for removing dynamic object point clouds in a point cloud map, comprising the steps of:

[0010] (1) The vehicle is normally running, and the data acquisition unit synchronously acquires the point cloud data of the surrounding environment information obtained by laser radar scanning and the 3D pose information of the integrated navigation;

[0011] (2) The dynamic target elimination unit identifies the motion of the dynamic target point cloud according to the surrounding environment information obtained by laser radar scanning, and eliminates the dynamic target;

[0012] (3) The map generation unit generates a high-precision point cloud map of the surrounding environment according to the 3D pose of the integrated navigation and the point cloud frame of the eliminated dynamic target.

[0013] Further, in step (1), the 3D pose output by the integrated navigation and the scanning data output by the laser radar are synchronously acquired to obtain a sequence of integrated navigation poses Position_queue_rtk and a sequence of laser radar scanning data frames Pointcloud_queue_lidar.

[0014] Further, in step (2), the method specifically comprises the steps of:

[0015] (2.1) Starting from the first frame of the sequence of laser radar scanning data frames Pointcloud_queue_lidar, 5 consecutive frames of laser radar scanning data are extracted into a subsequence Pointcloud_queue_submap;

[0016] (2.2) Starting from the first frame of the subsequence Pointcloud_queue_submap, 1 frame of laser radar scanning data is extracted, and a non-ground point extraction algorithm is performed on the frame of point cloud to obtain non-ground points in the frame of point cloud, which are stored in a sequence of non-ground points Pointcloud_none_ground;

[0017] (2.3) Using the pose information synchronously acquired by the sequence of integrated navigation poses Position_queue_rtk and the subsequence Pointcloud_queue_submap laser radar scanning data, combining the extrinsic matrix of the integrated navigation to the laser radar stitching adjacent point cloud frames except the current point cloud frame to obtain a local point cloud map Submap_lidar;

[0018] (2.4) Traversing each point in the sequence of non-ground points Pointcloud_none_ground, calculating the nearest point of the point in the local point cloud map Submap_lidar and the distance to the nearest point; storing the points in the sequence of non-ground points Pointcloud_none_ground with a distance less than a threshold to a possible motion point cluster Pointcloud_moving;

[0019] (2.5) Perform the Euclidean clustering algorithm in the possible moving point cluster Pointcloud_moving, remove the cluster whose point number is less than the threshold value, and get the moving target cluster Cluster_moving[i];

[0020] (2.6) Repeat steps 2.2 to 2.5 until all point clouds in the subsequence Pointcloud_queue_submap are traversed to obtain the moving target cluster Cluster_moving[i], i ranges from 0 to 4;

[0021] (2.7) Repeat steps 2.1 to 2.6 until all point clouds in the laser radar scan data frame sequence Pointcloud_queue_lidar are traversed to obtain the moving target cluster Cluster_moving[i], i ranges from 0 to 'n-1';

[0022] (2.8) Start from i=0, track the motion trajectory of the dynamic target cluster in the moving target cluster Cluster_moving[i], and obtain the speed and motion direction of the moving target point cloud;

[0023] (2.9) Extract the moving target point cloud cluster whose data association is successful and whose speed is greater than the threshold value in step 2.8 to the to-be-removed point cloud cluster Custer_moving_verified[i];

[0024] (2.10) Traverse the laser radar scan data frame sequence Pointcloud_queue_lidar, remove the dynamic target point cloud cluster Custer_moving_verified[i] in the point cloud frame Pointcloud_queue_lidar[i], and obtain the point cloud frame Pointcloud_queue_lidar_none_moving[i] after removing the dynamic target point cloud.

[0025] Further, in step (2.8), take Cluster_moving[i] and Cluster_moving[i+1] from i=0, combine the time information of Cluster_moving[i] and Cluster_moving[i+1] respectively, combine the navigation pose information and the extrinsic matrix of the navigation to the laser radar Calculate the speed and motion direction of a moving object cluster in Cluster_moving[i] moving to the corresponding position in Cluster_moving[i+1].

[0026] Further, the data association is used to determine which point cloud clusters in Cluster_moving[i] and Cluster_moving[i+1] are the point clouds of the same moving target in two frames of point clouds; the data association attribute is that the ratio of the number of points in the cluster of a moving target in the cluster Cluster_moving[i+1] to the number of points in the cluster of a moving target in the cluster Cluster_moving[i].

[0027] Further, in step (3), the pose sequence Position_queue_rtk and the pose information of the laser radar scanning data synchronously collected with the point cloud frame Pointcloud_queue_lidar_none_moving[i] after the dynamic target point cloud is removed are used to combine the external parameter matrix of the integrated navigation to the laser radar The adjacent point cloud frames from i=0 to i=n-1 are spliced to obtain a static point cloud map Map_lidar_static not containing dynamic target point clouds.

[0028] A system for removing dynamic object point clouds in a point cloud map, comprising a laser radar and an integrated navigation mounted on the top of a vehicle, and a data acquisition unit, a dynamic target removal unit and a map generation unit; the method for removing dynamic object point clouds in a point cloud map is realized.

[0029] The present application has the beneficial effect that, compared with the prior art, the present application uses the information of the point cloud frame in two dimensions of time and space to track the motion (speed and motion direction) of the dynamic target point cloud, and reduces the false deletion of static targets.

[0030] When the present application uses data association to track the motion of the point cloud where the moving target is located, the ratio of the number of points in the target cluster in the adjacent point cloud frames is introduced as a new attribute in data association, which improves the effect of tracking the moving target by using only the point cloud output by the laser radar (the correctness of the association of the same target is improved). BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 The present application is a method flowchart for removing dynamic object point clouds in a point cloud map. DETAILED DESCRIPTION

[0032] The technical solutions of the present application will be further described below in combination with the drawings and examples. The following examples are only used to more clearly illustrate the technical solutions of the present application, and cannot be used to limit the protection scope of the present application.

[0033] The method for removing dynamic object point clouds in a point cloud map according to the present application needs to use a system hardware architecture including a mechanical laser radar installed on the top of a vehicle and a combined navigation installed on the top of the vehicle. The laser radar scans to obtain surrounding environment information, and the combined navigation collects 3D pose information.

[0034] The combined navigation sends a 1hz PPS satellite synchronization signal to the laser radar installed on the top of the vehicle. After the laser radar receives the PPS satellite synchronization signal, the collection time of the laser radar is synchronized to the combined navigation time (also called GPS time).

[0035] The method for removing dynamic object point clouds in a point cloud map according to the present application needs to use a system software architecture including a data collection unit, a dynamic target removal unit and a map generation unit.

[0036] The data collection unit synchronously collects point cloud data of the surrounding environment information scanned by the laser radar and synchronously collects 3D pose information of the combined navigation.

[0037] The dynamic target removal unit uses the surrounding environment information scanned by the laser radar to identify, track and remove moving targets.

[0038] The map generation unit uses the 3D pose output by the combined navigation and the point cloud frame (with dynamic objects removed) collected by the laser radar to generate a high-precision point cloud map of the surrounding environment.

[0039] As shown in Figure 1 The method for removing dynamic object point clouds in a point cloud map according to the present application includes the following steps:

[0040] (1) The vehicle normally drives on a road to be surveyed, and the data collection unit synchronously collects laser radar and combined navigation data;

[0041] Wherein, the extrinsic matrix of the combined navigation to the laser radar is denoted as

[0042] (2) Continuously synchronously collect the 3D pose output by the combined navigation and the scanning data frame output by the laser radar to obtain a combined navigation pose sequence Position_queue_rtk and a laser radar scanning data frame sequence Pointcloud_queue_lidar.

[0043] (3) From the first frame of the laser radar scanning data frame sequence Pointcloud_queue_lidar, extract 5 continuous laser radar scanning data to a subsequence Pointcloud_queue_submap.

[0044] (4) From the first frame of the subsequence Pointcloud_queue_submap, extract 1 frame of lidar scan data and perform the non-ground point extraction algorithm on this frame of point cloud, and store the non-ground points in this frame of point cloud obtained in the non-ground point sequence Pointcloud_none_ground;

[0045] (5) Use the combined navigation pose sequence Position_queue_rtk and the pose information synchronously collected by the subsequence Pointcloud_queue_submap lidar scan data, combined with the extrinsic matrix of the combined navigation to the lidar Splice the adjacent point cloud frames except the current point cloud frame to obtain the local point cloud map Submap_lidar;

[0046] (6) Traverse each point in the non-ground point sequence Pointcloud_none_ground, calculate the nearest point in the local point cloud map Submap_lidar and the distance to the nearest point; store the points in Pointcloud_none_ground with a distance less than a threshold (for example, 0.1 meters) to the possible moving point cluster Pointcloud_moving;

[0047] (7) Perform the Euclidean clustering algorithm (Euclidean distance less than 0.1 meters) in the possible moving point cluster Pointcloud_moving, and delete the cluster cluster whose number of points is less than a threshold (10 points) in the cluster, to obtain the moving target cluster Cluster_moving[i];

[0048] (8) Repeat steps 4 to 7 until all point clouds in the subsequence Pointcloud_queue_submap are traversed to obtain the moving target cluster Cluster_moving[i] (i ranges from 0 to 4);

[0049] (9) Repeat steps 3 to 8 until all point clouds in the lidar scan data frame sequence Pointcloud_queue_lidar are traversed to obtain the moving target cluster Cluster_moving[i] (i ranges from 0 to 'n-1');

[0050] (10) Starting from i=0, track the motion trajectory of the dynamic target cluster in the moving target cluster Cluster_moving[i], to obtain the speed and motion direction of the moving target point cloud;

[0051] a、Cluster_moving[i] represents the position information of the dynamic target in the 3D space in the current time point cloud frame;

[0052] b. Take Cluster_moving[i] and Cluster_moving[i+1] from i = 0, combine the time information of the point cloud frame when Cluster_moving[i] and Cluster_moving[i+1] are respectively located, combine the navigation pose information and the extrinsic matrix of the navigation to the laser radar The speed and motion direction of a moving object cluster in Cluster_moving[i] moving to the corresponding position in Cluster_moving[i+1] can be calculated;

[0053] c. Data association is needed in step b to determine which point cloud clusters in Cluster_moving[i] and Cluster_moving[i+1] are the same moving target point cloud in the two frames of point cloud clusters;

[0054] In addition to the traditional data association attributes (speed, motion direction), the present application introduces a new data association attribute, the ratio of the number of points in the cluster cluster of a moving target in Cluster_moving[i+1] to the number of points in the cluster cluster of a moving target in Cluster_moving[i]; if the motion direction of the dynamic target is opposite to the motion direction of the collection vehicle, this ratio is used in data association calculation, if the motion direction of the dynamic target is the same as the motion direction of the collection vehicle, the inverse of this ratio is used in data association calculation.

[0055] (11) Extract the moving target point cloud cluster in step 10 that is successfully data associated and whose speed is greater than the threshold (0.1 m / s) to the point cloud cluster to be removed Custer_moving_verified[i];

[0056] The point with a speed less than the threshold and not successfully data associated is considered to be a false dynamic target caused by the radar scanning the same static object from different angles, and is not added to the point cloud cluster to be removed Custer_moving_verified[i].

[0057] (12) Traverse the laser radar scan data frame sequence Pointcloud_queue_lidar, remove the dynamic target point cloud cluster Custer_moving_verified[i] in the point cloud frame Pointcloud_queue_lidar[i], and obtain the point cloud frame Pointcloud_queue_lidar_none_moving[i] after removing the dynamic target point cloud.

[0058] (13) The pose information of the laser radar scan data collected synchronously by the pose sequence Position_queue_rtk and Pointcloud_queue_lidar_none_moving[i] is combined with the external parameter matrix of the integrated navigation to the laser radar The static point cloud map Map_lidar_static not containing the dynamic target point cloud is obtained by splicing the adjacent point cloud frames from i=0 to i=n-1.

[0059] The application has the beneficial effect that, compared with the prior art, the application uses the information of the point cloud frames in two dimensions of time and space to track the motion (speed and motion direction) of the dynamic target point cloud, and reduces the false deletion of the static target.

[0060] When the application is used to track the motion of the point cloud where the moving target is located, the ratio of the number of points of the target clustering cluster in the adjacent point cloud frames is introduced as a new attribute when data association, which improves the effect of tracking the moving target by using only the point cloud output by the laser radar (the correctness of the association of the same target is improved).

[0061] The applicant of the present application has made a detailed description and explanation of the embodiments of the present application in combination with the drawings of the specification, but those skilled in the art should understand that the above embodiments are only preferred embodiments of the present application, and the detailed description is only to help the reader better understand the spirit of the present application, and is not a limitation on the protection scope of the present application, on the contrary, any improvement or modification based on the spirit of the present application should fall within the protection scope of the present application.

Claims

1. A method for removing dynamic object point clouds from a point cloud map, characterized in that, Including the following steps: (1) When the vehicle is driving normally, the data acquisition unit synchronously acquires point cloud data of the surrounding environment information obtained by the LiDAR scanning, as well as 3D pose information of the integrated navigation; synchronously acquires 3D pose output by the integrated navigation and scanning data output by the LiDAR to obtain the integrated navigation pose sequence Position_queue_rtk and the LiDAR scanning data frame sequence Pointcloud_queue_lidar. (2) The dynamic target elimination unit identifies and tracks the movement of the dynamic target point cloud and eliminates the dynamic target based on the surrounding environment information obtained by the lidar scan; The specific steps include: (2.1) Starting from the first frame of the lidar scanning data frame sequence Pointcloud_queue_lidar, extract five consecutive frames of lidar scanning data into the subsequence Pointcloud_queue_submap; (2.2) Starting from the first frame of the subsequence Pointcloud_queue_submap, extract one frame of lidar scanning data and perform a non-ground point extraction algorithm on this frame of point cloud to obtain the non-ground points in the point cloud of this frame and store them in the non-ground point sequence Pointcloud_none_ground; (2.3) Using the pose information synchronously collected by the LiDAR scanning data of the integrated navigation pose sequence Position_queue_rtk and the subsequence Pointcloud_queue_submap, combined with the extrinsic parameter matrix from integrated navigation to LiDAR, adjacent point cloud frames other than the current point cloud frame are stitched together to obtain the local point cloud map Submap_lidar; (2.4) Traverse each point in the non-ground point sequence Pointcloud_none_ground, calculate the nearest point of this point in the local point cloud map Submap_lidar, and the distance to the nearest point; store the possible moving point cluster Pointcloud_moving from the points in the non-ground point sequence Pointcloud_none_ground whose distance is less than the threshold. (2.5) Perform Euclidean clustering algorithm in the possible moving point cluster Pointcloud_moving, delete the clusters in which the number of points is less than the threshold, and obtain the moving target cluster Cluster_moving[i]; (2.6) Repeat steps 2.2 to 2.5 until all point clouds in the subsequence Pointcloud_queue_submap have been traversed to obtain the moving target cluster Cluster_moving[i], where i ranges from 0 to 4; (2.7) Repeat steps 2.1 to 2.6 until all point clouds in the LiDAR scan data frame sequence Pointcloud_queue_lidar have been traversed to obtain the moving target cluster Cluster_moving[i], where i ranges from 0 to 'n-1'; (2.8) Starting from i=0, track the motion trajectory of the dynamic target cluster in the moving target cluster Cluster_moving[i] to obtain the velocity and direction of motion of the moving target point cloud; (2.9) Extract the point cloud clusters of moving targets that have been successfully associated in step 2.8 and whose speed is greater than the threshold to the point cloud clusters to be removed: Custer_moving_verified[i]; (2.10) Traverse the LiDAR scan data frame sequence Pointcloud_queue_lidar, remove the dynamic target point cloud cluster Custer_moving_verified[i] in the point cloud frame Pointcloud_queue_lidar[i], and obtain the point cloud frame Pointcloud_queue_lidar_none_moving[i] after removing the dynamic target point cloud; (3) The map generation unit generates a high-precision point cloud map of the surrounding environment based on the 3D pose of the integrated navigation and the point cloud frames after removing dynamic targets.

2. The method for removing dynamic object point clouds from a point cloud map according to claim 1, characterized in that, In step (2.8), starting from i=0, Cluster_moving[i] and Cluster_moving[i+1] are taken. Combining the time information of the point cloud frames where Cluster_moving[i] and Cluster_moving[i+1] are respectively located, the navigation pose information and the external parameter matrix of the combined navigation to the lidar are combined to calculate the speed and direction of motion of a moving object cluster in Cluster_moving[i] to the corresponding position in Cluster_moving[i+1].

3. The method for removing dynamic object point clouds from a point cloud map according to claim 2, characterized in that, Use data association to determine which point cloud clusters in Cluster_moving[i] and Cluster_moving[i+1] are point clouds of the same moving target in the two frame point cloud clusters; the data association attribute is the ratio of the number of points in the cluster of a moving target in Cluster_moving[i+1] to the number of points in the cluster of a moving target in Cluster_moving[i].

4. The method for removing dynamic object point clouds from a point cloud map according to claim 3, characterized in that, In step (3), the pose information collected synchronously from the pose sequence Position_queue_rtk and Pointcloud_queue_lidar_none_moving[i] LiDAR scanning data is combined with the external parameter matrix from the combined navigation to the LiDAR to stitch together the adjacent point cloud frames from i=0 to i=n-1 to obtain a static point cloud map Map_lidar_static that does not contain dynamic target point clouds.

5. A system for removing dynamic object point clouds from point cloud maps, characterized in that, It includes a lidar and integrated navigation system mounted on the top of the vehicle, as well as a data acquisition unit, a dynamic target removal unit, and a map generation unit; used to implement the method for removing dynamic object point clouds from point cloud maps as described in any one of claims 1-4.

Images