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

• URL: http://arxiv.org/abs/2605.01860v1
• Date: Sun, 03 May 2026 13:06:20 GMT
• Title: Optimizing Trajectory-Trees in Belief Space: An Application from Model Predictive Control to Task and Motion Planning
• Abstract summary: This paper explores the benefits of computing arborescent trajectories (trajectory-trees) instead of commonly used sequential trajectories for partially observable robotic planning problems.<n>Unlike sequential trajectories, which model a single forward evolution of the system, trajectory-trees capture multiple possible contingencies.
• Score: 12.236092368066332
• License: http://arxiv.org/licenses/nonexclusive-distrib/1.0/
• 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.

Related papers

• Dispersive Forward Tree Search for Optimal Control: Coverage, Complexity, and Computation [15.03677748041754]
  This paper develops a propagation-based kinodynamic planner with deterministic finite-sample near-optimality guarantees.<n>We show that a forward tree of locally dispersive control commands contains a near-optimal trajectory at a certified tree size.<n>We implement the resulting search algorithm, Dispersive Forward Tree search (DFT*), as breadth-first expansion of the forward tree.
  arXiv  Detail & Related papers  (2026-08-26T18:46:17Z)
• FactorDrive: Adaptive Multi-Step Reasoning Driven by Planning-Critical Factors for End-to-End Autonomous Driving [8.241982806652256]
  We propose FactorDrive, an end-to-end framework for adaptive multi-step reasoning driven by planning-critical factors (PCFs)<n>PCF-CoT dataset grounds planning reasoning in trajectory-relevant spatial-physical evidence.<n>QS-GRPO guides Monte Carlo Tree Search (MCTS) with trajectory-level planning rewards to discover reasoning paths with higher planning quality.
  arXiv  Detail & Related papers  (2026-08-10T13:26:09Z)
• Advantage-Guided Gate: Reshaping Open-Ended Reasoning for Vision-Based Spatial Intelligence [70.8951332710039]
  Multimodal large language models (MLLMs) have demonstrated significant potential in complex spatial scene understanding and reasoning tasks.<n>However, their open-ended reasoning process is prone to decision errors and error accumulation, leading to instability in answer quality.<n>We propose an advantage-guided gating framework that dynamically intervenes in and corrects deviations during the reasoning process.
  arXiv  Detail & Related papers  (2026-08-08T07:46:04Z)
• Sampling-Based Control via Entropy-Regularized Optimal Transport [18.278741443124655]
  OT-MPC is a sampling-based algorithm that overcomes limitations through an entropy-regularized optimal transport formulation.<n>We derive closed-form, gradient-free updates via the Sinkhorn algorithm, enabling real-time performance.
  arXiv  Detail & Related papers  (2026-05-04T02:07:15Z)
• HAD: Combining Hierarchical Diffusion with Metric-Decoupled RL for End-to-End Driving [51.268878540511054]
  We propose HAD, an end-to-end planning framework with a Hierarchical Diffusion Policy.<n>We show that HAD achieves new state-of-the-art performance on both NAVSIM and HUGSIM.
  arXiv  Detail & Related papers  (2026-04-04T04:12:47Z)
• PILOT: Planning via Internalized Latent Optimization Trajectories for Large Language Models [51.43746425777865]
  Large Language Models (LLMs) often lack the capacity to formulate global strategies, leading to error propagation in long-horizon tasks.<n>We propose PILOT, a framework designed to internalize the strategic oversight of large models into intrinsic Latent Guidance.
  arXiv  Detail & Related papers  (2026-01-07T12:38:56Z)
• Planning with Adaptive World Models for Autonomous Driving [50.4439896514353]
  We present nuPlan, a real-world motion planning benchmark that captures multi-agent interactions.<n>We learn to model such unique behaviors with BehaviorNet, a graph convolutional neural network (GCNN)<n>We also present AdaptiveDriver, a model-predictive control (MPC) based planner that unrolls different world models conditioned on BehaviorNet's predictions.
  arXiv  Detail & Related papers  (2024-06-15T18:53:45Z)
• Learning Logic Specifications for Policy Guidance in POMDPs: an Inductive Logic Programming Approach [57.788675205519986]
  We learn high-quality traces from POMDP executions generated by any solver. We exploit data- and time-efficient Indu Logic Programming (ILP) to generate interpretable belief-based policy specifications. We show that learneds expressed in Answer Set Programming (ASP) yield performance superior to neural networks and similar to optimal handcrafted task-specifics within lower computational time.
  arXiv  Detail & Related papers  (2024-02-29T15:36:01Z)
• Interactive Joint Planning for Autonomous Vehicles [19.479300967537675]
  In interactive driving scenarios, the actions of one agent greatly influences those of its neighbors. We present Interactive Joint Planning (IJP) that bridges MPC with learned prediction models. IJP significantly outperforms the baselines that are either without joint optimization or running sampling-based planning.
  arXiv  Detail & Related papers  (2023-10-27T17:48:25Z)
• Tree-Planner: Efficient Close-loop Task Planning with Large Language Models [63.06270302774049]
  Tree-Planner reframes task planning with Large Language Models into three distinct phases. Tree-Planner achieves state-of-the-art performance while maintaining high efficiency.
  arXiv  Detail & Related papers  (2023-10-12T17:59:50Z)
• Stochastic Finite State Control of POMDPs with LTL Specifications [14.163899014007647]
  Partially observable Markov decision processes (POMDPs) provide a modeling framework for autonomous decision making under uncertainty. This paper considers the quantitative problem of synthesizing sub-optimal finite state controllers (sFSCs) for POMDPs. We propose a bounded policy algorithm, leading to a controlled growth in sFSC size and an any time algorithm, where the performance of the controller improves with successive iterations.
  arXiv  Detail & Related papers  (2020-01-21T18:10:47Z)

This list is automatically generated from the titles and abstracts of the papers in this site.

This site does not guarantee the quality of this site (including all information) and is not responsible for any consequences.