Situationally-aware Path Planning Exploiting 3D Scene Graphs

1. Problem Statement

Traditional sampling-based path planners (e.g., RRT*, PRM*, BIT*) for indoor robots rely on dense sampling of the configuration space (C-space). While effective, these methods suffer from:

• Computational Inefficiency: As environment size and complexity increase, the search space grows rapidly, leading to high planning times.
• Lack of Semantic Understanding: They treat the environment as a geometric void, ignoring high-level semantic structures (e.g., rooms, doorways, floors) that humans use for intuitive navigation.
• Poor Scalability in Complex Environments: They struggle in environments with narrow passageways and limited lines of sight, often failing to find feasible paths within time constraints.
• Inefficient Replanning: When a path becomes invalid (e.g., a door closes), traditional planners often recompute the entire path from scratch rather than reusing valid segments.

Existing approaches using semantic information often rely on non-hierarchical representations or focus solely on task feasibility rather than optimizing geometric path planning efficiency.

2. Methodology: S-Path Architecture

The proposed S-Path planner leverages the hierarchical structure of 3D Scene Graphs to decompose global planning into manageable subproblems. The system operates in a two-stage process:

A. Input Data & Environment Setup

• Inputs: A metric-semantic 3D scene graph (containing nodes for walls, rooms, and doorways with geometric properties) and a metric mesh (ESDF) of the environment.
• Contour Generation: The system projects wall planes to generate 2.5D "contours" for rooms and doorways. These contours define the valid geometric regions associated with specific semantic nodes.
• Semantic Graph Construction: A weighted graph is created where nodes are rooms/doorways and edge weights represent Euclidean distances between centroids. Non-traversable doorways are assigned infinite weights.

B. Two-Stage Planning Process

1. Semantic Planning (High-Level):

   • Given a start ($p_s$) and goal ($p_g$), the system identifies the associated rooms ($R_s, R_g$).
   • An A search* is performed on the semantic graph to find the shortest sequence of rooms and doorways ($\Pi_{sem}$).
   • This generates a coarse geometric path ($\Pi_{geo}$) consisting of waypoints at the centers of the identified doorways.
   • Constraint: The global C-space is restricted to the union of contours ($C'$) of only the rooms and doorways in the semantic path, drastically reducing the sampling space.

2. Geometric Planning (Low-Level):

   • Subproblem Decomposition: The coarse path is broken into independent subproblems (e.g., Start $\to$ Doorway 1, Doorway 1 $\to$ Doorway 2, etc.).
   • Parallel Execution: These subproblems are solved simultaneously using a thread pool and sampling-based planners (OMPL library).
   • Resource Allocation: Time budgets for each subproblem are allocated based on an "effort heuristic" (correlated with the area of the contour and distance), ensuring complex subproblems get more time.
   • Stitching: Local solutions are stitched together at the doorway centers to form the final global path.

C. Replanning Mechanism

• Dynamic Updates: If the scene graph changes (e.g., a door closes) or a path becomes infeasible, S-Path triggers a replanning routine.
• Efficient Reuse: Instead of recomputing everything, the system:
  1. Updates the semantic graph (assigning infinite weights to blocked edges).
  2. Re-runs semantic planning.
  3. Caches and Reuses: Retrieves previously solved geometric subproblems from memory. Only the affected portions of the path are re-planned.
  4. Updates edge weights to encourage the reuse of previously successful paths.

3. Key Contributions

1. Hierarchical Decomposition: Exploits the topology of 3D scene graphs to constrain geometric planning to only relevant semantic regions, significantly reducing the C-space.
2. Parallel Subproblem Solving: Decomposes the global planning problem into independent, smaller subproblems that can be solved in parallel, drastically reducing planning latency.
3. Resource-Efficient Replanning: Introduces a mechanism to isolate failures and reuse cached subproblem solutions, minimizing the cost of replanning in dynamic environments.
4. Human-Interpretable Output: Generates paths defined by semantic concepts (e.g., "Go from Room A to Room B via Doorway C"), facilitating human-robot interaction.
5. ROS Framework: Implementation as a ROS-based framework utilizing OMPL, providing a lightweight interface for integrating 3D scene graph semantics.

4. Experimental Results

The authors evaluated S-Path in four environments (two real-world construction sites/buildings and two synthetic large-scale environments) using BIT*, PRM*, and RRT* as baseline planners.

• Planning Efficiency:
  • S-Path achieved an average 6$\times$ reduction in planning time compared to classical sampling-based planners.
  • In specific pairings (e.g., with PRM*), efficiency gains reached up to 25$\times$.
  • S-Path successfully found paths in scenarios where baseline planners failed to converge within the time budget.
• Path Optimality:
  • Path lengths were comparable to baselines, with a slight average increase of 2.5% (due to the constraint of passing through doorway centers). This trade-off is considered acceptable given the massive time savings.
• Replanning Performance:
  • In dynamic scenarios with blockages, S-Path demonstrated up to 52$\times$ efficiency improvement over baselines by reusing cached subproblems.
  • It consistently produced shorter paths than baselines in replanning scenarios (avg. 14.9% shorter) because it could quickly navigate around obstacles using the semantic structure.
• Parallel Speedup:
  • Parallel execution showed speedups correlating with the number of subproblems. Synthetic environments showed higher speedups due to fewer irregular obstacles.
  • Semantic planning overhead was negligible (<1 ms).

5. Significance and Limitations

Significance:
S-Path bridges the gap between high-level semantic reasoning and low-level geometric motion planning. It transforms path planning from a purely geometric search into a structured, interpretable, and highly efficient process. This is particularly crucial for large-scale indoor robotics (e.g., warehouse, hospital, or office navigation) where real-time responsiveness and human interpretability are critical.

Limitations:

• Doorway Constraint: The requirement to pass through the exact center of doorways can lead to slightly suboptimal paths, especially in wide doorways.
• Geometry Assumptions: The contour generation assumes rooms are bounded by planar walls. Complex curved architectures or concave rooms require preprocessing (e.g., splitting into convex polygons).
• Map Dependency: The system assumes an accurate initial map and does not inherently handle sensor uncertainties or map errors during execution.

Future Work:
The authors plan to refine path optimality via local-to-global optimization, extend contour generation for complex curved geometries, and incorporate robustness against environmental uncertainties.