A POMDP-based hierarchical planning framework for manipulation under pose uncertainty

TL;DR

The paper tackles manipulation under large pose uncertainty in unstructured environments by using binary contact signals within a POMDP. It introduces a hierarchical belief representation that first reasons in a 3D volumetric space to shrink the potentially occupied region, then transitions to a refined particle-space view for final pose estimation, all solved by a closed-loop, anytime planner with a 1-second budget. Key contributions include the two-phase hierarchical belief framework, a modified RTDP-Bel based solver that focuses on the most probable outcomes, and the demonstration of real-time, high-precision plug insertion with substantial improvements over greedy baselines (97%+ success in simulation and 93% real-world in the reported tasks). The work enables robust, real-time manipulation under large pose uncertainties in domestic-style settings, offering a scalable approach to contact-based localization when visual feedback is limited.

Abstract

Robots often face challenges in domestic environments where visual feedback is ineffective, such as retrieving objects obstructed by occlusions or finding a light switch in the dark. In these cases, utilizing contacts to localize the target object can be effective. We propose an online planning framework using binary contact signals for manipulation tasks with pose uncertainty, formulated as a Partially Observable Markov Decision Process (POMDP). Naively representing the belief as a particle set makes planning infeasible due to the large uncertainties in domestic settings, as identifying the best sequence of actions requires rolling out thousands of actions across millions of particles, taking significant compute time. To address this, we propose a hierarchical belief representation. Initially, we represent the uncertainty coarsely in a 3D volumetric space. Policies that refine uncertainty in this space are computed and executed, and once uncertainty is sufficiently reduced, the problem is translated back into the particle space for further refinement before task completion. We utilize a closed-loop planning and execution framework with a heuristic-search-based anytime solver that computes partial policies within a limited time budget. The performance of the framework is demonstrated both in real world and in simulation on the high-precision task of inserting a plug into a port using a UR10e manipulator, resolving positional uncertainties up to 50 centimeters and angular uncertainties close to $2π$. Experimental results highlight the framework's effectiveness, achieving a 93\% success rate in the real world and over 50\% improvement in solution quality compared to greedy baselines, significantly accelerating planning and enabling real-time solutions for complex problems.

Figures (4)

• Figure 1: Experimental setup for plug insertion using a UR10e manipulator. Top: The port is located inside a shelf and out of view of the robot (uncertainty along $y, z, roll$). Bottom: Movable base set up (uncertainty along $x, y, z, yaw$).
• Figure 2: The first phase of the framework involves reasoning about pose uncertainty in the volumetric space, using contact observations from actions to reduce the potentially occupied volume. Once uncertainty is reduced to a manageable level, we transition to a finer representation in the particle space by mapping the reduced volumetric uncertainty to a smaller set of feasible poses. We then execute policies that will help reduce uncertainty in this space sufficient enough to complete the task.
• Figure 3: If an action results in no contact, the target object is guaranteed to not occupy any part of the swept volume.
• Figure 4: If an action $a$ results in contact at configuration $q_{collision}$, the target object is guaranteed to not be farther than $\mathcal{D}_{max}$ from $\mathcal{S}(q_{collision}, a)$.