Interactive-FAR:Interactive, Fast and Adaptable Routing for Navigation Among Movable Obstacles in Complex Unknown Environments

TL;DR

InteractiveFAR tackles real-time navigation in unknown, cluttered environments with movable obstacles by integrating online mapping and manipulation-aware planning. It introduces a dynamic Directed Visibility Graph $\mathcal{G}$ that encodes traversability and manipulation strategies, coupled with kinodynamic interaction planning and online affordance updates. The system supports milliseconds-scale global re-planning and obstacle manipulation, updating the graph as new sensor data arrive. Experiments show travel-time reductions up to 33% and path-efficiency gains up to 49%, with speed advantages over baselines, and the authors release the code in a docker-based demo.

Abstract

This paper introduces a real-time algorithm for navigating complex unknown environments cluttered with movable obstacles. Our algorithm achieves fast, adaptable routing by actively attempting to manipulate obstacles during path planning and adjusting the global plan from sensor feedback. The main contributions include an improved dynamic Directed Visibility Graph (DV-graph) for rapid global path searching, a real-time interaction planning method that adapts online from new sensory perceptions, and a comprehensive framework designed for interactive navigation in complex unknown or partially known environments. Our algorithm is capable of replanning the global path in several milliseconds. It can also attempt to move obstacles, update their affordances, and adapt strategies accordingly. Extensive experiments validate that our algorithm reduces the travel time by 33%, achieves up to 49% higher path efficiency, and runs faster than traditional methods by orders of magnitude in complex environments. It has been demonstrated to be the most efficient solution in terms of speed and efficiency for interactive navigation in environments of such complexity. We also open-source our code in the docker demo to facilitate future research.

Figures (8)

• Figure 1: Illustration of the interactive navigation through an unknown environment. The colorful curve is the vehicle's trajectory. The obstacles are extracted as polygons in red, and the movable objects (orange pixels) are registered as green polygons. The cyan dots represent topological waypoints. The Directed Visibility Graph is marked as cyan and yellow lines. (a) Overview. (b-c) The robot can manipulate the movable obstacles during the navigation and can switch the contact points during manipulation. (d) The robot attempts to move a heavy object ①. When the robot contacts the object, the cost of moving it is updated from force feedback. If the object is affordable, the robot pushes it, otherwise it replans the alternate strategy to execute. The robot can quickly adapt to new sensory observations no matter whether a movable object is affordable or not.
• Figure 2: A diagram of our system architecture.
• Figure 3: (a-b) Path-disconnected components $\{\mathcal{C}^i_{local}\}$ of a local region $\{ \mathcal{P}^i_{local}\}$. Green polygon represents $\mathcal{P}_{mov}$ and grey ones are $\mathcal{P}_{bg}$. They are inflated based on the robot dimension. Topological waypoints $\{\mathbf{p}_{topo}\}$ are marked as purple, potential manipulation strategies are represented as directed edges in purple. (c) Demonstration of the process to choose $\{\mathbf{p}^i_{topo}\}$. Intersected points $\mathbf{p}_{inter}$ are marked in cyan. (d) Illustration of the interaction planning module. Contact point are marked as blue.
• Figure 4: Illustration of the problem decomposition. (a) The main problem contains $n(n-1)$ sub-problems. (b-d) Three typical sub-problems. The upper-right corner of (b) explains the calculation of the heuristics. The left-upper corner of (d) demonstrates the case when there are multiple heuristic polygons.
• Figure 5: Hybrid A* push search with contact point switch.

Theorems & Definitions (3)

• Theorem 1
• Lemma 1
• Theorem 2