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

TL;DR

Reliable autonomous navigation in disaster-affected indoor spaces is challenged by unknown movable obstacles and limited perception. The paper presents a LiDAR-only adaptive cost-map framework integrated into ROS2 Nav2, featuring a Movable Obstacles Layer to label unknown returns and a Slow-Pose Progress Checker to escalate costs based on motion feedback. Key contributions include persistent obstacle labeling with graded costs and a velocity-driven replanning loop that enables interaction with obstacles without vision. In Gazebo simulations, the approach yields higher goal-reach rates and fewer deadlocks compared with static baselines, with traversal times remaining comparable and movability cues effective for light/immovable objects. The method is lightweight, vision-free, ROS2-native, and well-suited for deployment on resource-constrained SAR robots, with an open-source release planned.

Abstract

Reliable navigation in disaster-response and other unstructured indoor settings requires robots not only to avoid obstacles but also to recognise when those obstacles can be pushed aside. We present an adaptive, LiDAR and odometry-based path-planning framework that embeds this capability into the ROS2 Nav2 stack. A new Movable Obstacles Layer labels all LiDAR returns missing from a prior static map as tentatively movable and assigns a reduced traversal cost. A companion Slow-Pose Progress Checker monitors the ratio of commanded to actual velocity; when the robot slows appreciably, the local cost is raised from light to heavy, and on a stall to lethal, prompting the global planner to back out and re-route. Gazebo evaluations on a Scout Mini, spanning isolated objects and cluttered corridors, show higher goal-reach rates and fewer deadlocks than a no-layer baseline, with traversal times broadly comparable. Because the method relies only on planar scans and CPU-level computation, it suits resource-constrained search and rescue robots and integrates into heterogeneous platforms with minimal engineering. Overall, the results indicate that interaction-aware cost maps are a lightweight, ROS2-native extension for navigating among potentially movable obstacles in unstructured settings. The full implementation will be released as open source athttps://costmap-namo.github.io.

Figures (7)

• Figure 1: Scout Mini in a corridor world. The robot pushes an obstacle, detects reduced speed, and labels it as heavy.
• Figure 2: System architecture of the proposed adaptive cost-map navigation framework. Sensor data are processed through cost-map layers, augmented by obstacle movability assessment and cost escalation, before global and local planning with MPPI control.
• Figure 3: Detailed flowchart of the adaptive cost-map pipeline and Rviz2 Cost-Map color scheme. The Movable Obstacles Layer compares LiDAR scans to the static SLAM map to classify movable obstacles and update cost-map values, while the Slow-Pose Progress Checker monitors velocity ratios to escalate obstacle costs and trigger recovery behaviors.
• Figure 4: The maps used as the respective 3 scenarios for the experimental evaluation.
• Figure 5: Scenario 1-b (heavy obstacle): annotated costmap sequence showing the initial plan, insertion of an unmapped return as “movable,” slowdown-triggered escalation to “heavy,” and the resulting re-route to the goal.