本文档属于 Robotics Tutorial 项目，作者：Pengfei Guo，达妙科技。采用 CC BY 4.0 协议，转载请注明出处。

第 5 章　协同搬运与力控——抓取矩阵、内力分配与协调阻抗¶

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

一句话定位：当多台机器人同时抓住同一个刚体一起搬运时，它们就被这个物体**焊死成了一条闭合运动链**——任何一台多用一分力，这分力都不会让物体动得更快，而是变成了挤压物体的"内力"。本章从这条闭链约束的运动学出发，完整讲透**抓取矩阵（grasp matrix）如何把接触力映射成物体合力旋量、内力/外力如何沿零空间分解、leader-follower 与去中心化两条力协调路线的本质差异、阻抗/导纳如何在多机之间协调、以及为什么"无源性"是判断这套耦合系统稳不稳的终极标尺**。读完本章，你能把第 1-4 章的"谁去搬、怎么走、怎么不撞"延续到最后一公里——"一起把它稳稳抬起来"。

前置自测¶

开始前先回答下面 5 个问题。答不出 2 题以上，建议先回前置章节补齐——本章每一步推导都建立在这些基础之上，欠了账会在内力分解和无源性证明处卡住。

（接 §F02/操作空间动力学） 单臂操作空间动力学方程 \(\Lambda(x)\ddot{x} + \mu(x,\dot{x}) + p(x) = F\) 中，操作空间惯量矩阵 \(\Lambda = (JM^{-1}J^\top)^{-1}\) 的物理含义是什么？为什么它不等于把关节惯量 \(M\) 简单"搬"到笛卡尔空间？雅可比转置 \(\tau = J^\top F\) 又是怎么来的？（答不出 → 回 05_运动控制/20_机械臂 F02 操作空间动力学）
（接 §F01/阻抗导纳二分法） Hogan 阻抗律 \(F = M_d\ddot{\tilde{x}} + D_d\dot{\tilde{x}} + K_d\tilde{x}\) 中三个矩阵分别整形什么物理行为？阻抗（impedance）和导纳（admittance）的因果方向有什么区别——谁以运动为输入、力为输出？为什么刚性环境下导纳控制容易不稳？（答不出 → 回 05_运动控制/20_机械臂 F01 力控导论）
（接 §D3.4 / 线性代数） 给定一个"宽矩阵" \(G\in\mathbb{R}^{6\times 3N}\)（行少列多，\(3N>6\)），方程 \(Gf = w\) 的解集是什么结构？Moore-Penrose 伪逆 \(G^+ = G^\top(GG^\top)^{-1}\) 给出的特解有什么最优性？零空间投影矩阵 \(I - G^+G\) 把任意向量投到哪里？（答不出 → 回 §D3.4 Richter 无约束化 / Part 0 数值线性代数）
（接 §3.x / 共识基础） 什么叫去中心化（decentralized）控制？为什么"每个机器人只用本地测量、不和别人通信"在多机搬运里反而是个优点？隐式通信（implicit communication）通过什么物理量传递信息？（答不出 → 回本 Part 第 1、2 章多机系统全景 / 共识算法）
（接 §F06 / Lyapunov） 一个系统"无源（passive）"的能量定义是什么？为什么无源性能推出稳定性？两个无源系统按反馈连接后还无源吗？这条性质和我们要分析的"多机器人 + 柔性物体"耦合系统有什么关系？（答不出 → 回 05_运动控制/20_机械臂 F06 变阻抗无源性 / 控制理论基础）

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

\(\Lambda\) 是机器人在操作空间末端"感受到"的等效惯量——推动末端某方向时，需要克服的不仅是该方向的连杆质量，还有通过雅可比耦合进来的全身惯量。它不等于简单搬运，因为 \(J\) 把关节空间的各向异性惯量做了非线性混合，结果是一个随构型 \(q\) 变化、且非对角的 \(6\times 6\) 矩阵。\(\tau = J^\top F\) 来自虚功原理：末端力 \(F\) 做的虚功 \(F^\top \delta x\) 必须等于关节力矩做的虚功 \(\tau^\top\delta q\)，代入 \(\delta x = J\delta q\) 即得。
\(M_d\) 整形虚拟惯量（对加速度的阻抗）、\(D_d\) 整形阻尼（吸收能量、决定振荡衰减）、\(K_d\) 整形刚度（位置偏差的回复力）。阻抗以运动（位移/速度）为输入、力为输出（"你推我多少，我回你多少力"）；导纳反过来，以力为输入、运动为输出。刚性环境下导纳不稳是因为环境刚度极大时，微小位置指令产生巨大接触力，经导纳的"力→运动"环路放大，闭环增益超过 1。
\(3N>6\) 时 \(Gf=w\) 欠定，解集是一个 \((3N-6)\) 维仿射子空间：\(f = G^+w + (I-G^+G)\xi\)，\(\xi\) 任取。\(G^+w\) 是其中范数最小的特解，且位于 \(G\) 的行空间 \(\mathrm{row}(G)=\mathrm{range}(G^\top)\)。\(I-G^+G\) 把任意 \(\xi\) 投影到 \(G\) 的零空间 \(\ker(G)\)。这正是内力的栖身之所。
去中心化指每个 agent 的控制律只依赖本地可测量，不依赖全局状态或显式通信。优点是无单点故障、无通信延迟/带宽瓶颈、可任意增减机器人数量（可扩展）。隐式通信通过被搬物体本身传递——一台机器人推物体，物体的运动被其他机器人在各自接触点处测到，物体成了"共享黑板"。
无源：存在下有界的储能函数 \(V\ge 0\)，使 \(\dot V \le s^\top f\)（输出功率），即系统耗散的能量不超过外界注入的能量，自己不凭空产生能量。无源 ⇒ 稳定，因为储能函数天然是 Lyapunov 候选。两个无源系统**负反馈**互联后仍无源（无源性在反馈下闭合）——这正是我们能把"机器人控制器"和"被搬物体 + 环境"分别证明无源、再断言整体稳定的理论依据。

本章目标¶

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

从闭链约束推导抓取矩阵：把 \(N\) 个机器人刚性抓持同一物体的系统建模为闭合运动链，写出抓取矩阵 \(G\) 的解析结构（每列 \([I_3;\ \widehat{r}_i]\) 的来历），解释 \(w = G f\) 与 \(\dot{x}_i = G_i^\top v_o\) 的对偶（力映射 vs 速度映射），并说明为什么 \(G\) 的零空间维度恒为 \(3N-6\)。
严格分解内力与外力：从数学上证明接触力分解 \(f = G^+ w + (I-G^+G)f_{\text{int}}\) 中两项的正交性与物理意义，理解为什么 Moore-Penrose 伪逆给出的不是"非挤压（nonsqueezing）"解，掌握 Walker 的非挤压伪逆和基于抓取点几何的内力基构造。
对比 leader-follower 与去中心化两条路线：讲清主从架构（leader 用阻抗跟踪轨迹、follower 通过物体运动估计意图）与全去中心化架构（每个 agent 同构、仅靠物体运动隐式协调）各自的假设、信息流、可扩展性与失效模式，能为一个具体场景选对架构。
设计协调阻抗 / 物体阻抗控制器：推导 object impedance（把期望阻抗加在被搬物体的质心而非各末端）的控制律，理解它与"各末端独立阻抗"的区别，掌握把物体级期望力旋量通过 \(G^+\) 与内力调节分配回各机器人的完整管线。
做负载分配优化：把"如何把外力旋量分给 \(N\) 个机器人"形式化为带摩擦锥/力限约束的二次规划（QP），理解等分、加权伪逆、QP 优化三种策略的取舍，并用代码实现可实时求解的力分配器。
用无源性判稳并保证稳定：用 Lyapunov / 无源性框架分析多机搬运闭环，理解能量罐（energy tank）、波变量（wave variable）如何在变阻抗与通信延迟下保住无源性，把 §F06 的单臂无源性工具迁移到多机耦合系统。
打通与单臂/双臂力控章节的知识网络：明确本章的抓取矩阵理论与 §D03 双臂协调力控、§F02 操作空间动力学、§F06 无源性是同一套数学在不同 agent 数下的展开，能在多足绳索搬运（Sombolestan）、被动臂搬运（PACC）、零通信学习搬运（decPLM）等前沿工作之间建立联系。

本章知识导航¶

本章的知识结构是一棵以"多台机器人如何一起稳稳搬运一个物体"为根的树。树干是四个递进的核心问题，树枝是每个问题的数学工具和工程实现。

多机抓同一物体，运动如何耦合？ (§5.1 闭链约束 + 抓取矩阵 G)
    │
    ▼
施加的力里，哪部分搬物体、哪部分挤物体？ (§5.2 内力/外力分解)
    │
    ├─→ 谁来决策？  ┌─ leader-follower (§5.3) ── 主从阻抗 + 意图估计
    │              └─ 去中心化 (§5.4) ──────── 隐式通信 + 同构 agent
    │
    ├─→ 用什么力学行为搬？ (§5.5 协调阻抗 / object impedance)
    │
    ├─→ 外力旋量怎么分给 N 个机器人？ (§5.6 负载分配 QP)
    │
    └─→ 这套耦合系统稳不稳？ (§5.7 无源性与稳定性)
              │
              ▼
        前沿与综合 (§5.8) ── 多足绳索 / 被动臂 / 零通信学习搬运

小节	主题	难度	一句话
§5.1	闭链约束与抓取矩阵	⭐⭐⭐	\(w=Gf\) 与速度对偶 \(\dot{x}_i=G_i^\top v_o\)，零空间维度 \(3N-6\)
§5.2	内力/外力分解	⭐⭐⭐	\(f=G^+w+(I-G^+G)f_{\text{int}}\)，挤压与非挤压伪逆
§5.3	leader-follower 力协调	⭐⭐⭐	leader 阻抗领航，follower 估计意图、放大力
§5.4	去中心化力控	⭐⭐⭐	物体当"共享黑板"，隐式通信，Force-ANTS
§5.5	协调阻抗与物体阻抗	⭐⭐⭐	阻抗加在物体质心 vs 加在各末端
§5.6	负载分配优化	⭐⭐⭐	摩擦锥/力限下的 QP，等分 vs 加权 vs 优化
§5.7	无源性与稳定性	⭐⭐⭐⭐	能量罐、波变量、反馈无源闭合
§5.8	前沿工作与综合实战	⭐⭐⭐⭐	绳索/被动臂/零通信学习；MuJoCo 双 Go2 搬运

两条阅读线：

理论线（理解数学）：§5.1→§5.2→§5.5→§5.7，重点是抓取矩阵、内力分解、object impedance 推导与无源性证明。
工程线（会用就行）：§5.1→§5.2→§5.4→§5.6→§5.8，重点是力分解代码、去中心化策略、QP 力分配与仿真搭建。

无论哪条线，§5.1 和 §5.2 都是必读——它们是所有后续内容的地基。§5.7 是唯一的 ⭐⭐⭐⭐ 硬核理论小节，值得花最多时间。

前置知识桥接¶

本章紧接本 Part 的第 1-4 章，并大量复用 05_运动控制子课程二（力控）的单臂理论。这里把最关键的前置点重新激活——你不必翻回去就能跟上。

回顾本 Part 第 1-4 章（离散决策 + 连续协调）：第 1 章给了"看待多机系统"的图论地图（通信图 \(G_{\text{comm}}\)、Laplacian、\(\lambda_2\)、集中/分布/去中心三大架构），第 2 章给了连续协调的数学引擎（共识 \(\dot{x}=-Lx\)），第 3 章解决了"谁去搬哪个、走哪条路不相撞"（任务分配 + MAPF），第 4 章把连续协调推进到分布式 MPC 编队。本章是这条链的**最后一环**：前面解决了"分工、路径、编队运动"，本章解决"多台机器人共同抓住同一物体后，力如何协调"。一个关键区别要先说清——第 1-4 章的耦合是通过"信息"（通信图）耦合的，本章的耦合是通过"物理实体"（被搬物体这条闭链）耦合的。被搬物体既是约束、又是隐式的通信信道，这是本章和前四章在耦合机理上的本质不同。

本质洞察：前四章的多机器人通过**通信图**间接相连——你给我发消息我才知道你的状态。本章的多机器人通过**被搬的刚体**直接相连——我不需要你发消息，我推物体，你在你的接触点立刻感受到。物体把"通信"硬件化成了一条钢梁。理解这个，就理解了为什么多机搬运能做到"零通信"（§5.4、§5.8 的 decPLM）——通信信道已经焊在物体上了。

回顾 §F02（操作空间动力学）：F02 建立了把机器人动力学从关节空间投到笛卡尔末端空间的工具——操作空间惯量 \(\Lambda=(JM^{-1}J^\top)^{-1}\)、雅可比转置力映射 \(\tau=J^\top F\)。本章的抓取矩阵 \(G\) 与雅可比 \(J\) 是**同一个数学对象在不同层级的化身**：\(J\) 把单臂关节速度映射到末端速度，\(G^\top\) 把物体速度映射到各接触点速度。你在 F02 学的"雅可比零空间 = 冗余关节自运动"，在本章会原样变成"抓取矩阵零空间 = 内力"。

回顾 §F01 / §F06（阻抗、无源性）：F01 建立了阻抗/导纳二分法，F06 建立了变阻抗的无源性危机与能量罐解法。本章 §5.5 把单臂阻抗推广到"物体阻抗"，§5.7 把 F06 的单臂能量罐推广到多机耦合——核心工具（储能函数、\(\dot V\le s^\top f\)、能量罐放行阀）完全一致，只是 agent 从 1 个变成 \(N\) 个，耦合通道从"机器人-环境"变成"机器人-物体-机器人"。

回顾 §D3.4（伪逆与零空间）：D3 在多项式轨迹生成里用端点导数做变量替换时，反复用到伪逆 \(G^+=G^\top(GG^\top)^{-1}\) 和零空间投影 \(I-G^+G\)。本章这两个对象会以完全相同的形式出现，只不过物理含义从"最小范数轨迹系数"变成"最小范数接触力 + 内力"。如果你在 D3 理解了"伪逆给出行空间里的最小范数特解、零空间投影给出齐次解的全部自由度"，本章的内力分解对你就是同一首歌换了歌词。

前向预告：本章把被搬物体当作**刚体**、抓持当作**刚性固连**。这两个假设让闭链约束变成干净的代数关系（\(w=Gf\)）。但真实搬运中物体可能柔性变形、抓持可能打滑或只是单边接触（如多足用脚"捏"物体）。这些放松会把本章的等式约束变成不等式约束（摩擦锥、单边接触），§5.6 的 QP 力分配会处理摩擦锥，而柔性物体与非刚性抓持的完整处理属于更前沿的内容，§5.8 会给出指针。现在只需记住：刚体 + 刚性抓持是理解多机搬运的"球面奶牛"，先把它吃透，再逐步放松假设。

如果跳过本章会怎样¶

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

场景一："两台机器人各自跟踪轨迹，物体被挤爆了"。 你让两台机械臂抓住一根钢管，各自用阻抗控制跟踪自己末端的期望轨迹。仿真里两条轨迹都"跟得很好"，但真机一上，钢管要么被两臂越夹越紧（电流飙升、夹具变形），要么两臂"打架"导致物体剧烈抖动。根本原因是：两个独立的末端阻抗控制器**没有协调内力**——它们各自的期望位置之间哪怕只差 1 毫米，在刚性钢管上也会产生巨大的内力。没有本章的内力/外力分解，你不知道这 1 毫米的位置误差为什么会变成几百牛的挤压力，更不知道该用零空间投影把内力单独拎出来控制。

场景二："加了第三台机器人，系统突然不稳了"。 你的双机搬运跑得很好，为了搬更重的负载加了第三台。结果系统开始低频振荡甚至发散。你怀疑是某台机器人的增益问题，逐个调参却越调越糟。根本原因是：多机耦合系统的稳定性不是各子系统稳定性的简单叠加——三台机器人通过物体形成的反馈回路可能注入净能量，破坏整体无源性。没有本章 §5.7 的无源性分析，你只能盲目试错，无法从能量流的角度定位"哪个环节在产生能量"。

场景三："想让机器人队伍零通信协作，却不知从何下手"。 你看到 decPLM 这类工作宣称"2-10 台 Go2 零通信搬运"，很想复现，但完全不理解：机器人之间不通信，怎么知道该往哪用力？根本原因是：你没有建立"被搬物体 = 隐式通信信道"的认知模型。没有本章 §5.4 的去中心化力控原理（隐式通信、Force-ANTS 的力放大机制），"零通信协作"对你只是一个魔法名词，无法落地成控制律。

预计阅读时间¶

模式	时长	适合
精读	18-24 小时	第一次系统学多机协同搬运与力控：逐节读动机→推导→代码，亲手推导抓取矩阵零空间维度、内力分解正交性、object impedance 控制律与无源性证明，搭一遍 MuJoCo 双 Go2 搬运，做完每节练习。建议分 5-6 次。
速读	5-7 小时	有单臂力控基础（已读 F01-F06）、想建立多机搬运全局图景：读每节的"动机"和"理论"小标题、框住的关键公式、§5.8 综合实战，跳过代码细节和完整证明。
速查	50-70 分钟	已学过、回来查特定公式或结论：直接定位到对应小节，看 boxed 公式 + 内力分解表 + 架构对比表 + API 速查。

科研发展脉络¶

在钻进具体方法前，先把这条研究线的来龙去脉理清——知道每个方法"从哪来、解决了前人什么痛点、又留下什么给后人"，比孤立地记名字有用得多。多机协同搬运是一个**四十年理论积淀（1986 起）+ 近五年学习革命**的领域。

年份	论文	Venue	核心贡献
1986	Orin & Oh, "Control of Force Distribution in Robotic Mechanisms Containing Closed Kinematic Chains"	ASME JDSMC	奠基：闭链系统力分配的一般公式，伪逆解
1988	Walker, Freeman, Marcus, "Internal Object Loading for Multiple Cooperating Robot Manipulators"	ICRA	非挤压伪逆：指出 Moore-Penrose 解会挤压物体，构造唯一非挤压分配
1988	Khatib, "Object Manipulation in a Multi-Effector Robot System"	ISRR	增广物体模型：把物体动力学和操作空间控制统一
1996	Caccavale, Chiacchio 等, 双臂协调控制系列	IEEE T-RA	对称构型 + object impedance：绝对/相对运动解耦，阻抗加在物体上
2000	Khatib 等, "Unified Approach for Motion and Force Control"	IEEE T-RA	操作空间内力/运动解耦：本章 §5.2 分解的理论母体
2008	Wimböck, Ott, Hirzinger, "Comparison of Object-Level Grasp Controllers"	DLR / IJRR	物体级阻抗 + 内力共线：多指/多臂统一阻抗框架
2019	Verginis, Dimarogonas, "Cooperative Manipulation via Internal Force Regulation: A Rigidity Theory Perspective"	RA-L / arXiv 1911.01297	刚度理论视角：抓取矩阵与刚度矩阵的 range-nullspace 关系，能量最优内力
2020	Culbertson, Schwager 等, "Decentralized Adaptive Control for Collaborative Manipulation of Rigid Bodies"	T-RO	去中心化自适应：无需负载惯性，每 agent 本地自适应
2018	Wang, Schwager, "Collaborative Multi-Robot Transportation via Implicit Communication"	Frontiers Robot. AI	隐式通信：物体运动即通信，无显式通信的力协调
2021	Carey, Werfel, "Collective Transport via Implicit Communication (Force-ANTS)"	ICRA	力放大 N-robot 系统：leader 小力 → follower 对齐放大
2022	Sombolestan, Nguyen, "Collaborative Navigation and Manipulation of Cable-towed Load by Multiple Quadrupeds"	IROS	多四足 + 绳索：分布式力控搬运
2024	Turrisi 等, "PACC: Passive-Arm Approach for High-Payload Collaborative Carrying"	IROS / arXiv 2403.19862	被动臂 + MPC：被动关节吸收耦合，简化建模
2025	Pandit 等, "decPLM: Multi-Quadruped Cooperative Object Transport"	arXiv	2-10 Go2 零通信：pinch-lift-move 策略，IsaacLab 训练

关键实验室脉络：OSU（Orin 闭链力分配原创）→ Stanford ASL（Khatib 操作空间 + 增广物体）→ DLR（Caccavale/Wimböck/Ott 物体级阻抗）→ KTH（Verginis/Dimarogonas 刚度理论 + 分布式）→ Stanford MSL（Schwager 去中心化自适应 + 隐式通信）→ Harvard（Werfel 群体搬运）→ Notre Dame / USC（Sombolestan/Nguyen 多足绳索）→ IIT DLS（Turrisi PACC）→ CMU/学习社区（decPLM 零通信学习）。

看这条线，有一条清晰的主线：决策从"集中式精确力分配"走向"去中心化隐式协调"，建模从"精确已知负载动力学"走向"自适应/学习免模型"。每一步都在减少对全局信息和精确模型的依赖，扩大可处理的规模与不确定性。本章 §5.1-5.2 讲数学母体（抓取矩阵、内力分解），§5.3-5.6 讲经典控制（leader-follower、去中心化、阻抗、QP），§5.7 讲稳定性基石（无源性），§5.8 讲学习时代的前沿。

本章符号约定¶

符号	含义	首见
\(N\)	协同搬运的机器人（接触点）数量	§5.1
\(\mathcal{O}\)	被搬运的刚体物体（object）	§5.1
\(w_o \in \mathbb{R}^6\)	作用在物体质心的合力旋量（wrench，力 + 力矩）	§5.1
\(f_i \in \mathbb{R}^3\)	第 \(i\) 个接触点施加的接触力	§5.1
\(f = [f_1^\top,\dots,f_N^\top]^\top \in \mathbb{R}^{3N}\)	所有接触力堆叠	§5.1
\(G \in \mathbb{R}^{6\times 3N}\)	抓取矩阵（grasp matrix）	§5.1
\(G_i \in \mathbb{R}^{6\times 3}\)	第 \(i\) 个接触点对应的抓取矩阵分块	§5.1
\(r_i \in \mathbb{R}^3\)	物体质心到第 \(i\) 接触点的位置向量	§5.1
\(\widehat{r}_i \in \mathbb{R}^{3\times 3}\)	\(r_i\) 的反对称（叉积）矩阵，\(\widehat{r}_i v = r_i\times v\)	§5.1
\(v_o = [\dot{p}_o^\top,\omega_o^\top]^\top \in \mathbb{R}^6\)	物体质心的广义速度（线速度 + 角速度）	§5.1
\(x_i \in \mathbb{R}^3\)	第 \(i\) 接触点的位置	§5.1
\(G^+ \in \mathbb{R}^{3N\times 6}\)	\(G\) 的（加权）伪逆	§5.2
\(w_{\text{ext}}\)	期望施加给物体的外部力旋量（驱动运动）	§5.2
\(f_{\text{int}} \in \mathbb{R}^{3N}\)	内力（internal force），位于 \(\ker G\)	§5.2
\(P_N = I - G^+G\)	抓取矩阵零空间投影矩阵	§5.2
\(E \in \mathbb{R}^{3N\times(3N-6)}\)	内力子空间的一组基（internal force basis）	§5.2
\(M_d, D_d, K_d\)	物体（或末端）期望阻抗的惯量/阻尼/刚度矩阵	§5.5
\(\tilde{x} = x - x_d\)	位置跟踪误差	§5.5
\(W\)	力分配权重矩阵（加权伪逆 \(G_W^+\) 用）	§5.6
\(\mu\)	库仑摩擦系数	§5.6
\(V\)	储能函数（Lyapunov / 无源性用）	§5.7
\(T_{\text{tank}}\)	能量罐储能	§5.7
\(u, v\)	波变量（wave variables）	§5.7
\(b\)	波阻抗（wave impedance / characteristic impedance）	§5.7

符号一致性提醒：本章的抓取矩阵约定 \(w_o = G f\)（力映射），与 §D03 双臂协调力控完全一致。注意有些文献用 \(G\) 表示"抓持映射"的转置（即 \(f = G^\top \lambda\) 风格），本章统一采用"\(G\) 把接触力映射到物体旋量"这一约定，全章不变。物体速度到接触点速度的对偶映射记为 \(\dot{x}_i = G_i^\top v_o\)。

§5.1 闭链约束与抓取矩阵 ⭐⭐⭐¶

这一节解决什么问题：当 \(N\) 台机器人同时刚性抓住同一个物体，它们的运动不再独立——物体把它们焊成了一条闭合运动链。这一节把这种耦合形式化：先从运动学约束推出"接触点速度"和"物体速度"的关系，再从静力学推出"接触力"和"物体合力旋量"的关系，最后把这两件事统一到一个矩阵——抓取矩阵 \(G\)。它是后面所有内力分解、力分配、阻抗协调的数学母体。

动机：两台机器人抬一根钢梁¶

设想最简单的协同搬运：两台机械臂，末端各自牢牢抓住一根钢梁的两端，要把它从地上平稳抬到桌面。每台机械臂自己都是一套完整的运动学系统，有自己的关节、自己的末端。在它们抓住钢梁之前，两台机械臂是两个**独立**的系统——你可以让左臂末端往任意方向动，右臂纹丝不动。

但抓住钢梁的那一刻，一切都变了。钢梁是刚体，它的两端被两个末端"焊"住了。现在如果左臂末端往上抬 1 厘米，右臂末端会发生什么？如果右臂保持不动，那么钢梁这个刚体就被要求"左端上移 1 厘米、右端不动"——这意味着钢梁要绕右端转一个角度。如果钢梁真是刚体、抓持真是刚性，那么右臂末端的位置和姿态就**被左臂的运动和钢梁的几何唯一决定了**——右臂不再有"保持不动"的自由。

这就是**闭合运动链（closed kinematic chain）：左臂 → 钢梁 → 右臂 → （通过共同的世界地面）→ 左臂，构成了一个环。环上任何一处的运动都会沿着环传播，约束其他所有处的运动。**搬运的本质困难，全部源自这条闭链。单臂操作是"开链"——末端可以自由地动；多机搬运是"闭链"——所有末端的运动被物体这个刚体硬性绑定。

现在把问题具体化。我们真正想控制的是**物体**——钢梁应该沿某条轨迹运动，最终停在桌面上。但我们能直接施加的是**各机器人末端的力**。于是两个核心问题浮现：

运动学问题：我希望物体以速度 \(v_o\) 运动，各机器人末端必须以什么速度运动才"兼容"这个物体运动而不撕裂闭链？
静力学问题：各机器人末端施加的接触力 \(f_i\) 合起来，对物体产生了多大的合力和合力矩？

抓取矩阵 \(G\) 同时回答这两个问题。它是连接"物体世界"和"机器人世界"的桥梁。

如果不建立抓取矩阵会怎样¶

假设我们拒绝引入抓取矩阵，固执地让每台机器人"各自为政"：每台机器人末端独立跟踪一条预先算好的笛卡尔轨迹。会发生三件糟糕的事。

第一，过约束与内力爆炸。 两台机器人末端 + 一个刚体，在三维空间里，自由度对不上。一个自由刚体有 6 个自由度。两个末端如果都用 6 维位姿独立控制，就是 12 个被控量去约束一个 6 自由度的刚体——多出来的 6 个约束维度无处安放，全部变成了**内力**（两端互相挤压/拉扯/扭转钢梁的力）。哪怕两条轨迹只差一个浮点误差，在刚性钢梁上也会被放大成巨大的内力。机器人电流飙升、夹具变形、钢梁被夹弯——这就是 §5.0"如果跳过本章"场景一描述的"物体被挤爆"。

第二，无法分配负载。 你想让两台机器人"各扛一半重量"，但"各自跟踪轨迹"的方案里根本没有"重量分配"这个旋钮。每台机器人只知道自己的位置误差，不知道物体总重多少、自己该出多少力。负载分配需要一个把"物体所需总力旋量"映射回"各机器人接触力"的工具——这正是 \(G\) 的伪逆要干的事。

第三，无法定义"协调"。 "协调搬运"的数学定义到底是什么？是两台机器人末端速度满足某个关系。没有 \(G\)，你说不清这个关系是什么，也就无法检验两台机器人是否"协调"。\(G^\top\) 给出的正是这个关系：\(\dot{x}_i = G_i^\top v_o\)——所有末端速度都必须是同一个物体速度 \(v_o\) 的像。

本质洞察：抓取矩阵不是一个"为了方便计算而引入的工具矩阵"，它是闭链约束的**数学化身**。\(\dim\ker G = 3N-6\) 这个数字——接触力空间维度 \(3N\) 减去物体自由度 \(6\)——精确地量化了"过约束的程度"：有多少维的力可以施加却不影响物体运动。这 \(3N-6\) 维，就是内力的容身之所，也是搬运一切麻烦的根源和一切精巧控制的着力点。

历史：从闭链力分配到抓取矩阵¶

抓取矩阵的概念有两条历史源流，最终汇成今天的统一框架。

一条来自**腿足机器人与闭链机构**。1986 年，Orin 和 Oh 在研究含闭合运动链的机器人（如四足站立时四条腿 + 地面 + 躯干构成的闭链）时，提出了闭链系统的力分配一般公式——如何把躯干所需的合力分配到各条腿的接触力上。他们最早系统地处理了"接触力多于任务自由度"导致的欠定问题，并用伪逆给出解。

另一条来自**多指抓取与多臂操作**。1988 年是关键的一年：Walker、Freeman 和 Marcus 在 ICRA 上指出，用 Moore-Penrose 伪逆分配接触力会在物体内部产生不必要的挤压（squeeze），并构造了唯一的"非挤压（nonsqueezing）"力分配；同年，Khatib 在 ISRR 提出"增广物体（augmented object）"模型，把被操作物体的动力学直接纳入操作空间控制框架。这两条线在 1990 年代被 Caccavale、Chiacchio 等人统一到双臂协调控制中，"抓取矩阵"作为标准术语固定下来。

类比的边界：多指手抓取（grasping）和多臂搬运（cooperative manipulation）共享同一个抓取矩阵数学——这就是为什么本章和 §D03 双臂力控用的是同一套 \(G\)。像的部分：都是多个接触点对同一刚体施力，都用 \(w=Gf\) 描述力映射，内力都在 \(\ker G\)。不像的部分：多指抓取的接触通常是**单边的**（手指只能推不能拉，接触力受摩擦锥约束，§5.6 会处理），而多臂刚性抓持是**双边的**（夹爪焊死物体，可推可拉可扭）。不要把"刚性抓持下内力可任意"这个结论延伸到"指尖接触"——指尖接触的内力必须保持在摩擦锥内，否则物体滑落。

理论：抓取矩阵的推导¶

我们分两步推导抓取矩阵：先从**静力学**（力的映射）推 \(w=Gf\)，再从**运动学**（速度的映射）推对偶关系，最后说明二者为何由同一个 \(G\) 联系。

第一步：静力学——接触力如何合成物体旋量¶

考虑被搬物体 \(\mathcal{O}\) 为刚体，质心在 \(p_o\)。第 \(i\) 个机器人在接触点 \(x_i\) 处对物体施加接触力 \(f_i\in\mathbb{R}^3\)（我们先只考虑力、不考虑接触力矩，对应"点接触"或"球铰抓持"模型；末端能传递力矩的"刚性抓持"模型留作推广，见本节末）。

这个接触力对物体质心产生两个效应：

平移效应：力 \(f_i\) 直接贡献到物体所受合力。
转动效应：力 \(f_i\) 作用在偏离质心的点 \(x_i\) 上，产生力矩 \(\tau_i = r_i \times f_i\)，其中 \(r_i = x_i - p_o\) 是从质心指向接触点的位置向量。

把 \(N\) 个接触点的贡献全部叠加，作用在物体质心的合力旋量（wrench）\(w_o = [F_o^\top, \tau_o^\top]^\top \in \mathbb{R}^6\) 为：

\[ F_o = \sum_{i=1}^N f_i, \qquad \tau_o = \sum_{i=1}^N r_i \times f_i . \]

现在把叉积写成矩阵乘法。回顾线性代数：叉积 \(r_i \times f_i\) 可以写成 \(\widehat{r}_i f_i\)，其中 \(\widehat{r}_i\) 是 \(r_i\) 的反对称矩阵（skew-symmetric matrix）：

\[ \widehat{r}_i = \begin{bmatrix} 0 & -r_{i,z} & r_{i,y} \\ r_{i,z} & 0 & -r_{i,x} \\ -r_{i,y} & r_{i,x} & 0 \end{bmatrix}, \qquad \widehat{r}_i f_i = r_i \times f_i . \]

为什么叉积等于反对称矩阵乘法？ 这不是巧合，是叉积线性性的必然。叉积 \(r\times(\cdot)\) 对第二个参数是线性算子，任何线性算子都能写成矩阵；把基向量 \(e_x,e_y,e_z\) 依次代入 \(r\times e_j\) 求出三列，拼起来就是上面的 \(\widehat{r}\)。反对称（\(\widehat{r}^\top=-\widehat{r}\)）则来自叉积的反交换律 \(r\times f = -f\times r\)。这个 \(\widehat{r}\) 在 §F02 操作空间动力学和 §D03 抓取矩阵里你已经见过，含义完全一致。

于是合力旋量写成堆叠形式：

\[ \boxed{\; w_o = \begin{bmatrix} F_o \\ \tau_o \end{bmatrix} = \underbrace{ \begin{bmatrix} I_3 & I_3 & \cdots & I_3 \\ \widehat{r}_1 & \widehat{r}_2 & \cdots & \widehat{r}_N \end{bmatrix} }_{G\,\in\,\mathbb{R}^{6\times 3N}} \underbrace{ \begin{bmatrix} f_1 \\ f_2 \\ \vdots \\ f_N \end{bmatrix} }_{f\,\in\,\mathbb{R}^{3N}} = G f . \;} \tag{5.1} \]

这就是**抓取矩阵方程**。\(G\) 的第 \(i\) 个 \(6\times 3\) 分块是

\[ G_i = \begin{bmatrix} I_3 \\ \widehat{r}_i \end{bmatrix} \in \mathbb{R}^{6\times 3}, \]

上半块 \(I_3\) 把接触力原样搬到物体合力（"力就是力"），下半块 \(\widehat{r}_i\) 把接触力通过力臂转成力矩（"杠杆效应"）。

每一项的物理意义务必看清：\(G\) 只依赖几何（各接触点相对质心的位置 \(r_i\)），不依赖任何动力学参数（物体质量、惯量都没出现）。这是 \(G\) 作为"纯运动学/几何"对象的本质——它描述的是力的几何合成，与物体多重无关。

第二步：运动学对偶——物体速度如何分发到接触点速度¶

现在反过来看运动。物体作为刚体，质心以广义速度 \(v_o = [\dot{p}_o^\top, \omega_o^\top]^\top\) 运动（\(\dot{p}_o\) 线速度，\(\omega_o\) 角速度）。刚体上任意一点 \(x_i\) 的速度由刚体运动学给出：

\[ \dot{x}_i = \dot{p}_o + \omega_o \times r_i = \dot{p}_o - r_i \times \omega_o = \dot{p}_o - \widehat{r}_i\,\omega_o . \]

写成矩阵形式（注意 \(\omega_o\times r_i = -r_i\times\omega_o = -\widehat r_i\omega_o\)，符号要小心）：

\[ \dot{x}_i = \begin{bmatrix} I_3 & -\widehat{r}_i \end{bmatrix} \begin{bmatrix} \dot{p}_o \\ \omega_o \end{bmatrix} = \begin{bmatrix} I_3 & \widehat{r}_i^\top \end{bmatrix} v_o = G_i^\top v_o , \]

最后一步用了 \(\widehat{r}_i^\top = -\widehat{r}_i\)（反对称）。所以接触点速度与物体速度的关系是

\[ \boxed{\; \dot{x}_i = G_i^\top v_o, \qquad \dot{X} = G^\top v_o, \;} \tag{5.2} \]

其中 \(\dot{X} = [\dot{x}_1^\top,\dots,\dot{x}_N^\top]^\top\) 是所有接触点速度的堆叠。

这就是闭链约束的运动学形式：所有接触点的速度都必须是同一个物体速度 \(v_o\) 通过 \(G^\top\) 的像。如果各机器人末端速度 \(\dot{x}_i\) 不满足"它们共同来自某个 \(v_o\)"，那它们就在试图撕裂刚体——物理上不可能，结果就是产生内力。式 (5.2) 是"协调"的精确数学定义。

为什么力映射和速度映射由同一个 \(G\) 联系？——对偶性¶

式 (5.1) 是 \(w_o = G f\)（力：接触力 → 物体旋量），式 (5.2) 是 \(\dot{X} = G^\top v_o\)（速度：物体速度 → 接触点速度）。一个用 \(G\)，一个用 \(G^\top\)，这不是巧合，是**功的守恒（运动静力学对偶，kineto-statics duality）**。

考虑接触界面传递的瞬时功率。从接触力一侧看，功率是 \(\sum_i f_i^\top \dot{x}_i = f^\top \dot{X}\)。从物体一侧看，作用在物体上的旋量做的功率是 \(w_o^\top v_o\)。刚性无滑动接触不储存也不耗散能量，两者必须相等：

\[ f^\top \dot{X} = w_o^\top v_o \;\Longrightarrow\; f^\top (G^\top v_o) = (Gf)^\top v_o \;\Longrightarrow\; f^\top G^\top v_o = f^\top G^\top v_o .\ \checkmark \]

恒等式成立——这证明了：只要力映射是 \(G\)，速度映射就必然是 \(G^\top\)。这与 §F02 里 \(\tau = J^\top F\)（力）和 \(\dot{x} = J\dot{q}\)（速度）由同一个 \(J\) 联系是完全相同的对偶结构。抓取矩阵 \(G\) 在多机层级扮演的角色，恰如雅可比 \(J\) 在单臂层级的角色。

本质洞察（对偶性的统一视角）：单臂的 \(J\)、多机的 \(G\)、多指手的抓取映射，本质都是同一件事——一个把"低维任务空间"和"高维执行器空间"联系起来的线性映射，其转置自动给出对偶方向的映射。记住这个统一视角，你就不必为每个新场景重新推导：看到力映射 \(A\)，立刻知道速度映射是 \(A^\top\)；看到执行器空间维度 \(n\) 大于任务空间维度 \(m\)，立刻知道零空间维度是 \(n-m\)，那里住着"不影响任务的执行器自由度"——单臂叫冗余自运动，多机叫内力。

零空间维度：为什么恒等于 \(3N-6\)¶

抓取矩阵 \(G\in\mathbb{R}^{6\times 3N}\)。当 \(N\ge 2\) 时 \(3N \ge 6\)，\(G\) 是"宽矩阵"（列多于行）。只要抓取点不退化（不全部共线、不全部重合），\(G\) 行满秩，\(\mathrm{rank}(G)=6\)。由秩-零化度定理（rank-nullity theorem）：

\[ \dim\ker G = 3N - \mathrm{rank}(G) = 3N - 6 . \]

这个数字是本章的灵魂。它的含义：在 \(3N\) 维的接触力空间里，有 \(6\) 维的力会"穿透" \(G\) 变成物体运动（外力/运动力），剩下 \(3N-6\) 维的力被 \(G\) 映射为零——施加它们物体一动不动，它们全部变成**内力**。

举几个具体数字建立直觉：

场景	\(N\)	接触力空间 \(3N\)	物体自由度	内力维度 \(3N-6\)
双臂搬运（点接触）	2	6	6	0
三机器人搬运	3	9	6	3
四足搬运/四指抓取	4	12	6	6
五机器人搬运	5	15	6	9

注意一个反直觉的结论：两台机器人用点接触（每点 3 维力）抓物体，内力维度是 0——没有内力！这看似与"双臂搬运会挤爆物体"矛盾。关键在"点接触"假设：如果每个接触只能传递 3 维力（不能传力矩），\(N=2\) 时 \(3N-6=0\) 确实无内力可调。但真实双臂用夹爪是**刚性抓持**，每个接触能传递 6 维力旋量（力 + 力矩），此时 \(G\in\mathbb{R}^{6\times 6N}\)，内力维度变成 \(6N-6\)，\(N=2\) 时是 \(6\) 维——这才是双臂搬运产生内力的来源。这个区别极其重要，我们在下一小节"刚性抓持推广"详细处理。

阶段小结：到这里我们完成了抓取矩阵的核心——式 (5.1) 力映射 \(w_o=Gf\)、式 (5.2) 速度对偶 \(\dot{X}=G^\top v_o\)、零空间维度 \(3N-6\)。接下来要做的是：把"点接触"放松到"刚性抓持"，然后写代码实现 \(G\)。

推广：从点接触到刚性抓持¶

上面用"点接触"（每接触点只传 3 维力）推导，得到 \(G\in\mathbb{R}^{6\times 3N}\)。真实搬运中，机械臂夹爪牢牢夹住物体属于**刚性抓持**——接触界面不仅传力，还传**力矩**。此时第 \(i\) 个接触传递的是 6 维力旋量 \(w_i = [f_i^\top, m_i^\top]^\top\in\mathbb{R}^6\)，抓取矩阵变为 \(G\in\mathbb{R}^{6\times 6N}\)，第 \(i\) 块是

\[ G_i^{\text{rigid}} = \begin{bmatrix} I_3 & 0 \\ \widehat{r}_i & I_3 \end{bmatrix} \in \mathbb{R}^{6\times 6}, \]

下半块多了一个 \(I_3\)：接触力矩 \(m_i\) 直接贡献到物体力矩（不经过力臂）。此时内力维度变为 \(6N-6\)。

下表把"接触模型 → 单点力维度 → 抓取矩阵尺寸 → 内力维度"系统化（这正是 R6E 要求的穷举式分类，而非随意举例）：

接触模型	物理	单点力维 \(c\)	\(G\) 尺寸	内力维度 \(cN-6\)（\(N{=}2\)）
点接触无摩擦	只传法向力	1	\(6\times N\)	\(N-6\)（\(N{<}6\) 时不能完全约束物体）
点接触带摩擦（PCWF）	传 3 维力，受摩擦锥约束	3	\(6\times 3N\)	\(3N-6\)（\(N{=}2\)：0）
软指接触（SF）	传 3 维力 + 法向扭矩	4	\(6\times 4N\)	\(4N-6\)（\(N{=}2\)：2）
刚性抓持	传 6 维力旋量	6	\(6\times 6N\)	\(6N-6\)（\(N{=}2\)：6）

本章后续除非特别说明，以**点接触带摩擦（每点 3 维力，\(G\in\mathbb{R}^{6\times 3N}\)）** 为默认模型（多足搬运、绳索搬运、多机器人末端球铰抓持都属于这一类）；涉及双臂刚性抓持时切换到 6 维模型。§D03 双臂力控用的就是刚性抓持版本，与此处推广一致。

代码实现：抓取矩阵的 Eigen 实现¶

理论讲完，用代码确认。下面的实现把式 (5.1) 的 \(G\) 组装出来。我们遵循"为什么→正确→错误→对比"的展开。

Step 1: 为什么这样写。 抓取矩阵每帧都可能要重算（物体运动 → \(r_i\) 变化 → \(G\) 变化），且尺寸 \(6\times 3N\) 在 \(N\) 不大时（多足搬运 \(N\le 4\)）是小矩阵。按 §F02/Eigen 选型原则：维度运行期才知道（\(N\) 可变）用动态矩阵 MatrixXd，但反对称块用固定大小 Matrix3d（栈分配、SIMD）。

Step 2: 正确写法。

// grasp_matrix.hpp —— 点接触模型 (每点 3 维力), G ∈ R^{6×3N}
#include <Eigen/Dense>
#include <vector>

// 反对称矩阵 skew(r): 满足 skew(r)*v == r.cross(v)
inline Eigen::Matrix3d skew(const Eigen::Vector3d& r) {
  Eigen::Matrix3d S;
  S <<      0, -r.z(),  r.y(),
       r.z(),      0, -r.x(),
      -r.y(),  r.x(),      0;
  return S;
}

// 组装抓取矩阵 G: w_o = G * f
//   contact_points: 各接触点在世界系的位置 x_i
//   load_com:       物体质心位置 p_o
//   返回 6×(3N) 矩阵
Eigen::MatrixXd buildGraspMatrix(
    const std::vector<Eigen::Vector3d>& contact_points,
    const Eigen::Vector3d& load_com)
{
  const int N = static_cast<int>(contact_points.size());
  Eigen::MatrixXd G(6, 3 * N);
  for (int i = 0; i < N; ++i) {
    const Eigen::Vector3d r_i = contact_points[i] - load_com; // 质心→接触点
    G.block<3, 3>(0, 3 * i) = Eigen::Matrix3d::Identity();     // 上块: I3 (力)
    G.block<3, 3>(3, 3 * i) = skew(r_i);                       // 下块: r̂ (力矩)
  }
  return G;
}

Step 3: 错误写法及原因。

// ❌ 错误 1: r_i 方向搞反 (用 com - point 而非 point - com)
Eigen::Vector3d r_i = load_com - contact_points[i];  // 符号反了!
// 后果: 力矩贡献整体反号, 物体会朝期望转动的反方向转。
//       仿真里表现为"一控制就翻", 极难一眼看出, 因为力(上块)是对的、
//       只有力矩(下块)错, 平移搬运看着正常、一旋转就发散。
// 正确: r_i = contact_points[i] - load_com; (质心指向接触点)

// ❌ 错误 2: 用世界系接触点但物体姿态变了却不更新 r_i
// 物体转动后, 接触点相对质心的几何 r_i 在世界系会变,
// 若缓存了初始 r_i 不更新, G 就过时了 → 力矩臂算错。
// 正确: 每帧用当前 contact_points 和 load_com 重算, 或在物体体坐标系
//       中固定 r_i^body, 用当前姿态 R_o 旋转: r_i = R_o * r_i^body。

// ❌ 错误 3: 把 skew 写成 Matrix3d S; (未初始化) 再逐元素赋值时漏项
Eigen::Matrix3d S;            // 6 个非对角元素是栈垃圾值
S(0,1) = -r.z(); S(0,2) = r.y(); /* 漏写对角线和下三角 */
// 后果: 对角线和未赋值元素是随机数, G 完全错乱。
// 正确: 用上面 skew() 一次性 << 全部 9 个元素 (含 3 个 0)。

Step 4: 三种构造方式对比。

// 方式 A: 逐块手动组装 (上面的实现, 最清晰, 推荐教学/理解)
//   优点: 每一块的物理含义一目了然; 缺点: 代码稍长

// 方式 B: 用刚体旋量库 (如 Pinocchio 的 SE3::act / Force) 自动处理
//   pinocchio::SE3 oMc(R_i, contact_points[i]);
//   各接触力旋量经 oMc.actInv() 变换到物体系再累加
//   优点: 自动处理坐标系变换、力矩传递; 缺点: 引入重型依赖

// 方式 C: 仅力部分 (点接触), 直接构造 [I; skew] 列块 (本节默认)
//   适合多足/绳索搬运, 不需要传力矩

// | 方式 | 可读性 | 依赖 | 适用 |
// |------|--------|------|------|
// | A    | ⭐⭐⭐  | 仅Eigen | 教学、点接触、N小 |
// | B    | ⭐⭐    | Pinocchio | 刚性抓持、需坐标变换 |
// | C    | ⭐⭐⭐⭐ | 仅Eigen | 多足/绳索快速实现 |

理论-工程桥接：抓取矩阵在多足搬运里长什么样¶

正因为 \(G\) 只依赖接触点几何 \(r_i\)，工程上多足机器人协同搬运时，接触点的几何布局直接决定了 \(G\) 的条件数和内力空间的"形状"——这不是抽象的数学，而是真实的设计决策。如果四台四足机器人把附着点布得太靠近（\(r_i\) 都很小且方向接近），\(G\) 的下半块 \(\widehat{r}_i\) 都很"弱"，物体的转动控制权威性低、\(GG^\top\) 病态，力分配（§5.6）会放大噪声。所以 Sombolestan 的多四足绳索搬运、decPLM 的 pinch-lift-move，都隐含地要求附着点"撑开"以获得良好的力矩臂——这正是 \(G\) 的几何在工程中的回响。

⚠️ §5.1 常见陷阱¶

陷阱 1（编程陷阱）：反对称矩阵符号反号导致"一转就翻"

⚠️ 编程陷阱: skew(r) 或 r_i 方向写反
   错误做法: 把 r_i 写成 load_com - contact_point, 或 skew 矩阵
            上/下三角符号写反 (常见: 把 -r.z() 写成 r.z())。
   现象: 纯平移搬运 (只用力、不用力矩) 一切正常, 但只要物体需要
        旋转, 力矩方向就整体反号, 控制器把物体往反方向越推越远,
        表现为"一旋转就发散/翻转"。因为力(上块 I3)是对的, bug
        藏在力矩(下块)里, 平移测试根本暴露不出来。
   根本原因: 抓取矩阵下半块 r̂_i 编码的是"力臂叉乘", 叉积反交换,
            r_i 反号或 skew 符号反号都让 τ = r×f 整体变 -τ。
   正确做法: 严格用 r_i = contact_point - load_com; skew 用统一函数
            (本节 skew())。自检: 验证 skew(r)*v == r.cross(v) 对
            随机 r,v 成立 (单元测试一行搞定), 且 G*f 对一个已知
            "纯力偶"接触力应给出预期力矩方向。

陷阱 2（概念误区）：以为"两台机器人点接触搬运一定有内力"

💡 概念误区: 认为只要两台机器人一起搬, 就一定要操心内力
   新手想法: "双臂搬钢管会挤爆物体 → 说明双臂搬运总有内力要控制"
   实际上: 内力维度 = cN - 6, c 是单接触力维度。两台机器人若是
          点接触 (c=3), 内力维度 = 3·2 - 6 = 0, 数学上根本没有
          内力自由度! "挤爆物体"发生在刚性抓持 (c=6, 内力维度 6)
          或两端用夹爪传力矩的情形。先分清接触模型, 再谈内力。
   为什么重要: 搞错接触模型会导致你为一个不存在的内力空间设计控制器
            (点接触双机), 或漏掉真实存在的内力 (刚性抓持双机)。
   延伸思考: 三台机器人点接触 (c=3) 内力维度 = 3, 此时即便是点接触
            也有内力 (三台可以互相"对拉"而不动物体)。"几台机器人
            开始有内力"取决于 cN 何时超过 6。

陷阱 3（思维陷阱）：把抓取矩阵当成"又一个要背的矩阵"

🧠 思维陷阱: 孤立地记忆抓取矩阵公式, 不和雅可比建立联系
   新手想法: "抓取矩阵 G = [I; r̂] 是多机搬运的专用公式, 背下来即可"
   实际上: G 和单臂雅可比 J 是同一类对象——任务空间↔执行器空间的
          线性映射, 转置给对偶方向。J: q̇→ẋ (速度) / τ=JᵀF (力);
          G: f→w (力) / Ẋ=Gᵀv_o (速度)。理解这个统一视角后, 你
          看到任何"低维任务、高维执行器"的映射都能立刻写出零空间
          维度 = 执行器维 - 任务维, 并知道零空间住着"不影响任务的
          自由度"(单臂:冗余自运动; 多机:内力; 多指:抓取内力)。
   正确思维: 学新映射时问三件事: (1) 力往哪个方向映射(确定 G 还是 Gᵀ)?
          (2) 哪边维度高(确定零空间在哪边)? (3) 零空间的物理意义
          是什么? 而非死记公式。
   自检方法: 不看书, 仅凭"功守恒"从 w=Gf 推出 Ẋ=Gᵀv_o; 仅凭秩-零化度
          说出任意接触模型的内力维度。

§5.1 练习¶

（推导题，⭐⭐） 三台机器人点接触抓持一个刚体，接触点相对质心分别为 \(r_1=(1,0,0)\)、\(r_2=(-0.5,\,0.866,\,0)\)、\(r_3=(-0.5,\,-0.866,\,0)\)（正三角形布局，单位米）。手写出 \(9\times 3\) 的 \(G^\top\)（即 \(\dot{X}=G^\top v_o\) 的映射），并验证 \(\mathrm{rank}(G)=6\)、\(\dim\ker G=3\)。给出 \(\ker G\) 的一组基向量的物理解释（提示：三台机器人"沿三条边互相对拉"且合力合力矩为零的力模式）。
（实现题，⭐⭐） 把本节 buildGraspMatrix 扩展为刚性抓持版本（每点 6 维力旋量，\(G\in\mathbb{R}^{6\times 6N}\)）。写一个单元测试：随机生成 \(N=2\) 的接触点与一组接触力旋量 \(w_i\)，验证 (a) \(G f\) 的力部分等于各 \(f_i\) 之和；(b) 当你施加一对"等大反向、沿连线"的力（纯内力）时，\(Gf=0\)。报告你构造的纯内力向量。
（设计扩展题，⭐⭐⭐） 物体在运动中姿态不断变化。设计一个 GraspModel 类，在物体体坐标系中固定存储各接触点 \(r_i^{\text{body}}\)，每帧输入当前物体姿态 \(R_o\in SO(3)\)，输出当前世界系抓取矩阵 \(G\)。讨论：为什么在体坐标系存储 \(r_i\) 比每帧重测世界系接触点更鲁棒？如果某台机器人在搬运中打滑、接触点偏移了，你的模型会出什么问题、如何检测？

§5.2 内力与外力分解 ⭐⭐⭐¶

这一节解决什么问题：§5.1 告诉我们 \(w_o=Gf\) 是欠定的——同一个物体运动可以由无穷多组接触力实现。这一节回答："这无穷多组里，哪一部分真正在搬物体、哪一部分只是在互相挤压？"我们把接触力 \(f\) 干净地劈成两半——外力（运动力） 负责让物体动，内力负责维持抓持（夹紧/张紧）却不影响运动。这个分解是协调阻抗（§5.5）、负载分配（§5.6）的直接前提，也是"为什么物体会被挤爆"的精确解释。

动机：同一个搬运，无穷多种用力方式¶

回到两台机器人抬钢梁。假设钢梁重 100 牛，要匀速向上抬（需要向上 100 牛的合力抵消重力）。下面三种用力方式都能完成任务：

方式甲：两台机器人各向上出 50 牛。合力 100 牛向上，✓。
方式乙：左机器人向上 80 牛、右机器人向上 20 牛。合力仍 100 牛向上，✓。
方式丙：两台各向上 50 牛，**同时**各自向内（朝对方）水平推 200 牛。竖直合力 100 牛向上、水平合力 0（左右抵消），✓。

三种方式对物体的**运动效果完全相同**（都是匀速上抬），但对物体的**内部受力天差地别**：方式丙里钢梁被两侧各 200 牛水平挤压——如果钢梁是易碎的玻璃管，方式丙会把它捏碎，方式甲不会。

这就引出核心问题：接触力 \(f\) 里，哪一部分决定运动、哪一部分决定挤压？我们需要一个数学手术，把 \(f\) 精确地切成"运动力"和"内力"两块，分别控制。运动力由任务（物体该怎么动）决定，内力由抓持需求（夹多紧才不滑落、又不夹碎）决定——这两个目标正交，应该独立设定。

如果不区分内外力会怎样¶

假设我们拒绝做分解，直接让控制器"算出一组能实现物体运动的接触力"就用。会遇到三个无法回避的问题。

第一，内力失控、物体被挤爆或脱手。 不分解，你就没有"内力"这个旋钮。控制器随便挑一组满足 \(Gf=w\) 的解——可能恰好带着巨大的挤压内力（玻璃管碎），也可能内力为负（机器人互相"对拉"，物体被扯松甚至脱手）。§5.0 场景一的"挤爆"和"脱手"，本质都是内力无人管。

第二，解的不唯一性无法消解。 \(Gf=w\) 有 \(3N-6\) 维解空间，不分解你就没有原则去挑哪一个。"随便挑一个"在工程上等于"行为不可预测"——今天求解器给你这组、明天数值扰动给你另一组，内力大小飘忽不定。

第三，无法把"运动控制"和"抓持控制"解耦设计。 运动控制器关心"物体跟没跟上轨迹"，抓持控制器关心"夹得紧不紧"。不分解，这两个目标揉在一组接触力里互相干扰——你调大抓持力，可能意外改变了运动力。分解之后，二者落在正交子空间，可以各调各的，互不影响。

本质洞察：内力/外力分解的深层意义，是把一个 \(3N\) 维的"混沌力空间"**正交分解**成两个物理意义清晰、且可独立指定的子空间：\(\mathbb{R}^{3N} = \underbrace{\mathrm{range}(G^+)}_{\text{运动力, 6维}} \oplus \underbrace{\ker(G)}_{\text{内力, }(3N-6)\text{维}}\)。运动力子空间由"物体该怎么动"驱动，内力子空间由"该怎么夹"驱动。这个正交分解不是数学游戏——它是"运动"与"抓持"两个物理任务在力空间里的天然边界。

历史：从伪逆解到非挤压分配¶

最朴素的分配是用 Moore-Penrose 伪逆：\(f = G^+ w\)，它给出满足 \(Gf=w\) 的**最小范数**解。1986 年 Orin-Oh 的闭链力分配、以及无数后续工作都从这里起步。但 1988 年 Walker、Freeman、Marcus 在 ICRA 上指出一个微妙而重要的问题：Moore-Penrose 伪逆给出的解，一般会在物体内部产生不必要的挤压力——它"最小范数"是在数学意义上最小，但并不"非挤压"。他们构造了考虑抓取几何的特殊伪逆（后称 Walker 非挤压伪逆），证明存在唯一的力分配使内力为零。

这个发现的意义在于：它第一次把"内力"从"伪逆解的副产品"提升为"需要显式设计的独立量"。现代多机搬运（Caccavale 的 object impedance、Wimböck-Ott 的物体级抓取控制、Verginis 的内力调节）全部建立在"内力可显式指定、且应独立于运动力设计"这一认识上。Verginis & Dimarogonas 2019 更进一步，从**刚度理论（rigidity theory）** 视角揭示了抓取矩阵的零空间与图刚度矩阵的 range-nullspace 关系，给出了能量最优的内力分配。

理论：内力/外力分解的推导¶

分解公式¶

接触力 \(f\in\mathbb{R}^{3N}\) 要满足 \(Gf = w_{\text{ext}}\)，其中 \(w_{\text{ext}}\) 是期望施加给物体的外部力旋量（由运动控制器算出，见 §5.5）。这是一个欠定线性方程，通解 = 特解 + 齐次解：

\[ \boxed{\; f = \underbrace{G^+ w_{\text{ext}}}_{f_{\text{ext}}\,(\text{运动力})} + \underbrace{(I - G^+G)\,\eta}_{f_{\text{int}}\,(\text{内力})}, \qquad \eta\in\mathbb{R}^{3N}\ \text{任取}. \;} \tag{5.3} \]

运动力（外力） \(f_{\text{ext}} = G^+ w_{\text{ext}}\)：满足 \(G f_{\text{ext}} = w_{\text{ext}}\)（确实产生期望旋量），且是最小范数特解，位于 \(\mathrm{range}(G^+) = \mathrm{range}(G^\top) = \mathrm{row}(G)\)。
内力 \(f_{\text{int}} = (I-G^+G)\eta = P_N\,\eta\)：被 \(G\) 映射为零（\(G f_{\text{int}} = 0\)，因为 \(GP_N = G - GG^+G = G-G = 0\)），不影响物体运动，位于 \(\ker G\)。\(\eta\) 是设计者指定的内力意图（"想夹多紧"）。

记零空间投影矩阵 \(P_N := I - G^+G\)。它是到 \(\ker G\) 的**正交投影**（当 \(G^+\) 取 Moore-Penrose 伪逆时），满足两个关键性质：

\[ P_N^2 = P_N\ (\text{幂等}),\qquad P_N^\top = P_N\ (\text{对称/正交投影}). \]

为什么 \(f_{\text{ext}}\) 和 \(f_{\text{int}}\) 正交？ \(f_{\text{ext}}\in\mathrm{row}(G)\)，\(f_{\text{int}}\in\ker(G)\)，而线性代数基本定理告诉我们 \(\mathrm{row}(G)\perp\ker(G)\)（行空间与零空间是正交补）。所以 \(f_{\text{ext}}^\top f_{\text{int}} = 0\) 恒成立。这个正交性是"运动力与内力互不干扰"的数学根基——你改变内力意图 \(\eta\)，\(f_{\text{int}}\) 在零空间里变，但 \(f_{\text{ext}}\) 纹丝不动，物体运动完全不受影响。这与 §F02 单臂零空间投影"次要任务不破坏主任务"是同一个数学。

内力为什么"不影响运动"——再确认¶

验证 \(f_{\text{int}}\) 真的不产生物体旋量：

\[ G f_{\text{int}} = G(I-G^+G)\eta = (G - GG^+G)\eta . \]

对 Moore-Penrose 伪逆，\(GG^+G = G\)（这是伪逆四条件之一），所以 \(Gf_{\text{int}} = (G-G)\eta = 0\)。✓

物理上：内力是各接触点之间"自相平衡"的力——它们的合力为零、合力矩也为零，所以对物体质心不产生任何净效应，只在物体内部造成应力（挤压或张紧）。

内力子空间的基：把 \((3N-6)\) 维内力参数化¶

\(f_{\text{int}}=P_N\eta\) 用 \(3N\) 维的 \(\eta\) 参数化一个只有 \(3N-6\) 维的子空间，是冗余的（\(\eta\) 里有 6 维被 \(P_N\) 投掉了，浪费）。更经济的做法是找到 \(\ker G\) 的一组基 \(E\in\mathbb{R}^{3N\times(3N-6)}\)（列向量张成 \(\ker G\)，即 \(GE=0\) 且 \(E\) 列满秩），用 \((3N-6)\) 维参数 \(\lambda\) 表示内力：

\[ f_{\text{int}} = E\,\lambda, \qquad \lambda\in\mathbb{R}^{3N-6} . \]

\(\lambda\) 的每个分量直接对应一个独立的"内力模式"的强度，物理意义清晰。怎么求 \(E\)？数值上对 \(G\) 做 SVD 或 QR，零奇异值对应的右奇异向量就是 \(\ker G\) 的标准正交基。

三机器人正三角形的内力基（具体例子）：三台机器人点接触（\(N=3\)，内力维度 \(3N-6=3\)）。\(\ker G\) 的三个基模式有清晰物理图像：

内力模式	物理图像	合力	合力矩
模式 1	三台沿"指向质心"方向同时内推（径向夹紧）	0	0
模式 2	三台沿切向同时施力（绕质心扭转的对抗）	0	0
模式 3	一对机器人对拉、第三台平衡（边内力）	0	0

每个模式的合力合力矩都为零（否则就不在 \(\ker G\) 里了），但都在物体内部产生真实应力。控制器通过设定 \(\lambda\) 选择"夹多紧"——通常 \(\lambda\) 取正值的径向夹紧模式，确保接触不滑脱（满足摩擦锥，§5.6）。

类比的边界：内力之于多机搬运，恰如"预紧力（preload）"之于螺栓连接。像的部分：都是不产生宏观运动、只在结构内部维持的力，都用来保证连接/接触不松脱。不像的部分：螺栓预紧力是静态、一次设定的；多机内力是动态、可实时调节的（接近时小、抬起时大、放下时归零），且必须保持在摩擦锥内否则打滑——螺栓没有"打滑"这个失效模式。

Moore-Penrose 解为什么"挤压"——Walker 的洞察¶

设计者常想要"零内力"分配——既能搬物体、又不在物体里造任何挤压。直觉上 \(f=G^+w_{\text{ext}}\)（取 \(\eta=0\)）应该零内力，因为它"最小范数"。但 Walker 1988 指出这**未必**是物理意义上的零内力分配。

问题出在"内力"的定义依赖于伪逆的加权方式。Moore-Penrose 伪逆最小化的是 \(\|f\|_2\)（接触力的欧氏范数），这在数学上对应一个特定的内力分解。但物理上"非挤压"要求的是**接触界面上没有相互挤压的分量**——这取决于接触点的几何与物体的刚度结构，未必与"最小欧氏范数"一致。

Walker 的非挤压伪逆通过引入与抓取几何相关的加权 \(W\)，构造加权伪逆 \(G_W^+ = W^{-1}G^\top(GW^{-1}G^\top)^{-1}\)，使得 \(f=G_W^+ w_{\text{ext}}\) 落在"无挤压子空间"内。关键结论："最小范数"和"非挤压"是两个不同的最优性，选哪个伪逆取决于你的目标。工程实践中，多数现代方法干脆放弃追求"伪逆自带零内力"，而是显式地把内力作为独立量设计（式 (5.3) 的 \(\eta\) 或 \(\lambda\)），把内力设为期望的夹紧值而非零——因为真实搬运本来就需要非零夹紧力防滑落。

本质洞察：不要指望"一个聪明的伪逆"自动给你理想的力分配。伪逆只决定**运动力**那 6 维怎么取（最小范数？加权最小？），它对**内力**那 \(3N-6\) 维是沉默的——内力必须由你根据抓持需求显式指定。把希望寄托在伪逆上，等于把一个需要工程判断的决策（夹多紧）外包给了一个数学默认值（恰好为零或恰好最小范数），这正是物体被挤爆或脱手的认知根源。

阶段小结：到这里我们完成了内力/外力分解的全部理论——式 (5.3) 通解、运动力 \(G^+w\) 与内力 \(E\lambda\) 的正交分解、内力基的构造、以及"伪逆不自动给非挤压解"的 Walker 洞察。接下来用代码把分解实现出来，并验证正交性。

代码实现：内力/外力分解与内力基¶

Step 1: 为什么这样写。 分解的核心是伪逆 \(G^+\) 和零空间基 \(E\)。\(GG^\top\) 在抓取点不退化时是 \(6\times 6\) 正定小矩阵，可用 Cholesky/LDLT 稳定求逆。零空间基用 SVD 提取最稳健（QR 也可，但 SVD 直接给出奇异值，便于检测退化）。

Step 2: 正确写法。

// internal_force.hpp
#include <Eigen/Dense>

struct ForceDecomposition {
  Eigen::MatrixXd G_pinv;   // G^+  (3N × 6)
  Eigen::MatrixXd P_null;   // I - G^+G  零空间投影 (3N × 3N)
  Eigen::MatrixXd E;        // ker(G) 的标准正交基 (3N × (3N-6))
};

// 由抓取矩阵 G 计算分解所需的全部算子
ForceDecomposition computeDecomposition(const Eigen::MatrixXd& G) {
  const int cols = G.cols();           // 3N
  ForceDecomposition d;

  // 1) Moore-Penrose 伪逆: G^+ = G^T (G G^T)^{-1}
  //    GG^T 是 6×6 正定 (抓取点不退化时), 用 LDLT 稳定求解
  Eigen::MatrixXd GGt = G * G.transpose();          // 6×6
  d.G_pinv = G.transpose() * GGt.ldlt().solve(
                 Eigen::MatrixXd::Identity(6, 6));   // 3N×6

  // 2) 零空间投影 P = I - G^+ G   (到 ker G 的正交投影)
  d.P_null = Eigen::MatrixXd::Identity(cols, cols) - d.G_pinv * G;

  // 3) ker(G) 的标准正交基: 对 G 做 SVD, 取零奇异值对应的右奇异向量
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(
      G, Eigen::ComputeFullV);
  const int rank = 6;                                // 满秩时 rank(G)=6
  d.E = svd.matrixV().rightCols(cols - rank);        // 3N×(3N-6)
  return d;
}

// 给定期望外力旋量 w_ext 和内力意图 lambda, 合成接触力 f
Eigen::VectorXd composeContactForces(
    const ForceDecomposition& d,
    const Eigen::VectorXd& w_ext,      // 6
    const Eigen::VectorXd& lambda)     // 3N-6
{
  Eigen::VectorXd f_motion   = d.G_pinv * w_ext;     // 运动力 (在 row(G))
  Eigen::VectorXd f_internal = d.E * lambda;          // 内力 (在 ker G)
  return f_motion + f_internal;
}

Step 3: 错误写法及原因。

// ❌ 错误 1: 用 (G^T G) 而非 (G G^T) 求伪逆
//    G^T G 是 3N×3N, 且因 G 行满秩、列亏秩, G^T G 奇异(秩仅6)!
Eigen::MatrixXd bad = (G.transpose()*G).inverse() * G.transpose(); // 崩
// 后果: G^T G 不可逆, .inverse() 给出 NaN/巨大值。
// 正确: G 行满秩用右伪逆 G^T(GG^T)^{-1}; GG^T 才是 6×6 可逆的。

// ❌ 错误 2: 把内力意图直接写进 w_ext
Eigen::VectorXd w_bad = w_ext_motion + squeeze_wrench; // 概念错误!
// 后果: 内力的合旋量本应为 0, 你把它塞进 w_ext 等于要求物体产生
//       一个不存在的运动, 运动力被污染。
// 正确: 内力只能通过 ker(G) 注入 (f_internal = E*lambda), 永远不进 w_ext。

// ❌ 错误 3: 用 P_null * (任意η) 当内力, 没归一化导致内力大小不可控
Eigen::VectorXd f_int = d.P_null * arbitrary_eta; // η 量纲不明
// 后果: f_int 的物理大小取决于 arbitrary_eta 的随意取值, 你说不清
//       "夹了多少牛"。
// 正确: 用标准正交基 E, lambda 的每个分量直接是该内力模式的强度(牛),
//       便于设定和限幅。

Step 4: 两种内力参数化对比。

// 方式 A: 投影法 f_int = (I - G^+G) η     (η ∈ R^{3N})
//   优点: 不需算基 E, 一步到位; 缺点: η 冗余(6维被投掉)、量纲不直观

// 方式 B: 基展开法 f_int = E λ            (λ ∈ R^{3N-6})
//   优点: λ 维度最小、每分量是独立内力模式强度、便于限幅与优化;
//   缺点: 需先算 E (SVD/QR)

// | 方式 | 参数维度 | 物理直观 | 算力 | 适用 |
// |------|---------|---------|------|------|
// | A 投影 | 3N (冗余) | 弱 | 低 | 快速原型、内力=0 时 |
// | B 基   | 3N-6 (最小) | 强 | 中(需SVD) | 内力优化、限幅、QP |

验证正交性（Python 辅助）¶

import numpy as np
# 三机器人正三角形点接触, 验证分解的正交性与零空间维度
r = [np.array([1,0,0]), np.array([-0.5, 0.866,0]), np.array([-0.5,-0.866,0])]
def skew(v): return np.array([[0,-v[2],v[1]],[v[2],0,-v[0]],[-v[1],v[0],0]])
G = np.hstack([np.vstack([np.eye(3), skew(ri)]) for ri in r])  # 6×9
assert np.linalg.matrix_rank(G) == 6                # 行满秩
Gp = G.T @ np.linalg.inv(G @ G.T)                   # 伪逆
P  = np.eye(9) - Gp @ G                             # 零空间投影
# 内力基: SVD 右奇异向量末 3 列
_,_,Vt = np.linalg.svd(G); E = Vt[6:].T            # 9×3
assert np.allclose(G @ E, 0)                        # GE=0, 确在零空间
# 正交性: 运动力 ⟂ 内力
w = np.array([0,0,100,0,0,0])                       # 100N 向上
f_motion = Gp @ w; f_int = E @ np.array([5,0,0])   # 某内力模式强度5
print("motion·internal =", f_motion @ f_int)        # ≈ 0, 正交!
print("G@f_int (应≈0) =", G @ f_int)                # 内力不产生旋量

理论-工程桥接：内力调节在真机里就是"夹紧力控制"¶

正因为内力位于 \(\ker G\)、与运动力正交，工程上才能给多机搬运系统装一个独立的"夹紧力旋钮"：抓取阶段把内力（径向夹紧模式 \(\lambda\)）从 0 缓升到防滑所需值，搬运中保持，放下时降回 0——全程不影响物体跟踪轨迹。decPLM 的 "pinch"（捏紧）、Sombolestan 绳索搬运的张力维持，本质都是在 \(\ker G\) 里调 \(\lambda\)。这就是抽象的"零空间"在真机上的物理化身：一个可以独立拧、不干扰运动的夹紧旋钮。

⚠️ §5.2 常见陷阱¶

陷阱 1（编程陷阱）：误用 \(G^\top G\) 求伪逆导致奇异

⚠️ 编程陷阱: 套用"左伪逆" (G^T G)^{-1} G^T 处理行满秩的宽 G
   错误做法: 不分清 G 是行满秩(宽)还是列满秩(高), 一律用左伪逆。
   现象: G^T G 是 3N×3N 但秩只有 6, 严重奇异, .inverse() 返回
        NaN 或天文数字, 整个力分配崩溃。
   根本原因: 左伪逆 (A^T A)^{-1}A^T 要求 A 列满秩; 抓取矩阵 G 是
            6×3N 行满秩(列亏秩), 必须用右伪逆 G^T(GG^T)^{-1}。
   正确做法: 行满秩宽矩阵 → 右伪逆 (GG^T 是 6×6 可逆); 列满秩高
            矩阵 → 左伪逆。自检: 打印 G G^T 的最小奇异值, 远大于 0
            说明抓取点不退化、可安全求逆。

陷阱 2（概念误区）：以为伪逆解 \(G^+w\) 自动零内力

💡 概念误区: 认为最小范数解 f=G^+w 就是"零挤压"分配
   新手想法: "G^+ 给最小范数解, 范数最小当然没多余的内力"
   实际上: 最小范数(数学)≠非挤压(物理)。G^+w 在 ker(G) 上的分量
          确实为 0 (它在 row(G) 里), 所以它的"内力分量"按
          Moore-Penrose 定义是 0——但这个"零内力"是相对于欧氏范数
          的, 物理接触界面上是否真无挤压, 取决于加权方式(Walker)。
          更重要的是: 真实搬运需要非零夹紧力防滑, 你本就不该追求零内力!
   为什么重要: 若盲信"伪逆=零内力", 你既可能误判系统无挤压(实则有),
            又会漏掉必须主动施加的防滑夹紧力, 导致物体滑落。
   延伸思考: 内力该取多少? 由摩擦锥定下界(夹紧到不滑)、由物体强度
            定上界(不夹碎), 是 §5.6 的优化问题, 不是伪逆的副产品。

陷阱 3（思维陷阱）：把内力当"误差"而非"需要主动设计的量"

🧠 思维陷阱: 视内力为应当消除的副作用
   新手想法: "内力不产生运动、还可能挤坏物体, 越小越好、最好为零"
   实际上: 内力是抓持的必需品。零内力意味着零夹紧力——除非物体被
          双边刚性焊住, 否则点接触/摩擦抓持下零夹紧 = 立刻滑落。
          正确目标不是"消除内力", 而是"把内力调到恰当值":
          下界 = 防滑所需(摩擦锥), 上界 = 不损坏物体。
   正确思维: 把内力看成和运动力平起平坐的设计变量——运动力服务
          "物体怎么动", 内力服务"物体怎么被稳稳抓住", 两者都要主动给。
   自检方法: 问自己"我的搬运若内力设为 0 会怎样?" 若答案是"物体滑落",
          说明内力是刚需; 若答案是"无影响"(刚性焊接), 才可设 0。

§5.2 练习¶

（推导题，⭐⭐） 证明零空间投影 \(P_N = I-G^+G\)（\(G^+\) 为 Moore-Penrose 伪逆）满足幂等性 \(P_N^2=P_N\) 和对称性 \(P_N^\top=P_N\)。再证明对任意 \(w_{\text{ext}}\) 和任意 \(\eta\)，运动力 \(G^+w_{\text{ext}}\) 与内力 \(P_N\eta\) 正交。（提示：用伪逆四条件 \(GG^+G=G\)、\(G^+GG^+=G^+\)、\((GG^+)^\top=GG^+\)、\((G^+G)^\top=G^+G\)。）
（实现题，⭐⭐⭐） 基于本节代码，实现一个 InternalForceRegulator：输入期望外力旋量 \(w_{\text{ext}}\)、期望夹紧强度（标量 \(s>0\)）、各接触点的内向单位方向，输出接触力 \(f\)，使得 (a) \(Gf=w_{\text{ext}}\)；(b) 内力对应"所有接触点以强度 \(s\) 径向内夹"。验证你的 \(f\) 的内力分量确实在 \(\ker G\) 里（\(G\cdot f_{\text{int}}\approx 0\)），并扫描 \(s\) 从 0 到 50 牛，画出"夹紧强度 vs 接触力范数"曲线。
（跨章综合题，⭐⭐⭐，综合 §5.1+§5.2+§D3.4） §D3.4 用伪逆把多项式轨迹的"固定/自由导数"分解，本节用伪逆把接触力的"运动/内力"分解。请用一张表对比两处伪逆的：(a) 被分解的物理量；(b) 行空间分量的物理意义；(c) 零空间分量的物理意义；(d) 谁是"任务约束"、谁是"自由设计量"。并论证：为什么这两个看似无关的问题（轨迹生成 vs 力分配）能用同一套"伪逆 + 零空间投影"数学统一——它们共同的抽象结构是什么？

§5.3 leader-follower 力协调 ⭐⭐⭐¶

这一节解决什么问题：§5.1-5.2 给了"力怎么分"的数学，但没回答"谁来决策"。当 \(N\) 台机器人搬一个物体，总得有人知道"物体该去哪"。最直接的答案是设一个 leader：leader 知道目标轨迹、用阻抗控制领着物体走，其余 follower 不知道目标，只通过感受物体的运动来"猜"leader 的意图并配合。这一节讲清这套主从架构的信息流、follower 如何估计意图、以及它为什么能用很小的 leader 力撬动整个队伍。

动机：搬一个长沙发上楼¶

想象你和朋友搬一个长沙发上楼。你们不可能每挪一步都开口商量"现在向左转 15 度、再向前 0.3 米"——太慢、太累。现实中怎么协作？通常一个人（在前面、看得见楼梯）当**主导**，另一个人（在后面）当**跟随**。主导者决定往哪走、怎么转，跟随者不需要知道完整计划，只需要**感受沙发传来的力和运动**，顺着它使劲就行。主导者向左带，跟随者感到沙发向左动，就配合向左推。

这套"一人主导、其余跟随、靠物体传递意图"的模式，就是 leader-follower（主从）架构。它的精妙在于：跟随者不需要任何关于目标的信息，也不需要和主导者通信——沙发本身就是通信信道。主导者把意图"写"进沙发的运动，跟随者从沙发的运动里"读"出意图。

放到机器人上：leader 机器人知道物体期望轨迹 \(x_d(t)\)，用阻抗控制驱动物体跟踪；follower 机器人不知道 \(x_d(t)\)，只测量物体（或自己接触点）的运动，据此推断 leader 想干什么，然后产生配合的力。

如果不用 leader-follower（全部机器人都当 leader）会怎样¶

一个自然的反问：为什么不让每台机器人都知道目标轨迹、都当 leader？让所有机器人都跟踪同一条物体轨迹，岂不更直接？这就是"全集中式"或"全主导"方案。它在三处会出问题。

第一，需要给每台机器人广播精确的、时间同步的目标轨迹。 所有机器人必须在每个时刻对"物体现在该在哪"达成比特级一致。任何通信延迟或不同步，都会让不同机器人跟踪略微不同的目标，在刚性物体上立刻产生内力（回到 §5.2 的挤压问题）。leader-follower 把"知道轨迹"的责任集中到一个 leader，从根本上消除了"多个 leader 目标不一致"的隐患。

第二，需要每台机器人都精确知道自己接触点相对物体的位置。 要跟踪物体轨迹，机器人得知道"物体在这个位姿时，我的接触点该在哪"——这需要精确的抓取几何标定。follower 方案绕开了这点：follower 不跟踪绝对轨迹，只对物体的相对运动做出反应。

第三，丧失"力放大"的好处。 这是 leader-follower 最迷人的特性（下面详述）：leader 只需用很小的力"提议"一个运动方向，follower 们会**对齐并放大**这个力，使物体获得远大于 leader 单独所能提供的驱动力。全主导方案里没有这种放大——每台机器人独立出全力，没有"小力撬大力"的杠杆。

本质洞察：leader-follower 不是"偷懒少给 follower 信息"，而是用**物体作为信息聚合器**，把"协调"从"通信问题"转化为"物理感知问题"。意图不通过网络传，而通过物体的运动传——这让系统对通信故障免疫、对机器人数量可扩展。代价是 follower 的响应有物理延迟（要先感受到运动才反应），且要求物体足够刚以快速传递意图。

历史：从遥操作主从到协作搬运¶

leader-follower 的思想源头是**遥操作（teleoperation）**——人操作 master 设备，远端 slave 机器人跟随，二者通过通信链路或物理介质耦合（§5.7 会深入遥操作的无源性）。1990 年代，研究者把这套主从思想从"人-机器人"推广到"机器人-机器人"协作搬运：Khatib 的多臂操作、Caccavale 的双臂协调里都有 leader/follower 角色的雏形。

2018 年 Wang 与 Schwager 的隐式通信搬运、2021 年 Carey 与 Werfel 的 Force-ANTS 把这套思想推到极致：follower 完全不通信、不知道目标，仅靠本地测量物体运动来配合 leader，并量化了 leader 的小力如何被 follower 放大（"Force-Amplifying N-robot Transport System"）。这条线证明了 leader-follower 可以做到极简——follower 只需一个力传感器和一条阻抗/导纳律。

理论：leader 与 follower 的控制律¶

leader：用阻抗领航¶

leader 知道物体期望轨迹 \(x_d(t)\)（位置 + 姿态）。它对物体施加一个阻抗律，让物体表现得像被一根弹簧-阻尼器拉向期望轨迹：

\[ w_{\text{leader}} = K_L(x_d - x_o) + D_L(\dot{x}_d - \dot{x}_o), \tag{5.4} \]

其中 \(x_o,\dot{x}_o\) 是物体（leader 可观测的部分）的实际位姿与速度，\(K_L,D_L\) 是 leader 的刚度/阻尼。这本质是 §F01 的阻抗律施加在物体上——leader 把物体"拉"向期望轨迹，但用的是柔性的力而非硬位置命令（所以遇到障碍/失配时不会硬顶）。leader 通过其接触点把 \(w_{\text{leader}}\) 注入物体（再经 §5.2 分解出该 leader 的接触力）。

follower：估计意图并配合¶

follower 不知道 \(x_d\)。它只能测量物体的运动（或自己接触点的力/运动）。follower 的目标是：顺着 leader 想要的方向出力，帮物体运动，同时维持抓持内力。常见两类 follower 策略：

策略 A：导纳型 follower（顺从物体运动）。 follower 测量物体施加在它接触点的力 \(f_{\text{sensed}}\)（这力包含了 leader 通过物体传来的"拖拽"），用导纳律产生顺从运动：

\[ M_F\ddot{x}_F + D_F\dot{x}_F = f_{\text{sensed}} - f_{\text{int,desired}}, \tag{5.5} \]

即 follower 像一个"被推着走"的质量-阻尼系统——leader 拖拽物体，物体拖拽 follower，follower 顺从地动起来，从而不阻碍 leader 的运动，同时减去期望内力部分以维持夹紧。

策略 B：力放大 follower（对齐并放大 leader 的力）。 这是 Force-ANTS 的核心。follower 估计物体的运动方向 \(\hat{d} = \dot{x}_o/\|\dot{x}_o\|\)（物体在往哪动，就是 leader 想要的方向），然后**主动沿该方向出力**：

\[ f_{\text{follower}} = \kappa\,\hat{d} + f_{\text{int}},\qquad \kappa>0 . \tag{5.6} \]

效果：leader 用一个小力起个头，让物体微微朝目标方向动；每个 follower 检测到这个运动方向，沿同方向加一份力 \(\kappa\hat d\)；\(N-1\) 个 follower 的力全部对齐叠加，物体获得的总驱动力是 leader 的许多倍。leader 像乐队指挥——它自己不发声（出力小），但让所有乐手（follower）的声音对齐放大。

意图估计：follower 怎么"读"出 leader 想法¶

follower 估计意图的本质是**从物体运动反推期望运动方向**。最简形式：物体当前速度方向就是期望方向（策略 B 的 \(\hat d\)）。更精细的可以用观测器估计物体加速度、或对力信号滤波去噪。Wang-Schwager 用物体在 follower 接触点处的局部运动作为隐式信息，证明了即使 follower 只测自己接触点（不需全局物体状态），也能正确配合。

类比的边界：follower 估计 leader 意图，像跳交谊舞时女伴跟随男伴的引带（lead）。像的部分：跟随者不预知舞步，仅靠搭手处传来的力/运动方向实时判断并配合，主导者用很轻的引带就能带动。不像的部分：舞伴是智能体能预判和"补偿"，机器人 follower 只做即时反应、不预判；且舞蹈引带是单接触点，多机搬运是多接触点闭链，follower 的配合还必须满足内力约束（不能把舞伴"夹碎"）。

信息流对比：leader-follower vs 全集中¶

下表系统对比两种架构（R6E 穷举式分类）：

维度	leader-follower	全集中（所有机器人跟踪轨迹）
谁知道目标轨迹	仅 leader	所有机器人
机器人间通信	可零通信（靠物体）	需广播同步轨迹
follower 所需传感	力/运动传感（本地）	全局物体状态 + 抓取几何标定
内力协调	leader 定运动、各自维持内力	易因目标不同步产生内力
可扩展性	高（加 follower 即可）	低（同步负担随 N 增长）
力放大	✓（follower 对齐放大）	✗
单点故障	leader 失效则群龙无首	无单一关键点（但更脆于不同步）
失效模式	leader 故障 / follower 误估方向	通信延迟 → 内力爆炸

理论-工程桥接：Force-ANTS 的力放大在搬重物时的实际意义¶

正因为 follower 会对齐放大 leader 的力，工程上 Force-ANTS 这类系统才能用一群"弱"机器人搬动远超单机能力的重物——leader 甚至可以是一个人（用手轻推），\(N-1\) 台机器人 follower 检测到推动方向后各加一份力，把人的轻推放大成足以移动重物的合力。这就是"力放大 N-robot 运输系统"名字的由来，也是把抽象的式 (5.6) 落到"人引导一群机器人搬重物"场景的直接价值。

⚠️ §5.3 常见陷阱¶

陷阱 1（概念误区）：以为 follower 必须知道物体的绝对轨迹

💡 概念误区: 认为 follower 也要拿到 x_d(t) 才能配合
   新手想法: "不知道目标在哪, follower 怎么知道往哪推?"
   实际上: follower 不需要绝对目标, 只需物体的"当前运动方向"——
          因为物体在动这件事本身就编码了 leader 的意图。物体往哪动,
          leader 就想往哪去, follower 沿该方向出力即可(式 5.6)。
          意图通过物体运动隐式传递, 不通过轨迹广播。
   为什么重要: 误以为要广播轨迹, 你就退回了全集中方案, 丢掉了零通信
            和可扩展的全部好处, 还引入了同步导致的内力风险。
   延伸思考: follower 估计的是"方向"(单位向量), 不是"位置"——这正是
            它对绝对定位误差鲁棒的原因。

陷阱 2（思维陷阱）：把 leader 力大小等同于物体驱动力

🧠 思维陷阱: 认为 leader 出多大力, 物体就被多大力驱动
   新手想法: "leader 是主导, 它的力决定物体运动, follower 只是辅助"
   实际上: 在力放大架构里, 物体的总驱动力 ≈ leader 力 + Σfollower力,
          而 follower 力之和可远大于 leader 力。leader 的作用是
          "定方向、起头", 不是"提供主要动力"。把 leader 力当总驱动力,
          会严重低估系统能搬的重量, 也会误调 leader 增益。
   正确思维: leader 定"往哪走"(方向/意图), follower 提供"使多大劲"
          (放大的力)。指挥不发声, 但决定乐曲。
   自检方法: 测量稳态时 leader 接触力与各 follower 接触力之比, 若
          follower 力显著更大, 说明放大机制在工作。

陷阱 3（编程陷阱）：follower 用未滤波的物体速度估计方向导致抖动

⚠️ 编程陷阱: 直接用 x_o 的数值差分速度算方向 d̂, 不滤波
   错误做法: d̂ = (x_o[k]-x_o[k-1]) / dt, 归一化后直接用。
   现象: 物体近似静止或低速时, 数值差分速度被噪声主导, d̂ 方向
        剧烈乱跳, follower 力方向随之抖动, 物体震颤甚至失稳。
   根本原因: 速度方向 = 速度/速度模, 当速度模接近 0 时, 方向对
            噪声极度敏感(0/0 型病态), 微小噪声→方向翻转。
   正确做法: (1) 对速度低通滤波后再算方向; (2) 速度模低于阈值时
            冻结上一方向或令 follower 力为 0(死区); (3) 用观测器
            (如卡尔曼)估计平滑速度。自检: 让物体静止, 观察 d̂ 是否
            稳定不乱跳。

§5.3 练习¶

（分析题，⭐⭐） 一个 leader + 三个 follower 搬运。leader 用式 (5.4) 出力，follower 用式 (5.6) 力放大，各 \(\kappa=20\) N。假设稳态时物体匀速、所有 follower 力完美对齐运动方向。若 leader 稳态出力 10 N，问物体获得的总驱动力是多少？要把一个需要 100 N 驱动力的重物匀速搬动，至少需要几个 follower（leader 力固定 10 N、每 follower \(\kappa=20\) N）？
（实现题，⭐⭐⭐） 在 2D 仿真里实现 leader-follower 搬运：一个刚性杆，leader 在左端用阻抗律 (5.4) 跟踪一条直线轨迹，一个 follower 在右端用力放大律 (5.6)。follower 只能读取杆的速度（加滤波）。验证：(a) 物体跟上轨迹；(b) 关闭 follower 时 leader 单独搬运更慢/更吃力，开启后更轻快（量化 leader 出力的下降）。处理好陷阱 3 的速度方向抖动。
（设计扩展题，⭐⭐⭐） leader-follower 的致命弱点是 leader 单点故障。设计一个"leader 选举 + 切换"机制：当 follower 检测到物体长时间不动（leader 可能故障），自动选举一个新 leader（如约定编号最小的健康机器人）。讨论：切换瞬间如何避免内力突变？这与本 Part 第 2 章共识算法（选举/一致性）有什么联系？

§5.4 去中心化力控：物体即共享黑板 ⭐⭐⭐¶

这一节解决什么问题：leader-follower 仍有一个"特殊角色"（leader）。能不能更彻底——让所有机器人**完全同构、平等、互不通信**，仅靠各自感受物体来协调？这一节讲去中心化（decentralized）力控：每个 agent 运行**完全相同**的本地控制律，把被搬物体当成一块所有机器人都能读写的"共享黑板"，意图与状态通过物体的物理运动隐式传递。这是多机搬运可扩展性的终极形态，也是 decPLM"零通信"的理论根基。

动机：蚂蚁搬食物¶

一群蚂蚁搬一块远比单只蚂蚁大的食物。没有蚂蚁是"指挥官"，没有蚂蚁知道"全局计划"，它们之间也不交换复杂信息。但食物却能被搬向蚁巢。怎么做到的？

答案是：每只蚂蚁都遵循同一条简单规则——大致是"朝蚁巢方向拽，同时感受食物的运动、顺着它调整"。当多数蚂蚁朝同一方向使劲时，少数方向不一致的蚂蚁会感受到食物被拽向某个方向，于是顺势调整自己的用力方向。食物的运动成了所有蚂蚁之间唯一的"通信"——一只蚂蚁的努力，通过食物的运动，被其他蚂蚁感知到。

这种"所有个体同构、规则相同、靠被操作物体隐式协调"的模式，就是**去中心化力控**。它与 §5.3 leader-follower 的根本区别：leader-follower 有角色分化（一个 leader、若干 follower，控制律不同），去中心化**所有 agent 跑完全相同的控制律**，没有任何特殊节点。

放到机器人：\(N\) 台同构机器人各自抓住物体，每台运行同一个本地控制器，输入只有它自己能测的量（自己接触点的力、自己感受到的物体运动），输出自己接触点的力。没有 leader，没有机器人间通信，没有全局协调器。

如果坚持用显式通信会怎样¶

去中心化的对立面是"显式通信协调"——机器人之间用网络交换状态，由某种协议（或中央节点）算出协调的力。它在三处会受限。

第一，通信延迟与带宽是硬瓶颈。 力控制环通常跑 1 kHz。要在 1 ms 内完成"采集状态→广播→接收→协调计算→下发"，对无线网络是巨大压力。延迟一旦超过控制周期，协调就滞后于物理，轻则性能下降，重则因延迟注入能量而失稳（§5.7 会从无源性角度解释延迟为何危险）。物体作为"通信信道"则是**零延迟**的——力以声速在刚体中传播，远快于任何无线网络。

第二，可扩展性受限。 显式通信的协调计算常随机器人数量 \(N\) 增长（广播 \(O(N)\)、集中求解 \(O(N^3)\)）。去中心化的本地控制律对每个 agent 是 \(O(1)\) 的——加多少机器人，每个机器人的计算量不变。这是 decPLM 能从 2 台扩到 10 台 Go2 而不改控制律的根本原因。

第三，单点与通信故障。 中央协调器是单点故障；无线通信可能丢包、被干扰。去中心化没有中央节点、不依赖通信链路，单台机器人故障只是少一份力，系统降级而不崩溃。

本质洞察：去中心化力控把"协调"这个看似需要全局信息的问题，分解成了 \(N\) 个**只用本地信息**的相同子问题，而全局协调"涌现（emerge）"于所有 agent 通过物体的物理耦合。这与第 2 章共识算法的精神一致——共识让每个节点只和邻居通信就达成全局一致；去中心化搬运更进一步，连"和邻居通信"都省了，物体就是所有 agent 共同的邻居。协调不是被某个节点计算出来的，而是从物理耦合中长出来的。

历史：从行为群体到隐式通信¶

去中心化多机搬运的思想根植于**群体机器人（swarm robotics）** 和**基于行为的控制（behavior-based control）——1990 年代 Mataric 等人研究的"简单规则涌现复杂群体行为"。Force-ANTS（Carey-Werfel 2021）、Wang-Schwager 2018 把这套思想精确化为**隐式通信（implicit communication）框架：明确指出物体的运动是 agent 间唯一的信息载体，并给出收敛性分析。Culbertson-Schwager 2020 的去中心化自适应控制证明了即使**不知道物体质量与惯性**，每个 agent 用本地自适应律也能协同搬运（这继承了 §5.0 脉络表里 Verginis 2019 的"无需负载参数"思想）。最新的 decPLM（Pandit 2025）用强化学习在 IsaacLab 里训练出零通信的 pinch-lift-move 策略，把 2-10 台 Go2 的去中心化搬运做到了实物。

理论：去中心化控制律与隐式通信¶

同构本地控制律¶

每个 agent \(i\) 运行**完全相同**的控制律，只用本地量。一个有代表性的结构：

\[ f_i = \underbrace{f_i^{\text{nav}}}_{\text{导航分量}} + \underbrace{f_i^{\text{comply}}}_{\text{顺从分量}} + \underbrace{f_i^{\text{int}}}_{\text{内力分量}} . \tag{5.7} \]

导航分量 \(f_i^{\text{nav}}\)：每个 agent 自己知道（或感知到）大致目标方向（如都朝蚁巢/目标点），独立产生一份朝向目标的力。这是"共同目标"的去中心化体现——不需要协调，因为所有 agent 的目标方向大体一致。
顺从分量 \(f_i^{\text{comply}}\)：agent 感受到物体的实际运动（被其他 agent 影响的结果），用导纳律顺从它，避免与群体"对抗"。这是隐式协调的核心——它让少数方向不一致的 agent 自动向多数妥协。
内力分量 \(f_i^{\text{int}}\)：维持抓持/夹紧，由本地约定的内力意图产生（§5.2）。

隐式通信：物体如何传递信息¶

关键机制：agent \(i\) 出的力 → 改变物体运动 → agent \(j\) 在自己接触点测到这个运动变化 → agent \(j\) 据此调整自己的力。物体把"agent \(i\) 的动作"翻译成"物体运动"，再翻译成"agent \(j\) 的观测"。信息流是：

\[ f_i \;\xrightarrow{\;G\;}\; w_o \;\xrightarrow{\text{物体动力学}}\; v_o \;\xrightarrow{\;G_j^\top\;}\; \dot{x}_j \;\xrightarrow{\text{agent } j \text{ 测量}}\; f_j . \]

注意这条链里没有任何网络通信——全是物理量经过抓取矩阵的正向（\(G\)）和对偶（\(G_j^\top\)）映射。物体扮演了一个零延迟、无限带宽、永不丢包的广播信道，代价是这个信道只能传递"物体的运动"这一种信息（不能传递任意比特）。

类比的边界：去中心化搬运的物体，像一群人合力推一辆抛锚汽车时的"车"。像的部分：没人指挥，每人朝大致前方推，靠车的运动感知整体节奏并调整自己的力，多数人方向一致时少数人自然被带动。不像的部分：人能看到彼此、能喊话（有额外通信通道），机器人去中心化严格只靠车（物体）这一个通道；且人会预判，机器人只即时反应。把"人推车"的协调能力高估到机器人上，会让你低估去中心化对"物体刚度、感知质量"的苛刻要求。

收敛性的直觉¶

为什么这套各自为政的规则能收敛到协调搬运而不是一团乱？直觉是：顺从分量 \(f_i^{\text{comply}}\) 充当了**负反馈**——任何与群体不一致的力都会引起物体运动，该运动被 agent 自己测到后，导纳律驱使它顺从、减少对抗。这类似第 2 章共识的"误差驱动收敛"：共识里节点向邻居均值靠拢，这里 agent 向物体运动（群体合意的物理体现）靠拢。严格收敛性需要无源性论证（§5.7）——每个 agent 的导纳律无源、物体无源，反馈互联保持无源，从而稳定收敛。

三种架构总对比¶

把 §5.3-5.4 的三种"谁来决策"架构放一起（贯穿本章的核心分类）：

维度	全集中	leader-follower	去中心化
角色	中央协调器 + 同构执行	1 leader + N-1 follower	全同构
谁知道目标	中央	leader	所有（仅大致方向）
通信	重（状态↔中央）	轻或零（靠物体）	零（靠物体）
协调计算	\(O(N^3)\) 集中	leader \(O(1)\)	各 \(O(1)\)
可扩展性	差	中	优
鲁棒性（单点）	差（中央）	中（leader）	优（无关键点）
精度/最优性	高（全局信息）	中	较低（仅本地信息）
代表工作	经典操作空间控制	Force-ANTS, Wang-Schwager	decPLM, Culbertson-Schwager
适用	少机、高精度、有可靠通信	人引导、半自主	大规模、零通信、强鲁棒

本质洞察：这三种架构不是"谁更先进"，而是**信息可得性与系统规模的权衡谱**。全局信息可得且 \(N\) 小 → 集中式拿最优；信息受限或 \(N\) 大 → 去中心化拿可扩展与鲁棒，代价是放弃部分最优性。这与第 3 章 MAPF 的"耦合（最优但不可扩展）vs 解耦（可扩展但次优）"是同一条权衡轴在力控层的回响。

理论-工程桥接：decPLM 的零通信为何可行¶

正因为物体是零延迟的隐式信道，decPLM 才能让 2-10 台 Go2 在**完全不通信**下协同搬运——每台 Go2 跑同一个 RL 策略，输入只有本体感知（自身姿态、足端力/接触），通过物体运动隐式感知其他 Go2 的努力。"pinch-lift-move"三阶段（捏紧→抬起→移动）对应式 (5.7) 里内力分量（pinch）和导航分量（move）的时序组合。零通信不是魔法，而是把通信负担转移给了物理——这是 §5.0 场景三"零通信协作怎么落地"的答案。

⚠️ §5.4 常见陷阱¶

陷阱 1（概念误区）：以为"去中心化"等于"无协调/各干各的"

💡 概念误区: 把去中心化理解成"机器人互不影响、独立搬运"
   新手想法: "不通信、各跑各的控制律, 那不就是 N 个独立机器人?"
   实际上: 去中心化 ≠ 无耦合。机器人通过物体强耦合——这正是协调的
          来源。它们不通过"网络"协调, 但通过"物体"强烈地相互影响。
          顺从分量让每个 agent 对群体合力做负反馈, 协调从物理耦合中
          涌现。"独立"的只是控制律的计算, 不是物理上的相互作用。
   为什么重要: 误以为各干各的, 你会漏掉顺从分量(隐式协调的核心),
            写出真正独立的控制律, 结果机器人互相对抗、物体被撕扯。
   延伸思考: 去中心化系统的全局行为是"涌现"的, 不是任何单个 agent
            "计算"出来的——这是它和集中式最深的哲学差异。

陷阱 2（思维陷阱）：认为去中心化总是优于集中式

🧠 思维陷阱: 觉得"零通信、可扩展、鲁棒"全是优点, 去中心化碾压集中式
   新手想法: "既然去中心化不依赖通信又能扩展, 那就该一律用它"
   实际上: 去中心化用"放弃全局信息"换"可扩展与鲁棒", 代价是精度与
          最优性下降——仅靠本地信息, 无法做出全局最优的力分配。
          少机、高精度、有可靠通信的场景(如工厂里两台标定好的机械臂
          精密装配), 集中式用全局信息能做得更好。架构选择是权衡, 不是
          优劣排序。
   正确思维: 按"信息可得性 × 规模 × 精度需求"选架构: 信息全+小规模+
          高精度 → 集中; 信息受限或大规模或要强鲁棒 → 去中心化。
   自检方法: 问"我能否可靠拿到全局状态、N 是否大、精度要求多高?",
          三问决定架构。

陷阱 3（编程陷阱）：所有 agent 跑同一控制律却用了不一致的坐标系

⚠️ 编程陷阱: "同构控制律"实现时各 agent 用各自局部坐标系且未对齐
   错误做法: 每个 agent 在自己机体系下计算导航力/顺从力, 但没把它们
            统一到物体系或世界系, 误以为"代码相同就是同构"。
   现象: 各 agent 的"朝目标"方向在不同坐标系下指向不同的物理方向,
        合力相互抵消或乱指, 物体原地打转或乱走, 看起来像没协调。
   根本原因: "同构"指物理行为同构(都朝同一物理目标使劲), 不是"代码
            字节相同"。坐标系不对齐时, 相同代码产生不同物理方向。
   正确做法: 明确所有力的参考系。导航方向应在共同参考系(世界系/物体
            系)中定义, 再变换到各 agent 执行系。自检: 让所有 agent
            目标设为同一世界方向, 检查各接触力在世界系下是否真同向。

§5.4 练习¶

（分析题，⭐⭐） 解释为什么去中心化搬运中"顺从分量"\(f_i^{\text{comply}}\) 是协调收敛的关键。如果去掉顺从分量、只保留导航分量和内力分量，会发生什么？用"两台机器人导航方向有 30 度夹角"的具体例子说明（一台想往北、一台想往东北）。
（实现题，⭐⭐⭐） 在 2D 仿真实现 \(N=3\) 去中心化搬运：每台机器人跑式 (5.7) 的同一控制律（导航分量朝同一目标点 + 顺从分量对物体速度做导纳 + 固定内力）。三台机器人初始导航方向因定位噪声略有不同。验证：(a) 物体最终被搬向目标；(b) 关闭顺从分量后三台"对抗"、物体偏离或抖动。务必处理陷阱 3 的坐标系对齐。
（跨章综合题，⭐⭐⭐，综合 §5.4 + 第 2 章共识 + 第 3 章 MAPF） 论述"去中心化"思想在本 Part 三处的体现：第 2 章共识（节点靠邻居通信达成一致）、第 3 章去中心化 MAPF/PIBT（机器人靠局部规则避碰）、本章去中心化搬运（agent 靠物体隐式协调）。三者的"局部信息""协调机制""涌现的全局行为"分别是什么？它们共享的权衡（放弃什么换取什么）是什么？

§5.5 协调阻抗与物体阻抗 ⭐⭐⭐¶

这一节解决什么问题：前面解决了"力怎么分"（§5.2）和"谁决策"（§5.3-5.4），但还没说清"用什么力学行为搬"。单臂力控里我们学过阻抗控制（让末端表现得像弹簧-阻尼器）。多机搬运里，阻抗该加在哪——是各机器人末端各自一套阻抗，还是给物体整体一套阻抗？这一节讲 object impedance（物体阻抗）：把期望阻抗加在被搬物体的质心上，让**物体**像弹簧-阻尼系统那样响应外部接触，再把物体级期望力旋量分配回各机器人。这是 Caccavale、Wimböck-Ott 等经典框架的核心，也是把单臂阻抗推广到多机的正确方式。

动机：物体撞到障碍时，谁该"软"下来¶

两台机器人搬一块平板，平板前缘撞上了一面墙。我们希望系统表现出柔顺——不要硬顶（会损坏机器人、墙或物体），而要像弹簧一样"知难而退"。问题是：阻抗（柔顺）应该体现在哪个层面？

选项一：各末端独立阻抗。 每台机器人末端各自跑一套阻抗律（§F04 笛卡尔阻抗），各自对自己接触点的位置偏差产生回复力。问题：两套独立阻抗各管各的末端，它们对"物体整体该怎么柔顺响应碰撞"没有共识。平板撞墙时，靠墙那侧的机器人感到大力、另一侧没感觉，两台的阻抗响应不协调，物体的整体行为难以预测，还容易产生内力（两末端期望位置不一致）。

选项二：物体阻抗。 把阻抗加在**物体质心**上——让物体整体表现得像一个有期望惯量 \(M_d\)、阻尼 \(D_d\)、刚度 \(K_d\) 的刚体：物体受外力（撞墙）时，按这个期望阻抗"退让"。控制器先算出"为实现物体的期望阻抗行为，物体需要承受多大合力旋量 \(w_{\text{ext}}\)"，再用 §5.2 的分解把 \(w_{\text{ext}}\) 分配给各机器人。这样**物体作为一个整体柔顺**，碰撞响应协调一致，内力被单独管理。

直觉上选项二明显更合理——我们关心的是物体的行为，就该把期望行为定义在物体上。这就是 object impedance 的核心思想。

如果用各末端独立阻抗会怎样¶

坚持选项一（各末端独立阻抗），会暴露三个问题。

第一，碰撞响应不协调。 物体受到的外部扰动（碰撞、人推）作用在物体某处，但只有附近的机器人末端"感觉到"并柔顺退让，远处的机器人末端不退让。结果物体的响应取决于碰撞位置，行为不一致、不可预测。物体阻抗则让整个物体按统一的 \(M_d,D_d,K_d\) 响应，与碰撞位置无关。

第二，内力与运动阻抗纠缠。 各末端阻抗的回复力里，既含"让物体运动"的分量，也含"末端间相互挤压"的分量，二者揉在一起。调阻抗参数时，你可能想让物体运动更柔顺，却意外改变了内力。物体阻抗 + §5.2 分解则把运动阻抗（物体级 \(K_d,D_d\)）和内力（\(\ker G\) 里独立设定）彻底解耦。

第三，期望位置不一致即内力。 各末端独立阻抗需要给每个末端一个期望位置 \(x_{d,i}\)。这些期望位置必须严格满足刚体约束（都来自同一个物体期望位姿），否则不一致的期望位置在刚性物体上直接产生内力。物体阻抗只需一个物体期望位姿 \(x_{o,d}\)，各末端位置由刚体几何自动导出，天然一致。

本质洞察：物体阻抗与各末端独立阻抗的区别，本质是**"在哪个空间定义期望行为"**。各末端阻抗在 \(3N\) 维接触空间定义期望（过定义，必然引入内力纠缠）；物体阻抗在 6 维物体空间定义期望（恰好定义，运动与内力干净分离）。永远在"任务真正所在的空间"（这里是 6 维物体空间）定义期望行为，再映射到执行器空间——这是力控的一条普适原则，与 §F02"操作空间控制在任务空间定义行为"一脉相承。

历史：从对称构型到物体级阻抗¶

Caccavale、Chiacchio 等在 1990 年代为双臂协调提出**对称构型公式（symmetric formulation）**：用"绝对运动"（物体质心位姿）和"相对运动"（两末端相对位姿，编码内力）两组变量描述双臂，把阻抗加在绝对运动上、力调节加在相对运动上。这正是物体阻抗的早期形态——绝对运动 = 物体运动、相对运动 = 内力维度。Wimböck、Ott、Hirzinger 2008 在 DLR 系统比较了多种物体级抓取控制器，提出统一的物体级阻抗框架，把多指手和多臂统一处理，并明确了内力共线（internal force along grasp lines）的设计。这条线确立了"物体阻抗 + 内力调节"作为多机/多指柔顺操作的标准范式。

理论：物体阻抗控制律¶

期望物体阻抗行为¶

我们希望被搬物体表现得像一个目标阻抗系统。设物体期望位姿 \(x_{o,d}\)、实际位姿 \(x_o\)，跟踪误差 \(\tilde{x}_o = x_o - x_{o,d}\)。期望阻抗动力学（在物体质心、6 维）：

\[ M_d\ddot{\tilde{x}}_o + D_d\dot{\tilde{x}}_o + K_d\tilde{x}_o = w_{\text{ext,env}}, \tag{5.8} \]

其中 \(w_{\text{ext,env}}\) 是环境施加给物体的外部力旋量（碰撞、人推等）。这条式子说：物体偏离期望轨迹时，受到弹簧 \(K_d\) 的回复、阻尼 \(D_d\) 的耗散；遇到环境力时，按期望惯量 \(M_d\) 的"软硬"退让。\(M_d,D_d,K_d\) 是 \(6\times 6\) 矩阵，整形物体整体的柔顺行为。这与 §F01 Hogan 阻抗律形式完全相同，只是作用对象从"末端"变成"物体质心"。

求所需物体合力旋量¶

物体的真实动力学（刚体）为：

\[ M_o\ddot{x}_o + C_o(\dot{x}_o) = w_o^{\text{robots}} + w_{\text{ext,env}} + w_{\text{grav}}, \tag{5.9} \]

其中 \(M_o\) 是物体的 \(6\times 6\) 空间惯量矩阵，\(C_o\) 含科氏/离心项，\(w_o^{\text{robots}}=Gf\) 是所有机器人施加的合力旋量，\(w_{\text{grav}}\) 是重力旋量。我们要算出机器人该施加多大 \(w_o^{\text{robots}}\) 才能让物体满足期望阻抗 (5.8)。

把 (5.8) 解出期望加速度 \(\ddot{x}_o = \ddot{x}_{o,d} - M_d^{-1}(D_d\dot{\tilde{x}}_o + K_d\tilde{x}_o - w_{\text{ext,env}})\)，代入 (5.9) 求 \(w_o^{\text{robots}}\)：

\[ \boxed{\; w_{\text{ext}} := w_o^{\text{robots}} = M_o\ddot{x}_o^{\text{des}} + C_o(\dot{x}_o) - w_{\text{grav}} - w_{\text{ext,env}}, \;} \tag{5.10} \]

其中 \(\ddot{x}_o^{\text{des}}\) 是上面由阻抗律解出的期望加速度。这就是物体级控制器要让机器人共同施加的外力旋量。注意它需要物体动力学参数 \(M_o,C_o\)——这是**基于模型**的物体阻抗。若物体参数未知，可改用 §5.0 脉络表里 Verginis/Culbertson 的**自适应**方案（在线估计 \(M_o\)）或去中心化方案，避开对精确模型的依赖。

完整管线：物体阻抗 → 接触力¶

把 (5.10) 算出的 \(w_{\text{ext}}\) 经 §5.2 分解，加上独立设定的内力，得到各机器人接触力：

\[ f = G^+ w_{\text{ext}} + E\lambda_{\text{int}} . \tag{5.11} \]

然后每台机器人 \(i\) 取出自己那 3 维 \(f_i\)，通过 §F02 雅可比转置 \(\tau_i = J_i^\top f_i\) 转成自己的关节力矩执行。完整数据流：

物体期望轨迹 x_{o,d}
   │ (阻抗律 5.8)
   ▼
期望物体加速度 ẍ_o^des
   │ (物体动力学 5.10)
   ▼
所需物体合力旋量 w_ext  ──┐
                          │ (内力意图 λ_int)
   ┌──────────────────────┘
   ▼ (分解 5.11: G^+ w_ext + E λ_int)
各接触力 f_i
   │ (雅可比转置 τ_i = J_i^T f_i, 见 §F02)
   ▼
各机器人关节力矩 τ_i

类比的边界：物体阻抗让被搬物体像"装在虚拟弹簧-阻尼悬架上的刚体"。像的部分：物体对外部扰动的响应由 \(M_d,D_d,K_d\) 整形，像悬架决定车身对路面冲击的响应——刚度大则硬、阻尼大则不晃。不像的部分：真实悬架是被动元件（参数固定、本征无源），物体阻抗是主动控制（参数可在线变，变阻抗会有无源性风险，见 §5.7）；且悬架只在一个方向，物体阻抗是 6 维全姿态。把"悬架天然稳定"的直觉照搬到主动变阻抗，会忽略 §5.7 要解决的能量注入危险。

内力阻抗：连内力也可以是柔顺的¶

更精细的框架（Wimböck-Ott）不仅给物体运动加阻抗，也给**内力加阻抗**——内力不是设成硬值，而是让内力误差也表现出弹簧-阻尼行为：\(\lambda_{\text{int}}\) 由内力跟踪误差经一个内力阻抗律产生。好处：抓持也变柔顺，物体被外力顶时夹紧力能平滑调整而非硬抗。这把"运动柔顺"和"抓持柔顺"统一在阻抗框架下，二者分别作用在 \(\mathrm{range}(G^\top)\)（运动）和 \(\ker G\)（内力）正交子空间，互不干扰。

阶段小结：到这里我们完成了物体阻抗的完整推导——期望阻抗 (5.8)、所需合旋量 (5.10)、分配管线 (5.11)，以及运动阻抗与内力阻抗在正交子空间的解耦。接下来给出实现要点，然后进入负载分配优化（§5.6）。

代码实现：物体阻抗控制器骨架¶

// object_impedance.hpp
#include <Eigen/Dense>
#include "grasp_matrix.hpp"
#include "internal_force.hpp"

struct ObjectImpedanceParams {
  Eigen::Matrix<double,6,6> Md, Dd, Kd;   // 物体期望阻抗
  Eigen::Matrix<double,6,6> Mo;           // 物体空间惯量(模型已知)
};

// 一拍物体阻抗控制: 输出各机器人接触力 f (3N)
Eigen::VectorXd objectImpedanceStep(
    const ObjectImpedanceParams& p,
    const Eigen::MatrixXd& G,                 // 当前抓取矩阵 6×3N
    const Eigen::Matrix<double,6,1>& x_err,   // 物体位姿误差 x_o - x_{o,d}
    const Eigen::Matrix<double,6,1>& xd_err,  // 速度误差
    const Eigen::Matrix<double,6,1>& xdd_des, // 期望前馈加速度 ẍ_{o,d}
    const Eigen::Matrix<double,6,1>& w_env,   // 环境力旋量(测/估)
    const Eigen::Matrix<double,6,1>& w_grav,  // 重力旋量
    const Eigen::VectorXd& lambda_int)        // 内力意图 (3N-6)
{
  // 1) 阻抗律解期望物体加速度: ẍ = ẍ_d - Md^{-1}(Dd ẋ_err + Kd x_err - w_env)
  Eigen::Matrix<double,6,1> xdd =
      xdd_des - p.Md.ldlt().solve(p.Dd*xd_err + p.Kd*x_err - w_env);

  // 2) 由物体动力学求所需合力旋量 (略去 Coriolis, 准静态/低速可忽略)
  Eigen::Matrix<double,6,1> w_ext = p.Mo*xdd - w_grav - w_env;

  // 3) 分解并加内力 (复用 §5.2)
  ForceDecomposition d = computeDecomposition(G);
  return d.G_pinv * w_ext + d.E * lambda_int;   // = 式 (5.11)
}

// ❌ 常见错误: 把环境力 w_env 的符号搞反, 或漏掉重力补偿
//   - w_env 符号错: 物体撞墙时不退让反而"主动顶上去", 危险!
//   - 漏 w_grav: 机器人没补偿重力, 物体被搬起后整体下坠, 控制器
//     拼命用运动力补、内力被挤占, 抓持可能滑脱。
//   正确: w_env 取"环境施加给物体"的方向; 重力旋量必须显式补偿。

理论-工程桥接：为什么 DLR 的多指手和双臂用同一套物体阻抗¶

正因为物体阻抗把期望行为定义在被操作物体上、与接触点数量解耦，DLR 才能用**同一套物体阻抗框架**统一处理多指手抓取（\(N\) 个指尖）和双臂搬运（2 个末端）——区别只在抓取矩阵 \(G\) 的尺寸和内力维度，控制律结构（式 5.8-5.11）完全一致。这就是 §5.0 目标 7 强调的"同一套数学在不同 agent 数下展开"：你在本章学的物体阻抗，原样适用于 §D03 双臂、多指灵巧手、多足搬运。

⚠️ §5.5 常见陷阱¶

陷阱 1（概念误区）：把物体阻抗等同于"每个末端各跑一套阻抗"

💡 概念误区: 以为多机阻抗就是 N 个单臂阻抗控制器的并列
   新手想法: "每台机器人末端跑笛卡尔阻抗, 合起来就是多机阻抗"
   实际上: N 个独立末端阻抗 ≠ 物体阻抗。前者在 3N 维接触空间过定义
          期望, 运动与内力纠缠、碰撞响应不协调; 后者在 6 维物体空间
          定义统一阻抗, 再分配, 运动与内力解耦。两者行为差异巨大。
   为什么重要: 用 N 个独立末端阻抗, 你既无法保证物体整体柔顺一致,
            又会被期望位置不一致引入的内力反复折磨。
   延伸思考: 何时各末端阻抗也可接受? 当物体不是刚性闭链(如各机器人
            搬独立物体), 或内力维度为 0(双机点接触)时。

陷阱 2（编程陷阱）：物体阻抗漏掉重力旋量补偿

⚠️ 编程陷阱: 计算 w_ext 时忘记减去/补偿物体重力 w_grav
   错误做法: w_ext = Mo*xdd - w_env, 漏掉 w_grav。
   现象: 物体被抬起后持续下坠, 控制器用越来越大的运动力去补,
        运动力挤占了接触力预算, 内力(夹紧)被压缩, 物体可能滑脱;
        或表现为物体稳态位置低于期望(稳态误差)。
   根本原因: 物体阻抗的运动方程必须包含所有作用在物体上的力,
            重力是恒定外力, 不补偿就成了持续扰动。
   正确做法: 显式建模重力旋量 w_grav = [m_o g; r_cog × m_o g] 并在
            w_ext 中补偿。自检: 物体悬停(期望静止)时, 各接触力之和
            的竖直分量应≈物体重量。

陷阱 3（思维陷阱）：以为加大物体刚度 \(K_d\) 总能提升跟踪精度

🧠 思维陷阱: 把物体阻抗的 K_d 当成"越大跟得越准"的旋钮
   新手想法: "跟踪误差大? 调大 K_d, 弹簧硬一点就跟得紧了"
   实际上: K_d 过大有三害: (1) 牺牲柔顺——撞到障碍/人时硬顶, 失去
          力控的安全意义; (2) 放大内力——刚性物体上微小误差经大 K_d
          变成大接触力, 挤压加剧; (3) 逼近稳定边界——大刚度需大阻尼
          配合, 否则振荡, 且对建模误差敏感。物体阻抗的价值恰在"柔",
          一味加 K_d 等于退回硬位置控制。
   正确思维: K_d 按任务选: 精密对接需较大但仍有限的刚度+足够阻尼;
          人机协作/避撞需小刚度。跟踪误差大应先查前馈(ẍ_d)、重力
          补偿、模型精度, 而非无脑加 K_d。
   自检方法: 提高 K_d 后检查接触力峰值和内力是否超限、阶跃响应是否
          振荡; 若是, 说明已过度。

§5.5 练习¶

（推导题，⭐⭐） 从期望物体阻抗 (5.8) 出发，完整推导所需机器人合力旋量 (5.10) 的每一步，明确每一项（\(M_o\ddot{x}^{\text{des}}\)、\(C_o\)、\(-w_{\text{grav}}\)、\(-w_{\text{ext,env}}\)）的物理意义。然后讨论：当 \(M_d = M_o\)（期望惯量取真实惯量）时式子如何简化？这种取法有什么物理含义（物体"感觉不到"惯量被改变）？
（实现题，⭐⭐⭐） 扩展本节 objectImpedanceStep，加入"内力阻抗"——内力意图 \(\lambda_{\text{int}}\) 不再是常数，而由内力跟踪误差经一个一阶阻抗律产生（输入期望内力、测得内力，输出 \(\lambda_{\text{int}}\)）。在 MuJoCo 双臂搬运里测试：给物体施加一个外部冲击，对比"硬内力"和"内力阻抗"两种方式下夹紧力的响应平滑度。
（设计扩展题，⭐⭐⭐） 物体阻抗 (5.10) 依赖已知物体惯量 \(M_o\)。设计一个回退方案：当 \(M_o\) 未知时，如何用准静态假设（忽略 \(M_o\ddot{x}\) 项，只补偿重力 + 阻抗回复）退化出一个不需要惯量的简化物体阻抗控制器？讨论这个简化在什么速度范围内可接受、何时会失效（提示：联系 §5.0 脉络表 Verginis 2019"仅适用准静态"的局限）。

§5.6 负载分配优化：摩擦锥下的 QP ⭐⭐⭐¶

这一节解决什么问题：§5.2 用伪逆把外力旋量分给各机器人，但伪逆有两个盲点：(1) 它不管接触力是否物理可行——可能要求某个接触点拉物体（点接触/摩擦抓持拉不了）或超出机器人力极限；(2) 它最小化的是欧氏范数，未必是你真正想优化的（如让重物分给更强的机器人）。这一节把负载分配升级为**带约束的二次规划（QP）**：在满足合力旋量、摩擦锥（不打滑）、力上限（不超限）的前提下，优化一个你定义的目标。这是从"能搬"到"搬得稳、搬得巧"的关键一步。

动机：伪逆给出的力，机器人做不到¶

三台机器人用脚（点接触带摩擦）托举一块平板。用伪逆 \(f=G^+w_{\text{ext}}\) 算出分配后，你兴冲冲下发，却发现：

某个接触点的伪逆解要求**向下拉**物体（法向力为负）。但脚和平板是单边接触——脚只能往上推，不能往下拉！这个力物理上做不到，机器人脚直接离开物体，抓持崩溃。
另一个接触点的切向力远超摩擦锥——要求的水平力比 \(\mu\times\)法向力还大，物理上必然打滑。物体从这个点滑脱。
还有一个接触点要求 500 N，超过了那台机器人的力极限 300 N，电机饱和，实际出力不足，物体倾斜。

伪逆对这些**物理约束一无所知**——它只解了一个无约束的最小范数问题。真实搬运里，接触力必须满足：合成正确的物体旋量（等式约束）、法向力非负（单边接触）、切向力在摩擦锥内（不打滑）、不超力极限（不饱和）。这些约束让"力分配"从一个线性代数问题（伪逆）变成一个**带约束优化问题（QP）**。

如果只用伪逆不加约束会怎样¶

无约束伪逆在三类情形必然出事。

第一，单边接触被违反。 点接触、脚踩、绳索牵引都是单边的（只能推/拉其中一个方向）。伪逆不知道这点，可能给出反方向的力，接触直接断开。

第二，摩擦锥被违反，物体打滑。 摩擦能提供的切向力有上限 \(\|f_t\|\le\mu f_n\)。伪逆的最小范数解经常把切向力用得很满（因为它不知道有这个限制），导致打滑。物体一滑，抓取几何 \(r_i\) 变了，\(G\) 变了，整个控制失准，常常雪崩式失败。

第三，无法表达"分配偏好"。 你想让强壮的机器人多担、虚弱的少担，或让某个姿态好的机器人主导——伪逆的"最小范数"是写死的偏好（所有机器人力的欧氏范数最小），不能表达你的工程意图。QP 的目标函数可以自由编码这些偏好（加权）。

本质洞察：从伪逆到 QP，是从"求一个解"到"在可行集里求最优解"的跃迁。伪逆假装力空间是无约束的 \(\mathbb{R}^{3N}\)，QP 正视力空间被摩擦锥、单边接触、力极限切割成的**可行多面体/锥**。多机搬运的真正难点不在"算出一组力"，而在"算出一组**物理可实现**的力"——后者必须是约束优化。这与第 3 章 MAPF 从"各自最短路"到"带冲突约束的搜索"是同一种认识升级：现实约束让简单解法失效，必须显式建模约束。

历史：从力分配到接触力优化¶

把接触力分配写成优化问题的传统，可追溯到 Orin-Oh 1986 的闭链力分配（已含约束意识）和抓取力优化文献（Buss、Han、Cheng 等 1990s-2000s 研究在摩擦锥约束下最小化抓取力）。在**腿足机器人**领域，这套方法以"接触力优化 / 力分配 QP"之名成为站立平衡与运动控制的标配——把躯干所需净力旋量分配到各支撑足的接触力，约束是摩擦锥（线性化为摩擦金字塔）和单边法向力。这与多机搬运的负载分配是**同一个数学问题**（都是"把物体所需旋量分给多个受摩擦约束的接触点"），只是"接触点"从"足"变成"机器人末端"。现代实现普遍用 QP 求解器（qpOASES、OSQP、quadprog）在每个控制周期实时求解。

理论：负载分配 QP 的构造¶

决策变量与目标¶

决策变量是所有接触力 \(f\in\mathbb{R}^{3N}\)。一个通用目标函数：

\[ \min_{f}\; \tfrac{1}{2} f^\top W f + g^\top f, \tag{5.12} \]

\(W\succ 0\) 是权重矩阵，\(g\) 是线性项。常见取法：

\(W=I\)：最小化总接触力范数（≈伪逆，但带约束）。
\(W=\mathrm{blkdiag}(w_1 I_3,\dots,w_N I_3)\)，\(w_i\) 大则惩罚 agent \(i\) 出力 → 让弱机器人少担、强机器人多担。
加项惩罚切向力（鼓励接近法向、远离摩擦锥边界，提升抗滑裕度）。

等式约束：合成正确旋量¶

接触力必须合成期望物体外力旋量（来自 §5.5 物体阻抗）：

\[ G f = w_{\text{ext}} . \tag{5.13} \]

这把 §5.2 的"必须满足 \(Gf=w\)"作为硬等式约束放进 QP。QP 会在所有满足 (5.13) 的力（即 §5.2 的解空间 \(G^+w + \ker G\)）里挑最优的——QP 自动地在内力子空间里优化，因为内力正是 (5.13) 的解空间自由度。

不等式约束：摩擦锥、单边、力限¶

单边接触（法向力非负）：设接触点 \(i\) 的外法向（指向物体内/接触面法向）为 \(n_i\)，则法向力 \(f_{i,n}=n_i^\top f_i \ge 0\)（只能推不能拉；绳索则相反，只能拉不能推）：

\[ n_i^\top f_i \ge 0 . \tag{5.14} \]

摩擦锥（不打滑）：切向力受库仑摩擦限制 \(\|f_{i,t}\|\le \mu_i f_{i,n}\)。这是二阶锥约束，QP（二次规划）不能直接处理非线性锥，故**线性化为摩擦金字塔（friction pyramid）**——用 \(k\) 个切向方向把圆锥近似成多面锥：

\[ \pm t_{i,j}^\top f_i \le \mu_i\, n_i^\top f_i,\quad j=1,\dots,k/2, \tag{5.15} \]

\(t_{i,j}\) 是切平面内的采样方向。\(k\) 越大近似越精（\(k=4\) 是常用的方金字塔，\(k=8\) 更精）。代价是约束数随 \(k\) 增长。

力上限（不饱和）：\(\|f_i\|\le f_i^{\max}\)，线性化或用框约束 \(|f_{i,x}|,|f_{i,y}|,|f_{i,z}|\le f^{\max}/\sqrt{3}\) 等。

把 (5.14)(5.15) 及力限堆成 \(Af\le b\)，QP 标准形：

\[ \boxed{\; \min_{f}\ \tfrac12 f^\top W f + g^\top f \quad\text{s.t.}\quad Gf = w_{\text{ext}},\;\; Af\le b . \;} \tag{5.16} \]

三种分配策略对比（穷举式分类）¶

策略	方法	约束处理	计算量	适用
等分	\(f_i = w_{\text{ext}}/N\)（仅力，忽略力矩/几何）	无	极低	对称布局、粗略估计
加权伪逆	\(G_W^+ = W^{-1}G^\top(GW^{-1}G^\top)^{-1}\)	无（不保证可行）	低（闭式）	无单边/摩擦约束、需偏好加权
QP 优化	解 (5.16)	摩擦锥+单边+力限	中（迭代求解器）	真实接触、需保证可行性

等分最简但常违反力矩平衡（非对称布局时物体会转）；加权伪逆能编码偏好且有闭式解，但不保证摩擦锥/单边可行；QP 最完备但需实时求解器。工程上：原型用加权伪逆快速验证，上真机用 QP 保证物理可行。

类比的边界：负载分配 QP 与"几个人合抬一张桌子，怎么站位、怎么使劲最省力又不让桌子滑"类似。像的部分：都要满足"桌子被抬起且不转"（等式约束），每人的手不能打滑（摩擦约束）、不能用超过自己力气的劲（力限），并在可行方式里挑最舒服的（目标）。不像的部分：人能凭经验和实时触觉自动满足这些约束，QP 必须把每条约束显式写成不等式且线性化摩擦锥；人的"省力"是模糊感觉，QP 的目标必须是精确的二次型。把人的直觉协调高估到机器人上，会让你忽略约束建模的繁琐与摩擦锥线性化的精度损失。

阶段小结：到这里我们把负载分配从伪逆升级为带摩擦锥/单边/力限约束的 QP (5.16)，并对比了等分/加权伪逆/QP 三策略。接下来用 OSQP 风格代码实现一个可实时的力分配 QP。

代码实现：负载分配 QP¶

// load_distribution_qp.hpp —— 用 OSQP/qpOASES 风格接口求解 (5.16)
#include <Eigen/Dense>
// 假设有一个 solveQP(H, g, A_eq, b_eq, A_ineq, b_ineq) -> VectorXd 接口

struct ContactConstraint {
  Eigen::Vector3d normal;   // 接触法向 n_i (指向物体, 法向力沿此为正)
  double mu;                // 摩擦系数
  double f_max;             // 该接触力上限
};

// 构造并求解负载分配 QP
Eigen::VectorXd solveLoadDistribution(
    const Eigen::MatrixXd& G,                       // 6×3N
    const Eigen::Matrix<double,6,1>& w_ext,         // 期望物体旋量
    const std::vector<ContactConstraint>& contacts, // N 个接触约束
    const Eigen::VectorXd& weights)                 // 各接触力惩罚权 (N)
{
  const int N = static_cast<int>(contacts.size());
  const int n = 3 * N;

  // --- 目标 1/2 fᵀ W f : W = blkdiag(w_i I3), 弱机器人权大→少出力 ---
  Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n, n);
  for (int i = 0; i < N; ++i)
    H.block<3,3>(3*i,3*i) = weights(i) * Eigen::Matrix3d::Identity();
  H += 1e-6 * Eigen::MatrixXd::Identity(n, n);  // 正则保证严格正定
  Eigen::VectorXd g = Eigen::VectorXd::Zero(n);

  // --- 等式约束 G f = w_ext ---
  Eigen::MatrixXd A_eq = G;
  Eigen::VectorXd b_eq = w_ext;

  // --- 不等式约束: 单边(法向≥0) + 摩擦金字塔(4边) + 力限 ---
  std::vector<Eigen::RowVectorXd> rows; std::vector<double> rhs;
  for (int i = 0; i < N; ++i) {
    const auto& c = contacts[i];
    // 构造该接触点的切向基 t1, t2 (与 normal 正交)
    Eigen::Vector3d n_i = c.normal.normalized();
    Eigen::Vector3d t1 = n_i.unitOrthogonal();
    Eigen::Vector3d t2 = n_i.cross(t1);
    auto block = [&](const Eigen::Vector3d& v){
      Eigen::RowVectorXd r = Eigen::RowVectorXd::Zero(n);
      r.segment<3>(3*i) = v.transpose(); return r; };
    // 单边: -nᵀf ≤ 0  (即 nᵀf ≥ 0)
    rows.push_back(-block(n_i)); rhs.push_back(0.0);
    // 摩擦金字塔: ±t1ᵀf ≤ μ nᵀf, ±t2ᵀf ≤ μ nᵀf  → 移项成 ≤0
    for (const auto& t : {t1, t2})
      for (double s : {+1.0, -1.0}) {
        rows.push_back(block(s*t) - c.mu*block(n_i)); rhs.push_back(0.0);
      }
    // 力限(保守框约束): 各分量 |f| ≤ f_max
    for (int a=0;a<3;++a){
      Eigen::Vector3d e=Eigen::Vector3d::Zero(); e(a)=1;
      rows.push_back( block(e)); rhs.push_back(c.f_max);
      rows.push_back(-block(e)); rhs.push_back(c.f_max);
    }
  }
  Eigen::MatrixXd A_in(rows.size(), n); Eigen::VectorXd b_in(rows.size());
  for (size_t k=0;k<rows.size();++k){ A_in.row(k)=rows[k]; b_in(k)=rhs[k]; }

  return solveQP(H, g, A_eq, b_eq, A_in, b_in);  // 调用 QP 求解器
}

// ❌ 错误 1: 摩擦约束写成二阶锥 ||f_t|| ≤ μ f_n 直接喂给 QP 求解器
//   QP(二次规划)只接受线性不等式, 二阶锥需 SOCP 求解器。必须线性化
//   为金字塔(本例 4 边)。误用会导致求解器报错或忽略该约束。
// ❌ 错误 2: Hessian H 不正则化(纯 blkdiag 可能半正定)→ QP 退化、解不唯一
//   加 1e-6 I 保证严格正定。
// ❌ 错误 3: 忘记单边约束, 只加摩擦 → 求解器可能给出"拉"接触的解,
//   点接触/脚/绳索物理上做不到。单边约束 nᵀf≥0 必不可少。

理论-工程桥接：腿足 WBC 的力分配 QP 与多机搬运是同一个 QP¶

正因为"把物体所需旋量分给多个受摩擦约束的接触点"是同一个数学，腿足机器人 WBC（如 §F08 提到的 WBIC/OCS2）里每周期求解的足端力分配 QP，与多机搬运的负载分配 QP (5.16) 结构完全一致——只是"接触点"是足、"物体"是机器人躯干。掌握了本节的 QP，你既能做多机搬运的力分配，也能读懂腿足控制的接触力优化；qpOASES/OSQP 这些求解器在两个领域通用。这是 §5.0 强调的"同一套数学在不同场景展开"的又一例证。

⚠️ §5.6 常见陷阱¶

陷阱 1（编程陷阱）：把二阶摩擦锥直接喂给 QP 求解器

⚠️ 编程陷阱: 用 ||f_t|| ≤ μ f_n (二阶锥)作为 QP 约束
   错误做法: 直接把欧氏范数形式的摩擦锥写进二次规划。
   现象: QP 求解器(只接受线性约束)报错、或静默忽略该约束给出
        打滑的解; 用了 SOCP 求解器才能处理但算力大增。
   根本原因: 二次规划(QP)的约束必须是线性的; 摩擦锥 ||f_t||≤μf_n
            是二阶锥, 属于 SOCP 范畴。
   正确做法: 线性化为摩擦金字塔(式 5.15), 用 k 个切向方向近似圆锥。
            k=4(方金字塔)最常用, k=8 更精。自检: 增大 k 看解是否
            收敛、打滑是否减少。需要精确锥时改用 SOCP 求解器。

陷阱 2（概念误区）：以为加权伪逆能满足摩擦/单边约束

💡 概念误区: 认为给伪逆加权(G_W^+)就能处理物理约束
   新手想法: "加权伪逆能编码偏好, 调好权重就能避免打滑/拉力了"
   实际上: 加权伪逆只改变在解空间里选哪个最小(加权)范数解, 它仍是
          无约束闭式解, 不知道摩擦锥和单边约束的存在——给出的解
          照样可能要求拉接触或超摩擦锥。权重影响"分配偏好", 不构成
          "可行性保证"。
   为什么重要: 误信加权伪逆可行, 你会跳过 QP 直接上加权伪逆, 真机
            一接触就打滑/脱手, 还以为是权重没调好, 越调越乱。
   延伸思考: 加权伪逆 = 无约束 QP 的闭式解。一旦有不等式约束(摩擦/
            单边/力限), 就没有闭式解, 必须迭代求解 QP。

陷阱 3（思维陷阱）：QP 无解时归咎于求解器而非物理不可行

🧠 思维陷阱: QP 报"infeasible"就怀疑求解器有 bug 或参数没调好
   新手想法: "OSQP 说无解, 换个求解器/调调容差应该就好了"
   实际上: QP 无解往往是物理事实——在当前接触几何、摩擦系数、力限下,
          根本不存在能合成所需旋量又不打滑/不超限的接触力。例如要
          搬的物体太重、摩擦太小、接触点布局太差。换求解器不会变出
          物理上不存在的解。
   正确思维: QP infeasible 是有用的诊断信号: 说明任务超出了当前抓取
          配置的能力。应对: 增大摩擦(换材料/增法向预紧)、改善接触
          布局(撑开 r_i)、降低运动需求(减小加速度→减小 w_ext)、或
          加机器人。而非死磕求解器。
   自检方法: 放松约束(临时调大 f_max、μ)看是否变可行, 定位是哪类
          约束被违反, 据此调整物理配置。

§5.6 练习¶

（分析题，⭐⭐） 三台机器人正三角形托举一块 200 N 的平板（竖直向上），接触均为点接触带摩擦、\(\mu=0.5\)、法向竖直向上。若用等分策略，每点出力多少？这个等分解满足摩擦锥吗（切向力为 0、法向 66.7 N）？现在平板要水平加速（需额外水平合力 60 N），等分策略如何处理水平力？为什么此时需要 QP？
（实现题，⭐⭐⭐） 用 OSQP 的 Python 绑定（osqp）实现式 (5.16) 的负载分配 QP，针对练习 1 的三机器人托举场景。加入摩擦金字塔（\(k=4\)）、单边、力限约束。测试：(a) 纯竖直托举时各点力；(b) 逐渐增大水平加速度，观察何时 QP 变 infeasible（摩擦锥耗尽）；(c) 把 \(\mu\) 从 0.5 调到 0.8，infeasible 阈值如何变化。
（设计扩展题，⭐⭐⭐） 负载分配 QP 每个控制周期都要解一次（1 kHz）。讨论实时性优化：(a) 为什么用"热启动（warm start，用上周期解初始化）"能加速 OSQP？(b) 如果某周期 QP 求解超时，应如何安全降级（提示：回退到上周期解 / 加权伪逆 + 限幅）？(c) 摩擦金字塔边数 \(k\) 如何在精度与求解速度间权衡？

§5.7 无源性与稳定性 ⭐⭐⭐⭐¶

这一节解决什么问题：前面设计了各种控制律，但它们组合在一起——\(N\) 个机器人 + 物体 + 环境形成的大闭环——稳不稳？传统 Lyapunov 分析对这种高维、可变结构、含通信延迟的系统极难逐一处理。本节引入一把"万能尺"：无源性（passivity）。它把稳定性问题转化为能量问题——只要每个子系统都不凭空产生能量，且按正确方式互联，整体就稳。这一节讲清无源性为何蕴含稳定、多机搬运为何天然适合无源性分析、以及当变阻抗（§5.5）和通信延迟（§5.4 的对立面）威胁无源性时，能量罐与波变量如何把无源性"买回来"。这是本章唯一的研究级小节，也是把零散控制律焊成稳定系统的理论基石。

动机：为什么"每个控制器都稳"不等于"系统稳"¶

§5.0 场景二："双机搬运很稳，加第三台就发散。"这个现象击碎了一个常见幻觉——子系统稳定 ⇏ 整体稳定。三台机器人各自的阻抗控制器单独看都是稳定的（弹簧-阻尼系统），但它们通过物体形成的反馈回路可能注入净能量，使整体发散。

为什么会这样？因为稳定性在**互联**下不是简单叠加的。两个稳定系统反馈连接，结果可能不稳（经典控制里"两个稳定环节构成的闭环可能不稳"是常识）。多机搬运正是一堆控制器通过物体反馈互联——单独分析每个控制器毫无用处，必须分析互联后的整体。

但整体分析太难了：维度高（\(N\) 台机器人各有动力学）、结构可变（机器人可增减、接触可断合）、含延迟（若有通信）、含时变参数（变阻抗）。逐一推 Lyapunov 函数几乎不可能。我们需要一个**模块化、可组合**的稳定性判据——能对每个子系统单独验证，再断言整体。

这把尺就是无源性。它的威力在于：无源性在互联下是封闭的——若干无源子系统按正确拓扑（反馈/并联）连接，整体仍无源，从而稳定。这让我们能"分而治之"：分别证明"机器人控制器无源""物体无源""环境无源"，再断言整体稳定，无需推一个巨大的 Lyapunov 函数。

如果只用 Lyapunov 逐系统分析会怎样¶

坚持传统 Lyapunov（为整个 \(N\) 机系统构造一个 Lyapunov 函数并证 \(\dot V<0\)），会遇到三个困难。

第一，维度灾难与重构负担。 \(N\) 变了、接触拓扑变了、加了延迟，整个 Lyapunov 函数就得重新构造证明。无源性是模块化的——加一台机器人只需验证它无源，不必重证全局。

第二，难以处理延迟与时变。 通信延迟使系统成为时滞系统，时变阻抗使系统非自治，二者都让 Lyapunov 函数的构造异常困难。无源性框架有现成工具（散射理论/波变量处理延迟、能量罐处理时变），直接套用。

第三，丢失物理直觉。 Lyapunov 函数常是"凑"出来的数学构造，物理意义不明。无源性的储能函数就是系统的物理能量，\(\dot V\le\) 输入功率直接说"系统不造能量"——物理直觉清晰，便于定位"哪个环节在产能"。

本质洞察：无源性把"稳定性"从"求解微分方程的渐近行为"重新表述为"追踪能量的流动"。一个系统稳不稳，等价于问"它会不会凭空生出能量"。这个视角的革命性在于：能量是可叠加、可在子系统间传递、可在互联处守恒的物理量——所以稳定性分析变成了能量记账，而记账是模块化、可组合的。这正是它碾压逐系统 Lyapunov 的根本原因。

历史：从电路网络到遥操作再到协作¶

无源性源于**电路网络理论**——无源元件（电阻、电容、电感）不产生能量，无源网络互联仍无源。1960s-70s 这套理论被引入控制（Willems 的耗散系统理论）。1980s-90s，遥操作**领域遇到了通信延迟导致不稳的难题：Anderson 与 Spong 1989 用**散射理论（scattering theory） 证明了延迟通信信道是无源的关键条件；Niemeyer 与 Slotine 1991 提出**波变量（wave variables），把延迟信道改造成无源，从根本上解决了延迟遥操作的稳定性。2000s，时域无源性方法（Hannaford-Ryu 的 **Time-Domain Passivity / 能量罐 energy tank）提供了在线监测并保证无源的工具。这些工具被自然地迁移到多机协作搬运——因为"多机器人 + 物体"和"主从遥操作"共享同一个无源性骨架（§5.3 已指出 leader-follower 源于遥操作）。

理论：无源性的定义与互联定理¶

无源性定义¶

考虑一个系统，输入 \(u\)、输出 \(y\)（取为"功率共轭对"，如力与速度，\(u^\top y\) 是功率）。系统**无源**，若存在下有界的储能函数 \(V(x)\ge 0\)，使得对所有时间：

\[ \boxed{\;\dot{V} \le u^\top y \quad\Longleftrightarrow\quad V(t)-V(0)\le \int_0^t u^\top y\,d\tau .\;} \tag{5.17} \]

含义：系统储能的增量不超过外界注入的能量（功率积分）。系统**不会凭空产生能量**——它至多储存或耗散外界给的能量。若 \(\dot V \le u^\top y - \delta\|y\|^2\)（\(\delta>0\)）则**严格输出无源**（额外耗散）。

无源 ⇒ 稳定¶

取储能函数 \(V\) 作 Lyapunov 候选。当外部输入为零（\(u=0\)）时，(5.17) 给出 \(\dot V\le 0\)——储能不增，系统状态有界、收敛到能量极小点。这就是"无源蕴含（Lyapunov 意义下）稳定"。直观：一个不产能、且能耗散能量的系统，最终会"安定"到低能量状态。

互联定理：无源性在反馈/并联下封闭¶

这是无源性最有用的性质。两个无源系统 \(\Sigma_1,\Sigma_2\)：

负反馈互联（\(\Sigma_1\) 输出经 \(\Sigma_2\) 反馈回输入）：整体无源。储能函数取 \(V=V_1+V_2\)，功率在互联处守恒，整体 \(\dot V\le u_{\text{ext}}^\top y_{\text{ext}}\)。
并联（同输入、输出相加）：整体无源。

推论（本章关键）：若每个机器人控制器无源、物体（刚体）无源、环境无源，则整个"机器人-物体-环境"互联系统无源，从而稳定。 这就是把零散控制律焊成稳定系统的定理。

多机搬运为何天然适合无源性¶

回顾 §5.4 的信息流：\(f_i\xrightarrow{G}w_o\xrightarrow{\text{物体}}v_o\xrightarrow{G_j^\top}\dot x_j\)。注意 \(G\) 与 \(G^\top\) 的对偶（§5.1 的功守恒 \(f^\top\dot X = w_o^\top v_o\)）意味着：抓取界面本身不产生也不耗散能量——它是无源的（功率守恒地传递）。物体作为刚体，动能 \(\frac12 v_o^\top M_o v_o\) 是天然储能函数，物体也无源。所以只要各机器人控制器设计成无源（如标准阻抗律：弹簧势能 + 阻尼耗散），整个系统自动无源稳定。

本质洞察：多机搬运的稳定性"免费"地继承自三件事——(1) 抓取界面无源（功守恒，\(G/G^\top\) 对偶保证）；(2) 物体无源（刚体动能作储能）；(3) 标准阻抗控制器无源（势能 + 耗散）。三个无源系统互联即稳。麻烦只在**破坏无源性的因素**：变阻抗（注入能量）和通信延迟（产生能量）。本节后半就是修复这两个破坏者。

标准阻抗律为何无源¶

验证 §5.5 物体阻抗（设前馈/重力已补偿，看误差动力学）\(M_d\ddot{\tilde x}+D_d\dot{\tilde x}+K_d\tilde x = w_{\text{env}}\)，输入 \(w_{\text{env}}\)、输出 \(\dot{\tilde x}\)。取储能 \(V=\frac12\dot{\tilde x}^\top M_d\dot{\tilde x}+\frac12\tilde x^\top K_d\tilde x\)（动能 + 弹簧势能，\(\ge 0\)）。求导：

\[ \dot V = \dot{\tilde x}^\top M_d\ddot{\tilde x}+\dot{\tilde x}^\top K_d\tilde x = \dot{\tilde x}^\top(w_{\text{env}}-D_d\dot{\tilde x}-K_d\tilde x)+\dot{\tilde x}^\top K_d\tilde x = \dot{\tilde x}^\top w_{\text{env}} - \dot{\tilde x}^\top D_d\dot{\tilde x} . \]

由于 \(D_d\succ 0\)，\(\dot{\tilde x}^\top D_d\dot{\tilde x}\ge 0\)，故 \(\dot V\le \dot{\tilde x}^\top w_{\text{env}} = u^\top y\)。满足无源性 (5.17)，且严格输出无源（阻尼项额外耗散）。 这就是固定参数阻抗律稳定的能量证明——只要 \(M_d,D_d,K_d\) 是常正定矩阵。

破坏者一：变阻抗与能量罐¶

为什么变阻抗危险¶

§5.5 陷阱 3 和 §F06 都提到：在线改变刚度 \(K_d\) 会注入能量。看上面的证明——若 \(K_d=K_d(t)\) 时变，储能 \(V=\dots+\frac12\tilde x^\top K_d(t)\tilde x\) 求导多出一项 \(\frac12\tilde x^\top \dot K_d\tilde x\)：

\[ \dot V = \dot{\tilde x}^\top w_{\text{env}} - \dot{\tilde x}^\top D_d\dot{\tilde x} + \tfrac12\tilde x^\top \dot K_d\,\tilde x . \]

当 \(\dot K_d\succ 0\)（刚度增大）时，\(\frac12\tilde x^\top\dot K_d\tilde x>0\) 凭空注入能量，可能使 \(\dot V>u^\top y\)——无源性被破坏，系统可能失稳。物理上：增大弹簧刚度（在弹簧已被拉伸时）相当于"给弹簧充能"，这能量来自控制器，是主动注入。多机搬运里每台机器人若用变阻抗（如接近时硬、接触时软），都可能成为产能源。

能量罐：给能量注入装一个"预算账户"¶

能量罐（energy tank）的思想：设一个虚拟能量储罐 \(T_{\text{tank}}\ge 0\)，所有"产能"的动作（如增大刚度）必须从罐里取能，罐空了就不许产能（冻结刚度变化或限制其方向）。罐的能量来自系统的耗散（阻尼耗散的能量被回收进罐）。这样系统对外永远无源——产能动作用的是之前耗散攒下的能量，不是凭空创造。

能量罐动力学（示意）：

\[ \dot T_{\text{tank}} = \underbrace{\dot{\tilde x}^\top D_d\dot{\tilde x}}_{\text{回收耗散}} - \underbrace{P_{\text{inject}}}_{\text{产能动作取能}},\qquad T_{\text{tank}}\ge T_{\min}>0 . \tag{5.18} \]

放行规则：当要做产能动作（\(P_{\text{inject}}>0\)，如增刚度）时，仅当 \(T_{\text{tank}}>T_{\min}\) 才放行，否则按比例缩减或禁止。整体储能取 \(V_{\text{total}}=V+T_{\text{tank}}\)，可证 \(\dot V_{\text{total}}\le u^\top y\)——带能量罐的变阻抗系统重新无源。这是 §F06 单臂能量罐在多机的直接推广：每台机器人挂一个能量罐，或物体级挂一个共享罐。

破坏者二：通信延迟与波变量¶

为什么延迟产生能量¶

若多机搬运确实需要通信（如交换状态做协调），通信延迟 \(T\) 会让"延迟信道"成为产能元件。直观：你在 \(t\) 时刻发出的力/速度信息，对方在 \(t+T\) 收到。这个时间错位使"发送端做的功"和"接收端收到的功"不匹配，差额可正可负——当为正时，信道凭空"创造"了能量，破坏无源性，导致延迟越大越易失稳。这正是早期遥操作"延迟一大就发散"的根源。

波变量：把延迟信道改造成无源¶

波变量（Niemeyer-Slotine）的精妙：不直接传"力"和"速度"，而传它们的**线性组合**——波变量。定义（一维示意，\(b\) 为波阻抗）：

\[ u = \frac{1}{\sqrt{2b}}(F + b\,\dot x),\qquad v = \frac{1}{\sqrt{2b}}(F - b\,\dot x), \tag{5.19} \]

\(u\) 是前向波、\(v\) 是返回波。关键性质：用波变量传输时，无论延迟 \(T\) 多大，信道储能 \(\ge 0\)（无源）。证明核心：信道功率 \(P = \frac12(\|u\|^2-\|v\|^2)\)，延迟只是把 \(u(t)\) 变成 \(u(t-T)\)、\(v(t)\) 变成 \(v(t-T)\)，可证信道在 \([0,t]\) 净吸收能量 \(\frac12\int(\,u^2(t-T)\dots)\ge 0\)——延迟信道用波变量传输后变成无源的"传输线"，不再产能。代价：波变量引入了"波反射"等动态，需调 \(b\) 折中响应与稳定。

类比的边界：波变量之于延迟通信，像声音在长管中以波的形式传播。像的部分：信息以"波"形式传递、有传播延迟、用特征阻抗 \(b\) 描述传输线、不会凭空增能（无源传输线）。不像的部分：物理声波是被动现象，波变量是主动编码控制信号的数学变换；且真实通信是离散采样的，波变量需配合采样无源化处理（否则离散化又会引入能量）。把"声波天然无源"照搬，会忽略离散实现时的额外无源化需求。

多机搬运里何时需要波变量¶

纯去中心化（§5.4，零通信）不需要波变量——它根本不通过网络传信息，靠物体（无源的物理信道）。波变量只在**确实需要显式通信协调**且延迟不可忽略时才需要（如机器人相距远、用无线网交换状态）。这给了一个清晰的工程判断：能用物体隐式通信（§5.4）就用，物理信道天然无源、零延迟；必须显式通信且有延迟时，用波变量把网络信道无源化。

阶段小结：到这里我们建立了完整的稳定性框架——无源定义 (5.17)、互联封闭定理、标准阻抗无源证明，以及两个破坏者（变阻抗、延迟）及其修复（能量罐 5.18、波变量 5.19）。这把"万能尺"让我们能模块化地判定和保证多机搬运的稳定性。

代码实现：能量罐放行阀¶

// energy_tank.hpp —— 保证变阻抗动作不破坏无源性
struct EnergyTank {
  double T = 5.0;          // 罐内能量(J), 初值给一些缓冲
  double T_min = 0.1;      // 下限, 低于此禁止产能动作
  double T_max = 10.0;     // 上限, 防止无限积累(超出则丢弃)

  // 每个控制周期: 回收阻尼耗散, 为产能动作扣能
  // dissipated: 本周期阻尼耗散的能量 (≥0), 回收进罐
  // requested_inject: 本周期产能动作(如增刚度)想注入的能量
  // 返回: 实际允许注入的能量 (≤ requested, 受罐余额限制)
  double regulate(double dissipated, double requested_inject) {
    T += dissipated;                         // 回收耗散
    if (T > T_max) T = T_max;                // 限幅, 多余耗散丢弃
    if (requested_inject <= 0) return requested_inject; // 耗能动作直接放行
    double available = T - T_min;            // 可用余额
    double allowed = std::min(requested_inject, std::max(0.0, available));
    T -= allowed;                            // 扣能
    return allowed;                          // 调用方据此缩放刚度变化率
  }
};

// ❌ 错误: 不设 T_min, 允许罐耗尽到 0 仍放行产能动作
//   罐到 0 还放行 → 实际注入了不存在的能量 → 无源性破坏。
//   必须保留 T_min>0 缓冲, 罐见底就冻结/缩减产能动作。
// ❌ 错误: 不限幅 T_max, 长期耗散使罐无限增大
//   → 攒了巨量"能量预算", 某刻可一次性大注入, 形同没有保护。
//   T_max 限制单次可动用的历史能量。

理论-工程桥接：无源性如何指导你调多机搬运的增益¶

无源性不只是事后判稳，更是设计指南。它告诉你：(1) 阻尼 \(D_d\) 必须正定且足够大——它是系统唯一的耗散源，太小则接近无源边界、易振荡；(2) 刚度变化要挂能量罐，否则变阻抗会偷偷产能；(3) 有通信延迟就用波变量，别指望调增益硬扛延迟。§5.0 场景二"加第三台就发散"，用无源性诊断就是：检查新加的机器人控制器是否无源（阻尼够不够、有没有不受控的产能项）、互联是否引入了产能通道。这把抽象的能量记账变成了具体的调参清单。

⚠️ §5.7 常见陷阱¶

陷阱 1（思维陷阱）：以为各子系统稳定则整体稳定

🧠 思维陷阱: 单独验证每个机器人控制器稳定就放心了
   新手想法: "每台机器人的阻抗控制器都是稳定的弹簧-阻尼, 合起来
            当然稳"
   实际上: 稳定性在反馈互联下不叠加——两个稳定系统反馈连接可能
          不稳。多机通过物体强反馈互联, 必须分析互联后的整体。
          正确的模块化判据是无源性(不是稳定性): 各子系统无源 +
          正确互联 ⇒ 整体无源 ⇒ 稳定。无源性可叠加, 稳定性不可。
   正确思维: 验证"无源"而非"稳定"作为模块化判据。问每个子系统
          "会不会产能", 而非"自己稳不稳"。
   自检方法: 为每个控制器写储能函数, 验 V̇≤uᵀy; 若某控制器产能
          (如未保护的变阻抗), 它就是整体失稳的嫌疑源。

陷阱 2（概念误区）：把无源性等同于稳定性

💡 概念误区: 认为"无源"和"稳定"是同义词
   新手想法: "无源就是稳定, 两个词换着用"
   实际上: 无源 ⇒ (Lyapunov 意义)稳定, 但反之不真——存在稳定却
          非无源的系统。无源是比稳定更强、且可组合的性质。我们用
          无源正是因为它能模块化组合(互联封闭), 稳定性本身不能。
          且无源是关于"能量"的输入输出性质, 稳定是关于"状态收敛"
          的内部性质, 概念层次不同。
   为什么重要: 混淆二者会让你误以为"证了稳定就等于证了可安全互联",
            但只有无源才保证互联后仍稳。
   延伸思考: 严格输出无源 + 可检测 ⇒ 渐近稳定, 这类定理把无源和
            稳定精确联系起来, 但二者不是等号。

陷阱 3（编程陷阱）：能量罐回收的"耗散能量"算错符号或来源

⚠️ 编程陷阱: 往能量罐里回收了本不该回收的能量
   错误做法: 把"环境注入的能量"也当耗散回收进罐, 或把阻尼耗散
            符号算反(本应≥0 却出现负值)。
   现象: 罐能量虚高, 系统获得了不属于自己耗散的"能量预算", 产能
        动作被过度放行, 无源性保护形同虚设, 变阻抗仍致失稳。
   根本原因: 能量罐只能回收系统自身耗散(阻尼项 ẋᵀDẋ≥0)的能量,
            不能把外界注入或测量噪声当能量收。
   正确做法: 严格只回收阻尼耗散 ẋᵀD_d ẋ(恒≥0); 验证回收量非负;
            环境交互能量不进罐。自检: 系统静止(ẋ=0)时回收应为 0,
            罐能量不应在静止时增长。

§5.7 练习¶

（证明题，⭐⭐⭐） 完整证明：固定参数的物体阻抗误差动力学 \(M_d\ddot{\tilde x}+D_d\dot{\tilde x}+K_d\tilde x=w_{\text{env}}\)（\(M_d,D_d,K_d\) 常正定）以 \(w_{\text{env}}\) 为输入、\(\dot{\tilde x}\) 为输出是无源的。然后证明：若 \(K_d=K_d(t)\) 单调增大，无源性可能被破坏，并指出被注入能量的具体项。最后说明能量罐如何修复（写出带罐的总储能函数）。
（分析题，⭐⭐⭐） §5.0 场景二"加第三台机器人就发散"。用无源性框架给出至少三个可能的"产能源"诊断假设（如：第三台控制器阻尼不足接近无源边界、它用了未保护的变阻抗、它与其他机器人间存在通信延迟未做波变量处理）。对每个假设，给出一个可操作的检验方法和对应的修复措施。
（跨章综合题，⭐⭐⭐⭐，综合 §5.7 + §F06 + §5.3 遥操作） 论述无源性这把"尺"如何贯穿三个场景：单臂变阻抗（§F06 能量罐）、主从遥操作（§5.3/本节波变量处理延迟）、多机协同搬运（本节互联封闭定理）。三者的"储能函数""破坏无源的因素""修复手段"分别是什么？为什么同一套无源性理论能统一这三个看似不同的问题——它们共享的物理本质（能量流动）是什么？

§5.8 前沿工作与综合实战 ⭐⭐⭐⭐¶

这一节解决什么问题：前七节建立了完整的理论与方法工具箱。这一节做两件事：(1) 用前面的概念框架解读三个代表性前沿工作（多足绳索搬运、被动臂搬运、零通信学习搬运），看经典理论如何在最新系统里活着；(2) 把全章知识串成一个可动手的综合实战——MuJoCo 双 Go2 协同搬运，作为累积项目"Mini-Cooperative-Transport"的本章模块。

前沿一：多足绳索搬运（Sombolestan & Nguyen 2022）¶

问题与方案。 多台四足机器人通过**绳索**牵引一个负载协同搬运（如几台 Go1 拖一个重物）。绳索是关键约束——它**只能拉、不能推**（单边约束的极端形式），且几乎不能传力矩。

用本章框架解读：

抓取矩阵：每根绳索的接触力 \(f_i\) 沿绳方向，只能为拉力（\(f_i\) 在绳方向投影 \(\ge 0\)）。抓取矩阵 \(G\) 把各绳张力映射到负载旋量，结构同 §5.1，但接触力被严格限制在"沿绳、且为拉"的一维射线上。
负载分配：变成 §5.6 的 QP，且约束特别强——每根绳的张力 \(\ge 0\)（单边）、\(\le\) 绳的强度上限。摩擦锥退化为"张力非负"。这是 §5.6"单边约束"最纯粹的体现。
架构：分布式力控——每台四足本地控制自己的绳张力，靠负载运动隐式协调（§5.4 思想）。
稳定性：绳的柔性引入额外动态，无源性分析（§5.7）需考虑绳作为弹性储能元件。

本质洞察：绳索搬运是多机搬运的"约束极限案例"——把"接触力可任意方向"收紧到"只能沿一根线拉"。它逼着你直面 §5.6 的单边约束和 §5.2 的内力含义（多根绳之间的张力分配就是内力调节：绳越紧内力越大、越稳但越费力）。理解了绳索搬运，你对一般接触的内力/可行性约束会有更深的体会。

前沿二：被动臂搬运（PACC, Turrisi 2024）¶

问题与方案。 高负载协同搬运中，机器人间的耦合（一台动，通过刚性物体扰动其他台）让控制极难。PACC 的巧思：不在四足足端硬抓物体（高耦合），而是在每台四足背上装一根**被动臂（passive arm，无电机）**，臂端通过万向节与负载连接。

用本章框架解读：

耦合解耦：被动臂的被动关节像"机械低通滤波器"，**吸收**机器人与负载间的高频交互力。这相当于在 §5.1 的刚性闭链里插入了柔性环节——闭链不再是刚性的，内力被被动臂的形变吸收，不会瞬间传遍全系统。
简化控制：因为被动臂吸收了耦合，每台四足可以跑自己的**标准 MPC**（如 OCS2），几乎不必显式处理多机耦合——这是用**硬件（被动臂）换软件复杂度**的工程权衡。
代价：需额外硬件（被动臂 + 万向节），最大负载受臂结构强度限制。

类比的边界：被动臂之于刚性抓持，像汽车悬架之于硬连接车轴。像的部分：都用被动柔性元件吸收冲击/耦合，让上游（车身/四足控制器）不必处理高频扰动。不像的部分：悬架主要隔振，被动臂还改变了闭链的力传递结构（把内力变成被动臂的形变）；且被动臂的引入改变了 §5.1 抓取矩阵的刚性假设。把"悬架=纯隔振"照搬会低估被动臂对内力空间的重构作用。

前沿三：零通信学习搬运（decPLM, Pandit 2025）¶

问题与方案。 2-10 台 Go2 四足**零通信**协同搬运，用强化学习（IsaacLab 训练）学出 pinch-lift-move（捏紧→抬起→移动）策略。每台 Go2 只用本体感知（自身姿态、足端接触/力），不与其他 Go2 通信。

用本章框架解读：

去中心化 + 隐式通信：这是 §5.4 的纯粹实现——同构策略、零通信、靠物体运动隐式协调。每台 Go2 的策略输入只有本地观测，协调从物理耦合中涌现。
pinch = 内力调节：捏紧阶段对应 §5.2 在 \(\ker G\) 里增大内力（夹紧防滑）；lift/move 对应施加外力旋量（§5.5 物体运动）。学习策略隐式地学会了内力/外力的分配时序。
学习替代显式建模：经典方法（§5.5）需要物体惯量 \(M_o\)，decPLM 用 RL 免模型——策略从经验中隐式习得如何应对未知负载，呼应 §5.0 脉络表"从精确模型走向自适应/学习"的主线终点。

本质洞察：decPLM 不是抛弃了本章理论，而是用神经网络**隐式地学会了**本章显式推导的东西——它的策略内部一定编码了某种"内力/外力分解"和"隐式协调"，只是没有写成 \(G^+w+E\lambda\) 的解析形式。理解本章理论，你才能看懂 decPLM 学到了什么、它的 pinch-lift-move 为何对应内力调节与外力施加，以及它的局限（学习的策略缺乏无源性保证，§5.7 的稳定性证明对它不直接适用，这正是学习型搬运的开放问题）。

前沿全景与开放问题¶

工作	架构	接触	建模	本章对应	开放问题
Sombolestan 2022	分布式	绳索(单边拉)	模型	§5.4+§5.6 单边QP	绳柔性下的稳定性
PACC 2024	各自MPC	被动臂+万向节	模型(解耦)	§5.1 柔性闭链	被动臂参数设计、负载上限
decPLM 2025	去中心化学习	pinch(足)	免模型RL	§5.4+§5.2 内力	学习策略的稳定性保证、泛化
Verginis 2019	分布式自适应	刚性	自适应(免惯量)	§5.5+§5.7	动态(非准静态)搬运

核心开放问题：(1) 动态搬运的内力优化——负载惯性效应显著（如四足跑步时搬运）时，如何实时优化内力？现有多数方法（Verginis）限准静态。(2) 学习型搬运的稳定性保证——decPLM 等 RL 策略缺乏 §5.7 的无源性证明，如何把无源性约束嵌入学习？(3) 柔性物体搬运——本章假设刚体，真实物体（布料、长杆、液体容器）的柔性使闭链约束变成偏微分方程，理论尚不成熟。

累积项目：Mini-Cooperative-Transport 本章模块¶

累积项目说明：本 Part 的累积项目是从零搭建一个多机协作系统。前几章已搭建：第 1 章通信图与系统建模、第 2 章共识协调、第 3 章任务分配与 MAPF（planning/ 模块）、第 4 章分布式 MPC 编队。本章新增 cooperative_transport/ 模块——多机协同搬运与力控。

本章新增模块结构：

cooperative_transport/
├── grasp_matrix.hpp          # §5.1: 抓取矩阵组装 (点接触/刚性抓持)
├── internal_force.hpp        # §5.2: 内力/外力分解, 零空间基
├── object_impedance.hpp      # §5.5: 物体阻抗控制器
├── load_distribution_qp.hpp  # §5.6: 摩擦锥下负载分配 QP
├── energy_tank.hpp           # §5.7: 无源性保护(变阻抗放行阀)
├── decentralized_ctrl.hpp    # §5.4: 去中心化同构控制律
└── sim/
    └── dual_go2_carry.py     # MuJoCo 双 Go2 协同搬运仿真

实战目标：在 MuJoCo 中搭建双 Go2 + 刚性杆场景，实现协同搬运。分阶段递进（渐隐示例法：从给定到独立）。

Phase 1（完全跟随）：搭场景 + 抓取矩阵。 给定 MuJoCo 场景 XML（两个 Go2 + 一根刚性杆，杆两端与各 Go2 躯干/夹持点固连）。调用 buildGraspMatrix 计算当前 \(G\)，验证 \(\mathrm{rank}(G)\) 和内力维度。

# dual_go2_carry.py (Phase 1 骨架)
import mujoco, numpy as np
model = mujoco.MjModel.from_xml_path("dual_go2_bar.xml")
data  = mujoco.MjData(model)

def grasp_matrix(contact_pts, com):
    def skew(r): return np.array([[0,-r[2],r[1]],[r[2],0,-r[0]],[-r[1],r[0],0]])
    return np.hstack([np.vstack([np.eye(3), skew(p-com)]) for p in contact_pts])

# 每帧: 取两 Go2 抓持点世界坐标和杆质心, 算 G
# contact_pts = [data.site_xpos[id_left], data.site_xpos[id_right]]
# com = data.body(bar_id).xpos
# G = grasp_matrix(contact_pts, com)   # 6×6 (双机点接触)

Phase 2（填空）：物体阻抗 + 力分配。 给定物体期望轨迹（杆从地面抬到目标高度），用物体阻抗 (5.8)-(5.11) 算所需旋量、用负载分配 QP (5.16) 分给两 Go2。关键步骤留空让你填：

# Phase 2: 填空
def control_step(data, x_des, xd_des):
    G = grasp_matrix(...)                  # 复用 Phase 1
    x_err  = _____                          # 填: 杆位姿误差
    xd_err = _____                          # 填: 杆速度误差
    # 物体阻抗求所需旋量 (式 5.8 + 5.10)
    xdd = xdd_des - inv(Md) @ (Dd@xd_err + Kd@x_err - w_env)
    w_ext = Mo @ xdd - w_grav               # 重力旋量务必补偿!
    # 负载分配 (式 5.16, 此处简化为伪逆+内力, QP 见 load_distribution_qp)
    f = pinv(G) @ w_ext + E @ lambda_int    # 填: E, lambda_int 怎么取?
    # 各 Go2: 把接触力经雅可比转置变关节力矩 (略, 见 §F02)
    return f

Phase 3（主导）：去中心化版本 + 鲁棒性测试。 改写为去中心化（§5.4）：每个 Go2 跑同一本地控制律，仅用本地观测。测试鲁棒性：(a) 一个 Go2 注入定位噪声，观察隐式协调是否仍收敛；(b) 给杆施加外部冲击，验证物体阻抗的柔顺响应；(c) 用能量罐保护一个变阻抗策略，对比有/无能量罐的稳定性。

Phase 4（独立）：扩展到三机/绳索。 把场景扩展为三个 Go2（内力维度从 0 变 3，需主动调内力），或改为绳索牵引（单边约束，QP 必须处理）。这是把本章理论独立应用到新配置的检验。

验收标准：杆被平稳抬起并跟踪目标轨迹（位置误差 < 5 cm）、两 Go2 接触力之和竖直分量 ≈ 杆重、内力维持在合理范围（不挤爆、不脱手）、去中心化版本在定位噪声下仍收敛。

§5.8 练习¶

（精读题，⭐⭐⭐） 精读 decPLM（decplm.github.io 或 arXiv 2025）的方法部分，用本章语言标注：(a) 它的"去中心化"对应 §5.4 的哪些要素？(b) pinch-lift-move 三阶段分别对应内力/外力的什么操作？(c) 它如何绕开物体惯量未知（对比 §5.5 基于模型 vs Verginis 自适应 vs decPLM 学习）？(d) 它缺少 §5.7 的什么保证？
（实战题，⭐⭐⭐） 完成累积项目 Phase 1-2：在 MuJoCo 搭双 Go2 + 刚性杆场景，实现物体阻抗 + 伪逆力分配，把杆从地面抬到 0.5 m 高。报告：物体跟踪误差曲线、两 Go2 接触力曲线、验证接触力之和竖直分量 ≈ 杆重。
（跨章综合题，⭐⭐⭐⭐，综合全章 + 第 1-4 章） 设计一个完整的多机搬运任务管线，串起本 Part 全部知识：第 3 章任务分配决定"哪几台机器人去搬哪个物体"、第 3 章 MAPF/第 4 章编队 MPC 让它们无碰撞地移动到物体旁、本章 §5.1-5.7 完成抓取与协同搬运。画出完整数据流图，标注每一步用到的章节、关键算法和接口。讨论：从"移动到物体旁"切换到"协同搬运"的瞬间，控制架构如何平滑过渡（编队 MPC → 闭链力控）？这个切换点最容易出什么问题？

本章常见误解汇总¶

误解	正确理解	出处
抓取矩阵是多机搬运的专用工具矩阵	它是闭链约束的数学化身，与单臂雅可比 \(J\) 同构（力映射 \(G\)、速度对偶 \(G^\top\)），零空间住着内力	§5.1
两台机器人一起搬就一定有内力要控制	内力维度 = \(cN-6\)。点接触双机（\(c{=}3\)）内力维度为 0；刚性抓持（\(c{=}6\)）才有 6 维内力	§5.1
Moore-Penrose 伪逆解自动给零内力/非挤压分配	最小范数 ≠ 非挤压；且真实搬运需非零夹紧力。内力必须显式设计，不是伪逆副产品	§5.2
内力是应当消除的副作用，越小越好	内力是抓持必需品（防滑），目标是"调到恰当值"（防滑下界、不损坏上界），不是消除	§5.2
follower 必须知道物体绝对轨迹才能配合	follower 只需物体当前运动方向——意图经物体运动隐式传递，不需轨迹广播	§5.3
leader 出多大力物体就被多大力驱动	力放大架构里 follower 力之和可远大于 leader 力；leader 定方向、follower 提供动力	§5.3
去中心化 = 机器人各干各的、互不影响	去中心化机器人通过物体强耦合，协调从物理耦合涌现；"独立"的只是计算不是物理	§5.4
去中心化总优于集中式	去中心化用"放弃全局信息"换可扩展与鲁棒，代价是精度。架构是权衡不是优劣排序	§5.4
多机阻抗 = N 个末端各跑一套阻抗	物体阻抗在 6 维物体空间统一定义（运动/内力解耦），N 个末端独立阻抗会过定义、纠缠内力	§5.5
加大物体刚度 \(K_d\) 总能提升跟踪精度	\(K_d\) 过大牺牲柔顺、放大内力、逼近稳定边界；跟踪差应先查前馈/重力补偿/模型	§5.5
加权伪逆能满足摩擦/单边约束	加权伪逆只改"选哪个最小范数解"，仍无约束，照样可能要求拉接触或超摩擦锥	§5.6
QP 无解是求解器问题	QP infeasible 常是物理事实（物体太重/摩擦太小/布局太差），是有用的诊断信号	§5.6
各子系统稳定则整体稳定	稳定性在反馈互联下不叠加；模块化判据是无源性（可叠加），不是稳定性	§5.7
无源性 = 稳定性	无源 ⇒ 稳定，反之不真。无源是更强且可组合的性质，这正是用它的原因	§5.7

本章小结¶

本章从一个朴素观察出发——多台机器人抓住同一物体就被焊成闭合运动链——系统地建立了协同搬运与力控的完整知识体系。

主线回顾：

闭链约束 → 抓取矩阵（§5.1）：闭链耦合的数学化身是抓取矩阵 \(G\)，力映射 \(w_o=Gf\) 与速度对偶 \(\dot X=G^\top v_o\) 由功守恒联系，零空间维度 \(cN-6\) 量化了内力自由度。
内力/外力分解（§5.2）：接触力正交分解为运动力（\(\mathrm{row}\,G\)，搬物体）与内力（\(\ker G\)，夹物体），\(f=G^+w_{\text{ext}}+E\lambda\)，二者独立设计。
谁来决策（§5.3-5.4）：leader-follower（主从、意图估计、力放大）与去中心化（同构、隐式通信、物体即共享黑板）两条路线，构成"全集中—主从—去中心化"权衡谱。
用什么行为搬（§5.5）：物体阻抗把期望柔顺定义在物体质心（而非各末端），运动阻抗与内力阻抗在正交子空间解耦。
力怎么分（§5.6）：负载分配从伪逆升级为带摩擦锥/单边/力限的 QP，保证物理可行。
稳不稳（§5.7）：无源性作为模块化判据（互联封闭），能量罐修复变阻抗、波变量修复延迟。
前沿与实战（§5.8）：绳索/被动臂/零通信学习三个前沿用经典框架解读，MuJoCo 双 Go2 搬运串起全章。

贯穿全章的本质洞察：多机搬运的一切——耦合、内力、协调、稳定——都源自"被搬物体把机器人焊成闭链"这一事实。物体既是约束（闭链）、又是隐式通信信道（零通信协作的根基）、还是无源的能量传递媒介（稳定性的来源）。抓取矩阵 \(G\) 及其零空间是描述这一切的统一语言，而它与单臂雅可比、多指抓取共享同一数学结构。

术语速查表¶

术语（中 / 英）	一句话定义
抓取矩阵（Grasp Matrix, \(G\)）	把各接触力映射到物体合力旋量的 \(6\times cN\) 矩阵，\(w_o=Gf\)
闭合运动链（Closed Kinematic Chain）	多机器人 + 刚体 + 共同基座构成的力/运动相互约束的环
力旋量（Wrench, \(w\)）	力 + 力矩的 6 维组合，\([F^\top,\tau^\top]^\top\)
内力（Internal Force, \(f_{\text{int}}\)）	位于 \(\ker G\) 的接触力，不产生物体运动，维持抓持（夹紧/张紧）
运动力/外力（Motion/External Force）	位于 \(\mathrm{row}\,G\) 的接触力，产生期望物体旋量，\(G^+w_{\text{ext}}\)
零空间投影（Null-space Projection, \(I-G^+G\)）	把任意接触力投到 \(\ker G\)（内力子空间）的正交投影
非挤压伪逆（Nonsqueezing Pseudoinverse）	Walker 构造的特殊加权伪逆，使力分配不在物体内产生挤压
leader-follower（主从架构）	一台 leader 知目标用阻抗领航，follower 靠物体运动估计意图配合
去中心化（Decentralized）	所有 agent 同构、仅用本地测量、不通信，靠物体隐式协调
隐式通信（Implicit Communication）	通过被搬物体的运动传递意图/状态，无显式网络通信
物体阻抗（Object Impedance）	把期望阻抗（\(M_d,D_d,K_d\)）加在物体质心，使物体整体柔顺
负载分配（Load Distribution）	把物体所需外力旋量分配给各机器人接触力的问题
摩擦锥/摩擦金字塔（Friction Cone/Pyramid）	切向力 \(\le\mu\times\)法向力的约束；线性化为多面锥便于 QP
单边接触（Unilateral Contact）	接触只能推或只能拉（如脚只能推、绳只能拉），法向力非负
无源性（Passivity）	存在储能 \(V\ge 0\) 使 \(\dot V\le u^\top y\)，系统不凭空产能
能量罐（Energy Tank）	给产能动作（如变阻抗）设的能量预算账户，由耗散回收充能
波变量（Wave Variables）	力与速度的线性组合，使延迟通信信道无源化
波阻抗（Wave Impedance, \(b\)）	波变量定义中的特征阻抗参数，折中响应与稳定

知识点总表¶

编号	知识点	核心要点	对应节	难度
1	闭链约束	多机+刚体=闭合运动链，运动相互约束	§5.1	⭐⭐⭐
2	抓取矩阵 \(G\)	\(w_o=Gf\)，列 \([I_3;\widehat r_i]\)，纯几何不含动力学	§5.1	⭐⭐⭐
3	速度对偶	\(\dot X=G^\top v_o\)，由功守恒与力映射对偶	§5.1	⭐⭐⭐
4	零空间维度	\(\dim\ker G=cN-6\)，量化内力自由度	§5.1	⭐⭐⭐
5	接触模型分类	点接触(\(c{=}3\))/软指(\(c{=}4\))/刚性抓持(\(c{=}6\))	§5.1	⭐⭐
6	内力/外力分解	\(f=G^+w+E\lambda\)，正交分解	§5.2	⭐⭐⭐
7	零空间投影性质	幂等+对称，运动力⟂内力	§5.2	⭐⭐⭐
8	内力基 \(E\)	\(\ker G\) 标准正交基，\(\lambda\) 是内力模式强度	§5.2	⭐⭐⭐
9	非挤压伪逆	最小范数≠非挤压，内力须显式设计	§5.2	⭐⭐⭐
10	leader 阻抗律	leader 用阻抗领航物体	§5.3	⭐⭐⭐
11	follower 力放大	对齐物体运动方向，放大 leader 力	§5.3	⭐⭐⭐
12	意图估计	物体运动方向即 leader 意图	§5.3	⭐⭐⭐
13	去中心化同构控制	导航+顺从+内力三分量，本地律	§5.4	⭐⭐⭐
14	隐式通信	物体=零延迟无源广播信道	§5.4	⭐⭐⭐
15	三架构权衡谱	全集中—主从—去中心化，信息vs规模	§5.4	⭐⭐⭐
16	物体阻抗律	阻抗加在物体质心，6维统一柔顺	§5.5	⭐⭐⭐
17	物体阻抗→接触力管线	阻抗→所需旋量→分解→关节力矩	§5.5	⭐⭐⭐
18	内力阻抗	内力也柔顺，与运动阻抗正交解耦	§5.5	⭐⭐⭐
19	负载分配 QP	等式(旋量)+不等式(摩擦/单边/力限)	§5.6	⭐⭐⭐
20	摩擦金字塔	二阶锥线性化为多面锥供 QP	§5.6	⭐⭐⭐
21	三分配策略	等分/加权伪逆/QP 取舍	§5.6	⭐⭐⭐
22	无源性定义与判稳	\(\dot V\le u^\top y\)，无源⇒稳定	§5.7	⭐⭐⭐⭐
23	互联封闭定理	无源子系统互联仍无源	§5.7	⭐⭐⭐⭐
24	能量罐	变阻抗产能须从罐取能	§5.7	⭐⭐⭐⭐
25	波变量	延迟信道无源化	§5.7	⭐⭐⭐⭐
26	前沿解读	绳索/被动臂/零通信学习	§5.8	⭐⭐⭐⭐

延伸阅读¶

奠基与经典（⭐⭐⭐）：

Orin & Oh, "Control of Force Distribution in Robotic Mechanisms Containing Closed Kinematic Chains," ASME J. Dyn. Sys. Meas. Control, 1986 — 闭链力分配的一般公式，本章 §5.6 的源头。
Walker, Freeman & Marcus, "Internal Object Loading for Multiple Cooperating Robot Manipulators," ICRA, 1988 — 非挤压伪逆，内力的经典分析。
Khatib, "Object Manipulation in a Multi-Effector Robot System," ISRR, 1988；Khatib et al., "Unified Approach for Motion and Force Control," IEEE T-RA, 2000 — 增广物体模型与操作空间内力/运动解耦，§5.2/§5.5 理论母体。
Caccavale & Chiacchio et al., 双臂协调控制系列, IEEE T-RA, 1990s — 对称构型（绝对/相对运动）与物体阻抗雏形。

物体级阻抗与抓取控制（⭐⭐⭐）：

Wimböck, Ott & Hirzinger, "Comparison of Object-Level Grasp Controllers for Dynamic Dexterous Manipulation," IJRR, 2012（DLR）— 物体级阻抗统一框架，多指/多臂通用，§5.5 现代形态。
Caccavale et al., "Six-DOF Impedance Control of Dual-Arm Cooperative Manipulators," IEEE/ASME T-Mech, 2008 — 双臂六自由度物体阻抗。

分布式与去中心化（⭐⭐⭐⭐）：

Verginis & Dimarogonas, "Cooperative Manipulation via Internal Force Regulation: A Rigidity Theory Perspective," RA-L / arXiv:1911.01297, 2019 — 抓取矩阵与刚度矩阵的 range-nullspace 关系、能量最优内力。
Culbertson & Schwager, "Decentralized Adaptive Control for Collaborative Manipulation of Rigid Bodies," IEEE T-RO, 2020 — 去中心化自适应，免负载惯性。
Wang & Schwager, "Collaborative Multi-Robot Transportation in Obstacle-Cluttered Environments via Implicit Communication," Frontiers Robotics & AI, 2018 — 隐式通信框架。
Carey & Werfel, "Collective Transport of Unconstrained Objects via Implicit Communication" (Force-ANTS), ICRA, 2021 — 力放大 N-robot 系统。

无源性与遥操作（⭐⭐⭐⭐）：

Anderson & Spong, "Bilateral Control of Teleoperators with Time Delay," IEEE T-AC, 1989 — 散射理论解决延迟。
Niemeyer & Slotine, "Stable Adaptive Teleoperation," IEEE J. Oceanic Eng., 1991 — 波变量。
Hannaford & Ryu, "Time-Domain Passivity Control of Haptic Interfaces," IEEE T-RA, 2002 — 时域无源性 / 能量罐。
Nuño, Basañez & Ortega, "Passivity-based Control for Bilateral Teleoperation: A Tutorial," Automatica, 2011 — 无源遥操作综述。

前沿系统（⭐⭐⭐⭐，研究级）：

Sombolestan & Nguyen, "Collaborative Navigation and Manipulation of Cable-towed Load by Multiple Quadrupedal Robots," IROS, 2022 — 多足绳索搬运。
Turrisi et al., "PACC: A Passive-Arm Approach for High-Payload Collaborative Carrying with Quadruped Robots Using MPC," IROS / arXiv:2403.19862, 2024 — 被动臂搬运。
Pandit et al., "decPLM: Multi-Quadruped Cooperative Object Transport," arXiv, 2025（decplm.github.io）— 零通信学习搬运。

开源项目（动手）：

MQE（ziyanx02/multiagent-quadruped-environment）— IsaacGym 多四足环境，支持 2-4 agent 协同搬运。
decPLM（decplm.github.io）— 训练代码 + IsaacLab 环境配置。
力分配 QP 求解器：OSQP（osqp.org）、qpOASES——本章 §5.6 实时力分配可直接使用。

本章与后续章节的关系¶

章节	关系	铺垫的知识点
§70 异构多机协同	本章是同构搬运基础；异构搬运需按机型分配不同子任务/力，复用本章抓取矩阵 + 负载分配 QP	抓取矩阵、负载分配、leader-follower
§80 双臂/多臂协同操作	双臂是 \(N{=}2\) 刚性抓持的特例；§D03 双臂力控直接用本章抓取矩阵与内力分解	内力/外力分解、物体阻抗
§90 多足协同 Loco-Manipulation	移动 + 搬运的耦合；本章力控 + 第 4 章编队 MPC 的融合	物体阻抗、去中心化、QP 力分配
§100 人形四足臂协同控制	多形态协同搬运，本章框架的综合应用	全章
§110-130 MARL 系列	decPLM 式学习搬运的 MARL 视角；本章理论是解读学习策略的物理框架	去中心化、隐式通信、内力
§140 Mini-MultiBot 综合实战	累积项目收口，直接复用本章 `cooperative_transport/` 模块	全章模块

与 05_运动控制力控章节的交叉引用（横向关联，已在正文反复使用）：

§F01 力控导论（阻抗/导纳二分法） ↔ 本章 §5.5 物体阻抗：单臂阻抗律推广到物体质心。
§F02 操作空间动力学 ↔ 本章 §5.1：雅可比 \(J\)（单臂）与抓取矩阵 \(G\)（多机）同构，\(\tau=J^\top F\) 与 \(\dot X=G^\top v_o\) 同源。
§F03 经典力控算法（摩擦锥） ↔ 本章 §5.6：摩擦锥/金字塔约束在负载分配 QP 中复用。
§F06 变阻抗无源性与碰撞安全 ↔ 本章 §5.7：单臂能量罐推广到多机耦合，无源性框架一脉相承。
§F08 腿足 MPC/WBC 联合力控 ↔ 本章 §5.6：足端力分配 QP 与多机负载分配 QP 结构同一。
§D03 双臂协调力控（Grasp Matrix / 内力分解 / Object Impedance） ↔ 本章 §5.1/§5.2/§5.5：同一套抓取矩阵理论在 \(N{=}2\) 刚性抓持的展开，本章是其多机推广。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关章节
物体被越夹越紧 / 接触力电流飙升	内力失控（无内力调节，或各末端独立阻抗期望不一致）	1. 检查是否用了物体阻抗而非各末端独立阻抗 2. 打印内力分量 \(f_{\text{int}}=E\lambda\) 看是否单调增 3. 确认期望位姿只在物体级定义、各末端由刚体几何导出	§5.2, §5.5
物体从某接触点滑脱	该点切向力超摩擦锥 / 内力（夹紧）不足	1. 检查 QP 是否含摩擦锥约束（非伪逆）2. 打印各点 \(\\|f_t\\|\) 与 \(\mu f_n\) 比值 3. 增大内力夹紧强度或法向预紧	§5.6, §5.2
物体平移正常、一旋转就发散/翻转	抓取矩阵下块 \(\widehat r_i\) 符号错 / \(r_i\) 方向反	1. 单元测试 `skew(r)*v==r.cross(v)` 2. 确认 \(r_i=\) 接触点\(-\)质心 3. 用纯平移测试通过、纯旋转测试定位	§5.1
力分配 QP 报 infeasible	物理不可行（物体太重/摩擦太小/布局差）	1. 临时放松 \(f_{\max},\mu\) 看是否变可行 2. 检查接触点是否撑开（\(r_i\) 几何）3. 降低运动加速度（减小 \(w_{\text{ext}}\)）4. 加机器人	§5.6
follower 力方向剧烈抖动	用未滤波的物体速度算方向，低速时 0/0 病态	1. 对物体速度低通滤波 2. 速度模低于阈值设死区/冻结方向 3. 用观测器估计平滑速度	§5.3
加机器人后系统低频振荡/发散	无源性被破坏（新控制器阻尼不足/变阻抗产能/通信延迟）	1. 验证新控制器无源（写储能函数查 \(\dot V\le u^\top y\)）2. 变阻抗是否挂能量罐 3. 有通信延迟是否用波变量 4. 增大阻尼 \(D_d\)	§5.7
去中心化搬运物体原地打转/乱走	各 agent "同构控制律"坐标系未对齐	1. 确认导航方向在共同参考系定义 2. 检查各接触力在世界系是否真同向 3. 验证顺从分量未被关闭	§5.4
物体被抬起后持续下坠/稳态低于期望	漏重力旋量补偿	1. 检查 \(w_{\text{ext}}\) 是否含 \(-w_{\text{grav}}\) 2. 悬停时验证接触力竖直分量之和 ≈ 物体重	§5.5
变阻抗策略一调刚度就失稳	刚度增大注入能量，破坏无源性	1. 确认能量罐 \(T_{\min}>0\) 且罐空时冻结产能 2. 检查回收的耗散能量符号/来源正确 3. 限幅 \(T_{\max}\)	§5.7

API 速查表¶

函数 / 类	签名	说明	节
`skew`	`Matrix3d skew(const Vector3d& r)`	反对称矩阵，满足 `skew(r)*v == r.cross(v)`	§5.1
`buildGraspMatrix`	`MatrixXd buildGraspMatrix(const vector<Vector3d>& pts, const Vector3d& com)`	组装抓取矩阵 \(G\in\mathbb{R}^{6\times 3N}\)（点接触）	§5.1
`computeDecomposition`	`ForceDecomposition computeDecomposition(const MatrixXd& G)`	算 \(G^+\)、零空间投影 \(P_N\)、内力基 \(E\)	§5.2
`composeContactForces`	`VectorXd composeContactForces(const ForceDecomposition&, const VectorXd& w_ext, const VectorXd& lambda)`	合成接触力 \(f=G^+w_{\text{ext}}+E\lambda\)	§5.2
`objectImpedanceStep`	`VectorXd objectImpedanceStep(params, G, x_err, xd_err, xdd_des, w_env, w_grav, lambda_int)`	一拍物体阻抗，输出各接触力	§5.5
`solveLoadDistribution`	`VectorXd solveLoadDistribution(const MatrixXd& G, const Vector6d& w_ext, const vector<ContactConstraint>&, const VectorXd& weights)`	摩擦锥/单边/力限下的负载分配 QP	§5.6
`EnergyTank::regulate`	`double regulate(double dissipated, double requested_inject)`	能量罐放行阀，返回允许注入的能量	§5.7
`osqp` (Python)	`osqp.OSQP().setup(P,q,A,l,u); .solve()`	QP 求解器，支持热启动	§5.6
`mujoco` (Python)	`MjModel.from_xml_path; MjData; mj_step`	MuJoCo 仿真，搭双 Go2 搬运场景	§5.8

核心公式速查：

公式	表达式	节
抓取矩阵力映射	\(w_o = Gf\)，\(G_i=[I_3;\widehat r_i]\)	§5.1
速度对偶	\(\dot X = G^\top v_o\)	§5.1
内力维度	\(\dim\ker G = cN-6\)	§5.1
内力/外力分解	\(f = G^+w_{\text{ext}} + (I-G^+G)\eta = G^+w_{\text{ext}}+E\lambda\)	§5.2
右伪逆	\(G^+ = G^\top(GG^\top)^{-1}\)	§5.2
物体阻抗	\(M_d\ddot{\tilde x}+D_d\dot{\tilde x}+K_d\tilde x=w_{\text{env}}\)	§5.5
所需合旋量	\(w_{\text{ext}}=M_o\ddot x^{\text{des}}+C_o-w_{\text{grav}}-w_{\text{env}}\)	§5.5
负载分配 QP	\(\min\tfrac12 f^\top Wf+g^\top f\) s.t. \(Gf=w_{\text{ext}},Af\le b\)	§5.6
摩擦金字塔	\(\pm t_{i,j}^\top f_i\le\mu_i n_i^\top f_i\)	§5.6
无源性	\(\dot V\le u^\top y\)	§5.7
波变量	\(u=\tfrac{1}{\sqrt{2b}}(F+b\dot x),\ v=\tfrac{1}{\sqrt{2b}}(F-b\dot x)\)	§5.7

研究实践建议¶

入门（建立直觉，1-2 周）：先吃透 §5.1-5.2 的抓取矩阵与内力分解——这是所有内容的地基。动手做：用 Python 实现抓取矩阵、验证零空间维度、可视化三机器人的内力基。把"内力 = \(\ker G\)"这个图像刻进脑子。然后读 §D03 双臂力控，体会 \(N{=}2\) 特例与多机一般情形的连续性。

进阶（掌握控制，3-4 周）：实现 §5.5 物体阻抗 + §5.6 负载分配 QP，在 MuJoCo 双 Go2 上跑通累积项目 Phase 1-2。重点体会"物体阻抗 vs 各末端阻抗"的差异、"伪逆 vs QP"在可行性上的差异。读 Wimböck-Ott 2012 理解物体级控制的统一框架。

研究级（理论深入 + 前沿，持续）：啃透 §5.7 无源性——这是把控制律焊成稳定系统的理论基石，也是区分"会调参"和"懂原理"的分水岭。读 Verginis 2019（刚度理论视角）、Niemeyer-Slotine（波变量）原文。关注三个开放问题：动态搬运的内力优化、学习型搬运的稳定性保证、柔性物体搬运。decPLM 是切入"学习 + 搬运"的好起点，但务必用本章理论去解读它学到了什么、缺什么保证。

选型决策框架：拿到一个多机搬运问题，按序问：(1) 接触模型是什么（点接触/刚性抓持/绳索/单边）？→ 定 \(G\) 与内力维度。(2) 能否拿到全局信息、\(N\) 多大、精度要求多高？→ 选集中/主从/去中心化架构。(3) 物体动力学是否已知？→ 基于模型（物体阻抗）/ 自适应（Verginis）/ 学习（decPLM）。(4) 接触有约束吗？→ 用 QP（摩擦锥/单边）。(5) 有变阻抗或通信延迟吗？→ 加能量罐 / 波变量保无源。

版本信息速查¶

项目	版本 / 说明
抓取矩阵约定	\(w_o=Gf\)（力映射），与 §D03 双臂力控一致；速度对偶 \(\dot X=G^\top v_o\)
默认接触模型	点接触带摩擦（每点 3 维力，\(G\in\mathbb{R}^{6\times 3N}\)）；双臂处切换刚性抓持（6 维）
Eigen	3.4+（`block`、`ldlt`、`JacobiSVD`、`unitOrthogonal`）
QP 求解器	OSQP 1.0+ / qpOASES 3.2+（负载分配 QP）
MuJoCo	3.x（Python 绑定，双 Go2 搬运仿真）
IsaacLab	decPLM / MQE 多四足训练环境（前沿复现）
关键论文年份	Orin-Oh 1986、Walker 1988、Khatib 2000、Wimböck-Ott 2012、Verginis 2019、Culbertson-Schwager 2020、Sombolestan 2022、PACC 2024、decPLM 2025

第 5 章 协同搬运与力控——抓取矩阵、内力分配与协调阻抗¶

前置自测¶

本章目标¶

本章知识导航¶

前置知识桥接¶

如果跳过本章会怎样¶

预计阅读时间¶

科研发展脉络¶

本章符号约定¶

§5.1 闭链约束与抓取矩阵 ⭐⭐⭐¶

动机：两台机器人抬一根钢梁¶

如果不建立抓取矩阵会怎样¶

历史：从闭链力分配到抓取矩阵¶

理论：抓取矩阵的推导¶

第一步：静力学——接触力如何合成物体旋量¶

第二步：运动学对偶——物体速度如何分发到接触点速度¶

为什么力映射和速度映射由同一个 \(G\) 联系？——对偶性¶

零空间维度：为什么恒等于 \(3N-6\)¶

推广：从点接触到刚性抓持¶

代码实现：抓取矩阵的 Eigen 实现¶

理论-工程桥接：抓取矩阵在多足搬运里长什么样¶

⚠️ §5.1 常见陷阱¶

§5.1 练习¶

§5.2 内力与外力分解 ⭐⭐⭐¶

动机：同一个搬运，无穷多种用力方式¶

如果不区分内外力会怎样¶

历史：从伪逆解到非挤压分配¶

理论：内力/外力分解的推导¶

分解公式¶

内力为什么"不影响运动"——再确认¶

内力子空间的基：把 \((3N-6)\) 维内力参数化¶

Moore-Penrose 解为什么"挤压"——Walker 的洞察¶

代码实现：内力/外力分解与内力基¶

验证正交性（Python 辅助）¶

理论-工程桥接：内力调节在真机里就是"夹紧力控制"¶

⚠️ §5.2 常见陷阱¶

§5.2 练习¶

§5.3 leader-follower 力协调 ⭐⭐⭐¶

动机：搬一个长沙发上楼¶

如果不用 leader-follower（全部机器人都当 leader）会怎样¶

历史：从遥操作主从到协作搬运¶

理论：leader 与 follower 的控制律¶

leader：用阻抗领航¶

follower：估计意图并配合¶

意图估计：follower 怎么"读"出 leader 想法¶

信息流对比：leader-follower vs 全集中¶

理论-工程桥接：Force-ANTS 的力放大在搬重物时的实际意义¶

⚠️ §5.3 常见陷阱¶

§5.3 练习¶

§5.4 去中心化力控：物体即共享黑板 ⭐⭐⭐¶

动机：蚂蚁搬食物¶

如果坚持用显式通信会怎样¶

历史：从行为群体到隐式通信¶

理论：去中心化控制律与隐式通信¶

同构本地控制律¶

隐式通信：物体如何传递信息¶

收敛性的直觉¶

三种架构总对比¶

理论-工程桥接：decPLM 的零通信为何可行¶

⚠️ §5.4 常见陷阱¶

§5.4 练习¶

§5.5 协调阻抗与物体阻抗 ⭐⭐⭐¶

动机：物体撞到障碍时，谁该"软"下来¶

如果用各末端独立阻抗会怎样¶

历史：从对称构型到物体级阻抗¶

理论：物体阻抗控制律¶

期望物体阻抗行为¶

求所需物体合力旋量¶

完整管线：物体阻抗 → 接触力¶

内力阻抗：连内力也可以是柔顺的¶

代码实现：物体阻抗控制器骨架¶

理论-工程桥接：为什么 DLR 的多指手和双臂用同一套物体阻抗¶

⚠️ §5.5 常见陷阱¶

§5.5 练习¶

§5.6 负载分配优化：摩擦锥下的 QP ⭐⭐⭐¶

动机：伪逆给出的力，机器人做不到¶

如果只用伪逆不加约束会怎样¶

历史：从力分配到接触力优化¶

理论：负载分配 QP 的构造¶

决策变量与目标¶

第 5 章　协同搬运与力控——抓取矩阵、内力分配与协调阻抗¶