Novel Probabilistic Collision Detection for Manipulator Motion Planning Using HNSW

Abstract

Collision detection is very important for robot motion planning. The existing accurate collision detection algorithms regard the evaluation of each node as a discrete event, ignoring the correlation between nodes, resulting in low efficiency. In this paper, we propose a novel approach that transforms collision detection into a binary classification problem. In particular, the proposed method searches the k-nearest neighbor (KNN) of the new node and estimates its collision probability by the prior node. We perform the hierarchical navigable small world (HNSW) method to query the nearest neighbor data and store the detected nodes to build the database incrementally. In addition, this research develops a KNN query technique tailored for linear data, incorporating threshold segmentation to facilitate collision detection along continuous paths. Moreover, it refines the distance function of the collision classifier to enhance the precision of probability estimations. Simulation results demonstrate the effectiveness of the proposed method.

1. Introduction

Collision detection plays an important role in computer graphics and manipulator path planning [1,2,3,4]. Multi-degree-of-freedom manipulators are composed of several interconnected links that undergo positional changes during movement. Unlike a single rigid body such as a UAV or a mobile robot, the complexity of a manipulator prevents it from directly utilizing obstacle data in the workspace, which increases the complexity of map construction [5,6]. In response, a sampling-based motion planner (SBMP) has become an important solution for manipulator motion planning [7,8,9,10]. An SBMP gradually explores the required geometric paths by random sampling in configuration space (C-space) and by using a collision detection algorithm to ensure local path safety [11]. The topology of the obstacle manifold in high dimensional C-space is very complex. As a result, real-time performance is a challenge for SBMP using traditional collision detection. Collision detection has been documented to occupy the most computational time in motion planning, so improving its calculation speed is the key to improving the overall path planning efficiency [12].

Traditional collision detection, as depicted in Figure 1, firstly maps joint positions Q to link positions T = {T₁, …, T_i} by forward kinematics fk(∙), where i represents the link count. The links and obstacles are then represented using convex geometries such as capsules, spheres, and boxes. Obstacle information is described by the set O = {Obs₁, Obs₂, …, Obs_j}, where each obstacle Obs_j is characterized by the coordinates P_j and radius r_j of m points. Subsequently, the distance d between these geometries is computed to determine the occurrence of collisions, which are classified as C. If d is not greater than 0, it is judged to be a collision, in which case C is defined as 1; otherwise, it is a non-collision state C = 0. For continuous path collision detection, the path X(τ) is discretized at a resolution K and detection is repeated for each sample point. This process is known as the exact collision detection method. Collision detection is transformed into the problem of computing the minimum distance between convex geometries.

There are numerous methods for calculating the Euclidean distance between two simple convex geometries. Mohammad Safeea et al. introduced an efficient approach to compute the minimum distance between capsules based on QR decomposition [13] and used it for the path planning of a robotic arm [14,15]. Reference [16] proposed a new calculation method of the distance between the sphere and the capsule and used it to detect the collision of the links of manipulators.

The problem of computing the distance between general convex geometries is more complicated. The Gilbert–Johnson–Keerthi (GJK) algorithm [17] and the Lin-Canny algorithm [18] are widely used. The GJK algorithm reduces the complexity of convex geometry by Minkowski distance, which has been widely used in computer graphics [19]. The swept volume intersection (SVI) method is suitable for detecting collisions along continuous paths [20]. It improves accuracy by assessing the collision situation based on the overlap rate. However, the calculation of the scanning volume of objects with complex geometry is very expensive [21]. The hierarchical bounding box (BVH) methods are proposed to enhance computing speed and memory optimization [22]. However, BVH methods may be too conservative in space, leading to inefficient exploration [23]. Exact collision detection algorithms have been well developed and most of them have been integrated in the flexible collision library (FCL) [24]. The exact collision detection methods ignore the correlation of states in C-space, which makes them time-consuming.

Learning-based methods provide an alternative strategy, which fits the obstacle manifolds in C-space to obtain information about the free configuration space (C_free). Learning-based methods reduce the number of exact collision detections and significantly improve computational efficiency [25]. Pan et al. used incremental support vector machines (SVM) to learn an accurate model of the C_free but the accuracy was not high and could not be applied to an unstructured environment [26]. Configuration space decomposition strategy can further improve the accuracy of collision detection methods based on SVM [27]. Huh attempts to fit the C_free of a high-DOF manipulator using a Gaussian mixture model (GMM) but the fitting capability is limited [28]. Nikhil Das et al. proposed a collision detection model based on online learning, which is called Fastron [29]. Fastron learns the offline dataset based on Kernel Perceptron and obtains the optimal model to represent the boundary of the free configuration space within the maximum number of iterations [30]. A new kernel function is designed based on Fastron, which enhances the relationship between kinematics and configuration space and improves the accuracy of collision detection [31]. The DiffCo method is similar to Fastron in that obstacle manifold information is learned through a nonparametric kernel perceptron but it is mainly applied to optimization-based motion planners [32].

The above methods typically require the support of an offline dataset, making it difficult to transfer fitting capabilities to robots outside of the dataset. In addition, these methods are inapplicable for continuous path collision detections. Addressing these shortcomings, Pan Jia et al. proposed a K-nearest neighbors-based (KNN-based) collision classification method utilizing locality-sensitive hashing (LSH) for nearest neighbor queries and KNN for collision probability estimation [33]. The composite collision detection method (CCD) combined with the KNN method improves the accuracy in some special cases and can be applied to the RRT algorithm [34]. However, limited by the query efficiency, the computation speed of the KNN-based method is not maximized.

The KNN method, designed to identify the nearest neighbor information, has garnered significant attention across multiple domains, including databases [35,36], image processing [37], and deep learning-based control [38,39]. To reduce the complexity of the data search, practical implementations of KNN often employ approximate solutions. Among these, the graph-based KNN approach stands out as a prominent method for approximate querying [40]. The graph-based method constructs a neighborhood graph, where each data point corresponds to a node, the edges connecting the nodes define the neighborhood relationship, and the search speed is improved by the edges [41]. HNSW (hierarchical navigable small world) is a widely influential graph-based approximate KNN method [42]. HNSW is recognized as one of the most effective algorithms for querying speeds with fewer than 10,000 nearest neighbors and guarantees perfect recall [43]. KNN-based collision detection only needs to query a small number of nearest neighbors (less than 100) of the new node. Consequently, the HNSW algorithm is particularly well-suited to this task.

In this paper, a fast collision detection method is proposed to transform the collision detection into a binary classification problem. It incrementally constructs a collision database throughout the planning phase, queries this database for state information near the target using the HNSW method, and introduces an enhanced Gaussian probabilistic classifier for state classification. By leveraging local state correlations and circumventing the inefficiencies of exact collision detection, this method requires no training and facilitates collision detections on continuous paths. The primary contributions include the following:

• An incremental construction approach for a collision information database based on HNSW;
• A KNN query technique for linear data employing minimum threshold segmentation;
• A novel collision classifier, tailored to the kinematic characteristics of the manipulator.

The rest of the article is organized as follows. Section 2 introduces the HNSW algorithm and describes the KNN query way for point state and line state. Section 3 presents a new collision classifier based on the kinematic characteristics of the manipulator. Simulation experiments in Section 4 verify the superior performance of the proposed method. The conclusion is given in Section 5.

2. Construction of a Collision Database Based on HNSW

This section presents the technical details of KNN queries in the proposed approach. For ease of reading, a nomenclature table is provided in Abbreviation part, which records the variables and abbreviations in the following sections.

2.1. Introduction to HNSW

The goal of the KNN query is to find the k closest neighbors to the target. The HNSW (hierarchical navigable small world) method is a graph-based approximate nearest neighbor (ANN) search algorithm primarily employed for efficiently handling KNN queries in high-dimensional data. HNSW merges the characteristics of small-world networks with a hierarchical strategy, accelerating queries and insertion operations through a multi-layered graph structure. The database construction entails the following:

• Hierarchical structure: HNSW constructs a multi-layered graph, with each layer being a navigable graph. The bottom layer (level 0) contains all nodes, with higher layers progressively having fewer nodes. Each node, upon insertion, is randomly assigned a maximum layer, ensuring sparsity in higher layers;
• Node insertion: A new node is first located in the highest layer graph using a greedy search to find its nearest neighbors, then moves down layer by layer, performing a local greedy search at each level until reaching level 0. At each layer, the node selectively connects to other nodes in that layer based on distance criteria;
• Connection strategy: Each node maintains a limited-size list of its perceived nearest neighbors. This constrains each node’s out-degree, maintaining the graph’s sparsity while ensuring efficient search capability.

The KNN query process for a new node unfolds as follows:

• Initiating Search: Given a query point, the search begins at the highest layer, employing a greedy strategy to find the nearest neighbor at the current layer;
• Descending Through Layers: Once a local nearest neighbor is found at a current layer, the search moves to the next lower layer, continuing the search from this basis. This process is repeated down to level 0;
• Greedy Search: At each layer, the next closest node is greedily selected by comparing distances between the node and the query point until no closer node can be found.

This paper implemented HNSW by using the open-source code (https://github.com/nmslib/hnswlib, accessed on 6 April 2024.) and Euclidean distance as the metrics. The parameters in the HNSW algorithm all use the values suggested in reference [42].

2.2. Construction of Collision Database

A node in C-space is defined by a set of joint angles, denoted as Q = [q₁, q₂, …, q_n]. The database stores these nodes Q with an associated collision label C, where C can be either collision-free (C = 0) or collision (C = 1). Using the HNSW method, we incrementally construct the collision database as shown in Figure 2.

As depicted in Figure 2, the planner initially obtains Q through random sampling in C-space, then uses HNSW to search for the k-nearest neighbors set N of Q. Condition 1 is introduced here to exclude nodes that are significantly distant from other nodes, as their collision probability is hard to estimable based on neighboring information.

Condition 1.

The number of neighbors satisfies  $k\ge k_{\min }$. A neighbor is a node whose distance from the new target is at most a threshold D_d, where D_d = 0.1·Scale, and Scale is the size of the database.

When Condition 1 is satisfied, the collision probability p of Q is estimated. If p satisfies Condition 2, the collision label C is determined by comparing p with the threshold t; C = 1 if p > t and C = 0 otherwise. The value of t will affect the accuracy of classification and it is suggested to be 0.2 to 0.5 in this paper. If Condition 1 is not satisfied, the label C of Q is obtained by exact collision detection and the node [Q C] is added to the database.

Condition 2.

The collision probability p of a node is not equal to 0.5. The reason for rejecting the classification of nodes with p = 0.5 is that these nodes are on the edge of the obstacle manifold and it is difficult for the classifier to accurately decide the collision category.

In summary, when both Conditions 1 and 2 are satisfied, the classification is completed by the collision probability of Q; otherwise, exact collision detection is performed on Q. After obtaining the collision label C, Q is added to the collision database.

2.3. KNN Query Method for Line Data Based on Threshold Segmentation

Line data P = [Q_t, Q_t+1] representing manipulator motion from Q_t to Q_t+1 pose a greater collision detection frequency in SBMP than single nodes. The dimensions of nodes Q and P cannot be aligned, so it is difficult to directly use the method shown in Figure 2 to perform KNN queries on P. In reference [33], a method of augmenting vectors was proposed to make Q and P have the same dimension, denoted as the LSH method. However, LSH complicates computations and risks false positives by treating line data as straight lines, as illustrated in Figure 3a.

To address these issues, we introduce a threshold segmentation-based method, where D_d segments line data into a set D of points, reducing computational complexity and minimizing false positives, as defined in Equation (1).

$$\mathit{P}=\left[{\mathit{Q}}_t,{\mathit{Q}}_{t+1}\right]\to D=\underset{n}{\underbrace{\{{\mathit{Q}}_t,\dots ,{\mathit{Q}}_i,\dots ,{\mathit{Q}}_{t+1}\}}},n\ge 2,i=0,\dots ,m,$$

(1)

where n is the number of split points, m = g(norm(P)/D_d), the norm( ) computes the length of a vector, and the g( ) is the rounding function.

The Q_i is calculated by Equation (2), where v is the unit vector of P.

$$Q_i=Q_t+i\cdot D_d\cdot v\hspace{0.17em},\hspace{0.17em}i=0,\dots ,m,$$

(2)

The elements in D are then traversed to obtain the neighbors and the union of the neighbors of all elements is the neighborhood of the line data.

Since the range S_f of the segmented query is smaller than the valid range S, the proposed method will lose a small number of positive neighbor nodes of line data, as shown in Figure 3b. The upper bound of the probability of the loss can be obtained from Equation (3). Despite a potential loss of positive neighbors due to smaller query ranges, this method’s impact on collision detection accuracy is negligible, making it an acceptable compromise.

$$p=1-\frac{S_f}{S}=\frac{2\left(m-2\right)D_d^2\left(1-\frac{\pi }{6}-\frac{\sqrt{3}}{4}\right)}{2\left(m-2\right)D_d^2}=\left(1-\frac{\pi }{6}-\frac{\sqrt{3}}{4}\right)<0.044,$$

(3)

In the range [−π, π], 10,000 sets of 2D, 3D, and 6D nodes are randomly selected as the database to test the performance of the LSH method and the proposed method. Figure 4 illustrates the query results for the 2D and 3D databases, with red dots representing queries by the proposed method and yellow dots indicating those by the LSH method. The proposed method demonstrated a notable reduction in false positives, a trend consistent across the 6D database as well, as presented in Figure 5. This indicates the proposed method’s superior accuracy in filtering relevant data across multiple dimensions.

KNN queries are performed on 50 different sets of continuous paths on three datasets. To fairly compare the performance of the two methods, brute-force query is used in both retrieval methods; the final time performance of the two methods in the continuous path scene is shown in Figure 6a, which shows the query time of the proposed method in the three datasets.

The proposed method exhibits superior efficiency in query times compared to the LSH method across datasets of varying dimensions. Specifically, the average query times for the proposed method are 7.1 × 10⁻⁴ s for 2D, 6.7 × 10⁻⁴ s for 3D, and 1.0 × 10⁻³ s for 6D datasets, as recorded in Figure 6a. In contrast, the LSH method shows average query times of 3.46 × 10⁻², 3.52 × 10⁻², and 4.01 × 10⁻² s for 2D, 3D, and 6D datasets, respectively, as indicated in Figure 6b. Thus, the proposed method achieves a query time efficiency improvement by two orders of magnitude for 2D and 3D datasets and a 97.5% reduction for 6D datasets. These simulation results underscore the proposed method’s significant time-saving advantage over the LSH approach.

In summary, the proposed method offers three key benefits in KNN queries of line data over the LSH method:

(1)

Lower Memory Requirement: It maintains the original database vector form without needing extra memory for storing augmented vectors;

(2)

Elimination of Post-Processing: By segmenting line data and querying neighbor data within each segment’s maximum range, it circumvents the need to filter out false positives, a step required by the LSH method;

(3)

Reduced Time Consumption: Utilizing the Euclidean distance for queries in the d-dimensional database minimizes computational complexity relative to the LSH’s augmented vector approach.

3. Design of Collision Classifier

It is considered that within a reasonable range of D_d, the node under test has a probability consistency with its neighbor set. Therefore, the collision probability P can be estimated according to the collision situation of neighboring nodes and the type of collision can be judged by P. Based on this theory, we design a new collision classifier.

Firstly, we use a Gaussian function to weigh the influence of neighbors at different distances and the weights are defined by Equation (4).

$$w_i=e^{-\gamma ·\rho \left(Q,x_i\right)},$$

(4)

where Q is the target node or a line data, x_i is the ith neighbor data, ρ() is the distance function, and γ is the weight controlling the scale, which is calculated by Equation (5).

$$\gamma =\raisebox{1ex}{$1$}\!\left/ \!\raisebox{-1ex}{${\left(0.05\cdot scale\right)}^2$}\right.,$$

(5)

As the distance between the neighbor node and the node or line data to be tested decreases, the probability of the two collision types being the same increases. The distance function ρ() is used to calculate the similarity of state variables, which is crucial for collision type estimation. Each joint has different effects on the movement of the link and the way to calculate the node distance will lead to changes in the motion configuration of the manipulator [44]. The influence of different joints is assigned by weights and the distance function is defined as

$$\rho =\Delta Q^{\mathrm{T}}M\Delta Q,,$$

(6)

where ΔQ is the joint distance vector and M is a diagonal matrix whose elements are joint weights. M can be customized by users; this paper suggests diag(0.32, 0.27, 0.23, 0.05, 0.2, and 0.05). The collision probability of node Q is estimated by Equation (7).

$$\mathrm{P}\left(c\left(Q\right)=1|N\right)=\mathrm{sigmoid}\left({\mu }_2+{\sigma }_{12}^{\mathrm{T}}{\sigma }_1^{-1}\left(Q-{\mu }_1\right)\right),$$

(7)

where N is the set of neighbors and the sigmoid function is used to improve the sensitivity of the classifier. Here, μ₁ and μ₂ are the mean values of the Gaussian distribution and σ₁₂ and σ₁ are the variances of the Gaussian distribution [45], which can be obtained from Equations (8) and (9), respectively. The parameter c_i is the collision state of x_i and the value is 0 or 1.

$${\mu }_1=\frac{{\displaystyle {\sum }_i{w}_i{x}_i}}{{\displaystyle {\sum }_i{w}_i}},\hspace{0.17em}\hspace{0.17em}{\mu }_2=\frac{{\displaystyle {\sum }_i{w}_i{c}_i}}{{\displaystyle {\sum }_i{w}_i}},$$

(8)

$$\begin{array}{l}{\mathit{\sigma }}_1=\frac{{\displaystyle {\sum }_i{w}_i\left(p-x_i\right){\left(p-x_i\right)}^{\mathrm{T}}}}{{\displaystyle {\sum }_i{w}_i}},\hspace{0.17em}\hspace{0.17em}{\sigma }_2=\frac{{\displaystyle {\sum }_i{w}_i{\left(c_i-{\mu }_2\right)}^2}}{{\displaystyle {\sum }_i{w}_i}}\\ {\sigma }_{12}=\frac{{\displaystyle {\sum }_i{w}_i\left(x_i-{\mu }_1\right)\left(c_i-{\mu }_2\right)}}{{\displaystyle {\sum }_i{w}_i}}\end{array},$$

(9)

To classify a node’s collision state, a threshold t ~ (0, 1) is applied. If P[c(Q) = 1|D] > t, Q is deemed colliding; otherwise, it is collision-free. A threshold of 0.2 is empirically chosen. For line data, the collision probability calculation differs and is outlined in a distinct equation, as shown in Equation (10).

$$\mathrm{P}\left(c\left(P\right)=1|N\right)=\mathrm{sigmoid}\left({\mu }_2+{\sigma }_{12}^{\mathrm{T}}{\sigma }_1^{-1}\left(\mathrm{Near}\left(l,{\mu }_1\right)-{\mu }_1\right)\right),$$

(10)

where the function Near(P, x) returns the point closest to x on P. The other variables are calculated by Equations (8) and (9) but it should be noted that the variables in the weighted distance function need to be changed to line data P.

The collision state of P is predicted using a similar threshold strategy, with t = 0.25 for line data, to determine whether the line segment is colliding or collision-free. This methodology underscores a systematic approach to estimating collision probabilities and classifying collision states for both nodes and line data, leveraging the relational dynamics within a node’s neighbor set.

4. Simulation and Results

4.1. Introduction to the Obstacle Environment and Databases

In this paper, the UR5 robot is used as the simulation object, as shown in Figure 7, where a_i is the length of the connecting rod and q_i is the rotation angle of the i-th joint. The axes of adjacent joints are orthogonal or parallel.

Four obstacle environments were simulated and designed, which were defined as cylinder scene, drawer scene, box scene, and sphere scene, respectively, as shown in Figure 8. The capsules are used to represent the linkage of the UR5 robot and obstacles are composed of spheres, capsules, and planes. The experiments were performed on an Intel^Ⓡ CoreTM i7-9750H, @2.60GHz CPU.

The cylinder scene has only one obstacle, the drawer scene and the box scene have 17 obstacles, including 12 capsules and 5 planes, and the sphere scene has 4 sphere obstacles. The database is composed of 30,000 sets of random sampling nodes in the configuration space and the data form is Q = [q₁, q₂,..., q₆, c]. The ratio of positive samples to negative samples was 1:3. The test set is divided into two categories, 1000 discrete nodes Q_s, and 1000 continuous path P_s. We use FCL to obtain the real collision situation of all the data.

Accuracy (Ac), specificity (TNR), and sensitivity (TPR) were used to measure the performance of the algorithm. Ac was the prediction accuracy rate, TPR was the proportion of samples correctly classified in collisions, and TNR was the proportion of samples correctly classified without collisions. The three metrics are defined in Equation (11).

$$Ac=\frac{\mathrm{TP}+\mathrm{TN}}{\mathrm{FP}+\mathrm{TN}+\mathrm{TP}+\mathrm{FN}},TNR=\frac{\mathrm{TN}}{\mathrm{FP}+\mathrm{TN}},TPR=\frac{\mathrm{TP}}{\mathrm{TP}+\mathrm{FN}}$$

(11)

where TP is the number of true positive samples, TN is the number of true negative samples, FP is the number of false positive samples, and FN is the number of false negative samples.

We use three methods for collision detection of test sets, namely the Fastron method [29], the LSH method [33], and the proposed method. The three algorithms are implemented at the same hardware level and the specific parameters of Fastron and LSH algorithms are detailed in their references.

4.2. Collision Detection of a Single Node

In examining the average detection times across four distinct scenarios, as delineated in Figure 9, it is observed that the Fastron method significantly outpaces the LSH and proposed methods in prediction speed, achieving microsecond-level efficiency. Nonetheless, this advantage is mitigated by the additional temporal expenditure required for Fastron’s training phase, which escalates exponentially with the augmentation of the training set size, ranging from 1 ms to 900 ms. Consequently, the ostensibly superior speed of Fastron is counterbalanced by its extensive training duration, eroding its overall temporal advantage.

The comparative analysis presented in Figure 9a–f highlights the performance enhancements afforded by the proposed method over the LSH method across varying training set sizes. Specifically, with a training set size of 1000, the proposed method exhibited improvements in prediction speed by 14.8%, 37.2%, 40.0%, and 38.3% across the four simulated environments. Incremental increases in training set size to 5000; 10,000; 15,000; 20,000; and 25,000 further amplified the proposed method’s performance advantage, with respective speed enhancements of 23.0%, 47.4%, 54.1%, 48.6%; 51.7%, 25.7%, 13.7%, 44.2%; 18.2%, 36.4%, 58.7%, 33.5%; 26.4%, 34.0%, 32.5%, and 43.5% and finally, 10.8%, 44.1%, 52.0%, and 25.6%.

Disregarding the duration necessary for training, the Fastron method distinctly leads in detection time, surpassing the other two methodologies by approximately an order of magnitude. Nevertheless, the LSH and the proposed methods have their inherent advantages, chiefly their non-reliance on a training phase. Furthermore, the proposed method benefits from a hierarchical design, ensuring that the time required for nearest neighbor retrieval via HNSW remains stable irrespective of increases in the training set size. Figure 10a–f shows the performance of the three methods on the three metrics. Obviously, the proposed method performs best in four scenes with training samples of different sizes.

Figure 10a delineates that, with a sample size of 1000, the Fastron method’s accuracy falls below 50% across all but one scenario, suggesting its limitations in complex environments with minimal training data. Conversely, the method proposed herein achieves significantly higher accuracy rates-surpassing the LSH method by margins of 6.0%, 16.9%, 28.7%, and 17.0% across the four evaluated scenes. This discrepancy highlights the proposed method’s superior ability to correctly identify collision instances.

As the sample size escalates to the range of 5000 to 20,000, the Fastron method shows an uptick in detection accuracy, surpassing 50% in all scenarios and occasionally exceeding 60%. However, these figures, while improved, remain modest. During this expansion, the relative performance gap between the LSH and proposed methods widens, with the latter maintaining an exceedingly high accuracy level of over 98% across all scenarios and sample sizes, underscoring its robustness and adaptability.

Upon further increasing the sample size to 25,000, the LSH method experiences a notable decline in detection accuracy. This reduction is attributed to the method’s susceptibility to the effective range or k-value parameter, complicating the extraction of obstacle features and causing positive sample neighbors to erroneously influence classification outcomes. The distance function introduced by the proposed methodology effectively mitigates such interference, sustaining remarkable accuracy levels of 100%, 99.9%, 99.6%, and 99.4% across the four scenes. This performance underscores the proposed method’s capacity to provide reliable collision detection in robotic path planning.

4.3. Collision Detection of Continuous Paths

According to the above analysis, it can be seen that the performance of the Fastron method in a complex environment is not ideal. Therefore, Fastron is excluded from the comparison. To ensure a balance between computational efficiency and detection accuracy, a training set with a fixed size of 2000 samples was employed, with a sampling frequency of 500 for precise collision detection along continuous paths.

Table 1. Execution time in four environments for exact collision detection, the LSH method, and our method (ms).

Method	Scene 1	Scene 2	Scene 3	Scene 4
Exact collision detection	51.06	76.78	106.24	121.37
LSH-based	8.40	8.22	9.85	7.90
Ours	1.72	2.27	1.91	1.89

presents the execution times for exact collision detection, LSH-based methods, and the proposed method across four distinct environments. The recorded times for the LSH-based method reflect only correctly predicted samples to mitigate the distortion from anomalous samples on the analysis’s integrity.

The data reveal that the proposed method outperforms the exact collision detection and LSH methods in terms of speed, boasting improvements of 96.6% and 79.5% in Scene 1, 97.0% and 72.4% in Scene 2, 98.2% and 80.6% in Scene 3, and 98.4% and 76.1% in Scene 4, respectively. These findings underscore the proposed method’s significant efficiency across all evaluated environments.

Furthermore, as illustrated in Figure 11, the proposed method demonstrates consistently high accuracy in detecting collisions along continuous paths, with accuracies of 99.1%, 98.7%, 98.7%, and 98.2% in the four scenarios, respectively. The true positive rate (TPR) confirms the proposed method’s reliability in accurately identifying collisions. Conversely, the LSH method’s performance is markedly impaired by irrelevant samples, leading to significantly reduced accuracies of 26.8%, 31.2%, 26.8%, and 30.9%, with true negative rates (TNR) of 23.7%, 23.9%, and 26.1%, respectively.

Simulation results show that the performance of the proposed method for continuous path detection (Ac, TNR, and TPR) is much better than that of the LSH method in four scenarios. In the detection of continuous paths, our method still maintains the same detection speed advantage as the discrete configuration sample, which is attributed to the method in Section 2.3. In addition, compared with the LSH method, the HNSW method has advantages in KNN queries.

4.4. Collision Detection for Manipulators with Different Degrees of Freedom

To evaluate the proposed method’s performance across manipulators with varying degrees of freedom (DOF), we conducted simulations on 2DOF and 3DOF manipulator systems. The obstacles and their associated collision manifolds for both manipulators are depicted in Figure 12.

Initially, 10,000 random nodes are sampled within the configuration space of each manipulator; the FCL is utilized to ascertain the true collision status of each node. Subsequently, a subset of 5000 samples was selected as the test set to assess the accuracy (Ac) and execution time of both the proposed method and the LSH method, with the comparative results detailed in

Table 2. The Ac and execution time of the LSH method and our method on the two manipulators.

Method	Number of DOF	Ac (%)	Time (μs)
LSH-based	2-DOF	94.47	720.43
	3-DOF	89.21	1323.04
Ours	2-DOF	99.57	479.11
	3-DOF	98.83	494.06

.

Table 2 demonstrates that the LSH and the proposed method both exhibit high accuracy in collision detection for the 2DOF manipulator, achieving 94.47% and 99.57% accuracy, respectively. Our method not only improves accuracy by 5% but also reduces the computation time by 33.50%. For the 3DOF manipulator’s collision detection, the proposed method’s time efficiency is even more pronounced, yielding a 62.65% time reduction compared to the LSH method while simultaneously increasing accuracy by 9.6%.

The above results show that the proposed method consistently delivers high accuracy and speed across manipulators with varying DOF, substantiating its robustness and efficiency in diverse robotic configurations.

5. Conclusions

This paper presents a rapid collision detection method based on probability classification, which can be used for collision detection of continuous paths. The proposed method constructs the collision database incrementally and the KNN query is performed on the target based on the HNSW method. Moreover, the challenge of continuous path KNN query is solved by adopting a threshold segmentation strategy, which effectively reduces the influence of irrelevant samples, thus improving the efficiency and accuracy of the query. Finally, the collision state classifier based on the weighted distance function is used to calculate the collision probability of the target state and complete the collision detection. Simulation results verify the effectiveness of the proposed method. Extending the proposed method to dynamic environments is a future work.