2D laser radar point cloud filtering algorithm

Abstract

The invention relates to the technical field of laser radars, and particularly provides a 2D laser radar point cloud filtering algorithm. The method comprises the following steps: setting a filtering level theta, wherein theta is the slope of an object irradiated by a radar beam; periodically acquiring point cloud data, wherein each circle of point cloud comprises N points; filtering the point cloud of each circle from the ith point in sequence, and calculating the tolerance range of the ith point; calculating the number of the i-th point, the first n points of the i-th point and the last n points of the i-th point in the tolerance range; when the number of the points in the tolerance range is greater than or equal to a preset number, the ith point is an effective point; when the number of the points in the tolerance range is smaller than the preset number, the ith point is an invalid point, the numerical value of the ith point is set to be infinite, and the method effectively eliminates noisy points and restores the real contour of the scanned object.

Description

Technical Field

[0001] The present invention relates to the field of laser radar technology, and in particular to a 2D laser radar point cloud filtering algorithm. Background Art

[0002] Ideally, the laser radar emits a straight line with a point cross section. However, due to the divergence angle of the laser, the installation position, the collimating lens surface, and the slight differences in the structure, the laser beam will diffuse and the cross section will be circular or rectangular with a certain area. Figure 1 As shown in the figure, if the light beam hits the edge of an object, or two objects in front and behind, the return light will become stronger or weaker, causing the measured distance to increase or decrease, forming front and back jump points, and the true outline of the scanned object cannot be restored. Random errors generated when measuring distance, intrusion of flying insects or dust particles, etc., will also distort the true outline of the scanned object. Summary of the invention

[0003] In view of this, the present invention provides a 2D laser radar point cloud filtering algorithm to eliminate front jump points and back jump points and restore the true outline of the scanned object.

[0004] In a first aspect, the present invention provides a 2D laser radar filtering algorithm, the method comprising:

[0005] Step 1, set the filtering level θ, where θ is the slope of the radar beam illuminating the object;

[0006] Step 2: Periodically obtain point cloud data, each circle of point cloud has N points;

[0007] Step 3: The point cloud of each circle is filtered from the i-th point in turn, and the tolerance range of the i-th point is calculated, where i is a positive integer;

[0008] Step 4: Calculate the number of points within the tolerance range from the inth point to the i+nth point, a total of 2n+1 points; in represents the n points before the i-th point, i+n represents the n points after the i-th point; the value range of i is [n+1,Nn];

[0009] Step 5. When the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is a valid point; when the number of points within the tolerance range is less than the preset number m, the i-th point is an invalid point, and the value of the i-th point is set to infinity; the i+1th point is taken as the i-th point, and the point cloud of each circle is filtered in sequence from the i-th point to calculate the tolerance range of the i-th point, where i is a positive integer.

[0010] Optionally, step 1 includes:

[0011] Fix the position of the reflective tape test piece, do not place any shielding objects behind the left edge of the test piece, and place a white board at the position where the noise points are generated behind the right edge of the test piece; fix the test piece and the white board, and adjust the measurement distance between the radar and the test piece forward and backward to make the maximum number of noise points appear. At this time, the number of noise points is m; set the filtering level θ and the number of points involved in filtering n from small to large, and get the minimum value that makes the noise points disappear.

[0012] Optionally, step 3 includes:

[0013] Take the i-th point of the point cloud, assume that the measurement distance between the radar and the test piece is D, the filtering level is θ, and the tolerance value is L. When the 2n+1 points before and after the i-th point are all within the error range L, that is, the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is considered to be a valid point; D1, D2, a, and b are auxiliary lines, and D = D1 + D2; L is obtained by trigonometric calculation, and its expression is:

[0014] L=(tan(nγ) / (tan(θ)-tan(nγ)))*D;

[0015] Here, γ is the angular resolution.

[0016] Optionally, step 4 includes:

[0017] The filtering level is θ. When the slope of the radar beam irradiating the object is greater than or equal to θ, all 2n+1 points from the inth point to the i+nth point will be within the range of D±L. When the slope of the radar beam irradiating the object is less than θ, at most 2n-1 points will be within the range of D±L.

[0018] Optionally, step 5 includes:

[0019] Get the distance D of the i-th point. If there are greater than or equal to m points in the range of D±L among the 2n+1 points from the in-th point to the i+n-th point, the current point is a valid point. If there are less than m points in the range of D±L among the 2n+1 points, the i-th point is an invalid point and the value of the i-th point is set to infinity.

[0020] The i+1th point is taken as the ith point, and the point cloud of each circle is filtered in sequence from the ith point, and the tolerance range of the ith point is calculated, where i is a positive integer.

[0021] Optionally, obtaining L according to trigonometric calculation includes:

[0022] a= D2*tan(nγ) (1);

[0023] b=(D+L)*tan(nγ) (2);

[0024] a / D1=b / L (3);

[0025] D1 / D2=tan(nγ) / tan(θ) (4);

[0026] From formulas (1), (2) and (3), we can get:

[0027] D1 / D2=L / (L+D) (5);

[0028] From formula (4) and (5), we can get:

[0029] tan(nγ) / (tan(θ)=L / (L+D) (6);

[0030] From formula (6), we can get:

[0031] L=(tan(nγ) / (tan(θ)-tan(nγ)))*D;

[0032] From this, the relationship between the tolerance value L and the measured distance D can be obtained.

[0033] In the technical solution provided by the present invention, the method includes setting a filtering level θ, where θ is the slope of the radar beam irradiating the object; periodically acquiring point cloud data, each circle of point cloud is N points; filtering each circle of point cloud from the i-th point in turn, and calculating the tolerance range of the i-th point; calculating the number of points within the tolerance range of the i-th point and the n points before and after the n points; when the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is a valid point; when the number of points within the tolerance range is less than the preset number m, the i-th point is an invalid point, and the value of the i-th point is set to infinity. The method effectively eliminates noise points and restores the true contour of the scanned object. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0035] Figure 1 A schematic diagram of noise points provided by an embodiment of the present invention;

[0036] Figure 2 A flowchart of a laser radar point cloud filtering algorithm provided by an embodiment of the present invention;

[0037] Figure 3 A schematic diagram of filter level calibration provided by an embodiment of the present invention;

[0038] Figure 4 A schematic diagram of a point cloud filtering algorithm provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are 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.

[0040] It should be clear that 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.

[0041] The terms used in the embodiments of the present invention are only for the purpose of describing specific embodiments, and are not intended to limit the present invention. The singular forms "a", "said" and "the" used in the embodiments of the present invention are also intended to include plural forms, unless the context clearly indicates other meanings.

[0042] It should be understood that the term "and / or" used in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship.

[0043] The word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining" or "in response to detecting", depending on the context. Similarly, the phrases "if it is determined" or "if (stated condition or event) is detected" may be interpreted as "when it is determined" or "in response to determining" or "when detecting (stated condition or event)" or "in response to detecting (stated condition or event)", depending on the context.

[0044] Figure 2 The flowchart of the laser radar point cloud filtering algorithm provided by the embodiment of the present invention is as follows: Figure 2 As shown, the method includes:

[0045] Step 1: Set the filter level θ, where θ is the slope of the radar beam illuminating the object.

[0046] In the embodiment of the present invention, the reflective tape test piece is fixed, no shielding is placed behind the left edge of the test piece, and a white board is placed at the position where the noise point is generated behind the right edge of the test piece. Figure 1 As shown, the image formed at this time; Figure 3 As shown, the test piece and the whiteboard are fixed, and the measuring distance between the radar and the test piece is adjusted forward and backward to make the number of forward and backward jump points the largest. The filtering levels are set from small to large in sequence to obtain the minimum value that makes the noise points disappear.

[0047] In some embodiments, the noise points include front jump points, back jump points, intrusion of flying insects and dust, and random errors generated when measuring distance.

[0048] Step 2: Periodically obtain point cloud data. Each circle of point cloud has N points in total.

[0049] Step 3: Each circle of point cloud is filtered from the i-th point in turn, and the tolerance range of the i-th point is calculated, where i is a positive integer.

[0050] In the embodiment of the present invention, step 3 includes:

[0051] Take the i-th point of the point cloud, assume that the measurement distance between the radar and the test piece is D, the filtering level is θ, and the tolerance value is L. When the 2n+1 points before and after the i-th point are all within the error range L, that is, the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is considered to be a valid point; D1, D2, a, and b are auxiliary lines, and D = D1 + D2; L is obtained by trigonometric calculation, and its expression is:

[0052] L=(tan(nγ) / (tan(θ)-tan(nγ)))*D;

[0053] Here, γ is the angular resolution.

[0054] In the embodiment of the present invention, Figure 4 As shown, the trigonometric calculation to obtain L includes:

[0055] a= D2*tan(nγ) (1);

[0056] b=(D+L)*tan(nγ) (2);

[0057] a / D1=b / L (3);

[0058] D1 / D2=tan(nγ) / tan(θ) (4);

[0059] From formulas (1), (2) and (3), we can get:

[0060] D1 / D2=L / (L+D) (5);

[0061] From formula (4) and (5), we can get:

[0062] tan(nγ) / (tan(θ)=L / (L+D) (6);

[0063] From formula (6), we can get:

[0064] L=(tan(nγ) / (tan(θ)-tan(nγ)))*D;

[0065] From this, the relationship between the tolerance value L and the measured distance D can be obtained.

[0066] Step 4: Calculate the number of points within the tolerance range from the inth point to the i+nth point, a total of 2n+1 points; in represents the n points before the i-th point, i+n represents the n points after the i-th point; the value range of i is [n+1,Nn].

[0067] In an embodiment of the present invention, the filtering level is θ. When the slope of the radar beam irradiating the object is greater than or equal to θ, the inth point, the i-n+1th point, ..., the i-th point, ..., the i+n-1th point, the i+nth point, a total of 2n+1 points will all be within the range of D±L; when the slope of the radar beam irradiating the object is less than θ, at most 2n-1 points will be within the range of D±L.

[0068] Step 5. When the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is a valid point; when the number of points within the tolerance range is less than the preset number m, the i-th point is an invalid point, and the value of the i-th point is set to infinity; the i+1th point is taken as the i-th point, and the point cloud of each circle is filtered in sequence from the i-th point to calculate the tolerance range of the i-th point, where i is a positive integer.

[0069] In the embodiment of the present invention, the physical meaning of m is that at most m noise points clustered together can be filtered out, and m<2n+1.

[0070] In the embodiment of the present invention, step 5 includes:

[0071] Get the distance D of the i-th point. If there are greater than or equal to m points in the range of D±L among the in-th point, i-n+1-th point, ..., i-th point, ..., i+n-1-th point, i+n-th point, a total of 2n+1 points, then the current point is a valid point; if there are less than m points in the range of D±L among the 2n+1 points, then the i-th point is an invalid point, and the value of the i-th point is set to infinity;

[0072] The i+1th point is taken as the ith point, and the point cloud of each circle is filtered in sequence from the ith point, and the tolerance range of the ith point is calculated, where i is a positive integer.

[0073] In the embodiment of the present invention, when the radar beam irradiates an object whose slope is less than an angle θ, the point is an invalid point and is set to be infinite.

[0074] The present invention performs filtering according to the point cloud density, and according to the characteristics of the point cloud being dense near and sparse far away, the tolerance value range is adaptively adjusted, that is, the density lower limit is automatically adjusted to achieve the purpose of filtering according to density.

[0075] In the technical solution provided by the present invention, the method includes setting a filtering level θ, where θ is the slope of the radar beam irradiating the object; periodically acquiring point cloud data, each circle of point cloud is N points; filtering each circle of point cloud from the i-th point in turn, and calculating the tolerance range of the i-th point; calculating the number of points within the tolerance range of the i-th point and the n points before and after the n points; when the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is a valid point; when the number of points within the tolerance range is less than the preset number m, the i-th point is an invalid point, and the value of the i-th point is set to infinity. The method effectively eliminates noise points and restores the true contour of the scanned object.

[0076] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0077] 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 2D laser radar point cloud filtering algorithm, characterized in that: The method comprises: Step 1, set the filtering level θ, where θ is the slope of the radar beam illuminating the object; Step 2: Periodically obtain point cloud data, and each circle of point cloud has N points in total; Step 3: The point cloud of each circle is filtered from the i-th point in turn, and the tolerance range of the i-th point is calculated, where i is a positive integer; Step 4: Calculate the number of points within the tolerance range from the inth point to the i+nth point, a total of 2n+1 points; in represents the n points before the i-th point, i+n represents the n points after the i-th point; the value range of i is [n+1,Nn]; Step 5. When the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is a valid point; when the number of points within the tolerance range is less than the preset number m, the i-th point is an invalid point, and the value of the i-th point is set to infinity; the i+1th point is taken as the i-th point, and the point cloud of each circle is filtered in sequence from the i-th point to calculate the tolerance range of the i-th point, where i is a positive integer.

2. The method according to claim 1, characterized in that The step 1 comprises: Fix the position of the reflective tape test piece, do not place any shielding objects behind the left edge of the test piece, and place a white board at the position where the noise points are generated behind the right edge of the test piece; fix the test piece and the white board, and adjust the measurement distance between the radar and the test piece forward and backward to make the maximum number of noise points appear. At this time, the number of noise points is m; set the filtering level θ and the number of points involved in filtering n from small to large, and get the minimum value that makes the noise points disappear.

3. The method according to claim 1, characterized in that The step 3 comprises: Take the i-th point of the point cloud, assume that the measurement distance between the radar and the test piece is D, the filtering level is θ, and the tolerance value is L. When the 2n+1 points before and after the i-th point are all within the error range L, that is, the number of points within the tolerance range is greater than or equal to the preset number m, the i-th point is considered to be a valid point; D1, D2, a, and b are auxiliary lines, and D = D1 + D2; L is obtained by trigonometric calculation, and its expression is: L=(tan(nγ) / (tan(θ)-tan(nγ)))*D; Here, γ is the angular resolution.

4. The method according to claim 1, characterized in that: The step 4 comprises: The filtering level is θ. When the slope of the radar beam irradiating the object is greater than or equal to θ, all 2n+1 points from the inth point to the i+nth point will be within the range of D±L. When the slope of the radar beam irradiating the object is less than θ, at most 2n-1 points will be within the range of D±L.

5. The method according to claim 1, characterized in that The step 5 comprises: Get the distance D of the i-th point. If there are greater than or equal to m points in the range of D±L among the 2n+1 points from the in-th point to the i+n-th point, the current point is a valid point. If there are less than m points in the range of D±L among the 2n+1 points, the i-th point is an invalid point and the value of the i-th point is set to infinity. The i+1th point is taken as the i-th point, and the point cloud of each circle is filtered in sequence from the i-th point, and the tolerance range of the i-th point is calculated, where i is a positive integer.

6. The method according to claim 3, characterized in that: The trigonometric calculations to obtain L include: a= D2*tan(nγ) (1); b=(D+L)*tan(nγ) (2); a / D1=b / L (3); D1 / D2=tan(nγ) / tan(θ) (4); From formulas (1), (2) and (3), we can get: D1 / D2=L / (L+D) (5); From formula (4) and (5), we can get: tan(nγ) / (tan(θ)=L / (L+D) (6); From formula (6), we can get: L=(tan(nγ) / (tan(θ)-tan(nγ)))*D; From this, the relationship between the tolerance value L and the measured distance D can be obtained.

Images