TAMP_T4 逻辑-几何规划 LGP (Logic-Geometric Programming)¶

难度: ⭐⭐⭐ ~ ⭐⭐⭐⭐ (本章是 TAMP 优化式范式的纵深，整体偏进阶，含较多优化数学) 前置知识: TAMP_T1（TAMP 核心挑战、LGP 入门、采样运动规划）、TAMP_T3（PDDLStream，本章的对照范式）、轨迹优化基础（代价函数、约束、梯度下降，最好接触过 MPC 或 iLQR）、一阶逻辑 核心参考: Toussaint (2015, IJCAI), "Logic-Geometric Programming: An Optimization-Based Approach to Combined Task and Motion Planning"; Toussaint (2014), "KOMO: Newton Methods for k-Order Markov Constrained Motion Problems"; Toussaint & Lopes (2017, ICRA), "Multi-Bound Tree Search for Logic-Geometric Programming in Cooperative Manipulation Domains" 与既有章节的关系: 本章是 TAMP_T3（PDDLStream，流式/采样范式）的并列对照——缝合符号-几何鸿沟的另一条主流路：优化式。TAMP_T3 §10.1 已埋下"采样 vs 优化"的分野，本章把优化式这一支讲透。它也深化 TAMP_T1 §5 当入门讲过的 LGP。

0. 前置自测¶

开始本章之前，请完成下面五道自测题。它们检验本章依赖的四块基础——TAMP 核心难题（符号-几何鸿沟）、轨迹优化（代价、约束、求解）、PDDLStream（本章的对照范式），以及 优化数学（雅可比、最小二乘、Newton 法、拉格朗日乘子——§4 的硬核推导会密集用到）。如果两道以上答不出来，先回对应前置章节。

#	问题	期望掌握程度	答不出来回到
Q1	TAMP 的"符号-几何鸿沟"指什么？为什么任务层（离散）和运动层（连续）不能简单解耦？	能说出离散命题 vs 连续几何，动作可行性依赖几何	TAMP_T1 §2.5
Q2	轨迹优化把"找一条好轨迹"表达成什么数学问题？代价函数（cost）和约束（constraint）各起什么作用？	能说出"带约束的最优化"，代价定义好坏、约束定义可行	轨迹优化基础、MPC 入门
Q3	PDDLStream（TAMP_T3）如何处理连续几何参数？它的核心机制 Stream 是什么？	能说出用采样器按需生成、认证事实反馈给符号搜索	TAMP_T3 §3-§5
Q4	一个"抓取位姿"是 6 个连续自由度。如果不用采样（PDDLStream 的办法），还能怎么确定它的具体值？	能想到"把它当优化变量，让优化器解出来"——这正是本章的思路	TAMP_T1 §5（LGP 入门）
Q5	这些概念你熟悉吗：函数 $\phi(x)$ 的雅可比 $J=\partial\phi/\partial x$；最小二乘（极小化平方和 $\\|\phi\\|^2$）；Newton 法的迭代思想；用拉格朗日乘子处理等式约束？	能说出各自大意（§4.6–§4.7 会从这些出发推导，不熟可边读边补）	任意数值优化教材；§4 延伸阅读（Nocedal & Wright）

参考答案 (点击展开)

**Q1**: 符号-几何鸿沟指任务层用**离散命题**（`On(cup, table)`）描述世界，运动层用**连续向量**（关节角、位姿）描述世界。不能解耦是因为动作的**可行性依赖几何**——"把杯子放到架子上"这个符号动作能否执行，取决于机械臂够不够得到、放上去稳不稳，这些是几何问题。符号层若看不见几何，就会规划出几何上不可行的计划。 **Q2**: 轨迹优化把"找一条好轨迹"表达成一个**带约束的最优化问题**：决策变量是轨迹（一串构型或控制量），**代价函数**衡量轨迹的好坏（如总能量、平滑度、到目标的距离），**约束**定义什么轨迹是可行的（如不碰撞、满足动力学、关节限位）。求解就是在满足所有约束的前提下，找使代价最小的轨迹。 **Q3**: PDDLStream 用 **Stream**（条件化的采样器）按需生成连续参数（抓取位姿、IK 构型、运动路径），并把"这个值满足某约束"以**认证事实**的形式反馈给符号搜索，让符号搜索和几何采样交替进行。核心是"采样 + 搜索"的循环。 **Q4**: 不用采样，可以把抓取位姿当成**优化问题的决策变量**——和轨迹一起，作为连续变量交给优化器，让优化器在满足约束（够得到、不碰撞、抓得稳）的前提下解出它的具体值。这就是 LGP 的思路：**不采样、而是优化**。把"该抓哪、放哪、怎么动"全部写进一个大的优化问题联立求解。这正是本章与 PDDLStream 的根本分野。 **Q5**: 这些是 §4 推导的工具箱。**雅可比** $J=\partial\phi/\partial x$ 是向量值函数 $\phi$ 对自变量的偏导矩阵（第 $i$ 行是 $\phi_i$ 的梯度）。**最小二乘**是极小化残差平方和 $\|\phi(x)\|^2$ 的一类问题，KOMO 的代价正是这种形式（§4.6）。**Newton 法**用二阶信息迭代逼近最优，每步解一个线性系统、更新、再线性化（§4.3）。**拉格朗日乘子**把等式约束 $g(x)=0$ 并进目标（$+\lambda^\top g$），用乘子刻画约束的"影子价格"，增广拉格朗日在此基础上再加罚项（§4.7）。这四样不熟也没关系——§4.6–§4.12 会从头推，配合延伸阅读的 Nocedal & Wright 边读边补即可。

1. 本章目标¶

学完本章后，你应当能够：

解释 LGP（逻辑-几何规划）的核心思想：把符号动作序列的选择与连续轨迹的优化，统一进一个混合优化问题，用连续优化（而非采样）确定几何参数。
写出 LGP 的数学形式：决策变量（轨迹 + 动作参数）、代价函数（路径代价 + 末态评价）、以及逻辑如何生成约束（切换一致性约束、依赖符号状态的几何约束）。
描述并推导 KOMO（k-order Markov 优化）如何求解 LGP 的几何层：把轨迹离散化、用 k 阶 Markov 结构表达代价与约束；推出 Gauss-Newton 的正规方程、理解增广拉格朗日如何用乘子让有限罚权重也能精确满足硬约束、说明带状结构为何让求解关于时间步 $T$ 线性。
理解 LGP 的求解架构——符号层的树搜索 + 几何层的轨迹优化，以及 multi-bound tree search 如何用多层下界（P1/P2/P3）高效剪枝。
对比 LGP 与 PDDLStream（TAMP_T3）：采样 vs 优化、各自擅长的问题类型（组合摆放 vs 接触丰富）、为什么 LGP 特别适合工具使用与富接触操作。
判断何时该用 LGP、何时该用 PDDLStream，并了解二者结合的前沿方向。

本章知识导航¶

本章回答一个问题：不用采样，如何让一个优化器同时决定"做什么动作"和"怎么动"？ TAMP_T3 用采样（Stream）缝合符号-几何鸿沟，本章用优化——把整个任务-运动问题写成一个带约束的大优化问题，逻辑负责生成约束，优化负责求解几何。整章围绕"逻辑生成约束、优化求解几何"这一核心展开：

                  根问题：不用采样，如何联立决定"做什么"和"怎么动"？
                                    │
            ┌───────────────────────┼───────────────────────┐
            │                       │                       │
       【是什么】                【怎么求解】              【怎么用/对比】
            │                       │                       │
   §2 从采样到优化的转向        §4 KOMO:几何层求解器       §7 LGP vs PDDLStream
   ├─ §3 已讲为何要另一条路     ├─ 轨迹离散化+k阶Markov     ├─ 采样 vs 优化的本质
   └─ 优化式的动机             └─ Newton 类方法求解            │
            │                        │                    §8 真实应用
   §3 LGP 核心:逻辑生成约束     §5 求解架构:树搜索+优化    ├─ 工具使用/富接触/序列操作
   ├─ 动作参数=优化变量        ├─ 符号树 + 几何优化            │
   ├─ 切换一致性约束           ├─ multi-bound 三层下界    §9 局限与前沿
   └─ 依赖符号状态的几何约束    └─ P1/P2/P3 剪枝           ├─ 局部最优/初值/求解慢
            │                        │                    └─ 与学习/PDDLStream 结合
            └────────────────────────┴── §6 完整案例:搭积木塔/工具使用

怎么读这张图：左列建立"为什么用优化、LGP 的核心结构"（§2-§3），中列是本章硬核——KOMO 几何层求解（§4）与树搜索 + 优化的求解架构（§5）。右列是对比与应用（§7 与 PDDLStream 对照、§8 应用、§9 局限）。⭐⭐⭐ 的 §3（LGP 核心）和 §5（求解架构）是必须掌握的主干；⭐⭐⭐⭐ 的 §4（KOMO 数学）是进阶。

主干与分支：第一遍务必拿下 §3（LGP 把什么写进优化、逻辑如何生成约束）、§5（树搜索 + 优化的两层架构、multi-bound 思想）、§7（与 PDDLStream 的对比）。§4 的 KOMO 优化细节可第二遍深入——其中 §4.1–§4.5 是概念骨架（必读），§4.6–§4.10 是数学内核（Gauss-Newton 正规方程、增广拉格朗日、带状复杂度、雅可比推演、与 iLQR 等的对比），想吃透 KOMO 或将来自己实现/调参时再精读。

前置知识桥接¶

本章站在三块基石上，各用几行重新激活。

回顾 TAMP_T1 §2.5 与 §5：符号-几何鸿沟，LGP 入门。 TAMP_T1 揭示了符号-几何鸿沟，并在 §5 把 LGP 当作缝合鸿沟的一种范式做过入门介绍——提到它是 Toussaint 的工作、用 KOMO 联合优化。本章把这个入门展开成"原理 + 数学 + 求解架构"，让你不仅知道 LGP 是什么，还理解它为什么这样设计、内部怎么求解。

回顾 TAMP_T3：PDDLStream 与"采样 vs 优化"的分野。 TAMP_T3 讲了 PDDLStream——用采样器（Stream）按需生成几何参数，符号搜索和采样交替。其 §10.1 明确埋下了对照：缝合符号-几何鸿沟有两条主流路，③流式（采样，PDDLStream）和②优化式（LGP，本章）。当时给的判断是"选择型约束（抓放摆）用 PDDLStream，调优型约束（推倒插、力控）用 LGP"。本章把优化式这一支讲透，并在 §7 完成与 PDDLStream 的系统对比。

激活轨迹优化的基础。 本章把任务-运动问题表达成带约束的最优化。你需要熟悉：决策变量、代价函数（衡量好坏）、约束（等式/不等式，定义可行域）、以及梯度类求解（Newton 法、序列二次规划 SQP 的思想）。如果你学过 MPC（模型预测控制）或 iLQR，那么 KOMO 会很亲切——它本质就是一个把轨迹当变量、用 Newton 法求解的带约束轨迹优化器。

本质洞察：TAMP_T3 和本章是一对"双子章"——同一个符号-几何鸿沟，两种截然不同的缝合哲学。PDDLStream 说"几何参数太多太连续，我采样出一些候选，让符号搜索挑"；LGP 说"几何参数是连续的，那就别采样，直接把它们当优化变量，连同轨迹一起解出最优"。读这两章的正确方式，是始终把它们对照着看——每学 LGP 的一个设计，就回想 PDDLStream 在同一问题上怎么做。理解了这对分野，你就掌握了 TAMP 集成的两大范式，能按问题特性正确选型（§7 的核心）。

如果跳过本章会怎样¶

场景一（只学了 PDDLStream）：你以为 TAMP 就是"采样 + 符号搜索"。遇到工具使用、推、倒、铰接物体操作这类接触丰富、几何量需要连续调优的任务，你会发现 PDDLStream 的采样很别扭——采一百个抓取位姿可能没一个力控得恰到好处。你不知道这类问题正是 LGP（优化式）的主场。缺了本章，你的 TAMP 工具箱只有一半。
场景二（做长序列操作）：你需要规划"搭一座 5 层积木塔"这类长序列、且每步几何强耦合（下层没放稳上层就塌）的任务。PDDLStream 的逐步采样在这种全局几何耦合下很吃力，而 LGP 把整个序列写进一个优化问题、全局求解的能力正适合。不懂 LGP，你会在该用优化的地方硬用采样。
场景三（想理解 TAMP 的优化视角）：现代 TAMP 与轨迹优化、最优控制、甚至扩散模型规划（生成式）的联系，都建立在"把规划看成优化"这个视角上。LGP 是这个视角在 TAMP 的奠基性工作。跳过它，你会缺少理解这一大类方法的根基。

预计阅读时间¶

模式	覆盖范围	预计时间
精读	全部小节 + KOMO 数学推导（§4.6–§4.10）+ §6 完整案例 + 练习	16-20 小时
速读	§2/§3/§5 主干 + 跳过 §4 KOMO 数学细节	5-6 小时
实战	§3 LGP 结构 + §5 求解架构 + §7 与 PDDLStream 选型 + §8 应用	6-8 小时
速查	知识导航 + §3 核心结构 + §5 multi-bound + §7 对比表	1.5 小时

2. 从采样到优化：另一条缝合鸿沟的路 ⭐⭐⭐¶

2.1 回到那个连续参数难题¶

TAMP_T3 §2.1 用一个最小例子立住了符号-几何鸿沟：机械臂把杯子从桌上抓起、放到架子上。符号上是 pick(cup) 然后 place(cup, shelf)，但每个动作背后藏着连续参数——抓取位姿 $g$、抓取构型 $q_1$、放置位姿 $p$、放置构型 $q_2$、运动轨迹 $\tau$，而且它们彼此耦合（$g$ 决定 $q_1$，$q_1$ 约束 $q_2$……）。

TAMP_T3 的回答是采样：用 Stream 按需生成这些参数的候选值，让符号搜索挑。本章给出另一个回答：把这些参数全部当成优化变量，连同轨迹一起，写进一个带约束的大优化问题，让优化器一次性解出最优的一组。

同一个难题, 两种回答:
  PDDLStream(T3): 几何参数 g,q1,p,q2,τ 连续无穷 → 采样出候选, 符号搜索挑
  LGP(本章):      几何参数 g,q1,p,q2,τ 连续 → 当优化变量, 优化器解出最优
                  "做什么动作"(离散) + "参数取什么值"(连续) 联立成一个优化问题

2.2 为什么需要优化式这条路：采样的软肋¶

PDDLStream 的采样范式很强，但有两类问题它天生别扭，正是这两类问题催生了优化式：

软肋一：需要"连续调优"的几何量。 采样擅长"从候选里选一个就行"的离散选择——抓哪个位姿、放哪个位置。但有些几何量的好坏是连续的程度问题：倒水的倾角（倒多少、洒不洒）、推动的力度与接触点、插入的对齐精度、轨迹的平滑度与能耗。对这些，"采一百个候选挑一个"远不如"用梯度连续地调到最优"。采样是离散地撞，优化是连续地调。

软肋二：全局耦合的长序列。 搭一座 5 层积木塔——下面的积木放得稍偏，上面就会塌。这种整个序列的几何强耦合下，PDDLStream 逐步采样、逐步验证的方式很吃力（前面采的值要等后面才知道好不好）。而优化式把整个序列的所有构型、所有动作参数写进一个优化问题，全局地协同求解——它"一眼看到底"，能让第 1 层的放置就考虑到第 5 层的稳定。

本质洞察：采样与优化处理连续量的哲学截然不同——采样是"离散地枚举可能性"，优化是"连续地改进一个解"。这对应了整个规控领域反复出现的分野：你在运动规划里见过 RRT（采样）vs CHOMP/TrajOpt（优化），在 MPC 里见过 MPPI（采样）vs iLQR（梯度优化）。LGP vs PDDLStream 是这条分野在 TAMP 层的体现。判断该用哪个，问一句：你的几何难点是"选择"还是"调优"？ 选择型（离散候选够用）→ 采样（PDDLStream）；调优型（需要连续地精调力、角度、平滑度）→ 优化（LGP）。这个判断会贯穿本章，并在 §7 系统化。

2.3 优化式的代价：天下没有免费的午餐¶

优化式不是万能的，它的能力有对应的代价，先说清楚，免得你以为它处处优于采样：

非凸与局部最优：把动作参数 + 轨迹写进一个优化问题，这个问题通常是高度非凸的（碰撞约束、接触切换都非凸）。优化器只能找到局部最优，初值不好就陷在坏解里。采样则没有"局部最优"困扰（它是全局撒点）。
需要好的初值与建模：优化对初值敏感，且要求把约束写成可微（或可处理）的形式。建模一个新领域的 LGP 约束，比写 PDDLStream 的 Stream 更需要优化经验。
离散结构仍需搜索："做什么动作序列"这个离散选择，优化处理不了——LGP 仍然需要一个符号层的树搜索来枚举动作骨架，几何优化只解决"给定骨架后参数取什么"。所以 LGP 不是"纯优化"，而是"树搜索 + 优化"（§5）。

本质洞察：采样和优化各有不可消除的代价——采样的代价是"离散化的粗糙 + 盲目"，优化的代价是"非凸局部最优 + 初值敏感"。没有哪个范式免费地更优，这正是 TAMP 至今仍是开放问题、且前沿在探索二者结合（§9）的根本原因。本章讲透优化式，不是要你抛弃 PDDLStream，而是给你工具箱里补上对称的另一半——遇到调优型、强耦合的问题时，你知道该换 LGP。

⚠️ 常见陷阱¶

陷阱 2-1（思维陷阱）：以为优化式 LGP 总比采样式 PDDLStream 更"高级"。 - 错误描述：看到"联立优化"听起来更统一更优雅，就认为 LGP 处处优于 PDDLStream。 - 现象/后果：在"组合摆放"这类离散选择为主的问题上硬用 LGP，陷入非凸优化的局部最优与初值困扰，反而不如 PDDLStream 采样来得干脆。 - 根本原因：采样和优化各有主场（§2.2、§2.3）。"统一优雅"不等于"实际更好"。 - 正确做法：按几何难点是"选择型"还是"调优型"选（§2.2 本质洞察）。组合摆放→PDDLStream；富接触/调优/强耦合长序列→LGP。

陷阱 2-2（概念误区）：以为 LGP 是"纯优化"，不需要符号搜索。 - 错误描述：把 LGP 理解成"把整个 TAMP 写成一个优化问题然后一键求解"。 - 现象/后果：困惑于"离散的动作选择怎么用连续优化做"，或试图把动作选择也编码成连续变量导致问题爆炸。 - 根本原因：动作序列是离散的，优化处理不了离散组合选择。LGP 是"符号树搜索枚举骨架 + 几何优化求解参数"两层（§5）。 - 正确做法：理解 LGP 的两层结构——逻辑层搜索动作序列，几何层对每个候选序列做轨迹优化。优化只负责"给定序列后的连续部分"。

练习¶

（⭐⭐） 对 TAMP_T3 §2.1 的 pick-and-place，分别用一句话描述 PDDLStream 和 LGP 会怎么确定"放置位姿 $p$"。指出二者在"如何对待连续参数"上的根本区别。
（⭐⭐⭐） 举一个"调优型"几何难点的具体任务（如倒水、插销），说明为什么采样一百个候选不如用优化连续地调。再举一个"选择型"的任务，说明为什么那里采样反而更合适。
（⭐⭐） §2.3 说 LGP 有"局部最优 + 初值敏感"的代价。结合你对轨迹优化（或 MPC/iLQR）的了解，解释为什么"把碰撞约束写进优化"会让问题非凸。

3. LGP 的核心：逻辑生成约束，优化求解几何 ⭐⭐⭐¶

§2 讲了"为什么用优化"。本节讲 LGP 到底怎么把任务-运动问题写成一个优化问题——这是全章的概念核心。理解一句话：逻辑负责生成约束，优化负责求解几何。

3.1 LGP 是什么：一句话与一张图¶

LGP（Logic-Geometric Programming，逻辑-几何规划）由 Toussaint 在 2015 年提出。它的优化目标是路径上的典型最优控制目标——running cost 加末构型的评价 ψ——但这个优化受一系列等式和不等式约束的约束；逻辑的作用，就是定义这些约束。

一句话：LGP 把"机器人要做的整个操作序列"写成一个带约束的轨迹优化问题，其中"做什么动作"由符号逻辑决定，而逻辑的每个决策都翻译成施加在连续轨迹上的约束。

LGP 的基本结构:
  决策变量: 整条轨迹 x(t) + 各动作的几何参数(抓取/放置位姿等)
  目标:     最小化 路径代价(平滑/能量) + 末态评价 ψ(到达目标)
  约束:     由逻辑(符号动作序列)生成 ——
            ① 切换一致性约束: 动作切换处的构型要与动作算子一致
            ② 几何/微分约束: 每段轨迹要满足当前符号状态对应的几何约束
                            (不碰撞、抓握保持、支撑稳定...)
  求解:     符号层搜索动作序列 → 每个序列生成一组约束 → 几何层优化轨迹

3.2 关键一步：动作参数成为优化变量¶

LGP 与 PDDLStream 最根本的区别，藏在"动作参数怎么确定"这一步。

PDDLStream 用 Stream 采样抓取位姿 $g$、放置位姿 $p$。LGP 的做法完全不同——它把这些参数作为优化问题的决策变量，让优化器解出来。Toussaint & Lopes 的工作把这一步讲得很清楚：引入物体与桌面之间的关节（平面平移和旋转）等新自由度，这些新自由度代表经典意义上的"动作参数"——抓取或放置的几何参数；把它们作为有效自由度引入运动学，就能与整体路径一致地、联合地优化它们。

动作参数 = 优化变量(LGP 的精髓):
  PDDLStream: 抓取位姿 g 由 sample-grasp 采样 → 离散候选
  LGP:        抓取位姿 g 是优化变量 → 与轨迹 x(t) 一起被优化器解出
              "抓哪里"不是采来的, 是优化出来的(在满足够得到/不碰/稳定约束下最优)

本质洞察：把动作参数变成优化变量，是 LGP 全部威力的来源。这样做的深远后果是：抓取位姿、放置位姿、轨迹，全都在同一个优化问题里被同时、协调地决定——优化器选抓取位姿时，"知道"这个抓取会让后续的放置和轨迹付出多大代价，于是能选一个全局最优的。这正是 §2.2"全局耦合长序列"软肋的解药：采样是局部地、一个个地定参数，优化是全局地、一起定所有参数。代价（§2.3）是这个联合问题非凸、难解——但对强耦合问题，这个代价值得付。

3.3 逻辑如何生成约束：两类约束¶

LGP 的名字里"逻辑"和"几何"如何接合？答案是：符号动作序列的每个决策，都翻译成施加在连续轨迹上的约束。 Toussaint 2015 的形式化里，约束分两类。

第一类：切换一致性约束（kinematic switch consistency）。 这类约束关乎切换构型 $x(t_k)$ 与动作算子 $a_k$ 的一致性。直观说：当符号上发生一个动作切换（如 pick 让杯子从"在桌上"变为"在手上"），切换那一刻的几何构型必须与这个动作一致——抓取发生时，手的位姿必须恰好在杯子的抓取位姿上，杯子的运动学从此挂接到手上。

第二类：依赖符号状态的几何约束。 这类约束在路径 $x(t)$ 上施加（几何与微分）约束，依赖于当前的符号状态 $s_k(t)$——其中 $k(t)$ 标记当前时刻属于哪个符号阶段。直观说：在"手持杯子移动"这个符号阶段，整段轨迹都要满足"杯子跟着手一起动、不洒、不碰"的几何约束；切换到"杯子已放下"阶段，约束就变成另一组。

逻辑 → 约束 的翻译(一个 pick-place 序列):
  符号序列:  [初始] --pick--> [持物] --place--> [放下]
                     ↑切换            ↑切换
  切换约束:  pick 时刻: 手位姿 = 杯抓取位姿(挂接)
            place 时刻: 杯位姿 = 目标放置位姿(脱接, 且稳定支撑)
  阶段约束:  [初始]段: 手空自由移动, 不碰障碍
            [持物]段: 杯随手动(运动学挂接), 整段不碰、不洒
            [放下]段: 手离开, 杯静止于支撑面
  —— 同一条轨迹 x(t), 不同时间段受不同约束, 约束由符号阶段决定

本质洞察：LGP 的精妙在于把"离散的符号决策"转化为"连续轨迹上的分段约束"。符号序列不是和几何并列的另一个东西，而是几何优化问题的"约束生成器"——选定一个动作序列，就等于选定了一组施加在轨迹不同时间段上的约束。这与 PDDLStream 形成镜像对比：PDDLStream 里符号谓词由几何采样器认证（几何 → 符号），LGP 里符号动作生成几何约束（符号 → 几何）。两者都在符号与几何间架桥，但信息流向和接合方式相反——这是理解两个范式差异的最深一层。

3.4 LGP 的完整数学形式（选读）¶

把前面的话写成数学。LGP 求解的优化问题大致形如（简化自 Toussaint 2015）：

\[ \min_{x(t),\, a_{1:K}} \quad \int_0^T c(x(t), \dot{x}(t), \ddot{x}(t))\, dt \;+\; \psi(x(T)) \]

约束（受逻辑 $a_{1:K}$ 支配）：

\[ \begin{aligned} &\text{切换一致性:} && h_{\text{switch}}(x(t_k), a_k) = 0, && k=1,\dots,K \\ &\text{阶段几何约束:} && g_{\text{path}}(x(t), s_k) \le 0, && t \in [t_k, t_{k+1}] \\ &\text{动力学/运动学:} && \text{(KOMO 的 k 阶 Markov 结构, 见 §4)} \end{aligned} \]

其中：$x(t)$ 是构型轨迹（含动作参数作为有效自由度），$a_{1:K}$ 是符号动作序列（离散，由树搜索枚举），$c$ 是路径运行代价（平滑/能量），$\psi$ 是末态评价（衡量是否达成目标），$h_{\text{switch}}$ 是切换一致性约束，$g_{\text{path}}$ 是依赖符号状态 $s_k$ 的几何约束。

读这个式子的关键：外层 $\min$ 同时对连续的 $x(t)$ 和离散的 $a_{1:K}$ 优化。但离散的 $a_{1:K}$ 不能用梯度，所以实际求解是分层的——树搜索枚举 $a_{1:K}$，对每个固定的 $a_{1:K}$，剩下的就是一个纯连续的带约束轨迹优化（这部分交给 KOMO，§4）。§5 详解这个分层求解。

3.5 逻辑层：符号动作如何定义¶

前面反复说"符号动作序列生成约束"，但符号动作本身怎么定义？这是逻辑层的细节，也是 LGP 与经典符号规划（PDDL）的接合点。

LGP 的符号动作 = 前提 + 效果 + 它引入的几何约束/自由度。 和 PDDL 的动作类似（前提、效果），但多了一层"几何附着"——每个符号动作还规定它在切换点和后续阶段施加哪些几何约束、引入哪些有效自由度。以 grasp 为例：

LGP 的符号动作 grasp(robot, obj) 的三个部分:
  ① 符号前提(像 PDDL): (hand-empty robot) ∧ (reachable robot obj)
  ② 符号效果(像 PDDL): (holding robot obj) ∧ ¬(hand-empty robot)
  ③ 几何附着(LGP 特有):
     - 切换约束: 切换时刻 手末端位姿 = obj 的某抓取位姿(引入抓取位姿作有效自由度)
     - 运动学变化: obj 从"固定在世界/桌面"改为"挂接到手"(kinematic switch)
     - 后续阶段约束: 持物期间 obj 随手刚性运动, 整体不碰撞

第③部分是 LGP 区别于纯 PDDL 的关键——PDDL 的动作只改符号状态，LGP 的动作额外触发运动学结构变化和几何约束。这正是"逻辑-几何"名字的由来：每个逻辑动作都有几何后果。

符号状态包含"运动学模式"。 LGP 的符号状态不只是命题集合，还隐含当前的运动学模式（kinematic mode）——哪些物体挂接在哪、哪些是自由的。grasp 把杯子的运动学从"挂接到桌面"切换到"挂接到手"，place 再切回去。约束关乎切换构型与动作算子的一致性——每次模式切换都要保证构型一致（抓取时手恰好在抓取位姿、放置时杯恰好在放置位姿且稳定支撑）。

符号状态的运动学模式(以 pick-place 为例):
  初始: cup 挂接 table (自由放置)
  --grasp--> 模式切换: cup 挂接 hand (随手动)        ← 运动学结构变了!
  --place--> 模式切换: cup 挂接 shelf (新的支撑)
  每次切换 = 一个 kinematic switch, 对应一组切换一致性约束

本质洞察：LGP 的逻辑层与 PDDL 的关系，是"继承 + 几何扩展"——它沿用 PDDL 的前提/效果来描述符号状态转移（这部分可以用现成的符号规划器搜索，如 §5 树搜索、Diverse-LGP 用 off-the-shelf 规划器），但给每个动作附加了"几何后果"：运动学模式切换、切换一致性约束、阶段几何约束。理解这一点，就理解了 LGP 为何能同时利用符号规划的成熟成果（树搜索、启发式）和优化的几何能力——它不是抛弃 PDDL 另起炉灶，而是在 PDDL 的符号骨架上"挂"几何约束。这与 PDDLStream "在 PDDL 上加 Stream"是平行的思路（TAMP_T3 §4）：两个范式都选择扩展 PDDL 而非替代它，因为 PDDL 的符号搜索基础设施太有价值。差别在于挂什么——PDDLStream 挂采样器（Stream），LGP 挂几何约束。

3.6 LGP 的优化视角：双层结构与混合整数非线性规划（选读）⭐⭐⭐⭐¶

§3.4 写出了 LGP 的优化式，并说"外层 $\min$ 同时对连续 $x(t)$ 和离散 $a_{1:K}$ 优化，但实际分层求解"。本节把这个"分层"形式化——它揭示 LGP 在最优化理论里的真实身份，也解释了 §5 的求解架构为什么必然长成那样。

LGP 本质是一个混合整数非线性规划（MINLP）。 回顾 §3.4 的优化式：决策变量里，轨迹 $x(t)$（含动作参数）是连续的，动作序列 $a_{1:K}$ 是离散的（每个 $a_k$ 从有限的动作算子集合里取）。"同时优化连续变量 + 离散变量、且目标/约束非线性"——这在最优化里有个标准名字：混合整数非线性规划（Mixed-Integer Nonlinear Program, MINLP）。MINLP 是出了名的难（NP-hard：离散部分组合爆炸 × 连续部分非凸），没有通用的高效精确解法。LGP 不去硬解这个 MINLP，而是利用它的结构做双层分解。

双层分解：外层离散、内层连续。 关键观察：离散变量 $a_{1:K}$ 一旦固定，剩下的就是一个纯连续的非线性规划（NLP）——即 §3.4 那个对 $x(t)$ 的带约束轨迹优化（交给 §4 的 KOMO）。于是可把 MINLP 写成双层（bilevel）形式：

\[ \underbrace{\min_{a_{1:K}}\; V(a_{1:K})}_{\text{外层：离散，树搜索}}, \qquad \underbrace{V(a_{1:K}) = \min_{x(t)}\Big[\textstyle\int c\,dt + \psi(x(T))\Big]\ \text{s.t. } a_{1:K}\text{ 生成的约束（§3.3）}}_{\text{内层：连续 NLP，KOMO 求解}} \]

其中内层的值函数 $V(a_{1:K})$ 是"给定这个动作序列后，最优轨迹的代价"；若该 NLP 不可行（几何上走不通），记 $V(a_{1:K}) = +\infty$。外层就在所有动作序列里，挑使 $V$ 最小（且有限）的那个。

为什么这个结构决定了 §5 的架构。 双层分解一旦写出，§5 的"树搜索 + 优化"就是它的字面实现，一一对应：

外层 $\min$ over $a_{1:K}$ ＝符号树搜索（§5.1）——枚举离散动作序列；
内层 $V(a_{1:K})$ ＝对每个序列解一个 KOMO（§4）；
$V(a_{1:K}) = \infty$（内层 NLP 不可行）＝几何不可行 → 剪枝（§5.1 的可行性反馈）；
multi-bound（§5.3）＝给 $V$ 造廉价下界 $V_{\text{P1}} \le V_{\text{P2}} \le V_{\text{P3}} = V$，用便宜的下界提前判定 $V=\infty$。

换句话说，§5 不是"一种可能的求解法"，而是这个双层结构几乎唯一自然的实现——离散外层只能搜，连续内层才能优化。

与 TAMP 大图景的接口。 这个 MINLP/双层视角把 LGP 接回 TAMP 的本质：TAMP 就是"离散任务 + 连续运动"的耦合（TAMP_T1 §2.5 的符号-几何鸿沟），而"离散 + 连续耦合优化"正是 MINLP。PDDLStream（T3）也在解同一个 MINLP，只是它用采样把连续部分离散化、归约成纯离散搜索（T3 §5.1）；LGP 则保留连续部分、用优化解内层。于是 §7 的两范式分野，在 MINLP 视角下看得更透：同一个 MINLP，PDDLStream "离散化后搜索"，LGP "双层分解后外搜内优"。

本质洞察：把 LGP 看成"一个被双层分解的 MINLP"，是理解它最凝练的视角——它一句话点明了 LGP 全部设计的根源：因为离散动作 + 连续轨迹的联合优化是 MINLP（难解），所以必须分层（外离散、内连续），所以离散层只能搜索、连续层才能优化（§5），所以需要可行性反馈剪枝（内层 $\infty$ → 外层剪），所以 multi-bound 用下界加速（给内层值造廉价下界）。§2–§5 你逐节学到的每个设计，在这个视角下都不再是孤立选择，而是"MINLP + 双层分解"这个根上长出的必然枝叶。更普适地说：每当你遇到"离散决策 + 连续优化"的问题（调度 + 轨迹、选型 + 参数、结构设计 + 连续松弛），先认出它是 MINLP、再想双层分解，你就有了一套通用的拆解框架——LGP 只是这套框架在 TAMP 上的一个精彩实例。

⚠️ 常见陷阱¶

陷阱 3-1（概念误区）：把"逻辑生成约束"理解成"逻辑和几何是两个独立模块顺序执行"。 - 错误描述：以为 LGP 是"先用逻辑规划出动作序列，再独立地做轨迹优化"，两步解耦。 - 现象/后果：退化成 TAMP_T1 §2.4 批判的 plan-then-check，丢失 LGP 联合优化的核心价值。 - 根本原因：LGP 的逻辑不是"先规划完"，而是为几何优化生成约束；而几何优化的可行性又反过来决定哪些符号序列可行（§5 的树搜索会因几何不可行而剪枝）。二者紧耦合。 - 正确做法：理解逻辑与几何通过"约束生成 + 可行性反馈"双向耦合——逻辑序列定义约束，几何优化的成败反馈回符号树搜索（§5）。

陷阱 3-2（概念误区）：以为动作参数像 PDDLStream 那样是采样来的。 - 错误描述：混淆 LGP 和 PDDLStream，以为 LGP 里抓取位姿也是采样候选。 - 现象/后果：无法理解 LGP 为什么能"连续调优"几何量。 - 根本原因：LGP 的动作参数是优化变量（§3.2），由优化器在约束下解出，不是采样候选里挑的。 - 正确做法：牢记 LGP 的根本特征——动作参数作为有效自由度进入优化，与轨迹联合求解。

陷阱 3-3（思维陷阱）：忽略切换一致性约束，以为只要每段轨迹各自可行就行。 - 错误描述：只关注每个符号阶段内的几何约束，忽略阶段之间切换点的一致性。 - 现象/后果：优化出的"轨迹"在动作切换处不连贯——比如抓取时手没真正到杯子位姿上，物理上不可能。 - 根本原因：切换一致性约束（$h_{\text{switch}}$）保证相邻阶段在切换点几何衔接（抓取时手位姿=杯抓取位姿）。漏了它，各段轨迹拼不成物理可行的整体。 - 正确做法：切换约束和阶段约束同等重要——前者保证"段与段衔接"，后者保证"段内可行"，缺一不可。

练习¶

（⭐⭐） 用自己的话解释"逻辑生成约束"：以 place(cup, shelf) 这个动作为例，说明它会在轨迹上生成哪些约束（切换约束 + 阶段约束）。
（⭐⭐⭐） 对照 TAMP_T3 §3：PDDLStream 里"几何认证符号"（采样器认证 Kin 等谓词），LGP 里"符号生成几何"（动作生成轨迹约束）。画一张对照图，标出两者信息流向的方向，并说明为什么说它们是"镜像"的。
（⭐⭐⭐⭐，数学） 解读 §3.4 的优化式：(a) 为什么外层 $\min$ 对 $x(t)$ 和 $a_{1:K}$ 同时优化，但实际求解要分层？(b) 给定一个固定的动作序列 $a_{1:K}$，剩下的优化问题是什么类型（凸/非凸、连续/离散）？(c) $\psi(x(T))$ 这一项在搭积木塔任务里会怎么设计（提示：衡量塔是否搭成、是否稳定）？
（⭐⭐⭐⭐，理论） §3.6 说 LGP 是"被双层分解的 MINLP"。(a) 用一句话说明为什么"离散动作 + 连续轨迹的联合优化"属于 MINLP。(b) 写出内层值函数 $V(a_{1:K})$ 的含义，并解释 $V=+\infty$ 对应什么几何情形。(c) 论证：为什么这个双层结构使得"离散层只能搜索、连续层才能优化"，从而 §5 的两层架构是必然而非任选？

4. KOMO：求解几何层的优化器 ⭐⭐⭐⭐¶

§3 讲了 LGP 把问题写成什么样的优化。但"给定动作序列后那个连续轨迹优化问题"具体怎么解？答案是 KOMO（k-order Markov Optimization，k 阶马尔可夫优化）——Toussaint 设计的轨迹优化器，是 LGP 几何层的引擎。本节讲清它的结构。这是本章数学最重的一节（⭐⭐⭐⭐），第一遍可只抓核心思想。

4.1 KOMO 要解的问题：把轨迹当变量¶

KOMO 来自 Toussaint 2014 的工作 "KOMO: Newton Methods for k-Order Markov Constrained Motion Problems"。它解决的是一个带约束的轨迹优化问题：给定起止条件和一组约束，找一条代价最小的轨迹。

第一步是把连续轨迹离散化。把时间 $[0, T]$ 切成 $T$ 个时间步，轨迹就变成一串构型 $x_1, x_2, \dots, x_T$（每个 $x_t$ 是一个完整的机器人 + 物体构型）。优化的决策变量就是这一长串构型。

KOMO 的离散化:
  连续轨迹 x(t), t∈[0,T]  →  离散构型序列 x_1, x_2, ..., x_T
  决策变量: 所有时间步的构型拼起来的大向量 (维度 = T × 单步构型维度)
  目标: 找这串构型, 使总代价最小, 且满足所有约束

4.2 "k 阶 Markov"是什么意思¶

KOMO 名字里的核心是"k 阶 Markov"。它指：代价函数和约束，只依赖于相邻的 k+1 个连续时间步。

为什么这样设计？因为机器人运动的物理量天然是"局部时间"的： - 速度依赖相邻 2 个构型（$x_t, x_{t-1}$）——1 阶。 - 加速度依赖相邻 3 个构型（$x_t, x_{t-1}, x_{t-2}$）——2 阶。 - 加加速度（jerk）依赖相邻 4 个——3 阶。

所以平滑代价（惩罚加速度）、动力学约束这些，都只牵涉一个小时间窗内的几个构型。这就是"k 阶 Markov 结构"。

k 阶 Markov 结构(以 2 阶为例, 惩罚加速度):
  代价 = Σ_t  φ(x_{t-2}, x_{t-1}, x_t)    ← 每项只依赖连续 3 个构型
  约束 同理: 每个约束只牵涉一个 k+1 步的窗口
  关键后果: 目标的 Hessian(二阶导矩阵)是"带状的"(banded)
           —— 只有相邻时间步之间非零, 远离的为零

本质洞察：k 阶 Markov 结构不是数学上的随意约定，而是物理局部性的直接编码——运动的代价（平滑、能量）和约束（动力学、限位）本质都是"局部时间"的，只关心此刻附近几步。把这个局部性显式地写进问题结构，带来一个巨大的计算红利：目标的 Hessian 矩阵是带状稀疏的。这意味着 Newton 法每步求解（解一个线性系统）的代价从稠密矩阵的 $O((Tn)^3)$ 降到带状矩阵的近似 $O(T n^3)$——与时间步数 $T$ 线性。这就是 KOMO 能高效优化长轨迹的根本原因。这个"用问题结构换计算效率"的思路，与你在 MPC（稀疏 KKT 系统）、SLAM（稀疏信息矩阵）、概率图模型（因子分解）里见到的是同一种智慧。

4.3 用 Newton 类方法求解¶

有了离散化的轨迹变量和 k 阶 Markov 结构的代价/约束，KOMO 用 Newton 类方法（Gauss-Newton / 增广拉格朗日）求解这个带约束优化。核心循环：

KOMO 求解循环(概念版):
  初始化轨迹 x_1...x_T (如直线插值)
  重复:
    1. 在当前轨迹处, 线性化约束、二次近似代价
    2. 利用 banded Hessian 高效解一个线性系统, 得到更新方向 Δx
    3. 沿 Δx 更新轨迹(带线搜索)
    4. 用增广拉格朗日/罚函数处理约束违反
  直到收敛(梯度足够小、约束足够满足)

关键点： - Gauss-Newton：代价多是"二次型之和"（如 $\sum \|\cdot\|^2$），Gauss-Newton 用雅可比近似 Hessian，省去算二阶导。 - 增广拉格朗日 / 罚函数：处理等式与不等式约束（碰撞、限位、切换一致性）——把约束违反作为惩罚项加进目标，迭代地收紧。 - banded 线性代数：每步解线性系统时利用 §4.2 的带状结构，做到关于 $T$ 线性。

4.4 特征函数：KOMO 统一表达代价与约束的核心抽象¶

前面说 KOMO 用 Newton 法解带约束优化，但有个关键问题没讲：那么多不同的代价（平滑、能量、到目标距离）和约束（碰撞、限位、切换一致性），KOMO 怎么用一套统一的机制表达？ 答案是 KOMO 最精妙的抽象——特征函数（feature，又称 task map）。

核心思想：一切都是特征函数的平方和。 KOMO 把每一个代价项和约束，都表达成一个特征函数 $\phi_i(x)$——它从（若干时间步的）构型映射到一个向量。然后整个优化问题统一写成这些特征的"非线性平方和约束"形式。它定义一个约束的平方和问题，适合 Gauss-Newton 方法；设 $J = \nabla_{x_{0:T}} \Phi$ 是全局雅可比。

特征函数(feature)统一一切:
  每个代价/约束 = 一个特征函数 φ_i(x_{t-k:t})  (从相邻 k+1 步构型 → 向量)
  例子:
    平滑代价   → φ = 加速度(x_{t-1},x_t,x_{t+1} 的二阶差分)
    碰撞约束   → φ = 碰撞距离(x_t)         要求 ≥ 0
    位置目标   → φ = 末端位姿 - 目标位姿(x_T) 要求 = 0
    切换一致性 → φ = 手位姿 - 抓取位姿(x_{t_k}) 要求 = 0
  优化问题统一形式:
    min  Σ_i ||φ_i(x)||²       (平方和代价)
    s.t. φ_j(x) = 0  (等式约束特征)
         φ_k(x) ≤ 0  (不等式约束特征)

每个特征带一个"类型"和雅可比。 一个特征函数除了计算值 $\phi_i(x)$，还要能算它对构型的雅可比 $\nabla \phi_i$（一阶导）——这是 Gauss-Newton 所需。特征的"类型"标记它是代价（sos，sum-of-squares）、等式约束（eq）、还是不等式约束（ineq）。

本质洞察：特征函数是 KOMO（乃至现代轨迹优化）的"统一语言"——它把五花八门的代价和约束，归约成同一种东西：一个能算值、能算雅可比、带类型标记的向量函数。这个抽象的威力有三层。其一，统一求解：所有代价/约束都是平方和/约束特征，Gauss-Newton 和增广拉格朗日可以一视同仁地处理，不必为每种代价写专门求解逻辑。其二，模块化建模：要给任务加一个新约束（如"杯口朝上"），只需写一个新特征函数（算值 + 雅可比），插进问题即可——这正是 §3.3"逻辑生成约束"在代码层的落地：每个符号约束对应一个特征。其三，自动微分友好：特征只需提供雅可比，现代框架可用自动微分自动算，大大降低建模成本。这种"用统一的特征/残差抽象组织优化问题"的思路，你在 SLAM 的因子图（每个因子是一个残差）、最小二乘问题、深度学习的损失项里都见过——它是把复杂优化问题模块化的通用智慧。

特征函数视角下，LGP 的"逻辑生成约束"变得很具体：符号动作序列选定后，每个动作的前提/效果翻译成一组特征函数（切换一致性是 eq 特征，阶段碰撞是 ineq 特征，抓握保持通过运动学挂接），全部塞进 KOMO 的平方和问题。逻辑的作用，就是决定哪些特征函数出现在问题里、在哪些时间步上。

4.5 KOMO 如何承接 LGP 的约束¶

回到 §3.3：LGP 的逻辑生成两类约束（切换一致性、阶段几何约束）。KOMO 正是承接它们的求解器：

LGP 的约束（§3.3）	在 KOMO 里如何表达
切换一致性约束	在切换时间步 $t_k$ 上施加等式约束（手位姿 = 抓取位姿）
阶段几何约束（不碰撞）	每个时间步上的不等式约束（碰撞距离 ≥ 0）
阶段几何约束（抓握保持）	持物阶段：杯随手动，作为运动学挂接（有效自由度）
动作参数（抓取/放置位姿）	作为额外的优化变量（有效自由度，§3.2）
平滑/能量代价	k 阶 Markov 代价项（惩罚加速度，§4.2）

也就是说：LGP 选定一个动作序列 → 翻译成一组约束和代价项 → 组装成一个 KOMO 问题 → KOMO 用 Newton 法解出最优轨迹（含动作参数）。一个符号序列对应一个 KOMO 问题。

本质洞察：KOMO 是 LGP 的"几何计算引擎"，二者是"问题定义"与"求解器"的关系——LGP 定义"要优化什么"（§3 的结构），KOMO 解决"怎么高效优化"（§4 的算法）。这种分工和 PDDLStream 与 FastDownward 的关系异曲同工：PDDLStream 定义问题、把它归约为有限 PDDL，FastDownward 当符号搜索引擎；LGP 定义问题、把每个符号序列变成轨迹优化，KOMO 当几何优化引擎。理解一个 TAMP 框架，要分清"它定义了什么问题结构"和"它用什么求解器解"——前者是范式创新，后者是工程支撑。

4.6 Gauss-Newton：从平方和到正规方程 ⭐⭐⭐⭐¶

§4.3 说 KOMO 用 Gauss-Newton 求解、§4.4 说"每个特征只需提供雅可比"，但都只点了名字、没说具体怎么算。本节把 Gauss-Newton 拆开——它是理解"为什么 KOMO 只问你要一阶导，就能又快又稳地优化长轨迹"的第一块拼图。

为什么不用"标准"Newton 法。 Newton 法求一个无约束极小：在当前点把目标做二阶 Taylor 展开，解线性系统 $\nabla^2\Phi\,\Delta x = -\nabla\Phi$ 得到更新方向 $\Delta x$。它收敛极快（局部二次收敛），但代价是要算目标的Hessian $\nabla^2\Phi$（二阶导矩阵）。对 KOMO 这种目标——成百上千个特征的平方和——完整 Hessian 既贵（每个特征都要算二阶导），又可能不正定。不正定是个致命问题：Newton 方向 $\Delta x = -(\nabla^2\Phi)^{-1}\nabla\Phi$ 只有当 $\nabla^2\Phi$ 正定时才保证是下降方向，否则优化可能朝代价升高的方向走、发散。Gauss-Newton 正是为"平方和目标"量身定做的捷径，一举绕开这两个麻烦。

平方和目标的梯度与 Hessian。 回顾 §4.4：KOMO 把所有 sos（sum-of-squares，平方和）代价特征的输出首尾相接，堆成一个大向量 $\phi(x) = [\phi_1(x);\, \phi_2(x);\, \dots]$，平方和目标就是

\[ \Phi(x) = \sum_i \|\phi_i(x)\|^2 = \phi(x)^\top \phi(x). \tag{4.1} \]

记 $J = \dfrac{\partial \phi}{\partial x}$ 是 §4.4 出现过的全局雅可比——它的第 $r$ 行是 $\phi$ 的第 $r$ 个分量对决策变量 $x$（整条轨迹）的梯度。对 $(4.1)$ 求一阶导，用链式法则（$\phi^\top\phi$ 对 $x$ 求导，外层导数 $2\phi$ 乘内层导数 $J$）：

\[ \nabla\Phi = 2\, J^\top \phi. \tag{4.2} \]

再对 $(4.2)$ 求一次导（对 $2J^\top\phi$ 用乘积法则——$J$ 随 $x$ 变、$\phi$ 也随 $x$ 变）：

\[ \nabla^2\Phi = \underbrace{2\, J^\top J}_{\text{Gauss-Newton 项（只含一阶导）}} + \underbrace{2\sum_r \phi_r\, \nabla^2\phi_r}_{\text{曲率项（含二阶导 }\nabla^2\phi_r\text{）}}. \tag{4.3} \]

第一项 $2J^\top J$ 只用到一阶导 $J$；第二项是"曲率项"，要算每个特征的二阶导 $\nabla^2\phi_r$，且被残差 $\phi_r$ 加权。

Gauss-Newton 近似：扔掉曲率项。 Gauss-Newton 的全部内容就一句话——把 Hessian 近似成 $2J^\top J$，扔掉那个需要二阶导的曲率项 $2\sum_r\phi_r\nabla^2\phi_r$。 为什么敢扔？两个理由：

残差小时可忽略：接近最优解时，多数特征残差 $\phi_r$ 已经很小（代价趋近零），曲率项里每一项都乘了个小量 $\phi_r$，整体可忽略。
近线性时本就小：若特征 $\phi_r$ 接近线性（如下文 §4.9 的加速度平滑特征是严格线性的），$\nabla^2\phi_r \approx 0$ 甚至 $=0$，曲率项本就消失。

扔掉曲率项的回报有三层：(1) 只需一阶导 $J$——这正是 §4.4 反复强调"每个特征只需提供值和雅可比"的根本原因，二阶导的活儿被省掉了；(2) $J^\top J$ 永远半正定（对任意向量 $v$，$v^\top J^\top J v = \|Jv\|^2 \ge 0$）——保证更新方向是下降方向，不会像完整 Newton 那样因 Hessian 不正定而发散；(3) 稀疏结构继承——$J$ 的带状性（§4.8 详述）直接传给 $J^\top J$。

正规方程：每一步到底解什么。 把近似 Hessian $2J^\top J$ 代进 Newton 方程 $\nabla^2\Phi\,\Delta x = -\nabla\Phi$，左右两边公共的系数 2 约掉：

\[ \boxed{\;(J^\top J)\,\Delta x = -\,J^\top \phi\;} \tag{4.4} \]

这就是 Gauss-Newton 正规方程（normal equations）——KOMO 每次迭代真正在解的线性系统。解出方向 $\Delta x$ 后，沿它更新轨迹 $x \leftarrow x + \alpha\,\Delta x$，步长 $\alpha$ 由线搜索确定（保证代价真的下降，而非盲目走一整步）。把它对回 §4.3 那个"概念版求解循环"，就具体了：

KOMO 求解循环(Gauss-Newton 具体版, 对照 §4.3 概念版):
  初始化轨迹 x = (x_1,...,x_T)  (如直线插值, §4.1)
  重复:
    1. 在当前 x 处算所有特征值 φ 与全局雅可比 J = ∂φ/∂x   ← 只要一阶导!
    2. 解正规方程 (JᵀJ) Δx = -Jᵀφ   ← 利用 JᵀJ 的带状结构(§4.8)高效解
    3. 线搜索定步长 α, 更新 x ← x + αΔx
    4. (有约束时)外层更新乘子, 见 §4.7 增广拉格朗日
  直到 ‖Jᵀφ‖ 足够小(梯度近零)且约束足够满足

数值稳健：Levenberg-Marquardt 阻尼。 $J^\top J$ 可能接近奇异（存在"零曲率"的退化方向，比如某段轨迹的某些自由度暂时不被任何特征约束），此时直接解 $(4.4)$ 会让 $\Delta x$ 在退化方向上爆掉。实践中加一个阻尼项：

\[ (J^\top J + \lambda_{\text{LM}}\, I)\,\Delta x = -\,J^\top\phi. \tag{4.5} \]

$\lambda_{\text{LM}}$ 大 → 矩阵被对角项主导，更新退化成小步长的梯度下降（稳但慢）；$\lambda_{\text{LM}}$ 小 → 退化成纯 Gauss-Newton（快但在退化方向上险）。自适应地调 $\lambda_{\text{LM}}$（这步代价降了就调小、升了就调大），就是 Levenberg-Marquardt（LM）。注意这个阻尼系数 $\lambda_{\text{LM}}$ 与下一节约束的拉格朗日乘子 $\lambda$ 是完全不同的两个量，本章在符号上以下标区分。

本质洞察：Gauss-Newton 的精髓，是用问题的平方和结构换掉了最贵的那部分计算（二阶导）——它不去老老实实算目标的真实曲率 $\nabla^2\Phi$，而是用"雅可比的内积 $J^\top J$"做一个总是半正定的代用 Hessian。这个代用品在残差小、特征近线性时几乎和真 Hessian 一样好，却只要一阶导。你在 SLAM 的光束法平差（bundle adjustment 最小化重投影误差平方和）、传感器标定的最小二乘、乃至某些神经网络二阶优化器里见到的，都是同一个 $J^\top J$——它是"平方和最优化"这一大类问题的通用引擎。理解了这一点，KOMO §4.4 那句"特征只需提供值和雅可比"就不再是抽象口号：正因为求解器用的是 Gauss-Newton，它只问你要一阶导，二阶导的负担被 $J^\top J$ 这个近似一笔勾销了。 这也是 LGP 能把"加一个新约束"做得如此轻量（§4.4：只插一个特征）的底层原因——新特征只需会算自己的值和雅可比，就能无缝并入 $J$。

4.7 增广拉格朗日：把硬约束逼成可解的子问题 ⭐⭐⭐⭐¶

§4.6 的 Gauss-Newton 解的是无约束平方和。但 §3.3 列出的 LGP 约束——切换一致性是硬等式（抓取那一刻手位姿必须精确等于抓取位姿）、碰撞与限位是硬不等式（距离必须 $\ge 0$）——这些是不能违反的硬约束，绝不能只当代价软惩罚一下了事。KOMO 用增广拉格朗日法（Augmented Lagrangian, AL）把"带硬约束的问题"，转化成一串"无约束平方和子问题"，每个子问题交给 §4.6 的 Gauss-Newton 解。本节讲清这个转化，它是 KOMO 求解骨架的另一半。

先看最朴素的办法：罚函数，以及它为什么不够。 要让等式约束 $g(x)=0$（这里 $g$ 泛指任一硬等式，如切换一致性 $h_{\text{switch}}$）被满足，最直接的想法是给目标加个二次罚项：

\[ \min_x\ \Phi(x) + \frac{\mu}{2}\,\|g(x)\|^2. \tag{4.6} \]

罚权重 $\mu$ 越大，违反约束的代价越高，最优解越接近满足约束。问题是：只有 $\mu\to\infty$ 才能精确满足。直觉上，对任何有限的 $\mu$，最优解都会为了多降一点 $\Phi$ 而容忍稍微违反一点约束——因为违反带来的罚（有限）可能小于降 $\Phi$ 的收益。更糟的是，$\mu$ 一大，$(4.6)$ 在 §4.6 的 $J^\top J$ 里对应这部分的权重畸大，矩阵病态（条件数爆炸），Gauss-Newton 解得不准、收敛极慢。这就是"硬约束软处理"的根本困境：要精确就得无穷大罚，要无穷大罚就病态。

增广拉格朗日：加一个乘子，让有限 $\mu$ 也能精确满足。 AL 的关键改良是：在二次罚之外，再加一个线性的拉格朗日项 $\lambda^\top g(x)$：

\[ L_A(x, \lambda;\, \mu) = \Phi(x) + \lambda^\top g(x) + \frac{\mu}{2}\,\|g(x)\|^2. \tag{4.7} \]

其中 $\lambda$ 是拉格朗日乘子（每个等式约束配一个分量）。求解分内外两层：

内层：固定 $\lambda, \mu$，对 $x$ 极小化 $L_A$。注意 $\lambda^\top g$ 和 $\frac{\mu}{2}\|g\|^2$ 都能改写成特征平方和的形式并入目标，所以内层就是一个无约束平方和问题——直接交给 §4.6 的 Gauss-Newton。
外层：内层解出 $x^\star$ 后，更新乘子

\[ \lambda \;\leftarrow\; \lambda + \mu\, g(x^\star), \tag{4.8} \]

并（按需）适度增大 $\mu$。然后带着新的 $\lambda, \mu$ 回到内层，如此交替直到约束被满足。

为什么这个乘子更新能让有限 $\mu$ 精确满足约束？ 这是 AL 的灵魂，值得推一遍。内层最优（对 $x$ 求 $L_A$ 的梯度为零）的稳定性条件是：

\[ \nabla\Phi(x^\star) + \big(\lambda + \mu\, g(x^\star)\big)^\top \nabla g(x^\star) = 0. \tag{4.9} \]

而原约束问题真正的最优性（KKT）条件是：存在真乘子 $\lambda^\star$ 使

\[ \nabla\Phi(x^\star) + \lambda^{\star\top}\,\nabla g(x^\star) = 0,\qquad g(x^\star)=0. \tag{4.10} \]

把 $(4.9)$ 和 $(4.10)$ 一比对：当外层迭代收敛、$x^\star$ 稳定下来，必有 $\lambda + \mu\, g(x^\star) \to \lambda^\star$。也就是说，乘子项 $\lambda$ 替代了"靠无穷大 $\mu$"的角色——更新式 $(4.8)$ 恰好让 $\lambda$ 一步步逼近真乘子 $\lambda^\star$。一旦 $\lambda \to \lambda^\star$，由 $\lambda + \mu g \to \lambda^\star$ 反推出 $\mu\, g(x^\star) \to 0$，即 $g(x^\star)\to 0$ 自动成立，而 $\mu$ 不必趋于无穷。这就是 AL 比纯罚函数高明的地方：用一个会自我更新的乘子，换掉了"$\mu\to\infty$"带来的病态。

不等式约束怎么办。 碰撞、关节限位是不等式 $h(x)\le 0$。AL 处理它们用"单边"罚——只在约束被违反（或对应乘子为正）时激活：

\[ \frac{\mu}{2}\sum_j \max\!\Big(0,\; h_j(x) + \tfrac{\lambda_j}{\mu}\Big)^2,\qquad \lambda_j \leftarrow \max\!\big(0,\; \lambda_j + \mu\, h_j(x^\star)\big). \tag{4.11} \]

直观理解：当 $h_j \le -\lambda_j/\mu$（约束满足且有裕度），$\max(0,\cdot)$ 取零，这一项关掉，完全不影响优化；当约束快违反或已违反，这一项变成二次罚，把解"推回"可行域。乘子更新里的 $\max(0,\cdot)$ 保证 $\lambda_j \ge 0$（不等式乘子必须非负，这是 KKT 互补松弛的要求）。

接回 §3.3：哪类约束用哪种处理。 把 AL 的两套机制对到 LGP 生成的约束上：

LGP 约束（§3.3）	数学类型	在 AL 里如何进入
切换一致性 $h_{\text{switch}}(x(t_k),a_k)=0$	等式	线性乘子项 $\lambda^\top h_{\text{switch}}$ + 二次罚 $\frac{\mu}{2}\\|h_{\text{switch}}\\|^2$，外层更新 $\lambda$
碰撞距离 $\ge 0$	不等式	单边罚 $\max(0,\cdot)^2$，乘子非负
关节限位	不等式	单边罚
抓握保持（杯随手动）	运动学挂接	不入约束，化为有效自由度（§4.5），自动满足
平滑 / 能量 sos 代价	（非约束）	直接进平方和目标 $\Phi$，不需乘子

AL 与 SQP 的一句话区别。 处理约束优化还有另一条主流路——序列二次规划（SQP）：每步把约束线性化、构造一个二次规划（QP）子问题求解（这是后文 §4.10 提到的 TrajOpt 的选择）。AL 与 SQP 是兄弟而非对错：AL 走"罚 + 乘子外环"，SQP 走"线性化约束 + 解 QP"。KOMO 选 AL，因为它和 §4.6 的 Gauss-Newton 内环天然契合——内环始终是个无约束平方和，求解器只需会解 $(4.4)$ 一种系统。

本质洞察：增广拉格朗日的智慧，是把一个"难"问题（带硬约束）化成一串"易"问题（无约束平方和），用外层的乘子更新把约束逼到精确满足——而不必把罚权重 $\mu$ 推到无穷大引发病态。这与 §5 的 multi-bound、与 TAMP_T3 的乐观对象，是同一种工程哲学在不同层面的体现：不正面硬刚困难（无穷大罚 / 全精度优化 / 真实采样），而是用一个迭代逼近的外环，把困难拆成可承受的内环。 对 LGP 尤其性命攸关：§3.3 那些硬约束（抓取时手必须精确落在抓取位姿、绝不能与障碍相碰）正是靠 AL 才能在有限计算下被精确满足——切换一致性差一点，整条轨迹物理上就拼不成一个整体（这正是 §3.3 陷阱 3-3 警告的）。Gauss-Newton（§4.6）解内环、增广拉格朗日（本节）管外环，两者合起来，才是 KOMO 完整的求解骨架。 你若把 KOMO 看成黑盒，记住这一句就够；若要自己实现或调它，这两层就是你所有旋钮（初值、$\mu$ 调度、乘子更新、LM 阻尼）的来源。

4.8 带状 Hessian 与求解复杂度：为何关于 T 线性 ⭐⭐⭐⭐¶

§4.2 抛出过一个论断：k 阶 Markov 结构让 Hessian 带状，使 Newton 求解"关于时间步 $T$ 线性"。§4.6 给出了那个真正要解的矩阵（$J^\top J$），本节兑现这个论断——讲清"带状"从哪来、为什么它把求解复杂度从立方压到线性。这是 KOMO 能优化几百上千步长轨迹的命脉，少了它，前面的 Gauss-Newton 只是"能解"，谈不上"高效"。

决策变量怎么排。 把整条离散轨迹的所有构型首尾相接，堆成一个大向量 $x = [x_1;\, x_2;\, \dots;\, x_T]$，每个 $x_t \in \mathbb{R}^n$（$n$ 是单步构型维度，含机器人关节 + 物体自由度 + 动作参数）。于是 §4.6 正规方程 $(4.4)$ 里要解的系统 $(J^\top J)\Delta x = -J^\top\phi$，是一个 $Tn \times Tn$ 的线性系统。$T$ 一大（长轨迹），这个矩阵就极大。

为什么 $J^\top J$ 是块带状的。 关键在 §4.2 的 k 阶 Markov 性质。一个 k 阶特征 $\phi_i$ 只依赖相邻的 $k+1$ 个构型 $x_{t-k}, \dots, x_t$。所以它对 $x$ 的雅可比行 $\nabla\phi_i$，只在这 $k+1$ 个时间块对应的列上非零、其余全零。它对 Gauss-Newton Hessian 的贡献 $J_i^\top J_i$（外积），就只在这 $k+1$ 个时间块构成的 $(k+1)\times(k+1)$ 块窗口内非零。把所有特征的贡献加起来，$J^\top J$ 只在"时间相距 $\le k$"的块之间非零——这就是块带状（block-banded），半带宽 $b \approx k\cdot n$。

2 阶 Markov(惩罚加速度) → JᵀJ 块三对角:
    x_1   x_2   x_3   x_4  ...  x_T
x_1 [■]   [■]    0     0          0      ■ = n×n 非零块
x_2 [■]   [■]   [■]    0          0      0 = 零块
x_3  0    [■]   [■]   [■]         0
x_4  0     0    [■]   [■]   ...   0      半带宽 b ≈ 2n
...                              [■]     (相距>2 的时间块全零)
x_T  0     0     0    ...  [■]   [■]

一个 2 阶特征（如加速度 $\phi(x_{t-1},x_t,x_{t+1})$）只"碰"相邻 3 个构型块，于是它只在这 3 块构成的小窗口内填非零。加总所有特征，$J^\top J$ 就成了块三对角（半带宽 $\approx 2n$）。3 阶（惩罚 jerk）则带宽变 $\approx 3n$，但仍是窄带。

带状 Cholesky：算一笔复杂度账。 解一个对称正定线性系统的标准方法是 Cholesky 分解（$M = LL^\top$，再两次回代）。代价取决于矩阵稀疏结构：

稠密求解 $N\times N$ 系统（$N = Tn$）：$O(N^3) = O((Tn)^3)$——关于 $T$ 立方。举例 $T=500, n=10$，则 $N=5000$，$N^3 \approx 1.25\times10^{11}$，而每次 Gauss-Newton 迭代都要解一次，几十次迭代彻底不可行。
带状 Cholesky：对半带宽 $b$ 的对称正定带状矩阵，分解只在带内填元素，每列做 $O(b^2)$ 功、共 $N$ 列，代价 $O(N b^2) = O(Tn\cdot(kn)^2) = O(T\,k^2 n^3)$——关于 $T$ 线性。同样 $T=500, n=10, k=2$：$\approx 500\times 4\times 1000 = 2\times10^6$，比稠密快约 5 个数量级。

$J^\top J$ 恰好半正定（§4.6）又带状，正好满足带状 Cholesky 的前提，于是把每次 Gauss-Newton 迭代的线性求解从 $O((Tn)^3)$ 降到 $O(T k^2 n^3)$——关于轨迹长度 $T$ 线性。这就是 KOMO 名字里 "k 阶 Markov" 的全部计算回报：$k$ 直接设定带宽 $kn$，进而设定复杂度；$k$ 越大（更高阶平滑），带越宽、每步越贵，但始终关于 $T$ 线性。

本质洞察：「k 阶 Markov → 带状 Hessian → 线性复杂度」是一条把物理局部性翻译成计算效率的完整链条，每一环都不能少：物理上运动的代价只关心邻近几步（局部性，§4.2）→ 数学上特征只依赖 $k+1$ 个连续构型（k 阶）→ 线性代数上 $J^\top J$ 只在邻近时间块非零（带状）→ 算法上带状 Cholesky 关于 $T$ 线性（效率）。这条链条你在整个规控与 SLAM 里反复见到同构的版本：SLAM 的信息矩阵因"每个观测只关联邻近位姿/路标"而稀疏，用稀疏 Cholesky 解；MPC 的 KKT 系统因"动力学只耦合相邻时刻"而块三对角，用 Riccati 递推（本质也是利用这种带状/时间结构）线性求解；概率图模型的因子分解把这种局部依赖显式画成图。记住这条链条，你就握住了"长序列优化为什么能快"的通用钥匙——它从来不是靠更强的硬件，而是靠把问题里的局部性显式编码进求解器的稀疏结构。

4.9 特征雅可比的完整推演：以末端位置为例 ⭐⭐⭐⭐¶

§4.4 说"每个特征要能算值和雅可比"，§4.6 与 §4.8 反复用到这个雅可比 $J$。但一个特征的雅可比具体怎么算？本节挑一个最常用的特征——末端执行器位置——从头推一遍它的值和雅可比，让"特征 + 雅可比"这套抽象彻底落地，也顺手回答 §4.4 练习留下的"怎么为一个新约束设计特征"。

特征的值。 设在时间步 $t$，机器人构型为 $q_t$（它是 $x_t$ 的一部分），末端执行器在世界系的位置由正运动学给出 $p_{ee}(q_t)\in\mathbb{R}^3$。要约束末端在 $t$ 时刻到达目标位置 $p^\star$（如 §3.3 的切换一致性"抓取时手在抓取位姿"），就定义一个等式特征：

\[ \phi(x_t) = p_{ee}(q_t) - p^\star,\qquad \text{要求 } \phi = 0. \tag{4.12} \]

它的雅可比就是机械臂的位置雅可比。 对 $(4.12)$ 关于 $q_t$ 求导：

\[ \nabla_{q_t}\phi = \frac{\partial p_{ee}}{\partial q_t} = J_{ee}(q_t)\in\mathbb{R}^{3\times n}. \tag{4.13} \]

这正是机器人学里早已成熟的机械臂位置雅可比 $J_{ee}$。回顾运动学基础：它的第 $i$ 列描述"单独动关节 $i$ 时末端位置的瞬时变化"，对转动关节是

\[ \big[J_{ee}\big]_{:,i} = \hat{z}_i \times (p_{ee} - o_i), \tag{4.14} \]

其中 $\hat{z}_i$ 是关节 $i$ 的旋转轴（世界系）、$o_i$ 是关节 $i$ 的原点位置；对平动关节则是 $[J_{ee}]_{:,i} = \hat{z}_i$。所以这个特征的雅可比根本不用现推——它就是你在机械臂运动学里推过的那个 $J_{ee}$，KOMO 直接调用即可（rai/KOMO 内部为每种几何特征实现了对应雅可比，或用自动微分代劳）。

它在全局雅可比 $J$ 里落在哪。 $(4.12)$ 这个特征只依赖单个时间步 $x_t$（属于 0 阶 Markov——只牵涉 1 个构型）。所以它在全局雅可比 $J$（$Tn$ 列）里只有第 $t$ 个时间块的那 $n$ 列非零、等于 $J_{ee}(q_t)$，其余全零。这正是 §4.8 带状结构的微观来源：一个 0 阶特征贡献一个对角块，一个 2 阶特征贡献跨 3 块的窗口——把它们叠起来，就拼出了那个块带状的 $J^\top J$。

对照一个 2 阶特征：加速度平滑。 再看 §4.4/§4.8 用过的平滑特征——加速度的二阶差分

\[ \phi(x_{t-1}, x_t, x_{t+1}) = x_{t-1} - 2x_t + x_{t+1}. \tag{4.15} \]

它对三个时间步的雅可比是常数：$\partial\phi/\partial x_{t-1} = I$，$\partial\phi/\partial x_t = -2I$，$\partial\phi/\partial x_{t+1} = I$，即 $[\,I,\; -2I,\; I\,]$。注意它是一个严格线性的特征 → $\nabla^2\phi = 0$ → §4.6 扔掉的曲率项对它精确为零，Gauss-Newton 对平滑代价是精确的（没有任何近似损失）。同时它牵涉 3 个连续时间块 → 正是 §4.8 带宽 $\approx 2n$ 的来源。一个 0 阶位置特征 + 一堆 2 阶平滑特征，就解释了 §4.8 那张块三对角图的全部非零结构。

动手：为"杯口朝上不洒水"设计特征（呼应 §4.4 练习）。 设杯子在 $t$ 时刻的姿态为旋转矩阵 $R(q_t)$（构型的函数）、杯子本体的"向上轴"为单位向量 $\hat{n}_{\text{cup}}$，世界竖直方向为 $\hat{z}_{\text{world}}$。要让杯口尽量朝上（不洒水），定义特征

\[ \phi(x_t) = R(q_t)\,\hat{n}_{\text{cup}} - \hat{z}_{\text{world}}. \tag{4.16} \]

若要求"完全竖直"就当等式特征（$\phi=0$）；若只要求"倾角不超过阈值"，则改成不等式特征（倾角的余弦 $\ge$ 阈值）。它的雅可比通过链式法则穿过 $R(q)$ 求得——本质是机械臂的姿态雅可比（angular Jacobian）的组合，同样是运动学现成件。加这个约束要做的全部事情就是：写出 $(4.16)$ 这个 $\phi$ 及其雅可比，把它插在"持物阶段"的那些时间步上（§4.5 表里"持物段"的约束）。求解器一行都不用改——这就是 §4.4 说"加新约束只需插一个特征"的字面含义。

本质洞察：这个推演揭穿了 KOMO"特征 + 雅可比"抽象其实没有魔法——它把机器人学里早已成熟的运动学雅可比（位置雅可比 $J_{ee}$、姿态雅可比），原封不动地当作优化的梯度来源。一个特征无非是"从构型算出某个你关心的量"（末端在哪、杯口朝哪），它的雅可比无非是"这个量对构型的偏导"，而机器人学早就给了你计算各种雅可比的工具。所以 §3.3"逻辑生成约束"在代码层真正发生的事是：每个符号动作激活一组特征函数，每个特征调用现成的运动学雅可比，全部塞进 §4.6 的 $J$。 理解了这一层，你就明白 §4.4 那句"加新约束只需插一个特征、不必改求解器"的底气从何而来——求解器要的只是"值"和"雅可比"这两样标准件，而你早就会造它们。从"会算机械臂雅可比"到"会给 KOMO 写约束特征"，中间没有鸿沟，只隔着这一层认识。

4.10 KOMO 在轨迹优化家族中的位置：与 iLQR / CHOMP / 配点法 ⭐⭐⭐¶

学到这里你大概会问：KOMO 和我在 MPC、在规控其他线见过的轨迹优化器——iLQR、CHOMP、TrajOpt、直接配点——是什么关系？它们都"找一条好轨迹"、都用类 Newton 法、都利用时间稀疏。本节把 KOMO 放进这个家族，标清它的坐标。这既帮你把已有的 MPC/iLQR 知识迁移过来（R14 的跨章复用），也让你看清一个常被忽略的设计逻辑：LGP 为什么偏偏选 KOMO 当它的几何引擎。

共同基因。 这个家族的成员，骨架是一致的：把连续轨迹离散化 → 写成一个大的稀疏非线性规划（NLP）或一串二次规划（QP）→ 用类 Newton 的迭代求解，且每步都利用"代价/约束只耦合邻近时刻"的时间稀疏来加速。它们的差异，集中在四个维度：(a) 用什么变量空间，(b) 动力学如何进入，(c) 硬约束如何处理，(d) 稀疏线性求解如何组织。

维度	KOMO	iLQR / DDP	CHOMP	TrajOpt (SQP)	直接配点
变量空间	构型 $x_t$（速度/加速度用差分得到）	状态-控制 $(x_t, u_t)$	构型路径	构型 / 状态	状态 + 控制
动力学如何进入	代价/约束特征（差分，§4.9）	显式前向 rollout（积分动力学）	不显式（平滑度量隐含）	作为约束	配点等式约束
硬约束（碰撞/限位）	增广拉格朗日（§4.7）精确满足	弱（需 AL-iLQR / 障碍函数变体）	软（距离场代价，不保证满足）	SQP + 符号距离约束	NLP 约束
稀疏求解组织	带状 Cholesky 直接分解（§4.8）	Riccati 反向递推（动态规划）	协变梯度（按平滑度量）	稀疏 QP	稀疏 NLP 求解器
设计哲学	特征平方和 + 增广拉格朗日	动态规划	协变梯度下降	信赖域 SQP	同时法（simultaneous）

下面拆开三个最关键的对比。

构型空间 vs 状态-控制空间。 KOMO 优化的是构型 $x_t$（位置层），速度、加速度通过相邻步的有限差分得到（§4.9 的 $(4.15)$）；iLQR 优化的是状态-控制对 $(x_t, u_t)$，并通过显式前向积分动力学（rollout）把控制变成状态。后果是：KOMO 天然适合"构型层"任务（够到、放置、运动学切换），LGP 的切换一致性、抓握挂接都活在构型层；iLQR 天然适合"控制层"动力学（力矩、欠驱动、接触力），是无人机/腿足那类强动力学系统的主场。对 LGP 的操作问题（几何为主、接触通过约束建模），构型空间是更贴合的选择。

硬约束——这是 LGP 选 KOMO 的决定性理由。 LGP 生成的是硬约束：切换一致性是必须精确满足的等式（差一点轨迹就拼不起来，§3.3 陷阱 3-3），无碰撞是绝不能违反的不等式。KOMO 的"特征 + 增广拉格朗日"机制（§4.4、§4.7）正是为吞下这类硬约束而生——每个硬约束就是一个 eq/ineq 特征，AL 的外层乘子把它逼到精确满足。相比之下，经典 iLQR/DDP 是在无约束代价上做动态规划，处理硬路径约束很吃力（要靠 AL-iLQR、障碍函数等变体打补丁）；CHOMP 更是只有软的距离场代价，不保证约束满足。而 TrajOpt（Schulman 等，SQP + 符号距离碰撞约束）在精神上其实和 KOMO 很近——两者都以"约束为一等公民"。所以在这个家族里，KOMO 与 TrajOpt 同属"约束驱动"分支，iLQR/CHOMP 属"软代价驱动"分支。

稀疏求解怎么组织。 KOMO 把整个带状 KKT 系统直接做带状 Cholesky 分解（§4.8）；iLQR 把同样的时间结构通过 Riccati 反向递推（一个动态规划的后向扫描）来分解。数学上两者都在利用块三对角的时间结构，只是组织方式不同——一个是直接矩阵分解、一个是 DP 递推，同样收获关于 $T$ 线性的复杂度，只是记账方式有别。理解这一点，你就不会把"KOMO 的带状 Cholesky"和"iLQR 的 Riccati"当成两种不相干的魔法——它们是同一个时间稀疏结构的两种利用法。

本质洞察：把 KOMO 放进轨迹优化家族，你会发现它并非"另一种神秘算法"，而是这个家族里"约束驱动"那一支的代表——和 TrajOpt 同宗（都以约束为一等公民），与 iLQR/CHOMP 的"软代价驱动"分支相对。这个定位不是偶然：LGP 的核心是"逻辑生成（硬）约束"（§3.3），它需要的几何引擎，母语必须是"约束"。KOMO 的特征 + 增广拉格朗日机制（§4.4、§4.7）恰好让 LGP 的切换一致性（硬等式）、碰撞（硬不等式）一一对应地落地——换成 iLQR 那种"软代价 + 动态规划"的引擎，反而要和 LGP 的硬约束较劲。这解释了一个常被忽略的设计逻辑：TAMP 范式（LGP）和它的求解器（KOMO）不是随意搭配，而是"约束式的问题"配"约束式的求解器"。 理解了这层匹配，你将来选或造一个 TAMP/轨迹优化系统时就有了判据：先看你的问题生成的是硬约束还是软偏好，再选母语匹配的优化器——硬约束为主选 KOMO/TrajOpt 这类，强动力学软代价为主选 iLQR/MPC 这类。这也呼应 §2.2 那条贯穿规控的"采样 vs 优化"分野——现在你看到，即便都在"优化"阵营内部，也还有"约束式 vs 代价式"的更细分野。

4.11 手工走一遍一次 Gauss-Newton 迭代（最小数值例子）⭐⭐⭐⭐¶

§4.6 给了正规方程 $(J^\top J)\Delta x = -J^\top\phi$，但公式不动手算一遍总隔一层。本节用一个2 自由度的最小例子，把一次 Gauss-Newton 迭代的每个量——$\phi$、$J$、$J^\top J$、$\Delta x$——都算出具体数字，让 §4.6 彻底落地。这是检验"你真的懂正规方程"的最好方式。

设定一个能手算的微型问题。 决策变量 $x=(x_1, x_2)$（想象成一条两步轨迹的两个标量构型）。三个 sos 特征（都线性，便于手算）：

\[ \phi_1 = x_1 - 1,\qquad \phi_2 = x_2 - x_1,\qquad \phi_3 = x_2 - 3. \]

直观：$\phi_1$ 想把 $x_1$ 拉到 1，$\phi_3$ 想把 $x_2$ 拉到 3，$\phi_2$ 是"平滑"项想让 $x_2$ 靠近 $x_1$。三个愿望互相矛盾，优化求的是平方和最小的折中。目标 $\Phi(x) = \phi_1^2 + \phi_2^2 + \phi_3^2$。

第一步：写出雅可比 $J$（常数，因为特征线性）。 $\phi = [\,x_1-1;\; x_2-x_1;\; x_2-3\,]$，对 $(x_1,x_2)$ 求偏导：

\[ J = \frac{\partial\phi}{\partial x} = \begin{pmatrix} 1 & 0 \\ -1 & 1 \\ 0 & 1 \end{pmatrix}. \]

（第 1 行 $\partial\phi_1=(1,0)$；第 2 行 $\partial\phi_2=(-1,1)$；第 3 行 $\partial\phi_3=(0,1)$。）

第二步：算 $J^\top J$（Gauss-Newton 近似 Hessian）。

\[ J^\top J = \begin{pmatrix} 1 & -1 & 0 \\ 0 & 1 & 1 \end{pmatrix}\begin{pmatrix} 1 & 0 \\ -1 & 1 \\ 0 & 1 \end{pmatrix} = \begin{pmatrix} 2 & -1 \\ -1 & 2 \end{pmatrix}. \]

它对称、正定（特征值 1 和 3），正是 §4.6 说的"恒半正定"——这里还严格正定，因为问题良约束。

第三步：在初值处算残差与梯度。 取初值 $x^{(0)}=(0,0)$：

\[ \phi(x^{(0)}) = \begin{pmatrix} 0-1 \\ 0-0 \\ 0-3 \end{pmatrix} = \begin{pmatrix} -1 \\ 0 \\ -3 \end{pmatrix},\qquad J^\top\phi = \begin{pmatrix} 1 & -1 & 0 \\ 0 & 1 & 1 \end{pmatrix}\begin{pmatrix} -1 \\ 0 \\ -3 \end{pmatrix} = \begin{pmatrix} -1 \\ -3 \end{pmatrix}. \]

（梯度 $\nabla\Phi = 2J^\top\phi = (-2,-6)$，非零，说明初值不是最优。）

第四步：解正规方程得更新方向。 $(J^\top J)\Delta x = -J^\top\phi$：

\[ \begin{pmatrix} 2 & -1 \\ -1 & 2 \end{pmatrix}\Delta x = \begin{pmatrix} 1 \\ 3 \end{pmatrix}. \]

矩阵行列式 $=2\cdot2-(-1)(-1)=3$，逆 $=\tfrac{1}{3}\begin{pmatrix}2 & 1\\ 1 & 2\end{pmatrix}$，于是

\[ \Delta x = \frac{1}{3}\begin{pmatrix}2 & 1\\ 1 & 2\end{pmatrix}\begin{pmatrix}1\\3\end{pmatrix} = \frac{1}{3}\begin{pmatrix}5\\7\end{pmatrix} = \begin{pmatrix}1.667\\2.333\end{pmatrix}. \]

第五步：更新并验证。 线性问题步长取 $\alpha=1$，$x^{(1)} = x^{(0)}+\Delta x = (1.667,\,2.333)$。验证它真是最优——在 $x^{(1)}$ 处重算残差 $\phi = [\tfrac23;\ \tfrac23;\ -\tfrac23]$，则

\[ J^\top\phi(x^{(1)}) = \begin{pmatrix} 1 & -1 & 0 \\ 0 & 1 & 1 \end{pmatrix}\begin{pmatrix} 2/3 \\ 2/3 \\ -2/3 \end{pmatrix} = \begin{pmatrix} 0 \\ 0 \end{pmatrix}. \]

梯度归零 → $x^{(1)}$ 就是最优解，一步 Gauss-Newton 到位。注意残差并非零（三个矛盾愿望不可能同时满足），但梯度为零——这是平方和的"最小折中"，正是最小二乘的几何含义。

为什么只要一步？ 因为三个特征全是线性的，§4.6 式 4.3 扔掉的曲率项 $\sum_r\phi_r\nabla^2\phi_r$ 精确为零（线性特征 $\nabla^2\phi_r=0$），Gauss-Newton 对线性 sos 问题等价于直接解最小二乘，无近似、一步收敛。若特征非线性（如末端位置含三角函数、§4.9），$J$ 随 $x$ 变，就要在新点重新线性化、再解一次正规方程，迭代多步直到梯度足够小——这就是 §4.3 那个"重复"循环的由来。

本质洞察：这个手算例子把 §4.6 的抽象公式坐实成了五个具体步骤——算特征值、算雅可比、组 $J^\top J$、解正规方程、更新——这正是 KOMO 每次迭代在几千维上做的事，只是这里是 2 维、能用笔算。两个细节值得记住：其一，线性特征一步到位、非线性特征要迭代，这解释了为什么平滑代价（线性）从不是 KOMO 收敛的难点，难点永远在非线性的几何特征（碰撞、姿态）；其二，梯度为零 ≠ 残差为零——优化求的是矛盾目标间的平方和最优折中，而不是让每个目标都满足（那是过约束、通常无解）。理解了这个 2 维例子，你再看 §6.5 那个几十维的从零实现，就只是"同一套步骤，换成 numpy 矩阵、加一层增广拉格朗日外环"而已。

4.12 手工走一遍增广拉格朗日：一变量、一约束 ⭐⭐⭐⭐¶

§4.11 手算了 Gauss-Newton 内层，§4.7 论证了增广拉格朗日外层"有限 $\mu$ 即可精确满足约束"。但这个论断不动手验一遍，总像在背结论。本节用一个一变量、一约束的最小例子，把外层乘子更新逐轮算出数字，亲眼看约束违反怎么被逼到零、乘子怎么爬向真值，并和"纯罚函数"对比，坐实 AL 的优势。

设定。 极小化 sos 目标 $\Phi(x)=(x-3)^2$（即一个特征 $\phi=x-3$，无约束最优在 $x=3$），但要满足硬等式约束 $g(x)=x-1=0$（强制 $x=1$）。两个愿望冲突：目标想去 3，约束钉在 1。增广拉格朗日：

\[ L_A(x;\lambda,\mu) = (x-3)^2 + \lambda(x-1) + \tfrac{\mu}{2}(x-1)^2. \]

内层有闭式解。 对 $x$ 求导置零：$\frac{\partial L_A}{\partial x} = 2(x-3) + \lambda + \mu(x-1) = 0$，解出

\[ x = \frac{6 - \lambda + \mu}{2 + \mu}. \]

（这里内层一步到位，因为目标和约束都是二次/线性——正是 §4.11 说的线性特征下 Gauss-Newton 精确。）

外层逐轮手算。 从 $\lambda=0,\ \mu=1$ 出发，每轮：解内层得 $x$ → 算约束违反 $g=x-1$ → 更新 $\lambda\leftarrow\lambda+\mu g$（§4.7 式 4.8）→ 增大 $\mu$。逐轮数字（保留三位）：

外层轮	$\lambda$（更新前）	$\mu$	内层解 $x=\frac{6-\lambda+\mu}{2+\mu}$	约束违反 $g=x-1$	更新后 $\lambda$
0	0	1	$7/3=2.333$	$1.333$	$1.333$
1	$1.333$	2	$6.667/4=1.667$	$0.667$	$2.667$
2	$2.667$	4	$7.333/6=1.222$	$0.222$	$3.556$
3	$3.556$	8	$10.444/10=1.044$	$0.044$	$3.911$
4	$3.911$	16	$18.089/18=1.005$	$0.005$	$3.991$

看三件事同时发生：约束违反 $g$ 一路降（$1.333\to0.667\to0.222\to0.044\to0.005$，朝零去）；乘子 $\lambda$ 爬向真值（$0\to1.333\to2.667\to3.556\to3.911\to3.991$）；而 $\mu$ 虽在增大，仍有限（到 16 就够把 $g$ 压到 $0.005$）。$\lambda$ 爬向的真值是多少？在真实 KKT 点（§4.7 式 4.10），$\nabla\Phi + \lambda^\star\nabla g = 0$，即 $2(x-3)+\lambda^\star=0$，在 $x=1$ 处给出 $\lambda^\star = 4$——正是 $\lambda$ 逼近的值。这就是 §4.7 式 4.9 vs 4.10 的对照在数字上的体现：乘子更新让 $\lambda+\mu g\to\lambda^\star$，于是 $g\to0$ 而 $\mu$ 不必到无穷。

和纯罚函数对比（AL 的优势，量化版）。 若去掉乘子（始终 $\lambda=0$，纯罚函数），内层解变成 $x=\frac{6+\mu}{2+\mu}$，约束违反 $g = \frac{4}{2+\mu}$。要把 $g$ 压到 $0.005$，需 $\frac{4}{2+\mu}=0.005$，即 $\mu\approx 798$！对比增广拉格朗日只用 $\mu=16$ 就达到同样精度——纯罚函数需要约 50 倍大的 $\mu$。而过大的 $\mu$ 会让内层 Hessian（§4.7 含 $\mu J_g^\top J_g$ 项）病态、数值求解困难（§4.7 陷阱 4-4 正是此理）。这一行数字，就是 §4.7"为什么要乘子、而非一味加大罚"的全部答案。

本质洞察：这张表把 §4.7 最核心、也最容易当口号背的那句话——"有限 $\mu$ 即可精确满足约束"——变成了你能复算的五行数字。两个要点值得焊进记忆：其一，乘子 $\lambda$ 不是辅助参数，而是问题的"影子价格"，它会自动爬向 KKT 乘子 $\lambda^\star$（这里是 4），爬到位时约束就满足了；纯罚函数没有这个机制，只能靠 $\mu\to\infty$ 硬压。其二，AL 用"$\lambda$ 收敛 + $\mu$ 适度"替代了"$\mu$ 爆炸"，从而避开数值病态——这就是几乎所有现代约束优化器（含 KOMO）默认用增广拉格朗日而非纯罚的根本原因。把 §4.11（GN 内层手算）和本节（AL 外层手算）合起来看，你就用纯手算走通了 §6.5 那个 numpy 实现的两层循环的每一步——代码不过是把这两张手算表，搬到几十维上自动重复而已。

⚠️ 常见陷阱¶

陷阱 4-1（概念误区）：以为 KOMO 的"k 阶"指轨迹的样条阶数或多项式次数。 - 错误描述：把 k 阶 Markov 理解成"用 k 次多项式拟合轨迹"。 - 现象/后果：误解 KOMO 的结构，无法理解 banded Hessian 从何而来。 - 根本原因："k 阶 Markov"指代价/约束依赖相邻 k+1 个时间步（速度 1 阶、加速度 2 阶），是时间上的局部依赖，与多项式次数无关。 - 正确做法：理解 k 阶 = 代价/约束的时间局部性（牵涉的连续时间步数），这直接决定 Hessian 的带宽。

陷阱 4-2（思维陷阱）：忽视初值，以为 Newton 法总能收敛到好解。 - 错误描述：随便给个初值就指望 KOMO 优化出全局最优轨迹。 - 现象/后果：陷入局部最优（如轨迹卡在障碍一侧、抓取姿势别扭），或不收敛。 - 根本原因：LGP/KOMO 的问题非凸（§2.3），Newton 类方法是局部优化，强烈依赖初值。 - 正确做法：用合理初值（直线插值、上一层界的解、运动学启发式）；§5 的 multi-bound 正是用粗层解为细层提供初值的策略之一。

陷阱 4-3（概念误区）：把 Gauss-Newton 当成标准 Newton 法，以为它也要算 Hessian。 - 错误描述：以为 Gauss-Newton 求解也需要目标的二阶导（完整 Hessian $\nabla^2\Phi$）。 - 现象/后果：要么真去算昂贵且可能不正定的完整 Hessian（违背 §4.4"只需雅可比"的设计），要么困惑"只给一阶导怎么做 Newton 步"。 - 根本原因：Gauss-Newton 用 $J^\top J$ 近似 Hessian（§4.6 式 4.3 扔掉了曲率项 $\sum_r\phi_r\nabla^2\phi_r$），只需一阶导 $J$；$J^\top J$ 还恒半正定，保证下降方向。 - 正确做法：记住 Gauss-Newton = 平方和专用、只需雅可比、$J^\top J$ 恒半正定——这正是 §4.4"特征只需提供值和雅可比"成立的前提。要更稳就加 LM 阻尼（式 4.5）。

陷阱 4-4（思维陷阱）：用纯罚函数代替增广拉格朗日，以为把 $\mu$ 调大就能满足硬约束。 - 错误描述：处理碰撞、切换一致性时，只加二次罚项 $\frac{\mu}{2}\|g\|^2$，靠调大 $\mu$ 逼近约束满足。 - 现象/后果：$\mu$ 调大则 $J^\top J$ 病态、收敛慢、解不准；$\mu$ 不够大则约束被违反——手没真正落到抓取位姿、轨迹擦着障碍走。 - 根本原因：纯罚函数只有 $\mu\to\infty$ 才精确满足（§4.7 式 4.6），任何有限 $\mu$ 都会为多降代价而容忍一点违反。 - 正确做法：用增广拉格朗日——在罚项外加乘子 $\lambda$ 并外层更新 $\lambda\leftarrow\lambda+\mu g$（式 4.8），让 $\lambda$ 逼近真乘子 $\lambda^\star$，从而有限 $\mu$ 下也能 $g(x^\star)\to 0$，避免病态。

陷阱 4-5（概念误区）：以为"带状 Hessian"是一种近似，丢掉了远距离时间步之间的耦合。 - 错误描述：以为把 $J^\top J$ 当带状处理，是"忽略相距远的时间步之间的二阶耦合"的近似手段。 - 现象/后果：担心带状求解不准、或试图人为加回"远距离耦合项"，把简单问题复杂化。 - 根本原因：在 k 阶 Markov 下，相距 $>k$ 的时间步之间的二阶导本来就精确为零（§4.8）——没有任何特征同时牵涉它们。带状捕捉了所有非零项，是精确稀疏，不是近似。 - 正确做法：理解带状是问题结构的精确反映（k 阶 → 半带宽 $kn$）；带状 Cholesky 解的是精确系统，没有任何近似损失。唯一的"近似"在 Gauss-Newton 扔曲率项那一步（陷阱 4-3），与带状无关。

练习¶

（⭐⭐） 解释为什么"惩罚加速度的平滑代价"是 2 阶 Markov 的。再问：如果代价惩罚 jerk（加加速度），是几阶？牵涉相邻几个时间步？
（⭐⭐⭐） 为什么 k 阶 Markov 结构会让 Hessian 是带状（banded）的？带状结构如何让 Newton 法每步求解的复杂度关于时间步数 $T$ 从立方降到线性？（提示：带状矩阵的线性求解可用带状 Cholesky。）
（⭐⭐⭐） §4.5 的表里，"持物阶段杯随手动"被表达为"运动学挂接（有效自由度）"。解释这相比"给杯子位姿加一堆等式约束"有什么好处（提示：减少变量、自动满足刚性连接）。
（⭐⭐⭐） §4.4 说"一切都是特征函数的平方和"。为"杯口朝上不洒水"这个约束设计一个特征函数：它从哪些构型算什么向量、是 eq 还是 ineq 类型、雅可比大致对什么求导？说明加这个约束为什么只需"插一个特征"而不必改求解器。
（⭐⭐⭐⭐，跨域对比） KOMO 和 iLQR/DDP 都是轨迹优化器，都用类 Newton 法、都利用时间稀疏。对比二者：(a) KOMO 在构型空间表示轨迹（不含速度相空间），iLQR 通常在状态-控制 $(x,u)$ 空间——这个区别有什么影响？(b) 为什么 KOMO 的"特征函数 + 增广拉格朗日"更适合处理 LGP 那种硬的等式/不等式约束（碰撞、切换一致性），而经典 iLQR 处理硬约束较吃力？（§4.10 已给出完整对比，先自己尝试，再回去对照。）
（⭐⭐⭐，数学） 从 $\Phi(x)=\phi(x)^\top\phi(x)$ 出发，亲手推导梯度 $\nabla\Phi = 2J^\top\phi$ 与 Hessian $\nabla^2\Phi = 2J^\top J + 2\sum_r\phi_r\nabla^2\phi_r$（写清每一步用了链式法则还是乘积法则）。再说明：对一个严格线性的特征（如加速度平滑 $\phi=x_{t-1}-2x_t+x_{t+1}$），为什么 Gauss-Newton 不是近似、而是精确？（提示：算它的 $\nabla^2\phi$。）
（⭐⭐⭐，数学） 对一个只含等式约束 $g(x)=0$ 的问题，写出增广拉格朗日 $L_A(x,\lambda;\mu)$。在内层最优点比较 $L_A$ 的稳定性条件（式 4.9）与真实 KKT 条件（式 4.10），论证为什么外层更新 $\lambda\leftarrow\lambda+\mu g$ 能让有限 $\mu$ 下 $g(x^\star)\to 0$。再用一句话说清这相比"纯罚函数靠 $\mu\to\infty$"好在哪。
（⭐⭐⭐⭐，综合） 估算复杂度：一条 $T=300$、单步构型维 $n=12$、2 阶 Markov（半带宽 $b=2n$）的轨迹。分别算稠密求解 $O((Tn)^3)$ 与带状 Cholesky $O(Tn\cdot b^2)$ 的数量级，给出加速比。再问：若把平滑阶从 2 阶（加速度）提到 3 阶（jerk），带宽 $b$ 和复杂度各怎么变？为什么仍然关于 $T$ 线性？
（⭐⭐⭐，手算） 仿照 §4.11，对特征 $\phi_1=x_1-2,\ \phi_2=x_2-x_1,\ \phi_3=x_2$（初值 $x^{(0)}=(0,0)$）手算一次 Gauss-Newton 迭代：写出 $J$、$J^\top J$、$J^\top\phi$，解正规方程得 $\Delta x$ 与 $x^{(1)}$，并验证 $x^{(1)}$ 处梯度是否归零。说明它为什么仍是一步到位。
（⭐⭐⭐⭐，手算） 仿照 §4.12，对目标 $\Phi=(x-5)^2$、约束 $g=x-2=0$ 手算增广拉格朗日：写出内层闭式解 $x(\lambda,\mu)$，从 $\lambda=0,\mu=1$ 起算三轮（每轮 $\mu$ 翻倍），列出 $x$、$g$、更新后 $\lambda$。$\lambda$ 在逼近哪个真值（先用 KKT 条件求 $\lambda^\star$）？再算：纯罚函数要把 $g$ 压到 0.01 需多大的 $\mu$，对比 AL 的 $\mu$。

5. 求解架构：树搜索 + 优化 + 多层下界 ⭐⭐⭐⭐¶

§3 给了 LGP 的问题结构，§4 给了几何层求解器 KOMO。但还有 §3.4 末尾的关键问题没回答：离散的动作序列 $a_{1:K}$ 怎么搜？ 优化处理不了离散选择，所以 LGP 需要一个符号层的树搜索来枚举动作骨架，再对每个骨架用 KOMO 验证几何可行性。本节讲这个"树搜索 + 优化"的两层架构，以及让它高效的 multi-bound 技术。这是 LGP 工程上能跑起来的关键。

5.1 两层架构：符号树 + 几何优化¶

LGP 的求解是两层嵌套：

LGP 两层求解架构:
┌─────────────────────────────────────────────────┐
│ 符号层: 动作序列树搜索                            │
│   节点 = 一个符号状态; 边 = 一个符号动作          │
│   从初始状态出发, 搜索到达目标的动作序列          │
│   每个候选序列 a_{1:K} 传给几何层验证             │
└────────────────────┬────────────────────────────┘
                    │ 候选动作序列
                    ▼
┌─────────────────────────────────────────────────┐
│ 几何层: KOMO 轨迹优化(§4)                         │
│   该序列生成一组约束(§3.3) → 组装 KOMO 问题       │
│   优化求解: 成功(找到可行轨迹) or 失败(不可行)    │
└────────────────────┬────────────────────────────┘
                    │ 可行性 + 代价 反馈
                    ▼
        符号层据此剪枝: 几何不可行的序列及其子树被剪掉

LGP 形式化的离散变量诱导出一棵搜索树，包含从初始状态 $s_0$ 出发的符号状态序列；叶节点（$g \subseteq s$，即目标满足）是解的候选；每个节点可以通过求解从根到该节点的状态序列所诱导的 NLP（非线性规划）来测试可行性。

这个架构的关键是双向：符号层向几何层提交候选序列，几何层向符号层反馈可行性。几何不可行的序列被剪枝——这正是 §3 陷阱 3-1 强调的"逻辑与几何双向耦合"，也是 LGP 区别于 plan-then-check 的核心。

5.2 朴素求解为什么慢：每个序列都要解一个 NLP¶

朴素的两层求解有个致命开销：每测试一个候选动作序列，都要解一个完整的 KOMO 轨迹优化（NLP）。而轨迹优化（尤其长序列、精细时间步）是昂贵的——一个序列可能要解几秒。符号树又可能有成百上千个候选序列。全部精确求解，慢到不可用。

在标准求解器（Toussaint and Lopes 2017）中，树是逐节点探索的，每个节点通过求解其诱导的 NLP 来测试可行性——如果每个 NLP 都是完整精度，开销巨大。这就引出了 LGP 的关键加速技术：multi-bound tree search（多层下界树搜索）。

5.3 multi-bound：用多层下界由粗到精剪枝¶

multi-bound tree search（Toussaint & Lopes 2017）的核心思想：不要对每个候选序列都解完整精度的 NLP，而是先用便宜的、粗糙的"下界"问题快速筛掉不可行的，只对通过粗筛的少数序列才解完整精度。

关键在于这些下界问题满足：如果一个粗糙下界问题都不可行，那么完整问题必然不可行（下界不可行 → 原问题不可行）。所以可以用便宜的下界做必要条件检查，快速剪枝。每个松弛问题的可行性是原始 NLP 可行性的必要条件，即充当下界，同时计算上快得多。

Toussaint & Lopes 设计了三个层级的界，由粗到精：

multi-bound 三层(由粗到精, Toussaint & Lopes 2017):
  P1(最粗): 不优化整条序列, 只优化"有效运动学"——
           节点构型 + 其父节点构型两个构型, 父构型按其有效运动学优化
           最便宜, 第一道筛子
  P2(中):   和 P3 同样的 KOMO 问题, 但时间分辨率粗得多——
           一个符号决策只用两个时间步(关键帧 + 一个中间帧)
           例: 5 个操作决策 → 只 10 个构型的 NLP, 优化很快
  P3(最精): 反映原始 LGP 的精细时间步全路径优化
           最贵, 只对通过 P1、P2 的序列才解

P3 是反映原始 LGP 的精细时间步全路径优化；P2 是与 P3 完全相同的 KOMO 问题，但时间分辨率粗得多——一个符号决策只用两个时间步，即只优化关键帧加一个中间帧（例如 5 个操作决策的序列只是一个 10 个构型的 NLP，优化起来相对快）；最粗的界 P1 完全不优化序列，依赖"有效运动学"概念，只优化与节点关联的构型及其父节点构型。

本质洞察：multi-bound 的智慧，是把"昂贵的可行性检查"做成一道道由便宜到昂贵的筛子——绝大多数不可行的动作序列，在最便宜的 P1（只看两个构型够不够得到）就被筛掉了，根本不必解昂贵的全路径优化 P3。这与你在搜索算法里见过的"可容许启发式剪枝"（A* 用便宜的下界估计剪掉无望节点）是同一个思想，也与 PDDLStream 的"乐观对象"惰性思路（TAMP_T3 §5.2，先用假想值规划，需要才真正采样）异曲同工——都是"先用便宜的近似探路，只在必要时才付昂贵的精确代价"。下界的"可行性是原问题可行的必要条件"这一性质，保证了剪枝不会误杀真正可行的序列（粗层可行才进细层，粗层不可行必然原问题不可行）。这是 LGP 能在合理时间内求解的工程命脉。

5.4 完整求解流程¶

把两层架构和 multi-bound 合起来，LGP 的完整求解流程：

LGP 完整求解(multi-bound tree search):
  符号树搜索, 对每个候选动作序列(从根到某叶):
    1. 解 P1(最粗下界): 不可行 → 剪掉该序列及子树, 继续
    2. 通过 P1 → 解 P2(中等): 不可行 → 剪掉, 继续
    3. 通过 P2 → 解 P3(完整精度): 
         可行 → 找到解! 返回完整轨迹(含动作参数)
         不可行 → 剪掉, 继续
  结合 branch-and-bound / MCTS 思路引导树搜索顺序(优先展开有希望的分支)

Toussaint & Lopes 提出一个新的近似求解器方案，结合 branch-and-bound 和 MCTS 的思想，利用多个层级的界来更好地引导搜索。也就是说，multi-bound 不仅用于剪枝，还用于引导搜索顺序——优先展开下界代价低（更有希望）的分支。

5.5 剪枝追踪：一个具体例子走一遍¶

抽象的"三层下界"不容易有体感。本节用一个具体场景，追踪 multi-bound 实际怎么剪枝。

场景：机械臂要把桌上的杯子放到一个较高的架子上。符号层有几种候选动作序列（不同的抓取方式、是否需要中间重新抓握）。我们看 multi-bound 怎么快速筛掉不可行的。

multi-bound 剪枝追踪(放杯子到高架子):

候选序列 A: [直接 pick → place 到高架]
  P1(最粗, 只看两构型够不够到):
    检查: 抓取构型够到杯子? ✓  放置构型够到高架? ✗ (架子太高, 单臂够不到)
    → P1 不可行 → 剪掉序列 A 及其所有子序列   (省了 P2、P3 的昂贵优化!)

候选序列 B: [pick → 中途换站位 → place 到高架]
  P1(最粗):
    检查: 各关键构型两两够得到? ✓ (换站位后能够到高架)
    → P1 可行 → 进入 P2
  P2(中等, 粗时间分辨率, 每决策2步):
    解一个 ~6 构型的粗 NLP: 轨迹大体可行? 无明显碰撞? ✓
    → P2 可行 → 进入 P3
  P3(完整精度, 精细时间步全路径):
    解完整 KOMO: 找到平滑无碰的完整轨迹 ✓
    → P3 可行 → 返回解!

候选序列 C: [pick → place, 但抓取方式导致放置时手与架子碰]
  P1: 够得到检查通过 ✓ → P2
  P2(粗 NLP): 粗略优化已显示放置段手与架子碰撞 ✗
    → P2 不可行 → 剪掉序列 C   (没付 P3 完整优化的代价)

这个追踪揭示了 multi-bound 的效率来源： - 序列 A 在最便宜的 P1（只检查两个构型够不够得到）就被剪掉——根本没碰昂贵的轨迹优化。多数"明显不可行"（够不到）的序列都死在这里。 - 序列 C 通过了 P1（够得到），但在中等成本的 P2（粗轨迹优化）暴露了碰撞——比完整的 P3 便宜得多就被剪掉。 - 只有真正可行的序列 B 才付出完整 P3 优化的代价。

本质洞察：multi-bound 的效率本质是把昂贵的判断推迟、把便宜的判断提前——用 P1（两构型够不够得到，几乎免费）筛掉大批"够不到"的序列，用 P2（粗轨迹，中等成本）筛掉"会碰撞/动力学不可行"的序列，只对闯过两关的少数序列付 P3（完整优化）的昂贵代价。这与数据库查询优化的"谓词下推"（先用便宜的过滤条件减少数据量，再做昂贵的连接）、或短路求值（cheap() && expensive()，便宜的假了就不算贵的）是同一种智慧。关键前提是 §5.3 强调的下界的必要条件性质：P1/P2 不可行 → 原问题必不可行，所以提前剪枝是安全的，不会误杀真正可行的序列（序列 B 闯过每一关正因为它真的可行）。这是"由粗到精、便宜优先"能既快又正确的根本保证。

5.6 multi-bound 的效率从哪来：一个定量估算 ⭐⭐⭐¶

§5.3–§5.5 从定性角度讲了 P1/P2/P3 怎么由粗到精剪枝，§5.5 还走了一个具体例子。但"为什么快"到底快多少？本节给一个定量估算，用数字坐实那份直觉，也帮你判断"我的问题值不值得上 multi-bound"。

取一个有代表性的规模。 设符号树有 $N = 1000$ 个候选动作序列（叶节点）。三层界各自一次求解的成本（用相对单位）：P1 ≈ 1、P2 ≈ 10、P3 ≈ 100——P3 是精细时间步的全路径 NLP，最贵，比只看两个构型的 P1 贵约两个数量级，这个比例对长轨迹任务是现实的。剪枝率（经验性）：P1 筛掉 80%（大批"够不到"的序列在最便宜处就死），通过 P1 的序列里 P2 再筛掉 80%。

两种策略的成本账。

朴素(对每个候选都解完整 P3):
  1000 个序列 × 100 (P3 成本) = 100,000 单位

multi-bound(由粗到精):
  P1 全跑:        1000 × 1   = 1,000
  通过 P1 的 200 个跑 P2:  200 × 10  = 2,000
  通过 P2 的 40 个跑 P3:    40 × 100 = 4,000
  合计                            = 7,000 单位

加速比 ≈ 100,000 / 7,000 ≈ 14×

朴素方法对所有序列硬解 P3，花 $100{,}000$ 单位；multi-bound 让绝大多数序列死在便宜的 P1/P2，只有 40 个真正可行的候选才付 P3 的代价，合计 $7{,}000$ 单位——快约 14 倍。

这个账揭示两条规律。

规律一：P3/P1 成本比越大，省得越多。 全精度优化越贵（相对粗界），multi-bound 的价值越大。长时域、精细时间步的任务，P3 极贵（一次几秒），multi-bound 几乎是必需品；反之若 P3 本就便宜，剪枝省下的不多。
规律二：不可行序列占比越高，省得越多。 越多"明显不可行（够不到）"的序列死在最便宜的 P1，收益越大。这恰好说明 multi-bound 在"候选多、且多数不可行"的难问题上最闪光——而这正是 §5.2 描述的 TAMP 典型困境（符号树大、几何可行的序列稀疏）。

一个反面提醒。 如果几乎所有序列都几何可行（TAMP 里罕见），那 multi-bound 的级联反而增加开销——因为每个序列都得先付 P1+P2 才轮到 P3，等于在 P3 之外白交了过路费。multi-bound 隐含假设了 TAMP 的常态：多数符号骨架在几何上不可行。这也是 §5.3 反复强调"下界的必要条件性质"的原因——正是它让"提前剪枝"既安全又划算。

本质洞察：这个数字账把 §5.3–§5.5 的定性直觉坐实了：multi-bound 的加速不是均匀的常数倍，而是随问题变难而放大——候选越多、全精度优化越贵、不可行序列占比越高，它省得越多。这解释了一个看似矛盾的现象：multi-bound 在简单问题上几乎无用（甚至略增开销，因为闯关也要成本），却在最难的长时域富接触问题上不可或缺。这与许多"分层筛选"机制的特性一致（CPU 的多级 cache、网络的多级限流、招聘的多轮筛选）——筛选层的价值，正比于它替你省掉的"昂贵操作"的量。 所以用 multi-bound 的正确姿势是先问一句：我的问题是不是"候选多、多数不可行、全解很贵"？是，它就是命脉；不是，朴素求解可能就够，别为级联本身的开销买单。

5.7 符号树搜索的内部：分支、引导与经典规划接口 ⭐⭐⭐¶

§5.1–§5.6 把几何层（KOMO）和剪枝（multi-bound）讲透了，但"符号树搜索"本身一直当黑箱用——它到底怎么搜？本节补上这一层。理解它，你才知道 LGP 的组合复杂度卡在哪、为什么 Diverse-LGP 与学习引导这些前沿都在改它。

树的结构：节点、边、分支因子。 回顾 §5.1：节点 = 一个符号状态（含命题集合 + 运动学模式，§3.5），边 = 一个符号动作（grasp / place / …）。从根 $s_0$ 出发，每施加一个可用动作就生成一个子节点；到达满足目标 $g$ 的节点，就得到一个候选动作序列（根到该节点的路径）。分支因子 = 每个状态下可用（前提满足）的动作数。操作领域里这个数可能很大：$N$ 个物体 × $M$ 种动作 × 参数化的离散选择（抓哪个、放哪），轻易上百。深度 = 序列长度 $K$。候选序列数 $\sim$ 分支因子$^K$——这就是 §5.2"成百上千候选"、§9.1 局限二"长序列组合爆炸"的来源。

怎么定展开顺序：branch-and-bound + MCTS。 不能盲目穷举这棵指数树。§5.4 提过 Toussaint & Lopes 用 branch-and-bound 结合 MCTS 引导：

branch-and-bound：用 multi-bound 的下界 $V_{\text{P1}}/V_{\text{P2}}$（§3.6 的 $V$ 的下界）当"界"——下界已经很差（代价高）的分支，优先级调低甚至直接剪。
MCTS（蒙特卡洛树搜索）：像下棋 AI 那样用"选择-展开-评估-回传"在树里有偏好地采样，把计算预算投向更有希望的分支，而非均匀铺开。

两者合起来：树搜索不是 BFS/DFS 地穷举，而是被下界和价值估计引导着优先钻探"既可能可行、代价又低"的分支。

与经典符号规划器的接口。 §3.5 的本质洞察说过：LGP 的符号层继承 PDDL。所以这棵树的搜索，可以直接复用经典 AI 规划的成熟机器：

用现成的 PDDL 规划器（如 Fast Downward）生成候选符号计划，再交几何层验证——Diverse-LGP（§10.2）正是这么做：用"多样化规划"一次生成一组差异大的候选序列，避免树搜索在相似的、都不可行的序列上空转。
经典规划的启发式（如 $h_{\text{FF}}$、landmark）可引导符号搜索朝目标走——RHH-LGP（§8.2）的"几何启发式"就是把几何可行性信息注入这一层。

这就是为什么 §3.5 强调 LGP "在 PDDL 骨架上挂几何"如此值钱：符号层不必重造搜索轮子，几十年的经典规划成果直接可用。

组合复杂度卡在哪：这一层，不是几何层。 一个常见误解是以为 LGP 慢在 KOMO（几何优化）。其实在 multi-bound 之后，单个序列的几何验证已被便宜的下界挡掉大半（§5.6）；真正的指数瓶颈在符号树本身——序列数随长度指数增长。所以 LGP 提速的前沿，多在改这一层：Diverse-LGP 改探索策略、RHH-LGP 用接收时域把长序列截成短窗口（§8.2）、学习方法预测哪些符号分支值得展开（§9.2）。看清"瓶颈在符号搜索的组合性、而非单次几何优化"，你才知道该往哪使劲。

本质洞察：把符号树搜索这层揭开，你会发现 LGP 的两层其实是"两个领域的联姻"：符号层是经典 AI 规划（离散搜索、启发式、PDDL），几何层是数值最优化（KOMO、Gauss-Newton、增广拉格朗日）。LGP 的工程智慧，很大一部分在于让这两个本来各说各话的领域高效协作——符号层用经典规划的成熟搜索快速提议骨架，几何层用优化验证并反馈，multi-bound 做两层之间廉价的"过滤翻译"。这解释了为什么 §3.5 反复强调"挂在 PDDL 上而非另起炉灶"：正因符号层复用了经典规划，LGP 才能把创新集中在"如何让逻辑生成几何约束、如何高效验证"上，把符号搜索这件难而成熟的事外包出去。对你的启示是：理解任何"符号 + 数值"的混合系统（不止 LGP），都要分别看清它的符号引擎和数值引擎各是什么、它们怎么对接——抓住这个接口，就抓住了系统的关键。

⚠️ 常见陷阱¶

陷阱 5-1（概念误区）：以为 LGP 是"纯优化一键求解"，没有搜索。 - 错误描述：以为把问题写成 §3.4 的优化式后，丢给优化器就出解。 - 现象/后果：无法理解离散动作序列怎么定，或困惑于"优化怎么处理离散选择"。 - 根本原因：离散动作序列 $a_{1:K}$ 要靠符号层树搜索枚举（§5.1），优化只解决每个序列的连续部分。LGP = 树搜索 + 优化。 - 正确做法：牢记两层架构——符号树搜索枚举骨架，几何优化（KOMO）验证每个骨架。

陷阱 5-2（思维陷阱）：对每个候选序列都解完整精度 NLP。 - 错误描述：实现 LGP 时，每个动作序列都直接解完整的 KOMO（P3）。 - 现象/后果：求解极慢——大量不可行序列也付了昂贵的全路径优化代价。 - 根本原因：完整 NLP 昂贵，而多数序列不可行（§5.2）。不用多层下界剪枝，开销爆炸。 - 正确做法：用 multi-bound——先解便宜的 P1/P2 下界快速剪枝，只对通过的序列解 P3（§5.3）。

陷阱 5-3（概念误区）：误以为下界 P1/P2 的"可行"就代表原问题可行。 - 错误描述：以为通过了 P1 或 P2 就找到解了。 - 现象/后果：把"下界可行"当成"原问题可行"，得到实际不可行的"解"。 - 根本原因：下界的可行性只是原问题可行的必要条件（下界不可行→原问题必不可行；但下界可行 ≠ 原问题可行）。只有 P3（完整精度）可行才是真解。 - 正确做法：P1/P2 只用于剪枝（排除不可行的），最终可行性必须由 P3 确认。

练习¶

（⭐⭐） 用自己的话解释 LGP 的两层架构：符号层和几何层各做什么、如何交互（提交什么、反馈什么）。为什么说这个"可行性反馈"让 LGP 不是 plan-then-check？
（⭐⭐⭐） 解释 multi-bound 的剪枝为什么"安全"（不会误杀可行序列）：用"下界可行性是原问题可行的必要条件"这一性质论证。再说明为什么 P1 比 P3 便宜得多。
（⭐⭐⭐） 对照 TAMP_T3 §5.2 的乐观对象惰性思路：PDDLStream"先用假想值规划、需要才采样"，LGP"先解粗下界、通过才解精确"。这两种"先便宜近似、必要才精确"的策略，分别在避免什么浪费？
（⭐⭐⭐⭐，综合） 一个搭 5 层积木塔的任务，符号树有很多种放置顺序。描述 multi-bound 如何工作：哪些不可行顺序会在 P1 被剪掉？为什么 P2（10 个构型的粗 NLP）能在不解完整轨迹的情况下，识别出"这个顺序大体可行"？
（⭐⭐⭐，数学） 用 §5.6 的成本模型重算：若 P1 剪枝率提到 90%（其余设定不变：$N=1000$，P1/P2/P3 成本 1/10/100，P2 剪掉通过者的 80%），multi-bound 总成本与加速比是多少？再把 P3 成本从 100 提到 500（更精细的长轨迹），加速比又如何变？这两个变化分别对应 §5.6 的哪条规律？
（⭐⭐⭐，分析） 据 §5.7：(a) 若一个领域有 $N=6$ 个物体、每步约 20 个可用动作，长度 $K=8$ 的候选序列数量级是多少？这说明瓶颈在符号层还是几何层？(b) 解释 branch-and-bound 与 MCTS 各自如何避免盲目穷举这棵树。(c) Diverse-LGP 用"多样化规划"一次生成一组差异大的候选序列，为什么这比逐个展开相似序列更高效？

6. 完整案例：从搭积木塔看 LGP 全流程 ⭐⭐⭐¶

前面分别讲了 LGP 的结构（§3）、KOMO 求解器（§4）、树搜索架构（§5）。本节用 Toussaint 原始论文的标志性例子——搭积木塔——把它们串起来，并给一个可运行的概念性最小实现，让"逻辑生成约束 + 优化求解"真正落地。

6.1 任务：搭一座塔¶

任务很简单描述、却很考验 TAMP：桌上有若干积木，要把它们叠成一座塔。在塔问题中，一摞物体是否物理稳定，取决于物体之间如何相互支撑；在末构型的符号表示 $s_K$ 中，每当物体 X 支撑物体 Y（从下方），就包含谓词 (support X Y)；目标函数 $\psi(x(T))$ 的设计主要考察这些支撑关系。

这个例子精准体现了 §2.2 说的"全局耦合长序列"——下层积木放偏一点，上层就会塌。每块积木的放置位置不能孤立决定，必须考虑整座塔的稳定。这正是 LGP（全局联合优化）相对 PDDLStream（逐步采样）的主场。

6.2 LGP 怎么解这个塔¶

按 §3-§5 的机制走一遍：

搭积木塔的 LGP 求解:
  符号层(§5 树搜索): 枚举放置顺序
    [放A] → [放B在A上] → [放C在B上] → ...  (一个动作序列)
  每个序列生成约束(§3.3):
    切换约束: 每次放置时, 积木位姿 = 目标叠放位姿(在下方积木正上方)
    阶段约束: 抓握保持、不碰已搭部分、每步之后塔要稳定
    动作参数: 每块积木的精确放置位姿(x,y偏移、朝向)= 优化变量(§3.2)
  末态评价 ψ(§6.1): 衡量塔搭成且稳定(支撑关系满足、重心对齐)
  几何层(§4 KOMO): 对每个放置顺序, 优化所有积木的放置位姿 + 整条轨迹
    使塔稳定(重心在支撑面内)、轨迹平滑无碰
  multi-bound(§5.3): 不稳的放置顺序在粗下界就被剪掉

关键体会：LGP 优化每块积木放置位姿时，是全局协同的——它会让下层积木放在能支撑整座塔重心的位置，因为末态评价 $\psi$ 把"整座塔稳定"作为目标。这是采样难以做到的全局性。

6.3 概念性最小实现：一个"逻辑约束 + 优化"的玩具¶

真实的 LGP 用 KOMO（需要专门的优化库和机器人模型），本地难跑。但 LGP 的核心思想——"逻辑决定约束、优化求解连续参数"——可以用一个极简的玩具完整演示。下面用 scipy.optimize 解一个一维"叠塔"问题：把 3 块积木叠起来，优化每块的水平位置，使塔尽量稳（每块重心尽量对齐下方积木中心），同时满足"叠放"的逻辑约束。

import numpy as np
from scipy.optimize import minimize

# ===== 任务: 3 块积木叠塔, 优化每块的水平位置 x1,x2,x3 =====
# 逻辑序列(符号层给定): 放block0(底) → 放block1(在0上) → 放block2(在1上)
# 这个序列"生成"以下约束与代价(对应 §3.3):

BLOCK_W = 1.0          # 积木宽度
GROUND_X = 0.0         # 底座中心(block0 期望在此附近)

def stack_cost(x):
    """代价 = 末态评价 ψ: 塔越稳代价越小。
    稳定 ≈ 每块积木重心尽量对齐其下方积木中心(§6.1 的支撑关系)。"""
    x0, x1, x2 = x
    # 相邻积木中心错位的平方和(错位越小越稳)——对应 ψ 对支撑关系的考察
    misalign = (x1 - x0)**2 + (x2 - x1)**2
    # 底块尽量靠近底座中心(可选的偏好)
    base = 0.1 * (x0 - GROUND_X)**2
    return misalign + base

def stability_constraints(x):
    """阶段约束(§3.3): 每块积木重心必须落在下方积木支撑面内, 否则塔倒。
    返回值 ≥ 0 表示满足(scipy 不等式约束约定)。"""
    x0, x1, x2 = x
    half = BLOCK_W / 2
    # block1 重心 x1 必须在 block0 顶面 [x0-half, x0+half] 内
    c1_left  = (x1 - (x0 - half))      # ≥0: x1 不超出左边
    c1_right = ((x0 + half) - x1)      # ≥0: x1 不超出右边
    # block2 重心 x2 必须在 block1 顶面内
    c2_left  = (x2 - (x1 - half))
    c2_right = ((x1 + half) - x2)
    return np.array([c1_left, c1_right, c2_left, c2_right])

# ===== 求解: 这就是"给定逻辑序列后的几何优化"(KOMO 在真实 LGP 里做的事) =====
x_init = np.array([0.5, -0.3, 0.8])    # 一个不太稳的初始猜测(故意错位)
cons = [{"type": "ineq", "fun": stability_constraints}]
result = minimize(stack_cost, x_init, constraints=cons, method="SLSQP")

print(f"优化前位置: {x_init}, 代价 {stack_cost(x_init):.4f}")
print(f"优化后位置: {result.x}, 代价 {stack_cost(result.x):.4f}")
print(f"约束满足(都≥0): {stability_constraints(result.x)}")
# 验证: 优化后三块应近乎对齐(都接近同一 x), 塔最稳
aligned = np.allclose(result.x, result.x[0], atol=0.05)
print(f"三块是否对齐(最稳): {aligned}")

运行结果：优化器从一个错位的初始猜测出发，在"每块必须落在下方支撑面内"的约束下，把三块积木的水平位置调到近乎对齐（最稳）。这个玩具虽小，但完整体现了 LGP 的内核：

逻辑生成约束：放置顺序（符号序列）决定了"谁支撑谁"，从而生成 stability_constraints（每块落在下方支撑面内）——对应 §3.3 的阶段约束。
动作参数是优化变量：每块的水平位置 $x_0, x_1, x_2$ 是优化变量（对应 §3.2 的动作参数），由优化器解出，不是采样的。
末态评价 ψ：stack_cost 衡量塔的稳定性（重心对齐），对应 §6.1 的 $\psi$。
优化求解：minimize（SLSQP，带约束优化）扮演 KOMO 的角色——在约束下最小化代价。

本质洞察：这个 30 行玩具和真实 LGP 的差距，恰好标出了真实 LGP 多做了什么：(1) 变量——这里只有 3 个标量位置，真实 LGP 优化整条轨迹（成百上千个构型）+ 所有动作参数；(2) 约束——这里只有支撑面约束，真实 LGP 还有碰撞、动力学、切换一致性、抓握保持；(3) 求解器——这里用通用 SLSQP，真实 LGP 用 KOMO（利用 k 阶 Markov 带状结构高效解长轨迹，§4.2）；(4) 符号层——这里放置顺序写死了，真实 LGP 用树搜索枚举顺序 + multi-bound 剪枝（§5）。但内核完全一致：逻辑定义约束、动作参数作优化变量、优化器在约束下求最优。读懂这个玩具，你就抓住了 LGP 的灵魂——剩下的都是把每个部件换成更强的实现。

6.4 进阶玩具：带轨迹与切换约束的 pick-and-place¶

§6.3 的叠塔玩具只优化了静态位置，没有轨迹。本节给一个更接近真实 LGP 的玩具——优化一条离散轨迹，让末端执行器从起点出发，在指定时刻"抓取"（经过杯子位姿）、再在指定时刻"放置"（经过目标位姿），其间走最平滑的路。这把 §3.3 的切换一致性约束和 §4.2 的 k 阶 Markov 平滑代价都落地了。

import numpy as np
from scipy.optimize import minimize

# ===== 2D pick-and-place 的 LGP 式轨迹优化 =====
# 末端执行器在 2D 平面, 轨迹离散为 T 步(对应 §4.1 的离散化)。
# 任务: 起点 → (t_grasp 时刻)抓取杯子 → (t_place 时刻)放到目标。
T = 20                              # 轨迹步数
DIM = 2
t_grasp, t_place = 8, 16            # 抓取/放置的切换时刻
ee_start  = np.array([0.0, 0.0])    # 末端起点
cup_pos   = np.array([2.0, 1.0])    # 杯子位置(抓取目标)
place_pos = np.array([4.0, 0.5])    # 放置目标

def unpack(z): return z.reshape(T, DIM)

def cost(z):
    """代价 = 平滑(加速度平方和)——这正是 §4.2 的 2 阶 Markov 代价。"""
    x = unpack(z)
    acc = x[2:] - 2*x[1:-1] + x[:-2]        # 二阶差分 ≈ 加速度
    return np.sum(acc**2)

def eq_constraints(z):
    """等式约束: 起点 + 两个切换一致性约束(§3.3 的 h_switch)。"""
    x = unpack(z)
    c = []
    c.extend(x[0] - ee_start)               # 起点固定
    c.extend(x[t_grasp] - cup_pos)          # 切换1: 抓取时刻末端 = 杯位置
    c.extend(x[t_place] - place_pos)        # 切换2: 放置时刻末端 = 目标
    return np.array(c)

# 初值: 直线插值(§4.2 陷阱 4-2 强调好初值的重要)
z0 = np.concatenate([ee_start + (place_pos-ee_start)*i/(T-1) for i in range(T)])
cons = [{"type": "eq", "fun": eq_constraints}]
res = minimize(cost, z0, constraints=cons, method="SLSQP",
               options={"maxiter": 200, "ftol": 1e-8})

x = unpack(res.x)
print(f"抓取点(t={t_grasp}): {np.round(x[t_grasp],2)} (应≈[2,1])")
print(f"放置点(t={t_place}): {np.round(x[t_place],2)} (应≈[4,0.5])")
print(f"等式约束最大违反: {np.max(np.abs(eq_constraints(res.x))):.2e}")
# 输出:
#   抓取点(t=8): [2. 1.]
#   放置点(t=16): [4.  0.5]
#   等式约束最大违反: 0.00e+00

运行结果：优化器解出一条 20 步轨迹，自动"弯"过杯子位置（t=8 抓取）再到目标（t=16 放置），三个切换约束都精确满足，其间走最平滑的路。这个玩具比叠塔多了三个真实 LGP 的要素：

离散轨迹作变量（§4.1）：决策变量是 20 个时间步的位置，不再是 3 个静态标量——这就是 KOMO 优化的"构型序列"的缩影。
切换一致性约束（§3.3）：eq_constraints 强制轨迹在 t=8、t=16 经过指定位姿——这正是 $h_{\text{switch}}$（抓取时刻末端=抓取位姿）。
k 阶 Markov 平滑代价（§4.2）：cost 用二阶差分（加速度）平方和，正是 2 阶 Markov 代价，每项只依赖相邻 3 步。

本质洞察：把 §6.3（静态位置）和 §6.4（轨迹 + 切换）合起来看，你就拼出了真实 LGP 的两大支柱——动作参数优化（叠塔的位置、本节可扩展的抓取位姿）和轨迹优化 + 切换约束（本节的平滑路径 + 经过切换点）。真实 LGP 把这两者合进一个优化问题：轨迹、动作参数、切换约束、阶段约束全部联立，由 KOMO 一次求解。本节为了清晰把抓取位姿固定了（设为杯位置），但你在练习里会把它也变成优化变量——那时就完整复现了 §3.2"动作参数与轨迹联合优化"的精髓：优化器同时决定"抓哪里"和"怎么走过去"。从这两个玩具到真实 LGP，缺的只是"更多维度（真实构型空间）、更多约束（碰撞/动力学）、更强求解器（KOMO 的带状结构）、加一层符号树搜索（§5）"——内核你已经握在手里了。

6.5 从零实现一个迷你 KOMO 求解器：Gauss-Newton + 增广拉格朗日 ⭐⭐⭐⭐¶

§6.3 和 §6.4 的玩具都把求解交给 scipy.minimize 这个黑箱——你看到了"逻辑约束 + 优化求解"的结构，却没看到 §4.6/§4.7 的求解器内核怎么转。本节打开黑箱：用纯 numpy 从零实现一个迷你 KOMO，亲手写出 Gauss-Newton 内层（§4.6 的正规方程）和 增广拉格朗日外层（§4.7 的乘子更新），在 §6.4 那个 2D pick-and-place 上跑。跑通它，你就把 §4 的数学和 §6 的落地真正接上了。

import numpy as np

# ===== 从零实现迷你 KOMO: Gauss-Newton(§4.6) + 增广拉格朗日(§4.7) =====
# 2D 轨迹, T 步; sos 代价=加速度平滑; 硬等式约束=起点+抓取点+放置点(切换一致性,§3.3)
T, DIM = 12, 2
t_grasp, t_place = 5, 10
ee_start  = np.array([0.0, 0.0])
cup_pos   = np.array([2.0, 1.0])
place_pos = np.array([4.0, 0.5])
n = T * DIM                                        # 决策变量总维度

def smooth_residual_and_J(x):
    """sos 特征: 加速度二阶差分 φ_s = x_{t-1}-2x_t+x_{t+1} (§4.9 式4.15, 线性特征)。"""
    X = x.reshape(T, DIM)
    acc = X[2:] - 2*X[1:-1] + X[:-2]               # (T-2, DIM) 加速度
    phi = acc.reshape(-1)
    Js = np.zeros((phi.size, n))                   # 雅可比(常数, 因特征线性)
    for i in range(T-2):
        for d in range(DIM):
            r = i*DIM + d
            Js[r, (i)*DIM + d]   += 1.0            # 对 x_{t-1} 的导数 = I
            Js[r, (i+1)*DIM + d] += -2.0           # 对 x_t   的导数 = -2I
            Js[r, (i+2)*DIM + d] += 1.0            # 对 x_{t+1} 的导数 = I
    return phi, Js

def constraint_residual_and_J(x):
    """硬等式 g(x)=0: 起点固定 + 两个切换一致性约束(§3.3 的 h_switch)。"""
    X = x.reshape(T, DIM)
    g = np.concatenate([X[0]-ee_start, X[t_grasp]-cup_pos, X[t_place]-place_pos])
    Jg = np.zeros((g.size, n))
    for k, t in enumerate([0, t_grasp, t_place]):  # 每个约束只碰一个时间步(0阶)
        for d in range(DIM):
            Jg[k*DIM + d, t*DIM + d] = 1.0
    return g, Jg

def solve_inner_GN(x, lam, mu, iters=5):
    """内层: 固定 λ,μ, 用 Gauss-Newton 极小化增广拉格朗日 L_A (§4.6)。"""
    for _ in range(iters):
        phi_s, Js = smooth_residual_and_J(x)
        g, Jg     = constraint_residual_and_J(x)
        # 增广拉格朗日 L_A = ‖φ_s‖² + λᵀg + (μ/2)‖g‖² 的梯度与 GN 近似 Hessian:
        grad = 2*Js.T @ phi_s + Jg.T @ (lam + mu*g)            # ∇L_A (§4.7 式4.9)
        H    = 2*Js.T @ Js + mu * (Jg.T @ Jg)                  # GN 近似 Hessian
        H   += 1e-9 * np.eye(n)                                # 轻微 LM 阻尼(§4.6 式4.5)
        dx   = np.linalg.solve(H, -grad)                       # 解正规方程(§4.6 式4.4)
        x    = x + dx                                          # 线性问题, 步长取 1
    return x

# ===== 外层: 增广拉格朗日的乘子更新循环 (§4.7 式4.8) =====
x   = np.zeros(n)                                  # 初值: 全零(故意差)
lam = np.zeros(3*DIM)                              # 拉格朗日乘子
mu  = 1.0                                          # 罚权重(从小开始)
print("外层   约束最大违反    ‖g‖")
for outer in range(7):
    x = solve_inner_GN(x, lam, mu)                 # 内层 GN 解一个无约束子问题
    g, _ = constraint_residual_and_J(x)
    print(f"  {outer}      {np.max(np.abs(g)):.3e}    {np.linalg.norm(g):.3e}")
    lam = lam + mu * g                             # 乘子更新: λ ← λ + μg  (关键!)
    mu  = min(mu*3, 1e4)                            # 适度增大罚权重(不必到无穷)

X = x.reshape(T, DIM)
print(f"\n抓取点(t={t_grasp}): {np.round(X[t_grasp],3)} (应≈[2,1])")
print(f"放置点(t={t_place}): {np.round(X[t_place],3)} (应≈[4,0.5])")
# 典型输出: 约束最大违反随外层迭代从 ~1e0 一路降到 ~1e-7,
#           说明乘子更新让有限 μ 也把硬约束精确满足了 —— 正是 §4.7 的论断。

这段代码逐块对应 §4 的数学，把它读成"§4 的可执行版"：

smooth_residual_and_J 是一个 sos 特征（§4.4）——加速度二阶差分（§4.9 式 4.15），它的雅可比是常数 $[I,-2I,I]$，正因线性，Gauss-Newton 对它精确（§4.6）。
constraint_residual_and_J 是 §3.3 的切换一致性硬等式——起点、抓取点、放置点必须精确命中（$h_{\text{switch}}=0$）。每个约束只碰一个时间步，是 0 阶特征。
solve_inner_GN 是 §4.6 的内层：grad 和 H 严格按 §4.7 式 4.9 与 GN 近似写出，np.linalg.solve(H, -grad) 就是在解正规方程式 4.4，那行 1e-9*np.eye 就是 LM 阻尼式 4.5。
外层 lam = lam + mu*g 是 §4.7 式 4.8 的乘子更新——全段代码的灵魂。正是它，让你看到约束违反逐轮下降到 $10^{-7}$，而 $\mu$ 始终有限（最多到几百），印证了 §4.7"有限 $\mu$ 即可精确满足"的核心论断。

把它和 §6.4 对照：§6.4 调 scipy.minimize 一行解决，但你看不见里面发生什么；§6.5 把那"一行"拆成了内层 GN + 外层 AL 的几十行，每一行都能对到 §4 的一个公式。这就是"会用"和"懂原理"的差别——前者够你跑通玩具，后者让你在 KOMO 不收敛时知道去调哪个旋钮（初值、$\mu$ 调度、乘子、阻尼）。

本质洞察：这个从零实现戳破了"KOMO 是高深黑箱"的错觉——它的内核不过是一个解正规方程的内层循环，套一个更新乘子的外层循环，两层都能用十几行 numpy 写清。真实的 rai/KOMO 比它强在四处（§6.3 本质洞察列过的规模/约束/求解器/符号层），但求解骨架完全是这个形状：Gauss-Newton 解 sos、增广拉格朗日逼硬约束、带状线代数扛长轨迹（下节 §6.6 补上这第三块）。你若把本节代码、§6.4 的切换约束、§6.3 的动作参数三者在脑中合并，就拼出了真实 KOMO 的全部要件。从这里到工业级 LGP，缺的只是工程打磨，没有新的"魔法原理"——这正是吃透 §4 数学后该有的底气。

6.6 带状结构的实测：稠密 vs 带状求解 ⭐⭐⭐¶

§4.8 论证了"k 阶 Markov → 带状 Hessian → 求解关于 $T$ 线性"，§6.5 的迷你 KOMO 却用了稠密的 np.linalg.solve（因为 $T=12$ 太小，无所谓）。本节把 §4.8 的复杂度论断跑成实测：对一个平滑轨迹优化的 $J^\top J$，比较稠密求解和带状求解随轨迹长度 $T$ 增长的耗时，亲眼看带状如何线性、稠密如何立方。

import numpy as np
from scipy.linalg import solve_banded
import time

# ===== 实测 §4.8: 带状 vs 稠密求解, 看复杂度如何随 T 变化 =====
def build_smoothness_normal_matrix(T):
    """加速度平滑 φ=x_{t-1}-2x_t+x_{t+1} 的 JᵀJ (1D)。
    二阶差分算子 D 让 JᵀJ = DᵀD 成为对称五对角(半带宽 2)。"""
    D = np.zeros((T-2, T))
    for i in range(T-2):
        D[i, i:i+3] = [1, -2, 1]
    return D.T @ D + 1e-3*np.eye(T)            # +微正则保正定(D 有常数/线性零空间)

def banded_solve(M, b, bw=2):
    """把带状矩阵压成 scipy 的对角存储格式, 用带状求解器(内部带状 Cholesky)。"""
    n = M.shape[0]
    ab = np.zeros((2*bw+1, n))
    for i in range(n):
        for j in range(max(0, i-bw), min(n, i+bw+1)):
            ab[bw + i - j, j] = M[i, j]        # 只存带内元素
    return solve_banded((bw, bw), ab, b)

print("   T      稠密(ms)    带状(ms)    稠密/带状")
for T in [100, 200, 400, 800, 1600]:
    M = build_smoothness_normal_matrix(T)
    b = np.random.randn(T)
    t0 = time.perf_counter(); xd = np.linalg.solve(M, b); td = (time.perf_counter()-t0)*1e3
    t0 = time.perf_counter(); xb = banded_solve(M, b);    tb = (time.perf_counter()-t0)*1e3
    assert np.allclose(xd, xb, atol=1e-6)      # 两者解相同: 带状不是近似(§4.8 陷阱4-5)
    print(f"{T:6d}   {td:8.2f}   {tb:9.3f}   {td/tb:8.1f}x")
# 观察(数量级, 随机器而异):
#   稠密耗时随 T 约三次方增长(T 翻倍, 耗时约 ×8); 带状随 T 近似线性(T 翻倍, 耗时约 ×2)。
#   故 T 越大, "稠密/带状"比值越悬殊 —— 这就是 §4.8 "关于 T 线性"的实测体现。

两个要点：

带状不是近似，是精确稀疏。 assert np.allclose(xd, xb) 每次都通过——带状求解和稠密求解给出完全相同的解。带状只是不去碰那些本来就为零的远距离块（§4.8），不丢任何信息。这正是 §4.8 陷阱 4-5 要破的误解：带状捕捉了所有非零项。
优势随 $T$ 放大。 稠密求解每次 $O(T^3)$（$T$ 翻倍耗时约 8 倍），带状求解 $O(T\cdot b^2)$（$T$ 翻倍耗时约 2 倍）。所以轨迹越长，带状的相对优势越悬殊——这把 §4.8 那个"$10^{11}$ vs $10^6$"的纸面账，变成了你能在自己机器上复现的曲线。

本质洞察：§6.6 把 §4.8 从"我相信它线性"变成了"我看到它线性"。这个差别对工程判断很重要——当有人告诉你"KOMO 能优化上千步的长轨迹"，你现在知道这不是吹嘘，而是带状结构的数学必然，且你能亲手测出来。更普适的一课是：优化器的可扩展性，几乎总是写在它利用的稀疏结构里。看一个轨迹优化/SLAM/MPC 库快不快、能不能上大规模，第一个该问的不是"它用什么语言写的"，而是"它有没有利用问题的稀疏/带状/块结构"。利用了，就关于规模线性或近线性；没利用（退化成稠密），再快的语言也救不了 $O(N^3)$。§4.8 的链条 + §6.6 的实测，给了你这把判断可扩展性的尺子。

6.7 从玩具到真实库，与 LGP 完整算法 ⭐⭐⭐¶

§6.3–§6.6 的玩具，已经把 LGP 几何层的每个部件都手写了一遍：动作参数作变量（§6.3）、切换约束 + 平滑代价（§6.4）、Gauss-Newton + 增广拉格朗日（§6.5）、带状求解（§6.6）。本节做两件收尾的事：把这些部件对到真实 LGP 库，再用一段完整算法伪代码把 §3+§4+§5 拧成一个整体。

从玩具到真实库（rai/KOMO）。 真实 LGP 常用 Toussaint 组的 rai/KOMO 库。从你写的玩具到真实库，变的是规模与接口，不变的是内核。下表把你手写过的东西，对到真实库的概念（示意性，具体 API 以库文档为准）：

玩具里你手写的	真实 rai/KOMO 里对应	章节依据
决策变量 = 轨迹展平的大向量	KOMO 的构型路径（设步数、k 阶）	§4.1
`smooth_residual` 加速度平滑	一个 sos 特征（加速度），库内置	§4.4 / §4.9
`constraint_residual` 切换约束	eq 类型目标，挂在切换时间步上	§3.3
碰撞（玩具略去）	ineq 特征（库自带碰撞距离 + 雅可比）	§4.4
持物期杯随手动（玩具略）	运动学挂接（改运动学树，非加约束）	§4.5
手写 GN + AL 求解循环	库内置的 Gauss-Newton + 增广拉格朗日	§4.6 / §4.7
手写带状求解	库内部的稀疏 / 带状线性代数	§4.8
枚举放置顺序（玩具写死）	符号树搜索 + multi-bound	§5

在真实库里你主要做两件事：(1) 声明问题——指定轨迹步数与 k 阶，把每个代价/约束作为一个"目标"加进去（给定它作用的时间区间、特征类型 sos/eq/ineq、目标值与权重）；(2) 调用求解——库自动跑完 §4.6–§4.8 的 GN + AL + 带状求解。§3.3 那句"逻辑生成约束"在这里就有了字面落点：符号动作序列决定了你要加哪些目标特征、在哪些时间步上——grasp 加一组切换 + 挂接，place 加另一组，持物段加保持 + 不碰。换一个动作序列，就是换一组 addObjective 调用。

LGP 完整算法（合 §3+§4+§5）。 把符号树搜索、multi-bound 剪枝、KOMO(GN+AL) 求解拧成一个伪代码，这是全章算法内容的总装：

算法: LGP 完整求解 (multi-bound tree search + KOMO)
输入: 初始符号状态 s0, 目标 g, 领域(动作算子 + 几何附着)
输出: 可行动作序列 a_{1:K} 及轨迹 x*, 或 "无解"

 1: 初始化符号搜索树, 根 = s0
 2: while 树中有未展开候选 do
 3:    a_{1:K} ← 取一个候选动作序列      // 由 branch-and-bound / MCTS 引导(§5.4)
 4:    // ---- multi-bound 由粗到精剪枝(§5.3) ----
 5:    if not 可行(P1: 有效运动学下界) then
 6:        剪掉 a_{1:K} 及子树; continue   // 最便宜, 筛掉"够不到"
 7:    if not 可行(P2: 粗时间分辨率 NLP) then
 8:        剪掉 a_{1:K}; continue          // 中等, 筛掉"碰撞/动力学不可行"
 9:    x*, ok ← KOMO_solve(a_{1:K})        // P3 完整精度(§4), 见子程序
10:    if ok then return (a_{1:K}, x*)     // 找到解!
11:    else 剪掉 a_{1:K}; continue
12: return "无解"

子程序 KOMO_solve(a_{1:K}):              // §4: 内层 GN + 外层增广拉格朗日
13:    由 a_{1:K} 生成特征: sos 代价 + eq/ineq 约束(§3.3, §4.5, §6.7 上表)
14:    初始化轨迹 x(直线插值/上层界的解), 乘子 λ←0, 罚 μ←μ0
15:    repeat                              // 外层: 增广拉格朗日(§4.7)
16:        repeat                          // 内层: Gauss-Newton(§4.6)
17:            算 φ, J;  grad ← 2·Jsᵀφs + Jgᵀ(λ+μg)
18:            H ← 2·JsᵀJs + μ·JgᵀJg (+ LM 阻尼)
19:            解带状正规方程 H·Δx = -grad  // 带状 Cholesky(§4.8)
20:            线搜索定 α;  x ← x + α·Δx
21:        until 内层收敛
22:        λ ← λ + μg;  μ ← min(增大 μ, μmax)  // 乘子更新(§4.7 式4.8)
23:    until 约束满足且代价稳定
24:    return x, (约束是否在容差内满足)

读这段伪代码的正确方式：外层（1–12 行）是 §5 的符号树搜索 + multi-bound——它管"试哪个动作序列、怎么便宜地剪枝"；子程序（13–24 行）是 §4 的 KOMO——它管"给定一个序列，怎么解出几何"，而 §4 的两层（GN 内、AL 外）就是 16–22 行那个双重 repeat。两段拼起来，正是 §3.4 末尾说的"树搜索枚举 $a_{1:K}$、对每个固定序列做连续轨迹优化"的完整落地。

本质洞察：这段伪代码是全章的"总装图"——它让你一眼看清 LGP 的三层嵌套：符号树搜索（外）→ multi-bound 剪枝（中）→ KOMO 的 GN+AL（内），且内层的 GN 与 AL 自己又是一对内外循环。这种"循环套循环"的结构不是 LGP 独有的偶然，而是几乎所有"离散决策 + 连续优化"混合问题的通用骨架（你在分支定界 + 凸松弛、在 MPC 的 SQP、在双层优化里都会见到同构的嵌套）。掌握这张总装图，你就不会再把 LGP 的各部分当孤立知识点——§2 的"为什么优化"、§3 的"逻辑生成约束"、§4 的"KOMO 怎么解"、§5 的"怎么搜怎么剪"，全都各就各位地嵌在这 24 行里。把这段伪代码默写下来、能逐行讲出每行对应哪一节，就是你学完本章硬核部分（§3–§6）的最好检验。

6.8 串起来跑一遍：最小端到端 LGP demo ⭐⭐⭐⭐¶

§6.5 给了 KOMO 求解器、§6.7 给了完整算法的伪代码，但还差一个能跑的全流程：把符号枚举（§5 外层）、KOMO 求解（§4 内层）、按可行性与代价选优（§6.7）串成一个真能运行的脚本。本节就是这个收尾——一个最小但完整的端到端 LGP，让你看着 §6.7 的伪代码变成真实输出。

import numpy as np
from itertools import permutations

# ===== 最小端到端 LGP: 符号枚举(§5) + KOMO(§4,§6.5) + 选优(§6.7 伪代码) =====
# 任务: 把两个物体 A,B 依次放到堆叠位(先放的在下、后放的在上)。
# 符号选择 = 堆叠顺序(A先 or B先); 几何 = 每个顺序对应一条 KOMO 轨迹。
T, DIM = 16, 2
objects = {"A": np.array([0.0, 0.0]), "B": np.array([3.0, 0.0])}
base    = np.array([1.5, 0.0])      # 堆叠基座(桌面)
ee_home = np.array([0.5, 2.0])      # 手爪初始位置(偏向 A 一侧, 故意打破对称)
reach_xmax = 3.5                    # 可达性约束: 手爪 x 不超过此值(模拟臂展上限)
n = T * DIM

def smooth_resid_J(x):              # sos: 加速度平滑(§4.9), 线性特征
    X = x.reshape(T, DIM)
    acc = X[2:] - 2*X[1:-1] + X[:-2]
    Js = np.zeros((acc.size, n))
    for i in range(T-2):
        for d in range(DIM):
            r = i*DIM + d
            Js[r,(i)*DIM+d] += 1; Js[r,(i+1)*DIM+d] += -2; Js[r,(i+2)*DIM+d] += 1
    return acc.reshape(-1), Js

def make_constraints(order):
    """给定堆叠顺序, 生成切换一致性 eq 约束(§3.3):
       手爪起点固定; 抓取时刻经过物体; 放置时刻经过对应堆叠高度。"""
    specs = [(0, ee_home)]                            # 起点
    times = [(3, 7), (10, 14)]                        # 两个物体的(抓取,放置)时刻
    for k, name in enumerate(order):
        tg, tp = times[k]
        place_h = base + np.array([0.0, 0.3*k])       # 第 k 个堆在 0.3k 高(先放的在下)
        specs += [(tg, objects[name]), (tp, place_h)]
    def resid_J(x):
        X = x.reshape(T, DIM); rs, Js = [], []
        for (t, target) in specs:
            rs.append(X[t]-target)
            J = np.zeros((DIM, n))
            for d in range(DIM): J[d, t*DIM+d] = 1.0
            Js.append(J)
        return np.concatenate(rs), np.vstack(Js)
    return resid_J

def komo_solve(order):
    """内层 KOMO(§6.5): GN + 增广拉格朗日; 返回(轨迹, 代价, 是否可行)。"""
    cons_J = make_constraints(order)
    x = np.linspace(ee_home, base, T).reshape(-1)     # 直线初值
    lam = np.zeros(cons_J(x)[0].size); mu = 1.0
    for _ in range(8):                                # 外层增广拉格朗日(§4.7)
        for _ in range(5):                            # 内层 Gauss-Newton(§4.6)
            ps, Js = smooth_resid_J(x); g, Jg = cons_J(x)
            grad = 2*Js.T@ps + Jg.T@(lam + mu*g)
            H = 2*Js.T@Js + mu*(Jg.T@Jg) + 1e-9*np.eye(n)
            x = x + np.linalg.solve(H, -grad)         # 解正规方程(§4.6)
        g, _ = cons_J(x); lam = lam + mu*g; mu = min(mu*3, 1e4)  # 乘子更新(§4.7)
    X = x.reshape(T, DIM)
    cost = float(np.sum((X[2:]-2*X[1:-1]+X[:-2])**2)) # 平滑代价(越小越好)
    feasible = (np.max(np.abs(cons_J(x)[0])) < 1e-4   # 约束满足
                and np.max(X[:,0]) <= reach_xmax+1e-6)# 且不违反可达性
    return X, cost, feasible

# ===== 外层: 枚举符号顺序, 各解 KOMO, 选可行且最便宜者 (§6.7 第1-12行) =====
best = None
print("符号顺序     可行?    平滑代价")
for order in permutations(objects.keys()):
    X, cost, ok = komo_solve(list(order))
    print(f"  {'→'.join(order):6s}    {str(ok):5s}   {cost:.4f}")
    if ok and (best is None or cost < best[1]):
        best = (order, cost, X)

if best is None:
    print("\n无可行顺序(所有符号骨架都几何不可行)")
else:
    print(f"\n选中顺序: {'→'.join(best[0])}  (平滑代价 {best[1]:.4f})")
# 解读: 手爪初始在 A 一侧, 故"先 A 后 B"的轨迹更顺、代价更低, LGP 选它。
#       这就是 §3.6 外搜内优、§6.7 伪代码"枚举→KOMO→选优"的最小可运行缩影。
#       若把 reach_xmax 调到 <3, 抓 B(x=3)那步会判不可行, 对应顺序被自动跳过
#       —— 正是伪代码"内层不可行→外层剪枝"的体现。

这段脚本就是 §6.7 伪代码的可运行版，逐块对得上：

外层 for order in permutations(...) ＝伪代码第 2–3 行的符号序列枚举（§5 的树搜索，这里物体少所以直接全排列）。
komo_solve ＝伪代码第 9 行 + 子程序（§4 的 KOMO，复用 §6.5 的 GN + AL 内核）。
make_constraints ＝伪代码第 13 行"由 $a_{1:K}$ 生成特征"——同一个符号顺序生成不同的 eq 约束，这正是 §3.3"逻辑生成约束"的字面体现：换一个堆叠顺序，就换一组放置高度约束。
feasible 判定 + if ok and cost < best[1] ＝伪代码第 10–11 行的可行性筛选与选优（也是 §3.6 外层 $\min_a V(a)$ 的落地：在可行顺序里挑代价最小的）。

把它和 §6.5 对照：§6.5 解一个固定顺序的轨迹，§6.8 在外面套一层枚举 + 选优，就从"几何层求解器"升级成了"完整的 TAMP 求解器"。这几十行虽小，却五脏俱全地复现了 LGP 的双层骨架。

本质洞察：这个能跑的 demo 是全章的"收口"——它让 §6.7 那段静态伪代码动了起来，你能亲眼看到"枚举符号顺序 → 每个顺序解 KOMO → 按可行性和代价选优"这条主线产出真实数字。三点值得带走：其一，符号选择真的在改变几何约束（换顺序换约束），这是 §3.3 最直观的实证；其二，外层是离散搜索、内层是连续优化，§3.6 的双层 MINLP 在这里不再抽象，而是 for 循环套 komo_solve 的真实代码结构；其三，从这个 demo 到工业级 LGP，缺的只有规模与打磨——把全排列换成被引导的树搜索（§5.7）、把 komo_solve 换成带 multi-bound 的级联（§5.3）、把 2D 玩具换成真实机器人模型，骨架完全不变。你若能把本节代码默写出来、并逐块讲清它对应 §3–§6 的哪一节，就真正打通了本章的任督二脉：从"逻辑生成约束"的思想，到"树搜索 + KOMO"的架构，到"GN + 增广拉格朗日"的内核，再到一个亲手跑通的端到端实现。

练习¶

（⭐⭐） 运行 §6.3 的玩具，做两个修改实验：(a) 把初始猜测 x_init 改得更离谱（如 [2.0, -2.0, 2.0]），看优化器能否仍调到对齐，体会初值的影响（§4.2 陷阱）；(b) 把积木宽度 BLOCK_W 改小（如 0.3），看约束变紧后塔还能否搭起来。
（⭐⭐⭐） 给 §6.3 的玩具加一个"动作参数"维度：让每块积木除了水平位置，还能优化一个朝向角 $\theta$，并加一个"朝向尽量水平"的代价。这模拟 §3.2 的动作参数联合优化。
（⭐⭐⭐） §6.2 说 LGP 优化下层积木位置时会"考虑整座塔的稳定"。在 §6.3 的玩具里，这体现为 stack_cost 同时含所有积木的错位项。解释为什么"把所有积木位置放进一个优化问题"能做到全局协同，而"逐块采样位置"做不到。
（⭐⭐⭐⭐） 把 §6.3 从"放置顺序写死"改成"枚举两种放置顺序，各优化一次，选代价低的"——这就朝 §5 的树搜索 + 优化迈了一步。讨论当积木变多时，为什么需要 multi-bound 剪枝而非枚举所有顺序。
（⭐⭐⭐，动手） 运行 §6.4 的轨迹优化玩具，做两个实验：(a) 把 t_grasp 改到 4（更早抓取），观察轨迹形状如何变；(b) 故意把 cup_pos 设到很远（如 [8,8]），看平滑代价如何增大——体会切换约束如何"拉扯"轨迹。
（⭐⭐⭐⭐，扩展） 把 §6.4 的抓取位姿从"固定为杯位置"改成优化变量：允许末端在杯子周围一个小范围内选抓取点（加一个"抓取点在杯子附近"的约束），让优化器同时决定"抓杯子哪个位置"和"怎么走"。这就完整复现了 §3.2 的"动作参数与轨迹联合优化"。讨论这个改动后，解出的抓取点为什么会偏向"让整条轨迹更平滑"的位置。
（⭐⭐⭐，动手） 运行 §6.5 的从零实现，做两个实验：(a) 把外层乘子更新那行 lam = lam + mu*g 注释掉（退化成纯罚函数），观察约束违反还能否降到 $10^{-7}$、需要把 $\mu$ 调到多大才勉强满足——体会 §4.7 为什么要乘子；(b) 把初值从全零改成直线插值，看内外层迭代次数是否减少。
（⭐⭐⭐，动手） 运行 §6.6 的带状实测，记录稠密与带状耗时随 $T$ 的增长。验证：$T$ 翻倍时，稠密耗时约 ×8（三次方）、带状约 ×2（线性）吗？再把半带宽参数 bw 从 2 改成 3（模拟 3 阶 jerk 平滑），看带状耗时怎么变、仍是否线性。
（⭐⭐⭐⭐，综合） 对着 §6.7 的完整算法伪代码，逐行标注每一行对应本章哪一节（§2–§5）。再回答：第 5–8 行的 multi-bound 剪枝，和第 13–24 行的 KOMO_solve，谁被调用的次数多？为什么这个次数差正是 multi-bound 价值的来源（呼应 §5.6）？
（⭐⭐⭐⭐，动手） 运行 §6.8 的端到端 demo：(a) 把 ee_home 从偏 A 一侧改到正中 (1.5,2.0)，两个顺序的代价会变得接近吗？为什么？(b) 把 reach_xmax 调到 2.8，观察输出——为什么会"无可行顺序"（提示：两个顺序都要抓 $x=3$ 的 B）？(c) 给 demo 加入第三个物体 C，看枚举的顺序数怎么增长，体会 §5.7 说的组合瓶颈。

7. LGP 与 PDDLStream：两大范式的系统对比 ⭐⭐⭐¶

本章和 TAMP_T3 是"双子章"——缝合符号-几何鸿沟的两条主流路。本节做系统对比，是两章的合页，也是你选型的依据。TAMP_T3 §10.1 给过简表，这里讲透。

7.1 根本区别：信息流向与求解方式¶

维度	PDDLStream（流式，T3）	LGP（优化式，本章）
核心机制	符号搜索调用采样器（Stream）	符号序列生成约束，优化求解
处理连续参数	采样（生成离散候选）	优化（当决策变量解出）
符号↔几何信息流	几何认证符号（采样器认证谓词，几何→符号）	符号生成几何（动作生成约束，符号→几何）
求解主体	离散符号搜索 + 黑盒采样	离散树搜索 + 连续轨迹优化
几何引擎	IK/碰撞/RRT（黑盒采样器）	KOMO（带约束轨迹优化器，§4）
数学形态	归约为一系列有限 PDDL（T3 §5.1）	逻辑生成约束的混合优化（§3.4）
加速技术	optimistic 乐观对象（惰性采样，T3 §5.2）	multi-bound 多层下界（惰性优化，§5.3）

本质洞察：PDDLStream 和 LGP 在最深层是镜像对称的。信息流向相反：PDDLStream 是"几何采样器认证符号谓词"（几何告诉符号什么可行），LGP 是"符号动作生成几何约束"（符号告诉几何要满足什么）。但二者的加速技术却惊人地相似——都是"先便宜近似、必要才精确"：PDDLStream 的乐观对象（先用假想值规划，需要才采样）≈ LGP 的 multi-bound（先解粗下界，通过才解精确）。这种"镜像的结构 + 相似的智慧"说明：缝合符号-几何鸿沟的深层难题是共通的（都要管理"昂贵的几何计算"与"组合的符号搜索"的交互），只是 PDDLStream 从采样侧切入、LGP 从优化侧切入。理解了这层对称，你对整个 TAMP 集成范式就有了统一的视角。

7.2 各自擅长什么：选型依据¶

任务特征	推荐范式	原因
组合摆放（抓放摆，物体多）	PDDLStream	离散选择为主，采样候选自然
富接触操作（推、倒、插、力控）	LGP	几何量需连续调优，优化擅长
工具使用（够、撬、敲）	LGP	接触 + 力 + 几何强耦合，优化的主场
全局耦合长序列（搭塔、码垛）	LGP	全局联合优化，一眼看到底
大量离散物体的重排	PDDLStream	组合爆炸下采样比优化的非凸更稳
几何约束是"等式/精度"（对齐、插入）	LGP	等式约束优化能精确满足，采样难精确命中

一句话选型（呼应 §2.2、TAMP_T0 §6 决策树）：几何难点是选择型（离散候选够用）→ PDDLStream；是调优型（需连续精调力/角度/对齐/全局稳定）→ LGP。

本质洞察：这张选型表的背后是一条清晰的判据——问题的几何约束本质上是"挑一个"还是"调到最优"。抓放摆的"够不够得到"是布尔判定（够/不够），采样判定很自然；插销的"对齐精度"、倒水的"倾角"、搭塔的"重心"是连续的程度，需要优化连续地逼近。这条判据不仅区分 LGP 与 PDDLStream，它实际上是整个机器人学"采样 vs 优化"分野的统一判据——你在 §2.2 见过它贯穿运动规划（RRT vs TrajOpt）、MPC（MPPI vs iLQR）。掌握"选择型 vs 调优型"这把尺子，你就能在任何"采样还是优化"的岔路口做出正确判断。

7.3 不是二选一：结合的前沿¶

实践中两者常结合，取长补短（§9 详述）： - 用 PDDLStream 式的符号 + 采样快速找到一个可行骨架，再用 LGP 式的优化精化轨迹（尤其接触段）。 - 用采样为 LGP 的非凸优化提供初值（缓解 §2.3 的初值敏感）。 - 学习方法（GNN/扩散）为两者提供引导（引导采样 or 引导优化初值/搜索顺序）。

7.4 一个任务，两种解法：并排走一遍 ⭐⭐⭐¶

§7.1–§7.3 的对比都在抽象层面。本节挑一个具体任务——"把一本书插进书架已有书的窄缝"——让 PDDLStream 和 LGP 各走一遍，把 §7.2 的选型判据落到实处。这个任务有意思在于它同时含"选择型"和"调优型"成分，正好暴露两个范式的不同侧重（它也正是本节末练习 1(c) 那道题）。

任务。 书架上已有一排书，留了一道刚好够宽的缝。要把手里的书插进去：抓书 → 移到缝前 → 对准 → 插入到位。难点在插入段要求高对齐精度（歪一点就卡住或撞到邻书），且抓取方式影响能否对齐。

PDDLStream 的解法（采样式，T3）。

PDDLStream 走一遍(插书):
  符号搜索:  pick(book) → move → insert(book, slot)
  连续参数靠 Stream 采样:
    sample-grasp(book)       → 一批抓取位姿候选
    sample-insert-pose(slot) → 一批"书在缝中"的目标位姿候选
    inverse-kin / motion     → 为候选采 IK 构型与无碰路径
  采样器认证谓词(Kin/Motion/CFree), 反馈给符号搜索拼计划
  难点暴露: "对齐精度"是连续量 —— 采 100 个插入位姿,
            可能没几个对得够准; 在"窄缝高精度对齐"上要撞很多次运气

LGP 的解法（优化式，本章）。

LGP 走一遍(插书):
  符号序列:  [pick] → [持书] → [insert]      (树搜索枚举, §5)
  生成约束(§3.3) + 动作参数作变量(§3.2):
    切换:  抓取时手=抓取位姿(变量); 插入完成时书=缝内目标位姿
    阶段:  持书段书随手动、不碰邻书; 插入段"书的轴线对准缝的轴线"(eq 特征)
    参数:  抓取位姿 + 插入轨迹 全是优化变量
  KOMO 联合优化(§4): 在"对齐"等式约束 + "不碰邻书"不等式约束下,
            连续解出抓取位姿与插入轨迹
  难点应对: "对齐精度"作为 eq 特征被增广拉格朗日精确逼到满足(§4.7),
            且抓取位姿会被优化成"利于对齐"的那个(全局协同, §3.2)

并排看差异。

子问题	PDDLStream	LGP
抓取位姿	采样候选里挑一个	优化变量，解出"利于后续对齐"的
插入对齐	采样目标位姿，难精确命中	eq 约束，增广拉格朗日精确满足（§4.7）
不碰邻书	采样 + CFree 检测	ineq 约束（§4.7）
全局协同	弱（抓取与插入分开采）	强（抓取 + 插入联合优化，§3.2）

这个并排揭示 §7.2 判据的实质：插书的"对齐精度"是典型的调优型几何难点——采样要撞运气，优化用 eq 约束精确逼近。所以这个任务偏 LGP 的主场。但若书架很空、缝很宽（对齐不再是难点，退化成"放进一个大致区域"），它就变回选择型，PDDLStream 的采样反而更省心——这恰说明选型不看任务名字，而看几何难点的性质（§7.2）。

本质洞察：把同一个任务用两种范式各走一遍，比任何对比表都更能让你"长出选型的直觉"。两点体会最关键：其一，同一任务的不同子问题，可能分属不同范式的主场——插书里"抓哪"偏选择、"对齐"偏调优，这正是 §7.3"结合"路线的现实基础（用采样定抓取骨架、用优化精化插入段）。其二，判据落地靠问一句"这个几何量是挑一个就行，还是要连续逼到精确"——对齐精度要"逼到精确"（eq 约束）就指向优化，抓取方式"挑一个能用的"就够便可采样。带着这个具体例子回看 §7.2 的表，你会发现那些抽象的"选择型/调优型"标签，全都能在插书这一个任务里找到对应的影子。下次遇到新任务，就在脑中给它走一遍这样的并排——你已经有了模板。

练习¶

（⭐⭐） 对下列任务用 §7.2 的判据选 LGP 或 PDDLStream，说明几何难点是"选择型"还是"调优型"：(a) 把 10 个零件分拣到对应料盒；(b) 用螺丝刀拧紧螺丝；(c) 把书插进书架已有书的缝隙；(d) 搭一座 6 层积木金字塔。
（⭐⭐⭐） §7.1 说两个范式"信息流向相反"。用 pick 动作的"抓取位姿"具体说明：PDDLStream 里这个位姿怎么进入符号世界（几何→符号），LGP 里它怎么被符号约束（符号→几何）。
（⭐⭐⭐） §7.1 说乐观对象（PDDLStream）和 multi-bound（LGP）是"相似的智慧"。详细对比：它们各自的"便宜近似"是什么、"昂贵精确"是什么、"从近似升级到精确"的触发条件是什么。
（⭐⭐⭐⭐，综合） 设计一个"先 PDDLStream 找骨架、再 LGP 精化"的混合流程处理"用夹爪把杯子递给人"任务（含接触的 handover）。说明哪部分用采样（骨架/抓取候选）、哪部分用优化（handover 段的力控轨迹）、二者如何交接。

8. 真实应用：LGP 的主场 ⭐⭐⭐¶

前面用搭积木塔讲透了原理。本节看 LGP 在真实、复杂任务上的应用，让你对它的能力边界有实感——尤其是它相对 PDDLStream 的主场：工具使用与富接触操作。

8.1 工具使用与富接触：LGP 的标志性主场¶

LGP 最有代表性的应用，是工具使用——这是采样范式最吃力、优化范式最闪光的地方。Toussaint 等人 2018 年的工作把这点展示得淋漓尽致：把序列机器人操作问题整体地表达为数学规划的一阶逻辑扩展——一个在完整世界轨迹上的非线性约束规划，其中符号状态-动作序列定义了(不)等式约束；这个框架使机器人能优先选择各种交互模式，并获得诸如"利用接触来减少不确定性"这样有趣的行为。

为什么工具使用是 LGP 主场？因为它集齐了 LGP 擅长的所有特征： - 接触丰富：用工具够、撬、敲、推，全是连续接触力，需要连续调优（而非采样离散候选）。 - 几何强耦合：工具的抓握位姿决定了能施加什么力、够到哪里，与后续动作强耦合——需要全局联合优化。 - 物理推理：要推理接触动力学（敲多重、撬多深），这天然是优化问题（在物理约束下求最优作用）。

工具使用为何是 LGP 主场(对比 PDDLStream):
  任务: 用棍子把够不到的物体拨到手边
  PDDLStream: 采样"怎么抓棍子""棍子怎么碰物体"——离散候选难覆盖连续的接触力/角度
  LGP: 把"抓棍子位姿+棍子接触点+施力轨迹"全作优化变量, 在接触动力学约束下
       联合优化出"如何抓、如何拨最省力且够得到"——连续调优, 一气呵成

本质洞察：工具使用是检验 TAMP 范式的"试金石"——它逼出了采样与优化的根本差异。采样面对"接触力该多大、工具该怎么倾斜"这类连续量，只能离散地撞运气；优化能在接触动力学的约束下连续地求最优。LGP 把整个工具使用序列（抓工具→接触→施力→达成）写进一个轨迹优化，让"利用接触"成为优化目标的自然结果——甚至能自发产生"用接触来减少不确定性"这类巧妙行为（如先让工具碰一下物体确认位置）。这种"物理智能从优化中涌现"的特质，是 LGP 优化式范式最深刻的魅力，也是它在富接触操作上不可替代的根本原因。

8.2 长时域序列操作：RHH-LGP¶

LGP 的另一个主场是长时域、多物体的序列操作。但长序列带来组合爆炸（动作序列树指数增长），朴素 LGP 会很慢。RHH-LGP（Braun, Ortiz-Haro, Toussaint, Oguz, 2022）针对性地改进：序列决策与运动规划会诱导组合复杂度；对长时域任务，尤其当环境包含许多可交互物体时，规划效率更为关键；RHH-LGP 提出一个基于 LGP 的 TAMP 方法，有效利用基于几何的启发式来求解长时域操作任务，再用接收时域 (receding horizon) 形式进一步提升效率。

它的两个关键改进： - 几何启发式：用几何信息引导符号树搜索（哪些动作序列更有希望），减少要优化的候选——把 §5 的 multi-bound 进一步加入领域启发式。 - 接收时域 (receding horizon)：不一次规划整个长序列，而是规划一段、执行、再规划下一段——把长序列拆成滚动的短窗口，控制每次优化的规模。

它的能力令人印象深刻：能求解需要多个机器人（包括一个移动机器人和蛇形步行机器人）自主形成新颖的异构运动学结构的任务——比如蛇形机器人爬上多达 64 级台阶、异构机器人协同搭出新结构。

8.3 LGP 在真实系统中的定位¶

综合这些应用，LGP 在真实系统里的定位：

LGP 的工程定位:
  最适合: 富接触操作(工具使用/推/插/力控)、全局耦合长序列(搭塔/码垛/装配)、
         需要物理推理的操作(利用接触、动力学)
  常配合: 接收时域(长序列, §8.2)、学习启发式(引导搜索, §9)、
         反应式 MPC(执行层跟踪, 应对扰动)
  上游:   符号目标(可由 LLM 给出, 接 T7)
  下游:   优化出的轨迹交给控制器执行(可配反应式 MPC 应对扰动)

本质洞察：LGP 和 PDDLStream 在真实系统里的定位，恰好互补地覆盖了 TAMP 的两类难题。PDDLStream 守着"组合摆放"（物体多、离散选择为主），LGP 守着"物理操作"（接触、力、全局几何耦合）。一个成熟的机器人系统，很可能两者都需要——用 PDDLStream 处理"把哪些零件搬到哪"的组合调度，用 LGP 处理"如何拧紧这颗螺丝"的接触操作。这呼应了 TAMP_T0 §3 的方法谱系：流式（板块③）和优化式（板块②）不是竞争关系，而是任务层工具箱里互补的两件工具。学完 T3 和本章，你的 TAMP 工具箱才算完整。

8.4 走查一个工具任务：约束结构逐动作展开 ⭐⭐⭐¶

§8.1 说工具使用是 LGP 主场，但"主场"二字仍是抽象判断。本节挑一个具体工具任务——"球滚到沙发底下，机器人手够不到，得用一根木棍把球拨出来再抓起"——把它的符号序列和每个动作生成的约束逐条列出，让你亲眼看到 §3.3"逻辑生成约束"在一个真实多步任务里长什么样，也看到 §6.7 伪代码第 13 行"由 $a_{1:K}$ 生成特征"具体展开成什么。

符号序列（树搜索会找到的骨架，§5）：

\[ [\text{抓棍}] \to [\text{伸到沙发下}] \to [\text{扫球出来}] \to [\text{放下棍}] \to [\text{抓球}] \]

逐动作的约束（§3.3 的三类：模式切换、切换一致性 eq、阶段几何 ineq，外加 sos 代价）：

动作	运动学模式切换	切换一致性（eq）	阶段几何约束（ineq）
抓棍	棍：地面 → 附着手爪	抓取时刻手爪位姿 = 合法抓握棍的位姿	接近时手臂不碰沙发/地面
伸到沙发下	持棍移动	棍末端到达球前的接触准备位姿	棍从窄缝伸入不撞沙发底（间隙）；手臂不进沙发
扫球出来	棍 ↔ 球建立接触	棍上接触点 = 球面接触点（接触一致性）	推球方向把球往外带；球的路径绕开沙发腿
放下棍	棍：附着 → 地面	释放时刻棍位姿 = 稳定放置位姿	撤棍不碰已扫出的球
抓球	球：地面 → 附着手爪	抓取时刻手爪位姿 = 抓握球的位姿	球已在可达区；手不碰沙发

每个动作还各带一项 sos 平滑代价（轨迹加速度小），扫球段额外有"推力适度"的 sos 项。

三个关键约束，正是 LGP 主场的体现。

"棍从窄缝伸入不撞沙发底"（伸入段的 ineq）是连续的间隙 + 对齐约束——棍要以合适角度、合适深度伸进一道窄缝。这是典型的调优型几何难点（§7.2）：采样很难撞中"恰好伸得进又够得着"的位姿，优化能在间隙不等式下连续逼出来。
"棍上接触点 = 球面接触点"（扫球段的 eq）是接触一致性约束——这一步进入富接触模式（§8.1），棍和球的接触被写成一个等式特征，增广拉格朗日把它精确逼到满足（§4.7）。"用接触完成任务"在这里成了优化约束的自然结果，而非要靠采样去碰运气的离散事件。
跨动作的几何耦合：这五步绝不独立——扫球段把球推到哪（终点），必须落在抓球段的可达区内；伸入段的路径决定了棍能否到达扫球的起点；抓棍的抓握位姿（抓棍子哪一端、什么角度）又决定了伸入时棍的可用长度与可达角度。

全局协同求解。 正因这些跨动作耦合，LGP 不会一步步孤立地解每个动作，而是把整条序列的所有变量（每段轨迹 + 各动作参数：抓握位姿、伸入深度、接触点、推球方向）放进一个 KOMO 联合优化（§3.2、§6），让上述所有 eq/ineq/sos 在同一个问题里被同时满足。于是"抓棍子的哪一端"会被自动选成"利于后面伸入和扫球"的那一端——这正是 §3.2 全局协同的威力，也是 §6 搭塔案例同一个道理在工具任务上的再现。

本质洞察：这个走查把 §8.1 那句抽象的"工具使用是 LGP 主场"，落成了一张能逐行核对的约束表——你看到每个符号动作如何"生成约束"（§3.3）、看到"窄缝间隙""接触点一致""推球方向"这些连续量为什么必须优化而非采样（§7.2 调优型）、看到跨动作的几何耦合为什么逼着你做全局联合优化而非分步求解（§3.2）。把它和 §6.7 的伪代码对照：这一整串 eq/ineq/sos，就是伪代码第 13 行"由 $a_{1:K}$ 生成特征"在一个真实任务里的完整展开；把它和 §3.6 对照：这张表里的离散选择（抓哪端、走哪条缝）是外层树搜索的事，连续求解（具体位姿、轨迹、接触点）是内层 KOMO 的事。读懂这一个例子，你就把本章从 §3（逻辑生成约束）、§4（KOMO 怎么解）、§6（全局协同）、§7（选型判据）到 §8（主场）的所有抽象，收束到了一个能在脑中完整运行的工具任务上——这正是检验"学通了 LGP"的最好标志。

练习¶

（⭐⭐） §8.1 说工具使用是 LGP 主场。具体分析"用锤子敲钉子"这个任务：哪些几何量需要连续调优（而非采样离散候选）？为什么这让它适合 LGP 而非 PDDLStream？
（⭐⭐⭐） RHH-LGP 用"接收时域"处理长序列。解释接收时域如何控制每次优化的规模（对比一次性优化整个长序列），以及它的代价是什么（提示：滚动规划可能不是全局最优）。
（⭐⭐⭐） §8.3 说成熟系统可能 LGP 和 PDDLStream 都需要。设计一个仓库场景，说明哪部分用 PDDLStream（组合调度）、哪部分用 LGP（接触操作），二者如何分工。
（⭐⭐⭐⭐，综合） 仿照 §8.4，给"开抽屉、从抽屉里取出一支笔、再关上抽屉"这个任务：(a) 写出符号动作序列；(b) 为每个动作列出运动学模式切换、切换一致性（eq）、阶段几何约束（ineq）各一例；(c) 指出哪个约束是连续调优型（应交给优化而非采样），以及存在哪些跨动作几何耦合，从而说明为什么要全局联合求解。

9. 局限与前沿 ⭐⭐⭐¶

诚实面对局限是用对工具的前提。本节讲 LGP 的主要局限及其改进方向，并为后续章节铺垫。

9.1 LGP 的三类局限¶

局限一：非凸优化的局部最优与初值敏感。 这是 §2.3 已埋下、最根本的局限。LGP 的几何优化（KOMO）是非凸的（碰撞、接触切换都非凸），只能找局部最优，初值不好就陷在坏解或不收敛。相比之下 PDDLStream 的采样没有"局部最优"困扰。改进方向：用采样或学习提供好初值、多初值重启、凸松弛。

局限二：每个符号序列要解一个 NLP，长序列下昂贵。 即便有 multi-bound 剪枝（§5.3），长时域、多物体时符号树仍可能巨大，要解的 NLP 数量多。改进方向：几何启发式引导搜索（RHH-LGP，§8.2）、接收时域、学习剪枝。

局限三：建模门槛高。 把一个新领域的操作写成 LGP 的约束（切换一致性、阶段几何约束、合适的代价），比写 PDDLStream 的 Stream 更需要优化与建模经验。约束写得不好，优化要么不收敛要么解出怪异轨迹。改进方向：标准化约束库、从演示学习约束、基础模型辅助建模。

局限	表现	改进方向
非凸局部最优	陷坏解/不收敛，初值敏感	采样/学习提供初值、多重启、凸松弛
长序列 NLP 昂贵	符号树大、NLP 多，慢	几何启发式、接收时域（RHH-LGP）、学习剪枝
建模门槛高	约束难写、易出怪异解	约束库、从演示学约束、基础模型辅助

本质洞察：LGP 的三类局限，与 PDDLStream 的三类局限（TAMP_T3 §10.3：长时域慢、接触弱、效率）恰好互补——LGP 的局限一（非凸局部最优）正是 PDDLStream 的强项（采样无局部最优），而 PDDLStream 的局限二（接触弱）正是 LGP 的强项（优化擅长接触）。两个范式的优势和局限互为镜像，这从根本上解释了为什么前沿都在探索二者结合（§9.2）：不是因为单个范式不够好，而是因为它们的长短板恰好咬合，结合能取长补短。这也再次印证了 §7.1 的"镜像对称"——连优劣势都是镜像的。

9.2 前沿：与学习、与 PDDLStream 结合¶

LGP 的前沿方向，大多围绕"缓解局限"和"与其他范式结合"：

学习引导优化：用神经网络预测好的初值、预测可达性（如 R-LGP 用 reachability margin 引导腿足操作）、或学习剪枝符号树，缓解局限一、二。
与采样结合：用 PDDLStream 式采样快速找可行骨架 + 提供初值，再用 LGP 精化（尤其接触段）——取 §7.3 说的两范式之长。
不确定性扩展：把 LGP 扩展到随机域（Ha, Driess, Toussaint 2020 用控制-推断对偶，把 LGP 解释为对后验路径分布拟合高斯混合）——接 T6 不确定性 TAMP。
反应式执行：Sequence-of-constraints MPC 等把 LGP 思想用于反应式控制，应对执行时的扰动——接执行层（T5）。
基础模型 + LGP：用 LLM/VLM 给出符号目标或约束，LGP 负责几何求解——接 T7。

本质洞察：LGP 的前沿轨迹，与整个 TAMP 领域的演进方向一致——从"纯优化求解"走向"优化 + 学习 + 采样 + 反应式"的混合。这呼应 TAMP_T0 §5 的历史钟摆和 TAMP_T3 §10.5 的 LLM 讨论：没有单一范式能独霸，未来属于会把符号、几何、采样、优化、学习各取所长地编织在一起的系统。LGP 作为优化式的奠基，它的价值不仅在于"优化式怎么做 TAMP"，更在于它贡献了一个统一的视角——"把整个操作序列看成一个带逻辑约束的轨迹优化"——这个视角深刻影响了后续的可微规划、扩散式规划等生成式方法。学懂 LGP，你就握住了理解 TAMP 优化视角的钥匙。

9.3 学习引导 LGP：把神经网络嵌进哪一层 ⭐⭐⭐¶

§9.2 把"学习引导"列为一句话的前沿方向，但学习到底嵌进 LGP 的哪一层、怎么嵌、治哪个局限，值得单独说清——这也是连接本章与课程"端到端与扩散式规划"方向的桥。回顾 §3.6/§5.7：LGP 是"符号树搜索（外）+ KOMO（内）"的双层结构，组合瓶颈在符号层（§5.7）、初值敏感在几何层（§9.1）。神经网络可以分别嵌进这两层，各治一种病。

嵌进符号层：引导/剪枝树搜索（治组合爆炸，局限二）。 符号树随长度指数增长（§5.7），但绝大多数分支几何不可行、白搜。学习的切入点是预测"哪个符号分支值得展开"：

学一个可行性分类器：输入符号状态 + 候选动作，输出"这个动作后大概率几何可行吗"，把低分分支推后或剪掉——相当于给 §5.7 的 branch-and-bound 提供一个学习版的界。
学一个 MCTS 的价值/策略网络：像下棋 AI 那样，用网络给树搜索的展开提供先验，把预算投向有希望的分支。

效果：把"盲目指数搜索"变成"被引导的搜索"，在长序列任务上显著减少要验证的候选数——正对应 §5.7 说的"前沿多在改符号层"。

嵌进几何层：热启动 KOMO（治初值敏感/局部最优，局限一）。 KOMO 是非凸优化，初值差会陷坏局部最优（§9.1、§4.2 陷阱 4-2）。学习的切入点是预测一个好初值：

学一个轨迹预测器：输入任务（物体位姿、目标），输出一条粗轨迹或关键构型，作为 KOMO 的初值，替代 §6.5 那个朴素的直线插值。好初值让 Gauss-Newton（§4.6）更快收敛、更少陷坏区。
学可达性/抓取预测：如 §9.2 提到的 R-LGP 用 reachability margin 引导，预测"从哪抓、站哪里"利于后续，给动作参数（§3.2）一个好的优化起点。

效果：缓解局限一——不是消除非凸，而是用学习把优化送进好盆地，让"优化找到的局部最优"恰好落在全局最优附近。

与扩散式/生成式规划的接口。 更激进的方向是把"生成一条轨迹"本身交给生成模型（扩散模型），再用 LGP 的约束做投影/引导——这就接到了课程"端到端与扩散式规划"方向：扩散模型学到轨迹分布、采样出候选，LGP 的硬约束（切换一致性、碰撞）把采样结果投影到可行流形。这里 LGP 的角色从"求解器"变成"约束投影器/可行性裁判"。值得注意 §9.2 第三条提到的对偶视角（Ha et al. 2020 把 LGP 解释为对后验路径分布拟合）：它在数学上预示了"优化式"与"生成式/推断式"的深层联系——同一个问题，既能当优化解（KOMO），也能当分布推断解（扩散/控制-推断对偶）。

本质洞察：学习引导 LGP 的关键，不是"用神经网络替换 LGP"，而是认清 LGP 双层结构的两个痛点（符号层组合爆炸、几何层初值敏感），让学习各打一处：在符号层当"搜索的先验/剪枝器"，在几何层当"优化的热启动器"。这种"学习提供引导、优化/搜索保证正确"的分工，是当前 TAMP 与学习结合最稳健的范式——它既借了学习的快（预测好方向），又保留了优化/搜索的可靠（约束被精确满足、可行性有保证），避开了纯学习方法"可能输出不可行解"的软肋。理解这个分工，你就明白为什么 §9.2 的本质洞察说"未来属于混合系统"：不是把范式搅成一锅，而是让每个部件做它最擅长的事——学习管直觉与先验，优化管约束与精确，搜索管离散与组合。这也为你读"端到端与扩散式规划"方向埋下伏笔：那边从"纯学习/生成"出发，会反过来遇到"如何保证可行性"的问题，而 LGP 这边的约束机制，正是那个问题的一种答案。

练习¶

（⭐⭐） §9.1 说 LGP 的局限一（非凸局部最优）是 PDDLStream 的强项。解释为什么采样范式"没有局部最优困扰"，而优化范式有。
（⭐⭐⭐） §9.2 列了"用采样为 LGP 提供初值"的结合方向。具体说明：对一个抓取-放置任务，怎么用 PDDLStream 采样出的抓取候选，作为 LGP 优化抓取位姿的初值？这如何缓解 LGP 的初值敏感？
（⭐⭐⭐⭐，跨章综合） §9.1 说两个范式的局限"互为镜像"。列一张表，左列 LGP 三局限、右列 PDDLStream 三局限（查 TAMP_T3 §10.3），逐条说明它们如何互补，并论证"为什么结合能取长补短"。
（⭐⭐⭐⭐，前沿） 据 §9.3：(a) 学习引导 LGP 的两个嵌入点（符号层、几何层）各治哪个局限、各以什么形式（分类器/价值网络/轨迹预测器）嵌入？(b) 为什么说"学习提供引导、优化/搜索保证正确"比"纯学习端到端"更稳健（提示：纯学习方法的输出有什么保证上的软肋）？(c) 在扩散式规划里，LGP 的约束扮演什么角色？

10. LGP 变体全景：一个范式的多个分支 ⭐⭐⭐¶

LGP 自 2015 年提出后，发展出一系列变体，各自针对 §9.1 的某个局限或某类新问题。本节系统梳理这些分支，帮你建立 LGP 生态的全景图——遇到具体问题时，知道该找哪个变体。这些变体也是 §9.2 前沿方向的具体化。

10.1 按"改进目标"梳理变体¶

变体	针对的问题	核心改进	代表工作
原始 LGP	（奠基）	逻辑生成约束 + KOMO 求解	Toussaint 2015
Multi-Bound LGP	求解慢	P1/P2/P3 多层下界剪枝（§5.3）	Toussaint & Lopes 2017
Diverse-LGP	符号层探索不足	用多样化规划探索可行逻辑序列	Ortiz-Haro et al.
RHH-LGP	长时域多物体	几何启发式 + 接收时域（§8.2）	Braun et al. 2022
可微物理 LGP	富接触/工具使用	可微物理 + 稳定模式（§8.1）	Toussaint et al. 2018
不确定性 LGP	几何不确定	随机域扩展、控制-推断对偶	Ha, Driess, Toussaint 2020
R-LGP / RAKOMO	腿足/移动操作	可达性裕度引导优化	（R-LGP 2024）
Sequence-of-constraints MPC	执行时扰动	反应式、时间最优控制	Toussaint et al. 2022

10.2 几个关键变体详解¶

Diverse-LGP：改进符号层的探索。 原始 LGP 的符号树搜索可能反复尝试相似的、都不可行的逻辑序列。Diverse-LGP 引入多样化规划（diverse planning）——让符号层生成一组差异大的候选逻辑序列，更高效地探索可行逻辑计划的空间。这个方法在逻辑层利用先进的多样化规划来探索可行逻辑计划的空间，并最小化需要在连续几何层求解的优化问题数量。直觉：与其在一个区域反复试，不如撒开探索不同的"解法思路"。

可微物理 LGP：让接触可优化。 §8.1 提过的 Toussaint et al. 2018，关键创新是把物理（接触动力学）做成可微的，从而能用梯度优化接触相关的量（接触力、接触时机）。这个框架使机器人能优先选择各种交互模式，并获得诸如"利用接触来减少不确定性"这样有趣的行为。这是 LGP 攻入"富接触操作"主场的技术基石——没有可微物理，接触量就难以用梯度优化。

不确定性 LGP：从确定到随机。 原始 LGP 假设几何完全确定。Ha, Driess & Toussaint 2020 把它扩展到随机域：基于控制-推断对偶，把随机域的 LGP 解释为对后验路径分布拟合一个高斯混合，其中每个逻辑剖面定义一个单一的高斯路径分布。直觉：不确定性下，"最优轨迹"变成"最优的轨迹分布"，不同逻辑序列对应分布的不同模态。这是 LGP 接入 T6（不确定性 TAMP）的桥梁。

R-LGP：为腿足/移动操作引入可达性。 移动操作和腿足机器人有特殊的可达性约束（站位决定能够到哪）。R-LGP / RAKOMO 把可达性裕度（reachability margin）作为优化准则引入 KOMO，用神经网络预测裕度并优化——让梯度优化适配腿足操作。

本质洞察：LGP 的变体谱系，是一个范式"开枝散叶"的典型样本——核心内核（逻辑生成约束 + 优化求解）不变，每个变体针对一个具体痛点做局部改良。Multi-Bound 改求解效率、Diverse 改符号探索、可微物理改接触处理、不确定性版改随机扩展、R-LGP 改可达性、反应式版改执行。这种"内核稳定、分支繁茂"的演化，恰恰证明了 LGP 内核的生命力——它定义的"把操作序列看成带逻辑约束的轨迹优化"这个框架足够通用，能容纳各种扩展。对你的实践启示是：遇到一个 LGP 不好解的问题，先判断卡在哪个环节（符号探索？接触？不确定性？长时域？），再找对应的变体——而不必从头另起炉灶。这也是理解任何成熟方法族的通用思路：抓住不变的内核，理解各分支针对的痛点。

10.3 变体选择速查¶

遇到问题, 选哪个 LGP 变体:
  求解太慢(候选序列多)        → Multi-Bound(§5.3) / Diverse-LGP
  长时域、物体多              → RHH-LGP(几何启发式+接收时域)
  富接触、工具使用、要推理力   → 可微物理 LGP
  几何有不确定性(感知噪声)     → 不确定性 LGP(随机域扩展)
  腿足/移动操作(可达性关键)    → R-LGP / RAKOMO
  执行时有扰动、要反应式       → Sequence-of-constraints MPC
  组合摆放为主、离散选择多      → 考虑换 PDDLStream(§7, 不是 LGP 的主场)

练习¶

（⭐⭐） §10.1 的变体表里，挑三个变体，分别说明它针对 §9.1 的哪个局限（局部最优/长序列贵/建模难）或哪类新问题。
（⭐⭐⭐） Diverse-LGP 用"多样化规划"改进符号层探索。解释为什么"生成一组差异大的候选逻辑序列"比"在一个区域反复试"更高效（提示：结合 §5 的树搜索——相似序列往往在几何上同样不可行）。
（⭐⭐⭐） 不确定性 LGP 把"最优轨迹"变成"最优轨迹分布"，不同逻辑序列对应分布的不同模态。用搭塔任务举例：感知噪声下，为什么"用哪种顺序搭"会对应不同的"结果分布"？
（⭐⭐⭐⭐，综合） 一个真实任务"移动机器人走到桌边、用工具把够不到的杯子拨过来、再抓起放进柜子"，综合了移动、工具使用、长序列。讨论该组合哪些 LGP 变体（提示：R-LGP 处理移动可达、可微物理处理工具接触、RHH-LGP 处理长序列），以及它们能否协同。

误解	正确理解	对应节
优化式 LGP 总比采样式 PDDLStream 高级	各有主场：组合摆放用 PDDLStream，富接触/调优用 LGP	§2 陷阱 2-1
LGP 是"纯优化一键求解"，没有搜索	LGP = 符号树搜索枚举骨架 + 几何优化求解参数，两层	§2 陷阱 2-2、§5 陷阱 5-1
逻辑和几何是两个独立模块顺序执行	逻辑生成约束、几何反馈可行性，双向耦合（非 plan-then-check）	§3 陷阱 3-1
LGP 的动作参数像 PDDLStream 那样采样	动作参数是优化变量，由优化器在约束下解出	§3 陷阱 3-2
只要每段轨迹各自可行就行	还需切换一致性约束保证段间衔接（抓取时手位姿=抓取位姿）	§3 陷阱 3-3
KOMO 的"k 阶"指样条/多项式次数	指代价/约束依赖相邻 k+1 个时间步（时间局部性）	§4 陷阱 4-1
Newton 法总能收敛到好解	非凸问题只找局部最优，强烈依赖初值	§4 陷阱 4-2
对每个候选序列都解完整精度 NLP	用 multi-bound 多层下界先剪枝，只对通过的解 P3	§5 陷阱 5-2
下界 P1/P2 可行就代表原问题可行	下界可行只是必要条件，最终可行性由 P3 确认	§5 陷阱 5-3
LGP 和 PDDLStream 二选一	两范式互补、优劣镜像，前沿常结合（采样供初值+优化精化）	§7.3、§9.2

本章小结¶

本章讲透了缝合符号-几何鸿沟的优化式范式 LGP——与 TAMP_T3 的 PDDLStream（采样式）构成一对双子。回顾全章逻辑链：

§2 从采样到优化：PDDLStream 用采样，LGP 用优化。优化式的动机是采样的两个软肋——需要连续调优的几何量（力、角度、对齐）、全局耦合的长序列（搭塔）。代价是非凸、局部最优、初值敏感。
§3 LGP 核心：逻辑生成约束，优化求解几何。关键一步是把动作参数变成优化变量（与轨迹联合优化）；逻辑通过两类约束接入——切换一致性约束、依赖符号状态的阶段几何约束。从优化理论看，LGP 本质是一个被双层分解的 MINLP（外层离散搜动作序列、内层连续解 NLP），这解释了 §5 架构的必然性（§3.6）。
§4 KOMO：几何层求解器。把轨迹离散化、用 k 阶 Markov 结构（代价/约束只依赖相邻 k+1 步）带来带状 Hessian。求解骨架是两层：内层 Gauss-Newton（平方和目标，正规方程 $(J^\top J)\Delta x=-J^\top\phi$，只需雅可比），外层增广拉格朗日（乘子 + 罚，让有限 $\mu$ 也能精确满足切换一致性、碰撞这些硬约束）；带状结构让每步线性求解关于时间步 $T$ 线性（带状 Cholesky）。特征的雅可比就是机器人学现成的运动学雅可比（§4.9）。在轨迹优化家族里，KOMO 属"约束驱动"分支（与 TrajOpt 同宗，区别于 iLQR/CHOMP 的软代价），这正是 LGP 选它的原因（§4.10）。
§5 求解架构：符号树搜索 + 几何优化两层；multi-bound 用三层下界（P1 有效运动学 / P2 粗时间分辨率 / P3 完整精度）由粗到精剪枝，避免对每个序列都解昂贵 NLP——其加速随候选数/成本比/不可行占比放大（§5.6 给了定量账）。符号树搜索本身用 branch-and-bound + MCTS 引导、并复用经典 PDDL 规划器，组合瓶颈正在这一符号层（§5.7）。
§6 完整案例：搭积木塔体现"全局耦合长序列"；优化玩具落地"逻辑约束 + 动作参数作变量 + 优化求解"的内核（§6.3/§6.4）。进一步打开求解器黑箱——§6.5 用纯 numpy 从零实现 Gauss-Newton + 增广拉格朗日，§6.6 实测带状 vs 稠密求解关于 $T$ 的复杂度差异，§6.7 把 §3+§4+§5 拧成一段完整算法伪代码（树搜索→multi-bound→KOMO 三层嵌套）。
§7 与 PDDLStream 对比：信息流向镜像（几何认证符号 vs 符号生成几何），加速技术相似（乐观对象 vs multi-bound）；选型看几何难点是选择型（PDDLStream）还是调优型（LGP）；§7.4 用"窄缝插书"把两种解法并排走了一遍，让判据落地。
§8 真实应用：工具使用与富接触是 LGP 标志主场；RHH-LGP 用几何启发式 + 接收时域处理长时域多物体；§8.4 把"用木棍够球"的工具任务逐动作展开成 eq/ineq/sos 约束，落实了"逻辑生成约束"。
§9 局限与前沿：非凸局部最优、长序列 NLP 昂贵、建模门槛高；与 PDDLStream 局限互为镜像，前沿在学习引导 + 与采样结合 + 不确定性扩展——学习可分别嵌入符号层（剪枝/引导搜索）与几何层（热启动 KOMO），遵循"学习引导、优化保证"的分工（§9.3）。

一句话收束本章：LGP 把整个操作序列写成一个带逻辑约束的轨迹优化——逻辑定义约束、动作参数作变量、KOMO 求解——用"连续优化"而非"离散采样"缝合符号-几何鸿沟，是富接触与全局耦合操作的主场。

学完本章的自检清单（每条都能讲清，才算真正吃透）：

这十一条若条条能讲清，你就不只是"读过 LGP"，而是真正握住了它的思想内核、数学骨架与工程实现——足以在自己的 TAMP 问题上判断该不该用它、用它时该怎么搭。

术语速查表¶

术语	一句话定义
LGP（逻辑-几何规划）	把操作序列写成带逻辑约束的轨迹优化的 TAMP 范式（Toussaint 2015）
逻辑生成约束	符号动作序列翻译成施加在轨迹上的约束（LGP 核心机制）
动作参数作优化变量	抓取/放置位姿等作为有效自由度，与轨迹联合优化（非采样）
切换一致性约束	动作切换处构型与动作算子一致（抓取时手位姿=抓取位姿）
阶段几何约束	依赖当前符号状态的几何约束（持物段：杯随手动、不碰、不洒）
末态评价 ψ	衡量是否达成目标的代价项（搭塔：塔稳定、支撑关系满足）
KOMO	k 阶 Markov 优化，LGP 的几何层轨迹优化器（Toussaint 2014）
k 阶 Markov 结构	代价/约束只依赖相邻 k+1 个时间步（速度 1 阶、加速度 2 阶）
带状 Hessian	k 阶 Markov 带来的稀疏结构，使 Newton 求解关于时间步线性
Gauss-Newton	平方和目标专用法，用 $J^\top J$ 近似 Hessian，只需雅可比（§4.6）
增广拉格朗日	罚 + 乘子的约束优化法，有限罚权重即可精确满足硬约束（§4.7）
带状 Cholesky	利用带状结构的线性求解，复杂度关于 $T$ 线性（§4.8）
树搜索 + 优化（两层架构）	符号层搜索动作序列，几何层 KOMO 优化每个序列
multi-bound tree search	用多层下界（P1/P2/P3）由粗到精剪枝（Toussaint & Lopes 2017）
P1/P2/P3	有效运动学界 / 粗时间分辨率界 / 完整精度优化
下界可行性必要条件	下界不可行→原问题必不可行，保证剪枝不误杀
RHH-LGP	几何启发式 + 接收时域的长时域 LGP（Braun et al. 2022）
双层 MINLP	LGP 的优化身份：离散 + 连续混合规划，分解为外搜内优（§3.6）
值函数 $V(a_{1:K})$	给定动作序列后内层 NLP 的最优代价；不可行记 $+\infty$（§3.6）

符号表¶

本章数学部分（尤其 §3.4、§4）引入的符号汇总如下。

符号	含义	主要出现
$x(t)$	连续构型轨迹（含动作参数作有效自由度）	§3.1
$x_1,\dots,x_T$	离散化后的构型序列（KOMO 的决策变量）	§4.1
$x_t\in\mathbb{R}^n$	第 $t$ 步构型，单步维度 $n$	§4.8
$T$	轨迹离散化的时间步数	§4.1
$a_{1:K}$	符号动作序列（离散，由树搜索枚举）	§3.4
$K$	动作序列长度（切换次数）	§3.4
$s_k$	第 $k$ 段的符号状态（含运动学模式）	§3.3
$t_k$	第 $k$ 个切换时刻	§3.4
$c(\cdot)$	路径运行代价（平滑 / 能量）	§3.4
$\psi(x(T))$	末态评价（衡量是否达成目标）	§3.4
$h_{\text{switch}}$	切换一致性约束（等式）	§3.3
$g_{\text{path}}$	阶段几何约束（不等式）	§3.3
$k$	k 阶 Markov 的阶（代价/约束牵涉 $k+1$ 个连续步）	§4.2
$\phi_i(x)$	第 $i$ 个特征函数（feature / task map）	§4.4
$\phi(x)$	所有 sos 特征堆成的大向量	§4.6
$\Phi(x)$	平方和目标 $\phi^\top\phi$	§4.6
$J=\partial\phi/\partial x$	全局雅可比	§4.6
$J^\top J$	Gauss-Newton 近似 Hessian（恒半正定、带状）	§4.6
$\Delta x$	Gauss-Newton 更新方向	§4.6
$\lambda_{\text{LM}}$	Levenberg-Marquardt 阻尼系数	§4.6
$\lambda$	拉格朗日乘子（约束，与 $\lambda_{\text{LM}}$ 不同）	§4.7
$\mu$	增广拉格朗日罚权重	§4.7
$L_A(x,\lambda;\mu)$	增广拉格朗日函数	§4.7
$g(x)=0,\ h(x)\le0$	等式 / 不等式约束（通称）	§4.7
$b\approx kn$	$J^\top J$ 的半带宽	§4.8
$p_{ee}(q)$	末端执行器位置（正运动学）	§4.9
$J_{ee}(q)$	机械臂位置雅可比（$3\times n$）	§4.9
$\hat z_i,\ o_i$	关节 $i$ 的旋转轴 / 原点（世界系）	§4.9
P1 / P2 / P3	三层下界：有效运动学 / 粗时间分辨率 / 全精度	§5.3
$V(a_{1:K})$	内层 NLP 值函数（给定动作序列的最优代价）	§3.6

知识点总表¶

#	知识点	核心要点	对应节	难度
1	采样 vs 优化的分野	选择型用采样，调优型用优化	§2.2	⭐⭐⭐
2	优化式的代价	非凸、局部最优、初值敏感	§2.3	⭐⭐⭐
3	LGP 核心结构	逻辑生成约束，优化求解几何	§3.1	⭐⭐⭐
4	动作参数作优化变量	抓取/放置位姿与轨迹联合优化	§3.2	⭐⭐⭐
5	两类约束	切换一致性 + 阶段几何约束	§3.3	⭐⭐⭐
6	LGP 数学形式	min 路径代价+末态评价 s.t. 逻辑约束	§3.4	⭐⭐⭐⭐
7	KOMO 离散化	轨迹→构型序列，构型序列作变量	§4.1	⭐⭐⭐
8	k 阶 Markov 结构	依赖相邻 k+1 步，带状 Hessian	§4.2	⭐⭐⭐⭐
9	Newton 类求解	Gauss-Newton + 增广拉格朗日	§4.3	⭐⭐⭐⭐
10	两层求解架构	符号树搜索 + 几何优化	§5.1	⭐⭐⭐⭐
11	multi-bound 剪枝	P1/P2/P3 由粗到精，下界剪枝	§5.3	⭐⭐⭐⭐
12	搭塔案例	全局耦合长序列，全局协同优化	§6	⭐⭐⭐
13	LGP vs PDDLStream	信息流镜像，加速技术相似	§7.1	⭐⭐⭐
14	选型判据	选择型→PDDLStream，调优型→LGP	§7.2	⭐⭐⭐
15	工具使用主场	富接触、力、几何强耦合	§8.1	⭐⭐⭐
16	RHH-LGP	几何启发式 + 接收时域	§8.2	⭐⭐⭐
17	三类局限	局部最优/长序列贵/建模难	§9.1	⭐⭐⭐
18	局限互补与结合	与 PDDLStream 优劣镜像，前沿结合	§9.2	⭐⭐⭐
19	Gauss-Newton 正规方程	$\Phi=\phi^\top\phi$，$(J^\top J)\Delta x=-J^\top\phi$，只需雅可比	§4.6	⭐⭐⭐⭐
20	增广拉格朗日	乘子 $\lambda$ + 罚 $\mu$，有限 $\mu$ 精确满足硬约束	§4.7	⭐⭐⭐⭐
21	带状 Hessian 与复杂度	k 阶 → 带宽 $kn$ → 带状 Cholesky 关于 $T$ 线性	§4.8	⭐⭐⭐⭐
22	特征雅可比推演	末端位置特征的雅可比就是机械臂位置雅可比	§4.9	⭐⭐⭐⭐
23	KOMO 在轨迹优化家族	约束驱动分支（vs iLQR/CHOMP 软代价），故 LGP 选它	§4.10	⭐⭐⭐
24	multi-bound 定量估算	加速随候选数/成本比/不可行占比放大	§5.6	⭐⭐⭐
25	Gauss-Newton 手算	一步流程：φ→J→JᵀJ→解正规方程→更新	§4.11	⭐⭐⭐⭐
26	从零实现 KOMO	numpy 手写 GN 内层 + AL 外层	§6.5	⭐⭐⭐⭐
27	带状求解实测	带状关于 T 线性、稠密立方，且带状非近似	§6.6	⭐⭐⭐
28	LGP 完整算法	树搜索→multi-bound→KOMO(GN+AL) 三层嵌套	§6.7	⭐⭐⭐
29	双层 MINLP 视角	离散外层搜索 + 连续内层 NLP，$V(a)$ 值函数	§3.6	⭐⭐⭐⭐
30	增广拉格朗日手算	乘子爬向 $\lambda^\star$、有限 $\mu$ 即满足，胜纯罚	§4.12	⭐⭐⭐⭐
31	符号树搜索内部	分支因子、B&B+MCTS 引导、瓶颈在符号层	§5.7	⭐⭐⭐
32	两范式并排例	窄缝插书：对齐属调优型→优化精确满足	§7.4	⭐⭐⭐
33	工具任务约束走查	逐动作 eq/ineq/sos + 跨动作耦合→全局求解	§8.4	⭐⭐⭐
34	端到端 LGP demo	枚举顺序→KOMO→选优, 双层骨架可运行缩影	§6.8	⭐⭐⭐⭐
35	学习引导 LGP	学习引导符号层/热启动几何层, 引导+保证分工	§9.3	⭐⭐⭐

延伸阅读¶

核心论文（LGP，§3-§5）： - Toussaint (2015), "Logic-Geometric Programming: An Optimization-Based Approach to Combined Task and Motion Planning," IJCAI. ⭐⭐⭐⭐ —— 本章主参考，LGP 的原始论文，逻辑生成约束的形式化、搭塔案例。 - Toussaint (2014), "KOMO: Newton Methods for k-Order Markov Constrained Motion Problems," arXiv:1407.0414. ⭐⭐⭐⭐ —— 几何层求解器 KOMO，§4 的来源。 - Toussaint & Lopes (2017), "Multi-Bound Tree Search for Logic-Geometric Programming in Cooperative Manipulation Domains," ICRA. ⭐⭐⭐⭐ —— multi-bound tree search（P1/P2/P3 三层下界），§5 的来源。

应用与扩展（§8-§9）： - Toussaint, Allen, Smith & Tenenbaum (2018), "Differentiable Physics and Stable Modes for Tool-Use and Manipulation Planning," RSS. ⭐⭐⭐⭐ —— 工具使用、可微物理，§8.1 的来源，展示 LGP 富接触主场。 - Braun, Ortiz-Haro, Toussaint & Oguz (2022), "RHH-LGP: Receding Horizon and Heuristics-Based Logic-Geometric Programming," IROS（arXiv:2110.03420）. ⭐⭐⭐ —— 长时域 LGP（几何启发式 + 接收时域），§8.2 的来源。 - Ha, Driess & Toussaint (2020), "Probabilistic Framework for Constrained Manipulations and TAMP under Uncertainty"（arXiv:2003.04259）. ⭐⭐⭐ —— LGP 的不确定性扩展，接 T6。 - Ortiz-Haro et al., "Conflict-Directed Diverse Planning for Logic-Geometric Programming," ICAPS. ⭐⭐⭐ —— 用多样化规划改进 LGP 符号层，§5 的延伸。

优化基础（§4 的数学）： - Nocedal & Wright (2006), Numerical Optimization, 2nd ed., Springer. ⭐⭐⭐⭐ —— Gauss-Newton（§4.6）、增广拉格朗日（§4.7）、KKT 条件的权威教材级推导，想把 §4 的数学补到无跳步看这本（尤其第 10 章最小二乘、第 17 章罚函数与增广拉格朗日）。 - Boyd & Vandenberghe (2004), Convex Optimization, Cambridge. ⭐⭐⭐ —— 最小二乘、正规方程、对偶与 KKT 的清晰背景，配 §4.6/§4.11 的平方和最小化理解。 - 稀疏/带状线性代数：任意数值线性代数教材中"带状矩阵的 Cholesky 分解"一节，或 SciPy linalg.solve_banded 文档。⭐⭐ —— §4.8/§6.6 带状复杂度的实现依据。

动手与实现（§6 的代码）： - Toussaint 组的 rai/KOMO 开源库（github.com/MarcToussaint/rai）。⭐⭐⭐⭐ —— §6.7 那张映射表对应的真实库；§6.5/§6.8 的从零实现可视作它的玩具版，读懂玩具再上手真库会顺很多。 - 本章 §6.5（从零实现 GN + 增广拉格朗日）、§6.6（带状实测）、§6.8（端到端 demo）的代码本身即"最小可复现实现"，建议亲手跑通、逐块对照 §4 公式。⭐⭐⭐⭐

对照与背景： - TAMP_T3《PDDLStream 与流式集成》。⭐⭐⭐⭐ —— 本章的对照范式，必须对照阅读。 - TAMP_T1 §5（LGP 入门）、轨迹优化教材（CHOMP/TrajOpt）、KKT 条件与 SQP。⭐⭐ —— 本章的前置。

本章与后续章节的关系¶

后续章节	本章哪部分为它铺垫	它如何深化/对比本章
T5 行为树	§8.3 优化出的轨迹交执行层	行为树负责执行监控；优化的开环计划需执行层闭环跟踪
T6 不确定性 TAMP	§9.2 LGP 的不确定性扩展	随机域 LGP（控制-推断对偶），信念空间下的优化
T7 大模型规划	§8.3 符号目标可由 LLM 给出、§9.2 基础模型辅助	LLM/VLM 给目标或约束，LGP 求解几何
T8 多机 TAMP	§5（multi-bound 源于合作操作）、§8.2 异构多机	多机协同操作的联合优化
T9 综合实战	§7 选型、§9.2 与采样结合	把 LGP 与 PDDLStream/学习整合的完整系统

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关节
KOMO 优化不收敛	初值差 / 约束冲突 / 问题非凸陷坏区	1. 换合理初值（直线插值/粗层解）2. 检查约束是否互相矛盾 3. 多初值重启	§4.2 陷阱 4-2
GN 更新方向爆掉/在某些自由度上发散	$J^\top J$ 近奇异（退化方向，欠约束）	1. 加 Levenberg-Marquardt 阻尼 $\lambda_{\text{LM}}I$（式 4.5）2. 检查是否有自由度没被任何特征约束 3. 自适应调阻尼	§4.6 陷阱 4-3
约束总差一点满足（手没精确到位/轨迹擦障碍）	用了纯罚函数、$\mu$ 不够大	1. 改用增广拉格朗日：加乘子并外层更新 $\lambda\leftarrow\lambda+\mu g$（式 4.8）2. 适度增大 $\mu$ 而非推到无穷 3. 检查是否漏了该硬的约束	§4.7 陷阱 4-4
优化出的轨迹在切换处不连贯	缺切换一致性约束	1. 检查每个动作切换点是否有 $h_{\text{switch}}$ 2. 抓取时手位姿是否=抓取位姿 3. 运动学挂接是否正确	§3.3 陷阱 3-3
求解极慢，每个序列都耗时	对每个候选都解完整 NLP（P3）	1. 用 multi-bound 先解 P1/P2 剪枝 2. 确认 P1 是否够便宜 3. 加几何启发式引导搜索	§5 陷阱 5-2
找到的"解"实际不可行	把下界 P1/P2 可行当成原问题可行	1. 确认最终可行性由 P3 确认 2. P1/P2 仅用于剪枝	§5 陷阱 5-3
优化出怪异/别扭的轨迹	代价函数或约束建模不当	1. 检查代价是否漏了平滑/能量项 2. 检查约束是否过松（缺碰撞/限位）3. 检查末态评价 ψ 是否正确刻画目标	§3、§9.1 局限三
长序列任务规划不出来	符号树爆炸、NLP 太多	1. 用 RHH-LGP 的接收时域拆短 2. 加几何启发式剪枝 3. 检查目标是否可达	§8.2、§9.1 局限二
接触/工具任务优化失败	接触动力学约束非凸难解	1. 检查接触模式是否需显式建模 2. 用好初值（接触点先验）3. 考虑可微物理框架	§8.1
该用采样的问题硬用 LGP 卡住	选型错误（组合摆放用了优化）	1. 判断几何难点是选择型还是调优型 2. 组合摆放改用 PDDLStream	§7.2 陷阱 2-1
从零实现的 AL 约束迟迟不满足	缺乘子更新 / $\mu$ 调度太慢 / 内层迭代不够	1. 确认每个外层都有 $\lambda\leftarrow\lambda+\mu g$ 2. $\mu$ 几何增大 3. 内层 GN 多迭代几步	§6.5、§4.7
带状解与稠密解对不上	带状打包索引错 / 半带宽设小了	1. 核对 `ab` 矩阵填充 2. 半带宽 $b\ge kn$ 3. 用 `assert np.allclose` 定位	§6.6、§4.8
端到端 demo 报"无可行顺序"	可达/约束设置过紧，所有骨架都不可行	1. 放宽 `reach_xmax` 2. 检查放置高度/切换约束 3. 确认目标本可达	§6.8

方法速查表¶

方法/概念	用途	关键点	对应节
LGP 优化式	把操作序列写成带逻辑约束的轨迹优化	逻辑生成约束、动作参数作变量	§3
动作参数作有效自由度	抓取/放置位姿与轨迹联合优化	不采样、由优化器解出	§3.2
切换一致性约束	保证动作切换处几何衔接	$h_{\text{switch}}(x(t_k),a_k)=0$	§3.3
KOMO	几何层轨迹优化求解器	离散化 + k 阶 Markov + Newton	§4
k 阶 Markov	代价/约束的时间局部性	带状 Hessian → 关于 T 线性	§4.2
Gauss-Newton	平方和目标的求解内核	$(J^\top J)\Delta x=-J^\top\phi$，只需雅可比	§4.6
增广拉格朗日	处理硬等式/不等式约束	乘子 + 罚，外层更新 $\lambda\leftarrow\lambda+\mu g$	§4.7
树搜索 + 优化	LGP 两层求解架构	符号枚举骨架 + 几何验证	§5.1
multi-bound	多层下界剪枝	P1/P2/P3 由粗到精，下界必要条件	§5.3
接收时域（RHH-LGP）	长序列拆滚动窗口	控制每次优化规模	§8.2
选型判据	LGP vs PDDLStream	选择型→采样，调优型→优化	§7.2
双层 MINLP 分解	解释 LGP 求解架构的由来	外层离散搜索 + 内层连续 NLP，$V(a)$ 值函数	§3.6
符号树搜索	LGP 离散外层的内部机制	分支因子、B&B + MCTS 引导、复用 PDDL 规划器	§5.7

研究实践建议¶

学习 LGP 有一条清晰的能力进阶路线，每阶段对应本章不同部分。

第一阶段——理解优化视角（对应 §2-§3）：先彻底理解"采样 vs 优化"的分野（§2.2）和 LGP 的核心三件事——逻辑生成约束、动作参数作优化变量、两类约束（§3）。对照 TAMP_T3 的 PDDLStream 来学，每学一个 LGP 设计就回想 PDDLStream 怎么做。检验标准：能解释为什么工具使用适合 LGP 而非 PDDLStream。

第二阶段——动手优化（对应 §4、§6）：跑通 §6.3 的优化玩具，体会"逻辑约束 + 动作参数作变量 + 优化求解"的内核。再吃透 KOMO 的数学内核——亲手做一遍 §4.11 的 Gauss-Newton 手算、跑通 §6.5 的从零实现（GN 内层 + 增广拉格朗日外层）、用 §6.6 实测带状求解的线性复杂度，把 §4.6–§4.8 的公式和能跑的代码对上。若有条件，用真实的 LGP 库（如 Toussaint 组的 rai/KOMO）跑一个搭塔或 pick-place。检验标准：能改 §6.3/§6.5 玩具加入新约束/新动作参数，能讲清 §6.7 伪代码每行对应哪一节。

第三阶段——理解求解架构（对应 §5、§7）：吃透树搜索 + 优化的两层架构和 multi-bound 剪枝（§5），完成与 PDDLStream 的系统对比（§7）。检验标准：能解释 multi-bound 为什么安全（下界必要条件）、能用"选择型 vs 调优型"判据为任意任务选范式。

第四阶段——前沿（对应 §8-§9）：关注 LGP 的前沿——学习引导优化（缓解初值/搜索）、与采样结合（取两范式之长）、不确定性扩展、反应式执行。这些都建立在本章对"逻辑约束 + 优化求解"内核的理解之上。LGP 的优化视角也是理解可微规划、扩散式规划等生成式 TAMP 的根基——往这些方向延伸时，回到本章的"把操作序列看成轨迹优化"这一核心。

累积项目（贯穿全章，建议亲手搭一遍）：本章的 §6 代码不是孤立片段，而是一个可以渐进搭建的最小 LGP 系统，建议按下面四步累积，每步都跑通再进下一步：

几何层雏形（§6.3/§6.4）：先用 scipy.minimize 解一个固定动作序列的 2D pick-and-place，体会"切换约束 + 平滑代价 + 动作参数作变量"。
打开求解器（§6.5）：把 scipy 黑箱换成自己手写的 Gauss-Newton 内层 + 增广拉格朗日外层，让 §4.6/§4.7 的公式变成你能逐行解释的代码。
接上符号层（§6.8）：在几何层外面套一层符号顺序枚举 + 按可行性/代价选优，得到一个真能跑的端到端 LGP——这就是 §6.7 伪代码的可运行版。
逼近真实（挑战）：把全排列换成被引导的树搜索（§5.7）、把单次 KOMO 换成带 multi-bound 的级联（§5.3）、把约束加上碰撞 ineq，或干脆迁移到 rai/KOMO 真库重做一遍。

完成这条链，你对 LGP 的理解就从"读懂了"变成"造得出"——这是本章硬核部分（§3–§6）的终极检验。

下一章预告：TAMP_T5《行为树与执行》将从"规划"转向"执行"——前面 T1-T4（PDDLStream/LGP）都在解决"算出一个计划"，但真实机器人执行计划时会遇到失败、扰动、环境变化。行为树是机器人执行层的主流范式，它负责把规划出的计划稳健地执行下去：监控、恢复、必要时触发重规划。本章 §8.3 说"优化出的轨迹交执行层"，T5 就是那个执行层。我们将精读 Nav2 的默认行为树，理解执行层如何把 T1-T4 的"会算"变成真机上的"能用"。

外层轮	\(\lambda\)（更新前）	\(\mu\)	内层解 \(x=\frac{6-\lambda+\mu}{2+\mu}\)	约束违反 \(g=x-1\)	更新后 \(\lambda\)
0	0	1	\(7/3=2.333\)	\(1.333\)	\(1.333\)
1	\(1.333\)	2	\(6.667/4=1.667\)	\(0.667\)	\(2.667\)
2	\(2.667\)	4	\(7.333/6=1.222\)	\(0.222\)	\(3.556\)
3	\(3.556\)	8	\(10.444/10=1.044\)	\(0.044\)	\(3.911\)
4	\(3.911\)	16	\(18.089/18=1.005\)	\(0.005\)	\(3.991\)

LGP 的约束（§3.3）	在 KOMO 里如何表达
切换一致性约束	在切换时间步 \(t_k\) 上施加等式约束（手位姿 = 抓取位姿）
阶段几何约束（不碰撞）	每个时间步上的不等式约束（碰撞距离 ≥ 0）
阶段几何约束（抓握保持）	持物阶段：杯随手动，作为运动学挂接（有效自由度）
动作参数（抓取/放置位姿）	作为额外的优化变量（有效自由度，§3.2）
平滑/能量代价	k 阶 Markov 代价项（惩罚加速度，§4.2）

LGP 约束（§3.3）	数学类型	在 AL 里如何进入
切换一致性 \(h_{\text{switch}}(x(t_k),a_k)=0\)	等式	线性乘子项 \(\lambda^\top h_{\text{switch}}\) + 二次罚 \(\frac{\mu}{2}\\|h_{\text{switch}}\\|^2\)，外层更新 \(\lambda\)
碰撞距离 \(\ge 0\)	不等式	单边罚 \(\max(0,\cdot)^2\)，乘子非负
关节限位	不等式	单边罚
抓握保持（杯随手动）	运动学挂接	不入约束，化为有效自由度（§4.5），自动满足
平滑 / 能量 sos 代价	（非约束）	直接进平方和目标 \(\Phi\)，不需乘子

维度	KOMO	iLQR / DDP	CHOMP	TrajOpt (SQP)	直接配点
变量空间	构型 \(x_t\)（速度/加速度用差分得到）	状态-控制 \((x_t, u_t)\)	构型路径	构型 / 状态	状态 + 控制
动力学如何进入	代价/约束特征（差分，§4.9）	显式前向 rollout（积分动力学）	不显式（平滑度量隐含）	作为约束	配点等式约束
硬约束（碰撞/限位）	增广拉格朗日（§4.7）精确满足	弱（需 AL-iLQR / 障碍函数变体）	软（距离场代价，不保证满足）	SQP + 符号距离约束	NLP 约束
稀疏求解组织	带状 Cholesky 直接分解（§4.8）	Riccati 反向递推（动态规划）	协变梯度（按平滑度量）	稀疏 QP	稀疏 NLP 求解器
设计哲学	特征平方和 + 增广拉格朗日	动态规划	协变梯度下降	信赖域 SQP	同时法（simultaneous）

符号	含义	主要出现
\(x(t)\)	连续构型轨迹（含动作参数作有效自由度）	§3.1
\(x_1,\dots,x_T\)	离散化后的构型序列（KOMO 的决策变量）	§4.1
\(x_t\in\mathbb{R}^n\)	第 \(t\) 步构型，单步维度 \(n\)	§4.8
\(T\)	轨迹离散化的时间步数	§4.1
\(a_{1:K}\)	符号动作序列（离散，由树搜索枚举）	§3.4
\(K\)	动作序列长度（切换次数）	§3.4
\(s_k\)	第 \(k\) 段的符号状态（含运动学模式）	§3.3
\(t_k\)	第 \(k\) 个切换时刻	§3.4
\(c(\cdot)\)	路径运行代价（平滑 / 能量）	§3.4
\(\psi(x(T))\)	末态评价（衡量是否达成目标）	§3.4
\(h_{\text{switch}}\)	切换一致性约束（等式）	§3.3
\(g_{\text{path}}\)	阶段几何约束（不等式）	§3.3
\(k\)	k 阶 Markov 的阶（代价/约束牵涉 \(k+1\) 个连续步）	§4.2
\(\phi_i(x)\)	第 \(i\) 个特征函数（feature / task map）	§4.4
\(\phi(x)\)	所有 sos 特征堆成的大向量	§4.6
\(\Phi(x)\)	平方和目标 \(\phi^\top\phi\)	§4.6
\(J=\partial\phi/\partial x\)	全局雅可比	§4.6
\(J^\top J\)	Gauss-Newton 近似 Hessian（恒半正定、带状）	§4.6
\(\Delta x\)	Gauss-Newton 更新方向	§4.6
\(\lambda_{\text{LM}}\)	Levenberg-Marquardt 阻尼系数	§4.6
\(\lambda\)	拉格朗日乘子（约束，与 \(\lambda_{\text{LM}}\) 不同）	§4.7
\(\mu\)	增广拉格朗日罚权重	§4.7
\(L_A(x,\lambda;\mu)\)	增广拉格朗日函数	§4.7
\(g(x)=0,\ h(x)\le0\)	等式 / 不等式约束（通称）	§4.7
\(b\approx kn\)	\(J^\top J\) 的半带宽	§4.8
\(p_{ee}(q)\)	末端执行器位置（正运动学）	§4.9
\(J_{ee}(q)\)	机械臂位置雅可比（\(3\times n\)）	§4.9
\(\hat z_i,\ o_i\)	关节 \(i\) 的旋转轴 / 原点（世界系）	§4.9
P1 / P2 / P3	三层下界：有效运动学 / 粗时间分辨率 / 全精度	§5.3
\(V(a_{1:K})\)	内层 NLP 值函数（给定动作序列的最优代价）	§3.6