Adaptive Cost-Map-based Path Planning in Partially Unknown Environments with Movable Obstacles
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 ROS 2 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.
Methodology
Cost-map
Our Movable Obstacles Layer is integrated into ROS 2 Nav2’s layered costmap architecture, sitting above the base obstacle layer. It augments the standard static, obstacle, and inflation layers with adaptive costs derived from LiDAR data, enabling dynamic adjustment without altering the underlying Nav2 stack.
Movable Obstacles Layer
This diagram shows how LiDAR returns are compared with a static SLAM map to detect previously unseen obstacles. New detections are labeled as movable with reduced cost values and stored in memory so their identity persists when displaced. The layer updates these costs in real time based on interaction feedback from the robot.
Slow-Pose Progress Checker
The Slow-Pose Progress Checker monitors the ratio between commanded and actual velocity using odometry data. When the robot slows or stalls while pushing an object, it publishes messages that elevate nearby obstacle costs from light to heavy or lethal, prompting replanning and recovery behaviors.