Situationally-aware Path Planning Exploiting 3D Scene Graphs

TL;DR

This paper introduces S-Path, a novel path planning method that exploits 3D Scene Graphs to improve efficiency and interpretability in indoor environments, achieving significant speedups and robustness.

Contribution

S-Path is a new two-stage, situationally-aware path planner that leverages semantic scene graphs for faster, more interpretable path planning with a replan mechanism for infeasible paths.

Findings

S-Path reduces planning time by an average of 6x.

It maintains path optimality comparable to classical methods.

It outperforms classical planners in complex scenarios.

Abstract

3D Scene Graphs integrate both metric and semantic information, yet their structure remains underutilized for improving path planning efficiency and interpretability. In this work, we present S-Path, a situationally-aware path planner that leverages the metric-semantic structure of indoor 3D Scene Graphs to significantly enhance planning efficiency. S-Path follows a two-stage process: it first performs a search over a semantic graph derived from the scene graph to yield a human-understandable high-level path. This also identifies relevant regions for planning, which later allows the decomposition of the problem into smaller, independent subproblems that can be solved in parallel. We also introduce a replanning mechanism that, in the event of an infeasible path, reuses information from previously solved subproblems to update semantic heuristics and prioritize reuse to further improve the efficiency of future planning attempts. Extensive experiments on both real-world and simulated environments show that S-Path achieves average reductions of 6x in planning time while maintaining comparable path optimality to classical sampling-based planners and surpassing them in complex scenarios, making it an efficient and interpretable path planner for environments represented by indoor 3D Scene Graphs. Code available at: https://github.com/snt-arg/spath_ros