Efficient Collision Detection Framework For Enhancing Collision-Free Robot Motion

Download as pdf or txt
Download as pdf or txt
You are on page 1of 7

Efficient Collision Detection Framework

for Enhancing Collision-Free Robot Motion


Xiankun Zhu, Yucheng Xin, Shoujie Li, Houde Liu† , Chongkun Xia, Bin Liang

Abstract— Fast and efficient collision detection is essential


for motion generation in robotics. In this paper, we propose
an efficient collision detection framework based on the Signed
Distance Field (SDF) of robots, seamlessly integrated with a self-
collision detection module. Firstly, we decompose the robot’s
arXiv:2409.14955v1 [cs.RO] 23 Sep 2024

SDF using forward kinematics and leverage multiple extremely


lightweight networks in parallel to efficiently approximate
the SDF. Moreover, we introduce support vector machines to
integrate the self-collision detection module into the framework, (b) Trajectory Optimization
which we refer to as the SDF-SC framework. Using statistical
features, our approach unifies the representation of collision
distance for both SDF and self-collision detection. During this
process, we maintain and utilize the differentiable properties
of the framework to optimize collision-free robot trajectories.
Finally, we develop a reactive motion controller based on our
framework, enabling real-time avoidance of multiple dynamic
obstacles. While maintaining high accuracy, our framework
achieves inference speeds up to five times faster than previous
methods. Experimental results on the Franka robotic arm
demonstrate the effectiveness of our approach. Project page: (a) SDF and Self-Collision (c) Reactive Controller
https://sites.google.com/view/icra2025-sdfsc. Fig. 1: Overview of our work. (a) We combine our own
I. INTRODUCTION lightweight SDF framework with self-collision detection to
obtain a new collision detection framework SDF-SC. (b)
Efficiently generating safe operational trajectories for Trajectory Optimization based on the SDF-SC. (c) We use
robots is a critical challenge in the increasingly common SDF-SC to enable reactive control of the robot.
context of human-robot interactionn [1]. Motion planning al-
gorithms need to comprehensively consider external and self- a point to the surface of an object, which perfectly meets
collision issues. Much like how humans perceive obstacles, the collision detectors’ demand for quantifying the collision
collision detection serves as the robot’s means of sensing distance between robots and obstacles. When applied to
environmental obstacles, ensuring it avoids collisions with motion planning, SDF is extended to represent manifolds of
its surroundings, humans, and itself [2]. Due to their low general equality constraints [8], facilitating real-time trajec-
computational efficiency, traditional collision detection meth- tory smoothing and enabling full-body control tasks [9], [10].
ods, which are generally based on the geometric information In learning SDF models, Koptev et al. [11] employ a multi-
of obstacles and robot [3], [4], struggle to meet the current layer perceptron (MLP) to fit neural implicit signed distance
demand for real-time generation of collision-free trajectories functions. Liu et al. [12] propose a method that employs
in complex and dynamic environments. composite SDF networks to improve the fitting accuracy
The introduction of the Signed Distance Field (SDF) of articulated robots. Bernstein polynomials are employed
[5] concept offers a novel approach to addressing collision as basis functions for SDF, further enhancing storage and
detection challenges. Originally a key research area in com- computational efficiency [13]. These approaches learn SDF
puter vision and graphics, SDF has been widely used to models coupled with different joint configurations. However,
efficiently represent complex shapes [6], [7]. Migrating to the complexity of network architectures or the verbosity
the field of collision detection, SDF provides an intuitive of functional representations often limits the precision and
and effective way of determining the shortest distance from speed of collision distance inference. Moreover, SDF has
This work was supported by National Natural Science Foundation of
inherent limitations: it can only calculate the distance to ex-
China (No. 62203260, 92248304), The Shenzhen Science Fund for Distin- ternal obstacles, but cannot account for self-collision, which
guished Young Scholars (RCJC20210706091946001), Tsinghua SIGS Cross is a critical and non-negligible issue in both simulations and
Research and Innovation Fund (JC2021005).
Xiankun Zhu, Yucheng Xin, Shoujie Li, Houde Liu and Bin Liang
the real world [14], [15].
are with Shenzhen International Graduate School, Tsinghua University, In this paper, we utilize a forward kinematic chain to opti-
Shenzhen 518055, China. mize networks framework. During this process, we discover
Chongkun Xia is with School of Advanced Manufacturing, Sun Yat-sen
University, shenzhen 518107, China that extremely shallow networks are sufficient to achieve
†Corresponding authors: Houde Liu ([email protected]). excellent learning outcomes for the SDFs of each link.
Moreover, we employ a parallel framework to significantly
accelerate inference, increasing the speed for thousands of
points to five times that of previous methods [11], [12]. The
joint boundaries of self-collisions and external collisions are
robustly determined using a data-driven approach based on
kernel perceptrons [16], [17]. Building on this, we represent
the self-collision boundary using support vectors and develop
a processing function that leverages the statistical properties
of support vector machine (SVM) [18] results to calculate the
self-collision distance. By integrating self-collision and the
external collision distance, we address a critical limitation of Fig. 2: Overall algorithm pipeline for estimating collision
the prior SDF frameworks, which did not account for self- distance. Collision distance refers to the safety margin be-
collisions during planning. This leads to the development of tween the robot and potential collisions.
our final integrated system, termed the SDF-SC framework.
We apply the SDF-SC framework as inequality constraints through the forward kinematic chain. We map 3D query
within the optimization problem to evaluate its effectiveness points p ∈ R3 from the base coordinate system onto the
in generating collision-free trajectories. Additionally, we coordinate systems of each link through matrix transforma-
integrate SDF-SC with a reactive control algorithm to tackle tion, which can be written as
the challenge of a robot avoiding multiple dynamic obstacles. −1
Experimental results demonstrate that this integration signif- pk = (Tk ) p (2)
icantly enhances the robot’s dynamic response to external where pk denotes the coordinate representation of point p
and self-collision risks, leading to more robust and adaptable in the coordinate system of each link. Owing to the rigid
performance in real-world scenarios. attachment of the coordinate system to the link, the metric
The contributions of our work are as follows: distance dk from a point to the k th link is solely determined
• We propose an efficient and lightweight parallel net- by pk . It should be noted that when a point is located within
works framework for learning SDF, achieving high fit- the robot, we represent its distance to the surface of the links
ting accuracy while significantly accelerating inference with negative values.
speed. The distance from a point to an entire articulated robotic
• We augment the framework by incorporating integrated system is defined as the minimum distance from that point to
self-collision avoidance, while maintaining continuity each individual link within the system. For convenience, we
and differentiability throughout the process. employ the function Γ(q, p) to represent the SDF value from
• We demonstrate the effectiveness of SDF-SC framework a point p to the articulated robot with joint configuration q,
through rigorous trajectory optimization experiments with the following equation:
and leverage it to design a robust reactive controller.
Γ(q, p) = min(d1 (p1 ), d2 (p2 ), · · · , dK (pK )) (3)
II. F RAMEWORK C OMPOSITION
Deviating from the traditional approach of directly mapping
In this section, we introduce a holistic framework for the relationship from the joint configuration q and point p to
integrated collision detection, which includes a parallel the fuction Γ(q, p) via a network [11], [19], our methodology
networks framework for learning the SDF of articulated employs the learning of the distance functions dk for each
robots, an algorithm for self-collision evaluation, and dataset link to effectively fit Γ(q, p).
construction. The overall algorithmic pipeline for estimating We employ a series of lightweight neural networks to
the collision distance is depicted in Fig. 2, with the Franka approximate dk (pk ), with paralleling these networks to
Emika Panda robot serving as the subject for demonstration. expedite the computation speed. The minimum output among
A. Parallel Signed Distance Field Networks them is defined as SDF value for the whole articulated robot.
Consider an articulated robot with n degrees of freedom B. Self-Collision Detection
and K links, where the joint angles are defined as q = Calculating the geometric distances associated with self-

[q1 , q2 , . . . , qn ] ∈ Rn . Each link is considered as a rigid collisions in robots is an exceedingly laborious and time-
K
body, and the coordinate system {Sk }k=1 is rigidly attached consuming process [20]. However, due to the static and
to the rigid body. Let S0 denote the base coordinate system, unique nature of self-collision regions in the joint space of
and Tk ∈ SE(3) represent the homogeneous transformation articulated robots [21], [22], it is feasible to learn a con-
matrix from S0 to Sk . tinuous differentiable function that models the self-collision
boundary between colliding and free joint configurations.
Tk = F Kk (q) (1)
To learn the self-collision function, we perform uniform
where Tk is determined by the Denavit-Hartenberg (DH) sampling of configurations within the joint space and sub-
parameters and joint angles q of the articulated robot, and sequently categorize them into binary classes based on
the computation of the transformation matrix can be achieved whether they collide or not. The sampled configurations qi
are constrained within the joint limits q − , q + , and these TABLE I: Performance metrics of the self-collision detection
constraints are utilized to normalize all configurations to qn : Dataset Acc TPR TNR
Train 0.99 0.95 0.99
− +
q < qi < q , i = 1, . . . , Ns (4) Validation 0.97 0.86 0.98
Test 0.97 0.88 0.98
The label yi = +1 is used to denote that qi is in the self-
collision-free class, while yi = −1 indicates that qi is in
the self-colliding class. Subsequently, we employ a SVM
algorithm to construct a model for self-collision detection,
where the decision function takes the following form:
Nv
X
S(q) = αi yi K (q, q i ) + b (5)
i=1
where q i and yi represent the support vectors obtained from
the SVM algorithm and their respective collision labels. Nv (a) (b)
is the number of support vectors. 0 ≤ αi ≤ C denotes
the weight of the support vectors and C is an adjustable Fig. 3: (a) The score distribution of the self-collision detec-
parameter that serves as a penalty for errors, helping to tion model on the validation set. (b) The impact of different
prevent overfitting. b ∈ R denotes the bias term in the network architectures on fitting accuracy.
decision rule.
Within the purview of kernel function selection, empirical For the seamless integration and enhancement of self-
evidence from prior studies has demonstrated that both the collision detection with collision distance metrics, the robotic
Radial Basis Function (RBF) kernel and forward kinematics configurations are delineated into three distinct regions based
(FK) kernel [23] manifest commendable efficacy. However, on statistical features: distant, proximal, and transgressing
in deference to the exigencies of rapid self-collision de- the self-collision boundary. The proximal region of the
2
tection, the RBF kernel K (q, q i ) = e−γ∥q−qi ∥ has been self-collision boundary is delineated according to Gaussian
selected for its computational efficiency. distribution principles as the interval (u − σ, u + σ).
Within our framework, the collision distance is computed
C. Dataset Construction using the following formula:
In order to facilitate algorithmic comparison with prior
work, it is imperative to ensure the comparability of our D(q, p) = Γ(q, p) + P (S(q)) (6)
dataset with those utilized previously. Consequently, our where P denotes a processing function for the self-collision
sampling methodology aligns with the approaches employed scores. The equation for ∂D is naturally derived as follows:
in prior studies for dataset construction [11], [12], where
data points p are more densely collected near link surfaces. ∂D(q, p) ∂Γ(q, p) ∂P ∂S
= + · (7)
Additionally, we have supplemented our dataset with dis- ∂q ∂q ∂S ∂q
tances obtained from the GJK in simulation environments,
Nv
providing a valid supplement. Ultimately, the dataset for each ∂S X 2

link comprises 50 million entries. = −2γ αi yi e−γ∥q−qi ∥ (q − q i ) (8)


∂q i=1
In preparation for self-collision detection, we create train-
ing and validation datasets containing 100,000 and 30,000 In the case of single-arm robots, such as the Franka, self-
configurations, along with a test set of 50,000 configurations. collision configurations represent a special case, with their
Self-collision labels are generated using the self-collision occurrence being significantly less frequent than external
detection function from the FCL library [3]. collisions. Consequently, we expect the collision distance
III. H YPERPARAMETER S EARCH AND F RAMEWORK to approximate the robot’s SDF in regions deep within the
E VALUATION self-collision boundary, and to invert to negative values as
an indicator of constraint when configurations transgress the
A. Hyperparameter Search boundary. Furthermore, the gradient ∂P is aligned with ∂Γ,
Following hyperparameter optimization of the SVM both directing towards increasing collision distance.
model, we select C = 50 and γ = 1.0 as the optimal param- To this end, we formulate a continuous, monotonically
eters, resulting in a total of Nv = 10446 support vectors that increasing processing function P , bounded both above and
constitute the self-collision boundary. The score distribution below, as presented below:
of our trained model, as exhibited on the validation set, is
1
depicted in Fig. 3(a), indicates that S(q) = 0 represents the P (s) = − (9)
self-collision boundary constituted by the support vectors. 1 + eks+b0
When configurations are classified for self-collision based on where the parameters k and b0 are determined by set-
the boundary, the distribution of misclassified configuration ting a suitable interval for P (u − σ, u + σ) in the range
scores approximates a Gaussian distribution X ∼ N µ, σ 2 . [−0.95, −0.001] to fit the statistical characteristics of S.
Before evaluating the performance of external collision TABLE II: Comparison of computational times for collision
distance detection, we conduct an optimization search for distance by various algorithms
the number of layers and nodes in the parallel SDF network
Number of Query Pointss 1 100 1000 10000
to determine the most effective architectural parameters for SDF-SC (ours) ,ms 0.98 1.02 1.05 1.21
the networks. The layer count ranges from 1 to 5, while the Composite SDF1,ms 4.33 4.78 - 6.63
node count varies between 32, 64, and 128. Each network Neural-JSDF ,ms 0.18 0.42 1.13 8.98
RDF(N=8) ,ms 1.12 1.21 1.27 2.12
in the parallel configuration uniformly employs a Root GJK ,ms 0.95 87.4 815 7984
Mean Squared Error (RMSE) loss function, with the Adam 1 The data cited in this row is derived from paper [12].
optimizer used consistently across all. The ReLU function is
selected as the activation function and the maximum number
of iterations is set to 100 to ensure network convergence.
The impact of different network frameworks on the final
RMSE convergence is illustrated in Fig. 3(b). Experiments
reveal that a parallel configuration comprising merely two
layers of networks is sufficient to achieve an excellent fit for
the articulated robot’s SDF, with additional layers providing
minimal benefit in terms of RMSE reduction and, conversely,
increasing the inference time. To balance accuracy and
inference speed, we select a configuration of 2 layers with Fig. 4: The reconstruction of the Franka Emika Panda’s links
64 nodes as the final architecture parameters for our parallel is performed based on the distance isosurfaces D(q, p) =0
SDF networks, which makes our model significantly lighter cm (solid) and D(q, p) =5 cm (transparent). Different colors
compared to previous work [11], [12]. represent different links.

B. Framework Evaluation
generally five times greater than that of previous methods in
All experiments are conducted using an Intel Core i7- standard applications.
13700KF and an NVIDIA GeForce RTX 4070 GPU, with The average RMSE across the links of the articulated robot
all GPU operations performed using batch processing. serves as a metric for the quality of SDF fitting. Distances are
To evaluate the framework’s effectiveness for self-collision categorized with d <0.1 m as the region close to the robot
detection, we evaluate it on the test dataset, employing and 0.1 m< d <1.2 m as the region distant from the robot.
Accuracy (Acc), True Negative Rate (TNR), and True Pos- We compare our results within these two regions with those
itive Rate (TPR) as metrics. The collision distance D(q, p) of the Neural-JSDF, Composite SDF and RDF algorithms.
boundary for classification is set at u − σ. A high TPR The results are as shown in the Table III.
prevents the misclassification of collision-free configurations
as colliding, while a low TPR may result in overly con- TABLE III: Comparing the SDF accuracy obtained by the
servative detection. On the other hand, a high TNR is the algorithms
most critical metric that should be maximized, as it prevents Region d < 0.1 m 0.1 m < d < 1.2 m
the erroneous labeling of colliding configurations as free. SDF-SC (ours), cm 0.16 0.28
The self-collision detection model within our framework Composite SDF, cm 0.21 0.36
Neural-JSDF, cm 1.04 1.06
achieves commendable performance across various datasets, RDF(N=8), cm 0.41 0.54
maintaining a TNR of over 0.98 in all cases, as detailed in
the Table I.
In our experimental analysis, we evaluate the mean com- Our method exhibits superior accuracy in both distance
putational latency of our comprehensive framework, denoted intervals, with an overall precision of 0.19 cm, attaining
as SDF-SC, across a spectrum of 3D query points. This millimeter-level accuracy. To more vividly demonstrate the
evaluation is conducted in comparison with the relevant accuracy of our model, we utilize the integrated framework
Composite-SDF [12], Neural-JSDF [11], RDF [13] and the to generate a 3D point cloud, thereby reconstructing the
standard Gilbert-Johnson-Keerthi (GJK) [4] algorithm. The surface of the Panda robot at D(q, p) =0 cm. As illustrated
computations within the SDF-SC framework encompass not in Fig. 4, the SDF reconstructed by our method accurately
only the forward kinematics chain and the parallel SDF reflects the surface contours of the Panda robot, with very
networks but also self-collision detection. The experimental uniform widths between adjacent isosurfaces, indicating that
results are presented in Table II. Despite the comprehen- the integrated framework can effectively obtain the precise
sive nature of these computations, our detection efficiency SDF of the articulated robot.
remains highly superior, exhibiting only a slight disadvan- The evaluations conducted indicate that the integration of
tage when the number of query points is relatively low in self-collision and external collision detection into a unified
comparison to the Neural-JSDF algorithm. However, in gen- collision distance D(q, p) within our framework is viable,
eral scenarios where environmental modeling often requires and it performs effectively in detecting both types of colli-
sampling several thousand points, our detection efficiency is sions.
IV. M ONTION P LANNING E XPERIMENTS
In this section, we substantiate the superiority of our
framework in robotic motion planning through two categories
of experiments: 1) Trajectory Optimization 2) Reactive Con-
trol. During planning, we utilize D(q, p) as the collision
distance to simultaneously avoid self-collisions and external
collisions.
Fig. 5: Trajectory optimization in simple (left) and complex
A. Trajectory Optimization with SDF-SC (right) scenarios, where the red dashed line represents the
To achieve the most efficient trajectory while meeting trajectory generated by RRT*, and the green dashed line
the requirement of avoiding static obstacles, we define the represents the trajectory optimized using SDF-SS with RRT-
optimization problem as follows: connect.
T
X −1
∥F K q t+1 − F K (q t ) ∥2

min f (q) =
q
t=0

subject to D(q, p) + ϵ ≤ 0
∥q t+1 − q t ∥ ≤ wmax
(10)
∥ee(q t+1 ) − ee(q t )∥ ≤ v max
q− ≤ q ≤ q+
where f (q) denotes the cost function of the optimization
problem. T represents the number of planned path points (a) (b)
or time duration and F K stands for the forward kinematics Fig. 6: (a) Collision distance during the movement period.
chain of each link. ϵ denotes the safety margin, which is (b) Comparative analysis of trajectory planning methods.
employed to augment the level of conservatism in collision
avoidance. ee denotes the position of the end-effector, while
To validate the self-collision detection capabilities of our
v max represents the maximum velocity for the end-effector.
framework, we deliberately introduce self-collision configu-
To demonstrate the broad effectiveness of our optimization
rations into the initial trajectories. Our framework is tasked
algorithm, we design scenarios with a simple environment
with detecting the presence of self-collisions along these tra-
containing three obstacles and a complex environment con-
jectories and optimizing them to eliminate such incidents. We
taining ten obstacles, as illustrated in Fig. 5. In our ap-
design approximately 20 such trajectories, and our system
proach, we utilize the RRTconnect [24] algorithm to swiftly
successfully detects 95% of the self-collisions, while also
generate an initial trajectory, followed by employing the
optimizing the trajectories to avoid these points. However, it
SLSQP [25] optimizer for our optimization problem, thereby
is important to note that due to the use of a steep gradient
obtaining the final optimized trajectory. Since our framework
to escape self-collisions, the trajectory transitions were not
is differentiable, the gradients of the constraint functions
as smooth as desired, necessitating additional interpolation
can be obtained using the automatic differentiation functions
to refine the affected segments. Our framework is capable
in the PyTorch library. As a benchmark, we use the RRT*
of detecting both external and self-collisions within the
algorithm [26], which is widely applied in path planning and
trajectory, while also using gradient guidance to optimize
theoretically proven to achieve asymptotic optimality [27].
trajectory and avoid collisions.
The maximum iteration time for RRT* is set to 50 s. Fig. 6(a)
shows the variation of collision distance over the movement
time during our trajectory planning experiments in a simple B. Reactive Control with SDF-SC
scenario for various algorithms. By minimizing the path cost, Inspired by NEO [28], we utilize the SDF-SC framework
our approach allows the robot arm’s trajectory to approach to design a reactive controller, designated as NEO-SS, in-
obstacles more closely, without exceeding safety margins. tended for real-time dynamic obstacle avoidance. We define
Additionally, we perform 50 planning experiments in both the following quadratic programming (QP) problem [29] for
simple and complex scenarios, and then take the average of the controller:
the results from both sets. The comparative results are illus-
1 ⊤
trated in Fig. 6(b). Benefiting from the fully differentiable min g(x) = x Qx + C ⊤ x
nature of our framework, which requires only approximately x 2
3 ms per differentiation, we significantly reduce the path
subject to Jx = ν
cost in trajectory planning using SDF-SC significantly, while
maintaining a high success rate and relatively fast computa- Ax ≤ B (11)
tion time. X− ≤ x ≤ X+
where   TABLE IV: Comparison between NEO and NEO-SS

x= ∈ R(n+6) (12) Controller NEO NEO-SS(ours)
δ Average time 3ms 6ms
Success rate 58% 86%
δ is a slack variable with respect to q̇, representing the dif-
ference between the desired and actual end-effector velocity
ν. C maximizes the manipulability of the robot, which is
synthesized from J m [30] and the identity matrix. In NEO,
the inequality constraints of a QP problem are represented
as:
d − ds
˙
J dl (q̃)q̃(t) ≤ξ − n̂⊤orl ṗol (t) (13) (b)
di − ds
where q̃ represents the subset of joint variables that affect the
distance to the obstacles, denoted by J d . di for the activation
distance of the controller and ds for the safe stopping
distance. The variable l denotes the index of obstacles, and
the presence of multiple obstacles leads to a substantial
increase in the constraints derived from the equations, which (a) (c)
significantly impacts the success rate of solving the optimiza- Fig. 7: (a) Collision distance during the movement period.
tion problem. In our SDF-SC framework, obstacles are not (b) Simulation experiment. (c) Real robot experiment.
represented as individual geometric shapes but are instead
denoted as a collection of point clouds. Consequently, we
comprises a Franka Emika robotic arm and a Realsense
can improve the inequality constraints as follows:
D435i camera mounted at a fixed location for detecting
∂D ˙ D − ds obstacles. The robotic arm endeavors to execute reactive
q̃ ≤ ξ − ṗ − s (14)
∂q̃ di − ds control measures to avoid the introduced dynamical obstacles
where ṗ represents the maximum velocity of the detected concurrent with its mission to reach the designated target.
obstacle points, and s denotes the self-collision boundary, During the experimental procedure, the robotic arm exhibits
which is utilized to constrain the configurations to prevent a distinct aversion to obstacles. As the obstacles approach
self-collisions. within a certain threshold, the arm articulates its joints to
To assess the effectiveness of the improvements, we con- move away from the obstacles, endeavoring to maintain the
struct a simulation scenario as depicted in Fig. 7(b). In the collision distance within a safe margin. Once the obstacles
scenario, multiple obstacle spheres are moving toward the exit the reactive controller’s activation range, the arm re-
robotic arm, which must complete the task of moving to a sumes its trajectory toward the target posture, calculated
specified posture while avoiding these dynamic obstacles. In based on the inverse kinematics solution. For demonstra-
order to compare the effectiveness of the algorithm before tion videos in both simulation and real-world environments,
and after the improvement of the inequality constraints, we please refer to the attached video files.
set the adjustable parameters of NEO and NEO-SS to be
V. CONCLUSIONS
consistent, with di =0.4m, ds =0.02m, and ξ = 1.0, and
testing experiments are conducted under the same scenario. In this paper, we improved the framework of learning
The experimental data is illustrated in Fig. 7(a). It is evident SDF based on the forward kinematics of articulated robots,
that NEO-SS exhibits reduced fluctuations, effectively keep- utilizing multiple extremely lightweight networks in par-
ing the distance well within the safety threshold. Conversely, allel to more efficiently approximate SDF. Additionally,
NEO exhibits excessive fluctuations and results in a collision. we innovatively introduced self-collision detection into the
To evaluate the robustness of the reactive control algo- framework, resulting in a collision detector that completely
rithm, we randomize the positions and velocities of the prevents collisions, termed SDF-SC. We then evaluated the
obstacles within a certain range. The velocity of each ob- detector’s performance in both self-collision and external
stacle fluctuates randomly around 0.2 m/s. We perform 50 collision detection, achieving highly satisfactory results in
experiments to ascertain the per-step computational latency both domains. Furthermore, we utilized the SDF-SC as a
of two control algorithms and to evaluate the efficacy of collision constraint in trajectory optimization, enabling the
collision avoidance in each experiment. The compiled data robot to minimize path cost during planning while ensuring
regarding per-step latency and success rates are articulated collision-free operation. Lastly, we integrated SDF-SC into
in Table IV. The experimental data indicates that in complex the design of a reactive controller, successfully achieving
scenarios involving multiple dynamic obstacles, the NEO-SS dynamic obstacle avoidance in complex environments.
algorithm demonstrates superior robustness compared to the There are aspects of our approach that offer potential
NEO algorithm. for further improvement. In the future, we plan to extend
As shown in Fig. 7(c), we implement our reactive control our framework to dual-arm robots to further enhance their
algorithm NEO-SS on a robot arm platform. The platform capabilities.
R EFERENCES [20] M. Koptev, N. Figueroa and A. Billard, ”Real-Time Self-Collision
Avoidance in Joint Space for Humanoid Robots,” in IEEE Robotics
[1] S. Shi, J. Chen and Y. Li, ”Hybrid Safety Certificate for Fast Collision and Automation Letters, vol. 6, no. 2, pp. 1240-1247, April 2021.
Checking in Sampling-Based Motion Planning,” in IEEE Robotics and [21] Rakita, Daniel , B. Mutlu , and M. Gleicher . ”RelaxedIK: Real-time
Automation Letters, vol. 8, no. 1, pp. 113-120, Jan. 2023. Synthesis of Accurate and Feasible Robot Arm Motion.” Robotics:
Science and Systems 2018.
[2] C. Quintero-Peña, W. Thomason, Z. Kingston, A. Kyrillidis and L. E.
[22] Kim, Jihwan and Frank Chongwoo Park. “PairwiseNet: Pairwise
Kavraki, ”Stochastic Implicit Neural Signed Distance Functions for
Collision Distance Learning for High-dof Robot Systems.” Conference
Safe Motion Planning under Sensing Uncertainty,” 2024 IEEE Inter-
on Robot Learning (2023). 229:2863-2877.
national Conference on Robotics and Automation (ICRA), Yokohama,
[23] N. Das and M. C. Yip, ”Forward Kinematics Kernel for Improved
Japan, 2024, pp. 2360-2367.
Proxy Collision Checking,” in IEEE Robotics and Automation Letters,
[3] J. Pan, S. Chitta and D. Manocha, ”FCL: A general purpose library for
vol. 5, no. 2, pp. 2349-2356, April 2020.
collision and proximity queries,” 2012 IEEE International Conference
[24] J. J. Kuffner and S. M. LaValle, “RRT-connect: An efficient approach
on Robotics and Automation, Saint Paul, MN, USA, 2012, pp. 3859-
to single-query path planning,” in Proc. Millennium Conf. IEEE Int.
3866.
Conf. Robot. Automat., 2000, pp. 995–1001.
[4] E. G. Gilbert, D. W. Johnson and S. S. Keerthi, ”A fast procedure for [25] D. Kraft, “Algorithm 733: TOMP–Fortran modules for optimal control
computing the distance between complex objects in three-dimensional calculations,” ACM Trans. Math. Softw., vol. 20, no. 3, pp. 262–281,
space,” in IEEE Journal on Robotics and Automation, vol. 4, no. 2, 1994.
pp. 193-203, April 1988. [26] Karaman S, Frazzoli E, “Sampling-based algorithms for optimal
[5] J. J. Park, P. Florence, J. Straub, R. Newcombe, and S. Lovegrove, motion planning,” The International Journal of Robotics Research.
“Deepsdf: Learning continuous signed distance functions for shape 2011;30(7):846-894.
representation,” in Proc. IEEE Conf. on Computer Vision and Pattern [27] M. Elbanhawi and M. Simic, ”Sampling-Based Robot Motion Plan-
Recognition (CVPR), 2019, pp. 165–174. ning: A Review,” in IEEE Access, vol. 2, pp. 56-77, 2014.
[6] M. Macklin, K. Erleben, M. M¨uller, N. Chentanez, S. Jeschke, and Z. [28] J. Haviland and P. Corke, ”NEO: A Novel Expeditious Optimisation
Corse, “Local optimization for robust signed distance field collision,” Algorithm for Reactive Motion Control of Manipulators,” in IEEE
Proceedings of the ACM on Computer Graphics and Interactive Robotics and Automation Letters, vol. 6, no. 2, pp. 1043-1050, April
Techniques, vol. 3, no. 1, pp. 1–17, 2020. 2021.
[7] D. Joho, J. Schwinn and K. Safronov, ”Neural Implicit Swept Vol- [29] J. Haviland, N. Sünderhauf and P. Corke, ”A Holistic Approach to
ume Models for Fast Collision Detection,” 2024 IEEE International Reactive Mobile Manipulation,” in IEEE Robotics and Automation
Conference on Robotics and Automation (ICRA), Yokohama, Japan, Letters, vol. 7, no. 2, pp. 3122-3129, April 2022.
2024, pp. 15402-15408. [30] J. Park, W. Chung, and Y. Youm, “Computation of gradient of
[8] G. Sutanto, I. R. Fern´andez, P. Englert, R. K. Ramachandran, and manipulability for kinematically redundant manipulators including
G. Sukhatme, “Learning equality constraints for motion planning on dual manipulators system,” Trans. Control, Automat. Syst. Eng., vol.
manifolds,” in Proc. Conference on Robot Learning (CoRL). PMLR, 1, no. 1, pp. 8–15, 1999.
2021, pp. 2292–2305.
[9] S. Fujii and Q. -C. Pham, ”Realtime Trajectory Smoothing with Neural
Nets,” 2022 International Conference on Robotics and Automation
(ICRA), Philadelphia, PA, USA, 2022, pp. 7248-7254.
[10] P. Liu, K. Zhang, D. Tateo, S. Jauhri, J. Peters and G. Chalvatzaki,
”Regularized Deep Signed Distance Fields for Reactive Motion Gener-
ation,” 2022 IEEE/RSJ International Conference on Intelligent Robots
and Systems (IROS), Kyoto, Japan, 2022, pp. 6673-6680.
[11] M. Koptev, N. Figueroa and A. Billard, ”Neural Joint Space Implicit
Signed Distance Functions for Reactive Robot Manipulator Control,”
in IEEE Robotics and Automation Letters, vol. 8, no. 2, pp. 480-487,
Feb. 2023.
[12] B. Liu, G. Jiang, F. Zhao and X. Mei, ”Collision-Free Motion
Generation Based on Stochastic Optimization and Composite Signed
Distance Field Networks of Articulated Robot,” in IEEE Robotics and
Automation Letters, vol. 8, no. 11, pp. 7082-7089, Nov. 2023.
[13] Y. Li, Y. Zhang, A. Razmjoo and S. Calinon, ”Representing Robot Ge-
ometry as Distance Fields: Applications to Whole-body Manipulation,”
2024 IEEE International Conference on Robotics and Automation
(ICRA), Yokohama, Japan, 2024, pp. 15351-15357.
[14] C. Fang, A. Rocchi, E. M. Hoffman, N. G. Tsagarakis and D. G.
Caldwell, ”Efficient self-collision avoidance based on focus of interest
for humanoid robots,” 2015 IEEE-RAS 15th International Conference
on Humanoid Robots (Humanoids), Seoul, Korea (South), 2015, pp.
1060-1066.
[15] S. S. M. Salehian, N. Figueroa, and A. Billard, “A unified framework
for coordinated multi-arm motion planning,” Int. J. Robot. Res., vol.
37, no. 10, pp. 1205–1232, 2018.
[16] N. Das and M. Yip, ”Learning-Based Proxy Collision Detection
for Robot Motion Planning Applications,” in IEEE Transactions on
Robotics, vol. 36, no. 4, pp. 1096-1114, Aug. 2020.
[17] Y. Zhi, N. Das and M. Yip, ”DiffCo: Autodifferentiable Proxy Col-
lision Detection With Multiclass Labels for Safety-Aware Trajectory
Optimization,” in IEEE Transactions on Robotics, vol. 38, no. 5, pp.
2668-2685, Oct. 2022.
[18] T. Joachims and C.-N. Yu, “Sparse kernel SVMs via cutting-plane
training,” Mach. Learn., vol. 76, pp. 179–193, 08 2009.
[19] J. Chase Kew, B. Ichter, M. Bandari, T.-W. E. Lee, and A. Faust,
“Neural collision clearance estimator for batched motion planning,”
in Proc. Int’l Workshop on Algorithmic Foundations of Robotics, pp.
73–89, 2020.

You might also like