A Hamilton-Jacobi Reachability-Guided Search Framework for Efficient and Safe Indoor Planar Robot Navigation

1. Problem Statement

The paper addresses the challenge of autonomous indoor planar robot navigation in environments containing both static obstacles and moving agents (humans). The core problem involves generating trajectories that are simultaneously:

• Computationally Efficient: Capable of real-time planning despite the high dimensionality of the state space (position, velocity, heading).
• Safe: Guaranteeing collision avoidance not just reactively, but proactively, accounting for system dynamics and worst-case disturbances.
• Adaptable: Handling changing goals and partially unknown dynamic environments without requiring full replanning from scratch.

Existing methods face trade-offs:

• Graph Search (e.g., A, D):** Efficient but often rely on geometric heuristics that ignore dynamics, leading to infeasible paths or local minima in non-holonomic systems.
• Hamilton-Jacobi (HJ) Reachability: Provides rigorous safety guarantees and dynamic feasibility but is computationally expensive to solve online and struggles with changing goals or unknown environments.

2. Methodology

The authors propose a hybrid framework that decouples the computational burden by performing heavy HJ reachability analysis offline and integrating the results into an online graph search process.

A. Offline Precomputation

Three specific HJ value functions are precomputed and stored for online lookup:

1. General Time-to-Reach (TTR) Function ($\Phi$):
   • Computed in an obstacle-free space with the goal at the origin.
   • Serves as a dynamics-aware heuristic. Unlike Euclidean distance, it accounts for the robot's kinematic constraints (e.g., turning radius, acceleration limits), providing a more accurate estimate of the cost-to-go.
2. Static-Map Backward Reachable Tube (BRT) ($V_{map}$):
   • Represents the set of states from which the robot is inevitably driven into a static obstacle within a time horizon, assuming no control intervention.
   • Used as a predictive pruning mechanism to eliminate nodes that would lead to collisions with static obstacles, even if the robot is not currently colliding.
3. Human-Robot BRT ($V_{rel}$):
   • Computed in a relative state space (robot-centric) using a pursuit-evasion differential game formulation.
   • Models the worst-case interaction between the robot and a human agent.
   • Used to prune nodes that would lead to collisions with moving humans.

B. Online Navigation Framework

The online planner uses an A or Anytime Nonparametric A (ANA*)** algorithm enhanced with the precomputed functions:

• Heuristic Guidance: The general TTR function replaces standard distance-based heuristics. It guides the search toward the goal while respecting non-holonomic constraints, significantly reducing the number of expanded nodes.
• Predictive Pruning:
  • Static Pruning: Nodes falling within the Static-Map BRT are removed before expansion.
  • Dynamic Pruning (Forward-Invariant): For human interactions, the framework employs a Forward-Invariant (FI) pruning strategy. Instead of simply checking if a state is currently unsafe, it checks the rate of change of the safety value function ($\frac{\partial V}{\partial t}$). If the safety margin is decreasing too rapidly, the node is pruned. This ensures the robot avoids trajectories that will become unsafe in the near future, even if they are currently safe.
• Adaptive Safety: An adaptive mechanism adjusts the conservativeness of the human-robot BRT based on the relative distance and angular alignment between the robot and the human, balancing safety with navigational efficiency.

3. Key Contributions

1. Integrated Framework: A novel architecture that combines offline HJ reachability with online graph search, leveraging the theoretical guarantees of HJ while maintaining the flexibility and speed of graph search.
2. Unified Integration Strategy:
   • Use of TTR for heuristic acceleration (improving search efficiency).
   • Use of BRTs for predictive pruning of both static and dynamic hazards (improving safety).
3. Forward-Invariant Pruning: Introduction of a dynamic pruning method for human-robot interaction that monitors the derivative of the value function, enabling proactive collision avoidance rather than reactive stopping.
4. Empirical Validation: Extensive validation through high-fidelity simulations and real-world experiments on a Quanser QBot platform in human-populated environments.

4. Results

The proposed method was evaluated against baselines using Euclidean distance heuristics (Dist) and reactive obstacle pruning (OBS).

• Search Efficiency (Simulation):
  • TTR Heuristic: Reduced node expansions by ~95% compared to distance-based heuristics in A* (from $1.29 \times 10^7$ to $6.48 \times 10^5$ nodes) and significantly improved success rates in ANA* for complex orientation tasks.
  • BRT Pruning: While adding computational overhead per node, it reduced total node expansions by 15% in static environments and was critical for solving tasks where reactive pruning failed.
• Safety and Robustness:
  • Static Environments: The TTR + BRT combination achieved a 100% success rate in dense, cluttered rooms where baselines failed (0% success).
  • Dynamic Environments: In human-populated scenarios, the FI-BRT method achieved 0 collisions in all tests, whereas reactive baselines (OBS) suffered collisions.
  • Trade-off: The FI-BRT method was more conservative (longer travel times) than finite-horizon methods but provided superior safety guarantees, successfully navigating head-on encounters where reactive methods failed.
• Real-World Performance:
  • The framework successfully navigated a physical robot through static and dynamic scenarios, demonstrating generalization beyond simulation.
  • In dynamic tests with moving humans, the method maintained zero collisions while completing patrol tasks, whereas reactive baselines collided in 100% of multi-human trials.

5. Significance

This work bridges a critical gap between optimal control theory (HJ reachability) and discrete planning (graph search).

• Scalability: By moving the heavy computation of solving Partial Differential Equations (PDEs) offline, the framework makes rigorous safety guarantees feasible for real-time, online planning.
• Safety-Critical Navigation: It moves beyond reactive collision avoidance to predictive safety, ensuring that robots do not enter states that are dynamically infeasible to recover from, even in the presence of adversarial or unpredictable human behavior.
• Practical Applicability: The successful real-world deployment demonstrates that complex theoretical control methods can be effectively integrated into practical robotic systems for safe navigation in human-centric environments.