Method for detecting obstacle of unmanned vehicle in unstructured environment

By adopting a hierarchical clustering method in lidar obstacle detection, high-density and sparse point clouds are processed separately in unstructured environments, the problems of missed detection, false detection and insufficient real-time performance in the prior art are solved, and more efficient obstacle detection performance is achieved.

CN119805489AActive Publication Date: 2025-04-11KUNMING UNIV OF SCI & TECH
View PDF 8 Cites 0 Cited by

Patent Information

Application Number
CN202411869029.3
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-12-18
Publication Date
2025-04-11
Estimated Expiration
2044-12-18

AI Technical Summary

Technical Problem

In a complex and changeable unstructured environment with a wide variety of obstacle species, the existing lidar obstacle detection methods have problems such as missed detection, missed detection, and insufficient real-time performance.

Method used

The hierarchical clustering method is used to process high-density point clouds close to the sensor and sparse point clouds far away from the sensor respectively. Specific methods include: for high-density point clouds, the hash table is used to manage point cloud data, and the three-dimensional voxel clustering algorithm based on spherical coordinate system is adopted; for sparse point clouds, the point cloud data is used to manage point cloud data, and the point cloud clustering algorithm based on eigenvalues ​​and normal vector angle constraints are adopted.

Benefits of technology

It improves the performance of obstacle detection in complex unstructured environments, can more accurately distinguish different types of objects, and edge detection is more continuous and obvious, suitable for processing high-density and sparse point cloud data.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN119805489A_ABST
    Figure CN119805489A_ABST
Patent Text Reader

Abstract

The invention discloses a method for detecting an obstacle of an unmanned vehicle in an unstructured environment, and belongs to the technical field of automatic driving. According to the invention, based on the characteristic that the radar point cloud density is obviously changed along with the change of the distance between the radar and the sensor, a hierarchical clustering obstacle detection method is used for respectively processing high-density point clouds closer to the sensor and sparse point clouds farther from the sensor. For high-density point clouds closer to a sensor, a three-dimensional voxel grid clustering algorithm based on a spherical coordinate system is adopted to detect obstacles, and connectivity between the point clouds is defined through adjacent grids, so that data complexity is effectively reduced, and the method is suitable for processing high-density close-range point cloud data; for sparse point clouds far away from a sensor, a point cloud clustering algorithm based on feature value and normal vector angle constraint is adopted to detect obstacles, the algorithm can better utilize local geometric information of each point, and higher robustness and accuracy are shown in a sparse scene.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention belongs to the technical field of autonomous driving, and in particular relates to a method for detecting obstacles of an unmanned vehicle in an unstructured environment. Background Art

[0002] The realization of unmanned driving depends on the vehicle's perception of external information. Complete and accurate environmental information is the basis for path planning, behavioral decision-making and motion control. Obstacle detection is an important research content of environmental perception. At present, the sensors used for obstacle detection are lidar and cameras. Cameras are greatly affected by lighting factors, and depth information is difficult to obtain. Lidar has strong anti-interference ability and can obtain accurate distance information and reflection intensity information, so it is widely used in unmanned vehicles.

[0003] At present, the use of LiDAR for obstacle detection of unmanned vehicles in structured environments has made significant progress. However, in complex and changeable unstructured environments with a wide variety of obstacles, existing LiDAR obstacle detection methods have problems such as missed detection, false detection, and lack of real-time performance. Summary of the invention

[0004] In view of the above shortcomings of the prior art, the present invention provides a method for detecting obstacles of an unmanned vehicle in an unstructured environment.

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

[0006] A method for detecting obstacles of an unmanned vehicle in an unstructured environment comprises the following steps:

[0007] (1) Obtain the vehicle's 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 away 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 the spherical coordinate system is used to detect obstacles;

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

[0010] As a specific implementation of the present invention, in step (2), obstacles are detected using a three-dimensional voxel clustering algorithm based on a spherical coordinate system, which specifically includes:

[0011] S2-1: Convert the high-density point cloud coordinates close to the lidar sensor from the Cartesian coordinate system to the spherical coordinate system:

[0012]

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

[0014] S2-2: Construct a hash table and map the three-dimensional voxels into 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] Where: V i,j,k is a single three-dimensional voxel; P(ρ,θ,φ) represents the point cloud existing in the three-dimensional voxel; △ρ, △θ, △φ represent the unit size parameters of the voxel in the ρ, θ, φ directions respectively; i, j, k are the values ​​of the three-dimensional voxel in the ρ, θ, φ directions respectively.

[0019] S2-3: Search the neighboring voxels of the 3D voxel through the hash table. Assuming that the index of any voxel is (ρ,θ,φ), then the neighboring voxels are

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

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

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

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

[0024] S2-4: Clustering of 3D voxels and their neighboring voxels:

[0025]

[0026] Where: C(V i ) and C(V j ) represent voxel V i With voxel V j The cluster number of i ) is the voxel V i The neighboring voxels of ; Cnew represents the new cluster number.

[0027] As a specific implementation of the present invention, in step (3), the obstacle detection is performed using a point cloud clustering algorithm based on eigenvalue and normal vector angle constraints, which specifically includes:

[0028] S3-1. Construct KDTree for sparse point cloud data, use 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 the Euclidean distance to data point t.

[0029] S3-2. Use the first K / 2 neighbor points of the data point in descending order to construct the covariance matrix D, and then perform eigenvalue decomposition on the covariance matrix D.

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

[0031] As a specific implementation of the present invention, the K nearest neighbor algorithm searches for the domain of each data point t, specifically including the following steps:

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

[0033] Then, during the search process, we traverse the nodes of the KDTree and calculate the distance from the query point q to the current node p. t The Euclidean distance of dist(q,p t ) is less than the current maximum distance dist max , then update the neighboring points in the priority queue Q and update dist max ;

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

[0035]

[0036] In the formula, (q (n) -p t (n) ) 2represents 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 query point q to the current node p t spatial distance.

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

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

[0039]

[0040] Where: D represents the 3×3 covariance matrix; P t represents the coordinate vector of data point t; Represents the mean vector of the first K / 2 neighbor points, and T is the matrix transpose symbol.

[0041] As a specific implementation of the present invention, the eigenvalue and eigenvector corresponding to the covariance moment D are obtained by eigenvalue decomposition:

[0042] D k =λ k ν k ;

[0043] Where: k is the eigenvalue of D, ν k is the eigenvector of D; assuming λ0 is the minimum eigenvalue of D, ν0 is the eigenvector corresponding to the minimum eigenvalue of D.

[0044] As a specific implementation of the present invention, in step S3-3, the clustering formula based on eigenvalue and normal vector angle constraints is:

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

[0046] Where: data point t' is any point in the neighborhood of data point t; θ th is the normal vector similarity threshold; ν0 (t) Represents the eigenvector corresponding to the minimum eigenvalue of data point t, ν0 (t ' ) The eigenvector corresponding to the minimum eigenvalue of data point t'.

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

[0048] Compared with the prior art, the beneficial effects of the present invention are as follows: compared with the traditional method of not performing hierarchical clustering on radar point clouds, the present invention is based on the characteristic that the density of radar point clouds changes significantly with the change of distance from the sensor, and uses a hierarchical clustering obstacle detection method to process high-density point clouds close to the sensor and sparse point clouds far from the sensor separately, which can improve the vehicle's more excellent obstacle detection performance in complex unstructured environments, and can more accurately and clearly distinguish objects of different categories, and the edge detection is more continuous and obvious. For high-density point clouds close to the sensor, a three-dimensional voxel grid clustering algorithm based on a spherical coordinate system is used to detect obstacles, and the connectivity between point clouds is defined by neighboring grids, thereby effectively reducing data complexity, which is suitable for processing high-density short-distance point cloud data; for sparse point clouds far from the sensor, a point cloud clustering algorithm based on eigenvalues ​​and normal vector angle constraints is used to detect obstacles. The algorithm can better utilize the local geometric information of each point and show higher robustness and accuracy in sparse scenes. BRIEF DESCRIPTION OF THE DRAWINGS

[0049] Figure 1 This is a flow chart of the obstacle detection method for unmanned vehicles in an unstructured environment described in the present invention.

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

[0051] Figure 3 A top view showing three-dimensional voxel clustering based on a spherical coordinate system.

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

[0053] Figure 5This is a comparison chart of different obstacle detection algorithms in this embodiment, where point clouds of different colors represent different obstacles. Figure (a) is an experimental scene; (b) is an obstacle detection result chart of adaptive_clustering, where adaptive_clustering is the adaptive_clustering obstacle detection algorithm; (c) is an obstacle detection result chart of CVC, where CVC is the CVC obstacle detection algorithm; (d) is an obstacle detection result chart of travel, where travel is the travel obstacle detection algorithm; (e) is an obstacle detection result chart of Example 1 of the present invention. DETAILED DESCRIPTION

[0054] In order to better illustrate the purpose, technical solutions 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 of an unmanned vehicle in an unstructured environment comprises the following steps:

[0057] (1) Obtain the vehicle lidar point cloud, such as Figure 2 As shown in the figure, the density of the LiDAR point cloud changes significantly with the distance from the vehicle sensor. The farther the human body is from the sensor, the sparser the point cloud. 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 away from the sensor.

[0058] (2) The obstacle detection method based on hierarchical clustering is used to process the high-density point cloud closer to the sensor and the sparse point cloud farther away from the sensor separately.

[0059] (3) Figure 3 As shown in Figure 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 the spherical coordinate system is used to detect obstacles, such as Figure 3 As shown, specifically including:

[0060] S3-1: Convert the high-density point cloud coordinates close to the lidar sensor from the Cartesian coordinate system to the spherical coordinate system:

[0061]

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

[0063] S3-2: Build a hash table and map the 3D voxels into the hash table. The hash table uses the 3D voxels to quickly find, store and manage the spatial position of each point in the point cloud, improve data processing efficiency and reduce computational complexity.

[0064] The 3D voxels are:

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

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

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

[0068] Where: V i,j,k is a single three-dimensional voxel; P(ρ, θ, φ) represents the point cloud existing in the three-dimensional voxel; △ρ, △θ, △φ represent the unit size parameters of the voxel in the ρ, θ, and φ directions, respectively. In this embodiment, △ρ, △θ, and △φ are 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, search for the neighboring voxels of the three-dimensional voxel. Assuming that the voxel index is (ρ, θ, φ), then the neighboring voxels are

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

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

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

[0073] Where: N is the index set of neighboring voxels of the target voxel; δ ρ , δ θ , δ φ is the domain offset of the three-dimensional voxel in the ρ, θ, and φ directions. In this embodiment, δ ρ , δ θ , δ φ are taken as 1.0, 1.0, 1.0 respectively; (ρ', θ', φ') are the neighboring voxel indices, ρ', θ', φ' are the values ​​of the neighboring voxels in the ρ, θ, φ directions respectively; this domain-based local search avoids the huge computational overhead brought by the global search and improves the search efficiency.

[0074] S3-4: Cluster the 3D voxels and their neighboring voxels. After the neighboring voxel search is completed, cluster them:

[0075]

[0076] Where: C(V i ) and C(V j ) represent voxel V i With voxel V j The cluster number of i ) is the voxel V i The neighboring voxels of new The clustering algorithm adopted by the present invention recursively updates the clustering number to ensure that adjacent voxels are classified into the same class, thereby improving the consistency in the clustering process and solving the problems of cluster merging and cluster inheritance in traditional algorithms.

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

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

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

[0080] During the search process, traverse the nodes of KDTree and calculate the query point q to the current node p t The Euclidean distance of:

[0081]

[0082] In the formula, (q (n) -p t (n) ) 2 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. In this embodiment, d is 3; dist(q,p t ) indicates the query point q to the current node p t If dist(q,p t ) is less than the current maximum distance dist max , then update the neighboring points in the priority queue Q and update distmax Throughout the search, KDTree uses partitioning hyperplanes for pruning to reduce unnecessary searches.

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

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

[0085]

[0086] Where: D represents the 3×3 covariance matrix; P t represents the coordinate vector of point t; Represents the mean vector of the first K / 2 neighbor points, and T is the matrix transpose symbol.

[0087] Through eigenvalue decomposition, the eigenvalue and eigenvector corresponding to the covariance moment D are obtained:

[0088] D k =λ k ν k

[0089] Where: k is the eigenvalue of D, ν k is the eigenvector of D. Assume that λ0 is the minimum eigenvalue of D, and ν0 is the eigenvector corresponding to the minimum eigenvalue of D.

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

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

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

[0093] use Figure 4 The experimental platform shown in the figure verifies the effect of the obstacle detection method for unmanned vehicles in unstructured environments described in the present invention. The platform is an independently designed all-terrain unmanned vehicle, equipped with a monocular camera, a 16-line laser radar, an inertial measurement system IMU, and an industrial computer. All sensor data has been connected to the industrial computer and can be obtained through the topic communication method of ROS. The experimental scene is selected as follows Figure 5 (a) shows a jungle environment with a significant slope.

[0094] By comparing the adaptive_clustering algorithm, CVC algorithm, travel algorithm and the algorithm described in the present invention: during the experiment, the experimental platform is controlled to collect data around the experimental scene, and then the adaptive_clustering algorithm, CVC algorithm, travel algorithm and the algorithm described in the present invention are used for processing respectively. Figure 5 (b)-(e) are obstacle images detected by different methods. It can be seen that compared with other algorithms, the algorithm proposed in the present invention shows more excellent obstacle detection performance in complex unstructured environments, can accurately and clearly distinguish objects of different categories, and the edge detection is more continuous and obvious. It also shows that the unmanned vehicle obstacle detection method in an unstructured environment described in the present invention has strong practical application value, can provide more accurate environmental perception for autonomous vehicles, and promote the development of autonomous vehicles.

[0095] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit the scope of protection of the present invention. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the essence and scope of the technical solution of the present invention.

Claims

1. A method for detecting obstacles of unmanned vehicles in unstructured environments, characterized in that: The steps include: (1) Obtaining the laser radar point cloud data of the vehicle, and dividing the laser radar point cloud data into high-density point cloud data close to the sensor and sparse point cloud data far from the sensor; the high-density point cloud data is the point cloud data of the obstacle within 20 meters of the sensor, and the sparse point cloud data is the point cloud data of the obstacle beyond 20 meters of 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 the spherical coordinate system is used to detect obstacles; (3) For sparse point clouds far away from the sensor, KDTree is used to manage the point cloud data, and a point cloud clustering algorithm based on eigenvalue and normal vector angle constraints is adopted to detect obstacles.

2. The method for detecting obstacles of unmanned vehicles in unstructured environments according to claim 1, characterized in that: In the step (2), obstacles are detected using a three-dimensional voxel clustering algorithm based on a spherical coordinate system, which specifically includes: S2-1: Convert the high-density point cloud coordinates from the Cartesian coordinate system to the spherical coordinate system: Where: ρ is the radial distance, which indicates the distance from the point to the origin; θ is the polar angle, which indicates the angle between the point and the x axis on the xy plane; φ is the azimuth, which indicates 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; S2-2: Construct a hash table and map the three-dimensional voxels into the hash table; the three-dimensional voxels are: V i,j,k ={P(ρ,θ,φ)|i△ρ≤ρ≤(i+1)△ρ, j△θ≤θ≤(j+1)△θ, k△φ≤φ≤(k+1)△φ}; Where: V i,j,k is a single 3D voxel; P(ρ,θ,φ) represents the point cloud existing in the 3D voxel; △ρ, △θ, △φ represent the unit size parameters of the 3D voxel in the ρ, θ, φ directions respectively; i, j, k are the values ​​of the 3D voxel in the ρ, θ, φ directions respectively; S2-3: Search the neighboring voxels of the 3D voxel through the hash table. Assuming that the index of any voxel is (ρ,θ,φ), then the neighboring voxels are N={(ρ',θ',φ')|ρ'∈{ρ-δ ρ ,p+d ρ }, θ'∈{θ-δ θ ,θ+δ θ }, φ'∈{φ-δ φ ,φ+δ φ }}; Where: N is the index set of neighboring voxels of the target 3D voxel; δ ρ ,δ θ ,δ φ are the domain offsets of three-dimensional voxels in the ρ, θ, and φ directions respectively; (ρ', θ', φ') are the neighboring voxel indexes, and ρ', θ', φ' are the values ​​of the neighboring voxels in the ρ, θ, and φ directions respectively; S2-4: Clustering of 3D voxels and their neighboring voxels: Where: C(V i ) and C(V j ) represent voxel V i With voxel V j The cluster number of i ) is the voxel V i The neighboring voxels of new Indicates the new cluster number.

3. The method for detecting obstacles of unmanned vehicles in unstructured environments as claimed in claim 1, characterized in that: In the step (3), the obstacle is detected by using a point cloud clustering algorithm based on eigenvalue and normal vector angle constraints, which specifically includes: S3-1. Construct KDTree for sparse point cloud data, use K nearest neighbor algorithm to find the neighborhood of data point t of each sparse point cloud data, and sort the data points in the neighborhood in descending order according to the Euclidean distance to data point t; S3-2, construct a covariance matrix D using the first K / 2 neighboring points of the data point arranged in descending order, and then perform eigenvalue decomposition on the covariance matrix D; S3-3: Clustering of sparse point clouds far from the sensor.

4. The method for detecting obstacles of unmanned vehicles in unstructured environments as claimed in claim 1, characterized in that: The K nearest neighbor algorithm finds the neighborhood of each data point t, specifically including the following steps: First, initialize a priority queue Q to store the K nearest neighbors currently found and maintain the maximum distance dist among the K nearest neighbors currently found. max , initialized to infinity; Then, during the search process, we traverse the nodes of the KDTree and calculate the distance from the query point q to the current node p. t The Euclidean distance of dist(q,p t ) is less than the current maximum distance dist max , then update the neighboring points in the priority queue Q and update dist max ; The query point q to the current node p t The Euclidean distance of: In the formula, (q (n) -p t (n) ) 2 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 query point q to the current node p t spatial distance; Finally, when the KDTree leaf node is reached during the recursive process, all points in the leaf node are checked and the closest points are added to the priority queue Q.

5. The method for detecting obstacles of unmanned vehicles in unstructured environments as claimed in claim 1, characterized in that: The covariance matrix D is: Where: D represents the 3×3 covariance matrix; P t represents the coordinate vector of point t; Represents the mean vector of the K / 2 neighboring points before point t, and T is the matrix transpose symbol.

6. The method for detecting obstacles of unmanned vehicles in unstructured environments as claimed in claim 1, characterized in that: The eigenvalue and eigenvector corresponding to the covariance moment D are obtained by eigenvalue decomposition: Dv k =λ k n k ; Where: k is the eigenvalue of D, ν k is the eigenvector of D; assuming λ0 is the minimum eigenvalue of D, ν0 is the eigenvector corresponding to the minimum eigenvalue of D.

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

Citation Information

Patent Citations

  • Point cloud data processing method, neural network training method and related equipment

    CN114140841A

  • Unmanned vehicle trajectory prediction method and device

    CN114371703A

  • Target detection and identification method for intelligent driving vehicle under structured road

    CN114488194A

  • Robust and fast vehicle single-line laser radar point cloud clustering method

    CN116071571A

  • Obstacle detection method and intelligent forklift

    CN116148884A