Perceptive Hierarchical-Task MPC for Sequential Mobile Manipulation in Unstructured Semi-Static Environments

TL;DR

This work proposes a novel perceptive hierarchical-task model predictive control framework for efficient sequential mobile manipulation in unstructured, changing environments and leverages a Bayesian inference framework to explicitly model object-level changes and thereby maintain a temporally accurate representation of the 3D environment.

Abstract

As compared to typical mobile manipulation tasks, sequential mobile manipulation poses a unique challenge -- as the robot operates over extended periods, successful task completion is not solely dependent on consistent motion generation but also on the robot's awareness and adaptivity to changes in the operating environment. While existing motion planners can generate whole-body trajectories to complete sequential tasks, they typically assume that the environment remains static and rely on precomputed maps. This assumption often breaks down during long-term operations, where semi-static changes such as object removal, introduction, or shifts are common. In this work, we propose a novel perceptive hierarchical-task model predictive control (HTMPC) framework for efficient sequential mobile manipulation in unstructured, changing environments. To tackle the challenge, we leverage a Bayesian inference framework to explicitly model object-level changes and thereby maintain a temporally accurate representation of the 3D environment; this up-to-date representation is embedded in a lexicographic optimization framework to enable efficient execution of sequential tasks. We validate our perceptive HTMPC approach through both simulated and real-robot experiments. In contrast to baseline methods, our approach systematically accounts for moved and phantom obstacles, successfully completing sequential tasks with higher efficiency and reactivity, without relying on prior maps or external infrastructure.

Figures (11)

• Figure 1: Demonstration of our proposed closed-loop perceptive HTMPC control system performing sequential mobile manipulation tasks in semi-static environments. The left and right panels highlight the robot’s response to changed boxes between visits, along with a snapshot of its updated 3D map, illustrating our system's capability to handle semi-static changes in the scene via online perception and motion planning. A video of the demo is available at http://tiny.cc/peception-htmpc.
• Figure 2: Proposed perceptive HTMPC framework for sequential mobile manipulation tasks in unstructured semi-static environments. Key components are highlighted. The Perceptive HTMPC closes the perception–control loop by using the robot state and map produced by the perception and mapping module, enabling safe and reactive behavior in semi-static environments.
• Figure 3: Illustration of the whole-body navigation task in semi-static environments. The robot is tasked to make a round trip between two points. Before the robot returns, obstacles are rearranged outside the robot's FoV (white). Experiments are performed both in simulation and on the real robot.
• Figure 4: Comparison of the proposed mapping with change detection (right) against a baseline that assumes a static environment (left)Rosinol20icra-Kimera. Our method produces shorter paths after scene change (phase 2) by maintaining an artifact-free map without the phantom obstacles presented in the baseline.
• Figure 5: Time evolution of the object consistency for the three labeled objects in \ref{['fig:MS-WBN']} tracked by the proposed perceptive-HTMPC framework. After scene change (red dashed line), in Phase 2, the proposed mapping system detected the relocated objects and removed them (red cross) from the map when their consistency expectation dropped below the set threshold.