Robotic Arm Path Planning in Narrow Spaces Using Fast Collision Detection of High-dimensional Configurations
-
摘要: 针对机械臂在狭窄空间中路径规划效率与成功率低、碰撞检测耗时占比高的问题, 提出一种基于高维构型空间快速碰撞检测的在线路径规划算法. 该算法以RRT-Connect为基线路径搜索框架, 并行运行基于高维构型在线聚类的快速碰撞检测模块. 其中, 后者包括高维构型数据集均衡采样与快速碰撞检测模型在线训练两个阶段. 具体而言, 数据集在线构建阶段通过引入启发式策略以充分挖掘狭窄通道内的自由构型, 克服仅通过均匀采样获取的数据集中碰撞构型、自由构型数量不均衡的问题, 为后续快速碰撞检测模型训练提供可靠的数据支撑; 数据集构建完成后, 通过对碰撞、自由构型在线聚类, 以簇的形式表征高维构型空间下两类构型的分布; 基于训练得到的簇模型, 将基线算法中基于包围盒的碰撞检测转化为采样构型与聚类簇间的距离计算, 极大降低单次碰撞检测耗时, 进而有效提升算法整体搜索效率. 通过在简单、开放、封闭三类狭窄环境下的仿真测试与实验验证, 表明所提算法在路径搜索效率和成功率方面具有显著优势.Abstract: Aiming at the problems of low efficiency and success rate, high percentage of time-consuming collision detection in robotic arm path planning within narrow spaces, this paper proposes an online path planning algorithm for narrow spaces utilizing online fast collision detection in high-dimensional configuration space. The algorithm takes RRT-Connect as the baseline path search framework, and runs the fast collision detection module based on high-dimensional configurations online clustering in parallel. The module includes balanced sampling of the high-dimensional configuration dataset and online training of the fast collision detection model. Specifically, in the phase of dataset online construction, a heuristic strategy is introduced to fully explore the free configurations in narrow space, overcoming the problem that uniform sampling leads to an uneven number of collision and free configurations in the dataset, and to provide effective data support for the subsequent model training; After the dataset construction, the distribution of the two types of configurations in the high-dimensional space is characterized in the form of clusters by online clustering of collision and free configurations; Using the trained cluster model, the collision detection based on bounding box in the baseline algorithm can be transformed into the calculation of the distance between the sampled configuration and the cluster, which greatly reduces the time consuming for a single collision detection and effectively improves the efficiency of proposed algorithm. Through simulation tests and experimental verification in simple, opened and closed three types of narrow environment, the results show that the proposed algorithm has significant advantages in terms of path searching efficiency and success rate.
-
图 1 狭窄空间下基于高维构型空间快速碰撞检测的机械臂在线路径规划算法框图
Fig. 1 Block diagram of online robotic arm path planning in narrow spaces using fast collision detection in high-dimensional configuration space
图 3 兼顾全局性与狭窄特性的数据集在线构建策略图解
Fig. 3 Diagram of the online dataset construction that balances the global and narrow characteristics
图 4 构型与聚类簇之间距离计算图示
Fig. 4 Diagram of distance calculation between configurations and clustering clusters
图 8 简单狭窄环境仿真测试: 物体抓取与分
Fig. 8 Simple narrow environment simulation test: object grasping and sorting
图 9 开放式狭窄环境仿真测试: 配网带电作业
Fig. 9 Opened narrow environment simulation test: live-line working in distribution networks
图 10 封闭式狭窄环境仿真测试: 重大装备加工制造
Fig. 10 Closed narrow environment simulation test: major equipment processing and manufacturing
图 12 机械臂实机验证环境示意图
Fig. 12 Schematic diagram of real machine verification environment of robotic arm
图 13 狭窄环境下机械臂规划路径示意图
Fig. 13 Schematic diagram of robotic arm planning path in narrow environment
表 1 简单狭窄环境下算法性能对比
Table 1 Algorithm performance comparison in simple narrow environment
采样方法 成功率 规划时间均值/s 平均碰撞检测次数 Ours 0.90 $ {\boldsymbol{0.94\pm0.18}} $ 14336 RRT-Connect 0.90 $ 1.24\pm0.68 $ 23759 SDCL 0.90 $ 1.79\pm0.42 $ 23830 RRT 0.10 $ 1.87\pm0.01 $ 31543 Lazy-RRT 0.17 $ 2.17\pm5.93 $ 314 TRRT - - - PRM 0.17 $ 4.27\pm4.64 $ 62357 Lazy-PRM - - - KPIECE 0.13 $ 4.42\pm6.03 $ 76696 BKPIECE 0.87 $ 3.54\pm3.33 $ 60330 EST - - - 
下载: 导出CSV
表 2 开放式狭窄环境下算法性能对比
Table 2 Algorithm performance comparison in opened narrow environments
采样方法 成功率 平均时间均值/s 平均碰撞检测次数 Ours 0.83 $ 19.50\pm5.01 $ 83319 RRT-Connect 0.83 $ 46.76\pm37.10 $ 221551 SDCL 0.80 $ 19.09\pm1.22 $ 78357 RRT - - - Lazy-RRT - - - TRRT - - - PRM 0.03 $ 30.99\pm0 $ 90170 Lazy-PRM - - - KPIECE - - - BKPIECE 0.50 $ 42.06\pm5.31 $ 130682 EST - - -
下载: 导出CSV
表 3 封闭式狭窄环境下算法性能对比
Table 3 Algorithm performance comparison in closed narrow environments
采样方法 成功率 平均时间均值/s 平均碰撞检测次数 Ours 0.87 $ {\boldsymbol{35.05\pm39.82}} $ 332764 RRT-Connect 0.73 $ 84.40\pm2.22 $ 1007664 SDCL 0.90 $ 41.67\pm1.81 $ 377318 RRT - - - Lazy-RRT - - - TRRT - - - PRM - - - Lazy-PRM - - - KPIECE - - - BKPIECE 0.97 $ 84.17\pm53.95 $ 460425 EST - - -
下载: 导出CSV
表 4 实机实验算法性能对比
Table 4 Algorithm performance comparison in real-world experiments
环境类别 算法 规划成功率 规划耗时均值/s 开放式狭窄实机环境 Ours $ {\bf{1.00}} $ 1.05 RRT-Connect 0.83 1.53 SDCL 0.83 2.41 封闭式狭窄实机环境 Ours $ {\bf{1.00}} $ 0.38 RRT-Connect 1.00 0.63 SDCL 0.83 2.99
下载: 导出CSV
-
[1] 王耀南, 江一鸣, 姜娇, 张辉, 谭浩然, 彭伟星, 等. 机器人感知与控制关键技术及其智能制造应用. 自动化学报, 2023, 49(3): 494−513 doi: 10.16383/j.aas.c220995Wang Yao-Nan, Jiang Yi-Ming, Jiang Jiao, Zhang Hui, Tan Hao-Ran, Peng Wei-Xing, et al. Key technologies of robot perception and control and its intelligent manufacturing applications. Acta Automatica Sinica, 2023, 49(3): 494−513 doi: 10.16383/j.aas.c220995 [2] 张振国, 毛建旭, 谭浩然, 王耀南, 张雪波, 江一鸣. 重大装备制造多机器人任务分配与运动规划技术研究综述. 自动化学报, 2024, 50(1): 21−41 doi: 10.16383/j.aas.c220957Zhang Zhen-Guo, Mao Jian-Xu, Tan Hao-Ran, Wang Yao-Nan, Zhang Xue-Bo, Jiang Yi-Ming. A review of task allocation and motion planning for multi-robot in major equipment manufacturing. Acta Automatica Sinica, 2024, 50(1): 21−41 doi: 10.16383/j.aas.c220957 [3] 胡子琦, 苑晶, 公岩松, 俞诗卓, 张雪波. 面向配网带电作业机器人的输电线引线重建. 控制工程, 2021, 28(11): 2123−2130Hu Zi-Qi, Yuan Jing, Gong Yan-Song, Yu Shi-Zhuo, Zhang Xue-Bo. Reconstruction of lead wires of power lines for live-line working robots in distribution networks. Control Engineering of China, 2021, 28(11): 2123−2130 [4] 柏光瑞, 郑育祥, 吴凯, 吴少雷, 郭锐, 赵玉良, 等. 面向配网带电作业机器人的导线识别定位方法. 电网技术, 2023, 47(6): 2604−2611Bai Guang-Rui, Zheng Yu-Xiang, Wu Kai, Wu Shao-Lei, Guo Rui, Zhao Yu-Liang, et al. Power line identification and localization for live working robot of distribution line. Power System Technology, 2023, 47(6): 2604−2611 [5] 张会文, 苏贇, 刘玉旺, 赵新刚, 付成龙, 冷雨泉. SGPD: 复杂环境下的稳定抓取位姿检测. 机器人, 2024, 46(6): 703−712 doi: 10.13973/j.cnki.robot.230338Zhang Hui-Wen, Su Yun, Liu Yu-Wang, Zhao Xin-Gang, Fu Cheng-Long, Leng Yu-Quan. SGPD: stable grasp pose detection in complex environment. Robot, 2024, 46(6): 703−712 doi: 10.13973/j.cnki.robot.230338 [6] 蔡鹏, 岳晓奎. 基于HS-RRV算法的空间机械臂在轨装配运动规划. 机器人, 2023, 45(2): 166−178 doi: 10.13973/j.cnki.robot.220024Cai Peng, Yue Xiao-Kui. Motion Planning of Space manipulator for on-orbit assembly based on HS-RRV algorithm. Robot, 2023, 45(2): 166−178 doi: 10.13973/j.cnki.robot.220024 [7] Ratliff N, Zucker M, Bagnell J A, Srinivasa S. CHOMP: gradient optimization techniques for efficient motion planning. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Kobe, Japan: IEEE, 2009. 489−494 [8] Kalakrishnan M, Chitta S, Theodorou E, Paster P, Schaal S. STOMP: stochastic trajectory optimization for motion planning. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Shanghai, China: IEEE, 2011. 4569−4574 [9] Lavalle S. Rapidly-exploring random trees: a new tool for path planning. Research Report, 1999 [10] Kavraki L E, Svestka P, Latombe J C, Overmars M H. Probabilistic roadmaps for path planning in high-dimensional configuration spaces. IEEE Transactions on Robotics and Automation, 2002, 12(4): 566−580 doi: 10.1109/70.508439 [11] Das N, Yip M. Learning-based proxy collision detection for robot motion planning applications. IEEE Transactions on Robotics, 2020, 36(4): 1096−1114 doi: 10.1109/TRO.2020.2974094 [12] Boor V, Overmars M H, Van Der Stappen A F. The gaussian sampling strategy for probabilistic roadmap planners. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Detroit, USA: IEEE, 1999. 1018−1023 [13] Hsu D, Jiang T, Reif J, Sun Z. The bridge test for sampling narrow passages with probabilistic roadmap planners. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Taipei, China: IEEE, 2003. 4420−4426 [14] Xun X, Zhang R, Chai S, Chai R, Xia Y. Light reflection-guided RRT*: efficient path planning in narrow passages. IEEE Robotics and Automation Letters, 2025, 10(11): 11474−11481 doi: 10.1109/LRA.2025.3606385 [15] Ichter B, Harrison J, Pavone M. Learning sampling distributions for robot motion planning. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Brisbane, Australia: IEEE, 2018. 7087−7094 [16] Qureshi A H, Miao Y, Simeonov A, Yip M C. Motion planning networks: bridging the gap between learning-based and classical motion planners. IEEE Transactions on Robotics, 2020, 37(1): 48−66 doi: 10.1109/tro.2020.3006716 [17] Wang Y, Duan X, He J. Learning-based motion planning with mixture density networks. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Yokohama, Japan: IEEE, 2024. 16482−16488 [18] Wang R, Wang S, Ye J, Zhang Y, Hao L. Learning-based motion planning leveraging multivariate deep evidential regression. IEEE Transactions on Robotics, 2025, 41: 5816−5834 doi: 10.1109/TRO.2025.3613568 [19] Li S, Dantam N T. Sample-driven connectivity learning for motion planning in narrow passages. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). London, United Kingdom: IEEE, 2023. 5681−5687 [20] Bohlin R, Kavraki L E. Path planning using lazy PRM. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). San Francisco, USA: IEEE, 2000. 521−528 [21] Koptev M, Figueroa N, Billard A. Neural joint space implicit signed distance functions for reactive robot manipulator control. IEEE Robotics and Automation Letters, 2022, 8(2): 480−487 [22] Liu B, Jiang G, Zhao F, Mei X. Collision-free motion generation based on stochastic optimization and composite signed distance field networks of articulated robot. IEEE Robotics and Automation Letters, 2023, 8(11): 7082−7089 doi: 10.1109/LRA.2023.3311357 [23] Joho D, Schwinn J, Safronov K. Neural implicit swept volume models for fast collision detection. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Yokohama, Japan: IEEE, 2024. 15402−15408 [24] Lee M, Lee J, Lee D. Narrow passage path planning using collision constraint interpolation. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Atlanta, USA: IEEE, 2025. 1−7 [25] Huh J, Lee D D. Learning high-dimensional mixture models for fast collision detection in rapidly-exploring random trees. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Stockholm, Sweden: IEEE, 2016. 63−69 [26] Yu C, Gao S. Reducing collision checking for sampling-based motion planning using graph neural networks. Advances in Neural Information Processing Systems, 2021, 34: 4274−4289 [27] Xu J, He Y, Tian H, Wei Z. A random path sampling-based method for motion planning in many dimensions. IEEE Transactions on Instrumentation and Measurement, 2022, 73: 1−8 [28] Huang S, Wang Z, Li P, Jia B, Liu T, Zhu Y, et al. Diffusion-based generation, optimization, and planning in 3d scenes. In: Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition(CVPR). Vancouver, Canada: IEEE, 2023. 16750−16761 [29] Carvalho J, Le A T, Kicki P, Koert D, Peters J. Motion planning diffusion: learning and adapting robot motion planning with diffusion models. IEEE Transactions on Robotics, 2025, 41: 4881−4901 doi: 10.1109/TRO.2025.3593109 [30] Zhang Q, Liu Z, Fan H, Liu G, Zeng B, Liu S. Flowpolicy: enabling fast and robust 3d flow-based policy via consistency flow matching for robot manipulation. In: Proceedings of the AAAI Conference on Artificial Intelligence. Philadelphia, USA: AAAI Press, 2025. 14754−14762 [31] Das D, Lu Y, Plaku E, Xiao X. Motion memory: leveraging past experiences to accelerate future motion planning. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Yokohama, Japan: IEEE, 2024. 16467−16474 [32] Kuffner J J, LaValle S M. RRT-connect: an efficient approach to single-query path planning. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). San Francisco, USA: IEEE, 2000. 995−1001 [33] Dhillon I S, Modha D S. Concept decompositions for large sparse text data using clustering. Machine Learning, 2001, 42(1): 143−175 doi: 10.1023/a:1007612920971 [34] Hornik K, Feinerer I, Kober M, Buchta C. Spherical k-means clustering. Journal of Statistical Software, 2012, 50: 1−22 [35] Li M, Xu D, Zhang D, Zou J. The seeding algorithms for spherical k-means clustering. Journal of Global Optimization, 2020, 76(4): 695−708 doi: 10.1007/s10898-019-00779-w [36] Chitta S, Sucan I, Cousins S. Moveit![ros topics]. IEEE Robotics & Automation Magazine, 2012, 19(1): 18−19 [37] Sucan I A, Moll M, Kavraki L E. The open motion planning library. IEEE Robotics & Automation Magazine, 2012, 19(4): 72−82 doi: 10.1109/MRA.2012.2205651 [38] Sucan I A, Kavraki L E. A sampling-based tree planner for systems with complex dynamics. IEEE Transactions on Robotics, 2011, 28(1): 116−131 [39] Jaillet L, Cortés J, Siméon T. Sampling-based path planning on configuration-space costmaps. IEEE Transactions on Robotics, 2010, 26(4): 635−646 doi: 10.1109/TRO.2010.2049527 [40] Hsu D, Latombe J C, Motwani R. Path planning in expansive configuration spaces. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Albuquerque, USA: IEEE, 1997. 2719−2726 [41] Zhu X, Xin Y, Li S, Liu H, Xia C, Liang B. Efficient collision detection framework for enhancing collision-free robot motion. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Atlanta, USA, : IEEE, 2025. 16162−16168 [42] Fujii S, Pham Q C. Realtime trajectory smoothing with neural nets. In: Proceedings of IEEE International Conference on Robotics and Automation (ICRA). Philadelphia, USA: IEEE, 2022. 7248−7254 -
下载:
计量
- 文章访问数: 10
- HTML全文浏览量: 8
- 被引次数: 0
