第 8 章　多足协同 Loco-Manipulation——腿即臂、协同步态与多机负载分配

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

本章定位：❌ 纯多足协同——本章是整个"多机协作"part（第 1-10 章）里唯一把**会走路的腿**同时当作**移动底盘**和**操作末端**来用的一章。前面第 5 章（协同搬运）和第 7 章（双臂多臂）讲的是"末端已经握住了物体，怎么分力"，它们默认底座是固定的或运动学独立的；本章要回答一个更难的问题：当每个 agent 的"手"就是它赖以站立的"脚"，当施加在物体上的接触力同时也是支撑机体不摔倒的地面反作用力，多机协同该怎么做？ 这把"运动（Loco）"和"操作（Manipulation）"在最底层——接触力——上锁死在了一起，这正是 "Loco-Manipulation" 这个词的全部张力所在。

一句话定位：从 2023 年 Kim–Hamed 的双 A1 分层协同（模型派标杆）到 2025 年 decPLM 的十台 Go2 零通信搬运（学习派标杆），本章完整讲透多足协同操作的三条技术主线——互联单刚体（SRB）网络 + 全息约束的模型化协同、whole-body 把"腿即臂"统一进一个 QP、以及接触力作为隐式通信的学习化涌现协调，并把推/搬/抬三种协作模式放在同一个力学框架下对比。

本章与众不同的视角：本项目在 05_运动控制/10_足式/ 已经有一整套**单体足式**控制课程（从 LIPM/SRB 简化模型、MPC、WBC 到 RL 训练栈），在 05_运动控制/30_复合/280_多机协作LocoMani.md 也有一章从**复合机器人**视角讲多机协作 loco-manipulation（任务分解、延迟鲁棒、局部自治）。本章既不重复单体足式的动力学推导，也不重复复合章的任务分解，而是站在**多机协作 part 的力学立场**，把单体足式的 SRB/WBC 工具升级为"\(N\) 个互联 SRB + 物体"的网络，把第 5 章的 grasp matrix、第 7 章的内力分解、第 4 章的分布式 MPC 全部"装到腿上"再跑一遍。读完本章你会发现：腿足把"协同搬运"从一个静态的力分配问题，变成了一个力分配与动态平衡耦合的时空问题——这是本章全部难度的来源，也是它独立成章的理由。

---

前置自测

📋 答不出 \(\ge 2\) 题 → 标 ① 的回第 4 章（分布式 MPC），标 ② 的回第 5/7 章（grasp matrix 与内力），标 ③ 的回 05_运动控制/10_足式 单体足式，标 ④ 的回第 1/2 章。本章会大量复用这些前置，但不重新教它们。

1. （接第 5 章 ②） 写出 grasp matrix \(G\) 的定义：\(N\) 个接触点的接触力 \(f_{\text{contacts}}\in\mathbb{R}^{3N}\) 如何映射到物体 wrench \(F_{\text{obj}}\in\mathbb{R}^6\)？内力（internal force）与运动力（motion force）的分解 \(f=G^{+}F_{\text{obj}}+(I-G^{+}G)f_{\text{int}}\) 中，为什么第二项不产生物体净运动？本章会把"接触点"换成"机器人附着点"，把这套分解原封不动搬过来。

2. （接 ③ 单体足式） 什么是**单刚体动力学（Single Rigid Body, SRB）模型**？它把腿足机器人简化成什么？质心动力学 \(m\ddot{p}=\sum_i f_i - mg\) 和角动量动力学 \(\dot{L}=\sum_i (r_i-p)\times f_i\) 中，\(f_i\) 是什么、\(r_i\) 是什么？为什么 SRB 假设"腿质量可忽略"？（答不出 → 回 05_运动控制/10_足式/70_腿足简化模型理论.md §51.4 SRB 模型）

3. （接 ③ 单体足式） 什么是**摩擦锥（friction cone）约束**和**单边接触（unilateral contact）约束**？为什么腿足机器人的地面反作用力（Ground Reaction Force, GRF）只能"推"不能"拉"、且切向力受法向力限制？线性化摩擦锥（friction pyramid）长什么样？（答不出 → 回 05_运动控制/10_足式/80_接触力学与约束优化.md）

4. （接 ③ 单体足式） 什么是 whole-body control（WBC，全身控制） 的分层 QP？它如何把"质心轨迹跟踪、足端力跟踪、摆动腿跟踪"这些任务按优先级塞进一个或一串 QP？为什么 MPC（在简化模型上）和 WBC（在全阶模型上）通常是**双层**配合而非二选一？（答不出 → 回 05_运动控制/10_足式/90_WBC分层优化与TSID.md）

5. （接第 1 章 ④ / 第 4 章 ①） 集中式联合优化的 \(O(N^3)\) 爆炸是怎么回事？为什么 \(N\) 台机器人的联合 MPC 维度随 \(N\) 增长、求解时间随 \(N^3\) 左右增长？分布式 MPC（ADMM / 对偶分解）用什么换取了可扩展性？（答不出 → 回第 4 章 §4.1、§4.3、§4.4）

---

前置自测参考答案要点

先自己答，再对照。这些是本章每一步推导的地基，欠账会在 §8.3（互联 SRB 网络）卡住。

1. \(F_{\text{obj}}=G\,f_{\text{contacts}}\)，\(G\in\mathbb{R}^{6\times 3N}\) 第 \(i\) 列块 \(g_i=\begin{bmatrix}I_3\\ [r_i]_\times\end{bmatrix}\)（\(r_i\) 为接触点到物体质心向量，\([\cdot]_\times\) 是反对称叉乘矩阵）。\(N\ge2\) 时 \(G\) 行满秩，\(\dim\ker(G)=3N-6\) 即内力空间。第二项 \((I-G^{+}G)f_{\text{int}}\in\ker(G)\)，代入 \(G(I-G^{+}G)=G-GG^{+}G=G-G=0\)（用伪逆性质 \(GG^{+}G=G\)），故对物体零净 wrench——它只在物体内部"较劲"。

2. SRB 把整机简化成一个**带转动惯量的刚性质量块**，忽略腿的质量与四肢运动对总动量的贡献。\(f_i\) 是第 \(i\) 条腿的地面反作用力（3 维），\(r_i\) 是该足端接触点位置，\(p\) 是质心位置。腿质量可忽略是因为四足/人形的腿相对躯干轻（A1 单腿约占体重的 5-8%），且摆动腿的动量变化在中低速下远小于躯干——这让角动量项里只剩"力臂×力"而无四肢转动惯量项，把非线性浮动基动力学压缩成一个 6 维刚体方程。

3. 摩擦锥：切向力 \(\|f_t\|\le\mu f_n\)（\(\mu\) 摩擦系数，\(f_n\) 法向力），物理上脚不能在地面打滑。单边接触：\(f_n\ge0\)，脚只能"踩"（推地）不能"粘"（拉地）——地面不会拽住脚。线性化为四棱锥：\(|f_{t,x}|\le\tfrac{\mu}{\sqrt2}f_n\) 且 \(|f_{t,y}|\le\tfrac{\mu}{\sqrt2}f_n\)，把二阶锥约束降为线性不等式，让 QP/MPC 可用更快的 QP 求解器。

4. WBC 分层 QP 把任务按优先级排序，高优先级任务在其零空间内才轮到低优先级，决策变量通常是 \([\ddot q,\ f,\ \tau]\)（广义加速度、接触力、关节力矩），约束含全阶浮动基动力学、接触约束、力矩限幅。MPC 在 SRB（低维、看远）上算"未来该往哪走、每只脚出多大力"，WBC 在全阶模型（高维、看当下）上把这个力指令落实到每个关节力矩并维持全身一致——一个管"规划层的远见"，一个管"执行层的精确"，故双层互补。

5. 联合 MPC 把 \(N\) 机的状态/输入拼成一个大向量，维度 \(\propto N\)；QP/Riccati 求解的稠密线性代数随维度三次方增长，故 \(\sim O(N^3)\)。分布式 MPC 用 ADMM 把联合问题按 agent 拆成 \(N\) 个小子问题并行解，仅交换边界/对偶变量（一致性裁决），把单次求解降到 \(O(N)\times\) 小问题代价，换来可扩展性——代价是需要多轮迭代收敛、且耦合约束只在收敛时才严格满足。

---

本章目标

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

1. 建立"腿即臂"的统一视角并量化其代价：说清为什么腿足机器人不需要额外机械臂就能操作（4 条 3-DoF 腿 + 6-DoF 浮动躯干提供的工作空间），写出"用一条腿当臂"时支撑多边形（support polygon）收缩导致的稳定性损失，并据此判断一个任务该用"腾出一条腿操作"还是"全脚支撑+躯干/整机推压"。

2. 把单体 SRB 升级为互联 SRB 网络：从第 4 章的单体 SRB 出发，推导 \(N\) 个机器人 + 1 个物体的耦合动力学，写出**全息约束（holonomic constraint）**如何把"机器人附着点位移一致"编码进优化，理解 Kim–Hamed 为何保留**相互作用 wrench（interaction wrench）\(\lambda\) 作为决策变量**而非消去它，并用牛顿第三定律解释物体侧的力平衡。

3. 把 grasp matrix / 内力分解搬到多足上：复用第 5/7 章的 \(f=G^{+}F_{\text{obj}}+(I-G^{+}G)f_{\text{int}}\)，说明在多足搬运中"运动力"如何分给各机器人、"内力"如何被各机器人的全身控制维护，并解释为什么多足的内力调控比固定臂更难（每个 agent 的施力能力受自身平衡和摩擦锥双重约束）。

4. 推导并对比三种协作模式（推/搬/抬）的力学：用同一个 wrench 平衡框架写出**协同推（pushing，非抓持、纯摩擦/法向接触）、**协同搬（carrying，刚性/夹持耦合）、**协同抬（lifting，pinch 捏举 + 高度维持）**的约束差异，说明为什么"推"最鲁棒但最难精确控姿、"抬"最精确但对协调和接触维持要求最高。

5. 构建多机 CoM 协调与负载分配的分层架构：把"高层 MPC 规划物体轨迹 + 各机 CoM/接触力 → 低层 WBC 落实到关节"的双层结构在多机上展开，说明 CoM 协调中**踏步起伏（locomotion）与负载操作（manipulation）的频段分离**，并用 QP 写出兼顾"摩擦锥 + 各 agent 能力差异 + 内力期望"的负载分配。

6. 解释学习化协同的涌现机制并与模型派对比：讲清 decPLM 的 **constellation reward（星座奖励）**如何把"位置+姿态对齐"统一成一组点的距离从而逼出刚性接触行为，**接触力作为隐式通信**如何在零显式通信下涌现协调，**permutation-invariant（排列不变）**网络与参数共享如何实现 \(N{=}2\) 训练→\(N{=}10\) 部署的零样本扩展，并能在一张表里对比"模型派（MPC/SQP）vs 学习派（MARL）"在多足搬运中的优劣与适用边界。

7. 按真实问题选型并接到执行：给定一个"\(k\) 台四足搬一个物体过一段地形"的真实任务，能按"是否需要精确控姿 × 是否需要通信 × 物体能否抓持 × 规模 × 是否上真机"选出 MPC / 分布式 MPC / 集中式 SQP / MARL 中的合适方案，并说明各自的 sim-to-real 风险（尤其接触力的仿真-真实差异）与缓解手段。

本章知识导航

一句话定位：本章把"多机协同"从"末端已握住物体的力分配"（第 5/7 章）和"机体各自独立运动的编队"（第 4 章）推进到**最纠缠的一种耦合——施力的手就是站立的脚**。这把第 4 章的"互联 SRB 网络"、第 5 章的"grasp matrix 与内力"、第 7 章的"内力调控与主从/协调辩论"、以及 05_运动控制/10_足式 的"单体 MPC+WBC"在"多足搬一个物体"这一个场景上**全部焊死在一起**，再叠加一条第 10 章 MARL 的学习化路线。

本章六条主线及其递进关系：

主线	解决的问题	关键工具	对应小节
腿即臂的视角与代价	不加臂，腿怎么操作？腾腿 vs 整机推压怎么选？	工作空间、支撑多边形收缩、稳定裕度	§8.1
互联 SRB 网络	\(N\) 机 + 物体的耦合动力学怎么写？	全息约束、interaction wrench、牛顿第三定律	§8.2
多足负载分配	运动力/内力怎么分给各机？为什么比固定臂难？	grasp 伪逆、内力空间、摩擦锥+平衡双约束、QP	§8.3
推/搬/抬三模式	三种协作模式的力学差异与选型？	非抓持接触、刚性耦合、pinch 捏举、wrench 平衡	§8.4
协同步态与 CoM 协调	边走边搬，步态怎么协调？质心起伏与负载怎么解耦？	接触时序、频段分离、双层 MPC+WBC、whole-body	§8.5
学习化涌现协调	零通信怎么协调？怎么扩展到 N 台？	constellation reward、接触力隐式通信、排列不变、MARL	§8.6
模型 vs 学习总览与选型	这么多方法，真实任务选哪个？	全景总表、sim-to-real、选型决策框架	§8.7

推荐阅读路径：§8.1 是视角地基（必读，它决定你怎么"看"后面所有内容）→ §8.2 互联 SRB 网络是模型派的数学引擎（模型方向必读，需跟着推导）→ §8.3 负载分配是把第 5/7 章接到腿上的关键一节（做力分配必读、需动手）→ §8.4 推/搬/抬是把抽象力学落到三类真实任务（必读，建立任务直觉）→ §8.5 协同步态把"静态搬"升级为"边走边搬"（做动态搬运必读、最硬核）→ §8.6 学习化协同是与模型派并列的另一条腿（做 RL 方向必读，模型方向可速读结论）→ §8.7 是全景与选型收口（建议最后通读、当索引常查）。§8.2、§8.3、§8.5、§8.6 是四个 ⭐⭐⭐/⭐⭐⭐⭐ 的硬核小节，值得花最多时间。

前置知识桥接

本章紧接第 4、5、7 章并大量复用 05_运动控制/10_足式，这里把最关键的前置点重新激活——你不必翻回去就能跟上：

• 回顾第 4 章：互联 SRB 网络的雏形。第 4 章 §4.5 我们已经把"会走路的机器人"装进了分布式 MPC——把每台机器人建模为一个单刚体（SRB），\(N\) 台机器人通过"编队约束"耦合，并指出忽略机间耦合力直接堆 \(N\) 个单体 SRB 会出问题。本章把这个雏形推到极致：第 4 章的耦合是**软的**（保持队形，机间不接触），本章的耦合是**硬的**（机间通过一个共享刚性物体接触，可写出精确的代数全息约束）。第 4 章的"机间力"是可选的协调项，本章的"interaction wrench"是必须显式建模的物理约束。你会看到第 4 章的 \(\rho\) 调节、对偶分解、ADMM 在本章的负载分配上再次出场。

• 回顾第 5 章：grasp matrix 与内力/运动力分解。第 5 章用 \(G\in\mathbb{R}^{6\times 3N}\) 把接触力映射到物体 wrench：\(F_{\text{obj}}=Gf_{\text{contacts}}\)，并把接触力分解为运动力 \(G^{+}F_{\text{obj}}\)（驱动物体运动）和内力 \((I-G^{+}G)f_{\text{int}}\)（落在 \(\ker(G)\)，互相挤压）。本章把"接触点"换成"机器人附着点"：每台四足背上的接触/夹持点就是一个"手指"，多台四足搬一个物体就是一个 \(N\) 指 grasp。第 5 章讲的是这套分解的静力学，本章把它和**腿足的动态平衡**耦合起来——这是最关键的升级：固定臂施力只受摩擦锥约束，多足施力还同时受"不能把自己推翻"的约束。

• 回顾第 7 章：内力调控与"主从 vs 协调"辩论。第 7 章把双臂当作 \(N=2\) 的多机协同操作，深入了内力闭环维护（不能开环施加）、object/internal 双层阻抗、以及集中/分布/去中心化三分法下的主从 vs 对称协调辩论。本章把这场辩论搬到多足：decPLM 的"零通信参数共享"是一种极端去中心化的对称协调，Kim–Hamed 的集中式 MPC 是集中式，其分布式版本是介于两者之间的 leader-follower 软化。第 7 章的内力直觉在这里直接复用。

• 回顾 05_运动控制/10_足式：单体 SRB + MPC + WBC。单体足式课程给了我们三件核心工具：(1) SRB 简化模型（§51.4，质心+角动量动力学）；(2) 凸 MPC 在 SRB 上规划足端力（§120 步态、接触序列）；(3) WBC 分层 QP 把力指令落实到关节（§90 WBC/TSID）。本章不重新推导这三件工具，而是把每一件都"复制 \(N\) 份再加耦合"：\(N\) 个 SRB、一个考虑机间约束的联合/分布式 MPC、\(N\) 个各自跑但共享内力目标的 WBC。如果你对这三件工具薄弱，强烈建议先回单体足式补齐。

前向预告：本章的多足协同是"同构（homogeneous）多足 + 一个物体"。下一章（第 9 章，人形四足臂协同控制）会进一步处理**异构形态**（人形 + 四足 + 带臂）的协同 loco-manipulation；而第 10 章（MARL 基础）会把本章 §8.6 用到的 MAPPO、参数共享、CTDE 等学习工具系统化。现在只需记住：本章 §8.6 的学习化协同是第 10 章 MARL 在"多足搬运"这个具体任务上的一次完整落地预演——你在这里先见到它的威力，第 10 章再补齐它的理论。

如果跳过本章会怎样

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

• 场景一（把"协同搬运"当静态力分配，机器人边搬边摔）：你读完第 5 章，以为多足搬运就是"算出 grasp matrix，分配运动力和内力，各机器人按分配出力"。于是你让两台四足夹着一根梁，用第 5 章的 QP 算出每台该出多大力，直接下发。结果梁是端起来了，但其中一台四足为了提供它"该出的那份力"，质心被拉出了支撑多边形——它摔了。根本原因：固定臂的基座焊在地上，施多大力都不会翻；但四足施加在物体上的每一牛顿力，按牛顿第三定律都有等大反力作用回它自己的躯干，这个反力必须被它的足端 GRF 平衡，否则质心失稳。§8.2-8.3 会告诉你：多足负载分配必须把"摩擦锥 + 不翻倒"作为**双重约束**联合求解，而不能像固定臂那样只看摩擦锥。

• 场景二（边走边搬，步态把物体晃飞）：你让四台四足抬着一张桌子往前走。每台四足独立跑自己的小跑（trot）步态——抬左前右后、再抬右前左后，质心随之周期性起伏几厘米。四台机器人的步态相位没有协调，于是桌子被四个不同步的起伏"拧"得剧烈晃动，最后桌上的东西飞了出去。根本原因：单机搬运时质心起伏只影响自己，多机搬运时每台机器人的质心起伏都通过刚性物体传给其他机器人，步态的周期性扰动成了注入物体的噪声。§8.5 会告诉你：协同步态必须做相位协调和频段分离——让"操作物体的低频运动"和"踏步的高频起伏"在频域上分开，甚至刻意让各机步态反相以相互抵消起伏。

• 场景三（零通信搬运，机器人互相对抗）：你想做一个去中心化的多足搬运，每台机器人不通信、各自跑一个策略。你天真地让每台机器人都独立地"把物体推向目标"。结果两台机器人从物体两侧推，推力方向相反，物体纹丝不动，两台机器人互相对抗、空耗能量。根本原因：没有协调机制时，"各自把物体推向目标"在物理上会退化成内力对抗。§8.6 会告诉你：要么用 constellation reward 让每台机器人学会"维持与物体的相对位姿"从而隐式同步（decPLM 的做法），要么用分层 MARL 让一个高层策略给各机分配不冲突的子目标（MA-Push 的做法）——零通信不等于零协调，协调可以藏在奖励设计和接触力里。

预计阅读时间

模式	时长	适合
精读（跟着推完互联 SRB 网络、手算双足全息约束、实现多足负载分配 QP、读懂 constellation reward 与协同步态频段分离）	18-24 小时	第一次系统学多足协同 loco-manipulation 的工程师，建议分 4-6 次
速读（读懂三条主线流程与适用边界，跳过推导细节与代码）	5-7 小时	已有单体足式 + 多机协作背景，查漏补缺
速查（互联 SRB 网络方程 + 推/搬/抬对比表 + 模型 vs 学习总表 + 选型决策框架 + 故障排查）	50-60 分钟	实现多足协同搬运时回查

---

科研发展脉络

在钻进具体方法前，先把这条研究线的来龙去脉理清——知道每个方法"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记名字有用得多。多足协同 loco-manipulation 是一个**年轻且爆发**的方向：2023 年才有第一批严肃的双机标杆，2025 年学习派已经做到十台零通信。

年份	论文	Venue	核心贡献
2019	Fawcett, Hamed 等, "Distributed Feedback Controllers for Stable Cooperative Locomotion"	ACC	奠基：虚拟约束 + 全息约束建模两机协同步行的早期工作
2023	Kim, Fawcett, Kamidi, Ames, Hamed, "Layered Control for Cooperative Locomotion of Two Quadrupedal Robots"	T-RO	模型派标杆：双 A1 全息约束 + 高层 SRB-MPC + 低层分布式 QP；集中式与分布式两种
2023	De Vincenti, Coros, "Centralized MPC for Collaborative Loco-Manipulation"	RSS	robot/payload/gait-agnostic SQP：一套稀疏 SQP 解任意多足 + 任意负载 + 任意步态
2024	An, Lee, Bjelonic, De Vincenti, Hutter, "Solving Multi-Entity Robotic Problems Using Permutation Invariant Neural Networks"	RSS	排列不变网络：对 agent 数量与排列不变的策略，天然可扩展
2024	Feng 等, "Learning Multi-Agent Loco-Manipulation for Long-Horizon Quadrupedal Pushing"（MA-Push）	arXiv	分层 MARL 推大物体：高层 RRT+集中策略给子目标，中层去中心化共享策略
2025	Pandit, Shrestha, Fern 等, "decPLM: Multi-Quadruped Cooperative Object Transport"	arXiv	学习派标杆：2-10 Go2+臂零通信捏举搬运；constellation reward + MAPPO + 域随机化
2025	"Safety-Critical Centralized NMPC for Cooperative Payload Transportation by Two Quadrupeds"	arXiv	安全攸关：双 Go2 + 全息约束 + 高阶 CBF；11ms@60Hz；负载达自重 74.67%
2026	"ACLM: ADMM-Based Distributed MPC for Collaborative Loco-Manipulation"	arXiv	分布式 MPC + WBC：用 ADMM 处理预抓持协同 loco-manipulation 的耦合

两条清晰的主线：

1. 模型派主线：Fawcett/Hamed 的虚拟约束（2019）→ Kim–Hamed 的分层 SRB-MPC（2023）→ De Vincenti–Coros 的 agnostic SQP（2023）→ 安全攸关 CBF-NMPC（2025）→ ACLM 的 ADMM 分布式（2026）。这条线的演进是：从两机到任意机、从无安全保证到 CBF 硬保证、从集中式到分布式——每一步都在改善可扩展性和安全性，核心数学始终是"互联 SRB + 全息约束 + 把 interaction wrench 当决策变量"。

2. 学习派主线：MA-Push 的分层 MARL 推大物体（2024）→ An–Hutter 的排列不变网络（2024）→ decPLM 的零通信捏举搬运（2025）。这条线的演进是：从有通信/有集中高层到完全去中心化、从固定 agent 数到任意 agent 数、从推（非抓持）到捏举（pinch）——核心思想是"接触力作为隐式通信"和"奖励设计逼出协调"。

关键实验室脉络：Virginia Tech（Hamed 组，模型派全息约束+分层 MPC 的大本营，与 Caltech Ames 组合作）→ ETH Zürich（Coros CRL 组的 agnostic SQP + Hutter 组的排列不变学习）→ Oregon State / 多校（decPLM 的去中心化学习）。看这条线，模型派从 VT/Caltech 出发追求"可证明的稳定与安全"，学习派从 ETH/OSU 出发追求"可扩展的涌现协调"——两条线在 2025-2026 开始交汇（如 ACLM 把学习增强塞进分布式 MPC）。

本质洞察：多足协同 loco-manipulation 的核心矛盾，不是"怎么搬动物体"（那是第 5 章的力分配问题），而是**"怎么在搬动物体的同时不把自己搞翻"**。固定臂操作把"操作"和"支撑"解耦了（基座负责支撑、臂负责操作），而腿足把两者重新耦合回同一组接触力——这个再耦合，正是从"协同搬运"到"多足协同 loco-manipulation"的本质跃迁，也是本章所有方法（无论模型派还是学习派）真正在解决的问题。

---

本章符号约定

符号	含义	首见
\(N\)	协同机器人数量	§8.1
\(p_i,\ \dot p_i\)	第 \(i\) 台机器人质心位置 / 速度	§8.2
\(\theta_i,\ \omega_i\)	第 \(i\) 台机器人姿态（欧拉角）/ 角速度	§8.2
\(x_i=\mathrm{col}(p_i,\theta_i,\dot p_i,\omega_i)\)	第 \(i\) 台机器人 SRB 状态	§8.2
\(m_i,\ I_i\)	第 \(i\) 台机器人质量 / 转动惯量	§8.2
\(f_{ij}\)	第 \(i\) 台机器人第 \(j\) 条腿的地面反作用力（GRF）	§8.2
\(r_{ij}\)	第 \(i\) 台机器人第 \(j\) 个足端接触点位置	§8.2
\(x_L,\ p_L,\ \theta_L\)	物体（payload/load）状态 / 位置 / 姿态	§8.2
\(m_L,\ I_L\)	物体质量 / 转动惯量	§8.2
\(\lambda_{iL}\)	第 \(i\) 台机器人与物体间的相互作用 wrench（interaction wrench）	§8.2
\(\phi_{iL}\)	第 \(i\) 台机器人与物体间的全息约束（holonomic constraint）	§8.2
\(G\)	grasp matrix（接触力→物体 wrench 映射）	§8.3
\(F_{\text{obj}}\)	作用在物体上的合 wrench	§8.3
\(f_{\text{int}}\)	内力（落在 \(\ker G\) 内）	§8.3
\(\mu\)	摩擦系数	§8.3
\(\sigma_{ij}(t)\)	接触时序 / 步态（0/1 变量，松弛为连续）	§8.5
\(\mathcal{S}_i\)	第 \(i\) 台机器人的支撑多边形（support polygon）	§8.1
\(h(\cdot)\)	控制屏障函数（Control Barrier Function, CBF）	§8.4
\(\pi_\theta\)	策略网络（参数 \(\theta\)）	§8.6
\(d_{\text{con}}\)	星座距离（constellation distance）	§8.6

---

§8.1 腿即臂：视角、工作空间与"腾腿 vs 整机推压"的代价 ⭐⭐

这一节解决什么问题：在动手推任何公式之前，先得想清楚一件事——腿足机器人凭什么能"操作"物体？它的"手"在哪里？当我们说"多足协同 loco-manipulation"，每个 agent 到底用什么部位去接触物体？这一节建立"腿即臂"的统一视角，量化"腾出一条腿当臂"的稳定性代价，并给出"腾腿操作"和"全脚支撑+整机推压"的选型判据。它是后面所有力学推导的物理起点——你必须先知道力作用在哪、约束从哪来，才谈得上写方程。

动机：一台四足，凭什么能"操作"？

想象一个最朴素的场景：地上有一个箱子，你想让一台 Unitree Go2（一台没有任何机械臂的四足机器人）把它推到墙角。你会怎么做？

最直觉的答案是：让机器人走到箱子边，用它的**身体（躯干前部）顶着箱子往前推。这就已经是一次 loco-manipulation 了——机器人在**运动（loco，走过去并持续前进）**的同时**操作（manipulation，对箱子施加了一个有目的的力）。注意这里发生了一件微妙的事：机器人**没有"手"**，它用来操作的"末端"是它的躯干，而提供推力的，归根结底是它四条腿的地面反作用力（GRF）——腿往后蹬地，地面反推机器人前进，机器人把这个力通过躯干传给箱子。

现在把场景升级：箱子很重，一台推不动，需要两台、三台 Go2 一起推。或者箱子需要被**抬起来**而不只是推——这时机器人就得腾出某种"抓持"能力。Go2 没有夹爪，但 decPLM 的做法是给每台 Go2 装一个轻量臂（D1 arm），用臂末端的橡胶垫去"捏"住物体。又或者，像更早的一些工作那样，机器人**抬起一条前腿**，把这条腿当作一条 3-DoF 的机械臂去够、去推、去按物体——剩下三条腿继续支撑。

这三种做法——躯干推、加臂捏、腿当臂——揭示了多足 loco-manipulation 的第一个核心问题：腿足机器人的"操作末端"不是唯一的，而是一个谱系。理解这个谱系，是理解后面所有方法的前提。

如果照搬"固定臂操作"的思维会怎样

在动手之前，先看一个**反面**：如果我们把"操作"完全按照固定机械臂的思维来理解，会在多足上栽什么跟头？

固定机械臂操作的标准心智模型是：基座（base）焊死在地上或固定平台上，负责提供一个稳定的、不会动的支撑；臂（arm）从基座伸出，负责操作。这个模型的关键假设是**操作与支撑彻底解耦**——臂施加多大的力、朝什么方向施力，都不会影响基座的稳定性，因为基座被螺栓拧死了，地面/平台会无条件提供任何需要的反作用力。第 5 章和第 7 章的 grasp matrix、内力分解，全都建立在这个"基座无条件稳定"的前提上。

把这个心智模型照搬到四足上，会立刻出三个问题：

1. 基座会动，而且必须靠操作用到的同一组力来稳住。四足的"基座"是它的浮动躯干，靠四条腿的 GRF 悬在空中。当它用躯干顶箱子、或用一条腿推物体时，按牛顿第三定律，物体会给它一个等大反力。这个反力作用在躯干上，必须被剩下支撑腿的 GRF 平衡掉——否则躯干的质心会被推出支撑多边形，机器人翻倒。操作力和支撑力是同一组力的两个面孔，根本没法解耦。

2. 施力能力受"不翻倒"约束，而不只是摩擦锥。固定臂施力的唯一约束是末端摩擦锥（别打滑）和电机力矩上限。四足施力还多了一条**致命约束：合力不能让质心失稳**。一台四足理论上能用腿提供很大的水平力，但如果这个力让它的质心投影跑出支撑多边形，它会先于"推动物体"而摔倒。

3. "腾出一条腿当臂"要付支撑面积的代价。如果用腿当臂，四足就从"四脚支撑"变成"三脚支撑"，支撑多边形从一个四边形缩成一个三角形，面积骤减，稳定裕度大幅下降。固定臂没有这个问题——它有几条臂就用几条，不影响支撑。

本质洞察：固定臂操作的世界里，"支撑"是免费的、无限的、与"操作"正交的；多足 loco-manipulation 的世界里，"支撑"是稀缺的、有限的、与"操作"共享同一组接触力的。本章后面所有看起来比第 5/7 章复杂的地方，根源都在这一句话——支撑与操作的再耦合。

历史：从"加臂"到"腿即臂"的两条路

腿足机器人获得操作能力，历史上走了两条路，至今仍在并行发展。

第一条路：加专用臂（legged manipulator）。最直接的思路是承认"腿是用来走的、臂是用来操作的"，于是在四足背上装一条机械臂（如 ANYmal + Kinova、Go2 + D1），或做成人形（双腿走、双臂操作）。这条路的好处是分工明确、可直接复用成熟的机械臂操作算法；代价是增加了重量、复杂度和成本，且臂的操作仍受底座（腿）平衡的约束（这正是 05_运动控制/30_复合 复合机器人章的主题）。decPLM（2025）走的是这条路的多机版——每台 Go2 加一条轻臂去捏物体。

第二条路：腿即臂（legs as limbs / loco-manipulation without arms）。一个更激进也更优雅的观察是：四足的腿本身就是 3-DoF 的操作臂。四条 3-DoF 腿加一个 6-DoF 浮动躯干，提供了相当大的工作空间和操作灵活性——为什么非要再加一条臂？于是有了"抬起一条腿去推门、按按钮、移动物体"的工作，以及更普遍的"用躯干本身作为操作面去推压物体"的协同推（collaborative pushing）。Kim–Hamed（2023）的双 A1 协同、MA-Push（2024）的多四足推大物体，走的都是这条"不加臂"的路。

本质洞察：这两条路不是非此即彼，而是同一个连续谱的两端。谱的一端是"专用臂操作"（操作末端与支撑完全分离），另一端是"整机推压"（操作末端就是支撑机体本身），中间是"腿当临时臂"（一条腿在支撑和操作之间切换）。本章的力学框架对整个谱系通用——无论操作末端是臂、是腿、还是躯干，它对物体施加的都是一个 wrench，受的都是"摩擦锥 + 不翻倒"约束。这是本章能用一套互联 SRB 网络统一处理所有形态的原因。

理论：工作空间、支撑多边形与稳定裕度的量化

现在把上面的直觉量化。我们关心三个量：操作工作空间、支撑多边形、稳定裕度。

（1）腿作为操作臂的工作空间。 一条四足腿通常有 3 个自由度（髋外展 hip abduction、髋俯仰 hip flexion、膝 knee），其足端在躯干坐标系下的可达空间是一个三维区域。当这条腿被"抬起来当臂"时，它能触及的操作空间就是这个区域（减去与地面/自身碰撞的部分）。这个空间不大——典型四足腿展开约 0.3-0.5 m——但对"推一个就在脚边的物体""按一个低处的按钮"已经够用。再叠加 6-DoF 浮动躯干（机器人可以走过去、可以俯身、可以侧倾），整机的可操作工作空间被大幅放大。这正是"腿即臂"成立的物理基础：腿的小工作空间 × 躯干的大移动性 = 足够的操作能力。

（2）支撑多边形与"腾腿"的代价。 定义机器人 \(i\) 的**支撑多边形（support polygon）** \(\mathcal{S}_i\) 为所有**当前触地足端**在水平面投影的凸包。静态稳定的必要条件是：质心（CoM）的水平投影落在 \(\mathcal{S}_i\) 内（更严格的动态判据用 ZMP/capture point，见 05_运动控制/10_足式/70_腿足简化模型理论.md §51.3）。

• 四脚支撑：\(\mathcal{S}_i\) 是四个足端构成的四边形，面积大，质心有较大的安全冗余。
• 三脚支撑（腾起一条腿当臂）：\(\mathcal{S}_i\) 退化为三个足端构成的三角形。问题来了——质心原本大致在四边形中心，腾掉一条腿后，三角形的形心通常偏离原质心投影，机器人必须先把质心**主动移到**新三角形内（通常向对角方向挪），才能安全地抬腿。这个"挪质心"动作要花时间、要协调，且挪的过程中稳定裕度很低。

我们可以粗略量化"腾腿"的代价。设四足为矩形布局，足距前后 \(2a\)、左右 \(2b\)，质心在中心。四脚支撑时质心到最近多边形边的距离（一种稳定裕度）约为 \(\min(a,b)\)。腾起一条前腿后，剩下三足（另一前足 + 两后足）构成三角形，质心若不移动，它到三角形最近边的裕度会显著下降，某些布局下甚至**质心直接落在三角形外**（裕度为负，立刻翻倒）。因此抬腿前必须把质心向后、向对侧移动 —— 这就是为什么真实的四足"用腿操作"动作总是先看到机器人"重心后坐"再抬腿。

（3）整机推压 vs 腾腿操作的选型判据。 综合工作空间和稳定裕度，给出一个工程选型表：

维度	整机推压（躯干/全脚）	腾腿操作（一腿当臂）	加专用臂
操作末端	躯干表面 / 全身	一条抬起的腿	独立机械臂
支撑	四脚全支撑，裕度大	三脚支撑，裕度小	四脚全支撑，裕度大
可施加力	大（全身可发力）	中（单腿力 + 平衡受限）	中（臂力 + 平衡受限）
操作精度	低（难精确控物体姿态）	中	高
抓持/抬举能力	仅推（非抓持）	弱（单腿难捏持）	强（夹爪可捏举）
适用任务	推大重物、协同推	够、按、轻推近处物体	精确取放、捏举搬运
典型工作	Kim–Hamed、MA-Push	抬腿够物类工作	decPLM、复合机器人

选型判据（一句话）：要力不要精度、物体不可抓 → 整机推压；要够到特定点、不需大力 → 腾腿；要精确取放或抬举 → 加臂。多机协同时，这个判据决定了你后面用哪条技术主线——整机推压天然适合 MA-Push 式的推（§8.4 推模式 + §8.6 MARL），加臂捏举天然适合 decPLM 式的抬（§8.4 抬模式 + §8.6 MARL）和 Kim–Hamed 式的刚性搬（§8.4 搬模式 + §8.2-8.3 模型派）。

深入：一个"腾腿代价"的完整数值算例

上面的稳定裕度公式偏抽象，这里用一组具体数字把"腾腿到底损失多少裕度、质心至少要挪多远"算到底。它演示的是练习 8.1.2 的方法（数字不同，思路一致），读完你就能对任意四足布局自己估。

设一台四足俯视为矩形，四足在躯干坐标系（原点为质心投影）下位于

\[ \text{FL}=(+0.18,+0.10),\ \text{FR}=(+0.18,-0.10),\ \text{HL}=(-0.18,+0.10),\ \text{HR}=(-0.18,-0.10)\ \text{(单位 m)} \]

第一步：四脚支撑的稳定裕度。 四足凸包是一个 \(0.36\times0.20\) 的矩形，质心投影在原点（矩形正中）。静态稳定裕度定义为质心投影到凸包最近边的垂直距离。矩形四条边到中心的距离分别是 \(\{0.18,0.18,0.10,0.10\}\)（前后边 \(0.18\)、左右边 \(0.10\)）。取最小值：

\[ d_{\text{margin}}^{(4)}=\min(0.18,0.10)=0.10\ \text{m} \]

也就是说四脚站立时，质心可以在保持稳定的前提下横向移动近 \(10\) cm——裕度相当宽松。注意瓶颈在**左右方向**（\(0.10<0.18\)），因为这台机器人前后比左右"站得开"。这解释了一个常识：腿距窄的机器人侧向更容易翻。

第二步：腾起右前腿（FR）后的三角形。 抬起 FR，剩下 \(\text{FL}=(+0.18,+0.10)\)、\(\text{HL}=(-0.18,+0.10)\)、\(\text{HR}=(-0.18,-0.10)\) 三点构成三角形。先判断原点（质心投影）是否在三角形内。

用重心坐标判定。三角形顶点记 \(A=\text{FL}\)、\(B=\text{HL}\)、\(C=\text{HR}\)，目标点 \(P=(0,0)\)。三角形有向面积

\[ 2S_{ABC}=(x_B-x_A)(y_C-y_A)-(x_C-x_A)(y_B-y_A)=(-0.36)(-0.20)-(-0.36)(0)=0.072 \]

再算 \(P\) 对三条边的有向面积符号。计算 \(P\) 关于边 \(BC\)、\(CA\)、\(AB\) 的重心坐标 \(u,v,w\)（应全部同号才在内部）。代入得 \(u=\tfrac{(x_B-x_P)(y_C-y_P)-(x_C-x_P)(y_B-y_P)}{2S}=\tfrac{(-0.18)(-0.10)-(-0.18)(0.10)}{0.072}=\tfrac{0.018+0.018}{0.072}=0.5\)，类似可得另两个为 \(0.25,0.25\)，三者皆正且和为 \(1\)——原点在三角形内。

第三步：三脚支撑的裕度，及"必须挪多远"。 原点虽在三角形内，但裕度比四脚小得多。算原点到三角形三条边的距离，最近的一条是斜边 \(\text{FL}\to\text{HR}\)（从 \((+0.18,+0.10)\) 到 \((-0.18,-0.10)\)，过原点附近）。该边所在直线方程为 \(\tfrac{x}{0.18}=\tfrac{y}{0.10}\)，即 \(0.10x-0.18y=0\)，原点到它的距离

\[ d=\frac{|0.10\cdot0-0.18\cdot0|}{\sqrt{0.10^2+0.18^2}}=0\ \text{m} \]

裕度为零！ 原点恰好落在斜边上——质心投影正卡在三角形的边界。这意味着：抬起 FR 的瞬间，机器人处于**临界稳定**，任何微小扰动（甚至抬腿动作本身的惯性力）都会把它推翻。要安全抬腿，必须先把质心**向支撑三角形内部**（即向左后方 \(\text{HL}\) 方向）移动。要拿回 \(d_{\text{margin}}\ge0.04\) m 的裕度（一个保守工程值），质心需沿斜边法向 \((0.10,-0.18)/0.206\) 的反方向移动约 \(0.04\) m，对应躯干在世界系里向左后约 \(4\) cm 的"重心后坐+侧坐"。

本质洞察（数值版）：这个算例把"腾腿代价"从定性变成了定量——同一台机器人，四脚裕度 \(0.10\) m、三脚裕度 \(0\) m，抬一条腿直接吃掉了全部裕度。这正是为什么真实四足"用腿够物"前必然先做一个明显的"重心转移"动作：不是设计者画蛇添足，而是力学的硬要求。多机协同时，每台要腾腿操作的机器人都要付这笔裕度，这也是 §8.1 选型表把"腾腿操作"的支撑裕度标为"小"、把"够、按、轻推"标为其适用任务的根本原因——腾腿后能用的操作力被平衡约束卡得很紧。

阶段小结：到这里 §8.1 的物理图景完整了——我们有了"腿即臂"的谱系（躯干推/腿当臂/加臂）、量化了腾腿的支撑代价（上面的算例）、给出了选型判据。接下来 §8.2 要做的，是把"\(N\) 个会摔倒的手指 + 一个物体"这句话写成可以求解的耦合动力学方程。

多机视角：N 台机器人，N 个"手指"

把上面的单机视角推广到多机，得到本章的核心抽象：\(N\) 台多足机器人协同操作一个物体，等价于一个 \(N\) 指 grasp，其中每个"手指"是一台完整的、必须自己维持平衡的移动机器人。

这个抽象一句话讲清了多足协同与多臂协同（第 7 章）的异同：

• 相同：都是多个末端共持一个物体，都有 grasp matrix \(G\)、都有运动力/内力分解、都要做负载分配。第 5/7 章的全部力分配数学**直接适用**。
• 不同：多臂的"手指"挂在固定基座上（手指可以任意施力），多足的"手指"是一台会摔倒的机器人（每个手指的施力受自身平衡约束）。多足协同 = 多臂协同 + 每个 agent 一个"不翻倒"约束 + 每个 agent 一个"会移动的基座"。

本质洞察：理解了这个抽象，本章的结构就清晰了——§8.2 建立 \(N\) 个"会摔倒的手指 + 物体"的耦合动力学（互联 SRB 网络），§8.3 把第 5/7 章的力分配加上"不翻倒"约束（多足负载分配），§8.4 区分这些"手指"是推、是捏、还是刚性夹（推/搬/抬），§8.5 处理"手指会移动"带来的步态协调，§8.6 用学习的方法让"手指"们零通信地涌现协调。整章就是在给"\(N\) 个会摔倒的手指共持一个物体"这个抽象逐层填充细节。

⚠️ 常见陷阱

陷阱 8.1.1（概念误区）：把"loco-manipulation"理解成"先走过去，再操作"两个独立阶段 - 错误描述：以为 loco-manipulation 就是"导航到物体旁（loco）"然后"停下来操作（manipulation）"，两个阶段顺序执行、互不影响。 - 现象/后果：按这个理解设计的系统，在"边走边推""边走边抬"这类**同时运动与操作**的任务上完全失效——因为它没有处理"运动产生的扰动注入操作"和"操作的反力影响运动平衡"的耦合。 - 根本原因："loco-manipulation"这个词的全部价值就在于 loco 和 manipulation 在同一时刻、通过同一组接触力耦合发生。把它拆成两个独立阶段，等于丢掉了这个领域的核心问题。 - 正确做法：始终把 loco 和 manipulation 看成**同时**的、通过接触力耦合的。判断一个方法是否真的在做 loco-manipulation，就看它有没有显式处理"接触力既要支撑机体又要操作物体"这个双重角色。

陷阱 8.1.2（思维陷阱）：把多足协同直接当成"多个固定臂协同"，忽略平衡约束 - 错误描述：拿第 7 章的多臂负载分配 QP（只含摩擦锥约束），直接套到多足上，算出每台机器人该出多大力就下发。 - 现象/后果：力分配在"物体侧"完全正确（合力合力矩对），但某台机器人为了出它"该出的那份力"，质心被反力拉出支撑多边形，摔倒。 - 根本原因：固定臂的基座无条件稳定，施力只受摩擦锥约束；多足的"基座"（躯干）靠腿悬空，施力的反作用力必须被自身 GRF 平衡，多了一条"不翻倒"约束。第 7 章的 QP 缺这条约束。 - 正确做法：多足负载分配必须把"摩擦锥 + 各 agent 不翻倒（CoM 在支撑多边形内 / ZMP 可行）"作为**双重约束**联合求解（§8.3）。绝不能只搬第 7 章的力空间分配而丢掉平衡。

陷阱 8.1.3（概念误区）：以为"腿当臂"可以随时抬腿，不考虑质心转移 - 错误描述：想用一条前腿去够物体，直接命令这条腿抬起、伸向物体，不做任何质心调整。 - 现象/后果：腿一抬起，机器人从四脚变三脚支撑，若质心未提前移入新的三角支撑区，机器人瞬间向抬腿侧翻倒。 - 根本原因：四脚支撑多边形是四边形，腾掉一条腿后退化为三角形，原质心投影往往落在新三角形外或裕度极小。 - 正确做法：抬腿操作必须分两步——先把质心主动转移到剩余三足构成的三角形安全区（通常向对角方向"重心后坐/侧坐"），确认裕度足够后再抬腿。这与单体足式的"静态步态"质心转移逻辑一致（见 05_运动控制/10_足式 步态章）。

练习

练习 8.1.1（分析题·选型） 给定三个任务：(a) 三台四足把一辆抛锚的购物车推出泥地；(b) 一台四足按下墙上 0.4 m 高的电梯按钮；(c) 四台四足把一张餐桌端起来搬过 5 cm 高的门槛。对每个任务，用本节的选型表判断应该用"整机推压 / 腾腿操作 / 加专用臂"中的哪种，并说明你的判据（要力还是要精度？物体可抓持吗？需要抬离地面吗？）。对任务 (c)，进一步说明为什么"端起来"比"推着走"对协调的要求高得多。

练习 8.1.2（计算题·支撑多边形） 一台四足俯视为矩形，四足位于 \((\pm0.20,\ \pm0.12)\) m（前后 \(\times\) 左右），质心投影在原点。(1) 计算四脚支撑时质心到支撑多边形最近边的距离（稳定裕度）。(2) 现在抬起右前腿 \((+0.20,-0.12)\)，剩下左前 \((+0.20,+0.12)\)、左后 \((-0.20,+0.12)\)、右后 \((-0.20,-0.12)\) 构成三角形。判断原点（质心投影）是否还在这个三角形内；若在，求新的稳定裕度，若不在，求质心至少要向哪个方向移动多少才能重新进入。(3) 由此说明"抬腿操作"为什么必须先转移质心。

练习 8.1.3（开放思考题） 本节说"多足协同 = 多臂协同 + 每个 agent 一个不翻倒约束 + 每个 agent 一个会移动的基座"。请反过来想：如果把多足机器人的"基座会移动"这个特性**也看成一种能力而非负担**，它给协同操作带来了哪些固定臂没有的**优势**？（提示：固定臂的工作空间是死的，多足可以走位重新分布受力点、可以用整机移动给物体施加超出单点摩擦锥的等效力、可以在物体周围动态重构 grasp 几何……）列举至少 3 条，并各举一个能利用该优势的任务场景。

---

§8.2 互联 SRB 网络：N 台机器人 + 物体的耦合动力学 ⭐⭐⭐⭐

这一节解决什么问题：§8.1 给了物理直觉——"\(N\) 个会摔倒的手指共持一个物体"。这一节把它写成精确的方程：\(N\) 台机器人的单刚体（SRB）动力学，加上一个物体的刚体动力学，再用**全息约束（holonomic constraint）**把它们焊在一起，得到一个可以塞进 MPC 的耦合动力学系统。这是整个模型派的数学引擎——Kim–Hamed、安全攸关 NMPC、ACLM 全都建立在这套互联 SRB 网络上。读懂这一节，你就读懂了模型派多足协同的全部骨架。

动机：单体 SRB 不够用了

回顾 05_运动控制/10_足式/70_腿足简化模型理论.md §51.4：单体足式控制把一台四足简化成一个单刚体（SRB），写出两条动力学方程——质心动力学和角动量动力学：

\[ m\,\ddot p = \sum_{j=1}^{4} f_{j} - m g, \qquad \frac{d}{dt}(I\omega) = \sum_{j=1}^{4} (r_{j}-p)\times f_{j} \]

其中 \(p\) 是质心、\(f_j\) 是第 \(j\) 条腿的 GRF、\(r_j\) 是足端位置、\(I\) 是转动惯量、\(g\) 是重力加速度。这个 6 维模型（3 维平移 + 3 维转动）配上凸 MPC，是单体四足动态运动的主力工具。

现在的问题是：这个模型里没有"物体"，也没有"另一台机器人"。 当两台四足夹着一根梁时，每台机器人受的力不再只有四条腿的 GRF——还多了一个来自梁的**相互作用力**。而梁本身也有自己的动力学（它有质量、有转动惯量、要被搬到目标位姿）。单体 SRB 完全没有描述这些。我们需要把它扩展成一个**网络**：\(N\) 个 SRB（机器人）+ 1 个刚体（物体）+ 把它们连起来的约束。

如果直接堆 N 个独立 SRB 会怎样

最偷懒的扩展是：写 \(N\) 个互不相干的单体 SRB 方程，物体当成一个外部已知的"负载"，平摊到每台机器人头上。这正是第 4 章 §4.5 警告过的错误，这里再深化一层。

设想两台四足搬一根梁，你把梁重 \(m_L g\) 平均分成两份，每台机器人的 SRB 方程里加一个 \(\tfrac12 m_L g\) 的向下外力：

\[ m_i \ddot p_i = \sum_j f_{ij} - m_i g - \tfrac12 m_L g\, e_z \quad(\text{错误的解耦}) \]

这个模型有三个致命缺陷：

1. 没有捕捉机间的水平耦合力。如果机器人 1 突然加速向前，梁是刚性的，它会立刻把机器人 2 也往前拽——这是一个水平的相互作用力，上面的模型里根本没有。结果两台机器人的控制器各算各的，谁也不知道对方在拽自己，协调无从谈起。

2. 没有捕捉物体的转动。把物体简化成"两份重力"，丢掉了物体的转动惯量 \(I_L\) 和姿态 \(\theta_L\)。当两台机器人一前一后抬梁，梁会俯仰；当它们一左一右，梁会滚转。这些转动会改变每台机器人承受的力臂，进而改变它们的平衡——解耦模型完全看不到。

3. 力分配是"猜"的而非"解"的。凭什么是 \(\tfrac12\) 平摊？如果两台机器人能力不同、站位不同、地形不同，最优分配根本不是均分。把分配写死成 \(\tfrac12\)，等于放弃了优化负载分配的机会（§8.3 正是要优化这个）。

本质洞察：解耦 \(N\) 个 SRB 的根本错误，是把"物体"降格成"外部已知负载"。但物体不是外部的——它是**系统的一部分**，它的运动由 \(N\) 台机器人共同决定，它的反作用力又共同影响 \(N\) 台机器人。正确的做法是把物体当成网络中**与机器人平等的一个节点**，让它的动力学和机器人的动力学在同一个优化里联立求解。这就是"互联 SRB 网络"与"解耦 SRB"的本质区别。

历史：从虚拟约束到互联 SRB 网络

把多个腿足机器人耦合建模的思路，源头是 Hamed 组的早期工作。Fawcett, Hamed 等（ACC 2019） 用**虚拟约束（virtual constraint）+ 全息约束**的混合系统框架，建模两台四足协同步行——这是把"多台腿足机器人物理耦合"严格写进控制理论的奠基工作，但它偏重稳定性证明（混合零动态、Poincaré 映射），工程上偏重。

Kim, Fawcett, Kamidi, Ames, Hamed（T-RO 2023） 把这套思路落地成可实时运行的**分层架构**：高层用 SRB 模型的 MPC 规划，低层用分布式非线性/QP 控制器跟踪，并明确把两机通过物体的耦合写成**刚性全息约束**。这是本节要详细展开的"互联 SRB 网络"的标准形式。

安全攸关 NMPC（2025） 进一步把这个网络写成三个 SRB（两机 + 物体）的形式，并把**相互作用 wrench 保留为决策变量**——这是工程上极漂亮的一招，本节会重点讲为什么这样做。ACLM（2026） 则用 ADMM 把这个网络的求解分布式化。

这条线的核心数学**始终没变**：\(N\) 个 SRB + 物体 SRB + 全息约束 + interaction wrench 作决策变量。变的只是"怎么解"（集中 SQP → CBF-NMPC → ADMM 分布式）。所以这一节把数学讲透，后面 §8.3、§8.5 就只是在它上面加约束和换求解器。

理论：互联 SRB 网络的完整推导

我们一步步搭建这个网络。系统由 \(N\) 台机器人（索引 \(i\in\{1,\dots,N\}\)）和 1 个物体（索引 \(L\)，load）组成。

第 1 步：每台机器人的 SRB 动力学（加上来自物体的力）。

把单体 SRB 方程加上来自物体的相互作用 wrench。设 \(\lambda_{iL}\in\mathbb{R}^6\) 是物体作用在机器人 \(i\) 上的 wrench（前 3 维力 \(f_{iL}\)、后 3 维力矩 \(\tau_{iL}\)），作用在机器人的附着点 \(c_i\)（机器人与物体接触/连接的点）。机器人 \(i\) 的状态记作 \(x_i=\mathrm{col}(p_i,\theta_i,\dot p_i,\omega_i)\)，动力学为：

\[ \boxed{ \begin{aligned} m_i\,\ddot p_i &= \sum_{j} f_{ij} \;+\; f_{iL} \;-\; m_i g\, e_z \\ \frac{d}{dt}(I_i \omega_i) &= \sum_{j} (r_{ij}-p_i)\times f_{ij} \;+\; (c_i - p_i)\times f_{iL} \;+\; \tau_{iL} \end{aligned}} \]

对照单体 SRB，多出来的就是 \(f_{iL}\)（物体给的力）和它的力臂项 \((c_i-p_i)\times f_{iL}\) 加上力矩 \(\tau_{iL}\)。这就是"物体的存在"在机器人侧的全部体现。注意 \(\sum_j f_{ij}\) 仍是这台机器人四条腿的 GRF——它现在要同时干两件事：支撑自身重力 \(m_i g\)、**并**平衡来自物体的反力 \(f_{iL}\)。这正是 §8.1 说的"支撑与操作共享同一组力"的数学体现。

第 2 步：物体的刚体动力学（受所有机器人的反作用力）。

物体是一个刚体，受 \(N\) 台机器人通过附着点施加的 wrench。按**牛顿第三定律**，机器人 \(i\) 受到物体的 \(\lambda_{iL}\)，则物体受到机器人 \(i\) 的 \(-\lambda_{iL}\)（反向）。物体状态 \(x_L=\mathrm{col}(p_L,\theta_L,\dot p_L,\omega_L)\)，动力学为：

\[ \boxed{ \begin{aligned} m_L\,\ddot p_L &= -\sum_{i=1}^{N} f_{iL} \;-\; m_L g\, e_z \\ \frac{d}{dt}(I_L \omega_L) &= -\sum_{i=1}^{N}\Big[(c_i - p_L)\times f_{iL} + \tau_{iL}\Big] \end{aligned}} \]

负号是牛顿第三定律的直接结果——机器人推物体多用力，物体就反推机器人多用力，两者大小相等、方向相反。这个负号是整个网络耦合的关键：它让物体的运动和机器人的运动通过同一组 \(\lambda_{iL}\) 锁死。

阶段小结：到这里我们写出了 \(N+1\) 个刚体的动力学方程（\(N\) 个机器人 + 1 个物体）。它们通过 \(N\) 组相互作用 wrench \(\lambda_{iL}\) 耦合：每个 \(\lambda_{iL}\) 在机器人 \(i\) 的方程里以 \(+\) 出现、在物体方程里以 \(-\) 出现。接下来要回答一个关键问题：这些 \(\lambda_{iL}\) 从哪来？它们是已知的吗？——答案是它们由"机器人和物体必须保持连接"这个几何约束决定，这就是全息约束。

第 3 步：全息约束——把机器人和物体焊在一起。

\(\lambda_{iL}\) 不是凭空给定的，它由一个几何事实决定：机器人 \(i\) 的附着点 \(c_i\) 必须和物体上的对应连接点保持某种固定关系（刚性连接、或夹持）。这个"保持固定关系"就是**全息约束（holonomic constraint）**——一个只含位置/姿态、不含速度的代数约束 \(\phi_{iL}(x_i, x_L)=0\)。

最常见的是**刚性连接**（如 Kim–Hamed 的双 A1 用一根刚性杆连接，或安全攸关 NMPC 的刚性机械接口）。刚性连接意味着附着点的相对位置和相对姿态固定。一个完全刚性的 6-DoF 连接给出 6 个约束：

\[ \phi_{iL}(x_i,x_L) = \begin{bmatrix} \text{position}(c_i) - \text{position of attach point on } L \\ \text{orientation}(c_i) - \text{orientation of attach point on } L \end{bmatrix} = 0 \in \mathbb{R}^6 \]

初学者视角的疑问：你可能会问——为什么有时是 6 维约束、有时是 5 维？安全攸关 NMPC（2025）用的是"三平移 + 两转动（roll/pitch），放开 yaw"的 5 维约束。原因是它的机械接口允许两机绕竖直轴相对转动（像一个允许转动的铰），这给系统多一个自由度、更鲁棒。约束的维数取决于物理连接方式：完全焊死 = 6 维，允许某些方向相对运动 = 更少维。这正是 §8.4 推/搬/抬三模式的数学分界点——推是非抓持（约束维数最少甚至单边），刚性搬是 6 维约束，捏举介于中间。

第 4 步：把全息约束微分两次，变成可用于动力学的形式。

MPC/优化需要的是加速度层面的约束。对 \(\phi_{iL}(x_i,x_L)=0\) 关于时间求两次导（链式法则），得到**加速度层全息约束**：

\[ \boxed{\ddot\phi_{iL}(x, u) = J_{iL}\,\ddot q + \dot J_{iL}\,\dot q = 0} \]

其中 \(J_{iL}=\partial\phi_{iL}/\partial q\) 是约束雅可比，\(q\) 是广义坐标。这个 \(\ddot\phi_{iL}=0\) 就是连接 \(\lambda_{iL}\) 和系统运动的桥梁——它和动力学方程联立，唯一确定了 \(\lambda_{iL}\)。直观理解：约束告诉我们"附着点不能相对滑动/分离"，要维持这个不滑动，连接处必须产生恰好的力 \(\lambda_{iL}\)，就像两个齿轮咬合时接触点必然产生恰好的啮合力。

第 5 步：组装成完整系统。

把 \(N\) 台机器人的动力学、物体动力学、\(N\) 组全息约束堆在一起，得到完整的**互联 SRB 网络**。决策变量是：

\[ \underbrace{x_1,\dots,x_N}_{\text{机器人状态}},\quad \underbrace{x_L}_{\text{物体状态}},\quad \underbrace{f_{ij}}_{\text{各腿 GRF}},\quad \underbrace{\lambda_{1L},\dots,\lambda_{NL}}_{\text{相互作用 wrench}} \]

约束是：\(N\) 条机器人动力学 + 1 条物体动力学 + \(N\) 条全息约束 + 摩擦锥（§8.3）+ 接触时序（§8.5）。

一个手算到底的例子：两机抬梁的静态平衡

抽象方程容易让人云里雾里，我们用一个能手算到底的最小例子把上面五步走通——两台四足一前一后抬一根均质梁，静止水平。这个例子小到能在纸上算完，却包含了互联 SRB 网络的全部要素。

设置：梁质量 \(m_L\)、长 \(2\ell\)、均质（质心在中点）。两台机器人质量均为 \(m\)，分别在梁两端 \(x=\pm\ell\) 处刚性连接（附着点 \(c_1=(-\ell,0,h)\)、\(c_2=(+\ell,0,h)\)，\(h\) 为梁离地高度）。系统完全静止水平。求每台机器人对梁施加的 wrench \(\lambda_{iL}\) 和每台的腿 GRF 该如何配合。

第 1 步：写物体（梁）的平衡。 静止 → 加速度为零 → 物体动力学退化为静力平衡。梁受两机反力 \(-f_{1L}, -f_{2L}\)（机器人给梁的力是 \(-\lambda\) 中的力部分）和重力 \(-m_L g e_z\)：

\[ \text{力：}\ -f_{1L}-f_{2L}-m_L g e_z = 0 \;\Rightarrow\; f_{1L,z}+f_{2L,z}=m_L g \]

\[ \text{绕梁心力矩：}\ -(c_1-p_L)\times f_{1L} - (c_2-p_L)\times f_{2L}=0 \]

\(c_1-p_L=(-\ell,0,0)\)、\(c_2-p_L=(+\ell,0,0)\)（取梁心为原点、忽略 \(h\) 对水平力矩的贡献，只看竖直力产生的俯仰力矩）。竖直力 \(f_{iL,z}\) 产生绕 \(y\) 轴的力矩：

\[ -(-\ell)f_{1L,z} - (+\ell)f_{2L,z} = 0 \;\Rightarrow\; \ell f_{1L,z}-\ell f_{2L,z}=0 \;\Rightarrow\; f_{1L,z}=f_{2L,z} \]

第 2 步：解出 wrench。 联立两式：\(f_{1L,z}=f_{2L,z}=\tfrac12 m_L g\)。对称站位、静止水平 → 均分梁重，符合直觉。这就是"运动力"——它驱动（这里是支撑）梁。注意水平分量 \(f_{iL,x}, f_{iL,y}\) 和力矩此时为零（静止无水平加速度、刚性连接的力矩自由度被内力占据）。

第 3 步：内力在哪？ grasp matrix \(G\in\mathbb{R}^{6\times12}\)（两机各 6 维 wrench），\(\dim\ker G=12-6=6\)。上面解出的是特解（运动力）；内力空间 6 维，对应两机之间 6 维"较劲"——比如两机可以同时沿梁轴向**对拉**（把梁拉紧）或**对压**（把梁压弯），合力对梁为零（内力），但改变梁内部应力。静止抬梁时通常令内力为零（或一个小的轴向预紧防连接松动）。

第 4 步：回到机器人侧——每台的腿 GRF 必须平衡 \(-\lambda_{iL}\)。 机器人 1 受梁反力 \(-f_{1L}=(0,0,-\tfrac12 m_L g)\)（梁往下压机器人 1）。机器人 1 的四条腿 GRF 之和必须平衡自重**加**这个反力：

\[ \sum_j f_{1j,z}=m g + \tfrac12 m_L g \]

这就是关键——机器人 1 的腿不仅要扛自己的体重 \(mg\)，还要扛半根梁 \(\tfrac12 m_L g\)。如果 \(\tfrac12 m_L g\) 大到让某条腿的 GRF 超出摩擦锥或让质心失稳，机器人 1 就扛不住——这正是 §8.3 负载分配要检查的"不翻倒"可行性。

第 5 步：数值代入感受量级。 设 \(m=15\) kg（Go2）、\(m_L=10\) kg（梁）、\(g=9.81\)。则每台对梁施加 \(f_{iL,z}=\tfrac12\times10\times9.81=49.05\) N 向上托力；每台四条腿合计要出 \(\sum_j f_{ij,z}=(15+5)\times9.81=196.2\) N（自重 147.15 N + 半梁 49.05 N）。四脚均分则每条腿约 49 N 竖直力。若机器人迈步（trot）只剩两条对角腿支撑，每条腿要出约 98 N——翻倍。这定量地说明了 §8.5 为什么"边走边搬"时施力能力随步态相位变化、负载分配必须时变。

阶段小结：这个手算例子走通了互联 SRB 网络的五步——物体平衡解出运动力（均分梁重）、识别内力空间（6 维轴向较劲）、回到机器人侧得到"腿要多扛半根梁"、数值代入看到迈步时每腿负担翻倍。抽象的 \(N+1\) 刚体方程，落到两机抬梁就是这么具体。 把这个例子从静止推广到运动（梁要加速/俯仰）、从两机推广到 \(N\) 机，就是完整的互联 SRB 网络 MPC。

关键设计：为什么 interaction wrench 要作为决策变量而非消去

这是模型派最精妙、也最容易被初学者忽略的一招，单独拎出来讲。

朴素想法是"消去 \(\lambda_{iL}\)"。 既然全息约束 \(\ddot\phi_{iL}=0\) 加动力学方程能唯一确定 \(\lambda_{iL}\)，理论上可以解出 \(\lambda_{iL}\) 的表达式（一堆矩阵求逆），代回机器人和物体的动力学，得到一个**没有 \(\lambda_{iL}\) 的、降维的**系统。约束消失了、变量少了，看起来更"干净"。

但 Kim–Hamed 和安全攸关 NMPC 都明确选择保留 \(\lambda_{iL}\)。 原因有三：

1. 稀疏性。消去 \(\lambda_{iL}\) 需要矩阵求逆，会把原本稀疏的动力学（每个方程只涉及少数变量）变成**稠密**的（每个变量都和所有其他变量耦合）。而保留 \(\lambda_{iL}\)、把约束 \(\ddot\phi=0\) 显式留在优化里，系统矩阵是**稀疏**的——这对 SQP/MPC 求解器至关重要。安全攸关 NMPC 正是"sparsity-exploiting SQP"，596 个决策变量却能 11ms 解完、60Hz 运行，靠的就是不消去 \(\lambda\) 带来的稀疏性。

2. \(\lambda_{iL}\) 本身就是我们想要的量。\(\lambda_{iL}\) 是每台机器人施加在物体上的力——这正是 §8.3 负载分配要优化、要约束（摩擦锥）、要监控的物理量。把它消去了，反而要再反解回来。保留它，负载分配就是直接对决策变量加约束，自然而然。

3. 数值条件更好。消去过程的矩阵求逆若接近奇异（如几何构型退化），会引入数值病态。保留 \(\lambda\) 让求解器用稳定的方式（如对约束用拉格朗日乘子）处理，条件数更可控。

本质洞察：保留 interaction wrench 作为决策变量，体现了优化建模的一个普适智慧——"显式约束 + 多变量"往往比"隐式消元 + 少变量"更好解。少变量看起来简单，但消元把稀疏耦合变成稠密耦合，得不偿失。这和第 4 章 §4.5"互联 SRB 网络"、第 7 章保留内力作决策变量是同一个思想，也和数值优化里"保持问题稀疏"的黄金法则一脉相承。

理论-工程桥接：正因为保留 \(\lambda_{iL}\) 让系统保持稀疏，安全攸关 NMPC 才能在两台 Go2 上做到 596 决策变量、11ms 求解、60Hz——这不是巧合，而是"不消元"这个建模决策直接换来的实时性。如果消去 \(\lambda\)，稠密化后的同等问题在同样硬件上很可能掉到 10-20Hz，无法实时。

代码：组装互联 SRB 网络（为什么→正确→对比）

下面用 Python 伪代码演示如何把互联 SRB 网络组装成一个稀疏的约束系统（实际工程中会用 CasADi/Crocoddyl/OCS2，这里展示骨架以理解结构）。

Step 1: 先讲为什么要这样组织数据结构。

为什么要把"机器人"和"物体"做成同构的刚体节点，而不是把物体硬编码进机器人？

因为 De Vincenti–Coros 的 agnostic SQP 的核心卖点就是 robot/payload-agnostic——
同一套代码要能跑"2 机器人 + 桌子""3 机器人 + 梁""2 机器人 + 1 双足"。
如果把物体硬编码进每个机器人的方程，换一个物体就要改代码。
正确的做法是：物体和机器人都是"刚体节点"，只是机器人有腿（GRF 输入）、物体没有；
它们通过"约束边"连接。换物体 = 换一个刚体节点 + 换几条约束边，算法不变。

规则：把系统建成"刚体节点 + 约束边"的图，机器人和物体是平等节点，
     全息约束是边。这样 N、物体形状、连接方式都是数据而非代码。

Step 2: 给出正确写法（图结构组装）。

import numpy as np

class RigidBodyNode:
    """一个刚体节点：可以是机器人(有腿)，也可以是物体(无腿)。"""
    def __init__(self, name, mass, inertia, n_legs=0):
        self.name = name
        self.m = mass                 # 标量质量
        self.I = inertia              # 3x3 转动惯量
        self.n_legs = n_legs          # 机器人>0，物体=0
        # 状态 x = [p(3), theta(3), pdot(3), omega(3)] = 12 维
        self.x_dim = 12
        # 每条腿一个 3 维 GRF 作为输入
        self.u_dim = 3 * n_legs

class HolonomicEdge:
    """全息约束边：连接一个机器人节点和物体节点。"""
    def __init__(self, robot, load, attach_robot, attach_load, dof=6):
        self.robot = robot
        self.load = load
        self.attach_robot = attach_robot  # 附着点在机器人体坐标系
        self.attach_load = attach_load    # 附着点在物体体坐标系
        self.dof = dof                    # 6=刚性, 5=放开yaw, ...
        # 每条边引入一个 dof 维的 interaction wrench 作为决策变量
        self.lam_dim = dof

def assemble_network(nodes, edges):
    """组装互联 SRB 网络的决策变量布局与约束计数。"""
    # 决策变量：所有节点状态 + 所有机器人腿力 + 所有边的 interaction wrench
    n_x   = sum(nd.x_dim for nd in nodes)
    n_u   = sum(nd.u_dim for nd in nodes)          # 物体贡献 0
    n_lam = sum(e.lam_dim for e in edges)          # 关键：lambda 作决策变量
    n_dec = n_x + n_u + n_lam

    # 约束：每节点 6 维动力学 + 每边 dof 维加速度层全息约束
    n_dyn = 6 * len(nodes)                          # 机器人+物体一视同仁
    n_hol = sum(e.dof for e in edges)
    n_con = n_dyn + n_hol
    return dict(n_dec=n_dec, n_con=n_con,
                n_x=n_x, n_u=n_u, n_lam=n_lam,
                n_dyn=n_dyn, n_hol=n_hol)

# 例：2 台四足(各4腿) + 1 张桌子，刚性连接
go2_A = RigidBodyNode("go2_A", mass=15.0, inertia=np.diag([0.1,0.4,0.4]), n_legs=4)
go2_B = RigidBodyNode("go2_B", mass=15.0, inertia=np.diag([0.1,0.4,0.4]), n_legs=4)
table = RigidBodyNode("table", mass=8.0,  inertia=np.diag([0.5,0.5,0.9]), n_legs=0)
e1 = HolonomicEdge(go2_A, table, attach_robot=[0.2,0,0.3], attach_load=[-0.4,0,0], dof=6)
e2 = HolonomicEdge(go2_B, table, attach_robot=[0.2,0,0.3], attach_load=[+0.4,0,0], dof=6)
info = assemble_network([go2_A, go2_B, table], [e1, e2])
# info: n_dec = 36(状态) + 24(腿力) + 12(wrench) = 72 决策变量/时间步

Step 3: 给出错误写法并解释为什么错。

# ❌ 错误 1：把物体硬编码成机器人受的外力，不建物体节点
def robot_dyn_WRONG(x_robot, f_legs, payload_mass):
    # 把负载当成平摊到机器人的重力——丢掉了机间耦合和物体转动
    a = (sum(f_legs) - x_robot.m * G - 0.5 * payload_mass * G) / x_robot.m
    return a
# 问题：换一个物体/换机器人数量就要改公式；机器人1加速时机器人2
#       感受不到拽力；物体俯仰/滚转完全看不到。这正是 §8.2"如果直接堆
#       N 个独立 SRB 会怎样"批判的解耦错误。

# ❌ 错误 2：消去 interaction wrench 以"减少变量"
def eliminate_lambda_WRONG(M_dyn, J_hol):
    # 解析消去 lambda：lambda = (J M^{-1} J^T)^{-1} (...)
    Schur = J_hol @ np.linalg.inv(M_dyn) @ J_hol.T   # 稠密化！
    lam = np.linalg.solve(Schur, ...)                # 可能病态
    # 代回得到 reduced dynamics —— 看起来变量少了
    return lam
# 问题：(1) J M^{-1} J^T 把稀疏系统变稠密，SQP 求解从 11ms 退化到几十 ms；
#       (2) 矩阵求逆在几何退化构型下病态；
#       (3) lambda 本来就是负载分配要用的量，消了还得反解回来。
#       正确做法见 Step 2：保留 lambda 作决策变量，约束 ddot_phi=0 显式留在优化里。

# ❌ 错误 3：全息约束只在位置层写，不微分到加速度层
def hol_constraint_WRONG(phi):
    return phi == 0          # 只约束位置
# 问题：MPC/动力学在加速度层工作，只写 phi=0 无法和 ddot 动力学联立，
#       会出现约束漂移（constraint drift）——积分几步后附着点慢慢分离。
#       正确做法：用 ddot_phi = J*ddq + Jdot*dq = 0（必要时加 Baumgarte 稳定项）。

Step 4: 不同建模方式的对比。

| 建模方式 | 机间耦合 | 物体转动 | 稀疏性 | 可换物体/N | 适用 |
|---------|---------|---------|--------|-----------|------|
| 解耦 N 个 SRB（错误） | ❌ 丢失 | ❌ 丢失 | 稀疏 | 否(硬编码) | 反面教材 |
| 消元 reduced（次优） | ✅ | ✅ | ❌ 稠密 | 是 | 小规模、不在意速度 |
| 互联 SRB 网络（推荐） | ✅ | ✅ | ✅ 稀疏 | ✅ 是 | Kim–Hamed/NMPC/SQP |

多视角理解（工程类比）：互联 SRB 网络的结构，和电路仿真里的"节点-支路"模型高度相似——刚体节点像电路节点（有状态/电压），全息约束像支路（强制节点间关系），interaction wrench 像支路电流（连接两节点的相互作用）。相似之处：都用"节点 + 连接 + 流量"建模相互作用，都靠"保持稀疏"获得高效求解（电路仿真用稀疏 LU，SQP 用稀疏 Riccati）。不同之处：电路的支路关系是线性的（欧姆定律），全息约束是非线性的（含叉乘、旋转），所以 SRB 网络要 SQP 迭代而非一次线性求解。不要把这个类比延伸到"能用电路求解器解机器人"——非线性和接触约束让两者求解器完全不同。

⚠️ 常见陷阱

陷阱 8.2.1（概念误区）：把物体当"外部已知负载"而非系统节点 - 错误描述：建模时把被搬物体简化成施加在机器人上的已知外力（如平摊的重力），不给物体单独的状态和动力学方程。 - 现象/后果：机间水平耦合力、物体俯仰/滚转全部丢失；一台机器人的动作无法通过物体传给另一台；多机协调彻底失效。 - 根本原因：物体的运动由 \(N\) 台机器人共同决定、反作用力又共同影响 \(N\) 台机器人，它是系统内生的一部分，不是外部已知量。 - 正确做法：把物体建成与机器人平等的刚体节点（有状态 \(x_L\)、有动力学），通过全息约束和 interaction wrench 与机器人联立求解。

陷阱 8.2.2（思维陷阱）：为"减少变量"消去 interaction wrench - 错误描述：觉得保留 \(N\) 组 \(\lambda_{iL}\) 让决策变量变多，于是解析消去它们得到降维系统。 - 现象/后果：系统矩阵从稀疏变稠密，SQP/MPC 求解时间成倍增加，原本能 60Hz 实时的问题掉到十几 Hz；几何退化时还可能数值病态。 - 根本原因：消元的 Schur 补 \(J M^{-1} J^\top\) 是稠密的，破坏了原问题的稀疏结构；而稀疏性是实时 MPC 的命根子。 - 正确做法：保留 \(\lambda_{iL}\) 作决策变量，把全息约束 \(\ddot\phi=0\) 显式留在优化里。变量多但稀疏，反而快。这也让负载分配能直接对 \(\lambda\) 加约束。

陷阱 8.2.3（编程陷阱）：全息约束只在位置层施加，导致约束漂移 - 错误描述：在 MPC 里只写位置层约束 \(\phi_{iL}=0\)，不微分到加速度层，或微分了但没加镇定项。 - 现象/后果：仿真/积分几步后，机器人附着点和物体连接点慢慢分离或穿透（constraint drift），物体"飘走"或机器人"陷进"物体。 - 根本原因：动力学在加速度层工作，纯位置约束与加速度动力学不在同一层，数值积分会累积约束误差。 - 正确做法：用加速度层约束 \(\ddot\phi_{iL}=J\ddot q+\dot J\dot q=0\)，并在数值实现中加 Baumgarte 镇定项 \(\ddot\phi + 2\alpha\dot\phi + \beta^2\phi=0\)（\(\alpha,\beta>0\)）把漂移拉回。

练习

练习 8.2.1（推导题·两机抬梁） 两台质量均为 \(m\) 的四足，一前一后刚性连接抬一根质量 \(m_L\)、长 \(2\ell\) 的均质梁（转动惯量 \(I_L\)）。设两机附着点分别在梁的两端 \(\pm\ell\) 处。(1) 写出梁的质心动力学和角动量动力学（用 \(f_{1L}, f_{2L}\) 表示）。(2) 在静态平衡（梁水平静止）下，求每台机器人该提供的竖直力 \(f_{iL,z}\)。(3) 若现在要让梁绕质心匀角加速度 \(\dot\omega_L\) 俯仰，两台机器人的竖直力该如何不均等分配？写出表达式。

练习 8.2.2（分析题·约束维数） 比较三种连接方式的全息约束维数及其后果：(a) 完全刚性焊接（如焊死的杆）；(b) 球铰连接（允许三轴相对转动，约束三平移）；(c) 安全攸关 NMPC 的"放开 yaw"连接（约束三平移 + roll + pitch）。对每种方式：写出约束维数，说明系统多了几个相对自由度，并讨论这些自由度对协同的好处（鲁棒性）和坏处（需要额外协调）。

练习 8.2.3（调试挑战） 给你一段同事写的两机搬运 MPC 代码，运行时物体在 3 秒后缓慢"穿透"进机器人躯干。检查发现约束部分只写了 constraint: phi(x_robot, x_load) == 0，且动力学是在加速度层用 RK4 积分的。(1) 指出 bug 的根本原因（提示：约束层 vs 动力学层不匹配）。(2) 写出修复后的加速度层约束（含 Baumgarte 镇定）。(3) 解释为什么纯位置约束在解析上"看起来对"但数值上会漂移。

---

§8.3 多足负载分配：grasp matrix 遇上"不翻倒"约束 ⭐⭐⭐⭐

这一节解决什么问题：§8.2 的互联 SRB 网络里，每台机器人对物体施加的 wrench \(\lambda_{iL}\) 是决策变量。但一组合法的 \(\lambda_{iL}\) 有无穷多种——它们只要合成出物体需要的总 wrench 即可，剩下的自由度（内力）怎么分？这就是**负载分配（load allocation / force distribution）问题。它在第 5/7 章已经以"固定臂"的形式出现过，本节把它搬到多足上，并加上腿足特有的、也是最关键的一条约束——**每台机器人不能为了出力而把自己推翻。这是"多足负载分配"区别于"多臂负载分配"的唯一但致命的差异。

动机：合法的力分配有无穷多种

接 §8.2。两台四足抬一张桌子，桌子要保持静止水平。物体侧的力平衡要求两台机器人提供的竖直力之和等于桌重：\(f_{1L,z}+f_{2L,z}=m_L g\)。但这只是一个方程、两个未知数——有无穷多种分配满足它：\((f_{1L,z},f_{2L,z})\) 可以是 \((\tfrac12 m_L g,\tfrac12 m_L g)\)、也可以是 \((\tfrac34 m_L g,\tfrac14 m_L g)\)、甚至 \((1.5 m_L g, -0.5 m_L g)\)（一台抬、一台压）。

哪种分配最好？这取决于很多因素：两台机器人的负重能力是否相同？它们的站位（力臂）是否对称？地形是否一边软一边硬？更重要的是——哪种分配下，每台机器人都能稳稳站住不翻？ 这最后一条，就是多足负载分配的核心。

如果照搬第 7 章的多臂分配会怎样

第 7 章（双臂多臂）已经给出了固定臂负载分配的完整框架，我们先回顾它，再看搬到多足上缺了什么。

回顾第 7 章 §7.7：固定臂的负载分配。 把 \(N\) 个末端的接触力 \(f_{\text{contacts}}\) 通过 grasp matrix \(G\) 映射到物体 wrench：\(F_{\text{obj}}=G f_{\text{contacts}}\)。一般解是**特解（运动力）+ 零空间（内力）**：

\[ f_{\text{contacts}} = \underbrace{G^{+} F_{\text{obj}}}_{\text{运动力：驱动物体}} + \underbrace{(I - G^{+}G)\,f_{\text{int}}}_{\text{内力：落在 }\ker G,\text{ 不动物体}} \]

固定臂的负载分配 QP 是：在这个一般解里选 \(f_{\text{int}}\)，使总接触力满足每个末端的**摩擦锥**、并最小化某个代价（如力范数）：

\[ \min_{f_{\text{int}}}\ \|f_{\text{contacts}}\|^2 \quad \text{s.t.}\quad \|f_{t,i}\|\le\mu f_{n,i},\ f_{n,i}\ge0\ \ \forall i \]

这个框架在多足上 100% 适用——除了它漏掉了一条约束。 把"末端接触力"换成"机器人施加在物体上的力 \(\lambda_{iL}\)"，把"接触点"换成"机器人附着点"，grasp matrix、运动力/内力分解、摩擦锥全部原样成立。唯一漏掉的是：固定臂的基座无条件稳定，所以它的负载分配只需考虑摩擦锥；而多足的"基座"靠腿悬空，每台机器人施加的 \(\lambda_{iL}\) 的反作用力 \(-\lambda_{iL}\) 必须被它自己的腿 GRF 平衡，且这个平衡不能让质心失稳。

对比性思维（不是 X 而是 Y）：多足负载分配**不是**"在摩擦锥内分力"（那是固定臂），而是"在摩擦锥**与每台机器人的平衡可行域的交集**内分力"。少了"平衡可行域"这一刀，分配出来的力在物体侧完全正确，机器人却会摔——这正是 §8.1 陷阱 8.1.2 的根源，现在我们把它写成数学约束。

历史：从静态力闭合到动态平衡耦合

负载分配的数学源头是**抓取力闭合（grasp force closure）**理论（多指手抓取，1980s-90s，Murray/Li/Sastry 的经典框架），它给出了 grasp matrix、内力、摩擦锥的全部工具。第 5 章讲的就是这套静力学。

把它推广到"会移动的手指"（移动机械臂协同搬运），出现了**分布式自适应搬运**（如第 5 章提到的 Verginis 2019）等工作，处理了"基座会动"但仍假设基座稳定。

真正把"平衡约束"硬塞进负载分配的，是腿足协同的工作：Kim–Hamed 的分层 MPC 在高层就把每台机器人的 GRF（含支撑与操作）联合优化，自然把"不翻倒"编码进 SRB 动力学 + 摩擦锥；安全攸关 NMPC 进一步用 CBF 把安全（含不撞、隐含不翻）写成硬约束。多足负载分配的本质演进，是从"静态力闭合"到"力闭合 ⊗ 动态平衡"的张量积——两个约束集的交，缺一不可。

理论：多足负载分配的完整 QP

现在写出完整的多足负载分配。它有两个层次的约束：物体侧（力要对）和**机器人侧**（每台不能翻）。

第 1 步：物体侧——grasp matrix 与运动力/内力分解。

把 \(N\) 台机器人对物体的 wrench 堆成 \(\boldsymbol\lambda=\mathrm{col}(\lambda_{1L},\dots,\lambda_{NL})\in\mathbb{R}^{6N}\)（每台 6 维 wrench）。物体所需的总 wrench（含重力、期望加速度）记作 \(F_{\text{obj}}^{\text{des}}\)。grasp matrix \(G\in\mathbb{R}^{6\times 6N}\) 把各机 wrench 映射到物体合 wrench：

\[ F_{\text{obj}}^{\text{des}} = G\,\boldsymbol\lambda,\qquad G = \big[\, G_1 \mid G_2 \mid \dots \mid G_N \,\big],\quad G_i=\begin{bmatrix} I_3 & 0 \\ [c_i-p_L]_\times & I_3\end{bmatrix} \]

（这里每台机器人传 6 维 wrench——力 + 力矩，因为刚性连接能传力矩，与第 7 章双臂夹爪同理。）一般解：

\[ \boldsymbol\lambda = G^{+}F_{\text{obj}}^{\text{des}} + (I-G^{+}G)\,\boldsymbol\lambda_{\text{int}},\qquad \dim\ker(G)=6N-6 \]

内力空间维度 \(6N-6\)：\(N=2\) 时是 6 维（两机之间 6 维相对"较劲"），\(N=3\) 时是 12 维，以此类推。这部分和第 5/7 章一字不差。

第 2 步：机器人侧——每台机器人的"不翻倒"约束。

这是新增的、腿足特有的部分。机器人 \(i\) 受到物体的反力 \(-\lambda_{iL}\)。要让机器人 \(i\) 稳定，它的腿 GRF \(\{f_{ij}\}\) 必须同时满足：

(a) 机器人 \(i\) 自身的力/力矩平衡（把 \(-\lambda_{iL}\) 当外力代入机器人 SRB 平衡）：

\[ \sum_j f_{ij} = m_i g\,e_z + f_{iL}^{(\text{react})},\qquad \sum_j (r_{ij}-p_i)\times f_{ij} = \big[(c_i-p_i)\times f_{iL} + \tau_{iL}\big]^{(\text{react})} \]

其中右端的 react 项就是物体反力 \(-\lambda_{iL}\) 折算的力和力矩（符号按机器人受力方向）。

(b) 每条腿的摩擦锥与单边接触（脚不打滑、不拉地）：

\[ f_{ij,z}\ge0,\qquad |f_{ij,x}|\le\tfrac{\mu}{\sqrt2}f_{ij,z},\qquad |f_{ij,y}|\le\tfrac{\mu}{\sqrt2}f_{ij,z}\quad\forall j \]

(c) 质心可行（不翻倒）：存在一组满足 (a)(b) 的 GRF。这等价于一个**可行性约束**——只要 QP 能为机器人 \(i\) 找到满足 (a)(b) 的 \(\{f_{ij}\}\)，机器人 \(i\) 就不会翻。换句话说，"不翻倒"被隐式编码为"机器人 \(i\) 的 GRF 子问题可行"。

本质洞察：多足负载分配里"不翻倒"约束的优雅之处，在于它**不需要显式写 ZMP/支撑多边形不等式**，而是直接编码为"这台机器人的 GRF 平衡子问题有可行解"。如果某个分配 \(\lambda_{iL}\) 大到机器人 \(i\) 的腿无论怎么出力都无法平衡（GRF 子问题不可行），这个分配就被自动排除。这把"几何稳定判据"转化成了"力学可行性"——和单体足式 WBC 里"GRF 在摩擦锥内即不翻"是同一个思想，只是现在 GRF 还要额外平衡物体反力。

第 3 步：组装成联合 QP。

把物体侧的力闭合、机器人侧的平衡+摩擦锥，连同一个代价函数（如最小化总力 + 内力跟踪期望 + 各机能力加权），写成一个 QP：

\[ \boxed{ \begin{aligned} \min_{\boldsymbol\lambda,\,\{f_{ij}\}}\ & \sum_i w_i\|\lambda_{iL}\|^2 + \|\boldsymbol\lambda_{\text{int}}-\boldsymbol\lambda_{\text{int}}^{\text{des}}\|^2 + \sum_{ij}\|f_{ij}\|^2 \\ \text{s.t.}\ & G\boldsymbol\lambda = F_{\text{obj}}^{\text{des}} & \text{(物体力闭合)}\\ & \text{机器人 } i \text{ 的力/力矩平衡 (a)} & \forall i\\ & f_{ij,z}\ge0,\ \text{摩擦锥 (b)} & \forall i,j \end{aligned}} \]

其中 \(w_i\) 是各机能力权重（弱的机器人权重大→分配少出力），\(\boldsymbol\lambda_{\text{int}}^{\text{des}}\) 是期望内力（通常取小的正挤压力以保持接触不松脱，见 §8.4 抬模式）。

这个 QP 与第 7 章 §7.7 的多臂分配 QP 的唯一结构差异，就是多了"机器人 \(i\) 的力/力矩平衡 (a)"和它引入的腿 GRF 变量 \(\{f_{ij}\}\)。 把 (a) 和 \(\{f_{ij}\}\) 删掉，就退化回第 7 章的固定臂分配。

理论-工程桥接：这个 QP 在工程上通常不单独求解，而是**融进 §8.2 的互联 SRB 网络 MPC**——MPC 的每个时间步本来就在优化 \(\lambda_{iL}\) 和 \(f_{ij}\)，摩擦锥和平衡本来就是它的约束。所以"负载分配"在模型派里不是一个独立模块，而是 MPC 优化的**自然副产品**：你解完 MPC，\(\lambda_{iL}\) 就是分配结果。把它单独拎出来讲，是为了让你看清"分配"这件事的力学本质；工程实现时它和 MPC 是一体的。

为什么多足内力调控比固定臂更难

第 7 章强调内力必须"闭环维护"而非"开环施加"。多足把这个难度又加了一层，值得单独说清。

固定臂的内力调控：两只固定臂夹物体，内力是两臂"对挤"的力。调控它只需让每只臂对"相对位姿偏差"表现出期望刚度（object/internal 阻抗）——基座稳定，臂可以专心调内力。

多足的内力调控：每台机器人调内力时，同时还要维持自身平衡。增大内力（夹得更紧）意味着每台机器人要对物体施加更大的对挤力，其反力又要被腿 GRF 平衡——内力越大，腿的负担越重，越接近摩擦锥/平衡边界。于是出现一个**耦合**：内力调控和平衡维持抢夺同一组腿 GRF 的"预算"。

对比性思维（反事实）：如果不考虑这个耦合、像固定臂那样自由调内力会怎样？→ 为了把物体夹得足够紧（大内力）防止松脱，每台机器人对物体猛施对挤力 → 反力把腿 GRF 推到摩擦锥边界 → 脚打滑或质心失稳 → 机器人摔，物体掉。结论：多足内力必须在"夹紧（防松脱）"和"留平衡余量（防翻）"之间折中，这个折中固定臂没有。decPLM 用 constellation reward 的"维持相对位姿"隐式地学到了这个折中（§8.6），模型派用 QP 的"内力期望 + 平衡约束"显式地求这个折中。

深入：一个两机抬桌的分配数值算例

把上面的 QP 用一组具体数字走一遍，你会更直观地看到"运动力 + 内力"是怎么分的、"能力权重"怎么改变倾斜、以及"不翻倒"约束什么时候开始咬人。这演示的是累积项目里程碑 1 和练习 8.3.1 的方法。

设置。 两台四足从两端抬一张质量 \(m_L=10\) kg 的均质细杆（近似一维），重力 \(g=9.8\)。杆水平、保持静止（期望加速度为 0），杆心 \(p_L\) 在两机连线中点。机器人 1 在 \(c_1=(-0.5,0,0)\)、机器人 2 在 \(c_2=(+0.5,0,0)\)（相对杆心，单位 m），只考虑竖直力与绕杆心的俯仰力矩这个最小子问题。物体侧需求：合力 \(F_z=m_L g=98\) N 向上、合力矩为 0。

第一步：grasp matrix 与运动力（最小范数解）。 这个二维子问题里，设两机各出竖直力 \(f_{1z},f_{2z}\)，grasp 关系是

\[ \underbrace{\begin{bmatrix}1&1\\-0.5&+0.5\end{bmatrix}}_{G}\begin{bmatrix}f_{1z}\\f_{2z}\end{bmatrix}=\begin{bmatrix}98\\0\end{bmatrix} \]

第二行是绕杆心的力矩平衡（机器人 1 在 \(-0.5\) 处、力臂取负）。\(G\) 在这里恰好可逆，解唯一：\(f_{1z}=f_{2z}=49\) N。这就是对称布局下的**运动力**——各扛一半，符合直觉。

第二步：内力（零空间）在哪里？ 当 \(G\) 满秩且方程数 = 未知数时（如这个 \(2\times2\)），\(\ker G=\{0\}\)，没有内力自由度——竖直子问题被完全决定。内力要出现，得放开更多维度：比如允许两机施加水平对挤力 \((f_{1x},f_{2x})\)。一对等大反向的水平力 \((+F_{\text{int}},-F_{\text{int}})\) 满足 \(G\) 的零空间（合力合力矩都为 0，不动杆），这就是**内力**——它把杆"夹紧"但不抬不推。对刚性搬，内力可设为 0（连接已能传力）；对捏举（§8.4），内力必须设正值才能靠摩擦托住，这正是 §8.4 抬模式的命脉。

第三步：能力权重如何制造倾斜。 现在假设机器人 1 较弱、机器人 2 较强，工程上想让强者多扛。但注意——上面竖直子问题**无自由度**，改权重也改不了 \(49{:}49\)（力矩平衡锁死了它）。要让分配倾斜，必须**改几何**：把弱者 1 往杆心挪、强者 2 往杆端挪。设新站位 \(c_1=(-0.3,0,0)\)、\(c_2=(+0.7,0,0)\)，力矩平衡变为 \(-0.3 f_{1z}+0.7 f_{2z}=0\)，联立 \(f_{1z}+f_{2z}=98\) 解得 \(f_{2z}=29.4\) N、\(f_{1z}=68.6\) N。

咦，怎么是弱者 1 扛得更多（\(68.6>29.4\)）？因为力矩平衡要求杆心两侧力矩相消，离杆心近的一方力臂短、必须出更大的力**才能平衡远方一侧。这是一个极易踩的反直觉点：**要让强者多扛，应该把强者放在离杆心近的位置（短力臂端），而不是远端。 这正是练习 8.3.1 让你"调能力权重观察倾斜"背后真正起作用的物理——能力权重 \(w_i\) 在 QP 里影响的是有内力自由度时的内力分配，而竖直承重的倾斜主要由**站位几何（力臂）**决定。

第四步："不翻倒"约束何时开始咬人。 继续上面的不对称站位，机器人 1 现在要扛 \(68.6\) N 的物体反力。它的 4 条腿要平衡 "自重 \(+\) 这 \(68.6\) N 向下的反力"。设机器人 1 自重 \(15\) kg（\(147\) N），则它的腿总共要顶 \(147+68.6=215.6\) N。只要这个力能在它的支撑多边形内、摩擦锥内分配（GRF 子问题可行），机器人 1 就站得住。但若继续增加桌重（如 \(m_L=40\) kg），机器人 1 要扛的反力升到约 \(274\) N，叠加自重逼近其腿力上限，GRF 子问题趋于不可行——QP 在某个临界桌重 \(m_L^*\) 处突然报 infeasible，这就是练习 8.3.1 要找的"不可行临界质量"。它的物理含义是：超过 \(m_L^*\)，无论怎么分配，离杆心近的那台机器人都站不住，必须减载、重新站位（拉开力臂）或加机器人。

本质洞察（数值版）：这个算例暴露了多足负载分配三个层次的"自由度"——(1) 竖直承重**主要由站位几何（力臂）决定，对称布局必然均分；(2) **内力**是零空间自由度，刚性搬可置 0、捏举必置正；(3) **不翻倒**是一道硬天花板，在临界载荷处让整个分配从"有解"突变为"无解"。固定臂只有前两层，多足多了第三层——而正是第三层把"力分配"从一道线性代数题变成了一道**带可行性边界的优化题。

代码：多足负载分配 QP（为什么→正确→错误）

Step 1: 先讲为什么要联合优化腿力。

为什么负载分配 QP 里必须包含每台机器人的腿力 f_ij 作为变量？

因为"这个 lambda 分配机器人扛不扛得住"这件事，只有把腿力一起优化才知道。
如果只优化 lambda（物体侧），你能保证物体力闭合，但不知道某台机器人
是否能用它的 4 条腿在摩擦锥内平衡掉 -lambda 的反力。
把 f_ij 拉进来当变量、加上"机器人 i 力平衡 + 摩擦锥"约束，
QP 的可行性就自动等价于"每台机器人都站得住"。
不可行 = 这个分配会让某台机器人翻 = QP 自动拒绝它。

规则：负载分配要"物体侧 lambda + 机器人侧腿力 f_ij"联合优化，
     用 QP 可行性自动编码"不翻倒"。只优化 lambda 是固定臂思维，会摔。

Step 2: 正确写法。

import numpy as np
import cvxpy as cp

def multilegged_load_allocation(G, F_obj_des, robots, mu=0.6):
    """
    多足负载分配 QP。
    G:          6 x 6N  grasp matrix
    F_obj_des:  6       物体所需合 wrench
    robots:     list，每个含 m_i, g, p_i, c_i, 足端位置 r_ij, 能力权重 w_i
    返回:       每台机器人对物体的 wrench lambda_i 和腿力 f_ij
    """
    N = len(robots)
    lam = cp.Variable(6 * N)                 # 物体侧：各机 wrench
    fz = []                                   # 机器人侧：各机腿力
    constraints = []
    cost = 0

    # --- 物体侧：力闭合 ---
    constraints += [G @ lam == F_obj_des]

    for i, rb in enumerate(robots):
        lam_i = lam[6*i : 6*i+6]
        f_i = cp.Variable((4, 3))            # 4 条腿，每条 3 维 GRF
        fz.append(f_i)

        # --- 机器人 i 力平衡：腿力之和 = 自重 + 物体反力 ---
        react_force  = -lam_i[0:3]           # 物体反力(力)
        react_torque = -lam_i[3:6]           # 物体反力(力矩)
        constraints += [cp.sum(f_i, axis=0)
                        == rb.m * rb.g * np.array([0,0,1]) - react_force]
        # 力矩平衡（绕质心）：sum (r_ij - p_i) x f_ij = react_torque + 反力力臂
        torque = sum(cp.cross(rb.r[j] - rb.p, f_i[j]) for j in range(4))
        constraints += [torque == -react_torque
                        - cp.cross(rb.c - rb.p, react_force)]

        # --- 摩擦锥 + 单边接触（线性化金字塔）---
        for j in range(4):
            constraints += [f_i[j,2] >= 0]                       # 不拉地
            constraints += [cp.abs(f_i[j,0]) <= mu/np.sqrt(2)*f_i[j,2]]
            constraints += [cp.abs(f_i[j,1]) <= mu/np.sqrt(2)*f_i[j,2]]

        cost += rb.w * cp.sum_squares(lam_i) + cp.sum_squares(f_i)

    prob = cp.Problem(cp.Minimize(cost), constraints)
    prob.solve()
    if prob.status != cp.OPTIMAL:
        raise RuntimeError("分配不可行——某台机器人扛不住，需减载/重新站位/加机器人")
    return lam.value, [f.value for f in fz]

Step 3: 错误写法。

# ❌ 错误 1：只优化物体侧 lambda，不含腿力和平衡约束（固定臂思维）
def alloc_WRONG(G, F_obj_des):
    lam = cp.Variable(G.shape[1])
    prob = cp.Problem(cp.Minimize(cp.sum_squares(lam)),
                      [G @ lam == F_obj_des])     # 只有力闭合！
    prob.solve()
    return lam.value
# 问题：物体侧力对了，但没人保证某台机器人能平衡 -lambda 的反力。
#       直接下发→某台机器人质心被拉出支撑多边形→摔。§8.1 陷阱 8.1.2。

# ❌ 错误 2：把"不翻倒"写成事后检查而非约束
def alloc_then_check_WRONG(G, F_obj_des, robots):
    lam = alloc_WRONG(G, F_obj_des)              # 先随便分
    for i, rb in enumerate(robots):
        if not robot_can_balance(rb, lam[6*i:6*i+6]):
            print("机器人", i, "扛不住")          # 只是打印，没改分配
    return lam
# 问题：检查出不可行也无能为力——分配已经定了。正确做法是把平衡
#       作为约束放进 QP，让求解器在可行域内找分配，而非分完再检查。

# ❌ 错误 3：内力期望设为 0（不施加挤压力）
#    constraints 不含 lambda_int_des，cost 不奖励内力
# 问题：非抓持/捏举场景下内力=0 意味着接触随时可能松脱（法向力为零），
#       物体一受扰就掉。正确做法：设一个小的正内力期望保持"咬合"（§8.4 抬模式）。

Step 4: 与第 7 章固定臂分配的对比。

| 维度 | 第7章 固定臂分配 | 本章 多足分配 |
|------|----------------|--------------|
| 决策变量 | 末端接触力 | 各机 wrench λ + 各机腿力 f_ij |
| 约束 | 摩擦锥 | 摩擦锥 ⊗ 每台机器人力平衡(不翻) |
| 不可行含义 | 抓不住(力闭合失败) | 抓不住 或 某台机器人会翻 |
| 内力调控 | 自由(基座稳) | 受"夹紧vs留平衡余量"折中约束 |
| 工程实现 | 独立 QP | 通常融进互联 SRB 网络 MPC |

⚠️ 常见陷阱

陷阱 8.3.1（思维陷阱）：负载分配只看物体侧力闭合，不含机器人平衡 - 错误描述：把多足负载分配写成"在 grasp 零空间里选内力满足摩擦锥"，和固定臂一模一样，不加机器人自身平衡约束。 - 现象/后果：物体侧合力合力矩完全正确，但某台机器人为提供分到的力，质心被反力拉出支撑多边形而摔。 - 根本原因：固定臂基座无条件稳定，分配只受摩擦锥约束；多足"基座"悬空，分到的 \(\lambda_{iL}\) 的反力必须被自身腿 GRF 在摩擦锥内平衡，否则翻。 - 正确做法：把各机腿力 \(f_{ij}\) 拉进 QP 作变量，加"机器人 \(i\) 力/力矩平衡 + 摩擦锥"约束，用 QP 可行性自动编码"不翻倒"。

陷阱 8.3.2（概念误区）：把"不翻倒"写成事后检查而非优化约束 - 错误描述：先按物体侧分配力，再逐台检查"这台扛不扛得住"，扛不住就报警，但不改分配。 - 现象/后果：检查出不可行也无法补救，分配已下发；或者反复试错调分配，效率极低且无收敛保证。 - 根本原因：可行性是约束不是事后判据。把它放在优化外面，求解器无法利用它在可行域内搜索。 - 正确做法：平衡约束必须在 QP 内部，让求解器只在"所有机器人都站得住"的可行域里找最优分配。不可行时 QP 直接告诉你"无解"，触发减载/重新站位/加机器人。

陷阱 8.3.3（编程陷阱）：摩擦锥用二阶锥而非线性化，拖慢实时 QP - 错误描述：直接用 \(\|f_t\|\le\mu f_n\) 的二阶锥（SOC）约束，喂给只支持 QP 的快速求解器。 - 现象/后果：要么求解器报错（不支持 SOCP），要么改用通用 SOCP 求解器导致单步求解从 1ms 涨到 10ms，无法在 1kHz 控制环用。 - 根本原因：二阶锥是非线性约束，QP 求解器（如 OSQP、qpOASES）只接受线性约束；SOCP 求解器慢得多。 - 正确做法：线性化为四棱锥（friction pyramid）\(|f_{t,x}|\le\tfrac{\mu}{\sqrt2}f_n\)、\(|f_{t,y}|\le\tfrac{\mu}{\sqrt2}f_n\)，用纯 QP 求解器。代价是略保守（内接金字塔小于圆锥），但换来实时性。这与单体足式 MPC 的摩擦锥处理一致。

练习

练习 8.3.1（实现题·分配 QP） 基于本节的 multilegged_load_allocation 框架，实现一个完整的两机抬桌负载分配器并做实验：(1) 两台相同 Go2 对称站位抬 8 kg 桌子，验证分配结果接近均分。(2) 把其中一台的能力权重 \(w_i\) 调大 3 倍（模拟一台电量低/电机弱），观察分配如何向另一台倾斜。(3) 逐渐增大桌重，找到 QP 首次变不可行的临界质量——这就是这个站位下两机能搬的极限。解释为什么这个极限低于"两台机器人最大负重之和"。

练习 8.3.2（推导题·内力折中） 两台四足捏举（pinch）一个无法抓持的圆柱（只靠侧向挤压的摩擦力托住，见 §8.4 抬模式）。设每台从一侧施加水平挤压力 \(f_{\text{int}}\)（内力），圆柱重 \(m_L g\) 靠两侧摩擦力托住，摩擦系数 \(\mu\)。(1) 写出"圆柱不下滑"对 \(f_{\text{int}}\) 的下界（摩擦力 \(\ge\) 重力）。(2) 写出"机器人不被反力推翻"对 \(f_{\text{int}}\) 的上界（用机器人质心到支撑边界的裕度粗略估计）。(3) 说明这个 \([\text{下界},\text{上界}]\) 区间为什么可能为空——此时单靠两台捏不住，必须加机器人或换抓持方式。

练习 8.3.3（跨章综合题·接第 4、5、7 章） 把本章的多足负载分配做成**分布式**版本。(1) 用第 5/7 章的 grasp matrix 和内力分解写出集中式 QP（已在本节给出）。(2) 用第 4 章的 ADMM 把这个 QP 按机器人拆分：每台机器人本地优化自己的 \(\lambda_{iL}\) 和 \(f_{ij}\)，通过一致性约束 \(G\boldsymbol\lambda=F_{\text{obj}}^{\text{des}}\) 协调。写出 ADMM 的 x-更新（各机本地 QP）、z-更新（物体力闭合投影）、对偶更新。(3) 用第 2 章的共识解释：ADMM 的对偶变量迭代本质上是各机对"物体合力该是多少"达成一致的过程。说明这个分布式版本相比集中式的优势（可扩展、各机只需本地+邻居信息）和代价（多轮迭代、收敛前力闭合不严格满足）。

---

§8.4 推 / 搬 / 抬：三种协作模式的力学统一 ⭐⭐⭐

这一节解决什么问题：前两节建立了通用的力学框架（互联 SRB 网络 + 负载分配）。但真实的多足协同操作有截然不同的几种"姿势"——有的机器人**推**着物体走（不抓持，纯靠法向/摩擦接触），有的**刚性夹持搬**着走（焊死的连接），有的**捏举**着走（pinch，靠两侧挤压把无法抓持的物体托起）。这一节把这三种模式放进同一个 wrench 平衡框架，揭示它们的**唯一本质差异——接触约束的类型与维数**，并据此说明为什么"推"最鲁棒、"抬"最精确、"搬"居中。理解三模式，你才能把抽象力学对应到具体任务。

动机：同一个框架，三种截然不同的"接触"

回到 §8.2 的全息约束。当时我们说"连接方式决定约束维数：刚性 6 维、放开 yaw 5 维……"。这一节把这句话展开成一个完整的谱系。

考虑三个真实任务：

• 任务 A（推）：三台四足用躯干顶着一个大冰箱，把它推过房间。机器人和冰箱之间**没有任何抓持**——只是躯干压着冰箱表面，靠法向接触力（推）和摩擦力（防侧滑）作用。机器人随时可以松开（退后一步，接触就断了）。
• 任务 B（搬）：两台四足通过焊死的刚性杆连接，杆中间挂着重物，像两个人用扁担抬。机器人和物体（杆）之间是**完全刚性的 6-DoF 连接**——能传力、能传力矩，不能相对运动。这是 Kim–Hamed 双 A1 的设置。
• 任务 C（抬/捏举）：两台带臂四足（decPLM 的 Go2+D1）从两侧用臂上的橡胶垫**捏住**一个圆木，把它从地上托起来搬走。圆木无法被"抓握"（太粗、没有把手），只能靠两侧的**挤压摩擦力**托住——这就是 pinch（捏举）。接触是单边的（垫子只能推不能拉物体），但需要足够的挤压内力维持摩擦托举。

这三个任务，物体侧的力学完全一样（都是 \(F_{\text{obj}}=G\boldsymbol\lambda\)，物体被一组 wrench 驱动）。差异全部集中在"机器人能对物体施加什么样的 wrench"——即接触约束。 这一节就是要把这个差异讲透。

如果不区分接触类型、一律按刚性搬来建模会怎样

一个常见的偷懒是：不管推、搬、抬，一律按 §8.2 的刚性 6-DoF 全息约束建模。这会在推和抬上出大问题。

对推（任务 A）：刚性约束假设机器人和物体焊死、能传任意方向的力（包括拉力）。但推是非抓持的——机器人**只能推不能拉**物体（一拉，躯干就离开物体表面，接触断了）。如果按刚性建模，优化器可能算出"机器人 1 拉、机器人 2 推"的分配——物理上机器人 1 根本拉不住冰箱，冰箱会脱离它。刚性建模允许了物理上不存在的拉力。

对抬（任务 C）：捏举靠侧向挤压的摩擦力托举，法向（挤压方向）只能推不能拉，且竖直托举力完全来自摩擦（受 \(\mu f_n\) 限制）。如果按刚性建模，优化器以为机器人能直接对物体施加任意竖直力（像焊死那样），于是低估了所需的挤压内力——实际部署时物体因挤压不足而从垫子间滑落。刚性建模忽略了"托举力受摩擦限制"这一捏举的命脉。

对比性思维（反事实）：如果三模式不分、一律刚性建模 → 推会算出不存在的拉力、抬会低估挤压内力 → 仿真里完美、真机上物体脱离或滑落。根本原因：刚性约束是三模式中**约束最强、自由度最少、能力最大**的一种（能传 6 维 wrench 含拉力和力矩）；推和抬的接触能力**更弱**（不能拉、托举受摩擦限），用强约束近似弱接触，等于假设机器人有它实际没有的能力。

历史：从抓持操作到非抓持操作

机器人操作的经典理论（第 5 章、Murray-Li-Sastry）默认**抓持（prehensile）**——手指/夹爪握住物体，能传力也能传力矩。这套理论成熟、精确，但要求物体可被抓握。

现实中大量物体无法抓握（太大、太重、太滑、没有把手——冰箱、原木、沙发）。于是**非抓持操作（non-prehensile manipulation）兴起：推（pushing）、滚（rolling）、捏举（pinching）等只靠接触力而不靠抓握的操作。多足协同里，MA-Push（2024）专攻协同推大物体，decPLM（2025）专攻捏举搬运无法抓持的物体——它们的卖点恰恰是**处理了抓持理论覆盖不到的非抓持接触。

而 Kim–Hamed（2023）、安全攸关 NMPC（2025）走的是抓持（刚性连接）路线——它们用机械接口把物体和机器人焊死，从而能用最强的接触能力换取最精确的控制。

本质洞察：推/搬/抬的演进，是机器人操作从"抓持"走向"非抓持"在多足领域的投影。抓持（刚性搬）能力强、精度高，但要求物体可连接；非抓持（推、捏举）能力弱、精度低，但能处理任意物体。多足协同选哪种模式，本质是在"物体能否被连接/抓持"和"任务需要多高精度"之间做的权衡——这正是 §8.1 选型表的力学根源。

理论：三模式的接触约束统一表

现在把三模式的差异精确化。关键是每台机器人能对物体施加的 wrench \(\lambda_{iL}\) 受什么约束（即接触约束集 \(\mathcal{C}_i\)）。

模式 1：推（pushing，非抓持，点/面接触）。

机器人通过躯干/腿压在物体表面。接触力只有**法向推力 \(f_n\ge0\)（不能拉）+ 切向摩擦力 \(\|f_t\|\le\mu f_n\)（防滑），**不能传力矩（面接触可传有限力矩，简化常忽略或建为小范围）。约束集：

\[ \mathcal{C}_i^{\text{push}}=\big\{\lambda_{iL}=(f_n, f_t, \tau\approx0)\ :\ f_n\ge0,\ \|f_t\|\le\mu f_n\big\} \]

这是**单边 + 摩擦锥**约束，自由度最少。注意 \(f_n\ge0\) 是关键——这把"不能拉"编码进去了。

模式 2：搬（carrying，刚性抓持连接）。

机器人通过刚性连接（杆/机械接口/夹爪握死）连物体。能传**完整 6 维 wrench（力 + 力矩，可拉可推）**。约束集（几乎无约束，只受连接强度/电机力矩限）：

\[ \mathcal{C}_i^{\text{carry}}=\big\{\lambda_{iL}\in\mathbb{R}^6\ :\ \|\lambda_{iL}\|\le\lambda_{\max}\big\} \]

这是约束最弱、能力最强的模式。全息约束是 6 维（§8.2 标准形式）。

模式 3：抬/捏举（pinch-lift，非抓持，侧向挤压摩擦托举）。

机器人从两侧（或多侧）用垫子挤压物体，靠摩擦力托举。每个接触点：挤压法向 \(f_n\ge0\)（只能压）+ 切向（含竖直托举）摩擦 \(\|f_t\|\le\mu f_n\)。与推的关键差异：托举力（竖直方向）属于切向摩擦，受 \(\mu f_n\) 限制，所以必须有足够大的挤压内力 \(f_n\) 才能托起重物。约束集形式同推，但**内力（挤压）下界由托举需求决定**：

\[ \mathcal{C}_i^{\text{pinch}}=\big\{(f_n,f_t):\ f_n\ge f_n^{\min},\ \|f_t\|\le\mu f_n\big\},\quad f_n^{\min}\ \text{由 }\sum\mu f_n\ge m_L g\text{ 定} \]

统一视角：三模式是同一个"摩擦锥 + 单边接触"约束的不同强度——

\[ \boxed{ \begin{array}{l|c|c|c} \text{模式} & \text{可传力矩} & \text{可拉} & \text{内力下界} \\\hline \text{推 push} & \text{否} & \text{否}(f_n\ge0) & 0 \\ \text{搬 carry} & \text{是} & \text{是} & \text{无}(\text{刚性}) \\ \text{抬 pinch} & \text{否} & \text{否}(f_n\ge0) & f_n^{\min}>0(\text{托举}) \end{array}} \]

阶段小结：到这里我们看到三模式的本质——它们是同一套 wrench 平衡 + 摩擦锥框架，只是**接触约束集 \(\mathcal{C}_i\) 的强弱不同**：搬最强（6 维、可拉可推可力矩），推最弱（单边、无力矩、零内力），抬居中（单边、无力矩、但需正内力托举）。把 \(\mathcal{C}_i\) 换一下，§8.2-8.3 的全部框架就分别覆盖了三模式。这就是"统一力学框架"的含义。

为什么推最鲁棒、抬最精确、搬居中

从约束集的强弱，可以直接推出三模式的工程特性。

推最鲁棒，但最难精确控姿。 推是非抓持的，机器人随时可以松开/重新接触，单台失效不会"拽住"物体——容错性最好。MA-Push 正是利用这点做多机推大物体。但推不能传力矩、不能拉，控制物体的姿态（朝向）极难——你只能靠多台机器人从不同点推、用推力的合力矩间接控姿，而且物体一旦转过头无法"拉回来"。所以推适合"把大物体大致弄到某区域"，不适合"精确摆正"。

抬最精确（在非抓持里），但对协调和接触维持要求最高。 捏举能把物体托离地面、能较精确地控制物体位姿（decPLM 用 constellation reward 精确跟踪物体位姿）。但它命悬一线于**挤压内力**——内力不足物体滑落、内力过大机器人被反力推翻（§8.3 内力折中），且多台必须**同步**维持挤压（一台松手物体就掉）。所以抬适合"精确搬运可托举的物体"，但对协调（同步捏紧）和 sim-to-real（接触力精确）要求最苛刻。

搬居中，能力最强但要求物体可刚性连接。 刚性搬能传 6 维 wrench，既能精确控位姿、又能稳定承重，是能力最全面的模式。代价是**要求物体可被刚性连接**（焊杆、机械接口、夹爪握死）——现实中很多物体做不到。且刚性连接放大了机间扰动传递（一台抖，刚性杆把抖动直接传给另一台），需要更强的协调。

多视角理解（双重解读）：三模式的鲁棒性-精度权衡，可以从两个角度理解。角度 1（接触能力视角）：约束越弱（推），能力越小但容错越好；约束越强（搬），能力越大但越依赖连接成立。角度 2（信息耦合视角）：推的机间耦合最松（非抓持，扰动不强制传递），搬的机间耦合最紧（刚性，扰动强制传递），抬居中（靠内力耦合，可调）。两个角度殊途同归：耦合越松越鲁棒、越紧越精确——这和第 7 章主从 vs 协调的辩论、第 4 章软编队 vs 硬约束的对比是同一条主线。

深入：捏举内力的可行区间——一个完整算例

捏举（抬模式）最反直觉的地方是：内力既有**下界**（太松托不住、掉）又有**上界**（太紧机器人被反力推翻），可行区间是一条夹在两者之间的窄带，而且这条带会随物体倾角收缩到为空。这里把练习 8.4.2 的力学用具体数字算到底，让你看清"临界倾角"是怎么冒出来的。

设置。 两台四足从左右两侧水平捏举一个质量 \(m_L=8\) kg 的竖直圆柱（\(m_L g=78.4\) N），接触面竖直，垫子与圆柱间摩擦系数 \(\mu=0.7\)。两侧各施加水平挤压法向力 \(f_n\)（大小相等、方向相对，构成内力），靠竖直摩擦力托住圆柱。

第一步：下界（防下滑）。 圆柱竖直不下滑，要求两侧竖直摩擦力之和 \(\ge\) 重力。每侧最大静摩擦 \(=\mu f_n\)，两侧共 \(2\mu f_n\)：

\[ 2\mu f_n\ge m_L g\ \Rightarrow\ f_n\ge f_n^{\min}=\frac{m_L g}{2\mu}=\frac{78.4}{2\times0.7}=56\ \text{N} \]

工程上还要留裕度（防摩擦估计偏乐观、防扰动），取 \(1.3\times\)，则实际下界约 \(f_n\ge 73\) N。

第二步：上界（防翻倒）。 每台机器人施加 \(f_n\) 的水平挤压，圆柱给它一个等大反向的水平反力 \(f_n\)（把机器人往外推）。这个水平反力作用在机器人接触点（设在质心上方 \(h=0.25\) m），对机器人质心产生一个**倾覆力矩** \(M_{\text{tip}}=f_n\cdot h\)。机器人靠自身的"支撑多边形 \(\times\) 自重"提供的抗倾覆力矩 \(M_{\text{resist}}=m_i g\cdot d\)（\(d\) 为质心投影到支撑多边形外侧边的距离，即稳定裕度）来抵抗。设机器人自重 \(15\) kg（\(147\) N）、外侧裕度 \(d=0.10\) m：

\[ M_{\text{tip}}\le M_{\text{resist}}\ \Rightarrow\ f_n\cdot 0.25\le 147\times 0.10\ \Rightarrow\ f_n\le f_n^{\max}=\frac{14.7}{0.25}=58.8\ \text{N} \]

第三步：可行区间。 水平捏举（\(\phi=0\)）的可行内力区间是

\[ f_n\in[\,56,\ 58.8\,]\ \text{N} \]

只有 \(2.8\) N 宽！ 而且如果按第一步留 \(1.3\times\) 裕度（下界升到 \(73\) N），下界已经超过上界——区间为空，这台机器人捏不住。这立刻解释了 decPLM 为什么强调"姿态信息至关重要"，以及为什么真机捏举如此娇贵：内力的安全窗口本就极窄。

第四步：倾角如何吃掉可行区间。 现在把圆柱倾斜搬运，接触面与竖直成 \(\phi\) 角。重力沿接触面的下滑分量变为 \(m_L g\cos\phi\)？不——更糟：倾斜后摩擦要同时抵抗"沿面下滑"且法向有效压力的几何关系改变，下界随 \(\phi\) 增大而**上升**（需更大 \(f_n\)）。同时倾斜使圆柱重心外移，机器人承受的倾覆力矩增大，上界随 \(\phi\) 下降。两条线相向而行，必在某个**临界倾角 \(\phi^*\)** 相交——超过 \(\phi^*\)，下界 \(>\) 上界，可行区间为空：

\[ f_n^{\min}(\phi)\ \uparrow,\quad f_n^{\max}(\phi)\ \downarrow,\quad \exists\,\phi^*:\ f_n^{\min}(\phi^*)=f_n^{\max}(\phi^*)\ \Rightarrow\ \phi>\phi^*\text{ 捏不住} \]

本质洞察（数值版）：捏举的可行内力区间 \([f_n^{\min},f_n^{\max}]\) 是一条"下界顶上来、上界压下去"的窄带，临界倾角 \(\phi^*\) 处带宽归零。这是 §8.3"内力折中（夹紧 vs 不翻）"和 §8.4"抬最精确但最娇贵"两句话的精确数值化身。它也直接推出一条工程铁律：捏举搬运要尽量保持物体竖直（小 \(\phi\)）、尽量贴近机器人质心（小 \(h\)）、尽量站宽（大 \(d\)）——三者都在帮你撑开那条窄带。decPLM 把姿态喂进 constellation reward，本质就是在让策略学会"别让 \(\phi\) 跑到 \(\phi^*\) 附近"。

接触维持与安全：CBF 视角（接安全攸关 NMPC）

三模式都面临一个共同挑战：接触不能丢、机器人不能撞。安全攸关 NMPC（2025）用**控制屏障函数（Control Barrier Function, CBF）**把这类安全约束写成硬保证，值得一讲。

CBF 的核心思想：定义一个安全函数 \(h(x)\ge0\) 表示"安全"（如 \(h=\) 机器人到障碍距离 \(-\) 安全裕度，或 \(h=\) 挤压内力 \(-\) 防滑下界），通过约束其变化率保证 \(h\) 永不变负（前向不变性）：

\[ \dot h(x,u) \ge -\alpha\,h(x)\qquad(\alpha>0) \]

安全攸关 NMPC 用**高阶离散 CBF（HOCBF）**处理 SRB 的二阶（力/力矩输入相对度 \(r=2\)）系统：对每个机器人/物体与每个障碍，\(h_{i,\ell}(x_i):=\|g(x_i)-o_\ell\|-d_{th}\)，HOCBF 条件 \(\psi_2^{i,\ell}\ge0\) 保证碰撞-free 集前向不变。结果：双 Go2 + 5kg 负载安全穿过 6 个障碍、零碰撞，11ms@60Hz，负载达自重 74.67%（超过无安全保证工作的约 55%）。

理论-工程桥接：CBF 把"安全"从"训练/调参希望它安全"升级为"数学上保证它安全"。在多足协同里这尤其重要——多台机器人 + 物体在狭窄空间移动，碰撞和接触丢失的风险远高于单机。CBF 的代价是要在 NMPC 里加非线性约束、且需要 \(h\) 的解析梯度，但换来的"硬安全保证"对真机部署是值得的。这也是模型派相对学习派的一大优势——可证明的安全（§8.7 对比）。

⚠️ 常见陷阱

陷阱 8.4.1（概念误区）：用刚性约束建模非抓持的推/抬 - 错误描述：不管推、搬、抬，一律用 §8.2 的刚性 6-DoF 全息约束建模，图省事。 - 现象/后果：推会算出物理上不存在的"拉力"（机器人拉不住物体，物体脱离）；抬会低估所需挤压内力（物体从垫子间滑落）。仿真完美、真机失败。 - 根本原因：刚性约束是三模式中能力最强的（可拉、可传力矩），用它近似能力更弱的非抓持接触（不能拉、托举受摩擦限），等于假设机器人有它实际没有的能力。 - 正确做法：按模式选接触约束集——推/抬用"单边 \(f_n\ge0\) + 摩擦锥"，抬额外加内力下界 \(f_n^{\min}\)（托举需求），搬才用刚性 6 维。

陷阱 8.4.2（思维陷阱）：以为"推"也能精确控物体姿态 - 错误描述：想用多机推的方式精确地把物体摆成某个朝向（如把箱子推到正对门口、严丝合缝）。 - 现象/后果：物体姿态反复过冲、震荡，怎么也调不准；一旦转过头无法拉回，只能绕一大圈重推。 - 根本原因：推是非抓持的，不能拉、（简化下）不能传力矩，只能靠多点推力的合力矩间接、单向地影响姿态——这是欠驱动的、不可逆的。 - 正确做法：要精确控姿就别用纯推——改用刚性搬（可传力矩）或捏举（可较精确跟踪位姿）。推只适合"把物体大致弄到某区域"。需要精确摆正时，最后一步可切换到夹持/捏举（模式切换，见 §8.7）。

陷阱 8.4.3（编程陷阱）：捏举时内力期望设为定值，不随负载/姿态自适应 - 错误描述：捏举搬运时把挤压内力 \(f_n\) 设成一个固定常数，不随物体重量、倾角变化调整。 - 现象/后果：物体轻时挤压过度（浪费能量、可能压坏物体、把机器人推到平衡边界）；物体重或倾斜时挤压不足（摩擦托举力 \(<\) 重力分量，物体下滑掉落）。 - 根本原因：托举所需的最小挤压内力 \(f_n^{\min}\) 由 \(\sum\mu f_n\ge m_L g\cos\phi\)（\(\phi\) 为接触面倾角）决定，是随负载和姿态变化的量，不是常数。 - 正确做法：内力期望应在线自适应——按当前估计的物体重量和接触几何算 \(f_n^{\min}\)，留安全裕度（如 \(1.3\times f_n^{\min}\)），再受 §8.3 的"不翻倒"上界约束。decPLM 用 constellation reward 隐式学到了这种自适应（§8.6）。

练习

练习 8.4.1（分析题·模式选型） 对下列任务各选一种协作模式（推/搬/抬）并说明理由（用约束集强弱 + 鲁棒性/精度权衡）：(a) 多台四足把一堆建筑废料大致清到墙角；(b) 两台四足把一根标准钢梁精确放到两个支座上（位置误差 < 2cm）；(c) 多台四足把一个无把手的大油桶从地面搬上 20cm 高的台子。对 (c)，进一步说明为什么必须是"抬"而非"推"或"搬"。

练习 8.4.2（推导题·捏举内力） 两台四足从两侧水平捏举一个重 \(m_L g\) 的竖直圆柱，接触面竖直，摩擦系数 \(\mu\)。(1) 写出"圆柱不竖直下滑"对每侧挤压法向力 \(f_n\) 的下界（两侧摩擦力之和 \(\ge\) 重力）。(2) 若圆柱被倾斜搬运、接触面与竖直成 \(\phi\) 角，下界如何变化？(3) 结合 §8.3 的"不翻倒"上界，画出 \(f_n\) 的可行区间随 \(\phi\) 变化的示意——指出存在一个临界倾角 \(\phi^*\)，超过它可行区间为空（捏不住）。这解释了为什么 decPLM 报告"姿态信息对重载/大倾角尤其关键"。

练习 8.4.3（设计扩展题·模式切换） 一个真实任务需要先**推**一个大箱子穿过房间（粗定位），再把它**精确摆正**到指定位姿。(1) 设计一个状态机，规定何时从"推"模式切换到"夹持/捏举"模式（提示：用物体到目标的距离/姿态误差作为切换条件）。(2) 切换瞬间会遇到什么力学问题（接触约束集突变、内力从 0 跳到正值）？如何平滑过渡？(3) 解释为什么"全程用一种模式"不如"分阶段切换模式"——把它和 §8.1 的选型判据联系起来。

---

§8.5 协同步态与 CoM 协调：从"站着搬"到"边走边搬" ⭐⭐⭐⭐

这一节解决什么问题：前面三节默认机器人站着不动（四脚支撑）操作物体。但真正的 loco-manipulation 是**边走边操作**——机器人要迈步前进、同时维持对物体的协同操作。一旦机器人迈步，两个新麻烦出现：(1) 迈步时支撑腿减少，GRF 重新分布，对物体的施力能力周期性变化；(2) 每台机器人迈步时质心周期性起伏，这个起伏通过物体传给其他机器人，成了注入物体的扰动。这一节处理**协同步态（cooperative gait）的相位协调**和 CoM 协调的频段分离，并把它们装进双层 MPC+WBC 架构。这是把静态搬运升级为动态搬运的关键一跃，也是本章最硬核的一节。

动机：迈步把"搬"变成了时变问题

回顾 05_运动控制/10_足式/120_步态管理与接触序列.md：单体四足行走靠**步态（gait）**——四条腿按某种相位序列交替"支撑（stance，触地出力）"和"摆动（swing，抬起前移）"。小跑（trot）是对角腿同步（左前+右后一组、右前+左后一组）；溜蹄（pace）是同侧腿同步；漫步（walk）是四腿依次迈。

单机搬运时（如果它能边走边搬），迈步带来一个直接问题：摆动相的腿不触地、不出 GRF，所以这条腿无法参与"平衡物体反力"。四脚支撑时有 4 条腿分担物体反力，trot 时只有 2 条腿（对角）支撑——施力能力和平衡裕度都减半。这意味着 §8.3 的负载分配必须是**时变的**：随步态相位，每台机器人能提供的 wrench 上限周期性变化。

多机搬运时，问题升级为**协调**：

1. 施力能力的相位耦合：如果两台机器人步态相位相同（同时摆动同一对腿），它们会在同一时刻同时进入"低施力能力"状态——物体可能在这一瞬间失去足够支撑而下坠。

2. 质心起伏的扰动注入：每台机器人 trot 时质心周期性上下起伏几厘米。多台机器人若不协调，各自的起伏通过刚性物体叠加，把物体"拧"得剧烈晃动（§8.1 场景二）。

这两个问题都指向同一个解决方向：协同步态必须做相位协调，CoM 起伏必须和物体操作做频段分离。

如果各机独立跑步态会怎样

看反面：四台四足抬桌子，每台独立跑自己的 trot，相位随机、互不协调。

后果一：物体被多频起伏"拧"。 四台机器人的质心起伏是四个独立的周期信号（频率约 2-4 Hz），相位随机。它们通过刚性桌子叠加——桌子的某个角被一台往上顶的同时另一个角被一台往下拽，桌子剧烈俯仰滚转。桌上的东西飞出去。

后果二：施力能力的"共振空窗"。 若某两台机器人碰巧相位相同、同时摆动，物体在那一瞬同时失去这两台的部分支撑。如果这种"同时低支撑"周期性发生，物体会周期性下坠-回弹，形成危险的振荡。

后果三：内力的周期性失控。 迈步改变每台机器人的施力几何（支撑腿位置变了，力臂变了），未协调时各机施加的内力分量周期性错配，物体内部被周期性"较劲"，连接处受周期性冲击载荷——长期可能损坏连接或物体。

对比性思维（反事实）：如果各机步态完全独立 → 多频起伏叠加把物体拧晃 + 共振空窗使物体周期下坠 + 内力周期失控。根本原因：单机的步态扰动只影响自己，多机搬运把每台的步态扰动通过刚性物体强制耦合，步态从"各自的内部节律"变成了"注入共享物体的多源噪声"。要消除它，必须协调相位（让起伏相互抵消或错开）并在频域上把"操作"和"踏步"分开。

历史：从单机步态到协同步态

单机步态的理论（接触序列、相位、ZMP/capture point 平衡）在 05_运动控制/10_足式 已经成熟。把它推广到多机协同，Kim–Hamed（2023）是标杆：他们的高层 SRB-MPC 联合规划两台机器人的接触时序和质心轨迹，让步态相位在优化里协调，而不是各跑各的。

De Vincenti–Coros（2023）更进一步——他们的 agnostic SQP 把**接触时序 \(\sigma_{ij}(t)\) 也当成优化变量**（0/1 变量松弛为连续），让"每台机器人什么时候迈哪条腿"从优化中涌现，而不是预设固定步态。这是"gait-agnostic"的含义：步态不是输入，是输出。

学习派（decPLM、MA-Push）则用一个**预训练的、冻结的底层运动控制器**处理单机步态，上层策略只发"基座速度命令"——把步态协调隐式地交给"上层命令 + 接触力反馈"。decPLM 明确把策略分成"底层冻结 locomotion + 高层 arm/base 命令"两层。

本质洞察：协同步态的核心，是**把"步态相位"从各机的私有变量提升为多机的协调变量**。模型派把它放进联合 MPC 显式优化（甚至让步态从优化涌现），学习派把它藏进"上层命令 + 接触力隐式同步"。两条路殊途同归——都在解决"别让各机的踏步节律打架"。这与第 2 章共识"让各 agent 达成一致"、第 4 章编队"频段分离"是同一个协调思想在步态上的体现。

理论：相位协调与频段分离

（1）相位协调：让起伏相互抵消。

设每台机器人 trot 的质心竖直起伏近似为正弦 \(z_i(t)=A\sin(2\pi f t+\varphi_i)\)（\(A\) 幅值、\(f\) 步频、\(\varphi_i\) 相位）。物体受到的净竖直扰动正比于各机起伏之和 \(\sum_i z_i(t)\)。

• 同相（\(\varphi_i\) 全相同）：\(\sum_i z_i = NA\sin(\cdot)\)，扰动幅值放大 \(N\) 倍——最坏。
• 反相协调（\(N=2\)，\(\varphi_2=\varphi_1+\pi\)）：\(z_1+z_2=A[\sin(\cdot)+\sin(\cdot+\pi)]=0\)——起伏完全抵消！
• 均布相位（\(\varphi_i=\varphi_1+2\pi(i-1)/N\)）：对 \(N\ge2\)，\(\sum_i\sin(2\pi ft+\varphi_i)\) 在多数频率下大幅抵消（类似多相整流）。

结论：协同步态应让各机相位**反相/均布**，使质心起伏在物体上相互抵消，而非同相叠加。这是协同步态相位协调最直接的设计原则。

（2）频段分离：把"操作"和"踏步"在频域分开。

接 04_移动机器人规控/50_多机器人协作/50_分布式MPC多足编队.md §4.2 的"频段分离"思想（编队约束作用在质心、踏步起伏是高频）。这里把它用于操作：

• **操作物体的运动**是**低频**的（把桌子搬到目标，秒级、< 1 Hz）。
• **踏步的质心起伏**是**高频**的（步频 2-4 Hz）。

设计上让这两个频段分开：高层 MPC 规划物体的低频期望轨迹（物体怎么走），各机的高频踏步起伏被视为叠加在上面的"载波"，通过 (1) 的相位协调相互抵消、并通过低层 WBC 在每台机器人内部吸收（不让它传到物体）。关键：高层不试图控制踏步起伏（那是低层 WBC 的事），低层不试图规划物体轨迹（那是高层 MPC 的事）——各管一个频段。

理论-工程桥接：频段分离在工程上直接体现为**双层 MPC+WBC 的频率分工**——高层 MPC 跑慢（如 60Hz，看远、管低频物体轨迹和接触时序），低层 WBC 跑快（如 500Hz，看近、管高频踏步和瞬时平衡）。安全攸关 NMPC 正是 60Hz；NMPC-with-manipulator 工作报告 NMPC 60Hz + WBC 500Hz。这个频率比（约 1:8）不是随意定的，正是为了让两层各自覆盖的频段不重叠又能衔接。

（3）接触时序作为优化变量（gait-agnostic）。

De Vincenti–Coros 把接触时序 \(\sigma_{ij}(t)\in\{0,1\}\)（机器人 \(i\) 的腿 \(j\) 在 \(t\) 是否触地）当优化变量。触地（\(\sigma=1\)）才能出 GRF，所以约束：

\[ f_{ij,z}\le \sigma_{ij}\,f_{\max},\qquad \sigma_{ij}\in\{0,1\}\ \xrightarrow{\text{松弛}}\ \sigma_{ij}\in[0,1] \]

并配合**接触互补约束** \(\sigma_{ij}\cdot(p_{\text{foot},ij}-p_{\text{ground}})=0\)（触地时足在地面、抬起时足离地）。松弛为连续后用 SQP 解。这让步态从优化涌现，同一套代码能跑不同步态。

深入：相位相消与拍频——一个完整算例

"反相/均布相消"听起来玄，这里用具体数字把它算实，并揭示一个工程上极易踩的坑——步频哪怕只差一点点，完美的相位相消也会随时间漂移成最坏的同相叠加（拍频）。这正是练习 8.5.1 第三问的核心。

设置。 每台机器人 trot 的质心竖直起伏 \(z_i(t)=A\sin(2\pi f t+\varphi_i)\)，幅值 \(A=3\) cm、步频 \(f=2.5\) Hz。物体受到的净竖直扰动正比于 \(S(t)=\sum_i z_i(t)\)。

情形一：\(N=2\) 反相。 取 \(\varphi_1=0\)、\(\varphi_2=\pi\)：

\[ S(t)=A\sin(2\pi ft)+A\sin(2\pi ft+\pi)=A\sin(2\pi ft)-A\sin(2\pi ft)=0 \]

净扰动**恒为零**——两台机器人的起伏在物体上完全抵消。物体感受不到任何踏步起伏，平稳如静止搬运。

情形二：\(N=3\) 均布。 取 \(\varphi_i=2\pi(i-1)/3=\{0,\,120^\circ,\,240^\circ\}\)。利用相量求和：三个等幅、相位互差 \(120^\circ\) 的正弦量之和为零（这正是三相交流电"零线无电流"的同一数学）：

\[ S(t)=A\big[\sin\theta+\sin(\theta+120^\circ)+\sin(\theta+240^\circ)\big]=0,\quad \theta=2\pi ft \]

对**单一频率**，均布相位同样完全相消。这就是 §8.5 理论里"\(N\ge3\) 均布相消"的来历——它和多相整流、三相电是一个原理。

情形三：同相（最坏）。 取 \(\varphi_i\) 全为 0，\(S(t)=NA\sin(2\pi ft)\)，\(N=3\) 时幅值 \(9\) cm——比单台还大 3 倍。物体被狠狠拧晃。三种情形对比：

相位策略	\(N=2\) 净扰动幅值	\(N=3\) 净扰动幅值	物体表现
同相	\(2A=6\) cm	\(3A=9\) cm	剧烈起伏，东西飞出
反相/均布	\(0\)	\(0\)	平稳，理论完全相消

情形四：拍频陷阱（步频不一致）。 现实中两台机器人步频很难精确相等。设机器人 1 步频 \(f_1=2.50\) Hz、机器人 2 \(f_2=2.55\) Hz（只差 \(2\%\)），初始反相。两者的相位差 \(\Delta\varphi(t)=2\pi(f_2-f_1)t+\pi\) 会随时间漂移。叠加幅值

\[ |S(t)|=2A\left|\cos\frac{\Delta\varphi(t)}{2}\right| \]

它以**拍频** \(f_{\text{beat}}=|f_2-f_1|=0.05\) Hz（周期 \(20\) s）在 \(0\)（完美相消）和 \(2A=6\) cm（完全同相、最坏）之间缓慢振荡。也就是说：一开始相消得好好的，约 \(10\) s 后就漂成了最坏的同相叠加，物体周期性地每 \(20\) s 剧烈晃一阵。 这是开环分配固定相位最隐蔽的失效模式，仿真里短时间看不出来、真机长时间跑必然现形。

理论-工程桥接：拍频陷阱告诉我们，相位相消不能开环设定一次了事，必须闭环锁住——要么各机步频由一个共享时钟/共识协议同步（联系第 2 章共识：让 \(f_i\) 达成一致），要么用接触力反馈在线微调相位（测到物体起伏变大就调相位）。decPLM 的"接触力隐式通信"恰好天然抗拍频——物体起伏一变大，各机从接触力里感知到并自适应调整，无需显式同步时钟。这是学习派"零通信涌现协调"相对模型派"开环规划相位"的一个被低估的优势。

阶段小结：到这里 §8.5 的动态搬运图景完整了——相位协调（相消，但要防拍频）、频段分离（高层管低频物体、低层管高频踏步）、whole-body 统一（腿臂躯干都是接触分支）。下面 §8.6 转向完全不同的一条路：不建任何模型、不显式协调，让协调从学习中**涌现**。

whole-body 协同：把"腿即臂"统一进一个 QP

§8.1 说腿可以当臂用。在 whole-body control 的视角下，腿和臂（如果有）根本不分家——它们都是浮动基座上的接触/操作分支，统一进一个分层 QP。这是 "whole-body loco-manipulation" 的核心。

回顾 05_运动控制/10_足式/90_WBC分层优化与TSID.md：单机 WBC 把"质心轨迹、足端力、摆动腿、姿态"等任务按优先级塞进分层 QP，决策变量 \([\ddot q, f, \tau]\)。多足协同 loco-manipulation 的 whole-body 把这个 QP 升级：

• 任务层增加"物体操作任务"：除了维持自身平衡（质心、姿态），还要跟踪分到的物体 wrench \(\lambda_{iL}\)（来自 §8.3 负载分配）。
• 接触集统一：触地的腿（支撑）、推物体的躯干/腿、捏物体的臂——都是"接触分支"，在 QP 里一视同仁地受摩擦锥约束。
• 优先级：通常"自身不摔倒"高于"操作物体"——宁可暂时少出点操作力，也不能翻。这个优先级在分层 QP 里通过"高优任务在其零空间才轮到低优"实现。

多视角理解（工程类比）：whole-body 把"腿即臂"统一进一个 QP，类比于操作系统把"所有 I/O 设备统一成文件"——无论键盘、磁盘、网络，都用 read/write 接口。相似之处：都用一个统一抽象（"接触分支"/"文件"）覆盖异构实体（腿/臂/躯干、键盘/磁盘），让上层逻辑不必区分。不同之处：文件接口是离散的读写，WBC 接触是连续的力/运动且受物理约束（摩擦锥、动力学）耦合，远比文件复杂。不要把类比延伸到"WBC 像文件系统那样简单"——它每步都要解一个带物理约束的 QP。

代码：协同步态的相位协调（为什么→正确→对比）

Step 1: 先讲为什么要协调相位。

为什么多机搬运的步态相位不能各跑各的，而要协调？

因为每台机器人 trot 时质心周期性起伏，这个起伏通过刚性物体传给其他机器人。
N 台同相 → 起伏叠加放大 N 倍 → 物体剧烈晃动甚至东西飞出。
N 台反相/均布 → 起伏在物体上相互抵消 → 物体平稳。

规则：协同搬运时，各机步态相位应反相(N=2)或均布(N≥3)，
     让质心竖直起伏在物体上相消，而非同相叠加。
     高层 MPC 管低频物体轨迹，低层 WBC 管高频踏步——频段分离。

Step 2: 正确写法。

import numpy as np

def cooperative_gait_phases(N, base_phase=0.0):
    """为 N 台协同搬运的机器人分配步态相位，使质心起伏相互抵消。"""
    # 均布相位：第 i 台滞后 2*pi*i/N
    return [base_phase + 2*np.pi*i/N for i in range(N)]

def residual_heave_disturbance(phases, A, f, t):
    """给定各机相位，计算注入物体的净竖直起伏扰动 sum_i z_i(t)。"""
    return sum(A*np.sin(2*np.pi*f*t + ph) for ph in phases)

# 验证：2 台反相 → 净扰动恒为 0
phases2 = cooperative_gait_phases(2)        # [0, pi]
t = np.linspace(0, 2, 200)
heave2 = [residual_heave_disturbance(phases2, A=0.03, f=3.0, ti) for ti in t]
assert np.allclose(heave2, 0, atol=1e-9)    # 起伏完全抵消！

# 4 台均布 → 净扰动大幅衰减（理论上对单频完全相消）
phases4 = cooperative_gait_phases(4)        # [0, pi/2, pi, 3pi/2]
heave4 = [residual_heave_disturbance(phases4, A=0.03, f=3.0, ti) for ti in t]
print("4机均布净起伏幅值:", np.max(np.abs(heave4)))   # ~ 接近 0

# 双层频率分工：高层 MPC 管物体低频，低层 WBC 管踏步高频
HIGH_LEVEL_HZ = 60      # 物体轨迹 + 接触时序（看远、低频）
LOW_LEVEL_HZ  = 500     # 踏步 + 瞬时平衡（看近、高频）
# 频率比 ~1:8，保证两层频段不重叠又能衔接

Step 3: 错误写法。

# ❌ 错误 1：各机独立跑步态，相位随机
def independent_gaits_WRONG(N):
    return [np.random.uniform(0, 2*np.pi) for _ in range(N)]
# 问题：随机相位下 sum_i z_i 期望幅值 ~ sqrt(N)*A，物体被多频起伏拧晃。
#       §8.1 场景二、§8.5 后果一。正确做法：均布/反相相位主动相消。

# ❌ 错误 2：高层 MPC 试图直接控制踏步起伏（频段越界）
def high_level_controls_heave_WRONG():
    # 高层 60Hz，却要跟踪 3Hz 的踏步起伏 —— 采样不足 + 越权
    pass
# 问题：60Hz 高层无法跟踪 3Hz 起伏（采样勉强、相位滞后大），且这本是
#       低层 WBC 500Hz 该管的。正确做法：高层只管 <1Hz 物体轨迹，
#       踏步起伏交给低层 WBC 吸收——频段分离。

# ❌ 错误 3：所有机器人步态同时进入摆动同一对腿（同相）
#    各机 trot 相位相同 → 同时低支撑 → 物体共振空窗
# 问题：物体周期性同时失去多台支撑而下坠-回弹。正确做法：错开相位，
#       保证任意时刻总有足够多的腿支撑物体。

Step 4: 单机步态 vs 协同步态对比。

| 维度 | 单机步态 | 协同步态 |
|------|---------|---------|
| 相位 | 自定，只影响自己 | 须协调(反相/均布)，影响物体 |
| 起伏后果 | 自身平衡问题 | 通过物体传给他机，须相消 |
| 接触时序 | 单机优化/预设 | 联合优化/全局协调(gait-agnostic) |
| 平衡裕度 | 自身支撑多边形 | 自身 + 物体施力能力的相位耦合 |
| 频段分工 | MPC+WBC 双层 | 双层 + 物体低频/踏步高频分离 |

⚠️ 常见陷阱

陷阱 8.5.1（思维陷阱）：多机搬运时各机独立跑步态 - 错误描述：让每台机器人用自己单机调好的步态控制器，相位互不协调，直接一起搬。 - 现象/后果：各机质心起伏（2-4Hz）通过刚性物体叠加，物体剧烈俯仰滚转，载荷飞出；某些相位组合下物体周期性下坠（共振空窗）。 - 根本原因：单机步态扰动只影响自己，多机搬运把每台的踏步起伏通过物体强制耦合成多源噪声；同相时放大 \(N\) 倍。 - 正确做法：协调各机步态相位为反相（\(N=2\)）或均布（\(N\ge3\)），让质心起伏在物体上相互抵消；高层联合规划接触时序避免共振空窗。

陷阱 8.5.2（概念误区）：把物体轨迹和踏步起伏放在同一控制层处理 - 错误描述：用一个单层控制器同时管"物体搬到哪"（低频）和"腿怎么迈"（高频）。 - 现象/后果：要么控制频率不够跟踪高频踏步（物体轨迹层若 60Hz 跟不上 3Hz 起伏的相位），要么为跟高频而把整层提到 500Hz 导致物体轨迹优化（含远期预测）算不过来。 - 根本原因：物体操作和踏步在频域差一个数量级，单层无法兼顾"看得远（低频物体）"和"反应快（高频平衡）"。 - 正确做法：双层架构——高层 MPC 慢（60Hz，远期物体轨迹+接触时序），低层 WBC 快（500Hz，瞬时踏步+平衡），频段分离、各管一段。

陷阱 8.5.3（编程陷阱）：负载分配用四脚支撑的固定值，不随步态相位更新 - 错误描述：边走边搬时，仍用站立（四脚支撑）算出的负载分配，迈步时不更新。 - 现象/后果：摆动相某条腿不触地、无法出 GRF，但分配仍假设它在出力——实际平衡方程不满足，机器人在摆动相失稳或物体瞬时失支撑。 - 根本原因：迈步使支撑腿集合时变，每台机器人的施力能力随相位周期性变化，固定分配与实际触地状态不符。 - 正确做法：负载分配必须时变——按当前触地腿集合（\(\sigma_{ij}=1\) 的腿）实时重算各机能提供的 wrench 上限，并在 MPC 预测时域里考虑未来相位变化。这就是把 §8.3 的 QP 嵌进随步态滚动的 MPC。

练习

练习 8.5.1（推导题·相位相消） 三台四足均布相位（\(\varphi_i=2\pi(i-1)/3\)）抬一个平台，每台质心竖直起伏 \(z_i=A\sin(2\pi ft+\varphi_i)\)。(1) 证明净竖直起伏 \(\sum_{i=1}^3 z_i(t)\equiv0\)（提示：三个相位差 \(120°\) 的等幅正弦之和为零）。(2) 若步频有 \(\pm10\%\) 的不一致（各机 \(f\) 略不同），净起伏还为零吗？定性说明会出现什么（拍频/缓慢漂移）。(3) 由此说明协同步态不仅要相位协调，还要**步频同步**——这又指向第 2 章的什么机制？

练习 8.5.2（分析题·频段分离） 某团队做四足协同搬运，物体轨迹和踏步都用一个 100Hz 的控制器处理，效果差。(1) 用频段分离原理诊断问题：物体操作（< 1Hz）和踏步起伏（3Hz）混在一层，100Hz 对哪个不够？对哪个浪费？(2) 给出双层重构方案，指定每层的频率和职责。(3) 估算：若高层 MPC 预测时域 1.5s、每步要解一个含 4 机 + 物体的 NLP，60Hz 意味着每次必须在多少毫秒内解完？这对求解器（稀疏 SQP）提出了什么要求？（联系 §8.2 保留 interaction wrench 的稀疏性）

练习 8.5.3（设计扩展题·whole-body 任务优先级） 用分层 WBC（接 05_运动控制/10_足式 §90）为一台"边走边用一条前腿推物体"的四足设计任务优先级。可选任务：(a) 维持质心在支撑多边形内（不摔）；(b) 摆动腿跟踪落脚点；(c) 用抬起的前腿对物体施加期望推力；(d) 躯干姿态水平。(1) 给这四个任务排优先级并说明理由（哪个绝不能让步？哪个可以妥协？）。(2) 解释为什么"推物体"通常要排在"不摔倒"之后——把它和 §8.1 陷阱 8.1.2、§8.3 的"不翻倒"约束联系起来。(3) 当机器人即将失稳时，分层 QP 会自动牺牲哪个任务？这体现了优先级设计的什么好处？

---

§8.6 学习化涌现协调：零通信、constellation reward 与排列不变扩展 ⭐⭐⭐⭐

这一节解决什么问题：前五节是模型派——精确建模、显式优化、可证明安全，但要求知道物体模型、连接方式、且求解器要快。学习派走完全不同的路：不显式建模耦合，让协调从奖励和物理交互中涌现。这一节讲透学习派的三个核心机制：decPLM 的 **constellation reward（星座奖励）**如何把"刚性接触行为"逼出来、**接触力作为隐式通信**如何在零显式通信下实现协调、**permutation-invariant + 参数共享**如何实现 \(N{=}2\) 训练→\(N{=}10\) 部署的零样本扩展。这是与模型派并列的另一条腿，也是第 10 章 MARL 在多足搬运上的完整落地预演。

动机：模型派的三个"如果没有"

模型派很美，但它依赖三个前提，现实中常常缺失：

1. 如果不知道物体的精确模型呢？ 互联 SRB 网络要物体的质量 \(m_L\)、转动惯量 \(I_L\)。搬一个不知道里面装了什么的箱子、一根重量不均的原木——模型从哪来？

2. 如果机器人之间不能通信呢？ Kim–Hamed 的集中式 MPC 要一个中央节点收集所有机器人状态、统一求解、下发。在通信受限/拒止的环境（地下、灾后、强干扰），中央协调不可行。

3. 如果机器人数量随时变、且很多呢？ 模型派的联合 MPC 维度随 \(N\) 增长、求解 \(O(N^3)\) 爆炸（第 4 章）。2 台还行，10 台呢？而且部署时数量可能临时变化（一台没电退出）。

decPLM 的回答惊人：10 台 Go2，零通信，每台只观察自身状态和接触力，搬运沙发——而且策略只用 2 台训练，部署时 2 到 10 台都行。 它怎么做到的？这一节拆解。

如果天真地让每台机器人独立"把物体推向目标"会怎样

看反面：最朴素的去中心化想法——每台机器人独立地、贪婪地"把物体推向目标位置"。

后果：内力对抗、空耗能量。 两台机器人在物体两侧，都想把物体推向目标。机器人 A 在左侧往右推，机器人 B 在右侧往左推（因为对 B 来说，往左才是朝目标方向）——两个推力方向相反，在物体上抵消，物体纹丝不动，两台机器人互相对抗、空耗能量（§8.1 场景三）。

更微妙的后果：没有"维持接触"的激励。 天真奖励只看"物体到目标的距离"，不奖励"维持与物体的稳定接触"。于是机器人可能学到"撞一下物体就跑"的退化策略——短暂接触给物体一个冲量，然后失去接触，物体很快停下，再撞一下……既不平稳也不高效。

对比性思维（不是 X 而是 Y）：有效的去中心化协调，不是**让每台机器人独立追求团队目标（那会退化成内力对抗），**而是**让每台机器人追求一个**精心设计的、能隐式诱导协调的局部目标。decPLM 的关键洞察是：与其让机器人"把物体推向目标"，不如让它"维持自己与物体的特定相对位姿"——当每台都维持正确的相对位姿时，物体自然被协调地搬动。协调藏在"维持相对位姿"这个局部目标里，而非显式的团队协商里。

历史：从有通信 MARL 到零通信涌现

多机协同的学习方法有一条从"强协调"到"弱协调"的演进。

MA-Push（2024）：分层 MARL + 集中高层。 用三层架构：高层 RRT 规划器 + 集中自适应策略给物体生成子目标；中层去中心化目标条件策略让各机协调推向子目标；底层运动控制器。它**有一个集中的高层**给子目标——协调主要靠高层统筹，中层只是执行。这是"半集中"的协调。

An–Hutter 排列不变网络（RSS 2024）：可扩展的注意力协调。 用排列不变神经网络（对 agent 排列和数量不变）+ 无模型 RL，让控制 agent 自主判断不同实体（其他机器人、目标）的相对重要性，不受排序或固定容量限制。在多台轮足四足上验证。这解决了"可扩展"——网络结构天然支持任意 agent 数。

decPLM（2025）：完全去中心化、零通信、涌现协调。 走到极致——无中央高层、无机间通信、每台只用本地观测。协调完全靠"参数共享 + constellation reward + 接触力隐式同步"涌现。这是"弱协调"的极致，也是本节的主角。

本质洞察：学习派的演进主线是**"把协调从显式（通信/中央节点）逐步推进到隐式（奖励设计/物理交互）"**。模型派的协调是显式的（MPC 里写出耦合约束），学习派的极致（decPLM）是隐式的（协调藏在奖励和接触力里，没有任何一行代码"显式协调"）。这两个极端——显式 vs 隐式协调——正是 §8.7 选型的核心维度，也呼应第 7 章的主从（显式）vs 对称协调（隐式）辩论。

理论：decPLM 的三大机制

机制 1：constellation reward（星座奖励）——把刚性接触"逼"出来。

decPLM 的核心创新。问题：如何让一个不抓持的捏举接触表现得像刚性连接（物体跟着机器人走、相对位姿保持）？答案：奖励机器人维持一组**点**（"星座"）与目标点的对齐。

定义**星座距离**：给定机器人当前的一组点 \(P=\{p_1,\dots,p_K\}\) 和目标点集 \(P^*=\{p_1^*,\dots,p_K^*\}\)，

\[ \boxed{d_{\text{con}}(P, P^*) = \frac{1}{K}\sum_{k=1}^{K}\|p_k - p_k^*\|_2^2} \]

decPLM 给每台机器人定义**两个星座**：

• 接触星座（end-effector contact constellation）：沿臂垫法向取 5 个共线点（跨 50cm），匹配到接触帧法向。奖励 \(R_{\text{contact}}=-d_{\text{con}}(P_{\text{pad}}, P_{\text{cf}})\)。关键：一组点（而非单点）的对齐同时编码了位置**和**法向姿态——5 个共线点对齐了，不仅垫子位置对，朝向也对了。

• 基座跟踪星座（base tracking constellation）：在基座上取 3 个非共线点（约 10cm 立方体角），匹配到"刚性运动学假设下"的目标位置。奖励 \(R_{\text{track}}=-d_{\text{con}}(P_{\text{base}}, P_{\text{rigid}})\)。3 个非共线点对齐 = 基座的平移**和**转动都对（3 点定一个刚体位姿）。

总奖励 \(R_{\text{constellation}}=R_{\text{contact}}+R_{\text{track}}\)。

本质洞察：constellation reward 的精妙在于——用"一组点的距离"统一编码了位置和姿态，而不需要分别写位置误差和姿态误差（姿态误差涉及 \(SO(3)\) 上的度量，难处理）。几何上，\(\ge3\) 个非共线点的位置完全确定一个刚体的 6-DoF 位姿，所以"点集对齐"等价于"位姿对齐"，却用简单的欧氏距离平方表达。这把一个 \(SE(3)\) 跟踪问题降维成一组 \(\mathbb{R}^3\) 点的回归——优雅且对 RL 友好（奖励光滑、无 \(SO(3)\) 的奇异）。decPLM 报告这个奖励对协调"至关重要（critical）"。

多视角理解（类比 ICP/点云配准）：constellation 距离和点云配准里的 ICP（Iterative Closest Point）代价高度相似——都是"两组点的对应距离平方和"。相似之处：都用点集对齐隐式表达刚体位姿对齐，都避免直接处理旋转。不同之处：ICP 是离线求两组点云的最优刚体变换，constellation reward 是在线给 RL 一个标量奖励信号；ICP 的点对应要搜索，constellation 的点对应是预先固定的（哪个点对哪个目标点已知）。不要把类比延伸到"decPLM 在做配准"——它是在用配准式的度量做奖励塑形。

机制 2：接触力作为隐式通信——零通信如何协调。

decPLM 每台机器人只观察：自身本体感知（关节角/速度/IMU）、接触力、物体相对位姿（可选）——没有其他机器人的任何信息，没有通信。协调如何涌现？靠三条：

1. 参数共享（shared policy）：所有机器人用同一个策略网络 \(\pi_\theta\)。同一个"大脑"在相似情境下做相似决策——天然的隐式协调。它们不需要协商，因为它们"想法一致"。

2. 接触力作为物理信道：当机器人 A 推/抬物体，物体运动，机器人 B 通过**自己感受到的接触力变化**间接"得知"A 的动作。接触力是穿过物体的物理信号——A 用力 → 物体动 → B 的接触力变 → B 调整。物体本身成了通信媒介。这正是第 7 章说的"边既可以是无线链路，也可以是穿过刚性物体的力"。

3. 接触帧命令的几何一致性：把团队级的物体期望运动转换成每台机器人的接触帧命令：\(v_{\text{cf}}^{(r)}=v_{\text{pl}}+\omega_{\text{pl}}\times p_{\text{offset}}^{(r)}\)，\(\omega_{\text{cf}}^{(r)}=\omega_{\text{pl}}\)。每台机器人收到的是"在它那个接触点上物体该怎么动"——这些命令几何上自洽（都来自同一个物体刚体运动），所以各机即使不通信，目标也天然协调。

本质洞察：decPLM 的"零通信"不是"零协调"，而是**把协调从显式通信转移到了三个隐式渠道：共享的大脑（参数共享）、共享的物理媒介（接触力）、共享的几何目标（接触帧命令）。这三者合起来，让各机在不交换一个比特的情况下行动一致。这是对"协调必须靠通信"这一直觉的深刻反驳——**物理交互本身就是一种通信。

机制 3：permutation-invariant + 参数共享——零样本扩展。

decPLM 报告：策略只用 \(N=2\) 训练，零样本部署到 \(N=2,4,6,8,10\)，且团队越大跟踪越好（线速度误差从 2 台的 ~0.1 m/s 降到 10 台的 ~0.02 m/s，掉落率从 5% 降到 <1%）。为什么能扩展？

• **参数共享**让策略与"我是第几台"无关——每台跑同一个 \(\pi_\theta\)，加几台就是多几个跑同一策略的个体，策略本身不变。
• **局部观测**让策略输入维度与 \(N\) 无关——每台只看自己 + 物体，不看其他机器人，所以 \(N\) 变化不改变输入维度。
• An–Hutter 的**排列不变网络**进一步处理"需要看其他实体"的情况——用对输入排列不变的架构（如注意力/平均池化），让策略对"其他机器人的数量和顺序"不敏感。

对比性思维（反事实）：如果用"拼接所有机器人状态"的中心化策略会怎样？→ 输入维度 \(\propto N\)，训练时定的 \(N\) 一变，网络结构就得改、得重训 → 无法零样本扩展。根本原因：中心化策略把 \(N\) 焊进了网络结构；参数共享 + 局部观测 + 排列不变把 \(N\) 从结构里解耦出来。这是学习派可扩展性的根源，也是它相对模型派 \(O(N^3)\) 的关键优势——加机器人几乎零成本。

深入：为什么"团队越大跟踪越好"——一个误差相消的算例

decPLM 报告了一个反直觉的现象：机器人越多，物体跟踪反而越准、掉落率反而越低（线速度误差从 2 台的 ~0.1 m/s 降到 10 台的 ~0.02 m/s）。直觉上"人多嘴杂应该更难协调"，为什么恰恰相反？这里给一个定量解释——它本质是**独立误差的统计相消**，和"多次测量取平均降噪"是同一个数学。

模型。 每台机器人对物体施加的力，可分解为"理想协调分量 \(\bar f\)"（所有机器人一致想让物体去的方向）加上"个体误差 \(\varepsilon_i\)"（各机策略的随机偏差、本体感知噪声、sim-to-real 扰动）。关键假设：在参数共享 + 局部观测下，各机的误差 \(\varepsilon_i\) 是**近似独立同分布**的零均值随机量（每台看自己的局部观测、独立决策），设方差 \(\sigma^2\)。物体受到的合力是各机力之和：

\[ F_{\text{obj}}=\sum_{i=1}^{N}(\bar f+\varepsilon_i)=N\bar f+\sum_{i=1}^{N}\varepsilon_i \]

误差的相对大小随 N 衰减。 信号项是 \(N\bar f\)（随 \(N\) 线性增长），噪声项 \(\sum\varepsilon_i\) 是 \(N\) 个独立零均值量之和，其标准差按独立和的规律只增长到 \(\sqrt N\,\sigma\)。于是**相对扰动**（噪声/信号）

\[ \frac{\text{std}\big(\sum_i\varepsilon_i\big)}{\|N\bar f\|}=\frac{\sqrt N\,\sigma}{N\|\bar f\|}=\frac{\sigma}{\sqrt N\,\|\bar f\|}\ \propto\ \frac{1}{\sqrt N} \]

相对误差按 \(1/\sqrt N\) 衰减！代入数字验证：\(N=2\) 时相对误差正比于 \(1/\sqrt2\approx0.71\)，\(N=10\) 时正比于 \(1/\sqrt{10}\approx0.32\)——比值约 \(0.32/0.71\approx0.45\)，与 decPLM 报告的"\(0.02/0.1=0.2\)"量级吻合（实测衰减更快，因为大团队还摊薄了单台失效的影响）。物体作为一个物理低通滤波器，把各机独立的高频抖动平均掉了，机器人越多、平均越充分、物体越平稳。

为什么掉落率也降。 同理，单台机器人偶发的接触力骤降（快要掉的前兆），在 \(N\) 台里只占 \(1/N\) 的合力份额，其余 \(N{-}1\) 台的接触力能临时补上——团队越大，单点失效的冗余越足。这就是 decPLM 掉落率从 2 台 \(5\%\) 降到 10 台 \(<1\%\) 的力学根源：冗余度随 \(N\) 提升。

本质洞察（数值版）：模型派的"多即是难"（\(O(N^3)\) 求解爆炸）和学习派的"多即是稳"（误差 \(1/\sqrt N\) 相消、冗余增加）形成鲜明对照，这是两派最深刻的分野之一。根源在于**协调方式**：模型派把 \(N\) 台耦合进一个联合优化，维度和耦合都随 \(N\) 暴涨；学习派让 \(N\) 台各自独立决策、靠物体物理地平均，\(N\) 反而成了降噪和冗余的来源。这条 \(1/\sqrt N\) 律也提示了学习派多足协同的一个甜区——中到大规模（\(N\gtrsim5\)）恰是学习派相对模型派优势最大的区间，呼应 §8.7 选型决策树"大规模 → 学习派/分布式"的分支。但要注意它的前提是误差独立——若所有机器人因同一个 sim-to-real 偏差而**同向**犯错（误差不独立），相消失效，这正是接触力 sim-to-real gap 危险的深层原因（§8.6 sim-to-real 小节）。

decPLM 训练设置（工程细节）

把 decPLM 的训练配置整理出来，供动手参考：

# decPLM 训练设置（IsaacLab + RSL-RL）
平台: Unitree Go2 + D1 臂, 自定义橡胶接触垫
仿真: IsaacLab, 2048 并行环境
算法: MAPPO (Multi-Agent PPO), CTDE（集中训练分散执行）
策略架构:
  底层 locomotion: 预训练并冻结的运动控制器（3层 128 MLP, ELU）
  高层: 2层 128 MLP，输入 = 本体感知 + 接触帧位姿(SE3,7D,可选)
        + 接触帧命令(v,ω,h) + 时间同步信号 t_sync∈[0,5]
        输出 = 臂关节目标 + 基座命令；50Hz 执行，完全分散
训练: N=2 训练
课程: 3 阶段——Pinch(7s) → Lift(14s) → Move(14s)，逐步解锁命令模态
critic: 非对称（训练时观察所有机器人本地观测，稳定学习；执行时不需要）
域随机化:
  物体质量: [0.1, 2] kg（论文主实验范围；扩展到重载需 cf+ 姿态信息）
  接触参数、外力、初始位姿、传感/动作噪声、机器人动力学
零样本泛化:
  N: 2 训练 → 2/4/6/8/10 部署（团队越大跟踪越好）
  物体: 圆木(2kg,2机)/桶(3kg,3机)/沙发(10kg,5机) 等分布外物体
真机: 2/3/4 台 Go2，轻物体(1-2kg, 安全考虑)；存在显著 sim-to-real gap

理论-工程桥接：注意"非对称 critic（asymmetric critic）"这个 CTDE 的关键技巧——训练时 critic 能看到所有机器人的本地观测（上帝视角，稳定价值估计），但执行时只用 actor（本地观测，真正分散）。这让 decPLM 在训练期享受集中信息的稳定性、部署期享受分散执行的可扩展性。这是 §8.7 对比中"学习派如何兼顾训练稳定与部署分散"的答案，也是第 10 章 MARL 的核心范式之一。

sim-to-real：学习派的阿喀琉斯之踵

decPLM 诚实地报告了**显著的 sim-to-real gap**，归因于：未知的臂动力学、关节编码器偏移、有限的臂力矩、物体可变形性。这暴露了学习派多足协同的核心软肋——接触力的仿真-真实差异。

为什么接触力尤其难？接触是刚性、间歇、高频的物理现象，仿真器（即使 IsaacSim/MuJoCo）对接触刚度、摩擦、碰撞的建模都是近似；而多足协同**全靠接触力协调**（机制 2），接触力一旦 sim≠real，协调就崩。这比单机 locomotion 的 sim-to-real 更难——单机主要是足-地接触（相对简单、可域随机化覆盖），多足协同还有"机器人-物体""物体-环境"多重接触，且这些接触力是协调信号本身。

缓解手段（综合文献与单机经验）：(1) 重度域随机化（接触参数、质量、摩擦——decPLM 已用）；(2) 接触力观测加噪训练；(3) 课程学习从易到难；(4) 真机微调/在线适应。但 decPLM 的诚实报告说明这仍是开放问题（§8.7 开放问题）。

对比性思维（模型派 vs 学习派的 sim-to-real）：模型派的 sim-to-real 风险在**模型失配**（SRB 简化、物体参数不准）但有 CBF 等硬安全保证兜底；学习派的风险在**接触力分布偏移**且缺乏形式化安全保证。两者都怕接触，但怕的方式不同——模型派可以"知道自己不知道"（约束违反可检测），学习派的策略是黑箱（失败时难诊断）。这是 §8.7 选型必须权衡的。

⚠️ 常见陷阱

陷阱 8.6.1（思维陷阱）：以为"零通信"等于"零协调" - 错误描述：看到 decPLM 零通信，就以为它的机器人各自为政、没有协调机制，于是自己做去中心化时让每台机器人完全独立追求团队目标。 - 现象/后果：机器人退化成内力对抗（互相推抵消）或"撞一下就跑"，物体搬不动或剧烈震荡。 - 根本原因：零通信不等于零协调——decPLM 的协调藏在参数共享、接触力隐式通信、接触帧命令几何一致性这三个隐式渠道里。去掉显式通信不等于去掉协调机制。 - 正确做法：去中心化协调必须设计隐式协调渠道——用参数共享让"大脑一致"、用 constellation reward 让"局部目标诱导全局协调"、把团队目标几何分解成自洽的个体命令。协调可以隐式但不能没有。

陷阱 8.6.2（概念误区）：用单点位置误差代替 constellation reward 做姿态对齐 - 错误描述：想让接触维持刚性行为，只奖励"接触点位置接近目标"，用单点位置误差。 - 现象/后果：位置对了但姿态不受约束——机器人可能位置对、朝向乱，接触法向偏离，捏举不稳或推力方向错。 - 根本原因：单个点的位置只有 3 个自由度，无法约束刚体的 3 个转动自由度。要约束完整 6-DoF 位姿，需要 \(\ge3\) 个非共线点（或共线点约束法向）。 - 正确做法：用 constellation reward——一组点（接触星座用共线点定法向，基座星座用非共线点定全位姿）的距离平方和，同时编码位置和姿态。这是 decPLM 报告"对协调至关重要"的设计。

陷阱 8.6.3（编程陷阱）：用拼接全体状态的中心化策略，导致无法扩展 agent 数 - 错误描述：MARL 策略输入用"所有机器人状态拼接"，训练时定 \(N=4\)，网络输入维度写死为 \(4\times d\)。 - 现象/后果：部署时想用 6 台，输入维度对不上，网络无法直接用；想换 agent 数就得改结构重训，零样本扩展不可能。 - 根本原因：拼接式输入把 \(N\) 焊进了网络结构，策略与"有几台、谁是谁"强耦合。 - 正确做法：用参数共享 + 局部观测（每台只看自己+物体，输入维度与 \(N\) 无关）；若需感知其他实体，用排列不变架构（注意力/池化）让策略对 agent 数量和顺序不变。这样 \(N=2\) 训练能零样本到 \(N=10\)。

练习

练习 8.6.1（实现题·constellation reward） 实现 decPLM 的 constellation reward 并验证它对姿态的约束。(1) 给定一个刚体的目标位姿（位置 + 四元数），在刚体上取 3 个非共线点，计算它们的目标位置。(2) 实现 \(d_{\text{con}}=\frac1K\sum\|p_k-p_k^*\|^2\)。(3) 做实验：固定刚体位置不变，只旋转刚体一个小角度，观察 \(d_{\text{con}}\) 如何随旋转增大——验证它确实惩罚姿态偏差（对比：单点位置误差在只旋转、位置不变时为零，完全不约束姿态）。(4) 解释为什么用 3 个非共线点而不是 2 个或 1 个。

练习 8.6.2（分析题·隐式通信带宽） decPLM 靠接触力做隐式通信。(1) 定性分析：接触力作为通信信道的"带宽"受什么限制？（提示：物体的刚度——越刚，A 的动作越快传到 B；越软/有间隙，传递越慢越失真。）(2) 对比"刚性搬（§8.4 模式2）"和"非抓持推（模式1）"哪种的接触力隐式通信带宽更高，为什么。(3) 由此说明：为什么 decPLM 的 constellation reward 要努力维持"刚性接触行为"——刚性接触不仅利于搬运，还利于隐式通信。把这条与第 7 章"穿过刚性物体的力是一种通信边"联系起来。

练习 8.6.3（跨章综合题·接第 10 章 + §8.2 模型派） 对比 decPLM（学习派零通信）和 Kim–Hamed（模型派集中 MPC）在"信息传递方式"上的本质区别（这正是骨架思考题）。(1) Kim–Hamed 通过什么传递信息（通信拓扑 + 中央求解）？decPLM 通过什么（接触力 + 参数共享）？(2) 用第 1 章的集中/分布/去中心化三分法给两者定位，并说明 Kim–Hamed 的分布式版本（用 ADMM/leader-follower）处在谱系的什么位置。(3) 设计一个思想实验：在通信完全可靠 vs 完全拒止两种极端环境下，分别该选哪种方法，为什么？(4) 展望：能否把两者结合——用模型派提供安全保证（CBF）、用学习派提供零通信涌现协调？（这正是 ACLM 等 2026 工作的方向，联系 §8.7。）

---

§8.7 模型派 vs 学习派：全景总表与选型决策 ⭐⭐⭐

这一节解决什么问题：前六节把多足协同 loco-manipulation 的两条主线（模型派 §8.2-8.5、学习派 §8.6）都讲透了。但拿到一个真实任务——"\(k\) 台四足搬一个物体过一段地形"——到底该选哪条线、哪个具体方法？这一节把全章方法收进一张"能力矩阵"，给出一个可操作的选型决策框架，并诚实列出这个年轻方向的开放问题。这是全章的收口，也是你以后回查的索引。

动机：没有银弹

读完前六节，一个自然的诱惑是"选一个最好的方法用到底"。但多足协同 loco-manipulation 没有银弹——模型派和学习派各有不可替代的优势区间：

• 要**可证明的安全**（医疗、载人、贵重物体）→ 模型派（CBF 硬保证），学习派给不了形式化安全。
• 要**零通信、大规模、未知物体**（灾后、群体）→ 学习派（涌现协调、零样本扩展），模型派的集中 MPC 扩展不了、要物体模型。
• 要**精确控位姿** → 模型派（显式优化物体轨迹）或学习派的 constellation 跟踪，纯推模式都不行。
• 要**快速部署、无需建模** → 学习派（端到端训练），模型派要逐项建模。

选型的本质，是把任务需求映射到这两条线的优势区间。

理论：全章方法能力矩阵

把全章涉及的代表性方法收进一张表，按七个维度打分。

方法	派别	协调方式	通信需求	可扩展性	安全保证	需物体模型	控姿精度	代表场景
Kim–Hamed 集中 MPC	模型	集中求解	高(中央)	差(\(O(N^3)\))	隐式(约束)	是	高	双 A1 刚性搬+地形
Kim–Hamed 分布 MPC	模型	分布(leader-follower)	中(邻居)	中	隐式	是	高	双 A1 降算力
De Vincenti–Coros agnostic SQP	模型	集中求解	高	中	隐式	是(但agnostic)	高	任意多足+任意负载+任意步态
安全攸关 CBF-NMPC	模型	集中求解	高	差	硬(CBF)	是	高	双 Go2 避障搬运,载重74%
ACLM (ADMM 分布 MPC)	模型	分布(ADMM)	中(邻居)	中-好	隐式	是	高	预抓持协同,可扩展
MA-Push (分层 MARL)	学习	半集中(高层子目标)	中(高层)	中-好	无	否	中(推)	多机推大物体过障碍
An–Hutter 排列不变	学习	去中心(注意力)	低	好(排列不变)	无	否	中	多轮足四足可扩展协同
decPLM (零通信 MARL)	学习	去中心(零通信)	无	好(零样本2→10)	无	否(域随机)	中-高(constellation)	2-10 Go2+臂捏举搬运

系统性分类：这张表把方法按"派别 × 协调方式 × 七维能力"做了穷举式分类。读它的方式不是记每一行，而是看**两条对角线**：左上（模型派集中）安全/精确但不可扩展、要模型；右下（学习派去中心）可扩展/无需模型但无安全保证、控姿稍弱。中间地带（分布式 MPC、半集中 MARL）是两条线相互靠拢的产物——分布式 MPC 借鉴学习派的可扩展，半集中 MARL 借鉴模型派的统筹。2025-2026 的前沿正是这个中间地带的融合。

选型决策框架

给定一个真实任务，按以下决策树选方法：

多足协同 loco-manipulation 选型决策树
│
├─ Q1: 需要可证明的安全保证吗？（载人/贵重/医疗/狭窄避障）
│   ├─ 是 → 模型派 + CBF（安全攸关 NMPC 路线）。其余维度在模型派内选。
│   └─ 否 → 继续 Q2
│
├─ Q2: 机器人之间能可靠通信吗？
│   ├─ 不能（拒止/地下/强干扰）→ 学习派去中心（decPLM 零通信路线）
│   └─ 能 → 继续 Q3
│
├─ Q3: 知道物体的精确模型吗？（质量/惯量/形状）
│   ├─ 不知道 且 难建模 → 学习派（域随机化覆盖,decPLM/MA-Push）
│   └─ 知道 → 继续 Q4
│
├─ Q4: 机器人规模多大？会动态变化吗？
│   ├─ 大(≥4)或动态变 → 分布式 MPC(ACLM) 或 学习派(排列不变)
│   └─ 小(2-3)固定 → 集中 MPC（Kim–Hamed/agnostic SQP，最精确）
│
└─ Q5: 物体能抓持/连接吗？任务要多精确控姿？
    ├─ 可连接 + 高精度 → 刚性搬 + 模型派 MPC（§8.4 搬模式）
    ├─ 不可抓 + 中精度 + 需托离地 → 捏举 + 学习派 constellation（§8.4 抬模式）
    └─ 不可抓 + 低精度(大致到位) → 推 + 学习派 MARL（§8.4 推模式）

一句话选型口诀：要安全找 CBF，要零通信找 decPLM，不知模型找域随机，要规模找分布式/排列不变，要精度找刚性 MPC，物体没法抓就推或捏。

理论-工程桥接：这个决策树的每个分支都对应一个真实的工程约束——安全（监管/事故责任）、通信（部署环境）、建模（物体已知性）、规模（任务体量）、抓持（物体形态）。选型不是选"最先进的方法"，而是选"最匹配约束的方法"。一个 2 台四足搬已知桌子过平地的任务，用 decPLM 是杀鸡用牛刀（还引入 sim-to-real 风险），用 Kim–Hamed 集中 MPC 反而又快又准又安全。反过来，10 台搬未知沙发过废墟，模型派根本跑不起来，只能 decPLM。

模型派与学习派的融合趋势（前沿）

2025-2026 的研究正在把两条线缝起来，值得了解：

• ACLM（2026）：用 ADMM 把模型派的 MPC 分布式化，逼近学习派的可扩展性，同时保留模型派的可解释和（部分）保证。
• 学习增强的分布式预测控制（如 "Safe Distributed Learning-Enhanced Predictive Control for Multiple Quadrupedal Robots"）：在分布式 MPC 里嵌入学习组件（学残差动力学、学协调策略），用 CBF 兜底安全——模型派的安全 + 学习派的适应。
• 排列不变 + MPC warmstart：用排列不变学习策略给 MPC 提供初值/热启动，加速求解、改善可扩展。

本质洞察：模型派和学习派不是对立的，而是**互补的两个极端**——模型派强在"先验知识 + 可证明保证"，学习派强在"无模型适应 + 可扩展涌现"。融合的方向是明确的：用模型派守住安全底线（CBF/约束），用学习派填补模型未知（残差/接触/协调），用分布式架构换取可扩展。这正是第 10 章 MARL 之后、以及 04_移动机器人规控/50_多机器人协作/130_MARL与传统规控混合.md 要深入的"学习与规控混合"主题在多足协同上的具体体现。

开放问题

这个年轻方向有大量开放问题，列出几个最重要的供研究参考：

1. 接触力的 sim-to-real gap（最关键）：多足协同全靠接触力协调，而接触在仿真中建模最不准。decPLM 诚实报告了显著 gap。如何让接触力在仿真和真实间一致，是学习派落地的最大障碍（联系 §8.6）。

2. 学习派的形式化安全：学习策略是黑箱，没有 CBF 那样的硬保证。如何给学习的多足协同策略提供安全证书（safety certificate），是模型-学习融合的核心难题。

3. 异构多足协同：本章假设同构（同型号机器人）。异构（不同型号、不同能力、四足+人形+带臂）协同是第 9 章主题，但其力分配、协调更难——能力差异大的 agent 如何公平且稳定地分担负载？

4. 大规模（>10）涌现协调的理论：decPLM 实验上 2→10 可扩展，但缺乏理论保证——什么条件下涌现协调成立？什么时候会崩（如某个临界 \(N\)）？

5. 长时程、多阶段任务：真实任务常是"走过去→抓→搬→放→走开"的长序列（MA-Push 的 long-horizon、De Vincenti 的 pick-and-place curriculum 在攻这个），多阶段间的衔接、失败恢复仍不成熟。

⚠️ 常见陷阱

陷阱 8.7.1（思维陷阱）：盲目选"最先进/最 fancy"的方法 - 错误描述：看到 decPLM 十台零通信很酷，不管任务约束就选它；或迷信模型派的严谨，不管规模就上集中 MPC。 - 现象/后果：用 decPLM 搬已知物体过平地→引入不必要的 sim-to-real 风险、调参成本；用集中 MPC 搬未知物体或 10 台→根本跑不起来。 - 根本原因：方法的"先进"是相对任务约束而言的。脱离约束谈优劣是空中楼阁。 - 正确做法：用选型决策树，把任务的硬约束（安全/通信/建模/规模/抓持）逐一过一遍，选最匹配约束的方法，而非最 fancy 的。

陷阱 8.7.2（概念误区）：以为模型派和学习派非此即彼 - 错误描述：认为必须在"全模型"和"全学习"之间二选一，忽略中间地带和融合方案。 - 现象/后果：错过"模型派守安全 + 学习派填未知"的融合方案（ACLM、学习增强 MPC），在纯模型/纯学习的局限里硬撑。 - 根本原因：把两条线看成对立而非互补。实际上它们的优势正交（先验保证 vs 无模型适应），天然适合融合。 - 正确做法：考虑混合方案——用 CBF/约束守安全底线，用学习填补模型未知（残差、接触、协调），用分布式架构换可扩展。这是前沿方向，也是 §8.7 融合趋势的核心。

练习

练习 8.7.1（综合选型题） 对下列五个真实任务，用选型决策树各选一个方法，并写出你走过的决策分支：(a) 两台四足在工厂里把一个已知重量的标准料箱精确搬到货架（位置误差<2cm，有可靠 WiFi）；(b) 八台四足在地震废墟（无通信）协作把一块预制板大致清开；(c) 三台带臂四足在医院把一个贵重医疗设备搬过走廊（绝不能撞墙、不能掉）；(d) 五台四足搬一个不知道里面装什么的大箱子过草地；(e) 两台四足把一根原木从地面抬上卡车。

练习 8.7.2（分析题·融合方案设计） 为"6 台四足在通信不稳定的户外搬运一个重量未知、但绝不能掉落（贵重）的设备"设计一个**模型-学习融合**方案。(1) 哪部分用学习（提示：未知物体 + 通信不稳 → 接触力涌现协调）？(2) 哪部分用模型派守安全（提示：绝不能掉 → 某种硬约束/保证）？(3) 两部分如何衔接（学习策略输出 → 模型派安全过滤？还是模型派提供 backbone → 学习填残差？）？(4) 列出这个方案相比纯学习、纯模型各自的优势。

练习 8.7.3（开放研究题） 从本节五个开放问题中选一个，写一份半页的"研究设想"：(1) 清晰陈述问题（为什么它难、卡在哪）；(2) 提出一个你认为可行的研究思路（哪怕粗糙）；(3) 设计一个最小验证实验（在仿真里怎么验证你的思路有效）；(4) 预判最可能的失败模式。（这是博士选题级的练习——目的是训练你把"读到的方向"变成"能做的课题"。）

---

综合贯通：一个场景串起全章七节

这一节不引入新知识，而是把 §8.1–§8.7 的工具在**同一个具体任务**上从头到尾走一遍。教学上这叫"装配演示"——你已经认识了每个零件，现在看它们如何咬合成一台完整的机器。读完它，七节就从"七个孤立主题"变成了"一条端到端的流水线"。

任务：三台 Unitree Go2（各加一条 D1 轻臂）把一张约 \(12\) kg 的方桌，从仓库一端搬到 \(8\) m 外的另一端，途中要绕过一根立柱，桌面上放着一杯不能洒的水。可靠 WiFi 可用，桌子重量已大致已知。

我们按"先选型、再建模、后控制"的顺序，把每一步挂到对应的节。

第 0 步（§8.1 物理起点）：这是不是 loco-manipulation？操作末端在哪？ 是——机器人要边走（loco，走 8 m）边维持对桌子的操作（manipulation，托住不让水洒）。每台 Go2 的操作末端是 D1 臂的橡胶垫，不是腿也不是躯干（桌子要抬离地、不能推）。立即套用 §8.1 的核心抽象：这是一个 3 指 grasp，每个"手指"是一台会摔倒、会移动的 Go2。 三台都不能翻、都要协调移动——这就锁定了后面所有约束的来源。

第 1 步（§8.7 选型决策树）：选哪条技术线？ 过一遍决策树：Q1 需要可证明安全吗？——有杯水不能洒、但不载人不医疗，属"中等谨慎"，CBF 可选可不选；Q2 能通信吗？——WiFi 可靠，能；Q3 知道物体模型吗？——桌重已知，知道；Q4 规模？——\(N=3\)，小且固定。决策树走到"小规模 + 已知模型 + 可通信"→ **集中 MPC（模型派）**最合适（Kim–Hamed / De Vincenti 风格），又快又准。这印证了陷阱 8.7.1：这种任务用 decPLM 是杀鸡用牛刀。

第 2 步（§8.4 模式选择）：推、搬、还是抬？ 桌子要抬离地（过不了"推"，推不能离地）、桌子不能刚性焊接到机器人（过不了纯"搬"，没有机械接口）、但可以用臂垫从桌腿/桌沿捏住托起——抬（pinch-lift）模式。于是接触约束集取 §8.4 的 \(\mathcal{C}^{\text{pinch}}\)：单边 \(f_n\ge0\)、摩擦锥 \(\|f_t\|\le\mu f_n\)、且内力有正下界（托举需求）。

第 3 步（§8.2 建模）：写出互联 SRB 网络。 建 4 个刚体节点——3 台 Go2 的 SRB + 1 张桌子的刚体；建 3 条全息约束边（每台臂垫到桌子的接触点）。关键决策（陷阱 8.2.2）：保留 3 个 interaction wrench \(\lambda_{iL}\) 作 MPC 决策变量（别消元，保稀疏），约束微分到加速度层 \(\ddot\phi=0\) 加 Baumgarte 镇定（陷阱 8.2.3，防桌子慢慢从垫子间滑脱）。

第 4 步（§8.3 负载分配）：力怎么分，谁会翻？ 三台对称站在桌子三边，运动力大致均分桌重（每台托 \(\approx 39\) N 竖直 + 自重）。但这是捏举，必须叠加内力——用 §8.4 第三步的算法在线算挤压内力下界 \(f_n^{\min}=m_Lg/(2\mu\cdot\text{侧数})\) 保证不滑，又受 §8.3 的"不翻倒"上界约束（别把哪台 Go2 自己推翻）。这一步的 QP 可行性自动回答"这个分配三台都站得住吗"——若桌子再重些逼近临界质量，QP 会报不可行，提示减载或加第四台。

第 5 步（§8.5 协同步态）：边走边搬怎么不晃洒水？ 三台要走 8 m，必然迈步。用 §8.5 的相位协调给三台分配**均布相位**（\(0,120°,240°\)），让各机 trot 的质心起伏在桌子上相消（§8.5 算例情形二，理论净起伏为零，杯水才不洒）。同时频段分离：高层 MPC（60Hz）规划桌子绕过立柱的低频轨迹，低层 WBC（500Hz）吸收各机踏步高频。还要警惕拍频陷阱（§8.5 算例情形四）——三台步频得靠 WiFi 上的共享时钟同步（联系第 2 章共识），否则跑十几秒后相消会漂移成同相、水就洒了。

第 6 步（避立柱 → §8.4 CBF）：怎么保证不撞柱子？ 在 MPC 里给"每台机器人/桌子到立柱的距离"加一个高阶 CBF 约束 \(h\ge0\)，把"绕过立柱"从"希望它绕"升级为"数学保证它绕"。这就是安全攸关 NMPC 路线，代价是 NMPC 多几条非线性约束、求解略慢，但换来硬安全。

第 7 步（求解预算 → §8.2/§8.5）：跑得动吗？ 3 台 + 桌子 + 接触时序的 NLP，要在 \(1/60\approx16.7\) ms 内解完（练习 8.5.2）。靠的就是第 3 步保的稀疏性（保留 wrench 不消元）+ 摩擦锥线性化为金字塔（陷阱 8.3.3，用 QP 而非二阶锥）+ 上一时刻解热启动。这正是安全攸关 NMPC 报告的 \(11\) ms@60Hz 量级。

把七步连起来，就是一条完整的流水线：

\[ \underbrace{\text{§8.1 抽象}}_{\text{N 个会摔的手指}}\to\underbrace{\text{§8.7 选型}}_{\text{集中 MPC}}\to\underbrace{\text{§8.4 模式}}_{\text{抬}}\to\underbrace{\text{§8.2 建模}}_{\text{互联 SRB 网络}}\to\underbrace{\text{§8.3 分配}}_{\text{QP+不翻倒}}\to\underbrace{\text{§8.5 步态}}_{\text{相位相消+频段分离}}\to\underbrace{\text{§8.4 CBF}}_{\text{硬安全}} \]

本质洞察（全章贯通）：注意这条流水线里，每一步的输出都是下一步的输入，而每一步的约束最终都回到同一个根——"支撑与操作共享同一组接触力"。§8.1 的"会摔的手指"决定了 §8.3 必须加不翻倒约束；§8.4 的"抬"决定了 §8.2 的接触约束集和 §8.3 的正内力；§8.5 的迈步又让 §8.3 的分配变成时变。七节不是七个独立工具箱，而是**同一个力学内核（再耦合）在不同环节的展开**。如果把任务换成"10 台搬未知沙发过废墟无通信"，第 1 步选型就会拐向 decPLM（§8.6），后面整条流水线随之换成学习派——但驱动选择的那个内核问题（接触力既撑机体又操作物体）一模一样。这就是本章最想让你带走的东西：不是记住七个方法，而是看见它们都是同一个问题的不同切面。

对照：同一类任务换成学习派的流水线。 上面的承诺值得兑现——把任务改成"\(10\) 台 Go2+臂，把一张**不知道里面装什么**的沙发（约 \(30\) kg），在**无通信**的废墟里大致清到一侧，地面碎石起伏"，看流水线如何整条切换：

• §8.7 选型：Q2 不能通信 → 直接拐向学习派去中心（decPLM 路线），模型派的集中 MPC 在这里根本起不来（要物体模型、要中央通信、\(N{=}10\) 求解爆炸）。
• §8.4 模式：沙发可从两侧捏举 → 仍是抬模式，但接触约束集这次不写进 QP，而是靠 constellation reward 隐式诱导（§8.6 机制 1）。
• §8.2 vs §8.6 建模：不建互联 SRB 网络——物体模型未知。改用域随机化覆盖质量/接触参数的分布（decPLM 训练设置），让策略对"不知道沙发多重"鲁棒。
• §8.3 分配：没有显式 QP 分配。力分配从"参数共享 + 接触力隐式通信 + 接触帧命令几何一致"中涌现（§8.6 机制 2）——十台不商量，靠物体物理地把各机的力平均、协调。
• §8.5 步态：底层 locomotion 控制器预训练冻结，处理碎石地面；高层只发基座/臂命令。相位协调不显式算，靠接触力反馈隐式同步（天然抗拍频，§8.5 算例情形四的优势版）。
• 规模效应（§8.7）：这恰是学习派的甜区——\(10\) 台不仅不是负担，反而让误差按 \(1/\sqrt N\) 相消、冗余增加（§8.6 算例），跟踪比 \(2\) 台更稳。

两条流水线**入口不同（选型分叉）、内核相同（再耦合）、出口都是"十/三台协调地搬动一个物体"**。把它们并排放，你就看清了：模型派和学习派不是两套要分别死记的知识，而是同一个力学问题在"已知/未知、有/无通信、小/大规模"三个轴上的两种投影。

这两条流水线还有一个共同的隐含前提——所有机器人同构（同型号、同能力）。一旦放开它，让人形、四足、带臂机器人混在一起协同搬运，能力差异会让"力怎么公平又稳定地分"变成新难题——这正是第 9 章（100_人形四足臂协同控制.md）要接着展开的异构协同。本章的互联 SRB 网络、负载分配 QP、推/搬/抬框架在异构上仍然成立，但 §8.3 的能力权重 \(w_i\) 会从"锦上添花"变成"生死攸关"。你现在只需记住：本章建立的整套力学骨架是异构协同的地基，下一章是在这地基上盖更高的楼。

---

本章常见误解汇总

把全章七节的核心误区集中成一张表，便于回查。每条都对应正文某节的陷阱，遇到时可定位回去深读。

#	常见误解	正确理解	对应节
1	loco-manipulation 是"先走过去再操作"两个独立阶段	loco 和 manipulation 在同一时刻、通过同一组接触力耦合发生；分阶段建模丢掉了核心问题	§8.1
2	多足协同 = 多个固定臂协同，只看摩擦锥	多足 = 多臂 + 每台一个"不翻倒"约束 + 会移动的基座；施力受摩擦锥**与**平衡双重约束	§8.1, §8.3
3	腿当臂可随时抬腿	抬腿使支撑多边形从四边形退化为三角形，必须先把质心转移到新三角形安全区	§8.1
4	把被搬物体当"外部已知负载"	物体是系统内生节点（有状态、有动力学），须通过全息约束与机器人联立求解	§8.2
5	消去 interaction wrench 以减少变量	消元把稀疏系统变稠密、拖慢求解、可能病态；应保留 \(\lambda\) 作决策变量保持稀疏	§8.2
6	全息约束只在位置层写	须微分到加速度层 \(\ddot\phi=0\) 并加 Baumgarte 镇定，否则约束漂移、物体穿透/飘走	§8.2
7	负载分配只看物体侧力闭合	须把各机腿力拉进 QP，加"机器人力平衡+摩擦锥"，用可行性编码"不翻倒"	§8.3
8	用刚性约束建模非抓持的推/抬	推不能拉（\(f_n\ge0\)）、抬托举受摩擦限；用强约束近似弱接触会算出不存在的能力	§8.4
9	推也能精确控物体姿态	推是欠驱动、不可逆的；精确控姿要用刚性搬或捏举	§8.4
10	多机搬运各机独立跑步态	各机起伏经物体叠加把物体拧晃；须相位反相/均布相消、频段分离	§8.5
11	物体轨迹和踏步起伏放同一控制层	须双层——高层 MPC 管低频物体轨迹，低层 WBC 管高频踏步平衡	§8.5
12	"零通信"等于"零协调"	协调藏在参数共享、接触力隐式通信、接触帧命令几何一致性三个隐式渠道	§8.6
13	用单点位置误差做姿态对齐	单点只约束 3 个平移自由度；须用 constellation（\(\ge 3\) 非共线点）同时编码位置+姿态	§8.6
14	用拼接全体状态的中心化策略	把 \(N\) 焊进结构、无法扩展；须参数共享+局部观测+排列不变	§8.6
15	盲目选最先进/最 fancy 的方法	方法的优劣相对任务约束而言；用决策树选最匹配约束的方法	§8.7

---

本章小结

本章把"多机协同"推进到最纠缠的一种形态——施力的手就是站立的脚。一句话概括全章：多足协同 loco-manipulation = 第 5/7 章的力分配 + 每台机器人一个"不翻倒"约束 + 边走边操作的步态协调，再叠加模型派（精确/安全）与学习派（可扩展/无模型）两条技术路线。

七节的逻辑链：

1. **§8.1 腿即臂**建立物理起点——"\(N\) 个会摔倒的手指共持一个物体"，量化腾腿的支撑多边形代价，给出推压/腾腿/加臂选型。
2. **§8.2 互联 SRB 网络**搭起模型派引擎——\(N\) 个 SRB + 物体 + 全息约束 + 保留 interaction wrench 作决策变量（保稀疏）。
3. **§8.3 多足负载分配**把第 5/7 章的 grasp 分解搬到腿上，加"不翻倒"约束（摩擦锥 ⊗ 平衡），用 QP 可行性编码稳定。
4. **§8.4 推/搬/抬**用一套 wrench 平衡框架统一三模式，差异全在接触约束集强弱——推最鲁棒、抬最精确、搬居中。
5. **§8.5 协同步态**把静态搬升级为边走边搬——相位相消、频段分离、双层 MPC+WBC、whole-body 统一腿与臂。
6. **§8.6 学习化涌现协调**讲透学习派——constellation reward 逼出刚性接触、接触力作隐式通信、排列不变实现零样本扩展。
7. **§8.7 全景与选型**把方法收进能力矩阵，给出选型决策树和模型-学习融合趋势。

本质洞察（全章收束）：贯穿全章的一条主线是**"支撑与操作的再耦合"**——固定臂操作把支撑和操作解耦（基座管支撑、臂管操作），腿足把它们重新焊回同一组接触力。本章所有看起来比第 5/7 章复杂的地方（不翻倒约束、协同步态、内力折中、接触力隐式通信），根源都在这个再耦合。理解了它，你就抓住了多足协同 loco-manipulation 区别于一切其他协同操作的本质。

一页纸速记：把全章压成一句话 + 一个公式 + 三组对立

如果只能带走三样东西，带走下面这三样——它们是全章的"压缩包"，遇到具体问题时先在脑中解压它，再去翻对应节的细节。

一句话：多足协同 loco-manipulation = "\(N\) 个会摔倒、会移动的手指共持一个物体"，难点全部来自**支撑与操作共享同一组接触力**。

一个公式（万变不离其宗的物体侧力学）：

\[ F_{\text{obj}}^{\text{des}}=G\,\boldsymbol\lambda,\qquad \boldsymbol\lambda=\underbrace{G^{+}F_{\text{obj}}^{\text{des}}}_{\text{运动力}}+\underbrace{(I-G^{+}G)\boldsymbol\lambda_{\text{int}}}_{\text{内力}},\quad \text{s.t. 每台机器人 }\lambda_{iL}\in\mathcal{C}_i\cap\{\text{不翻倒}\} \]

记住它的读法：物体要的合 wrench 由各机分摊（grasp matrix \(G\)），分摊有"运动力 + 内力"两部分自由度，而每台能出的力被"接触约束集 \(\mathcal{C}_i\)（推/搬/抬不同）"和"不翻倒"双重夹住。全章七节都在给这一个式子的不同部分填细节——\(G\) 来自站位（§8.3）、\(\mathcal{C}_i\) 来自模式（§8.4）、不翻倒来自平衡（§8.1/8.3）、\(\boldsymbol\lambda\) 随步态时变（§8.5）、模型派显式解它/学习派隐式逼近它（§8.2 vs §8.6）。

三组对立（选型和理解全靠它们）：

对立	一端	另一端	在哪节
接触强度	推（弱、鲁棒、难控姿）	搬（强、精确、需连接）	§8.4
协调方式	显式（通信/中央 MPC、可证安全）	隐式（接触力/参数共享、可扩展）	§8.2 vs §8.6
规模效应	模型派"多即是难"（\(O(N^3)\)）	学习派"多即是稳"（误差 \(1/\sqrt N\)）	§8.7
运动状态	站着搬（静态、四脚支撑、裕度大）	边走边搬（时变、迈步、要相位相消+频段分离）	§8.5

这四组对立两两不独立——选了"边走边搬"（运动状态）就逼出"分配时变"（§8.3 嵌进滚动 MPC），选了"隐式协调"（协调方式）往往伴随"大规模甜区"（规模效应）。它们是同一个再耦合内核在四个轴上的投影，记住轴比记住点更省脑子。

本质洞察（记忆锚点）：这"一句话 + 一公式 + 三对立"不是知识点的罗列，而是一张**导航图**——任何多足协同问题进来，先问"这是哪种接触强度？要显式还是隐式协调？规模多大？"，三个问题答完，§8.7 的选型树基本就走通了，再回到那一个公式去填具体的 \(G\)、\(\mathcal{C}_i\) 和不翻倒约束。把这页纸刻进脑子，全章 2000 行就有了一个随时能展开的索引。

术语速查表

术语	英文	一句话定义
腿即臂	legs as limbs / loco-manipulation without arms	用腿/躯干本身作操作末端，不加专用机械臂
支撑多边形	support polygon	触地足端水平投影的凸包；质心投影须落在其内（静态稳定）
单刚体模型	Single Rigid Body (SRB)	把腿足机器人简化为带转动惯量的刚性质量块，忽略腿质量
互联 SRB 网络	interconnected SRB network	\(N\) 个机器人 SRB + 物体刚体 + 全息约束耦合的联合动力学
全息约束	holonomic constraint	只含位置/姿态的代数约束 \(\phi=0\)，把机器人附着点焊到物体
相互作用 wrench	interaction wrench \(\lambda\)	机器人与物体间的接触力+力矩，模型派保留它作决策变量
grasp matrix	grasp matrix \(G\)	把各接触/附着点的力映射到物体合 wrench：\(F_{\text{obj}}=G\boldsymbol\lambda\)
内力	internal force	落在 \(\ker G\) 内、不产生物体净运动、各接触点互相挤压的力
运动力	motion force	\(G^+F_{\text{obj}}\)，驱动物体运动的接触力分量
摩擦锥	friction cone	\(\|f_t\|\le\mu f_n\)，脚/接触不打滑的约束；常线性化为金字塔
单边接触	unilateral contact	\(f_n\ge0\)，接触只能推不能拉
协同推	collaborative pushing	非抓持、靠法向+摩擦接触的多机推物体
捏举	pinch-lift	非抓持、靠两侧挤压摩擦力托起无法抓持的物体
协同步态	cooperative gait	多机步态相位协调（反相/均布）使质心起伏在物体上相消
频段分离	frequency separation	物体操作（低频）与踏步起伏（高频）在频域分开，分层处理
whole-body control	whole-body control (WBC)	把腿/臂/躯干统一为接触分支、按优先级塞进分层 QP
星座奖励	constellation reward	用一组点的距离平方和同时编码位置+姿态，逼出刚性接触行为
接触力隐式通信	implicit communication via contact	物体作为物理媒介传递机间动作信息，实现零显式通信协调
排列不变	permutation invariant	策略对 agent 数量和顺序不敏感，支持零样本扩展 agent 数
控制屏障函数	Control Barrier Function (CBF)	通过约束安全函数变化率保证前向不变性，提供硬安全保证
CTDE	centralized training, decentralized execution	训练时 critic 用全局信息（稳定），执行时 actor 只用本地（可扩展）

知识点总表

编号	知识点	核心要点	对应节	难度
K1	腿即臂视角	腿小工作空间 × 躯干大移动性 = 操作能力；多足=会摔倒的手指	§8.1	⭐⭐
K2	支撑多边形与腾腿代价	四脚→三脚退化，须先转移质心	§8.1	⭐⭐
K3	推压/腾腿/加臂选型	要力→推压、要够到→腾腿、要精确/抬举→加臂	§8.1	⭐⭐
K4	互联 SRB 网络	\(N\) SRB+物体+全息约束；物体是内生节点	§8.2	⭐⭐⭐⭐
K5	牛顿第三定律耦合	\(\lambda\) 在机器人侧+、物体侧−，锁死两者运动	§8.2	⭐⭐⭐
K6	全息约束微分到加速度层	\(\ddot\phi=0\) + Baumgarte 镇定，防漂移	§8.2	⭐⭐⭐
K7	保留 interaction wrench	保稀疏、\(\lambda\) 本就是要的量、数值好	§8.2	⭐⭐⭐⭐
K8	多足负载分配 QP	摩擦锥 ⊗ 不翻倒；腿力进 QP，可行性编码稳定	§8.3	⭐⭐⭐⭐
K9	多足内力折中	夹紧（防松脱）vs 留平衡余量（防翻），固定臂无此折中	§8.3	⭐⭐⭐
K10	推/搬/抬接触约束	同一摩擦锥框架，约束集强弱不同	§8.4	⭐⭐⭐
K11	三模式权衡	推鲁棒难控姿、抬精确难维持、搬全面需可连接	§8.4	⭐⭐⭐
K12	CBF 安全保证	高阶 CBF 保证碰撞-free 集前向不变	§8.4	⭐⭐⭐
K13	协同步态相位相消	反相(\(N=2\))/均布(\(N\ge3\))使起伏在物体上相消	§8.5	⭐⭐⭐⭐
K14	频段分离 + 双层	高层 MPC(60Hz)管物体低频，低层 WBC(500Hz)管踏步高频	§8.5	⭐⭐⭐⭐
K15	whole-body 统一	腿/臂/躯干都是接触分支，分层 QP 一视同仁	§8.5	⭐⭐⭐
K16	constellation reward	点集距离同时编码位置+姿态，逼出刚性接触	§8.6	⭐⭐⭐⭐
K17	接触力隐式通信	参数共享+接触力+接触帧命令三渠道实现零通信协调	§8.6	⭐⭐⭐⭐
K18	排列不变零样本扩展	参数共享+局部观测+排列不变，\(N{=}2\) 训练→\(N{=}10\) 部署	§8.6	⭐⭐⭐⭐
K19	接触力 sim-to-real	多足协同全靠接触力，而接触仿真最不准——学习派最大障碍	§8.6	⭐⭐⭐
K20	模型 vs 学习选型	安全→CBF、零通信→decPLM、未知模型→域随机、规模→分布式	§8.7	⭐⭐⭐

---

累积项目：本章新增模块

多机协作 part 累积项目（Mini-MultiBot，见第 13 章 140_Mini-MultiBot综合实战.md）：本 part 每章为这个综合项目添加一个模块。本章新增"多足协同搬运"模块。

本章新增模块：coop_loco_manip/

coop_loco_manip/
├── interconnected_srb.py      # §8.2 互联 SRB 网络组装（刚体节点+全息边）
├── load_allocation_qp.py      # §8.3 多足负载分配 QP（摩擦锥+不翻倒）
├── contact_modes.py           # §8.4 推/搬/抬三模式接触约束集
├── cooperative_gait.py        # §8.5 协同步态相位协调 + 频段分离
├── constellation_reward.py    # §8.6 星座奖励（点集对齐）
└── mode_selector.py           # §8.7 选型决策树（任务约束→方法）

集成方式：把本章模块接到第 4 章（分布式 MPC）和第 13 章（Mini-MultiBot）的调度层——当 Mini-MultiBot 的任务分配（第 3 章）派出"\(k\) 台四足搬一个物体"的任务时，由 mode_selector 选模式与方法，load_allocation_qp 算分配，cooperative_gait 协调步态，下发给各机的单体足式控制器（复用 05_运动控制/10_足式 的 MPC+WBC）。

本章里程碑（可运行的最小成果）：

1. 静态分配验证：用 load_allocation_qp 让两台仿真四足对称抬一张桌子，验证均分；调能力权重观察倾斜；增重找不可行临界质量（练习 8.3.1）。
2. 相位相消验证：用 cooperative_gait 给 2/4 台机器人分配反相/均布相位，验证净质心起伏趋零（练习 8.5.1）。
3. constellation 姿态约束验证：用 constellation_reward 验证点集距离对纯旋转敏感、单点位置误差不敏感（练习 8.6.1）。
4. 选型端到端：给 mode_selector 喂五个练习 8.7.1 的任务，输出选型并打印决策路径。

里程碑验收标准（量化的"做到什么程度算过"）：

里程碑	验收判据（可量化）	对应节/练习
1 静态分配	对称布局两机竖直力误差 <1%；能复现"强者放短力臂端多承重"的反直觉；能报告不可行临界质量 \(m_L^*\)	§8.3 算例、练习 8.3.1
2 相位相消	\(N{=}2\) 反相净起伏数值 <1e-3·\(A\)；\(N{=}3\) 均布同样相消；能演示步频差 2% 时的拍频漂移	§8.5 算例、练习 8.5.1
3 constellation	纯旋转 \(5°\) 时 \(d_{\text{con}}\) 明显增大、单点位置误差 \(\approx 0\)；说明为何用 3 非共线点	§8.6 机制1、练习 8.6.1
4 选型端到端	五个任务的选型与决策路径与 §8.7 决策树一致；能解释每个分支的硬约束	§8.7 决策树、练习 8.7.1

理论-工程桥接：把"里程碑"配上"可量化验收判据"，是工程复现区别于"跑通了就行"的关键——没有数值判据，你无法判断"看起来对"是真对还是巧合。这四条判据分别卡住了全章四个最易出错的点（力臂反直觉、拍频、姿态编码、选型一致性），通过它们基本能确认你对 §8.3/§8.5/§8.6/§8.7 的核心理解落到了可运行的代码上。

---

延伸阅读

分类标注难度：⭐⭐ 入门 / ⭐⭐⭐ 进阶 / ⭐⭐⭐⭐ 研究级。

模型派核心论文

• ⭐⭐⭐⭐ Kim, Fawcett, Kamidi, Ames, Akbari Hamed, "Layered Control for Cooperative Locomotion of Two Quadrupedal Robots: Centralized and Distributed Approaches," T-RO 2023（arXiv:2211.06913）——本章模型派标杆，双 A1 全息约束 + 分层 SRB-MPC + 分布式 QP。必读。
• ⭐⭐⭐⭐ De Vincenti, Coros, "Centralized Model Predictive Control for Collaborative Loco-Manipulation," RSS 2023（crl.ethz.ch/papers/CMPC.pdf）——robot/payload/gait-agnostic SQP，一套代码跑任意多足+任意负载+任意步态。必读。
• ⭐⭐⭐⭐ "Safety-Critical Centralized Nonlinear MPC for Cooperative Payload Transportation by Two Quadrupedal Robots," 2025（arXiv:2604.03200）——三 SRB + 全息约束 + 高阶 CBF，11ms@60Hz，载重 74.67%。安全攸关路线必读。
• ⭐⭐⭐ Fawcett, Hamed 等, "Distributed Feedback Controllers for Stable Cooperative Locomotion of Quadrupedal Robots: A Virtual Constraint Approach," ACC 2019（arXiv:1902.03690）——奠基性工作，虚拟约束 + 全息约束建模两机协同。
• ⭐⭐⭐ "ACLM: ADMM-Based Distributed Model Predictive Control for Collaborative Loco-Manipulation," 2026（admm-clm.github.io）——用 ADMM 分布式化模型派 MPC，可扩展。

学习派核心论文

• ⭐⭐⭐⭐ Pandit, Shrestha, Fern 等, "Multi-Quadruped Cooperative Object Transport: Learning Decentralized Pinch-Lift-Move (decPLM)," 2025（decplm.github.io，arXiv:2509.14342）——本章学习派标杆，零通信 + constellation reward + 2→10 零样本扩展。必读。
• ⭐⭐⭐ Feng 等, "Learning Multi-Agent Loco-Manipulation for Long-Horizon Quadrupedal Pushing (MA-Push)," 2024（collaborative-mapush.github.io，arXiv:2411.07104）——分层 MARL 推大物体，高层 RRT+集中策略给子目标。
• ⭐⭐⭐ An, Lee, Bjelonic, De Vincenti, Hutter, "Solving Multi-Entity Robotic Problems Using Permutation Invariant Neural Networks," RSS 2024（arXiv:2402.18345）——排列不变网络，可扩展多机协同。
• ⭐⭐⭐ "Shared Object Manipulation with a Team of Collaborative Quadrupeds," 2025（arXiv:2510.00682）——混合运动-力控的多足共操作。

前置/交叉教材与本项目章节

• ⭐⭐⭐ Murray, Li, Sastry, A Mathematical Introduction to Robotic Manipulation, Ch.5（grasp matrix、内力、力闭合的经典推导）——第 5/7 章的源头。
• ⭐⭐ 本项目 05_运动控制/10_足式/70_腿足简化模型理论.md（SRB 模型）、80_接触力学与约束优化.md（摩擦锥）、90_WBC分层优化与TSID.md（WBC）、120_步态管理与接触序列.md（步态）——本章的单体足式前置。
• ⭐⭐ 本项目 05_运动控制/30_复合/280_多机协作LocoMani.md（从复合机器人视角的多机协作 loco-manipulation，任务分解/延迟鲁棒）——本章的姊妹篇，互补视角。

---

本章与后续章节的关系

后续章节	关系	本章铺垫的知识点
第 9 章 人形四足臂协同控制（100_人形四足臂协同控制.md）	直接延伸	本章是同构多足协同，第 9 章推广到**异构形态**（人形+四足+带臂）协同；本章的互联 SRB 网络、负载分配、推/搬/抬在异构上仍适用，但能力差异带来新的分配难题
第 10 章 MARL 基础（110_MARL基础.md）	理论补全	本章 §8.6 用到的 MAPPO、参数共享、CTDE、排列不变在第 10 章系统化；本章是 MARL 在多足搬运上的落地预演
第 13 章 MARL 与传统规控混合（130_MARL与传统规控混合.md）	方法融合	本章 §8.7 的"模型-学习融合"在第 13 章深入；CBF 守安全 + 学习填未知的范式
第 13 章 Mini-MultiBot 综合实战（140_Mini-MultiBot综合实战.md）	项目集成	本章的 coop_loco_manip/ 模块接入 Mini-MultiBot 调度层，与第 3 章任务分配、第 4 章分布式 MPC 协同
05_运动控制/10_足式（单体足式）	依赖底座	本章每台机器人的底层 MPC+WBC 直接复用单体足式控制器；本章在其上加多机耦合
05_运动控制/30_复合/280_多机协作LocoMani.md	姊妹篇	互补视角——本章重力学（互联 SRB/负载分配），复合章重任务（任务分解/局部自治）

---

🔧 故障排查手册

多足协同 loco-manipulation 实现中最常见的 7 类故障。每个给出"症状 → 可能原因 → 排查步骤 → 相关章节"。前 5 类是核心故障（力分配、sim-to-real、步态晃动、内力对抗、求解），后 2 类是动态/异构场景的进阶故障（拍频、能力瓶颈）。

故障 1：力分配在物体侧正确，但某台机器人搬着搬着就摔了

项	内容
症状	负载分配 QP 求解成功、物体合力合力矩验证无误，下发后某台机器人质心被拉出支撑多边形而摔倒
可能原因	(1) 分配 QP 只含物体侧力闭合，缺机器人侧平衡约束（最常见）；(2) 平衡约束写了但摩擦系数 \(\mu\) 设太乐观（真机更滑）；(3) 用了四脚支撑分配但机器人实际在迈步（摆动腿不出力）
排查步骤	① 检查 QP 约束里有没有"机器人 \(i\) 力/力矩平衡 + 摩擦锥"——没有就是陷阱 8.3.1；② 把 \(\mu\) 调保守（如 0.4→0.3）重试；③ 检查分配是否随步态相位更新（陷阱 8.5.3）；④ 单独验证摔倒那台的 GRF 子问题是否可行
相关章节	§8.1（不翻倒）、§8.3（负载分配 QP）、§8.5（时变分配）

故障 2：仿真里完美，真机上物体脱离/滑落

项	内容
症状	仿真训练/规划的策略在真机上，物体从机器人接触处脱离（推）或从垫子间滑落（捏举）
可能原因	(1) 用刚性约束建模了非抓持接触，仿真允许了不存在的拉力（陷阱 8.4.1）；(2) 捏举内力设定值不随负载自适应（陷阱 8.4.3）；(3) 接触力 sim-to-real gap（接触刚度/摩擦仿真不准）
排查步骤	① 确认接触模型与实际匹配——非抓持必须 \(f_n\ge0\)、无力矩；② 检查捏举内力是否按 \(\sum\mu f_n\ge m_L g\) 在线算并留裕度；③ 增大域随机化（接触参数、摩擦、质量）重训；④ 真机加接触力传感校准
相关章节	§8.4（接触模式）、§8.6（sim-to-real）

故障 3：边走边搬时物体剧烈晃动，载荷飞出

项	内容
症状	静态搬运稳定，但一旦机器人开始迈步行走，物体剧烈俯仰/滚转，桌上的东西飞出去
可能原因	(1) 各机步态相位未协调，质心起伏同相叠加（最常见，陷阱 8.5.1）；(2) 物体轨迹和踏步放同一控制层、频率不匹配（陷阱 8.5.2）；(3) 步频不一致产生拍频（练习 8.5.1）
排查步骤	① 检查各机步态相位——同相就改反相/均布；② 测物体起伏的主频，若接近步频（2-4Hz）则是步态扰动注入；③ 确认是否双层架构、高低层频率是否分离；④ 检查各机步频是否同步（联系第 2 章共识）
相关章节	§8.5（协同步态、频段分离）

故障 4：去中心化多机互相对抗，物体不动且空耗能量

项	内容
症状	多台机器人接触物体，但物体几乎不动，各机电流/力矩很大（在使劲），明显在互相对抗
可能原因	(1) 天真奖励"各自把物体推向目标"退化成内力对抗（陷阱 8.6.1）；(2) 没有隐式协调渠道（无参数共享/无 constellation/无接触帧命令分解）；(3) 内力期望未设或设反
排查步骤	① 检查奖励是否含"维持与物体相对位姿"（constellation）而非只有"物体到目标距离"；② 确认是否参数共享（同一策略）；③ 把团队目标几何分解成各机自洽的接触帧命令；④ 监控内力大小——异常大说明在对抗
相关章节	§8.6（涌现协调机制）、§8.1（内力对抗场景）

故障 5：互联 SRB 网络 MPC 求解慢/不收敛/物体穿透

项	内容
症状	(a) MPC 单步求解时间远超预算（如 >50ms）无法实时；或 (b) SQP 不收敛；或 (c) 物体随时间缓慢穿透进机器人躯干
可能原因	(a) 消去了 interaction wrench 导致系统稠密化（陷阱 8.2.2）；(b) 摩擦锥用二阶锥而非线性化（陷阱 8.3.3），或初值差；(c) 全息约束只在位置层、无加速度层镇定（陷阱 8.2.3）
排查步骤	① 检查是否保留 \(\lambda\) 作决策变量、系统矩阵是否稀疏——稠密就是消元的锅；② 把摩擦锥线性化为金字塔、用 QP 求解器；③ 给 SQP 好的热启动（上一步解/学习策略初值）；④ 约束改加速度层 \(\ddot\phi=0\) + Baumgarte（\(\ddot\phi+2\alpha\dot\phi+\beta^2\phi=0\)）
相关章节	§8.2（保稀疏、约束镇定）、§8.3（摩擦锥线性化）、§8.5（求解预算）

故障 6：边走边搬初期平稳，每隔十几秒物体周期性剧烈晃一阵

项	内容
症状	多机搬运起步后物体很稳（相位相消生效），但持续行走十几秒到几十秒后，物体开始周期性地（缓慢节律，如每 \(15\)–\(30\) s 一次）剧烈起伏一阵，随后又平稳，循环往复
可能原因	各机步频不完全一致（哪怕只差 \(1\)–\(2\%\)），固定的初始相位差随时间漂移——拍频效应（§8.5 算例情形四）；相消被开环设定一次后无人维护
排查步骤	① 测各机实际步频，看是否有微小差异；② 观察晃动节律周期，若约等于 $1/
相关章节	§8.5（相位相消、拍频陷阱）、第 2 章（步频共识同步）

故障 7：异构/不对称团队中，某一台总是先到能力极限（先摔或先饱和）

项	内容
症状	多台能力不同（或站位不对称）的机器人协同搬运，物体侧合力合力矩都对，但总是固定的某一台先达到力矩/电流上限、先逼近平衡边界，成为整个团队的瓶颈
可能原因	(1) 负载分配未按各机能力加权（\(w_i\) 没设或设反），强弱一视同仁；(2) 站位几何不利——把弱者放在了离物体质心近的短力臂端，反而要它出更大的承重力（§8.3 算例第三步的反直觉点）；(3) 内力期望统一设值，未考虑各机平衡余量差异
排查步骤	① 检查 QP 的能力权重 \(w_i\) 是否按真实负重/力矩上限设置（弱者权重大→少分力）；② 重新规划站位——让强者靠近物体质心（短力臂、多承重），弱者远离（长力臂、少承重）；③ 监控各机的 GRF 余量（到摩擦锥/平衡边界的距离），让分配均衡各机余量而非均分力；④ 若仍有瓶颈，考虑加机器人或换异构协同方法（接第 9 章）
相关章节	§8.3（能力加权分配、力臂与承重的反直觉关系）、§8.1（不翻倒约束）、第 9 章（异构协同）

---

API 速查表

本章涉及的核心接口签名与说明。Python 部分为本章自实现的教学框架，工程库部分为生产环境常用。

Python 教学 API（本章 coop_loco_manip/）

# §8.2 互联 SRB 网络
class RigidBodyNode(name, mass, inertia, n_legs=0)
    # 刚体节点：机器人(n_legs>0)或物体(n_legs=0)，统一抽象
class HolonomicEdge(robot, load, attach_robot, attach_load, dof=6)
    # 全息约束边；dof: 6=刚性, 5=放开yaw, 3=球铰
assemble_network(nodes, edges) -> dict
    # 组装网络，返回决策变量/约束计数（含 interaction wrench 维度）

# §8.3 多足负载分配
multilegged_load_allocation(G, F_obj_des, robots, mu=0.6)
    -> (lambda_per_robot, leg_forces_per_robot)
    # 联合优化各机 wrench 与腿力；QP 可行性=每台站得住；不可行抛异常

# §8.5 协同步态
cooperative_gait_phases(N, base_phase=0.0) -> list[float]
    # 为 N 台机器人分配均布相位，使质心起伏在物体上相消
residual_heave_disturbance(phases, A, f, t) -> float
    # 计算给定相位下注入物体的净竖直起伏扰动

# §8.6 星座奖励
constellation_distance(P, P_target) -> float
    # d_con = (1/K) Σ||p_k - p_k*||²，同时编码位置+姿态

工程库 API（生产环境）

// OCS2（多足 MPC，见 05_运动控制/10_足式/110）—— 可扩展到互联 SRB 网络
ocs2::SystemDynamicsBase            // 定义互联 SRB 动力学（多刚体+约束）
ocs2::ConstraintBase                // 全息约束 + 摩擦锥（加速度层）
ocs2::SqpSolver                     // 稀疏 SQP（保 interaction wrench 稀疏性）

// Crocoddyl（DDP/FDDP，De Vincenti 风格 Lie 群积分）
crocoddyl::StateMultibody           // 浮动基状态，含 SE(3)/四元数积分
crocoddyl::ContactModelMultiple     // 多接触（腿+附着点统一）
crocoddyl::CostModelSum             // 物体跟踪 + 力正则 + 内力期望

// 摩擦锥（两库通用）
FrictionConeConstraint(mu, n_dir=4) // 线性化金字塔，QP 友好

// IsaacLab + RSL-RL（学习派 decPLM 风格）
ManagerBasedRLEnv                   // 多 agent 环境，参数共享策略
MAPPO / multi-agent PPO             // CTDE：非对称 critic + 分散 actor

接口	关键参数	易错点
HolonomicEdge(dof=)	dof=6 刚性 / 5 放开yaw	用错维数→过约束（卡死）或欠约束（飘）
multilegged_load_allocation	robots[i].w 能力权重	漏平衡约束→分配出会摔的力（陷阱 8.3.1）
FrictionConeConstraint	mu, n_dir	用二阶锥→QP 求解器报错/变慢（陷阱 8.3.3）
constellation_distance	点集 \(\ge 3\) 非共线	用单点→不约束姿态（陷阱 8.6.2）
MAPPO critic	训练用全局/执行用本地	执行时误用全局观测→无法分散部署

---

研究实践建议

分层次给出，从"会用"到"做研究"。

入门（先会跑通）：

1. 先把单体足式（05_运动控制/10_足式）的 MPC+WBC 跑通——多足协同的每台机器人都是一个单体控制器，单体不会就别谈多机。
2. 在 MQE（ziyanx02/multiagent-quadruped-environment，IsaacGym 多四足环境，~200★）里跑通双 Go2 协同平移，对比有/无接触力观测的成功率（呼应骨架练习 A 型）。
3. 实现本章的 load_allocation_qp 和 cooperative_gait，做完累积项目里程碑 1-2。

进阶（理解深处）：

1. 精读 De Vincenti–Coros RSS 2023 的 Section III-IV，画出"SQP 优化变量→约束→求解器"的数据流（呼应骨架练习 B 型）。亲手推一遍互联 SRB 网络的全息约束微分（练习 8.2.1）。
2. 复现 decPLM 的 constellation reward（练习 8.6.1），理解它为什么对协调"至关重要"。
3. 做模型派 vs 学习派的对比实验：同一个双机搬运任务，分别用集中 MPC（建模型）和 MARL（端到端），对比建模成本、求解时间、鲁棒性、sim-to-real。

研究级（做新东西）：

1. 攻接触力 sim-to-real（最值得做）：设计接触力的域随机化/系统辨识/在线适应方案，量化它对多足协同成功率的提升。这是学习派落地的最大障碍。
2. 模型-学习融合：用 CBF 给学习策略加安全证书，或用学习填模型派的残差动力学（练习 8.7.2、8.7.3）。这是 2026 前沿。
3. 异构多足协同（接第 9 章）：研究能力差异大的 agent（人形+四足）如何公平稳定地分担负载。
4. 大规模涌现协调的理论：decPLM 实验上 2→10 可扩展，但缺理论——什么条件下涌现协调成立、何时崩。

复现清单（动手前对照，少走弯路）：

无论走模型派还是学习派，下面这张清单能帮你在动手前理清"先验证什么、再验证什么"，避免一上来就跳进最难的接触 sim-to-real。它把全章的关键检查点按"由易到难、由静到动、由仿真到真机"排成一条复现路径。

阶段	要复现/验证什么	成功判据	最易踩的坑（回查）
0. 单体打底	单台四足 MPC+WBC 站立/行走	单台能稳定 trot、推墙不摔	单体不过关别谈多机（§8.1）
1. 静态分配	两机对称抬一物，QP 分配	均分；调站位看倾斜；增重找临界质量	漏不翻倒约束（陷阱 8.3.1）、强者放错力臂端（§8.3 算例）
2. 接触模式	推/抬两模式各跑一次	推不出现拉力；抬算出正内力下界	一律刚性建模（陷阱 8.4.1）、内力设定值（陷阱 8.4.3）
3. 互联建模	组装 N 机+物体网络 MPC	系统矩阵稀疏、求解 <预算	消元致稠密（陷阱 8.2.2）、约束不到加速度层（陷阱 8.2.3）
4. 静态搬运	多机站着协同托住物体	物体静止、各机不摔	内力对抗（§8.1 场景三）、摩擦锥用二阶锥（陷阱 8.3.3）
5. 动态搬运	多机边走边搬（相位协调）	物体起伏趋零、长时不漂	相位不协调（陷阱 8.5.1）、拍频（故障 6）
6. 仿真泛化	域随机化 + 分布外物体/数量	换物体/换 N 仍稳	中心化策略锁死 N（陷阱 8.6.3）
7. 真机迁移	真机轻载验证	接触不脱、可重复	接触力 sim-to-real gap（§8.6，故障 2）

理论-工程桥接：这张清单的顺序本身就是一条教学法——每一阶段都把上一阶段当作已通过的黑箱。第 5 阶段的动态搬运假设第 1 阶段的分配、第 3 阶段的建模都对；如果第 5 阶段出问题，先回退确认前面阶段仍成立，而不是在最复杂的动态场景里大海捞针。这正是 §8.5 故障排查"先静态后动态、先单台后多台"分而治之思想的复现版。大多数项目卡在第 7 阶段（真机接触），这也是本章反复强调"接触力 sim-to-real 是最大障碍"的原因。

本质洞察（研究方向）：这个方向最大的机会，不在"再做一个更大规模的 demo"，而在**把模型派的安全保证和学习派的涌现协调真正缝合起来**——让系统既能像 CBF-NMPC 那样"绝不出事"，又能像 decPLM 那样"零通信、不知模型也能协调"。谁先把这两者优雅地统一，谁就定义了这个方向的下一个标杆。

---

版本信息速查

项目	版本/配置	说明
本章文档版本	v1.0（2026-06）	算法工程教学；多足协同 loco-manipulation
Kim–Hamed 平台	2× Unitree A1	T-RO 2023，刚性杆连接 + 变地形
安全攸关 NMPC 平台	2× Unitree Go2（各 ~15kg）	11ms@60Hz，596 决策变量，载重 74.67%
decPLM 平台	Unitree Go2 + D1 臂	IsaacLab + RSL-RL，2048 环境，MAPPO
decPLM 训练规模	N=2 训练 → 2-10 部署	域随机化物体质量 [0.1,2]kg（主实验）
推荐仿真	IsaacLab / IsaacGym（学习）	MQE 多四足环境支持 2-4 Go2
推荐 MPC 求解	OCS2 / Crocoddyl（模型）	稀疏 SQP，保 interaction wrench 稀疏性
推荐 QP 求解器	OSQP / qpOASES	负载分配 QP；摩擦锥须线性化
控制频率（典型）	高层 MPC 60Hz / 低层 WBC 500Hz	频段分离，比例约 1:8

---

结语：多足协同 loco-manipulation 是一个 2023 年才真正起步、2025-2026 正在爆发的方向。它把本项目"多机协作"part 的所有工具——共识、分布式优化、任务分配、grasp matrix、内力、分布式 MPC——和"运动控制"part 的足式工具——SRB、MPC、WBC、步态——在"多足搬一个物体"这一个场景上全部焊死在一起，再叠加一条 MARL 的学习化路线。本章给了你这个交叉点的完整力学骨架和两条技术主线的全景。但记住：这个方向最深刻的问题——支撑与操作的再耦合、接触力作为协调媒介、模型保证与学习涌现的统一——大多还是开放的。你读完本章，不是到了终点，而是站到了一个正在快速生长的前沿的起跑线上。