Part IV · 第 14 章运动规划 · 14.4

任务与运动规划 (TAMP)

“把杯子放进托盘”听起来是一步，可如果杯子前面挡着一个盒子、托盘里还剩不下多少地方，机器人就得先决定做哪些事、按什么顺序（任务规划），再为每一步选出具体的抓取、摆放位置和无碰撞轨迹（运动规划），而这两层互相牵制。本节先讲符号规划（STRIPS / PDDL 在谓词上的搜索），再看几何怎样让纯符号的计划失效，然后亲手运行采样器、可行性检查、回溯，以及“规划—绑定—学习”的完整循环，最后用逻辑-几何规划（LGP）把连续参数放进一个优化问题里联合求解。

约 40 分钟前置：14.1 图搜索与 A* · 14.2 采样式规划 · 14.3 轨迹优化 · 7.5 逆运动学 6 个交互

1为什么机器人需要它

14.1 到 14.3 节回答的都是同一个问题：已知起点构型和终点构型，找一条不碰撞的路径。可是让机器人“收拾桌子”“装洗碗机”“把零件装进夹具”时，终点构型本身并不知道。机器人要先想清楚做什么：先拿哪个物体、要不要先把挡路的东西挪开、东西放到哪儿；还要想清楚怎么做：从哪一侧抓、放在托盘的哪个位置、手臂沿什么轨迹过去。前者是离散的选择（动作的种类和顺序），后者是连续的选择（抓取位姿、摆放位姿、关节构型、轨迹）。

这两层不能分开解决。只看符号，“拿起杯子，放进托盘”是一个完美的两步计划；可杯子前面如果有个盒子挡住了夹爪的来路，第一步在几何上就做不到。反过来，只看几何，运动规划器根本不知道“先挪盒子”这种动作该不该做。任务与运动规划（Task and Motion Planning，TAMP）就是把两层放到同一个搜索里：符号层提出动作序列，几何层为每个动作找具体参数并检查可行性，失败的信息再反馈给符号层。

本节的路线：先用积木世界讲清楚符号规划（第 2 节 FIG 1），再看几何怎样让它失效（FIG 2）；第 3 节运行一个完整的 TAMP 循环（FIG 3）；第 4 节拆开循环里的两个零件：采样器（FIG 4）和回溯（FIG 5）；第 5 节换一个视角，把连续参数交给优化（LGP，FIG 6）。

2直觉：符号规划，以及它看不见的东西

世界 = 一组为真的命题

经典的符号规划（STRIPS，1971）把世界描述成一组为真的谓词：on(C,A)（C 在 A 上）、ontable(A)、clear(B)（B 上面没东西）、handempty（手是空的）。没写出来的命题就当作假。每个动作有三张表：前提（执行前必须为真的命题）、添加（执行后变真的）、删除（执行后变假的）。例如 unstack(C,A) 的前提是 on(C,A)、clear(C)、handempty，它添加 holding(C) 和 clear(A)，删除 on(C,A)、clear(C)、handempty。

这样一来，规划就是 14.1 节的图搜索：节点是状态（命题集合），边是可执行的动作，目标是包含所有目标命题的任一状态。BFS 给出最短计划，A* 用启发式少展开一些状态。下图是经典的积木世界，左边是当前状态，右边是目标。你可以点下面的动作按钮自己动手（只有前提满足的动作才会出现），也可以让 BFS 或 A* 来规划。

FIG 1积木世界里的 STRIPS：状态是命题集合，动作改写命题

左：当前状态（侧视，灰色横线是桌面，右上角是夹爪）；右：目标状态，目标满足时标题旁出现 ✓。下方第一行是当前状态里所有为真的命题，属于目标的命题带绿框；第二行是目标命题，未满足的带红色虚线框。第三行是上一个动作的三张表：蓝框为前提，绿框为添加，红色删除线为删除。“BFS / A*”从当前状态规划；“展开”是从队列里取出并扩展的状态个数，“可达”是从初始状态出发能到达的全部状态数。A* 的启发式是“还没满足的目标命题个数”。

试一试

在“Sussman 异常”里只用动作按钮（不用规划器的“执行”）把积木摆成目标。最少要 6 个动作；先把 A 放到 B 上的人会发现，接下来为了把 B 放到 C 上又得把 A 拿下来。
切到“四块积木”，在初始状态分别按“BFS”和“A*”：两者都给出 8 步的计划，但 BFS 展开了 86 个状态（可达状态共 125 个），A* 只展开 32 个。

符号层擅长什么

“Sussman 异常”说明了为什么要搜索整个动作序列，而不是一个子目标一个子目标地完成：目标 on(A,B) 和 on(B,C) 互相干扰，先完成任何一个都得在之后拆掉它。符号层用几十个状态就理清了这种顺序上的依赖，它不需要知道积木在哪个坐标、有多大。这既是它的力量，也是它的盲区。

几何让符号计划失效

换一个更像真实机器人的场景：俯视一个三面有墙、只能从前面伸手进去的货架。目标是把琥珀色的物体 T 放进右边的托盘。符号模型里只有 on(T,shelf)、handempty 这类命题，pick(T) 的前提都满足，所以符号规划器满怀信心地给出两步计划 pick(T) → place(T,tray)。可是夹爪要从前面开口一路伸到 T 那里，它扫过的区域（下图的长条）如果碰到蓝色的 B，这一步在几何上就不可行。同样，托盘里的青色杯子 M 放在角落时 T 还放得下，放在中间时四周剩下的空隙都比 T 窄，place(T,tray) 找不到任何位置。

这两种失败符号层都看不见：它的命题里没有“谁挡着谁”，也没有“托盘还剩多少空间”。你当然可以把这些写成命题，但它们取决于连续的位置和尺寸，事先把所有可能的 blocks(x,y) 都算出来，等于把几何问题提前解了一遍。

FIG 2符号上完美的计划，几何上可能做不到

俯视图，单位 m。货架内部 60 × 40 cm，左、右、后三面是墙（粗线），只能从下方的开口伸进去。拖动 T、B（限制在货架里）和托盘中的杯子 M（限制在托盘里）。长条是夹爪抓 T 时扫过的区域：张开的两指从开口一直伸到 T 的中线，宽度是 T 的宽度加两侧各 2 cm 的手指；它与别的物体相交时变成红色并标出挡路的物体，否则是绿色。托盘里的绿色区域是 T 的中心可以放的位置（T 要完全在托盘内，并与 M 保持 1 cm 间隙）；没有这样的位置时托盘标成红色。托盘 20 × 20 cm，T 与 M 都是 8 × 8 cm。

挪开 B（或挪动 T），让夹爪的通道变成绿色：符号计划没有变，变的只是几何。
把杯子 M 拖到托盘中间，让 T 再也放不进去。M 的中心落在托盘正中约 6 × 6 cm 的方块里时，四周的空隙都只剩不到 9 cm（T 的 8 cm 加 1 cm 间隙），T 放不进去。

3动手：规划—绑定—学习的循环

TAMP 最常见的结构是一个循环，下面这个版本接近 Srivastava 等人（2014）的“接口层”和 PDDLStream 的做法：

规划：符号规划器（这里是 BFS）在当前已知的命题下找最短的动作序列，叫作计划骨架（plan skeleton）。骨架里的动作还缺连续参数：从哪儿抓、放在哪儿。
绑定：按顺序为骨架里每个动作找连续参数，并在几何上检查。pick 检查夹爪通道是否畅通；place 在目标区域里随机采样摆放位置，最多 40 次，取第一个不碰撞的。
学习：如果某一步失败，分析原因并写成新的命题，例如通道被 B 挡住就加入 blocks(B,T)，托盘因为 M 放不下就加入“M 还在托盘里时 T 放不进托盘”；带着新命题回到第 1 步。

新命题让符号规划器“看见”了它原本看不见的几何事实：pick(T) 多了一个前提“没有还在货架上的物体挡着 T”，于是最短计划变成先挪 B。下图从默认场景开始，按“单步”看循环一步步走，按“执行”看机器人照着找到的计划动起来。运行前可以拖动 T、B、C 和托盘里的 M 改变场景。

FIG 3完整的 TAMP 循环：符号规划、几何绑定、从失败中学习

俯视图。左边是放挪开物体用的边桌，中间是只能从前面伸进去的货架，右边是托盘（里面有杯子 M）。目标只有一条：in(T,tray)。右侧（窄屏时在下方）的日志按迭代列出：符号规划器给出的骨架（BFS 展开的状态数）、每个动作的绑定结果（✓ 或 ✗）、学到的新命题（紫色）。单步时画面显示当前正在检查的几何状态：失败的通道画成红色，挡路的物体加红框；成功的通道画成绿色。每次绑定都从真实的初始场景重新开始模拟。摆放采样用固定随机种子，所以同一个场景每次结果相同。

在默认场景里一直按“单步”（或按“运行”）直到找到计划：第一个骨架因为 B 挡路失败，学到 blocks(B,T) 之后第二个骨架（4 个动作）成功。再按“执行”看一遍。
改变场景，使最终计划需要 6 个动作。两种办法：把 C 拖到 B 正前方（于是 C 挡 B、B 挡 T），或者把 M 拖到托盘中间（于是要先把 M 挪走）。看日志里循环用了几次迭代、学到了什么。

核心洞见

符号层负责“组合”：在离散的动作序列里搜索，几十个状态就能排好顺序。几何层负责“诚实”：它知道真实的尺寸和位置，能否定看似可行的步骤。两层之间的接口是失败时返回什么：只说“失败了”，符号层下次多半提出同一个骨架；说清楚“B 挡着 T”，符号层就能把它变成新的前提，绕开同一类失败。

4循环里的两个零件：采样器与回溯

采样器：产生连续参数，再检查可行性

FIG 3 里的 pick 只做了一次通道检查，真实的机械臂要回答的问题多得多：从哪个方向抓？那个抓取位姿手臂够得着吗（逆运动学）？够得着的话，手臂和手指会不会撞到旁边的东西（碰撞检测）？TAMP 系统把这些拆成两类零件：采样器（生成候选的抓取方向、摆放位置）和检查器（逆运动学、碰撞检测、稳定性）。PDDLStream 把这样一对“生成 + 认证”叫作一个 stream：输入物体，输出抓取，并认证“这是 T 的一个有效抓取”这条命题。

下图是一个俯视的平面 3R 机械臂（7.5 节两连杆逆解再加一个腕关节）。夹爪可以从任意方向 $\varphi$ 去抓罐子。采样器在 $[0,2\pi)$ 上均匀随机地取 40 个方向，每个方向依次检查：腕部点在两连杆的工作空间里吗（不在就是琥珀色“够不着”）？两种肘部解里有没有一个不碰撞（都碰就是红色“碰撞”）？剩下的是绿色“可行”。

FIG 4抓取采样器：逆运动学与碰撞检测筛掉大部分候选

俯视图，单位 m。机械臂基座在原点，连杆长 36 cm、30 cm，腕到罐心 10 cm；罐子半径 3.5 cm。可拖动罐子和两个障碍物（障碍物不能压到罐子上）。罐子周围一圈小点是 40 个采样方向的结果：绿 = 可行，琥珀 = 够不着，红 = 碰撞；每个点画在夹爪伸过来的那一侧。青色虚线圆是两连杆能到达的最远距离 66 cm：腕部点必须在圆内。滑块选一个方向 $\varphi$，画出对应的手臂（可行时画成蓝色，碰撞时画成红色，够不着时只画出腕部目标 ✕）。“密集估计”用 720 个等间距方向估计各类所占的比例 $p$。

转动滑块 $\varphi$，找一个手臂够得着、但会撞到障碍物的方向（手臂变红）。逆运动学有解不等于这个抓取能用。
拖动障碍物把罐子围起来，让可行比例 $p$ 降到 25% 以下但不为 0：采样器平均要试 $1/p\ge4$ 次才碰到一个可行抓取。（例如把下方的窄障碍放到罐子和基座之间。）

回溯：前面的连续选择会卡死后面的步骤

有了采样器，最简单的绑定方法是贪心：按顺序给每个动作取第一个可行的参数，不再回头。这在动作彼此无关时没问题，可 TAMP 里的动作几乎总是相关的：第一个盒子放在哪儿，决定了第二、第三个盒子还剩多少地方。下图把三个宽 12、20、15 cm 的盒子依次放到一段隔板上，隔板长度 = 盒宽之和 + 余量。每个盒子最多采样 $K$ 次。回溯（深度优先搜索）在某一层用完 $K$ 次也找不到位置时，退回上一层换一个位置再试。

两种采样器：拒绝采样在整段隔板上均匀取位置，撞到已有的盒子就作废；自由空间采样只在还空着、放得下的位置里取，从不作废。注意第二种已经是“完美”的单步采样器，但贪心时它仍然会失败：前两个盒子落在中间，把剩下的空间切成两段，每段都比第三个盒子窄。

FIG 5贪心与回溯：在连续参数上的深度优先搜索

上：侧视的隔板（灰色横线），已放下的盒子按层编号；刚被拒绝的采样画成红色虚线框。下：搜索树，横轴是采样的先后顺序，第 $k$ 行是第 $k$ 个盒子的采样，线连到它的父节点（上一个盒子的位置）。红 ✕ = 碰撞被拒，琥珀 = 当前路径上，绿 = 最终成功的路径，灰点 = 被回溯放弃的位置。统计读数在当前设置下对两种方法各跑 400 个随机种子。

用默认设置选“贪心”并运行：失败（换种子直到失败也行）。不换种子，改选“回溯”再运行：同一组随机数下，退回去换掉前面盒子的位置之后成功了。
保持自由空间采样，把余量降到 5 cm 或更小：统计里贪心的成功率跌到 40% 以下，而回溯的成功率仍是贪心的两倍以上。

一个想法，几种看法

约束满足：固定一个骨架之后，找参数就是一个约束满足问题（CSP），变量是各个动作的连续参数，约束是逆运动学、碰撞、区域；回溯是 CSP 的标准解法。搜索：FIG 5 下半部分就是一棵搜索树，和 FIG 1 的状态图一样，只是分支来自采样而不是有限的动作表。概率：若第 $k$ 步一次采样成功的概率是 $p_k$，平均要试 $1/p_k$ 次（几何分布，4.9 节的拒绝采样）；但各步的 $p_k$ 依赖前面的选择，所以“各步相乘”只在各步独立时成立。

5另一个视角：用优化代替采样（逻辑-几何规划）

采样加回溯是“先猜、再检查、不行就退”。Toussaint（2015）提出的逻辑-几何规划（LGP）换了个思路：骨架一旦定下来，所有连续参数（各个关键时刻的构型、抓取、摆放）就组成一个非线性规划，可以一次联合求解，让后面动作的约束直接影响前面的选择。外层仍是对骨架的符号搜索，内层是 14.3 节那样的优化。

下面的侧视例子：一根 30 cm 的棒子平放在桌上，要插进左边架子上的槽里，至少插进 $d_{\min}$。夹爪必须留在槽外，所以只能抓棒子靠外的那一端。夹爪从左上方出发，贪心地选“离我最近”的抓取点，就会抓到左端，而插槽要求抓右边，插入那一步无解。联合优化同时看见两步的约束，直接选出可以插入的抓取。如果棒子右端被一根横梁盖住，直接插入的骨架就不可行，LGP 只能换一个更长的骨架：先放到右边空地上，换个位置重新抓，再插入。

为了能把整个可行集画出来，这里只用“关键帧”层次：变量是各个关键时刻夹爪的位置，代价是相邻关键帧之间距离的平方和（起点 → 抓取 → … → 回到起点），约束都是线性的，用增广拉格朗日法（6.7 节）求解。完整的 LGP 还会在关键帧之间优化整条轨迹。

FIG 6LGP：在骨架之内联合优化关键帧，在骨架之间做符号搜索

左：侧视场景，单位 m。左边是带槽的架子（开口在 x = 0.20 m，槽深 32 cm），中间是桌上的棒子，带斜线的横梁盖住它右端的一部分，右边虚线括号是可以临时放下棒子的空地；$\mathsf S$ 是夹爪的起点和终点。算出的解里，各关键帧的夹爪画成琥珀色并编号，青色虚线是关键帧之间的连线，琥珀色轮廓是棒子在各关键帧的位置。右：直接插入骨架的两个变量平面，横轴是抓取点相对棒子中心的偏移 $s$，纵轴是插入时棒子中心的位置 $p$；绿色多边形是可行集，蓝色椭圆是代价等高线，琥珀色折线是增广拉格朗日法每次外层迭代后的位置，紫色竖虚线是贪心选的抓取 $s$。

默认设置下先按“贪心逐步”：抓在 $s=-13$ cm（离起点最近的一端），插入那一步无解。再按“联合优化”：增广拉格朗日法收敛到 $s=7$ cm、$p=17$ cm，约束违反小于 $10^{-6}$ m。
把横梁覆盖比例加到 0.25 或以上，再按“LGP 搜索”：直接插入的骨架不可行（可行集消失，约束违反停在一个正数上，f = 0.25 时是 5 mm），搜索转向 4 个动作的“放下再抓”骨架并找到解。

采样还是优化？

采样器把每个约束当成黑盒，只要能检查就能用，组合起来也简单；代价是参数之间的耦合只能靠回溯慢慢发现。优化把整个骨架的约束一起交给求解器，耦合直接体现在梯度里，解也更平滑；代价是需要可微的约束和好的初值，而且非凸问题上求解器失败并不能证明骨架不可行。FIG 6 能准确判断“不可行”，只是因为这里的约束全是线性的。

6再看数学

提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮：FIG 1 的前提 / 添加 / 删除命题，FIG 2 的夹爪通道和托盘可行区，FIG 4 的够不着与碰撞的采样点，FIG 3 日志里的骨架，FIG 5 已放下的盒子位置，FIG 6 的关键帧和可行集。

STRIPS 与 PDDL

给定一个有限的命题集合 $\mathcal P$，状态 $s\subseteq\mathcal P$ 是为真的命题（没列出的为假，即“封闭世界假设”）。动作 $a$ 由三个集合描述，可执行条件与转移函数是

$$ a \text{ applicable in } s \iff \hl{pre}{\mathrm{pre}(a)}\subseteq s,\qquad \gamma(s,a)=\big(s\setminus\hl{del}{\mathrm{del}(a)}\big)\cup\hl{add}{\mathrm{add}(a)} $$

计划是一串动作 $a_1,\dots,a_n$，从初始状态 $s_0$ 依次可执行，并且最终状态包含目标 $G$：$G\subseteq\gamma(\cdots\gamma(s_0,a_1)\cdots,a_n)$。实际中动作写成带参数的模式，规划器把参数换成具体物体得到“落地”动作。PDDL（规划领域定义语言，1998）是描述它们的标准语言，FIG 1 的 stack 写出来是：

(:action stack
  :parameters (?x ?y)
  :precondition (and (holding ?x) (clear ?y))
  :effect (and (on ?x ?y) (clear ?x) (handempty)
               (not (holding ?x)) (not (clear ?y))))

启发式。FIG 1 的 A* 用 $h(s)=|G\setminus s|$。当目标只包含 on 命题时它是可采纳的：只有 stack 能让一个 on 命题变真，而每个 stack 只添加一个 on，所以还差几个目标命题，至少就还要几步。每一步最多让 $h$ 变化 1，所以它也是一致的（14.1 节的两个条件）。实用的规划器（FF、Fast Downward）用更强的启发式，例如忽略删除表的“松弛计划”长度：信息量大得多，但不再可采纳。

规模。命题个数随物体数多项式增长，状态数却是指数增长：3 块积木（含手的状态）有 22 个可达状态，4 块有 125 个。判定一般 STRIPS 问题有没有计划是 PSPACE 完全的（Bylander 1994）。所以 TAMP 里的符号层要尽量小：只放和任务有关的物体与动作。

混合问题：骨架与绑定

TAMP 的状态既有离散部分 $s$（命题），也有连续部分 $x$：机器人构型 $\mathbf q\in\mathcal C$（7.2 节）和各物体的位姿。动作带连续参数，例如 $\mathrm{pick}(o,g,\mathbf q)$ 里的抓取 $g$（夹爪相对物体的位姿）和构型 $\mathbf q$，$\mathrm{place}(o,p,g,\mathbf q)$ 里的摆放位姿 $p$。一个计划骨架是一串只确定了离散部分的动作，绑定就是为它找连续参数：

$$ \hl{skel}{\pi=\big(a_1(\boldsymbol\theta_1),\dots,a_K(\boldsymbol\theta_K)\big)},\qquad \text{find } \hl{theta}{\boldsymbol\theta_{1:K}}\ \text{ such that }\ \bigwedge_{k=1}^{K}\Big[\hl{kin}{\mathrm{Kin}(\mathbf q_k,o_k,p_k,g_k)}\wedge\hl{coll}{\mathrm{CFree}(\boldsymbol\tau_k,x_{k-1})}\wedge\hl{place}{p_k\in\mathcal R_k}\Big] $$

$\mathrm{Kin}$ 要求 $\mathbf q_k$ 的正运动学把夹爪送到 $p_k\cdot g_k$（物体位姿乘以抓取，FIG 4 的“够得着”）；$\mathrm{CFree}$ 要求第 $k$ 段轨迹 $\boldsymbol\tau_k$ 在当时的场景 $x_{k-1}$ 中不碰撞（FIG 2、FIG 4 的碰撞检查，轨迹本身来自 14.2 节的 RRT 或 14.3 节的优化）；$\mathcal R_k$ 是允许的摆放区域（FIG 2 的托盘）。关键在于同一个参数出现在多个约束里：抓取 $g$ 在 pick 时选定，却要在 place 时满足约束（FIG 6）；第一个盒子的位置 $p_1$ 会进入后面所有盒子的 $\mathrm{CFree}$（FIG 5）。这正是贪心会失败、需要回溯或联合优化的原因。

完整的循环（FIG 3）可以写成：

F ← ∅                                  # learned statements
loop:
    π ← TaskPlan(s0, G, F)             # shortest skeleton consistent with F
    if π = none: return FAIL
    θ, why ← Bind(π, budget)           # samplers + checks + backtracking
    if θ ≠ none: return (π, θ)
    F ← F ∪ Diagnose(why)              # e.g. blocks(B,T), noroom(T,tray | M)
    budget ← grow(budget)              # keep revisiting old skeletons with more samples

最后一行很重要：采样有限次失败并不能证明骨架不可行（也许只是没采到），所以完备的算法要不断回到旧骨架、给更多样本。若每个采样器在可行集上有正的概率（可行集有非零体积），这样的算法是概率完备的：有解时，找到解的概率随时间趋于 1，和 14.2 节的 RRT 一样。

PDDLStream（Garrett、Lozano-Pérez、Kaelbling，2020）把采样器写进 PDDL：一个 stream 声明输入、输出和它认证的命题，例如（由论文中的例子简化而来）

(:stream sample-grasp
  :inputs (?o)  :domain (Graspable ?o)
  :outputs (?g) :certified (Grasp ?o ?g))
(:stream inverse-kinematics
  :inputs (?o ?p ?g) :domain (and (Pose ?o ?p) (Grasp ?o ?g))
  :outputs (?q)      :certified (Kin ?o ?p ?g ?q))

它的“聚焦”算法先假设每个 stream 都能产生一个乐观的占位值，让符号规划器在这些占位值上规划，得到一个骨架；然后只调用这个骨架用到的 stream 去产生真实的值。被求值过的 stream 实例“层级”会升高，下一轮乐观规划不能再依赖同样的求值，规划器就转向别的选择；允许的层级随时间增大，所以每个实例以后还会被再次求值，不会被永久排除。这和 FIG 3 的循环是同一个思想：符号规划器决定先算哪些几何量，几何计算的结果再回流成命题。

逻辑-几何规划

LGP 把整个问题写成一个对骨架和轨迹的联合优化（Toussaint 2015）：离散状态 $s_k$ 决定在每段时间里哪些约束生效（例如“物体固定在夹爪上”），切换时刻的约束 $h_{\text{switch}}$ 描述动作本身（例如“在 $t_k$ 时夹爪到达抓取位姿”）：

$$ \min_{K,\;a_{1:K},\;x(t)}\ \int_0^T c\big(x,\dot x,\ddot x\big)\,dt\quad\text{s.t.}\quad \begin{cases} h_{\text{path}}(x(t),s_{k(t)})=0,\ \ g_{\text{path}}(x(t),s_{k(t)})\le0\\ \hl{kf}{h_{\text{switch}}\big(x(t_k),a_k\big)=0}\\ s_k=\mathrm{succ}(s_{k-1},a_k),\quad G\subseteq s_K\end{cases} $$

外层对骨架 $a_{1:K}$ 做树搜索，内层对每个骨架解一个非线性规划。完整轨迹的 NLP 很贵，所以 LGP 用一串由粗到细的“界”：先只解最终位姿，再解关键帧序列，最后才解整条轨迹；粗一级不可行的骨架直接剪掉。FIG 6 就是关键帧这一级：变量 $\mathbf z$（直接插入时 $\mathbf z=(s,p)$），关键帧 $\hl{kf}{\mathbf x_k}=C_k\mathbf z+\mathbf d_k$ 是 $\mathbf z$ 的仿射函数，

$$ \min_{\mathbf z\in\hl{feas}{\mathcal F_\pi}}\ f(\mathbf z)=\sum_{k=1}^{K+1}\big\|\mathbf x_k-\mathbf x_{k-1}\big\|^2,\qquad \hl{feas}{\mathcal F_\pi}=\{\mathbf z:\ A\mathbf z\le\mathbf b\},\qquad \mathbf x_0=\mathbf x_{K+1}=\mathsf S $$

直接插入骨架的约束是：抓取点在棒子上且不在横梁下 $s_{\min}\le s\le s_{\max}-fL$；插入深度 $x_o-(p-L/2)\ge d_{\min}$；棒子不顶穿槽底；夹爪留在槽外 $p+s\ge x_o+0.04$。最后一条把 $s$ 和 $p$ 耦合起来，正是它让贪心失败。增广拉格朗日法交替做两件事（6.7 节）：

$$ \mathbf z\leftarrow\arg\min_{\mathbf z}\ f(\mathbf z)+\frac{\rho}{2}\sum_i\max\!\Big(0,\ \mathbf a_i^\top\mathbf z-b_i+\frac{\lambda_i}{\rho}\Big)^2,\qquad \lambda_i\leftarrow\max\big(0,\ \lambda_i+\rho\,(\mathbf a_i^\top\mathbf z-b_i)\big) $$

内层问题是二次函数加单侧二次罚项，用几步牛顿法就能精确求解；外层把乘子 $\lambda_i$ 推向 KKT 条件里的值，$\rho$ 每次乘 3。FIG 6 的默认情况 6 次外层迭代后违反量就低于 $10^{-9}$ m。可行集为空时乘子和 $\rho$ 不断增长、违反量却停在一个正数上，这就是“这个骨架不可行”的信号。

向下精化与抽象的选择

如果每个符号上可行的骨架在几何上都能绑定（“向下精化性质”），TAMP 就退化成先规划、后填参数。现实里这几乎从不成立（FIG 2），于是所有 TAMP 方法都在回答同一个问题：怎样让几何的失败尽早、尽量具体地告诉符号层。符号抽象太粗，循环要转很多圈；太细（例如把格子化的位置都写成命题），符号搜索本身就爆炸。

7机器人中的它

真实系统里

研究系统：PDDLStream 是开源的 Python 库，用 Fast Downward 做符号搜索，自带 PyBullet 中的抓取、摆放、做饭与清洁等示例；Kaelbling 和 Lozano-Pérez 的 HPN（Hierarchical Planning in the Now，2011）在分层抽象上规划，只把眼前的一步细化到几何，执行中不断重规划；Dantam 等人（2016）把 TAMP 写成增量的约束求解（SMT）；Toussaint 组的 LGP 用 KOMO 优化器解关键帧和轨迹，用在工具使用等需要接触和物理的任务上。 工业与 ROS：MoveIt Task Constructor 把一个任务写成固定结构的“阶段”管线（生成抓取、逆运动学、在阶段之间用运动规划器连接），在每个阶段的多个候选解之间搜索：这相当于骨架由人写死、只做绑定的 TAMP。ROS 2 的 PlanSys2 用 PDDL 规划、用行为树执行动作。大多数已部署的系统仍然是手写的任务结构（状态机或行为树）加运动规划器，完整的 TAMP 主要用于研究和高度多变的任务。 大模型：近年有工作让语言模型提出高层计划（SayCan 把 LLM 对每个技能的打分与学到的“这个技能此刻能否成功”的估计相乘，Code as Policies 让 LLM 直接写机器人程序），或者让 LLM 把自然语言翻译成 PDDL 再交给经典规划器（LLM+P）；几何可行性仍要由本节这样的检查来保证。这部分在 Part VII 展开。

和前面各节的关系。TAMP 是一个“调用者”：符号层的搜索就是 14.1 节的 BFS / A*，只是节点变成了命题集合；动作之间的轨迹由 14.2 节的 RRT / PRM 或 14.3 节的轨迹优化来算；抓取候选可以来自 9.3 节的力封闭评估；可达性来自 7.5 节的逆运动学。一次 TAMP 求解里，运动规划器和碰撞检测往往被调用成千上万次，所以它们的速度直接决定 TAMP 的速度；PDDLStream 这类方法花很多心思在“少调用、晚调用”上。

8常见误区

误区 1：相信没有经过几何检查的符号计划

FIG 2 的两步计划在符号上无懈可击，在几何上却做不到。符号模型只是抽象，抽象丢掉的东西（遮挡、空间、可达性、稳定性）随时可能让某一步失败。计划的每一步都要绑定到具体参数并检查，执行时还要准备好重规划，因为感知到的几何本身就有误差。

误区 2：贪心地接受第一个可行的样本

FIG 5：即使单步采样器是完美的，贪心绑定在余量小时也会大概率失败，因为前面的连续选择决定了后面还剩什么。要么回溯（并给每层一个预算，避免在一个死胡同里无限采样），要么像 LGP 那样把整个骨架的约束一起优化，要么设计“为后面留余地”的采样器（例如靠边摆放），但这类启发式会牺牲完备性。

误区 3：失败时只返回“失败”

如果几何层只说“这个骨架不行”，符号层能做的只有把这个骨架拉黑，然后尝试下一个，而下一个往往因为同样的原因失败（比如换个地方放 T，但 B 依然挡着）。FIG 3 能两次迭代就成功，是因为失败被诊断成了具体的命题 blocks(B,T)，一次排除了一整类骨架。诊断太具体也不好：把“B 在 (0.31, 0.15) 时挡着 T”写成命题，B 稍微一动它就没用了。

误区 4：采样器只在“好看”的值里取

为了快，人们常把抓取限制在四个正方向、把摆放限制在区域中心。这在空旷场景里很有效，可 FIG 4 里被围住的罐子可能只剩一个窄窄的可行方向区间，四个正方向恰好全落在外面。采样器必须对整个可行集有正的概率，算法才可能概率完备；好的做法是“先试启发式的值，再退回到随机采样”，并且用约束感知的采样（例如先采抓取再解逆运动学，而不是在关节空间盲采）提高 $p$。

误区 5：把所有物体和动作都塞进符号层

状态数随物体数指数增长（FIG 1：3 块积木 22 个状态，4 块 125 个），厨房里几十个物体时符号搜索本身就会撑爆，而每个多余的骨架还要触发昂贵的几何检查。实际系统只把和目标相关的物体放进规划问题（挡路的物体通过失败反馈才被加进来，正如 FIG 3 里的 B 和 C），并用分层规划把长任务拆开（HPN）。

9小练习

1. 状态 $s=\{$on(A,B), ontable(B), clear(A), handempty$\}$。执行 unstack(A,B) 之后的状态是？

$\gamma(s,a)=(s\setminus\mathrm{del})\cup\mathrm{add}$：删除 on(A,B)、clear(A)、handempty，添加 holding(A)、clear(B)，ontable(B) 不受影响，得到选项 C。选项 A 忘了删除 clear(A)（被拿着的积木不算“上面空着”）也忘了添加 clear(B)；选项 B 没删除 on(A,B)；选项 D 是再执行一次 putdown(A) 之后的状态。

2. 在 FIG 2 的默认场景里，符号规划器给出 pick(T) → place(T,tray)，而夹爪通道被 B 挡住。最有用的做法是？

挡路是场景的事实，不是采样运气：只要 B 不动，pick(T) 的任何参数都不行（排除 A）。只拉黑骨架，下一个骨架（比如把 T 放到边桌）同样要先 pick(T)，还会失败（排除 C）。运动规划器只在一个固定的“模式”里找路径，它不会自己产生“先挪 B”这种离散决策（排除 D）。具体的失败命题让符号层一次绕开整类失败，这正是 FIG 3 的做法。

3. FIG 5 里自由空间采样器从不产生碰撞的位置，为什么贪心绑定仍然会失败？

自由空间采样器的每个样本都可行，所以 $K$ 再大，贪心也只会用第一个样本（排除 B）；隔板长度 = 盒宽之和 + 余量，总是放得下（排除 C）。失败来自参数之间的耦合：第三个盒子的可行集依赖前两个盒子的位置，可能已经是空集。回溯回到前面换位置，所以能成功。

4. 目标是 $G=\{$on(A,B), on(B,C)$\}$。为什么 $h(s)=|G\setminus s|$ 是可采纳的启发式？

可采纳 = 从不高估剩余代价。每个缺失的 on 目标至少需要一个不同的 stack，所以 $h$ 是下界。它远小于真实代价：Sussman 初始状态 $h=2$，而最优计划要 6 步（排除 B）。积木世界当然有删除表（排除 C）；BFS 不用启发式（排除 D）。

5. FIG 6 默认设置下贪心失败、联合优化成功，原因是？

约束 $p+s\ge x_o+0.04$ 把两个动作的参数耦合起来。贪心先按“离起点最近”选 $s=-13$ cm，此后不存在满足插入深度的 $p$；联合优化在可行集 $\mathcal F_\pi$ 上取最小，落在 $s=7$ cm、$p=17$ cm 的角点上。默认设置下这个骨架是可行的（排除 D），两者的代价函数相同（排除 B）。

10延伸阅读

R. E. Fikes & N. J. Nilsson, STRIPS: A New Approach to the Application of Theorem Proving to Problem Solving, Artificial Intelligence 2(3–4), 1971 — 前提 / 添加 / 删除表的出处
M. Ghallab, D. Nau & P. Traverso, Automated Planning: Theory and Practice, Morgan Kaufmann, 2004 — 经典规划、PDDL、启发式搜索的教科书
J. Hoffmann & B. Nebel, The FF Planning System: Fast Plan Generation Through Heuristic Search, JAIR 14, 2001; M. Helmert, The Fast Downward Planning System, JAIR 26, 2006 — 松弛计划启发式与现代符号规划器
L. P. Kaelbling & T. Lozano-Pérez, Hierarchical Task and Motion Planning in the Now, ICRA 2011 — 分层、边执行边细化的 TAMP
S. Srivastava, E. Fang, L. Riano, R. Chitnis, S. Russell & P. Abbeel, Combined Task and Motion Planning Through an Extensible Planner-Independent Interface Layer, ICRA 2014 — 把几何失败翻译成命题反馈给任意符号规划器
M. Toussaint, Logic-Geometric Programming: An Optimization-Based Approach to Combined Task and Motion Planning, IJCAI 2015 — LGP 的原始论文
N. T. Dantam, Z. K. Kingston, S. Chaudhuri & L. E. Kavraki, Incremental Task and Motion Planning: A Constraint-Based Approach, RSS 2016 — 用增量 SMT 求解做 TAMP
C. R. Garrett, T. Lozano-Pérez & L. P. Kaelbling, PDDLStream: Integrating Symbolic Planners and Blackbox Samplers via Optimistic Adaptive Planning, ICAPS 2020 — stream、乐观规划与聚焦算法
C. R. Garrett, R. Chitnis, R. Holladay, B. Kim, T. Silver, L. P. Kaelbling & T. Lozano-Pérez, Integrated Task and Motion Planning, Annual Review of Control, Robotics, and Autonomous Systems 4, 2021 — 全面的综述，从这里入门