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

2026-05-03

AI Summary

This work addresses the challenge of robot planning under partial observability, where observation-dependent branching decisions render conventional sequential trajectories inadequate for handling uncertainty. The paper introduces tree-structured trajectories into partially observable model predictive control (MPC) and task and motion planning (TAMP), explicitly modeling multiple belief-state evolution paths induced by observations. Key contributions include a distributed augmented Lagrangian algorithm (D-AuLa) enabling parallel optimization, an extension of logical geometric programming (LGP) to support hierarchical decision-making in belief space, and a macro-action policy to enhance scalability. Experimental results demonstrate that the proposed approach significantly reduces control cost and meets real-time requirements in autonomous driving scenarios, validates effectiveness on small-scale problems, and extends to larger-scale applications through exploratory strategies.

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.

Problem

• partially observable planning
• trajectory-trees
• belief space
• task and motion planning
• model predictive control

Innovation

• trajectory-trees
• belief space optimization
• partially observable MPC
• task and motion planning
• Distributed Augmented Lagrangian