CN108460416A - A kind of structured road feasible zone extracting method based on three-dimensional laser radar - Google Patents

Abstract

The invention discloses a method for extracting a feasible region of a structured road based on a three-dimensional laser radar. The method comprises the following steps: 1) the three-dimensional laser radar scans the surrounding environment, obtains point cloud information of the surrounding environment, and obtains the point cloud information from the laser radar Convert the coordinate system to the local Cartesian coordinate system; 2) Extract the 3D lidar data points of interest; 3) Extract the lidar scanning single line by using the method of radar detection angle clustering; 4) Use the K-means clustering algorithm to obtain the optimal cluster Point set; 5) Using height features to extract obstacles; 6) Combined with DBSCAN clustering method to extract road edges; 7) Carry out polynomial curve fitting on passable road surface according to the least square method. The invention can effectively extract the passable area of the road surface in real time, has high precision and high reliability, and has a small judgment error rate in the identification process, and can be widely used in actual occasions of extracting the feasible area of structured roads based on three-dimensional laser radar.

Description

A Feasible Region Extraction Method for Structured Roads Based on 3D LiDAR

technical field

The invention relates to unmanned driving technology, in particular to a method for extracting feasible regions of structured roads based on three-dimensional laser radar.

Background technique

Unmanned vehicles are intelligent vehicles that use on-board sensing systems to perceive the surrounding environment of the vehicle, plan their own driving route and control the vehicle to reach the destination successfully. Unmanned vehicles are generally equipped with lidar and cameras as on-board sensors to detect road areas and obstacle areas. Since the visual sensor is greatly affected by environmental factors, the detection range of the single-line lidar is very limited. Therefore, 3D lidar has been widely used in intelligent driving.

The current 3D lidar processing algorithms are mainly divided into two categories: grid map-based segmentation algorithm and graph-based ground detection.

1. Perform threshold segmentation and distance transformation on the obstacle grid map to improve its continuity, and then extract the passable area based on area growth. 2. The grid map is processed as an undirected graph, and the Markov random field is used to classify the grid, and the passable area is extracted from it. But the accuracy of this method depends on the size of the grid and the detection range of the lidar. Montemerlo proposes to use the distance between the three-dimensional data rings to detect whether a single three-dimensional laser point belongs to the ground area; Douillard uses the mesh method to construct the map, and determines the ground point through the gradient method. Compared with raster-based algorithms, graph-based segmentation algorithms have higher precision, but are susceptible to noise interference, and the algorithm has poor robustness.

Contents of the invention

The technical problem to be solved by the present invention is to provide a method for extracting feasible regions of structured roads based on three-dimensional laser radar in view of the defects in the prior art.

The technical solution adopted by the present invention to solve the technical problem is: a method for extracting feasible regions of structured roads based on three-dimensional laser radar, comprising the following steps:

1) Scanning the surrounding environment through a three-dimensional laser radar to obtain point cloud data information of the surrounding environment, and converting the point cloud data from the laser radar coordinate system to the local Cartesian coordinate system; the three-dimensional laser radar is arranged at the center of the top of the vehicle.

2) Extract the three-dimensional laser radar interest data points from the point cloud data information; the interest data points refer to the point cloud data in front of the vehicle, that is, the data within the spatial range of 20 meters in front of the vehicle, 10 meters left and right, and 30 meters above point;

3) According to the data points of interest, the lidar scanning single line is extracted by using the method of radar detection angle clustering;

4) Scan the single line of the extracted lidar, and use the K-means clustering algorithm to obtain the optimal clustering point set;

5) Obstacles are extracted using height features according to the optimal clustering point set;

6) According to the optimal clustering point set, use the density feature combined with the DBSCAN clustering method to extract the roadside;

7) Carry out polynomial curve fitting on the passable road surface according to the least square method, and finally extract the feasible region of the road ahead of the structured road.

According to the above scheme, the coordinate system of the lidar in the step 1 described in step 1) is based on the placement position of the lidar as the origin, the forward direction of the vehicle as the x-axis direction in the lidar coordinate system, and the left and right directions of the vehicle as the laser The y-axis direction in the radar coordinate system, the up and down of the vehicle as the z-axis direction in the lidar coordinate system; the local Cartesian coordinate system is based on the intersection of the longitudinal central axis of the vehicle body and the front of the car as the origin, and the forward direction of the vehicle as the local Cartesian coordinates The x-axis direction in the system, the left and right direction of the vehicle is taken as the y-axis direction in the local Cartesian coordinate system, and the up and down of the vehicle is taken as the z-axis direction in the local Cartesian coordinate system.

According to the above-mentioned scheme, in the described step 3), the method of utilizing the radar detection angle clustering to extract the laser radar scanning single line specifically includes:

3.1) During the three-dimensional lidar scanning process, the point cloud information is expressed in the Cartesian coordinate system, and the point cloud information is transformed into the spherical coordinate system, using the formula The angle between each point in the point cloud data and the z-axis can be obtained;

3.2) After obtaining the elevation angle of each point, cluster the data points with the same elevation angle.

According to the above scheme, the K-means clustering algorithm that obtains the optimal clustering point set in the step 4) is an improved K-means clustering algorithm, specifically comprising the following steps:

4.1) For the single-line scanning data set χ={x ₁ ,...,x _n } extracted in step 3), use the AIC criterion to obtain the optimal number of clusters, and use the formula AIC(k)=2k+nln(SSR /n) Obtain the optimal number of clusters, where k is the number of clusters, n is the number of observations, SSR is the residual sum of squares of the data set, and the AIC is minimized by finding an appropriate k;

4.2) Randomly select a sample from the data set as the initial clustering center c;

4.3) Calculate the distance between each sample and the current existing cluster center, then calculate the probability of each sample being selected as the next cluster center, and finally select the next cluster center according to the roulette method, that is, the probability is large The probability of the cluster center being selected is high;

4.4) Repeat step 4.2) until a total of k cluster centers C={C ₁ , C ₂ ,...,C _k } are selected;

4.5) For each sample x _i in the data set, calculate the distance between each sample and these cluster centers and classify it into the class corresponding to the cluster center with the smallest distance;

4.6) Recalculate the centers of clusters with changes;

4.7) Calculate the standard measure function, and terminate when the function converges; if not, return to step 4.5).

According to the above scheme, in the step 5), using the height feature to extract obstacles specifically includes:

5.1) After using the improved K-means clustering algorithm to obtain the optimal clustering point set for the extracted scanning single line, solve the mean value and variance of the z coordinates of the points in each type of point concentration;

5.2) Let the height of the obstacle be 30cm, when the mean value of the z coordinates in the category is greater than 30cm and the variance of the midpoint of the category is within the set threshold, the point set of this type is considered to be an obstacle point set;

5.3) Eliminate the point set identified as an obstacle from the single line point set.

According to the above-mentioned scheme, in the described step 6) in conjunction with the DBSCAN clustering method, the concrete steps of extracting the roadside are as follows:

6.1) Firstly, select point p arbitrarily in the extracted scanning single line set, and search for all density-reachable points around. If p is the core point, then in the field whose radius is Eps, all points and p belong to the same cluster. These points are the candidate points;

6.2) Continuously expand the cluster by finding points that are reachable with the candidate point density until a complete cluster is formed;

6.3) If point p is not a core point, mark point p as a noise point;

6.4) Repeat the above steps until all points are clustered or marked as noise points;

6.5) Since the density of data points generated by laser radar scanning to the roadside section in the structured road will be greater than that of the normal ground, use DBSCAN to extract the roadside according to the density after clustering;

6.6) Eliminate the extracted points along the road.

According to the above scheme, carrying out polynomial curve fitting to the passable road surface according to the method of least squares in said step 7) specifically includes:

7.1) For the single-line point set that has eliminated obstacles and roadsides in the scanning single-line, use the least square method to fit the remaining point set;

7.2) Repeat the above process to obtain the fitting curve of the passable area of all the remaining points of the single line.

The beneficial effects produced by the present invention are:

1. When the present invention extracts ground points, a large number of noise points are filtered from the height direction, which greatly simplifies data processing.

2. When processing the three-dimensional laser radar data, the present invention uses the elevation angle to classify the data and extract the data of each single line of the three-dimensional laser radar. Such processing can make data processing simpler and more effective.

3. The present invention adopts the improved K-means algorithm when each single-line point set is clustered, and the algorithm can automatically select the optimum cluster number according to the information of the points in the point concentration, and the accuracy of clustering is higher than The original K-means algorithm has been improved.

4. When using the method of the present invention to conduct a field test, the road extraction takes an average of 55ms per frame, the maximum time-consuming for a single frame is 80ms, and the minimum time-consuming is 42ms, which has good real-time performance.

Description of drawings

The present invention will be further described below in conjunction with accompanying drawing and embodiment, in the accompanying drawing:

Fig. 1 is the method flowchart of the embodiment of the present invention;

Fig. 2 is the original point cloud image after the correction transformation of the embodiment of the present invention;

Fig. 3 is the interest point cloud figure of the embodiment of the present invention;

Fig. 4 is a single-line data extraction diagram of an embodiment of the present invention;

Fig. 5 is the clustering distinction diagram that the improved K-means algorithm of the embodiment of the present invention obtains;

Fig. 6 is the point cloud diagram of obstacles screened out by the height mean value and variance of the embodiment of the present invention;

Fig. 7 is the road edge extraction figure that utilizes DBSCAN to obtain in the embodiment of the present invention;

Fig. 8 is a fitting diagram of the least squares method in the feasible region of the road obtained in the embodiment of the present invention.

Detailed ways

In order to make the object, technical solution and advantages of the present invention more clear, the present invention will be further described in detail below in conjunction with the examples. It should be understood that the specific embodiments described here are only used to explain the present invention, not to limit the present invention.

As shown in Figure 1, a method for extracting a feasible region of a structured road based on a three-dimensional laser radar provided by the present invention includes the following steps:

Step 1: The 3D lidar scans the surrounding environment, obtains the point cloud information of the surrounding environment, and converts the point cloud data from the lidar coordinate system to the local Cartesian coordinate system after correction, as shown in Figure 2. The original point cloud map after system correction and conversion ;

Step 1 specifically includes:

The coordinate system of the lidar is based on the location where the lidar is placed as the origin, the direction of the vehicle is taken as the x-axis direction in the lidar coordinate system, the left and right directions of the vehicle are taken as the y-axis direction in the lidar coordinate system, and the up and down of the vehicle are taken as the lidar coordinates The z-axis direction in the system.

The local Cartesian coordinate system is based on the intersection of the longitudinal central axis of the vehicle body and the front of the vehicle as the origin, the forward direction of the vehicle is taken as the x-axis direction in the local Cartesian coordinate system, the left and right direction of the vehicle is taken as the y-axis direction in the local Cartesian coordinate system, and the up and down of the vehicle as the z-axis direction in the local Cartesian coordinate system.

Since the installation of the laser radar can not guarantee the absolute level, the scanning inclination will produce a certain error, so the standard of the present invention needs to be corrected before the coordinate conversion, so that the point projection angle and the original data scanning angle are unified, and then the coordinate system of the laser radar is converted into the local Cartesian coordinate system.

Step 2: Extract the 3D lidar interest data points, as shown in Figure 3, the interest point cloud image obtained after the system processing;

The point cloud data in front of the vehicle is mainly considered in the extraction of the interest data points of the 3D lidar in step 2, that is, the data points within the spatial range of 20 meters in front of the vehicle, 10 meters left and right, and 30 meters above are extracted.

Step 3: Utilize the method for radar detection angle clustering to extract the laser radar scanning single line, as shown in Fig. 4 is the single line extraction figure that the present invention obtains;

The three-dimensional laser radar used in the embodiment of the present invention is a Velodyne 16-line laser radar. The radar emits 16 laser lines in the vertical scanning range, scans 360° in the horizontal direction to obtain surrounding environment information, and collects about 20,000 points per frame. For such The amount of information collected more, the present invention adopts the method for extracting scanning single line earlier; Specifically as follows:

3.1) The 3D lidar expresses the point cloud information in the Cartesian coordinate system during the scanning process. We transform these point cloud information into the spherical coordinate system, using the formula The angle between each point in the point cloud data and the z-axis can be obtained.

3.2) After obtaining the elevation angle of each point, cluster the data points with the same elevation angle.

Step 4: obtain optimal clustering point set with improved K-means clustering algorithm, as Fig. 5 is the clustering distinction diagram that the present invention utilizes improved K-means algorithm to obtain;

The improved K-means clustering algorithm to obtain the optimal clustering point set specifically includes:

4.1) For the data set χ={x ₁ ,...,x _n }, use the AIC criterion to obtain the optimal number of clusters, and use the formula AIC(k)=2k+nln(SSR/n) to obtain the optimal number of clusters , where k is the number of clusters, n is the number of observations, SSR is the residual sum of squares of the data set, and the AIC is minimized by finding a suitable k;

4.2) Randomly select a sample from the data set as the initial clustering center c;

4.3) First calculate the distance between each sample and the current existing cluster center, then calculate the probability of each sample being selected as the next cluster center, and finally select the next cluster center according to the roulette method, that is, the probability Large cluster centers have a high probability of being selected;

4.4) Repeat step 4.2) until a total of k cluster centers C={C ₁ , C ₂ ,...,C _k } are selected;

4.5) For each sample x _i in the data set, calculate the distance between each sample and these cluster centers and classify it into the class corresponding to the cluster center with the smallest distance;

4.6) Recalculate the centers of clusters with changes;

4.7) Calculate the standard measurement function, when the function convergence is satisfied, then terminate; if not satisfied, then return to step 4.5);

Step 5: Use the height feature to extract obstacles, as shown in Figure 6, which is the point cloud map of obstacles screened out by the present invention using the height mean and variance;

Step 5) utilize height feature extraction obstacle to specifically include:

5.1) After using the improved K-means clustering algorithm to obtain the optimal clustering point set for the extracted scanning single line, solve the mean value and variance of the z coordinates of the points in each type of point concentration;

5.2) Let the height of the obstacle be 30cm, when the mean value of the z coordinates in the category is greater than 30cm and the variance of the midpoint of the category is within the set threshold, the point set of this type is considered to be an obstacle point set;

5.3) Eliminate the point set identified as an obstacle from the single line point set.

Step 6: extract road edge in conjunction with DBSCAN clustering method, as Fig. 7 is the road edge extraction figure that the present invention utilizes DBSCAN to obtain;

In step 6), the specific steps of extracting the roadside in combination with the DBSCAN clustering method are as follows:

6.1) Firstly, select point p arbitrarily in the extracted scanning single line set, and search for all density-reachable points around. If p is the core point, then in the field whose radius is Eps, all points and p belong to the same cluster. These points are the candidate points;

6.2) Continuously expand the cluster by finding points that are reachable with the candidate point density until a complete cluster is formed;

6.3) If point p is not a core point, mark point p as a noise point;

6.4) Repeat the above steps until all points are clustered or marked as noise points;

6.5) Since the density of data points generated by laser radar scanning to the roadside section in the structured road will be greater than that of the normal ground, use DBSCAN to extract the roadside according to the density after clustering;

6.6) Eliminate the extracted points along the road.

Step 7: Carry out polynomial curve fitting on the passable road surface according to the least square method, and finally extract the feasible region of the road ahead of the structured road.

Carrying out polynomial curve fitting to the passable road surface according to the least square method in step 7) specifically includes:

7.1) For the single-line point set that has eliminated obstacles and roadsides in the scanning single-line, use the least square method to fit the remaining point set;

7.2) Repeat the above process to obtain the fitting curve of the passable area of all the remaining points of the single line.

It should be understood that those skilled in the art can make improvements or changes based on the above description, and all these improvements and changes should belong to the protection scope of the appended claims of the present invention.

Claims (7)

1. A method for extracting a structured road feasible region based on three-dimensional laser radar, comprising the following steps: 1) scan the surrounding environment by three-dimensional laser radar, obtain the point cloud data information of the surrounding environment, and convert the point cloud data from the coordinate system of the laser radar to the local Cartesian coordinate system; the three-dimensional laser radar is arranged at the center of the top of the vehicle; 2) Extract the three-dimensional laser radar interest data points from the point cloud data information; the interest data points refer to the point cloud data in front of the vehicle, that is, the data within the spatial range of 20 meters in front of the vehicle, 10 meters left and right, and 30 meters above point; 3) According to the data points of interest, the lidar scanning single line is extracted by using the method of radar detection angle clustering; 4) Scan the single line of the extracted lidar, and use the improved K-means clustering algorithm to obtain the optimal clustering point set; 5) Obstacles are extracted using height features according to the optimal clustering point set; 6) According to the optimal clustering point set, use the density feature combined with the DBSCAN clustering method to extract the roadside; 7) Carry out polynomial curve fitting on the passable road surface according to the least square method, and finally extract the feasible region of the road ahead of the structured road. 2. The method for extracting the feasible region of structured roads according to claim 1, wherein the coordinate system of the laser radar in the step 1 described in the step 1) is based on the placement position of the laser radar as the origin, and the forward direction of the vehicle is As the x-axis direction in the laser radar coordinate system, the left and right directions of the vehicle are used as the y-axis direction in the laser radar coordinate system, and the up and down of the vehicle are used as the z-axis direction in the laser radar coordinate system; The intersection point of the longitudinal central axis and the front of the vehicle is the origin, the forward direction of the vehicle is taken as the x-axis direction in the local Cartesian coordinate system, the left and right direction of the vehicle is taken as the y-axis direction in the local Cartesian coordinate system, and the up and down of the vehicle is taken as z in the local Cartesian coordinate system axis direction. 3. The method for extracting the feasible region of the structured road according to claim 1, wherein, in the step 3), the method of utilizing radar detection angle clustering to extract the laser radar scanning single line specifically includes: 3.1) During the three-dimensional lidar scanning process, the point cloud information is expressed in the Cartesian coordinate system, and the point cloud information is transformed into the spherical coordinate system, using the formula The angle between each point in the point cloud data and the z-axis can be obtained; 3.2) After obtaining the elevation angle of each point, cluster the data points with the same elevation angle. 4. the method for extracting structured road feasible region according to claim 1, is characterized in that, the K-means clustering algorithm of obtaining optimal clustering point set in described step 4) is improved K-means clustering The algorithm specifically includes the following steps: 4.1) For the single-line scanning data set χ={x ₁ ,...,x _n } extracted in step 3), use the AIC criterion to obtain the optimal number of clusters, and use the formula AIC(k)=2k+n ln( SSR/n) to obtain the optimal number of clusters, where k is the number of clusters, n is the number of observations, SSR is the residual sum of squares of the data set, and the AIC is minimized by finding an appropriate k; 4.2) Randomly select a sample from the data set as the initial clustering center c; 4.3) Calculate the distance between each sample and the current existing cluster center, then calculate the probability of each sample being selected as the next cluster center, and finally select the next cluster center according to the roulette method, that is, the probability is large The probability of the cluster center being selected is high; 4.4) Repeat step 4.2) until a total of k cluster centers C={C ₁ ,C ₂ ,...,C _k } are selected; 4.5) For each sample x _i in the data set, calculate the distance between each sample and these cluster centers and classify it into the class corresponding to the cluster center with the smallest distance; 4.6) Recalculate the centers of clusters with changes; 4.7) Calculate the standard measure function, and terminate when the function converges; if not, return to step 4.5). 5. The method for extracting structured road feasible region according to claim 1, wherein the step 5) utilizes height features to extract obstacles specifically comprising: 5.1) After using the clustering algorithm to obtain the optimal clustering point set for the extracted scanning single line, solve the mean value and variance of the z coordinates of points in each type of point concentration; 5.2) Let the height of the obstacle be 30cm, when the mean value of the z coordinates in the category is greater than 30cm and the variance of the midpoint of the category is within the set threshold, the point set of this type is considered to be an obstacle point set; 5.3) Eliminate the point set identified as an obstacle from the single line point set. 6. the method for extracting the feasible region of structured road according to claim 1, is characterized in that, in the described step 6), the specific steps of extracting the roadside in conjunction with the DBSCAN clustering method are as follows: 6.1) Firstly, select point p arbitrarily in the extracted scanning single line set, and search for all density-reachable points around. If p is the core point, then in the field whose radius is Eps, all points and p belong to the same cluster. These points are the candidate points; 6.2) Continuously expand the cluster by finding points that are reachable with the candidate point density until a complete cluster is formed; 6.3) If point p is not a core point, mark point p as a noise point; 6.4) Repeat the above steps until all points are clustered or marked as noise points; 6.5) Since the density of data points generated by laser radar scanning to the roadside section in the structured road will be greater than that of the normal ground, use DBSCAN to extract the roadside according to the density after clustering; 6.6) Eliminate the extracted points along the road. 7. The method for extracting the feasible region of structured roads according to claim 1, wherein in said step 7), carrying out polynomial curve fitting to the passable road surface according to the least squares method specifically includes: 7.1) For the single-line point set that has eliminated obstacles and roadsides in the scanning single-line, use the least square method to fit the remaining point set; 7.2) Repeat the above process to obtain the fitting curve of the passable area of all the remaining points of the single line.