任务与运动规划理论

概述

任务与运动规划（Task and Motion Planning, TAMP）是具身智能的核心问题之一：如何将高层任务目标分解为可执行的物理动作序列？本文系统介绍 TAMP 的理论框架，包括 PDDL 形式化、HTN 层次分解、PDDLStream 的采样整合，以及 LLM 作为现代 TAMP 引擎的新范式。

1. TAMP 问题定义

1.1 为什么需要 TAMP

传统上，任务规划和运动规划是分开研究的：

任务规划：在符号层面推理动作序列（如"先拿杯子，再倒水"）
运动规划：在几何/物理层面生成无碰撞轨迹

但实际机器人操作中，二者紧密耦合：

某些符号上可行的计划在几何上不可行（如物体被遮挡，无法直接抓取）
运动约束影响任务层面的决策（如先移开障碍物）
需要同时考虑离散决策（做什么）和连续参数（怎么做）

1.2 TAMP 架构

flowchart TB
    subgraph 任务层
        A[任务目标] --> B[符号规划器]
        B --> C[动作骨架<br/>pick→place→pour]
    end
    subgraph 几何层
        C --> D[参数采样]
        D --> E[运动规划]
        E --> F[可行性检查]
    end
    F -->|不可行| B
    F -->|可行| G[执行轨迹]

    style A fill:#e3f2fd
    style B fill:#f3e5f5
    style C fill:#fff8e1
    style D fill:#e8f5e9
    style E fill:#e8f5e9
    style F fill:#fce4ec

1.3 形式化定义

TAMP 问题可以定义为一个元组 \(\langle \mathcal{S}, \mathcal{A}, s_0, G, \mathcal{C} \rangle\)：

\(\mathcal{S} = \mathcal{S}_{\text{discrete}} \times \mathcal{S}_{\text{continuous}}\)：混合状态空间
\(\mathcal{A}\)：动作集合，每个动作有离散参数和连续参数
\(s_0\)：初始状态
\(G\)：目标条件
\(\mathcal{C}\)：几何/物理约束

2. PDDL：规划领域定义语言

2.1 PDDL 基础

PDDL（Planning Domain Definition Language）是任务规划的标准形式化语言，包含两个部分：

Domain（领域）：定义类型、谓词和动作模板

(define (domain pick-and-place)
  (:predicates
    (on ?obj ?surface)
    (holding ?obj)
    (clear ?obj)
    (arm-empty)
  )

  (:action pick
    :parameters (?obj ?surface)
    :precondition (and 
      (on ?obj ?surface) 
      (clear ?obj) 
      (arm-empty))
    :effect (and 
      (holding ?obj) 
      (not (on ?obj ?surface)) 
      (not (arm-empty))
      (clear ?surface))
  )

  (:action place
    :parameters (?obj ?surface)
    :precondition (and 
      (holding ?obj) 
      (clear ?surface))
    :effect (and 
      (on ?obj ?surface) 
      (not (holding ?obj)) 
      (arm-empty)
      (not (clear ?surface)))
  )
)

Problem（问题）：定义具体的对象、初始状态和目标

(define (problem example)
  (:domain pick-and-place)
  (:objects cup plate table counter)
  (:init 
    (on cup table) 
    (on plate counter) 
    (clear cup) 
    (clear plate) 
    (arm-empty))
  (:goal (and 
    (on cup counter) 
    (on plate table)))
)

2.2 动作模式（Action Schema）

每个动作模式包含：

\[\text{Action} = \langle \text{name}, \text{params}, \text{pre}, \text{eff}^+, \text{eff}^- \rangle\]

前置条件（Precondition）：动作执行前必须满足的条件
正效果（Positive Effects）：动作执行后变为真的谓词
负效果（Negative Effects）：动作执行后变为假的谓词

2.3 规划算法

经典规划可以视为图搜索问题：

\[f(n) = g(n) + h(n)\]

其中：

\(g(n)\)：从初始状态到节点 \(n\) 的代价
\(h(n)\)：从 \(n\) 到目标的启发式估计

常用启发式：

启发式	定义	特点
\(h_{\text{add}}\)	假设子目标独立，求和各子目标代价	非 admissible，但信息量大
\(h_{\text{max}}\)	取各子目标代价的最大值	Admissible，但不够紧
\(h_{\text{FF}}\)	放松图（忽略删除效果）的代价估计	常用，性能好
\(h_{\text{LM-cut}}\)	基于 landmarks 的最优删除松弛	Admissible，很紧

主流规划器：

Fast Downward：支持多种启发式和搜索策略
LAMA：多启发式惰性搜索
Pyperplan：Python 实现，适合教学

3. PDDLStream：采样整合

3.1 PDDL 的局限

标准 PDDL 假设所有参数来自有限离散集合，但机器人操作涉及连续参数：

抓取位姿 \(T_{\text{grasp}} \in SE(3)\)
放置位置 \((x, y, z) \in \mathbb{R}^3\)
运动轨迹 \(\tau: [0,1] \rightarrow \mathcal{C}\)

3.2 PDDLStream 框架

Garrett et al. (2020) 提出 PDDLStream，通过流（Stream）将连续参数采样整合到符号规划中：

Stream 定义：

(:stream sample-grasp
  :inputs (?obj)
  :domain (Graspable ?obj)
  :outputs (?grasp)
  :certified (GraspPose ?obj ?grasp)
)

(:stream plan-motion
  :inputs (?q1 ?q2)
  :domain (and (Config ?q1) (Config ?q2))
  :outputs (?traj)
  :certified (and (Motion ?q1 ?traj ?q2) (Collision-free ?traj))
)

工作流程：

符号规划器生成抽象计划骨架
对骨架中的连续参数调用相应的 Stream 进行采样
检查采样结果是否满足几何约束
若不满足，回溯并尝试不同的骨架或采样

3.3 采样策略

自适应采样：优先采样更可能成功的方案

\[p(\text{sample}) \propto \exp\left(-\frac{\text{failure\_count}}{\tau}\right)\]

基于学习的采样：用神经网络学习采样分布

\[T_{\text{grasp}} \sim q_\phi(T | o_{\text{obj}}, s_{\text{scene}})\]

4. 层次任务网络（HTN）

4.1 HTN 基本概念

HTN（Hierarchical Task Network）通过层次分解将复杂任务逐步细化：

核心思想：

\[\text{抽象任务} \xrightarrow{\text{分解方法}} \text{子任务序列} \xrightarrow{\text{递归分解}} \text{原始动作}\]

4.2 形式化

HTN 领域定义为 \(\langle \mathcal{O}, \mathcal{M} \rangle\)：

\(\mathcal{O}\)：原始算子（primitive operators），即可直接执行的动作
\(\mathcal{M}\)：方法（methods），定义如何分解复合任务

方法的定义：

\[m = \langle \text{task}, \text{precondition}, \text{subtasks} \rangle\]

示例：

Method: make-coffee
  Task: (prepare-drink coffee)
  Precondition: (and (have coffee-beans) (have water) (have cup))
  Subtasks: 
    1. (grind coffee-beans)
    2. (boil water)
    3. (brew ground-coffee hot-water)
    4. (pour coffee cup)

4.3 HTN vs 经典规划

维度	经典规划 (STRIPS/PDDL)	HTN
目标表达	目标状态谓词	待完成的任务
搜索空间	状态空间	分解空间
领域知识	仅动作模式	分解方法（更多先验）
计划质量	可保证最优（某些启发式）	依赖方法质量
计算效率	可能较慢	利用领域知识加速

5. LLM 作为现代 TAMP

5.1 LLM 的零样本任务分解

大语言模型天然具备任务分解能力：

传统 TAMP：需要手工定义领域和方法

LLM-TAMP：直接从自然语言指令生成任务计划

User: "帮我做一杯咖啡"
LLM: 
1. 走到厨房台面
2. 拿起咖啡杯
3. 放到咖啡机下方
4. 按下咖啡机按钮
5. 等待咖啡制作完成
6. 拿起咖啡杯
7. 送到用户手中

5.2 代表性工作

SayCan（Ahn et al., 2022）

LLM 提供语义评分，机器人技能提供可行性评分：

\[a^* = \arg\max_a \underbrace{p_{\text{LLM}}(\text{useful} | a, l)}_{\text{语义得分}} \cdot \underbrace{p_{\text{affordance}}(\text{possible} | a, s)}_{\text{可行性得分}}\]

Code as Policies（Liang et al., 2023）

LLM 直接生成可执行的 Python 代码：

# LLM 生成的策略代码
def pour_water_into_cup():
    bottle = detect("water bottle")
    cup = detect("cup")
    grasp(bottle)
    move_to(above(cup))
    tilt(angle=90, duration=3)
    place(bottle, on=table)

Inner Monologue（Huang et al., 2022）

引入环境反馈的闭环 LLM 规划：

LLM: 拿起红色杯子
环境反馈: 抓取失败，杯子在障碍物后面
LLM: 先移开障碍物
环境反馈: 成功移开
LLM: 现在拿起红色杯子

5.3 LLM-TAMP vs 传统 TAMP

维度	传统 TAMP	LLM-TAMP
领域定义	需要手工 PDDL	自然语言即可
泛化性	限于预定义领域	开放词汇、零样本
可靠性	形式化保证	可能产生幻觉
最优性	可以保证	不保证
物理可行性	通过几何检查	需要额外验证
常识推理	有限	强大

5.4 融合方案

最新研究尝试将二者优势结合：

LLM + PDDL：LLM 生成 PDDL 领域定义，传统规划器求解
LLM + 验证器：LLM 生成计划，形式化工具验证可行性
LLM + 约束求解：LLM 提供高层结构，约束求解器处理细节

6. 运动规划基础

6.1 配置空间（Configuration Space）

运动规划在配置空间 \(\mathcal{C}\) 中进行：

\(\mathcal{C}_{\text{free}}\)：自由空间（无碰撞）
\(\mathcal{C}_{\text{obs}}\)：障碍物空间
目标：在 \(\mathcal{C}_{\text{free}}\) 中找到从 \(q_{\text{start}}\) 到 \(q_{\text{goal}}\) 的路径

6.2 基于采样的规划

RRT（Rapidly-exploring Random Tree）：

算法 RRT:
1. 初始化树 T = {q_start}
2. 重复:
   a. 随机采样 q_rand ∈ C
   b. 找到 T 中最近的节点 q_near
   c. 向 q_rand 方向扩展 Δq 得到 q_new
   d. 若 edge(q_near, q_new) ∈ C_free:
      将 q_new 加入 T
3. 直到 q_goal 被连接

RRT*（渐近最优）：

在 RRT 基础上加入重连（rewire）步骤，保证随着采样点增加，路径代价收敛到最优：

\[\lim_{n \to \infty} \text{cost}(\text{RRT}^*) = \text{cost}^*\]

6.3 轨迹优化

将运动规划表示为优化问题：

\[\min_\tau \int_0^T \left[ \|\ddot{q}(t)\|^2 + \lambda \cdot c_{\text{obs}}(q(t)) \right] dt\]

\[\text{s.t.} \quad q(0) = q_{\text{start}}, \quad q(T) = q_{\text{goal}}, \quad q(t) \in \mathcal{C}_{\text{free}}\]

代表方法：

CHOMP：协变哈密顿优化
TrajOpt：序列凸优化
STOMP：随机轨迹优化

7. TAMP 的开放挑战

7.1 可扩展性

物体数量增加时，组合爆炸问题
长 horizon 任务的规划效率
多机器人协调规划

7.2 不确定性处理

感知不确定性：物体位姿估计误差
执行不确定性：动作效果不精确
环境不确定性：动态变化的环境

7.3 学习与规划的融合

学习的启发式加速搜索
从经验中学习分解方法
神经符号方法：连续表征 + 离散推理

参考资料

Garrett, C. R. et al. (2020). "PDDLStream: Integrating Symbolic Planners and Blackbox Samplers"
Ahn, M. et al. (2022). "Do As I Can, Not As I Say: Grounding Language in Robotic Affordances"
Liang, J. et al. (2023). "Code as Policies: Language Model Programs for Embodied Control"
Huang, W. et al. (2022). "Inner Monologue: Embodied Reasoning through Planning with Language Models"
LaValle, S. M. (2006). Planning Algorithms
Ghallab, M. et al. (2004). Automated Planning: Theory and Practice

相关笔记：