Adaptive Cost-Map-based Path Planning in Partially Unknown Environments with Movable Obstacles

Technical Summary: Adaptive Cost-Map-based Path Planning in Unknown Environments with Movable Obstacles

Problem Statement
Reliable autonomous navigation in disaster-response scenarios (e.g., collapsed buildings, smoke-filled corridors) requires robots to operate in unstructured environments where obstacles may be movable. Traditional planners often assume static obstacles or rely on rich RGB-D perception and semantic maps to classify objects. However, these vision-heavy approaches are fragile in disaster zones due to dust, smoke, and poor lighting, and they often exceed the computational budgets of resource-constrained Search and Rescue (SAR) robots. The core challenge addressed is enabling a robot to determine, in real-time and without vision or prior semantic knowledge, whether an unknown obstacle can be pushed aside to reach a goal, and to adjust its navigation plan accordingly within a standard ROS 2 stack.

Methodology
The authors propose a LiDAR-only, adaptive cost-map framework integrated into the ROS 2 Nav2 stack. The system avoids pre-training and complex vision pipelines, relying instead on planar scans and CPU-level computation. The architecture consists of two primary custom plugins:

1. Movable Obstacles Layer: This layer compares incoming LiDAR scans against a pre-existing static SLAM map. Returns that do not coincide with known static structures are initially classified as "movable" and assigned a reduced traversal cost ("light"). The system uses connected-component labeling to maintain obstacle identity even during partial occlusion. A distance-to-wall filter prevents false positives by keeping obstacles adjacent to static walls as lethal. The layer supports weighted soft inflation, allowing the robot to prefer avoidance but attempt pushing when no collision-free route exists.
2. Slow-Pose Progress Checker: This module monitors the ratio between commanded velocity and actual odometry-derived velocity. It distinguishes between intentional slow maneuvers (e.g., turning) and genuine obstruction.
   • Cost Escalation: If the robot slows significantly while pushing, the cost of the nearest obstacle is escalated from "light" to "heavy." If the robot stalls completely, the cost is escalated to "lethal."
   • Re-planning: These cost updates trigger the global planner (NavFn) to back out and re-route.
   • Execution: The local planner utilizes the Model Predictive Path Integral (MPPI) controller, selected for its ability to perform complex maneuvers, including backward motion, to reposition the robot for optimal obstacle interaction.

Key Contributions

• ROS 2 Native NAMO Formulation: A Navigation Among Movable Obstacles (NAMO) approach that eliminates the need for vision or semantic prerequisites, designed for direct integration with the Nav2 stack.
• Adaptive Costmap Plugin: A "Movable Obstacles Layer" that tags LiDAR-detected unknown objects with adaptive traversal costs, updating them dynamically based on interaction feedback.
• Velocity-Based Feedback Loop: A custom "Slow-Pose Progress Checker" that sends real-time "heavy" or "lethal" signals to the costmap layer based on motion performance, enabling a graded response to obstacle resistance without premature abandonment of pushable objects.

Experimental Results
The system was evaluated in Gazebo simulations using a Scout Mini robot across three scenarios: isolated obstacles, cluttered corridors with mixed movability, and complex double-row obstacles.

• Success Rates: The adaptive system achieved significantly higher goal-reach rates compared to a baseline static obstacle planner, which failed entirely in scenarios requiring obstacle interaction (e.g., 100% success in corridor scenarios vs. "impossible" for the baseline).
• Efficiency: Traversal times were generally improved or comparable to the baseline. For instance, in Scenario 1-a, the adaptive system was 33% faster (27.79s vs. 41.71s). In cases involving immovable obstacles (1-c), the system accepted longer traversal times to avoid futile pushing attempts.
• Accuracy: Movability estimation was highly accurate for light (100%) and immovable (95%) objects. Accuracy dropped for "heavy" obstacles (60–70%), highlighting the difficulty in distinguishing pushable-but-difficult objects under varying approach conditions.
• Limitations Observed: The study noted challenges with clustered obstacles near walls causing ambiguous classification due to LiDAR occlusion, and occasional localization drift leading to costmap misalignments. The velocity-based checker sometimes struggled to distinguish between legitimate low-speed maneuvering and pushing failure.

Significance and Claims
The paper claims that interaction-aware cost maps provide a lightweight, ROS 2-native extension for navigating unstructured settings. The significance lies in its ability to enable SAR robots to navigate among potentially movable obstacles without relying on fragile vision pipelines or GPU-intensive inference. By relying solely on planar scans and standard navigation stacks, the method is suitable for resource-constrained platforms and can be integrated into heterogeneous systems with minimal engineering overhead. The authors position this as a practical step toward robust disaster response robotics, acknowledging that while the method is not perfect for all obstacle types (particularly heavy ones), it offers a robust, low-compute alternative to vision-dependent NAMO solutions. The full implementation is released as open source.