第 9 章　人形/四足+臂协同控制——异构具身体的能力互补与跨形态协调¶

性质：算法工程教学 | 难度跨度：⭐⭐ ~ ⭐⭐⭐⭐ | 预计精读：16-20 小时

一句话定位：当一个机器人团队里同时有**人形（humanoid）、四足（quadruped）和带臂的移动操作平台（legged manipulator）——三种身体、三套动力学、三种长处——它们如何**分工、协调、共同抬起一张桌子？本章把前八章的多机协作工具（图、共识、任务分配、分布式 MPC、内力分配、异构编队）从"同构或弱异构"推进到**强异构具身（heterogeneous embodiment）的极端**：参与者不仅传感器和速度不同，连**自由度数、平衡机理、接触方式**都根本不同。我们要解决三件事——按能力把任务派对（capability-aware allocation）、让形态各异的个体在同一个物理任务上协调（cross-morphology coordination）、以及**用一个统一的 whole-body 框架同时描述它们（unified whole-body formulation）**。

前置自测¶

开始前先回答下面 5 个问题。答不出 2 题以上，建议先回前置章节补齐——本章每一步都建立在前八章的多机协作地基与 05 运动控制方向的 loco-manipulation 单体基础之上，欠了账会在第三节（跨形态协调）卡住。

（接第 5 章 / §60）协同搬运里的"抓取矩阵（grasp matrix）\(G\)"是什么？ \(N\) 个接触点抓同一个刚体时，物体所受的合力旋量 \(w\) 与各接触力 \(f_i\) 的关系 \(w = G f\) 中，\(G\) 的维度是多少？为什么 \(G\) 的**零空间**对应"内力（internal force）"？本章三个异构体抓桌子时，会直接复用这套抓取矩阵语言。（答不出 → 回 §60 协同搬运与力控）
（接第 6 章 / §70）"能力互补（capability complementarity）"在异构编队里指什么？ 一架无人机和一台地面车，谁适合侦察、谁适合搬运？为什么"按代价矩阵 \(C\) 做匈牙利指派"不足以表达"这台机器人根本干不了这件事"这种硬约束？（答不出 → 回 §70 异构多机协同，§6.1-6.2）
（接第 3 章 / §40）CBBA（基于共识的捆绑算法）的两阶段结构是什么？ 它如何保证分布式拍卖不冲突？本章把任务派给人形/四足/臂时，会在 CBBA 的"出价函数"里塞进**形态相关的能力得分**——你需要先记得 CBBA 的出价是怎么算的。（答不出 → 回 §40，§3.2）
（接 05 运动控制 / 浮动基座 WBC）什么叫"浮动基座（floating base）"？ 一个 \(n\) 自由度的腿足或人形机器人，它的广义坐标 \(q\) 为什么是 \(6+n\) 维而不是 \(n\) 维？whole-body QP 里"接触约束""动力学约束""任务目标"分别是什么？本章的统一 whole-body 框架就是把多个浮动基座系统**拼**成一个大 QP。（答不出 → 回 05 运动控制方向 10_足式/90_WBC分层优化与TSID.md 与 20_机械臂/F07_浮动基座WBC理论.md）
（接第 4 章 / §50）分布式 MPC 用 ADMM 把"联合优化"切成"每个机器人本地子问题 + 一致性协调"。 ADMM 的三步（本地更新 / 全局聚合 / 对偶上升）分别在做什么？为什么它天然适合"每个机器人有自己的动力学、但共享一个耦合约束"的结构？本章的异构体各自动力学不同，正是 ADMM 大显身手的场景。（答不出 → 回 §50 分布式 MPC，§4.3-4.4）

参考答案要点（先自己答，再对照）：

抓取矩阵 \(G \in \mathbb{R}^{6 \times 3N}\)（每个接触点贡献 3 维力，若含力矩则 \(6N\) 列），把 \(3N\) 维接触力堆叠向量映射到物体的 6 维合力旋量 \(w=(f, \tau)\)。\(G\) 通常宽于高（\(3N > 6\)），其零空间 \(\mathcal{N}(G)=\{f : Gf=0\}\) 里的接触力对物体**不产生任何净运动**——这正是"内力"：它只在物体内部相互抵消，用来夹紧物体、调节摩擦裕度。本章三体抓桌子时，谁出多少内力、谁出多少操作力，就是在 \(\mathcal{N}(G)\) 里分配。
能力互补指**不同形态的个体各自擅长不同子任务**，团队整体覆盖了任何单一形态都覆盖不了的任务空间。无人机适合侦察（高视角、快速覆盖），地面车适合搬运（大载荷、稳定底盘）。匈牙利指派假设"任意机器人都能做任意任务，只是代价不同"；但异构场景里存在**硬可行性约束**（无人机抓不起 50kg 的箱子，代价不是"高"而是"无穷/不可行"），必须在分配模型里显式加入能力-任务的可行性掩码（capability mask），而非仅靠连续代价。
CBBA 两阶段：①**捆绑构建（bundle construction）——每个机器人本地贪婪地往自己的任务包里加边际收益最高的任务；②**冲突消解（conflict resolution）——通过邻居间的"带时间戳最大值共识"，对每个任务全网达成"归谁"的一致，败者把任务踢出包重新竞拍。出价 = 边际得分（marginal score），本章会把"形态-任务匹配度"乘进这个边际得分。
浮动基座指机器人的根连杆（base）在世界系中**没有固定**，可以自由平移和旋转，所以广义坐标 \(q = (p_{\text{base}} \in \mathbb{R}^3,\ R_{\text{base}} \in SO(3),\ q_{\text{joint}} \in \mathbb{R}^n)\) 共 \(6+n\) 维（姿态常用四元数 4 维表示但只有 3 个自由度）。Whole-body QP 的三类要素：动力学约束（\(M\dot v + h = S^\top \tau + J_c^\top f\)，牛顿-欧拉方程）、接触约束（接触点不滑动/不穿透 + 摩擦锥）、任务目标（末端跟踪、质心调节、姿态保持，写成加权二次代价或分层零空间）。
ADMM 三步：①**本地更新**——每个机器人固定邻居的"共识变量"，只优化自己的决策变量（本地 MPC 子问题）；②**全局聚合**——把各机器人对共享变量的估计做平均（或加权），形成共识值；③**对偶上升**——更新拉格朗日乘子，惩罚"本地估计与共识值的偏差"。它适合"可分离目标 + 耦合约束"：每个机器人目标独立（自己的动力学和代价），但通过少数耦合约束（如负载附着点位移一致）连在一起，ADMM 让耦合只通过共识变量传递，无需共享各自完整模型。

本章目标¶

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

精确定义"强异构具身协作"问题：用一个统一的形式化框架描述"人形 + 四足 + 臂"团队——刻画每个个体的形态向量（morphology vector）、能力画像（capability profile）、以及团队层面的任务-能力匹配，说清它与第 6 章"弱异构（空地）编队"的本质区别在哪。
设计能力互补的任务分配：把第 3 章的 CBBA 出价函数扩展为**形态感知出价（morphology-aware bidding）**，引入能力可行性掩码与能力-任务效用矩阵，并实现一个能把"侦察派给人形、精细操作派给臂、重载移动派给四足"的分配器；论证为什么纯代价匈牙利在此失效。
推导跨形态协调控制的三种范式：①**领导者-跟随者（leader-follower）+ 阻抗协调**——一个个体主导物体轨迹、其余通过力反馈柔顺跟随；②**统一抓取矩阵 + 内力分配**——把三个异构末端当作同一刚体的接触点，在零空间里分配内力；③**分布式 MPC（ADMM）协调**——每个异构体跑自己的 whole-body MPC，通过一致性约束协调负载。说清三者各自的假设、通信需求与失效边界。
构建统一 whole-body 框架：把多个**不同自由度、不同接触模式**的浮动基座系统拼装成一个团队级 whole-body QP——写出堆叠的动力学约束、跨体耦合约束（共抓物体的运动学闭链）、以及分层任务栈，理解为什么"统一"不等于"集中式求解"。
解析 RoboDuet / FALCON 的双智能体（dual-agent）协调机制：理解 RoboDuet 如何用"loco 策略 + arm 策略"两个策略通过相互动作引导实现单体 whole-body loco-manipulation，FALCON 如何用"下肢稳定 + 上肢力自适应"双智能体解耦训练——并把这种**体内双智能体协调**与**体间多智能体协调**做严格类比和区分。
设计并分析一个三体协同操作场景：以"人形（精细操作）+ 四足×2（移动支撑）+ 桌面物体"为例，完整走一遍"任务分配 → 角色指派 → 内力规划 → whole-body 跟踪 → 平衡补偿"流程，并能预测各环节的故障模式。
建立与 05 运动控制复合方向的交叉引用地图：说清本章（多机视角的异构协同）与复合方向单体 loco-manipulation（四足+臂动力学、力敏感人形、多机协作 LocoMani）的分工——哪些是"一个身体内部的协调"、哪些是"多个身体之间的协调"，以及它们共享哪些数学工具。

本章知识导航¶

本章的知识结构是一棵以"形态各异的机器人如何共同完成一个物理操作任务"为根的树。树干是四个递进的核心问题，树枝是每个问题的技术细节、范式对比与工程实现。

强异构具身协作"问题是什么、难在哪？ (§9.1 问题形式化)
    │
    ▼
谁来干哪件事？——能力互补任务分配 (§9.2)
    │   形态感知出价 + 能力掩码 + 角色指派
    ▼
形态各异的个体怎么在同一物理任务上协调？ (§9.3 跨形态协调控制)
    │
    ├─→ 范式 A：领导者-跟随者 + 阻抗柔顺
    ├─→ 范式 B：统一抓取矩阵 + 内力分配（零空间）
    └─→ 范式 C：分布式 MPC（ADMM）一致性协调
    │
    ▼
能不能用一套数学同时描述它们？ (§9.4 统一 whole-body 框架)
    │   堆叠浮动基座动力学 + 跨体闭链耦合 + 分层任务栈
    ▼
体内协调 vs 体间协调：双智能体范式 (§9.5 RoboDuet/FALCON 精读)
    │
    ▼
端到端场景串讲 (§9.6 人形+四足×2 协同抬桌)
    │
    ▼
与 05 复合方向的交叉地图 + 前沿与开放问题 (§9.7)

小节	主题	难度	一句话
§9.1	强异构具身协作问题形式化	⭐⭐	形态向量 + 能力画像 + 任务-能力匹配，比第 6 章弱异构再进一步
§9.2	能力互补任务分配	⭐⭐⭐	CBBA 出价里塞进形态匹配度 + 能力可行性掩码
§9.3	跨形态协调控制三范式	⭐⭐⭐⭐	领导跟随 / 内力分配 / 分布式 MPC，三条路各有取舍
§9.4	统一 whole-body 框架	⭐⭐⭐⭐	多个异构浮动基座拼成一个团队级 QP（统一≠集中）
§9.5	双智能体协调（RoboDuet/FALCON）	⭐⭐⭐	体内双策略协调，与体间多机协调的镜像
§9.6	端到端场景：人形+四足×2 抬桌	⭐⭐⭐	把全章工具串成一条流水线
§9.7	复合方向交叉地图 + 前沿	⭐⭐	体内/体间分工 + sim2real + 开放问题

两条阅读线：

协调控制线（理解物理协调）：§9.1→§9.3→§9.4→§9.6，重点是内力分配、whole-body 拼装、平衡补偿的推导
多智能体决策线（理解分工与学习）：§9.1→§9.2→§9.5→§9.7，重点是能力分配、双智能体范式与 MARL 前沿

无论哪条线，§9.1 都是必读——它定义了贯穿全章的形态/能力符号与问题边界。§9.3 与 §9.4 是两个 ⭐⭐⭐⭐ 的硬核小节，值得花最多时间。

科研发展脉络¶

在钻进具体方法前，先把这条研究线的来龙去脉理清——知道每个工作"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记名字有用得多。本表是本章骨架保留的核心脉络，下面正文会逐一展开其技术内核。

年份	论文	Venue	引用	核心贡献
2021	Zimmermann 等, "Go Fetch: Mobile Manipulation in Unstructured Environments"	IROS	~60	四足+臂移动操作先驱——把臂装上 ANYmal，开启 legged manipulation 工程线
2024	Pan 等, "RoboDuet: Cooperative Policy for Whole-Body Loco-Manipulation"	RA-L	~新	双策略协调——loco 策略 + arm 策略相互引导，跨四足本体零样本迁移
2024	Fu 等, "Mobile ALOHA"	CoRL	~440	移动双臂操作标杆——底盘+双臂的低成本遥操作与模仿学习
2025	Zhang 等, "FALCON: Force-Adaptive Humanoid Loco-Manipulation"	arXiv→CoRL	~新	双智能体力自适应——下肢稳定 + 上肢力补偿，人形 3D 末端力交互
2025	Pandit 等, "Learning Decentralized Multi-Biped Control for Payload Transport"	CoRL	~新	多人形搬运——刚性附着的多双足载体涌现协调，免重训换数量/构型
2025	Sleiman/Bjelonic 等 (ETH), "Shared Object Manipulation with a Team of Collaborative Quadrupeds"	arXiv	~新	多足+臂团队搬运——力闭合抓取 + 团队级混合运动-力控

关键实验室脉络：ETH RSL（Go Fetch / ALMA / Shared Object Manipulation——四足+臂工程化与多机搬运）→ Stanford IRIS（Mobile ALOHA / RoCo——移动操作与 LLM 多机协调）→ CMU LeCAR（FALCON / ASAP——力自适应人形 loco-manipulation）→ Oregon State DRL（Cassie 多双足搬运）→ SJTU/HKU（RoboDuet——双策略 loco-manipulation）。

看这条线，有两条交织的主线：第一条是"单体 loco-manipulation 的成熟"——从 Go Fetch 把臂装上四足，到 RoboDuet/FALCON 用双（智能体/策略）解耦"动"与"操作"，单个异构体已能稳健地边走边操作；第二条是"从单体走向多体团队"——decPLM/多双足搬运/Shared Object Manipulation 让多个（同构或异构）loco-manipulator 共抬一个物体。本章正处在两条线的**交汇点**：当团队里的成员本身就是形态各异的 loco-manipulator（人形 + 四足 + 臂），如何把"体内协调"（whole-body）和"体间协调"（multi-robot）统一起来。这是 2025 年仍然开放的前沿。

本质洞察：异构具身协作的难点不在"异构"这个标签，而在**异构破坏了对称性**。同构多机（如 10 台一样的 Go2）可以参数共享、可以假设可互换、可以用置换不变（permutation-invariant）网络——因为换任意两个个体系统不变。一旦个体形态不同，这种对称性消失：人形和四足**不可互换**，任务分配不能只看代价、协调控制不能假设同样的动力学、学习不能简单参数共享。本章几乎所有的技术难点，追根溯源都是在**补偿这个被打破的对称性**。

前置知识桥接¶

本章是 Part（多机协作）的第 9 章，紧接第 5、6 章并大量复用 05 运动控制方向的单体 loco-manipulation 基础。这里把最关键的前置点重新激活——你不必翻回去就能跟上：

回顾第 5 章（§60 协同搬运与力控）：抓取矩阵与内力分配。第 5 章我们用抓取矩阵 \(G\) 把"多个接触点的力"映射到"物体的合力旋量"，并指出 \(G\) 的零空间对应**内力**（不推动物体、只夹紧物体的力）。协同搬运的核心问题是：给定期望的物体合力旋量 \(w_d\)，怎么把它分配成各机器人的接触力 \(f_i\)，使得既产生 \(w_d\)、又满足每个机器人的摩擦锥与力矩限制。本章把这套语言原封不动地搬过来——只不过现在三个接触点分别是**人形的手、四足-A 的背、四足-B 的背**，它们的力能力天差地别，内力分配要照顾这种不对称。
回顾第 6 章（§70 异构多机协同）：能力互补与异构编队。第 6 章处理的是**弱异构**——空中无人机 + 地面车这类"传感器/速度/视角不同，但都能移动、都能搭载"的异构。我们在那里学了能力画像、空地联合定位、跨模态融合。本章是它的**强异构延伸**：参与者连**自由度、平衡机理、接触方式**都不同（人形双足动态平衡 vs 四足静/动态稳定 vs 固定臂无需平衡），能力互补不再只是"分头干"，而要"在同一个物理任务上协调发力"。
回顾第 4 章（§50 分布式 MPC）：ADMM 切分联合优化。第 4 章我们用 ADMM 把"多足编队的联合 MPC"切成"每足本地子问题 + 一致性协调"。本章的范式 C（§9.3.3）直接复用这套机器：每个异构体跑自己的 whole-body MPC（人形的、四足的各不相同），通过对"共抓物体轨迹"的一致性约束协调——ADMM 的妙处正在于它**不要求各子系统模型相同**，天然适配异构。
回顾 05 运动控制方向：浮动基座 whole-body 控制。05 方向的 10_足式/90_WBC分层优化与TSID.md 和 20_机械臂/F07_浮动基座WBC理论.md 建立了单个浮动基座系统的 whole-body QP——广义坐标 \(q=(p,R,q_j)\in \mathbb{R}^{6+n}\)、动力学约束、接触约束、分层任务栈。本章 §9.4 的"统一框架"就是把多个这样的 QP **拼**成一个团队级 QP。如果你对单体 WBC 还不熟，强烈建议先读那两章——本章不重新教单体 WBC，只教怎么把多个拼起来。

前向预告：本章固定了"谁有什么形态"（团队组成是给定的）。下一章（第 10 章 MARL 基础，§110）和第 11 章（MARL 多机运动协调，§120）将把"协调策略"本身变成**学出来的**——用多智能体强化学习（MARL）替代本章的手工设计协调律。现在只需要知道：本章的范式 A/B/C 是"模型驱动"的协调，它们可解释、可验证，但需要精确模型；MARL 是"数据驱动"的协调，它能处理接触/摩擦等难建模因素，但牺牲可解释性和安全保证。第 12 章（MARL+规控混合，§130）会把两者缝起来。

如果跳过本章会怎样¶

跳过本章，你会在三个具体的地方卡住。

场景一（"用匈牙利分配，结果把搬箱子派给了无人机"）。 你有一个混合团队——人形、四足、臂——和一批任务（侦察、开门、搬重物）。你图省事，沿用第 3 章的匈牙利指派：算一个"机器人×任务"的代价矩阵，求最小代价完美匹配。结果分配器把"搬 30kg 重物"派给了载荷能力只有 3kg 的固定臂——因为在代价矩阵里它"恰好"代价最低（臂离物体最近）。匈牙利不知道"臂根本搬不动"。§9.2 会告诉你：异构分配必须先用**能力可行性掩码**把"做不了"的组合代价设为 \(+\infty\)（或在出价阶段直接排除），再在可行集上优化，并且出价要乘进**形态-任务匹配度**而非只看距离。

场景二（"三个机器人一起抬桌子，桌子被内力撕裂/某个机器人被拽倒"）。 你让人形和两台四足各抓桌子一角，每个都独立地"跟踪桌子目标轨迹"。结果：三者对桌子姿态的估计有微小偏差，各自施加的力在物体内部不抵消——要么把桌子越夹越紧直到结构吱嘎作响（内力失配），要么人形为了跟踪轨迹施加了一个侧向力，把正在静态站立的四足-A 拽离平衡区摔倒（操作力反噬平衡）。§9.3 会告诉你：多体协调必须显式地在**统一抓取矩阵的零空间**里协商内力，并且每个个体的 whole-body 控制器必须把"操作产生的反作用力"作为已知扰动**前馈补偿**进平衡约束——这正是 FALCON 的"力自适应"在多体场景的推广。

场景三（"代码里人形用一套控制栈、四足用另一套，物体在两套之间消失了"）。 你给人形写了一个 whole-body 控制器，给四足写了另一个，各自独立运行。当它们共抬一个物体时，物体的运动学闭链约束（三个手/背的相对位姿被物体刚性锁定）没有任何一方负责维护——物体在两套独立控制器的"接缝"处失去了协调，出现拉扯或脱手。§9.4 会告诉你：哪怕你不做集中式求解，也必须有一个**统一的运动学闭链约束**把各异构体的末端绑在物体上，并把这个约束分发到各方的 MPC/WBC 里作为耦合项——"统一框架"解决的就是这个"接缝丢失"问题。

预计阅读时间¶

模式	时长	适合
精读	16-20 小时	第一次系统学异构具身协同：逐节读动机→推导→代码，亲手推导统一抓取矩阵与团队级 whole-body QP 的拼装，跑通 §9.6 的端到端场景伪代码，做完每节练习。建议分 4-5 次。
速读	5-7 小时	有多机协作与单体 loco-manipulation 背景、想建立全局图景：读每节"动机"+"理论"小标题、框住的关键公式、§9.3 三范式对比表、§9.6 端到端流程，跳过代码细节。
速查	40-60 分钟	已学过、回来查特定内容：直接定位到对应小节，看 boxed 公式 + 三范式对比表 + 角色指派表 + API 速查 + 故障排查。

本章符号约定¶

符号	含义	首见
\(\mathcal{R} = \{r_1, \dots, r_K\}\)	异构团队的 \(K\) 个个体	§9.1
\(\mathbf{m}_k\)	个体 \(r_k\) 的形态向量（morphology vector）	§9.1
\(\mathbf{c}_k\)	个体 \(r_k\) 的能力画像（capability profile）	§9.1
\(U_{kj}\)	个体 \(r_k\) 对任务 \(\tau_j\) 的能力效用	§9.2
\(\Phi_{kj} \in \{0,1\}\)	能力可行性掩码（\(r_k\) 能否做 \(\tau_j\)）	§9.2
\(b_{kj}\)	形态感知出价（morphology-aware bid）	§9.2
\(q_k = (p_k, R_k, q_{j,k})\)	个体 \(r_k\) 的浮动基座广义坐标，\(\in \mathbb{R}^{6+n_k}\)	§9.4
\(v_k, \tau_k\)	个体 \(r_k\) 的广义速度、关节力矩	§9.4
\(M_k, h_k\)	个体 \(r_k\) 的质量矩阵、非线性项	§9.4
\(J_{c,k}, J_{e,k}\)	个体 \(r_k\) 的接触雅可比、末端（操作）雅可比	§9.4
\(G\)	团队对共抓物体的统一抓取矩阵	§9.3
\(f_i\)	第 \(i\) 个抓取接触点的力（旋量）	§9.3
\(w_o, x_o\)	物体所受合力旋量、物体位姿	§9.3
\(f_{\text{int}}\)	内力（\(\in \mathcal{N}(G)\)）	§9.3
\(\pi_{\text{loco}}, \pi_{\text{arm}}\)	RoboDuet 的运动策略、臂策略	§9.5
\(F_{\text{ext}}\)	末端外力（FALCON 力课程的施力）	§9.5

§9.1 强异构具身协作问题形式化 ⭐⭐¶

这一节解决什么问题：在动手设计任何算法之前，我们要先把"人形+四足+臂协同"这件事**说清楚**——什么叫"异构具身"？它比第 6 章的空地异构编队"强"在哪？一个混合团队的状态、能力、任务到底该用什么数学对象描述？这一节建立贯穿全章的形式化语言：形态向量、能力画像、任务-能力匹配。后面四节（分配、协调、统一框架、场景）全部建立在这套语言上。

动机：当队友的"身体"根本不一样¶

设想一个家庭服务场景。你的机器人团队要完成一件事：把客厅那张沉重的实木茶几从 A 处搬到 B 处，途中要绕过散落的玩具，并在搬之前先把茶几上的台灯拿下来放到一边。

你手头有三个机器人：

一台**人形机器人**（humanoid，如 Unitree G1 / Fourier GR-1）——双臂灵巧、能做精细抓取（拿台灯）、双足可跨越障碍，但**双足动态平衡脆弱**，承受侧向力的能力有限。
两台**四足机器人**（quadruped，如 Unitree Go2 / ANYmal）——四条腿提供宽大支撑多边形、承重和抗扰强、能在杂乱地面稳健行走，但**没有灵巧手**（即便背上装了臂，臂的工作空间和精度也不如人形双臂）。
一台**固定基座的协作机械臂**（如果场景里有的话，比如装在桌边的 Franka）——定位精度极高、力控细腻，但**不能移动**，工作空间被基座锁死。

现在的核心问题是：这三种身体、三套动力学、三种长处，怎么配合着把这件事做完？

直觉上每个人都能说出大致分工："让人形拿台灯（它有灵巧手），让两台四足抬茶几（它们承重强、稳），机械臂这次用不上（茶几不在它够得着的地方）。" 但要把这个直觉变成**可执行的算法**，必须先回答一连串更尖锐的问题：

"人形有灵巧手"——怎么用数字表达"人形比四足更适合精细抓取"？这个"更适合"是连续的程度，还是非黑即白的能/不能？
"两台四足抬茶几"——它们各抬一角，发的力必须协调（否则茶几被拧、或某台被拽倒）。这个协调的数学约束是什么？
"人形也来帮忙扶一下茶几"——如果人形也参与抬，它脆弱的平衡能承受多大的茶几反作用力？超过了会怎样？
这三个机器人**彼此不可替换**：你不能说"随便派两个去抬"，因为四足抬得动、人形未必抬得稳。这种不可替换性，在第 3 章匈牙利指派的"代价矩阵"框架里**根本无法表达**——匈牙利假设任意机器人都能做任意任务。

这些问题的共同根源，是参与者的**身体本质不同**。要系统地处理它们，我们需要一套描述"身体差异"的形式化语言。

如果不形式化、只凭直觉会怎样¶

有人会说："分工不就是常识吗？人形干灵巧活、四足干粗活，还要什么形式化？" 让我们看看只凭直觉会在哪里翻车——这正是 R6B 的反事实推理：去掉形式化这一步，后果是什么？

后果一：分配无法自动化、无法扩展。 "人形拿台灯"这种分工，在 3 个机器人、3 个任务时你能口算。但服务机器人真实场景里可能是 8 个异构机器人、20 个任务，且任务和机器人都在动态变化（新任务到达、某机器人电量耗尽退出）。没有形式化的能力-任务匹配，你无法写出一个能在任意团队组成上自动跑的分配器，只能为每个场景手工硬编码——这在工程上不可接受。

后果二：协调律没有统一接口。 如果"人形扶茶几"和"四足抬茶几"被当成两类完全不同的事情分别处理，那么当人形和四足**同时**接触茶几时，没有任何一段代码知道该如何协调它们对茶几的合力——因为你从未把"人形的手对茶几的作用"和"四足的背对茶几的作用"统一到同一个数学对象（合力旋量）里。形式化的价值，是给异构的接触一个**统一接口**。

后果三：可行性与安全无法验证。 "人形也来帮忙扶"——能不能扶、扶多大力，取决于人形平衡能承受多大扰动。如果没有把"形态→能力上限"的映射形式化，你就无法在派任务前**验证**这个分配是否安全，只能让机器人去试，试到摔倒为止。

本质洞察：形式化不是学术洁癖，而是**把"不可替换性"变成算法能消费的输入**。同构多机之所以"不需要"形式化每个个体（直接编号 1..N 即可），是因为它们可互换——身份不携带信息。异构多机里，个体的"身份"（它是人形还是四足）携带了决定性的信息（能做什么、能承受什么），这个信息必须被显式编码，算法才能利用它。

历史：从同构、弱异构到强异构¶

异构多机协作的研究是逐步"加难度"演进的，理解这条线能帮你定位本章在整个谱系里的位置。

第一阶段——同构多机（homogeneous）。 早期多机器人研究（1990s-2000s 的 swarm robotics、formation control）几乎都假设个体相同：一群一样的轮式小车、一群一样的无人机。研究重点是**协调机制本身**（编队、共识、覆盖），个体差异被刻意抽象掉。这一阶段的成果（图论建模、共识、势场编队）我们在第 1、2 章学过，它们是所有多机的地基，但回避了异构。

第二阶段——弱异构（weakly heterogeneous）。 2010s 起，空地协同（air-ground collaboration）成为热点：无人机 + 地面车（UAV-UGV）团队。个体不同了——飞行 vs 地面、快 vs 慢、高视角 vs 近距操作——但这种异构是**功能互补型**的：各干各的（无人机侦察、地面车执行），它们的协作主要在**信息层**（无人机给地面车提供地图/目标），物理交互很少。第 6 章（§70）讲的就是这一阶段：能力画像、跨模态融合、空地联合定位。它的关键特征是：异构体很少在同一个物理任务上同时发力。

第三阶段——强异构具身（strongly heterogeneous embodiment）。 这是本章的主战场，也是 2023-2025 年的前沿。随着 loco-manipulation 的成熟（四足装臂、人形成熟），出现了一类新需求：形态各异的个体必须在同一个物理操作任务上紧耦合协调——共抬一个物体、协同装配、互相支撑。这里的异构是**具身层（embodiment-level）的：不仅功能不同，连**自由度数、平衡机理、接触动力学**都不同。代表工作如 ETH 的 Shared Object Manipulation（多足+臂团队搬运）、Oregon State 的多双足搬运、以及把人形拉进团队的各种探索。它的关键特征是：**异构体在同一物体上同时施力，物理耦合极强。

阶段	异构程度	协作主要发生在	代表场景	对应章节
同构	无（个体相同）	协调机制层	编队、覆盖、蜂群	第 1-2 章
弱异构	功能/传感器不同	信息层	空地协同（UAV+UGV）	第 6 章 §70
强异构具身	自由度/平衡/接触都不同	物理交互层	人形+四足+臂共抬物体	本章 §9

本章正是把多机协作推到这个谱系的最右端。理解这个定位很重要：本章不是"第 6 章的复习"，而是它的**质变延伸**——当协作从信息层下沉到物理交互层，几乎所有工具都要重新设计。

理论：形态向量、能力画像与任务-能力匹配¶

现在建立形式化语言。我们分三层：描述**个体身体**（形态向量）、描述**个体能做什么**（能力画像）、描述**任务需要什么**（任务-能力匹配）。

1. 团队与个体¶

一个异构团队记作

\[ \mathcal{R} = \{r_1, r_2, \dots, r_K\} \]

共 \(K\) 个个体。与同构多机不同，这里的下标 \(k\) 不是可互换的编号，而是携带身份信息的标识——\(r_1\) 是人形、\(r_2,r_3\) 是四足，这个对应关系是问题的一部分，不能随意置换。

2. 形态向量 \(\mathbf{m}_k\)¶

每个个体 \(r_k\) 用一个**形态向量（morphology vector）** \(\mathbf{m}_k\) 刻画其物理身体。它至少包含以下分量：

\[ \boxed{\ \mathbf{m}_k = \big(\ \underbrace{n_k}_{\text{关节自由度}},\ \underbrace{\beta_k}_{\text{平衡类型}},\ \underbrace{\mathcal{C}_k}_{\text{接触模式集}},\ \underbrace{\mathcal{W}_k}_{\text{工作空间}},\ \underbrace{\lambda_k^{\max}}_{\text{载荷/力上限}},\ \underbrace{\nu_k}_{\text{移动性}}\ \big)\ } \]

逐项解释（这些是后面所有算法的输入，务必理解每一项的物理含义）：

\(n_k\)：关节自由度数。人形通常 \(n \approx 20\text{-}30\)（双腿+双臂+腰+颈），四足 \(n=12\)（4 腿×3），四足+单臂 \(n=12+6=18\)，固定臂 \(n=6\text{-}7\)。这个数直接决定该个体广义坐标的维度（\(6+n_k\)，若浮动基座）。
\(\beta_k\)：平衡类型，取值如 \(\{\text{双足动态平衡},\ \text{四足支撑多边形},\ \text{固定基座（无需平衡）}\}\)。这是异构最尖锐的差异——它决定了"该个体能承受多大外部扰动"。双足动态平衡最脆弱，固定基座最鲁棒。
\(\mathcal{C}_k\)：接触模式集，该个体能用哪些部位与环境/物体接触。人形：双足（locomotion）+ 双手（manipulation）；四足：四足端（locomotion）+ 背载平台或臂末端（manipulation）；固定臂：仅末端执行器。
\(\mathcal{W}_k\)：操作工作空间，末端执行器能够到达的空间区域（相对于 base）。固定臂的 \(\mathcal{W}\) 被基座锁死；移动平台的 \(\mathcal{W}\) 是"base 可达区域 ⊕ 臂工作空间"的 Minkowski 和——这是移动操作的核心优势。
\(\lambda_k^{\max}\)：载荷/操作力上限，该个体末端能稳定施加的最大力/力矩。固定臂可能很大（基座吸收反力），人形受限于平衡（侧向力上限低），四足介于两者之间。
\(\nu_k\)：移动性，移动速度、可跨越障碍高度、地形适应性。固定臂 \(\nu=0\)，四足地形适应最强，人形可上下台阶但速度受限。

多视角理解（A）——形态向量的两种读法。形态向量可以从两个互补角度理解： - 角度 1（约束视角）：\(\mathbf{m}_k\) 是个体 \(r_k\) 的**能力边界**——它划出了"\(r_k\) 做不到什么"。\(\lambda_k^{\max}=3\text{kg}\) 意味着派给它超过 3kg 的搬运任务就是不可行约束。 - 角度 2（特征视角）：\(\mathbf{m}_k\) 是个体的**特征向量**——它是任务-能力匹配函数的输入，决定了"\(r_k\) 在各类任务上的得分"。

同一个 \(\mathbf{m}_k\)，在 §9.2 的分配里既用作硬约束（角度 1，生成可行性掩码），又用作软偏好（角度 2，生成出价得分）。这两种用法都不可省——只用约束会丢失"虽然能做但不擅长"的信息，只用偏好会派出"根本做不到"的分配。

3. 能力画像 \(\mathbf{c}_k\)¶

形态向量描述"身体"，但任务分配真正关心的是"这个身体能完成哪类**任务原语（task primitive）、完成得多好"。我们把这层抽象记作**能力画像（capability profile）：

\[ \mathbf{c}_k : \mathcal{P} \to [0, 1] \cup \{\bot\} \]

其中 \(\mathcal{P} = \{\text{精细抓取},\ \text{重载搬运},\ \text{快速移动},\ \text{越障},\ \text{精密对位},\ \text{侦察},\ \dots\}\) 是一组**任务原语**。\(\mathbf{c}_k(p) \in [0,1]\) 表示个体 \(r_k\) 执行原语 \(p\) 的**胜任度**（1 = 完美胜任，0 = 勉强能做），\(\bot\) 表示**根本做不了**（硬不可行）。

能力画像是形态向量经过一个**能力评估函数** \(g\) 计算出来的：\(\mathbf{c}_k = g(\mathbf{m}_k)\)。例如：

精细抓取胜任度 \(\propto\)（是否有灵巧手）×（末端精度）/（平衡敏感度）——人形高、四足+夹爪中、固定臂高。
重载搬运胜任度 \(\propto \lambda_k^{\max} \times\)（接触稳定性）——四足高、固定臂中（受工作空间限制）、人形低。
快速移动 \(\propto \nu_k\)——四足高、人形中、固定臂 \(\bot\)。

下面给出本章贯穿示例（人形 \(r_1\) + 四足 \(r_2,r_3\)）的能力画像表，后面所有讨论都基于它：

任务原语 \(p\)	人形 \(r_1\)	四足 \(r_2\)	四足 \(r_3\)	说明
精细抓取（拿台灯）	0.95	0.40	0.40	人形灵巧手优势明显
重载搬运（抬茶几）	0.50	0.90	0.90	四足承重稳，人形受平衡限制
快速移动	0.60	0.85	0.85	四足地形适应 + 速度优
越障（绕玩具）	0.70	0.80	0.80	都行，四足略优
精密对位（放台灯到指定点）	0.90	0.45	0.45	人形末端精度高
侦察（先扫一遍客厅）	0.75	0.65	0.65	人形视角高、传感器全

注意表中**没有 \(\bot\)——因为这个团队里每个原语至少都"能做"（四足背上若装了简易夹爪也能勉强抓取）。但如果团队里有一台**固定臂，它的"快速移动"和"越障"就是 \(\bot\)（硬不可行），这正是后面能力掩码要拦截的情形。

3.5 能力评估函数 \(g\) 的构造——把"身体"翻译成"能力"¶

上面说能力画像由 \(\mathbf{c}_k=g(\mathbf{m}_k)\) 算出，但 \(g\) 长什么样、怎么从形态向量算出那张能力表？这一步是工程落地的关键——上表里的 0.95、0.40 不是拍脑袋填的，而应有可复现的计算依据。否则 §9.2 的分配就建在沙地上（"垃圾进垃圾出"）。我们给出三种构造 \(g\) 的方法，从简到繁，各有适用场景。

方法一：可解释的乘积式打分（白盒，推荐入门）。 把每个任务原语的胜任度写成形态分量的显式函数。以"精细抓取"为例：

\[ c_k(\text{精细抓取}) = \underbrace{\mathbb{1}[\text{有灵巧手}]}_{\text{0/1 门控}} \cdot \underbrace{\sigma\!\big(a\cdot \text{末端精度}_k\big)}_{\in(0,1)} \cdot \underbrace{\frac{1}{1+b\cdot \text{平衡敏感度}_k}}_{\text{平衡越敏感越扣分}} \]

逐项读：第一项是**硬门控**——没有灵巧手（只有夹爪/无末端）直接 0（退化为 \(\bot\) 的边界）；第二项把"末端定位精度"经 sigmoid 压到 \((0,1)\)，精度越高越接近 1；第三项是**惩罚因子**——平衡越敏感（人形 base 一动末端就晃）扣分越多。这种乘积式的好处是**可解释、可调试**：某个分数不合理，能逐项查是哪个因子的问题。重载搬运、快速移动同理各写一个打分函数（重载 \(\propto \lambda^{\max}\times\)接触稳定性，移动 \(\propto \nu\)）。

理论-工程桥接（D）——为什么用乘积而非加权和。这里用**乘积**（各因子相乘）而非加权和（各因子相加），是有意为之。加权和会让"某项很差"被"某项很好"补偿——比如一个没有手的平台，若用加权和，可能靠"末端精度高"把精细抓取分拉上来，荒谬。乘积式则让**任何一个关键因子为 0 就整体为 0**（短板一票否决），这符合"能力的木桶效应"：精细抓取需要手 AND 精度 AND 平衡稳，缺一不可。乘积式天然编码了"必要条件的合取（AND）"，加权和编码的是"可补偿的折中"——选哪个取决于该原语的因子是"都必需"还是"可互补"。这个区分直接决定你的 \(g\) 写对没写对。

方法二：物理仿真标定（灰盒，推荐工程）。 白盒打分的系数（上面的 \(a,b\)、各权重）从哪来？最可靠的来源是**让每个个体在仿真里实际试做每个原语，用成功率/精度/能耗标定胜任度**：

对每个个体 r_k 和每个原语 p：
   在仿真里跑 N 次 p（随机化初始条件/目标）
   记录：成功率 s、平均完成精度 e、平均能耗/时间 cost
   c_k(p) = f(s, e, cost)   # 如 s 加权 + 精度奖励 - 能耗惩罚，归一化到[0,1]
   若 s < s_min（如 5%）：判定 c_k(p) = ⊥（硬不可行）

这样能力画像**有实测依据**，且 \(\bot\) 的判定也客观（成功率低于阈值即不可行）。代价是要为每个"个体×原语"跑一批仿真，但这是一次性离线投入，换来分配层的可靠性。

方法三：数据/学习驱动（黑盒，研究级）。 当原语复杂、难以手工建模（如"在杂乱环境抓取不规则物体"），可用学习的方式：收集个体执行各原语的数据，训一个回归模型预测胜任度，甚至端到端学一个 \(g_\theta(\mathbf{m}_k)\to\mathbf{c}_k\)。这与 §9.7 提到的"LLM 做能力分配"（RoCo）相通——让大模型基于形态描述直接推断能力。黑盒方法表达力最强，但牺牲可解释性，且需要数据。

系统性分类（E）——三种 \(g\) 沿"可解释 ↔ 表达力"谱排列。方法一（白盒乘积）最可解释、最易调试，但表达力受限于你手写的函数形式；方法二（仿真标定）用实测数据校准白盒系数，兼顾可解释和准确，是工程主流；方法三（学习）表达力最强、能处理复杂原语，但黑盒、需数据。这和本章反复出现的"模型 ↔ 学习"光谱同构（§9.5、§9.7）——能力评估这一步同样有模型方法和学习方法的取舍。入门用方法一建立直觉，工程用方法二上线，研究级问题（难建模原语）才上方法三。无论哪种，关键是 \(g\) 要**可复现**——同样的形态向量必须算出同样的能力画像，否则分配结果不可重复。

一个反面教训：能力画像的"自评"陷阱。 一个隐蔽错误是让每个个体**自己声明**能力（如出厂参数表写"载荷 5kg"），而不实测。问题在于：标称载荷 5kg 往往是**理想条件**（基座固定、垂直提举），而实际任务里个体可能在移动/侧向受力——此时有效载荷远低于标称。若能力画像直接抄标称值，分配器会高估个体能力，派出实际做不到的任务。这是方法二（仿真/实测标定）优于"抄参数表"的根本原因——胜任度必须在任务相关的条件下度量，而非标称的理想条件。

4. 任务-能力匹配¶

最后，任务侧。一个任务 \(\tau_j\) 由它**需要哪些原语、各需多强**来刻画——记作需求向量 \(\mathbf{d}_j : \mathcal{P} \to [0,1]\)。例如"抬茶几"任务 \(\tau_{\text{table}}\) 的需求：重载搬运 \(0.9\)、快速移动 \(0.4\)、精细抓取 \(0.1\)（抓桌角不需太巧）。

个体 \(r_k\) 对任务 \(\tau_j\) 的**能力效用（capability utility）** 定义为需求与能力的匹配度——一个加权内积：

\[ \boxed{\ U_{kj} = \sum_{p \in \mathcal{P}} d_j(p)\, \cdot\, \phi\big(\mathbf{c}_k(p)\big),\quad \phi(\bot) = -\infty\ } \]

这里 \(\phi\) 把胜任度映射为效用（通常 \(\phi(x)=x\) 或某个凹函数以体现边际递减），且关键地把 \(\bot\) 映射到 \(-\infty\)——只要任务需要某个原语而个体根本做不了，效用就崩到负无穷，自动排除。\(U_{kj}\) 越大，表示 \(r_k\) 越**适合**做 \(\tau_j\)。

用上表算一下：抬茶几任务 \(U_{1,\text{table}}\)（人形）\(\approx 0.9\times0.5 + 0.4\times0.6 + 0.1\times0.95 = 0.785\)；\(U_{2,\text{table}}\)（四足）\(\approx 0.9\times0.9 + 0.4\times0.85 + 0.1\times0.4 = 1.19\)。四足效用明显高于人形——这就是"四足更适合抬茶几"这个直觉的定量表达。

理论-工程桥接（D）：能力效用 \(U_{kj}\) 不是抽象的——它将直接成为 §9.2 中 CBBA 出价函数的核心项。我们把第 3 章学的"出价 = 边际收益"扩展为"出价 = 边际收益 × 能力效用"，于是 CBBA 的分布式拍卖自动倾向于把任务派给最适合的形态。形式化在这里**直接变成了代码里的一个乘法因子**——这正是 R6D 要求的"推导完立刻连接工程"。

5. 强异构带来的三个结构性后果¶

把上面的形式化串起来，我们能精确地说出强异构相比同构/弱异构多了哪三个结构性难点——这也是后续三节分别要攻克的：

分配层（§9.2）：效用矩阵 \(U\) 含 \(-\infty\) 项（硬不可行），普通匈牙利/线性指派不能直接用，必须先掩码再优化，且要处理"一个任务需要多个互补个体"的情形（抬茶几要 2 台四足）。
协调层（§9.3）：异构体的接触模式 \(\mathcal{C}_k\) 不同（人形用手、四足用背），但共抬一个物体时必须统一到同一个抓取矩阵 \(G\)；且平衡类型 \(\beta_k\) 不同意味着每个个体能承受的操作反力上限不同，内力分配要照顾这种不对称。
统一描述层（§9.4）：自由度 \(n_k\) 不同导致各个体广义坐标维度不同（\(6+n_k\)），要把它们拼成一个团队级 whole-body 优化，必须处理"维度异构 + 接触模式异构 + 平衡约束异构"的堆叠。

这三个后果，就是本章接下来的三块硬骨头。

6. 深入一层：三种身体的动力学到底差在哪¶

形态向量和能力画像是"接口层"的抽象。但要真正理解协调为什么难，必须下沉到**动力学层**——看清人形、四足、臂三种身体的运动方程到底差在哪。这一小节回答初学者最该问的问题："不都是浮动基座系统吗？拼起来不就行了？" 答案是：它们的浮动基座方程**形式相同，但每一项的性质天差地别**，正是这些差异让协调成为难题。

回顾单体浮动基座动力学（§9.4 会详用）：\(M(q)\dot v + h(q,v) = S^\top\tau + J_c^\top f_c + J_e^\top f_e\)。三种身体的差异体现在每一项：

动力学要素	人形（humanoid）	四足（quadruped）	固定臂（fixed arm）
广义坐标维度 \(6+n\)	\(6+23\approx29\)（高）	\(6+12=18\)（中）	\(0+6=6\)（无浮动基座）
接触雅可比 \(J_c\)	双足，接触点少（2 个面）	四足，接触点多（4 个点）	无环境接触（基座固定）
支撑多边形	小（两脚间），动态平衡	大（四脚围成），准静态可稳	无（基座承担一切）
欠驱动程度	强欠驱动（6 个浮动 DOF 不可直接驱动 + 平衡耦合）	中欠驱动（6 个浮动 DOF，但接触多、易约束）	全驱动（无浮动 DOF）
质心-接触耦合	极强（CoM 稍偏离支撑区即倾倒）	较弱（支撑区大、容错高）	无（CoM 随便动，基座扛着）
操作反力承受	弱（侧向反力直接威胁平衡）	强（四足分摊反力）	极强（基座吸收全部反力）

对比性思维（B）——"都是浮动基座"是个危险的相似性。人形、四足、（移动）臂确实都能写成 \(M\dot v + h = S^\top\tau + J^\top f\) 的浮动基座形式——这是它们"像"的部分。但**不要把这个相似性延伸到"所以可以同样地控制它们"。关键不像的部分在于**欠驱动与平衡的耦合强度：人形是强欠驱动 + 质心-接触强耦合（平衡是悬在刀尖上的实时任务），四足是中欠驱动 + 弱耦合（平衡相对宽裕），固定臂根本不欠驱动（无平衡问题）。同一个 whole-body QP 框架套在三者上，约束的紧迫程度完全不同：人形的平衡约束是命门（违反即倒），固定臂压根没有平衡约束。这个差异是 §9.4 分层任务栈把"人形平衡 P1"置于高优先级的根本原因，也是 §9.3 内力分配要给人形单独加侧向力上限的根本原因。

这张表解释了本章几乎所有设计决策的"为什么"： - 为什么人形当跟随者而非领导者（§9.6 陷阱 2）？因为人形质心-接触强耦合 + 操作反力承受弱——让它主导承重，平衡随时会崩。 - 为什么内力分配要给人形单独加侧向力约束（§9.3 陷阱 2）？因为只有人形的"能输出多大力"和"输出后还能否站稳"严重不一致（强欠驱动 + 强耦合）。 - 为什么团队动力学是块对角的（§9.4）？因为三种身体没有共享关节/base，各自的 \(M_k\) 互不交叠，只通过操作力 \(f_e\) 耦合。 - 为什么 ADMM 适合（§9.3-C）？因为块对角结构天然可分离，各体解自己的 \(M_k\dot v_k\) 子问题，耦合只在 \(f_e\)。

本质洞察：异构协调的全部技术难点，最终都可以追溯到**这张动力学差异表里的某一行**。当你在调试一个协调问题时（物体被拽、人形倒了、内力发散），问自己："这个症状对应表里哪一行的差异没被正确处理？" ——多半是"质心-接触耦合"或"操作反力承受"那两行（人形最脆弱的两项）没在约束/优先级里被尊重。把这张表刻在脑子里，本章后四节的所有公式你都能"知其所以然"。

这三个后果加上这张动力学差异表，就是本章接下来三节（分配、协调、统一框架）要逐一攻克的全部理论基础。

7. 贯穿全章的主线：异构破坏了什么对称性（深入）¶

本章反复说"异构破坏了同构多机的对称性"，并把它当作几乎所有技术难点的总根源。这句话听起来抽象——这一小节把它**讲到数学层**，看清被破坏的到底是什么对称性、为什么这个破坏会一路传导到分配、协调、可扩展性。理解这一层，你会发现 §9.2-§9.4 的每个技术其实都在"修补"同一处破缺。

同构多机的对称性：置换不变（permutation invariance）。 在同构多机（第 1-5 章，\(K\) 个一模一样的机器人）里，有一个深刻的数学性质：交换任意两个机器人的编号，整个问题不变。形式化地，设 \(\sigma\) 是机器人编号的任意置换，系统的代价/动力学/约束满足

\[ J\big(x_{\sigma(1)},\dots,x_{\sigma(K)}\big) = J\big(x_1,\dots,x_K\big),\quad \forall\,\sigma\in S_K \]

即代价函数对置换群 \(S_K\) 不变。直观说：机器人 1 和机器人 2 完全可互换，谁是"1 号"纯粹是个标签。这个对称性极其有用——它让你可以用**参数共享**（所有个体用同一个策略/控制器，因为它们等价）、置换不变网络（如平均池化、图神经网络的对称聚合）来处理任意数量个体，且天然支持数量扩展（多一个个体只是多一个等价单元）。同构多机 MARL 能轻松扩到上百个体，全靠这个对称性。

强异构打破了它。 在强异构团队里，交换人形 \(r_1\) 和四足 \(r_2\) 的编号，问题**变了**——因为它们的形态向量 \(\mathbf{m}_1\neq\mathbf{m}_2\)、能力画像 \(\mathbf{c}_1\neq\mathbf{c}_2\)、动力学 \(M_1\neq M_2\)、维度 \(n_1\neq n_2\)。代价函数不再对 \(S_K\) 不变：

\[ J\big(\dots,\underbrace{x_2}_{\text{把四足放到人形的位置}},\dots,\underbrace{x_1}_{\text{把人形放到四足的位置}},\dots\big) \neq J(x_1,x_2,\dots) \]

下标 \(k\) 不再是可丢弃的标签，而**携带不可置换的身份**（§9.1 开头就强调过 \(r_1\) 是人形这件事是"问题的一部分"）。置换对称性破缺了。

本质洞察——本章的所有技术，都是在"对称性破缺"后重建结构。一旦置换对称性没了，同构多机赖以扩展的工具（参数共享、置换不变聚合）全部失效——你不能用同一个策略控制人形和四足（它们动力学不同），不能把它们的状态对称地平均池化（它们不等价）。本章三节正是对这个破缺的三处修补： - §9.2（分配）：同构下任务和个体可任意配对（对称），异构下要用能力效用 \(U_{kj}\) 和掩码 \(\Phi_{kj}\) 显式编码"谁和谁配"的非对称性——出价函数里乘进的 \(U_{kj}\)，本质是给破缺的对称性补上"身份信息"。 - §9.3（协调）：同构下各体出力可均摊（对称），异构下要用**加权**内力分配（\(w_k\propto 1/\lambda_k^{\max}\)）和**个体特定**约束（人形侧向力上限）打破均摊——加权矩阵 \(W\) 就是非对称性在力层的编码。 - §9.4（统一框架）：同构下团队动力学是对称的块（所有 \(M_k\) 相同），异构下是**异尺寸**的块对角（各 \(M_k\) 维度/内容不同）——可变块尺寸的组装，就是在堆叠层尊重破缺的对称性。

把这三处连起来看：异构团队的工程，本质是"在失去置换对称性后，用显式的身份信息（能力画像/加权/异尺寸块）重建可计算的结构"。这就是为什么 §9.1 把"对称性破缺"立为全章主线——它不是一句修辞，而是 §9.2-§9.4 每个公式的共同母题。

这个破缺的代价，正是 §9.7 开放问题 2。 置换对称性还带来一个隐性红利——可扩展性：同构系统加一个个体不增加本质复杂度（多一个等价单元而已）。强异构破坏了它，于是"如何为强异构团队设计可扩展（数量、形态都可变）的协调"成了真正的开放难题（§9.7 开放问题 2）。同构 MARL 的置换不变网络在这里失效，需要新的表示（如把形态向量作为条件输入的形态条件化网络、异构图神经网络）——这正是第 10-12 章学习方法要攻的方向。

对比性思维（B）——弱异构 vs 强异构在对称性上的区别。第 6 章的弱异构（空地协同）其实**部分保留**了对称性：虽然无人机和地面车不同，但它们的协作在**信息层**（共享地图/目标），可以用异构图神经网络做"带类型的对称聚合"（同类型内部仍对称）。强异构的协作在**物理交互层**（共抬一个物体、力实时耦合），连"同类型内部对称"都不够用——因为人形和四足共抓物体时，它们对物体的贡献通过**几何（接触点位置）进一步个体化，连同类型的两台四足（\(r_2,r_3\)）都因接触点不同而不可互换。**耦合越深入物理层，对称性破缺越彻底——这就是强异构比弱异构难一个数量级的数学根源（呼应陷阱 1 的"信息层 vs 物理交互层"）。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：把"强异构"误当成"弱异构 + 更多传感器"。 - 错误描述：以为本章只是第 6 章空地协同的"传感器再多样一点"的版本，沿用同样的能力画像 + 信息融合套路即可。 - 现象/后果：你设计的协作只停留在"分头干 + 信息共享"层面，一旦遇到"多个异构体同时抬一个物体"就完全没有工具——因为弱异构框架里根本没有"统一抓取矩阵""内力分配""跨体平衡补偿"这些物理交互层的概念。 - 根本原因：弱异构的协作发生在**信息层**，强异构的协作发生在**物理交互层**。两者差一个数量级的耦合强度——信息层耦合可以异步、可以丢包重传；物理交互层耦合是**实时、连续、不可丢**的（力的传递不会等你重传）。 - 正确做法：识别你的场景属于哪一层。判据：异构体是否需要**同时**对**同一个物理对象**施力？是 → 强异构（用本章 §9.3/§9.4）；否（只是信息互通）→ 弱异构（用第 6 章足矣）。

陷阱 2（思维陷阱）：用单一标量"能力分"概括一个机器人。 - 错误描述：给每个机器人打一个总能力分（"人形 8 分、四足 7 分"），分配时按总分排序。 - 现象/后果：分配器把"高分"机器人派去做它其实不擅长的任务——人形总分高，于是被派去搬重物（它搬运分其实很低），结果搬不稳。 - 根本原因：能力是**多维**的（精细 vs 重载 vs 移动），不同任务对各维度的需求权重完全不同。压成一个标量丢失了"任务-能力匹配"的全部信息——这正是为什么我们要用需求向量 \(\mathbf{d}_j\) 与能力画像 \(\mathbf{c}_k\) 的**加权内积** \(U_{kj}\)，而不是一个总分。 - 正确做法：永远在**任务-个体配对**的粒度上算效用 \(U_{kj}\)，让同一个机器人对不同任务有不同得分。能力是个向量，匹配是个内积，不是排行榜。

陷阱 3（概念误区）：忽略 \(\bot\)（硬不可行）与"低效用"的区别。 - 错误描述：把"做不了"当成"代价很高"，用一个大但有限的数（如 \(10^6\)）表示不可行。 - 现象/后果：当所有可行选项都恰好不可用时（罕见但会发生），优化器会"两害相权取其轻"地选一个 \(10^6\) 的不可行分配——让无人机去搬 50kg，因为"虽然 \(10^6\) 但比其他 \(2\times10^6\) 的强"。系统会派出一个物理上不可能完成的任务。 - 根本原因：有限大代价仍然参与比较和权衡，而 \(-\infty\)（或可行性掩码 \(\Phi_{kj}=0\)）是**绝对排除**，不参与任何权衡。 - 正确做法：硬不可行用布尔掩码 \(\Phi_{kj}\in\{0,1\}\) 或 \(-\infty\) 效用表示，在优化前就把这些配对从可行集里**删掉**，而不是给个大代价让它们继续竞争。

练习¶

（建模题） 为你熟悉的一个三机器人团队（可以是 G1 人形 + 2×Go2，或自选）写出每个个体的形态向量 \(\mathbf{m}_k\) 的具体数值（查阅各机器人的真实规格：自由度、负载、速度）。然后定义一组 5 个任务原语，用能力评估函数 \(g\)（你自己设计的合理映射）算出每个个体的能力画像表。讨论：哪些原语上三者差异最大？这个差异在分配时意味着什么？
（反例构造题） 构造一个具体的任务集和团队，使得"按单一能力总分排序分配"产生明显次优甚至不可行的结果，但"按任务-能力效用 \(U_{kj}\) 分配"能给出正确分配。给出两种分配的具体方案和各自的总效用，量化差距。
（边界思辨题） 形态向量 \(\mathbf{m}_k\) 里的"平衡类型 \(\beta_k\)"是离散的（双足/四足/固定）。但现实中存在**连续谱**——比如一台轮足机器人在轮模式下接近固定基座的稳定性、在足模式下接近四足。讨论：如何把 \(\beta_k\) 从离散标签改造成连续量？这对能力效用 \(U_{kj}\) 的计算有什么影响？（提示：可联系 05 运动控制方向的轮足模式切换 30_复合/100_模式切换.md。）

§9.2 能力互补任务分配——形态感知的 CBBA ⭐⭐⭐¶

这一节解决什么问题：上一节我们有了"谁适合干什么"的定量表达（能力效用 \(U_{kj}\)）。这一节把它变成一个**可执行的分配器**：在异构团队里，把一批任务派给最合适的形态，且要满足"某些任务必须多个互补个体协作"（抬茶几要 2 台四足）、"某些配对硬不可行"（无人机搬重物）。我们从集中式基线讲起，揭示纯匈牙利为什么在此失效，然后把第 3 章的 CBBA 升级为**形态感知出价**的分布式分配器。

动机：把第 3 章的分配器搬到异构团队¶

回到第 3 章（§40），我们学过两类任务分配：集中式的**匈牙利算法**（最优、\(O(n^3)\)、要求中央分配器）和分布式的 CBBA（次优、50% 最优界、靠共识消解冲突）。它们都假设一个隐含前提：任意机器人都能做任意任务，区别只在代价高低。

这个前提在同构甚至弱异构里勉强成立（无人机也能"去某个点"，只是代价/方式不同）。但在强异构具身团队里，它**根本不成立**：

硬不可行配对：固定臂的"快速移动"代价不是"很高"，而是**不存在**（\(U=-\infty\)）。匈牙利的代价矩阵无法优雅地表达 \(-\infty\)——你得用大数 hack，而我们在 §9.1 陷阱 3 看到大数 hack 会导致优化器选出不可行解。
协作任务（coalition）：抬茶几不是"派一个机器人"，而是"派恰好 2 台承重强的个体协作"。第 3 章的指派问题是"一对一"（每任务一个机器人），无法表达"一个任务需要一个由特定形态组成的联盟"。
互补性而非替代性：第 3 章分配追求"每个任务找代价最低的机器人"，本质是**替代**（谁来都行，挑便宜的）。异构分配追求"让团队整体的能力覆盖任务集"，本质是**互补**（人形补四足的灵巧短板，四足补人形的承重短板）。

所以我们不能照搬第 3 章，要做三处升级：(1) 用能力效用 \(U_{kj}\)（含 \(-\infty\)）替代纯代价；(2) 支持联盟任务；(3) 在 CBBA 的出价里编码形态匹配。这一节逐一攻克。

如果直接套用纯匈牙利会怎样¶

让我们具体演算一遍，看纯匈牙利在异构团队上怎么翻车（R6B 反事实）。

设置：团队 = {人形 \(r_1\)，四足 \(r_2\)，四足 \(r_3\)}，任务 = {拿台灯 \(\tau_1\)，抬茶几（需 2 个）\(\tau_2\)，侦察 \(\tau_3\)}。我们用"距离代价"建一个 \(3\times3\) 代价矩阵（匈牙利只认距离/时间这类标量代价），假设此刻人形离茶几最近、四足离台灯近：

\[ C = \begin{pmatrix} & \tau_1(\text{台灯}) & \tau_2(\text{茶几}) & \tau_3(\text{侦察}) \\ r_1(\text{人形}) & 3.0 & \mathbf{1.0} & 2.5 \\ r_2(\text{四足}) & \mathbf{1.2} & 2.0 & 2.2 \\ r_3(\text{四足}) & 1.5 & 2.1 & \mathbf{1.8} \end{pmatrix} \]

匈牙利求最小代价完美匹配，得到：\(r_1\to\tau_2\)（代价 1.0）、\(r_2\to\tau_1\)（1.2）、\(r_3\to\tau_3\)（1.8），总代价 4.0，看起来很优。

但这个分配在物理上是灾难：

\(r_1\to\tau_2\)：把**抬茶几**派给了**人形**——人形承重弱、平衡脆弱，单独抬茶几极可能失稳。而且 \(\tau_2\) 需要 2 个个体，匈牙利只派了 1 个。
\(r_2\to\tau_1\)：把**拿台灯**派给了**四足**——四足没有灵巧手，抓台灯笨拙甚至抓不住。
真正擅长抬茶几的两台四足，一台被派去拿台灯、一台去侦察——能力完全错配。

匈牙利"最优"的是距离总和，但它对**能力**一无所知。它把距离当成唯一标准，于是产出了一个距离最优、能力最差的分配。这就是纯匈牙利在异构团队的根本失效：优化错了目标。

历史：从指派问题到联盟形成与市场机制¶

异构任务分配的理论根基，是在第 3 章 Gerkey–Matarić 分类法之上的进一步细化。回顾一下：Gerkey–Matarić 用三个维度（ST/MT 单/多任务机器人、SR/MR 单/多机器人任务、IA/TA 瞬时/时间扩展分配）给 MRTA 问题分类。第 3 章主要处理最简单的 ST-SR-IA（单机器人单任务瞬时分配，有匈牙利最优解）。

异构协作天然落在更难的类别：抬茶几是 MR 任务（一个任务需要多个机器人），这就跳出了指派问题，进入**联盟形成（coalition formation）** 的范畴——你要决定的不是"任务配机器人"，而是"任务配一个机器人子集"。联盟形成一般是 NP-难的（要在指数多的子集里搜），所以实践中靠启发式或市场机制。

市场/拍卖机制（Bertsekas 拍卖算法、Dias 的市场化 MRTA 综述）天然适合编码异构能力：把任务拍卖出去，每个机器人按自己的能力效用出价，价高者得。 形态差异通过"出价高低"自动体现——擅长的任务出高价、不擅长的出低价、做不了的不出价。这正是 CBBA 的思想，也是我们升级的落点：CBBA 本来就是分布式拍卖，我们只需让出价函数变得"形态感知"。

理论：形态感知的能力分配¶

我们分三步把第 3 章的 CBBA 升级为异构分配器：能力可行性掩码 → 形态感知出价 → 联盟任务处理。

1. 能力可行性掩码 \(\Phi\)¶

第一道关卡是把"硬不可行"挡在优化之外。定义可行性掩码

\[ \boxed{\ \Phi_{kj} = \begin{cases} 1, & U_{kj} > -\infty \ \text{（即 } r_k \text{ 能做 } \tau_j\text{）} \\ 0, & \text{否则} \end{cases}\ } \]

掩码的作用是在分配的**每一步**都强制 \(\Phi_{kj}=0\) 的配对不被选中。在 CBBA 的捆绑构建阶段，个体 \(r_k\) 在挑任务时只在 \(\{j : \Phi_{kj}=1\}\) 里挑——做不了的任务**根本不进入它的候选集**。这与 §9.1 陷阱 3 呼应：掩码是绝对排除，不是大代价。

掩码的来源是形态向量：\(\Phi_{kj} = \prod_{p:\,d_j(p)>0} \mathbb{1}[\mathbf{c}_k(p)\neq\bot]\)——只要任务 \(\tau_j\) 需要的某个原语 \(p\) 是 \(r_k\) 做不了的（\(\bot\)），整个掩码就是 0。一个固定臂面对"快速移动"需求的任务，掩码直接为 0。

2. 形态感知出价 \(b_{kj}\)¶

CBBA 的核心是每个个体对每个任务算一个**出价（bid）**，出价高者在冲突消解时胜出。第 3 章的出价是边际得分（marginal score）——把任务 \(\tau_j\) 加入当前任务包 \(\mathcal{B}_k\) 后，得分的增量。我们把它升级为形态感知出价：

\[ \boxed{\ b_{kj} = \Phi_{kj} \cdot \underbrace{U_{kj}}_{\text{能力效用}} \cdot \underbrace{\exp\!\big(-\gamma\, \Delta t_{kj}(\mathcal{B}_k)\big)}_{\text{时间折扣（边际）}}\ } \]

逐项解释：

\(\Phi_{kj}\)：可行性闸门。做不了 → 出价为 0 → 永远抢不到这个任务（也不该抢）。
\(U_{kj}\)：§9.1 的能力效用。擅长的任务效用高 → 出价高 → 倾向于抢到。这是形态信息进入分配的关键通道——人形对"拿台灯"的 \(U\) 远高于四足，所以人形对台灯出高价。
\(\exp(-\gamma\Delta t_{kj}(\mathcal{B}_k))\)：边际时间折扣，沿用 CBBA 的"边际"思想。\(\Delta t_{kj}\) 是把 \(\tau_j\) 插入当前任务包后的额外耗时（取决于已有任务 \(\mathcal{B}_k\) 的顺序）。任务包越满、插入越费时，折扣越大——这保证了边际收益递减（DMG），从而继承 CBBA 的 50% 最优界（关于 DMG 与 50% 界的证明见第 3 章 §3.2，这里直接复用结论）。

对比性思维（B）——为什么折扣用乘性指数 \(e^{-\gamma\Delta t}\) 而非减性 \(U-\gamma\Delta t\)。一个自然的疑问：时间惩罚为什么不直接从效用里**减去**（\(b=U-\gamma\Delta t\)），而要**乘**一个指数衰减？两者差别关键：减性惩罚下，一个高效用任务即使插得很远（\(\Delta t\) 大），出价仍可能为正且较高，于是个体会"贪心地把擅长但很远的任务也揽进包里"，任务包膨胀、全局分配低效；乘性指数下，\(\Delta t\) 大时 \(e^{-\gamma\Delta t}\to 0\)，再高的效用也被压到接近 0——远任务自动失去竞争力，个体只揽"既擅长又顺路"的任务。更重要的是，乘性指数严格保证**边际收益随任务包增大单调递减**（DMG），而减性惩罚不一定（若效用项不随包变化，减一个常数不改变边际结构）。DMG 是 CBBA 那条 50% 最优界的命根——没有 DMG，第 3 章的收敛性和最优界证明全部失效。所以这里的"乘性"不是随意选的，是为了不破坏继承自第 3 章的理论保证（§9.2 方法论的底线：换出价函数，但不能换掉让 CBBA 成立的数学性质）。

理论-工程桥接（D）：注意这个出价公式是**第 3 章 CBBA 出价与 §9.1 能力效用的乘积**——形式化在这里真的变成了代码里的一个乘法。原版 CBBA 的出价只有时间折扣项；我们乘上 \(\Phi_{kj}\cdot U_{kj}\)，就让分布式拍卖自动具备了"形态感知"。整个 CBBA 的两阶段框架（捆绑构建 + 共识冲突消解）、收敛性、复杂度**完全不变**，我们只改了一个标量函数。这是"复用前置工具"的范本：不重新发明分配算法，只在出价接口注入异构信息。

3. 联盟任务（coalition task）处理¶

最后一关：抬茶几需要 2 个个体协作，这超出了标准 CBBA（每任务一个赢家）。处理方法是**任务拆分（task splitting）——把联盟任务 \(\tau_2\)（需 \(n_{\text{req}}=2\) 个承重个体）拆成 \(n_{\text{req}}\) 个**带角色的子任务：

\[ \tau_2 \longrightarrow \{\tau_2^{(1)}\ (\text{抬左角}),\ \tau_2^{(2)}\ (\text{抬右角})\} \]

每个子任务带一个**角色（role）**（左角/右角，决定接触点位置）。然后这两个子任务进入正常的 CBBA 拍卖，由出价机制自然地把它们派给出价最高的 2 个个体（必然是两台四足，因为它们对"重载搬运"出价高）。

但有个新约束：两个子任务不能派给同一个个体（一个机器人不能同时抬两个角），且最好派给"能协调"的个体。前者通过 CBBA 的冲突消解保证（一个个体的任务包里两个子任务会冲突）；后者通过在出价里加一个**协调亲和项**——若 \(r_k\) 与已认领另一子任务的个体 \(r_{k'}\) 形态相近、通信良好，则出价加成。这鼓励"两台四足一起抬"而非"一台四足 + 一台人形勉强凑"。

4. 完整分配流程¶

把三步串起来，异构 CBBA 的完整流程如下：

输入：团队 R={r_1..r_K}，任务集 T（含联盟任务），能力画像 {c_k}，通信图 G

步骤 0（预处理）：
  for 每个联盟任务 τ_j（需 n_req 个）:
      拆成 n_req 个带角色的子任务，加入 T
  for 每个 (r_k, τ_j):
      计算能力效用 U_kj 和可行性掩码 Φ_kj

阶段 1（本地捆绑构建，每个 r_k 独立）：
  B_k ← 空任务包
  while 还能加任务:
      对每个候选任务 τ_j（仅 Φ_kj=1 的）:
          算形态感知出价 b_kj（含当前 B_k 的边际折扣 + 协调亲和）
      把出价最高的任务加入 B_k（记录出价和插入位置）

阶段 2（基于共识的冲突消解，邻居间迭代）：
  repeat（每轮与邻居交换"每个任务的最高出价 + 时间戳"）:
      对每个任务 τ_j:
          全网取最高出价者为暂定赢家（带时间戳最大值共识）
          若 r_k 在某任务上被人超过 → 从 B_k 踢出该任务及其后续，重新竞拍
  until 全网对所有任务的归属达成一致（收敛轮数 ∝ 通信图直径）

输出：每个任务 τ_j 的执行个体（联盟任务 → 一组个体 + 各自角色）

对我们的客厅场景，这个流程的输出应该是：

任务	赢家	出价依据
拿台灯 \(\tau_1\)	人形 \(r_1\)	\(U_{1,\text{灯}}=0.95\) 远高于四足的 0.40，出价碾压
抬茶几-左 \(\tau_2^{(1)}\)	四足 \(r_2\)	\(U_{2,\text{抬}}=0.90\)，且与 \(r_3\) 协调亲和高
抬茶几-右 \(\tau_2^{(2)}\)	四足 \(r_3\)	同上，两台四足互相加成
侦察 \(\tau_3\)	人形 \(r_1\)（拿完灯后）	边际折扣下仍可承接（视时序）

这正是开头直觉的算法化——且它能在 8 个机器人、20 个任务的规模上自动跑，无需为每个场景硬编码。

5. 手把手走一遍出价与冲突消解¶

为了让 CBBA 的两阶段真正"看得见"，我们用客厅场景手算一轮（简化：忽略时间折扣，出价 \(b_{kj}=\Phi_{kj}\cdot U_{kj}\)，任务都是单体任务先不拆联盟，便于演示冲突消解）。任务 = {拿台灯 \(\tau_1\)，扫一遍客厅 \(\tau_3\)}（抬桌联盟单独处理），团队 = {人形 \(r_1\)，四足 \(r_2\)，四足 \(r_3\)}。能力效用矩阵（取自 §9.1 能力画像表，简化加权）：

\[ U = \begin{pmatrix} & \tau_1(\text{台灯}) & \tau_3(\text{侦察}) \\ r_1(\text{人形}) & 0.95 & 0.75 \\ r_2(\text{四足}) & 0.40 & 0.65 \\ r_3(\text{四足}) & 0.40 & 0.65 \end{pmatrix} \]

阶段 1（本地捆绑构建，各自独立、并行）：

\(r_1\)（人形）：候选任务都可行（\(\Phi=1\)）。它对 \(\tau_1\) 出价 \(0.95\)、对 \(\tau_3\) 出价 \(0.75\)。贪婪地先把出价最高的 \(\tau_1\) 加入任务包 \(\mathcal{B}_1=\{\tau_1\}\)；若还能加，再考虑 \(\tau_3\)（边际折扣后），暂记 \(\mathcal{B}_1=\{\tau_1,\tau_3\}\)。
\(r_2\)（四足）：对 \(\tau_1\) 出价 \(0.40\)、\(\tau_3\) 出价 \(0.65\)。先加 \(\tau_3\)，\(\mathcal{B}_2=\{\tau_3\}\)。
\(r_3\)（四足）：同 \(r_2\)，\(\mathcal{B}_3=\{\tau_3\}\)。

此时出现**冲突**：\(\tau_1\) 被 \(r_1\) 认领；\(\tau_3\) 被 \(r_1,r_2,r_3\) 三方同时认领（各自本地贪婪，互相不知道）。这正是分布式拍卖必然产生的冲突，需要阶段 2 消解。

阶段 2（基于共识的冲突消解，邻居间迭代）：

每个机器人维护一张表："每个任务当前已知的最高出价 + 出价者"。通过与邻居交换这张表（带时间戳最大值共识），全网逐步对每个任务的归属达成一致。对 \(\tau_3\)：

任务 \(\tau_3\) 的最高出价竞争	\(r_1\) 出价	\(r_2\) 出价	\(r_3\) 出价	暂定赢家
各自的出价	0.75	0.65	0.65	\(r_1\)（0.75 最高）

共识收敛后，\(r_1\) 以 \(0.75\) 赢得 \(\tau_3\)，\(r_2,r_3\) 在 \(\tau_3\) 上**败北**——它们把 \(\tau_3\) 踢出任务包。但这里有个微妙之处：\(r_2,r_3\) 现在任务包空了，会**重新竞拍**还没归属的任务。而 \(\tau_1\) 已被 \(r_1\) 以 \(0.95\) 牢牢占据（\(r_2,r_3\) 对 \(\tau_1\) 只能出 \(0.40\)，抢不过）。于是最终：

\(\tau_1\)（台灯）→ \(r_1\)（人形，出价 0.95）
\(\tau_3\)（侦察）→ \(r_1\)（人形，出价 0.75）
\(r_2,r_3\)（四足）→ 空闲，转去竞拍抬桌联盟任务（它们对"重载"出价高，正好接住）

本质洞察——形态匹配让冲突消解"自动正确"。注意一个关键现象：冲突消解阶段，\(r_2,r_3\) 在灵巧任务（台灯）上**根本抢不过人形**（0.40 vs 0.95），在侦察上也输给人形（0.65 vs 0.75）。它们"被迫"去做自己擅长的重载任务。这不是偶然——形态感知出价让每个个体在自己擅长的任务上出价**碾压性地高**，于是冲突消解的"价高者得"自动把任务分流到最合适的形态。能力效用 \(U_{kj}\) 在这里扮演了"生态位隔离"的角色：人形和四足在出价上天然分化，避免了它们争抢彼此的强项。这就是 §9.2 对比性思维"互补而非替代"的微观机制——你看到的不是"抢同一个任务"，而是"各自被自己的高出价推向不同的生态位"。

对比性思维（B）——和纯 CBBA 的冲突消解对比。原版 CBBA（第 3 章）的出价只看时间/距离，冲突消解时**离任务近的赢**。在异构团队里这会出问题：恰好离台灯近的四足会赢得台灯（尽管它抓不好）。形态感知 CBBA 改变了这个动力学——离得近但不擅长**的个体出价低（\(U\) 小），**擅长但稍远**的个体出价高，于是冲突消解的结果从"近者得"变成"擅长者得"。同一个冲突消解机制（带时间戳最大值共识），喂进不同的出价函数，产出完全不同（且正确得多）的分配。这再次印证 §9.2 的方法论：**不改协调框架，只改出价接口。

对比性思维（B）——"不是替代而是互补"。第 3 章的分配本质是**替代竞争**：多个机器人争抢同一个任务，挑代价最低的，潜台词是"谁来都行"。本章的异构分配本质是**互补协作**：通过能力效用，让分配自然分化——人形去抢灵巧任务、四足去抢重载任务，它们**不争抢彼此的强项任务**，而是各自占据互补的生态位。出价公式里的 \(U_{kj}\) 就是这个"生态位分化"的引擎：它让每个个体在自己擅长的任务上出价远高于其他个体，从而自动避免错配。

6. 联盟任务的原子性（all-or-nothing）怎么实现¶

上面把联盟任务拆成带角色子任务后丢进普通 CBBA，但留了一个没讲透的难点——原子性。抬茶几需要 2 个个体**同时**到位，若只凑齐 1 个（另一个子任务流拍或赢家中途退出），那唯一认领的个体会孤零零地去抬一个需要两人的茶几，既危险又徒劳。标准 CBBA 没有这个"要么都成、要么都不做"的语义——它把每个子任务当独立的，不管联盟是否凑齐。这一小节补上原子性的实现，它是 §9.2 陷阱 1 的正面解法，也是后面跨章综合题里"联盟任务"环节的关键。

原子性的形式化。 设联盟任务 \(\tau_j\) 拆成子任务集 \(\{\tau_j^{(1)},\dots,\tau_j^{(n_{\text{req}})}\}\)。原子性要求：要么所有 \(n_{\text{req}}\) 个子任务都有可行赢家且承诺执行，要么一个都不执行（已认领者全部释放）。形式化为一个**联盟约束**：

\[ \boxed{\ \Big(\textstyle\sum_{m=1}^{n_{\text{req}}}\mathbb{1}[\tau_j^{(m)}\ \text{已被认领}]\Big) \in \{0,\ n_{\text{req}}\}\ } \]

即"已认领子任务数"只能是 0 或 \(n_{\text{req}}\)，不允许中间值（部分凑齐）。

实现一：两阶段提交（two-phase commit）。 借鉴分布式数据库的经典协议：

阶段 A（预认领 / prepare）：
  各子任务照常 CBBA 拍卖，赢家进入"暂定认领"状态（尚未承诺）
阶段 B（提交 / commit）：
  协调者（或发起子任务的个体）检查该联盟的所有子任务是否都有暂定赢家：
    若全部凑齐 → 广播 commit，所有暂定赢家转为正式承诺，开始执行
    若有缺额（某子任务无可行赢家 / 流拍）→ 广播 abort，
       所有暂定赢家释放该联盟的子任务，重新去竞拍别的任务

优点：语义清晰、与标准两阶段提交同构，正确性有保证。
代价：需要一个协调点（或发起者）做 commit/abort 决策，且引入一轮额外通信。在去中心化场景，可由"联盟内出价最高者"临时充当协调者。

实现二：联盟出价捆绑（bundle bid）。 更分布式的做法：不拆成独立子任务竞拍，而是让"能组成联盟的个体组合"对**整个联盟任务打一个捆绑出价**。例如四足 \(r_2,r_3\) 作为一个组合，对"抬茶几"出一个联合出价 \(b_{\{2,3\},j}=\min(U_{2j},U_{3j})\cdot(\text{协调亲和})\)（用 \(\min\) 体现"短板决定联盟强度"）。冲突消解时，整个组合作为一个原子单位竞争——赢则两者一起承诺，败则一起放弃，天然原子。

优点：原子性内建，无需 commit/abort 协议。
代价：组合数随团队规模组合爆炸（\(\binom{K}{n_{\text{req}}}\) 个可能联盟），要用启发式剪枝（只考虑形态相近、通信良好的组合）。这正是 §9.2 协调亲和项的用武之地——它预先筛掉不合理的联盟组合。

本质洞察——原子性是"任务的不可分性"在分配层的投影。为什么联盟任务需要原子性，而普通任务不需要？因为联盟任务背后是一个**物理上不可分的协作**——抬茶几这件事，1 个个体做"一半"不是"完成了 50%"，而是**彻底失败**（茶几抬不起来甚至砸下来）。任务的价值函数在"凑齐人数"这个点上是**阶跃的**（\(n_{\text{req}}-1\) 个个体收益为 0，第 \(n_{\text{req}}\) 个个体到位收益跳到全部）。原子性约束，正是把这个"阶跃价值"翻译到分配层——它阻止分配器在"收益还是 0"的部分联盟上浪费个体。凡是价值函数对参与人数有阶跃（而非线性递增）的任务，分配时都需要原子性。这个判据能帮你识别：搬重物（需够人数才抬得动）要原子性，分头侦察（每个个体独立产出）不需要。

对比性思维（B）——原子性 vs 贪婪逐个认领。不加原子性的贪婪做法（陷阱 1）是"先认领再说，凑不齐就将就"，结果是部分联盟的资源浪费 + 危险执行。原子性做法是"确认能凑齐才承诺"，宁可让个体暂时空闲去做别的，也不投入一个注定失败的部分联盟。两者的差别在**承诺的时机**：贪婪在拍卖时就承诺（不可回滚），原子在确认联盟完整后才承诺（预认领可回滚）。这也是为什么实现一要分"预认领/提交"两阶段——把"认领"和"承诺"解耦，给原子性检查留出回滚窗口。

7. 动态能力与重分配：当个体的能力随状态改变¶

到此为止，能力画像 \(\mathbf{c}_k\) 都被当成**静态**的（出厂就定、不变）。但真实任务里能力是**随状态变化**的——这是动态重分配（故障排查 #7、§9.6 练习 3 的核心）的根源，值得在正文讲清，而非只留作练习。

为什么能力是状态相关的 \(\mathbf{c}_k(s_k)\)。 三类典型的状态依赖：

资源耗尽：四足电量从 100% 掉到 15%，它的"重载搬运"和"快速移动"胜任度显著下降（电量不足以驱动大扭矩/高速）——\(\mathbf{c}_k(\text{重载})\) 是电量的减函数。
能力被占用：人形已经一只手抓着台灯，它的"精细抓取"能力只剩一只手——再来一个需双手的精细任务，胜任度下降甚至变 \(\bot\)。
构型/位置相关：个体当前位姿离任务区远、或处在不利构型（接近关节限位、奇异位形），其有效操作能力临时降低。

于是出价函数从静态 \(b_{kj}=\Phi_{kj}U_{kj}e^{-\gamma\Delta t}\) 变成**状态相关**的 \(b_{kj}(s_k)=\Phi_{kj}(s_k)\,U_{kj}(s_k)\,e^{-\gamma\Delta t}\)——掩码和效用都随状态实时更新。这把分配从"一次性"变成"持续重评估"。

动态能力如何驱动重分配。 一旦能力随状态变，分配就**不能只算一次**——必须周期性或事件触发地重拍卖（呼应 §9.2 陷阱 2、第 3 章 §3.5 终身 MAPF）。关键场景：

四足 r_2 抬桌过程中电量耗尽（c_2(重载) 骤降 → 低于阈值 → Φ_2,抬桌 = 0）：
  ① r_2 的可行性掩码对"抬桌"翻为 0 → 它对该任务出价归零 → 主动释放任务
  ② 触发联盟任务重拍卖（抬桌联盟缺额，原子性检查触发，见子节 6）
  ③ 剩余可行个体重新竞拍：若有空闲四足 r_4，它接过 r_2 的角色；
     若无 → 联盟无法凑齐 → 系统降级（如先把桌子稳稳放下，而非硬撑）

本质洞察——动态能力把"分配"和"故障恢复"统一成同一个机制。静态分配把"分配"（开局派活）和"故障恢复"（有人挂了怎么办）当成两回事。引入状态相关能力 \(\mathbf{c}_k(s_k)\) 后，它们**统一了**：故障（电量耗尽、个体失效）不过是"某个体在某任务上的胜任度 \(\mathbf{c}_k\) 跌到 \(\bot\)"这一普遍现象的特例。于是**同一个持续运行的形态感知 CBBA，既做初始分配，又做故障恢复**——失效个体的能力自动掉到 \(\bot\)、出价归零、任务被重拍卖给可行者，无需单独写一套故障处理逻辑。这就是为什么 §9.6 流水线把分配做成"持续服务"而非"一次性调用"：把能力做成状态的函数，鲁棒性就内建在分配机制里了。这也是故障排查 #7（"个体失效后整队瘫痪"）的根治——瘫痪的根源往往是用了静态分配，而非动态重评估。

对比性思维（B）——动态能力的代价：收敛性 vs 响应性。静态能力下 CBBA 收敛干净（出价不变，冲突消解一次到位）。动态能力引入一个新麻烦：出价随状态实时变，冲突消解可能**震荡**——\(r_2\) 刚抢到任务，移动后构型变差出价降，又被 \(r_3\) 抢走，\(r_3\) 抢到后同样问题……来回拉锯，谁也定不下来。这是响应性（快速适应状态变化）和稳定性（分配别老变）的内在张力。缓解手段：(1) 给出价加**迟滞/阻尼**（已认领者有一个"保留加成"，避免微小状态变化就被抢）；(2) 时间戳锁定（一旦认领，锁定一个最小执行窗口才允许被抢）；(3) 重拍卖**事件触发**而非高频周期触发（只在能力发生显著变化——如跨过 \(\bot\) 阈值——时才重拍）。这三条都是"别让分配过于敏感"，和范式 A 阻抗"别调太硬"、ADMM"别期望单轮收敛"是同一类工程智慧——协调系统的稳定性，常常要靠主动引入一点迟钝来换。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：把联盟任务当成多个独立的单体任务。 - 错误描述：抬茶几需要 2 个个体，于是简单地建 2 个独立任务"抬茶几-A""抬茶几-B"，分别拍卖，不加任何协调约束。 - 现象/后果：可能派给两个**不能协调**的个体（一台四足 + 一台人形），或两个赢家在执行时对茶几目标轨迹的理解不一致，导致抬的时候拉扯。更糟的是若一个子任务流拍（没人接），另一个被接了，结果一个机器人孤零零地去抬一个需要两人的茶几。 - 根本原因：联盟任务的子任务之间有**强耦合**（同一物体、需协调、要么都成要么都不做），独立拍卖丢失了这层耦合。 - 正确做法：子任务保留角色标签 + 协调亲和项 + 原子性检查（all-or-nothing：若联盟未凑齐，已认领者释放任务）。把"2 个一起"作为约束显式编码，而非寄望于独立拍卖恰好凑对。

陷阱 2（思维陷阱）：用静态分配应对动态团队。 - 错误描述：开局算一次 CBBA 分配，之后整个任务执行期间不再重新分配。 - 现象/后果：执行中某台四足电量耗尽退出、或新任务（"门铃响了去开门"）到达，静态分配无法响应——退出的四足留下的茶几一角没人接，新任务无人认领。 - 根本原因：异构团队的真实场景是**动态**的（个体可能失效、任务可能到达），这对应 Gerkey–Matarić 分类的 TA（time-extended） 维度，而非静态 IA。 - 正确做法：CBBA 本就支持**周期性重拍卖**（每隔一段时间或事件触发时重跑两阶段）。设计时把分配做成一个持续运行的服务，而非一次性调用；个体失效时其任务包自动释放、触发邻居重竞拍。这与第 3 章 §3.5 的终身 MAPF（lifelong）思想一致。

陷阱 3（编程陷阱）：能力效用 \(U_{kj}\) 与代价 \(c_{kj}\) 符号方向混用。 - 错误描述：代码里 CBBA 出价用"效用越高越好"（max），但计算 \(U_{kj}\) 时不小心混入了"距离代价"（越低越好）而忘了取负，导致出价方向反了。 - 现象/后果：分配器把任务派给**最不适合 + 最远**的个体——因为它把高代价误当成高效用。Bug 隐蔽：流程跑通、不报错，只是分配结果离谱。 - 根本原因：效用（utility，越大越好）和代价（cost，越小越好）是相反方向的标量，混用是异构分配代码最常见的符号 bug。 - 正确做法：在出价函数里**统一到效用方向**——所有"越小越好"的量（距离、耗时、能耗）进入出价前先取负或用 \(\exp(-\cdot)\) 包裹（如出价公式里的时间折扣项）；所有"越大越好"的量（能力胜任度）直接相乘。写单测：构造"近但不擅长"vs"远但擅长"两个候选，断言擅长者出价更高。

练习¶

（实现题） 给出第 3 章 CBBA 的可运行框架（捆绑构建 + 冲突消解两阶段），你的任务是把出价函数 compute_bid(robot, task, bundle) 改造为形态感知版本：加入能力效用 \(U_{kj}\)、可行性掩码 \(\Phi_{kj}\)、协调亲和项。用本章客厅场景（3 机器人 + 3 任务，其中抬茶几为联盟任务）测试，验证输出与正文表格一致。再加一台固定臂 \(r_4\)（"快速移动"\(=\bot\)），验证它不会被派去任何需要移动的任务。

（调试挑战） 下面这段异构 CBBA 出价代码有一个 bug，会导致重载任务被派给人形而非四足。定位并修复：

def compute_bid(robot, task, bundle):
    util = capability_utility(robot, task)   # 能力效用，越大越好
    cost = travel_cost(robot, task, bundle)  # 行驶代价，越大越差
    if not feasible(robot, task):
        return 0.0
    return util + cost          # ← 这里有问题

（提示：见陷阱 3。还要思考 util 和 cost 的量纲/尺度是否可比。）

（设计扩展题） 现实中能力画像 \(\mathbf{c}_k\) 可能随状态变化——比如四足电量低时"重载搬运"胜任度下降、人形已抓着台灯时"精细抓取"能力被占用。把静态能力画像扩展为**状态相关的动态能力** \(\mathbf{c}_k(s_k)\)，重新设计出价函数，并讨论这会如何影响 CBBA 的收敛性（提示：动态出价可能导致冲突消解震荡，需要加阻尼或时间戳锁定）。

§9.3 跨形态协调控制三范式 ⭐⭐⭐⭐¶

这一节解决什么问题：分配解决了"谁干什么"，但当人形和两台四足**同时**抓着同一张茶几时，真正的难题才开始——它们的力必须**协调**，否则茶几被内力撕裂、或某个个体被操作反力拽倒。这一节给出三种协调范式：①领导者-跟随者 + 阻抗柔顺；②统一抓取矩阵 + 内力分配；③分布式 MPC（ADMM）一致性协调。这是全章最硬的一节——它把第 5 章的抓取矩阵、第 4 章的 ADMM、05 方向的 whole-body 全部熔在一起。

动机：三个不一样的身体，抬同一张桌子¶

设想分配已完成：人形 \(r_1\) 抓茶几一侧，四足 \(r_2,r_3\) 各用背载平台（或背上的臂）顶住另两个角。现在它们要**一起把茶几抬起来、平移到 B 处、再放下**。

这件事的物理本质是：三个异构末端通过茶几形成了一个闭运动链（closed kinematic chain）。茶几是刚体，它把三个接触点的运动**锁死**——一旦茶几位姿确定，三个接触点的位置就被刚性约束绑定。同时，三个接触点施加的力**叠加**成茶几所受的合力旋量，决定茶几怎么动。

协调的核心矛盾有两个：

矛盾一：运动学一致性 vs 各自的控制器。 三个个体各有自己的 whole-body 控制器（人形的、四足的），如果它们各自独立地"跟踪茶几目标轨迹"，由于状态估计误差、模型误差、通信延迟，三者对"茶几现在在哪、该往哪走"的判断会有微小偏差。这些偏差在刚性闭链里**无处释放**——它们转化为内力（把茶几越夹越紧或互相拉扯），轻则物体变形/打滑，重则结构损坏。

矛盾二：操作力 vs 平衡。 人形为了抬茶几要施加向上的力，茶几通过接触给人形一个**反作用力**。这个反力如果有侧向分量，会把人形的质心推出平衡区——人形的双足动态平衡（\(\beta_1=\text{双足}\)）承受侧向扰动的能力远低于四足（\(\beta_{2,3}=\text{四足}\)）。如果不把这个操作反力**前馈补偿**进人形的平衡控制，人形会被自己抬的茶几拽倒。

这两个矛盾，就是跨形态协调要解决的全部。三种范式是三种不同的"解法哲学"，各有适用场景。

如果三个个体各自独立跟踪会怎样¶

最朴素的做法（也是新手最常犯的）：给每个个体一个"茶几目标轨迹 \(x_o^d(t)\)"，让它们各自的 whole-body 控制器独立地把自己的末端"贴着茶几"并跟踪 \(x_o^d(t)\)。我们来推演这个"无协调"方案的崩溃过程（R6B 反事实，配合具体物理）：

初始小偏差：\(t=0\) 时三者末端都精确贴合茶几。但人形的状态估计认为茶几在 \(x_o + \delta_1\)，四足-A 认为在 \(x_o + \delta_2\)（\(\delta\) 是各自的估计误差，毫米级）。
偏差转为内力：三者各自朝自己认为的目标发力。由于茶几刚性，这些不一致的"期望位移"无法都满足，转化为接触力的不一致——出现一个净内力 \(f_{\text{int}}\neq 0\)。茶几不会移动（内力对物体合力无贡献），但接触点承受越来越大的挤压/剪切。
内力发散：每个控制器看到"末端没到达目标"，于是**加大**发力试图纠正——但更大的力只会产生更大的内力（因为偏差仍在）。这是一个**正反馈**：内力越大，物体/接触越紧张，最终打滑、变形或脱手。
平衡反噬：与此同时，人形施加的力中的侧向分量没有被它的平衡控制器预期到（它以为自己只在跟踪轨迹），质心被推出支撑区，人形开始倾倒。倾倒又改变了它的末端位置，进一步加剧偏差——第二个正反馈。

结论：无协调的独立跟踪在刚性闭链上是不稳定的。物体刚性把误差"困"在系统里转化为内力，而异构的平衡能力差异让操作反力成为压垮脆弱个体的最后一根稻草。三种范式，本质上都是在**打破这两个正反馈**——给内力一个释放/协商的渠道，给操作反力一个补偿的通道。

本质洞察：协调的根本任务不是"让大家动作一致"，而是**管理刚性约束下不可避免的误差**。只要有多个个体接触同一刚体，状态估计/模型/通信误差就必然产生内力不匹配——你无法消除误差，只能给误差一个**出口**。三种范式是三种不同的"误差出口设计"：领导跟随让跟随者**柔顺地让步**（阻抗），内力分配**显式协商**内力（零空间），分布式 MPC 让大家**对物体轨迹达成共识**（一致性约束）。理解了这一点，三种范式就不再是孤立的招式，而是同一个问题的三个解。

历史：从遥操作双臂到 legged 团队搬运¶

跨形态协调控制的工具，大多源自**双臂协调操作（dual-arm coordinated manipulation）** 的成熟理论，再迁移到异构 legged 团队。

1980s-1990s：固定双臂协调。机器人学界研究两条固定机械臂抓同一物体，奠定了抓取矩阵（grasp matrix）、内力（internal force）、对称/非对称协调、阻抗协调等概念。这是本章范式 A、B 的理论祖先（本项目 05 方向 20_机械臂/D01-D04 双臂子课程详细讲了这套）。
2000s：领导者-跟随者范式。在双臂/多机协调中，"一个主导、其余柔顺跟随"成为经典架构——主臂规划物体轨迹，从臂用阻抗/导纳控制柔顺地跟随，避免内力发散。简单、鲁棒、通信需求低。
2010s：移动操作与浮动基座。臂被装上移动平台（轮式、四足），协调问题从"固定基座"变为"浮动基座"——多了平衡/稳定的耦合。ETH 的 ALMA（ANYmal + 臂）、Go Fetch 把这套推向四足。
2023-2025：legged 团队的协同 loco-manipulation。ETH 的 Shared Object Manipulation（多足+臂团队，把双臂的混合运动-力控扩展到一队 legged manipulator，力闭合抓取）、De Vincenti 的 robot-agnostic SQP（第 8 章 §90 详讲）、decPLM 的零通信 MARL 搬运（第 8 章）——这些是范式 B、C 在异构 legged 团队上的最新形态。

这条线的演进逻辑是：理论在固定双臂上成熟 → 加上浮动基座（平衡耦合）→ 扩展到异构多体（形态差异）。本章站在这条线的终点，把成熟的协调理论"翻译"到强异构具身团队。

理论：三种协调范式¶

范式 A：领导者-跟随者 + 阻抗协调¶

核心思想：指定一个个体为**领导者（leader），由它主导物体的期望轨迹；其余为**跟随者（follower），用阻抗/导纳控制**柔顺地**跟随，把误差以"让步"的形式释放掉，而非硬抗。

谁当领导者？在异构团队里，让能力最强、最稳的个体当领导者——通常是承重稳、平衡好的四足，或者（若操作精度主导）定位最准的个体。领导者跑一个普通的轨迹跟踪（位置控制），它"以为"自己单独在搬物体。

跟随者的关键是**阻抗控制（impedance control）**。回顾 05 方向 20_机械臂/F04_笛卡尔阻抗控制.md：阻抗控制让末端表现得像一个虚拟的弹簧-阻尼系统

\[ \boxed{\ F_{\text{follower}} = K_p (x_o^{\text{ref}} - x_{\text{ee}}) + K_d(\dot{x}_o^{\text{ref}} - \dot{x}_{\text{ee}}) \ } \]

跟随者不**硬性**要求自己的末端 \(x_{\text{ee}}\) 精确到达某个位置，而是施加一个**正比于偏差的柔性力**。当领导者拉着物体走、跟随者的末端被物体带动偏离参考时，阻抗律产生一个温和的恢复力——它**跟随**物体而不**对抗**物体。

多视角理解（A）——阻抗协调像"两个人抬桌子时的默契"。想象你和朋友抬一张桌子，你是领导者、决定往哪走。你的朋友（跟随者）不会僵硬地坚持"我的手必须在某个绝对位置"——他会**感受**桌子的拉力，顺着你走的方向柔顺地移动手。如果他僵硬地锁死手的位置（位置控制），你俩很快就会因为步调不一致而把桌子拧变形或互相较劲。阻抗控制就是给跟随者这种"顺着劲走"的默契。 像的部分：柔顺让步、靠感受力来协调。不像的部分：人类靠本体感觉和预测，机器人靠显式的力/位偏差反馈，且机器人的"柔顺度"\(K_p, K_d\) 是可精确设定的参数——太硬退化为位置控制（内力发散），太软则跟不上领导者（脱节）。这个边界很关键。

异构适配：跟随者的阻抗增益 \(K_p, K_d\) 要**按形态调**。脆弱平衡的人形当跟随者时，阻抗要**软**（小 \(K_p\)）——它经不起大的接触力，宁可多让步；承重稳的四足当跟随者时阻抗可以**硬**一些。这就是形态差异在协调律里的体现。

优缺点： - 优点：通信需求极低（跟随者只需知道物体参考轨迹，甚至只靠力反馈就能跟随，无需实时通信）；鲁棒（柔顺天然吸收误差）；实现简单。这正是 decPLM"零通信"协调的物理基础——接触力本身就是协调信号。 - 缺点：领导者-跟随者是**非对称**的，领导者承担全部规划负担、且领导者失效则全队瘫痪；内力不被**显式控制**（只是被阻抗"软化"），无法精确调节夹紧力；对快速、精密的协同操作不够精确。

阻抗增益怎么按形态定量整定。 上面说"人形软、四足硬"，但具体软到什么数值？陷阱 1 说"太硬退化为位置控制、太软跟不上"，这给出了整定的两个边界——我们把它定量化，让"按形态调阻抗"从口号变成可计算的配方。

上界（不能太硬）：内力安全。 阻抗产生的接触力是 \(F_{\text{follower}}=K_p\,e+K_d\,\dot e\)（\(e\) 是位置偏差）。在预期的最大状态估计/跟踪偏差 \(e_{\max}\) 下，这个力不能超过两条线中更紧的那条——物体结构强度 \(F_{\text{struct}}\) 和该跟随者的平衡承受上限 \(\bar F_k^{\text{balance}}\)：

\[ \boxed{\ K_p^{(k)} \le \frac{\min\!\big(F_{\text{struct}},\ \bar F_k^{\text{balance}}\big)}{e_{\max}}\ } \]

对人形（\(\bar F_1^{\text{balance}}\) 小，如 12N）和四足（\(\bar F^{\text{balance}}\) 大，如 200N），同样的 \(e_{\max}\) 下，人形的 \(K_p\) 上界比四足小一个数量级——这就是"人形必须更软"的定量根源：不是经验法则，而是平衡上限除以偏差得出的硬上限。

下界（不能太软）：跟得上领导者。 跟随者要跟上领导者以速度 \(v_{\text{lead}}\) 拖动物体。稳态下，阻抗力要能克服跟随者自身末端运动所需的力（含惯量/摩擦阻力 \(F_{\text{drag}}\)）。若 \(K_p\) 太小，稳态跟随偏差 \(e_{\text{ss}}=F_{\text{drag}}/K_p\) 会大到跟随者严重滞后、甚至脱离物体。要求 \(e_{\text{ss}}\le e_{\text{allow}}\)（容许滞后），得：

\[ K_p^{(k)} \ge \frac{F_{\text{drag}}}{e_{\text{allow}}} \]

阻尼 \(K_d\) 按临界阻尼定。 给定 \(K_p\) 和跟随者末端等效惯量 \(m_{\text{eff}}\)，取临界阻尼附近 \(K_d \approx 2\zeta\sqrt{K_p\,m_{\text{eff}}}\)（\(\zeta\approx 0.7\text{-}1.0\)），避免欠阻尼振荡（抬物体时末端来回弹）或过阻尼迟钝。

理论-工程桥接（D）——整定区间就是"上界 ÷ 下界"留的窗口。把两条边界合起来：\(\dfrac{F_{\text{drag}}}{e_{\text{allow}}}\le K_p^{(k)}\le\dfrac{\min(F_{\text{struct}},\bar F_k^{\text{balance}})}{e_{\max}}\)。这个区间是否非空，本身就是一个**可行性诊断**：若下界超过上界（区间为空），说明这个个体**当不了这个任务的柔顺跟随者**——它要么太弱（平衡上限撑不起克服阻力所需的力），要么物体太脆（结构强度撑不起跟上领导所需的力）。此时别硬调阻抗，应换范式（如范式 B 显式分配，把重负担转给四足）或换角色（让这个个体别当承力跟随者）。整定区间为空，是 §9.2 能力分配该把它排除在该角色之外的信号——这里阻抗整定和上游分配在数值上接上了头。

对比性思维（B）——按形态调阻抗 vs 统一调一个增益。一个常见偷懒做法是给所有跟随者用同一套 \(K_p,K_d\)（"调一次省事"）。在异构团队里这必然出错：对人形够软的增益，对四足可能软到跟不上（脱节）；对四足合适的增益，对人形可能硬到拽倒它（超过 \(\bar F_1^{\text{balance}}\)）。统一增益的隐含假设是"所有跟随者平衡能力/力上限相同"——这恰恰是异构团队被打破的对称性（§9.1 本质洞察）。异构的每一处"统一参数"，都是同构假设的残留，都要按形态拆开——这条原则贯穿全章（出价函数 §9.2、内力权重 §9.3-B、阻抗增益这里）。

范式 B：统一抓取矩阵 + 内力分配¶

核心思想：把三个异构末端当成**同一个刚体（茶几）的三个接触点**，建立统一抓取矩阵 \(G\)，然后**显式地**把期望物体运动分解为各接触力，并在 \(G\) 的零空间里**主动分配内力**。这是双臂协调操作理论的直接推广，最精确，但需要全局协调。

统一抓取矩阵。设茶几质心位姿为 \(x_o\)，三个接触点相对质心的位置为 \(r_i\)（\(i=1,2,3\)，分别是人形手、四足-A 背、四足-B 背的接触点）。每个接触点施加一个力旋量 \(f_i = (f_i^{\text{lin}}, \tau_i)\in\mathbb{R}^6\)（线性力 + 力矩；若是点接触只有 \(\mathbb{R}^3\)）。物体所受合力旋量 \(w_o\) 是各接触贡献之和：

\[ \boxed{\ w_o = \sum_{i=1}^{3} \begin{bmatrix} I_3 & 0 \\ [r_i]_\times & I_3 \end{bmatrix} f_i \ \triangleq\ G\, f,\quad f = \begin{bmatrix} f_1 \\ f_2 \\ f_3 \end{bmatrix}\in\mathbb{R}^{18} \ } \]

其中 \([r_i]_\times\) 是 \(r_i\) 的反对称（叉积）矩阵——它把接触点的线性力转化为对质心的力矩贡献（杠杆效应）。\(G\in\mathbb{R}^{6\times 18}\) 是统一抓取矩阵（这里堆叠了三个 \(6\times 6\) 的接触映射块）。这与第 5 章 §60 的抓取矩阵**完全同构**——区别仅在于三个接触点来自**异构**的个体，但数学对象一模一样。这正是 §9.1 强调的"统一接口"的兑现：异构的手/背，在 \(G\) 里被抹平为统一的接触点。

正问题与逆问题。给定期望的物体合力旋量 \(w_o^d\)（由物体期望加速度通过牛顿-欧拉算出：\(w_o^d = M_o \dot{v}_o^d + h_o\)，\(M_o\) 是物体质量矩阵），我们要求各接触力 \(f\) 使 \(Gf = w_o^d\)。因为 \(G\) 是 \(6\times 18\) 的**宽矩阵**（接触自由度 18 远多于物体的 6 个自由度），解**不唯一**——有 \(18-6=12\) 维的解空间。通解为：

\[ \boxed{\ f = \underbrace{G^{+} w_o^d}_{\text{操作力（特解）}} + \underbrace{N_G\, \eta}_{\text{内力（零空间）}}\ } \]

\(G^{+} = G^\top(GG^\top)^{-1}\) 是 \(G\) 的伪逆，给出产生 \(w_o^d\) 的**最小范数操作力**——它落在 \(G\) 的行空间，是"恰好把茶几按要求推动"的力。
\(N_G\) 是 \(G\) 零空间的基（\(GN_G=0\)），\(\eta\in\mathbb{R}^{12}\) 是自由参数。\(N_G\eta\) 是**内力**——它满足 \(G(N_G\eta)=0\)，对茶几**不产生任何净运动**，纯粹在接触点之间相互抵消。内力的作用是**夹紧物体、调节摩擦裕度、避免脱手**。

本质洞察：内力 \(f_{\text{int}}=N_G\eta\) 落在 \(\mathcal{N}(G)\) 中绝非巧合，而是"对物体零贡献"的数学必然——任何不推动物体的接触力组合，按定义就满足 \(Gf=0\)，即落在零空间。这与第 5 章协同搬运、05 方向 20_机械臂/D03_双臂协调力控.md 里的内力分解是同一个数学事实的不同应用面。内力不是"多余的力"，而是**多接触系统稳定抓持的必需品**——没有内力，物体只靠摩擦极限平衡，稍有扰动就打滑。

那 12 维内力到底是些什么力？ "\(18-6=12\) 维零空间"听着抽象，落到三体抓茶几的物理图像，这 12 个内力自由度是可以**具体数出来**的，数清它们能让你对"内力分配在调什么"有实感。三个接触点、每点 6 维力旋量（线力 + 力矩），共 18 维；物体只有 6 维合力旋量被这些力决定，剩下 12 维是内力。它们大致分为：

接触点之间的"对挤/对拉"力（夹紧）：任意两个接触点沿连线方向相互施加大小相等、方向相反的力——净力为零（不推物体），但把物体夹住。三点之间有多对这样的方向，是内力的主体（提供摩擦裕度、防打滑）。
各接触点的局部力矩（拧）：若是面接触（人形手掌、四足背平台），每个接触点可施加绕接触法向的力矩，这些力矩可以相互抵消成净零力矩，但调节各点的抗扭裕度。
切向的相互剪切：接触点切平面内相互抵消的剪切力，调节各点离摩擦锥边界的距离。

理论-工程桥接（D）——内力自由度就是内力 QP 的"可调旋钮"。内力分配 QP（下面那个）里的决策变量 \(\eta\in\mathbb{R}^{12}\)，每一维就对应上面某个内力模式的强度。QP 在这 12 个旋钮上优化，目标是"在满足物体动力学（操作力已定）的前提下，让各接触力尽量小（加权）、都在摩擦锥内、人形侧向力不超限、法向力不低于最小夹紧"。理解这 12 维的物理含义，调试时就知道该往哪拧：物体打滑 → 加大夹紧类内力（对挤力）；人形侧向超限 → 减小会给人形增加侧向分量的那些内力模式；接触抗扭不足 → 调局部力矩模式。把零空间从"一个抽象的 12 维子空间"变成"一组可命名的物理旋钮"，是把范式 B 从公式用到实处的关键一步。

异构内力分配——本章的关键升级。在同构双臂里，内力 \(\eta\) 通常对称分配（两臂均摊夹紧力）。但异构团队**不能对称分配**，因为各个体的力能力 \(\lambda_k^{\max}\) 和平衡承受能力天差地别。我们把内力分配写成一个带形态约束的 QP：

\[ \boxed{\begin{aligned} \min_{\eta}\quad & \sum_{i=1}^{3} w_i \|f_i\|^2 \quad (\text{各接触力的加权范数，权重 } w_i \propto 1/\lambda_i^{\max}) \\ \text{s.t.}\quad & f = G^{+}w_o^d + N_G\eta \\ & f_i \in \mathcal{FC}_i \quad (\text{摩擦锥：} \|f_i^{\text{tangent}}\| \le \mu_i f_i^{\text{normal}}) \\ & \|f_i^{\text{lateral on } r_1}\| \le \bar{F}_1^{\text{balance}} \quad (\text{人形侧向力上限，保平衡}) \\ & f_i^{\text{normal}} \ge f_{\min} \quad (\text{最小夹紧力，防脱手}) \end{aligned}} \]

逐项的工程含义： - 目标：最小化加权接触力——权重 \(w_i\propto 1/\lambda_i^{\max}\) 让力能力弱的个体（人形）少出力，力能力强的（四足）多担。这是异构不对称分配的核心。 - 摩擦锥约束 \(\mathcal{FC}_i\)：每个接触点的切向力不能超过法向力乘摩擦系数，否则打滑。注意各个体接触面摩擦系数 \(\mu_i\) 不同（人形橡胶手 vs 四足金属背）。 - 人形侧向力上限 \(\bar{F}_1^{\text{balance}}\)：这是异构特有的约束——人形当前平衡状态能承受的最大侧向操作反力。超过它人形就会倾倒。这个上限**动态变化**（取决于人形当前质心位置、支撑相），需要从人形的平衡控制器实时反馈。 - 最小法向力 \(f_{\min}\)：保证每个接触都"压住"物体，不脱手。

这个 QP 在每个控制周期求解，输出各接触力 \(f_i\)，再分发给各个体的 whole-body 控制器作为末端力目标。

摩擦锥约束到底怎么写进 QP——二阶锥与它的线性化。 上面 QP 里 \(\|f_i^{\text{tangent}}\|\le\mu_i f_i^{\text{normal}}\) 这一行看着简单，但它是这个 QP 能不能高效求解的关键，初学者最容易在这里栽跟头。摩擦锥的精确形式是一个**二阶锥（second-order cone）**：

\[ \mathcal{FC}_i = \big\{\,f_i:\ \sqrt{(f_i^{x})^2+(f_i^{y})^2}\ \le\ \mu_i\, f_i^{z},\ \ f_i^{z}\ge 0\,\big\} \]

（设接触面法向为 \(z\)，切向为 \(x,y\)。）这个约束是**凸的**，所以带它的问题是一个二阶锥规划（SOCP），理论上可直接用 SOCP 求解器（如 ECOS、Mosek）解。但很多 whole-body/力分配框架为了用更快的 QP 求解器（OSQP、qpOASES，只接受线性约束 + 二次目标），会把二阶锥**线性化**为一个内接的**多面体锥（金字塔近似，polyhedral cone）**：

\[ \mathcal{FC}_i^{\text{lin}} = \big\{\,f_i:\ \pm f_i^{x}\le \tfrac{\mu_i}{\sqrt2} f_i^{z},\ \ \pm f_i^{y}\le \tfrac{\mu_i}{\sqrt2} f_i^{z},\ \ f_i^{z}\ge 0\,\big\} \]

即用 4 个（或 2\(m\) 个，\(m\) 越大越精确）线性不等式围出一个内接于真锥的金字塔。

对比性思维（B）——精确二阶锥 vs 线性化金字塔。两种写法各有取舍，选错会出问题： - 内接金字塔（最常用）：保守——金字塔在真锥**内部**，满足金字塔约束的力**一定**满足真锥（不会误判打滑为不打滑），安全。代价是切掉了锥的"角"，会**误判一些本可行的力为不可行**（保守拒绝），可能让 QP 在临界工况下找不到解。边数越多越逼近真锥但约束越多。 - 外接金字塔：相反，包住真锥，不会保守拒绝，但会**误判打滑的力为安全**（危险），实践中**绝不用**于摩擦约束（宁可保守，不可乐观）。 - 精确二阶锥：无近似误差，但要 SOCP 求解器，单周期求解通常比 QP 慢，高频控制环里可能吃不消。

工程默认：高频内力分配用**内接金字塔 + QP**（够快、保守安全），离线/中频规划或对精度极敏感时用**精确 SOCP**。记住方向：摩擦约束的近似永远往**保守（内接）**那侧偏——这和安全约束"宁紧勿松"是同一个工程哲学。

理论-工程桥接（D）——异构让金字塔近似更微妙。同构双臂里所有接触摩擦系数 \(\mu\) 相同，一套金字塔边数走天下。异构团队里各接触面 \(\mu_i\) 差异大（人形橡胶手 \(\mu\approx0.8\)、四足金属背 \(\mu\approx0.3\)），摩擦锥的"胖瘦"各不同——\(\mu\) 小的锥又窄又尖，金字塔近似的相对误差更大（切掉的角占比更高），更容易保守拒绝。所以异构内力 QP 里，对 \(\mu\) 小的接触点（四足金属背）值得用更多边数的金字塔（或对它单独用精确二阶锥），对 \(\mu\) 大的（人形手）可以粗一点。这是"按形态调"原则（§9.1 对称性破缺）在求解器配置层的又一次体现——连摩擦锥的离散精度都该按接触点的形态属性区别对待。

一个数值直觉——异构加权如何改变分配。设三体竖直抬一张 \(mg=200\)N 的茶几（纯竖直支撑，\(w_o^d=(0,0,200,0,0,0)\)），三个接触点对称布置。若不加权（同构假设），最小范数操作力让三点**均摊**——各出约 \(67\)N 竖直力。现在加入异构权重：人形力能力 \(\lambda_1^{\max}=80\)N（弱）、四足 \(\lambda_{2,3}^{\max}=300\)N（强），权重 \(w_i\propto 1/\lambda_i^{\max}\)。QP 的最优解会把竖直力**向四足倾斜**——人形可能只出约 \(25\text{-}35\)N，两台四足各出约 \(82\text{-}88\)N。直觉解释：加权范数惩罚"让弱者多出力"（人形的 \(\|f_1\|^2\) 被乘以大权重），所以最优解自然把负担推给能力强的四足。这就是 §9.1"能力效用"在力层面的兑现——分配不仅在任务层（谁干）照顾形态，在力层（出多少力）同样照顾形态。

理论-工程桥接（D）——加权伪逆与加权范数的等价。上面的"加权最小范数"其实有闭式起点：不带不等式约束时，加权最小范数解是 \(f = W^{-1}G^\top(GW^{-1}G^\top)^{-1}w_o^d\)（加权伪逆），它直接把力向低权重（高能力）接触点分配。工程上常用这个加权伪逆给一个**初值**，再用 QP 加摩擦锥/平衡/最小夹紧力等不等式约束精修。理解这一点能帮你调试：若不加权伪逆初值就让弱者多出力，多半是权重矩阵 \(W\) 写反了（应 \(\propto 1/\lambda^{\max}\) 而非 \(\propto\lambda^{\max}\)）。

优缺点： - 优点：精确——内力被显式控制，能精确调节夹紧力、严格满足摩擦锥和平衡约束；天然处理异构（通过加权和形态约束）。 - 缺点：需要**全局信息**（统一的 \(G\)、物体状态、各个体能力上限），通常需要一个协调节点或全网共识；对物体状态估计误差敏感（\(G\) 依赖接触点位置 \(r_i\)，估计错了内力分配就错）。

阶段小结：到这里我们完成了范式 A（柔顺协调）和范式 B（显式内力分配）——前者把误差用阻抗"软化"、后者把内力在零空间"算清"。接下来的范式 C 要做的是第三条路：不集中算内力，而是让各异构体**分布式地对物体轨迹达成共识**，用第 4 章的 ADMM 切分。它兼具 A 的分布式和 B 的精确。

范式 C：分布式 MPC（ADMM）一致性协调¶

核心思想：每个异构体跑**自己的 whole-body MPC**（人形的、四足的，各不相同），通过对"共抓物体的轨迹"施加**一致性约束**来协调——用第 4 章的 ADMM 把这个耦合的联合优化切成各自的本地子问题 + 共识。这是范式 A（简单但不精确）和范式 B（精确但需集中协调）的折中：既分布式、又精确。

回顾第 4 章 §50：ADMM 把"多足编队联合 MPC"切成"每足本地子问题 + 一致性变量协调"。它的妙处是**不要求各子系统模型相同**——每个机器人只需暴露它对"共享变量"的估计，无需共享完整动力学。这恰好是异构团队梦寐以求的性质。

问题结构。每个个体 \(r_k\) 有自己的 whole-body MPC，决策变量是它未来 \(H\) 步的状态-控制轨迹 \(X_k = (q_k, v_k, \tau_k, f_k)_{0:H}\)。它的本地目标 \(J_k\) 是"跟踪自己的末端目标 + 保持自己的平衡 + 省力"。耦合**来自一个共享变量——**物体轨迹 \(x_o\)：每个个体对物体未来轨迹有一个本地估计 \(x_o^{(k)}\)（它认为物体该怎么动），协调的目标是让所有个体的物体轨迹估计**达成一致**：\(x_o^{(1)}=x_o^{(2)}=x_o^{(3)}=z\)（\(z\) 是全局共识的物体轨迹）。

写成 ADMM 标准形式（共享变量是物体轨迹 \(z\)）：

\[ \boxed{\begin{aligned} \min_{\{X_k\},\, z}\quad & \sum_{k=1}^{3} J_k(X_k) \\ \text{s.t.}\quad & \text{个体 } k \text{ 的 whole-body 动力学 + 接触 + 平衡约束（本地）} \\ & \text{末端-物体运动学闭链：} x_{\text{ee},k}(X_k) = T_k(x_o^{(k)}) \quad (\text{末端贴合物体}) \\ & x_o^{(k)} = z \quad (\forall k,\ \text{一致性约束}) \end{aligned}} \]

其中 \(T_k\) 是把物体位姿映射到个体 \(r_k\) 接触点应在位置的刚性变换（由抓取几何决定）。最后一行 \(x_o^{(k)}=z\) 是 ADMM 要分解的耦合约束。

ADMM 迭代（每个 MPC 周期内迭代到收敛或固定轮数）：

重复直到收敛：
  ① 本地更新（每个 r_k 并行，无需通信）：
       X_k, x_o^(k) ← argmin J_k(X_k) + (ρ/2)‖x_o^(k) - z + u_k‖²
                       s.t. r_k 的 whole-body 动力学/接触/平衡约束
                            x_ee,k = T_k(x_o^(k))
     —— 每个个体解自己的 whole-body MPC，多了一个"物体轨迹靠近共识 z"的惩罚项

  ② 全局聚合（需通信/协调节点）：
       z ← (1/3) Σ_k (x_o^(k) + u_k)
     —— 把三个个体对物体轨迹的估计平均，得到新共识

  ③ 对偶上升（每个 r_k 本地）：
       u_k ← u_k + (x_o^(k) - z)
     —— 更新缩放对偶变量，惩罚"本地估计与共识的偏差"

逐步理解（务必抓住每步在物理上做什么）： - 步①：每个个体独立地规划"我怎么动最好，同时让物体按我估计的轨迹走"。人形解人形的 MPC（带它的双足平衡约束 + 侧向力上限），四足解四足的 MPC（带它的四足接触约束）——模型完全不同，但接口一致（都暴露 \(x_o^{(k)}\)）。\(\rho\) 是惩罚权重，逼着本地估计向共识靠拢。 - 步②：协调器（或全网共识）把三个估计平均成 \(z\)。这是唯一需要通信的步骤，且只交换**物体轨迹**（低维），不交换各自的完整状态/模型——通信极省。 - 步③：对偶变量 \(u_k\) 累积"本地与共识的历史偏差"，把它反馈进步①的惩罚项，最终逼所有个体的物体估计严格一致。

收敛后，所有个体对物体轨迹达成一致（\(x_o^{(k)}=z\)），各自的 whole-body 轨迹也都满足自己的物理约束且与物体协调——内力问题被一致性约束隐式解决（大家对物体运动看法一致 → 不再产生不一致的内力），平衡问题被各自 MPC 的本地约束解决（人形 MPC 自带侧向力上限）。

理论-工程桥接（D）：范式 C 是第 4 章 ADMM 的**直接复用 + 异构升级**。第 4 章里 ADMM 协调的是"同构多足的质心轨迹"，这里协调的是"异构体共抓的物体轨迹"——ADMM 框架一字不改，只是 (1) 各本地子问题从"同构 SRB MPC"换成"异构 whole-body MPC（人形的/四足的各异）"，(2) 共识变量从"编队几何"换成"物体轨迹"。这再次印证本章的方法论：异构性几乎总是通过"换本地子问题、保协调框架"来吸收，而非重写协调算法。这与 §9.2 异构 CBBA"只换出价函数"是同一个套路。

惩罚参数 \(\rho\) 怎么调——ADMM 调试的头号超参。 故障排查 #5 和 §9.3 陷阱 4 都提到"网格搜 \(\rho\)"，但 \(\rho\) 到底在调什么、往哪调，值得讲清——它是 ADMM 实践中最常需要拧的旋钮。\(\rho\) 是增广拉格朗日里那个二次惩罚项 \(\frac{\rho}{2}\|x_o^{(k)}-z\|^2\) 的权重，它平衡两种残差：

\[ \underbrace{r^{\text{prim}} = \|x_o^{(k)} - z\|}_{\text{原始残差：本地估计与共识的偏差}},\qquad \underbrace{r^{\text{dual}} = \rho\,\|z^{(t)} - z^{(t-1)}\|}_{\text{对偶残差：共识的变化量}} \]

\(\rho\) 太大：强逼本地估计贴近共识 \(z\)——原始残差 \(r^{\text{prim}}\) 掉得快，但本地子问题被"靠近 \(z\)"这个惩罚主导，几乎不顾自己的物理目标（跟踪/平衡），且共识 \(z\) 在迭代间剧烈变化（对偶残差大），整体震荡（这正是故障 #5 的"物体轨迹估计振荡"）。
\(\rho\) 太小：本地子问题自由发挥、各算各的——本地物理目标满足得好，但谁也不向共识靠拢，原始残差 \(r^{\text{prim}}\) 迟迟不降，协调不收敛（各体对物体轨迹始终不一致 → 残余内力）。
目标：让两个残差**同步下降**。理论上 ADMM 收敛要求两者都趋于 0，\(\rho\) 的作用就是平衡它们的下降速度。

自适应 \(\rho\)（residual balancing，工程常用）： 与其网格搜一个固定 \(\rho\)，更稳健的是让 \(\rho\) 随残差**自动调整**：

\[ \rho^{(t+1)} = \begin{cases} \tau\,\rho^{(t)}, & r^{\text{prim}} > \kappa\, r^{\text{dual}}\quad(\text{原始残差太大，加大 }\rho\text{ 逼一致}) \\ \rho^{(t)}/\tau, & r^{\text{dual}} > \kappa\, r^{\text{prim}}\quad(\text{对偶残差太大，减小 }\rho\text{ 防震荡}) \\ \rho^{(t)}, & \text{否则} \end{cases} \]

（典型 \(\tau\approx2,\ \kappa\approx10\)。）它的逻辑很直白：哪个残差大就往压它的方向调 \(\rho\)，让两者保持在同一量级。这比手调固定 \(\rho\) 鲁棒得多，是异构场景（各体子问题尺度不同、固定 \(\rho\) 难以普适）的首选。

本质洞察——\(\rho\) 调的是"自私 vs 合群"的平衡。把 ADMM 的本地更新看成每个个体的"小算盘"：\(\rho\) 大，个体被强迫"合群"（紧贴全局共识，牺牲自己的物理目标）；\(\rho\) 小，个体"自私"（只顾自己解得好，不管和别人一不一致）。协调的艺术，是让个体**既照顾自己的物理约束（平衡/跟踪），又逐步向集体共识靠拢**——这恰是 \(\rho\) 适中时发生的。残差平衡法的妙处在于它不需要你预知"多自私才合适"，而是**看着两种残差动态地调节这个平衡**。这也呼应了本章反复出现的工程智慧（阻抗别太硬、分配别太敏感、读陈旧值换独立性）：协调系统的好参数，往往不是某个固定的"最优值"，而是一个能随状态自适应的平衡机制。

三范式对比¶

维度	A 领导者-跟随者	B 统一抓取矩阵 + 内力分配	C 分布式 MPC (ADMM)
协调机制	主导 + 柔顺跟随（阻抗）	显式内力分配（零空间 QP）	物体轨迹一致性（共识）
误差出口	跟随者柔顺让步	主动协商内力	对物体轨迹达成共识
内力控制	隐式（阻抗软化）	显式精确	隐式（一致 → 无失配内力）
通信需求	极低（甚至零通信）	高（需全局 \(G\)、物体状态）	中（每轮共享物体轨迹）
精度	中	高	高
鲁棒性（单点失效）	低（领导者失效全瘫）	中（依赖协调节点）	高（去中心化）
异构适配方式	按形态调阻抗增益	加权 + 形态约束（QP）	各跑各的 whole-body MPC
计算负担	低	中（一个集中 QP）	高（每体一个 MPC + ADMM 迭代）
代表工作	经典双臂、decPLM 接触隐式协调	Shared Object Manip (ETH)、双臂力控	De Vincenti SQP、分布式 loco-mani
何时用	通信受限、要鲁棒、精度要求不高	精密协同、有协调节点、模型准	既要分布式又要精确、算力充足

系统性分类（E）——三范式不是三选一，而是一个谱。它们沿"集中 ↔ 分布"和"隐式 ↔ 显式协调"两个维度分布：A 在"分布 + 隐式"角（各自柔顺，无需协商），B 在"集中 + 显式"角（一个 QP 算清所有内力），C 在中间（分布式地显式协商物体轨迹）。实践中常**混用**：用 A 的阻抗做底层柔顺安全垫（防止任何瞬间内力爆炸），用 C 的 ADMM 做上层轨迹协调，用 B 的内力 QP 在关键操作（如精密插入）时临时接管。选型不是"哪个最好"，而是"当前任务的通信/算力/精度约束落在谱的哪一段"。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：把跟随者的阻抗增益调得和领导者一样硬。 - 错误描述：范式 A 里，为了"精确跟随"，把跟随者的阻抗 \(K_p\) 调得很大（接近位置控制）。 - 现象/后果：跟随者变得僵硬，不再柔顺让步——退化为多个个体的独立位置跟踪，于是 §9.3 开头推演的"内力发散"重现，物体被拉扯甚至脱手。脆弱平衡的人形当跟随者时还会被硬抗产生的反力拽倒。 - 根本原因：阻抗协调的**全部价值**在于"柔顺让步给误差一个出口"。增益太硬就抽掉了这个出口，把柔顺协调变回了无协调的独立跟踪。 - 正确做法：跟随者阻抗必须**足够软**以吸收误差，按形态调（人形最软、四足可稍硬）。判据：在预期的状态估计误差下，阻抗产生的内力应远小于物体结构强度和各个体的力上限。宁可跟随略有滞后，不可硬抗发散。

陷阱 2（思维陷阱）：内力分配 QP 里忘了人形的动态平衡上限。 - 错误描述：范式 B 的内力 QP 只加了静态的摩擦锥和力能力上限 \(\lambda_k^{\max}\)，没加人形随状态变化的侧向力上限 \(\bar{F}_1^{\text{balance}}(s_1)\)。 - 现象/后果：QP 算出一个满足摩擦锥和力上限、但侧向分量超过人形当前平衡承受能力的内力分配。人形执行后被推出支撑区倾倒——QP 觉得"合法"，物理上却失稳。 - 根本原因：力能力上限 \(\lambda^{\max}\) 是"末端能输出多大力"（执行器极限），与"施加这个力后还能不能站稳"（平衡极限）是**两回事**。固定臂两者等价（基座吸收反力），但浮动基座的脆弱平衡个体，平衡极限远低于执行器极限，且**动态变化**。 - 正确做法：把"操作反力对平衡的影响"作为**动态约束**注入内力 QP，且该上限从人形的平衡控制器/ZMP 裕度实时反馈。更彻底的做法是范式 C——让人形自己的 whole-body MPC（自带平衡约束）来决定它能出多少力，而非外部 QP 硬塞。

陷阱 3（编程陷阱）：抓取矩阵 \(G\) 用了错误的接触点坐标系。 - 错误描述：范式 B 计算 \(G\) 时，三个接触点位置 \(r_i\) 混用了不同坐标系——有的用世界系、有的用物体系、有的用各自 base 系。 - 现象/后果：\(G\) 的杠杆项 \([r_i]_\times\) 算错，导致内力/操作力分解错误。物体明明只该平移，分解出的力却产生了寄生力矩，物体莫名旋转或内力不抵消。 - 根本原因：\(G\) 的定义要求所有接触点位置 \(r_i\) 表达在**同一个参考系**（通常是物体质心系），且 \(w_o^d\) 也在该系。坐标系不统一是多接触力控代码最隐蔽的 bug。 - 正确做法：严格统一坐标系——所有 \(r_i\)、所有力旋量、\(w_o^d\) 都在物体质心系（或统一世界系）表达，跨系转换用准确的伴随变换（adjoint）。写单测：给一个纯平移的 \(w_o^d\)，断言分解出的力**不产生净力矩**（\(\sum [r_i]_\times f_i^{\text{lin}} + \sum\tau_i = 0\) 的力矩分量为零）。

陷阱 4（概念误区）：以为范式 C 的 ADMM 一轮就收敛、可以直接用在 1kHz 控制环。 - 错误描述：把 ADMM 当成"算一次就出结果"，每个控制周期只跑一轮 ADMM 迭代，期望立即得到协调好的轨迹。 - 现象/后果：单轮 ADMM 远未收敛，各个体的物体轨迹估计仍不一致 → 残余内力 → 协调质量差，甚至振荡。 - 根本原因：ADMM 是**迭代**算法，需要多轮才收敛到一致。它适合在 MPC 的规划周期（如 10-50Hz）内跑多轮迭代，而非在 1kHz 的底层控制环里跑单轮。 - 正确做法：分层——ADMM 在中频（MPC 层，~50Hz）跑足够轮数得到协调好的物体轨迹和各体参考；底层 1kHz 的 whole-body 控制器只负责跟踪这个已协调好的参考（外加范式 A 的阻抗做安全垫）。永远不要在最内层控制环里指望 ADMM 实时收敛。

练习¶

（推导题） 对本章三体抓茶几场景，设三个接触点在物体质心系的位置为 \(r_1=(0.5,0,0)\)、\(r_2=(-0.5,0.4,0)\)、\(r_3=(-0.5,-0.4,0)\)（米），假设点接触（每点力 \(\in\mathbb{R}^3\)）。写出抓取矩阵 \(G\in\mathbb{R}^{6\times 9}\) 的完整数值。给定物体期望合力旋量 \(w_o^d=(0,0,mg,0,0,0)\)（纯竖直支撑，抵消重力 \(mg=200\)N），求最小范数操作力 \(G^+w_o^d\)，并说明此解下各接触点分别出多少竖直力——它体现了什么分配（提示：对称几何下应均摊）。
（设计扩展题） 在练习 1 基础上，茶几摩擦系数 \(\mu=0.5\)，要求每个接触点维持至少 \(f_{\min}=20\)N 的法向夹紧力。设计一个内力 \(\eta\)（在 \(G\) 零空间里）叠加到操作力上，使三个接触点都满足最小夹紧力和摩擦锥。写出内力分配 QP 并求解（可用数值工具）。然后改为异构权重——人形接触点 \(r_1\) 的权重设为四足的 3 倍（人形力弱），观察内力如何重新分配到两台四足。
（综合调试题） 给出一个范式 C（ADMM 协调）的简化仿真框架（三个个体用简化质点-末端模型代替完整 whole-body）。框架里 ADMM 不收敛——物体轨迹估计在三个值之间振荡。诊断可能的原因（提示：\(\rho\) 太大/太小？步②的聚合权重错误？对偶更新符号反了？），逐一排查并修复，画出修复前后 \(\|x_o^{(k)}-z\|\) 随迭代轮数的收敛曲线对比。
（跨范式思辨题） 三范式各有失效场景。为以下三种情况各选一个最合适的范式并论证：(a) 通信几乎中断、只能靠接触力协调；(b) 要把一根细长易碎的玻璃管精密插入孔中，对内力精度要求极高；(c) 五个异构体在崎岖地形协同搬运、无中央节点、算力充足。

§9.4 统一 whole-body 框架——异构浮动基座的拼装 ⭐⭐⭐⭐¶

这一节解决什么问题：范式 C 让每个个体跑自己的 whole-body MPC，但它们是怎么写出来的？人形、四足、臂的自由度不同、接触模式不同、平衡约束不同——能不能用**一套统一的数学**同时描述它们，把团队当成"一个大系统"？这一节给出统一 whole-body 框架：把多个异构浮动基座系统**堆叠**成一个团队级 QP，加上跨体闭链耦合和分层任务栈。核心要厘清一个反直觉的点——"统一描述"不等于"集中式求解"。

动机：能不能用一套数学描述整个团队¶

§9.3 的三范式，无论哪个，最终都要落到"每个个体怎么把协调目标（物体力/轨迹）变成关节力矩"——这是单体 whole-body 控制的活。05 运动控制方向已经把单体 whole-body 讲透了（足式 90_WBC分层优化与TSID.md、机械臂 F07_浮动基座WBC理论.md）：一个浮动基座系统的 whole-body QP 长这样（回顾，下面会逐项激活）：

\[ \min_{\dot v,\, \tau,\, f}\ \|J\dot v + \dot J v - \ddot x^{\text{des}}\|^2 \quad \text{s.t.}\ \underbrace{M\dot v + h = S^\top\tau + J_c^\top f}_{\text{动力学}},\ \underbrace{J_c\dot v + \dot J_c v = 0}_{\text{接触不动}},\ \underbrace{f\in\mathcal{FC}}_{\text{摩擦锥}} \]

现在的问题是：团队里有人形（\(n_1\approx25\)）、四足-A（\(n_2=12\)）、四足-B（\(n_3=12\)），它们各有一个这样的 QP。能不能把这三个 QP 拼成一个，统一地描述整个团队 + 它们共抓的物体？

这么做有什么好处？至少三个：(1) 概念统一——异构团队不再是"三个互不相干的控制器 + 一堆协调 hack"，而是一个结构清晰的大优化问题；(2) 耦合显式化——三个个体通过物体形成的闭链约束，在统一框架里是一个**显式的约束行**，不会像 §9.1 陷阱里那样"在接缝处丢失"；(3) 为分布式求解提供母体——有了统一的集中式描述，才能用 ADMM/对偶分解把它**切回**分布式（范式 C），数学上有据可依。

但这里埋着本节最关键的认知陷阱，先点破：写出统一的集中式描述，和实际用集中式求解，是两回事。 我们写统一框架是为了**理解结构、保证耦合不丢失、推导分布式分解**，而不是真的把一个上百维的大 QP 丢给单台计算机实时解（那会违背多机系统去中心化的初衷，且单点失效致命）。统一是"描述层"的，求解可以是"分布层"的。

如果不统一、各写各的会怎样¶

回到 §9.1 场景三的崩溃（R6B 反事实，这里深化）：人形用控制栈 X、四足用控制栈 Y，各自独立。问题出在哪？

问题的数学根源是闭链约束无人维护。 三个个体共抓物体，意味着它们的末端位姿被物体**刚性锁定**：

\[ x_{\text{ee},1} = T_{1o}\, x_o,\quad x_{\text{ee},2} = T_{2o}\, x_o,\quad x_{\text{ee},3} = T_{3o}\, x_o \]

（\(T_{ko}\) 是物体系到 \(r_k\) 接触点的固定变换。）这三个等式合起来，等价于**末端之间的相对位姿恒定**：\(x_{\text{ee},1}^{-1}x_{\text{ee},2}=\text{const}\) 等。这是一组**跨越多个个体**的约束——它不属于任何单个个体的 whole-body QP（每个个体的 QP 只知道自己的状态）。

如果各写各的，没有任何一个 QP 包含这组跨体约束。于是：人形的控制器努力把自己的手放到它**以为**物体在的地方，四足的控制器努力把背放到它以为物体在的地方——但没人保证这三个"以为"是相容的（满足闭链）。结果就是 §9.3 反复出现的内力发散/拉扯。

统一框架的价值，正是给这组闭链约束一个"家"。 把三个个体 + 物体放进同一个优化问题，闭链约束作为一个显式的约束行存在，求解器（无论集中还是分布式分解后）必须满足它——耦合不再丢失。这就是"统一"要解决的根本问题。

本质洞察：异构团队协作的数学本质，是一个**带跨体闭链约束的多浮动基座联合优化**。三范式（§9.3）是这个联合优化的三种"求解/近似策略"——领导跟随是用阻抗**软化**闭链约束，内力分配是把闭链约束**对偶化**为内力，分布式 MPC 是用 ADMM **分解**闭链约束。统一框架是它们共同的母问题。看清这一层，你就不会把三范式当成无关的招式，而是同一个 QP 的三种打开方式。

历史：从单体 WBC 到 multi-robot whole-body¶

whole-body 控制的统一框架思想，经历了从单体到多体的演进。

单体 WBC（2010s）：Sentis、Righetti 等建立了浮动基座系统的 whole-body 控制框架——operational space control、分层零空间投影（hierarchical null-space projection）、whole-body QP。核心是把"多个任务（末端、质心、姿态）+ 多个约束（动力学、接触、限位）"统一进一个（分层）优化。05 方向 90_WBC分层优化与TSID.md 详讲了 TSID（Task-Space Inverse Dynamics）这一主流实现。
浮动基座 + 操作（2018-2022）：把臂的操作任务纳入腿足 whole-body——ALMA、qm_control（05 方向 30_复合/170_qm_control精读.md）等把"末端操作力"作为 whole-body QP 的一个任务/约束，实现单体 loco-manipulation。
multi-robot whole-body（2023-2025）：De Vincenti 的 centralized MPC for collaborative loco-manipulation（第 8 章 §90）首次把"多个 legged 机器人 + 负载"写成**一个**联合 SQP——robot/payload/gait-agnostic，多个机器人的动力学 + 负载动力学 + 耦合约束统一在一个优化里。ETH 的 Shared Object Manipulation 进一步加入力闭合抓取的混合运动-力控。这是本节"统一框架"的工程原型。

这条线的关键洞察：whole-body 框架的"统一"能力是可组合的——它先统一了"单体的多任务多约束"，再统一了"单体的运动 + 操作"，最后统一了"多体 + 负载"。每一层都是把"更多互相耦合的东西"塞进同一个优化结构。本节就是走完最后一层。

理论：团队级 whole-body 优化¶

我们分三步拼装：堆叠各异构体的动力学 → 加入跨体闭链耦合 → 组织分层任务栈。

1. 堆叠异构动力学¶

团队有 \(K\) 个异构体（示例 \(K=3\)）+ 1 个被操作物体。每个个体 \(r_k\) 是一个浮动基座系统，广义坐标 \(q_k=(p_k,R_k,q_{j,k})\in\mathbb{R}^{6+n_k}\)，注意**维度 \(n_k\) 各不相同**（人形 25、四足 12）。它的动力学（牛顿-欧拉）：

\[ M_k(q_k)\,\dot v_k + h_k(q_k,v_k) = S_k^\top \tau_k + J_{c,k}^\top f_{c,k} + J_{e,k}^\top f_{e,k} \]

\(M_k\)：质量矩阵（\((6+n_k)\times(6+n_k)\)，各体尺寸不同）；\(h_k\)：科氏/重力等非线性项。
\(S_k\)：选择矩阵（浮动基座的 6 个自由度不可直接驱动，\(S_k=[0_{n_k\times 6}\ I_{n_k}]\)）。
\(J_{c,k}^\top f_{c,k}\)：环境接触力（人形的脚、四足的足端）——维持 locomotion/平衡。
\(J_{e,k}^\top f_{e,k}\)：操作接触力（人形的手、四足的背对物体施的力）——这是与物体的耦合入口，\(f_{e,k}\) 就是 §9.3 抓取矩阵里的 \(f_k\)。

物体动力学（自由刚体，被各个体的操作力推动）：

\[ M_o\,\dot v_o + h_o = \sum_{k=1}^{K} \text{Ad}_{T_{ok}}^\top\, f_{e,k} \ \triangleq\ G f_e \]

右边正是 §9.3 的统一抓取矩阵 \(G\) 作用在堆叠操作力 \(f_e=(f_{e,1},\dots,f_{e,K})\) 上——物体动力学和抓取矩阵在这里自然汇合。

把 \(K\) 个个体 + 物体的动力学**堆叠**成块对角结构：

\[ \boxed{\ \underbrace{\begin{bmatrix} M_1 & & & \\ & \ddots & & \\ & & M_K & \\ & & & M_o\end{bmatrix}}_{\text{块对角，团队质量矩阵}} \begin{bmatrix}\dot v_1 \\ \vdots \\ \dot v_K \\ \dot v_o\end{bmatrix} + \begin{bmatrix}h_1\\\vdots\\h_K\\h_o\end{bmatrix} = \begin{bmatrix}S_1^\top\tau_1 + J_{c,1}^\top f_{c,1}\\\vdots\\ S_K^\top\tau_K + J_{c,K}^\top f_{c,K}\\ 0\end{bmatrix} + \underbrace{\mathcal{J}_e^\top f_e}_{\text{操作力耦合}}\ } \]

关键观察：团队质量矩阵是块对角的——各个体的动力学**本身不直接耦合**（人形的质量矩阵里没有四足的项）。耦合**只通过操作力** \(f_e\)（最后一项 \(\mathcal{J}_e^\top f_e\)，它同时出现在各个体动力学和物体动力学里）。这个"块对角 + 稀疏耦合"结构是 §9.3 范式 C 能用 ADMM 分解的**数学根据**：块对角部分可并行（各体本地），耦合部分用共识协调。

多视角理解（A）——块对角 + 稀疏耦合像"几个人合抬桌子，但各自的肌肉是独立的"。把团队质量矩阵想象成一群人合抬一张桌子：每个人的肌肉/骨骼（各体的 \(M_k\)）是**完全独立**的系统——你发力不会直接改变我的肌肉状态，这对应质量矩阵的"块对角"（对角块互不交叠）。我们之间唯一的联系，是**都搭在同一张桌子上**：你抬高一点，桌子动一点，我通过桌子感受到——这对应"只通过操作力 \(f_e\) 耦合"（桌子是唯一的耦合媒介）。 像的部分：独立的"身体" + 唯一的共享媒介（桌子/物体）。不像的部分：人靠本体感觉和默契隐式协调，机器人要显式写出闭链约束和共识；且人的"块"是天生独立的，机器人的块对角是我们**刻意保持**的建模选择——一旦错误地引入个体间直接耦合（陷阱 1 的"焊成大机器人"），块对角就被破坏，分解失效。这个类比能帮你记住：统一框架的合法耦合只有一条通道（物体接触力），任何绕过物体的个体间直接耦合都是建模错误。

对比性思维（B）——"统一不是融合成一个怪物"。一个常见误解是"统一框架 = 把三个机器人焊成一个 60 自由度的巨型机器人来控制"。不是。统一框架保留了块对角结构——每个个体在数学上仍是独立的子系统，它们**没有共享关节、没有共享 base**，只通过物体接触力松散耦合。"统一"统一的是**描述的语言和耦合的接口**，而非把独立系统强行合并。正因为保留了块对角结构，统一框架才能被分解回分布式（否则就真成了不可分的巨型 QP）。这个区别决定了你的代码是"一个巨型不可分 QP"（错）还是"块结构可分解的联合优化"（对）。

2. 跨体闭链耦合约束¶

这是统一框架的灵魂——给 §9.4 开头那组"在接缝处丢失"的闭链约束一个显式的家。三个个体的操作末端贴合同一物体，约束有两层：

运动学闭链（位置层）：各末端位姿被物体锁定。在加速度层（whole-body QP 通常在加速度/力层工作）写成：

\[ \boxed{\ J_{e,k}\,\dot v_k + \dot J_{e,k} v_k = \text{Ad}_{T_{ok}}\big(\dot v_o\big) + (\text{速度相关项}),\quad k=1,\dots,K\ } \]

含义：个体 \(r_k\) 末端的加速度，必须等于物体加速度经刚性变换映射过来的值——末端"焊"在物体上，跟着物体动。这 \(K\) 个约束把各体的运动与物体运动绑定，等价地也把各体运动**互相绑定**（都等于同一个 \(\dot v_o\) 的映射）。这就是 §9.3 范式 C 里 \(x_{\text{ee},k}=T_k(x_o)\) 在加速度层的形式。

力的闭合（力层）：各操作力之和等于物体所需合力——这就是物体动力学 \(M_o\dot v_o + h_o = Gf_e\)，已在步 1 包含。

这两层一起，把"三个独立个体 + 一个物体"真正缝成一个协调的整体。注意：维护这些约束是统一框架的核心职责——无论后续集中求解还是分布式分解，这些约束都不能丢。

3. 分层任务栈¶

最后，团队有多个**任务目标**，且有**优先级**（高优先级任务不能被低优先级牺牲）。沿用单体 WBC 的分层零空间思想（05 方向 90_WBC分层优化与TSID.md），团队任务栈从高到低典型为：

优先级	任务	物理含义	谁负责
P0（硬约束）	各体动力学 + 接触不滑 + 摩擦锥 + 闭链耦合	物理可行性，绝不可违反	全体
P1（安全）	各体平衡（人形 ZMP/CoM、四足支撑多边形）	不摔倒——脆弱平衡个体的命门	各体本地
P2（任务）	物体轨迹跟踪 \(x_o\to x_o^d\)	把茶几搬到目标——团队主任务	团队协同
P3（操作精度）	内力调节、夹紧力维持	不脱手、不撕裂物体	内力分配（§9.3-B）
P4（次要）	姿态优雅、省能、关节远离限位	锦上添花	各体本地

本质洞察：分层的关键在 P1（平衡）高于 P2（任务）。这与单体 loco-manipulation 一致（FALCON 的下肢稳定优先于上肢操作），但在异构团队里更尖锐——因为不同个体的 P1 难度天差地别。人形的平衡（P1）极脆弱，必须优先保障；四足的平衡相对宽裕。统一框架的分层结构让"保人形不摔"自动高于"把茶几搬到位"：当两者冲突时（搬茶几要求的力会让人形失衡），求解器宁可牺牲茶几跟踪精度（P2），也不牺牲人形平衡（P1）。这正是 §9.3 陷阱 2 的根治——把平衡作为高优先级硬约束，而非内力 QP 里一个容易被忽略的项。

对比性思维（B）——软优先级 vs 严格分层两种实现。"P1 高于 P2"在代码里有两种实现方式（练习 2 要你选）：软优先级**把各任务加权塞进**一个 QP 的目标（\(\sum_i w_i\|\text{task}_i\|^2\)，\(w_{\text{平衡}}\gg w_{\text{跟踪}}\)），实现简单、求解快，但权重再大也**不是严格优先**——极端冲突下高权重任务仍可能被轻微牺牲（求解器在数值上做加权折中）。严格分层（lexicographic）**则级联多个 QP：先在 P1 的解空间里优化，再在"不改变 P1 最优"的**零空间**里优化 P2，依此类推——严格保证低优先级绝不破坏高优先级，但要解多个 QP、计算量大。对"保人形平衡 P1 > 搬茶几 P2"这个**安全攸关**的优先级，宁可用严格分层（或软优先级但给平衡一个**硬约束**而非大权重）——因为"人形不摔"是不容折中的红线，不能让求解器拿它和茶几跟踪做加权交易。这与摩擦锥近似"宁紧勿松"（§9.3-B）是同一个安全哲学：**安全约束要硬，不要软。

4. 统一 ≠ 集中：从母问题到分布式¶

把上面三步合起来，团队级 whole-body 优化的完整形式（概念形式）：

\[ \boxed{\begin{aligned} \min_{\{\dot v_k,\tau_k,f_{c,k},f_{e,k}\},\, \dot v_o}\quad & \sum_{k} w_k\, \text{(本地任务代价)}_k + w_o\,\|\dot v_o - \dot v_o^d\|^2 + \text{内力正则} \\ \text{s.t.}\quad & \text{各体动力学（块对角）} \quad \forall k \\ & \text{物体动力学 } M_o\dot v_o + h_o = Gf_e \\ & \text{跨体闭链耦合 } J_{e,k}\dot v_k+\dot J_{e,k}v_k = \text{Ad}_{T_{ok}}\dot v_o \quad \forall k \\ & \text{各体接触/摩擦/平衡/限位约束（本地）} \quad \forall k \end{aligned}} \]

这是一个 \(\sum_k(6+n_k) + 6\) 维（示例约 \(43+6\approx 49\) 维变量，加上力变量更多）的 QP。现在回到本节最关键的认知：

作为描述，这个统一 QP 是宝贵的——它清楚地展示了团队协作的完整结构，所有耦合都显式可见，没有任何东西"在接缝处丢失"。
作为求解，你**几乎从不**真的把它丢给单台计算机集中求解。原因：(1) 单点失效——协调器宕机全队瘫痪，违背多机鲁棒性初衷；(2) 通信——集中求解要求所有个体实时上传完整状态/模型到协调器，带宽和延迟难以承受；(3) 可扩展性——\(K\) 增大时集中 QP 规模爆炸。
正确姿态：用统一 QP 作**母问题**，对它做**对偶分解/ADMM**，切回 §9.3 范式 C 的分布式形式——块对角部分（各体动力学+本地约束）成为各体本地子问题（并行求解），耦合约束（物体动力学+闭链）通过共识变量（物体轨迹/力）协调。统一描述保证了分解的正确性（分解出的分布式算法确实在解同一个母问题），分布式求解保证了鲁棒性和可扩展性。

对比性思维（B）——"统一描述"vs"集中求解"是两个正交的维度。初学者常把"统一"和"集中"混为一谈，以为写了统一框架就必须集中求解。实际上：你可以**统一描述 + 集中求解**（小规模、有可靠协调器、要全局最优时，如 De Vincenti 的离线 SQP）；也可以**统一描述 + 分布式求解**（在线、要鲁棒、可扩展时，ADMM 分解）；甚至**不统一描述 + 分布式求解**（各写各的 + 手工协调 hack——这就是 §9.1 场景三的错误做法，因为没有统一描述，分布式协调缺乏数学依据，闭链约束丢失）。本章主张的是**统一描述 + 分布式求解**：用统一框架理解和保证正确性，用分布式求解获得鲁棒性。

5. 从统一 QP 到 ADMM：分解为什么是"对"的¶

上面说"用统一 QP 作母问题，对它做对偶分解/ADMM 切回分布式"。这句话不能只是口号——我们要看清**分解后的分布式算法确实在解同一个母问题**，否则范式 C（§9.3）就成了无根据的启发式。这一小节把这个分解走一遍（这也是范式 C 的数学根据）。

观察统一 QP 的结构：目标是**可分离的**（\(\sum_k\) 本地代价 + 物体代价），约束分两类——本地约束（各体动力学/接触/平衡，只涉及 \(r_k\) 自己的变量）和**耦合约束**（物体动力学 \(M_o\dot v_o=Gf_e-h_o\)、闭链 \(J_{e,k}\dot v_k=\text{Ad}\,\dot v_o\)，跨越多体）。这正是 ADMM 的标准适用结构："可分离目标 + 通过少数耦合约束相连"。

引入**共识变量** \(z\)（物体轨迹/加速度的全局共识），让每个个体持有一份本地副本 \(x_o^{(k)}\)，把耦合约束改写为"每个个体的本地物体估计都等于共识 \(z\)"。增广拉格朗日（augmented Lagrangian）为：

\[ \mathcal{L}_\rho = \sum_k \Big[ J_k(X_k) + \langle y_k,\ x_o^{(k)}-z\rangle + \tfrac{\rho}{2}\|x_o^{(k)}-z\|^2 \Big] \]

其中 \(y_k\) 是耦合约束的拉格朗日乘子（对偶变量），\(\rho\) 是增广项权重。ADMM 就是对 \(\mathcal{L}_\rho\) 交替优化 \(\{X_k\}\)、\(z\)、\(\{y_k\}\)：

\(\{X_k\}\) 步（固定 \(z,y\)）：\(\mathcal{L}_\rho\) 关于各 \(X_k\) 完全可分离（块对角结构！各体的项不交叉），所以可以**并行**地各解各的——这就是范式 C 的"本地更新"：每个个体解一个带"靠近共识 \(z\)"惩罚的本地 whole-body MPC。
\(z\) 步（固定 \(X,y\)）：\(\mathcal{L}_\rho\) 关于 \(z\) 是简单二次，最优解是各本地估计（加对偶修正）的**平均**——这就是"全局聚合"。
\(y_k\) 步：对偶上升 \(y_k\leftarrow y_k+\rho(x_o^{(k)}-z)\)——这就是"对偶更新"。

本质洞察——块对角是分解可行的充要结构。ADMM 的 \(\{X_k\}\) 步能并行，完全依赖于团队动力学的块对角结构（§9.4 步 1 反复强调的）。如果各体动力学真的耦合（比如它们共享关节，像一个大机器人），\(\mathcal{L}_\rho\) 关于 \(\{X_k\}\) 就不可分离，ADMM 退化、无法并行——那才真成了不可分的巨型 QP。正因为异构个体**没有物理连接、只通过物体力松耦合**，块对角结构成立，分解才可行。这就是为什么 §9.4 陷阱 1（"焊成大机器人"）是致命错误——它破坏了使分布式求解成为可能的唯一结构。统一框架的全部价值，恰恰在于它既统一了描述，又保留了使分解可行的块对角性——鱼和熊掌兼得，而这不是偶然，是异构团队"无刚性连接"的物理事实在数学上的投影。

这个分解证明了范式 C 的 ADMM 确实在求解统一 QP 母问题——分布式不是近似 hack，而是母问题的精确分解（凸情形下收敛到母问题最优；非凸 whole-body 情形下收敛到 KKT 点）。这就是"统一描述 + 分布式求解"在数学上站得住脚的根据。

6. 收敛速率与通信图：ADMM 协调要跑多少轮¶

§9.3 陷阱 4 强调"ADMM 是迭代算法，要跑足够轮数才收敛，不能在 1kHz 环里跑单轮"。但"足够轮数"是多少？这取决于一个关键因素——团队的通信图结构。这一小节把它讲清，它直接连到第 2 章的代数连通性，也是跨章综合题里"\(\lambda_2\) 很小会怎样"那一问的答案。

全局聚合那一步藏着通信图。 回顾 ADMM 的 \(z\) 步（全局聚合）：\(z\leftarrow\text{avg}_k(x_o^{(k)}+u_k)\)。在有中央协调节点时，这个平均一步完成。但多机系统常常**没有中央节点**（去中心化是范式 C 的卖点），此时"求所有个体的平均"本身就要靠**分布式平均共识**（第 2 章）——每个个体只能和**通信图上的邻居**交换信息，通过多轮迭代逼近全局平均：

\[ x^{(t+1)} = W\,x^{(t)},\quad W = I - \epsilon L \]

其中 \(L\) 是通信图的拉普拉斯矩阵，\(W\) 是共识权重矩阵（第 2 章详讲）。这个迭代收敛到全局平均的速率，由 \(W\) 的**第二大特征值** \(|\lambda_2(W)|\) 决定——而 \(\lambda_2(W)\) 直接关联拉普拉斯的代数连通性 \(\lambda_2(L)\)（Fiedler 值）。

于是 ADMM 的总迭代数 = 外层 ADMM 轮数 × 内层共识轮数。 关键结论：

\[ \boxed{\ \text{内层共识达到精度 } \delta \text{ 所需轮数} \ \sim\ \frac{\log(1/\delta)}{\log(1/|\lambda_2(W)|)}\ \propto\ \frac{1}{\lambda_2(L)}\ (\text{弱连接时})\ } \]

图连接稠密（\(\lambda_2(L)\) 大，如全连通/近全连通）：\(|\lambda_2(W)|\) 小，共识几轮就收敛，ADMM 的每个聚合步很快——可以在中频 MPC 周期里跑足够外层轮数。
图连接稀疏（\(\lambda_2(L)\) 小，如链状/长直径图）：\(|\lambda_2(W)|\) 接近 1，共识收敛极慢，每个聚合步要很多轮通信——ADMM 在固定的 MPC 周期内可能**跑不完一个完整的外层迭代**，协调质量塌缩（这正是 §9.3 陷阱 4 在稀疏图上的恶化形态）。

本质洞察——协调质量被通信图的"瓶颈"卡住。代数连通性 \(\lambda_2(L)\) 度量的是"图有多难被切断"——它小，意味着图里存在一个**通信瓶颈**（少数几条边连接两大块）。信息（物体轨迹共识）通过这个瓶颈传播很慢，于是无论各体本地 whole-body MPC 解得多好，它们**对物体轨迹达成一致的速度被瓶颈卡死**。这把一个看似纯控制的问题（内力协调）和一个纯图论的量（Fiedler 值）锁在了一起：异构团队的物理协调能力，上界于它的通信图连通性。这就是为什么多机系统设计要同时关心"控制律"和"通信拓扑"——再好的协调算法也快不过信息传播的物理极限。

跨章知识桥接（第 2 章 → 本章）。第 2 章我们学过：分布式平均共识的收敛速率由代数连通性 \(\lambda_2(L)\)（Fiedler 值）决定，\(\lambda_2\) 越大收敛越快；它还决定了图的"鲁棒性"（\(\lambda_2>0\) 当且仅当图连通）。本章这里**直接复用**这个结论：范式 C 的 ADMM 在去中心化实现时，其聚合步就是一次分布式平均共识，所以**整个协调的收敛速率继承了第 2 章的 \(\lambda_2\) 依赖**。不仅如此——§9.2 的 CBBA 冲突消解（带时间戳最大值共识）的收敛轮数也 \(\propto\) 通信图直径（与 \(\lambda_2\) 同向相关）。所以跨章综合题问"\(\lambda_2\) 很小会怎样"，答案是**双重恶化**：CBBA 分配收敛慢（任务归属迟迟定不下来）+ ADMM 协调收敛慢（物体轨迹一致性达不成）——分配层和控制层同时被通信瓶颈拖累。这是第 2 章的图论、第 3 章的分配、本章的协调在同一个量（\(\lambda_2\)）上的交汇。

工程对策。 既然协调被 \(\lambda_2\) 卡，对策有三：(1) 改善拓扑——增加关键连接边、避免链状结构（图设计层）；(2) 加速共识——用第 2 章的加速共识方法（如 Chebyshev 加速、多步共识）压低有效 \(|\lambda_2(W)|\)；(3) 降低协调频率需求——若图实在稀疏，就别指望高频精确协调，退回范式 A（阻抗，零通信、靠接触力本地协调）做底层安全垫，只在低频做粗协调。对策 3 再次印证三范式"按通信约束选型"（§9.3 系统性分类）——通信图稀疏本身就是选范式 A 而非 C 的强信号。

⚠️ 常见陷阱¶

陷阱 1（思维陷阱）：把统一框架理解成"焊成一个大机器人"。 - 错误描述：以为统一 whole-body 就是把 \(K\) 个机器人当成一个 \(\sum n_k\) 自由度的巨型刚体系统，用一个不可分的大 QP 控制。 - 现象/后果：写出一个巨型稠密 QP，无法分布式、单点失效、规模爆炸。更糟的是可能错误地引入"个体间共享关节加速度"之类不存在的耦合。 - 根本原因：忽略了团队动力学的**块对角结构**——各个体没有物理连接（不共享关节/base），只通过物体接触力松散耦合。统一的是描述语言，不是物理实体。 - 正确做法：始终保持块对角 + 稀疏耦合的结构意识。统一 QP 的质量矩阵必须是块对角的，耦合只出现在物体动力学和闭链约束行。这个结构是分布式分解的前提，不能破坏。

陷阱 2（概念误区）：闭链约束只在位置层加、不在加速度/力层加。 - 错误描述：whole-body QP 在加速度层工作，但闭链约束只写了位置层 \(x_{\text{ee},k}=T_k(x_o)\)，没微分到加速度层。 - 现象/后果：QP 看不到闭链约束（它只认加速度层的等式），各体末端加速度互不协调，物体被拉扯——位置层约束"够不着"加速度层的求解。 - 根本原因：whole-body QP 的决策变量是加速度/力，约束必须表达在同一层级。位置约束要微分两次到加速度层（\(\ddot x_{\text{ee},k}=\ddot{(T_kx_o)}\)），力约束要在力平衡层。 - 正确做法：把闭链约束微分到与 QP 决策变量一致的层级（通常加速度层），并加 Baumgarte 稳定项（\(+\,2\zeta\omega\,\dot e + \omega^2 e\)）抑制位置层漂移。这与单体 WBC 处理接触约束的方式一致。

陷阱 3（编程陷阱）：堆叠不同维度的个体时块尺寸写死。 - 错误描述：实现团队 QP 时，假设所有个体自由度相同（如都按 12 写），用固定块尺寸堆叠矩阵。 - 现象/后果：人形（25 自由度）的块被截断或错位，矩阵维度对不上，要么崩溃要么静默地算错（最危险）。 - 根本原因：异构团队的核心特征就是 \(n_k\) 不同，固定块尺寸是把同构假设偷偷塞进了代码。 - 正确做法：用**按个体可变的块尺寸**组装稀疏矩阵——维护每个个体的变量索引区间 [offset_k, offset_k + 6 + n_k)，按区间填充对应块。用稀疏矩阵库（Eigen Sparse / OSQP 的稀疏接口），既正确处理异构维度又利用块对角稀疏性。写断言检查总维度 \(=\sum_k(6+n_k)+6\)。

练习¶

（推导题） 对本章三体场景（人形 \(n_1=25\)、四足 \(n_2=n_3=12\)），写出团队 whole-body QP 的变量总维度（含各体广义加速度 + 关节力矩 + 环境接触力 + 操作力 + 物体加速度，自行设定各接触点数）。画出团队质量矩阵的块结构示意图，标出哪些块是对角块（各体本地）、哪些项是耦合项（操作力/闭链）。说明这个稀疏结构如何对应 ADMM 的"本地子问题 + 共识"切分。
（设计题） 把正文的分层任务栈（P0-P4）实例化为一个具体的加权 QP（软优先级，用大权重近似硬优先级）或分层 QP（严格零空间投影）。讨论两种实现的取舍：软优先级（单个 QP、可能违反高优先级）vs 分层（多个 QP 级联、严格优先但计算量大）。对"保人形平衡 P1 > 搬茶几 P2"这个关键优先级，你选哪种？为什么？
（综合思辨题） 本节主张"统一描述 + 分布式求解"。设计一个实验对比三种组合在你的三体抬桌任务上的表现：(a) 统一描述 + 集中求解（一个大 QP）；(b) 统一描述 + 分布式求解（ADMM 分解）；(c) 不统一 + 各自独立 + 阻抗协调（范式 A）。从协调精度、对单点失效的鲁棒性、通信量、计算延迟四个维度预测各自表现，并指出每种组合最适合的部署场景。

§9.5 双智能体协调——RoboDuet 与 FALCON 精读 ⭐⭐⭐¶

这一节解决什么问题：前四节都是**模型驱动**的协调（手工设计抓取矩阵、ADMM、whole-body QP）。但单体 loco-manipulation 的前沿（RoboDuet、FALCON）走了另一条路——学习驱动：用两个**策略/智能体**分别管"运动"和"操作"，让它们通过相互引导自动协调。这一节精读这两个工作，并揭示一个深刻的镜像关系——RoboDuet/FALCON 的"体内双智能体协调"，与本章前四节的"体间多智能体协调"，是同一个协调思想在两个尺度上的表现。理解这个镜像，你对"协调"的认识会上一个台阶。

动机：单体内部也有"协调"问题¶

前四节我们一直在讲"多个机器人之间"的协调。但请注意一个常被忽略的事实：一个单独的 loco-manipulator（如四足+臂、人形），其内部本身就有一个协调问题——

考虑 RoboDuet 处理的单体四足+臂：四足负责移动（locomotion），臂负责操作（manipulation）。当机器人边走边伸臂够取目标时，腿和臂必须协调：臂前伸会改变质心、给身体一个反力矩，腿必须调整姿态来补偿；反过来，腿走得颠簸会扰动臂末端，臂要补偿这个扰动才能稳定够取。这个"腿-臂协调"和本章"四足-人形协调"在结构上惊人地相似——都是**两个能力不同的子系统，必须协调它们对一个共同目标（whole-body 行为/物体）的贡献**。

多视角理解（A）——体内协调与体间协调的镜像。把单体 loco-manipulator 的"腿+臂"和异构团队的"四足+人形"并排看： - 像的部分：都是两个（或多个）能力互补、动力学不同**的部分，必须协调对共同目标的贡献；都面临"一个部分的动作给另一个部分制造扰动/反力"的核心矛盾（臂前伸扰动腿的平衡 ↔ 人形操作反力扰动四足平衡）；协调的本质都是"让一方把另一方的动作作为已知信息来补偿"。 - **不像的部分：体内协调的两部分**共享一个 base、有刚性连接**（腿和臂通过身体刚性相连），耦合是**强且即时**的（关节力矩瞬间传递）；体间协调的多体**没有刚性连接**，只通过物体接触松散耦合，耦合**弱且有延迟**（要通过物体力/通信传递）。所以体内协调可以用一个共享的 whole-body 控制器，体间协调必须分布式。

不要把这个类比延伸过头：体内协调的紧耦合让它能用"一个网络/一个 QP"，体间协调的松耦合迫使它分布式——这是两者方法选择分道扬镳的根本原因。但**协调的思想内核（互相把对方动作当已知来补偿）是相通的**。

这个镜像不是文字游戏——它意味着**单体 loco-manipulation 的协调方法（RoboDuet/FALCON）和多体协调方法（本章前四节）可以互相借鉴**。下面精读两个工作，你会看到它们的核心机制如何映射到本章的概念。

RoboDuet：两个协作策略实现单体 whole-body loco-manipulation¶

问题。RoboDuet（Pan 等, RA-L 2024）要解决的是：让一个四足+臂机器人**边走边操作**（whole-body loco-manipulation）——既要稳健移动，又要臂末端精确够取/操作目标。难点是"动"和"操作"互相干扰，单一策略难以同时学好两者。

核心方法——双策略协作。RoboDuet 不用一个端到端的 whole-body 策略，而是用**两个协作的策略**：

运动策略 \(\pi_{\text{loco}}\)（loco policy）：负责四足的 locomotion——保持平衡、跟踪 base 的速度/姿态指令。
臂策略 \(\pi_{\text{arm}}\)（arm policy）：负责臂的操作——跟踪末端执行器的目标位姿。

关键在于两者的**相互引导（mutual guidance）**机制：

\[ \boxed{\ \pi_{\text{loco}} \xrightarrow{\text{提供 base 姿态/运动}} \pi_{\text{arm}},\qquad \pi_{\text{arm}} \xrightarrow{\text{臂动作作为引导}} \pi_{\text{loco}}\ } \]

具体地：运动策略把臂的动作（或臂的目标）作为输入，据此调整身体姿态——比如臂要够取一个高处目标，运动策略主动让身体后仰/调整重心来扩大臂的可达空间并补偿反力矩。反过来，臂策略把运动策略产生的 base 运动**作为已知**，在臂控制中补偿这个 base 扰动，从而即使身体在动，末端仍能稳定跟踪目标。两个策略协同成一个**whole-body 控制器**。

理论-工程桥接（D）——RoboDuet 的相互引导 = 本章范式的学习版。把 RoboDuet 的机制映射到本章：运动策略把臂动作作为输入来补偿反力，正是 §9.3 陷阱 2 / §9.4 P1 说的"把操作反力前馈补偿进平衡控制"——只不过 RoboDuet 是**学**出这个补偿，本章前四节是**算**出这个补偿（通过动力学模型显式计算反力）。臂策略把 base 运动作为已知来补偿，正是"把另一子系统的动作作为已知扰动"的协调思想。RoboDuet 用 RL 学到了本章用模型推导的同一类协调律。

两阶段训练。RoboDuet 的训练分两步（这个设计本身很有启发）： 1. 阶段 1：先单独训练运动策略 \(\pi_{\text{loco}}\)，让四足获得基本移动能力（不管臂）。 2. 阶段 2：固定（或微调）运动策略，训练臂策略 \(\pi_{\text{arm}}\) 学会与运动策略协调——此时运动策略已稳定，给臂策略提供一个可靠的"运动基础"，训练更稳定。

跨本体零样本迁移。RoboDuet 的一个亮点：双策略框架支持**跨四足本体的零样本迁移**——在一种四足上训练的策略，能零样本迁移到形态/尺寸相近的另一种四足。原因是双策略的解耦让"运动"和"操作"各自相对独立于具体本体细节，运动策略封装了本体相关的部分，臂策略相对本体无关。性能上 RoboDuet 在挑战性 loco-manipulation 任务上比基线**成功率提升约 23%**。

本质洞察——解耦把"本体相关"和"本体无关"分到了不同策略。RoboDuet 能跨本体迁移，根子在解耦切对了地方：它把"与具体四足本体强相关的部分"（腿怎么动、步态怎么打——运动策略）和"相对本体无关的部分"（末端去哪、怎么稳定跟踪——臂策略）分到了两个策略里。换一个尺寸相近的四足，只有运动策略那部分需要适应，臂策略几乎不动——而运动策略封装的本体差异又小（相近四足的腿运动学接近）。对照本章 §9.1 的形态向量思路：这等于把策略沿"形态敏感度"做了切分——高形态敏感的逻辑归一个模块、低敏感的归另一个。这给体间多机协调一个迁移启示（§9.5 实践启示已提）：若想让异构团队的协调策略可迁移，也该把"形态特定"（各体怎么发力）和"协调通用"（物体轨迹怎么共识）分开——前者随个体换，后者复用。这正是 §9.4"统一描述（通用） + 各体本地子问题（特定）"在学习侧的镜像。

对比性思维（B）——双策略 vs 单一 whole-body 策略。为什么不用一个端到端的 whole-body 策略直接学"腿+臂"？RoboDuet 的反事实回答：单一策略要同时学习高维耦合的"动+操作"，探索空间巨大、奖励信号冲突（移动奖励和操作奖励互相干扰），训练难收敛、易学到平庸的折中。双策略把问题**解耦**——各自有清晰的奖励、各自的探索空间小，再通过相互引导学协调。这与 §9.4"统一描述但分布式求解"异曲同工：不是不要协调，而是**先解耦再协调**比"一锅煮"更可学/可解。这是贯穿单体和多体的深刻设计原则。

FALCON：双智能体实现人形力自适应 loco-manipulation¶

问题。FALCON（Zhang 等, 2025）瞄准更难的目标——人形**机器人的力自适应 loco-manipulation：人形要边走边用末端执行器与环境进行 **3D 力交互（推、拉、按压重物），同时保持双足平衡。这比 RoboDuet 的四足更难，因为人形的双足动态平衡远比四足脆弱，且要显式处理**末端外力**。FALCON 明确指出此前方法多局限于轻量任务或四足/轮式平台，人形的精确鲁棒 whole-body 力交互仍是难题。

核心方法——双智能体解耦 + 力自适应。FALCON 把 whole-body 控制分解为两个专门的智能体：

下肢智能体（lower-body agent）：在外部力扰动下保证**稳定的 locomotion**——这是人形的命门，必须扛住上肢操作传来的反力。
上肢智能体（upper-body agent）：精确跟踪末端执行器位置，并带**隐式自适应力补偿（implicit adaptive force compensation）**——在跟踪位置的同时适应所需的交互力。

两个智能体**联合训练**，各自有独立的奖励（解耦奖励让训练高效），但通过共享的 whole-body 物理耦合协调。

力课程（force curriculum）——FALCON 的关键创新。为了让人形学会承受末端力交互，FALCON 用一个**渐进的力课程**训练：

\[ \boxed{\ F_{\text{ext}}^{\text{train}}(t) = \text{curriculum}(t):\ \text{从 0 逐步增大施加在末端的外力幅值，且始终尊重力矩极限}\ } \]

训练时，逐步增大施加在末端执行器上的外力 \(F_{\text{ext}}\) 的幅度——从无力开始，慢慢加大，让下肢智能体逐渐学会在越来越大的力扰动下保持平衡，上肢智能体逐渐学会施加越来越大的交互力。课程式训练避免了"一上来就大力扰动导致策略崩溃"，且全程尊重关节力矩极限（不学出物理上做不到的策略）。

本质洞察——FALCON 的力课程 = 本章异构平衡约束的学习版。回顾 §9.3 陷阱 2 / §9.4 P1：异构团队里，脆弱平衡的人形能承受的操作反力有一个**动态上限** \(\bar F^{\text{balance}}\)，超过就倾倒，本章用显式约束（内力 QP 里的侧向力上限）来尊重它。FALCON 的力课程做的是**同一件事的学习版**——它不显式写出 \(\bar F^{\text{balance}}\)，而是通过课程式增大外力，让策略**隐式地学到**"我能承受多大力、超过会摔"，并学会在力上限内稳定操作。模型方法显式算上限，学习方法隐式学上限——殊途同归。这就是为什么把 FALCON 放进本章：它的下肢智能体所做的"扛住上肢反力保平衡"，正是本章"操作反力补偿"的 RL 实现。

深入：双智能体的观测-动作接口设计¶

RoboDuet 和 FALCON 都把 whole-body 拆成两个智能体，但**两个智能体如何交换信息**（接口设计）才是协调的关键。这里剖析这个接口，因为它直接对应本章前四节"协调信息怎么传"的核心问题。

考虑 FALCON 的两个智能体的观测-动作设计（按论文思路重构，具体维度因实现而异）：

下肢智能体（lower-body agent）：
  观测 o_lower = [ 本体感知（关节角/速度、IMU 姿态/角速度）,
                  base 速度指令,
                  ★ 上肢动作/末端目标（来自上肢智能体）,
                  ★ 估计的末端外力 F_ext（操作反力，作为已知扰动）]
  动作 a_lower = 下肢关节目标（腿 + 可能含腰）
  奖励 r_lower = 速度跟踪 + 存活（不倒）+ 能量 - 失衡惩罚

上肢智能体（upper-body agent）：
  观测 o_upper = [ 本体感知（臂关节角/速度）,
                  末端目标位姿,
                  ★ base 当前运动状态（来自下肢智能体/状态估计）]
  动作 a_upper = 上肢关节目标（臂）
  奖励 r_upper = 末端位置跟踪 + 隐式力适应 - 关节限位惩罚

打 ★ 的观测项就是**协调接口**——它们是一个智能体"看到"另一个智能体状态/动作的通道： - 下肢智能体看到**上肢动作 + 末端外力**，于是能**预判并补偿**操作反力对平衡的影响（这正是 §9.6 人形 whole-body 那行 external_force=reaction_on_hand 的学习版）。 - 上肢智能体看到 base 运动状态，于是能在身体晃动时仍稳定跟踪末端目标（补偿 base 扰动）。

理论-工程桥接（D）——观测里的 ★ 项 = 本章的耦合变量。把这个接口和本章前四节对照：FALCON 下肢智能体观测里的"末端外力 \(F_{\text{ext}}\)"，正是 §9.3-B 内力分配 QP 里人形要承受的操作反力；FALCON 上肢观测里的"base 运动状态"，正是 §9.3-C ADMM 里要协调一致的运动信息。模型方法把这些耦合变量写进约束（显式），学习方法把它们塞进观测（隐式让网络学会用）——同样的耦合信息，两种利用方式。理解这一点，你设计 MARL 协调时就知道"观测里该放什么"：放那些在模型方法里作为耦合约束出现的量（对方的动作、共享物体的状态、相互的作用力）。

对比性思维（B）——共享观测 vs 共享约束。体内双智能体靠**共享观测**协调（一方把另一方的状态/动作放进自己的观测）；本章体间多机靠**共享约束/共识变量**协调（闭链约束、物体轨迹一致性）。为什么不同？因为体内两智能体在**同一台机器人上**，状态零延迟可得（直接读关节编码器），共享观测廉价；体间多机的状态要**通信**（有延迟、会丢包），共享完整观测昂贵且不可靠，所以只共享**低维的共识变量**（物体轨迹），且用 ADMM 这类对延迟鲁棒的机制。这个差异（共享观测 vs 共享约束）是体内/体间协调在**信息接口**上的根本分野，和 §9.5 陷阱 1 说的"耦合强度差异"是一枚硬币的两面。

深入：双智能体训练的工程细节与典型失败模式¶

上面讲清了双智能体"协调什么"（接口里的 ★ 项）。但 RoboDuet/FALCON 真正难复现的部分在**怎么训出来**——双智能体联合训练有一组特有的失败模式，理解它们既能帮你复现这两个工作，也能让你看清"先解耦再协调"这个思想在学习侧要付出什么代价。这一小节把训练的工程细节拆开讲，它和本章前四节"模型方法怎么保证协调正确"形成对照：模型方法用约束保证正确，学习方法用**奖励设计 + 课程调度 + 训练顺序**逼出正确。

失败模式一：奖励冲突导致的"协调懒惰"。 双智能体各有独立奖励（下肢求存活+速度跟踪，上肢求末端跟踪），听起来解耦得很干净。但它们共享同一个物理身体——下肢为了"存活"可能学会一个保守策略：只要上肢一伸出去就立刻蹲下/收住身体，把质心压低求稳。这样下肢奖励很高（没摔），但上肢永远够不到远处目标（被下肢的保守姿态拖住）。反过来，上肢为了"够到目标"可能学会猛地甩臂，把下肢甩得踉跄。

这是**多目标 RL 的经典病**：两个奖励在共享状态上隐式竞争，各自的局部最优合起来是系统的差解。
根本原因：解耦的是**奖励信号和动作空间**，但**物理耦合没解耦**（共享 base、质心、接触）。奖励上的"各自为政"撞上物理上的"必须协同"。
缓解手段（RoboDuet/FALCON 都用了某种形式）：(1) 给两个奖励都加一个**共享的 whole-body 项**（如整体质心稳定、整体任务完成），把"系统好"显式写进各自奖励，避免纯局部优化；(2) 用**课程**先让一方稳定再引入另一方（见下）；(3) 奖励量纲要平衡——若存活奖励远大于跟踪奖励，策略会退化成"只求不摔、放弃操作"。

失败模式二：力课程的调度太快或太慢。 FALCON 的力课程是"从 0 逐步增大末端外力"，但**增大的速度**是个微妙的超参数：

\[ F_{\text{ext}}^{\max}(t) = F_0 + (F_{\text{target}} - F_0)\cdot \min\!\big(1,\ \text{clip}(\bar r - r_{\text{thresh}},\,0,\,1)\big) \]

一种常见做法是把课程进度**绑定到当前策略的表现** \(\bar r\)（性能驱动课程）：只有当策略在当前力水平上稳定达标（平均回报 \(\bar r\) 超过阈值 \(r_{\text{thresh}}\)）才提高 \(F_{\text{ext}}^{\max}\)，而非按固定时间表硬涨。

涨太快：策略还没学会在当前力下站稳，外力就加大了——下肢智能体反复被推倒，回报塌缩，训练发散（这正是"一上来就大力扰动导致策略崩溃"，FALCON 用课程规避的就是它）。
涨太慢：训练后期一直在小力区间打转，样本浪费，最终力上限学不到目标值，部署时遇到大负载就失稳。
正确做法：性能驱动的自适应课程（达标才升级），并对"升级步长"和"达标阈值"做小范围网格搜。这与第 10-12 章 MARL 的课程学习一脉相承（§9.7 前向预告）。

本质洞察——课程是把"难约束"拆成一串"易约束"。模型方法里，人形的平衡承受上限 \(\bar F^{\text{balance}}\) 是一个**硬约束**，求解器一次性必须满足。学习方法没有这个硬约束可写，于是用力课程把它**沿时间轴拆成一串逐渐收紧的软目标**——每个训练阶段只要求策略承受比上阶段略大的力。课程的本质，是用"一连串容易的子问题"逼近"一个困难的约束满足问题"。这个视角能让你迁移：任何模型方法里"一步到位的硬约束"，在学习侧往往都能改写成"沿某个维度渐进收紧的课程"（力、速度、地形难度、负载……）。反过来，若一个学习任务怎么都训不出来，先问：有没有一个硬约束被一步到位地要求了，能不能拆成课程？

失败模式三：训练顺序错配——先训协调、后训基础。 RoboDuet 的两阶段训练（先单独训运动策略，再固定/微调它训臂策略）不是随意的——顺序反了会失败。若先训臂策略（此时运动策略还是随机的），臂策略面对的是一个**剧烈乱动的 base**，它学到的"补偿"是针对随机噪声的，毫无意义；等运动策略后来稳定了，臂策略学到的补偿全部失效。

根本原因：双智能体里存在**依赖的方向性**——上肢的稳定操作**依赖**一个可靠的运动基础，反之依赖弱。有方向性依赖时，先训被依赖方（运动），后训依赖方（操作）。
这对应本章一个更普适的原则：协调有层次时，先固化下层、再学上层。和 §9.4 分层任务栈（P1 平衡 > P2 操作）同构——平衡是被操作依赖的基础，所以无论是约束优先级还是训练顺序，平衡都在前。
工程提示：阶段 2 微调运动策略时学习率要小（否则会破坏阶段 1 学到的稳定运动），或干脆冻结运动策略只训臂——RoboDuet 的"固定或微调"留了这个选择。

失败模式四：双智能体的 sim2real——比单策略更敏感的接口噪声。 §9.7 会讲整章的 sim2real，这里只点双智能体特有的一条：两个智能体通过 ★ 接口交换信息（如下肢观测里的"末端外力估计 \(F_{\text{ext}}\)"），而**这个估计在实机上是带噪的**（力/力矩传感器噪声、估计延迟）。仿真里 \(F_{\text{ext}}\) 往往是精确真值，策略学会了信任它；上实机，带噪的 \(F_{\text{ext}}\) 让下肢的反力补偿出错，平衡变差。

缓解：训练时就给接口量注入**噪声和延迟随机化**（domain randomization 不只随机化物理参数，也要随机化**智能体间通信的质量**），让策略学会在带噪接口下仍稳健协调。
这条对体间多机更是雪上加霜——体间的接口是**真·通信**（延迟更大、会丢包），所以本章前四节才偏好 ADMM 这类对延迟/丢包鲁棒的机制，而非依赖瞬时精确的状态共享（§9.5 陷阱 1、上面"共享观测 vs 共享约束"）。

理论-工程桥接（D）——四个失败模式各对应一个本章概念。把这四条收束回本章的模型视角：模式一（奖励冲突）↔ §9.4 分层任务栈解决的"优先级冲突"（模型方法用优先级显式裁决，学习方法用共享奖励隐式平衡）；模式二（力课程调度）↔ §9.3-B 内力 QP 的侧向力上限（一次性硬约束 vs 渐进软目标）；模式三（训练顺序）↔ §9.4 P1>P2 的优先级层次（依赖方向性）；模式四（接口噪声）↔ §9.5 共享观测 vs 共享约束的延迟鲁棒性。学习方法没有消灭这些协调难题，只是把它们从"约束设计"挪到了"训练设计"——这恰恰说明为什么第 12 章要把两者缝起来：模型方法提供约束骨架（让学习不必从零摸索这些难题），学习方法提供难建模因素的补偿。

陷阱 3（编程陷阱）：双智能体训练时两个策略用了不同的仿真步/控制频率。 - 错误描述：实现双智能体时，下肢策略按 50Hz 决策、上肢策略按 100Hz 决策（或反之），却共用一个物理仿真步，接口数据在两个频率间直接对齐。 - 现象/后果：高频策略读到的是低频策略**过期的动作/状态**，接口信息错位，协调质量在训练里就上不去；更隐蔽的是仿真步与策略步不整除时，外力/接触在子步里被重复或遗漏施加，物理失真，策略学到错误的动力学。 - 根本原因：双智能体的协调依赖接口数据的**时间一致性**——两个策略看到的必须是同一时刻（或已知时延）的对方状态。频率错配 + 不整除的仿真步打破了这个一致性。 - 正确做法：明确两个策略的决策频率关系（同频最简单；若必须异频，让一个是另一个的整数倍，并显式做零阶保持），仿真步要能被两者整除，接口数据带时间戳对齐。把"接口延迟"作为一个**显式建模量**（而非隐式的实现 bug）——这样它既能在训练里被随机化（应对模式四），又能在分析里被推理。

体内双智能体 ↔ 体间多智能体：完整映射¶

现在把 RoboDuet/FALCON 的体内协调与本章前四节的体间协调做一张完整映射表——这是本节的核心收获：

协调维度	体内（RoboDuet/FALCON）	体间（本章 §9.1-9.4）
被协调的单元	一个机器人的运动子系统 + 操作子系统	多个机器人（人形/四足/臂）
协调目标	whole-body 行为（边走边操作）	共抓物体的协同操作
耦合方式	刚性连接（共享 base、关节力矩瞬传）	物体接触力（松耦合、有延迟）
核心矛盾	操作扰动平衡（臂反力拽动腿/双足）	操作反力拽倒脆弱个体（人形）
协调机制	相互引导（一方动作作为另一方输入）	抓取矩阵/ADMM/阻抗（§9.3 三范式）
平衡-操作权衡	力课程隐式学（FALCON）/下肢优先	分层任务栈 P1>P2 显式算（§9.4）
方法范式	学习驱动（RL，可处理难建模接触）	模型驱动（可解释、可验证）
解耦思想	双策略/双智能体解耦再协调	块对角动力学 + 分布式求解（§9.4）
可扩展性	固定两部分（腿+臂）	任意 \(K\) 个异构体

系统性分类（E）——协调是一个跨尺度的统一概念。从这张表看，"协调"贯穿了三个尺度：①**子系统级**（一个机器人的腿+臂，RoboDuet/FALCON）；②**多体级**（几个机器人共抬物体，本章前四节）；③（往上还有）群体级（成百上千个体的蜂群，第 1-3 章）。三个尺度的协调，核心矛盾都是"管理多个能力不同的单元对共同目标的贡献 + 处理它们之间的相互扰动"，区别只在**耦合强度**（刚性→接触→通信）和**单元数量**（2→几个→上千），这决定了用集中/分布、模型/学习。理解这个跨尺度统一性，你就不会把"单体 loco-manipulation"和"多机协作"当成两门不相干的课——它们是同一个协调问题在不同尺度的展开。本章正是这两个尺度的**接合处**。

对本章的实践启示。RoboDuet/FALCON 的"先解耦再协调 + 平衡优先 + 课程式训练"思想，可以直接用到异构团队： - 先解耦再协调：呼应 §9.4 的"统一描述 + 分布式求解"——不要一锅煮。 - 平衡优先：呼应 §9.4 分层任务栈 P1>P2——脆弱个体的平衡是高优先级。 - 学习驱动协调可处理难建模因素：本章前四节的模型方法对接触/摩擦建模敏感（§9.7 会讲这是主要 sim2real gap），而 FALCON 式的学习方法能隐式吸收这些——这预告了第 10-12 章 MARL 方法的价值。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：把"体内双智能体"和"体间多机"混为一谈，以为方法可以直接搬。 - 错误描述：看到 RoboDuet 双策略协调单体腿臂效果好，就想直接用同样的"两个策略 + 相互引导"去协调两个独立机器人共抬物体。 - 现象/后果：直接搬会失败——RoboDuet 的相互引导假设两部分**共享 base、刚性连接、状态瞬时互通**（腿臂在同一台机器人上，观测/动作零延迟共享）。两个独立机器人之间没有这些条件（无刚性连接、状态要通信、有延迟），相互引导的紧耦合假设破裂。 - 根本原因：体内（刚性强耦合）和体间（接触松耦合）的物理耦合强度差一个数量级，紧耦合方法不能跨尺度直接迁移。 - 正确做法：迁移**思想**（先解耦再协调、平衡优先），但**机制**要按尺度重新设计——体间协调必须用分布式机制（ADMM/阻抗/通信），不能假设状态瞬时互通。映射表里"耦合方式"那一行就是迁移的边界。

陷阱 2（思维陷阱）：以为学习方法（RoboDuet/FALCON）能完全替代模型方法。 - 错误描述：看到 FALCON 不用显式建模就能力自适应，就认为本章前四节的抓取矩阵/whole-body QP 都过时了，直接上 RL 即可。 - 现象/后果：纯学习方法**缺乏安全保证和可解释性**——你无法证明它不会在某个未见状态下产生危险的内力或失稳，难以验证、难以认证，在安全关键场景（搬重物砸到人）风险高。且纯学习的多体协调当前规模和泛化仍受限。 - 根本原因：模型方法（可解释、可验证、有约束保证）和学习方法（处理难建模因素、免精确模型）各有不可替代的长处。 - 正确做法：两者互补而非替代。模型方法做安全骨架（硬约束保证不撞不翻不超力），学习方法做难建模部分的补偿（接触/摩擦自适应）。这正是第 12 章（§130 MARL+规控混合）和 CBF 安全层的主题——本章是模型方法的集大成，第 10-12 章引入学习，第 12 章把两者缝起来。

练习¶

（精读题） 阅读 RoboDuet 论文（arXiv:2403.17367）的方法部分，画出两个策略 \(\pi_{\text{loco}}, \pi_{\text{arm}}\) 的观测空间、动作空间、以及"相互引导"具体通过哪些量传递（哪些信息从 loco 流向 arm、哪些反向）。然后对照本章 §9.4 分层任务栈，标注 RoboDuet 隐式实现了哪些优先级（P1 平衡？P2 任务？）。
（对照题） FALCON 用"力课程"让人形隐式学到平衡承受的力上限；本章 §9.3-B 用内力 QP 的显式侧向力约束 \(\bar F^{\text{balance}}\) 来尊重这个上限。设计一个对照实验思路：在同一个人形力交互任务上，分别用（a）FALCON 式学习方法、（b）本章显式约束方法，对比它们在"已见力 / 未见超大力 / 突变力"三种测试下的表现，预测各自的成功与失效模式。
（设计扩展题） 把 RoboDuet 的"双策略 + 两阶段训练"思想扩展到本章的异构团队：设计一个"分层 MARL"方案——每个异构体内部用 RoboDuet 式双策略（loco+arm），体间用一个高层协调策略。讨论这个三层结构（体内双策略 × 体间协调策略）的训练顺序、观测设计、以及它如何对应本章 §9.4 的"统一描述 + 分布式求解"。这道题为你过渡到第 10-12 章 MARL 铺路。

§9.6 端到端场景串讲——人形+四足×2 协同抬桌 ⭐⭐⭐¶

这一节解决什么问题：前五节的工具（能力分配、三范式协调、统一框架、双智能体）是分开讲的。这一节把它们**串成一条完整的流水线**——从"客厅里有张茶几要搬"到各关节电流输出，走一遍人形+四足×2 协同抬桌的全过程。这既是对全章的综合，也是本章累积项目的核心模块。

场景设定¶

回到贯穿全章的客厅场景，精确化：

团队：人形 \(r_1\)（Unitree G1 类，\(n_1=23\)，灵巧手，双足动态平衡，侧向力承受弱）、四足 \(r_2,r_3\)（Unitree Go2 + 背载平台，\(n=12\)，承重强，四足支撑稳）。
物体：实木茶几，质量 \(m_o=18\)kg，长方形 \(1.0\times0.6\)m，三个抓取点（人形抓一长边中点用手压住、两台四足各用背顶住另一长边的两端）。
任务：把茶几从 A 处平移到 B 处（距离 3m），途中绕过地上一个玩具（绕障），保持茶几水平（防止上面万一有东西滑落），全程不撞墙、不让任何个体失稳、不让茶几被内力损坏。

完整流水线（六个阶段）¶

整个协同抬桌任务，是本章六节工具的级联。下面给出分层架构和每层职责——注意它和骨架保留的"Level 3→0 层次化架构"以及第 8 章 De Vincenti 多足搬运的对应关系。

┌─────────────────────────────────────────────────────────────────┐
│ 阶段 1：任务分配（§9.2，~0.1 Hz，事件触发）                          │
│   输入：任务描述"搬茶几从 A 到 B，绕玩具"+ 团队能力画像              │
│   方法：形态感知 CBBA → 联盟任务拆分（抬桌=2 子任务）+ 能力掩码      │
│   输出：角色指派 —— r1:辅助扶+保平衡, r2:抬左端(领导), r3:抬右端     │
├─────────────────────────────────────────────────────────────────┤
│ 阶段 2：角色与抓取规划（§9.3，任务级）                               │
│   输入：角色指派 + 茶几几何                                         │
│   方法：确定各接触点位置 r_i → 构建统一抓取矩阵 G → 选协调范式        │
│   输出：抓取矩阵 G、各接触点 frame、协调范式选择（这里选 C+A 混合）   │
├─────────────────────────────────────────────────────────────────┤
│ 阶段 3：物体轨迹规划 + 一致性协调（§9.3-C/§9.4，~20-50 Hz）          │
│   输入：A→B 路径（绕玩具）、茶几当前状态                            │
│   方法：分布式 MPC（ADMM）—— 各体本地 whole-body MPC + 物体轨迹共识 │
│   输出：协调一致的物体轨迹 x_o(t) + 各体末端参考 + 各体 base 参考     │
├─────────────────────────────────────────────────────────────────┤
│ 阶段 4：内力分配（§9.3-B，~50-100 Hz）                              │
│   输入：物体期望合力旋量 w_o^d（由 x_o(t) 算）、各体力/平衡上限       │
│   方法：内力分配 QP —— G^+ w_o^d + N_G η，异构加权 + 人形侧向约束     │
│   输出：各接触点期望力 f_i（含操作力 + 内力）                        │
├─────────────────────────────────────────────────────────────────┤
│ 阶段 5：各体 whole-body 跟踪 + 平衡补偿（§9.4，~200-500 Hz）         │
│   输入：本体末端力目标 f_i + base 参考 + 操作反力（已知扰动）         │
│   方法：各体 whole-body QP（分层 P0-P4，平衡 P1 > 任务 P2）          │
│         人形：把 f_1 反力前馈进平衡约束（RoboDuet/FALCON 思想）       │
│   输出：各体期望关节力矩 τ_k                                        │
├─────────────────────────────────────────────────────────────────┤
│ 阶段 6：关节力矩控制（~1 kHz）                                       │
│   输入：期望关节力矩 τ_k                                            │
│   方法：关节力矩/电流环（PD + 前馈）                                 │
│   输出：电机电流                                                    │
└─────────────────────────────────────────────────────────────────┘

底层安全垫（贯穿）：范式 A 阻抗 —— 跟随者(r1)末端始终叠加柔顺阻抗，
                   任何瞬间内力爆炸先被阻抗软化，再由上层 ADMM/内力 QP 纠正。

注意这个架构的几个设计决策（都来自前五节的结论）：

频率分层：分配最慢（事件触发）、ADMM 协调中频（MPC 层，§9.3 陷阱 4 要求 ADMM 在中频跑足轮数而非 1kHz 单轮）、whole-body 跟踪高频、关节环最快。这是 loco-manipulation 的标准分层（呼应骨架的 Level 3→0）。
范式混合：选了 C（ADMM 协调物体轨迹，鲁棒+精确）+ A（阻抗安全垫，防瞬间内力爆炸）+ B（内力分配 QP，精确控制夹紧力）。三范式不是三选一，而是各司其职（§9.3 系统性分类的兑现）。
人形特殊处理：人形作**跟随者**（不当领导者，因为它平衡脆弱、不宜主导），且它的 whole-body 跟踪里把操作反力前馈补偿进平衡约束（FALCON/RoboDuet 思想）。

关键环节的伪代码¶

下面给出阶段 4（内力分配）和阶段 5（人形平衡补偿）的伪代码——这两个是异构特性最集中的环节。其余环节（CBBA、ADMM）直接复用前几节和第 3、4 章的实现。

阶段 4：异构内力分配 QP

import numpy as np
from scipy.optimize import minimize  # 教学用；工程用 OSQP/qpOASES

def allocate_contact_forces(G, w_obj_des, contacts, balance_limits):
    """
    异构内力分配：把物体期望合力旋量分解为各接触力。
    G            : (6, 3K) 统一抓取矩阵（所有 r_i 在物体质心系，§9.3-B）
    w_obj_des    : (6,) 物体期望合力旋量（含抵消重力 + 期望加速度）
    contacts     : 各接触点的 {mu(摩擦系数), lambda_max(力上限), is_humanoid}
    balance_limits: 人形当前侧向力上限 F_lat_max（从人形平衡控制器实时反馈）
    返回         : f (3K,) 各接触点力（操作力 + 内力）
    """
    K = len(contacts)
    G_pinv = G.T @ np.linalg.inv(G @ G.T)      # 伪逆：最小范数操作力
    f_particular = G_pinv @ w_obj_des           # 特解（操作力，落在行空间）
    N = null_space(G)                            # 零空间基 (3K, 3K-6)，内力方向

    # 异构加权：力能力弱的接触点权重大（让它少出力）—— §9.3-B 核心
    W = np.diag(np.concatenate([
        np.full(3, 1.0 / c['lambda_max']) for c in contacts
    ]))

    def cost(eta):
        f = f_particular + N @ eta               # 通解 = 特解 + 零空间内力
        return f @ W @ f                          # 加权接触力范数

    def constraints(eta):
        f = f_particular + N @ eta
        cons = []
        for i, c in enumerate(contacts):
            fi = f[3*i:3*i+3]
            f_normal = fi[2]                      # 假设 z 为法向（接触面坐标系）
            f_tangent = np.linalg.norm(fi[:2])
            cons.append(c['mu'] * f_normal - f_tangent)   # 摩擦锥：>=0
            cons.append(f_normal - F_MIN)                  # 最小夹紧力：>=0
            if c['is_humanoid']:
                # 异构关键约束：人形侧向力不超过当前平衡承受上限
                cons.append(balance_limits - f_tangent)    # >=0
        return np.array(cons)

    res = minimize(cost, x0=np.zeros(N.shape[1]),
                   constraints={'type': 'ineq', 'fun': constraints})
    return f_particular + N @ res.x

这段代码的异构内核就两处：(1) 加权矩阵 W 让人形少出力；(2) is_humanoid 分支加的侧向力约束 balance_limits - f_tangent >= 0——这正是 §9.3 陷阱 2 的根治，且 balance_limits 来自人形平衡控制器**实时反馈**（动态变化）。

阶段 5：人形 whole-body 跟踪中的操作反力前馈补偿

def humanoid_wbc(q, v, ee_force_target, base_ref, contact_state):
    """
    人形 whole-body QP，关键是把操作反力作为已知扰动前馈进平衡约束。
    ee_force_target : 阶段4分配给人形手的力 f_1（人形施加给茶几的力）
    返回            : 期望关节力矩 tau
    """
    # 牛顿第三定律：茶几给人形手的反力 = -人形施加给茶几的力
    reaction_on_hand = -ee_force_target          # 已知扰动！(RoboDuet/FALCON 思想)

    # 分层任务栈（§9.4），P1 平衡 > P2 操作
    qp = WholeBodyQP(q, v)
    # P0 硬约束：动力学 + 接触不滑 + 摩擦锥 + 限位
    qp.add_dynamics_constraint()
    qp.add_contact_constraint(contact_state)     # 双足接触
    # P1 平衡（高优先级）：CoM/ZMP 约束，且把反力作为已知外力计入！
    qp.add_balance_constraint(external_force=reaction_on_hand)  # ← 关键
    # P2 操作（低优先级）：手末端施加 ee_force_target
    qp.add_ee_force_task(ee_force_target, priority=2)
    # P2 运动：base 跟踪 base_ref
    qp.add_base_tracking_task(base_ref, priority=2)
    # P4 次要：姿态、省能
    qp.add_posture_regularization(priority=4)

    tau = qp.solve()                             # 分层求解，平衡不被操作牺牲
    return tau

核心一行是 qp.add_balance_constraint(external_force=reaction_on_hand)——把操作反力作为已知外力计入平衡约束。没有这一行，人形的平衡控制器不知道手上正受着茶几的反力，会被拽倒（§9.3 反事实场景）；有了这一行，平衡控制器主动调整姿态/落脚来抵消这个已知反力。这就是 FALCON"下肢扛住上肢反力"在模型方法里的实现，也是 RoboDuet"运动策略补偿臂反力"的显式版。

走一遍：当人形够取台灯阶段切换到抬桌阶段¶

把抽象流程落到一个具体瞬间——任务从"人形拿台灯"切换到"三体抬桌"的过渡：

分配重算（阶段 1）：人形放下台灯后，CBBA 重拍卖（陷阱：必须支持动态重分配，§9.2 陷阱 2）。抬桌联盟任务此前已派给 \(r_2,r_3\)；现在人形空闲，出价加入辅助扶持角色（它对"重载"\(U\) 不高，故只当辅助/跟随，不当抬主力）。
抓取建立（阶段 2）：三个接触点位置确定，构建 \(G\)。选 C+A+B 混合范式。
协调启动（阶段 3）：ADMM 在 30Hz 开始协调物体轨迹。前几个周期 ADMM 迭代收敛（陷阱：给够迭代轮数，§9.3 陷阱 4），三体对"茶几怎么抬起来、怎么平移"达成一致。
抬起瞬间（阶段 4-5）：内力 QP 分配竖直支撑力——四足各担约 7-8kg 等效力（\(mg/2.4\) 量级，加权后四足多担），人形手只压住约 2-3kg（少担，因 \(W\) 权重大）+ 提供少量内力夹紧。人形 whole-body 把这个反力前馈进平衡，稳稳站住。（这一步的精确数字见后面"数值走查"小节：18kg 茶几抬起瞬间，加权后人形约 28N、两四足各约 79N——以及为什么"少出力"和"力矩平衡"的冲突恰恰需要内力来斡旋。这里的定性描述在那里被算到了具体牛顿数。）
平移绕障（阶段 3-6）：ADMM 协调的物体轨迹绕开玩具，各体跟踪。途中四足-A（领导）的步态推进物体，人形和四足-B 柔顺跟随（阻抗安全垫 + 各自 MPC）。
放下（阶段 1 触发下一任务）：到 B 处，内力 QP 逐步卸载竖直力，物体落地，三体松开，任务完成，触发分配器检查下一任务。

理论-工程桥接（D）：这条流水线把全章六节**全部用上了**——§9.2 分配、§9.3 三范式（C 协调 + A 安全垫 + B 内力）、§9.4 分层 whole-body、§9.5 反力补偿思想。它不是六个孤立工具的拼凑，而是一个有清晰频率分层和职责划分的**系统**。这正是 R6D 要求的"理论落到工程"的终极形态：每个推导出的公式，都在这条流水线里有一个明确的执行位置和频率。

数值走查：抬起瞬间的力预算¶

抽象流程容易"看懂了但不会算"。这一小节把"三体抬起 18kg 茶几的那一瞬间"用具体数字走一遍——它把 §9.3-B 的异构加权内力分配从公式变成可验算的算术，也让你对"人形到底少出多少力"有量感。

设定。茶几 \(m_o=18\)kg，重力 \(mg \approx 18\times 9.81 \approx 176.6\)N，三点抓取（人形手 \(r_1\)、四足背 \(r_2,r_3\)），抬起瞬间要求物体竖直加速度 \(a_z=0.5\,\text{m/s}^2\) 缓缓离地。物体期望竖直合力（牛顿第二定律，含克服重力 + 加速）：

\[ F_z^{\text{des}} = m_o(g + a_z) = 18\times(9.81+0.5) = 18\times 10.31 \approx 185.6\ \text{N} \]

接触点在物体质心系：\(r_1=(0.45,0,0)\)（人形在一长边中点）、\(r_2=(-0.45,0.30,0)\)、\(r_3=(-0.45,-0.30,0)\)（两四足在另一长边两端），单位米。期望合力旋量 \(w_o^d=(0,0,185.6,0,0,0)\)（纯竖直支撑，先不考虑平移加速度的水平分量）。

第一步：同构（不加权）基准——均摊。 若忽略能力差异，最小范数操作力把竖直力按几何对称均摊。注意几何并非完全对称（\(r_1\) 单独在一边，\(r_2,r_3\) 在另一边），但竖直力分量在"无力矩净输出"约束下的最小范数解，会让三点出力满足：竖直合力 \(=185.6\)N 且对质心**力矩为零**。设三点竖直力 \(f_{z,1},f_{z,2},f_{z,3}\)，绕 \(y\) 轴力矩平衡（\(x\) 方向杠杆）：

\[ \begin{cases} f_{z,1}+f_{z,2}+f_{z,3} = 185.6 & (\text{竖直合力}) \\ 0.45\,f_{z,1} - 0.45\,(f_{z,2}+f_{z,3}) = 0 & (\text{绕 } y \text{ 力矩，} x \text{ 杠杆})\end{cases} \]

由第二式 \(f_{z,1}=f_{z,2}+f_{z,3}\)，代入第一式得 \(f_{z,1}=92.8\)N、\(f_{z,2}+f_{z,3}=92.8\)N。再由 \(y\) 方向对称（\(r_2,r_3\) 关于 \(x\) 轴对称）得 \(f_{z,2}=f_{z,3}=46.4\)N。基准结论：人形独扛 92.8N（约 9.5kg 等效），两四足各 46.4N（约 4.7kg）——人形反而扛得最多！因为它独自占了一条边，几何上必须平衡另一边两点之和。这恰恰是危险的：人形承重弱、平衡脆弱，却被几何逼着多扛。

第二步：异构加权——把负担推给四足。 引入 §9.3-B 的加权范数，权重 \(w_i\propto 1/\lambda_i^{\max}\)。设人形力能力 \(\lambda_1^{\max}=80\)N（弱）、四足 \(\lambda_{2,3}^{\max}=300\)N（强），归一化权重 \(w_1=1/80,\ w_{2,3}=1/300\)。加权最小范数解在**满足同样的合力+力矩约束**下，最小化 \(\sum_i w_i f_{z,i}^2\)。用拉格朗日乘子法（约束如上），可解得竖直力向低权重（四足）倾斜。代入数值，近似解为：

\[ f_{z,1}\approx 28\ \text{N},\quad f_{z,2}=f_{z,3}\approx 78.8\ \text{N} \]

（验算：竖直合力 \(28+78.8\times2=185.6\)N ✓；力矩 \(0.45\times28 - 0.45\times157.6 = 0.45\times(28-157.6)\neq 0\) ——这里出现关键张力：纯加权最小范数**无法同时**让人形少出力**和**满足绕 \(y\) 力矩为零，除非允许物体有微小净力矩或调整几何/内力。）

本质洞察——"少出力"和"力矩平衡"的内在冲突，正是内力存在的理由。第二步暴露了一个深刻矛盾：几何（人形独占一边）要求人形多出竖直力以平衡力矩，而能力（人形弱）要求人形少出力。这两个要求在**纯操作力**（\(G\) 行空间）里**不可调和**——操作力被合力旋量唯一确定（最小范数解），没有自由度同时满足两者。解决之道正是 §9.3-B 的**内力**（\(G\) 零空间）：在不改变物体净受力的前提下，调整各点力的分配。具体地，让四足在多出竖直力的同时，通过内力（如四足之间一对相互的水平夹紧力 + 配套竖直分量）把"本该人形承担的力矩"接管过去——物体净受力不变（内力定义），但人形被解放出来少出力。这就是内力分配 QP 真正在做的事：用零空间自由度，在"满足物体动力学"和"尊重各体能力/平衡上限"之间斡旋。基准解（第一步）只用了操作力，所以人形被几何绑死；加权 QP（带内力 + 不等式约束）才能既满足物体动力学又解放人形——这是 §9.3-B 比"简单伪逆"高明的全部价值。

第三步：加人形侧向力上限——验证约束生效。 假设抬起时还需克服一点水平摩擦/扰动，物体期望水平力 \(F_x^{\text{des}}=15\)N。若不加约束，QP 可能让人形手出较大侧向力。但人形当前平衡裕度给出侧向力上限 \(\bar F_1^{\text{balance}}=12\)N（从平衡控制器实时反馈，§9.6 阶段 5）。内力 QP 的约束 balance_limits - f_tangent >= 0 此时**生效**：它强制人形手的水平力 \(\|f_{1,xy}\|\le 12\)N，把多出的 \(\ge 3\)N 水平需求**推给四足**承担。

若去掉这个约束（§9.6 练习 2-b 的故障注入），QP 会算出一个让人形手出约 \(15\times w_{\text{几何}}\) 的侧向力，可能超过 12N 上限 → 人形被推出支撑区倾倒。约束生效与否，是"人形站住"和"人形倒下"的分界。

频率预算：每个阶段一个周期要做多少算、留多少时间。 把六阶段的计算量和时间预算列成表，这是把"频率分层"（§9.6 陷阱 1）落到可核对的数字：

阶段	频率	单周期预算	主要计算	超时后果
1 任务分配（CBBA）	事件触发（~秒级）	< 100 ms	形态感知出价 + 共识轮	慢无妨（非实时环）
2 抓取规划	任务级（一次）	< 50 ms	构建 \(G\)、选范式	一次性，无实时压力
3 ADMM 协调	20-50 Hz（周期 20-50 ms）	< 周期的 70%	各体本地 MPC × ADMM 轮数	不收敛 → 内力残余（陷阱 4）
4 内力分配 QP	50-100 Hz（10-20 ms）	< 5 ms	一个小 QP（维度 \(3K\)）	抖动 → 力指令跳变
5 whole-body QP	200-500 Hz（2-5 ms）	< 2 ms	各体分层 QP	超时 → 跟踪失准、失稳
6 关节力矩环	1 kHz（1 ms）	< 0.5 ms	PD + 前馈	超时 → 直接失稳

理论-工程桥接（D）——预算表是"频率分层"的可核对形式。§9.6 陷阱 1 说"不同环节计算量和频率差几个数量级"，这张表把它量化：阶段 3 的 ADMM（要跑多轮本地 MPC）单周期可能几十毫秒，阶段 6 的关节环只有 1 毫秒——差了近两个数量级，强行同频必然一方拖垮另一方。表里"单周期预算 < 周期的 X%"是实时系统的硬指标：留出余量给抖动和抢占。设计多机 loco-manipulation 系统时，先画这张表、核对每层"算得完吗"，是避免陷阱 1 的第一步。这也解释了为什么 ADMM 必须在中频层（有几十毫秒可跑多轮），而非在 1kHz 关节环里挤单轮（§9.3 陷阱 4 的频率根源）。

两种延迟别混淆：响应延迟 vs 协调延迟。 频率分层下，系统有两条不同尺度的"反应时间"，分清它们能帮你判断"系统够不够快"：

响应延迟（reaction latency）：从一个**快变扰动**（如脚下打滑、被外力推）发生，到关节力矩做出反应的时间。它走的是**高频快路径**——扰动被本体感知捕获后，直接进高频 whole-body（阶段 5，~2ms）+ 关节环（阶段 6，~1ms），不必等低频的协调层。所以响应延迟约 = whole-body 周期 + 关节环周期 ≈ 几毫秒。这是平衡能扛住突发扰动的关键——它必须短（人形被推一下，几毫秒内就得调整，等不起协调层）。
协调延迟（coordination latency）：从一个**需要重新协调的变化**（如物体轨迹要改、有个体退出）发生，到团队达成新协调的时间。它走**低频慢路径**——要经过 ADMM 多轮收敛（阶段 3，几十 ms × 轮数）甚至重分配（阶段 1）。所以协调延迟可达几十到几百毫秒。这对**缓变**的协调目标（物体往哪走）是可接受的。

本质洞察——频率分层的精髓是"快扰动走快路、慢协调走慢路"。这两条延迟路径的分离，正是频率分层架构最深刻的价值：不是所有事情都要等最慢的环节。脚下打滑这种快扰动，绝不能等几十毫秒的 ADMM 协调——它必须在高频层就地反应（响应延迟几毫秒）；而"物体该往哪走"这种缓变协调，慢几十毫秒也无妨。把它们硬塞进同一个频率（陷阱 1），要么让快扰动等慢协调（人形先摔了 ADMM 还没算完），要么让慢协调挤进快环（ADMM 单轮不收敛）。好的架构让每个信号走匹配它变化速度的路径——这也呼应前面"读陈旧值"那条本质洞察：高频层用着低频层的陈旧参考，但自己对快扰动的响应一点不慢，因为快路径根本不依赖慢路径的当拍结果。

⚠️ 常见陷阱¶

陷阱 1（思维陷阱）：各阶段频率没分层，全堆在一个循环里。 - 错误描述：把分配、ADMM 协调、内力分配、whole-body、关节环全写在一个 1kHz 的循环里顺序执行。 - 现象/后果：分配/ADMM 这类重计算拖垮 1kHz 实时性，控制周期严重超时，关节力矩输出抖动甚至失稳。或反之，为了实时只跑单轮 ADMM 导致协调不收敛。 - 根本原因：不同环节的**计算量和必要频率差几个数量级**——分配是事件触发（秒级），ADMM 协调中频（几十 Hz、需多轮），whole-body 跟踪高频（几百 Hz），关节环最快（1kHz）。强行同频要么慢的拖垮快的，要么快的逼停慢的。 - 正确做法：严格频率分层，各层异步运行（多线程/多进程），上层输出作为下层的参考（缓存最新值）。这是所有 loco-manipulation 系统的标准架构（参考 05 方向 OCS2 双线程 MPC 10_足式/110_OCS2完整栈与双线程MPC.md）。

陷阱 2（概念误区）：让脆弱的人形当抬桌领导者。 - 错误描述：因为人形"最智能/传感器最全"，就让它当协调领导者主导物体轨迹和承重。 - 现象/后果：人形承重弱、平衡脆弱，主导抬桌时既扛不住重量又经不起协调误差产生的反力，容易失稳；它一旦失稳，整个协调（领导者）崩溃。 - 根本原因：领导者/主力角色应按**能力**指派（承重稳、抗扰强），而非按"智能程度"。人形的优势在灵巧操作，不在承重协调。 - 正确做法：让承重最稳的四足当领导者/抬主力，人形当辅助/跟随（柔顺阻抗、少承重）。角色指派回到 §9.2 的能力效用——抬桌领导角色的效用，四足远高于人形。

工程落地：频率分层怎么并发跑¶

频率预算表（前面）告诉你每层"算得完吗"，陷阱 1 告诉你"别堆一个循环里"——但**多层异步并发到底怎么搭**？这一小节补上骨架，把抽象的"频率分层"落成可实现的并发结构。它是把全章工具变成一个真能跑的系统的最后一公里。

核心模式：生产者-消费者 + 最新值缓存（latest-value buffer）。 各层运行在独立线程/进程，上层是下层的"生产者"——上层算出参考后写入一个**带锁的共享缓存**，下层在自己的高频循环里**读缓存里的最新值**（而非等上层算完）。关键设计：

[线程 A] 分配 + 抓取规划（事件触发）
   产出：角色指派、抓取矩阵 G → 写入 cache_plan
[线程 B] ADMM 协调（30 Hz 循环）
   读 cache_plan；跑足够轮 ADMM → 写入 cache_obj_traj（协调好的物体轨迹 + 各体参考）
[线程 C] 内力分配 QP（100 Hz 循环）
   读 cache_obj_traj 的最新值；解内力 QP → 写入 cache_forces（各接触点力目标）
[线程 D_k] 各体 whole-body QP（500 Hz 循环，每体一个）
   读 cache_forces + cache_obj_traj 的最新值；解分层 QP → 写入 cache_tau_k
[线程 E_k] 各体关节力矩环（1 kHz，最高优先级）
   读 cache_tau_k 的最新值；PD + 前馈 → 输出电流

缓存读写：每个 cache 用读写锁 / 双缓冲（double buffering）/ 无锁原子指针交换，
         保证高频读者永不阻塞在低频写者上（写者慢，但读者总能拿到一份完整的旧值）。

本质洞察——"读最新值而非等结果"是实时系统的命门。为什么下层读缓存的"最新值"而不是"等上层这一拍的新结果"？因为如果线程 C（100Hz）非要等线程 B（30Hz）出新结果才走，C 就被拖慢到 30Hz——快的被慢的拖垮，正是陷阱 1 的核心病。解法是解耦时间：下层永远用"手头最新的那份上层结果"跑自己的高频循环，哪怕这份结果是上一拍的（陈旧几毫秒到几十毫秒）。这在控制上是可接受的——上层参考变化本就缓慢（物体轨迹不会在 10ms 内剧变），用稍陈旧的参考跑高频跟踪，远好于让高频环停下来等。频率分层的本质，是用"参考的时间陈旧性"换"各层的时间独立性"——这是所有 loco-manipulation 实时系统（OCS2 双线程 MPC 等）的共同骨架。

理论-工程桥接（D）——陈旧性的上界就是稳定性的约束。"读陈旧值"不是没有代价：缓存里的值越旧，下层用的参考与真实意图偏离越大。所以这个架构能成立，有一个隐含条件——上层的更新周期要远短于参考量的有效变化时间常数。物体轨迹（秒级变化）用 30Hz（33ms 周期）协调，陈旧性 < 33ms 远小于轨迹变化尺度，安全；但若你想让某个变化很快的量（如瞬时接触力）走低频层，陈旧性就会超过它的变化尺度，系统失稳。这给出一条设计判据：一个量该放在哪一层，取决于它的变化速度——变化越快的量必须放在越高频的层（接触力/平衡在 500Hz+，物体轨迹在 30Hz，任务分配在事件级）。这正是频率预算表里"频率"列背后的物理依据，也解释了为什么平衡补偿（§9.6 阶段 5）必须在高频 whole-body 层、而非低频协调层——平衡状态变化快，慢了就摔。

练习¶

（综合实现题·累积项目核心） 在 MuJoCo 或 IsaacLab 中搭建本章三体场景（G1 + 2×Go2 + 茶几），实现这条六阶段流水线的简化版：阶段 1 用 §9.2 的形态感知 CBBA（可复用第 3 章框架）、阶段 4 用本节内力分配 QP、阶段 5 用各体 whole-body（可用简化 QP 或现成 WBC 库）。先实现"静态抬起"（不平移），验证三体能协调地把茶几抬离地面且人形不倒；再加平移和绕障。这是本章累积项目的核心模块。
（故障注入题） 在练习 1 基础上，注入三种故障并观察系统行为：(a) 关掉人形的反力前馈补偿（阶段 5 那一行），看人形是否被拽倒；(b) 把内力 QP 的人形侧向力约束去掉，看是否产生超限内力；(c) 让 ADMM 只跑 1 轮（不收敛），看物体轨迹协调质量如何退化。每种故障对应正文/前节的哪个陷阱？
（设计扩展题） 现在把团队扩展为"人形 + 四足×3"抬一张更大的桌子（4 个抓取点），其中一台四足在抬到一半时"电量耗尽"退出。设计系统如何响应：分配如何重算（剩余 3 体能否撑住 4 角桌？）、内力如何重新分配、whole-body 如何过渡。讨论这考验了本章哪些机制的鲁棒性（提示：动态重分配 §9.2、范式 C 去中心化鲁棒性 §9.3、分层任务栈降级 §9.4）。

§9.7 与 05 运动控制复合方向的交叉地图 + 前沿与开放问题 ⭐⭐¶

这一节解决什么问题：本章（多机视角的异构协同）和 05 运动控制复合方向（单体 loco-manipulation）讲的内容高度交叠，初学者很容易混淆"什么时候看哪边"。这一节画一张清晰的交叉地图——哪些是"一个身体内部的协调"（看 05 复合）、哪些是"多个身体之间的协调"（看本章），以及它们共享哪些数学工具。最后梳理前沿工作与开放问题。

体内 vs 体间：一张分工地图¶

本章 §9.5 已经埋下了核心区分——体内协调（一个机器人的腿+臂）vs 体间协调（多个机器人）。这个区分正是本章与 05 复合方向分工的主线。下表把两个方向的相关章节对应起来，告诉你遇到具体问题该查哪边：

你的问题	属于	看这里
单个四足+臂怎么边走边操作？（腿臂协调）	体内	05 复合 `160_四足臂动力学概览.md`、`170_qm_control精读.md`、`180_Deep_WBC精读.md`
单个浮动基座系统的 whole-body QP 怎么写？	体内	05 足式 `90_WBC分层优化与TSID.md`、05 机械臂 `F07_浮动基座WBC理论.md`
人形怎么边走边用末端施力且不倒？（力自适应）	体内	05 复合 `250_力敏感人形LocoMani.md`（FALCON/SoFTA/HOMIE）
人形上肢操作 + 下肢平衡怎么融合？	体内	06 具身 `RL运控/Ch20_人形全身控制.md`、05 复合 `220_经典人形全身控制.md`
腿足 MPC+WBC 在操作任务中怎么联合力控？	体内	05 机械臂 `F08_腿足MPC_WBC联合力控.md`
多个机器人怎么共抬一个物体？（内力分配）	体间	本章 §9.3、第 5 章 `60_协同搬运与力控.md`
多个异构机器人怎么分工？（能力分配）	体间	本章 §9.2、第 6 章 `70_异构多机协同.md`
多个机器人的联合 whole-body 怎么拼？	体间	本章 §9.4、第 8 章 `90_多足协同Loco-Manipulation.md`
多个 loco-manipulator 协作（任务分解、延迟鲁棒）？	体间	05 复合 `280_多机协作LocoMani.md`、本章全章
双臂/多臂协调操作（闭链、力分配）？	体间（同基座或多基座）	第 7 章 `80_双臂多臂协同操作.md`、05 机械臂 `D01-D04` 双臂子课程

系统性分类（E）——一句话判据。遇到一个 loco-manipulation 问题，先问：协调发生在一个 base 内部，还是跨多个 base？ - 一个 base 内部（腿+臂、上肢+下肢通过同一身体刚性相连）→ 体内协调 → 看 05 复合方向。核心工具：单体 whole-body QP、分层零空间、operational space control。 - 跨多个 base（多个独立机器人通过物体/通信松耦合）→ 体间协调 → 看本章和多机协作方向。核心工具：能力分配、抓取矩阵、ADMM、分布式 MPC。

边界情形：固定双臂共抓物体——两臂若共享 base（同一躯干上的两臂）偏体内，若是两台独立单臂机器人偏体间。本章 §9.4 的统一框架是两者的桥——它能同时描述"一个 base 多任务"和"多 base 多体"，区别只在块对角结构里有几个块。

共享的数学工具¶

虽然分工不同，体内和体间协调**共享一套数学工具**——这正是为什么本章能大量复用 05 方向的内容。共享工具清单：

工具	体内用法	体间用法（本章）	出处
浮动基座动力学	单体 \(M\dot v + h = S^\top\tau + J^\top f\)	堆叠成块对角团队动力学（§9.4）	05 足式 `50_空间向量与浮动基座动力学.md`
whole-body QP / 分层	单体多任务多约束	团队级分层任务栈（§9.4）	05 足式 `90_WBC分层优化与TSID.md`
抓取矩阵 / 内力	单臂抓取的力分析	多异构末端共抓的统一 \(G\)（§9.3）	第 5 章 `60`、05 机械臂 `D03`
阻抗 / 导纳控制	单臂柔顺力控	跟随者柔顺协调（范式 A，§9.3）	05 机械臂 `F04`、`F05`
MPC + 接触	单体 loco-manipulation MPC	各体本地 MPC + ADMM 协调（§9.3-C）	05 足式 `110_OCS2`、第 4 章 `50`
双智能体/双策略	单体 loco+arm 解耦（RoboDuet/FALCON）	启发体间"先解耦再协调"（§9.5）	05 复合 `250`、本章 §9.5

本质洞察：体内和体间协调共享数学工具，绝非偶然——它们是**同一类问题**（管理多个能力不同的单元对共同目标的贡献，§9.5 的跨尺度统一性）。数学不区分"这两个发力单元是同一身体的腿和臂，还是两台不同机器人"——抓取矩阵照样写、whole-body QP 照样拼、阻抗照样调。区别只在物理耦合强度（刚性 vs 接触），它决定了集中还是分布求解。学好 05 方向的单体 loco-manipulation，你已经掌握了本章一半的工具；本章教的是把这些工具从"一个身体"推广到"多个身体"时，新增的那部分（能力分配、跨体闭链、分布式协调）。

模型方法 vs 学习方法：一张取舍清单¶

本章是**模型驱动**协调的集大成（§9.1-9.4 全是手工推导的抓取矩阵、内力 QP、ADMM、whole-body）；§9.5 引出了**学习驱动**的协调（RoboDuet/FALCON），第 10-12 章会全面展开。两条路不是对立的，但初学者常困惑"什么时候该用哪个"。这一小节把全章散落各处的对照（§9.5 陷阱 2、§9.7 开放问题 1、章末结语）收成一张可操作的取舍清单——它也是你过渡到第 10-12 章前该建立的全局判断。

维度	模型方法（本章 §9.1-9.4）	学习方法（§9.5、第 10-12 章）
可解释性	高——每个力/约束有明确物理含义，能逐项调试	低——策略是黑盒，难定位"为什么这么动"
安全保证	强——硬约束（摩擦锥、平衡、力上限）可证明满足	弱——难证明未见状态下不失稳/不超力（需额外 CBF 层，第 12 章）
难建模因素（接触、摩擦、柔性）	差——依赖精确建模，sim2real gap 大（§9.7 开放问题 1）	好——能从数据隐式吸收难建模的接触/摩擦
数据需求	无需训练数据，给模型即可用	需大量交互数据 + 仿真训练（课程、随机化）
泛化到新构型/物体	需重新建模/重调，但有原理保证	部分免重训（RoboDuet 跨本体、decPLM 换数量），但泛化边界不可控
在线计算	QP/MPC 求解，可能重（尤其高维 whole-body）	前向推理快（网络一次前向），但训练昂贵
协调精度	高（显式算内力/轨迹），受模型精度限	中-高，受训练覆盖限

系统性分类（E）——按"瓶颈在哪"选方法。这张表的用法不是"哪个更好"，而是**定位你的瓶颈**： - 瓶颈是**安全/认证**（搬重物砸到人不可接受）→ 模型方法打底（硬约束保证），学习只做补偿。 - 瓶颈是**难建模接触**（实机协调远差于仿真，§9.7 开放问题 1）→ 学习方法吸收这部分，模型给骨架。 - 瓶颈是**泛化**（要换不同数量/形态/物体频繁）→ 学习方法的免重训特性（decPLM/RoboDuet）有价值。 - 瓶颈是**没数据/没仿真**→ 只能模型方法。

几乎所有真实系统的瓶颈都不止一个，所以答案几乎总是**混合**——这正是第 12 章（§130 MARL+规控混合）的主题。本章的全部价值，是给这个混合提供"模型那一半"：可解释、可验证、有安全保证的骨架。

对比性思维（B）——"模型 vs 学习"和"集中 vs 分布"是两个独立的轴。初学者容易把"模型方法"等同于"集中式"、"学习方法"等同于"分布式"，这是误解。两者正交：模型方法可以分布式（范式 C 的分布式 MPC 就是模型 + 分布式）；学习方法也可以集中式（一个中心化的多智能体策略）。本章的范式 C（ADMM）是"模型 + 分布式"的典范，decPLM（第 8 章）是"学习 + 分布式"的典范，FALCON（§9.5）是"学习 + （单体内）集中协调"。设计系统时，先沿"模型↔学习"轴选（按瓶颈在建模还是安全），再沿"集中↔分布"轴选（按通信/鲁棒/规模）——两个决策分开做，别捆在一起。

前沿工作¶

本章的研究脉络（见章首科研发展脉络表）仍在快速演进。当前（2025）前沿集中在几个方向：

学习驱动的多体协调：decPLM（第 8 章 §90）证明了纯接触力 + MARL 能让多个四足**零通信**协调搬运、零样本泛化到不同数量/物体。把这条路推向**异构**团队（人形 + 四足混合 MARL）是开放前沿——Pandit 等的多双足搬运（章首表）已在同构多双足上验证了"刚性附着、免重训换构型"的涌现协调。
多体 whole-body 的统一求解器：De Vincenti 的 robot/payload/gait-agnostic SQP（第 8 章 §90）和 ETH 的 Shared Object Manipulation（章首表）代表了模型方法的最前沿——一套优化框架处理任意数量、任意形态的 legged manipulator + 任意负载。把人形（更高自由度、更脆弱平衡）稳健地纳入这类统一求解器仍有挑战。
LLM 驱动的异构任务分解：RoCo（Mandi 等，§9.5 提及的相关线）用 LLM 做多机器人任务分解 + 底层运动规划 + 闭环反馈。把这套用于强异构具身团队（让 LLM 理解"人形适合精细、四足适合重载"并自动分解）是一个活跃方向——它本质是用 LLM 替代/增强本章 §9.2 的能力分配。
力自适应的多体推广：FALCON（章首表）的"下肢稳定 + 上肢力自适应"目前是单体人形。把"力自适应"推广到多体——让团队整体对未知负载/外力自适应——是力敏感多体 loco-manipulation 的前沿（05 复合 250_力敏感人形LocoMani.md 是单体侧的对应章）。

开放问题¶

本章方法仍有未解决的硬骨头，这些是博士选题的富矿：

异构 sim-to-real gap（最尖锐）：本章模型方法对**接触和摩擦建模**高度敏感（§9.3 抓取矩阵依赖准确接触、内力分配依赖准确摩擦锥），但接触力在仿真和真实世界差异显著（第 8 章 §90、骨架"开放问题"均指出这是多足协同搬运的主要 gap）。异构团队更糟——人形手、四足背、不同材质接触面的摩擦/柔性各异，统一建模困难。学习方法（FALCON 式）能隐式吸收，但牺牲可解释性。**如何兼得**是开放问题。
强异构的可扩展协调：同构多机能用参数共享、置换不变网络（§9.1 本质洞察）轻松扩展到上百个体。强异构破坏了这种对称性——人形和四足不可互换，置换不变性失效。如何为强异构团队设计可扩展（数量、形态都可变）的协调，是 §9.1 那个"被打破的对称性"留下的根本难题。
体内与体间协调的统一学习框架：§9.5 揭示了体内（腿+臂）和体间（多机）协调的镜像，但目前两者用不同方法（体内常用单网络/单 QP，体间用分布式）。能否有一个统一的（分层）学习框架同时处理两个尺度（如练习 §9.5-3 的"体内双策略 × 体间协调策略"），是把 §9.5 的洞察工程化的开放方向。
安全保证下的学习协调：纯学习协调缺乏安全保证（§9.5 陷阱 2）。强异构搬运重物若失稳，后果严重（砸到人/物）。如何在 MARL 协调上叠加可证明的安全约束（CBF/GCBF+），是第 12 章（§130）的主题，也是异构具身协作落地的前提。
异构团队的评测与标准化（被低估但关键）：同构多机有相对成熟的评测基准（编队误差、覆盖率、收敛轮数）。强异构具身协作**缺乏公认的评测标准**——该用什么指标衡量"一个异构团队协调得好不好"？内力大小？任务完成率？对个体失效的鲁棒度？跨形态/数量的泛化？而且不同形态组合（人形+四足 vs 全四足 vs 加无人机）难以公平对比。没有好的评测，方法的进步就难以衡量和复现——这是一个看似工程、实则制约整个领域健康发展的开放问题。它也呼应本章 §9.1 能力画像标定的可复现性诉求：从能力度量到团队评测，强异构都缺一套标准化的度量框架。

前向预告：上述开放问题 2、3、4 的攻克，主要靠**学习方法**——这正是接下来第 10 章（MARL 基础 §110）、第 11 章（MARL 多机运动协调 §120）、第 12 章（MARL+规控混合 §130）的内容。本章是**模型驱动协调的集大成**：它给了你可解释、可验证的协调骨架。下三章引入数据驱动，第 12 章把模型骨架（安全/约束）与学习协调（适应难建模因素）缝起来——那时你会回头看本章，发现它正是混合方法里"模型"那一半的全部理论基础。

练习¶

（地图绘制题） 拿一个你感兴趣的具体场景（如"两台人形 + 一台四足在仓库协同打包"），把它涉及的每个子问题（移动、抓取、协调、平衡、分配……）逐一归类为"体内"还是"体间"，并标注该查本章哪一节或 05 复合方向哪一章。画出这个场景的完整知识依赖图。
（前沿调研题） 选章首科研发展脉络表中的一个工作（推荐 Shared Object Manipulation 或 FALCON 或 RoboDuet），精读其论文，用本章的语言（能力画像/抓取矩阵/三范式/统一框架/双智能体）重新表述它的核心方法。指出它对应本章哪些概念、又超出了本章哪些（即本章未覆盖的它的创新点）。
（开放问题思辨题） 针对开放问题 1（异构 sim-to-real gap），设计一个研究方案：如何让"模型方法的可解释/安全"与"学习方法吸收难建模接触"兼得？（提示：可考虑残差学习——模型给 baseline，学习只补偿模型误差；或考虑可微物理。）这道题没有标准答案，重在论证你的方案如何同时满足"可验证"和"准确"。

本章常见误解汇总¶

误解	正确理解	出处
强异构只是"弱异构 + 更多传感器"	弱异构协作在信息层，强异构协作在物理交互层（实时、连续、不可丢的力传递），耦合强一个数量级	§9.1 陷阱 1
一个机器人可以用单一"能力总分"概括	能力是多维向量，匹配是需求向量与能力画像的加权内积 \(U_{kj}\)，不是排行榜	§9.1 陷阱 2
"做不了"可以用一个大代价表示	硬不可行用可行性掩码 \(\Phi=0\) 或 \(-\infty\) 效用绝对排除，不参与权衡	§9.1/§9.2 陷阱 3
异构分配可以直接套用匈牙利算法	匈牙利优化的是距离总和、假设任意机器人能做任意任务，会把重载派给搬不动的个体；要用形态感知出价	§9.2 动机
联盟任务=多个独立单体任务	子任务强耦合（同物体、需协调、原子性），要带角色标签 + 协调亲和 + all-or-nothing	§9.2 陷阱 1
多体各自独立跟踪物体轨迹就能协调	刚性闭链把误差困成内力，正反馈发散；必须给误差出口（阻抗/内力分配/一致性）	§9.3 动机/反面
跟随者阻抗调得越硬越精确	阻抗的价值在柔顺让步给误差出口，太硬退化为无协调的独立跟踪，内力发散	§9.3 陷阱 1
力能力上限 \(\lambda^{\max}\) 就是能承受的操作反力上限	"能输出多大力"（执行器极限）≠"施力后还能否站稳"（平衡极限，动态变化）；浮动基座脆弱平衡个体两者差很多	§9.3 陷阱 2
统一 whole-body 框架=把多机焊成一个大机器人	团队动力学是块对角的（各体无物理连接，只靠物体力松耦合）；统一的是描述语言不是物理实体	§9.4 陷阱 1
统一描述就必须集中式求解	统一描述和集中求解是正交维度；主张统一描述（保证耦合不丢）+ 分布式求解（ADMM 分解，保鲁棒/可扩展）	§9.4 理论 4
体内双智能体方法（RoboDuet）可直接搬到体间多机	体内刚性强耦合（状态瞬时互通），体间接触松耦合（要通信、有延迟）；迁移思想不迁移机制	§9.5 陷阱 1
学习方法（FALCON）能完全替代模型方法	模型方法可解释/可验证/有约束保证，学习方法处理难建模因素；互补而非替代，第 12 章缝合	§9.5 陷阱 2
让最智能的人形当抬桌领导者	领导/主力按能力（承重稳、抗扰强）指派，不按智能程度；人形优势在灵巧不在承重	§9.6 陷阱 2

本章小结¶

本章把多机协作从"同构/弱异构"推进到**强异构具身**的极端——参与者的自由度、平衡机理、接触方式都根本不同。核心是三件事：按能力分工（§9.2 形态感知 CBBA）、跨形态协调（§9.3 三范式）、统一描述（§9.4 团队级 whole-body）。一条贯穿全章的主线是 §9.1 的本质洞察——异构破坏了同构多机的对称性，本章几乎所有技术难点都在补偿这个被打破的对称性。另一条主线是 §9.5 揭示的**跨尺度统一性**——体内（腿+臂）与体间（多机）协调是同一思想在不同耦合强度下的表现，这也是本章与 05 复合方向分工的依据。

一页纸复习卡片¶

把全章压成一张可以贴在显示器边的卡片。三条记忆锚点 + 一条逻辑链 + 一棵选型决策树。

三条记忆锚点（忘了别的也别忘这三条）：

对称性破缺是总根源（§9.1）。同构多机可置换、可参数共享、可对称聚合；强异构不可置换——下标携带身份。本章所有技术（能力效用、加权内力、异尺寸块）都是在破缺后用"显式身份信息"重建结构。遇到任何异构难题，先问："这里哪个'统一参数'是同构假设的残留，该按形态拆开？"
协调是管理误差，不是消除误差（§9.3）。多体抓同一刚体，估计/模型/通信误差必然产生内力——你消不掉，只能给出口。三范式 = 三种误差出口：阻抗（软化）、内力 QP（显式协商）、ADMM（轨迹共识）。
统一描述 ≠ 集中求解（§9.4）。统一框架是为了让耦合（跨体闭链）不在接缝处丢失，块对角结构保证它能分解回分布式。"统一"在描述层，"分布"在求解层，两者正交。

全章逻辑链（一句话串起七节）：

强异构破坏了置换对称性（§9.1）→ 所以分配要用能力效用显式编码"谁配谁"（§9.2）→ 配好后多体共抓物体产生不可避免的内力，要给误差出口（§9.3 三范式）→ 三范式的母问题是带跨体闭链约束的团队 whole-body，块对角使它可分解（§9.4）→ 同样的"协调"在单体腿臂尺度也存在，学习方法（RoboDuet/FALCON）与模型方法互为镜像（§9.5）→ 把六节工具按频率分层串成流水线（§9.6）→ 体内查 05 复合、体间查本章，开放问题靠学习方法（§9.7）。

协调范式选型决策树（拿到一个共抬任务，照着选）：

通信是否近乎中断 / 要极高鲁棒 / 精度要求不高？
├─ 是 → 范式 A（领导跟随 + 阻抗）：零通信、靠接触力协调，但内力不可精控
└─ 否 ↓
   是否要精密控制内力/夹紧力（如插孔、抓易碎物）？
   ├─ 是，且有可靠协调节点、模型准 → 范式 B（统一 G + 内力分配 QP）：最精确
   └─ 否 / 既要分布式又要精确、算力充足 ↓
      → 范式 C（分布式 MPC + ADMM）：各跑本地 whole-body、物体轨迹共识、去中心化鲁棒

实战提示：三者常混用 —— C 做上层轨迹协调 + A 做底层安全垫 + B 在关键操作临时接管（§9.6 流水线就是这么配的）。
角色指派铁律：领导者/承力主力按"承重稳、抗扰强"选（四足），不按"最智能"选（§9.6 陷阱 2）；
            脆弱平衡个体（人形）当跟随者，且其操作反力必须前馈进平衡约束（§9.3 陷阱 2 / §9.6 阶段 5）。

本质洞察——这张卡片的三条锚点其实是一条。"对称性破缺"（锚点 1）说的是异构在**分配/描述**上的后果，"管理误差"（锚点 2）说的是异构在**协调**上的后果，"统一≠集中"（锚点 3）说的是异构在**求解**上的后果——三者都是同一件事的不同侧面：因为个体不可互换（破缺），所以它们对共同物体的贡献既要显式区分（分配/加权）、又会因不一致而产生内力（协调）、还必须在保持区分的前提下可分解地求解（统一≠集中）。把这条想通，你对"异构协作难在哪"就有了一个能贯穿全章的统一答案，而不是七节零散的技巧。

术语速查表¶

术语	一句话定义
强异构具身（strongly heterogeneous embodiment）	参与个体的自由度/平衡机理/接触方式都不同，协作发生在物理交互层
形态向量（morphology vector）\(\mathbf{m}_k\)	刻画个体物理身体的向量：自由度、平衡类型、接触模式、工作空间、力上限、移动性
能力画像（capability profile）\(\mathbf{c}_k\)	个体对各任务原语的胜任度映射，\(\bot\) 表示硬不可行
能力效用（capability utility）\(U_{kj}\)	任务需求向量与能力画像的加权内积，刻画 \(r_k\) 多适合 \(\tau_j\)
可行性掩码（feasibility mask）\(\Phi_{kj}\)	布尔量，\(r_k\) 能否做 \(\tau_j\)，用于绝对排除硬不可行配对
形态感知出价（morphology-aware bid）	CBBA 出价 = 可行性掩码 × 能力效用 × 边际时间折扣
联盟任务（coalition task）	需多个特定形态个体协作的任务，拆成带角色子任务
统一抓取矩阵（unified grasp matrix）\(G\)	把多个异构末端当同一物体的接触点，映射接触力到物体合力旋量
内力（internal force）\(f_{\text{int}}\)	落在 \(G\) 零空间、对物体无净运动、用于夹紧物体的接触力
领导者-跟随者（leader-follower）	一个个体主导物体轨迹、其余用阻抗柔顺跟随的协调范式
跨体闭链耦合	各操作末端被共抓物体刚性锁定的约束，统一框架的核心
分层任务栈	团队 whole-body 的优先级任务组织，平衡 P1 高于任务 P2
体内/体间协调	一个机器人内部（腿+臂）vs 多个机器人之间的协调
双智能体（dual-agent，RoboDuet/FALCON）	用两个策略/智能体分别管运动和操作、相互引导实现 whole-body
力课程（force curriculum，FALCON）	训练时渐进增大末端外力，让策略隐式学到平衡承受上限

知识点总表¶

编号	知识点	核心要点	对应节	难度
9-1	强异构具身问题形式化	形态向量 + 能力画像 + 任务-能力匹配；异构破坏对称性	§9.1	⭐⭐
9-2	能力效用与可行性掩码	\(U_{kj}\) 加权内积、\(\Phi_{kj}\) 绝对排除、\(\bot\)≠大代价	§9.1-§9.2	⭐⭐
9-3	形态感知 CBBA	出价 = 掩码 × 效用 × 边际折扣；只换出价函数，保 CBBA 框架	§9.2	⭐⭐⭐
9-4	联盟任务处理	拆带角色子任务 + 协调亲和 + 原子性	§9.2	⭐⭐⭐
9-5	协调的本质	管理刚性约束下不可避免的误差，给误差出口	§9.3	⭐⭐⭐
9-6	范式 A 领导跟随+阻抗	主导+柔顺让步；通信低、鲁棒、精度中；按形态调阻抗	§9.3	⭐⭐⭐
9-7	范式 B 抓取矩阵+内力分配	统一 \(G\)、伪逆操作力 + 零空间内力、异构加权 QP + 人形侧向约束	§9.3	⭐⭐⭐⭐
9-8	范式 C 分布式 MPC (ADMM)	各体本地 whole-body MPC + 物体轨迹一致性；复用第 4 章 ADMM	§9.3	⭐⭐⭐⭐
9-9	三范式选型	沿"集中↔分布""隐式↔显式"两维分布，常混用	§9.3	⭐⭐⭐
9-10	堆叠异构动力学	块对角团队质量矩阵 + 操作力稀疏耦合	§9.4	⭐⭐⭐⭐
9-11	跨体闭链耦合约束	末端被物体锁定，微分到加速度层 + Baumgarte	§9.4	⭐⭐⭐⭐
9-12	分层任务栈	P0 硬约束 > P1 平衡 > P2 任务 > P3 内力 > P4 次要	§9.4	⭐⭐⭐
9-13	统一≠集中	统一描述（保耦合）+ 分布式求解（ADMM 分解，保鲁棒）	§9.4	⭐⭐⭐⭐
9-14	RoboDuet 双策略	loco+arm 相互引导、两阶段训练、跨本体零样本迁移	§9.5	⭐⭐⭐
9-15	FALCON 双智能体力自适应	下肢稳定+上肢力补偿、力课程隐式学平衡上限	§9.5	⭐⭐⭐
9-16	体内↔体间镜像	同一协调思想跨尺度；耦合强度决定集中/分布、模型/学习	§9.5	⭐⭐⭐
9-17	端到端流水线	六阶段频率分层 + 三范式混合 + 人形反力前馈	§9.6	⭐⭐⭐
9-18	体内/体间分工地图	一个 base 内部 vs 跨多 base；共享数学工具	§9.7	⭐⭐

累积项目：本章新增模块——异构三体协同抬桌¶

本项目贯穿多机协作方向各章。前几章你已搭建了通信图与共识（第 1-2 章）、任务分配与 MAPF（第 3 章）、分布式 MPC 编队（第 4 章）、协同搬运内力（第 5 章）、异构编队（第 6 章）。本章新增 HeteroLocoMani 模块——把这些拼成一个异构 loco-manipulation 系统。

本章新增模块清单（建议在累积项目仓库 multibot_project/hetero_locomani/ 下实现）：

hetero_locomani/
├── morphology.py          # §9.1 形态向量、能力画像、能力效用 U_kj、可行性掩码 Φ
├── hetero_cbba.py         # §9.2 形态感知 CBBA（复用第 3 章 cbba.py，重写 compute_bid）
│                          #      + 联盟任务拆分 + 角色指派
├── grasp_matrix.py        # §9.3 统一抓取矩阵 G、零空间基、伪逆操作力
├── force_allocation.py    # §9.3-B 异构内力分配 QP（加权 + 摩擦锥 + 人形侧向约束）
├── coordination/
│   ├── leader_follower.py #   范式 A：阻抗协调
│   └── admm_mpc.py        #   范式 C：复用第 4 章 admm.py，本地子问题换 whole-body MPC
├── team_wbc.py            # §9.4 团队级 whole-body（块对角组装 + 闭链耦合 + 分层栈）
├── humanoid_wbc.py        # §9.6 人形 whole-body + 操作反力前馈补偿
└── pipeline.py            # §9.6 六阶段流水线编排（频率分层、多线程）

与前章模块的复用关系（体现 R11 项目连贯性）： - hetero_cbba.py **复用**第 3 章 cbba.py 的两阶段框架，只重写出价函数（§9.2 强调的"只换出价"）。 - coordination/admm_mpc.py **复用**第 4 章 admm.py 的 ADMM 迭代，只把本地子问题从 SRB MPC 换成异构 whole-body MPC（§9.3-C 强调的"只换本地子问题"）。 - grasp_matrix.py 与 force_allocation.py **复用并推广**第 5 章协同搬运的抓取矩阵/内力代码到异构接触点。

验收标准（分级）： - ⭐ 基础：morphology.py + hetero_cbba.py 跑通客厅场景分配，输出与 §9.2 表格一致；加固定臂验证能力掩码。 - ⭐⭐ 进阶：force_allocation.py + humanoid_wbc.py，仿真中三体**静态抬起**茶几，人形不倒、内力满足摩擦锥。 - ⭐⭐⭐ 完整：pipeline.py 串起六阶段，三体**平移+绕障**搬运茶几到目标，频率分层正确。 - ⭐⭐⭐⭐ 研究级：注入故障（一台四足中途退出，§9.6 练习 3），系统鲁棒降级完成任务。

建议的评测指标（呼应 §9.7 开放问题 5）：实现时顺手记录这几个量，既能客观衡量你的协调质量，也是把"协调得好不好"量化的练习——(1) 内力大小 \(\|f_{\text{int}}\|\) 的时间曲线（越小越协调，发散即失配）；(2) 物体跟踪误差 \(\|x_o-x_o^d\|\)（任务完成度）；(3) 人形平衡裕度（ZMP 离支撑边界的距离，越大越稳，归零即倒）；(4) ADMM 收敛轮数 / 残差曲线（协调效率，对应子节 6 的 \(\rho\) 调试）；(5) 故障恢复时间（从个体退出到团队重新稳定的时长，对应动态重分配 §9.2 子节 7）。把这五条画成图，你的累积项目就有了可对比、可复现的评测面——而非只看"成没成"。

跨章综合题¶

本题需要综合本章与前面 2-3 章的知识，目的是打破章节隔阂、让知识形成网络。

综合题（综合第 2、3、4、5 章 + 本章）：设计并（伪代码）实现一个**完整的异构协同搬运系统**，要求串起以下各章工具： 1. 第 1-2 章：三个异构体（人形 + 2 四足）构成通信图 \(G_{\text{comm}}\)，用最大值共识维护"每个任务的最高出价"（CBBA 冲突消解的底层）。 2. 第 3 章 + 本章 §9.2：用形态感知 CBBA 把"搬茶几（联盟任务）+ 拿台灯 + 侦察"派给三体，输出角色指派。验证能力掩码拦截不可行配对。 3. 第 4 章 + 本章 §9.3-C/§9.4：用 ADMM 协调三体共抓茶几的物体轨迹，各体跑自己的 whole-body MPC（块对角动力学 + 跨体闭链耦合）。 4. 第 5 章 + 本章 §9.3-B：用统一抓取矩阵把物体期望合力旋量分解为各接触力，在零空间分配异构内力，尊重人形侧向力上限。

要求：画出从"通信图共识"到"各关节力矩"的完整数据流图，标注每个环节用了哪章的什么工具、运行在什么频率。讨论：如果通信图 \(G_{\text{comm}}\) 的代数连通性 \(\lambda_2\) 很小（连接稀疏），会如何影响第 2 步 CBBA 的收敛和第 3 步 ADMM 的协调？（提示：两者的收敛速率都与图的谱/直径相关——回顾第 2 章。）

延伸阅读¶

异构 loco-manipulation 团队（本章核心，⭐⭐⭐~⭐⭐⭐⭐）： - Sleiman, Bjelonic 等 (ETH RSL), "Shared Object Manipulation with a Team of Collaborative Quadrupeds" (arXiv:2510.00682, 2025) — 多足+臂团队、力闭合抓取、混合运动-力控。本章 §9.3-B/§9.4 的工程原型。 - De Vincenti, Coros, "Centralized MPC for Collaborative Loco-Manipulation" (RSS 2023) — robot/payload/gait-agnostic SQP，统一框架的母问题（第 8 章 §90 详讲）。 - Pandit 等, "Learning Decentralized Multi-Biped Control for Payload Transport" (CoRL 2025, arXiv:2406.17279) — 多双足搬运、免重训换构型、涌现协调。

单体 loco-manipulation 与双智能体（体内协调，⭐⭐⭐）： - Pan 等, "RoboDuet: Cooperative Policy for Whole-Body Legged Loco-Manipulation" (RA-L 2024, arXiv:2403.17367) — 双策略相互引导、跨本体迁移。§9.5 精读。 - Zhang 等, "FALCON: Learning Force-Adaptive Humanoid Loco-Manipulation" (arXiv:2505.06776, 2025) — 双智能体、力课程、人形 3D 力交互。§9.5 精读。 - Zimmermann 等, "Go Fetch: Mobile Manipulation in Unstructured Environments" (IROS 2021) — 四足+臂移动操作先驱，legged manipulation 工程起点。

多机器人协调与 LLM（分工，⭐⭐⭐）： - Mandi 等, "RoCo: Dialectic Multi-Robot Collaboration with Large Language Models" (ICRA 2024, arXiv:2307.04738) — LLM 做任务分解 + 多臂规划 + 闭环反馈。§9.2/§9.7 能力分配的学习/语言版。 - Fu 等, "Mobile ALOHA" (CoRL 2024) — 移动双臂操作、低成本遥操作与模仿。移动操作数据采集标杆。

经典理论基础（⭐⭐~⭐⭐⭐）： - 双臂协调操作的抓取矩阵/内力理论 — 见本项目 05 机械臂 D01-D04 双臂子课程，本章 §9.3 的理论祖先。 - 浮动基座 whole-body 控制（TSID、分层零空间）— 见本项目 05 足式 90_WBC分层优化与TSID.md、机械臂 F07_浮动基座WBC理论.md。 - Gerkey & Matarić, "A Formal Analysis and Taxonomy of Task Allocation in Multi-Robot Systems" (IJRR 2004) — MRTA 分类法，§9.2 联盟任务归类的依据（第 3 章 §3.1 详讲）。

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
第 10 章 MARL 基础（§110）	用数据驱动替代本章的手工协调	§9.5 双智能体范式、§9.2 能力分配是 MARL 协调要学的目标；本章的模型方法是 MARL 的对照基线
第 11 章 MARL 多机运动协调（§120）	学出本章 §9.3 的协调律	§9.3 三范式（领导跟随/内力/一致性）是 MARL 协调要逼近/超越的对象
第 12 章 MARL+规控混合（§130）	把本章模型骨架 + MARL 学习缝起来	§9.7 开放问题 4（安全保证）、§9.5 陷阱 2（模型与学习互补）直接导向 CBF 安全层
第 13 章 Mini-MultiBot 综合实战（§140）	复用本章 HeteroLocoMani 模块	累积项目的 `hetero_locomani/` 是综合实战的异构操作子系统
05 复合方向各章	体内协调（本章是体间对应）	§9.7 交叉地图：体内查 05 复合，体间查本章；共享 whole-body/抓取矩阵/ADMM 工具

🔧 故障排查手册¶

#	症状	可能原因	排查步骤	相关节
1	分配器把重载任务派给了人形/无人机（搬不动）	(a) 用了纯距离代价匈牙利；(b) 能力效用 \(U\) 与代价符号方向混用；(c) 可行性掩码未生效	① 检查出价函数是否含 \(\Phi_{kj}\cdot U_{kj}\)；② 单测"近但不擅长"vs"远但擅长"断言后者出价高；③ 打印不可行配对的掩码值，确认为 0	§9.2 陷阱 3、动机
2	三体抬物体时物体被越夹越紧/吱嘎作响（内力发散）	(a) 各体独立跟踪无协调；(b) 跟随者阻抗太硬；(c) 抓取矩阵坐标系不统一	① 确认是否有协调机制（ADMM/内力 QP/阻抗）而非独立跟踪；② 调软跟随者 \(K_p\)；③ 单测纯平移 \(w_o^d\) 断言分解力无净力矩	§9.3 动机、陷阱 1、陷阱 3
3	人形抬物体时被自己抬的物体拽倒	(a) whole-body 没把操作反力前馈进平衡约束；(b) 内力 QP 没加人形侧向力上限；(c) 侧向上限用了静态值而非动态反馈	① 检查 `add_balance_constraint(external_force=反力)` 是否存在；② 检查内力 QP 的 `is_humanoid` 侧向约束；③ 确认 \(\bar F^{\text{balance}}\) 从平衡控制器实时取	§9.3 陷阱 2、§9.6
4	物体在两套独立控制器接缝处脱手/拉扯	跨体闭链约束无人维护（各写各的，统一框架缺失）	① 确认有统一抓取矩阵 \(G\) 和闭链耦合约束；② 检查闭链约束是否微分到加速度层 + Baumgarte 稳定；③ 确认耦合约束分发到各体 MPC	§9.4 动机、陷阱 2
5	ADMM 协调不收敛/物体轨迹估计振荡	(a) 每周期只跑单轮 ADMM；(b) \(\rho\) 太大/太小；(c) 对偶更新符号反；(d) 在 1kHz 环里跑 ADMM	① 在 MPC 中频跑足够迭代轮数，不在 1kHz 环跑；② 网格搜 \(\rho\)；③ 检查对偶更新 \(u_k \mathrel{+}= (x_o^{(k)}-z)\) 符号；④ 画 \(\\|x_o^{(k)}-z\\|\) 收敛曲线	§9.3 陷阱 4、§9.6 陷阱 1
6	团队 whole-body QP 维度不匹配/崩溃或静默算错	堆叠异构个体时块尺寸写死（假设同构 \(n_k\) 相同）	① 用可变块尺寸 + 变量索引区间组装；② 断言总维度 \(=\sum_k(6+n_k)+6\)；③ 用稀疏矩阵库利用块对角	§9.4 陷阱 3
7	一台个体中途失效后整队瘫痪	(a) 用了领导者-跟随者且失效的是领导者；(b) 静态分配不支持动态重分配；(c) 用了集中式求解、协调器宕机	① 失效个体若是领导者，需推举新领导或切去中心化范式 C；② 启用 CBBA 周期性重拍卖；③ 优先分布式求解（§9.4）避免单点失效	§9.2 陷阱 2、§9.3 对比表、§9.4
8	实机协调远差于仿真（sim2real gap）	接触/摩擦建模不准（人形手/四足背/不同材质摩擦各异）	① 检查各接触点摩擦系数 \(\mu_i\) 是否按实测设；② 加摩擦/接触域随机化；③ 考虑学习方法补偿难建模接触（残差学习）	§9.7 开放问题 1

API 速查表¶

下列为本章伪代码与累积项目涉及的核心接口签名（教学约定，工程中按所用库调整）。

模块	接口	说明
morphology	`capability_utility(robot, task) -> float`	能力效用 \(U_{kj}\)=需求向量·能力画像，\(\bot\to-\infty\)
morphology	`feasibility_mask(robot, task) -> bool`	可行性掩码 \(\Phi_{kj}\)，硬不可行返回 False
hetero_cbba	`compute_bid(robot, task, bundle) -> float`	形态感知出价 = \(\Phi\cdot U\cdot e^{-\gamma\Delta t}\)（+协调亲和）
hetero_cbba	`split_coalition_task(task, n_req) -> list[SubTask]`	联盟任务拆为带角色子任务
hetero_cbba	`cbba_allocate(robots, tasks, comm_graph) -> Assignment`	两阶段 CBBA（捆绑构建 + 共识消解），支持周期重拍卖
grasp_matrix	`build_grasp_matrix(contact_points_in_obj_frame) -> G`	统一抓取矩阵 \(G\in\mathbb{R}^{6\times 3K}\)（所有点在物体质心系）
grasp_matrix	`null_space(G) -> N`	零空间基（内力方向），\(GN=0\)
force_allocation	`allocate_contact_forces(G, w_obj_des, contacts, balance_limits) -> f`	异构内力分配 QP：\(G^+w_o^d+N\eta\)，加权+摩擦锥+人形侧向约束
coordination	`impedance_force(x_ref, x_ee, Kp, Kd) -> F`	范式 A 阻抗律 \(K_p(x_{\text{ref}}-x_{\text{ee}})+K_d(\dot x_{\text{ref}}-\dot x_{\text{ee}})\)
coordination	`admm_coordinate(local_mpcs, comm_graph, rho, n_iter) -> obj_traj`	范式 C：本地 whole-body MPC + 物体轨迹一致性
team_wbc	`assemble_team_dynamics(robots) -> (M_block, coupling)`	块对角团队质量矩阵 + 操作力/闭链耦合
team_wbc	`add_closure_constraint(qp, J_e_k, obj_acc, baumgarte) -> None`	跨体闭链约束（加速度层 + Baumgarte 稳定）
humanoid_wbc	`humanoid_wbc(q, v, ee_force_target, base_ref, contact_state) -> tau`	人形 whole-body，操作反力前馈进平衡约束
pipeline	`run_pipeline(task_desc, team)`	六阶段编排，频率分层（分配/ADMM/内力/WBC/关节环异步）

研究实践建议¶

入门（建立直觉）： - 先把 05 复合方向的**单体** loco-manipulation 跑顺（四足+臂在 MuJoCo/IsaacLab 边走边抓）——这是本章一半的工具。再来看"多个身体"的协调。 - 用本章 §9.2 的形态感知 CBBA 做一个纯分配 demo（不涉及控制），直观感受"能力互补"如何自动把任务分化到不同形态。

进阶（动手协调）： - 实现 §9.6 的"静态抬起"——三体把茶几抬离地面且人形不倒。这一步会逼你真正理解内力分配和反力补偿（光看公式不够）。 - 对三范式各实现一个最小版本，在同一任务上对比通信量/精度/鲁棒性，亲手验证 §9.3 对比表。

研究级（攻开放问题）： - 选 §9.7 的一个开放问题深入。异构 sim2real（问题 1）和安全学习协调（问题 4）是当前最有价值、最缺好方法的方向。 - 关注章首脉络表里 2025 年的工作（Shared Object Manipulation、多双足搬运、FALCON），它们是博士选题的活跃前沿。 - 方法论建议：模型方法（本章）给你可解释的骨架和安全保证，学习方法（第 10-12 章）给你处理难建模因素的能力。最有前途的研究往往在两者的**结合处**（残差学习、模型引导 RL、学习增强的 whole-body）——不要把它们对立起来。

版本信息速查¶

项目	版本/取值	说明
文档类型	算法工程教学	text:code ≥ 60:40
章节定位	多机协作方向第 9 章	紧接第 5、6 章，承 05 复合单体 loco-manipulation
关键论文年份	2021（Go Fetch）~ 2025（FALCON/Shared Object Manip）	见科研发展脉络表
RoboDuet	arXiv:2403.17367（RA-L 2024）	双策略 loco-manipulation
FALCON	arXiv:2505.06776（2025）	双智能体力自适应人形
多双足搬运（Pandit）	arXiv:2406.17279（CoRL 2025）	免重训多双足协同
Shared Object Manipulation	arXiv:2510.00682（2025）	多足+臂团队搬运
RoCo	arXiv:2307.04738（ICRA 2024）	LLM 多机协作
求解器（伪代码）	scipy.optimize（教学）/ OSQP、qpOASES（工程）	QP 求解
仿真平台	MuJoCo / IsaacLab	累积项目验证

结语：本章站在两条研究主线的交汇点——单体 loco-manipulation 的成熟（§9.5）与多机协作的深入（§9.1-9.4）。你现在拥有了一套**模型驱动**的异构具身协调工具：会按能力分工、会跨形态协调、会用统一框架描述、会把它们串成端到端系统。但请记住 §9.5 陷阱 2 和 §9.7 的开放问题——模型方法是骨架，不是全部。接下来的第 10-12 章会引入**学习驱动**的协调，第 12 章把两者缝起来。那时回看本章，你会发现这里推导的每一个公式，都是混合方法里"可解释、可验证"那一半的全部根基。异构具身协作仍是 2025 年的前沿——这一章给了你进入它的完整地图。

第 9 章 人形/四足+臂协同控制——异构具身体的能力互补与跨形态协调¶

前置自测¶

本章目标¶

本章知识导航¶

科研发展脉络¶

前置知识桥接¶

如果跳过本章会怎样¶

预计阅读时间¶

本章符号约定¶

§9.1 强异构具身协作问题形式化 ⭐⭐¶

动机：当队友的"身体"根本不一样¶

如果不形式化、只凭直觉会怎样¶

历史：从同构、弱异构到强异构¶

理论：形态向量、能力画像与任务-能力匹配¶

1. 团队与个体¶

2. 形态向量 \(\mathbf{m}_k\)¶

3. 能力画像 \(\mathbf{c}_k\)¶

3.5 能力评估函数 \(g\) 的构造——把"身体"翻译成"能力"¶

4. 任务-能力匹配¶

5. 强异构带来的三个结构性后果¶

6. 深入一层：三种身体的动力学到底差在哪¶

7. 贯穿全章的主线：异构破坏了什么对称性（深入）¶

⚠️ 常见陷阱¶

练习¶

§9.2 能力互补任务分配——形态感知的 CBBA ⭐⭐⭐¶

动机：把第 3 章的分配器搬到异构团队¶

如果直接套用纯匈牙利会怎样¶

历史：从指派问题到联盟形成与市场机制¶

理论：形态感知的能力分配¶

1. 能力可行性掩码 \(\Phi\)¶

2. 形态感知出价 \(b_{kj}\)¶

3. 联盟任务（coalition task）处理¶

4. 完整分配流程¶

5. 手把手走一遍出价与冲突消解¶

6. 联盟任务的原子性（all-or-nothing）怎么实现¶

7. 动态能力与重分配：当个体的能力随状态改变¶

⚠️ 常见陷阱¶

练习¶

§9.3 跨形态协调控制三范式 ⭐⭐⭐⭐¶

动机：三个不一样的身体，抬同一张桌子¶

如果三个个体各自独立跟踪会怎样¶

历史：从遥操作双臂到 legged 团队搬运¶

理论：三种协调范式¶

范式 A：领导者-跟随者 + 阻抗协调¶

范式 B：统一抓取矩阵 + 内力分配¶

范式 C：分布式 MPC（ADMM）一致性协调¶

三范式对比¶

⚠️ 常见陷阱¶

练习¶

§9.4 统一 whole-body 框架——异构浮动基座的拼装 ⭐⭐⭐⭐¶

动机：能不能用一套数学描述整个团队¶

如果不统一、各写各的会怎样¶

历史：从单体 WBC 到 multi-robot whole-body¶

理论：团队级 whole-body 优化¶

1. 堆叠异构动力学¶

2. 跨体闭链耦合约束¶

3. 分层任务栈¶

4. 统一 ≠ 集中：从母问题到分布式¶

5. 从统一 QP 到 ADMM：分解为什么是"对"的¶

6. 收敛速率与通信图：ADMM 协调要跑多少轮¶

⚠️ 常见陷阱¶

练习¶

§9.5 双智能体协调——RoboDuet 与 FALCON 精读 ⭐⭐⭐¶

动机：单体内部也有"协调"问题¶

RoboDuet：两个协作策略实现单体 whole-body loco-manipulation¶

FALCON：双智能体实现人形力自适应 loco-manipulation¶

深入：双智能体的观测-动作接口设计¶

深入：双智能体训练的工程细节与典型失败模式¶

体内双智能体 ↔ 体间多智能体：完整映射¶

⚠️ 常见陷阱¶

练习¶

§9.6 端到端场景串讲——人形+四足×2 协同抬桌 ⭐⭐⭐¶

场景设定¶

完整流水线（六个阶段）¶

关键环节的伪代码¶

走一遍：当人形够取台灯阶段切换到抬桌阶段¶

数值走查：抬起瞬间的力预算¶

⚠️ 常见陷阱¶

工程落地：频率分层怎么并发跑¶

练习¶

第 9 章　人形/四足+臂协同控制——异构具身体的能力互补与跨形态协调¶