arXiv:2605.01860cs.RO2026-05

用树状轨迹优化不确定环境下的机器人规划,提升决策效率与适应性。

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

论文配图:Optimizing Trajectory-Trees in Belief Space: An Application from Model Predictive Control to Task and Motion Planning
图 1 · 摘自论文原文
  • 在信念空间中优化树状轨迹,分支捕捉观测带来的多种可能路径。
  • PO-MPC方法降低控制成本,实测在自动驾驶中表现优于传统序列轨迹。
  • 适用于需要实时响应的运动与任务规划,尤其适合小到中等规模信念状态问题。

本文研究在部分可观测机器人规划问题中,采用树状轨迹(trajectory-trees)替代传统序列轨迹的收益。在这些环境中,机器人需基于观测推断状态,最优行动依赖于观测结果。树状轨迹通过在信念状态可能演化为多场景时分支,自然建模这种依赖关系。相比仅描述单一演化的序列轨迹,树状轨迹可同时刻画多种潜在情形。首先,针对模型预测控制(MPC),提出一种单次分叉的树形优化方法(PO-MPC),显著降低控制代价。为此设计了分布式增广拉格朗日算法(D-AuLa),利用问题可分解性实现并行加速,满足MPC实时性要求,应用于线性和非线性系统,以自动驾驶为例验证效果。其次,在任务与运动规划(TAMP)中,提出基于决策树的任务层与轨迹树的运动层协同规划器(PO-LGP),扩展逻辑几何规划框架(LGP)至部分可观测场景。实验表明该方法在小规模信念空间上有效,通过优化探索性策略作为宏观动作,可扩展至更大规模问题。

原文摘要 · Abstract (English)

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. 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.

机器人规划信念空间树状轨迹MPC

Thank you to arXiv for use of its open access interoperability. PaperDance 不是 arXiv 官方产品;中文卡片由大模型生成,请以原文为准。