CN112433198A

CN112433198A - Method for extracting plane from three-dimensional point cloud data of laser radar

Info

Publication number: CN112433198A
Application number: CN201910786166.3A
Authority: CN
Inventors: 王远鹏; 张二阳; 葛凯
Original assignee: Suzhou Xunji Zhixing Robot Technology Co ltd
Current assignee: Suzhou Xunji Zhixing Robot Technology Co ltd
Priority date: 2019-08-24
Filing date: 2019-08-24
Publication date: 2021-03-02
Anticipated expiration: 2039-08-24
Also published as: CN112433198B

Abstract

The invention belongs to the technical field of artificial intelligence, and particularly discloses a method for extracting a plane from three-dimensional point cloud data of a laser radar, which comprises the following steps: s1, layering 3D point cloud data obtained by scanning of the multi-line laser radar; s2, extracting straight lines from the 2D point cloud data of each plane; s3, extracting face candidates from the plane straight line set obtained by all the planes; and S4, solving a plane equation by using a least square method according to the straight line end points in each group of plane straight line set, and extracting a plane in the three-dimensional point cloud data. The method can extract the straight lines from the 2D laser data of each layer more quickly, can extract the 3D plane by using all the straight lines of each layer, and has the advantages of high efficiency and no need of any priori knowledge about actual terrain. The 3D plane acquired by the method can be used for correcting single laser data falling in the plane, so that the fluctuation of the single laser data can be greatly reduced, and the reliability of the laser data is improved.

Description

Method for extracting plane from three-dimensional point cloud data of laser radar

Technical Field

The invention belongs to the technical field of artificial intelligence, and particularly relates to a method for extracting a plane from 3D data of a laser radar. The typical characteristic of the 3D laser data acquired by the multiline lidar is that one frame of 3D data is actually a combination of a plurality of sets of 2D laser data with different pitch angles.

Background

At present, due to the high cost of the 3D laser radar, the technical data about the 3D laser radar data preprocessing is less, and more is the 3D laser data processing technology. Although the existing 3D data processing technology can extract features from data, the features are often referred to as 3D objects, that is, the features include geometric information of the actual object and give the actual meaning of the object, such as extracting a car, a house, a pedestrian, and the like. Although these very powerful algorithms extract such rich information from the 3D laser data, they do not have the technical effort to improve the data quality itself, such as the repeated measurement accuracy, and so on, and have more strict requirements on the algorithms, either requiring very large resource consumption, or being based on artificial intelligence algorithms such as neural networks and so on that need to provide the required parameters in advance.

Still another document starts from the huge storage required by 3D data, and tries to approximate 3D data points by using a plane to reduce the storage of data, and mainly performs plane approximation on 3D laser data scanning a known (simple) environment (such as an empty room) by using an EM algorithm, converts the massive 3D data into several simple planes, and performs optimization by using the EM algorithm to obtain a good precision of the planes, for example, the situation that the front and back planes of the room are not parallel, which may exist in the former algorithm, does not occur. However, as for the application scenario, to use this algorithm, the general structure of the environment needs to be known in advance, and the environment cannot be too complex, and although the extracted surface can assist in increasing the confidence of the data, the implementation manner is still quite limited.

The 3D hough transform is another technique for extracting planes from 3D laser data. Direct 3D Hough transform extends 2D Hough transform to 3D, at discretized θ and

then use

For each point (x, y, z) at each discretized sum of θ

Middle voting, counting each pair after traversing all points

Number of tickets in (1), the ticket being deemed to exceed the threshold

Pairs represent a plane. The optimized 3D Hough transform preprocesses all data points first, the method is to obtain the normal vector of each point according to each point and the point near to the point, and only the normal vector represents

And voting is carried out. Whether a direct 3D hough transform or an optimized 3D hough transform, the temporal complexity is very high, or it experiences all the laser points in its traversal

The pair either needs to correctly compute the normal vector from the nearby points in the pre-processing. In addition, if the plane information needs to be extracted from the plane obtained by using the 3D hough transform, additional operations are required.

Disclosure of Invention

In view of the above, an object of the present invention is to solve the problems of the prior art, and provide a method for extracting a plane from 3D point cloud data of a laser radar, which can correct individual laser data falling within the plane without any a priori knowledge about actual terrain, and provide reliability of the laser data.

In order to achieve the purpose, the invention adopts the following technical scheme:

a method for extracting a plane from three-dimensional point cloud data of a laser radar is characterized by comprising the following steps:

s1, layering 3D point cloud data obtained by scanning of the multi-line laser radar, wherein each layer comprises a group of 2D point cloud data representing a plane, and the group of 2D point cloud data is scanned by the laser radar at intervals of a fixed angle delta_hThe measurement is carried out for one time to obtain;

s2, extracting straight lines from the 2D point cloud data of each plane, specifically: when the absolute value of the difference between two adjacent 2D point cloud data is less than c of the two data_maxThen the two adjacent 2D point cloud data belong to the same straight line, c_maxConverting distance values a and b in two 2D point cloud data into 2D coordinates, storing the 2D coordinates into a linear coordinate set, linearly fitting the 2D coordinates in the linear coordinate set after the 2D coordinates in the linear coordinate set reach a preset number to obtain a straight line, and storing parameters of the straight line, wherein the parameters comprise an open angle interval of a starting position to an origin O and two end points of the straight line;

s3, extracting the candidates of the plane from the plane straight line set obtained from all the planes, specifically: firstly, the minimum layer number N of a judgment plane is determined according to the attribute of the laser radar_thresholdTaking a straight line set in the top 2D laser plane, recording an angle interval occupied by each straight line, and storing each interval as a plane straight line set; traversing the rest 2D laser planes from top to bottom, screening all plane linear sets, and deleting angle intervals with the number smaller than N_thresholdThe set of planar lines of (a);

and S4, solving a plane equation by using a least square method according to the straight line end points in each group of plane straight line set, and extracting a plane in the three-dimensional point cloud data.

The main function of the invention is to extract planes from 3D laser data, which also includes an algorithm for extracting lines from 2D laser data. Compared with the prior art, the method can extract straight lines from each layer of 2D laser data more quickly, and can extract a 3D plane by using all the straight lines of each layer, wherein any priori knowledge about actual terrain is not needed. One direct use of the acquired 3D plane is to correct individual laser data falling within the plane, i.e. to modify the laser data (distance value) in the corresponding direction to the distance between the corresponding direction and the intersection of the plane, since the plane contains several laser data points, the fluctuation of the individual laser data can be greatly reduced, and the reliability of these corrected laser data is thus much higher than that of the laser data that has not been corrected.

Drawings

FIG. 1 is a schematic diagram of the present invention extracting planes from 2D laser data of each plane.

Detailed Description

The invention is further illustrated by the following figures and examples.

The invention discloses a method for extracting a plane from three-dimensional point cloud data of a laser radar, which takes a multiline laser radar to scan the surrounding environment as an example and comprises the following steps.

(one) extracting straight lines from the (2D) laser data of each plane

S1, layering 3D point cloud data obtained by scanning of the multi-line laser radar, wherein each layer comprises a group of 2D point cloud data representing a plane, and the group of 2D point cloud data is scanned by the laser radar at intervals of a fixed angle delta_hIs obtained by carrying out one measurement. In 3D multiline lidar, each layer acquires a set of 2D data, which is typically measured every small angle, which is the (planar) angular resolution, denoted δ_hRepresents δ_horitonal. For a typical 3D multiline radar,

such as delta_h＝0.5°。

S2, extracting straight lines from the 2D point cloud data of each plane, specifically: when the absolute value of the difference between two adjacent 2D point cloud data is less than c of the two data_maxThen the two adjacent 2D point cloud data belong to the same straight line, c_maxTwo adjacent laser beams are irradiated on two points A, B on the obstacle during the measurement of the laser radar with the origin point OThe maximum linear distance of the line segment AB between the two 2D point cloud data is converted into 2D coordinates according to the distance values a and b in the two 2D point cloud data, the 2D coordinates are stored in a linear coordinate set, after the 2D coordinates in the linear coordinate set reach a preset number, the 2D coordinates in the linear coordinate set are linearly fitted to obtain a straight line, and the parameters of the straight line are stored, wherein the parameters comprise an angle interval formed by the starting position and the stopping position relative to the original point O and two end points of the straight line.

The range value output by the laser radar generally has a minimum value, and the minimum value has two meanings, namely, the lower limit of the laser range (data smaller than the range does not exist), or the laser data smaller than the range can not ensure the accuracy. And since the lidar is often mounted in the center of the device, or for reasons of lidar protection, there is also a minimum distance between the lidar and the obstacle, unless it is the more extreme way of mounting the lidar (e.g., on the left and right boundaries), which is also a considerable amount. Taking the larger of the two minimum values as l₀According to experience, take l₀＝0.5m。

As shown in fig. 1, in Δ OAB, point O represents the position of the laser radar, and OA and OB are two adjacent laser paths emitted by the laser radar on a certain plane, so ═ AOB ═ δ_h(ii) a The line segment AB is a part of the obstacle, which is assumed to be the distance between the lidar and the line AB

Note the book

Then in Δ OAB, according to the cosine theorem, there are

c²＝a²+D²-2ab cosδ_h

At the same time according to l ≥ l₀Is provided with

l₀≥a sinθ

Plus the trigonometric identity

cos²θ+sin²θ＝1

Can be obtained by three simultaneous methods

Taking into account delta_hVery small, with a ≈ b + c, i.e.

That is, if two adjacent laser data form a straight line, the difference between the two data cannot exceed c at most_max. According to the convention before, δ_h＝0.5°，l₀0.5m, the proportionality coefficient

So utilize c_maxWhether the adjacent laser data belong to the same straight line or not can be judged very quickly.

Using c_maxThe pseudo code (flow) of the algorithm to extract the straight line from the 2D laser data is as follows:

the above algorithm is intended to be simple, so there are certain drawbacks that it is not possible to identify a broken line (treated equally as a straight line, then considered too diffuse to be discarded), nor two parallel lines that are very close together. The straight line that it can handle is a line segment that is far apart between end points, such as the boundary of different objective subjects.

For this drawback of the present algorithm, the following ways can be used to compensate:

A) on line 8, the sum of absolute values c of the differences between two adjacent data (distance values) is passed_maxThe comparison method is changed into the first judgmentSign of difference between two data, and directly summing the difference between two data_maxFor comparison. The distance between the point of interest and a fixed point outside the straight line is regular as the point of interest gradually moves from one end point of the line segment to the other end point, the distance between the point of interest and the fixed point outside the straight line is smaller as the point of interest is closer to the foot of the point outside the line on the straight line, namely when the point of interest is far away from the foot of the line, the distance is monotonously changed, so that the absolute value can be completely eliminated on the premise.

B) The rough parameters of this line can be determined first from the first few points (e.g. the first 10 2D coordinate points) and then c here_maxIt can be further calculated from the angle between the line and the straight line between the point of interest and the fixed point, which is a predictive technique, and to calculate c more accurately in this way_maxAnd parameters of the straight line can be updated in real time.

C) These points will be discarded directly after considering too much diffusion, but it is quite possible to try to analyze these 2D coordinate points one by one: the method comprises the steps of firstly using the first 10 (1 st to 10 th) 2D coordinate points as an initial straight line point set to determine the rough parameters of the straight line (if the 10 coordinate points are already dispersed, extending to the 2 nd to the 11 th, repeating until the proper 10 points are found or reaching the head), then trying to put each subsequent point into the straight line one by one, if the dispersion degree of all the 2D coordinate points after adding the point is not obviously increased, considering the point to belong to the straight line, and if the dispersion degree is obviously increased, the points before the point are all the points of the straight line, so that a part of the coordinate points are extracted from the dispersed points and form a straight line. This point and the next 9 points are then collected as the next initial set of straight line points, and the loop. After all the dispersion points are traversed, for each straight line, forward continuation is tried again, and if a point before a large dispersion point occurs in the previous straight line is tried to be placed in the next straight line, the dispersion degree of the data points in the two straight lines cannot be reduced at the same time.

However, these methods tend to have a relatively large time consumption, which conflicts with the high efficiency proposed by the present algorithm.

(II) extracting candidates of the plane from the straight line set obtained from all the M planes

And S3, extracting the candidates of the plane from the plane straight line set obtained by all the planes. This step can be very simple and can also be made very comprehensive. A simple way of looking more at the boundary level or near level is used here. This approach requires introducing a structure, a "plane line set", whose main body is a set of saved angle intervals, and the angle intervals are used to describe lines in a certain 2D plane, including the inclination angle of the 2D plane and the angle range of the lines open to the origin. Before the algorithm is operated, the minimum layer number N of the judgment plane is firstly determined according to the attributes (the number of threads and the resolution of the pitching angle) of the laser radar_thresholdIn the sense that only when a plane is actually physical at least by N_thresholdLayers may not be identified as facets until after laser detection, and facets below this threshold are ignored. In consideration of the third step, N is set_thresholdWhen, N is required to be satisfied_threshold≥3。

The straight lines obtained in (one) all reserve the occupied angle, so that the operation can be carried out as follows:

taking a straight line set in the top 2D laser plane, recording an angle interval occupied by each straight line, and storing each interval as a plane straight line set;

traversing the remaining 2D laser planes from top to bottom;

checking all plane straight line sets, if a certain set does not contain straight lines in adjacent planes above the current plane, identifying the plane straight line set as inactive, otherwise, identifying as active;

for each straight line in the current plane, taking an angle interval of the straight line, comparing the angle interval with the angle interval at the lowest part in all active plane straight line sets, comparing the overlapping intervals of the straight lines, and putting the straight line into the plane straight line set with the largest overlapping angle interval;

screening all plane linear sets, and deleting angle intervals with the number smaller than N_thresholdThe set of planar lines of (a).

In fact, this method has some problems due to its simplicity, such as difficulty in detecting a plane whose edge is not horizontal (e.g., a square plane whose boundary and horizontal plane form an angle of 45 °, and whose lowermost portion is only one point), and due to only setting a fixed N_thresholdTherefore, the recognition rate for relatively distant surfaces is relatively low, and the like. Although these problems can be solved (e.g. by extending already determined faces to find faces that are not horizontal boundaries, or by extending N_thresholdSet inversely proportional to distance, etc.), where this most straightforward approach is chosen for simplicity.

(III) calculating plane parameters according to each group of plane straight line sets

And S4, solving a plane equation by using a least square method according to the straight line end points in each group of plane straight line set, and extracting a plane in the three-dimensional point cloud data. In a plane line set (in the form of two end points thereof), a plurality of lines which should belong to the same plane are stored, and the problem is that the plane equation needs to be solved by using the lines.

Since the 3D lidar scans at a pitch angle, the plane equation that it can scan may take the form of Ax + By + D0, but these planes should not be horizontal planes, i.e. the plane equation Ax + By + Cz + D0 where a and B cannot be zero at the same time, so that the plane can be formulated as a plane

x＝By+Cz+D

Or

y＝Ax+Cz+D

Thus, the plane equation, i.e. the resulting function, can be solved using the least squares method

Or

(B, C, D) or (A, C, D) at the minimum, where the sum extends over all 3D coordinate points. Which equation to choose from can be directly calculated from the distribution of the 3D coordinates.

Also to extract the 3D coordinate points from the line equations, consider the two end points of each line (segment) because the end points can give the largest residual to the plane. One straight line can be determined due to two points in space, namely, each straight line corresponds to residual error

The contribution of (1) is only two points, and other points on the straight line are only linear combinations of the end points, so that the addition of other points on a line segment except the two end points does not have too great contribution.

After the above (one), (two) and (three), the plane in the 3D laser data is extracted, so that the laser data itself contained therein can be corrected, specifically, the original laser distance is covered by the distance from the origin to the plane in the azimuth of the corresponding laser data. Such a correction of the laser distance does not cause too large a variation in the laser distance value because the fluctuation of the laser data is limited. Since the laser data has a very high accuracy of orientation, and the plane equation is obtained from several data, the corrected laser data has a higher accuracy (lower accuracy of repeated measurement) than the original data.

Compared with the prior art, the invention has the following advantages:

1. the algorithm is only directed at a very common and very simple three-dimensional structure, namely a plane, and is easy and convenient to implement.

2. The efficiency is high. The algorithm for extracting the straight line has good efficiency and has the time complexity of O (N); respectively extracting straight lines from the laser data of different planes, wherein the straight lines can be calculated in parallel; and the way of acquiring the plane by using the straight line is simpler.

3. No a priori knowledge about the actual terrain is required.

The above embodiments are merely illustrative of the technical ideas and features of the present invention, and the purpose thereof is to enable those skilled in the art to understand the contents of the present invention and implement the present invention, and not to limit the protection scope of the present invention. All equivalent changes and modifications made according to the spirit of the present invention should be covered within the protection scope of the present invention.

Claims

1. A method for extracting a plane from three-dimensional point cloud data of a laser radar is characterized by comprising the following steps:

2. The method of extracting a plane from three-dimensional point cloud data of lidar according to claim 1, wherein the fixed angle δ 1 is defined as_hIs the pitch resolution of the lidar.

3. Method for extracting a plane from three-dimensional point cloud data of a lidar according to claim 2, wherein δ_h＝0.5°。

4. The method of extracting a plane from the three-dimensional point cloud data of lidar according to claim 1, wherein the predetermined number is at least 10 in step S2.

5. The method of claim 1, wherein in step S3, N is_threshold≥3。