J Shanghai Jiaotong Univ Sci

Journal of Shanghai Jiaotong University(Science) >

2026 , Vol. 31 >Issue 4: 881 - 897

DOI: https://doi.org/10.1007/s12204-025-2813-9

Automation & Computer Technologies

RSG-P: A Fast Global Path Planning Method Based on Route Scene Graph

Expand
  • School of Electronic Information and Electrical Engineering, Shanghai Jiao Tong University, Shanghai 200240, China

Received date: 2024-03-15

  Accepted date: 2024-06-07

  Online published: 2026-07-22

Fold

Abstract

Real-time global path planning for large-scale scenarios is a challenging problem due to the difficulty of maintaining high-definition maps and the high computational cost for real-time planning in changing environment.In this paper, we propose a global path planning framework based on route scene graph(RSG). Based on the hierarchical scene graph framework, we focus on mining spatial connectivity and extracting RSG representing scene road connections from precise obstacle information. The method updates the connection relationships between nodes locally through the minimum spanning tree to make the graph connections sparse. Loop processing ensures the optimality of planning without precise obstacle maps. With the abstract road information in RSG, road pre-planning and multi-level planning can be used to accelerate global path planning. We evaluate the method in both simulation and real environments. Compared to the search-based methods Far Planner(1-10ms), A, DLite(10-100ms), and the random sampling-based methods BIT*, SPARS(10-100ms), the method achieved the sub-millisecond level(0.1-1 ms) planning speed in over 10000m2 scenarios.

Cite this article

Tian Chensheng . RSG-P: A Fast Global Path Planning Method Based on Route Scene Graph[J]. Journal of Shanghai Jiaotong University(Science), 2026 , 31(4) : 881 -897 . DOI: 10.1007/s12204-025-2813-9

References

[1] ARMENI I, HE Z Y, ZAMIR A, et al. 3D scene graph: A structure for unified semantics, 3D space, and camera [C]//2019 IEEE/CVF International Conference on Computer Vision. Seoul: IEEE, 2019: 5663-5672.
[2] ROSINOL A, GUPTA A, ABATE M, et al. 3D dynamic scene graphs: Actionable spatial perception with places, objects, and humans [DB/OL]. (2020-02-15). https://arxiv.org/abs/2002.06289
[3] ROSINOL A, VIOLETTE A, ABATE M, et al. Kimera: : From SLAM to spatial perception with 3D dynamic scene graphs [J]. International Journal of Robotics Research, 2021, 40(12/13/14): 1510-1546.
[4] KIM U H, PARK J M, SONG T J, et al. 3-D scene graph: A sparse and semantic representation of physical environments for intelligent agents [J]. IEEE Transactions on Cybernetics, 2020, 50(12): 4921-4933.
[5] WALD J, DHAMO H, NAVAB N, et al. Learning 3D semantic scene graphs from 3D indoor reconstructions [C]//2020 IEEE/CVF Conference on Computer Vision and Pattern Recognition. Seattle: IEEE, 2020: 3961-3970.
[6] WU S C, WALD J, TATENO K, et al. SceneGraphFusion: Incremental 3D scene graph prediction from RGB-D sequences [C]//2021 IEEE/CVF Conference on Computer Vision and Pattern Recognition. Nashville: IEEE, 2021: 7515-7525.
[7] GOTHOSKAR N, CUSUMANO-TOWNER M, ZINBERG B, et al. 3DP3: 3D scene perception via probabilistic programming [DB/OL]. (2021-10-30). https://arxiv.org/abs/2111.00312
[8] IZATT G, TEDRAKE R. Scene understanding and distribution modeling with mixed-integer scene parsing [EB/OL]. [2024-03-15]. https://groups.csail.mit.edu/robotics-center/public_papers/Izatt21.pdf
[9] HUGHES N. Hydra: A spatial perception engine for constructing and optimizing 3D scene graphs in real-time [D]. Cambridge: Massachusetts Institute of Technology, 2022.
[10] CHANG Y, TIAN Y L, HOW J P, et al. Kimera-multi: A system for distributed multi-robot metric-semantic simultaneous localization and mapping [C]//2021 IEEE International Conference on Robotics and Automation. Xi’an: IEEE, 2021: 11210-11218.
[11] TIAN Y L, CHANG Y, HERRERA ARIAS F, et al. Kimera-multi: Robust, distributed, dense metric-semantic SLAM for multi-robot systems [J]. IEEE Transactions on Robotics, 2022, 38(4): 2022-2038.
[12] CHANG Y, BALLOTTA L, CARLONE L. D-lite: Navigation-oriented compression of 3D scene graphs for multi-robot collaboration [DB/OL]. (2022-09-13). https://arxiv.org/abs/2209.06111
[13] LAVALLE S M, KUFFNER J J. Rapidly-exploring random trees: Progress and prospects [M]//Algorithmic and computational robotics. Boca Raton: A K Peters/CRC Press, 2001: 303-307.
[14] KUFFNER J J, LAVALLE S M. RRT-connect: An efficient approach to single-query path planning [C]// IEEE International Conference on Robotics and Automation. San Francisco: IEEE, 2000: 995-1001.
[15] GAMMELL J D, SRINIVASA S S, BARFOOT T D. Informed RRT*: Optimal sampling-based path planning focused via direct sampling of an admissible ellipsoidal heuristic [C]//2014 IEEE/RSJ International Conference on Intelligent Robots and Systems. Chicago: IEEE, 2014: 2997-3004.
[16] GAMMELL J D, SRINIVASA S S, BARFOOT T D. Batch informed trees (BIT*): Sampling-based optimal planning via the heuristically guided search of implicit random geometric graphs [C]//2015 IEEE International Conference on Robotics and Automation. Seattle: IEEE, 2015: 3067-3074.
[17] KAVRAKI L E, SVESTKA P, LATOMBE J C, et al. Probabilistic roadmaps for path planning in high-dimensional configuration spaces [J]. IEEE Transactions on Robotics and Automation, 1996, 12(4): 566-580.
[18] BOHLIN R, KAVRAKI L E. Path planning using lazy PRM [C]// IEEE International Conference on Robotics and Automation. San Francisco: IEEE, 2000: 521-528.
[19] DOBSON A, BEKRIS K E. Improving sparse roadmap spanners [C]//2013 IEEE International Conference on Robotics and Automation. Karlsruhe: IEEE, 2013: 4106-4111.
[20] ZUCKER M, RATLIFF N, DRAGAN A D, et al. CHOMP: Covariant Hamiltonian optimization for motion planning [J]. International Journal of Robotics Research, 2013, 32(9/10): 1164-1193.
[21] KALAKRISHNAN M, CHITTA S, THEODOROU E, et al. STOMP: Stochastic trajectory optimization for motion planning [C]//2011 IEEE International Conference on Robotics and Automation. Shanghai: IEEE, 2011: 4569-4574.
[22] DOLGOV D, THRUN S, MONTEMERLO M, et al. Path planning for autonomous vehicles in unknown semi-structured environments [J]. International Journal of Robotics Research, 2010, 29(5): 485-501.
[23] DOLGOV D, THRUN S, MONTEMERLO M, et al. Practical search techniques in path planning for autonomous driving [C]// AAAI Workshop - Technical Report. Ann Arbor: AAAI, 2008: 32-37.
[24] RÖSMANN C, HOFFMANN F, BERTRAM T. Integrated online trajectory planning and optimization in distinctive topologies [J]. Robotics and Autonomous Systems, 2017, 88: 142-153.
[25] ROESMANN C, FEITEN W, WOESCH T, et al. Trajectory modification considering dynamic constraints of autonomous robots [C]//ROBOTIK 2012; 7th German Conference on Robotics. Munich: VDE, 2012: 1-6.
[26] KAHN G, ABBEEL P, LEVINE S. BADGR: An autonomous self-supervised learning-based navigation system [J]. IEEE Robotics and Automation Letters, 2021, 6(2): 1312-1319.
[27] CHEHELGAMI S, ASHTARI E, BASIRI M A, et al. Safe deep learning-based global path planning using a fast collision-free path generator [J]. Robotics and Autonomous Systems, 2023, 163: 104384.
[28] RICHTER C, VEGA-BROWN W, ROY N. Bayesian learning for safe high-speed navigation in unknown environments [M]//Robotics research. Cham: Springer, 2017: 325-341.
[29] GUO X, FANG Y C. Learning to navigate in unknown environments based on GMRP-N [C]//2019 IEEE 9th Annual International Conference on CYBER Technology in Automation, Control, and Intelligent Systems. Suzhou: IEEE, 2019: 1453-1458.
[30] TAKAHASHI T, SUN H, TIAN D, et al. Learning heuristic functions for mobile robot path planning using deep neural networks [J]. Proceedings of the International Conference on Automated Planning and Scheduling, 2019, 29: 764-772.
[31] MA N C, WANG J K, LIU J B, et al. Conditional generative adversarial networks for optimal path planning [J]. IEEE Transactions on Cognitive and Developmental Systems, 2022, 14(2): 662-671.
[32] LI J S, WANG S C, CHEN Z Y, et al. Lightweight neural path planning [C]//2023 IEEE/RSJ International Conference on Intelligent Robots and Systems. Detroit: IEEE, 2023: 6713-6718.
[33] DIJKSTRA E W. A note on two problems in connexion with graphs [J]. Numerische Mathematik, 1959, 1(1): 269-271.
[34] HART P E, NILSSON N J, RAPHAEL B. A formal basis for the heuristic determination of minimum cost paths [J]. IEEE Transactions on Systems Science and Cybernetics, 1968, 4(2): 100-107.
[35] STENTZ A. Optimal and efficient path planning for partially-known environments [C]// 1994 IEEE International Conference on Robotics and Automation. San Diego: IEEE, 1994: 3310-3317.
[36] KOENIG S, LIKHACHEV M. Fast replanning for navigation in unknown terrain [J]. IEEE Transactions on Robotics, 2005, 21(3): 354-363.
[37] YANG F, CAO C, ZHU H B, et al. FAR planner: Fast, attemptable route planner using dynamic visibility update [C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems. Kyoto: IEEE, 2022: 9-16.
[38] BLOCHLIGER F, FEHR M, DYMCZYK M, et al. Topomap: Topological mapping and navigation based on visual SLAM maps [C]//2018 IEEE International Conference on Robotics and Automation. Brisbane: IEEE, 2018: 3818-3825.
[39] CHEN X Y, ZHOU B Y, LIN J R, et al. Fast 3D sparse topological skeleton graph generation for mobile robot global planning [C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems. Kyoto: IEEE, 2022: 10283-10289.
[40] FANKHAUSER P, BLOESCH M, GEHRING C, et al. Robot-centric elevation mapping with uncertainty estimates [C]//Mobile Service Robotics. Poznan: World Scientific, 2014: 433-440.
[41] FANKHAUSER P, BLOESCH M, HUTTER M. Probabilistic terrain mapping for mobile robots with uncertain localization [J]. IEEE Robotics and Automation Letters, 2018, 3(4): 3019-3026.
[42] BLOESCH M, SOMMER H, LAIDLOW T, et al. A primer on the differential calculus of 3D orientations [DB/OL]. (2016-06-16). https://arxiv.org/abs/1606.05285
[43] KIM G, KIM A. Scan context: Egocentric spatial descriptor for place recognition within 3D point cloud map [C]//2018 IEEE/RSJ International Conference on Intelligent Robots and Systems. Madrid: IEEE, 2018: 4802-4809.
[44] TARJAN R. Depth-first search and linear graph algorithms [C]//12th Annual Symposium on Switching and Automata Theory. East Lansing: IEEE, 1971: 114-121.
[45] ZHANG J, HU C, CHADHA R G, et al. Falco: Fast likelihood-based collision avoidance with extension to human-guided navigation [J]. Journal of Field Robotics, 2020, 37(8): 1300-1313.
[46] PENG B, XIE H L, CHEN W D. ROLL: Long-term robust LiDAR-based localization with temporary mapping in changing environments [C]//2022 IEEE/RSJ International Conference on Intelligent Robots and Systems. Kyoto: IEEE, 2022: 2841-2847.
Options
Outlines

/

〈 〉