A fast point cloud extraction algorithm for drivable areas in mine autonomous driving

Abstract

The present invention relates to the field of positioning technology, and discloses an algorithm for fast extraction of point clouds of drivable areas in automatic driving of mines, comprising the following steps: S1, extraction on the right side of the center point of the road boundary line; Step 1: reading road boundary points from a map, resampling, calculating the distance between two adjacent points, if it is greater than a distance threshold L1, filling the interval points, and calculating the curvature of the filled points, if it is greater than R1, further filling the interval points, and setting the filling distance threshold to L2. The algorithm for fast extraction of point clouds of drivable areas in automatic driving of mines realizes accurate segmentation of drivable areas in complex mine environments through the steps of map data resampling, extraction of left and right road boundary lines, extraction of closed road boundary lines, and extraction of drivable areas.

Description

Technical Field

[0001] The present invention relates to the field of positioning technology, and in particular to a fast point cloud extraction algorithm for drivable areas in autonomous driving in underground mines. Background Art

[0002] With the rapid development of the autonomous driving industry and the increasing intelligence level in mining environments, autonomous driving technology is playing an increasingly important role in underground mines. Extracting effective drivable areas in underground mines has become a pressing issue. The complex road environment and uneven tunnel walls in underground mines make accurate filtering difficult using plane fitting. Furthermore, due to the large fluctuations in ground slopes, directly detecting drivable areas on the ground is still very complex. Model training requires a large amount of labeled data and is time-consuming. Even using map extraction methods, inaccurate positioning and complex and variable global path contours make it difficult to extract effective areas. Therefore, a fast method for extracting drivable areas suitable for underground mines is needed. Summary of the Invention

[0003] In response to the above-mentioned deficiencies in the prior art, the present invention provides a fast point cloud extraction algorithm for the drivable area in autonomous driving in underground mines, which can be applied to a variety of complex underground mine tunnel environments and effectively locate the underground mine environment.

[0004] The present invention provides the following technical solution: a fast point cloud extraction algorithm for drivable areas in autonomous driving in underground mines, comprising the following steps:

[0005] S1, extraction of the right side of the center point of the road boundary line;

[0006] Step 1: Read road boundary points from the map, resample them, calculate the distance between two adjacent points, and fill in the interval points if the distance is greater than the distance threshold L1. Calculate the curvature of the filled points. If the distance is greater than R1, fill in the interval points further. The filling distance threshold is set to L2.

[0007] Step 2: Search the conversion relationship from map to base_link through the tf tree, read the timestamp of the point cloud to be processed, and then search the corresponding conversion relationship from the tf tree according to the timestamp, and search for the conversion relationship between the two adjacent frames at that moment. If it exists, the point cloud to be processed will be converted to base_link. If it does not exist, the conversion fails, an alarm is issued, and the point cloud to be processed is output;

[0008] Step 3: Calculate the index point on the right side of the road. Store the road boundary point cloud in the ktree, and then select the closest point from it. The closest point obtained at this time is the point on the right side of the vehicle body.

[0009] Step 4: With the right side of the vehicle as the center, search for ordered points forward and backward. Index the points 50 meters backward from the center point, calculate the distance between any two points, and add them up to 50 meters. If the minimum index value is less than 0, set it to 0. Similarly, set the maximum index value. If the maximum index value is greater than the maximum value of the ordered points, set the maximum value to the maximum index of the right side to determine the starting and ending points of the right corner;

[0010] S2, extraction of the left boundary line of the road;

[0011] Step 1: Find the tf relationship and convert the point cloud of the right lidar to base_link. The point cloud of the left lidar is also converted to base_link.

[0012] Step 2: Calculate the temporary coordinate system on the right side. Based on the minimum value of the right index point and the previous value, the two points determine a coordinate system. The x-axis direction from the next point to the previous point is the x-axis of the new coordinate system. Rotate 90 degrees counterclockwise to the y-axis direction. Construct a rotation transformation matrix and translate 5 meters in the y-axis direction to obtain a transformation matrix. Then, convert the point cloud of the left road boundary line and the point cloud of the right boundary line to the new base_link.

[0013] Step 3: Convert the obtained left point cloud to base_link, then use ktree to find the nearest point within the set radius, then sort in ascending order, select the positive index direction, and for the points in the positive range, select the point with the y-axis greater than 0, and select the point closest to the vehicle body from it to obtain the index of the point in the left queue;

[0014] Step 4: For the largest point on the left, repeat steps 2 and 3, select the farthest point corresponding to the right index, establish a new coordinate system, and transform the left point cloud to base_link. Follow step 3 to obtain the farthest left point corresponding to the farthest right lane line point, and thus determine the farthest left point;

[0015] S3, road boundary filling and extraction;

[0016] Step 1: Determine whether the left and right boundary points are correctly generated. If so, convert the left and right boundary points to base_link and complete the road boundary continuity. Arrange the index of the left boundary line in reverse order and complete it to the right boundary point.

[0017] Step 2: According to the conversion relationship from base_link to map, the extracted closed road boundary is converted to the map coordinate system and rigidly matched through RANSAC. If the deviation exceeds the set value, the conversion relationship needs to be optimized through the ICP fine registration algorithm. Before fine registration, auxiliary matching lines are added within the set range in front of and behind the vehicle, and then fine registration is performed. Then the extracted road boundary line is converted to the map.

[0018] Step 3: Set the specified cropping range, then project the points within the cropping range into the XY plane, convert the three-dimensional coordinates of the point cloud into two-dimensional pixel values, and record the point cloud stored in each pixel. Then, binarize the projection and extract the pixels in the contour area based on the closed contour. In this way, the point cloud in the drivable area is obtained, that is, the closed area in the drivable area, and finally converted to base_link for subsequent detection.

[0019] Preferably, in step 3 of S1, the coordinate origin is used as the starting index point, all point clouds are searched, and all points are sorted in ascending order. At this time, the directionality of the point, that is, the size of the x-axis difference between the front and rear points, is used to select points greater than 0 and points with y values ​​less than 0. At this time, a series of points on the right side of the vehicle body are obtained, and the remaining points are stored in the ktree, and then the closest point is selected from it. The closest point obtained at this time is the point on the right side of the vehicle body.

[0020] Compared with the prior art, the present invention has the following beneficial effects:

[0021] This algorithm for fast extraction of point clouds of drivable areas in autonomous driving in underground mines achieves accurate segmentation of drivable areas in complex underground mining environments through steps such as map data resampling, left and right road boundary line extraction, closed road boundary line extraction, and drivable area extraction. It also uses a temporary coordinate system and ktree combination to complete the left and right side alignment endpoint search, solving the problem that the left and right side endpoints cannot be effectively aligned when extracting closed areas. The point cloud data is converted into an image area, and then the closed contour is extracted through the extracted drivable area boundary line, and the point cloud corresponding to the pixel points in the closed area is extracted to ensure that fast drivable area point cloud extraction can be achieved. BRIEF DESCRIPTION OF THE DRAWINGS

[0022] Figure 1 This is a flow chart of the right boundary of the road centerline of the present invention;

[0023] Figure 2 This is a flowchart for extracting the left boundary of the road centerline of the present invention;

[0024] Figure 3 This is the road boundary line extraction map of the present invention. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solutions and advantages of the embodiments of the present disclosure clearer, the technical solutions of the embodiments of the present disclosure will be clearly and completely described below in conjunction with the drawings of the embodiments of the present disclosure. In order to keep the following description of the embodiments of the present disclosure clear and concise, the present disclosure omits detailed descriptions of known functions and known components to avoid unnecessary confusion of the concepts of the present invention.

[0026] See also Figure 1-3 , a fast point cloud extraction algorithm for drivable areas in underground mining autonomous driving, including the following steps:

[0027] S1, extraction of the right side of the center point of the road boundary line;

[0028] Step 1: Read road boundary points from the map, resample them, calculate the distance between two adjacent points, and fill in the interval points if the distance is greater than the distance threshold L1. Calculate the curvature of the filled points. If the distance is greater than R1, fill in the interval points further. The filling distance threshold is set to L2.

[0029] Step 2: Search the conversion relationship from map to base_link through the tf tree, read the timestamp of the point cloud to be processed, and then search the corresponding conversion relationship from the tf tree according to the timestamp, and search for the conversion relationship between the two adjacent frames at that moment. If it exists, the point cloud to be processed will be converted to base_link. If it does not exist, the conversion fails, an alarm is issued, and the point cloud to be processed is output;

[0030] Step 3: Calculate the index points on the right side of the road. Store the road boundary point cloud in a ktree. Using the coordinate origin as the starting index point, search all point clouds and sort all points in ascending order. Then, based on the directionality of the points (i.e., the difference between the front and rear points on the x-axis), select points with values ​​greater than 0 and points with values ​​less than 0 on the y-axis. This results in a series of points on the right side of the vehicle. The remaining points are then stored in a ktree, and the closest point is selected. The closest point is the point on the right side of the vehicle.

[0031] Step 4: With the right side of the vehicle as the center, search for ordered points forward and backward. Index the points 50 meters backward from the center point, calculate the distance between any two points, and add up to 50 meters. If the minimum index value is less than 0, set it to 0. Similarly, set the maximum index value. If the maximum index value is greater than the maximum value of the ordered points, set the maximum value to the maximum index value of the right side to determine the starting and ending points of the right corner.

[0032] S2, extraction of the left boundary line of the road;

[0033] Step 1: Find the tf relationship and convert the point cloud of the right lidar to base_link. The point cloud of the left lidar is also converted to base_link.

[0034] Step 2: Calculate the temporary coordinate system on the right side. Based on the minimum value of the right index point and the previous value, the two points determine a coordinate system. The x-axis direction from the next point to the previous point is the x-axis of the new coordinate system. Rotate 90 degrees counterclockwise to the y-axis direction. Construct a rotation transformation matrix and translate 5 meters in the y-axis direction to obtain a transformation matrix. Then, convert the point cloud of the left road boundary line and the point cloud of the right boundary line to the new base_link.

[0035] Step 3: Convert the obtained left point cloud to base_link, then use ktree to find the nearest point within the set radius, then sort in ascending order, select the positive index direction, and for the points in the positive range, select the point with the y-axis greater than 0, and select the point closest to the vehicle body from it to obtain the index of the point in the left queue;

[0036] Step 4: For the largest point on the left, repeat steps 2 and 3, select the farthest point corresponding to the right index, establish a new coordinate system, and transform the left point cloud to base_link. Follow step 3 to obtain the farthest point on the left corresponding to the farthest right lane line point, thereby determining the farthest left point.

[0037] S3, road boundary filling and extraction;

[0038] Step 1: Determine whether the left and right boundary points are correctly generated. If so, convert the left and right boundary points to base_link and complete the road boundary continuity. Arrange the index of the left boundary line in reverse order and complete it to the right boundary point.

[0039] Step 2: According to the conversion relationship from base_link to map, the extracted closed road boundary is converted to the map coordinate system and rigidly matched through RANSAC. If the deviation exceeds the set value, the conversion relationship needs to be optimized through the ICP fine registration algorithm. Before fine registration, auxiliary matching lines are added within the set range in front of and behind the vehicle, and then fine registration is performed. Then the extracted road boundary line is converted to the map.

[0040] Step 3: Set the specified cropping range, then project the points within the cropping range into the XY plane, convert the three-dimensional coordinates of the point cloud into two-dimensional pixel values, and record the point cloud stored in each pixel. Then, binarize the projection and extract the pixels in the contour area based on the closed contour. In this way, the point cloud in the drivable area is obtained, that is, the closed area in the drivable area, and finally converted to base_link for subsequent detection.

[0041] The above embodiments are merely exemplary embodiments of the present invention and are not intended to limit the scope of the present invention. The scope of protection of the present invention is defined by the claims. Those skilled in the art may make various modifications or equivalent substitutions to the present invention within the spirit and scope of protection of the present invention, and such modifications or equivalent substitutions shall also be deemed to fall within the scope of protection of the present invention.

Claims

1. A fast point cloud extraction algorithm for drivable areas in mine automatic driving, characterized in that: The following steps are involved: S1, extraction of the right side of the center point of the road boundary line; Step 1: Read the road boundary points from the map, resample them, calculate the distance between two adjacent points, fill the interval points if it is greater than the distance threshold L1, and calculate the curvature of the filled points. If it is greater than R1, fill the interval points further, and set the filling distance threshold to L2; Step 2: Search the conversion relationship from map to base_link through the tf tree, read the timestamp of the point cloud to be processed, and then search the corresponding conversion relationship from the tf tree according to the timestamp, and search for the conversion relationship between the two adjacent frames at that moment. If it exists, the point cloud to be processed will be converted to base_link. If it does not exist, the conversion fails, an alarm is issued, and the point cloud to be processed is output; Step 3: Calculate the index point on the right side of the road, store the road boundary point cloud in the ktree, and then select the nearest point from it. The nearest point obtained at this time is the point on the right side of the vehicle body; Step 4: With the right side of the vehicle as the center, search for ordered points forward and backward, index the points 50 meters backward from the center point, calculate the distance between any two points, and add up to 50 meters. If the minimum index value is less than 0, set it to 0. Similarly, set the maximum index value. If the maximum index value is greater than the maximum value of the ordered points, set the maximum value to the maximum value of the right index, and determine the starting point and end point of the right corner; S2, extraction of the left boundary line of the road; Step 1: Find the tf relationship and convert the point cloud of the right laser radar to base_link. The point cloud of the left laser radar is also converted to base_link. Step 2: Calculate the temporary coordinate system on the right side. According to the minimum value of the right index point and the previous value, two points determine a coordinate system. The x-axis direction from the next point to the previous point is the x-axis of the new coordinate system. Rotate 90 degrees counterclockwise to the y-axis direction. Construct a rotation transformation matrix and translate 5 meters in the y-axis direction to obtain a transformation matrix. Then convert the point cloud of the left road boundary line and the point cloud of the right boundary line to the new base_link. Step 3: Convert the obtained left point cloud to base_link, then use ktree to find the nearest point within the set radius, then sort in ascending order, select the positive index direction, and for the points within the positive range, select the point whose y axis is greater than 0, and select the point closest to the vehicle body from them to obtain the index of the point in the left queue; Step 4: For the largest point on the left, repeat steps 2 and 3, select the farthest point corresponding to the right index, establish a new coordinate system, and transform the left point cloud to base_link. According to step 3, get the farthest point on the left corresponding to the farthest right lane line point, so as to determine the farthest left point; S3, road boundary filling and extraction; Step 1: Determine whether the points of the left and right boundary lines are correctly generated. If so, convert the points of the left and right boundary lines to base_link, complete the continuity of the road boundary lines, sort the indexes of the left boundary lines in reverse order, and complete them to the points of the right boundary points; Step 2: According to the conversion relationship from base_link to map, the extracted closed road boundary is converted to the map coordinate system, and rigid matching is performed through RANSAC. If the deviation exceeds the set value, the conversion relationship needs to be optimized through the ICP fine registration algorithm. Before fine registration, auxiliary matching lines are added within the set range in front of and behind the vehicle, and then fine registration is performed. Then the extracted road boundary line is converted to the map. Step 3: Set the set cropping range, then project the points within the cropping range into the XY plane, convert the three-dimensional coordinates of the point cloud into two-dimensional pixel values, and record the point cloud stored in each pixel. Then binarize the projection and extract the pixels in the contour area based on the closed contour. In this way, the point cloud in the drivable area is obtained, that is, the closed area in the driving area, and finally converted to base_link for subsequent detection.

2. According to claim 1, a fast extraction algorithm of point cloud of drivable area in mine automatic driving, characterized in that: In step 3 of S1, the origin of the coordinate system is used as the starting index point, all point clouds are searched, and all points are sorted in ascending order. At this time, the directionality of the points, that is, the size of the x-axis difference between the front and rear points, is used to select points greater than 0 and points with y values ​​less than 0. At this time, a series of points on the right side of the vehicle body are obtained, and the remaining points are stored in the ktree, and then the nearest point is selected from it. The nearest point obtained at this time is the point on the right side of the vehicle body.

Images