A method for obstacle detection in unstructured environments for autonomous vehicles

Abstract

This invention discloses a method for obstacle detection in unstructured environments for autonomous vehicles, belonging to the field of autonomous driving technology. Based on the characteristic that radar point cloud density changes significantly with distance from the sensor, this invention uses a hierarchical clustering obstacle detection method to process high-density point clouds closer to the sensor and sparse point clouds farther away. For high-density point clouds closer to the sensor, a three-dimensional voxel grid clustering algorithm based on a spherical coordinate system is used to detect obstacles. The connectivity between point clouds is defined by neighboring grids, effectively reducing data complexity and making it suitable for processing high-density, near-range point cloud data. For sparse point clouds farther away from the sensor, a point cloud clustering algorithm based on eigenvalues ​​and normal vector angle constraints is used to detect obstacles. This algorithm better utilizes the local geometric information of each point and exhibits higher robustness and accuracy in sparse scenarios.

Description

Technical Field

[0001] This invention belongs to the field of autonomous driving technology, specifically relating to a method for detecting obstacles in unmanned vehicles in unstructured environments. Background Technology

[0002] The realization of autonomous driving relies on the vehicle's perception of external information. Complete and accurate environmental information is the foundation for path planning, behavioral decision-making, and motion control, and obstacle detection is an important research area in environmental perception. Currently, the sensors used for obstacle detection include LiDAR and cameras. Cameras are greatly affected by lighting conditions, and it is difficult to obtain depth information. LiDAR, on the other hand, has strong anti-interference capabilities and can obtain accurate distance and reflection intensity information, and is therefore widely used in autonomous vehicles.

[0003] Currently, significant progress has been made in using lidar for obstacle detection in structured environments for autonomous vehicles. However, in complex and varied unstructured environments with diverse obstacle types, existing lidar obstacle detection methods suffer from problems such as missed detections, false detections, and insufficient real-time performance. Summary of the Invention

[0004] To address the shortcomings of the existing technologies, this invention provides a method for detecting obstacles in unstructured environments for autonomous vehicles.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] A method for detecting obstacles in unstructured environments for autonomous vehicles includes the following steps:

[0007] (1) Obtain the vehicle lidar point cloud and divide the lidar point cloud into a high-density point cloud that is closer to the sensor and a sparse point cloud that is farther from the sensor.

[0008] (2) For high-density point clouds that are close to the sensor, a hash table is used to manage the point cloud data, and a three-dimensional voxel clustering algorithm based on spherical coordinates is used to detect obstacles;

[0009] (3) For sparse point clouds that are far from the sensor, KDTree is used to manage the point cloud data, and a point cloud clustering algorithm based on feature value and normal vector angle constraints is used to detect obstacles.

[0010] As a specific embodiment of the present invention, in step (2), a three-dimensional voxel clustering algorithm based on a spherical coordinate system is used to detect obstacles, specifically including:

[0011] S2-1: Transform the coordinates of high-density point clouds closer to the lidar sensor from Cartesian coordinates to spherical coordinates.

[0012]

[0013] In the formula: ρ is the radial distance, representing the distance from the point to the origin; θ is the polar angle, representing the angle between the point and x on the xy plane; φ is the azimuth angle, representing the angle between the point and the z-axis; and x, y, and z are the coordinate values ​​of x, y, and z in the Cartesian coordinate system, respectively.

[0014] S2-2: Construct a hash table to map the three-dimensional voxels to the hash table; the three-dimensional voxels are:

[0015] V i,j,k ={P(ρ,θ,φ)|i△ρ≤ρ≤(i+1)△ρ,

[0016] j△θ≤θ≤(j+1)△θ,

[0017] k△φ≤φ≤(k+1)△φ};

[0018] In the formula: V i,j,k P(ρ,θ,φ) represents a single three-dimensional voxel; P(ρ,θ,φ) represents the point cloud existing in the three-dimensional voxel; △ρ, △θ, and △φ represent the unit size parameters of the voxel in the ρ, θ, and φ directions, respectively; i, j, and k are the values ​​of the three-dimensional voxel in the ρ, θ, and φ directions, respectively.

[0019] S2-3: A hash table is used to search for neighboring voxels of a 3D voxel. Assuming any voxel index is (ρ, θ, φ), then the neighboring voxels are...

[0020] Ν={(ρ',θ',φ')|ρ'∈{ρ-δ ρ ,ρ+δ ρ},

[0021] θ'∈{θ-δ θ ,θ+δ θ},

[0022] φ'∈{φ-δ φ ,φ+δ φ}};

[0023] In the formula: N is the set of neighboring voxel indices of the target 3D voxel; δ ρ δ θ δ φ ρ', θ', and φ' are the neighborhood offsets of the three-dimensional voxels in the ρ, θ, and φ directions, respectively; (ρ', θ', φ') are the neighboring voxel indices, where ρ', θ', and φ' are the values ​​of the neighboring voxels in the ρ, θ, and φ directions, respectively.

[0024] S2-4: Clustering of three-dimensional voxels and their neighboring voxels:

[0025]

[0026] In the formula: C(V) i ) and C(V j ) represent voxels V i With voxel V j Cluster number; N(V) i ) is voxel V i The nearest voxels; Cnew represents the new cluster number.

[0027] As a specific embodiment of the present invention, in step (3), a point cloud clustering algorithm based on feature values ​​and normal vector angle constraints is used to detect obstacles, specifically including:

[0028] S3-1. Construct a KDTree for sparse point cloud data, use the K-nearest neighbor algorithm to find the neighborhood of each data point t, and sort the data points in the neighborhood in descending order according to their Euclidean distance to data point t.

[0029] S3-2. Construct a covariance matrix D using the first K / 2 neighboring points of the data points arranged in descending order, and then perform eigenvalue decomposition on the covariance matrix D.

[0030] S3-3: Clustering of sparse point clouds that are far from the sensor.

[0031] As a specific embodiment of the present invention, the K-nearest neighbor algorithm for finding the neighborhood of each data point t specifically includes the following steps:

[0032] First, initialize a priority queue Q to store the K nearest neighbors found so far, and maintain the maximum distance dist among the K nearest neighbors found so far. max Initialize to infinity;

[0033] Then, during the search process, the nodes of the KDTree are traversed and the distance from the query point q to the current node p is calculated. t The Euclidean distance, if dist(q,p) t (less than the current maximum distance dist) max Then update the nearest neighbors in priority queue Q and update dist. max ;

[0034] The query point q is to the current node p t Euclidean distance:

[0035]

[0036] In the formula, (q (n) -p t (n) ) 2Dist(q,p) represents the squared distance difference between the query point and the node in the nth dimension; d represents the number of feature dimensions of the sample; t ) indicates the distance from query point q to the current node p. t Spatial distance.

[0037] Finally, when the KDTree leaf node is reached during the recursion, all points in the leaf node are checked, and the closest point is added to the priority queue Q.

[0038] As a specific embodiment of the present invention, the covariance matrix D:

[0039]

[0040] In the formula: D represents a 3×3 covariance matrix; P t The coordinate vector representing the data point t; The vector represents the mean of the first K / 2 neighboring points, and T is the matrix transpose symbol.

[0041] As a specific embodiment of the present invention, the eigenvalues ​​and eigenvectors corresponding to the covariance moment D are obtained through eigenvalue decomposition:

[0042] Dν k =λ k ν k ;

[0043] In the formula: λ k Let ν be an eigenvalue of D. k Let λ0 be the eigenvector of D; assume λ0 is the smallest eigenvalue of D, and ν0 is the eigenvector corresponding to the smallest eigenvalue of D.

[0044] As a specific embodiment of the present invention, in step S3-3, the clustering formula based on eigenvalues ​​and normal vector angle constraints is as follows:

[0045] min(arccos(ν0 (t) ,ν0 (t ' ) ),π-arccos(ν0 (t) ,ν0 (t') ))<θ th ;

[0046] In the formula: data point t' is any point in the neighborhood set of data point t; θ th ν0 is the threshold for normal vector similarity. (t) ν0 represents the eigenvector corresponding to the smallest eigenvalue of data point t. (t ' ) The eigenvector represents the smallest eigenvalue of data point t'.

[0047] When the above clustering formula is satisfied, data point t and data point t' are considered to be points on the same type of object; when data point t' satisfies λ0 (t') <R th When data point t is used as another new seed point for the object, the above steps are repeated with the data points in its neighborhood set until the iteration ends and λ0 is reached. (t') R is the minimum eigenvalue of data point t'. th The threshold value is the feature value.

[0048] Compared with existing technologies, the beneficial effects of this invention are as follows: Compared with traditional methods that do not perform hierarchical clustering of radar point clouds, this invention, based on the characteristic that radar point cloud density changes significantly with distance from the sensor, uses a hierarchical clustering obstacle detection method to process high-density point clouds closer to the sensor and sparse point clouds farther away from the sensor separately. This improves the vehicle's obstacle detection performance in complex unstructured environments, enabling more accurate and clear differentiation of different object categories, and resulting in more continuous and obvious edge detection. For high-density point clouds closer to the sensor, a three-dimensional voxel grid clustering algorithm based on spherical coordinates is used to detect obstacles. The connectivity between point clouds is defined by neighboring grids, effectively reducing data complexity and making it suitable for processing high-density, near-range point cloud data. For sparse point clouds farther away from the sensor, a point cloud clustering algorithm based on eigenvalues ​​and normal vector angle constraints is used to detect obstacles. This algorithm can better utilize the local geometric information of each point and exhibits higher robustness and accuracy in sparse scenarios. Attached Figure Description

[0049] Figure 1 This is a flowchart of the obstacle detection method for unmanned vehicles in unstructured environments described in this invention.

[0050] Figure 2 Examples of 3D LiDAR human body point cloud images at different distances.

[0051] Figure 3 This is a schematic top view of three-dimensional voxel clustering based on spherical coordinates.

[0052] Figure 4 This is the experimental platform described in this embodiment.

[0053] Figure 5Figure 1 shows a comparison of different obstacle detection algorithms in this embodiment, where point clouds of different colors represent different obstacles. Figure (a) shows the experimental scene; (b) shows the obstacle detection results of adaptive_clustering, where adaptive_clustering is the adaptive_clustering obstacle detection algorithm; (c) shows the obstacle detection results of CVC, where CVC is the CVC obstacle detection algorithm; (d) shows the obstacle detection results of travel, where travel is the travel obstacle detection algorithm; and (e) shows the obstacle detection results of Embodiment 1 of this invention. Detailed Implementation

[0054] To better illustrate the purpose, technical solution, and advantages of the present invention, the present invention will be further described below in conjunction with specific embodiments.

[0055] Example 1

[0056] like Figure 1 As shown, a method for detecting obstacles in an unstructured environment for autonomous vehicles includes the following steps:

[0057] (1) Obtain the vehicle's lidar point cloud, such as Figure 2 As shown, the point cloud density of LiDAR changes significantly with the distance from the vehicle sensor. The farther the human body is from the sensor, the sparser the point cloud becomes. Therefore, the LiDAR point cloud is divided into a high-density point cloud that is closer to the sensor and a sparse point cloud that is farther from the sensor.

[0058] (2) The obstacle detection method of hierarchical clustering is used to process the high-density point cloud that is close to the sensor and the sparse point cloud that is far from the sensor respectively.

[0059] (3) Figure 3 As shown, for high-density point clouds close to the sensor, a hash table is used to manage the point cloud data, and a three-dimensional voxel clustering algorithm based on spherical coordinates is used to detect obstacles. Figure 3 As shown, it specifically includes:

[0060] S3-1: Transform the coordinates of high-density point clouds closer to the lidar sensor from Cartesian coordinates to spherical coordinates.

[0061]

[0062] In the formula: ρ is the radial distance, representing the distance from the point to the origin; θ is the polar angle, representing the angle between the point and x on the xy plane; φ is the azimuth angle, representing the angle between the point and the z-axis; and x, y, and z are the coordinate values ​​of x, y, and z in the Cartesian coordinate system, respectively.

[0063] S3-2: Construct a hash table to map 3D voxels to the hash table. The hash table allows for the rapid lookup, storage, and management of the spatial positions of points in the point cloud using 3D voxels, improving data processing efficiency and reducing computational complexity.

[0064] The three-dimensional voxels are:

[0065] V i,j,k ={P(ρ,θ,φ)|i△ρ≤ρ≤(i+1)△ρ,

[0066] j△θ≤θ≤(j+1)△θ,

[0067] k△φ≤φ≤(k+1)△φ};

[0068] In the formula: V i,j,k P(ρ,θ,φ) represents a single three-dimensional voxel; P(ρ,θ,φ) represents the point cloud existing in the three-dimensional voxel; Δρ, Δθ, and Δφ represent the unit size parameters of the voxel in the ρ, θ, and φ directions, respectively. In this embodiment, Δρ, Δθ, and Δφ are taken as 2, 0.4, and 1.5, respectively; i, j, and k are the values ​​of the three-dimensional voxel in the ρ, θ, and φ directions.

[0069] S3-3: After constructing the hash table, perform a lookup of neighboring voxels for the 3D voxel. Assuming the voxel index is (ρ, θ, φ), then the neighboring voxels are...

[0070] Ν={(ρ',θ',φ')|ρ'∈{ρ-δ ρ ,ρ+δ ρ},

[0071] θ'∈{θ-δ θ ,θ+δ θ},

[0072] φ'∈{φ-δ φ ,φ+δ φ}};

[0073] In the formula: N is the set of neighboring voxel indices of the target voxel; δ ρ δ θ δ φ δ represents the neighborhood offset of a three-dimensional voxel in the ρ, θ, and φ directions. In this embodiment, δ ρ δ θ δ φ The values ​​are 1.0, 1.0, and 1.0 respectively; (ρ', θ', φ') is the index of the nearest voxel, where ρ', θ', and φ' are the values ​​of the nearest voxels in the ρ, θ, and φ directions, respectively; this local search based on the neighborhood avoids the huge computational overhead of global search and improves search efficiency.

[0074] S3-4: Cluster the 3D voxels and their neighboring voxels. After searching for neighboring voxels, cluster them as follows:

[0075]

[0076] In the formula: C(V) i ) and C(V j ) represent voxels V i With voxel V j Cluster number; N(V) i ) is voxel V i Neighboring voxels; C new This indicates the new cluster number. The clustering algorithm used in this invention recursively updates the cluster numbers, ensuring that adjacent voxels are assigned to the same cluster, thus improving consistency in the clustering process and solving the problems of cluster merging and cluster inheritance in traditional algorithms.

[0077] S4. For sparse point clouds that are far from the sensor, KDTree is used to manage the point cloud data, and a point cloud clustering algorithm based on eigenvalues ​​and normal vector angle constraints is used to detect obstacles, specifically including:

[0078] S4-1. Construct a KDTree from the sparse point cloud data. Use the K-Nearest Neighbors (KNN) algorithm to find the neighborhood of each data point t, and sort the data points in the neighborhood in descending order of their Euclidean distance to data point t. The K-Nearest Neighbors algorithm search steps are as follows:

[0079] First, initialize a priority queue Q to store the K nearest neighbors found so far, and maintain the maximum distance dist among the currently found nearest neighbors. max It is initialized to infinity.

[0080] During the search process, the nodes of the KDTree are traversed and the distance from the query point q to the current node p is calculated. t Euclidean distance:

[0081]

[0082] In the formula, (q (n) -p t (n) ) 2 This represents the squared distance difference between the query point and the node in the nth dimension. d represents the number of feature dimensions of the sample; in this embodiment, d is 3. dist(q,p) t ) indicates the distance from query point q to the current node p. t Spatial distance. If dist(q,p) t (less than the current maximum distance dist) max Then update the nearest neighbors in priority queue Q and update dist.max Throughout the search, KDTree uses partitioning hyperplanes for pruning to reduce unnecessary searches.

[0083] When the recursion reaches a leaf node of the KDTree, all points in the leaf node are checked, and the nearest point is added to the priority queue Q. Through this recursive search, the KDTree can efficiently find the K nearest neighbors of a given query point.

[0084] S4-2. To capture the local geometric features of a point and reduce the influence of noise, the K nearest neighbors of any point t are sorted in descending order according to their Euclidean distance to t, and the covariance matrix D is constructed using its first K / 2 neighbors:

[0085]

[0086] In the formula: D represents a 3×3 covariance matrix; P t The coordinate vector representing point t; The vector represents the mean of the first K / 2 neighboring points, and T is the matrix transpose symbol.

[0087] By eigenvalue decomposition, we obtain the eigenvalues ​​and eigenvectors corresponding to the covariance moment D:

[0088] Dν k =λ k ν k

[0089] In the formula: λ k Let ν be an eigenvalue of D. k Let λ0 be the eigenvector of D. Assume λ0 is the smallest eigenvalue of D, and ν0 is the eigenvector corresponding to the smallest eigenvalue of D.

[0090] S4-3. Taking point t as an example, using ν0 (t) Instead of the traditional Euclidean distance, use it to measure the relationship between a point t and its neighborhood set:

[0091] min(arccos(ν0 (t) ,ν0 (t ' ) ),π-arccos(ν0 (t) ,ν0 (t ' ) ))<θ th

[0092] In the formula: point t' is any point in the neighborhood set of point t; θ th In this embodiment, θ is the threshold for normal vector similarity. th Take 0.175; ν0 (t) ν0 represents the eigenvector corresponding to the smallest eigenvalue at point t. (t ') This represents the eigenvector corresponding to the smallest eigenvalue at point t'. When this formula is satisfied, points t and t' are considered to be points on the same type of object. Furthermore, when point t' satisfies λ0... (t') <R th When point t' is reached, it can be used as another new seed point for the object, and the above steps are repeated with points in its neighborhood set until the iteration ends. th As the feature value threshold, R in this embodiment th Take 5.

[0093] use Figure 4 The experimental platform shown verifies the effectiveness of the obstacle detection method for unmanned vehicles in unstructured environments described in this invention. This platform is a self-designed all-terrain unmanned vehicle equipped with a monocular camera, a 16-line LiDAR, an inertial measurement unit (IMU), and an industrial control computer. All sensor data is connected to the industrial control computer and can be acquired via ROS topic communication. The experimental scenario is selected as follows: Figure 5 (a) shows a jungle environment with a distinct slope.

[0094] The adaptive_clustering algorithm, CVC algorithm, travel algorithm, and the algorithm described in this invention were compared: During the experiment, the experimental platform was controlled to collect data around the experimental scene, and then the adaptive_clustering algorithm, CVC algorithm, travel algorithm, and the algorithm described in this invention were used to process the data respectively. Figure 5 Images (b)-(e) show obstacle images detected by different methods. It can be seen that, compared with other algorithms, the algorithm proposed in this invention exhibits superior obstacle detection performance in complex unstructured environments, accurately and clearly distinguishing different categories of objects, and demonstrating more continuous and obvious edge detection. This also demonstrates that the obstacle detection method for autonomous vehicles in unstructured environments described in this invention has strong practical application value, providing higher-precision environmental perception for autonomous vehicles and promoting their development.

[0095] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the essence and scope of the technical solutions of the present invention.

Claims

1. A method for detecting obstacles in unstructured environments for autonomous vehicles, characterized in that, Includes the following steps: (1) Acquire vehicle lidar point cloud data and divide the lidar point cloud data into high-density point cloud data that is close to the sensor and sparse point cloud data that is far from the sensor; the high-density point cloud data is the point cloud data of obstacles within 20 meters of the sensor, and the sparse point cloud data is the point cloud data of obstacles more than 20 meters away from the sensor. (2) For high-density point clouds that are close to the sensor, a hash table is used to manage the point cloud data, and a three-dimensional voxel clustering algorithm based on spherical coordinates is used to detect obstacles. S2-1: The coordinates of the high-density point cloud are transformed from the Cartesian coordinate system to the spherical coordinate system. In the formula: ρ is the radial distance, representing the distance from the point to the origin; θ is the polar angle, representing the angle between the point and x in the xy plane; The azimuth angle represents the angle between the point and the z-axis, where x, y, and z are the coordinates of the point in the Cartesian coordinate system. S2-2: Construct a hash table to map the three-dimensional voxels to the hash table; the three-dimensional voxels are: In the formula: V i,j,k It is a single three-dimensional voxel; This represents a point cloud existing in a three-dimensional voxel; Δρ, Δθ, Representing the three-dimensional voxels at ρ, θ, and θ respectively The unit dimension parameters of the direction; i, j, k are the three-dimensional voxels in ρ, θ, and k, respectively. Values ​​in direction; S2-3: Search for neighboring voxels of a 3D voxel using a hash table. Assume any voxel index is... Then the neighboring voxels are In the formula: N is the set of neighboring voxel indices of the target 3D voxel; δ ρ δ θ , These are three-dimensional voxels at ρ, θ, and The domain offset of the direction; For neighboring voxel indices, ρ', θ', These are neighboring voxels at ρ, θ, Values ​​in direction; S2-4: Clustering of three-dimensional voxels and their neighboring voxels: In the formula: C(V) i ) and C(V j ) represent voxels V i With voxel V j Cluster number; N(V i ) is voxel V i Neighboring voxels; C new Indicates the new cluster number; (3) For sparse point clouds that are far from the sensor, KDTree is used to manage the point cloud data. A point cloud clustering algorithm based on feature value and normal vector angle constraint is used to detect obstacles. S3-1, KDTree is constructed for sparse point cloud data. The K nearest neighbor algorithm is used to find the neighborhood of each sparse point cloud data data point t, and the data points in the neighborhood are sorted in descending order according to the Euclidean distance to the data point t. S3-2. Construct a covariance matrix D using the first K / 2 neighbor points of the data points arranged in descending order, and then perform eigenvalue decomposition on the covariance matrix D. S3-3: Clustering of sparse point clouds that are far from the sensor.

2. The method for detecting obstacles in unstructured environments for unmanned vehicles as described in claim 1, characterized in that, The K-nearest neighbor algorithm for finding the neighborhood of each data point t includes the following steps: First, initialize a priority queue Q to store the K nearest neighbors found so far, and maintain the maximum distance dist among the K nearest neighbors found so far. max Initialize to infinity; Then, during the search process, the nodes of the KDTree are traversed and the distance from the query point q to the current node p is calculated. t The Euclidean distance, if dist(q,p) t (less than the current maximum distance dist) max Then update the nearest neighbors in priority queue Q and update dist. max ; The query point q is to the current node p t Euclidean distance: In the formula, (q (n) -p t (n) ) 2 This represents the square of the distance difference between the query point and the node in the nth dimension; d represents the number of feature dimensions of the sample. dist(q,p t ) indicates the distance from query point q to the current node p. t Spatial distance; Finally, when the KDTree leaf node is reached during the recursion, all points in the leaf node are checked, and the closest point is added to the priority queue Q.

3. The method for detecting obstacles in unstructured environments for unmanned vehicles as described in claim 1, characterized in that, The covariance matrix D: In the formula: D represents a 3×3 covariance matrix; P t The coordinate vector representing point t; The vector represents the mean of the K / 2 neighboring points of point t, and T is the matrix transpose.

4. The method for detecting obstacles in an unstructured environment for unmanned vehicles as described in claim 1, characterized in that, The eigenvalues ​​and eigenvectors corresponding to the covariance moment D are obtained through eigenvalue decomposition: Dν k =λ k ν k ; In the formula: λ k Let ν be an eigenvalue of D. k Let λ0 be the eigenvector of D; assume λ0 is the smallest eigenvalue of D, and ν0 is the eigenvector corresponding to the smallest eigenvalue of D.

5. The method for detecting obstacles in an unstructured environment for unmanned vehicles as described in claim 1, characterized in that, In step S3-3, the formula for clustering sparse point clouds based on eigenvalues ​​and normal vector angle constraints is: min(arccos(ν0) (t) ,ν0 (t') ),π-arccos(ν0 (t) ,ν0 (t') ))<θ th ; In the formula: point t' is any point in the neighborhood set of data point t; θ th ν0 is the threshold for normal vector similarity. (t) ν0 represents the eigenvector corresponding to the smallest eigenvalue of data point t. (t') The eigenvector representing the smallest eigenvalue of data point t'; When the above clustering formula is satisfied, data point t and data point t' are considered to be points on the same type of object; when data point t' satisfies λ0 (t') <R th When the data point t' is used as another new seed point for the object, the above steps are repeated with the points in its neighborhood point set until the iteration ends, λ0 (t') Let t' be the minimum eigenvalue of the data point.

Images