Efficient Collision Detection Framework For Enhancing Collision-Free Robot Motion
Efficient Collision Detection Framework For Enhancing Collision-Free Robot Motion
Efficient Collision Detection Framework For Enhancing Collision-Free Robot Motion
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
q̇
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.