Unmanned vehicle obstacle detection method based on laser radar

Abstract

The invention belongs to the technical field of measurement for preventing collision, and particularly relates to an unmanned vehicle obstacle detection method based on a laser radar, which comprises the following steps: S1, acquiring vehicle laser radar point cloud data and managing the point cloud data by using KDTree; s2, calculating a mixed distance between radar point clouds; and S3, according to the relative distance between the point cloud and the sensor, calculating an obstacle mixing distance threshold and carrying out obstacle judgment. According to the invention, the problem that the detection precision is reduced because the measurement result of the sensor is easily influenced by noise, reflecting objects and environmental changes when the laser radar carries out obstacle detection in a complex environment is solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of measurement for anti-collision purposes, and particularly relates to an obstacle detection method for driverless vehicles based on lidar. Background Art

[0002] The realization of driverless depends on the vehicle's perception of external information, and an efficient and reliable obstacle detection method is crucial. Currently, the sensors commonly used for obstacle detection mainly include lidar and cameras. Cameras are easily affected under the condition of large changes in ambient light conditions and cannot directly provide accurate depth information. Therefore, their obstacle detection ability in complex environments is relatively limited. Lidar has strong anti-interference ability, can work stably under various light conditions, and provides accurate distance and reflection intensity information. Thus, it has been widely used in the field of driverless.

[0003] Currently, using lidar for obstacle detection, especially in structured environments, has made certain progress. By scanning the point cloud data in the environment with lidar, the system can identify and locate obstacles. However, the existing lidar obstacle detection methods still face some challenges, including insufficient robustness, missed detection and false detection problems. Especially in complex environments, the measurement results of the sensor are easily affected by noise, reflective objects and environmental changes, resulting in a decrease in detection accuracy, and further affecting the reliability and safety of the system. Summary of the Invention

[0004] The present invention aims to provide an obstacle detection method for driverless vehicles based on lidar to solve the problem that the measurement results of the sensor are easily affected by noise, reflective objects and environmental changes, resulting in a decrease in detection accuracy.

[0005] To achieve the above object, the solution of the present invention is: an obstacle detection method for driverless vehicles based on lidar, including the following steps:

[0006] S1: Obtain the lidar point cloud data of the vehicle and manage the point cloud data using KDTree;

[0007] S2: Calculate the mixed distance between the lidar point clouds;

[0008] S3: Calculate the obstacle mixed distance threshold according to the relative distance between the point cloud and the sensor and perform obstacle detection.

[0009] Optionally, in step S1, it further includes the step:

[0010] S11: Construct a KDTree for the point cloud data, use the K-nearest neighbor algorithm to find the neighborhood of each data point p, and sort the data points in the neighborhood in descending order according to the Euclidean distance to the p data point.

[0011] In this solution, the beneficial effects of using the K-nearest neighbor algorithm to find the neighborhood of each data point p are as follows:

[0012] (1) Discreteness of point cloud data: The point cloud data collected by lidar is often sparse and unstructured, and direct analysis is easily affected by noise. Therefore, it is necessary to find the local neighborhood of each point for more accurate subsequent processing.

[0013] (2) Local feature extraction: Through the K-nearest neighbor algorithm, the distribution of each point in the local space can be determined, which helps to calculate key features such as curvature, normal vector, and density, providing a basis for operations such as classification and segmentation.

[0014] (3) Improving computational efficiency: With the support of the KDTree structure, the K-nearest neighbor algorithm can achieve efficient nearest neighbor search, significantly reducing the computational complexity of neighborhood search in point cloud processing and improving the overall processing efficiency.

[0015] Optionally, in step S11, the K-nearest neighbor algorithm to find the neighborhood of each data point p specifically includes the following steps:

[0016] S111: Initialize a priority queue W to store the N nearest adjacent points of p found currently, and maintain the maximum distance dist among the N nearest adjacent points of p found currently. Initialize the values in the priority queue W to infinity; max

[0017] S112: During the search process, traverse the nodes of the KDTree and calculate the Euclidean distance from the query point q to the current node R p If the dist(q, R p ) is less than the current maximum distance dist max , then update the adjacent points in the priority queue W and update dist max ;

[0018] Among them, the calculation method of the Euclidean distance from the query point q to the current node R p is as follows:

[0019]

[0020] In the above formula, (q (j) - R p (j) ) 2 represents the square of the distance difference between the query point and the node in the j-th dimension, and d represents the number of feature dimensions of the sample;

[0021] S113: When reaching the leaf node of the KDTree during the recursive process, check all the points in the leaf node and add the point with the closest distance to the priority queue W.

[0022] Optionally, in step S2, the calculation of the mixing distance between radar point clouds includes:

[0023] S21: Calculate the Euclidean distance between radar point clouds:

[0024]

[0025] In the formula, x i , y i , z i and x t , y t , z t are the coordinate values of point i and point t in the Cartesian coordinate system, respectively;

[0026] S22: Calculate the cosine similarity between radar point clouds:

[0027] Use the first adjacent points of the data points sorted in descending order in the KDTree to construct the covariance matrix F, and then perform eigenvalue decomposition on the covariance matrix F;

[0028]

[0029] In the above formula: F represents a 3×3 covariance matrix; represents the mean vector of the first N / 2 adjacent points of p, and T is the matrix transpose symbol;

[0030] S23: Obtain the eigenvalues and eigenvectors corresponding to the covariance matrix F through eigenvalue decomposition:

[0031] Fν k = λ k ν k

[0032] In the above formula: λ k is the eigenvalue of F, and ν k is the eigenvector of F; assume that λ0 is the smallest eigenvalue of F and ν0 is the corresponding eigenvector;

[0033] The cosine similarity threshold is:

[0034] C (i,t) = min(arccos(ν0 (i) , ν0 (t) ), π - arccos(ν0 (i) , ν0 (t) ))

[0035] In the above formula: C (i,t) is the cosine similarity; point t is any point in the neighborhood point set of point i; ν0 (i)The eigenvector corresponding to the minimum eigenvalue of point i, ν0 (t) The eigenvector corresponding to the minimum eigenvalue of point t;

[0036] S24: Calculate the custom mixed distance combining Euclidean distance and cosine similarity:

[0037]

[0038] In the above formula: G (i,t) Is the custom mixed distance between point i and point t; Is the weight factor controlling the importance of the Euclidean distance; D (i,t) Is the Euclidean distance between point i and point t; β is the weight factor controlling the importance of the cosine similarity; C (i,t) Is the cosine similarity between point i and point t.

[0039] With the help of step S11: The local neighborhood determined by K-nearest neighbors can calculate the mixed distance between point clouds more accurately, making the subsequent obstacle detection more robust and avoiding misjudgment caused by noise or outliers.

[0040] Optionally, in step S24, The value range of is 0 to 5, and the value range of β is 0 to 10.

[0041] Optionally, in step S3, it further includes the step:

[0042] S31: Calculate the obstacle mixed distance threshold as follows:

[0043]

[0044] In the above formula: G th Is the obstacle mixed distance threshold; d min Is the minimum threshold for preventing misdetection at close range; k1 is the linear gain coefficient at close range; θ is the vertical angular resolution of the lidar; [r] is the integer part of the distance of the point cloud from the sensor; r th Is the distance segmentation threshold for distinguishing the point cloud closer to the distance sensor from the point cloud farther from the distance sensor; l is the long-distance adjustment coefficient to prevent the obstacle mixed distance threshold from being too large; k2 is the long-distance gain coefficient.

[0045] With the help of the local neighborhood information provided by the K-nearest neighbor algorithm in step S11, it helps to calculate the spatial distribution characteristics of the point cloud, so as to set the obstacle mixed distance threshold more accurately and improve the accuracy of obstacle detection.

[0046] Optionally, in step S3, it further includes the step:

[0047] S33: Judge the obstacle as follows:

[0048]

[0049] When G (i,t) ≤G th , it is considered that point i and point t belong to points on the same obstacle, and they are classified into the same category;

[0050] When G (i,t) >G th , it is considered that point i and point t belong to points on different obstacles.

[0051] The present invention comprehensively considers the spatial distance and directional characteristics of point cloud data, and proposes an innovative hybrid distance metric method. This method combines Euclidean distance and cosine similarity to quantify the relative relationship between point clouds. Compared with the traditional Euclidean distance, it can more effectively handle problems such as point cloud density change, noise interference, and local structural features, overcoming the limitations of traditional methods in complex environments. Secondly, aiming at the characteristic that the point cloud density fluctuates significantly with the distance between the sensor and the obstacle, the present invention proposes a spatial partitioning strategy based on nested circular regions. This strategy divides the horizontal space into multiple nested circular regions surrounding the sensor, and sets adaptive custom hybrid distance thresholds for each region. In this way, the present invention significantly improves the obstacle detection accuracy and reliability within different distance ranges. Description of the Drawings

[0052] Figure 1 is a flowchart of an obstacle detection method for a driverless vehicle based on lidar in an embodiment of the present invention;

[0053] Figure 2 is a schematic diagram of 3D lidar obstacle point clouds at different distances in an embodiment of the present invention;

[0054] Figure 3 is a schematic diagram of obstacle hybrid distance thresholds at different distances in an embodiment of the present invention;

[0055] Figure 4 is a schematic diagram of the experimental platform structure in an embodiment of the present invention;

[0056] Figure 5 is a detection effect diagram of jungle obstacles in an embodiment of the present invention. Detailed Embodiment

[0057] The following is a further detailed description through specific embodiments:

[0058] The marks in the attached drawings of the specification include: binocular camera 1, lidar 2, and industrial computer 3.

[0059] Embodiment

[0060] The experimental platform adopted in this embodiment: as shown in the attached Figure 4 driverless vehicle, this platform is configured with a binocular camera 1, a 16-line lidar 2, and an industrial control computer 3. All sensor data has been connected to the industrial control computer 3 and can be obtained through the topic communication method of ROS.

[0061] A method for obstacle detection of a driverless vehicle based on lidar in this embodiment includes the following steps:

[0062] S01: Configure and initialize the lidar 2 driver program to ensure that the lidar 2 can normally output point cloud data.

[0063] S02: Connect the output data stream of the lidar 2 to the ROS system through the driver interface of ROS.

[0064] S03: Use the Publisher and Subscriber mechanisms in ROS to achieve data transmission. Specifically, the lidar driver program will publish the real-time generated point cloud data to a specified topic, and other nodes can obtain this data by subscribing to the corresponding topic.

[0065] S04: In the process of processing point cloud data, adopt the ROS sensor_msgs / PointCloud2 message format to ensure that the point cloud data can be correctly parsed and passed to the subsequent processing module, that is, the working condition machine 4.

[0066] S11: Use KDTree to manage point cloud data; the specific steps are as follows:

[0067] Construct a KDTree for the point cloud data, use the K-nearest neighbor algorithm to find the neighborhood of each data point p, and sort the data points in the neighborhood in descending order according to the Euclidean distance to the p data point;

[0068] S111: Initialize a priority queue W to store the N nearest adjacent points of the current p found, and maintain the maximum distance dist among the N nearest adjacent points of the current p found max , initialized to infinity;

[0069] S112: During the search process, traverse the nodes of the KDTree and calculate the Euclidean distance from the query point q to the current node R p If dist(q, R p ) is less than the current maximum distance dist max , then update the adjacent points in the priority queue W and update dist max ;

[0070] The Euclidean distance from the query point q to the current node R pEuclidean distance:

[0071]

[0072] In the above formula, (q (j) -R p (j) ) 2 represents the square of the difference in distance between the query point and the node in the j-th dimension. d represents the number of feature dimensions of the sample. In this embodiment, d is taken as 3; dist(q, R p ) represents the spatial distance from the query point q to the current node R p ; if dist(q, R p ) is less than the current maximum distance dist max , then update the nearest neighbor points in the priority queue W and update dist max . In the entire search, the KDTree uses the partitioning hyperplane for pruning to reduce unnecessary searches.

[0073] S113: When reaching the leaf node of the KDTree during the recursive process, check all the points in the leaf node and add the point with the closest distance to the priority queue W.

[0074] S2: Calculate the mixed distance of Euclidean distance and cosine similarity between radar point clouds; the specific steps are as follows:

[0075] S21: Calculate the Euclidean distance between radar point clouds:

[0076]

[0077] In the above formula, D (i,t) is the Euclidean distance between any two point clouds i and t; x i , y i , z i and x t , y t , z t are the coordinate values of the two point clouds in the Cartesian coordinate system respectively.

[0078] S22: Calculate the cosine similarity between radar point clouds:

[0079] Use the first adjacent points of the data points sorted in descending order in the KDTree to construct the covariance matrix F, and then perform eigenvalue decomposition on the covariance matrix F;

[0080]

[0081] In the above formula: F represents a 3×3 covariance matrix; represents the mean vector of the first N / 2 adjacent points of p, and T is the matrix transpose symbol.

[0082] The eigenvalues and eigenvectors corresponding to the covariance matrix F are obtained through eigenvalue decomposition:

[0083] Fν k =λ k ν k ;

[0084] In the above formula: λ k is the eigenvalue of F, and ν k is the eigenvector of F; assume that λ0 is the minimum eigenvalue of F and ν0 is the corresponding eigenvector. k takes 0, 1, 2; λ0 and ν0 are the values when k takes 0.

[0085] The cosine similarity threshold is:

[0086] C (i,t) =min(arccos(ν0 (i) ,ν0 (t) ),π - arccos(ν0 (i) ,ν0 (t) ))

[0087] In the above formula: C (i,t) is the cosine similarity; point t is any point in the neighborhood point set of point i; ν0 (i) represents the eigenvector corresponding to the minimum eigenvalue of point i, and ν0 (t) represents the eigenvector corresponding to the minimum eigenvalue of point t.

[0088] S24: Calculate the custom mixed distance combining Euclidean distance and cosine similarity: The specific steps are as follows:

[0089]

[0090] In the above formula: G (i,t) is the custom mixed distance between point i and point t; is the weight factor controlling the importance of the Euclidean distance. In this embodiment is preferably 0.5; D (i,t) is the Euclidean distance between point i and point t; β is the weight factor controlling the importance of the cosine similarity. In this embodiment, β is preferably 3; C (i,t) is the cosine similarity between point i and point t.

[0091] S3: Calculate the obstacle mixed distance threshold according to the relative distance between the point cloud and the sensor and perform obstacle judgment:

[0092] S31: As Figure 2As shown in the figure, the lidar point cloud density changes significantly with the distance between the object and the vehicle sensor. Specifically, the closer the object is to the sensor, the higher the density of the point cloud, that is, the number of points per unit area is larger; conversely, when the object is farther from the sensor, the density of the point cloud is lower, and the number of points per unit area decreases. This density change is mainly due to the fact that the detection range and accuracy of the lidar are affected by the distance. Therefore, when processing point cloud data, the hybrid distance threshold of the obstacle needs to be dynamically adjusted according to the distance between the object and the sensor to ensure the accuracy and effectiveness of the data.

[0093] S32: As Figure 3 shown: The hybrid distance thresholds of different lidar regions are different, and the specific calculation is as follows:

[0094]

[0095] In the formula: G th is the hybrid distance threshold of the obstacle; d min is the minimum threshold for preventing false detection at close range. In this embodiment, d min is 0.3; k1 is the linear gain coefficient at close range, which is 1 in this embodiment; θ is the vertical angular resolution of the lidar. In this embodiment, a 16-line lidar is used, and it is 2°; [r] is the integer part of the distance from the point cloud to the sensor; r th is the distance segmentation threshold for distinguishing the point cloud closer to the sensor from the point cloud farther from the sensor. In this embodiment, it is taken as 10 m; l is the long-distance adjustment coefficient to prevent the hybrid distance threshold of the obstacle from being too large. In this embodiment, it is taken as 0.2; k2 is the long-distance gain coefficient, which is taken as 0.5 in this embodiment. S3.3: The obstacle judgment is as follows:

[0096]

[0097] When G (i,t) ≤G th , it is considered that point i and point t belong to the points on the same obstacle, and they are classified into the same category; when G (i,t) >G th , it is considered that point i and point t belong to the points on different obstacles.

[0098] Figure 5 is the result graph of this experiment. This experiment is for the close-range obstacle detection scenario. In order to verify the accuracy, robustness, and adaptability of the algorithm at short distances, the distance between the tree obstacles in the complex jungle is set within 1 meter, and the obstacle detection method based on lidar 2 proposed in this embodiment is verified. The experiment is carried out in a simulated complex jungle environment. The lidar is used to collect the point cloud data of the trees, and the method of this embodiment is used for obstacle detection.

[0099] The experimental results show that the algorithm of this embodiment can effectively process point cloud data with short distances and high densities, accurately identify and distinguish different trees. As shown in the point cloud map, this algorithm has successfully extracted and clearly distinguished the point cloud data of each tree from the complex jungle environment, demonstrating its superior obstacle detection ability in complex unstructured environments. The point cloud data of each tree is clearly shown in the figure. Even in the case of short distances and high-density point clouds, the algorithm of this embodiment can accurately identify the contour and shape of each tree. This indicates that the algorithm can effectively process point cloud data and maintain high precision even in complex jungle environments. Further, as the results show, even in the presence of noise points or small changes in the point cloud data, the algorithm of this embodiment can stably identify the point cloud of trees. This proves that the algorithm has strong resistance to noise and can provide reliable detection results in complex environments. At the same time, the experiment also shows that for point cloud data with different distances and different densities, the algorithm of this embodiment can adapt to these changes and accurately identify obstacles. This indicates that the algorithm has good adaptability and can dynamically adjust the detection strategy according to the point cloud density characteristics within different distance ranges.

[0100] In summary, compared with traditional methods, the algorithm of this embodiment shows significant advantages in short-distance obstacle detection. Through the hybrid distance metric method, it not only improves the high-precision detection ability of point cloud data, effectively distinguishing adjacent trees that are difficult to identify even under high-density point cloud conditions, but also enhances the robustness of the algorithm. By introducing cosine similarity, the algorithm is more insensitive to noise and data changes, reducing the cases of false detection and missed detection. In addition, the adaptability of the algorithm of this embodiment has also been improved. It adopts a spatial partitioning strategy based on nested circular regions, which can dynamically adjust the hybrid distance threshold according to the point cloud density changes within different distance ranges, thus maintaining high adaptability and accuracy in short-distance detection. These advantages provide more reliable support for the environmental perception ability of autonomous driving vehicles.

Claims

1. A method for detecting obstacles in a driverless vehicle based on lidar, characterized in that: Including the following steps: S1: Obtain the vehicle lidar point cloud data and manage the point cloud data using KDTree; S2: Calculate the hybrid distance between the lidar point clouds; S3: Calculate the obstacle hybrid distance threshold according to the relative distance between the point cloud and the sensor and perform obstacle judgment.

2. The method for detecting obstacles of a driverless vehicle based on lidar according to claim 1, wherein: In step S1, it also includes the steps: S11: Construct a KDTree for the point cloud data, use the K-nearest neighbor algorithm to find the neighborhood of each data point p, and sort the data points in the neighborhood in descending order according to the Euclidean distance to the p data point.

3. The method for detecting obstacles of a driverless vehicle based on lidar according to claim 2, wherein: In step S11, when the K-nearest neighbor algorithm finds the neighborhood of each data point p, the specific steps are as follows: S111: Initialize a priority queue W to store the N nearest neighboring points of p currently found, and maintain the maximum distance dist among the N nearest neighboring points of p currently found; initialize the values in the priority queue W to infinity; max , and initialize the values in the priority queue W to infinity; S112: During the search process, traverse the nodes of the KDTree and calculate the Euclidean distance from the query point q to the current node R p If dist(q, R p ) is less than the current maximum distance dist max , then update the nearest neighbor points in the priority queue W and update dist max ; Where The Euclidean distance calculation method from the query point q to the current node R p is as follows: In the above formula, represents the square of the distance difference between the query point and the node in the j-th dimension, and d represents the number of feature dimensions of the sample; S113: When reaching the leaf node of the KDTree during the recursive process, check all the points in the leaf node and add the nearest point to the priority queue W.

4. The method for detecting obstacles of a driverless vehicle based on lidar according to claim 3, wherein: In step S2, the calculation of the hybrid distance between the lidar point clouds includes: S21: Calculate the Euclidean distance between the lidar point clouds: where x i , y i , z i and x t , y t , z t are the coordinate values of point i and point t in the Cartesian coordinate system, respectively; S22: Calculate the cosine similarity between the lidar point clouds: Use the first adjacent points of p in the KDTree sorted in descending order to construct the covariance matrix F, and then perform eigenvalue decomposition on the covariance matrix F; In the above formula: F represents a 3×3 covariance matrix; represents the mean vector of the adjacent points of the first N / 2 p's, and T is the matrix transpose symbol; S23: Obtain the eigenvalues and eigenvectors corresponding to the covariance matrix F through eigenvalue decomposition: Fν k = λ k ν k In the above formula: λ k is the eigenvalue of F, and ν k is the eigenvector of F; assume that λ0 is the minimum eigenvalue of F and ν0 is the corresponding eigenvector; The cosine similarity threshold is: C (i,t) = min(arccos(ν0 (i) , ν0 (t) ), π - arccos(ν0 (i) , ν0 (t) )) In the above formula: C (i,t) is the cosine similarity; point t is any point in the neighborhood point set of point i; ν0 (i) represents the eigenvector corresponding to the minimum eigenvalue of point i, and ν0 (t) represents the eigenvector corresponding to the minimum eigenvalue of point t; S24: Calculate the custom hybrid distance combining the Euclidean distance and the cosine similarity: In the above formula: G (i,t) is the custom mixed distance between point i and point t; is the weight factor controlling the importance of the Euclidean distance; D (i,t) is the Euclidean distance between point i and point t; β is the weight factor controlling the importance of the cosine similarity; C (i,t) is the cosine similarity between point i and point t.

5. A method for detecting obstacles of a driverless vehicle based on lidar according to claim 4, characterized in that: In step S24, has a value range of 0 to 5, and β has a value range of 0 to 10.

6. A method for obstacle detection of a driverless vehicle based on lidar according to claim 5, characterized in that: In step S3, it also includes the steps: S31: The obstacle hybrid distance threshold is calculated as follows: In the above formula: G th is the obstacle mixing distance threshold; d min is the minimum threshold for preventing false detection at close range; k1 is the close-range linear gain coefficient; θ is the vertical angular resolution of the lidar; [r] is the integer part of the distance from the point cloud to the distance sensor; r th is the distance segmentation threshold for distinguishing the point cloud closer to the distance sensor from the point cloud farther from the distance sensor; l is the long-distance adjustment coefficient to prevent the obstacle mixing distance threshold from being too large; k2 is the long-distance gain coefficient.

7. The obstacle detection method for a driverless vehicle based on lidar according to claim 6, characterized in that: In step S3, it also includes the steps: S33: The obstacle judgment is as follows: When G (i,t) ≤ G th , it is considered that point i and point t are points on the same obstacle, and they are classified into the same category; When G (i,t) > G th , it is considered that point i and point t are points on different obstacles.

Images