Optimizing Trajectory-Trees in Belief Space: An Application from Model Predictive Control to Task and Motion Planning

TL;DR

This paper introduces trajectory-trees in belief space for robotic planning, demonstrating their advantages over sequential trajectories in model predictive control and task planning, with new algorithms for real-time optimization.

Contribution

It presents novel methods for optimizing trajectory-trees in belief space, including PO-MPC with D-AuLa and PO-LGP for task and motion planning under partial observability.

Findings

Trajectory-trees reduce control costs in MPC.

D-AuLa accelerates optimization via parallelization.

PO-LGP extends task planning to partially observable environments.

Abstract

This paper explores the benefits of computing arborescent trajectories (trajectory-trees) instead of commonly used sequential trajectories for partially observable robotic planning problems. In such environments, a robot infers knowledge from observations, and the optimal course of action depends on these observations. \revise{Trajectory-trees, optimized in belief space, naturally capture this dependency by branching where the belief state is expected to evolve into multiple distinct scenarios, such as upon receiving an observation. Unlike sequential trajectories, which model a single forward evolution of the system, trajectory-trees capture multiple possible contingencies.} First, we focus on Model Predictive Control (MPC) and demonstrate the benefits of planning tree-like trajectories. We formulate the control problem as the optimization of a tree with a single branching (PO-MPC). This improves performance by reducing control costs through more informed planning. To satisfy the real-time constraints of MPC, we develop an optimization algorithm called Distributed Augmented Lagrangian (D-AuLa), which leverages the decomposability of the PO-MPC formulation to parallelize and accelerate the optimization. We apply the method to both linear and non-linear MPC problems using autonomous driving examples. Second, we address Task And Motion Planning (TAMP), and introduce a planner (PO-LGP) reasoning on decision trees at task level, and trajectory-trees at motion-planning level. This approach builds upon the Logic-Geometric-Programming Framework (LGP) and extends it to partially observable problems. The experiments show the method's applicability to problems with a small belief state size, and scales to larger problems by optimizing explorative policies, which are used as macro-actions in an overarching task plan.