Efficient Hierarchical Robot Motion Planning Under Uncertainty and Hybrid Dynamics

Ajinkya Jain, Scott Niekum

Abstract

Noisy observations coupled with nonlinear dynamics pose one of the biggest challenges in robot motion planning. By decomposing nonlinear dynamics into a discrete set of local dynamics models, hybrid dynamics provide a natural way to model nonlinear dynamics, especially in systems with sudden discontinuities in dynamics due to factors such as contacts. We propose a hierarchical POMDP planner that develops cost-optimized motion plans for hybrid dynamics models. The hierarchical planner first develops a high-level motion plan to sequence the local dynamics models to be visited and then converts it into a detailed continuous state plan. This hierarchical planning approach results in a decomposition of the POMDP planning problem into smaller sub-parts that can be solved with significantly lower computational costs. The ability to sequence the visitation of local dynamics models also provides a powerful way to leverage the hybrid dynamics to reduce state uncertainty. We evaluate the proposed planner on a navigation task in the simulated domain and on an assembly task with a robotic manipulator, showing that our approach can solve tasks having high observation noise and nonlinear dynamics effectively with significantly lower computational costs compared to direct planning approaches.

Introduction

Robot motion planning becomes exceptionally complex when environments exhibit both nonlinear dynamics and significant observation uncertainty. Traditional frameworks like the Partially Observable Markov Decision Process (POMDP) are theoretically robust but often computationally intractable for long-horizon tasks. In contact-rich environments—such as industrial assembly—small errors in state estimation can lead to total failure if a robot misses a contact point or unexpectedly strikes an obstacle. This paper addresses these challenges by introducing a hierarchical planning approach that leverages hybrid dynamics to manage uncertainty and reduce computational overhead.

Hierarchical POMDP Planning Framework

The core innovation of this work is a hierarchical planner that decomposes the planning problem into two distinct levels:

  1. High-Level Discrete Planning: Instead of optimizing a continuous path from the outset, the planner first determines an optimal sequence of "local dynamics models" or modes. These modes represent different physical states, such as moving through free space versus sliding along a surface.
  2. Low-Level Continuous Planning: Once the high-level sequence is established, the system employs trajectory optimization (specifically Sequential Least Squares Programming) to generate the precise continuous path required to execute those transitions.

To manage uncertainty, the system tracks a Hybrid Belief Space, accounting for both the robot's physical position and the active dynamics mode. The planner utilizes "confidence maps" and the Hellinger distance—a statistical measure of the difference between probability distributions—to identify optimal physical locations for transitioning between modes. This allows for Active Uncertainty Reduction, where the robot strategically chooses paths that naturally decrease state uncertainty, such as using a wall to re-calibrate its position.

Experimental Evaluation

The proposed planner was evaluated in two primary domains designed to test its limits under high noise:

  • Simulated Walled Domain: A 2D navigation task where a robot must reach a goal despite extreme sensor noise. The environment includes perpendicular walls that serve as localization landmarks.
  • Real-World Airplane Assembly: Using a toy airplane from the YCB Object and Model Set, a robotic manipulator was tasked with inserting landing gear into a wing. This task required a precision of ±0.2 cm, even though the available sensors had an average noise of ±2.0 cm.
Sequence of time steps showing the robotic manipulator performing the airplane assembly task.

Results and Discussion

The hierarchical approach demonstrated significant improvements over direct planning methods in both efficiency and robustness. In the simulated walled domain, the hierarchical planner achieved a 5x speedup, finding solutions in approximately 10.7 seconds compared to 51.9 seconds for direct planning.

Comparison of planned and actual trajectories between direct planning (a) and the proposed hierarchical planner (b).

Beyond speed, the hierarchical planner proved far more resilient to noise. By treating physical constraints (like walls) as tools for localization rather than just obstacles to avoid, the robot was able to "feel" its way through the assembly task. This resulted in a 10x improvement in effective precision, allowing the system to successfully complete tasks requiring 0.2 cm accuracy using sensors with 2.0 cm of error. As shown in the trajectory comparisons, the hierarchical method maintains a much tighter alignment between the planned and actual trajectories by actively sequencing modes to reduce belief uncertainty.

Conclusion and Future Work

This research demonstrates that hierarchical decomposition and the strategic use of hybrid dynamics can make complex POMDP problems tractable and robust. While the current system requires expert-provided hybrid dynamics models, it sets the stage for more autonomous systems. Future work will focus on integrating autonomous learning methods to allow robots to discover these local dynamics models directly from environmental interaction, eventually leading to an end-to-end pipeline for complex, contact-rich task planning.

Summary updated

Abstract

Noisy observations coupled with nonlinear dynamics pose one of the biggest challenges in robot motion planning. By decomposing nonlinear dynamics into a discrete set of local dynamics models, hybrid dynamics provide a natural way to model nonlinear dynamics, especially in systems with sudden discontinuities in dynamics due to factors such as contacts. We propose a hierarchical POMDP planner that develops cost-optimized motion plans for hybrid dynamics models. The hierarchical planner first develops a high-level motion plan to sequence the local dynamics models to be visited and then converts it into a detailed continuous state plan. This hierarchical planning approach results in a decomposition of the POMDP planning problem into smaller sub-parts that can be solved with significantly lower computational costs. The ability to sequence the visitation of local dynamics models also provides a powerful way to leverage the hybrid dynamics to reduce state uncertainty. We evaluate the proposed planner on a navigation task in the simulated domain and on an assembly task with a robotic manipulator, showing that our approach can solve tasks having high observation noise and nonlinear dynamics effectively with significantly lower computational costs compared to direct planning approaches.
BibTeX
@inproceedings{jain2018efficient,
  author = {Ajinkya Jain and Scott Niekum},
  title = {{Efficient Hierarchical Robot Motion Planning Under Uncertainty and Hybrid Dynamics}},
  booktitle = {Conference on Robot Learning},
  year = {2018},
  url = {https://openreview.net/forum?id=7tGD5InuDaH},
}

Loading related content...

Figure