Method for filtering dynamic object in laser radar point cloud

Abstract

The invention belongs to the technical field of mobile robot autonomous navigation, and particularly relates to a method for filtering a dynamic object in a laser radar point cloud, which comprises the following steps: a segmentation and clustering step: based on geometric constraints, carrying out segmentation and clustering on the laser radar point cloud to obtain a point cloud cluster set; a dynamic object coarse screening step: based on the number of point clouds at different distances, identifying the point cloud cluster, and performing coarse screening on the dynamic object; a cloud cluster matching and associating step: carrying out matching and associating on the point cloud clusters of the roughly screened dynamic objects in adjacent frames; and a dynamic object filtering step: based on the motion relationship, identifying the dynamic object from the matched and associated point cloud clusters, and filtering.

Description

Technical Field

[0001] The present application belongs to the field of mobile robot autonomous navigation technology, and specifically relates to a method for filtering dynamic objects in a laser radar point cloud. Background Art

[0002] Mobile robots have advantages such as high flexibility and environmental adaptability, and are widely used in various fields.

[0003] Autonomous navigation technology is one of the core technologies of mobile robots, and it essentially solves the problem of getting from location A to location B. The main research issues of mobile robot autonomous navigation technology can be decomposed into three basic issues, namely, the mobile robot's self-positioning, environmental perception, path planning and obstacle avoidance. Among them, self-positioning and environmental perception are collectively referred to as Simultaneous Localization and Mapping (SLAM).

[0004] At present, most indoor SLAM systems are designed based on the assumption of a static environment. However, the indoor environment is not completely static, but usually a dynamic environment with the movement of objects, which will seriously affect the positioning accuracy and mapping quality of the mobile robot.

[0005] Most of the existing dynamic object filtering methods are intertwined with SLAM, which will have a certain degree of impact on positioning and mapping. At the front-end registration level, the existing dynamic object filtering methods require simultaneous filtering and registration, which has a high processing complexity, affecting the efficiency of front-end registration, and further affecting the trajectory accuracy of the mobile robot. At the mapping level, most of the existing dynamic object filtering methods ignore the positioning error problem caused by the movement of dynamic objects on the front-end registration.

[0006] This application is proposed in view of the above-mentioned technical defects. Summary of the invention

[0007] The purpose of this application is to provide a method for filtering dynamic objects in a lidar point cloud to overcome or alleviate at least one of the known technical deficiencies.

[0008] The technical solution of this application is:

[0009] A method for filtering dynamic objects in a laser radar point cloud, comprising:

[0010] Segmentation and clustering step: Based on geometric constraints, the lidar point cloud is segmented and clustered to obtain a point cloud cluster set;

[0011] Dynamic object rough screening step: Based on the number of point clouds at different distances, point cloud clusters are identified to roughly screen dynamic objects;

[0012] Cloud cluster matching and association step: roughly screen dynamic object point cloud clusters in adjacent frames for matching and association;

[0013] Dynamic object filtering step: Based on the motion relationship, dynamic objects are identified from the matching associated point cloud clusters and filtered out.

[0014] Optionally, in the above-mentioned method for filtering dynamic objects in a laser radar point cloud, the segmentation and clustering step specifically includes:

[0015] S21, calculating β value;

[0016] S22, if the β value is not less than the angle threshold β min , then the adjacent point clouds are not segmented;

[0017] If the β value is less than the angle threshold β min , then calculate the d value;

[0018] If the d value is greater than the distance threshold d max , then the adjacent point clouds are segmented;

[0019] If the d value is not greater than the distance threshold d max , then the adjacent point clouds are not segmented;

[0020] in,

[0021] β is the angle between the line between two adjacent laser scanning end points and the longer laser beam;

[0022] d is the distance between two adjacent laser scanning end points.

[0023] Optionally, in the above-mentioned method for filtering dynamic objects in the laser radar point cloud, the step of roughly screening dynamic objects specifically includes:

[0024] S31, calculating the distance L between the center of the point cloud cluster and the origin;

[0025] S32. If the distance L between the center of the point cloud cluster and the origin satisfies The point cloud cluster is roughly screened as a dynamic object, where N is the number of point clouds in the point cloud cluster, d0 is the set segment distance, and h, i are the thresholds of the number of point clouds contained in the point cloud cluster within the corresponding segment distance range.

[0026] Optionally, in the above-mentioned method for filtering dynamic objects in laser radar point cloud, the point cloud cluster matching and associating step includes:

[0027] S41, calculating the number n of point clouds in the roughly screened dynamic object point cloud cluster in the current frame and the previous frame, the distance d from the center position to the laser origin, and the area s;

[0028] S42, calculate the similarity difference ρ of the roughly screened dynamic object point cloud clusters in the current frame and the previous frame, and set the similarity difference ρ less than the similarity difference threshold ρ max Roughly screen dynamic object point cloud clusters for matching and association.

[0029] The similarity difference between the i-th and j-th roughly screened dynamic object point cloud clusters in the current frame and the previous frame is ρ i,j , calculated as follows:

[0030]

[0031] in,

[0032] n (t,i) 、n (t-1,j) is the number of point clouds in the i-th and j-th coarsely screened dynamic object point cloud clusters in the current frame and the previous frame;

[0033] d (t,i) ,d (t-1,j) is the distance from the center of the i-th and j-th roughly screened dynamic object point cloud clusters to the laser origin in the current frame and the previous frame;

[0034] s (t,i) 、s (t-1,j) is the area of ​​the i-th and j-th roughly screened dynamic object point cloud clusters in the current frame and the previous frame;

[0035] a, b, and c are the weight coefficients for calculating the similarity difference of the point cloud clusters of roughly screened dynamic objects.

[0036] Optionally, in the above-mentioned method for filtering dynamic objects in a laser radar point cloud, the dynamic object filtering step includes:

[0037] S51, calculating the center distance l of the matching associated point cloud cluster;

[0038] S52: If the center distance l of the matching associated point cloud cluster is less than the dynamic distance threshold D max , it is identified as a static object;

[0039] If the center distance l of the matching associated point cloud cluster is greater than the dynamic distance threshold D max , it is identified as a dynamic object and filtered out.

[0040] Optionally, the above method for filtering dynamic objects in a laser radar point cloud further includes:

[0041] Data preprocessing steps: Preprocess the lidar point cloud, including:

[0042] S11, obtain each laser beam p i , corresponding to the scanning time t i The odometer pose oi ;

[0043] S12, each odometer position o i The laser pose at the corresponding moment is converted to a unified coordinate system through coordinate transformation, and then converted to the laser coordinate system again through the coordinate transformation relationship, and the new pose is recorded as p i ′ , and obtain a new laser beam P ′ =(p i ′ ,i=1,2,…,N).

[0044] This application has at least the following beneficial technical effects:

[0045] A method for filtering dynamic objects in a lidar point cloud is provided. Based on data preprocessing, the lidar point cloud is segmented and clustered based on geometric constraints to obtain a point cloud cluster set, which can completely distinguish different objects. Then, based on the number of point clouds at different distances, the point cloud clusters are identified, and dynamic objects are roughly screened. The roughly screened point cloud clusters of dynamic objects in adjacent frames are matched and associated. Based on the motion relationship, dynamic objects are identified and filtered out from the matched and associated point cloud clusters. Dynamic objects can be filtered out efficiently and accurately from the lidar point cloud. BRIEF DESCRIPTION OF THE DRAWINGS

[0046] Figure 1 is a schematic diagram of a method for filtering dynamic objects in a laser radar point cloud provided in an embodiment of the present application;

[0047] Figure 2 It is a schematic diagram of the principle of the laser radar point cloud segmentation and clustering method based on geometric constraints provided in an embodiment of the present application;

[0048] Figure 3 This is a method for filtering dynamic objects in a lidar point cloud provided in an embodiment of the present application, and a comparison chart of the quality of mapping results when dynamic objects are filtered out and when dynamic objects are not filtered out.

[0049] In order to better illustrate the present embodiment, some contents of the drawings may be omitted, which is only used for illustrative purposes and should not be construed as limiting the present application. DETAILED DESCRIPTION

[0050] In order to make the technical solution and advantages of the present application clearer, the technical solution of the present application will be described in further detail in detail and in detail with reference to the accompanying drawings. It can be understood that the specific embodiments described here are only partial embodiments of the present application, which are only used to explain the present application, not to limit the present application. It should be noted that, for the convenience of description, only the parts related to the present application are shown in the accompanying drawings, and other related parts can refer to the general design.

[0051] In addition, unless otherwise defined, the technical terms or scientific terms used in the description of this application shall have the usual meanings understood by those of ordinary skill in the art to which this application belongs. The words used in the description of this application to indicate orientation are only used to indicate relative directions or positional relationships. When the absolute position of the described object changes, its relative positional relationship may also change accordingly. The word "include" used in the description of this application indicates that the elements or objects appearing before the word include the elements or objects listed after the word and their equivalents, but does not exclude other elements or objects.

[0052] The present application embodiment provides a method for filtering dynamic objects in a laser radar point cloud, such as Figure 1 As shown, the method includes data preprocessing step, segmentation and clustering step, dynamic object coarse screening step, point cloud cluster matching and association step, and dynamic object filtering step.

[0053] Data preprocessing step: Preprocess the lidar point cloud.

[0054] During the LiDAR scanning process, the distortion caused by motion cannot be ignored. The point cloud obtained directly from the LiDAR cannot be used directly for data processing. It is necessary to correct the distortion by combining motion models or sensor motion, and use an odometer to correct the LiDAR point cloud.

[0055] Assume that the start and end time of a LiDAR scan are and During this period, a total of N laser beams P are emitted (p i , i=1,…,N), the time interval between each laser beam is Δt l In this process, the corresponding odometer start time is recorded, which are and The following relations are satisfied:

[0056]

[0057] Among them, Δt o is the time difference between adjacent odometer data.

[0058] Since the odometer time and the LiDAR scanning time are not synchronized, we first need to interpolate the odometer data to obtain the odometer pose at the corresponding LiDAR scanning time, as follows:

[0059]

[0060] Among them, i is the odometer at the laser radar scanning time t i The pose corresponding to the moment, Interp is the interpolation function.

[0061] Since the time interval between adjacent laser beams is very short, the motion can be regarded as uniform motion within this time range, so the odometer pose corresponding to each moment can be obtained by linear interpolation. Then, the lidar point cloud is preprocessed:

[0062] S11, obtain each laser beam p i , corresponding to the scanning time t i The odometer pose o i ;

[0063] S12, each odometer position o i The laser pose at the corresponding moment is converted to a unified coordinate system through coordinate transformation, and then converted to the laser coordinate system again through the coordinate transformation relationship, and the new pose is recorded as p i ′ , and obtain a new laser beam P ′ =(p i ′ ,i=1,2,…,N), republish.

[0064] Segmentation and clustering steps: Based on geometric constraints, the lidar point cloud is segmented and clustered to obtain a point cloud cluster set.

[0065] After motion distortion correction of the LiDAR point cloud, a scanning frame close to the real image is obtained. Through point cloud segmentation and clustering, different objects in the room can be separated, which is convenient for subsequent filtering of dynamic objects.

[0066] The present application embodiment provides a laser radar point cloud segmentation and clustering method based on geometric constraints, the principle of which is as follows: Figure 2 As shown in the figure, point O is the origin of the laser coordinate system, that is, the center position of the laser radar; A and B are the end points of two adjacent laser beams, where OA is the longer side; α is the laser radar measurement angle, that is, the angle between two adjacent laser beams; β is the angle between the line between the two adjacent laser scanning end points and OA. The α value is determined by the laser radar itself, and the β value depends on the OA distance d1 and the OB distance d2. When point A and point B do not belong to the same object, the β value is small.

[0067] The formula for calculating the β value is:

[0068] d1 is the distance value of the farther point, and d2 is the distance value of the closer point.

[0069] When point A and point B belong to the same object, the β value will not be very small. Therefore, an angle threshold β can be set for the β value. min , when the β value is less than the angle threshold β minThis method can accurately segment and cluster objects in most common scenarios. However, if the shape of the object is special, such as uneven, the β value may be small, but it still belongs to one object.

[0070] Therefore, in the laser radar point cloud segmentation and clustering method based on geometric constraints provided in the embodiment of the present application, in addition to judging the angle, the distance between the laser radar point clouds is judged. If the angle judgment is wrong, when the two point clouds belong to two objects, the distance between the two point clouds will be relatively large, that is, the distance d between point A and point B is relatively large. Therefore, a distance threshold d can be further set for the d value. max , when the d value is greater than the distance threshold d max , the two point clouds are located on different objects.

[0071] The d value is calculated as follows:

[0072]

[0073] Segment and cluster the lidar point cloud. The specific process is as follows:

[0074] S21, calculating β value;

[0075] S22, if the β value is not less than the angle threshold β min , indicating that the corresponding two adjacent point clouds are point clouds on the same object and are not segmented;

[0076] If the β value is less than the angle threshold β min , indicating that the corresponding two adjacent point clouds may not be point clouds on the same object, and meet the conditions for segmentation, and the d value needs to be further calculated;

[0077] If the d value is greater than the distance threshold d max , indicating that the corresponding two adjacent point clouds are not point clouds on the same object, and segmentation is performed;

[0078] If the d value is not greater than the distance threshold d max , indicating that the corresponding two adjacent point clouds are point clouds on the same object and are not segmented.

[0079] According to the above method, all laser beams are traversed, the point cloud is segmented and clustered, and a point cloud cluster set can be obtained.

[0080] Dynamic object rough screening step: Based on the number of point clouds at different distances, point cloud clusters are identified and dynamic objects are roughly screened.

[0081] In an indoor environment, most objects are static objects, and people are the main dynamic objects. Therefore, the size of people is within a certain range.

[0082] Roughly screen potential dynamic objects from the set of segmented and clustered point cloud clusters.

[0083] Transform the laser beam to the laser radar coordinate system. The transformation relationship is as follows:

[0084]

[0085] Among them, x i and i is the coordinate of the i-th laser beam after transformation, L i is the distance measured by the i-th laser beam, θ i is the angle of the i-th laser beam.

[0086] The point cloud formed when the laser hits a nearby object is dense, while the point cloud formed when it hits a distant object is scattered. Based on this characteristic, different point cloud quantity thresholds at different distances can be set to distinguish potential dynamic objects.

[0087] S31, calculating the distance L between the center of the point cloud cluster and the origin;

[0088] S32. If the distance L between the center of the point cloud cluster and the origin satisfies The point cloud clusters are roughly screened as dynamic objects, where N represents the number of point clouds, d0 represents the set segment distance, and h, i represent the threshold of the number of point clouds contained in the point cloud clusters within the corresponding segment distance range, that is, the maximum number of point clouds contained in each point cloud cluster.

[0089] With the above method, only the point cloud clusters that meet the basic requirements can be screened out, and whether they are dynamic objects cannot be determined. Further analysis is required to determine the real dynamic objects.

[0090] Point cloud cluster matching and association step: In adjacent frames, roughly screen the dynamic object point cloud clusters for matching and association.

[0091] Dynamic objects have different positions in the corresponding frames at different times. If they are to be filtered out, the point cloud clusters representing the same object in the previous and next adjacent frames must be matched and associated first, that is, the expressions of the same object in different frames must be matched and associated.

[0092] Between adjacent frames, the features of the same object change very little, but the corresponding features of dynamic objects change greatly. Therefore, the present application implements a method for roughly screening dynamic object point cloud clusters for matching and association as follows:

[0093] S41, calculating the number n of point clouds in the roughly screened dynamic object point cloud cluster in the current frame and the previous frame, the distance d from the center position to the laser origin, and the area s;

[0094] S42, calculate the similarity difference ρ of the roughly screened dynamic object point cloud clusters in the current frame and the previous frame, and set the similarity difference ρ less than the similarity difference threshold ρ max Roughly screen dynamic object point cloud clusters for matching and association.

[0095] The similarity difference between the i-th and j-th roughly screened dynamic object point cloud clusters in the current frame and the previous frame is ρ i,j , calculated as follows:

[0096]

[0097] in,

[0098] n (t,i) 、n (t-1,j) is the number of point clouds in the i-th and j-th coarsely screened dynamic object point cloud clusters in the current frame and the previous frame;

[0099] d (t,i) d (t-1,j) is the distance from the center of the i-th and j-th roughly screened dynamic object point cloud clusters to the laser origin in the current frame and the previous frame;

[0100] s (t,i) 、s (t-1,j) is the area of ​​the i-th and j-th roughly screened dynamic object point cloud clusters in the current frame and the previous frame;

[0101] a, b, and c are the weight coefficients for calculating the similarity difference of the point cloud clusters of roughly screened dynamic objects.

[0102] Dynamic object filtering step: Based on the motion relationship, dynamic objects are identified from the matching associated point cloud clusters and filtered out.

[0103] Considering the dynamic objects in the current frame and the previous frame in the same coordinate system, the position of the static objects will not change between the two frames, while the position of the dynamic objects will change between the two frames.

[0104] The odometer and laser time have been aligned before. Through the mileage transformation information between the two provided by the wheel odometer, the homogeneous transformation T at the current moment and the point cloud projected to the coordinate system at the end of the current scanning cycle can be obtained. Projecting any point into the laser radar coordinate system at the end of the previous scanning cycle can obtain a point cloud. The details are as follows:

[0105]

[0106] S51, calculating the center distance l of the matching associated point cloud cluster;

[0107] S52: If the center distance l of the matching associated point cloud cluster is less than the dynamic distance threshold D max , it is identified as a static object;

[0108] If the center distance l of the matching associated point cloud cluster is greater than the dynamic distance threshold D max , it is identified as a dynamic object and filtered out.

[0109] Republish the LiDAR point cloud with dynamic objects filtered out.

[0110] The method for filtering dynamic objects in the lidar point cloud disclosed in the above-mentioned embodiment, based on data preprocessing, segments and clusters the lidar point cloud based on geometric constraints to obtain a set of point cloud clusters, which can completely distinguish different objects, and then identify the point cloud clusters based on the number of point clouds at different distances, roughly screen dynamic objects, and match and associate the roughly screened dynamic object point cloud clusters in adjacent frames. Based on the motion relationship, dynamic objects are identified and filtered out from the matched and associated point cloud clusters, and dynamic objects can be efficiently and accurately filtered out from the lidar point cloud.

[0111] The method for filtering dynamic objects in the lidar point cloud disclosed in the above embodiment can filter out the influence of dynamic objects from the lidar point cloud source level. When applied to the mobile robot SLAM system, it can achieve complete decoupling of dynamic object filtering and the SLAM process, ensuring that a static point cloud without interference from dynamic objects is sent to the SLAM system. It can effectively reduce the negative impact of dynamic objects on the positioning accuracy and map construction quality of the SLAM system, and improve the accuracy of map construction in dynamic environments and the positioning accuracy of mobile robots.

[0112] In a specific example, a mobile robot SLAM system is simulated through ROS, and the method for filtering dynamic objects in the lidar point cloud disclosed in the above embodiment is written into a corresponding file. As one of the nodes under the ROS architecture, the lidar point cloud is received, and the dynamic objects are filtered out and then republished. The SLAM system receives the lidar point cloud after the dynamic objects are filtered out, and performs synchronous positioning and mapping.

[0113] The quality of the mapping results is compared between the method for filtering dynamic objects in the laser radar point cloud without filtering the dynamic objects in the laser radar point cloud disclosed in the above embodiment and filtering the dynamic objects. Figure 3 As shown in the following table, the positioning error comparison shows that based on the dynamic object filtering method in the lidar point cloud disclosed in the above embodiment, the dynamic objects are filtered out, the mapping quality is significantly improved, the "ghost" area is significantly reduced, and the positioning accuracy is significantly improved. Root mean square error Maximum error (cm) Minimum error (cm) Average error (cm) Not filtered out 7.496 17.072 0.472 6.315 Filter out 7.083 16.024 0.425 5.898 Error comparison -5.51％ -6.14％ -9.96％ -6.60％

[0114] So far, the technical solution of the present application has been described in conjunction with the preferred embodiments shown in the accompanying drawings. Those skilled in the art should understand that the scope of protection of the present application is obviously not limited to these specific embodiments. Without departing from the principles of the present application, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will fall within the scope of protection of the present application.

Claims

1. A method for filtering dynamic objects in a laser radar point cloud, characterized in that: include: Segmentation and clustering step: Based on geometric constraints, the lidar point cloud is segmented and clustered to obtain a point cloud cluster set; Dynamic object rough screening step: Based on the number of point clouds at different distances, point cloud clusters are identified to roughly screen dynamic objects; Cloud cluster matching and association step: roughly screen dynamic object point cloud clusters in adjacent frames for matching and association; Dynamic object filtering step: Based on the motion relationship, dynamic objects are identified from the matching associated point cloud clusters and filtered out.

2. The method for filtering dynamic objects in laser radar point cloud according to claim 1, characterized in that: The segmentation and clustering steps specifically include: S21, calculating β value; S22, if the β value is not less than the angle threshold β min , then the adjacent point clouds are not segmented; If the β value is less than the angle threshold β min , then calculate the d value; If the d value is greater than the distance threshold d max , then the adjacent point clouds are segmented; If the d value is not greater than the distance threshold d max , then the adjacent point clouds are not segmented; in, β is the angle between the line between two adjacent laser scanning end points and the longer laser beam; d is the distance between two adjacent laser scanning end points.

3. The method for filtering dynamic objects in laser radar point cloud according to claim 1, characterized in that: The steps of rough screening of dynamic objects specifically include: S31, calculating the distance L between the center of the point cloud cluster and the origin; S32. If the distance L between the center of the point cloud cluster and the origin satisfies The point cloud cluster is roughly screened as a dynamic object, where N is the number of point clouds in the point cloud cluster, d0 is the set segment distance, and h, i are the thresholds of the number of point clouds contained in the point cloud cluster within the corresponding segment distance range.

4. The method for filtering dynamic objects in a laser radar point cloud according to claim 1, characterized in that: The point cloud cluster matching association steps include: S41, calculating the number n of point clouds in the roughly screened dynamic object point cloud cluster in the current frame and the previous frame, the distance d from the center position to the laser origin, and the area s; S42, calculate the similarity difference ρ of the roughly screened dynamic object point cloud clusters in the current frame and the previous frame, and set the similarity difference ρ less than the similarity difference threshold ρ max Roughly screen dynamic object point cloud clusters for matching and association. The similarity difference between the i-th and j-th roughly screened dynamic object point cloud clusters in the current frame and the previous frame is ρ i,j , calculated as follows: in, n (t,i) 、n (t-1,j) is the number of point clouds in the i-th and j-th coarsely screened dynamic object point cloud clusters in the current frame and the previous frame; d (t,i) d (t-1,j) is the distance from the center of the i-th and j-th roughly screened dynamic object point cloud clusters to the laser origin in the current frame and the previous frame; s (t,i) 、s (t-1,j) is the area of ​​the i-th and j-th roughly screened dynamic object point cloud clusters in the current frame and the previous frame; a, b, and c are the weight coefficients for calculating the similarity difference of the point cloud clusters of roughly screened dynamic objects.

5. The method for filtering dynamic objects in laser radar point cloud according to claim 1, characterized in that: Dynamic object filtering steps include: S51, calculating the center distance l of the matching associated point cloud cluster; S52: If the center distance l of the matching associated point cloud cluster is less than the dynamic distance threshold D max , it is identified as a static object; If the center distance l of the matching associated point cloud cluster is greater than the dynamic distance threshold D max , it is identified as a dynamic object and filtered out.

6. The method for filtering dynamic objects in laser radar point cloud according to claim 1, characterized in that: Also includes: Data preprocessing steps: Preprocess the lidar point cloud, including: S11, obtain each laser beam p i , corresponding to the scanning time t i The odometer pose o i ; S12, each odometer position o i The laser pose at the corresponding moment is converted to a unified coordinate system through coordinate transformation, and then converted to the laser coordinate system again through the coordinate transformation relationship, and the new pose is recorded as p i ′ , and obtain a new laser beam P ′ =(p i ′ ,i=1,2,…,N).

Images