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

Abstract

The application discloses a kind of road bifurcation detection methods based on multi-frame continuous point cloud.The method first collects the original point cloud data of intersection and records the pose information of each frame of laser radar IMU, then calculates the Euler angle of other frames relative to initial frame, obtains the rotation translation matrix of other frames under the coordinate system of original frame, after rotation translation, the point cloud data of other frames is fused with the data of original frame.The second multi-frame data is removed ground point cloud operation and rasterization operation, the light wave detection under polar coordinate system is established on the processed data to detect intersection feature, finally the optimal position matching algorithm is used to select the best result of intersection, including the number of intersection and intersection angle and position.The optimal result is saved to mobile robot, so that it is used for navigation task.The application enriches the information of intersection point cloud data by fusing multi-frame sparse point cloud data, and detects the intersection feature of intersection according to light wave detection, to more accurately detect the characteristics of intersection data.

Description

TECHNICAL FIELD

[0001] The present application belongs to the field of unmanned driving technology and intelligent transportation systems, and specifically relates to a method for detecting a fork in the road based on multi-frame point cloud data in an outdoor environment. BACKGROUND

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

[0003] In practical applications, road environments are complex and variable, with various dynamic and static obstacles such as pedestrians, vehicles, and buildings, making the detection of forks extremely challenging. Traditional image processing-based fork detection methods are susceptible to factors such as light changes, weather conditions, and occlusions 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, providing rich geometric information. However, in practical applications, single-frame point cloud data often cannot fully reflect the details of complex road environments due to limited data volume and perspective limitations. Therefore, the fusion and processing of multi-frame point cloud data have become an important means to improve the accuracy and robustness of fork detection. Currently, Livox lidar has become an important part of unmanned driving and intelligent transportation systems due to its unique scanning mode and efficient data acquisition capabilities. Some of its radars have IMU built-in, making data acquisition and synchronization easier. Unlike traditional mechanical lidars, Livox lidar uses a non-repeating scanning mode, which can cover a wider field of view in a shorter time while having 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 processing of multi-frame point cloud data, how to effectively remove the ground and obstacles, perform data rasterization, and accurately detect forks based on the multi-view beam model are technical difficulties. Removing ground data can avoid the influence of ground noise on the detection results, rasterization can simplify point cloud data and improve computational efficiency, and the beam model can simulate the propagation path of the beam in space to detect the location and angle of the fork. SUMMARY

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

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

[0008] First, use laser radar and IMU to collect intersection point cloud and pose data; then perform pose transformation and fusion processing on the point cloud data of the corresponding frame according to the IMU data;

[0009] Second, remove ground data and obstacles from the point cloud data after pose transformation and fusion; then perform rasterization on the data after ground removal to increase the computational efficiency and save computational resources;

[0010] Third, establish an equidistant continuous light wave detection mathematical model on the obtained fused and processed point cloud data to obtain the feature results of the front fork detection;

[0011] Fourth, use the optimal position matching algorithm to extract and filter the result features of all intersection detection results, and select the optimal intersection matching information as the final detection result.

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

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

[0014] 2) Initialize variables, including initial pose, speed and position. Calculate the pose, position information and relative homogeneous transformation matrix between frames by integrating the angular velocity and linear acceleration in the IMU data;

[0015] The calculation method is as follows:

[0016] Delta theta = omega delta t

[0017]

[0018]

[0019]

[0020]

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

[0022]

[0023] wherein, Δt represents a time step, Δθ is a rotation angle change, θ and u respectively represent a rotation angle and a rotation axis, wherein the rotation axis has three directions of xyz, and then a Δq quaternion is calculated, q t represents an updated attitude quaternion, and R represents a calculated rotation matrix.

[0024] Then, a matrix in the translation direction is calculated, and the specific calculation method is as follows:

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

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

[0027] t rel = p t -p0

[0028]

[0029]

[0030] wherein, v t represents a calculated velocity, p t represents a calculated position, t rel represents a relative translation vector, and R rel represents a finally calculated relative rotation matrix, wherein represents a transpose matrix of the rotation matrix of the original frame, and finally T rel , that is, a homogeneous transformation matrix between frames is calculated; by using the above calculation method, the homogeneous transformation between the required frame and the original frame is made, and then the fused and added point cloud data of multiple frames after transformation is obtained in the coordinate system of the original frame.

[0031] Further, in the second step, the point cloud data after pose transformation and fusion is subjected to ground and obstacle removal operation; the process of ground and obstacle removal operation is as follows: first, some points obviously not belonging to the ground are filtered out, then three points are randomly selected in the filtered data, a plane is fitted, and a series of judgments are made; the process is iterated, and finally the best plane is selected, and the points in the plane are removed, and the ground data removal operation is completed, then the point cloud data is subjected to clustering segmentation, and the obstacle removal is completed.

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

[0033] a) Set a height threshold, traverse the points in the point cloud data set, if the height value of the point is greater than the set height threshold, filter out the point, and keep all points within the threshold range, generate a preliminary filtered point cloud data set;

[0034] b) Set the number of iterations, in-plane point distance threshold and in-plane point number threshold in the preliminary filtered point cloud data set, and divide the point cloud data set into a certain number of regions;

[0035] c) In each divided point cloud region, randomly select three point seeds, fit a plane, calculate the normal vector of the fitted plane, and optimize the plane fitting combined with the normal vector information;

[0036] d) Calculate the distance of each point in the region to the plane, judge whether the distance is within the in-plane point distance threshold range, if it meets the range, add the point to the in-plane point set of the plane, if it does not meet, then calculate the distance of the remaining points, after traversing all points, judge whether the in-plane point number of the plane is greater than or equal to the set in-plane point number threshold, if it meets, record the

[0037] plane and its in-plane point number;

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

[0039] f) Then, the point cloud data after removing the ground data is subjected to clustering segmentation;

[0040] g) Set a feature threshold, analyze each cluster to determine its features including height, width and depth, first filter out the clusters obviously not belonging to the obstacle according to the feature threshold,

[0041] h) For the filtered point cloud data, detect the feature cluster conforming to the obstacle, remove it from the point cloud data, and save the point cloud data.

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

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

[0044] a) Define the voxel size 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 range of the grid network with the 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 it is in with the following formula, and assign the point cloud data points to the corresponding voxel grid. Use the key-value pair form of the hash table, the key is the voxel coordinate, and the value is the list of point cloud data points contained in the voxel;

[0046]

[0047] wherein 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, calculate the feature points, i.e. the center points (or centroids) in each voxel, and the specific calculation formula is as follows;

[0049]

[0050] wherein centroid represents the position of the centroid, N represents the number of points in the voxel set, p i represents the vector coordinates of the i-th voxel point;

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

[0052] Further, in the third step, the specific implementation steps of establishing an equidistant continuous light wave detection mathematical model and obtaining the feature results of the front fork detection are:

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

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

[0055]

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

[0057] c) According to the counted light wave ray angle, the occupied grid is divided into different angle regions, each angle region covers a range of 1 degree, as shown in the following formula:

[0058]

[0059] Where k ∈ {1, 2, 3,..., 360}, γ s,t represents the angle value of the occupied grid, γ section represents the occupied grid coordinate set in all angle regions;

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

[0061]

[0062] Where Z k is the set of all grids in the angle region γ section , l k represents the Euclidean distance of the nearest grid in each angle region from the viewpoint, and all the calculated Euclidean distances are normalized. For the region that is not calculated in the angle region, it means that all the grids in the region are not occupied, and the light wave Euclidean distance corresponding to the region is set to 1. Store all angle regions and their corresponding region light wave distances;

[0063] e) On the basis of the original viewpoint, set n virtual viewpoints at equal intervals in front, repeat the process of b-e based on each virtual viewpoint until all the calculations of the n virtual viewpoints are completed

[0064] The process is complete, and all the models established by the viewpoints are recorded as M0, M1, M2,..., M n , each model stores its corresponding angle region and region light wave distance.

[0065]

[0066] ​Further, in the fourth step, the optimal position matching algorithm is used to extract and filter the result features of all intersection detection results, and the specific implementation steps for selecting the optimal intersection matching information are as follows:

[0067] a) Set a threshold θ M and an error threshold μ M , and count the number of regions with a light wave distance of 1 in each corner region of a single model in a clockwise direction for 360 times, wherein the region

[0068] with a light wave distance of 1 is an unobstructed region, and if the difference between two consecutive corner regions with a light wave distance of 1 is within the range of μ M , the two regions are still assigned to the same detection space, otherwise, the two regions are assigned to the next detection space, and the loop continues.

[0069] b) After the loop ends, count the number of regions with a light wave distance of 1 in each corner region of all detection spaces, and if the total number is greater than θ M , the detection space is a valid detection space, otherwise, the detection space is removed.

[0070] c) For models M0, M1, M2,..., M n , perform steps a and b in sequence, and then calculate the average value of the corner regions of all valid detection spaces contained in each model as the final detection angle result of the single model, and save and record it as S i .

[0071] d) Combine S i of all viewpoints into a total angle set, and sort the set.

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

[0073] f) Check whether the first angle of the first grouping and the last angle of the last grouping of the grouping vector are within the similarity threshold range, if yes, append all angles of the last grouping to the first grouping, and remove the last grouping.

[0074] g) Set a segmentation threshold θ FSet an empty test segment angle vector and an old test angle value. Iterate through each group in the grouping vector. If the size of a group exceeds a certain value...

[0075] For example, calculate the median angle of the group, and if the median angle is the same as the previous median angle...

[0076] The difference in degree is less than the threshold θ F If the median angle is not found in the target group, skip that group; otherwise, add the median angle to the measurement.

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

[0078] If the first median angles differ by less than 360 degrees, then these two angles are merged to obtain the final set of bifurcation angles S. f ;

[0079] h) Define the dispersion μ i As shown below, calculate all S i With S f The element dispersion;

[0080]

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

[0082] g) Statistically compare all models with S f The element dispersion is considered, and the S with the smallest separation is selected. i For the final test result, record S. i The bifurcation angle results and viewpoint coordinates.

[0083] Compared with the prior art, the present invention has the following significant advantages: (1) High precision: By using multi-frame continuous point cloud data and IMU data for pose transformation and fusion processing, noise and errors in single-frame point cloud data can be effectively reduced, and the point cloud data of each frame can be accurately aligned, thereby improving the detection accuracy. (2) High efficiency: When processing point cloud data, removing the ground and obstacles first can significantly reduce the interference of irrelevant data and improve the computational efficiency. Rasterizing the point cloud data can effectively reduce the amount of data, improve the speed and efficiency of subsequent calculations, and further save computational resources. (3) Strong robustness: By establishing a mathematical model for continuous light wave detection with equal spacing, it can better cope with intersections of different types and shapes, and improve the robustness and applicability of detection. Attached Figure Description

[0084] Figure 1 For the multi-frame fusion point cloud view after pose conversion.

[0085] Figure 2 For the top-down grid map after ground and obstacle removal.

[0086] Figure 3 For the mathematical model of light wave detection.

[0087] Figure 4 For the optimal position result map based on the mathematical model of light wave detection.

[0088] Figure 5 For the flowchart of the method for detecting a fork in an outdoor environment based on multi-frame point cloud data. DETAILED DESCRIPTION

[0089] The present application is based on a multi-frame continuous point cloud road fork detection method, and the specific implementation manner comprises the following steps:

[0090] First step: collect intersection point cloud and pose data using laser radar and IMU; then perform pose transformation and fusion processing on the point cloud data of the corresponding frame according to the IMU data.

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

[0092] 2) initialize variables, including initial pose, speed and position. Calculate the attitude, position information and relative homogeneous transformation matrix between frames of each frame by integrating the angular velocity and linear acceleration in the IMU data;

[0093] The calculation method is as follows:

[0094] Δθ=ωΔt

[0095]

[0096]

[0097]

[0098]

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

[0100]

[0101] wherein, Δt represents a time step, Δθ is a rotation angle change, θ and u respectively represent a rotation angle and a rotation axis, wherein the rotation axis has three directions of xyz, and then the Δq quaternion is calculated, q t represents an updated attitude quaternion, and R represents a calculated rotation matrix.

[0102] Then, the matrix in the translation direction is calculated, and the specific calculation method is as follows:

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

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

[0105] t rel = p t -p0

[0106]

[0107]

[0108] wherein, v t represents a calculated velocity, p t represents a calculated position, t rel represents a relative translation vector, and R rel represents a finally calculated relative rotation matrix, wherein represents a transpose matrix of the rotation matrix of the original frame, and finally T rel , that is, a homogeneous transformation matrix between frames is calculated; after the required frame and the original frame are subjected to homogeneous transformation by using the above calculation method, the point cloud data of multiple frames after transformation is fused and added in the coordinate system of the original frame, and the specific result is as shown in Figure 1 .

[0109] Second step: remove ground data and obstacles from the point cloud data after pose transformation and fusion; then, rasterize the data after ground removal.

[0110] 1) After the pose transformation and fusion of the point cloud data, the ground and obstacle removal operation is performed; the process of ground and obstacle removal operation is as follows: first, filter out some points obviously not belonging to the ground, then randomly select three points in the filtered data, fit a plane, and make a series of judgments; iterate this process, finally select the best plane, remove it and the in-plane points, and the ground data removal operation is completed, then the point cloud data is clustered and segmented to complete the removal of obstacles; the specific algorithm flow is:

[0111] a) Set the height threshold, traverse the points in the point cloud data set, if the height value of the point is greater than the set height threshold, filter out the point, keep all points within the threshold range, generate the preliminary filtered point cloud data set;

[0112] b) Set the iteration number, in-plane point distance threshold and in-plane point number threshold in the preliminary filtered point cloud data set, divide the point cloud data set into a certain number of regions;

[0113] c) In each divided point cloud region, randomly select three point seeds, fit a plane, calculate the normal vector of the fitted plane, and optimize the plane fitting combined with the normal vector information;

[0114] d) Calculate the distance of each point in the region to the plane, judge whether its distance is within the in-plane point distance threshold range, if it meets the range, add the point to the in-plane point set of the plane, if it does not meet, then calculate the distance of the remaining points, after traversing all points, judge whether the in-plane point number of the plane is greater than or equal to the set in-plane point number threshold, if it meets, record the

[0115] plane and its in-plane point number;

[0116] e) Repeat the process of c and d until the set iteration number is met, count the in-plane point number of the recorded plane in all regions, take the plane model with the most in-plane points as the ground model, remove all in-plane points in the plane model from the original

[0117] point cloud model, and thus complete the ground removal operation process;

[0118]

[0119] f) Then, the point cloud data after removing the ground data is clustered and segmented;

[0120] g) Set the feature threshold, analyze each cluster to determine its features including height, width and depth, first filter out the clusters obviously not belonging to obstacles according to the feature threshold,

[0121] h) For the filtered point cloud data, detect the feature clusters meeting the obstacle, 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, perform a rasterization operation, and the final result is as shown in FIG. 3B, and the specific steps are as follows: Figure 2

[0123] a) Define the voxel size 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 range of the grid network with the spatial range;

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

[0125]

[0126] wherein 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, calculate the feature points, i.e. the center points (or centroids) in each voxel, and the specific calculation formula is as follows:

[0128]

[0129] wherein centroid represents the position of the centroid, 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 set of all voxels to form the rasterized point cloud data;

[0131] Step 3: Establish a continuous equal-interval light wave detection mathematical model on the obtained fused and processed point cloud data to obtain the feature results of the front fork detection, and the specific results are as shown in FIG. 4B. Figure 3

[0132] a) Project the ground-removed and rasterized point cloud data on a two-dimensional plane, and set the model center as the origin, and set the viewpoint coordinates (origin r , origin c );

[0133] b) Traverse all occupied grids, count and calculate the light wave ray angle 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, and γ grid is the angle value from the grid to the viewpoint coordinate;

[0136] c) According to the statistical light wave ray angle, the occupied grid is divided into different angle regions, each angle region covers a range of 1 degree, as shown in the following formula:

[0137]

[0138] where k e {1, 2, 3,..., 360}, γ s,t represents the angle value of the occupied grid, and γ section represents the set of occupied grid coordinates in all angle regions;

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

[0140]

[0141] where Z k is the set of all grids in the angle region γ section , and l k represents the Euclidean distance of the nearest grid to the viewpoint in each angle region, and all calculated Euclidean distances are normalized. For the region that is not calculated, it means that all grids in the region are not occupied, and the light wave Euclidean distance corresponding to the region is set to 1. Store all angle regions and their corresponding region light wave distances;

[0142] e) On the basis of the original viewpoint, set n virtual viewpoints at equal intervals in front, repeat the process of b-e based on each virtual viewpoint until the calculation of all virtual viewpoints is complete

[0143] The process is complete, and all viewpoints establish a model M0, M1, M2,..., M n , each model stores its corresponding angle region and its region light wave distance.

[0144] Fourth step: using the optimal position matching algorithm to extract and filter the result features of all intersection detection results, selecting the optimal intersection matching information as the final detection result, and the specific results are shown in Figure 4 .

[0145] a) Set the threshold θM and error threshold μ M , from the positive direction of the x-axis, counterclockwise cycle 360 times, statistics in each single model in each corner area of the light wave distance is 1, the area is no shelter area, if the difference between the two consecutive light wave distance is 1 Angle area in the range of μ M , it is still assigned in the same detection space, otherwise it is assigned to the next detection space, the cycle continues;

[0146] b) after the end of the cycle, statistics in each corner area of all detection space distance is 1, if the number of total sum is greater than θ M , the detection space is effective detection space, otherwise the detection space is rejected;

[0147] c) for model M0, M1, M2,..., M n , in turn, a, b step, then calculate the average value of the corner area contained in each model of all effective detection space as the final detection angle result of single model, save and record as S i ;

[0148] d) all the view of S 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 total angle set after sorting, according to the set similarity threshold, if the current temporary grouping is empty, the current angle is added to the temporary grouping, if the current angle and the last angle of the temporary grouping are within the threshold range, the current angle is added to the temporary grouping, otherwise, the temporary grouping is added to the grouping vector, empty temporary grouping and add the current angle to the new temporary grouping, finally, if the temporary grouping is not empty, it is added to the grouping vector;

[0150] f) check whether the first angle of the first group and the last angle of the last group of the grouping vector are within the similarity threshold range, if so, append all angles of the last group to the first group and remove the last group;

[0151] g) set the segmentation threshold θ F , set an empty test segmentation angle vector and an old test angle value, traverse each group in the grouping 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 θ FIf not, the median angle is added to the test segment angle vector and the old test angle value is updated. If the newly added median angle is within 360 degrees of the first median angle, the two angles are merged to obtain the final bifurcation angle set S f ;

[0152] h) define the dispersion μ i As shown below, all S i are calculated f element dispersion of S

[0153]

[0154] Where S fi and S ij represent the i-th angle value in the final bifurcation angle S f and each angle value in the i-th viewpoint model respectively, μ i is the last calculated dispersion value;

[0155] g) Calculate the element dispersion of all models and S f , select the S i with the smallest separation degree as the final detection result, and record the bifurcation angle result in S i and the viewpoint coordinates.

[0156] The application will be further described below with reference to 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 EXCERIAG2 SSD500GB; the laser radar and the IMU are connected to the NUC, and the NUC is connected to the mobile robot control chassis through the USB, and 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 EXCERIAG2 SSD 500GB.

[0158] This embodiment is based on the following software environment. Computer operating system: Ubuntu18.04, equipped with ROS Melodic robot operating system.

[0159] In combination with Figure 5 , the application is a road bifurcation detection method based on multiple frames of continuous point clouds, and the specific implementation steps are as follows:

[0160] Firstly, the laser radar and IMU are used to collect the intersection point cloud and pose data; then the point cloud data of the corresponding frame is pose transformed and fused according to the IMU data;

[0161] Secondly, the ground data and obstacles of the point cloud data after the pose transformation and fusion are removed; then the data after the ground removal is rasterized to increase the subsequent calculation efficiency and save the calculation resources;

[0162] Thirdly, the equal-interval continuous light wave detection mathematical model is established on the obtained fused and processed point cloud data, and the characteristic results of the front fork intersection detection are obtained;

[0163] Fourthly, the optimal position matching algorithm is used to extract and screen the result characteristics of all the intersection detection results, and the optimal intersection matching information is selected as the final detection result.

Claims

1. A method for detecting road forks based on multi-frame continuous point clouds, characterized in that, Includes the following steps: The first step is to use lidar and IMU to collect point cloud and pose data of the intersection; then, based on the IMU data, the point cloud data of the corresponding frames are subjected to pose transformation and fusion processing. The second step is to remove ground data and obstacles from the point cloud data after pose transformation and fusion. The data after removing the ground plane is then rasterized to increase subsequent computational efficiency and save computational resources. The third step is to establish an equidistant continuous light wave detection mathematical model on the obtained fused and processed point cloud data to obtain the feature results of the forward intersection detection. The fourth step is to use the optimal location matching algorithm to extract and filter the features of all the intersection detection results, and select the optimal intersection matching information as the final detection result. The specific algorithm implementation steps for the third step are as follows: a) Project the de-rasterized point cloud data onto a 2D plane, with the model center as the origin, and set it as the viewpoint coordinate (origin). r ,origin c ); b) Traverse all occupied grid cells, count and calculate their angles relative to the viewpoint. The calculation formula is as follows: Where (s,t) are the occupied raster coordinates, (origin) r ,origin c ) represents the viewpoint coordinates, γ grid This represents the angle value from the grid to the viewpoint coordinates; c) Based on the statistically determined angles of the light rays, the occupied grid is divided into different corner regions, each covering a range of 1 degree, as shown in the following formula: Where, k∈{1, 2, 3, ..., 360}, γ s,t γ represents the angle value of the occupied grid. section Represents the set of occupied grid coordinates across all corner regions; e) Traverse all the obtained corner regions. For each corner region, calculate the Euclidean distance between the nearest grid cell and the viewpoint within that region, and let this Euclidean distance be the light wave distance: Among them, Z k For the angular region γ section The set of all grid cells within, l k This represents the Euclidean distance of the grid closest to the viewpoint in each corner region, and normalizes all calculated Euclidean distances. For areas in the corner region that are not calculated, the corresponding light wave Euclidean distance is set to 1. f) Based on the original viewpoint, set n virtual viewpoints at equal intervals in front. Repeat steps b to e based on each virtual viewpoint until all calculations for the n virtual viewpoints are completed. The models established for all viewpoints are denoted as M0, M1, M2, ..., M n Each model stores its corresponding angular region and its region's optical distance.

2. The road fork detection method based on multi-frame continuous point cloud according to claim 1, characterized in that, The specific steps implemented in the first step are as follows: 1) Synchronize the timestamps of the IMU inertial navigation and radar point cloud data for data synchronization; Point cloud data acquired by the lidar: contains multiple frames, each frame being one point cloud data; Pose data acquired by the IMU: contains the angular velocity (ω) corresponding to each frame. x ,ω y ,ω z ) and linear acceleration (a x ,a y ,a z Six data points; 2) Initialize all variables, including initial pose, velocity, and position; calculate the pose, position information, and relative homogeneous transformation matrix between frames for each frame by integrating the angular velocity and linear acceleration from the IMU data; the calculation method is as follows: Δθ=ωΔt Where Δt represents the time step, Δθ is the change in rotation angle, θ and u represent the rotation angle and rotation axis, respectively, with the rotation axis having three directions: x, y, and z. Then, the quaternion Δq is calculated, where q... t R represents the updated pose quaternion, and R represents the calculated rotation matrix; Next, the matrix in the translation direction is calculated, and 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 p represents the calculation speed. t Indicates the calculated position, t rel R represents the relative translation vector. rel Represents the final calculated relative rotation matrix, where The transpose of the rotation matrix of the original frame, T rel This means calculating the homogeneous transformation matrix between frames; using the above calculation method, after performing a homogeneous transformation between the required frame and the original frame, the point cloud data of multiple frames after transformation 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 as described in claim 1, characterized in that, The specific implementation steps of the second step are as follows: 1) Remove ground and obstacle data from the point cloud data after pose transformation and fusion. The process for removing ground and obstacle data is as follows: First, filter out points that are clearly not part of the ground. Then, randomly select three points from the filtered data, fit a plane, and make a series of judgments. Iterate this process until the best plane is selected, and remove it and the points within the plane. This completes the ground data removal operation. Next, cluster and segment the point cloud data to remove obstacles. The specific algorithm flow is as follows: a) Set a height threshold, iterate through the points in the point cloud dataset, and if the height value of a point is greater than the set height threshold, filter out the point and keep all points whose height is within the threshold range to generate a preliminary filtered point cloud dataset. b) In the point cloud dataset after initial filtering, set the number of iterations, the in-plane point distance threshold, and the in-plane point number threshold to divide the point cloud dataset into a set number of regions; c) For each segmented point cloud region, three random point seeds are used to fit a plane, the normal vector of the fitted plane is calculated, and the plane fitting is optimized by combining the normal vector information. d) Calculate the distance from each point within the region to the plane, and determine whether the distance is within the in-plane point distance threshold. If it meets the threshold, add the point to the in-plane point set of the plane. If not satisfied, then calculate the distance to the remaining points. After traversing all points, determine whether the number of points in the plane is greater than or equal to the set threshold for the number of points in the plane. If satisfied, record the plane and the number of points in it. e) Repeat steps c and d until the set number of iterations is met. Count the number of in-plane points of the planes recorded in all regions. Use the plane model with the most in-plane points as the ground model. Remove all in-plane points from the plane model in the original point cloud model. f) Cluster and segment the point cloud data after removing ground data; g) Set feature thresholds, 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 thresholds. h) For the filtered point cloud data, detect feature clusters that match 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, perform a rasterization operation. The specific steps are as follows: a) Define the voxel size 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 raster network based on this spatial range. b) Traverse the point cloud data, calculate the voxel coordinates of each point using the following formula, and assign the point cloud data points to the corresponding voxel grids using a hash table in key-value pair format, where the key is the voxel coordinate. The value is a list of point cloud data points contained in that voxel; 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 points, i.e., the center points or mass points, in each voxel. The specific calculation formula for heart is shown below; Where centroid represents the location of the centroid, N represents the number of points in the voxel set, and p i The vector coordinates representing the i-th voxel; d) Calculate the set of center points of all voxels to form rasterized point cloud data.

4. The road fork detection method based on multi-frame continuous point cloud as described in claim 1, characterized in that, The specific algorithm implementation steps for the fourth step are as follows: a) Set the threshold θ M and error threshold μ M Starting from the positive x-axis, rotate counterclockwise 360 ​​times. Count the regions in each corner of a single model where the light wave distance is 1. These regions are considered unobstructed. If the difference between two consecutive corner regions with a light wave distance of 1 is within μ... M If it is within the range, it will still be assigned to the same detection space; otherwise, it will be assigned to the next detection space, and the cycle will continue. b) After the loop ends, count the number of corner regions in all detection spaces with a distance of 1. If the total number of such regions is greater than θ, then... M If the detection space is valid, then the detection space is valid; otherwise, the detection space is discarded. c) For model M0, M1, M2, ..., M n Steps a and b are executed sequentially. Then, the average value of the angular regions of all valid detection spaces contained in each model is calculated as the final detection angle result for a single model, and this value is saved and denoted as S. i ; d) S of all viewpoints i Merge them into a single overall angle set and sort that set; e) Set the similarity threshold θ S Set an empty temporary array and a grouping vector. Iterate through 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. f) Check if the first angle of the first group and the last angle of the last group of the grouped vectors are within the similarity threshold. If they are, append all the angles of the last group to the first group and remove the last group. g) Set the segment threshold θ F Set an empty test segment angle vector and an old test angle value. Iterate through each group in the group vector. If the size of a group exceeds a certain proportion, calculate the median angle of that group. If the difference between the median angle and the previous median angle is less than a threshold θ, then... F If the median angle is within 360 degrees of the first median angle, skip that group; 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 set of bifurcation angles S. f ; h) Define the dispersion μ i As shown below, calculate all S i With S f The element dispersion; Among them, S fi and S ij These represent the final bifurcation angle S. f The i-th angle value and For each angle value in the i-th viewpoint model, μ i g) Statistically calculate the dispersion value for all models and S. f The element dispersion is considered, and the S with the smallest separation is selected. i For the final test result, record S. i The bifurcation angle results and viewpoint coordinates.

Images