Abstract
Motion generation in cluttered, dense, and dynamic environments is a central topic in robotics, rendered as a multi-objective decision-making problem. Current approaches trade-off between safety and performance. On the one hand, reactive policies guarantee fast response to environmental changes at the risk of suboptimal behavior. On the other hand, planning-based motion generation provides feasible trajectories, but the high computational cost may limit the control frequency and thus safety. To combine the benefits of reactive policies and planning, we propose a hierarchical motion generation method. Moreover, we adopt probabilistic inference methods to formalize the hierarchical model and stochastic optimization. We realize this approach as a weighted product of stochastic, reactive expert policies, where planning is used to adaptively compute the optimal weights over the task horizon. This stochastic optimization avoids local optima and proposes feasible reactive plans that find paths in cluttered and dense environments. Our extensive experimental study in planar navigation and 6DoF manipulation shows that our proposed hierarchical motion generation method outperforms both myopic reactive controllers and online re-planning methods.
Introduction
Robot motion generation typically bifurcates into two distinct paradigms: Motion Planning and Reactive Control. Motion planners, such as trajectory optimization, are capable of long-horizon foresight, effectively avoiding local minima—scenarios where a robot becomes trapped by obstacles due to a lack of global perspective. However, the high computational cost of planning often restricts control frequency, potentially compromising safety in dynamic environments. Conversely, reactive controllers, like potential fields, offer instantaneous responses to environmental changes but are inherently "myopic." They prioritize immediate surroundings, making them susceptible to local traps or oscillations between conflicting objectives.
Methodology
The proposed Hierarchical Policy Blending as Inference (HiPBI) framework bridges this gap by integrating high-frequency reactivity with strategic look-ahead. The architecture consists of two levels:
- Low-Level (Reactive Experts): This layer utilizes Riemannian Motion Policies (RMPs), reformulated as a Product of Experts (PoE). Each expert represents a specific task objective (e.g., obstacle avoidance, goal reaching) as a Gaussian distribution.
- High-Level (The Planner): Rather than planning specific trajectories, the high-level planner optimizes the weights (temperatures) of these experts over a time horizon. Using iCEM (improved Cross-Entropy Method), the planner determines the optimal mixture of behaviors.
By framing this as Control as Inference, the system treats optimal weights as hidden variables in a Bayesian framework, employing Variational Inference to maximize the likelihood of safe goal attainment.
Experimental Evaluation
HiPBI was evaluated across three simulated environments:
- 2D Toy Maze: A point-mass navigating dynamic obstacles.
- 2D Toy Box: A particle must enter a moving box, a scenario designed to trap myopic controllers.
- 7DoF Manipulator: A high-dimensional simulation using a Franka Emika Panda robot arm. The task involves navigating through a cluttered space with both static and dynamic obstacles to reach goals.
Results
The experimental results highlight HiPBI's ability to overcome the limitations of standard reactive control. In the 2D Toy Box environment, the baseline RMPflow controller failed completely (0% success) due to local optima. In contrast, HiPBI achieved a 100% success rate with an adequate look-ahead horizon. Furthermore, HiPBI maintained a 100% safety rate in dynamic 2D environments, even as obstacle speeds increased, outperforming standard MPC-based planners in asynchronous modes. In high-dimensional 7DoF tasks, HiPBI successfully generated complex maneuvers to navigate cluttered spaces where reactive baselines frequently stalled.
Conclusion
HiPBI demonstrates that planning in the weight space of reactive experts provides a robust solution for complex robot navigation. While the sampling-based inference introduces some computational overhead, the framework's ability to maintain reactivity while avoiding local traps marks a significant advancement. Future work will explore more efficient collocation methods and the integration of learned priors from human demonstrations to further enhance performance in real-world deployments.
Summary updated