SM$^2$ITH: Safe Mobile Manipulation with Interactive Human Prediction Via Task-Hierarchical Bilevel Model Predictive Control
Francesco D'Orazio, Sepehr Samavi, Xintong Du, Siqi Zhou, Giuseppe Oriolo, Angela P. Schoellig
AI summary
Problem
Mobile manipulators struggle to safely navigate dynamic human crowds while executing multiple prioritized tasks, as existing methods either ignore human-robot reciprocity or rely on rigid, hard-to-tune weighted objectives.
Approach
SM2ITH unifies hierarchical task MPC with interactive human prediction by embedding human ORCA constraints and control barrier functions into a bilevel optimization framework, jointly planning robot commands and predicting human responses in real time.
Key results
- First whole-body control framework coupling human predictions with robot actions
- Outperforms weighted-sum and open-loop baselines in prioritization and efficiency
- Validated across 140 real-world runs on two mobile manipulator platforms
- Enforces strict lexicographic task priorities without manual weight tuning
Why it matters
Provides a scalable, safety-guaranteed control framework for deploying mobile manipulators in crowded, human-centered service environments.
Abstract
Mobile manipulators are designed to perform complex sequences of navigation and manipulation tasks in human-centered environments. While recent optimization-based methods such as Hierarchical Task Model Predictive Control (HTMPC) enable efficient multitask execution with strict task priorities, they have so far been applied mainly to static or structured scenarios. Extending these approaches to dynamic human-centered environments requires predictive models that capture how humans react to the actions of the robot. This work introduces Safe Mobile Manipulation with Interactive Human Prediction via Task-Hierarchical Bilevel Model Pre- dictive Control (SM2ITH), a unified framework that combines HTMPC with interactive human motion prediction through bilevel optimization that jointly accounts for robot and human dynamics. The framework is validated on two different mobile manipulators, the Stretch 3 and the Ridgeback–UR10, across three experimental settings: (i) delivery tasks with different navigation and manipulation priorities, (ii) sequential pick-and- place tasks with different human motion prediction models, and (iii) interactions involving adversarial human behavior. Our results highlight how interactive prediction enables safe and efficient coordination, outperforming baselines that rely on weighted objectives or open-loop human models. Code: https://github.com/utiasDSL/sm2ith.git