第 4 章　分布式 MPC 与多足编队协同运动¶

本章性质：❌ 纯多足协同——本章把前三章的"离散决策层"工具（图、共识、ADMM、任务分配）下沉到**连续控制层**，回答一个全新的问题：当一群带腿的机器人（quadruped）必须**同时**保持队形、协调质心、各自维持接触平衡，又不靠中央计算机统一指挥时，它们如何只靠彼此通信，在每个控制周期（毫秒级）内联合求解一个最优控制问题？答案是**分布式模型预测控制（Distributed Model Predictive Control, DMPC）。后续第 5 章的协同搬运（内力分配）、第 9 章的多足 Loco-Manipulation、第 13 章 Mini-MultiBot 的综合调度，都建立在本章的 SRB 网络模型与 ADMM 分解之上。 **读者画像：已读完第 1 章（掌握通信图 $G$、Laplacian $L=D-A$、代数连通性 $\lambda_2$、集中/分布/去中心三大架构）、第 2 章（掌握共识 $\dot x=-Lx$ 的收敛证明、双随机权重、临界时延 $\tau^\star=\pi/(2\lambda_N)$、ADMM 三步迭代与罚参数 $\rho$、编队三范式）、第 3 章（任务分配与 MAPF——知道"谁去做、走哪条路"在运动之前已经定好）的算法工程师；同时**必须熟悉单体四足 MPC**——单刚体（Single Rigid Body, SRB）模型、接触力 QP、摩擦锥、足端力到关节力矩的映射（WBC）。若单体四足 MPC 不熟，先回足式控制部分补齐。 在 Part 中的位置：本章是 Part 2（多机协同运动）的开篇与核心。第 1-3 章（Part 1）把"看系统、连续协调的数学引擎、离散决策"配齐了；本章是这些工具的**第一次大规模综合应用**——你会看到第 2 章的 ADMM 不再是教科书上的标量 QP，而是被用来切分一个 24 维、跨 20 步预测时域、带摩擦锥与刚性耦合约束的真实多足联合优化；第 2 章的共识不再是抽象的 $\dot x=-Lx$，而是变成 ADMM 协调步里"邻居预测轨迹的加权平均"。学完本章，你将第一次把"分布式优化理论"落到"会走路的真机器人编队"上。

前置自测¶

📋 答不出 ≥ 2 题 → 标 ① 的回第 1 章，标 ② 的回第 2 章 §2.3，标 ③ 的回足式控制（单体四足 MPC），标 ④ 的回凸优化基础，再来读本章。本章会**大量复用**这些前置，但不重新教它们——欠了账会在 §4.4（ADMM 分解）和 §4.5（SRB 网络）卡住。

（接第 2 章 §2.3，①②） 标准 ADMM 的三步迭代是哪三步？写出求解 $\min_{x,z} f(x)+g(z)\ \text{s.t.}\ Ax+Bz=c$ 的 $x$-更新、$z$-更新、$y$-更新。其中**对偶变量 $y$ 的物理含义**是什么？罚参数 $\rho$ 太大、太小分别会怎样？（答不出 → 回第 2 章 §2.3 ADMM 分布式优化）
（接第 1 章，①） 什么叫通信图的 Laplacian 矩阵 $L$？它的零特征值对应哪个特征向量？为什么"$L$ 有且仅有一个零特征值"等价于"图连通"？本章 §4.7 的 ADMM 协调步会直接用到 $L$ 的这个性质。
（接足式控制，③） **单刚体（SRB）模型**把四足机器人近似成什么？它的状态有几维、包含哪些量？为什么腿部质量可忽略时这个近似才准确？**摩擦锥（friction cone）约束**为什么常被近似成金字塔（pyramid）而不是直接用二阶锥？
（接凸优化基础，④） 什么叫**拉格朗日对偶**？把约束 $h(x)=0$ 移进目标得到的对偶函数 $g(\lambda)=\min_x \mathcal{L}(x,\lambda)$ 有什么性质（凹性、对偶间隙）？强对偶在什么条件下成立（Slater 条件）？
（接第 1 章 §1.1） 为什么把 $N$ 台机器人的轨迹优化拼成一个**联合 QP**，其求解成本会随 $N$ 三次方甚至更高地膨胀？这正是本章 DMPC 要回避的"集中式爆炸"。

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

三步：$x^{k+1}=\arg\min_x \mathcal{L}_\rho(x,z^k,y^k)$（固定 $z,y$ 优化 $x$）；$z^{k+1}=\arg\min_z \mathcal{L}_\rho(x^{k+1},z,y^k)$；$y^{k+1}=y^k+\rho(Ax^{k+1}+Bz^{k+1}-c)$（对偶上升）。其中 $\mathcal{L}_\rho=f+g+y^\top(Ax+Bz-c)+\frac{\rho}{2}\|Ax+Bz-c\|^2$。对偶变量 $y$ 是约束 $Ax+Bz=c$ 的**拉格朗日乘子**，物理上是"违反一致性约束的边际价格/影子价格"——它累积了历史违约量，逼迫 $x$ 和 $z$ 最终一致。$\rho$ 太大 → 一致性约束主导、本地子问题被压扁、收敛初期快但精度震荡；$\rho$ 太小 → 一致性约束太弱、需要很多轮才拉得拢。
$L=D-A$（度矩阵减邻接矩阵）。零特征值对应**全 1 向量 $\mathbf{1}$**（$L\mathbf{1}=\mathbf{0}$，因为每行元素和为 0）。零特征值的重数 = 图的连通分量个数；图连通 ⟺ 只有一个连通分量 ⟺ 零特征值重数为 1 ⟺ $\lambda_2>0$。
SRB 把四足近似成**一个质量集中在质心、带固定转动惯量的刚体**，腿被视为无质量的力矢量。状态 12 维：质心位置 $\mathbf{p}\in\mathbb{R}^3$、姿态（欧拉角或 $SO(3)$）3 维、质心线速度 $\dot{\mathbf{p}}\in\mathbb{R}^3$、机体角速度 $\boldsymbol{\omega}\in\mathbb{R}^3$。腿部质量可忽略时（如 Mini Cheetah，腿占全身质量 <10%），把腿的动量贡献略去引入的误差才小。摩擦锥 $\|f_{xy}\|\le\mu f_z$ 是**二阶锥**，写进 QP 会让问题变成 SOCP（求解慢）；用金字塔 $|f_x|\le\mu f_z/\sqrt2,\ |f_y|\le\mu f_z/\sqrt2$（4 个线性不等式）近似后，问题退化为标准 QP，求解器（如 OSQP）快得多。
拉格朗日对偶：$\mathcal{L}(x,\lambda)=f(x)+\lambda^\top h(x)$，对偶函数 $g(\lambda)=\min_x\mathcal{L}(x,\lambda)$。性质：$g$ 是 $\lambda$ 的**逐点下确界**，恒为**凹函数**（无论原问题是否凸）；$g(\lambda)\le f^\star$ 恒成立（弱对偶），差值 $f^\star-g^\star$ 称对偶间隙。Slater 条件（存在严格可行内点）成立时，凸问题强对偶 $f^\star=g^\star$。
联合 QP 的决策变量是所有机器人的状态+控制堆叠，维度 $\propto N$；QP 内点法每步要解一个 KKT 线性系统，稠密求解成本 $O((\text{变量数})^3)=O(N^3)$。$N$ 翻倍，成本 8 倍——这是第 1 章演示过的集中式瓶颈。

本章目标¶

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

**论证**为什么多足编队的联合最优控制问题必须分布式求解——从集中式 MPC 的 $O(N^3)$ 计算爆炸、单点故障、通信瓶颈三个角度，而非凭口号。
从零建模**编队约束：把"保持队形"写成机器人间的**相对位置/相对质心**等式或软约束，区分 position / displacement / distance-based 三种编队范式在 MPC 代价函数与约束中的不同写法，并说明它们如何使联合优化问题在机器人之间**耦合。
推导对偶分解：把带耦合约束的联合 MPC 通过拉格朗日松弛拆成 $N$ 个本地子问题 + 一个价格协调步，解释对偶变量为什么叫"影子价格"、对偶子梯度法为什么收敛慢、以及它和 ADMM 的本质区别。
从增广拉格朗日推导 ADMM 分布式 MPC：写出 consensus-ADMM 形式的 $x$-更新（本地 MPC + 二次惩罚）、$z$-更新（共识平均）、$y$-更新（对偶上升）三步，分析 $\rho$ 对收敛的影响与残差平衡（residual balancing）自适应调节，并理解为什么非凸接触约束下 ADMM 的收敛性无理论保证。
建立 SRB 网络模型：把 $N$ 个四足 + 共享负载建模为**互联单刚体网络（interconnected SRB network）**，推导刚性耦合约束产生的约束力 $\boldsymbol{\lambda}$（拉格朗日乘子），并把 CoM 协调与接触约束（摩擦锥、单边接触、踏步时序）统一写进每个 agent 的本地 MPC。
掌握分层架构：复现 Kim-Fawcett-Hamed（T-RO 2023）的"高层分布式 MPC → 中层步态调度 → 低层分布式 WBC"三层控制流，说明每一层的输入输出、为什么 WBC 可以纯本地无通信、以及一步通信延迟与一致性协议（agreement protocol）如何嵌入。
分析通信拓扑与稳定性：把第 2 章的共识收敛理论接到 DMPC 上——说明 ADMM 协调步本质是共识、通信拓扑（$\lambda_2$、直径、时延、丢包）如何影响 ADMM 收敛轮数与编队稳定性，并理解 DMPC 的递归可行性（recursive feasibility）与终端代价（terminal cost）这两个稳定性支柱。
**能用 Python/C++ 实现**一个双四足（或双积分器简化版）编队的 ADMM 分布式 MPC，对比它与集中式 MPC 在轨迹质量、计算时间、通信量上的差异。

本章知识导航¶

一句话定位：本章把"多个会走路的机器人保持队形协同前进"这件事，从"如果有一台超级计算机统一指挥该怎么解"（集中式 MPC），一步步拆解成"每台机器人自己解一小块、只和邻居交换预测轨迹"（分布式 MPC），并把这套方法落到多足机器人特有的难点上——质心（CoM）必须协调、每条腿的接触力必须满足摩擦锥、踏步时序还在不停切换。

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

主线	解决的问题	关键工具	对应小节
为什么要分布式	集中式 MPC 解一个 $N$ 机联合 QP 为什么不可行？	$O(N^3)$ 爆炸、单点故障、通信瓶颈、预测交换	§4.1
编队怎么建模	"保持队形"如何写进 MPC 的代价与约束？	position/displacement/distance 三范式、耦合约束	§4.2
对偶分解	带耦合约束的联合优化如何拆成本地子问题？	拉格朗日松弛、影子价格、对偶子梯度、慢收敛	§4.3
ADMM 求解	如何让分解又快又稳地收敛？	consensus-ADMM 三步、增广拉格朗日、$\rho$ 调节	§4.4
多足编队建模	多足特有的 CoM 协调与接触约束怎么进 MPC？	SRB 网络、刚性耦合力 $\boldsymbol{\lambda}$、摩擦锥、单边接触	§4.5
分层控制	MPC 的 SRB 级命令如何变成关节力矩？	高层 MPC→步态调度→分布式 WBC、纯本地计算	§4.6
拓扑与稳定性	通信质量如何影响收敛？编队稳不稳？	共识接口、$\lambda_2$/时延/丢包、递归可行性、终端代价	§4.7

推荐阅读路径：

理论线（理解数学）：§4.1→§4.3→§4.4→§4.7。重点是对偶分解的推导、ADMM 的增广拉格朗日来源、收敛性分析。这条线把第 2 章的 ADMM 从标量 QP 推广到真实多足联合优化，是本章的数学骨架。
工程线（会用就行）：§4.1→§4.2→§4.4→§4.5→§4.6。重点是 SRB 网络建模、ADMM 代码骨架、分层控制流。做仿真/真机的同学走这条线。
必读：§4.1（动机，全章地基）、§4.4（ADMM 分布式 MPC，全章核心算法，需动手实现）、§4.5（SRB 网络，多足编队的建模核心，需跟着推一遍耦合力）。§4.3、§4.4、§4.5、§4.7 是四个 ⭐⭐⭐⭐ 的硬核小节，值得花最多时间。

为什么必须分布式？ (§4.1 集中式 MPC 的三重瓶颈)
    │
    ▼
编队约束怎么写进 MPC？ (§4.2 三范式 + 耦合约束)
    │
    ├─→ 对偶分解 (§4.3) ──→ 影子价格 + 子梯度慢收敛
    │        │
    │        ▼
    ├─→ ADMM 分布式 MPC (§4.4) ──→ 增广拉格朗日修复慢收敛
    │        │
    │        ▼
    ├─→ SRB 网络与多足编队 (§4.5) ──→ CoM 协调 + 接触约束 + 耦合力 λ
    │        │
    │        ▼
    ├─→ 分层控制 (§4.6) ──→ MPC → 步态 → 分布式 WBC
    │
    └─→ 通信拓扑与稳定性 (§4.7) ──→ 共识接口 + 递归可行性
              │
              ▼
        累积项目：双四足 ADMM 编队仿真

前置知识桥接¶

本章紧接第 1-3 章并依赖单体四足 MPC，这里把最关键的前置点重新激活——你不必翻回去就能跟上：

回顾第 1 章：集中式架构的 $O(N^3)$ 爆炸。第 1 章 §1.1 用代码演示过：把 $N$ 台机器人的规划拼成一个联合优化，求解成本随 $N$ 三次方膨胀，且依赖一个中央节点（单点故障）。第 1 章把这定性为"集中式不可扩展"，本章 §4.1 会把它**量化**——一个双四足联合 QP 约 960 个变量、$N=4$ 时近 2000 变量，内点法每步 $O(N^3)$ 的 KKT 求解在 1kHz 控制循环里就吃不消了。这正是 DMPC 存在的理由。
回顾第 1 章：Laplacian $L$ 与连通性。第 1 章为通信图构造了 $L=D-A$，证明了 $L\mathbf{1}=\mathbf{0}$、$L$ 半正定、$\lambda_2>0\iff$ 图连通。本章 §4.7 会让 $L$ 的这些性质**直接决定 ADMM 协调步的收敛速度**——你会看到 ADMM 的共识平均本质上是在 $L$ 描述的图上做迭代，$\lambda_2$ 越大收敛越快。
回顾第 2 章：ADMM 三步迭代与 $\rho$。第 2 章 §2.3 从增广拉格朗日推导了标准 ADMM 的 $x$/$z$/$y$ 三步，并讲了罚参数 $\rho$ 控制"本地自主 vs 全局一致"的权衡。本章 §4.4 把这套**原封不动地搬到多足 MPC**——唯一的区别是：第 2 章的 $x$ 是标量或小向量，这里的 $x$ 是一个机器人在整个预测时域上的状态-控制轨迹（数百维），$z$ 是邻居间要一致的"接口变量"（如共享负载状态、相对位姿）。算法骨架完全一样，规模和物理含义变了。
回顾第 2 章：共识 $\dot x=-Lx$ 与编队三范式。第 2 章 §2.4 讲了 position/displacement/distance-based 三种编队范式，并指出 displacement-based 控制律本质就是共识协议作用在"相对位置误差"上。本章 §4.2 把这三范式**升级进 MPC**——不再是一个静态控制律，而是预测时域上的代价函数与约束，编队误差被滚动优化。
回顾单体四足 MPC：SRB + 接触力 QP。单体四足 MPC 把一台四足近似成单刚体（12 维状态），在预测时域上优化质心轨迹与四条腿的接触力，约束包括摩擦锥、单边接触（支撑腿 $f_z\ge0$、摆动腿 $f=0$）、力矩限制。本章 §4.5 把 $N$ 台这样的 SRB 用刚性耦合约束连成网络——单体 MPC 是本章的"原子"，本章是把 $N$ 个原子协调起来。

前向预告：本章的编队保持的是"机器人之间的相对位姿/质心"，但当机器人要**共同搬运一个负载**时，问题会多出一层——它们对负载施加的力必须协调，否则会把负载"撕裂"或内耗。第 5 章（§60_协同搬运与力控）会引入 grasp matrix 与**内力（internal force）分配**，把本章的"运动协调"扩展到"力协调"。现在只需要记住：本章管的是"一起走到哪"，第 5 章管的是"一起使多大劲"——本章 §4.5 的刚性耦合力 $\boldsymbol{\lambda}$ 就是通往第 5 章内力的桥梁。

如果跳过本章会怎样¶

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

场景一（双四足编队，集中式 MPC 实时性崩了）。 你想让两台 Go2 保持 1 米间距同步行走，图省事把它们的状态和控制堆成一个联合 MPC，扔给 OSQP 求解。$N=2$、预测 20 步时勉强能跑（约 960 变量，15ms），你加到 $N=4$ 想做个小编队，求解时间瞬间飙到 60ms 以上——控制频率掉到 16Hz，机器人开始摇晃失稳。你知道"集中式不可扩展"，但不知道**具体在哪一步爆炸**（内点法 KKT 求解的 $O(N^3)$），更不知道怎样把这个联合 QP 拆成每台机器人各解一小块。§4.1 和 §4.4 正是讲这个拆解。

场景二（ADMM 写出来了，却几百轮不收敛）。 你照着 §4.4 的骨架把联合 MPC 用 ADMM 切分，$\rho$ 随手取了 1.0，结果迭代要么剧烈震荡、要么 500 轮才勉强拉到一致，根本来不及在一个控制周期内算完。你在代码里到处找 bug，却找不到——因为这不是 bug。§4.4 会告诉你：$\rho$ 的量级直接决定收敛速度与稳定性，而且对非凸的接触约束，ADMM 根本没有收敛保证，工程上要靠 warm-start（用上一周期的解初始化）+ 固定迭代轮数 + 残差平衡自适应 $\rho$ 来"够用就停"。不懂这些，你会以为是实现错了。

场景三（多足编队建模，耦合力凭空多出来不知怎么处理）。 你想给"双四足抬一根刚性杆"建 MPC，照搬单体 SRB 模型，却发现两台机器人通过杆**互相施力**——这个力既不是接触力也不是重力，你不知道它从哪来、怎么写进动力学方程。§4.5 会告诉你：刚性连接产生**约束力 $\boldsymbol{\lambda}$（拉格朗日乘子）**，它由"附着点相对位置固定"这个约束微分两次后解出，必须显式进入每个 agent 的 SRB 动力学，并在 ADMM 的协调步里达成一致。不理解这个，你的双足编队动力学方程从一开始就是错的。

预计阅读时间¶

模式	时长	适合
精读（跟着推完对偶分解与 ADMM 增广拉格朗日、亲手实现双积分器/双四足 ADMM 编队、推导 SRB 耦合力、复现分层控制流）	22–30 小时	第一次系统学分布式 MPC 与多足编队协同的工程师，建议分 5-6 次
速读（读懂集中式→分布式的动机、ADMM 三步在 MPC 里的形式、SRB 网络结构与分层架构，跳过推导细节与代码）	6–8 小时	有 ADMM 与单体 MPC 背景、想建立全局图景
速查（集中 vs 分布对比表 + ADMM-MPC 三步公式 + SRB 网络方程 + 分层数据流 + 故障排查 + API 速查）	50–70 分钟	实现分布式 MPC 编队时回查

科研发展脉络¶

在钻进具体方法前，先把这条研究线的来龙去脉理清——知道每个方法"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记名字有用得多。

年份	论文	Venue	引用	核心贡献
2006	Dunbar & Murray, "Distributed Receding Horizon Control for Multi-Vehicle Formation Stabilization"	Automatica	~1500	奠基作：每车解本地 RHC + 交换预测轨迹；引入"假设轨迹"与一致性惩罚保证稳定
2008	Keviczky, Borrelli 等, "Decentralized Receding Horizon Control for Large Scale Dynamically Decoupled Systems"	Automatica/T-CST	~500	分布式 MPC 编队系统化——预测交换 + 迭代协商；动力学解耦、约束耦合的标准范式
2010s	Summers & Lygeros; Conte 等	CDC/ECC	—	ADMM/对偶分解求解 DMPC：把联合 OCP 的对偶用 ADMM 分布式求解（凸假设）；合作式 DMPC 的稳定性综合
2022	Kim, Hamed, "Cooperative Locomotion via Supervisory Predictive Control"	ACC	~30	监督式预测控制 + 分布式非线性控制器，多足协同的早期模板
2023	Kim, Fawcett, Kamidi, Ames, Hamed, "Layered Control for Cooperative Locomotion of Two Quadrupedal Robots: Centralized and Distributed Approaches"	T-RO	~80	多足协同标杆：互联 SRB 网络模型 + 集中式与分布式 MPC（一步通信延迟 + agreement protocol）+ 低层分布式非线性控制器
2023	De Vincenti, Coros, "Centralized MPC for Collaborative Loco-Manipulation"	RSS	~50	集中式 SQP：robot/payload/gait-agnostic；$SO(3)$ 李群四元数积分
2024	Turrisi 等, "PACC: Passive-Arm Approach for High-Payload Collaborative Carrying"	IROS	~新	被动臂方案——每个四足加被动臂托住负载，免去刚性耦合建模
2024	Fawcett, Amanzadeh, Hamed 等, "Distributed Data-Driven Predictive Control for Multi-Agent Collaborative Legged Locomotion"	—	~新	数据驱动模板模型 + 分布式预测控制，绕开精确动力学建模
2025/26	Imran, Hamed 等, "Obstacle-Aware Distributed NMPC" / "ADMM-Based Distributed MPC with CBF for Safe Multi-Robot Quadrupedal Locomotion"	J. Dyn. Sys. / arXiv	~新	4 台 A1 障碍环境分布式 NMPC；ADMM + 控制屏障函数（CBF）保证安全的多足分布式 MPC

关键实验室脉络：Caltech/UPenn（Dunbar/Murray 分布式 RHC 奠基）→ UC Berkeley（Keviczky/Borrelli DMPC 系统化）→ ETH/苏黎世（Summers/Lygeros ADMM 求解 DMPC、Conte 合作式 DMPC、De Vincenti/Coros 集中式 loco-manipulation）→ Virginia Tech ARCOL（Hamed 团队，多足协同分布式 MPC 的当前标杆，本章主线）。

看这条线，有一条清晰的主线：决策从"一个中央大脑解全局"走向"每个 agent 解本地 + 邻居协商"，而协商工具从早期的"交换预测轨迹 + 一致性惩罚"（Dunbar/Keviczky），演进到"对偶分解/ADMM 严格分解联合优化"（Summers/Conte），再落地到"多足特有的 SRB 网络 + 分层 MPC/WBC"（Hamed 团队）。本章以 Kim-Fawcett-Hamed T-RO 2023 为核心案例，因为它最完整地把"分布式 MPC + 多足 + 编队/协同"三者打通。

本质洞察：分布式 MPC 的研究史，本质是一部"如何把一个紧耦合的全局优化，安全地切成可并行的局部优化"的历史。早期方法（交换预测轨迹）切得"软"——靠惩罚项鼓励一致，不保证严格满足耦合约束；ADMM 切得"硬"——通过对偶变量强制收敛到满足耦合约束的解。多足把难度又抬高一层：耦合约束不再是线性的相对位置，而是带摩擦锥、单边接触的非凸约束，于是连 ADMM 的收敛保证都失效了——这正是 2024-2026 年这个方向仍在活跃发论文的原因。

本章符号约定¶

符号	含义	首见
$N$	编队中机器人（agent）数量	§4.1
$T$	MPC 预测时域步数（horizon）	§4.1
$i,j$	agent 索引；$j\in\mathcal{N}_i$ 表示 $j$ 是 $i$ 的通信邻居	§4.1
$\mathbf{x}_i$	agent $i$ 的状态（SRB 为 12 维：$[\mathbf{p}_i,\boldsymbol{\Theta}_i,\dot{\mathbf{p}}_i,\boldsymbol{\omega}_i]$）	§4.1
$\mathbf{u}_i$	agent $i$ 的控制（足端接触力堆叠）	§4.1
$\mathbf{X}_i,\mathbf{U}_i$	agent $i$ 在整个时域 $T$ 上的状态/控制轨迹（堆叠）	§4.1
$J_i(\cdot)$	agent $i$ 的本地代价	§4.1
$\mathbf{p}_i$	agent $i$ 的质心（CoM）位置 $\in\mathbb{R}^3$	§4.2
$\mathbf{d}_{ij}$	期望的 $i$-$j$ 相对位移（编队几何）	§4.2
$\boldsymbol{\lambda}$	拉格朗日乘子；§4.3 为对偶变量，§4.5 为刚性耦合约束力	§4.3
$g(\boldsymbol{\lambda})$	对偶函数	§4.3
$\mathbf{z}$	ADMM 的全局一致（consensus）变量	§4.4
$\mathbf{y}_i$	agent $i$ 的 ADMM 对偶变量（缩放形式 $\mathbf{u}_i^{\text{scaled}}$）	§4.4
$\rho$	ADMM 罚参数（penalty parameter）	§4.4
$r^k,s^k$	第 $k$ 轮的原始残差（primal residual）、对偶残差（dual residual）	§4.4
$\mathbf{f}_{ij}$	agent $i$ 第 $j$ 条腿的接触力	§4.5
$\mu$	摩擦系数	§4.5
$\mathbf{p}_i^{\text{att}}$	agent $i$ 在共享负载上的附着点位置	§4.5
$L$	通信图 Laplacian $=D-A$	§4.7
$\lambda_2$	$L$ 的代数连通性（第二小特征值）	§4.7

§4.1 从集中式到分布式：为什么联合 MPC 必须拆开 ⭐⭐⭐¶

这一节解决什么问题：在写任何分布式算法之前，必须先回答"为什么不能直接用集中式？"。这一节把"集中式 MPC 不可扩展"这个口号，拆成三个可量化、可观测的具体瓶颈（计算、通信、可靠性），并给出分布式 MPC 的基本思想——每个 agent 解本地 MPC、交换预测信息。它是 §4.3-§4.4 所有分解方法的动机地基。

动机：两台机器人好解，四台就崩¶

先把场景定下来。$N$ 台四足机器人要保持一个队形（比如一字排开、间距 1 米）向前行走。每台机器人用单刚体（SRB）模型描述，状态 12 维。我们希望在每个控制周期（典型 1kHz，即每 1ms 一次）求解一个 MPC：在未来 $T$ 步（比如 $T=20$，对应 0.4 秒预测）内，优化每台机器人的质心轨迹和四条腿的接触力，使得（a）每台机器人自身平衡、踏步可行；（b）机器人之间保持期望队形。

最直接的想法是**集中式**：把所有机器人的状态和控制堆成一个大向量，写一个联合优化问题，交给一台中央计算机求解，再把结果分发回各机器人。

\[ \min_{\mathbf{U}_1,\dots,\mathbf{U}_N}\ \sum_{i=1}^N J_i(\mathbf{X}_i,\mathbf{U}_i) + J_{\text{form}}(\mathbf{X}_1,\dots,\mathbf{X}_N) $$ $$ \text{s.t.}\quad \mathbf{x}_i(t{+}1)=f_i(\mathbf{x}_i(t),\mathbf{u}_i(t)),\ \forall i,t \quad(\text{各自动力学}) $$ $$ \quad\ \ \ \text{formation\_constraints}(\mathbf{X}_1,\dots,\mathbf{X}_N)=0 \quad(\text{编队耦合}) $$ $$ \quad\ \ \ \text{friction\_cone}(\mathbf{u}_i)\le0,\ \forall i \quad(\text{接触约束}) \]

这个问题在 $N=2$ 时确实能解。问题是：它解不动 $N=4$，更解不动 $N=10$。 为什么？这就是本节要拆开讲的。

如果坚持用集中式会怎样¶

集中式 MPC 的失败不是单一原因，而是三个瓶颈叠加。把它们摊开看：

瓶颈一：计算爆炸（$O(N^3)$）。 联合 MPC 的决策变量是所有机器人在整个时域上的控制（和被消去前的状态）。变量数 $\approx N\cdot(n_x+n_u)\cdot T$。对 SRB（$n_x=12$，每条腿 3 维力、4 条腿 $n_u=12$），$T=20$：

$N$	联合变量数（量级）	内点法每步 KKT 求解 $O(\text{vars}^3)$ 相对成本	1kHz 下能否实时
1（单体）	~480	1×	✅ 轻松
2	~960	~8×	✅ 勉强（~15ms）
4	~1920	~64×	❌ 掉到 ~16Hz
10	~4800	~1000×	❌ 完全不可能

回顾第 1 章 §1.1：那里我们用代码演示了联合 QP 的求解成本随 $N$ 三次方膨胀。这张表是它在多足 MPC 上的具体数字——$N$ 从 2 到 4，变量翻倍，但 KKT 求解成本翻了约 8 倍（$2^3$），控制频率从勉强可用直接掉到危险区。MPC 的价值在于高频滚动重规划，频率一掉，预测就跟不上扰动，机器人失稳。

瓶颈二：通信瓶颈与单点故障。 集中式需要一个中央节点：所有机器人每个周期把状态发给它，它算完再把控制发回去。这意味着（a）中央节点的上下行带宽 $\propto N$，机器人一多就堵；（b）中央节点宕机或与某台机器人断联，整个编队瘫痪——这是第 1 章讲的"单点故障"。在野外多机器人作业（搜救、巡检）中，可靠的中央通信往往根本不存在。

瓶颈三：隐私与模块化。 集中式要求中央节点知道每台机器人的完整模型（动力学参数、约束）。异构编队（第 6/7 章）里，不同机型的模型可能由不同厂商提供、不愿暴露；而且每加一台机器人就要重写整个联合问题，毫无模块化可言。

本质洞察：集中式 MPC 的三个瓶颈不是独立的工程缺陷，而是同一个根源的三个侧面——它把"协调"建模成了"一个全知全能的中心"。计算爆炸是因为中心要解全局；单点故障是因为中心不可替代；不可模块化是因为中心要懂全部。分布式 MPC 的全部努力，就是**取消这个中心**，让协调从"中心统一裁决"变成"邻居之间协商"。

反面对照：那能不能干脆"各管各的"（完全去中心、零协调）？¶

走到另一个极端：每台机器人完全独立解自己的单体 MPC，互不通信。这当然没有计算爆炸、没有单点故障。但它会怎样？

它根本保持不了队形。 每台机器人只优化"我自己走得好不好"，没有任何项告诉它"我离邻居该多远"。机器人之间的相对位置会随各自的扰动、各自的局部最优而漂移，队形分崩离析。更糟的是，如果它们共享一个负载（第 5 章），零协调会让它们对负载施加**互相冲突的力**，把负载内耗甚至撕裂。

不是 X 而是 Y：分布式 MPC 不是"把集中式问题简单地切成 $N$ 块各自解"（那是完全去中心，会丢掉协调），而是"切成 $N$ 块、但通过邻居间的信息交换让这些块协同收敛到（接近）联合最优解"。关键词是"协同收敛"——既要并行（解决计算爆炸），又要一致（保住编队）。这两个目标的张力，正是 §4.3-§4.4 所有分解算法要平衡的。

这就引出了分布式 MPC 的核心思想。

理论：分布式 MPC 的基本范式——预测交换¶

集中式（全知中心）和完全去中心（零协调）是两个极端。分布式 MPC 落在中间：每个 agent 解一个本地 MPC，但在求解时，把邻居的"预测轨迹"作为已知信息纳入考虑。

这个思想最早由 Dunbar & Murray（2006）和 Keviczky-Borrelli（2008）系统化，核心机制叫**预测交换（prediction exchange）或**假设轨迹（assumed trajectory）：

每个 agent 维护并广播自己的预测轨迹。agent $i$ 解完本地 MPC 后，得到自己未来 $T$ 步的状态预测 $\hat{\mathbf{X}}_i=[\hat{\mathbf{x}}_i(1),\dots,\hat{\mathbf{x}}_i(T)]$，把它发给所有邻居 $j\in\mathcal{N}_i$。
每个 agent 用邻居的预测构造本地的编队项。agent $i$ 解本地 MPC 时，编队代价 $J_{\text{form}}$ 里邻居 $j$ 的状态用**上一轮收到的 $\hat{\mathbf{X}}_j$** 代入（当作已知常量），于是耦合的 $J_{\text{form}}(\mathbf{X}_1,\dots,\mathbf{X}_N)$ 退化成只含 $\mathbf{X}_i$ 的本地项 $J_{\text{form}}^i(\mathbf{X}_i; \{\hat{\mathbf{X}}_j\}_{j\in\mathcal{N}_i})$。
迭代/滚动。各 agent 用新的预测再算一轮（同步协商），或直接进入下一个控制周期（异步、靠滚动收敛）。

形式上，agent $i$ 的本地问题变成：

\[ \min_{\mathbf{U}_i}\ J_i(\mathbf{X}_i,\mathbf{U}_i) + \sum_{j\in\mathcal{N}_i} J_{ij}(\mathbf{X}_i, \hat{\mathbf{X}}_j) $$ $$ \text{s.t.}\quad \mathbf{x}_i(t{+}1)=f_i(\mathbf{x}_i(t),\mathbf{u}_i(t)),\quad \text{friction\_cone}(\mathbf{u}_i)\le0 \]

注意：现在每个 agent 解的问题规模是**单体大小**（变量数 $\approx(n_x+n_u)T$，与 $N$ 无关），$N$ 个 agent 并行求解。计算爆炸解决了。

类比（像 X 不像 Y）：预测交换很像**一群人排队列队前进时各自的余光**。每个人主要盯着自己脚下走稳（本地 MPC），同时用余光瞥一眼左右邻居的位置和趋势（邻居的预测轨迹），据此微调自己的步伐保持队形。**像**的地方：没有教官统一指挥（无中心），每个人只看邻居（局部信息），整体队形却能维持。**不像**的地方：人的余光是连续实时的，而预测交换是**离散轮次**的——agent $i$ 用的是邻居**上一轮**算出的预测（有一步信息延迟），这个延迟正是 §4.7 要分析的稳定性隐患；而且人凭直觉调整，预测交换是在解一个严格的优化问题。

但预测交换有个根本弱点：它对编队约束的满足是"软"的。编队项 $J_{ij}$ 是惩罚项（鼓励 $\mathbf{X}_i$ 靠近期望相对位置），不是硬约束。如果两个 agent 各自基于对方的旧预测优化，它们可能**永远差一点点**而不完全一致——因为没有任何机制强制它们的预测互相吻合。这在"必须严格满足耦合约束"的场景（如刚性连接的负载，附着点位置必须精确一致）下不够用。

这正是引出**对偶分解（§4.3）和 **ADMM（§4.4）**的动机：它们用拉格朗日乘子/对偶变量，把"软鼓励"升级为"强制收敛到满足耦合约束的解"。Dunbar/Keviczky 的预测交换是分布式 MPC 的"第一代"，对偶分解/ADMM 是"第二代"——后者在前者的并行结构上，补上了**严格满足耦合约束的保证（凸情形下）。

理论-工程桥接：为什么实际系统（如 Kim-Fawcett-Hamed T-RO 2023）里既能看到"交换预测轨迹"又能看到"agreement protocol（一致性协议）"？因为它们是同一思想的两层：交换预测轨迹是**信息流**（我把我的计划告诉你），agreement protocol/ADMM 对偶更新是**协调律**（我们如何根据彼此的计划修正到一致）。预测交换提供原料，ADMM 提供炼法。理解这一点，你读多足分布式 MPC 论文时就不会被"prediction exchange""consensus""ADMM""agreement protocol"这些看似不同的术语绕晕——它们描述的是同一个分布式协调框架的不同侧面。

系统性分类：MPC 控制架构的完整谱系¶

为避免把"集中 vs 分布"看成非黑即白的二分，这里给一个**系统性的架构谱系**（呼应第 1 章三大架构，这里聚焦 MPC 语境）。按"谁掌握全局信息、谁做决策、需要多少通信"分类，多机 MPC 架构落在一条连续谱上：

架构	全局信息	决策主体	通信	最优性	鲁棒性	计算	典型代表
集中式（centralized）	中央节点全知	中央节点	全部状态上下行	全局最优	单点故障	$O(N^3)$	De Vincenti RSS 2023
分散式（decentralized）	各 agent 仅本地	各 agent	零通信	无协调（队形崩）	无单点	$O(1)$/agent	完全去中心（§4.1 反面）
分布式（distributed）	各 agent + 邻居	各 agent 协商	仅邻居、接口变量	近最优（gap）	无单点	单体/agent + 协调	Kim-Hamed T-RO 2023
分层（hierarchical）	分层（高层协调、低层本地）	分层	高层通信、低层本地	近最优	高层弱中心	分层分摊	监督式预测控制 + 分布式低层

关键区分（消除常见混淆）：

分散式（decentralized）≠ 分布式（distributed）。这是最常被混淆的一对。分散式 = 零通信、各自为政（§4.1 反面，队形崩）；分布式 = 邻居通信、协商一致（本章主角，队形保持）。中文里两词都常译"分布式"，但英文严格区分——读论文务必看清是 decentralized（无协调）还是 distributed（有协调）。
分布式 vs 分层不是对立的。Kim-Fawcett-Hamed 同时是分布式（高层 MPC 邻居协商）和分层（高层 MPC + 低层 WBC）——分层是"垂直"切分（按频率/抽象层），分布式是"水平"切分（按 agent）。一个真实系统两个维度都切（§4.6）。

本质洞察：这四种架构不是四个孤立的选项，而是**"协调强度"这条轴上的采样点**——从"中央全知全控"（集中式，协调最强但最脆）到"各自为政"（分散式，最鲁棒但无协调），分布式和分层落在中间，用"邻居通信"换取"协调 + 鲁棒"的平衡。选架构的本质是回答：我需要多强的协调？我能容忍多大的通信/计算成本？我的中央节点有多可靠？ 多足协同搬运需要强协调（耦合约束严格）+ 高鲁棒（野外无可靠中央）+ 实时（毫秒级），这三个需求的交集恰好把它推向"分布式 + 分层"——这就是为什么本章的主线方法是这个组合，而不是任意选的。

系统性分类（R6E）的价值：拿到一篇多机 MPC 论文，先把它放进这张谱系表的某一格（看它的通信模式和决策主体），你就立刻知道它的最优性/鲁棒性/计算的大致权衡，不必读完全文。这比记住一堆方法名有用——架构谱系是"思考框架"，具体方法是"框架里的实例"。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：以为分布式 MPC 一定比集中式"更好"。 - 错误描述：认为既然分布式解决了计算爆炸，就应该无脑用分布式替代集中式。 - 现象/后果：在 $N=2$、有可靠中央计算机的场景下用分布式，反而引入了 ADMM 迭代的额外延迟和"不收敛风险"，轨迹质量还比集中式差（有最优性 gap）。 - 根本原因：分布式是**用最优性换取可扩展性和鲁棒性**的权衡，不是免费午餐。集中式给出全局最优；分布式给出近似解（ADMM 在有限轮数内停，存在 gap）。 - 正确做法：按规模和场景选择——$N$ 小（≤2-3）、中央通信可靠 → 集中式（Kim-Hamed 论文中集中式 MPC 计算约 15ms、轨迹最优）；$N$ 大、需鲁棒性/模块化 → 分布式（各 agent 并行约 6ms，轨迹 <3% gap）。§4.7 给出完整选型框架。

陷阱 2（思维陷阱）：把"动力学解耦"和"约束解耦"混为一谈。 - 错误描述：以为既然每台机器人有自己的动力学方程（动力学解耦），整个问题就天然可分。 - 现象/后果：写本地 MPC 时漏掉了编队耦合的处理，结果各 agent 各走各的，队形崩溃（退化成完全去中心）。 - 根本原因：多足编队是**动力学解耦但约束（和代价）耦合**的系统——每台机器人的动力学 $f_i$ 确实只依赖自己的 $\mathbf{x}_i,\mathbf{u}_i$，但编队约束/代价 $J_{\text{form}}$ 把不同 agent 的状态绑在一起。Keviczky-Borrelli 2008 标题里的"dynamically decoupled"说的正是前者，但耦合恰恰来自后者。 - 正确做法：明确区分两类耦合。动力学解耦让"本地求解"成为可能（每个子问题有自己的动力学约束）；约束/代价耦合是"必须协调"的根源（§4.3-§4.4 的分解就是处理这一层）。

陷阱 3（概念误区）：以为预测交换里"用邻居的预测"是用当前轮的实时值。 - 错误描述：实现时假设 agent $i$ 能拿到邻居 $j$ 本轮**正在算的最新轨迹。 - **现象/后果：在异步/有通信延迟的真实系统里，这个假设不成立——你拿到的总是邻居上一轮的结果。如果代码逻辑依赖"实时同步"，会在真机上出现难以复现的震荡。 - 根本原因：分布式系统中信息天然有延迟。Kim-Fawcett-Hamed 明确建模为"one-step communication delay（一步通信延迟）"——本轮用邻居上一轮的预测。 - 正确做法：设计算法时显式假设"邻居信息滞后一步"，并在 §4.7 分析这个延迟对稳定性的影响。warm-start 时用滞后的邻居预测，不要假装能拿到实时值。

练习¶

（计算瓶颈量化） 假设内点法求解 QP 的成本为 $C(\text{vars})=\alpha\cdot\text{vars}^3$，且单体 MPC（480 变量）实测求解 10ms。估算 $N=2,4,6,8$ 的集中式联合 MPC 求解时间（忽略约束数变化的影响，只看变量立方）。在 1kHz 控制循环（预算 1ms）和 200Hz（预算 5ms）下，各自最多支持几台机器人集中式实时求解？这道题让你对"$O(N^3)$ 爆炸"有量化的体感。
（预测交换 vs 完全去中心） 用 Python 写一个最小例子：$N=3$ 个**双积分器**（$\ddot{\mathbf{p}}_i=\mathbf{u}_i$，2D）要保持一字队形（相邻间距 1m）向 $+x$ 移动。分别实现（a）完全去中心（每个 agent 只优化自己到目标的轨迹，无编队项）和（b）预测交换（每个 agent 的代价加入"与邻居预测保持期望相对位置"的惩罚项，用邻居上一轮预测）。对比两者在 30 个控制周期后的队形误差（相邻实际间距与 1m 的偏差）。预期结果：(a) 队形漂移，(b) 队形大致维持但有残差。
（开放设计） 阅读科研脉络表中 Dunbar & Murray 2006 与 Keviczky-Borrelli 2008 的摘要（可只读 abstract）。用一段话说明：为什么"交换预测轨迹"还不足以保证编队约束被严格满足？如果两个 agent 都用对方的旧预测、且各自的编队项只是惩罚而非硬约束，描述一个它们"永远差一点点"的具体情形。这道题为 §4.3 对偶分解的引入做铺垫。

§4.2 编队约束建模：把"保持队形"写进 MPC ⭐⭐⭐¶

这一节解决什么问题：§4.1 反复说"编队耦合"，但到底"保持队形"在数学上长什么样？这一节把第 2 章的编队三范式（position / displacement / distance-based）从"一个静态控制律"升级为"MPC 预测时域上的代价函数与约束"，说明每种范式如何决定了机器人间的**耦合结构**——而这个耦合结构正是 §4.3-§4.4 要分解的对象。建模错了，后面分解再漂亮也是错的。

动机：队形不是一个数，是一组关系¶

"保持一字队形，间距 1 米"——这句话怎么变成优化问题里的数学对象？

初学者的第一反应往往是：给每台机器人指定一个目标位置（agent 1 去 $(0,0)$，agent 2 去 $(1,0)$，agent 3 去 $(2,0)$），让每台机器人 MPC 跟踪自己的目标点。但这立刻暴露一个问题：如果整个编队要往前走、要转弯、要绕障碍，这些"绝对目标点"每个周期都得有人重新算并下发——这又退回到了需要中央节点的集中式。

更本质的观察是：队形描述的是机器人之间的"相对关系"，而不是每台机器人的"绝对位置"。 "间距 1 米"约束的是 agent $i$ 和 agent $j$ 的相对位移 $\mathbf{p}_j-\mathbf{p}_i$，而整个队形可以自由地平移、旋转。这个区别决定了：编队约束应该写成**相对量**，这样每台机器人只需和邻居比较，无需知道全局坐标——这正是分布式的前提。

回顾第 2 章 §2.4：那里我们讲了编队的三种范式，并指出 displacement-based 控制律本质就是共识协议作用在"相对位置误差"上。本节把那个**静态控制律**升级——不再是"当前误差 → 当前控制"，而是"在未来 $T$ 步的预测时域上，让相对误差的累积代价最小"。MPC 的滚动优化让编队对扰动和障碍有了预测性的应对能力。

如果用绝对位置建模会怎样¶

假设硬要用绝对目标点 $\mathbf{p}_i^{\text{ref}}$ 建模，编队代价写成 $\sum_i\|\mathbf{p}_i-\mathbf{p}_i^{\text{ref}}\|^2$。会出三个问题：

需要全局坐标系与中央规划器。谁来决定 $\mathbf{p}_i^{\text{ref}}$？编队前进时这些目标点持续变化，必须有个中央节点统一计算队形的整体运动并下发——单点故障回来了。
过约束（over-constrained）。绝对位置把队形的平移/旋转自由度也锁死了。但很多任务只关心"保持相对队形"，整体怎么移动应由更高层（或领航者）灵活决定。锁死全局位姿会让编队无法自然地避障或跟随。
不可分布式。$\|\mathbf{p}_i-\mathbf{p}_i^{\text{ref}}\|^2$ 看似可分（每项只含一个 agent），但 $\mathbf{p}_i^{\text{ref}}$ 的生成依赖全局信息，本质仍是中心化的。

不是 X 而是 Y：编队建模的核心不是"给每个机器人一个绝对目标"，而是"约束机器人之间的相对几何，让队形的整体位姿成为自由（或由领航者决定）的自由度"。前者把队形钉死在地图上，后者让队形像一个可平移可旋转的"刚性图案"漂浮在空间里。

历史：从 Tanner/Pappas 的刚性图到编队三范式¶

编队控制的数学源头之一是**图刚性理论（graph rigidity）**。把每台机器人看作图的顶点，机器人间的"相对位置约束"看作边，"队形可保持"等价于"约束图是刚性的"（rigid graph）——即满足这些边约束的构型，整体只剩下平移和旋转自由度。Tanner、Pappas、Kumar 在 2000 年代初把图刚性引入多机编队，奠定了"用相对量描述队形"的范式。

后来，编队控制按"用什么相对量约束、需要什么传感/通信"分成三个层次清晰的范式（Oh, Park, Ahn 2015 综述系统化）：

范式	约束量	需要的信息	队形保留的自由度	通信/传感要求
Position-based	绝对位置 $\mathbf{p}_i$	全局坐标	无（完全锁定）	全局定位（GPS/动捕）
Displacement-based	相对位移 $\mathbf{p}_j-\mathbf{p}_i$	共同朝向 + 相对位置	平移	邻居相对位置 + 朝向一致
Distance-based	相对距离 $\\|\mathbf{p}_j-\mathbf{p}_i\\|$	仅相对位置	平移 + 旋转	邻居相对距离

越往下，需要的信息越少（越分布式友好），但控制律越非线性、分析越难。Displacement-based 是分布式 MPC 编队最常用的折中——它只需邻居相对位置和一个共同朝向（如都朝向前进方向），约束是线性的（相对位移是状态的线性函数），写进 MPC 后保持凸性。本章后续以 displacement-based 为主。

理论：三范式在 MPC 中的代价与约束写法¶

现在把三范式落进 MPC。设期望编队几何由一组期望相对位移 $\{\mathbf{d}_{ij}\}$ 指定（$\mathbf{d}_{ij}$ = agent $j$ 相对 agent $i$ 的期望偏移，如一字队形中 $\mathbf{d}_{12}=(1,0,0)^\top$）。

(1) Displacement-based 编队代价（软约束形式）。 在预测时域 $T$ 上，对每条通信边 $(i,j)\in\mathcal{E}$、每个时刻 $t$，惩罚实际相对位移偏离期望：

\[ J_{\text{form}}=\sum_{t=1}^{T}\sum_{(i,j)\in\mathcal{E}} w_{ij}\big\|\,(\mathbf{p}_j(t)-\mathbf{p}_i(t)) - \mathbf{d}_{ij}\,\big\|^2 \]

这是一个**二次代价**，关于状态线性，保持 MPC 的凸性（QP）。$w_{ij}$ 是编队权重——越大越"硬"地维持队形。注意 $J_{\text{form}}$ 同时含 $\mathbf{p}_i$ 和 $\mathbf{p}_j$，这就是耦合的来源：agent $i$ 的最优控制依赖 agent $j$ 的轨迹，反之亦然。

(2) Displacement-based 硬约束形式。 如果队形必须严格满足（如刚性连接），写成等式约束：

\[ \mathbf{p}_j(t)-\mathbf{p}_i(t) = \mathbf{d}_{ij},\quad \forall (i,j)\in\mathcal{E},\ \forall t \]

硬约束保证队形精确，但**牺牲了避障/跟随的灵活性**，且让分布式求解更难（耦合从代价项变成耦合约束，必须用 §4.3-§4.4 的对偶/ADMM 严格处理）。实践中常用软约束（大权重）+ 关键约束硬化的混合。

(3) Distance-based 编队代价。 只约束距离，保留旋转自由度：

\[ J_{\text{form}}=\sum_{t}\sum_{(i,j)\in\mathcal{E}} w_{ij}\big(\,\|\mathbf{p}_j(t)-\mathbf{p}_i(t)\|^2 - \|\mathbf{d}_{ij}\|^2\,\big)^2 \]

注意这里是**距离平方的差的平方**（用平方距离避免开方的不可微）。这个代价关于状态是**四次的、非凸的**——这就是 distance-based 更难的代价：MPC 不再是 QP，要用 SQP 或非线性求解器，且可能陷入局部最优（不同旋转构型都满足距离约束）。

领航-跟随（leader-follower）作为锚定。 纯 displacement/distance 约束保留了整体平移/旋转自由度——队形"漂浮"着，整体往哪走没人管。实践中常指定一个**领航者（leader）**，它跟踪一条全局参考轨迹（由高层任务规划给出），其余**跟随者（follower）**只维持相对约束。这样既保留了分布式（跟随者只看邻居），又锚定了整体运动。Kim-Fawcett-Hamed 的双四足编队中，两台机器人通过共享负载耦合，整体运动由一条质心参考轨迹引导，是这种思想的体现。

本质洞察：编队三范式的差异，本质是**"用越少的信息换越多的队形自由度，代价是越强的非凸性"**这个权衡的三个采样点。Position-based 用最多信息（全局坐标）换最严的约束（零自由度），代价是线性、好解但不分布式；distance-based 用最少信息（仅距离）换最松的约束（平移+旋转自由），代价是四次非凸、难解但最分布式。Displacement-based 卡在中间——线性凸、平移自由、需要朝向一致，是分布式 MPC 的甜点区。选哪个范式，等于在"我能拿到什么信息"和"我能容忍多难的优化"之间定位。

类比（像 X 不像 Y）：三范式像描述一个队列的三种口令。Position-based 像"每个人站到地上画好的脚印上"（绝对、精确、需要预先画好脚印=全局坐标）；displacement-based 像"每个人保持在左边人的右后方 1 米"（相对、需要大家都知道哪是"前"=共同朝向）；distance-based 像"每个人和邻居保持 1 米臂展"（最松、转个方向也成立=旋转自由）。**像**的地方：都在描述同一个队形。**不像**的地方：约束强度和所需信息递减，而求解难度（非凸性）递增——这是类比对应不上的关键，口令的"难易"在人听来差不多，但在优化器看来是 QP 与非凸问题的天壤之别。

多足特有：编队约束作用在"质心"还是"机体"？¶

对四足机器人，一个容易被忽略的建模决策是：编队约束约束的是**质心（CoM）位置 $\mathbf{p}_i$** 还是某个机体参考点？SRB 模型的状态用的是质心，所以编队约束自然写在质心上。但这带来一个多足特有的微妙点：

四足行走时，质心相对地面**周期性起伏和摆动**（随踏步节律左右晃、上下颠）。如果编队约束硬性要求两台机器人的瞬时质心相对位移恒等于 $\mathbf{d}_{ij}$，会和各自的踏步动力学**打架**——为了维持瞬时队形，机器人可能被迫做出别扭的质心运动，破坏自身平衡。

理论-工程桥接：正因为质心随踏步起伏，工程上编队约束通常（a）用**软约束**（大权重而非硬等式），给质心的自然起伏留余地；或（b）约束**滤波后的/平均的**质心位置（去掉踏步频率的高频分量），只维持低频的队形趋势；或（c）在 SRB 这个**降阶模型**层面维持队形，把高频踏步细节留给底层 WBC（§4.6）去消化。这不是随意的选择，而是为了让"编队协调"和"自身平衡"这两个目标在不同频段上解耦——队形是低频的群体行为，平衡是高频的个体行为。理解这个频段分离，是多足编队建模区别于轮式/无人机编队的核心。

深入：编队代价的 Laplacian 结构——为什么编队就是"加权共识"¶

§4.2 反复说"displacement-based 编队本质是共识"（呼应第 2 章 §2.4）。这一小节把这句话**写成矩阵**，让你看清楚为什么——这是本章把第 1 章 Laplacian 和编队 MPC 焊接的关键一步，也是 §4.4 练习 2 的理论基础。

先看最简情形：忽略期望偏移（$\mathbf{d}_{ij}=\mathbf{0}$，即所有机器人要聚到一起），1 维、单时刻。编队代价：

\[ J_{\text{form}}=\sum_{(i,j)\in\mathcal{E}}w_{ij}(p_j-p_i)^2 \]

把它对堆叠向量 $\mathbf{p}=[p_1,\dots,p_N]^\top$ 写成二次型。展开 $\sum_{(i,j)}w_{ij}(p_j-p_i)^2$，你会发现它恰好等于：

\[ \boxed{\ J_{\text{form}}=\mathbf{p}^\top L_w\,\mathbf{p}\ } \]

其中 $L_w$ 是**加权 Laplacian**（$L_w=D_w-A_w$，$A_w$ 元素为 $w_{ij}$，$D_w$ 为加权度对角阵）！这不是巧合——回顾第 1 章：Laplacian 的二次型 $\mathbf{p}^\top L\mathbf{p}=\sum_{(i,j)\in\mathcal{E}}w_{ij}(p_j-p_i)^2$ 正是"边上差值的加权平方和"。编队代价的 Hessian 就是通信图的加权 Laplacian。

这立刻解释了三件事：

梯度下降编队 = 共识。对 $J_{\text{form}}=\mathbf{p}^\top L_w\mathbf{p}$ 做梯度下降 $\dot{\mathbf{p}}=-\nabla J_{\text{form}}=-2L_w\mathbf{p}$——这**就是第 2 章的连续共识协议 $\dot{\mathbf{p}}=-L\mathbf{p}$**（差一个常数因子）！所以"最小化编队代价"和"跑共识"在数学上是同一件事。第 2 章 §2.4 那句"displacement 控制律本质是共识"，在这里有了精确的矩阵证明。
收敛速率由 $\lambda_2(L_w)$ 决定。既然编队梯度流是 $\dot{\mathbf{p}}=-2L_w\mathbf{p}$，它收敛到一致（$L_w$ 零空间，即全相等）的速率由 $L_w$ 的第二小特征值 $\lambda_2$ 决定（第 2 章结论）——通信图越连通，编队收敛越快。这把"编队性能"和"通信拓扑"通过 $\lambda_2$ 定量绑定。
加偏移 $\mathbf{d}_{ij}$ 只是平移最优点。有期望偏移时 $J_{\text{form}}=\sum w_{ij}((p_j-p_i)-d_{ij})^2$，展开是 $\mathbf{p}^\top L_w\mathbf{p}+$（线性项）+ 常数。Hessian 仍是 $L_w$（偏移不改变二次项），只是把最优点从"全相等"平移到"满足期望相对位移"。所以**编队的"形状"由 $\mathbf{d}_{ij}$ 决定，编队的"收敛动力学"由 $L_w$（即拓扑）决定**——形状和动力学解耦。

本质洞察：编队控制 = 共识 + 偏移。共识让大家"聚拢"（趋于一致），偏移 $\mathbf{d}_{ij}$ 让"一致"变成"保持期望相对位置"。所以编队不是一个独立于共识的新问题，而是**"带偏移的共识"——把第 2 章的共识协议 $\dot{\mathbf{p}}=-L\mathbf{p}$ 的"目标"从"全相等"改成"相对位置 = $\mathbf{d}$"。这个洞察的威力在于：**第 2 章关于共识的一切结论（收敛性、速率 $\lambda_2$、时延阈值 $\tau^\star$、双随机权重）全部直接适用于编队——你不需要为编队重新发展一套理论，编队是共识的"贴了标签"的版本。这也是为什么本章 §4.7 能把第 2 章的共识理论无缝接到 DMPC 上。

反事实：如果通信图不连通（$\lambda_2=0$，$L_w$ 有多个零特征值）会怎样？编队梯度流 $\dot{\mathbf{p}}=-2L_w\mathbf{p}$ 不会收敛到全局一致队形——每个连通分量各自聚拢，分量之间的相对位置无法协调（没有通信链路约束它们）。这对应一个具体故障：编队"裂成几块"，每块内部队形正确，块与块之间漂移。根因就是 $\lambda_2=0$（图不连通）。这把第 1 章"$\lambda_2>0\iff$ 图连通"的抽象结论，变成了"编队会不会裂开"的工程判据。

在 MPC 里，这个 Laplacian 结构意味着：编队代价项的 Hessian 块是 $L_w\otimes I_3$（3 维空间，每个时刻），稀疏且结构化——这个稀疏性正是 ADMM 能高效分解的原因之一（agent 只和 Laplacian 里相邻的 agent 耦合，对应只和通信邻居协调）。

深入：编队与避障的张力——软约束权重的本质¶

编队 MPC 的一个现实难点（Imran-Hamed 2025 障碍环境分布式 NMPC 的核心）：保持队形和避开障碍常常冲突。一个障碍挡在编队路径上，要么编队变形绕过（破坏队形），要么硬保持队形撞上去（不可接受）。MPC 如何权衡？

这正是**软约束 vs 硬约束**选择的用武之地（呼应 §4.2 理论部分）：

避障必须是硬约束：$\text{dist}(\mathbf{p}_i,\text{obstacle})\ge r_{\text{safe}}$，绝不能违反（撞了就完了）。
编队应该是软约束：编队代价 $J_{\text{form}}$ 是惩罚项，可以为了避障**临时牺牲**队形。

于是 MPC 在避障硬约束下最小化（自身代价 + 编队软代价）——障碍逼近时，避障硬约束主导，编队软代价被"暂时放弃"（队形变形绕障）；障碍过后，编队软代价重新主导，队形恢复。软约束权重 $w_{ij}$ 的本质，就是"为了别的目标（避障、自身平衡）愿意牺牲多少队形"的量化。

本质洞察：编队 MPC 里"硬约束 vs 软约束"的划分，编码了**任务的优先级**——硬约束是"绝不能违反的"（避障、动力学、摩擦锥），软约束是"尽量满足、必要时可让步的"（队形、舒适度）。这不是技术细节，而是**安全与性能的边界**：把什么设成硬约束，等于声明"什么是底线"。多足编队的底线是"不撞 + 不摔"（避障 + 平衡硬约束），队形是"锦上添花"（软约束）。搞反了——把队形设成硬约束、避障设成软惩罚——机器人会为了保持队形撞上障碍，这是灾难性的。所以约束的软硬划分本身就是一个安全决策。

反事实：如果编队设成硬约束、避障设成软惩罚会怎样？障碍逼近时，编队硬约束强制队形不变，避障软惩罚被编队约束压制——MPC 宁可付一点避障惩罚也要保队形，结果**撞上障碍**。这就是约束软硬搞反的致命后果。它说明：约束的软硬不是随意的建模偏好，而是直接决定系统在冲突情境下的行为，必须按"安全优先级"严格设定。

理论-工程桥接：Imran-Hamed 2025 用控制屏障函数（CBF）把避障做成硬约束嵌进分布式 NMPC，正是这个思想的前沿实现——CBF 提供了一种"保证安全（避障）的同时尽量完成任务（编队）"的严格机制。这把 §4.7 提到的"非凸 + 安全"前沿和本节的"软硬约束权衡"连起来：安全（避障、CBF）是硬的、有保证的，任务（编队）是软的、尽力的。这条"硬安全 + 软任务"的设计原则，贯穿整个多足分布式 MPC 的工程实践。

这个软硬权衡也是累积项目（§4.8）可以扩展的方向：在双 Go2 编队场景里加一个障碍，观察编队如何"变形绕障再恢复"——这比无障碍的纯编队更接近真实任务，也让你亲眼看到软约束权重 $w_{ij}$ 调大调小对"避障时队形牺牲多少"的影响。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：用绝对位置代价冒充分布式编队。 - 错误描述：编队代价写成 $\sum_i\|\mathbf{p}_i-\mathbf{p}_i^{\text{ref}}\|^2$，以为每项只含一个 agent 就是分布式。 - 现象/后果：表面上每个 agent 解自己的项，但 $\mathbf{p}_i^{\text{ref}}$ 的生成需要全局信息和中央规划，本质仍是中心化；且队形整体运动僵硬，无法自然避障。 - 根本原因：分布式的判据不是"代价项是否可分"，而是"求解所需信息是否仅来自邻居"。绝对参考点把全局信息藏在了 $\mathbf{p}_i^{\text{ref}}$ 里。 - 正确做法：用相对量（displacement/distance）建模，编队代价显含邻居状态 $\mathbf{p}_j$，求解时只需邻居信息。整体运动由领航者或高层参考锚定。

陷阱 2（编程陷阱）：distance-based 代价直接用 $\|\mathbf{p}_j-\mathbf{p}_i\|$ 导致不可微。 - 错误描述：代价写成 $(\|\mathbf{p}_j-\mathbf{p}_i\|-d_{ij})^2$，含欧氏范数（开方）。 - 现象/后果：当 $\mathbf{p}_j\to\mathbf{p}_i$（两机器人重合）时，$\|\cdot\|$ 在原点不可微，梯度爆炸/NaN，求解器崩溃或给出垃圾解。 - 根本原因：欧氏范数 $\|\mathbf{v}\|=\sqrt{\mathbf{v}^\top\mathbf{v}}$ 在 $\mathbf{v}=\mathbf{0}$ 处梯度 $\mathbf{v}/\|\mathbf{v}\|$ 无定义。 - 正确做法：用**平方距离**写代价 $(\|\mathbf{p}_j-\mathbf{p}_i\|^2-d_{ij}^2)^2$，处处可微（多项式）；或对范数加小正则 $\sqrt{\mathbf{v}^\top\mathbf{v}+\epsilon}$。前者更常用。

陷阱 3（概念误区）：对四足硬约束瞬时质心相对位置。 - 错误描述：要求 $\mathbf{p}_j(t)-\mathbf{p}_i(t)=\mathbf{d}_{ij}$ 在每个 $t$ 严格成立，包括踏步起伏的高频分量。 - 现象/后果：机器人为维持瞬时队形被迫做别扭的质心运动，与踏步动力学冲突，自身平衡变差甚至摔倒。 - 根本原因：四足质心随踏步周期性起伏，把高频起伏也纳入硬队形约束，会让"编队"和"平衡"两个目标在高频上互相打架。 - 正确做法：用软约束（大权重）给起伏留余地，或只约束低频/滤波后的质心趋势，把高频细节交给底层 WBC。队形是低频群体行为，平衡是高频个体行为，应在频段上分离。

陷阱 4（思维陷阱）：忽略编队约束让联合问题"耦合"，以为各 agent 仍可独立解。 - 错误描述：建好编队代价后，仍以为每个 agent 可以无视邻居独立求解。 - 现象/后果：若真各自独立解（把邻居状态当固定值且从不更新），队形误差无法收敛，或收敛到次优。 - 根本原因：$J_{\text{form}}$ 显含多个 agent 状态，是耦合项；独立求解等于砍掉了协调机制。 - 正确做法：认识到编队建模的产物就是"耦合"，处理耦合正是 §4.3（对偶分解）和 §4.4（ADMM）的任务。建模和求解是两步，本节只负责正确地"制造"耦合，下两节负责"分解"耦合。

练习¶

（三范式对比实现） 对 $N=3$ 双积分器一字队形（间距 1m），分别用 displacement-based 软约束、distance-based 软约束写出编队代价，并在一个 QP/NLP 求解器（如 cvxpy / scipy.optimize）里实现单步优化。给两台机器人一个初始扰动（偏离队形），观察：displacement-based 恢复到唯一队形；distance-based 可能恢复到**旋转后的**队形（因保留旋转自由度）。验证 distance-based 代价的非凸性（不同初值收敛到不同构型）。
（耦合结构分析） 对一字队形 $1\!-\!2\!-\!3$（边集 $\mathcal{E}=\{(1,2),(2,3)\}$），写出 displacement-based 编队代价 $J_{\text{form}}$ 关于堆叠变量 $[\mathbf{p}_1,\mathbf{p}_2,\mathbf{p}_3]$ 的 Hessian 矩阵的稀疏结构（哪些块非零）。说明：为什么 agent 1 和 agent 3 不直接耦合（Hessian 对应块为零），但通过 agent 2 间接耦合？这个稀疏结构和通信图 Laplacian $L$ 有什么关系？（提示：编队代价的 Hessian 正比于 $L\otimes I$）这道题把第 1 章的 Laplacian 和编队代价连起来。
（多足建模思考） 查阅 Kim-Fawcett-Hamed T-RO 2023 论文（arXiv 2211.06913）中 SRB 网络的状态定义。说明：他们的编队/协同约束作用在质心层面，为什么这与"把高频踏步交给底层控制器"的分层思想一致？如果反过来把编队约束直接加在足端（foot）层面会有什么问题？这道题为 §4.5 的 SRB 网络建模和 §4.6 的分层控制铺垫。

§4.3 对偶分解与价格协调：把耦合约束"卖"给市场 ⭐⭐⭐⭐¶

这一节解决什么问题：§4.2 制造了耦合（编队约束/代价把不同 agent 绑在一起），现在要分解它。这一节讲分布式优化最经典的分解工具——对偶分解（dual decomposition）：通过拉格朗日松弛把耦合约束移进目标，让问题在 agent 之间可分，再用一个"价格"（对偶变量）协调它们。它是理解 ADMM（§4.4）的必经之路——ADMM 本质是"对偶分解 + 增广项"，不先懂对偶分解的思想和它的弱点，就理解不了 ADMM 为什么那样设计。这是本章的数学核心之一。

动机：耦合约束挡在"可分"的路上¶

回顾 §4.2 的硬约束编队问题。把它整理成一个标准的"耦合约束"结构。设 agent $i$ 的本地决策变量是 $\mathbf{X}_i$（它在时域上的轨迹），本地代价 $J_i(\mathbf{X}_i)$，本地约束 $\mathbf{X}_i\in\mathcal{C}_i$（动力学、摩擦锥等，只含自己）。编队耦合约束写成线性形式：

\[ \min_{\mathbf{X}_1,\dots,\mathbf{X}_N}\ \sum_{i=1}^N J_i(\mathbf{X}_i)\quad \text{s.t.}\ \mathbf{X}_i\in\mathcal{C}_i\ (\forall i),\quad \underbrace{\sum_{i=1}^N A_i\mathbf{X}_i = \mathbf{b}}_{\text{耦合约束}} \]

这里 $\sum_i A_i\mathbf{X}_i=\mathbf{b}$ 是把所有编队等式约束（如 $\mathbf{p}_j-\mathbf{p}_i=\mathbf{d}_{ij}$）堆叠成的统一形式。关键观察：如果没有这个耦合约束，目标 $\sum_i J_i(\mathbf{X}_i)$ 和本地约束 $\mathbf{X}_i\in\mathcal{C}_i$ 都是**完全可分**的——每个 agent 独立解 $\min_{\mathbf{X}_i\in\mathcal{C}_i}J_i(\mathbf{X}_i)$ 就行。耦合约束是唯一挡在"可分"路上的障碍。

那么问题变成：能不能把耦合约束"处理掉"，让问题恢复可分？ 对偶分解给出的答案是：把约束**移进目标**（拉格朗日松弛），代价是引入一个"价格"。

如果用罚函数硬塞会怎样¶

一个朴素想法是：不用对偶，直接把耦合约束做成二次罚项加进目标（就像 §4.2 的软约束）：

\[ \min_{\mathbf{X}_i\in\mathcal{C}_i}\ \sum_i J_i(\mathbf{X}_i) + \frac{\rho}{2}\Big\|\sum_i A_i\mathbf{X}_i-\mathbf{b}\Big\|^2 \]

这看似可行，但有两个致命问题：

罚项本身不可分。$\|\sum_i A_i\mathbf{X}_i-\mathbf{b}\|^2$ 展开后有**交叉项** $\mathbf{X}_i^\top A_i^\top A_j\mathbf{X}_j$（$i\ne j$），把所有 agent 又耦合回去了——加罚项非但没解耦，反而制造了新的稠密耦合。
罚函数法的老问题：要让约束精确满足，$\rho$ 必须 $\to\infty$，但 $\rho$ 太大会让问题病态（Hessian 条件数爆炸）。有限的 $\rho$ 只能近似满足约束。

这里先记住一个伏笔：ADMM 的全部巧妙，就在于它既要用增广（二次罚）项的"强制收敛"好处，又要绕开"罚项不可分"的坏处。 它通过引入一个辅助变量 $\mathbf{z}$ 把交叉项拆开——这是 §4.4 的核心。现在先看不带增广项的"纯对偶分解"如何实现可分。

历史：从拉格朗日对偶到 Dantzig-Wolfe 与价格协调¶

对偶分解的思想根源是**拉格朗日对偶**（19 世纪的拉格朗日乘子法 + 20 世纪的凸对偶理论）。1960 年代，Dantzig-Wolfe 分解和 Benders 分解把大规模线性规划按结构拆成主问题+子问题，奠定了"分解协调（decomposition-coordination）"的范式。Everett（1963）明确提出用拉格朗日乘子作为"资源价格"协调子系统。1970-80 年代，这套方法在经济学（资源分配）、电力系统（机组组合）、网络优化中大放异彩，对偶变量被赋予"影子价格（shadow price）"的经济学解释。Bertsekas 的并行与分布式计算专著（1989）系统总结了对偶分解的分布式实现。

本质洞察：对偶分解的灵魂是一个深刻的经济学类比——把"满足耦合约束"看成"市场出清（market clearing）"。耦合约束 $\sum_i A_i\mathbf{X}_i=\mathbf{b}$ 就像"总供给 = 总需求"；对偶变量 $\boldsymbol{\lambda}$ 就是商品的**价格**。每个 agent 是一个自利的市场参与者，它根据当前价格 $\boldsymbol{\lambda}$ 决定自己的"生产/消费"（解本地问题）；如果总供需不平衡（约束违反），就调整价格（对偶上升）——供过于求就降价，供不应求就涨价——直到市场出清（约束满足）。这不是比喻的修辞，而是数学上严格成立的对应：对偶子梯度法的每一步，就是一次"价格调整"。

理论：拉格朗日松弛 → 可分的对偶函数 → 价格迭代¶

现在严格推导。对耦合约束 $\sum_i A_i\mathbf{X}_i=\mathbf{b}$ 引入拉格朗日乘子 $\boldsymbol{\lambda}$，构造拉格朗日函数（注意：本地约束 $\mathbf{X}_i\in\mathcal{C}_i$ 不松弛，保留）：

\[ \mathcal{L}(\mathbf{X}_1,\dots,\mathbf{X}_N,\boldsymbol{\lambda}) = \sum_i J_i(\mathbf{X}_i) + \boldsymbol{\lambda}^\top\Big(\sum_i A_i\mathbf{X}_i-\mathbf{b}\Big) \]

第一步：分组。 把 $\boldsymbol{\lambda}^\top\sum_i A_i\mathbf{X}_i=\sum_i\boldsymbol{\lambda}^\top A_i\mathbf{X}_i$ 拆开，按 agent 重组：

\[ \mathcal{L} = \sum_i\big[\underbrace{J_i(\mathbf{X}_i)+\boldsymbol{\lambda}^\top A_i\mathbf{X}_i}_{\text{只含 }\mathbf{X}_i\text{！}}\big] - \boldsymbol{\lambda}^\top\mathbf{b} \]

这是对偶分解的关键一步：松弛耦合约束后，拉格朗日函数关于 $\{\mathbf{X}_i\}$ 完全可分**了！因为耦合约束被"价格化"了——它不再是一个把变量绑在一起的硬约束，而是变成了每个 agent 目标里的一个**线性价格项 $\boldsymbol{\lambda}^\top A_i\mathbf{X}_i$。

阶段小结：到这里我们完成了"解耦"——通过拉格朗日松弛，原本耦合的约束变成了各 agent 目标里的价格项，使拉格朗日函数可分。接下来要做的是：(1) 定义对偶函数（每个 agent 在给定价格下独立最小化）；(2) 设计价格如何更新（对偶上升）。

第二步：对偶函数 = 各 agent 独立最小化。 对偶函数 $g(\boldsymbol{\lambda})=\min_{\mathbf{X}_i\in\mathcal{C}_i}\mathcal{L}$。由可分性：

\[ g(\boldsymbol{\lambda}) = \sum_i\underbrace{\Big[\min_{\mathbf{X}_i\in\mathcal{C}_i}\ J_i(\mathbf{X}_i)+\boldsymbol{\lambda}^\top A_i\mathbf{X}_i\Big]}_{\text{agent }i\text{ 的本地子问题 }g_i(\boldsymbol{\lambda})} - \boldsymbol{\lambda}^\top\mathbf{b} \]

每个 agent 独立解一个本地子问题 $g_i(\boldsymbol{\lambda})=\min_{\mathbf{X}_i\in\mathcal{C}_i}J_i(\mathbf{X}_i)+\boldsymbol{\lambda}^\top A_i\mathbf{X}_i$——这就是"给定价格 $\boldsymbol{\lambda}$，我（agent $i$）的最优生产计划"。子问题只含本地变量、本地约束，规模是单体大小，可并行。

第三步：价格更新（对偶上升 / 子梯度法）。 对偶问题是 $\max_{\boldsymbol{\lambda}}g(\boldsymbol{\lambda})$（$g$ 凹，无约束）。$g$ 的（次）梯度有一个漂亮的形式：

\[ \nabla g(\boldsymbol{\lambda}) = \sum_i A_i\mathbf{X}_i^\star(\boldsymbol{\lambda}) - \mathbf{b} = \text{（当前价格下的）耦合约束残差} \]

其中 $\mathbf{X}_i^\star(\boldsymbol{\lambda})$ 是 agent $i$ 在价格 $\boldsymbol{\lambda}$ 下的本地最优解。于是价格按子梯度上升更新：

\[ \boxed{\ \boldsymbol{\lambda}^{k+1} = \boldsymbol{\lambda}^k + \alpha^k\Big(\sum_i A_i\mathbf{X}_i^\star(\boldsymbol{\lambda}^k)-\mathbf{b}\Big)\ } \]

$\alpha^k$ 是步长。这个更新的经济学含义完美对应市场：残差 $\sum_i A_i\mathbf{X}_i^\star-\mathbf{b}$ 就是"供需失衡"——大于 0（供过于求）则价格上调，小于 0（供不应求）则价格下调，直到残差为 0（市场出清，约束满足）。

完整的对偶分解算法（分布式）：

重复直到收敛：
  1. 广播当前价格 λ^k 给所有 agent（或在通信图上传播）
  2. 每个 agent i 并行解本地子问题：
        X_i* = argmin_{X_i ∈ C_i}  J_i(X_i) + (λ^k)^T A_i X_i
  3. 收集残差 r = Σ_i A_i X_i* - b（需要某种聚合：中央求和或图上共识）
  4. 价格更新：λ^{k+1} = λ^k + α^k · r

注意第 3 步的残差聚合 $\sum_i A_i\mathbf{X}_i^\star$ 需要把各 agent 的贡献加起来——这一步如何分布式实现（用第 2 章的**共识平均**而非中央求和），正是 §4.7 的话题，也是对偶分解/ADMM 与共识算法的接口。

反面：对偶分解为什么"慢"且"脆"——这正是 ADMM 的动机¶

对偶分解优雅，但有三个实际痛点，理解它们就理解了为什么工程上几乎都用 ADMM 而非纯对偶分解：

(1) 子梯度法收敛慢（$O(1/\sqrt{k})$ 或 $O(1/k)$）。 对偶函数 $g$ 一般不可微（在最优解附近有"折点"），只能用子梯度法，收敛速率慢，且需要精心调步长 $\alpha^k$（递减步长 $\sum\alpha^k=\infty,\sum(\alpha^k)^2<\infty$）。对需要每个控制周期（毫秒级）内收敛的 MPC，这个速度往往不够。

(2) 要求严格凸才有唯一本地解。 如果本地子问题 $J_i+\boldsymbol{\lambda}^\top A_i\mathbf{X}_i$ 不严格凸（如线性 $J_i$），$\mathbf{X}_i^\star(\boldsymbol{\lambda})$ 可能不唯一甚至"跳变"（价格微小变化导致最优解大幅跳动），对偶迭代剧烈震荡。

(3) 中间迭代不可行。对偶分解只在收敛时满足耦合约束；中间任何一轮的 $\{\mathbf{X}_i^\star\}$ **不满足**编队约束。如果 MPC 被迫提前停（时间到了），拿到的是一个违反队形的解。

不是 X 而是 Y：ADMM 不是"另一种全新的分解方法"，而是"对偶分解打了三个补丁"——(补丁1) 加增广二次项，让对偶函数变得可微、收敛加速到线性率；(补丁2) 增广项提供正则，即使本地不严格凸也能稳定；(补丁3) 引入辅助变量 $\mathbf{z}$ 解决增广项的不可分。换句话说，你必须先理解对偶分解的思想（价格协调）和它的三个弱点，才能理解 ADMM 的每一处设计都在精确地修复哪个弱点。这就是为什么本章要先讲对偶分解再讲 ADMM——跳过这一节直接学 ADMM，你只会背三步公式，不懂它为什么是这三步。

理论-工程桥接：尽管纯对偶分解工程上少用，但它的"价格"视角在多足编队里有直接的物理对应。回顾 §4.5 将要讲的：当两台四足通过刚性杆连接，杆里的**约束力 $\boldsymbol{\lambda}$**（拉格朗日乘子）就是对偶变量！它正是"维持刚性连接这个耦合约束"的价格——杆受拉就是正价格、受压就是负价格。所以对偶分解里抽象的"影子价格"，在多足编队里是一个你能用力传感器测到的真实的力。这个对应不是巧合：约束力在物理上就是"维持约束所需的力"，在数学上就是"维持约束的拉格朗日乘子"，二者是同一个 $\boldsymbol{\lambda}$。

走一遍：两 agent 的价格协调过程（手算 + 解读）¶

为了让"价格协调"从抽象变具体，把整个对偶迭代在一个最小例子上手算一遍。两个 agent，本地代价 $J_1(x_1)=\frac12(x_1-3)^2$、$J_2(x_2)=\frac12(x_2-1)^2$（agent 1 想去 3、agent 2 想去 1），耦合约束 $x_1=x_2$（要求一致，写成 $x_1-x_2=0$，即 $A_1=1,A_2=-1,b=0$）。无本地约束。

最优解（先用集中式对照）：$\min\frac12(x_1-3)^2+\frac12(x_2-1)^2$ s.t. $x_1=x_2$，令 $x_1=x_2=x$，对 $x$ 求导：$(x-3)+(x-1)=0\Rightarrow x=2$。所以联合最优是两 agent 都到 2（两愿望的中点）。

对偶分解过程：拉格朗日 $\mathcal{L}=\frac12(x_1-3)^2+\frac12(x_2-1)^2+\lambda(x_1-x_2)$。

本地子问题（给定价格 $\lambda$）：
agent 1：$\min_{x_1}\frac12(x_1-3)^2+\lambda x_1\Rightarrow x_1^\star(\lambda)=3-\lambda$（求导 $(x_1-3)+\lambda=0$）。
agent 2：$\min_{x_2}\frac12(x_2-1)^2-\lambda x_2\Rightarrow x_2^\star(\lambda)=1+\lambda$。
残差（次梯度）：$r(\lambda)=x_1^\star-x_2^\star=(3-\lambda)-(1+\lambda)=2-2\lambda$。
价格更新：$\lambda^{k+1}=\lambda^k+\alpha(2-2\lambda^k)$。

手算迭代（步长 $\alpha=0.25$，从 $\lambda^0=0$）：

$k$	$\lambda^k$	$x_1^\star=3-\lambda$	$x_2^\star=1+\lambda$	残差 $r=x_1-x_2$	解读
0	0.00	3.00	1.00	2.00	价格为 0，各去各的愿望，相差 2（违约最大）
1	0.50	2.50	1.50	1.00	价格上调，两者被拉近
2	0.75	2.25	1.75	0.50	继续拉近，残差减半
3	0.875	2.125	1.875	0.25	接近一致
…	→1.00	→2.00	→2.00	→0	收敛：价格 $\lambda^\star=1$，两 agent 都到 2

解读（价格协调的全过程）：

初始 $\lambda=0$（无价格）：耦合约束不起作用，agent 1 去 3、agent 2 去 1——这正是"完全去中心"的结果（§4.1 反面），相差 2。
价格逐步上调：残差 $r=2>0$（agent 1 在 agent 2 右边，违反一致），价格 $\lambda$ 上调。$\lambda>0$ 对 agent 1 是"惩罚 $x_1$ 太大"（$+\lambda x_1$），把它往左拉；对 agent 2 是"奖励 $x_2$ 增大"（$-\lambda x_2$），把它往右拉。一拉一推，两者靠拢。
收敛到 $\lambda^\star=1$：此时 $x_1=x_2=2$，残差 0，市场出清（约束满足）。$\lambda^\star=1$ 的物理含义：它是维持"$x_1=x_2$"这个约束所需的"价格"——也是这个约束的影子价格（最优值对约束右端 $b$ 的灵敏度）。

本质洞察：这张表把 §4.1 反面的"完全去中心"和本章的"分布式协调"放在了同一条迭代线的**两端**——$\lambda=0$（第 0 行）就是完全去中心（各去愿望、队形崩），$\lambda=\lambda^\star$（最后一行）就是协调到联合最优（队形成）。对偶迭代的本质，就是从"无协调"通过逐步调价走到"完美协调"的过程。 价格 $\lambda$ 是协调强度的旋钮：$\lambda=0$ 无协调，$\lambda$ 增大协调加强，$\lambda^\star$ 恰好协调。这让你直观看到"协调"在数学上到底是什么——不是一个模糊的概念，而是一个被价格驱动的、可量化的、逐步收敛的过程。

反事实：如果步长 $\alpha$ 取太大（如 $\alpha=1.5$）会怎样？$\lambda^{k+1}=\lambda^k+1.5(2-2\lambda^k)$，从 $\lambda=0$：$\lambda^1=3$，$x_1=0,x_2=4$，残差 $-4$（反向越过了，agent 1 跑到 agent 2 左边）；$\lambda^2=3+1.5(-4)\cdot... $ 震荡发散。这就是 §4.3 陷阱 4 的具体表现——步长过大导致价格"调过头"反复震荡。对照前面 $\alpha=0.25$ 的平稳收敛，你能亲手感受步长对子梯度法的敏感（也理解了为什么 ADMM 用固定 $\rho$ 却稳——增广项给了额外曲率，不靠精调步长）。

这个手算例子是 §4.3 练习 1-2 的"参考过程"。把它和 §4.4 练习 1 的 consensus-ADMM 同一问题对照，你会看到 ADMM 用增广项把这个"靠精调步长才稳"的过程变成了"固定 $\rho$ 就稳且更快"——那正是 ADMM 三个补丁的威力。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：松弛本地约束 $\mathbf{X}_i\in\mathcal{C}_i$。 - 错误描述：做拉格朗日松弛时，把所有约束（包括本地动力学、摩擦锥）都松弛掉。 - 现象/后果：本地子问题变成无约束最小化，解出的 $\mathbf{X}_i$ 违反动力学/摩擦锥，物理上无意义；且失去了"本地子问题保持单体 MPC 结构"的好处。 - 根本原因：对偶分解只需松弛**耦合**约束（把变量绑在一起的那些），本地约束应保留在各自子问题里。松弛本地约束既无必要（它本来就可分）又有害（丢失结构、解不可行）。 - 正确做法：只松弛耦合约束 $\sum_i A_i\mathbf{X}_i=\mathbf{b}$，本地约束 $\mathbf{X}_i\in\mathcal{C}_i$ 原样留在 agent $i$ 的子问题中。区分"耦合约束"与"本地约束"是对偶分解的第一要务。

陷阱 2（概念误区）：把对偶变量 $\boldsymbol{\lambda}$ 当成可以随便初始化、随便调的超参数。 - 错误描述：以为 $\boldsymbol{\lambda}$ 像学习率一样是个调参旋钮，初值无所谓。 - 现象/后果：$\boldsymbol{\lambda}$ 初值离最优价格太远时，对偶迭代要很多轮才收敛；在 MPC 里每周期冷启动 $\boldsymbol{\lambda}=0$ 会浪费大量迭代。 - 根本原因：$\boldsymbol{\lambda}$ 是有物理含义的（影子价格/约束力），它的最优值由问题决定，不是自由超参。但它**可以 warm-start**——相邻控制周期的最优价格变化不大。 - 正确做法：MPC 中用上一周期收敛的 $\boldsymbol{\lambda}$ 作为本周期初值（warm-start），大幅减少迭代轮数。这是把对偶/ADMM 用于实时 MPC 的关键工程技巧。

陷阱 3（思维陷阱）：用对偶分解时假设中间迭代的解可以直接执行。 - 错误描述：MPC 时间到了就拿当前轮的 $\{\mathbf{X}_i^\star\}$ 去执行，以为"差不多收敛了"。 - 现象/后果：中间解不满足耦合约束，机器人队形违反（如刚性约束未满足导致对负载施力冲突）。 - 根本原因：对偶分解只保证收敛时可行，中间迭代的原始变量不满足耦合约束（这是对偶法的固有性质，区别于原始可行方法）。 - 正确做法：要么跑到足够收敛（残差 < 阈值）再执行，要么改用对中间解更友好的方法（ADMM 的原始残差随轮数单调下降，中间解的约束违反更小、更可控）。这也是 ADMM 比纯对偶分解更适合实时 MPC 的原因之一。

陷阱 4（编程陷阱）：子梯度法步长 $\alpha^k$ 用固定常数。 - 错误描述：价格更新 $\boldsymbol{\lambda}^{k+1}=\boldsymbol{\lambda}^k+\alpha\cdot r$ 用固定 $\alpha$。 - 现象/后果：固定步长下子梯度法**不保证收敛到最优**，只能收敛到最优值附近的一个邻域内震荡。 - 根本原因：子梯度（区别于梯度）方向不一定是下降方向，固定步长无法消除震荡。理论要求递减步长 $\alpha^k\to0$ 且 $\sum\alpha^k=\infty$。 - 正确做法：用递减步长（如 $\alpha^k=a/(b+k)$）保证收敛；或干脆换 ADMM——ADMM 的"步长"就是固定的 $\rho$ 却能线性收敛，因为增广项提供了额外的曲率信息。

练习¶

（手推对偶分解） 考虑两个 agent 的简单耦合 QP：$\min \frac12 x_1^2+\frac12 x_2^2$ s.t. $x_1+x_2=2$（这里 $A_1=A_2=1,\ b=2$，无本地约束）。(a) 写出拉格朗日函数并验证它关于 $(x_1,x_2)$ 可分；(b) 解本地子问题得 $x_i^\star(\lambda)$；(c) 写出对偶函数 $g(\lambda)$ 并解析求最优 $\lambda^\star$；(d) 验证 $x_1^\star(\lambda^\star)=x_2^\star(\lambda^\star)=1$（最优解）。这道题让你完整走一遍对偶分解的机械流程。
（价格迭代仿真） 把上题用对偶子梯度法数值求解：从 $\lambda^0=0$ 开始，用步长 $\alpha^k=0.5/(1+k)$ 和固定步长 $\alpha=0.5$ 各跑 50 轮，画出 $\lambda^k$ 和残差 $r^k=x_1^\star+x_2^\star-2$ 的收敛曲线。对比两种步长：验证陷阱 4——固定步长在最优附近震荡不收敛，递减步长收敛。这道题让你亲眼看到子梯度法的慢和脆。
（影子价格的物理意义） 结合 §4.5 的预告思考：把上题的耦合约束 $x_1+x_2=2$ 想象成"两台机器人通过一根杆维持总位移为 2"。最优对偶变量 $\lambda^\star$ 对应杆里的什么物理量？如果把约束改成 $x_1+x_2=2.5$（杆变长），$\lambda^\star$ 会怎么变？用对偶变量的"灵敏度"含义（$\lambda^\star=\partial f^\star/\partial b$，最优值对约束右端的导数）解释。这道题把抽象的对偶变量和 §4.5 的物理约束力 $\boldsymbol{\lambda}$ 连起来。

§4.4 ADMM 分布式 MPC：又快又稳的分解 ⭐⭐⭐⭐¶

这一节解决什么问题：§4.3 给了对偶分解的思想和它的三个弱点（慢、脆、中间不可行）。这一节用 ADMM（交替方向乘子法）逐一修复它们，并把它落成多足分布式 MPC 的可执行算法。这是全章的**算法核心**——你会看到第 2 章 §2.3 学的 ADMM 三步迭代，在这里变成"每个 agent 解一个带二次惩罚的本地 MPC + 邻居预测的共识平均 + 对偶上升"。学完这一节，你应该能写出双四足 ADMM 编队的代码骨架。

动机：既要可分，又要快、稳、中间可用¶

把 §4.3 末尾的三个弱点摆在一起，ADMM 的设计目标就清楚了：

对偶分解的弱点	ADMM 的修复
子梯度法慢（$O(1/\sqrt k)$），需调步长	增广二次项让对偶可微，固定 $\rho$ 即可，凸情形线性收敛
本地不严格凸则震荡	增广项提供正则（强凸化本地子问题），稳定
中间迭代不满足耦合约束	原始残差单调下降，中间解约束违反更小、更可控

回顾第 2 章 §2.3：那里我们从增广拉格朗日推导了标准 ADMM 的 $x$/$z$/$y$ 三步，并讲了 $\rho$ 控制"本地自主 vs 全局一致"的权衡。本节做两件事：(1) 把那套推导**接到 §4.3 的耦合 MPC 上**，得到 consensus-ADMM 形式；(2) 把它**具体化到多足编队**，给出代码与 $\rho$ 调节的工程实践。

如果直接给联合 MPC 加增广项会怎样——回到"不可分"¶

§4.3 的反面已经指出：直接给耦合约束加二次罚 $\frac{\rho}{2}\|\sum_i A_i\mathbf{X}_i-\mathbf{b}\|^2$，展开后的交叉项 $\mathbf{X}_i^\top A_i^\top A_j\mathbf{X}_j$ 把 agent 又耦合回去。所以**不能直接加**。ADMM 的破局点是：引入一个辅助变量 $\mathbf{z}$，让每个 agent 的变量只和 $\mathbf{z}$ 比较，而不直接和别的 agent 比较——交叉项被 $\mathbf{z}$ "隔开"了。这就是 consensus-ADMM 的核心技巧，下面具体看。

历史：从 Glowinski/Gabay 到 Boyd 的 consensus-ADMM 复兴¶

ADMM 由 Glowinski-Marrocco 和 Gabay-Mercier 在 1970 年代提出，最初用于偏微分方程的数值解（变分不等式）。它沉寂了几十年，直到 2011 年 Boyd 等人的著名综述《Distributed Optimization and Statistical Learning via ADMM》把它重新带火——该文系统阐述了 consensus-ADMM（一致性 ADMM）：把"$N$ 个 agent 的变量要一致"建模为"每个 agent 有本地副本 + 一个全局共识变量 $\mathbf{z}$，约束本地副本 = $\mathbf{z}$"。这个形式天然适配分布式机器学习和多 agent 优化，成为此后十年分布式优化的主力工具。Summers-Lygeros、Conte 等人随后把它用于分布式 MPC，Hamed 团队把它用于多足协同。

本质洞察：consensus-ADMM 的核心创意是**"用一个虚拟的全局变量 $\mathbf{z}$ 当所有 agent 的'公共参照物'"**。直接让 $N$ 个 agent 两两一致，是 $O(N^2)$ 条耦合关系（稠密、不可分）；引入 $\mathbf{z}$ 后，每个 agent 只需和 $\mathbf{z}$ 一致，是 $N$ 条关系（星形、可分）。$\mathbf{z}$ 把"多边耦合"转化为"每个 agent 对中心的单边耦合"，而这个"中心" $\mathbf{z}$ 的更新恰好是一个**平均**操作——于是在分布式实现里，$\mathbf{z}$ 不需要真有一个中央节点，用第 2 章的共识平均就能算出来。这就是为什么 ADMM 协调步"本质是共识"。

理论：consensus-ADMM 形式的分布式 MPC¶

现在严格推导。我们用 consensus 形式重写编队 MPC。核心思想：为耦合的"接口变量"建立一个全局共识副本。

第一步：识别接口变量并引入共识副本。 编队耦合发生在机器人的某些"接口量"上——可能是相对位姿、共享负载状态、或边上的相对位移。把 agent $i$ 涉及耦合的那部分变量记为 $\mathbf{w}_i$（接口变量，如 agent $i$ 对共享负载状态的本地估计、或它在各条编队边上的位置贡献）。引入全局共识变量 $\mathbf{z}$，要求所有相关 agent 的接口副本与 $\mathbf{z}$ 一致：

\[ \min_{\{\mathbf{X}_i\},\mathbf{z}}\ \sum_i J_i(\mathbf{X}_i)\quad\text{s.t.}\ \mathbf{X}_i\in\mathcal{C}_i,\quad \underbrace{\mathbf{w}_i(\mathbf{X}_i)=\mathbf{z}_i\ (\forall i)}_{\text{一致性约束}} \]

这里 $\mathbf{z}_i$ 是 $\mathbf{z}$ 中与 agent $i$ 相关的分量（如果是全局共享负载，所有 $\mathbf{z}_i$ 指向同一个 $\mathbf{z}$；如果是边耦合，$\mathbf{z}_i$ 是边变量）。关键：现在每个 agent 的变量只通过"$=\mathbf{z}_i$"和全局变量绑定，agent 之间不再直接耦合。

第二步：增广拉格朗日。 对一致性约束 $\mathbf{w}_i(\mathbf{X}_i)=\mathbf{z}_i$ 引入对偶变量 $\mathbf{y}_i$ 和增广项：

\[ \mathcal{L}_\rho = \sum_i\Big[J_i(\mathbf{X}_i) + \mathbf{y}_i^\top(\mathbf{w}_i(\mathbf{X}_i)-\mathbf{z}_i) + \tfrac{\rho}{2}\|\mathbf{w}_i(\mathbf{X}_i)-\mathbf{z}_i\|^2\Big] \]

注意：增广项 $\|\mathbf{w}_i-\mathbf{z}_i\|^2$ 只含 agent $i$ 的变量和全局 $\mathbf{z}_i$，没有 agent 间的交叉项——这正是引入 $\mathbf{z}$ 的回报。

第三步：ADMM 三步交替。 ADMM 轮流优化 $\{\mathbf{X}_i\}$、$\mathbf{z}$、$\{\mathbf{y}_i\}$。用缩放对偶变量 $\mathbf{u}_i=\mathbf{y}_i/\rho$（缩放形式更简洁）：

(X-更新) 每个 agent 并行解本地 MPC（带二次惩罚）： $$ \boxed{ \mathbf{X}i^{k+1}=\arg\min $$ 这就是**单体 MPC 加一个把接口变量拉向共识值 }_i\in\mathcal{C}_i} J_i(\mathbf{X}_i)+\tfrac{\rho}{2}\big|\mathbf{w}_i(\mathbf{X}_i)-\mathbf{z}_i^k+\mathbf{u}_i^k\big|^2 $\mathbf{z}_i^k-\mathbf{u}_i^k$ 的二次惩罚**。规模是单体大小，可并行，且因为增广项是二次的，本地问题仍是 QP（凸 SRB MPC 情形）。

(Z-更新) 共识平均： $$ \boxed{ \mathbf{z}^{k+1}=\frac{1}{|\mathcal{I}z|}\sum $$ 全局共识变量更新为相关 agent 的接口副本（加对偶偏移）的**平均**——}_z}\big(\mathbf{w}_i(\mathbf{X}_i^{k+1})+\mathbf{u}_i^k\big) $\mathcal{I}_z$ 是与该 $\mathbf{z}$ 分量相关的 agent 集合。这一步就是第 2 章的共识平均！分布式实现时不需要中央节点，用共识迭代算这个平均（§4.7）。

(Y-更新) 对偶上升： $$ \boxed{ \mathbf{u}_i^{k+1}=\mathbf{u}_i^k+\mathbf{w}_i(\mathbf{X}_i^{k+1})-\mathbf{z}_i^{k+1} } $$ 对偶变量累积"本地副本与共识值的偏差"——和 §4.3 的价格更新同理，但这里因增广项的存在，收敛是线性的。

阶段小结：到这里我们得到了完整的 consensus-ADMM 分布式 MPC：X-更新（本地 MPC + 二次惩罚，并行）、Z-更新（共识平均，邻居协调）、Y-更新（对偶上升，逼迫一致）。把这三步和第 2 章 §2.3 的标准 ADMM 对照，你会发现**结构完全一样**，只是 $x\to\mathbf{X}_i$（轨迹）、$z\to\mathbf{z}$（共识接口）、惩罚项作用在接口变量 $\mathbf{w}_i$ 上。这就是"算法骨架不变、规模和物理含义变了"的含义。

把这套三步嵌进 MPC 的滚动框架，就是分布式 MPC 的完整循环：每个控制周期，各 agent 跑若干轮 ADMM（通常 warm-start + 固定 3-5 轮，而非跑到收敛），取第一步控制执行，滚动到下一周期。

代码：ADMM 分布式 MPC 协调器（为什么→正确→错误→对比）¶

下面把上面的三步落成代码。先讲为什么这样组织，再给正确实现，然后展示错误写法，最后对比同步/异步方案。

Step 1：先讲为什么这样写。

为什么 ADMM 协调器要把"本地 MPC 求解"和"共识/对偶更新"分成独立步骤？

因为这正对应分布式系统的两种操作：
  - 本地 MPC 求解 = 纯计算（CPU 密集），可在各 agent 上完全并行（#pragma omp / 多机并行）；
  - 共识平均 + 对偶更新 = 通信 + 轻量计算，需要邻居数据。

分开后：(a) 计算和通信的边界清晰，便于在真机上把"算"放本地、"传"走网络；
       (b) 可以独立优化各步——本地 MPC 用 warm-start 加速，通信步用第 2 章的共识压缩轮数；
       (c) 固定迭代轮数（不跑到收敛）成为可能——每轮就是一次"算+传"，时间可预算。

规则：X-更新（本地、并行、可 warm-start）；Z-更新（通信、共识平均）；Y-更新（本地、对偶累积）。
     实时 MPC 中三步循环固定 K 轮（K=3~5），用上周期解 warm-start，而非等收敛。

Step 2：给出正确写法。

// distributed_mpc/admm_coordinator.hpp
// consensus-ADMM 分布式 MPC 协调器（接口变量为全局共享量，如负载状态）
template<int N_AGENTS, int STATE_DIM, int CTRL_DIM>
class ADMMCoordinator {
public:
  using State = Eigen::Matrix<double, STATE_DIM, 1>;
  using Trajectory = std::vector<State>;   // 时域 T 上的状态轨迹

  struct AgentData {
    Trajectory w_local;     // 本地接口变量副本 w_i（本地 MPC 解出的接口量）
    Trajectory z_consensus; // ADMM 全局一致变量 z（缩放形式下各 agent 持有副本）
    Trajectory u_dual;      // ADMM 缩放对偶变量 u_i = y_i / rho
  };

  // 跑 K 轮 ADMM（实时 MPC 中 K 固定，如 3~5；不跑到收敛）
  void run(std::array<AgentData, N_AGENTS>& agents, double rho, int K) {
    for (int k = 0; k < K; ++k) {
      // === X-更新：每个 agent 并行解本地 MPC（带二次惩罚）===
      #pragma omp parallel for
      for (int i = 0; i < N_AGENTS; ++i) {
        // 惩罚把接口变量拉向 (z_i^k - u_i^k)
        // min J_i(X_i) + (rho/2)||w_i(X_i) - z_i^k + u_i^k||^2
        // 解出后把接口量写回 agents[i].w_local（warm-start 用上一轮解初始化）
        solve_local_mpc(i, agents[i], rho);
      }

      // === Z-更新：共识平均（分布式实现见 §4.7，这里写聚合形式）===
      // z^{k+1} = average over related agents of (w_i + u_i)
      for (int t = 0; t < horizon_; ++t) {
        State avg = State::Zero();
        for (int i = 0; i < N_AGENTS; ++i)
          avg += agents[i].w_local[t] + agents[i].u_dual[t];
        avg /= N_AGENTS;
        for (int i = 0; i < N_AGENTS; ++i)
          agents[i].z_consensus[t] = avg;   // 全局共享：所有 agent 同一个 z
      }

      // === Y-更新：对偶上升 u_i += w_i - z ===
      for (int i = 0; i < N_AGENTS; ++i)
        for (int t = 0; t < horizon_; ++t)
          agents[i].u_dual[t] += agents[i].w_local[t]
                               - agents[i].z_consensus[t];

      // === 残差监控（用于自适应 rho / 提前停）===
      auto [r_norm, s_norm] = compute_residuals(agents, rho);
      if (r_norm < eps_pri_ && s_norm < eps_dual_) break;  // 收敛则提前停
    }
  }

private:
  void solve_local_mpc(int i, AgentData& d, double rho);  // 调 OSQP/OCS2
  std::pair<double,double> compute_residuals(
      const std::array<AgentData,N_AGENTS>&, double rho);
  int horizon_;
  double eps_pri_ = 1e-3, eps_dual_ = 1e-3;
  std::array<std::vector<int>, N_AGENTS> neighbors_;
};

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

// ❌ 错误 1：Z-更新里漏掉对偶项 u_i，只平均 w_i
for (int t = 0; t < horizon_; ++t) {
  State avg = State::Zero();
  for (int i = 0; i < N_AGENTS; ++i) avg += agents[i].w_local[t]; // 漏了 + u_dual
  avg /= N_AGENTS;
  ...
}
// 问题：consensus-ADMM 的 z-更新是 average(w_i + u_i)，不是 average(w_i)。
//      漏掉 u_i 破坏了 ADMM 的对偶配平，导致收敛到错误的一致点或不收敛。
//      现象：残差降不到 0，编队有稳态偏差。极难发现，因为"看起来在平均"。

// ❌ 错误 2：每个控制周期冷启动（u 和 z 清零）
void on_new_control_cycle(std::array<AgentData,N_AGENTS>& agents) {
  for (auto& a : agents) { zero(a.u_dual); zero(a.z_consensus); } // 冷启动
  run(agents, rho_, K_);
}
// 问题：每周期从零开始，ADMM 要很多轮才收敛，固定 K=3~5 根本不够。
//      现象：编队跟踪滞后、抖动。相邻周期的最优解其实很接近，丢弃前一解是浪费。
//      正确：warm-start——保留上周期的 u、z（和本地 warm-start），K=3~5 即可够用。

// ❌ 错误 3：rho 取固定且过大，期望"收敛更快"
double rho_ = 1e4;  // 以为越大越快
// 问题：rho 过大 → 二次惩罚主导，本地 MPC 几乎只顾"贴近 z"而忽略自身代价 J_i，
//      原始残差掉得快但对偶残差大、解质量差、甚至本地 QP 病态。
//      现象：编队迅速"拉拢"但每台机器人运动僵硬、自身代价很差。
//      正确：rho 适中（如 1~100，问题相关），并用残差平衡自适应调节（见下文）。

Step 4：给出不同实现方式的对比。

// === 三种 ADMM 调度方式对比 ===

// 方式 A：同步 ADMM（所有 agent 每轮严格对齐）
//   X-更新全部完成 → Z-更新 → Y-更新 → 下一轮
//   优点：等价于标准 ADMM，收敛性有保证（凸情形）
//   缺点：被最慢的 agent 拖累（木桶效应）；要求全局同步时钟

// 方式 B：异步 ADMM（agent 算完就更新，不等别人）
//   各 agent 用收到的最新邻居数据立即更新
//   优点：无需全局同步，快 agent 不被慢 agent 拖累
//   缺点：收敛性分析更难，需额外条件（有界延迟）；可能震荡

// 方式 C：固定轮数 + warm-start（实时 MPC 标准做法）
//   每控制周期跑固定 K=3~5 轮同步 ADMM，用上周期解 warm-start
//   优点：每周期时间可预算（硬实时友好）；warm-start 让少轮够用
//   缺点：不保证收敛到最优（有 gap），但 gap 小且队形误差可控

// 对比：
// | 方式 | 收敛保证 | 实时性 | 同步要求 | 适用 |
// |------|---------|--------|---------|------|
// | A    | ✅ 凸   | 一般   | 全局时钟 | 仿真/有同步 |
// | B    | 需条件  | 好     | 无       | 大规模/异构 |
// | C    | gap     | ✅ 硬实时 | 周期内同步 | 真机多足 MPC（推荐）|

$\rho$ 调节与收敛性：理论与残差平衡¶

ADMM 的收敛速度对 $\rho$ 敏感。给出三条要点：

(1) 凸情形的收敛率。 对凸问题，ADMM 的目标值与约束违反以 $O(1/k)$ 收敛；若 $f$ 或 $g$ 之一强凸且光滑，可达 $O(1/k^2)$，甚至在适当条件下线性收敛。这比对偶子梯度法的 $O(1/\sqrt k)$ 快得多——这是 ADMM 优于纯对偶分解的关键定量优势。

(2) 残差与停止准则。 定义两个残差： - 原始残差 $r^k=\mathbf{w}_i(\mathbf{X}_i^k)-\mathbf{z}_i^k$（一致性违反，衡量"本地副本与共识差多少"）； - 对偶残差 $s^k=\rho(\mathbf{z}^k-\mathbf{z}^{k-1})$（共识变量的变化，衡量"还在不在动"）。当 $\|r^k\|$ 和 $\|s^k\|$ 都小于阈值时停止。$\rho$ 的作用直观：$\rho$ 大 → 重罚一致性违反 → $r^k$ 降得快但 $s^k$ 可能大；$\rho$ 小 → 反之。

(3) 残差平衡（residual balancing）自适应 $\rho$。 Boyd 综述给的经典自适应规则——让两个残差量级相当： $$ \rho^{k+1}=\begin{cases}\tau^{\text{incr}}\rho^k & \text{if } |r^k|>\nu|s^k| (\text{原始残差太大，加大 }\rho)\ \rho^k/\tau^{\text{decr}} & \text{if } |s^k|>\nu|r^k| (\text{对偶残差太大，减小 }\rho)\ \rho^k & \text{otherwise}\end{cases} $$ 典型 $\nu=10,\ \tau=2$。直觉：哪个残差大就调 $\rho$ 去压它，使两者同步下降。更先进的 AADMM 用对偶曲率估计自适应，收敛更快。

(4) 非凸的警告。 多足 MPC 的接触约束（摩擦锥金字塔是凸的，但单边接触 + 踏步时序切换、姿态 $SO(3)$ 是非凸的）使整个问题**非凸**。对非凸问题，ADMM 没有收敛保证——可能震荡或卡在局部点。工程上靠：(a) 用凸化的 SRB MPC（固定接触序列后摩擦锥金字塔使本地 QP 凸）；(b) warm-start 提供好初值；(c) 固定迭代轮数"够用就停"。这是 §4.7 和科研前沿（CBF-ADMM 等）仍在攻关的开放问题。

理论-工程桥接：为什么 Kim-Fawcett-Hamed 的分布式 MPC 实测只比集中式慢一点点（各 agent ~6ms，集中式 ~15ms）且 gap < 3%？正是因为他们用了"方式 C"——固定少轮 ADMM + warm-start + 在 SRB 这个降阶模型上凸化。降阶让本地 QP 小（~480 变量，单体规模），warm-start 让 3 轮够用，凸化让每轮的本地 QP 稳定可解。把这三个工程决策去掉任何一个，分布式 MPC 都会在真机上失败。理论（ADMM 三步）只是骨架，这些工程决策才是让它在毫秒级控制循环里真正跑起来的血肉。

类比（像 X 不像 Y）：ADMM 的 X/Z/Y 三步像**一个反复磨合的团队协作**。X-更新像"每个人先按自己理解 + 团队当前共识做一版方案"（本地优化 + 向共识靠拢）；Z-更新像"把大家的方案汇总取个折中作为新共识"（平均）；Y-更新像"记录每个人这次离共识差多远，下次开会时这个'欠账'要补上"（对偶累积）。**像**的地方：通过反复"各做各的 → 汇总 → 记账"逼近一致。**不像**的地方：真实团队靠人的判断，ADMM 每步都是严格的数学最优化，且 Y 步的"记账"（对偶变量）有精确的收敛理论保证它最终拉平——这是人治团队没有的数学保证。还有一个边界：这个类比在**非凸**时失效——团队磨合通常能达成共识，但非凸 ADMM 可能永远磨不拢（震荡）。

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：Z-更新漏掉对偶项 $\mathbf{u}_i$。 - 错误描述：共识平均写成 $\mathbf{z}=\text{avg}(\mathbf{w}_i)$ 而非 $\text{avg}(\mathbf{w}_i+\mathbf{u}_i)$。 - 现象/后果：残差降不到 0，编队有稳态偏差；"看起来在平均"所以极难发现。 - 根本原因：consensus-ADMM 的 z-更新必须含对偶偏移，否则破坏对偶配平，收敛到错误一致点。 - 正确做法：严格按公式 $\mathbf{z}^{k+1}=\text{avg}(\mathbf{w}_i^{k+1}+\mathbf{u}_i^k)$。写完用一个标量 toy 例子（练习 1）验证收敛到正确值再上规模。

陷阱 2（思维陷阱）：每控制周期冷启动 ADMM。 - 错误描述：每周期把 $\mathbf{z},\mathbf{u}$ 清零重新跑。 - 现象/后果：固定少轮根本不够收敛，编队跟踪滞后、抖动。 - 根本原因：相邻控制周期的最优解高度相似，冷启动丢弃了这个先验，浪费迭代。 - 正确做法：warm-start——保留上周期的 $\mathbf{z},\mathbf{u}$ 和本地解作为初值。这是实时 ADMM-MPC 的命脉，没有它固定轮数方案不成立。

陷阱 3（概念误区）：$\rho$ 越大收敛越快。 - 错误描述：把 $\rho$ 调到很大期望加速。 - 现象/后果：原始残差掉得快但对偶残差大、本地 QP 病态、机器人运动僵硬、解质量差。 - 根本原因：$\rho$ 过大让二次惩罚淹没本地代价 $J_i$，agent 只顾贴近共识而不顾自身最优；$\rho$ 还影响本地 QP 的条件数。 - 正确做法：$\rho$ 适中 + 残差平衡自适应。把 $\rho$ 理解为"全局一致 vs 本地自主"的权重旋钮（呼应第 2 章），而非"速度旋钮"。

陷阱 4（概念误区）：以为 ADMM 在非凸接触约束下也保证收敛。 - 错误描述：把凸 ADMM 的收敛保证直接套到含单边接触/姿态非凸的多足 MPC。 - 现象/后果：某些步态切换或大姿态变化时 ADMM 震荡或发散，却以为是代码 bug。 - 根本原因：ADMM 的收敛理论建立在凸性上；多足接触约束非凸，理论保证失效。 - 正确做法：凸化（固定接触序列后用摩擦锥金字塔）+ warm-start + 固定轮数"够用就停"，并接受有 gap。把非凸 ADMM 当"启发式但实践有效"的工具，不依赖它的理论收敛。

陷阱 5（编程陷阱）：用单一固定 $\rho$ 且不监控残差。 - 错误描述：设个 $\rho$ 就不管了，也不算 $r^k,s^k$。 - 现象/后果：无法判断是否收敛、无法触发提前停、无法自适应调 $\rho$，调参全靠盲试。 - 根本原因：缺少残差这个"仪表盘"，ADMM 的运行状态不可观测。 - 正确做法：每轮计算原始/对偶残差，用于停止准则、自适应 $\rho$、和调试（残差曲线一眼看出收敛健康度）。

练习¶

（实现 + 验证 consensus-ADMM） 用 Python 实现两 agent 共识：$\min \frac12(x_1-1)^2+\frac12(x_2-3)^2$ s.t. $x_1=x_2$（要求两 agent 一致）。写出 consensus-ADMM 三步（接口变量 $w_i=x_i$，共识 $z$，对偶 $u_i$），$\rho=1$ 跑 30 轮，验证收敛到 $x_1=x_2=2$（两目标的折中）。然后**故意漏掉 z-更新的 $u_i$ 项**（陷阱 1），观察收敛到错误值——亲手制造并诊断这个最隐蔽的 bug。
（$\rho$ 扫描与残差平衡） 在上题基础上，对 $\rho\in\{0.1,1,10,100\}$ 各跑 ADMM，画出原始残差 $r^k$ 和对偶残差 $s^k$ 的收敛曲线。观察：小 $\rho$ 原始残差降得慢，大 $\rho$ 对偶残差大。再实现残差平衡自适应 $\rho$（$\nu=10,\tau=2$），对比固定 $\rho$ 的收敛轮数。这道题让你对 $\rho$ 的作用有定量体感。
（双积分器 ADMM 编队，承接 §4.1/§4.2 练习） 把 §4.2 的 $N=3$ 双积分器一字队形 MPC 用 consensus-ADMM 分布式求解：接口变量取相邻 agent 在编队边上的相对位移。实现"方式 C"（固定 $K=5$ 轮 + warm-start）。对比集中式联合 MPC（一次性解全联合 QP）：(a) 队形跟踪误差；(b) 单周期计算时间（分布式取 max over agents）；(c) ADMM 轮数对 gap 的影响（$K=1,3,5,10$）。这道题是累积项目的核心模块，把本章前四节串起来。
（跨章综合题） 结合第 2 章（共识收敛率由 $\lambda_2$ 决定）+ 第 1 章（Laplacian）+ 本节（ADMM 的 Z-更新是共识平均）：在一字拓扑（链 $1\!-\!2\!-\!3\!-\!4\!-\!5$）和星形拓扑（中心连四个）下，分别估算 ADMM 的 Z-更新若用分布式共识迭代实现，需要多少轮共识才能算准平均（提示：链的直径 4、$\lambda_2$ 小；星的直径 2、$\lambda_2$ 大）。说明：为什么通信拓扑既影响 ADMM 外层迭代轮数，又影响每轮 Z-更新内层共识轮数？这道题预告 §4.7，并把三章的图论-共识-ADMM 完全打通。

§4.5 SRB 网络与多足编队：CoM 协调 + 接触约束 + 耦合力 ⭐⭐⭐⭐¶

这一节解决什么问题：前四节的分布式 MPC 框架（编队建模 + ADMM 分解）是通用的——双积分器、轮式车、无人机都能用。这一节把它**落到多足机器人特有的难点**：每台机器人不是一个点，而是一个会踏步、有质心起伏、每条腿要满足摩擦锥和单边接触的复杂系统；当它们共享负载时，还会产生一个凭空多出来的"约束力"。这是多足编队区别于一般多机编队的核心，也是本章标题"多足"二字的落脚点。是建模的硬核，必须跟着推一遍。

动机：把"会走路的机器人"装进分布式 MPC¶

回顾单体四足 MPC（前置）：一台四足被近似成单刚体（SRB），状态 12 维 $[\mathbf{p},\boldsymbol{\Theta},\dot{\mathbf{p}},\boldsymbol{\omega}]$，控制是四条腿的接触力 $\mathbf{f}_{i1},\dots,\mathbf{f}_{i4}$，动力学是牛顿-欧拉方程，约束是摩擦锥 + 单边接触 + 踏步时序。

现在把 $N$ 台这样的四足放进编队。最直接的想法是：每台机器人的本地 MPC 就是它的单体 SRB MPC，编队耦合（§4.2）加在质心上，用 ADMM（§4.4）协调。对"编队只靠相对位置耦合"的场景（如两台机器人保持间距并排走，不共享负载），这确实够了——本地 SRB MPC + 质心 displacement 约束 + ADMM，直接组装。

但一旦机器人**共享一个物理负载**（一起抬一根杆、一块板、一个箱子），事情变复杂：它们通过负载**互相施力**。agent $i$ 推负载，负载就推 agent $j$——这个力既不是 agent 自己的接触力（来自地面），也不是重力，而是**通过刚性连接传递的约束力**。它必须进入每台机器人的 SRB 动力学，否则动力学方程从根上就是错的（陷阱见 §4.1 场景三）。这就是 SRB 网络**模型要解决的：不是 $N$ 个孤立的 SRB，而是通过约束力耦合的**互联 SRB 网络（interconnected SRB network）——Kim-Fawcett-Hamed T-RO 2023 的核心建模。

如果忽略耦合力直接堆 $N$ 个单体 SRB 会怎样¶

假设建模时偷懒，把双四足抬杆当成两个独立的单体 SRB，各自动力学只含自己的接触力和重力，编队约束加在质心上。会出什么问题？

动力学方程缺项，物理错误。负载通过杆对每台机器人施加的力没出现在牛顿-欧拉方程里。MPC 预测的质心运动会偏离真实——因为它没算上杆传来的力。预测错，控制就错。
负载的运动无人负责。负载自身也是一个刚体，有自己的动力学（受两台机器人施的力 + 重力）。不建模负载，就无法保证负载被托住、被搬到目标。
内力失控。两台机器人对负载施的力如果不协调，会产生**内力**（互相对抗、挤压或拉扯负载）——轻则浪费能量，重则把负载弄坏或把机器人自己拽倒。这正是第 5 章（协同搬运）的核心，而它的根在本节的耦合力建模。

不是 X 而是 Y：多足编队的 SRB 网络不是"$N$ 个单体 SRB 的简单并列"，而是"$N$ 个单体 SRB + 共享负载 + 把它们绑在一起的刚性耦合约束（及其产生的约束力 $\boldsymbol{\lambda}$）"。少了耦合约束和约束力，就退化成 §4.1 批判过的"完全去中心"，物理上还不自洽。

历史：从单体 SRB 到互联 SRB 网络¶

单刚体（SRB）模型在足式机器人里的流行，源于 MIT Cheetah（Di Carlo, Wensing, Kim 等 2018）——他们发现对腿轻的四足，把全身近似成单刚体、用凸 MPC 优化接触力，既简单又准确，成为四足 MPC 的标准模板。SRB 之所以好，是因为它**保留了接触力与质心运动的本质耦合，又避免了全身高维动力学的复杂**。

把单体 SRB 推广到多机协同，关键一步是 Kim-Hamed 等人（ACC 2022 监督式预测控制 → T-RO 2023 分层控制）提出的**互联 SRB 网络**：每个 agent 一个 SRB，通过共享负载的刚性约束（holonomic constraint，完整约束）耦合，约束产生拉格朗日乘子（约束力）。这个建模的精妙在于：它让"多足协同"既保持了单体 SRB 的简洁（每个 agent 仍是 12 维 SRB），又显式刻画了协同的物理本质（约束力耦合）——从而可以直接套用 §4.4 的 ADMM 分解（约束力就是要在 agent 间达成一致的接口变量）。

本质洞察：互联 SRB 网络把"多足协同"这个看似复杂的问题，归约成了一个结构清晰的对象——$N$ 个简单子系统（SRB） + 把它们缝合的约束（产生约束力 $\boldsymbol{\lambda}$）。这个"子系统 + 缝合约束"的结构，正是 §4.3-§4.4 分布式优化最擅长处理的形式：子系统 → 本地子问题，缝合约束 → 要协调的耦合，约束力 $\boldsymbol{\lambda}$ → 对偶变量/接口变量。所以建模选择 SRB 网络不是偶然——它是为了让"多足协同"恰好落进"分布式优化"的标准框架里。建模和求解在这里是共生设计的。

理论：互联 SRB 网络的动力学¶

现在严格写出 SRB 网络模型。先单个 agent，再耦合。

(1) 单 agent SRB 动力学。 agent $i$ 质心位置 $\mathbf{p}_i\in\mathbb{R}^3$、姿态 $R_i\in SO(3)$（或欧拉角 $\boldsymbol{\Theta}_i$）、质量 $m_i$、机体惯量 $I_i$。它受：四条腿的地面接触力 $\mathbf{f}_{ij}$（$j=1\dots4$，作用点 $\mathbf{r}_{ij}$ 相对质心）、负载施加的耦合力 $\boldsymbol{\lambda}_i$（作用点为附着点 $\mathbf{p}_i^{\text{att}}$）、重力。牛顿-欧拉方程：

\[ m_i\ddot{\mathbf{p}}_i = \sum_{j=1}^{4}\mathbf{f}_{ij} + \boldsymbol{\lambda}_i + m_i\mathbf{g} $$ $$ \tfrac{d}{dt}(I_i\boldsymbol{\omega}_i) = \sum_{j=1}^{4}\mathbf{r}_{ij}\times\mathbf{f}_{ij} + (\mathbf{p}_i^{\text{att}}-\mathbf{p}_i)\times\boldsymbol{\lambda}_i \]

与单体 SRB 唯一的区别就是多了 $\boldsymbol{\lambda}_i$ 项（耦合力及其力矩）。这一项就是协同的全部物理内容。

(2) 共享负载动力学。 负载（质量 $m_L$、惯量 $I_L$、质心 $\mathbf{p}_L$）受各 agent 通过附着点施加的反作用力 $-\boldsymbol{\lambda}_i$（牛顿第三定律）+ 重力：

\[ m_L\ddot{\mathbf{p}}_L = -\sum_{i=1}^{N}\boldsymbol{\lambda}_i + m_L\mathbf{g},\qquad \tfrac{d}{dt}(I_L\boldsymbol{\omega}_L)=-\sum_i(\mathbf{p}_i^{\text{att}}-\mathbf{p}_L)\times\boldsymbol{\lambda}_i \]

(3) 刚性耦合约束（holonomic constraint）。 "刚性连接"意味着 agent $i$ 的附着点在负载坐标系里的位置固定。等价地，附着点的绝对位置由负载位姿决定：

\[ \mathbf{p}_i^{\text{att}} = \mathbf{p}_L + R_L\,\mathbf{a}_i \quad(\mathbf{a}_i=\text{附着点在负载系的固定坐标}) \]

或写成两个 agent 间的相对形式（如 Kim-Fawcett-Hamed 的 holonomic constraint）：附着点相对位置固定 $\mathbf{p}_i^{\text{att}}-\mathbf{p}_j^{\text{att}}=\mathbf{d}_{ij}^{\text{fixed}}$。

(4) 约束力 $\boldsymbol{\lambda}$ 从哪来——把约束微分两次。 这是初学者最困惑的地方：$\boldsymbol{\lambda}_i$ 不是独立给定的，它是**维持约束所需的力**，由约束方程解出。机理（拉格朗日乘子法的标准流程）：把位置层约束 $\Phi(\mathbf{q})=0$（$\mathbf{q}$ 为广义坐标）对时间求两次导，得到加速度层约束：

\[ \Phi(\mathbf{q})=0 \xrightarrow{\frac{d}{dt}} J_\Phi\dot{\mathbf{q}}=0 \xrightarrow{\frac{d}{dt}} J_\Phi\ddot{\mathbf{q}}+\dot{J}_\Phi\dot{\mathbf{q}}=0 \]

其中 $J_\Phi=\partial\Phi/\partial\mathbf{q}$ 是约束 Jacobian。约束力沿约束法向，写成 $J_\Phi^\top\boldsymbol{\lambda}$。把它代入各刚体的动力学 $M\ddot{\mathbf{q}}=\mathbf{Q}+J_\Phi^\top\boldsymbol{\lambda}$，与加速度层约束联立，解出：

\[ \boxed{\ \boldsymbol{\lambda}=-(J_\Phi M^{-1}J_\Phi^\top)^{-1}\big(J_\Phi M^{-1}\mathbf{Q}+\dot{J}_\Phi\dot{\mathbf{q}}\big)\ } \]

这就是约束力的解析表达式。物理含义：$\boldsymbol{\lambda}$ 恰好是让所有刚体的加速度满足"附着点保持刚性连接"所需的那个力。它正是 §4.3 预告的"影子价格"在物理上的化身——维持耦合约束的拉格朗日乘子，等于约束力。

阶段小结：到这里我们建立了完整的 SRB 网络：(1) 每个 agent 的 SRB 动力学（多了 $\boldsymbol{\lambda}_i$ 项）；(2) 负载动力学；(3) 刚性耦合约束；(4) 约束力 $\boldsymbol{\lambda}$ 的解析解。接下来把它装进分布式 MPC——每个 agent 的本地 MPC 含自己的 SRB + 接触约束，$\boldsymbol{\lambda}_i$ 是要在 agent 间协调的接口变量。

接触约束：摩擦锥、单边接触、踏步时序¶

多足 MPC 的本地约束（每个 agent 自己的 $\mathcal{C}_i$）比双积分器复杂得多，三类必须进 MPC：

(1) 摩擦锥（friction cone）。 接触力不能让脚打滑：切向力 $\le$ 法向力 $\times$ 摩擦系数 $\mu$。精确形式是二阶锥 $\sqrt{f_x^2+f_y^2}\le\mu f_z$（SOCP，慢）。MPC 里几乎都用**金字塔近似**（4 个线性不等式，QP 友好）：

\[ -\mu f_z\le f_x\le\mu f_z,\quad -\mu f_z\le f_y\le\mu f_z \quad(\text{或更精确的 }\sqrt2\text{ 缩放}) \]

回顾前置自测第 3 题：金字塔近似把 SOCP 降为 QP，让 OSQP 这类 QP 求解器能在毫秒内解完——这是多足 MPC 实时性的前提。

(2) 单边接触（unilateral contact）。 脚只能"推"地不能"拉"地：支撑腿法向力 $f_z\ge0$；摆动腿（不接触）力为零 $\mathbf{f}_{ij}=\mathbf{0}$。

(3) 踏步时序（contact schedule / gait）。 哪条腿在哪个时刻支撑、哪条摆动，由步态调度器给定（§4.6 中层）。MPC 在预测时域上**按给定的接触序列**施加上面的约束——支撑相用摩擦锥 + $f_z\ge0$，摆动相强制 $\mathbf{f}=\mathbf{0}$。关键：固定接触序列后，本地 MPC 是凸 QP（这是 §4.4 凸化的来源）；如果让 MPC 自己决定接触序列（接触时机优化），问题变成混合整数/非凸，难解得多。

把这些装进 agent $i$ 的本地 MPC（即 §4.4 X-更新里的 $\min_{\mathbf{X}_i\in\mathcal{C}_i}$）：

agent i 本地 MPC（凸 QP，固定接触序列）：
  min_{X_i, f_i, λ_i}  Σ_t [ ||x_i(t) - x_i^ref(t)||²_Q + ||f_i(t)||²_R ]
                       + (ρ/2)||λ_i - z_λ + u_i||²      ← ADMM 惩罚（λ 是接口变量）
  s.t.  SRB 动力学：m_i p̈_i = Σ_j f_ij + λ_i + m_i g
                    d/dt(I_i ω_i) = Σ_j r_ij × f_ij + (p_i^att - p_i) × λ_i
        摩擦锥金字塔：|f_ij,x| ≤ μ f_ij,z,  |f_ij,y| ≤ μ f_ij,z   (支撑腿)
        单边接触：f_ij,z ≥ 0 (支撑),  f_ij = 0 (摆动)
        踏步时序：按 contact_schedule(t) 施加上述约束

注意 ADMM 惩罚项作用在**耦合力 $\boldsymbol{\lambda}_i$** 上——这就是多足编队里的"接口变量"：两台机器人对负载施的力必须协调一致（满足负载动力学与刚性约束），$\boldsymbol{\lambda}$ 就是要在 ADMM 协调步达成一致的量。这把 §4.4 抽象的 $\mathbf{w}_i$ 具体化为物理的约束力。

CoM 协调：踏步起伏与编队的频段分离（呼应 §4.2）¶

§4.2 末尾的多足特有点在这里展开。SRB 网络的状态用质心，编队/协同约束作用在质心层面。但四足质心随踏步周期性起伏摆动——如果硬性协调瞬时质心，会和踏步动力学打架。SRB 网络如何处理？

答案是分层 + 降阶天然实现的频段分离：

SRB MPC 工作在**降阶模型**层面，优化的是质心的**低频趋势**（往哪走、整体协调），预测时域 0.3-0.5 秒覆盖几个踏步周期，自然平滑掉单步细节；
高频的踏步起伏、足端轨迹由**底层 WBC**（§4.6）处理；
耦合力 $\boldsymbol{\lambda}$ 在 SRB 层协调——两台机器人保证对负载的**净力/净力矩**协调（托住负载、搬向目标），而各自如何踏步是本地自由。

理论-工程桥接：这就是为什么 Kim-Fawcett-Hamed 用"SRB 网络（高层 MPC）+ 全阶非线性控制器（低层）"的分层——SRB 网络在低频协调质心与耦合力（群体行为），全阶控制器在高频跟踪足端与平衡（个体行为）。把协同放在降阶层、把平衡放在全阶层，本质是**按频率分解控制任务**：群体协调慢（编队、负载），个体平衡快（踏步、姿态）。这个频段分离不是工程技巧，而是多足协同得以分布式实现的结构性原因——慢的群体行为信息量小、可低频通信协调（ADMM 几轮够用），快的个体行为留本地（WBC 无需通信）。

类比（像 X 不像 Y）：SRB 网络协调多足，像**一队人合抬一张大桌子走路**。每个人主要管自己走稳、迈步（本地 SRB + 踏步=个体高频平衡）；同时大家要让桌子保持水平、一起朝门口移动（耦合力 $\boldsymbol{\lambda}$ 协调=群体低频协同）。**像**的地方：个体动作（迈步）和群体目标（搬桌子）在不同层面，各人迈步节奏不必一致，只要对桌子的合力协调。**不像**的地方：人靠手感和默契协调桌子受力，SRB 网络靠 ADMM 严格优化耦合力 $\boldsymbol{\lambda}$ 并保证满足负载动力学；而且人能实时感知桌子晃动即时调整，SRB-MPC 是预测式的（提前规划未来 0.4 秒）。边界：这个类比对"刚性连接"成立，若是绳索（柔性、只能拉不能推），耦合约束就不是刚性 holonomic 而是单边的，建模要变（见前沿 PACC 被动臂方案）。

深入：质心动力学的离散化与 MPC 预测方程¶

上面给的 SRB 网络是连续时间的微分方程。MPC 是离散优化，必须把它**离散化**成预测方程 $\mathbf{x}_i(t{+}1)=A_i\mathbf{x}_i(t)+B_i\mathbf{u}_i(t)+B_i^\lambda\boldsymbol{\lambda}_i(t)+\mathbf{c}_i$。这一步有几个多足特有的微妙处，初学者常踩坑，值得展开。

(1) 姿态的处理：欧拉角线性化 vs $SO(3)$。 SRB 的姿态在旋转群 $SO(3)$ 上，是非线性流形。两种处理：

欧拉角 + 小角度线性化（MIT Cheetah 凸 MPC 的做法）：假设机体姿态变化不大，把旋转动力学在当前姿态附近线性化，姿态用欧拉角（roll-pitch-yaw）表示。优点：得到**线性时变（LTV）**预测方程，问题是凸 QP。缺点：大姿态变化（如爬陡坡、剧烈机动）时线性化误差大。
$SO(3)$ 上的几何积分（De Vincenti-Coros RSS 2023 用四元数 + 李群积分）：在流形上精确积分，不做小角度近似。优点：大姿态精确。缺点：问题变非凸（SQP 求解），慢。

理论-工程桥接：为什么多足编队 MPC 大多选欧拉角线性化而非 $SO(3)$ 精确？因为编队/搬运任务中机体姿态通常变化平缓（平地搬运、保持负载水平），小角度假设成立，换来凸 QP 的实时性和 ADMM 的可分解性。$SO(3)$ 精确积分留给大机动场景。这又是一个"精度 vs 实时/可解"的工程权衡——和摩擦锥金字塔（§4.5）、固定接触序列（§4.5）、长时域替代终端集（§4.7）是同一类决策：在多足分布式 MPC 里，凸化是贯穿始终的主题，因为只有凸 QP 才能在毫秒级 + ADMM 分解下跑起来。

(2) 接触力作为控制输入的特殊性。 多足 MPC 的控制 $\mathbf{u}_i=[\mathbf{f}_{i1},\dots,\mathbf{f}_{i4}]$ 是接触力，不是常见的"加速度/力矩指令"。这带来两个特点：

控制直接进入动力学的**右端**（$m_i\ddot{\mathbf{p}}_i=\sum_j\mathbf{f}_{ij}+\dots$），$B_i$ 矩阵结构简单（力的求和 + 力矩的叉乘）。
接触力受**单边 + 摩擦锥**约束（只能在锥内），不是无约束的——这正是 MPC 必须显式建约束的原因，也是它区别于"无约束 LQR"的地方。

(3) 离散化方法的选择。 把连续 $\dot{\mathbf{x}}=f(\mathbf{x},\mathbf{u})$ 离散成 $\mathbf{x}(t{+}1)=\dots$，常用：

方法	公式	优劣
前向欧拉（forward Euler）	$\mathbf{x}(t{+}1)=\mathbf{x}(t)+\Delta t\cdot f$	最简单，大 $\Delta t$ 不稳定；多足 MPC 常用（$\Delta t$ 小）
零阶保持（ZOH）	对 LTI 精确离散 $A_d=e^{A\Delta t}$	线性化后用，更精确
半隐式/RK	更高阶	更准但更重，非线性 MPC 用

多足凸 MPC 通常前向欧拉 + 小 $\Delta t$（如 $\Delta t=0.02$s），在精度和简单间平衡。

(4) 完整的离散预测方程（单 agent，线性化后）。 把上述综合，agent $i$ 在固定接触序列下的离散 SRB 预测方程是 LTV 形式：

\[ \mathbf{x}_i(t{+}1)=A_i(t)\mathbf{x}_i(t)+\sum_{j\in\text{支撑腿}(t)}B_{ij}(t)\mathbf{f}_{ij}(t)+B_i^\lambda(t)\boldsymbol{\lambda}_i(t)+\mathbf{g}_i \]

其中 $A_i(t)$ 含线性化的旋转动力学，$B_{ij}(t)$ 依赖当前足端相对质心位置 $\mathbf{r}_{ij}(t)$（决定力矩臂），$\mathbf{g}_i$ 含重力。注意 $B_{ij}$ 随踏步**时变**（哪条腿支撑、足端在哪都在变），这是"线性时变"而非"线性时不变"的原因。把这个方程在时域 $T$ 上展开，就是 §4.5 本地 MPC 的动力学约束。

阶段小结：到这里，§4.5 的 SRB 网络从连续微分方程，落到了 MPC 能用的离散 LTV 预测方程——姿态用欧拉角线性化（保凸）、接触力作控制输入（受单边+摩擦锥约束）、前向欧拉离散、$B$ 矩阵随踏步时变。把这个离散方程 + 接触约束 + ADMM 惩罚装进本地 QP，就是分布式多足 MPC 的本地子问题。下面的陷阱覆盖建模这一步最常见的错误。

一个具体数值例子：双足抬杆的耦合力¶

为了让 $\boldsymbol{\lambda}$ 不停留在抽象，看一个简化数值例子（一维竖直方向）。两台机器人各在杆的一端，杆水平、质量 $m_L=5$kg、重力 $g=9.8$。杆刚性，要求两端竖直位置满足约束（杆不变形、不旋转，两端等高加速度）：$\ddot z_1^{\text{att}}=\ddot z_2^{\text{att}}=\ddot z_L$。

设两台机器人通过腿对地的净竖直力分别为 $F_1,F_2$（它们能控制的量），杆给机器人 1、2 的竖直耦合力为 $\lambda_1,\lambda_2$（向上为正，机器人托杆则 $\lambda$ 是杆压机器人的反力）。

杆动力学：$m_L\ddot z_L=\lambda_1+\lambda_2-m_L g$（两端托力 - 重力）。注意这里 $\lambda_i$ 是机器人托杆的力（作用在杆上）。
若要杆匀速（$\ddot z_L=0$）：$\lambda_1+\lambda_2=m_L g=49$N。这是**运动力约束**——两端托力之和必须等于杆重才能托住。
但 $\lambda_1,\lambda_2$ 的**分配**有一个自由度：$\lambda_1=49/2+\delta,\ \lambda_2=49/2-\delta$。$\delta$ 就是**内力**——$\delta\ne0$ 意味着一端多托、一端少托，杆受一个净力矩（若杆能转）或内部剪力。

本质洞察（数值版）：这个例子赤裸裸地展示了 §4.5 练习 4 和第 5 章的核心——耦合力 $\boldsymbol{\lambda}$ 分两部分：和（$\lambda_1+\lambda_2$）由负载动力学唯一确定（运动力，托住杆），差（$\delta$）是自由的（内力，必须由协调控制约束住）。ADMM 协调 $\boldsymbol{\lambda}$ 时，运动力部分被负载动力学约束钉死（两端必须合力托杆），内力部分 $\delta$ 必须被显式控制到 0 或安全范围（否则两台机器人一个使劲往上托、一个往下压，把杆内部应力拉爆，或互相把对方拽失衡）。这就是为什么"协调耦合力"不只是"让合力对"，还要"让内力受控"——前者是本章 §4.5，后者通向第 5 章 grasp matrix。

这个 49N、$\delta$ 的小例子，就是 Kim-Fawcett-Hamed 双 A1 搬 5kg 负载在做的事情的一维投影：MPC 协调每台机器人对负载的力，既要合力托住搬动负载（运动力），又要内力受控不内耗（内力），而这一切通过 ADMM 在 SRB 层协调 $\boldsymbol{\lambda}$ 完成。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：把耦合力 $\boldsymbol{\lambda}$ 当成可独立优化的控制输入。 - 错误描述：在 MPC 里把 $\boldsymbol{\lambda}_i$ 当成和接触力 $\mathbf{f}_{ij}$ 一样的自由决策变量，随便优化。 - 现象/后果：解出的 $\boldsymbol{\lambda}$ 不满足刚性约束（附着点位置漂移），双足对负载施力不自洽，仿真里负载乱飞或机器人被拽飞。 - 根本原因：$\boldsymbol{\lambda}$ 是约束力，由刚性约束 + 动力学**解出**（$\boldsymbol{\lambda}=-(J M^{-1}J^\top)^{-1}(\dots)$），不是独立自由量。它在 agent 间必须满足负载动力学与第三定律的一致性。 - 正确做法：把 $\boldsymbol{\lambda}_i$ 作为**接口/对偶变量**，通过 ADMM 协调使其在 agent 间一致且满足负载动力学；或显式用约束力公式代入。它的自由度受刚性约束严格限制。

陷阱 2（编程陷阱）：摩擦锥用精确二阶锥导致 MPC 解不动。 - 错误描述：本地 MPC 里直接写 $\sqrt{f_x^2+f_y^2}\le\mu f_z$。 - 现象/后果：问题变 SOCP，QP 求解器（OSQP）用不了，换 SOCP 求解器慢一个数量级，毫秒级控制循环超时。 - 根本原因：二阶锥是非线性约束，破坏了 QP 结构。 - 正确做法：用金字塔线性近似（4 个线性不等式），保持 QP，OSQP 毫秒内解完。代价是可行域略保守（金字塔内切于锥），实践可接受。

陷阱 3（概念误区）：忽略摆动腿力为零的约束。 - 错误描述：所有腿都加摩擦锥 + $f_z\ge0$，不区分支撑/摆动。 - 现象/后果：MPC 可能给摆动腿（悬空、不接触地面）分配非零力——物理上不可能（脚没碰地哪来的力），导致控制指令错误，机器人动作诡异。 - 根本原因：摆动腿不接触地面，接触力必须为零；只对支撑腿施加摩擦锥。 - 正确做法：按踏步时序 contact_schedule(t)，摆动相强制 $\mathbf{f}_{ij}(t)=\mathbf{0}$，支撑相施加摩擦锥 + 单边约束。

陷阱 4（思维陷阱）：把编队约束加在足端而非质心。 - 错误描述：为了"精确"，把编队/协同约束加在脚的位置上。 - 现象/后果：足端是高频运动（每步抬落），把编队约束加在足端会和踏步剧烈冲突，机器人无法正常迈步。 - 根本原因：违反频段分离——编队是低频群体行为，应在质心（降阶）层；足端是高频个体行为，属底层 WBC。 - 正确做法：编队/协同约束加在质心（SRB 层），足端细节交给底层。呼应 §4.2 陷阱 3 与 §4.6 分层。

陷阱 5（概念误区）：用刚性 holonomic 约束建模绳索/柔性连接。 - 错误描述：不管连接是刚性杆还是绳索，一律用 $\mathbf{p}_i^{\text{att}}-\mathbf{p}_j^{\text{att}}=\text{const}$。 - 现象/后果：绳索只能拉不能推（单边）、还能松弛，用刚性等式约束会建出错误的力（如绳索"推"负载）。 - 根本原因：刚性连接是双边等式约束（杆能拉能推），绳索是单边不等式约束（只能拉、可松弛）。 - 正确做法：刚性杆用 holonomic 等式约束（本节）；绳索用单边约束（$\boldsymbol{\lambda}$ 沿绳方向且 $\ge0$、长度 $\le$ 绳长）；被动臂方案（PACC）则用另一套建模。连接类型决定约束类型。

练习¶

（推导约束力） 考虑最简一维情形：两个质量 $m_1=m_2=1$ 的滑块由刚性杆连接，约束 $x_1-x_2=\ell$（杆长固定）。各自受外力 $F_1,F_2$。(a) 写出约束 Jacobian $J_\Phi$；(b) 把约束微分两次得加速度层约束；(c) 用约束力公式 $\boldsymbol{\lambda}=-(JM^{-1}J^\top)^{-1}(JM^{-1}\mathbf{Q}+\dot J\dot{\mathbf{q}})$ 解出杆中力 $\lambda$；(d) 验证 $\lambda=(F_2-F_1)/2$ 的物理意义（杆传递两端外力差的一半）。这道题让你亲手走一遍约束力的推导，对应 §4.3 练习 3 的物理版。
（SRB 网络动力学搭建） 为"双 Go2 + 刚性杆"写出完整的 SRB 网络动力学方程组（两个 agent SRB + 杆作为负载 + 刚性附着约束）。明确：状态向量维度（两个 12 维 SRB + 杆 6 维 = 30 维）、每个方程里 $\boldsymbol{\lambda}$ 出现的位置、约束 Jacobian 的结构。这道题是累积项目的 SRB 网络建模模块，对应本章原骨架的"A 型·SRB 网络建模"练习。
（接触约束实现） 用 cvxpy/OSQP 实现单台四足的一步 SRB MPC（含摩擦锥金字塔 + 单边接触 + 给定 trot 踏步时序），预测 10 步。给定一个期望质心速度，求解接触力。然后**故意把摩擦锥换成精确二阶锥**（用 SOCP），对比求解时间——亲手验证陷阱 2（金字塔 vs 二阶锥的实时性差异）。
（跨章综合题） 结合第 5 章预告（grasp matrix 与内力）+ 本节耦合力 $\boldsymbol{\lambda}$：双四足抬杆时，两台机器人对杆的合力决定杆的运动（运动力），而它们对抗的部分是内力（不产生杆运动、只在杆内部传递）。用本节的约束力 $\boldsymbol{\lambda}_1,\boldsymbol{\lambda}_2$ 表达：哪部分组合是运动力（搬动杆）、哪部分是内力（挤压/拉扯杆）？为什么 ADMM 协调 $\boldsymbol{\lambda}$ 时必须保证内力受控（否则把杆/机器人内耗坏）？这道题把本章的耦合力和第 5 章的内力分配打通。

§4.6 分层控制：从 MPC 命令到关节力矩 ⭐⭐⭐¶

这一节解决什么问题：§4.5 的分布式 MPC 输出的是 SRB 级命令——质心力/力矩、足端接触力、耦合力。但电机吃的是**关节力矩**，不是质心力。这一节讲分层控制的完整数据流：高层分布式 MPC → 中层步态调度 → 低层分布式 WBC（全身控制），并解释一个关键的工程优势——为什么 WBC 可以纯本地、无通信。这是把前几节的"优化"接到"真机执行"的最后一公里。

动机：MPC 给的是"宏观命令"，电机要的是"微观力矩"¶

回顾 §4.5：分布式 MPC 在 SRB（降阶）层工作，输出每台机器人的质心运动目标、四条腿的接触力 $\mathbf{f}_{ij}$、耦合力 $\boldsymbol{\lambda}_i$。但这些都是"宏观"的——它们描述的是"质心该怎么动、脚该使多大力"，而不是"髋关节、膝关节该输出多少力矩"。

电机只能控制关节力矩。从"足端接触力"到"关节力矩"，要经过腿的运动学/动力学映射（雅可比转置 $\boldsymbol{\tau}=J^\top\mathbf{f}$ 等）。而且 SRB 是个近似——它假设腿无质量、机器人是单刚体，真实机器人有 12+ 个关节、腿有质量、姿态在 $SO(3)$ 上。需要一个**全阶（full-order）控制器把 SRB 级命令"翻译"成满足全身动力学的关节力矩，同时处理 SRB 忽略的细节（腿部动力学、精确姿态、关节限位）。这个全阶控制器就是 **WBC（Whole-Body Control，全身控制）。

分层的理由（呼应 §4.5 的频段分离）：MPC 算得"慢但远"（预测 0.4 秒、计算几毫秒、可较低频如 100-500Hz），WBC 算得"快但近"（只看当前、计算极快、高频如 1kHz）。两层各司其职——MPC 管协调与规划（含通信），WBC 管执行与平衡（纯本地）。

如果不分层、让一个控制器全包会怎样¶

假设不分层，写一个"全阶分布式 MPC"——直接在关节力矩层面、用全身动力学、跨预测时域、还带编队耦合，一把求解。会怎样？

维度爆炸 × 非线性 × 耦合，三重暴击。全身动力学每台机器人 18+ 维（浮基 6 + 关节 12），$N$ 台 + 负载 + 预测时域 + 编队耦合，问题规模和非线性程度远超实时可解。
通信负担巨大。如果全阶量都要在 agent 间协调，通信量爆炸（关节级轨迹比质心级轨迹大一个数量级）。
数值条件差。全身动力学含快慢混合的动态（质心慢、关节快），统一在一个优化里数值病态。

不是 X 而是 Y：多足协同控制不是"一个无所不包的大优化"，而是"分层——降阶 MPC 管低频协调（含通信），全阶 WBC 管高频执行（纯本地）"。分层不是为了偷懒，而是因为"协调"和"执行"在频率、维度、通信需求上根本不同，强行合并会让两边都做不好。

历史：从 Whole-Body Control 到分层 loco-manipulation¶

WBC 的思想源于 Khatib 的操作空间控制（operational space control，1987）——用任务优先级和零空间投影协调多个任务（如同时控制末端力和姿态）。足式机器人里，WBC 发展为基于 QP 的全身控制（Feng, Kuindersma 等 2010s）：在一个 QP 里同时满足全身动力学、接触约束、跟踪 MPC 给的质心/足端目标，输出关节力矩。

把 MPC + WBC 组成分层架构（高层 MPC 规划质心、低层 WBC 跟踪），成为现代四足控制的标准（MIT Cheetah、ETH ANYmal）。Kim-Fawcett-Hamed T-RO 2023 把它推广到多机协同：高层**分布式** MPC（SRB 网络 + ADMM）+ 低层**分布式**非线性控制器（每台机器人本地跟踪）。"分布式 WBC"的关键洞察是：WBC 可以纯本地、无通信——因为协调已经在 MPC 层通过耦合力 $\boldsymbol{\lambda}$ 完成了，WBC 只需让本机跟踪 MPC 给的（已协调好的）命令。

理论：三层架构的数据流¶

完整的分层数据流（每台机器人 $i$ 本地运行，仅高层 MPC 需通信）：

┌─────────────────────────────────────────────────────────────┐
│  高层：分布式 MPC（SRB 网络 + ADMM）   频率 ~100-500Hz       │
│  输入：自身状态 x_i + 邻居预测（通信）+ 编队/任务参考         │
│  计算：consensus-ADMM（§4.4），本地 SRB MPC（§4.5）          │
│  输出：质心轨迹参考 + 足端接触力 f_ij + 耦合力 λ_i           │
│  ↕ 通信：仅交换接口变量（邻居预测/耦合力一致性，~KB 级）      │
└──────────────────────────┬──────────────────────────────────┘
                           │ SRB 级命令（已跨 agent 协调）
                           ▼
┌─────────────────────────────────────────────────────────────┐
│  中层：步态调度（gait scheduler）       频率 = 控制频率       │
│  输入：步态类型（trot/walk）、相位                            │
│  输出：contact_schedule(t)——哪条腿何时支撑/摆动              │
│         摆动腿足端轨迹（落脚点，如 Raibert 启发式）           │
│  （通常本地、可预设；多机可共享步态相位以同步）               │
└──────────────────────────┬──────────────────────────────────┘
                           │ 接触序列 + 足端目标
                           ▼
┌─────────────────────────────────────────────────────────────┐
│  低层：分布式 WBC（全身 QP）            频率 ~1kHz            │
│  输入：MPC 的足端力 f_ij + 耦合力 λ_i + 步态 + 全身状态       │
│  计算：本地全身 QP（满足全身动力学 + 接触约束）              │
│  输出：关节力矩 τ_i → 电机                                    │
│  ★ 纯本地、无通信——协调已由 MPC 的 λ_i 完成                  │
└─────────────────────────────────────────────────────────────┘

低层分布式 WBC 的本地 QP（每台机器人独立解，无通信）：

\[ \min_{\boldsymbol{\tau}_i,\,\mathbf{f}_i,\,\ddot{\mathbf{q}}_i}\ \big\|A_i\ddot{\mathbf{q}}_i - \ddot{\mathbf{q}}_i^{\text{des}}\big\|^2 + w_f\big\|\mathbf{f}_i-\mathbf{f}_i^{\text{MPC}}\big\|^2 + w_\tau\|\boldsymbol{\tau}_i\|^2 $$ $$ \text{s.t.}\quad M_i\ddot{\mathbf{q}}_i+\mathbf{h}_i = S_i^\top\boldsymbol{\tau}_i + J_{c,i}^\top\mathbf{f}_i + J_{\text{att},i}^\top\boldsymbol{\lambda}_i^{\text{MPC}}\quad(\text{全身动力学}) $$ $$ \quad\quad\ \ \text{friction\_cone}(\mathbf{f}_i),\quad \text{joint\_limits}(\boldsymbol{\tau}_i),\quad \text{contact constraints} \]

关键：耦合力 $\boldsymbol{\lambda}_i^{\text{MPC}}$ 由高层 MPC 的 ADMM 协调步给出（已经和邻居一致了），WBC 直接拿来用作全身动力学里的外力项 $J_{\text{att},i}^\top\boldsymbol{\lambda}_i^{\text{MPC}}$，不需要再和邻居通信。这就是"分布式 WBC 纯本地计算"的全部秘密——协调的"账"在 MPC 层已经算清，WBC 只管本机执行。

本质洞察：分层架构把"多机协调"这个需要通信的难题，完全限制在了高层 MPC 这一层——而且只在 SRB（降阶、低频、小数据量）层协调。一旦 MPC 通过耦合力 $\boldsymbol{\lambda}$ 达成一致，下游的步态和 WBC 就退化成 $N$ 个完全独立的本地问题，零通信、可任意高频。这是分层最深刻的价值：它把"必须通信的部分"压缩到最小（低频、降阶、KB 级），把"可以本地的部分"最大化（高频、全阶、零通信）。通信是分布式系统最贵、最不可靠的资源，分层架构本质是一套"通信预算最小化"的设计。这也回答了为什么 Kim-Fawcett-Hamed 能在 WiFi 上跑双四足协同——因为真正过网络的只有 MPC 层那几 KB 的接口变量。

类比（像 X 不像 Y）：分层架构像**一家公司的"战略层-战术层-执行层"**。高层 MPC 像高管层开会定方向（要协调、要沟通、频率低）；中层步态像部门经理把战略拆成任务（调度）；低层 WBC 像一线员工执行具体工作（按已定好的任务干、不用再开公司级大会）。**像**的地方：协调成本集中在高层，执行层各自高效干活。**不像**的地方：公司各层是人、靠语言沟通且会扯皮，分层控制各层是数学映射、信息流严格单向（上层命令下传、下层不反向干预上层决策）且有明确频率分工。边界：这个类比不要延伸到"下层能否影响上层"——真实系统里 WBC 的状态会反馈给 MPC（状态估计），不是纯单向，但**协调决策**是单向下传的。

深入：WBC 为什么是 QP——多任务的优先级与折中¶

§4.6 给的 WBC 是一个 QP（最小化加权任务误差）。但为什么 WBC 天然是优化问题、而不是一个简单的代数映射？理解这一点，才理解 WBC 在分层架构里的真正职责。

WBC 要同时满足多个相互竞争的任务：跟踪 MPC 给的质心加速度、跟踪足端力 $\mathbf{f}^{\text{MPC}}$、维持机体姿态、避免关节超限、满足全身动力学。这些任务**可能冲突**——比如完美跟踪质心加速度所需的接触力，可能超出摩擦锥或关节力矩限制。当任务冲突时，必须**折中**，而折中天然是一个优化问题（最小化加权误差 + 满足硬约束）。

两种 WBC 范式（系统性分类）：

范式	做法	优劣
加权 WBC（weighted）	所有任务放进一个 QP 目标，用权重 $w$ 平衡	简单、单 QP；但权重难调，软优先级
严格优先级 WBC（hierarchical/null-space）	高优先级任务先满足，低优先级在高优先级的零空间里做	严格优先级（平衡 > 跟踪）；多级 QP，复杂

严格优先级 WBC 用**零空间投影**——回顾 Khatib 操作空间控制：次要任务的解被投影到主任务的零空间，保证不破坏主任务。比如"维持平衡"是最高优先级，"跟踪 MPC 质心目标"是次优先级，后者只能在不破坏平衡的前提下尽力实现。

本质洞察：WBC 是 QP 而非代数映射，根源在于"从 SRB 降阶命令到全阶执行"是一个**欠定 + 受约束**的问题——欠定（关节冗余，多种关节力矩都能实现同一质心运动）需要优化挑一个最优，受约束（摩擦锥、关节限位）需要在可行域内挑。MPC 在 SRB 层"宏观规划"，WBC 在全阶层"微观分配 + 折中"。两者都是优化，但优化的对象不同：MPC 优化"未来轨迹"（跨时域），WBC 优化"当前瞬时的力矩分配"（单时刻）。这个"跨时域规划（MPC）+ 瞬时分配（WBC）"的分工，是分层架构的另一种解读——按"时间尺度"分（MPC 看未来、WBC 看当下）。

反事实：如果 WBC 不做优化、直接用 $\boldsymbol{\tau}=J^\top\mathbf{f}^{\text{MPC}}$（纯雅可比转置映射）会怎样？这忽略了全身动力学（腿部惯量、科氏力）、不处理关节限位、不能在任务冲突时折中——在静态或慢速时勉强能用，动态行走时质心跟踪差、可能违反关节限制而损坏电机。所以 WBC 必须是带约束的优化，纯代数映射不够。这反衬出 WBC 的"全身动力学约束 + 折中优化"不是多余的复杂，而是动态多足执行的必需。

这也解释了 §4.6 为什么强调 WBC 纯本地：WBC 的优化只用本机的全身状态 + MPC 传下来的（已协调好的）力命令，没有任何项需要邻居信息——协调的"账"在 MPC 层算清了，WBC 只做本机的力矩分配优化。把多机协调和单机执行如此干净地分开，正是分层架构的威力。

一步通信延迟与一致性协议（呼应 §4.1）¶

Kim-Fawcett-Hamed 明确把分布式 MPC 建模为带"one-step communication delay（一步通信延迟）+ agreement protocol（一致性协议）"。落到分层架构里：

一步延迟：高层 MPC 的 ADMM 协调步，agent $i$ 用的是邻居**上一控制周期**算出的接口变量（耦合力/预测），不是本周期实时值（§4.1 陷阱 3）。
一致性协议：ADMM 的 Z-更新（共识平均）+ Y-更新（对偶上升）就是 agreement protocol 的具体实现——它逼迫各 agent 的耦合力副本在迭代中趋于一致（满足负载动力学与第三定律）。

这个延迟是稳定性分析的关键隐患（§4.7）——它相当于在反馈回路里插入了一个滞后，可能降低稳定裕度，必须在控制器设计时考虑。

深入：多机步态同步——中层调度的协同维度¶

§4.6 的中层步态调度，在单体四足里只是个本地时钟（按相位决定支撑/摆动）。但在多足**协同搬运**里，它多出一个被初学者忽略的协同维度：多台机器人的步态相位要不要同步？

考虑双足抬一根刚性杆。两台机器人各自 trot（对角小跑），每个 trot 周期里都有"双支撑→单对角支撑"的切换，切换时质心和对地力会波动。如果两台机器人**步态完全异步**（相位随机），它们对杆的力波动也不同步——杆会被两端不协调的力波动"摇晃"，内力剧烈起伏，搬运不稳。

三种步态协同策略：

策略	做法	优劣
完全异步	各跑各的步态时钟，不协调相位	简单、无需同步通信；但力波动不协调，杆易晃
相位同步（in-phase）	两台机器人步态相位锁定一致	力波动同步、杆平稳；需共享步态相位（轻量通信）
反相同步（anti-phase）	两台机器人步态相位差半周期	一台双支撑时另一台单支撑，对杆的"支撑可靠度"互补，可能更稳

本质洞察：步态同步是多足协同**特有**的协调维度——轮式/无人机编队没有"步态"，它们的执行是连续的；多足执行是**离散切换**的（支撑腿在变），所以多了"切换时机要不要对齐"这个问题。这把协同从"协调连续的力/位置"扩展到"协调离散的接触事件"。从信息论看，步态相位是一个标量（相位角），共享它的通信开销极小（每周期一个 float），但对搬运平稳性影响巨大——这是"小通信、大收益"的又一例（呼应 §4.6 通信预算最小化）。

理论-工程桥接：步态相位同步可以用第 2 章的**共识**实现！把每台机器人的步态相位 $\phi_i$ 看作要达成一致的标量，跑相位共识（$\dot\phi_i=\omega+k\sum_{j\in\mathcal{N}_i}\sin(\phi_j-\phi_i)$，Kuramoto 振子同步）就能让相位锁定。所以多足步态同步是第 2 章共识理论在"离散接触事件"上的又一应用——共识不只用在 ADMM 的 Z-更新（连续接口变量），还能用在步态相位（协同时钟）。这显示了共识作为"分布式协调原子操作"的普适性：凡是"$N$ 个量要只靠邻居达成一致"，都是共识。

类比（像 X 不像 Y）：多机步态同步像**一队人合抬重物时喊"一二、一二"的号子**。号子让大家迈步同步（相位锁定），抬物的力波动协调，物体平稳。**像**的地方：通过共享一个简单的节拍信号（号子/相位）协调离散的动作时机（迈步/踏步切换）。**不像**的地方：人靠听觉同步号子，机器人靠通信共享相位 + Kuramoto 共识锁定；而且号子是全局广播（一个人喊全队听），分布式步态同步是只靠邻居（无全局广播）。边界：这个类比对"需要同步"的搬运成立，但若两台机器人独立行走不共享负载（如并排巡逻），步态就不必同步——不要把"协同搬运需要步态同步"推广到所有多足编队。

中层步态调度因此在协同场景下也带了一点"协同色彩"——它主要仍是本地的（每台机器人自己的支撑/摆动时钟），但相位可能需要和邻居共识同步。这是 §4.6 分层架构里"中层"在多机语境下的细微但重要的扩展。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：以为分布式 WBC 也要通信。 - 错误描述：既然 MPC 是分布式（要通信），就以为 WBC 也得和邻居交换信息。 - 现象/后果：在 WBC 层加无谓的通信，徒增 1kHz 高频通信负担（网络根本扛不住），还引入延迟。 - 根本原因：协调已在 MPC 层通过耦合力 $\boldsymbol{\lambda}$ 完成，WBC 只需本机跟踪已协调好的命令。 - 正确做法：WBC 纯本地——用 MPC 传下来的 $\boldsymbol{\lambda}_i^{\text{MPC}}$ 作为已知外力，独立解本地全身 QP，零通信、可 1kHz。

陷阱 2（思维陷阱）：把 MPC 的足端力直接当关节力矩发给电机。 - 错误描述：跳过 WBC，把 MPC 输出的接触力 $\mathbf{f}_{ij}$ 直接当作控制下发。 - 现象/后果：电机吃的是关节力矩不是足端力，直接发足端力是量纲/物理错误；且忽略了腿部动力学、关节限位。 - 根本原因：MPC 在 SRB 降阶层输出 SRB 级命令，必须经 WBC 映射到关节力矩并补上 SRB 忽略的全阶细节。 - 正确做法：MPC 足端力 $\mathbf{f}_{ij}$ → WBC（含 $\boldsymbol{\tau}=J^\top\mathbf{f}$ + 全身动力学）→ 关节力矩 $\boldsymbol{\tau}_i$ → 电机。

陷阱 3（概念误区）：忽略 MPC 与 WBC 的频率差异，让它们同频。 - 错误描述：MPC 和 WBC 都跑 1kHz（或都跑 100Hz）。 - 现象/后果：MPC 跑 1kHz 计算扛不住（ADMM 多轮 + 通信来不及）；WBC 跑 100Hz 则平衡反应太慢、易摔。 - 根本原因：MPC 算得慢但管远（低频够用），WBC 算得快管近（必须高频）——两层本就该不同频。 - 正确做法：MPC ~100-500Hz（含通信预算），WBC ~1kHz（纯本地高频）。MPC 的命令在 WBC 周期间保持或插值。

练习¶

（分层数据流梳理） 画出双 Go2 抬杆的完整分层数据流图，标注每一层的：输入、输出、计算内容、频率、是否通信。特别标出"通信只发生在高层 MPC、且只传接口变量"和"WBC 纯本地"。这道题帮你建立分层架构的整体心智模型，对应原骨架的"B 型·Kim-Hamed 精读"练习。
（WBC 本地 QP 实现） 用 pinocchio + OSQP 为单台四足实现一个简化 WBC：输入 MPC 给的足端力参考 $\mathbf{f}^{\text{MPC}}$ 和质心加速度目标，在全身动力学约束下求关节力矩 $\boldsymbol{\tau}$。验证：输出的 $\boldsymbol{\tau}$ 通过正动力学仿真后，质心加速度接近目标。然后加入耦合力 $\boldsymbol{\lambda}^{\text{MPC}}$ 项，观察它如何改变关节力矩分配。
（开放思考） Kim-Fawcett-Hamed 的低层用"全阶非线性控制器"而非 QP-WBC。查阅论文 Section 关于 low-level controller 的部分，对比"非线性反馈控制器"和"QP-based WBC"两种低层方案的优劣（计算量、约束处理能力、对模型误差的鲁棒性）。说明为什么两者都能纳入"低层、本地、跟踪 MPC 命令"的分层框架。

§4.7 通信拓扑、一致性与稳定性：把第 2 章接到 DMPC ⭐⭐⭐⭐¶

这一节解决什么问题：前面把分布式 MPC 的算法（§4.4）和多足建模（§4.5-§4.6）讲完了，但留了三个"理论欠账"：(1) §4.4 的 Z-更新（共识平均）如何真正分布式实现？(2) 通信质量（拓扑、时延、丢包）如何影响收敛与稳定？(3) 整个分布式 MPC 闭环稳定吗？这一节把第 2 章的共识理论正式接到 DMPC 上，回答这三个问题，并讲 DMPC 稳定性的两大支柱——递归可行性与终端代价。这是本章理论的收口，把第 1-2 章的图论-共识和本章的 MPC 完全焊接。

动机：ADMM 的"共识平均"到底怎么分布式算¶

回顾 §4.4 的 Z-更新：$\mathbf{z}^{k+1}=\frac{1}{N}\sum_i(\mathbf{w}_i+\mathbf{u}_i)$——这是一个**平均**。代码里我们用了一个 for 循环求和再除以 $N$，但这**隐含了一个中央节点**（谁来收集所有 $\mathbf{w}_i$ 求和？）。如果真有中央节点，那不就回到集中式的单点故障了吗？

这里正是第 2 章登场的地方。第 2 章 §2.1 证明了：只靠邻居通信的共识迭代 $x(k{+}1)=Wx(k)$（$W$ 双随机）能收敛到所有节点的平均值。所以 ADMM 的 Z-更新（求平均）可以用第 2 章的**分布式共识**实现——不需要中央节点，每个 agent 只和邻居交换，跑若干轮共识就能算出（近似）平均。这就把"分布式 MPC"真正变成了"无中心"。

回顾第 2 章 §2.1：那里我们证明了连续共识 $\dot x=-Lx$ 收敛到平均 $\bar x\mathbf{1}$，速率由代数连通性 $\lambda_2$ 决定（$e^{-\lambda_2 t}$）；离散共识 $x(k{+}1)=Wx(k)$ 要求 $W$ 双随机，速率由第二大特征值模 $|\rho_2(W)|$ 决定。本节直接用这两个结论来分析 ADMM 协调步。

如果通信拓扑/质量不管会怎样¶

假设实现分布式 MPC 时完全不考虑通信拓扑和质量，会踩三个坑：

拓扑差 → ADMM 收敛慢。如果用一字链拓扑（$\lambda_2$ 小、直径大），Z-更新的分布式共识要很多轮才算准平均，每轮 ADMM 的内层通信开销大；外层 ADMM 也因信息传播慢而收敛轮数多。链拓扑下双四足之间隔着中间节点，协调迟钝。
时延 → 失稳。第 2 章 §2.2 给了临界时延 $\tau^\star=\pi/(2\lambda_N)$——超过它共识本身就发散。叠加 §4.6 的一步通信延迟，反馈回路滞后增大，DMPC 闭环可能失稳。
丢包 → 残差不收敛。如果某轮邻居数据丢了还硬用旧值，ADMM 的对偶配平被打乱，残差降不下去，编队有持续偏差。

不是 X 而是 Y：分布式 MPC 的性能不只取决于"算法多聪明（ADMM、$\rho$ 调得多好）"，而是取决于"算法 × 通信拓扑的匹配"。同一套 ADMM，在 $\lambda_2$ 大的拓扑（如全连接、星形）上几轮收敛，在 $\lambda_2$ 小的拓扑（如长链）上几十轮还在挣扎。通信拓扑是分布式 MPC 的"地基"，地基不好楼盖不高。

历史：从共识到分布式优化的统一图景¶

第 2 章讲过共识（Olfati-Saber, Murray 2004 奠基）和 ADMM（Boyd 2011 复兴）。这两条线在 2010 年代汇合——人们意识到 ADMM 的协调步本质是共识，而共识是分布式优化的特例。有意思的是，"ADMM vs 平均共识谁更快"本身是个研究问题（Teixeira 等的工作表明：取决于问题和拓扑，ADMM 在通信轮数上常更省但每轮计算更重）。这条统一线的实践意义是：分布式 MPC 的工程师可以混搭——用 ADMM 做外层优化分解，用共识做内层平均的分布式实现，两者都吃同一张通信图的红利（$\lambda_2$ 大则两者都快）。

本质洞察：共识和 ADMM 不是两个独立的工具，而是**同一枚硬币的两面**——共识解决"$N$ 个标量/向量如何只靠邻居达成一致"，ADMM 解决"$N$ 个优化问题如何只靠邻居协调到联合最优"，而 ADMM 的协调步**就是**一次共识。所以第 2 章不是本章的"前置知识"那么简单，它是本章分布式实现的**内核**：拆掉 ADMM 的 Z-更新，里面跑的就是第 2 章的 $x(k{+}1)=Wx(k)$。理解这个嵌套（外层 ADMM 优化、内层共识平均），你就理解了整个分布式多足 MPC 的运行机理。

理论：ADMM Z-更新的分布式共识实现¶

把 §4.4 的 Z-更新用第 2 章的离散共识实现。要算的是平均 $\bar{\mathbf{v}}=\frac1N\sum_i\mathbf{v}_i$（$\mathbf{v}_i=\mathbf{w}_i+\mathbf{u}_i$）。每个 agent 初始化 $\mathbf{v}_i^{(0)}=\mathbf{v}_i$，跑共识迭代：

\[ \mathbf{v}_i^{(\ell+1)}=\sum_{j\in\mathcal{N}_i\cup\{i\}}W_{ij}\,\mathbf{v}_j^{(\ell)}\quad(\ell=0,1,\dots,L_{\text{cons}}) \]

$W$ 是双随机权重矩阵（第 2 章 §2.1：Metropolis-Hastings 权重是常用构造，保证双随机 + 连通）。由第 2 章定理，$\mathbf{v}_i^{(\ell)}\to\bar{\mathbf{v}}$，误差以 $|\rho_2(W)|^\ell$ 衰减。跑 $L_{\text{cons}}$ 轮后，每个 agent 都有了（近似）平均，即 $\mathbf{z}$。

两层迭代结构（分布式 MPC 的完整图景）：

外层：ADMM（§4.4），每轮 k：
  X-更新（本地 MPC，并行，无通信）
  Z-更新 = 内层共识：跑 L_cons 轮 v_i ← W·v_i（邻居通信）→ 得到平均 z
  Y-更新（本地对偶上升，无通信）
内层共识轮数 L_cons 取决于拓扑（λ_2 / |ρ_2(W)|）：λ_2 大 → L_cons 小

通信量分析（呼应原骨架）：每轮内层共识，每个 agent 向邻居发送 $\mathbf{v}_i$（接口变量，如 SRB 12 维 × 时域 T=20 = 240 维 ≈ 1.92KB）。外层 ADMM $K$ 轮 × 内层共识 $L_{\text{cons}}$ 轮 × 1.92KB ≈ 几十 KB/agent/控制周期。WiFi（数十 Mbps）下，每周期几十 KB、控制周期几 ms，完全可行。工程优化：很多实现直接交换预测轨迹（一步延迟），把 $L_{\text{cons}}$ 压到 1（只交换一次），用外层 ADMM 的多轮逐步逼近一致——这是计算/通信的权衡。

理论-工程桥接：为什么实践中 $L_{\text{cons}}$ 常取 1（每个 ADMM 外层只通信一次）而非跑到内层共识完全收敛？因为外层 ADMM 本身是迭代的——即使每轮 Z-更新只做一次不精确的共识（"inexact ADMM"），外层多轮下来仍能收敛（在有界误差条件下）。这是"不精确但够用"哲学的又一体现（呼应 §4.4 的固定轮数）：与其每轮把内层平均算到极致，不如内外层都"够用就停"，总通信轮数更少。Kim-Fawcett-Hamed 的"一步通信延迟 + agreement protocol"正是这种 $L_{\text{cons}}=1$ 的实现——每控制周期交换一次邻居预测，靠滚动 + ADMM 对偶逐步配平。

通信拓扑对编队的影响：$\lambda_2$、直径、时延、丢包¶

把第 1-2 章的图论量对应到 DMPC 性能：

拓扑量（第 1-2 章）	对 DMPC 的影响	工程含义
代数连通性 $\lambda_2$	越大，内层共识与外层 ADMM 收敛越快	编队拓扑尽量连通（多加几条通信边）
直径（diameter）	越大，信息传遍全网越慢	长链拓扑协调迟钝，避免
临界时延 $\tau^\star=\pi/(2\lambda_N)$	通信时延超过它，共识/协调发散	高频控制下时延预算紧张，需评估
丢包率	破坏双随机性/对偶配平，残差不收敛	用对丢包鲁棒的 ADMM 变体（异步、确认重传）

本质洞察：编队的**通信拓扑**和编队的**物理队形**是两张不同的图（呼应第 3 章的提醒）——物理上两台机器人挨得近，不代表它们通信链路好。分布式 MPC 的协调质量由**通信图**的 $\lambda_2$ 决定，而不是物理队形的紧密度。所以设计编队时，通信拓扑要单独规划：宁可让物理上远的机器人之间也保持一条可靠通信边（提升 $\lambda_2$），也不要让通信图退化成长链。这是多机系统"物理层"与"信息层"解耦的又一例证。

深入：收敛率的定量刻画与"够用就停"的本钱¶

§4.4 说 ADMM 凸情形 $O(1/k)$、强凸 $O(1/k^2)$，§4.7 说内层共识误差 $\propto|\rho_2(W)|^\ell$。把这些定量结论拼起来，才能回答工程上最实际的问题：到底要跑多少轮（外层 $K$、内层 $L_{\text{cons}}$）才"够用"？

(1) 外层 ADMM 轮数 $K$ 的估计。 凸情形原始残差 $\|r^k\|\sim O(1/k)$。要把残差降到阈值 $\epsilon$，需 $K\sim O(1/\epsilon)$ 轮（强凸时 $O(1/\sqrt\epsilon)$，快得多）。但实时 MPC 的关键武器是 warm-start——相邻控制周期的解高度相似，初始残差就很小，所以从 warm-start 起只需补几轮（$K=3\text{-}5$）就够。这就是为什么 §4.4"方式 C"成立：不是 ADMM 本身收敛快，而是 warm-start 把"起点"挪到了离最优解很近的地方，剩下的路就短了。

(2) 内层共识轮数 $L_{\text{cons}}$ 的估计。 误差 $\propto|\rho_2(W)|^{L_{\text{cons}}}$，要降到 $\epsilon$ 需 $L_{\text{cons}}\sim\frac{\ln\epsilon}{\ln|\rho_2(W)|}$。$|\rho_2|$ 越接近 1（拓扑越差、$\lambda_2$ 越小）这个数越大。但实践常取 $L_{\text{cons}}=1$（每外层只通信一次），靠外层 ADMM 多轮逐步配平——即"inexact ADMM"。这是把通信轮数压到最低的关键。

(3) 两层轮数的乘积 = 总通信成本。 真实代价是**外层 × 内层 = $K\cdot L_{\text{cons}}$ 次邻居通信**（每次发一个接口变量）。优化目标不是单独最小化 $K$ 或 $L_{\text{cons}}$，而是最小化乘积。极端：$L_{\text{cons}}=1$ 时内层最省但外层 $K$ 要大些；$L_{\text{cons}}$ 大时内层精确、外层 $K$ 小。最优点取决于拓扑和问题，要实测（§4.7 练习 2）。

策略	外层 $K$	内层 $L_{\text{cons}}$	总通信	适用
内层精确	小（3-5）	大（$\ge10$）	中-高	拓扑好（$\lambda_2$ 大）、$L_{\text{cons}}$ 本就小
内层不精确（inexact）	中（5-10）	1	低	实时多足 MPC 推荐
直接交换预测（一步延迟）	滚动（每周期 1 次）	—	最低	Kim-Fawcett-Hamed 的做法

本质洞察：实时分布式 MPC 的全部工程智慧，浓缩成一句话——"够用就停"在两个层面同时发生：外层 ADMM 不跑到收敛（warm-start + 固定 $K$），内层共识不算到精确（inexact + $L_{\text{cons}}=1$）。 两个"不精确"叠加，理论上似乎应该一塌糊涂，但实践中却 work——因为 MPC 是**滚动**的，每个周期只取第一步控制执行，剩下的"不精确"会在下个周期被新的测量 + 新的优化纠正。滚动机制是分布式 MPC 容忍双重不精确的根本原因。把这一点想透，你就理解了为什么"不收敛的 ADMM + 不精确的共识"能控住会走路的机器人编队——MPC 的滚动重规划在背后不断兜底。

反事实：如果不滚动（开环执行整条 ADMM 算出的轨迹）会怎样？双重不精确的误差无人纠正，会随时间累积——编队逐渐偏离、内力逐渐失控、最终失稳。这正反衬出滚动（闭环）的价值：它把"每周期的小不精确"限制住，不让它们累积成"开环的大灾难"。所以分布式 MPC 必须是闭环滚动的，开环用 ADMM 解一条长轨迹然后照着走是行不通的。

稳定性：递归可行性与终端代价¶

最后回答"分布式 MPC 闭环稳不稳"。MPC 的稳定性靠两个支柱（单体 MPC 已学，这里看分布式的特殊性）：

(1) 递归可行性（recursive feasibility）。 如果当前周期 MPC 有可行解，下一周期是否仍有可行解？分布式情形更难——每个 agent 基于邻居的（滞后的）假设轨迹求解，假设与实际的偏差可能累积导致不可行。经典处理（Dunbar、Conte 等）： - 兼容性约束（compatibility constraint）：限制 agent 的实际轨迹与它广播给邻居的假设轨迹的偏差（$\|\mathbf{x}_i^{\text{actual}}-\hat{\mathbf{x}}_i\|\le\delta$），保证邻居基于假设做的规划不会因偏差太大而失效。 - 约束收紧（constraint tightening）：留安全裕度，吸收分布式求解的不精确。

(2) 终端代价/终端约束（terminal cost / terminal set）。 MPC 的有限时域要逼近无限时域的最优，靠在时域末端加终端代价 $V_f$（近似 cost-to-go）+ 终端约束集（保证末态落在不变集内）。分布式情形需要**分布式的终端不变集计算**——这本身是难题（联合不变集难分布式算）。一些方法（如无终端代价的 DMPC，用松弛动态规划给出显式稳定性约束）绕开这个难点。

理论-工程桥接：为什么很多实际的多足分布式 MPC（包括 Kim-Fawcett-Hamed）不严格构造终端不变集，却仍能稳定运行？因为它们靠：(a) 足够长的预测时域（覆盖几个踏步周期，末端动态已"稳定"）；(b) warm-start 提供的好初值让每周期解接近上周期（隐式的连续性）；(c) 在 SRB 降阶层的凸化使本地 QP 总可行。严格的递归可行性 + 终端不变集是**充分条件**（理论保证），工程上常用这些**实践充分**的替代。这是理论与工程的典型张力：理论给最强保证但代价高（分布式不变集），工程取够用的近似（长时域 + warm-start + 凸化）。对研究者，弥合这个差距（给分布式多足 MPC 严格的稳定性证明）正是开放问题。

非凸的坦白：必须重申 §4.4 的警告——多足接触约束非凸，使得连 ADMM 的**收敛性**都无理论保证，更别说闭环稳定性。当前（2024-2026）的前沿（CBF-ADMM、数据驱动 DMPC）仍在攻关。本章给你的是**当前最佳实践 + 清醒的局限认知**：方法在仿真和受控实验中有效，但理论保证尚不完整。把这个认知带进研究，比假装一切都已证明更诚实、更有用。

深入：兼容性约束为什么是分布式 MPC 递归可行的关键¶

上面提到"兼容性约束（compatibility constraint）"保递归可行，但它为什么必要、怎么起作用，值得展开——这是分布式 MPC 区别于集中式 MPC 稳定性分析的**核心增量**，初学者常一带而过却不理解其深意。

问题的根源：假设与实际的偏差。 在分布式 MPC 里，agent $i$ 求解时把邻居 $j$ 的轨迹当作**已知的假设轨迹** $\hat{\mathbf{x}}_j$（§4.1 预测交换）。但 agent $j$ 实际**执行的轨迹 $\mathbf{x}_j^{\text{actual}}$ 可能和它之前广播的 $\hat{\mathbf{x}}_j$ 不同——因为 $j$ 在新周期里基于新信息重新优化了。于是：**agent $i$ 基于"$j$ 会走 $\hat{\mathbf{x}}_j$"做的规划，可能因为"$j$ 实际走了别的"而失效（比如 $i$ 留的避让空间不够、或编队约束因 $j$ 的偏移而违反）。

集中式没有这个问题——集中式一次解出所有 agent 的轨迹，不存在"假设别人会怎么走"。这个偏差是分布式**特有**的稳定性威胁。

兼容性约束的解法：限制偏差。 给每个 agent 加一条约束——它的实际轨迹不能偏离它广播的假设轨迹太多：

\[ \|\mathbf{x}_i^{\text{actual}}(t)-\hat{\mathbf{x}}_i(t)\|\le\delta,\quad\forall t \]

这样，邻居基于 $\hat{\mathbf{x}}_i$ 做的规划，至多面对 $\delta$ 的偏差——只要规划时留了 $\delta$ 的裕度（约束收紧），就不会失效。$\delta$ 是个权衡旋钮：

$\delta$ 太大 → 偏差大，邻居规划要留大裕度（保守，性能差），但 agent 自己灵活；
$\delta$ 太小 → agent 被迫贴着假设走（不灵活，可能不可行），但邻居几乎不用留裕度。

本质洞察：兼容性约束的精髓是**"我承诺我不会偏离我告诉你的计划太远，于是你可以放心地基于我的计划来规划"——它把分布式系统里"各自独立重规划"的不确定性，用一个显式的偏差上界 $\delta$ 兜住。这本质是一种**契约（contract）：每个 agent 牺牲一点自己的灵活性（被 $\delta$ 约束），换取整个系统的可预测性（邻居能信任它的广播）。没有这个契约，分布式 MPC 的递归可行性就建立在"假设别人不变"的沙地上，一旦别人变了就崩。有了它，可行性建立在"别人变化有界"的实地上。这是分布式协调"信任但验证（trust but verify）"哲学的数学化。

类比（像 X 不像 Y）：兼容性约束像**合租室友之间的"作息公约"**。每个人承诺"我作息不会偏离公布的时间表超过半小时"（$\delta$=半小时），于是大家可以基于彼此的时间表安排共用厨房/卫生间（基于假设规划）。**像**的地方：用一个偏差承诺让彼此的计划可互信、可协调。**不像**的地方：室友靠自觉守约，分布式 MPC 把 $\delta$ 写成硬约束强制满足；而且室友违约顶多尴尬，MPC 违反兼容性约束会直接导致递归不可行（系统失稳）。边界：这个类比说明"为什么需要承诺"，但不要延伸到"承诺的代价"——室友守约几乎无成本，而 MPC 的 $\delta$ 约束实实在在牺牲了控制性能，是有代价的权衡。

把兼容性约束 + 约束收紧 + 终端代价三者配齐，分布式 MPC 才有完整的递归可行性 + 稳定性保证（Dunbar 2006、Conte 2016 的理论框架）。但如前所述，多足的非凸性让这套理论无法直接套用——工程上退而求其次用长时域 + warm-start + 凸化。理解兼容性约束的**思想**（即使工程上没严格实现），能帮你判断一个分布式 MPC 在什么情况下会失稳：当某个 agent 的实际轨迹大幅偏离它的广播（如突遇大扰动急转向），而邻居没留够裕度时。

⚠️ 常见陷阱¶

陷阱 1（概念误区）：以为 ADMM 的 Z-更新（求平均）必须有中央节点。 - 错误描述：看到 Z-更新是 $\frac1N\sum_i$，就以为需要一个中心收集求和。 - 现象/后果：实现时真加了中央节点，退回单点故障，分布式名存实亡。 - 根本原因：平均可以用第 2 章的分布式共识（只邻居通信）实现，不需中心。 - 正确做法：Z-更新用共识迭代 $\mathbf{v}_i\leftarrow W\mathbf{v}_i$（$W$ 双随机）分布式算平均。$L_{\text{cons}}$ 轮后各 agent 都有近似平均，无中心。

陷阱 2（思维陷阱）：把物理队形的紧密度当作通信质量。 - 错误描述：以为物理上挨得近的机器人通信就好，协调就快。 - 现象/后果：用了物理近但通信差（如遮挡）的拓扑，ADMM 收敛慢、编队迟钝，却找不到原因。 - 根本原因：通信图和物理队形是两张图，协调质量由通信图 $\lambda_2$ 决定，与物理距离无必然关系。 - 正确做法：单独规划通信拓扑，保证 $\lambda_2$ 大（必要时让物理远的机器人也保持通信边），不依赖物理邻近。

陷阱 3（编程陷阱）：丢包时硬用旧值且不补偿。 - 错误描述：某轮没收到邻居数据，就用上一轮的旧值继续 ADMM，不做任何标记或补偿。 - 现象/后果：对偶配平被打乱，残差降不到 0，编队有持续偏差，且时好时坏（取决于丢包随机性）。 - 根本原因：ADMM 的收敛依赖各 agent 数据的一致更新，无补偿的丢包破坏这个一致性。 - 正确做法：用对丢包鲁棒的变体（异步 ADMM 在有界延迟下仍收敛、确认重传、或丢包时跳过该邻居并重归一化权重）。监控残差以发现异常。

陷阱 4（概念误区）：以为长预测时域 + warm-start 就等于严格稳定性保证。 - 错误描述：因为实践中长时域 + warm-start 跑得稳，就当成有了稳定性证明。 - 现象/后果：在未测试的工况（大扰动、新步态切换）下可能失稳，因为没有理论保证兜底。 - 根本原因：长时域 + warm-start 是实践充分条件，不是理论充分条件；非凸下尤其无保证。 - 正确做法：清醒区分"实践有效"和"理论保证"。关键应用要做充分的工况测试 + 安全监控（如 CBF 安全层），不把实践稳定误当数学证明。

练习¶

（拓扑 → 收敛轮数） 对 $N=5$ 的三种拓扑（全连接、星形、一字链），用 Metropolis 权重构造双随机 $W$，计算各自的 $|\rho_2(W)|$。估算把平均算到 1% 精度各需多少轮共识 $L_{\text{cons}}$（$|\rho_2|^{L}\le0.01$）。验证：全连接/星形（$\lambda_2$ 大）几轮够，长链（$\lambda_2$ 小）要十几轮。这道题把第 2 章的 $\lambda_2$ 和本章 ADMM 的内层通信开销定量连起来。
（嵌套迭代实现） 在 §4.4 练习 3 的双积分器 ADMM 编队基础上，把 Z-更新从"中央求平均"改成"分布式共识迭代"（$L_{\text{cons}}=1,3,10$）。对比：$L_{\text{cons}}$ 越小，外层 ADMM 需要越多轮才收敛（不精确 ADMM）；$L_{\text{cons}}$ 大则内层通信开销大。找到总通信轮数（外层 × 内层）最优的组合。这道题让你亲手体验两层迭代的权衡。
（时延稳定性） 用第 2 章 §2.2 的临界时延 $\tau^\star=\pi/(2\lambda_N)$，对一字链拓扑（$N=5$）计算 $\lambda_N$ 和 $\tau^\star$。说明：如果叠加 §4.6 的一步通信延迟（= 一个控制周期），在什么控制频率下这个延迟会接近 $\tau^\star$ 而威胁稳定？这道题把通信时延、控制频率、稳定性三者定量关联。
（跨章综合题 + 开放） 综合第 1 章（拓扑/$\lambda_2$）、第 2 章（共识/ADMM/时延）、第 3 章（通信图 vs 环境图）、本章（DMPC）：为一个真实的 4 台 Go2 抬板编队设计完整的通信与协调方案——选什么通信拓扑（理由）、ADMM 外层轮数 $K$ 与内层共识轮数 $L_{\text{cons}}$ 如何权衡、如何处理一步延迟与可能的丢包、稳定性靠什么保障。写成一段设计文档（200-300 字）。这道题是本章理论的总集成，把四章知识焊成一个工程决策。

§4.8 端到端实战：双四足 ADMM 编队的完整管线 ⭐⭐⭐¶

这一节解决什么问题：前七节把每个零件讲透了，但零件不等于整机。这一节把它们组装成一条**可运行的端到端管线**——从场景设定、模型搭建、ADMM 循环、分层执行到结果分析，并给出集中式 vs 分布式的完整对比实验设计。它是累积项目的集成节，也是你把本章知识变成"能跑的代码"的施工图。本节是"轻松段"（呼应规范的节奏控制）——不引入新理论，只把已学的串成流程。

场景与目标¶

设定：两台 Go2（四足，SRB 12 维）通过一根刚性杆抬一块 5kg 负载，要保持队形（间距由杆长决定）沿一条参考路径前进。目标：(a) 实现集中式 MPC 基线；(b) 实现 consensus-ADMM 分布式版本；(c) 对比轨迹质量、计算时间、通信量。

完整管线（七步）¶

Step 0  建模（§4.5）
  - 两个 agent SRB（各 12 维）+ 杆（负载，6 维）
  - 刚性附着约束 p_i^att = p_L + R_L a_i，约束力 λ_i
  - 接触：摩擦锥金字塔 + 单边 + trot 踏步时序

Step 1  集中式基线（§4.1）
  - 堆叠联合状态/控制，写联合 QP（含编队 + 耦合 + 接触约束）
  - OSQP 一次性求解，记录求解时间（预期 ~15ms @ N=2）

Step 2  分布式问题构造（§4.2-§4.4）
  - 接口变量：耦合力 λ_i（要在 agent 间一致，满足负载动力学）
  - 每个 agent 本地 MPC = 单体 SRB MPC + ADMM 惩罚项（§4.5 的本地 QP）
  - 编队约束作用在质心（displacement 软约束，§4.2）

Step 3  ADMM 外层循环（§4.4，方式 C）
  for k in 1..K (K=5, warm-start):
    X-更新：两 agent 并行解本地 MPC（OSQP）
    Z-更新：共识平均 λ（N=2 直接平均，或共识迭代，§4.7）
    Y-更新：对偶上升
    监控残差 r^k, s^k；残差平衡自适应 ρ

Step 4  分层执行（§4.6）
  - MPC 输出：质心参考 + 足端力 f_ij + 耦合力 λ_i
  - 步态调度：trot 接触序列 + 摆动腿落脚点
  - 分布式 WBC（本地）：足端力 + λ → 关节力矩 τ_i

Step 5  仿真闭环（MuJoCo/PyBullet）
  - 下发 τ_i，仿真一步，更新状态
  - 滚动到下一控制周期（warm-start 传递 z, u, 本地解）

Step 6  对比分析
  - 队形误差（杆姿态偏差、间距偏差）
  - 计算时间（集中式总时间 vs 分布式 max-over-agents）
  - 通信量（每周期 KB）
  - ADMM 轮数 K 对 gap 的影响曲线

预期结果与解读¶

呼应原骨架的实验对比（Kim-Hamed 双 A1 搬 5kg）：

指标	集中式 MPC	分布式 ADMM-MPC	解读
轨迹最优性	最优（基准）	<3% gap	分布式近似但够好
计算时间	~15ms（单核全包）	~6ms × 2（并行）	分布式并行更快
通信	集中式上下行 ∝ N	几 KB/agent/周期 ADMM	分布式通信轻、无单点
鲁棒性	单点故障	无中心、抗节点失效	分布式胜
$N$ 扩展性	$O(N^3)$ 爆炸	近线性（并行）	分布式可扩展

关键解读：分布式不是"全面更好"，而是"用 <3% 最优性 gap，换来并行加速 + 无单点故障 + 可扩展"。$N=2$ 且有可靠中央机时集中式够用；$N$ 大或要鲁棒性时分布式必需。这正是 §4.1 陷阱 1 的量化印证。

可运行参考骨架：双积分器 ADMM 编队（Python）¶

为了让管线落地，给一个**双积分器简化版**的可运行骨架（先不上四足全动力学，验证流程）。先讲为什么这样组织，再给代码。

为什么先用双积分器而非直接上 SRB？

因为分布式 MPC 的"流程正确性"（集中/分布等价、ADMM 收敛、warm-start）
和"多足建模复杂性"（SRB、摩擦锥、踏步）是两个正交的难点。
先用双积分器（ẍ=u，无约束、线性、凸）隔离出"流程"这一维度，
把 ADMM 三步、集中-分布对比、warm-start 全部验证正确，
再把"动力学模块"从双积分器换成 SRB（§4.5），只动一个模块，不动框架。

这是"先搭骨架后填血肉"的标准工程路径——避免一上来 SRB + ADMM
同时调试，bug 来源无法区分（是建模错还是 ADMM 错？）。

import numpy as np
import osqp
from scipy import sparse

# === 双积分器 agent 的本地 MPC（带 ADMM 惩罚）===
def build_local_mpc(x0, p_des_rel, z, u_dual, rho, T=10, dt=0.1):
    """agent 本地 MPC：跟踪到目标 + 编队接口变量的 ADMM 惩罚。
    接口变量 w_i = agent 在编队边上的位置贡献（这里简化为自身位置）。
    返回 OSQP 问题（P, q, A, l, u）。"""
    # 状态 [x, v]（1D 演示），控制 u；预测方程 x(t+1)=A x(t)+B u(t)
    Ad = np.array([[1, dt], [0, 1]]);  Bd = np.array([[0.5*dt*dt], [dt]])
    # 目标代价 + ADMM 惩罚 (rho/2)||w_i - z + u_dual||^2 装进 P, q
    # （此处省略堆叠细节，留作练习；关键是惩罚项把 w_i 拉向 z - u_dual）
    P, q, A, l, uu = assemble_qp(Ad, Bd, x0, p_des_rel, z, u_dual, rho, T)
    prob = osqp.OSQP();  prob.setup(P, q, A, l, uu, warm_start=True, verbose=False)
    return prob

# === consensus-ADMM 外层循环（方式 C：固定 K 轮 + warm-start）===
def admm_formation(agents, edges, rho=1.0, K=5):
    """agents: list of dict {x0, p_des, w, z, u}（warm-start 跨周期保留）。
    edges: 编队通信边。返回各 agent 第一步控制。"""
    for k in range(K):
        # --- X-更新：每个 agent 并行解本地 MPC ---
        for i, ag in enumerate(agents):
            prob = build_local_mpc(ag['x0'], ag['p_des'], ag['z'], ag['u'], rho)
            res = prob.solve()
            ag['w'] = extract_interface(res.x)        # 本地接口变量 w_i
        # --- Z-更新：共识平均 average(w_i + u_i)（关键：含 u_i！）---
        for i, ag in enumerate(agents):
            nbrs = neighbors(i, edges)
            vals = [agents[j]['w'] + agents[j]['u'] for j in nbrs + [i]]
            ag['z'] = np.mean(vals, axis=0)           # 分布式实现见 §4.7 共识
        # --- Y-更新：对偶上升 u_i += w_i - z ---
        for ag in agents:
            ag['u'] = ag['u'] + ag['w'] - ag['z']
        # --- 残差监控（停止准则 / 自适应 rho）---
        r = np.linalg.norm([ag['w'] - ag['z'] for ag in agents])
        if r < 1e-3:
            break
    return [first_control(ag) for ag in agents]       # 取第一步控制执行

# === 闭环滚动（warm-start 跨周期传递 z, u）===
def run_closed_loop(agents, edges, n_cycles=50):
    for c in range(n_cycles):
        controls = admm_formation(agents, edges)      # z, u 保留 → warm-start
        for ag, ui in zip(agents, controls):
            ag['x0'] = step_dynamics(ag['x0'], ui)     # 仿真一步
    # 对比：集中式 build_joint_mpc(agents, edges).solve() 作为基线

代码的三个要点对应本章核心：(1) Z-更新含 + u（§4.4 陷阱 1，最隐蔽的 bug）；(2) warm_start=True + z/u 跨周期保留（§4.4 方式 C 的命脉）；(3) 闭环滚动（§4.7 容忍双重不精确的根本）。把 step_dynamics/assemble_qp 从双积分器换成 SRB（§4.5），框架不变——这就是"先骨架后血肉"。

理论-工程桥接：注意 admm_formation 里 Z-更新写的是 np.mean（中央求平均），这在仿真里方便，但真机要换成 §4.7 的分布式共识 consensus_average(w+u, W, L_cons)。这个"仿真用中央平均、真机用分布式共识"的替换是从仿真到部署最关键的一步——很多在仿真里 work 的分布式 MPC，正是栽在这一步（以为 np.mean 就是分布式，忽略了它隐含中央节点）。骨架里特意保留这个接口，提醒你部署时必须替换。

⚠️ 常见陷阱¶

陷阱 1（编程陷阱）：集中式和分布式用了不同的代价/约束，对比不公平。 - 错误描述：两个实现的 $Q,R$ 权重、约束、踏步序列不一致就比较。 - 现象/后果：gap 看起来很大或很小，但其实是建模差异造成的，不反映分布式本身的近似损失。 - 根本原因：对比实验必须控制变量——除"集中 vs 分布求解方式"外其余完全相同。 - 正确做法：两个实现共用同一套模型、权重、约束、踏步序列、参考轨迹，只在"求解器"层面不同。

陷阱 2（思维陷阱）：用单次求解时间下结论，忽略 warm-start 的暖机效应。 - 错误描述：测第一个控制周期（冷启动）的时间作为代表。 - 现象/后果：冷启动时 ADMM 轮数多、时间长，低估了稳态性能。 - 根本原因：实时 MPC 稳态靠 warm-start，首周期不代表稳态。 - 正确做法：跑若干周期暖机后，测稳态的平均/最坏求解时间。

陷阱 3（编程陷阱）：MuJoCo 仿真步长与控制周期不匹配。 - 错误描述：仿真步长（如 0.001s）和 MPC 控制周期（如 0.02s）没对齐，或每仿真步都重解 MPC。 - 现象/后果：要么 MPC 算不过来（每步都解），要么控制滞后（步长太大）。 - 根本原因：仿真步长（物理积分精度）和控制周期（决策频率）是两个概念。 - 正确做法：仿真小步长积分（1kHz），WBC 跟着 1kHz，MPC 每 N 个仿真步解一次（如每 20 步=50Hz），命令在周期间保持。

练习¶

（端到端集成） 按上面七步实现双积分器简化版（先不上四足全动力学，用双积分器代替 SRB 验证流程）：双积分器编队 + 虚拟"杆"耦合约束 + ADMM。跑通集中式和分布式，复现对比表的趋势。这是累积项目的集成里程碑。
（升级到 SRB） 把双积分器换成 SRB（§4.5），加入摩擦锥、踏步、耦合力。在 MuJoCo 双 Go2 场景验证。对比集中式 vs 分布式的杆姿态保持精度。对应原骨架"A 型·双四足编队"练习。
（鲁棒性实验） 在分布式版本运行中，故意让一台机器人"掉线"几个周期（停止通信），观察编队如何降级（另一台靠最后已知信息维持）和恢复。对比：集中式若中央节点掉线则全队瘫痪。亲手验证分布式的无单点故障优势。

本章常见误解汇总¶

误解	正确理解	出处
分布式 MPC 总比集中式好	用 <3% 最优性 gap 换可扩展性 + 鲁棒性 + 无单点；$N$ 小且中央可靠时集中式更优	§4.1
"动力学解耦"等于"问题可分"	多足编队动力学解耦但约束/代价耦合；耦合恰恰来自编队约束，必须协调	§4.1
预测交换里能拿到邻居实时值	真实系统有一步通信延迟，用的是邻居上一轮的预测	§4.1/§4.6
编队就是给每个机器人一个绝对目标点	编队是相对几何约束，整体位姿是自由（或由领航者定）的自由度	§4.2
distance-based 代价直接用欧氏范数	范数在重合点不可微，必须用平方距离	§4.2
四足编队硬约束瞬时质心相对位置	质心随踏步起伏，硬约束会与平衡打架；用软约束 + 频段分离	§4.2/§4.5
对偶分解要松弛所有约束	只松弛耦合约束，本地约束保留在各子问题里	§4.3
对偶变量 $\lambda$ 是自由超参	$\lambda$ 是有物理含义的影子价格/约束力，可 warm-start	§4.3
ADMM 是全新方法	ADMM = 对偶分解 + 增广项 + 辅助变量，修复对偶分解的三个弱点	§4.4
Z-更新漏对偶项无所谓	必须 average$(\mathbf{w}_i+\mathbf{u}_i)$，漏 $\mathbf{u}_i$ 收敛到错误点	§4.4
$\rho$ 越大收敛越快	$\rho$ 是"一致 vs 自主"权重，过大让本地代价被淹没、QP 病态	§4.4
ADMM 在非凸下也保证收敛	多足接触非凸，ADMM 无收敛保证；靠凸化 + warm-start + 固定轮数	§4.4/§4.7
耦合力 $\lambda$ 是独立控制输入	$\lambda$ 是约束力，由刚性约束 + 动力学解出，受严格限制	§4.5
摩擦锥用精确二阶锥	二阶锥使问题变 SOCP（慢），用金字塔线性近似保持 QP	§4.5
分布式 WBC 也要通信	协调已在 MPC 层由 $\lambda$ 完成，WBC 纯本地、零通信	§4.6
MPC 足端力可直接发给电机	电机吃关节力矩，须经 WBC 映射 + 补全阶细节	§4.6
ADMM 的平均必须有中央节点	用第 2 章的分布式共识算平均，无需中心	§4.7
物理队形紧密 = 通信质量好	通信图与物理队形是两张图，协调质量由通信图 $\lambda_2$ 决定	§4.7
长时域 + warm-start = 稳定性证明	那是实践充分条件，非理论保证；非凸下尤其无保证	§4.7

本章小结¶

本章把"一群会走路的机器人如何只靠彼此通信、在毫秒级控制周期内联合保持队形协同前进"这个问题，从动机到算法到多足落地，完整走了一遍。主线是：集中式不可扩展（§4.1）→ 编队建模制造耦合（§4.2）→ 对偶分解拆耦合但慢（§4.3）→ ADMM 又快又稳地拆（§4.4）→ SRB 网络把多足装进框架（§4.5）→ 分层控制接到关节力矩（§4.6）→ 通信拓扑与稳定性收口（§4.7）→ 端到端集成（§4.8）。

三条贯穿全章的认知主线：

分布式是权衡，不是免费午餐：用最优性 gap 换可扩展性、鲁棒性、模块化。选集中还是分布，取决于 $N$、通信可靠性、对最优性的需求。
ADMM 的协调步本质是共识：第 2 章不是前置而是内核。外层 ADMM 优化分解、内层共识算平均，两者吃同一张通信图的红利（$\lambda_2$）。
多足的难点在频段分离与非凸：编队/协同是低频群体行为（SRB 层 + 通信协调），平衡是高频个体行为（WBC 层 + 本地）；接触约束非凸使理论保证失效，工程靠凸化 + warm-start + 够用就停。

术语速查表¶

术语	一句话定义
分布式 MPC（DMPC）	每个 agent 解本地 MPC + 邻居交换信息协调，替代集中式联合 MPC
预测交换（prediction exchange）	agent 广播自己的预测轨迹，邻居用它构造本地编队项
对偶分解（dual decomposition）	拉格朗日松弛耦合约束，使问题可分，用对偶变量（价格）协调
影子价格（shadow price）	对偶变量的经济学解释——维持约束的边际代价
ADMM	交替方向乘子法 = 对偶分解 + 增广二次项 + 辅助变量
consensus-ADMM	用全局共识变量 $\mathbf{z}$ 把多边耦合化为对中心的单边耦合
罚参数 $\rho$	ADMM 中"全局一致 vs 本地自主"的权重
残差平衡（residual balancing）	让原始/对偶残差量级相当的自适应 $\rho$ 规则
SRB 网络	$N$ 个单刚体 + 共享负载 + 刚性耦合约束的互联模型
耦合力 $\lambda$	维持刚性连接的约束力（拉格朗日乘子），协同的物理本质
摩擦锥金字塔	用线性不等式近似摩擦锥，保持 QP 结构
单边接触	脚只能推不能拉地面（$f_z\ge0$）；摆动腿力为零
分层控制	高层 MPC（协调、通信）→ 中层步态 → 低层 WBC（执行、本地）
分布式 WBC	低层全身控制纯本地、无通信（协调已由 MPC 的 $\lambda$ 完成）
兼容性约束（compatibility constraint）	限制实际轨迹与广播假设轨迹的偏差，保递归可行
递归可行性	当前周期可行则下周期仍可行
终端代价/终端集	MPC 末端加的代价/约束，保有限时域逼近无限时域稳定

知识点总表¶

编号	知识点	核心要点	对应节	难度
K1	集中式三重瓶颈	$O(N^3)$ 爆炸 + 单点故障 + 不可模块化	§4.1	⭐⭐
K2	预测交换范式	交换预测轨迹，编队耦合退化为本地项；软约束、有一步延迟	§4.1	⭐⭐⭐
K3	编队三范式	position/displacement/distance：信息越少、自由度越多、越非凸	§4.2	⭐⭐⭐
K4	编队约束的耦合本质	$J_{\text{form}}$ 显含多 agent 状态 → 耦合 → 需协调	§4.2	⭐⭐
K5	对偶分解	松弛耦合约束 → 可分 → 价格迭代（子梯度）；慢、脆、中间不可行	§4.3	⭐⭐⭐⭐
K6	影子价格 = 约束力	对偶变量在多足里是物理可测的耦合力 $\lambda$	§4.3/§4.5	⭐⭐⭐
K7	consensus-ADMM 三步	X（本地 MPC+惩罚）/Z（共识平均）/Y（对偶上升）	§4.4	⭐⭐⭐⭐
K8	$\rho$ 与残差平衡	$\rho$ 是一致-自主权重；残差平衡自适应；凸 $O(1/k)$	§4.4	⭐⭐⭐
K9	非凸无保证	接触约束非凸 → ADMM 无收敛保证 → 工程靠凸化+warm-start	§4.4/§4.7	⭐⭐⭐⭐
K10	SRB 网络模型	$N$ SRB + 负载 + 刚性约束 → 耦合力 $\lambda$	§4.5	⭐⭐⭐⭐
K11	约束力求解	$\lambda=-(JM^{-1}J^\top)^{-1}(JM^{-1}Q+\dot J\dot q)$，约束微分两次	§4.5	⭐⭐⭐⭐
K12	接触约束三件套	摩擦锥金字塔 + 单边接触 + 踏步时序；固定序列→凸 QP	§4.5	⭐⭐⭐
K13	频段分离	编队低频（质心层）+ 平衡高频（WBC 层）	§4.2/§4.5	⭐⭐⭐
K14	分层数据流	MPC（通信）→步态→WBC（本地）；通信预算最小化	§4.6	⭐⭐⭐
K15	分布式 WBC 纯本地	协调已由 MPC 的 $\lambda$ 完成，WBC 零通信	§4.6	⭐⭐⭐
K16	ADMM 协调步 = 共识	Z-更新用第 2 章分布式共识实现，无需中心	§4.7	⭐⭐⭐⭐
K17	两层迭代	外层 ADMM 优化、内层共识平均；$L_{\text{cons}}$ vs $K$ 权衡	§4.7	⭐⭐⭐
K18	拓扑影响	$\lambda_2$/直径/时延/丢包影响收敛与稳定；通信图≠物理队形	§4.7	⭐⭐⭐
K19	DMPC 稳定性支柱	递归可行性（兼容性约束）+ 终端代价；分布式不变集难算	§4.7	⭐⭐⭐⭐
K20	端到端管线	建模→集中基线→ADMM→分层执行→对比	§4.8	⭐⭐⭐

累积项目：本章新增模块¶

项目背景：本系列的累积项目是 Mini-MultiBot（第 13 章综合实战），逐章搭建一个完整的多机器人协作系统。本章为它新增"分布式 MPC 编队"模块，目录 mini_multibot/formation_dmpc/。

本章新增模块结构：

mini_multibot/formation_dmpc/
├── models/
│   ├── srb_agent.py            # 单 agent SRB 动力学（12 维）+ 接触约束
│   ├── srb_network.py          # 互联 SRB 网络（N agent + 负载 + 耦合约束）
│   └── coupling_force.py       # 约束力 λ 解析求解（§4.5 公式）
├── formation/
│   ├── formation_cost.py       # 三范式编队代价（displacement/distance）
│   └── leader_follower.py      # 领航-跟随锚定
├── solvers/
│   ├── centralized_mpc.py      # 集中式联合 MPC 基线（§4.1）
│   ├── dual_decomp.py          # 对偶分解（§4.3，教学对照用）
│   ├── admm_coordinator.py     # consensus-ADMM 协调器（§4.4，核心）
│   └── residual_balance.py     # 残差平衡自适应 ρ（§4.4）
├── hierarchy/
│   ├── gait_scheduler.py       # 中层步态调度（§4.6）
│   └── distributed_wbc.py      # 低层分布式 WBC（§4.6，本地）
├── comms/
│   ├── topology.py             # 通信拓扑 + Metropolis 权重（§4.7）
│   └── consensus_average.py    # ADMM Z-更新的分布式共识实现（§4.7）
└── experiments/
    ├── two_go2_carry.py        # 双 Go2 抬杆端到端（§4.8）
    └── compare_cent_dist.py    # 集中 vs 分布对比实验（§4.8）

与前几章模块的衔接：

comms/consensus_average.py 直接复用第 2 章累积项目的共识实现（consensus/ 模块）——ADMM 的 Z-更新就是共识平均。
comms/topology.py 复用第 1 章的图/Laplacian 工具（graph/ 模块）。
本章的 solvers/admm_coordinator.py 是第 2 章 ADMM toy 实现的"工程升级版"——从标量 QP 升级到多足轨迹优化。

本章交付物：跑通 experiments/compare_cent_dist.py，复现集中 vs 分布的对比表（轨迹 gap <3%、分布式并行更快、通信几 KB）。

为第 5 章预留接口：models/coupling_force.py 算出的耦合力 $\lambda$ 将被第 5 章（协同搬运）的内力分配模块消费——本章管"协调运动产生的总耦合力"，第 5 章管"把这个力分解成运动力 + 受控内力"。

延伸阅读¶

按难度和方向分类。⭐ 数表示阅读难度。

核心论文（必读）：

⭐⭐⭐ Kim, Fawcett, Kamidi, Ames, Hamed, "Layered Control for Cooperative Locomotion of Two Quadrupedal Robots: Centralized and Distributed Approaches", T-RO 2023（arXiv:2211.06913）——本章主线，互联 SRB 网络 + 集中/分布 MPC + 分层控制的完整范本。精读 Section III（SRB 网络）、IV（MPC）、V（低层控制）。
⭐⭐⭐⭐ Boyd, Parikh, Chu, Peleato, Eckstein, "Distributed Optimization and Statistical Learning via ADMM", Found. Trends ML 2011——ADMM 与 consensus-ADMM 的权威教程，§4.3-§4.4 的理论源头。读 §3（标准 ADMM）、§7（consensus）、§3.4（残差平衡）。

分布式 MPC 基础：

⭐⭐⭐ Dunbar & Murray, "Distributed Receding Horizon Control for Multi-Vehicle Formation Stabilization", Automatica 2006——预测交换范式奠基。
⭐⭐⭐ Keviczky, Borrelli, Balas, "Decentralized Receding Horizon Control for Large Scale Dynamically Decoupled Systems", Automatica 2008——DMPC 编队系统化。
⭐⭐⭐⭐ Conte, Jones, Morari, Zeilinger, "Distributed Synthesis and Stability of Cooperative DMPC", Automatica 2016——DMPC 稳定性综合（终端代价、递归可行）。
⭐⭐⭐ "A Brief Tutorial on Consensus ADMM for Distributed Optimization with Applications in Robotics", arXiv:2410.03753——机器人语境的 consensus-ADMM 教程，适合补 §4.4。

多足协同前沿：

⭐⭐⭐ De Vincenti & Coros, "Centralized MPC for Collaborative Loco-Manipulation", RSS 2023——集中式 SQP + $SO(3)$ 李群积分，对照集中式方案。
⭐⭐⭐ Turrisi 等, "PACC: Passive-Arm Approach for High-Payload Collaborative Carrying", IROS 2024——被动臂方案，免刚性耦合建模（§4.5 类比的边界案例）。
⭐⭐⭐⭐ Imran, Hamed 等, "ADMM-Based Distributed MPC with Control Barrier Functions for Safe Multi-Robot Quadrupedal Locomotion", arXiv 2025/26——ADMM + CBF 安全的多足分布式 MPC，§4.7 非凸/安全的前沿。
⭐⭐⭐ Fawcett, Amanzadeh, Hamed 等, "Distributed Data-Driven Predictive Control for Multi-Agent Collaborative Legged Locomotion"——数据驱动绕开精确建模。

单体 SRB MPC 背景（若不熟先补）：

⭐⭐ Di Carlo, Wensing, Katz, Bledt, Kim, "Dynamic Locomotion in the MIT Cheetah 3 via Convex MPC", IROS 2018——SRB 凸 MPC 标准模板。
⭐⭐⭐ "Convex MPC of Single Rigid Body Model on SO(3)"——SRB 在 $SO(3)$ 上的凸 MPC。

代码仓库：

⭐⭐⭐ leggedrobotics/ocs2——ETH RSL 的多体 MPC 框架，ocs2_centauro（四足+臂）可作多足 MPC 扩展基础。
⭐⭐⭐ osqp/osqp——本章本地 QP 求解器。
⭐⭐ stack-of-tasks/pinocchio——刚体动力学库，WBC 实现用。

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
第 5 章协同搬运与力控	直接延伸：本章的耦合力 $\lambda$ → 第 5 章的运动力/内力分解；grasp matrix 是 $\lambda$ 协调的进一步形式化	K6 影子价格=约束力、K10 SRB 网络、K11 约束力求解
第 6/7 章异构多机协同	本章双四足同构 → 异构（不同机型）；分布式/模块化正是异构的前提	K1 模块化、K7 ADMM、K14 分层
第 9 章多足 Loco-Manipulation	本章编队（移动）+ 第 5 章搬运（操作）→ 移动操作一体；分层架构直接复用	K10 SRB 网络、K14 分层数据流
第 12 章 MARL 与规控混合	本章基于模型的 DMPC ↔ MARL 的学习式协调；可混合（MPC 安全层 + MARL 策略）	K2 预测交换、K18 通信拓扑
第 13 章 Mini-MultiBot 综合	本章 `formation_dmpc/` 模块并入综合系统	全部累积项目模块

前向预告：第 5 章会发现，本章把"两台机器人对负载的总耦合力 $\lambda$ 协调一致"还不够——总力相同，但**怎么在两台机器人之间分配**（一台多使劲、一台少使劲）、以及它们之间的**内力**（互相挤压/拉扯负载但不移动它）是否受控，是搬运特有的新问题。本章 §4.5 练习 4 已埋下伏笔：$\lambda_1,\lambda_2$ 的某些组合是运动力（搬动负载）、某些是内力（内耗）。第 5 章的 grasp matrix $G$ 就是把这个分解形式化的工具。

🔧 故障排查手册¶

分布式 MPC 多足编队的调试是出了名的难——问题可能出在建模、求解、通信、分层任何一环。下面按"症状→可能原因→排查步骤→相关章节"组织 7 个高频故障场景。

场景 1：ADMM 几百轮不收敛，残差降不下去¶

项	内容
症状	原始残差 $r^k$ 或对偶残差 $s^k$ 长期不下降，编队有持续偏差，单周期 ADMM 跑不完
可能原因	(1) $\rho$ 取得不当（过大/过小）；(2) Z-更新漏了对偶项 $\mathbf{u}_i$；(3) 问题非凸（接触约束）ADMM 本就不保证收敛；(4) 冷启动每周期从零开始
排查步骤	① 先用标量 toy 例子（§4.4 练习 1）验证 ADMM 实现正确（收敛到已知值）；② 检查 Z-更新是否为 average$(\mathbf{w}_i+\mathbf{u}_i)$ 而非 average$(\mathbf{w}_i)$；③ 扫描 $\rho\in\{0.1,1,10,100\}$ 看残差曲线，或开残差平衡；④ 确认是否 warm-start（保留上周期 $\mathbf{z},\mathbf{u}$）；⑤ 若问题非凸，固定接触序列凸化，接受固定轮数有 gap
相关章节	§4.4（ADMM、$\rho$、残差平衡）、§4.7（非凸无保证）

场景 2：编队队形漂移/崩溃¶

项	内容
症状	机器人间相对位置逐渐偏离期望队形，或完全散开
可能原因	(1) 编队约束写成绝对位置（伪分布式）；(2) 各 agent 独立解、没真正协调（漏了耦合处理）；(3) 编队权重 $w_{ij}$ 太小、被自身代价淹没；(4) distance-based 收敛到旋转后的构型（保留旋转自由度）
排查步骤	① 确认编队代价显含邻居状态 $\mathbf{p}_j$（相对量），而非绝对参考点；② 检查 ADMM 是否真的在跑（Z/Y 更新有没有执行、邻居数据有没有用上）；③ 加大编队权重 $w_{ij}$ 或硬化关键约束；④ 若用 distance-based，确认旋转自由度是否是期望的（否则改 displacement-based 锚定朝向）
相关章节	§4.2（编队建模、三范式）、§4.1（耦合的必要性）

场景 3：负载乱飞/机器人被"拽飞"（仿真）¶

项	内容
症状	双足抬负载仿真中，负载或机器人速度爆炸、数值发散
可能原因	(1) 耦合力 $\lambda$ 当成自由控制变量乱优化；(2) 刚性约束建错（绳索当刚性杆）；(3) 约束力公式实现错（$J_\Phi$、$\dot J_\Phi$ 算错）；(4) 集中/分布的耦合力不一致（违反第三定律）
排查步骤	① 确认 $\lambda$ 由约束 + 动力学解出（§4.5 公式），不是自由变量；② 检查连接类型与约束类型匹配（刚性→等式、绳索→单边）；③ 用一维 toy（§4.5 练习 1）验证约束力公式；④ 检查两 agent 的 $\lambda_i$ 是否满足负载动力学 $m_L\ddot{\mathbf{p}}_L=-\sum_i\lambda_i+m_L g$
相关章节	§4.5（SRB 网络、约束力、连接类型）

场景 4：MPC 给摆动腿分配了非零力 / 机器人迈步诡异¶

项	内容
症状	悬空的摆动腿被分配接触力；或机器人动作别扭、踏不出正常步态
可能原因	(1) 没区分支撑/摆动腿，全加了摩擦锥；(2) 踏步时序 contact_schedule 错或没传给 MPC；(3) 编队约束加在足端而非质心，与踏步冲突
排查步骤	① 检查摆动相是否强制 $\mathbf{f}_{ij}=\mathbf{0}$；② 打印 contact_schedule 确认支撑/摆动相位正确；③ 确认编队约束作用在质心层、足端细节交 WBC；④ 检查 MPC 与步态调度的相位是否同步
相关章节	§4.5（接触约束、踏步时序）、§4.2/§4.6（频段分离）

场景 5：分布式版本计算/通信扛不住实时¶

项	内容
症状	分布式 MPC 单控制周期算不完，或通信延迟导致控制滞后
可能原因	(1) ADMM 轮数 $K$ 太多 / 内层共识 $L_{\text{cons}}$ 太多；(2) 没 warm-start，每周期冷启动；(3) 本地 MPC 没凸化（用了 SOCP/非凸求解）；(4) 通信拓扑差（$\lambda_2$ 小）使共识慢
排查步骤	① 降 $K$ 到 3-5 + warm-start（方式 C）；② 摩擦锥用金字塔保证本地 QP；③ 测量内外层轮数，用 $L_{\text{cons}}=1$（交换一次预测）；④ 改善通信拓扑（加边提升 $\lambda_2$）；⑤ 暖机后再测稳态时间（不看冷启动）
相关章节	§4.4（方式 C、warm-start）、§4.5（金字塔凸化）、§4.7（拓扑、两层迭代）

场景 6：集中式与分布式对比 gap 异常大¶

项	内容
症状	分布式比集中式轨迹差很多（远超 3-5%），怀疑实现错
可能原因	(1) 两实现的模型/权重/约束/踏步不一致（对比不公平）；(2) ADMM 轮数太少未收敛；(3) $\rho$ 不当；(4) 分布式的编队约束建模与集中式不等价
排查步骤	① 严格控制变量——两实现共用同一套模型/权重/约束/参考；② 增大 $K$ 看 gap 是否随轮数减小（确认是"未收敛"而非"实现错"）；③ 调 $\rho$ / 开残差平衡；④ 用同一组随机初值多次实验取统计
相关章节	§4.8（对比实验设计、陷阱）、§4.4（收敛）

场景 7：真机上时好时坏、难复现的震荡¶

项	内容
症状	仿真正常，真机偶发震荡或编队抖动，难复现
可能原因	(1) 通信丢包/延迟未补偿；(2) 一步通信延迟未在控制器设计中考虑；(3) 通信时延接近临界 $\tau^\star$；(4) 非凸下 ADMM 偶发卡局部点
排查步骤	① 记录通信日志查丢包/延迟；② 确认算法显式假设邻居信息滞后一步（§4.6）；③ 用 $\tau^\star=\pi/(2\lambda_N)$ 评估时延裕度，必要时降控制频率或改拓扑；④ 加安全监控层（如 CBF）兜底；⑤ 多次运行统计震荡触发条件
相关章节	§4.6（一步延迟）、§4.7（时延稳定性、丢包鲁棒、非凸坦白）

API 速查表¶

本章涉及的核心 API / 函数签名（伪代码 + 真实库）。

consensus-ADMM 协调器（§4.4，本章自实现）：

class ADMMCoordinator:
    def run(self, agents, rho, K) -> None
        """跑 K 轮 ADMM（方式 C，warm-start）。
        agents[i]: {w_local, z_consensus, u_dual}（轨迹）。
        每轮：X-更新（并行本地 MPC）→ Z-更新（共识平均）→ Y-更新（对偶上升）。"""

    def solve_local_mpc(self, i, agent_data, rho) -> Trajectory
        """agent i 本地 MPC + ADMM 二次惩罚 (rho/2)||w_i - z_i + u_i||²。"""

    def compute_residuals(self, agents, rho) -> (r_norm, s_norm)
        """原始残差 r = w_i - z_i；对偶残差 s = rho(z^k - z^{k-1})。"""

残差平衡自适应 ρ（§4.4）：

def residual_balance(rho, r_norm, s_norm, mu=10, tau=2) -> float
    """if r > mu*s: rho *= tau; elif s > mu*r: rho /= tau; return rho。"""

约束力求解（§4.5）：

def coupling_force(J_phi, M, Q, Jdot_phi, qdot) -> lambda_vec
    """λ = -(J M⁻¹ Jᵀ)⁻¹ (J M⁻¹ Q + J̇ q̇)。维持刚性约束的拉格朗日乘子。"""

编队代价（§4.2）：

def formation_cost_displacement(p, edges, d_des, w) -> float
    """Σ_{(i,j)∈E} w_ij ||(p_j - p_i) - d_ij||²。二次、凸。"""

def formation_cost_distance(p, edges, d_des, w) -> float
    """Σ w_ij (||p_j - p_i||² - ||d_ij||²)²。四次、非凸。用平方距离避免不可微。"""

通信拓扑与共识平均（§4.7）：

def metropolis_weights(adjacency) -> W
    """构造双随机权重矩阵 W（第 2 章）。W_ij = 1/(1+max(d_i,d_j)) for (i,j)∈E。"""

def consensus_average(v_local, W, L_cons) -> v_avg
    """跑 L_cons 轮 v ← W·v 分布式算平均（误差 ∝ |ρ₂(W)|^L）。"""

def algebraic_connectivity(L) -> lambda_2
    """Laplacian L 的第二小特征值，决定收敛速率。"""

OSQP（本地 QP 求解，真实库）：

import osqp
prob = osqp.OSQP()
prob.setup(P, q, A, l, u, warm_start=True)  # P:Hessian, A:约束, [l,u]:界
prob.warm_start(x=x_prev)                    # ★ warm-start，实时 MPC 命脉
res = prob.solve()                           # res.x: 解, res.info.status

pinocchio（刚体动力学，WBC 用，真实库）：

import pinocchio as pin
pin.crba(model, data, q)        # 质量矩阵 M
pin.nonLinearEffects(model, data, q, v)  # 非线性项 h（科氏 + 重力）
pin.computeJointJacobians(model, data, q)  # 接触/附着点 Jacobian

研究实践建议¶

按学习目标分层给建议。

入门（先会跑）：

从**双积分器**简化版起步（§4.8 练习 1）——不要一上来就上四足全动力学。先把"集中式 → 对偶分解 → ADMM → 分布式"的流程在双积分器上跑通，理解每一步在干什么。
严格按"标量 toy → 双积分器 → SRB"三级递进验证。每级都用已知答案（toy 的解析解、集中式的最优解）校验分布式实现。
优先实现**方式 C**（固定轮数 + warm-start），不要追求"跑到收敛"——实时 MPC 不需要也不允许。

进阶（理解为什么）：

亲手推一遍 §4.3 对偶分解和 §4.4 ADMM 的增广拉格朗日，理解 ADMM 的每一步在修复对偶分解的哪个弱点。背公式没用，要理解设计动机。
做 $\rho$ 扫描实验（§4.4 练习 2），亲眼看 $\rho$ 对原始/对偶残差的不同影响，建立"$\rho$ 是权重不是速度旋钮"的直觉。
推 §4.5 的约束力公式（练习 1），理解"影子价格 = 约束力"这个理论-物理对应——这是本章最深刻的洞察之一。

研究（推动前沿）：

本章反复强调的**非凸下 ADMM 无收敛保证**是真问题。若做研究，CBF-ADMM、数据驱动 DMPC、分布式不变集计算是有价值的方向。
关注 Hamed 团队（Virginia Tech ARCOL）的后续工作——他们是这个方向的当前标杆，新论文持续推进 $N$ 的规模、障碍环境、安全保证。
弥合"实践稳定"与"理论保证"的差距（给分布式多足 MPC 严格的递归可行性 + 稳定性证明）是开放且有影响力的问题。
实验上，从仿真（MuJoCo 双 Go2）做到真机有巨大鸿沟（通信、状态估计、模型误差）。如果有真机条件，通信层的工程（丢包补偿、时延管理）往往比算法本身更决定成败。

本质洞察（给研究者）：分布式多足 MPC 这个方向，最迷人也最危险的地方在于——它在仿真里"work"和在真机上"work"是两件事，而论文常常模糊这个界限。本章给你的清醒认知（非凸无保证、实践充分≠理论保证、通信图≠物理队形、warm-start 是命脉）不是悲观，而是让你在读论文、做研究时能问出对的问题："这个收敛保证是凸假设下的吗？""真机的通信延迟在控制器里建模了吗？""对比实验控制变量了吗？"能问出这些问题，你就从"会用方法"进阶到了"会判断方法"。

版本信息速查¶

项	版本 / 出处	说明
文档类型	算法工程教学	text:code ≥ 60:40
主线论文	Kim-Fawcett-Hamed, T-RO 2023（arXiv:2211.06913）	互联 SRB 网络 + 集中/分布 MPC + 分层控制
ADMM 理论	Boyd et al., Found. Trends ML 2011	consensus-ADMM、残差平衡
DMPC 奠基	Dunbar-Murray 2006；Keviczky-Borrelli 2008	预测交换范式
单体 SRB MPC	Di Carlo et al., IROS 2018（MIT Cheetah 3）	SRB 凸 MPC 模板
QP 求解器	OSQP（osqp.org）	本地 QP，支持 warm-start
刚体动力学	pinocchio	WBC 实现
MPC 框架（参考）	OCS2（leggedrobotics/ocs2）	多体 MPC，ocs2_centauro
仿真	MuJoCo / PyBullet	双 Go2 编队验证
前置章节	第 1 章（图论）、第 2 章（共识/ADMM）、单体四足 MPC	硬依赖
后续章节	第 5 章（协同搬运）、第 9 章（Loco-Manipulation）、第 13 章（综合实战）	直接延伸

结语：本章把第 1-3 章的"看系统、连续协调、离散决策"工具，第一次综合应用到"会走路的机器人编队"上。你学到的不只是一套算法（分布式 MPC + ADMM + SRB 网络 + 分层控制），更是一种**把紧耦合的全局问题安全地切成可并行的局部问题**的思维方式——这种思维在任何"多个智能体要协同又要各自自主"的系统里都适用。下一章（协同搬运）会沿着本章的耦合力 $\lambda$ 继续深入，从"一起走到哪"走向"一起使多大劲"。带着本章的清醒认知（分布式是权衡、非凸无保证、通信图是地基）前进。

符号	含义	首见
\(N\)	编队中机器人（agent）数量	§4.1
\(T\)	MPC 预测时域步数（horizon）	§4.1
\(i,j\)	agent 索引；\(j\in\mathcal{N}_i\) 表示 \(j\) 是 \(i\) 的通信邻居	§4.1
\(\mathbf{x}_i\)	agent \(i\) 的状态（SRB 为 12 维：\([\mathbf{p}_i,\boldsymbol{\Theta}_i,\dot{\mathbf{p}}_i,\boldsymbol{\omega}_i]\)）	§4.1
\(\mathbf{u}_i\)	agent \(i\) 的控制（足端接触力堆叠）	§4.1
\(\mathbf{X}_i,\mathbf{U}_i\)	agent \(i\) 在整个时域 \(T\) 上的状态/控制轨迹（堆叠）	§4.1
\(J_i(\cdot)\)	agent \(i\) 的本地代价	§4.1
\(\mathbf{p}_i\)	agent \(i\) 的质心（CoM）位置 \(\in\mathbb{R}^3\)	§4.2
\(\mathbf{d}_{ij}\)	期望的 \(i\)-\(j\) 相对位移（编队几何）	§4.2
\(\boldsymbol{\lambda}\)	拉格朗日乘子；§4.3 为对偶变量，§4.5 为刚性耦合约束力	§4.3
\(g(\boldsymbol{\lambda})\)	对偶函数	§4.3
\(\mathbf{z}\)	ADMM 的全局一致（consensus）变量	§4.4
\(\mathbf{y}_i\)	agent \(i\) 的 ADMM 对偶变量（缩放形式 \(\mathbf{u}_i^{\text{scaled}}\)）	§4.4
\(\rho\)	ADMM 罚参数（penalty parameter）	§4.4
\(r^k,s^k\)	第 \(k\) 轮的原始残差（primal residual）、对偶残差（dual residual）	§4.4
\(\mathbf{f}_{ij}\)	agent \(i\) 第 \(j\) 条腿的接触力	§4.5
\(\mu\)	摩擦系数	§4.5
\(\mathbf{p}_i^{\text{att}}\)	agent \(i\) 在共享负载上的附着点位置	§4.5
\(L\)	通信图 Laplacian \(=D-A\)	§4.7
\(\lambda_2\)	\(L\) 的代数连通性（第二小特征值）	§4.7

主线	解决的问题	关键工具	对应小节
为什么要分布式	集中式 MPC 解一个 \(N\) 机联合 QP 为什么不可行？	\(O(N^3)\) 爆炸、单点故障、通信瓶颈、预测交换	§4.1
编队怎么建模	"保持队形"如何写进 MPC 的代价与约束？	position/displacement/distance 三范式、耦合约束	§4.2
对偶分解	带耦合约束的联合优化如何拆成本地子问题？	拉格朗日松弛、影子价格、对偶子梯度、慢收敛	§4.3
ADMM 求解	如何让分解又快又稳地收敛？	consensus-ADMM 三步、增广拉格朗日、\(\rho\) 调节	§4.4
多足编队建模	多足特有的 CoM 协调与接触约束怎么进 MPC？	SRB 网络、刚性耦合力 \(\boldsymbol{\lambda}\)、摩擦锥、单边接触	§4.5
分层控制	MPC 的 SRB 级命令如何变成关节力矩？	高层 MPC→步态调度→分布式 WBC、纯本地计算	§4.6
拓扑与稳定性	通信质量如何影响收敛？编队稳不稳？	共识接口、\(\lambda_2\)/时延/丢包、递归可行性、终端代价	§4.7

范式	约束量	需要的信息	队形保留的自由度	通信/传感要求
Position-based	绝对位置 \(\mathbf{p}_i\)	全局坐标	无（完全锁定）	全局定位（GPS/动捕）
Displacement-based	相对位移 \(\mathbf{p}_j-\mathbf{p}_i\)	共同朝向 + 相对位置	平移	邻居相对位置 + 朝向一致
Distance-based	相对距离 \(\\|\mathbf{p}_j-\mathbf{p}_i\\|\)	仅相对位置	平移 + 旋转	邻居相对距离

对偶分解的弱点	ADMM 的修复
子梯度法慢（\(O(1/\sqrt k)\)），需调步长	增广二次项让对偶可微，固定 \(\rho\) 即可，凸情形线性收敛
本地不严格凸则震荡	增广项提供正则（强凸化本地子问题），稳定
中间迭代不满足耦合约束	原始残差单调下降，中间解约束违反更小、更可控

方法	公式	优劣
前向欧拉（forward Euler）	\(\mathbf{x}(t{+}1)=\mathbf{x}(t)+\Delta t\cdot f\)	最简单，大 \(\Delta t\) 不稳定；多足 MPC 常用（\(\Delta t\) 小）
零阶保持（ZOH）	对 LTI 精确离散 \(A_d=e^{A\Delta t}\)	线性化后用，更精确
半隐式/RK	更高阶	更准但更重，非线性 MPC 用

范式	做法	优劣
加权 WBC（weighted）	所有任务放进一个 QP 目标，用权重 \(w\) 平衡	简单、单 QP；但权重难调，软优先级
严格优先级 WBC（hierarchical/null-space）	高优先级任务先满足，低优先级在高优先级的零空间里做	严格优先级（平衡 > 跟踪）；多级 QP，复杂

拓扑量（第 1-2 章）	对 DMPC 的影响	工程含义
代数连通性 \(\lambda_2\)	越大，内层共识与外层 ADMM 收敛越快	编队拓扑尽量连通（多加几条通信边）
直径（diameter）	越大，信息传遍全网越慢	长链拓扑协调迟钝，避免
临界时延 \(\tau^\star=\pi/(2\lambda_N)\)	通信时延超过它，共识/协调发散	高频控制下时延预算紧张，需评估
丢包率	破坏双随机性/对偶配平，残差不收敛	用对丢包鲁棒的 ADMM 变体（异步、确认重传）

策略	外层 \(K\)	内层 \(L_{\text{cons}}\)	总通信	适用
内层精确	小（3-5）	大（\(\ge10\)）	中-高	拓扑好（\(\lambda_2\) 大）、\(L_{\text{cons}}\) 本就小
内层不精确（inexact）	中（5-10）	1	低	实时多足 MPC 推荐
直接交换预测（一步延迟）	滚动（每周期 1 次）	—	最低	Kim-Fawcett-Hamed 的做法

误解	正确理解	出处
分布式 MPC 总比集中式好	用 <3% 最优性 gap 换可扩展性 + 鲁棒性 + 无单点；\(N\) 小且中央可靠时集中式更优	§4.1
"动力学解耦"等于"问题可分"	多足编队动力学解耦但约束/代价耦合；耦合恰恰来自编队约束，必须协调	§4.1
预测交换里能拿到邻居实时值	真实系统有一步通信延迟，用的是邻居上一轮的预测	§4.1/§4.6
编队就是给每个机器人一个绝对目标点	编队是相对几何约束，整体位姿是自由（或由领航者定）的自由度	§4.2
distance-based 代价直接用欧氏范数	范数在重合点不可微，必须用平方距离	§4.2
四足编队硬约束瞬时质心相对位置	质心随踏步起伏，硬约束会与平衡打架；用软约束 + 频段分离	§4.2/§4.5
对偶分解要松弛所有约束	只松弛耦合约束，本地约束保留在各子问题里	§4.3
对偶变量 \(\lambda\) 是自由超参	\(\lambda\) 是有物理含义的影子价格/约束力，可 warm-start	§4.3
ADMM 是全新方法	ADMM = 对偶分解 + 增广项 + 辅助变量，修复对偶分解的三个弱点	§4.4
Z-更新漏对偶项无所谓	必须 average\((\mathbf{w}_i+\mathbf{u}_i)\)，漏 \(\mathbf{u}_i\) 收敛到错误点	§4.4
\(\rho\) 越大收敛越快	\(\rho\) 是"一致 vs 自主"权重，过大让本地代价被淹没、QP 病态	§4.4
ADMM 在非凸下也保证收敛	多足接触非凸，ADMM 无收敛保证；靠凸化 + warm-start + 固定轮数	§4.4/§4.7
耦合力 \(\lambda\) 是独立控制输入	\(\lambda\) 是约束力，由刚性约束 + 动力学解出，受严格限制	§4.5
摩擦锥用精确二阶锥	二阶锥使问题变 SOCP（慢），用金字塔线性近似保持 QP	§4.5
分布式 WBC 也要通信	协调已在 MPC 层由 \(\lambda\) 完成，WBC 纯本地、零通信	§4.6
MPC 足端力可直接发给电机	电机吃关节力矩，须经 WBC 映射 + 补全阶细节	§4.6
ADMM 的平均必须有中央节点	用第 2 章的分布式共识算平均，无需中心	§4.7
物理队形紧密 = 通信质量好	通信图与物理队形是两张图，协调质量由通信图 \(\lambda_2\) 决定	§4.7
长时域 + warm-start = 稳定性证明	那是实践充分条件，非理论保证；非凸下尤其无保证	§4.7

编号	知识点	核心要点	对应节	难度
K1	集中式三重瓶颈	\(O(N^3)\) 爆炸 + 单点故障 + 不可模块化	§4.1	⭐⭐
K2	预测交换范式	交换预测轨迹，编队耦合退化为本地项；软约束、有一步延迟	§4.1	⭐⭐⭐
K3	编队三范式	position/displacement/distance：信息越少、自由度越多、越非凸	§4.2	⭐⭐⭐
K4	编队约束的耦合本质	\(J_{\text{form}}\) 显含多 agent 状态 → 耦合 → 需协调	§4.2	⭐⭐
K5	对偶分解	松弛耦合约束 → 可分 → 价格迭代（子梯度）；慢、脆、中间不可行	§4.3	⭐⭐⭐⭐
K6	影子价格 = 约束力	对偶变量在多足里是物理可测的耦合力 \(\lambda\)	§4.3/§4.5	⭐⭐⭐
K7	consensus-ADMM 三步	X（本地 MPC+惩罚）/Z（共识平均）/Y（对偶上升）	§4.4	⭐⭐⭐⭐
K8	\(\rho\) 与残差平衡	\(\rho\) 是一致-自主权重；残差平衡自适应；凸 \(O(1/k)\)	§4.4	⭐⭐⭐
K9	非凸无保证	接触约束非凸 → ADMM 无收敛保证 → 工程靠凸化+warm-start	§4.4/§4.7	⭐⭐⭐⭐
K10	SRB 网络模型	\(N\) SRB + 负载 + 刚性约束 → 耦合力 \(\lambda\)	§4.5	⭐⭐⭐⭐
K11	约束力求解	\(\lambda=-(JM^{-1}J^\top)^{-1}(JM^{-1}Q+\dot J\dot q)\)，约束微分两次	§4.5	⭐⭐⭐⭐
K12	接触约束三件套	摩擦锥金字塔 + 单边接触 + 踏步时序；固定序列→凸 QP	§4.5	⭐⭐⭐
K13	频段分离	编队低频（质心层）+ 平衡高频（WBC 层）	§4.2/§4.5	⭐⭐⭐
K14	分层数据流	MPC（通信）→步态→WBC（本地）；通信预算最小化	§4.6	⭐⭐⭐
K15	分布式 WBC 纯本地	协调已由 MPC 的 \(\lambda\) 完成，WBC 零通信	§4.6	⭐⭐⭐
K16	ADMM 协调步 = 共识	Z-更新用第 2 章分布式共识实现，无需中心	§4.7	⭐⭐⭐⭐
K17	两层迭代	外层 ADMM 优化、内层共识平均；\(L_{\text{cons}}\) vs \(K\) 权衡	§4.7	⭐⭐⭐
K18	拓扑影响	\(\lambda_2\)/直径/时延/丢包影响收敛与稳定；通信图≠物理队形	§4.7	⭐⭐⭐
K19	DMPC 稳定性支柱	递归可行性（兼容性约束）+ 终端代价；分布式不变集难算	§4.7	⭐⭐⭐⭐
K20	端到端管线	建模→集中基线→ADMM→分层执行→对比	§4.8	⭐⭐⭐

后续章节	关系	本章铺垫的知识点
第 5 章协同搬运与力控	直接延伸：本章的耦合力 \(\lambda\) → 第 5 章的运动力/内力分解；grasp matrix 是 \(\lambda\) 协调的进一步形式化	K6 影子价格=约束力、K10 SRB 网络、K11 约束力求解
第 6/7 章异构多机协同	本章双四足同构 → 异构（不同机型）；分布式/模块化正是异构的前提	K1 模块化、K7 ADMM、K14 分层
第 9 章多足 Loco-Manipulation	本章编队（移动）+ 第 5 章搬运（操作）→ 移动操作一体；分层架构直接复用	K10 SRB 网络、K14 分层数据流
第 12 章 MARL 与规控混合	本章基于模型的 DMPC ↔ MARL 的学习式协调；可混合（MPC 安全层 + MARL 策略）	K2 预测交换、K18 通信拓扑
第 13 章 Mini-MultiBot 综合	本章 `formation_dmpc/` 模块并入综合系统	全部累积项目模块

项	内容
症状	原始残差 \(r^k\) 或对偶残差 \(s^k\) 长期不下降，编队有持续偏差，单周期 ADMM 跑不完
可能原因	(1) \(\rho\) 取得不当（过大/过小）；(2) Z-更新漏了对偶项 \(\mathbf{u}_i\)；(3) 问题非凸（接触约束）ADMM 本就不保证收敛；(4) 冷启动每周期从零开始
排查步骤	① 先用标量 toy 例子（§4.4 练习 1）验证 ADMM 实现正确（收敛到已知值）；② 检查 Z-更新是否为 average\((\mathbf{w}_i+\mathbf{u}_i)\) 而非 average\((\mathbf{w}_i)\)；③ 扫描 \(\rho\in\{0.1,1,10,100\}\) 看残差曲线，或开残差平衡；④ 确认是否 warm-start（保留上周期 \(\mathbf{z},\mathbf{u}\)）；⑤ 若问题非凸，固定接触序列凸化，接受固定轮数有 gap
相关章节	§4.4（ADMM、\(\rho\)、残差平衡）、§4.7（非凸无保证）

第 4 章 分布式 MPC 与多足编队协同运动¶

前置自测¶

本章目标¶

本章知识导航¶

前置知识桥接¶

如果跳过本章会怎样¶

预计阅读时间¶

科研发展脉络¶

本章符号约定¶

§4.1 从集中式到分布式：为什么联合 MPC 必须拆开 ⭐⭐⭐¶

动机：两台机器人好解，四台就崩¶

如果坚持用集中式会怎样¶

反面对照：那能不能干脆"各管各的"（完全去中心、零协调）？¶

理论：分布式 MPC 的基本范式——预测交换¶

系统性分类：MPC 控制架构的完整谱系¶

⚠️ 常见陷阱¶

练习¶

§4.2 编队约束建模：把"保持队形"写进 MPC ⭐⭐⭐¶

动机：队形不是一个数，是一组关系¶

如果用绝对位置建模会怎样¶

历史：从 Tanner/Pappas 的刚性图到编队三范式¶

理论：三范式在 MPC 中的代价与约束写法¶

多足特有：编队约束作用在"质心"还是"机体"？¶

深入：编队代价的 Laplacian 结构——为什么编队就是"加权共识"¶

深入：编队与避障的张力——软约束权重的本质¶

⚠️ 常见陷阱¶

练习¶

§4.3 对偶分解与价格协调：把耦合约束"卖"给市场 ⭐⭐⭐⭐¶

动机：耦合约束挡在"可分"的路上¶

如果用罚函数硬塞会怎样¶

历史：从拉格朗日对偶到 Dantzig-Wolfe 与价格协调¶

理论：拉格朗日松弛 → 可分的对偶函数 → 价格迭代¶

反面：对偶分解为什么"慢"且"脆"——这正是 ADMM 的动机¶

走一遍：两 agent 的价格协调过程（手算 + 解读）¶

⚠️ 常见陷阱¶

练习¶

§4.4 ADMM 分布式 MPC：又快又稳的分解 ⭐⭐⭐⭐¶

动机：既要可分，又要快、稳、中间可用¶

如果直接给联合 MPC 加增广项会怎样——回到"不可分"¶

历史：从 Glowinski/Gabay 到 Boyd 的 consensus-ADMM 复兴¶

理论：consensus-ADMM 形式的分布式 MPC¶

代码：ADMM 分布式 MPC 协调器（为什么→正确→错误→对比）¶

\(\rho\) 调节与收敛性：理论与残差平衡¶

⚠️ 常见陷阱¶

练习¶

§4.5 SRB 网络与多足编队：CoM 协调 + 接触约束 + 耦合力 ⭐⭐⭐⭐¶

动机：把"会走路的机器人"装进分布式 MPC¶

如果忽略耦合力直接堆 \(N\) 个单体 SRB 会怎样¶

历史：从单体 SRB 到互联 SRB 网络¶

理论：互联 SRB 网络的动力学¶

接触约束：摩擦锥、单边接触、踏步时序¶

CoM 协调：踏步起伏与编队的频段分离（呼应 §4.2）¶

深入：质心动力学的离散化与 MPC 预测方程¶

一个具体数值例子：双足抬杆的耦合力¶

⚠️ 常见陷阱¶

练习¶

§4.6 分层控制：从 MPC 命令到关节力矩 ⭐⭐⭐¶

动机：MPC 给的是"宏观命令"，电机要的是"微观力矩"¶

如果不分层、让一个控制器全包会怎样¶

历史：从 Whole-Body Control 到分层 loco-manipulation¶

理论：三层架构的数据流¶

深入：WBC 为什么是 QP——多任务的优先级与折中¶

一步通信延迟与一致性协议（呼应 §4.1）¶

深入：多机步态同步——中层调度的协同维度¶

⚠️ 常见陷阱¶

练习¶

§4.7 通信拓扑、一致性与稳定性：把第 2 章接到 DMPC ⭐⭐⭐⭐¶

动机：ADMM 的"共识平均"到底怎么分布式算¶

如果通信拓扑/质量不管会怎样¶

历史：从共识到分布式优化的统一图景¶

理论：ADMM Z-更新的分布式共识实现¶

通信拓扑对编队的影响：\(\lambda_2\)、直径、时延、丢包¶

深入：收敛率的定量刻画与"够用就停"的本钱¶

稳定性：递归可行性与终端代价¶

深入：兼容性约束为什么是分布式 MPC 递归可行的关键¶

⚠️ 常见陷阱¶

练习¶

§4.8 端到端实战：双四足 ADMM 编队的完整管线 ⭐⭐⭐¶

场景与目标¶

完整管线（七步）¶

第 4 章　分布式 MPC 与多足编队协同运动¶