Road fork detection method based on multi-frame continuous point cloud

Abstract

The invention discloses a road fork detection method based on multi-frame continuous point clouds. The method comprises the following steps: firstly, collecting original point cloud data of an intersection, recording pose information of each frame of a laser radar IMU, then calculating Euler angles of other frames relative to an initial frame, and obtaining a rotation translation matrix of other frames in a coordinate system of an original frame; and carrying out multi-frame fusion on the point cloud data of other frames after rotation and translation and the data of the original frame. The method comprises the following steps of: firstly, performing ground point cloud removal operation and rasterization operation on multi-frame data, establishing light wave detection under a polar coordinate system on the processed data to detect intersection characteristics of intersections, and finally, selecting an optimal intersection intersection result by adopting an optimal position matching algorithm, including the number of the intersections and intersection bifurcation angles and positions. And storing the optimal result to the mobile robot for a navigation task. According to the method, multi-frame fusion is carried out on the sparse point cloud data, the information of the intersection point cloud data is enriched, the intersection characteristics of the intersection are detected according to light wave detection, and the characteristics of the intersection data are detected more accurately.

Description

Technical Field

[0001] The invention belongs to the field of unmanned driving technology and intelligent transportation system, and specifically, is a method for detecting forks in road using multi-frame point cloud data in outdoor environment. Background Art

[0002] With the rapid development of driverless technology and intelligent transportation systems, automatic navigation systems are increasingly being used in urban traffic and complex road environments. In these applications, accurate detection of road forks is one of the key technologies to ensure the safe driving of driverless vehicles. Accurate detection of road forks can help driverless vehicles make correct path planning and decisions, thereby avoiding traffic accidents and improving driving efficiency and safety.

[0003] In practical applications, the road environment is complex and changeable, with various dynamic and static obstacles such as pedestrians, vehicles, and buildings, making the detection of forks in the road extremely challenging. Traditional fork detection methods based on image processing are easily affected by factors such as lighting changes, weather conditions, and occlusion in complex environments, resulting in inaccurate and unstable detection results.

[0004] Point cloud data is composed of a large number of discrete three-dimensional points and can provide rich geometric information. However, in practical applications, single-frame point cloud data often cannot fully reflect the detailed information in complex road environments due to the limitation of viewing angle and limited data volume. Therefore, the fusion and processing of multi-frame point cloud data has become an important means to improve the accuracy and robustness of fork detection. At present, Livox LiDAR has become an important part of unmanned driving and intelligent transportation systems with its unique scanning method and efficient data collection capabilities. Some of its radars have built-in IMU in the LiDAR, which facilitates data collection and synchronization. Unlike traditional mechanical LiDARs, Livox LiDARs use a non-repetitive scanning mode, which can cover a wider field of view in a shorter time and have a higher point cloud density. This makes Livox LiDAR perform better in dynamic and complex environments, providing high-precision three-dimensional point cloud data and significantly improving environmental perception capabilities.

[0005] In the process of processing multi-frame point cloud data, how to effectively remove the ground and obstacles, perform data rasterization processing, and accurately detect forks in the road based on the multi-viewpoint beam model are all technical difficulties. Removing ground data can avoid the influence of ground noise on the detection results, rasterization processing can simplify point cloud data and improve computing efficiency, and the beam model can simulate the propagation path of the light beam in space, thereby detecting the position and angle of the fork in the road. Summary of the invention

[0006] The purpose of the present invention is to provide a road fork detection method based on multi-frame continuous point clouds, which can accurately detect and locate road forks by aligning and fusing multi-frame point cloud data through mutual posture correction, and significantly improve the safety and driving efficiency of the system.

[0007] The technical solution to achieve the purpose of the present invention is: a road fork detection method based on multi-frame continuous point cloud. It includes the following steps:

[0008] The first step is to use LiDAR and IMU to collect intersection point cloud and posture data; then perform posture transformation and fusion processing on the point cloud data of the corresponding frame according to the IMU data;

[0009] The second step is to remove ground data and obstacles from the point cloud data after pose transformation and fusion; then rasterize the data after removing the ground data to increase the subsequent calculation efficiency and save computing resources;

[0010] The third step is to establish a mathematical model of equidistant continuous light wave detection based on the fused and processed point cloud data to obtain the characteristic results of the forward fork in the road detection.

[0011] The fourth step is to use the optimal position matching algorithm to extract and screen the result features of all the intersection detection results, and select the optimal intersection matching information as the final detection result.

[0012] Furthermore, the specific implementation steps of the first step are:

[0013] 1) Synchronize the timestamps of IMU inertial navigation and radar point cloud data to synchronize data; point cloud data collected by lidar: contains multiple frames, each frame is a point cloud data. The pose data collected by IMU: contains the angular velocity (ω) corresponding to each frame x ,ω y ,ω z ) and linear acceleration (a x ,a y ,a z ) six data;

[0014] 2) Initialize all variables, including initial posture, velocity, and position. By integrating the angular velocity and linear acceleration in the IMU data, the posture and position information of each frame and the relative homogeneous variation between frames are calculated.

[0015] The conversion matrix is ​​calculated as follows:

[0016] Δθ=ωΔt

[0017]

[0018]

[0019]

[0020]

[0021] q t =[w,x,y,z]

[0022]

[0023] Among them, Δt represents the time step, Δθ is the change in rotation angle, θ and u represent the rotation angle and rotation axis respectively, where the rotation axis has three directions xyz, and then calculate the Δq quaternion, q t represents the updated attitude quaternion, and R represents the calculated rotation matrix.

[0024] Then calculate the matrix in the translation direction. The specific calculation method is as follows:

[0025] v t =v 0 +a t Δt

[0026] p t =p 0 +v t Δt+0.5a t Δt 2

[0027] t rel =p t -p 0

[0028]

[0029]

[0030] Among them, v t Indicates the calculation speed, p t Indicates the calculated position, t rel Represents the relative translation vector, R rel represents the final calculated relative rotation matrix, where Represents the transposed matrix of the rotation matrix of the original frame, and finally T rel That is, the homogeneous transformation matrix between frames is calculated; after making a homogeneous transformation between the required frame and the original frame using the above calculation method, the transformed point cloud data of multiple frames are fused and added in the coordinate system of the original frame.

[0031] Furthermore, in the second step, the ground and obstacles are removed from the point cloud data after posture transformation and fusion; the process of removing the ground and obstacles is as follows: first, some points that obviously do not belong to the ground are filtered out, and then three points are randomly selected from the filtered data, a plane is fitted, and a series of judgments are made; this process is iterated, and finally the best plane is selected, and the plane and the points within the plane are removed, and the ground data removal operation is completed, and then the point cloud data is clustered and segmented to complete the removal of obstacles.

[0032] Furthermore, in the second step, the specific implementation steps of removing the ground and obstacle data are:

[0033] a) Set a height threshold, traverse the points in the point cloud dataset, and if the height value of the point is greater than the set height threshold, filter out the point, retain all points with heights within the threshold range, and generate a preliminarily filtered point cloud dataset;

[0034] b) setting the number of iterations, the in-plane point distance threshold, and the in-plane point number threshold in the point cloud data set after the preliminary filtering, and dividing the point cloud data set into the set number of regions;

[0035] c) For each divided point cloud area, three random point seeds are selected to fit a plane, the normal vector of the fitted plane is calculated, and the plane fitting is optimized based on the normal vector information;

[0036] d) Calculate the distance from each point in the area to the plane, and determine whether the distance is within the threshold range of the in-plane point distance. If it meets the range, add the point to the in-plane point set of the plane. If not, calculate the distance of the remaining points. After traversing all the points, determine whether the number of in-plane points of the plane is greater than or equal to the set in-plane point number threshold. If it meets the range, record it.

[0037] The plane and the number of points in it;

[0038] e) Repeat process c and d until the set number of iterations is met, count the number of in-plane points of the recorded planes in all regions, take the plane model with the most in-plane points as the ground model, and remove all in-plane points of the plane model from the original point cloud model, thus completing the ground removal process;

[0039] f) Then clustering and segmenting the point cloud data after removing the ground data;

[0040] g) Set the feature threshold, analyze each cluster, and determine its features including height, width, and depth. First, filter out clusters that are obviously not obstacles based on the feature threshold.

[0041] h) For the filtered point cloud data, detect the feature clusters that meet the obstacles, remove them from the point cloud data, and save the point cloud data.

[0042] Furthermore, in the second step, after removing the ground and obstacles from the point cloud data, a rasterization operation is performed on it.

[0043] In the second step, the specific implementation steps of rasterization are:

[0044] a) defining the voxel size, so as to set the size of each voxel in the above data, determine the spatial range of the point cloud data after removing the ground, and define the coverage of the grid network with this spatial range;

[0045] b) Use a hash table to store the data structure of the voxel grid, traverse the point cloud data, calculate the voxel coordinates where it is located using the following formula, and assign the point cloud data points to the corresponding voxel grid, using the key-value pair form of the hash table, where the key is the voxel coordinate and the value is the list of point cloud data points contained in the voxel;

[0046]

[0047] Among them, the size of each voxel is voxel_size x , voxel_size y and voxel_size z , x, y, and z represent the three-dimensional coordinates of each point;

[0048] c) Traverse all non-empty voxel grids and calculate the characteristic point in each voxel, that is, the center point (or centroid). The specific calculation formula is as follows;

[0049]

[0050] Among them, centroid represents the position of the center of mass, N represents the number of points in the voxel set, and p i Represents the vector coordinates of the i-th voxel point;

[0051] d) Count the center point sets of all voxels to form rasterized point cloud data;

[0052] Furthermore, in the third step, the specific implementation steps of establishing a mathematical model for equally spaced continuous light wave detection and obtaining characteristic results for detecting the fork in the road ahead are:

[0053] a) Project the de-grounded point cloud data onto a two-dimensional plane, with the model center as the origin and set as the viewpoint coordinates (origin r ,origin c );

[0054] b) Traverse all occupied grids, count and calculate the angle of the light wave ray relative to the viewpoint, and the calculation formula is as follows:

[0055]

[0056] Where (s, t) is the occupied grid coordinate, (origin r ,origin c ) is the viewpoint coordinate, γ grid The angle from the grid to the point coordinates;

[0057] c) Based on the statistically calculated light ray angles, the occupied grid is divided into different angular regions, each covering a range of 1 degree, as shown in the following formula:

[0058]

[0059] Among them, k∈{1, 2, 3, ...., 360}, γ s,t Represents the angle value of the occupancy grid, γ section represents the set of occupancy grid coordinates in all corner regions;

[0060] d) Traverse all the obtained angular region sets, and for each angular region, calculate the Euclidean distance between the nearest grid and the viewpoint in the interval, and let the Euclidean distance be the light wave distance:

[0061]

[0062] Among them, Z k is the angular area γ section The collection of all grids in l k Represents the Euclidean distance of the grid closest to the viewpoint in each angular region, and normalizes all calculated Euclidean distances. For the area not calculated in the angular region, it means that all grids in the region are not occupied, and the light wave Euclidean distance corresponding to the area is set to 1. All angular regions and their corresponding regional light wave distances are stored;

[0063] e) On the basis of the original viewpoint, n virtual viewpoints are set at equal intervals in front, and the process from b to e is repeated based on each virtual viewpoint until all calculations of n virtual viewpoints are completed.

[0064] The process is completed, and the model established for all viewpoints is recorded as M 0 ,M 1 ,M 2 ,...,M n , in each model

[0065] The corresponding angular area and its regional light wave distance are stored.

[0066] Furthermore, in the fourth step, the optimal position matching algorithm is used to extract and screen the result features of all the obtained intersection detection results, and the specific implementation steps of selecting the optimal intersection matching information are:

[0067] a) Set the threshold θ M And the error threshold μ M , starting from the positive direction of the x-axis, and circulating counterclockwise for 360 times, count the area where the light wave distance is 1 in each angular area of ​​a single model.

[0068] For an unobstructed area, if the difference between the angular areas of two consecutive light wave distances of 1 is μ M If it is within the range, it is still allocated in the same detection space, otherwise it is allocated as the next detection space and the cycle continues;

[0069] b) After the loop ends, count the number of angular regions with a distance of 1 in all detection spaces. If the sum of the numbers is greater than θ M , then the detection space is a valid detection space, otherwise the detection space is eliminated;

[0070] c) For model M 0 ,M 1 ,M 2 ,...,M n , execute steps a and b in sequence, and then calculate the average value of the angular area of ​​all valid detection spaces contained in each model as the final detection angle result of a single model, save it and record it as S i ;

[0071] d) S of all viewpoints i Merge into a total angle set and sort the set;

[0072] e) Set the similarity threshold θ S , set an empty temporary array and grouping vector, traverse the sorted total angle set, according to the set similarity threshold, if the current temporary group is empty, add the current angle to the temporary group, if the difference between the current angle and the last angle of the temporary group is within the threshold range, add the current angle to the temporary group, otherwise, add the temporary group to the grouping vector, clear the temporary group and add the current angle to the new temporary group, finally, if the temporary group is not empty, add it to the grouping vector;

[0073] f) checking whether the first angle of the first group of the grouped vectors and the last angle of the last group are within the similarity threshold range, if so, appending all the angles of the last group to the first group and removing the last group;

[0074] g) Set the segmentation threshold θ F, set an empty test segment angle vector and an old test angle value, traverse each group in the group vector, if the size of the group exceeds a certain ratio

[0075] For example, the median angle of the group is calculated. If the median angle is equal to the previous median angle

[0076] The difference in degree is less than the threshold θ F , then skip the group, otherwise, add the median angle to the measurement

[0077] Try segmenting the angle vector and update the old test angle value. If the newly added median angle is the same as

[0078] If the first median angle is within 360 degrees, the two angles are merged to obtain the final bifurcation angle set S f ;

[0079] h) Define the discreteness μμ i , as shown below, calculate all S i With S f The element discreteness of

[0080]

[0081] Among them, S fi and S ij Represent the final bifurcation angle S f The i-th angle value in the i-th viewpoint model and each angle value, μμ i is the final calculated discreteness value;

[0082] g) Statistics of all models and S f The element discreteness is selected, and the S with the smallest separation is selected. i The final test result is recorded as S i The bifurcation angle results and the viewpoint coordinates.

[0083] Compared with the prior art, the present invention has the following significant advantages: (1) High precision: By using multiple frames of continuous point cloud data and IMU data for posture transformation and fusion processing, the noise and error in a single frame of point cloud data can be effectively reduced, and each frame of point cloud data can be accurately aligned, thereby improving detection accuracy. (2) High efficiency: When processing point cloud data, first removing the ground and obstacles can significantly reduce the interference of irrelevant data and improve computing efficiency. Rasterizing the point cloud data can effectively reduce the amount of data, improve the speed and efficiency of subsequent calculations, and further save computing resources. (3) Strong robustness: By establishing a mathematical model for equally spaced continuous light wave detection, it is possible to better cope with intersections of different types and shapes, thereby improving the robustness and applicability of detection. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 It is a visual graph of multi-frame fused point cloud after pose conversion.

[0085] Figure 2 This is the top-down grid map after removing the ground and obstacles.

[0086] Figure 3 To establish the mathematical model diagram for light wave detection.

[0087] Figure 4 This is the optimal position result diagram based on the mathematical model of light wave detection.

[0088] Figure 5 Flowchart of a method for detecting forks in the road for multi-frame point cloud data of an outdoor environment. DETAILED DESCRIPTION

[0089] The present invention provides a road fork detection method based on multi-frame continuous point clouds, and its specific implementation method comprises the following steps:

[0090] Step 1: Use LiDAR and IMU to collect intersection point cloud and posture data; then perform posture transformation and fusion processing on the point cloud data of the corresponding frame according to the IMU data.

[0091] 1) Synchronize the timestamps of IMU inertial navigation and radar point cloud data to synchronize data; point cloud data collected by lidar: contains multiple frames, each frame is a point cloud data. The pose data collected by IMU: contains the angular velocity (ω) corresponding to each frame x ,ω y ,ω z ) and linear acceleration (a x ,a y ,a z ) six data;

[0092] 2) Initialize all variables, including initial posture, velocity, and position. By integrating the angular velocity and linear acceleration in the IMU data, the posture and position information of each frame and the relative homogeneous variation between frames are calculated.

[0093] The conversion matrix is ​​calculated as follows:

[0094] Δθ=ωΔt

[0095]

[0096]

[0097]

[0098]

[0099] qt =[w,x,y,z]

[0100]

[0101] Among them, Δt represents the time step, Δθ is the change in rotation angle, θ and u represent the rotation angle and rotation axis respectively, where the rotation axis has three directions xyz, and then calculate the Δq quaternion, q t represents the updated attitude quaternion, and R represents the calculated rotation matrix.

[0102] Then calculate the matrix in the translation direction. The specific calculation method is as follows:

[0103] v t =v 0 +a t Δt

[0104] p t =p 0 +v t Δt+0.5a t Δt 2

[0105] t rel =p t -p 0

[0106]

[0107]

[0108] Among them, v t Indicates the calculation speed, p t Indicates the calculated position, t rel Represents the relative translation vector, R rel represents the final calculated relative rotation matrix, where Represents the transposed matrix of the rotation matrix of the original frame, and finally T rel That is, the homogeneous transformation matrix between frames is calculated. After the required frame and the original frame are homogeneously transformed using the above calculation method, the transformed point cloud data of multiple frames are fused and added in the coordinate system of the original frame. The specific results are as follows: Figure 1 shown.

[0109] Step 2: Remove ground data and obstacles from the point cloud data after pose transformation and fusion; then rasterize the data after removing the ground data.

[0110] 1) Remove the ground and obstacles from the point cloud data after pose transformation and fusion; the process of removing the ground and obstacles is as follows: first filter out some points that are obviously not on the ground, then randomly select three points from the filtered data, fit the plane, and make a series of judgments; iterate this process, and finally select the best plane, remove it and the points in the plane, and the ground data removal operation is completed. Then cluster and segment the point cloud data to complete the removal of obstacles; the specific algorithm flow is as follows:

[0111] a) Set a height threshold, traverse the points in the point cloud dataset, and if the height value of the point is greater than the set height threshold, filter out the point, retain all points with heights within the threshold range, and generate a preliminarily filtered point cloud dataset;

[0112] b) setting the number of iterations, the in-plane point distance threshold, and the in-plane point number threshold in the point cloud data set after the preliminary filtering, and dividing the point cloud data set into the set number of regions;

[0113] c) For each divided point cloud area, three random point seeds are selected to fit a plane, the normal vector of the fitted plane is calculated, and the plane fitting is optimized based on the normal vector information;

[0114] d) Calculate the distance from each point in the area to the plane, and determine whether the distance is within the threshold range of the in-plane point distance. If it meets the range, add the point to the in-plane point set of the plane. If not, calculate the distance of the remaining points. After traversing all the points, determine whether the number of in-plane points of the plane is greater than or equal to the set in-plane point number threshold. If it meets the range, record it.

[0115] The plane and the number of points in it;

[0116] e) Repeat steps c and d until the set number of iterations is met, count the number of points in the plane recorded in all regions, and take the plane model with the most points in the plane as the ground model.

[0117] In the initial point cloud model, all points within the plane model are removed to complete the ground removal.

[0118] The operation process;

[0119] f) Then clustering and segmenting the point cloud data after removing the ground data;

[0120] g) Set the feature threshold, analyze each cluster, and determine its features including height, width, and depth. First, filter out clusters that are obviously not obstacles based on the feature threshold.

[0121] h) For the filtered point cloud data, detect the feature clusters that meet the obstacles, remove them from the point cloud data, and save the point cloud data.

[0122] 2) After removing the ground and obstacles from the point cloud data, rasterization is performed on it. The final result is as follows Figure 2 As shown, the specific steps are as follows:

[0123] a) defining the voxel size, so as to set the size of each voxel in the above data, determine the spatial range of the point cloud data after removing the ground, and define the coverage of the grid network with this spatial range;

[0124] b) Use a hash table to store the data structure of the voxel grid, traverse the point cloud data, calculate the voxel coordinates where it is located using the following formula, and assign the point cloud data points to the corresponding voxel grid, using the key-value pair form of the hash table, where the key is the voxel coordinate and the value is the list of point cloud data points contained in the voxel;

[0125]

[0126] Among them, the size of each voxel is voxel_size x , voxel_size y and voxel_size z , x, y, and z represent the three-dimensional coordinates of each point;

[0127] c) Traverse all non-empty voxel grids and calculate the characteristic point in each voxel, that is, the center point (or centroid). The specific calculation formula is as follows;

[0128]

[0129] Among them, centroid represents the position of the center of mass, N represents the number of points in the voxel set, and p i Represents the vector coordinates of the i-th voxel point;

[0130] d) Count the center point sets of all voxels to form rasterized point cloud data;

[0131] Step 3: Establish a mathematical model of equidistant continuous light wave detection based on the fused and processed point cloud data to obtain the characteristic results of the forward fork in the road. The specific results are as follows: Figure 3 shown.

[0132] a) Project the de-grounded point cloud data onto a two-dimensional plane, with the model center as the origin and set as the viewpoint coordinates (origin r ,origin c );

[0133] b) Traverse all occupied grids, count and calculate the angle of the light wave ray relative to the viewpoint, and the calculation formula is as follows:

[0134]

[0135] Where (s, t) is the occupied grid coordinate, (origin r ,origin c ) is the viewpoint coordinate, γ grid The angle from the grid to the point coordinates;

[0136] c) Based on the statistically calculated light ray angles, the occupied grid is divided into different angular regions, each covering a range of 1 degree, as shown in the following formula:

[0137]

[0138] Among them, k∈{1, 2, 3, ...., 360}, γ s,t Represents the angle value of the occupancy grid, γ section represents the set of occupancy grid coordinates in all corner regions;

[0139] d) Traverse all the obtained angular region sets, and for each angular region, calculate the Euclidean distance between the nearest grid and the viewpoint in the interval, and let the Euclidean distance be the light wave distance:

[0140]

[0141] Among them, Z k is the angular area γ section The collection of all grids in l k Represents the Euclidean distance of the grid closest to the viewpoint in each angular region, and normalizes all calculated Euclidean distances. For the area not calculated in the angular region, it means that all grids in the region are not occupied, and the light wave Euclidean distance corresponding to the area is set to 1. All angular regions and their corresponding regional light wave distances are stored;

[0142] e) On the basis of the original viewpoint, n virtual viewpoints are set at equal intervals in front, and the process from b to e is repeated based on each virtual viewpoint until all calculations of n virtual viewpoints are completed.

[0143] The process is completed, and the model established for all viewpoints is recorded as M 0 ,M 1 ,M 2 ,...,M n , each model stores its corresponding angular region and its regional light wave distance.

[0144] Step 4: Use the optimal position matching algorithm to extract and filter the result features of all the intersection detection results, and select the optimal intersection matching information as the final detection result. The specific results are as follows: Figure 4shown.

[0145] a) Set the threshold θ M And the error threshold μ M , starting from the positive direction of the x-axis, cycle counterclockwise for 360 times, and count the area where the light wave distance of each angular area in a single model is 1. This area is an unobstructed area. If the difference between two consecutive angular areas where the light wave distance is 1 is within μ M If it is within the range, it is still allocated in the same detection space, otherwise it is allocated as the next detection space and the cycle continues;

[0146] b) After the loop ends, count the number of angular regions with a distance of 1 in all detection spaces. If the sum of the numbers is greater than θ M , then the detection space is a valid detection space, otherwise the detection space is eliminated;

[0147] c) For model M 0 ,M 1 ,M 2 ,...,M n , execute steps a and b in sequence, and then calculate the average value of the angular area of ​​all valid detection spaces contained in each model as the final detection angle result of a single model, save it and record it as S i ;

[0148] d) S of all viewpoints i Merge into a total angle set and sort the set;

[0149] e) Set the similarity threshold θ S , set an empty temporary array and grouping vector, traverse the sorted total angle set, according to the set similarity threshold, if the current temporary group is empty, add the current angle to the temporary group, if the difference between the current angle and the last angle of the temporary group is within the threshold range, add the current angle to the temporary group, otherwise, add the temporary group to the grouping vector, clear the temporary group and add the current angle to the new temporary group, finally, if the temporary group is not empty, add it to the grouping vector;

[0150] f) checking whether the first angle of the first group of the grouped vectors and the last angle of the last group are within the similarity threshold range, if so, appending all the angles of the last group to the first group and removing the last group;

[0151] g) Set the segmentation threshold θ F , set an empty test segment angle vector and an old test angle value, traverse each group in the group vector, if the size of the group exceeds a certain proportion, calculate the median angle of the group, if the difference between the median angle and the previous median angle is less than the threshold θF , then skip the grouping, otherwise, add the median angle to the test segment angle vector and update the old test angle value. If the newly added median angle differs from the first median angle by less than 360 degrees, merge the two angles to obtain the final bifurcation angle set S f ;

[0152] h) Define the discreteness μμ i , as shown below, calculate all S i With S f The element discreteness of

[0153]

[0154] Among them, S fi and S ij Represent the final bifurcation angle S f The i-th angle value in the i-th viewpoint model and each angle value, μμ i is the final calculated discreteness value;

[0155] g) Statistics of all models and S f The element discreteness is selected, and the S with the smallest separation is selected. i The final test result is recorded as S i The bifurcation angle results and the viewpoint coordinates.

[0156] The present invention will be further described below in conjunction with the embodiments.

[0157] This embodiment is based on the following hardware platform. ① Acquisition platform: The laser radar is Livox MID-360 laser radar; the CPU is Intel Core i5-8259U; the memory is DDR4 2400MHz 16GB; the storage is Kioxia EXCERIAG2 SSD500GB; the laser radar and IMU are connected to NUC, and the NUC is connected to the mobile robot control chassis via USB. The power required by the device is provided by the lithium battery carried by the mobile robot itself. ② Experimental platform: The CPU is Intel Core i5-8259U; the memory is DDR4 2400MHz 16GB, and the storage is Kioxia EXCERIAG2 SSD 500GB.

[0158] This example is based on the following software environment: Computer operating system: Ubuntu 18.04, with ROS Melodic robot operating system.

[0159] Combination Figure 5 The present invention is a road fork detection method based on multi-frame continuous point cloud, and the specific implementation steps are as follows:

[0160] The first step is to use LiDAR and IMU to collect intersection point cloud and posture data; then perform posture transformation and fusion processing on the point cloud data of the corresponding frame according to the IMU data;

[0161] The second step is to remove ground data and obstacles from the point cloud data after pose transformation and fusion; then rasterize the data after removing the ground data to increase the subsequent calculation efficiency and save computing resources;

[0162] The third step is to establish a mathematical model of equidistant continuous light wave detection based on the fused and processed point cloud data to obtain the characteristic results of the forward fork in the road detection.

[0163] The fourth step is to use the optimal position matching algorithm to extract and screen the result features of all the intersection detection results, and select the optimal intersection matching information as the final detection result.

Claims

1. A road fork detection method based on multi-frame continuous point cloud, characterized in that: The following steps are involved: The first step is to use LiDAR and IMU to collect intersection point cloud and posture data; then perform posture transformation and fusion processing on the point cloud data of the corresponding frame according to the IMU data; The second step is to remove ground data and obstacles from the point cloud data after posture transformation and fusion; Then, the ground-removed data is rasterized to increase subsequent calculation efficiency and save computing resources; The third step is to establish a mathematical model of equidistant continuous light wave detection based on the fused and processed point cloud data to obtain the characteristic results of the forward fork in the road detection. The fourth step is to use the optimal position matching algorithm to extract and screen the result features of all the intersection detection results, and select the optimal intersection matching information as the final detection result.

2. The road fork detection method based on multi-frame continuous point cloud according to claim 1, characterized in that: The specific implementation steps of the first step are: 1) Synchronize the timestamps of IMU inertial navigation and radar point cloud data to synchronize data; point cloud data collected by lidar: contains multiple frames, each frame is a point cloud data; posture data collected by IMU: contains the angular velocity (ω) corresponding to each frame x ,ω y ,ω z ) and linear acceleration (a x ,a y ,a z ) six data; 2) Initialize each variable, including initial posture, velocity, and position; calculate the posture and position information of each frame and the relative homogeneous transformation matrix between frames by integrating the angular velocity and linear acceleration in the IMU data; the calculation method is as follows: Δθ=ωΔt q t =[w,x,y,z] Among them, Δt represents the time step, Δθ is the change in rotation angle, θ and u represent the rotation angle and rotation axis respectively, where the rotation axis has three directions xyz, and then calculate the Δq quaternion, q t represents the updated attitude quaternion, and R represents the calculated rotation matrix; Then calculate the matrix in the translation direction. The specific calculation method is as follows: v t =v0+a t Δt p t =p0+v t Δt+0.5a t Δt 2 t rel =p t -p0 Among them, v t Indicates the calculation speed, p t Indicates the calculated position, t rel Represents the relative translation vector, R rel represents the final calculated relative rotation matrix, where Represents the transposed matrix of the rotation matrix of the original frame, and finally T rel That is, the homogeneous transformation matrix between frames is calculated; after making a homogeneous transformation between the required frame and the original frame using the above calculation method, the transformed point cloud data of multiple frames are fused and added in the coordinate system of the original frame.

3. The road fork detection method based on multi-frame continuous point cloud according to claim 1, characterized in that: The specific implementation steps of the second step are: 1) Remove the ground and obstacles from the point cloud data after pose transformation and fusion; the process of removing the ground and obstacles is as follows: first filter out some points that are obviously not on the ground, then randomly select three points from the filtered data, fit the plane, and make a series of judgments; iterate this process, and finally select the best plane, remove it and the points in the plane, and the ground data removal operation is completed. Then cluster and segment the point cloud data to complete the removal of obstacles; the specific algorithm flow is as follows: a) Set a height threshold, traverse the points in the point cloud dataset, and if the height value of the point is greater than the set height threshold, filter out the point and retain all points whose height is within the threshold range. Generate a preliminary filtered point cloud dataset; b) setting the number of iterations, the in-plane point distance threshold, and the in-plane point number threshold in the point cloud data set after the preliminary filtering, and dividing the point cloud data set into the set number of regions; c) For each divided point cloud area, three random point seeds are selected to fit a plane, the normal vector of the fitted plane is calculated, and the plane fitting is optimized based on the normal vector information; d) Calculate the distance from each point in the area to the plane, and determine whether the distance is within the threshold range of the in-plane point distance. If it meets the range, add the point to the in-plane point set of the plane. If not, then calculate the distances of the remaining points. After traversing all the points, determine whether the number of points in the plane is greater than or equal to the set threshold of the number of points in the plane. If it is satisfied, record the plane and the number of points in the plane. e) Repeat processes c and d until the set number of iterations is met, count the number of in-plane points of the recorded planes in all regions, take the plane model with the most in-plane points as the ground model, and remove all in-plane points of the plane model from the original point cloud model; f) performing clustering and segmentation on the point cloud data after removing the ground data; g) Set feature thresholds, analyze each cluster, determine its features including height, width, and depth, and first filter out clusters that are obviously not obstacles based on the feature thresholds; h) For the filtered point cloud data, detect the feature clusters that meet the obstacles, remove them from the point cloud data, and save the point cloud data. 2) After removing the ground and obstacles from the point cloud data, rasterization is performed on it. The specific steps are as follows: a) defining the voxel size, so as to set the size of each voxel in the above data, determine the spatial range of the point cloud data after removing the ground, and define the coverage of the grid network with this spatial range; b) Traverse the point cloud data and calculate the voxel coordinates of the point cloud data points using the following formula, assign the point cloud data points to the corresponding voxel grid, and use the key-value pair form of the hash table, where the key is the voxel coordinate. The value is a list of point cloud data points contained in this voxel; Among them, the size of each voxel is voxel_size x , voxel_size y and voxel_size z , x, y, and z represent the three-dimensional coordinates of each point; c) Traverse all non-empty voxel grids and calculate the feature point in each voxel, i.e., the center point or mass point The specific calculation formula is as follows: Among them, centroid represents the position of the center of mass, N represents the number of points in the voxel set, and p i Represents the vector coordinates of the i-th voxel point; d) Count the center point sets of all voxels to form rasterized point cloud data.

4. The road fork detection method based on multi-frame continuous point cloud according to claim 1, characterized in that: The specific algorithm implementation steps of the third step are: a) Project the de-grounded point cloud data onto a two-dimensional plane, with the model center as the origin and set as the viewpoint coordinates (origin r ,origin c ); b) Traverse all occupied grids, count and calculate the angle of the light wave rays relative to the viewpoint, The calculation formula is as follows: Where (s, t) is the occupied grid coordinate, (origin r ,origin c ) is the viewpoint coordinate, γ grid The angle from the grid to the point coordinates; c) Divide the occupied grid into different angular regions according to the statistically calculated light ray angles, Each angular region covers a range of 1 degree, as shown in the following formula: Among them, k∈{1, 2, 3, ...., 360}, γ s,t represents the angle value of the occupancy grid, γ section represents the set of occupancy grid coordinates in all corner regions; e) Traverse all the obtained angular region sets, and for each angular region, calculate the Euclidean distance between the nearest grid and the viewpoint in the interval, and let the Euclidean distance be the light wave distance: Among them, Z k is the angular area γ section The collection of all grids in l k Represents the Euclidean distance of the grid closest to the viewpoint in each angular region, and normalizes all the calculated Euclidean distances. For the area not calculated in the angular region, the Euclidean distance of the light wave corresponding to the area is set to 1; f) On the basis of the original viewpoint, n virtual viewpoints are set at equal intervals in front of the original viewpoint. Repeat the process from b to e based on n virtual viewpoints until all calculations of n virtual viewpoints are completed. The process is completed, and the models established at all viewpoints are recorded as M0, M1, M2, ..., M n , in each model The corresponding angular area and its regional light wave distance are stored.

5. The method for detecting road forks based on multi-frame continuous point clouds according to claim 1, characterized in that: The specific algorithm implementation steps of the fourth step are: a) Set the threshold θ M And the error threshold μ M , starting from the positive direction of the x-axis, and circulating counterclockwise for 360 times, count the area where the light wave distance is 1 in each angular area of ​​a single model. For an unobstructed area, if the difference between the angular areas of two consecutive light wave distances of 1 is μ M Within the range, It is still assigned to the same detection space, otherwise it is assigned to the next detection space. Space, the cycle continues; b) After the loop is finished, count the number of corner regions with a distance of 1 in all detection spaces. If the sum of the numbers is greater than θ M , then the detection space is a valid detection space, otherwise the detection Space culling; c) For models M0,M1,M2,...,M n , execute steps a and b in sequence, and then calculate the average value of the angular area of ​​all valid detection spaces contained in each model as the final detection angle result of a single model, save it and record it as S i ; d) S of all viewpoints i Merge into a total angle set and sort the set; e) Set the similarity threshold θ S , set an empty temporary array and grouping vector, traverse the sorted total angle set, according to the set similarity threshold, if the current temporary group is empty, Then add the current angle to the temporary group. If the current angle is the same as the last angle in the temporary group If the angle difference is within the threshold range, the current angle is added to the temporary group; otherwise, Add the temporary group to the group vector, clear the temporary group and add the current angle to the new temporary group Group, finally, if the temporary group is not empty, add it to the group vector; f) checking whether the first angle of the first group of the grouped vectors and the last angle of the last group are within the similarity threshold range, if so, appending all the angles of the last group to the first group and removing the last group; g) Set the segmentation threshold θ F , set an empty test segment angle vector and an old test angle value, traverse each group in the group vector, if the size of the group exceeds a certain proportion, calculate the median angle of the group, if the difference between the median angle and the previous median angle is less than the threshold θ F , then skip the grouping, otherwise, add the median angle to the test segment angle vector and update the old test angle value. If the newly added median angle differs from the first median angle by less than 360 degrees, merge the two angles to obtain the final bifurcation angle set S f ; h) Define the discreteness μμ i , as shown below, calculate all S i With S f The element discreteness of Among them, S fi and S ij Represent the final bifurcation angle S f The i-th angle value in Each angle value in the i-th viewpoint model, μμ i is the final calculated discrete value; g) Statistics of all models and S f The element discreteness is selected, and the S with the smallest separation is selected. i The final test result is recorded as S i The bifurcation angle results and the viewpoint coordinates.

Images