第 6 章　异构多机协同——能力互补、空地联合与跨模态融合¶

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

一句话定位：前五章里所有机器人都被默认成"同一种东西"——同样的动力学、同样的传感器、同样的动作空间。本章把这个假设拆掉：当队伍里同时有"看得远的无人机、搬得重的四足、抓得准的机械臂"时，协同问题的每一层（任务分配、编队、感知、控制、学习）都要重写。本章完整讲透**异构性到底异构在哪、为什么异构系统不是同构系统的简单拼接、以及怎样把能力差异从负担变成红利**——从能力互补矩阵的形式化，到空地联合（UAV-UGV）的任务分配与跨模态定位，到异构编队的阻抗引导，再到 HAPPO 如何处理维度不一致的动作空间。

前置自测¶

开始前先回答下面 5 个问题。答不出 2 题以上，建议先回前置章节补齐——本章的每一节都建立在前五章的协同基础之上，本章只负责把"同构"假设替换成"异构"，不重新教共识、CBBA、MPC、grasp matrix 这些底层工具。

（接第 3 章）CBBA 的代价函数 $c_{ij}$ 表示什么？ 标准 CBBA 假设所有 agent 是同质的——如果某个任务只有会飞的机器人才能做，你打算怎么在 $c_{ij}$ 里表达"地面机器人根本做不了这个任务"？把代价设成一个很大的正数和设成 $-\infty$（或直接从可行集里剔除）有什么本质区别？（答不出 → 回第 3 章 §3.2 CBBA）
（接第 2 章）共识 $\dot{x} = -Lx$ 收敛到什么？ 它要求所有 agent 维护"同样维度、同样语义"的状态变量 $x_i$。如果队伍里无人机维护的是 4 维位姿、四足维护的是 12 维关节状态，这个共识协议还能直接跑吗？问题出在哪？（答不出 → 回第 2 章 §2.1 共识基础）
（接第 4 章）分布式 MPC 编队靠什么把各单体的局部优化耦合起来？ 第 4 章的编队里所有机器人共享同一个动力学模型模板（只是参数不同）。如果一个 agent 是二阶积分点质量、另一个是欠驱动的四旋翼（不能瞬间侧移），ADMM 的"邻居一致性约束"还能写成简单的相对位置相等吗？（答不出 → 回第 4 章 §4.2 分布式 MPC）
（接第 5 章）grasp matrix $G$ 把各接触点的力映射到物体合力旋量。 协同搬运里第 5 章默认每个机器人在接触点能施加的力集合（摩擦锥）是一样的。如果一只手是吸盘（只能拉）、一只手是夹爪（能推能拉）、一架无人机用绳索悬挂（只能沿绳方向拉），它们的力可行集（wrench cone）一样吗？这对内力分配意味着什么？（答不出 → 回第 5 章 §5.3 内力分配）
微分平坦（Differential Flatness）让四旋翼的规划在 4 维平坦输出空间里做。 一个差速驱动地面机器人（unicycle）的运动学约束是"不能横向移动"（非完整约束）。无人机和地面车的"可达运动集"在结构上有什么根本不同？为什么不能用同一条参考轨迹同时喂给二者？（答不出 → 回第 4 章移动机器人运动学 / 无人机微分平坦）

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

$c_{ij}$ 是机器人 $i$ 执行任务 $j$ 的代价（负效用）。表达"做不了"的正确方式是把 $(i,j)$ 从可行分配集合里**硬剔除**（即令任务 $j$ 的候选机器人集合只含具备所需能力者），而不是设一个大正数——大正数是"软"惩罚，优化器在极端情况下（所有别的选择都更糟）仍可能选它，且会污染拍卖的出价数值、破坏 CBBA 的收敛性质；$-\infty$/剔除是"硬"约束，从根本上保证不可行分配永不出现。这正是 §6.2 异构 CBBA 的核心改动。
共识收敛到初值的加权平均（双随机权重下是算术平均）。它的前提是所有 $x_i$ 同构——同维度、同语义、可逐分量比较和平均。无人机的 4 维位姿和四足的 12 维关节状态既不同维也不同义，对它们直接做 $-Lx$ 没有意义（"平均一个位姿和一组关节角"是无定义操作）。异构系统的共识必须先把异构状态投影到一个**共享的低维语义空间**（如机体在世界系下的 2D/3D 位置），只在这个公共子空间上做一致性——这是 §6.4 异构编队和 §6.5 跨模态融合都要处理的问题。
靠 ADMM 的对偶变量协调"重叠决策变量"的一致性，本质是惩罚邻居对同一物理量（如相对位置）的不一致估计。当动力学异构时，一致性约束不能再写成"相对位置恒等"——四旋翼有姿态-位置耦合、地面车有非完整约束，可达集形状不同。正确做法是在**任务空间/输出空间**（如编队几何）上写一致性，让各 agent 用各自的动力学模型去实现同一个输出目标——这是 §6.4 的核心。
不一样。吸盘的力可行集是"沿法向只能拉"的半空间（单边约束），夹爪是双边摩擦锥，绳索悬挂是"只能沿绳方向拉且力为正"的射线。它们的并集构成的物体可施加 wrench 空间是各 agent 可行集的 Minkowski 和——异构接触意味着内力分配要在**形状各异的可行锥**上做投影，不能用第 5 章的"对称均分"。这是 §6.6 异构协同操作的难点。
四旋翼是微分平坦系统，其平坦输出（位置+偏航）的任意光滑曲线都可执行——可达运动集在平坦输出空间里"无约束"（只受动力学边界限制）。差速车有非完整约束，瞬时速度方向被锁定在车头朝向，可达运动集是"亏秩"的（不能任意横移）。一条对无人机可行的轨迹（含横向平移段）喂给差速车会直接违反运动学约束。异构规划必须为每类机体生成**各自运动学可行**的轨迹，再在更高层（任务/几何）协调——这是贯穿 §6.4、§6.7 的主线。

本章目标¶

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

形式化异构性：用"能力维度 / 动力学维度 / 感知维度"三轴拆解一支机器人队伍的异构性，写出**能力互补矩阵（capability complementarity matrix）与**特质向量（trait vector），并解释 cumulative trait（可累加能力，如总负载）与 non-cumulative trait（不可累加能力，如最高飞行高度）的区别为什么决定了任务能否由联盟（coalition）拼出来。
从同构 CBBA 推导异构 CBBA：把第 3 章的 CBBA 扩展到能力约束、联合任务（coalition task）与能量约束三种异构因素，说明为什么"硬剔除不可行分配"优于"软惩罚"，并解释引入联合任务后为什么 DMG（边际收益递减）条件可能被破坏、50% 最优界因此可能失效。
设计空地联合（UAV-UGV）系统：讲清空地异构系统（Air-Ground Heterogeneous System, AGHS）的三类通信架构（中继 / mesh / leader-follower）各自的单点故障特性，并能为搜救、巡检、物流三类场景选对架构。
实现跨模态信息融合：说明无人机鸟瞰视角与地面机器人第一视角之间的**视角鸿沟（viewpoint gap）**与**模态鸿沟（modality gap）**为什么让简单的位姿广播失效，掌握用 2.5D 高程图配准、深度描述子检索、半分布式相对定位三条技术路线把异构感知拼成一致世界模型。
构建异构编队：实现 leader-follower + 改进人工势场（APF）的异构编队，理解 HetSwarm 的**阻抗引导（impedance-based guidance）**为什么能让地面机器人忽略"无人机能飞过但地面车要绕开"的矮障碍，并解释异构编队的一致性为什么必须建在公共输出空间而非状态空间。
理解异构 MARL：解释 HAPPO 如何用**多智能体优势分解（multi-agent advantage decomposition）+ 顺序更新（sequential update）**处理不同 agent 维度/语义不同的动作空间，说明它为什么必须用 No-Parameter-Sharing（NoPS）才能保证单调改进、以及由此带来的样本效率代价。
辨析异构与同构的边界：用一张总表说清"异构系统不是同构系统的并集"——分配、编队、感知、控制、学习五个层面各自因异构而产生的**新问题**，并能为一个真实的"四足 + 无人机"联合场景设计端到端方案，判断哪些前五章的工具可直接复用、哪些必须改写。

本章知识导航¶

本章的知识结构是一棵以"如何让能力各异的机器人高效协同"为根的树。树干是一条认知主线：先定义异构（§6.1）→ 再看异构如何改写每一层协同（分配 §6.2、通信 §6.3、编队 §6.4、感知 §6.5、操作 §6.6、学习 §6.7）→ 最后归纳异构与同构的本质边界（§6.8）。

                  什么是异构？(§6.1 异构性的三轴定义 + 能力互补矩阵)
                              │
        ┌─────────────┬───────┼───────┬─────────────┬──────────────┐
        ▼             ▼       ▼       ▼             ▼              ▼
   异构任务分配    异构通信   异构编队  跨模态感知    异构操作      异构学习
   (§6.2 能力约束  (§6.3 中继 (§6.4    (§6.5 视角/   (§6.6 异构   (§6.7 HAPPO
    CBBA+联盟)     /mesh/LF)  阻抗引导) 模态鸿沟融合) wrench cone) 动作空间)
        │             │       │       │             │              │
        └─────────────┴───────┴───────┴─────────────┴──────────────┘
                              │
                              ▼
                  异构 vs 同构的本质边界 (§6.8 五层对比总表 + 选型)
                              │
                              ▼
                  端到端案例：四足 + 无人机联合搜救 (§6.9)

小节	主题	难度	一句话
§6.1	异构性的三轴定义与能力互补矩阵	⭐⭐	能力/动力学/感知三轴；trait 向量；cumulative vs non-cumulative
§6.2	异构任务分配：能力感知的 CBBA 扩展	⭐⭐⭐	能力硬约束 + 联盟任务 + 能量约束；50% 界何时失效
§6.3	空地联合通信架构	⭐⭐	中继/mesh/leader-follower 的单点故障谱
§6.4	异构编队：leader-follower 与阻抗引导	⭐⭐⭐	公共输出空间一致性；HetSwarm 阻抗链；改进 APF
§6.5	跨模态信息融合	⭐⭐⭐⭐	视角鸿沟 + 模态鸿沟；2.5D 配准；半分布式相对定位
§6.6	异构协同操作	⭐⭐⭐	异构 wrench cone；不对称内力分配；空中-地面联合抓取
§6.7	异构 MARL：HAPPO 与异构动作空间	⭐⭐⭐⭐	优势分解 + 顺序更新；NoPS vs ParPS；维度不一致
§6.8	异构 vs 同构的本质边界	⭐⭐⭐	五层对比总表；分层协同架构；选型决策
§6.9	端到端案例与累积项目	⭐⭐	四足+无人机搜救全栈；Mini-MultiBot 异构模块

两条阅读线：

系统线（搭一个异构系统）：§6.1 → §6.2 → §6.3 → §6.4 → §6.9，重点是把分配、通信、编队、落地串成一条可运行的链。
算法线（吃透异构带来的新数学）：§6.1 → §6.2 → §6.5 → §6.7 → §6.8，重点是异构在分配、感知、学习三处引入的、同构系统里不存在的新问题。

无论哪条线，§6.1 都是必读——它定义的"三轴异构性"和"能力互补矩阵"是后面每一节的共同语言。§6.5 和 §6.7 是两个 ⭐⭐⭐⭐ 的硬核小节，是本章真正区别于"同构多机"的地方，值得花最多时间。

前置知识桥接¶

本章紧接 Part 1（多机基础）和 Part 2（协同运动），这里把最关键的前置点重新激活——你不必翻回去就能跟上：

回顾第 1 章：三大架构与通信图。第 1 章我们把多机系统分成集中式 / 分布式 / 去中心化三种架构，并用图 $G=(V,E)$ 建模"谁能和谁通信"。本章 §6.3 的空地通信架构是这三种架构在**异构节点**上的具体化——无人机和地面车的通信能力、续航、视野都不同，架构选择因此多了一层"哪类节点适合当中继 / 当 leader"的考量。第 1 章给的是同质节点的拓扑，本章给的是异质节点的拓扑。
回顾第 2 章：共识作为分布式裁决的原子操作。第 2 章证明了共识 $\dot{x}=-Lx$ 能让各 agent 只靠邻居通信收敛到一致（双随机权重下收敛到平均）。本章会两次用到它，但都要先做一步"投影"：§6.2 的异构 CBBA 冲突消解仍用最大值共识（这部分异构不影响——出价是标量），而 §6.4 异构编队的几何一致性必须先把异构状态投影到公共位置空间再做共识。异构性不改变共识的数学，但改变了"对什么做共识"。
回顾第 3 章：CBBA = 分布式拍卖 + 共识。第 3 章的 CBBA 分两阶段：本地 bundle 构建（每个 agent 贪婪地往自己的任务包里加边际收益最高的任务）+ 基于共识的冲突消解（带时间戳的最大值共识裁决谁中标）。它的 50% 最优界依赖**边际收益递减（DMG）条件。本章 §6.2 在这个骨架上加三样东西：能力硬约束（改候选集）、联盟任务（改 bundle 的原子单位）、能量约束（改代价函数）。**你会看到 CBBA 的两阶段结构原封不动，变的是喂给它的问题定义。
回顾第 4 章：分布式 MPC 编队。第 4 章让一支机器人编队用 ADMM 把"各自的单体 MPC"耦合成"保持队形的群体运动"，一致性约束写在相对位置上。本章 §6.4 指出：当动力学异构时，一致性不能再写在状态空间（不同 agent 状态不同维），必须上移到**输出空间 / 任务空间**（编队几何）。第 4 章是同构动力学的编队，本章是异构动力学的编队。
回顾第 5 章：grasp matrix 与内力分配。第 5 章用 grasp matrix $G$ 把各接触点的力映射到物体合力旋量，内力（internal wrench）落在 $G$ 的零空间里，第 5 章默认各 agent 的力可行集对称、内力均分。本章 §6.6 指出：异构接触（吸盘 / 夹爪 / 绳索悬挂）的力可行集形状各异，内力分配要在不对称的 wrench cone 上做投影。第 5 章是同构抓取的力分配，本章是异构抓取的力分配。

前向预告：本章是 Part 3（多机协同操作）之前承上启下的一章——它把前五章建立的"同构协同工具箱"系统地异构化。再往后，第 7-9 章（双臂/多臂协同、多足 loco-manipulation、人形+四足+臂混合体）是异构协同在**操作任务**上的深化，第 10-13 章（MARL 系列与 Mini-MultiBot 实战）则会把本章 §6.7 的异构 MARL 展开成完整的训练管线。现在只需要记住：异构不是某一章的专题，而是真实多机系统的常态——本章是从"同构理想"走向"异构现实"的转折点。

如果跳过本章会怎样¶

跳过第 6 章，你会在三个具体的地方栽跟头。

场景一（"我把 CBBA 的代价设成大正数，结果无人机被派去搬重物了"）。 你想复用第 3 章的 CBBA 处理一个"3 无人机 + 2 地面车"的混合队伍。为了表达"无人机搬不动重物"，你顺手把对应的 $c_{ij}$ 设成 $10^6$。大多数时候没问题，但在某次实例里所有地面车都已经满载，优化器为了"完成所有任务"硬是把搬运任务派给了无人机——因为 $10^6$ 虽然大，但仍是有限值，优化器认为"派一个超贵的总比不完成强"。§6.2 会告诉你：能力约束必须是**硬剔除**而非软惩罚，否则会出现这种"物理上不可能但数学上被选中"的灾难。

场景二（"无人机和地面车各自都定位得很准，拼在一起却差了两米"）。 你让无人机和地面车各自跑 SLAM，然后把各自的位姿广播出来，想直接拼成一张共享地图。结果两张地图错位严重——无人机的鸟瞰特征（屋顶、道路纹理）和地面车的第一视角特征（墙面、家具）几乎没有共视，回环检测失效，两个独立坐标系无法对齐。§6.5 会告诉你：跨模态、跨视角的异构感知存在**视角鸿沟**和**模态鸿沟**，简单的位姿广播解决不了坐标系对齐——你需要 2.5D 高程图配准、深度描述子或半分布式相对定位这类专门技术。

场景三（"我想用一个 MAPPO 同时训无人机和四足，动作维度对不上直接报错"）。 你打算用学过的 MARL 算法训一个异构团队，无人机 4 维控制、四足 12 维关节，你试图共享一个策略网络（参数共享），结果发现动作空间维度不一致，连网络都搭不起来；勉强 padding 到同一维度后，训练完全不收敛。§6.7 会告诉你：异构动作空间是 MARL 的真问题，HAPPO 用顺序更新 + 独立策略（NoPS）来处理，但代价是放弃参数共享、样本效率下降——这背后有一个"参数共享 vs 单调改进"的根本张力。

预计阅读时间¶

模式	时长	适合
精读	14-18 小时	第一次系统学异构多机协同的工程师：逐节读动机→推导→代码，亲手实现异构 CBBA、跑通 leader-follower 异构编队、推一遍 HAPPO 的优势分解，做完每节练习。建议分 5-6 次。
速读	4-6 小时	已有同构多机背景、想建立异构系统的全局图景：读每节的"动机""理论"小标题、能力互补矩阵、§6.8 五层对比总表、§6.9 端到端案例，跳过代码细节与公式推导。
速查	50-70 分钟	已学过、回来查特定内容：直接定位小节，看能力互补矩阵 + 异构 CBBA 三处改动 + 通信架构对比 + 跨模态融合三路线 + HAPPO 顺序更新 + 五层边界总表 + 故障排查。

科研发展脉络¶

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

年份	论文	Venue	核心贡献
2004	Gerkey & Matarić, "A Formal Analysis and Taxonomy of Task Allocation in Multi-Robot Systems"	IJRR	分类奠基：ST/MT、SR/MR、IA/TA 三维分类，首次让"异构任务能力"可被形式化讨论
2009	Choi, Brunet, How, "Consensus-Based Decentralized Auctions for Robust Task Allocation"	IEEE T-RO	CBBA：分布式拍卖 + 共识，50% 界；本章 §6.2 异构扩展的同构起点
2014	Michael 等, "Cooperative Manipulation and Transportation with Aerial Robots"	Auton. Robots	空中协同搬运先驱：多四旋翼绳索悬挂联合搬运，异构（飞行体+被搬物）操作的起点
2019	Ravichandar 等, "STRATA: A Unified Framework for Task Assignments in Large Teams of Heterogeneous Robots"	ICRA	特质化分配：robot→species、trait 连续向量、cumulative vs non-cumulative trait；能力感知分配的统一框架
2019	Tognon 等, "A Truly-Redundant Aerial Manipulator System..."	RA-L	空中冗余操作：飞行基座的冗余控制，异构操作的精度路线
2022	Kuba/Zhong 等, "Heterogeneous-Agent Reinforcement Learning (HAPPO/HATRPO)"	JMLR / ICLR	异构 MARL 奠基：多智能体优势分解 + 顺序更新，处理异构动作空间，单调改进保证
2023	Mandi 等, "RoCo: Dialectic Multi-Robot Collaboration with Large Language Models"	ICRA 2024	LLM 驱动异构协调：每个机器人一个 LLM agent 对话式分工，按"并行/串行、信息对称性、邻近度"三性质组织协作
2024	MDPI/Elsevier Survey, "Air-Ground Cooperative Systems: A Survey and Taxonomy"	Machines / RAS	空地联合综述：AGHS 分类，UAV-UGV 协作的感知-规划-控制全栈梳理
2025	HetSwarm, "Cooperative Navigation of Heterogeneous Swarm... through Impedance-based Guidance"	arXiv	异构编队阻抗引导：UAV 用 APF 引领、UGV 经阻抗链跟随并对矮障碍建临时阻抗，异构可达集差异的优雅处理
2025	Pandit 等, "Learning Decentralized Multi-Biped Control for Payload Transport"	CoRL	异构身体涌现协调：多人形搬运，去中心化策略下异构形态的协调涌现
2025	"Semi-distributed Cross-modal Air-Ground Relative Localization"	arXiv	跨模态相对定位：UGV/UAV 各自 SLAM + 深度描述子，相对定位与各自状态估计解耦

关键脉络：这条线有两条交织的主干。

第一条是**任务层**：Gerkey-Matarić（2004，让异构能力可被分类）→ CBBA（2009，分布式分配的同构骨架）→ STRATA（2019，把异构能力抽象成连续 trait 向量、区分可累加性）→ RoCo（2023，用 LLM 把异构能力的分工提到语义层）。主线是**异构能力的表达从"离散类型标签"走向"连续特质向量"再走向"自然语言语义"**，每一步都让"谁适合干什么"的刻画更精细。

第二条是**本体层**：Michael（2014，空中协同搬运）→ Tognon（2019，空中冗余操作）→ HAPPO（2022，异构动作空间的学习）→ AGHS 综述（2024，空地全栈）→ HetSwarm（2025，异构编队）→ 跨模态定位（2025，异构感知）。主线是**从"让异构本体能一起动"走向"让异构本体能一起感知、一起学习"**——协同的层次从执行层不断向感知层、决策层上移。

看这两条线，一个清晰的趋势是：异构性正在从"需要小心处理的麻烦"转变为"主动设计的红利"。早期工作把异构当约束（小心别让无人机搬重物），近期工作把异构当资源（正因为能力互补，才能完成单一机型做不到的任务）。本章 §6.1 的能力互补矩阵就是这个观念转变的形式化载体。

本章符号约定¶

符号	含义	首见
$\mathcal{R} = \{r_1, \dots, r_N\}$	机器人集合（异构，可分多个 species）	§6.1
$\mathcal{S} = \{s_1, \dots, s_K\}$	species（机型）集合，每个 species 内部同构	§6.1
$\mathbf{q}_i \in \mathbb{R}^U$	机器人 $i$ 的能力/特质向量（trait vector），$U$ 维	§6.1
$Q \in \mathbb{R}^{N \times U}$	能力矩阵（行=机器人，列=能力维度）	§6.1
$C^{\text{comp}}$	能力互补矩阵（capability complementarity matrix）	§6.1
$\mathbf{y}_j \in \mathbb{R}^U$	任务 $j$ 的能力需求向量	§6.1, §6.2
$a_i$	机器人 $i$ 的动作（异构：维度 $d_i$ 可不同）	§6.7
$c_{ij}$	机器人 $i$ 执行任务 $j$ 的代价	§6.2
$\mathcal{A}_j$	任务 $j$ 的合格机器人候选集（能力筛选后）	§6.2
$G$	grasp matrix（接触力→物体旋量）	§6.6
$\mathcal{W}_i$	机器人 $i$ 在接触点的可施加 wrench 锥	§6.6
$T_W^{B_i}$	机器人 $i$ 本体系到世界系的变换	§6.5
$A^{i_1:i_m}$	多智能体联合优势函数（HAPPO）	§6.7
$\pi^i$	智能体 $i$ 的独立策略（NoPS）	§6.7

§6.1 异构性的三轴定义与能力互补矩阵 ⭐⭐¶

这一节解决什么问题：在写任何异构协同算法之前，必须先回答一个看似简单却被普遍含糊处理的问题——"异构"到底指什么？ 两个机器人型号不同就叫异构吗？同型号但装了不同传感器呢？这一节给出一个可操作的三轴定义，并把"异构"从一个形容词变成一个可计算的对象——能力矩阵 $Q$ 和能力互补矩阵 $C^{\text{comp}}$。它是后面每一节的共同语言：§6.2 的分配在能力矩阵上做，§6.4 的编队要处理动力学轴的异构，§6.5 的融合要处理感知轴的异构。

动机：一个"显然"却没人说清的词¶

打开任何一篇多机器人论文，"异构（heterogeneous）"这个词出现的频率极高，但几乎没有两篇论文用的是同一个定义。有人说"我们的机器人是异构的"，指的是型号不同；有人指的是动力学不同；有人指的是传感器配置不同；还有人指的是它们跑不同的算法。这种含糊在写综述时无害，但当你要**实现**一个异构协同算法时，它会立刻变成一个具体的障碍：

你要写一个任务分配器。代码里需要一个数据结构来描述"机器人 $i$ 能做什么"。这个数据结构应该长什么样？是一个枚举类型（UAV / UGV / ARM）？还是一个能力布尔向量（[can_fly, can_carry_heavy, can_grasp]）？还是一个连续的数值向量（[max_payload=2.0, max_altitude=120, ...]）？

这三种选择对应三种完全不同的算法设计。如果你不先把"异构指什么"想清楚，你的数据结构会在写到第二个算法（比如从分配走到编队）时崩溃——因为分配关心的异构维度（能力）和编队关心的异构维度（动力学）根本不是一回事。

更糟的是，含糊的异构定义会让你**误判问题的难度**。一个常见的错误是："反正都是机器人，无非是参数不同，我把同构算法的参数改一改就行了。" 这句话对某些异构维度成立（比如最大速度不同，确实只是参数），但对另一些异构维度完全不成立（比如动作空间维度不同，这会让参数共享的神经网络根本搭不起来）。不区分"哪种异构只是参数差异、哪种异构是结构差异"，是异构系统设计中最致命的认知错误。

所以这一节的目标是：给"异构"一个分轴的、可计算的定义，让你在面对任何一支机器人队伍时，能准确说出"它在哪个轴上异构、异构到什么程度、这个异构会给哪一层协同带来什么具体麻烦"。

如果用最朴素的"型号标签"会怎样¶

最朴素的异构表示是给每个机器人贴一个**型号标签**：这是无人机、那是四足、那个是机械臂。代码里就是一个枚举。这种表示在最简单的演示里能用，但它有三个致命缺陷，每一个都会在工程中爆炸。

缺陷一：标签太粗，丢失能力的连续性。 "无人机"这个标签下，一台载重 0.2 kg 的微型四旋翼和一台载重 5 kg 的工业六旋翼天差地别，但它们共享同一个标签。如果你的分配器只看标签，它无法区分"这个搬运任务需要 3 kg 载重，微型机做不了但工业机可以"。能力本质上是**连续的数值**，型号标签把它压成了离散的类别，丢失了关键信息。

缺陷二：标签是封闭集，无法表达"能力的组合与缺失"。 真实机器人的能力不是型号决定的，而是配置决定的——同一台四足，装了机械臂就能操作、没装就不能；装了热成像就能夜间搜救、没装就不能。型号标签假设"型号 ⇒ 能力"是固定映射，但实际是"型号 + 配置 ⇒ 能力"。当你想表达"这台四足今天没带臂"时，枚举类型无能为力。

缺陷三：标签无法支持"能力累加"的推理。 异构协同最有价值的场景是**联盟（coalition）：单个机器人做不到的任务，由多个机器人的能力拼起来完成。比如"搬运一个 8 kg 的物体"，单台载重 5 kg 的机器人做不了，但两台一起可以——因为载重是可累加的。可是"飞到 120 米高度拍照"这个任务，两台最高只能飞 60 米的无人机加起来**也飞不到 120 米——因为高度不可累加。型号标签完全无法支持这种"哪些能力能拼、哪些不能拼"的推理，而这恰恰是异构联盟分配的核心。

本质洞察：型号标签的根本错误在于它把"机器人是什么"和"机器人能做什么"混为一谈。异构协同关心的从来不是"它是什么型号"，而是"它具备哪些能力、这些能力能否与他人的能力累加"。正确的异构表示应该以能力（capability/trait）为中心，而非以型号为中心——这正是 STRATA 框架（Ravichandar 等, ICRA 2019）的核心洞见，也是本节"能力矩阵"表示的出发点。

历史：从型号分类到特质向量¶

异构多机的表示方法经历了三代演进，每一代都在修复前一代的缺陷。

第一代：型号分类（2000s 早期）。 早期多机系统直接按物理型号组织，分配时用规则匹配（"侦察任务 → 派无人机"）。简单直接，但就是上面说的三个缺陷。

第二代：能力分类法（Gerkey & Matarić, IJRR 2004）。 Gerkey 和 Matarić 的多机任务分配（MRTA）分类法是第一次系统地把"任务对能力的需求"形式化。他们的三维分类——单任务/多任务机器人（ST/MT）、单机器人/多机器人任务（SR/MR）、瞬时/时间扩展分配（IA/TA）——让"异构能力"第一次有了讨论的框架。其中 SR/MR 这一维直接刻画了"任务能否由单机完成、还是必须多机协作"，这正是异构联盟的雏形。但这一代仍然把能力当作**离散的"有/无"**，没有连续的强度刻画。

第三代：连续特质向量（STRATA, Ravichandar 等, ICRA 2019）。 STRATA（Stochastic TRAit-based Task Assignment）做了关键的概念跃迁：把每个机器人的能力建模为一个连续的特质向量（trait vector），每一维是一个可量化的能力强度（载重 kg、续航 min、最高速度 m/s、传感器分辨率……）。更重要的是，STRATA 区分了两类特质：

可累加特质（cumulative trait）：多个机器人的该能力可以**相加**来满足任务需求。例如总载重——两台 5 kg 的机器人合起来能搬 10 kg。
不可累加特质（non-cumulative trait）：该能力**不能**通过多机相加来满足。例如最高飞行高度——两台 60 m 的无人机加起来仍然只能到 60 m；又如最高分辨率、最快单点速度。

这个区分看似简单，却是异构联盟分配能否正确工作的分水岭。STRATA 还借用了生物多样性的概念，把机器人按特质聚成**物种（species）——同一物种内部同构，物种之间异构。这给了我们一个干净的两层结构：**物种间异构、物种内同构，本章后面所有算法都建立在这个结构上。

注：本节的"三轴定义"是在 STRATA 的特质向量基础上的教学性扩展。STRATA 主要关注任务分配层的能力特质；本章把异构性扩展到三个轴（能力 / 动力学 / 感知），因为异构不只影响分配，还影响编队（动力学轴）和感知融合（感知轴）。这三个轴对应本章后面三组核心小节。

理论：异构性的三轴定义¶

我们把一支机器人队伍的异构性沿三个正交的轴来刻画。之所以是三个轴而不是一个笼统的"异构度"，是因为**不同轴的异构会给不同层的协同带来不同的麻烦**——把它们混在一起谈，就会犯"以为改改参数就行"的错误。

轴一：能力异构（capability heterogeneity）——影响"谁干什么"¶

能力异构指机器人**能完成的任务种类与强度**不同。这是最直观的异构，也是任务分配（§6.2）关心的唯一轴。我们用一个能力向量来刻画每个机器人：

\[ \mathbf{q}_i = (q_{i,1}, q_{i,2}, \dots, q_{i,U})^\top \in \mathbb{R}^U \]

其中 $U$ 是能力维度数，每一维 $q_{i,u}$ 是机器人 $i$ 在第 $u$ 种能力上的强度。把所有机器人的能力向量按行堆叠，得到**能力矩阵（capability matrix）**：

\[ Q = \begin{pmatrix} \mathbf{q}_1^\top \\ \mathbf{q}_2^\top \\ \vdots \\ \mathbf{q}_N^\top \end{pmatrix} \in \mathbb{R}^{N \times U}, \qquad Q_{iu} = q_{i,u} \]

行是机器人、列是能力维度。这个矩阵就是本章所有分配算法的输入。举一个具体的例子，一支"2 无人机 + 2 四足 + 1 机械臂"的队伍，取 $U=4$ 个能力维度（载重 kg / 鸟瞰感知范围 m / 操作精度 mm⁻¹ / 越障通过性 0-1）：

\[ Q = \begin{pmatrix} 0.5 & 150 & 0 & 1.0 \\ 0.5 & 150 & 0 & 1.0 \\ 30 & 20 & 2 & 0.8 \\ 30 & 20 & 2 & 0.8 \\ 50 & 5 & 100 & 0 \end{pmatrix} \begin{array}{l} \leftarrow \text{UAV}_1 \\ \leftarrow \text{UAV}_2 \\ \leftarrow \text{四足}_1 \\ \leftarrow \text{四足}_2 \\ \leftarrow \text{臂} \end{array} \]

读这个矩阵就能立刻看出能力互补的结构：无人机的感知范围列（150）远高于其他，机械臂的操作精度列（100）一骑绝尘，四足在载重和通过性上居中且均衡。异构性在这里变成了一个可计算、可比较、可优化的对象——这正是相比型号标签的根本进步。

每个任务也用一个**能力需求向量** $\mathbf{y}_j \in \mathbb{R}^U$ 描述，语义是"完成任务 $j$ 至少需要在每个能力维度上达到 $\mathbf{y}_j$ 的强度"。任务-能力匹配就退化为向量比较，下一节 §6.2 详述。

本质洞察：把能力写成向量、把队伍写成矩阵，最深刻的好处不是记法简洁，而是**它让"互补"这个直觉概念变得可度量**。两台机器人互补，数学上意味着它们的能力向量在不同维度上有峰值（"你强的地方我弱、我强的地方你弱"）；一支队伍能力全面，意味着能力矩阵的每一列都至少有一个大值。互补不再是定性的赞美，而是能力矩阵列覆盖性的定量性质。

轴二：动力学异构（dynamics heterogeneity）——影响"怎么动"¶

动力学异构指机器人的**运动模型在结构上**不同——不是参数不同（那只是同一模型的不同实例），而是**模型方程的形式、状态维度、约束类型**不同。这是编队（§6.4）和协同运动关心的轴，能力异构对它一无所知。

举三个结构上根本不同的运动模型：

机体	运动模型	状态维度	关键约束
二阶积分点质量	$\ddot{p} = u$	6（位置+速度）	无（全向）
差速驱动车（unicycle）	$\dot{x}=v\cos\theta,\ \dot{y}=v\sin\theta,\ \dot\theta=\omega$	3	非完整：不能横向移动
四旋翼（欠驱动）	$SE(3)$ 上的刚体 + 单向推力	12	欠驱动：水平加速度必须靠倾斜产生

这三者的差异不是"最大速度不同"这种参数差异，而是**可达运动集（reachable motion set）的结构差异**：

点质量的可达瞬时速度方向是各向同性的（任意方向都能动）；
差速车的可达瞬时速度被锁在车头朝向（一维，亏秩）；
四旋翼虽然位置上各向同性（微分平坦），但姿态和位置强耦合，且推力单向（不能"向下推"）。

这种结构差异的直接后果是：一条对某类机体可行的轨迹，喂给另一类可能直接违反约束。前置自测第 5 题就是这个点——含横向平移段的轨迹对四旋翼可行（它能侧飞，靠倾斜），对差速车不可行（非完整约束禁止横移）。这就是为什么 §6.4 说异构编队的一致性必须建在**输出空间**而非状态空间：不同 agent 的状态空间维度和约束都不同，没法直接比较，只能在它们共有的输出（如机体在世界系的位置）上对齐。

对比性思维（不是 X 而是 Y）：动力学异构**不是**"同一个动力学模型的不同参数"，而是"不同结构的动力学模型"。这个区分极其重要：参数异构可以用同一套控制器加增益调度（gain scheduling）处理——这其实还是同构控制的范畴；结构异构必须为每类机体设计各自的控制器，再在更高层协调。把结构异构误当参数异构，是异构编队失败的头号原因——你会发现一套精心调好的控制器在另一类机体上怎么都不稳定，因为它假设的运动模型根本不对。

轴三：感知异构（perception heterogeneity）——影响"看到什么"¶

感知异构指机器人的**传感模态、视角、坐标系**不同。这是跨模态融合（§6.5）关心的轴。它又可细分为三个子维度，每个都会让简单的"位姿广播 + 地图拼接"失效：

模态异构（modality）：无人机用下视相机、地面车用激光雷达、搜救机器人用热成像——传感器产生的数据类型根本不同（图像 vs 点云 vs 温度场），无法直接比对。
视角异构（viewpoint）：无人机鸟瞰、地面车平视。同一个物体在两个视角下的外观可能完全没有共视特征——屋顶（无人机看得到）和墙面（地面车看得到）是同一栋楼的不同面，深度学习的特征描述子在此处也对不上。
坐标系异构（frame）：每个机器人有自己的本体系 $B_i$ 和里程计原点，各自 SLAM 出来的地图在各自的坐标系里，没有共同的世界系基准。

这三个子维度叠加，构成 §6.5 要攻克的**视角鸿沟 + 模态鸿沟**。这里先埋一个种子：感知异构无法靠"让每个机器人把自己的位姿广播出去"解决——因为各自的位姿是在**各自的、未对齐的坐标系**里，广播出来的数字拼不到一起。

多视角理解（双重解读）：三轴异构可以从两个互补的角度理解，二者指向同一个工程结论。角度一（负担视角）：异构是麻烦——三个轴各自破坏了同构系统赖以工作的某个假设（能力轴破坏"谁都能干"、动力学轴破坏"状态可比"、感知轴破坏"观测可拼"），所以异构系统的每一层都要额外做"消解差异"的工作。角度二（资源视角）：异构是红利——正因为能力轴有差异，才有互补（无人机看得远、四足搬得重）；正因为动力学轴有差异，才能覆盖更广的运动模式（无人机飞越、四足越障）；正因为感知轴有差异，才能多模态互证（鸟瞰看全局、平视看细节）。这两个角度不矛盾——异构既是负担又是红利，工程的艺术就是用最小的"消解差异"代价换取最大的"能力互补"收益。本章每一节本质上都在做这个权衡：花多少力气消解差异（成本），换来多少协同能力（收益）。

三轴的正交性与耦合¶

这三个轴**在概念上正交**——一个机器人可以只在某一个轴上异构。例如：两台同型号无人机，一台装相机一台装激光雷达，则它们能力轴和动力学轴同构、仅感知轴异构。但在真实系统里三个轴往往**同时**异构（无人机 vs 四足在三个轴上全异构），这正是异构协同难的根源：你不能只解决一个轴的异构，三个轴的异构会在系统集成时相互纠缠——分配把任务派给了无人机（能力轴决策），但无人机的轨迹必须由它自己的动力学生成（动力学轴），而它感知到的目标位置又在它自己的坐标系里（感知轴），三者必须打通才能让任务真正落地。本章 §6.9 的端到端案例就是演示这三轴如何在一个系统里协同打通。

能力互补矩阵：把"互补"变成可计算的对象¶

有了能力矩阵 $Q$，我们可以进一步定义**能力互补矩阵（capability complementarity matrix）** $C^{\text{comp}}$，它度量队伍中任意两个机器人之间的互补程度。这是把"这两个机器人很互补"这句定性判断变成一个数的尝试。

一个机器人 pair $(i, k)$ 互补的直觉是：在某些能力维度上 $i$ 强 $k$ 弱、另一些维度上 $k$ 强 $i$ 弱——它们的能力峰值落在不同维度。我们用一个简单而有意义的度量：先把每个能力向量按列归一化（消除量纲差异，载重的 kg 和高度的 m 不能直接比），得到 $\hat{\mathbf{q}}_i$；互补度定义为"两者归一化能力在各维度上的'此消彼长'程度"。一种常用的定义是基于负相关：

\[ C^{\text{comp}}_{ik} = \frac{1}{U}\sum_{u=1}^{U} \big| \hat{q}_{i,u} - \hat{q}_{k,u} \big| \cdot \mathbb{1}[\text{维度 } u \text{ 上两者有显著差异}] \]

直观地说，$C^{\text{comp}}_{ik}$ 越大，表示 $i$ 和 $k$ 在能力上越互补（差异越大、越能填补彼此短板）；越小表示越同质（能力雷同、放在一起冗余）。注意它和"相似度"恰好相反——相似度高的两个机器人互补度低。

对比性思维（反事实）：如果一支队伍的能力互补矩阵 $C^{\text{comp}}$ 全是接近 0 的值会怎样？这意味着所有机器人能力雷同——这恰恰**退化回了同构系统**。同构系统就是"互补度处处为零"的异构系统的极限情形。反过来，互补矩阵的值越大、结构越丰富，队伍的异构红利越大——它能覆盖的任务种类越多。这给了我们一个判断"组建什么样的异构队伍"的定量指南：好的异构队伍不是机型越多越好，而是能力覆盖越全、互补结构越互不重叠越好。

下面用 Python 把能力矩阵和互补矩阵的构造实现出来，作为本章累积项目的第一块拼图。

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

为什么用 (N×U) 的稠密矩阵存能力，而不是给每个机器人一个能力枚举集合？

因为后面的分配、联盟、编队都要对能力做"数值运算"——
任务分配要比较"机器人能力 ≥ 任务需求"（向量逐分量比较）；
联盟拼接要把可累加能力"相加"（向量求和）；
互补度要算能力向量的"差异"（向量减法 + 范数）。
这些全是线性代数运算，稠密矩阵是最自然的载体，且能直接喂给 NumPy 向量化。

枚举集合（{can_fly, can_carry}）只能做集合运算（并、交），
无法表达"载重 5kg vs 30kg"这种强度差异，也无法做累加——
两个 can_carry 的并集还是 can_carry，丢失了"合起来能搬更重"的信息。

Step 2: 给出正确写法

import numpy as np

# 能力维度的语义（列）：定义清楚每一列是什么，避免后续串味
CAP_NAMES = ["payload_kg", "aerial_range_m", "precision_inv_mm", "traversability"]

# 是否可累加（cumulative）：决定联盟能否靠"相加"满足任务需求
#   payload 可累加（两机合搬更重）；aerial_range/precision/traversability 不可累加
CUMULATIVE = np.array([True, False, False, False])

# 能力矩阵 Q：行=机器人，列=能力维度（与 §6.1 正文的例子一致）
Q = np.array([
    [ 0.5, 150,   0, 1.0],   # UAV_1
    [ 0.5, 150,   0, 1.0],   # UAV_2
    [30.0,  20,   2, 0.8],   # quadruped_1
    [30.0,  20,   2, 0.8],   # quadruped_2
    [50.0,   5, 100, 0.0],   # arm
], dtype=float)

ROBOT_NAMES = ["UAV_1", "UAV_2", "Quad_1", "Quad_2", "Arm"]

def normalize_columns(Q):
    """按列归一化到 [0,1]，消除量纲差异。常数列（min==max）置 0.5 避免除零。"""
    col_min, col_max = Q.min(axis=0), Q.max(axis=0)
    span = col_max - col_min
    span[span == 0] = 1.0                 # 防止常数列除零
    Qn = (Q - col_min) / span
    Qn[:, (col_max - col_min) == 0] = 0.5 # 常数列无信息，取中性值
    return Qn

def complementarity_matrix(Q, sig_thresh=0.3):
    """能力互补矩阵 C_comp[i,k]：i、k 在各能力维度上的显著差异均值。
       值越大越互补；对角线（自己和自己）置 0。"""
    Qn = normalize_columns(Q)
    N, U = Qn.shape
    C = np.zeros((N, N))
    for i in range(N):
        for k in range(N):
            if i == k:
                continue
            diff = np.abs(Qn[i] - Qn[k])
            significant = diff > sig_thresh          # 只计显著差异的维度
            C[i, k] = np.sum(diff * significant) / U
    return C

C_comp = complementarity_matrix(Q)
print("能力互补矩阵 C_comp:\n", np.round(C_comp, 2))
# 期望观察：UAV 与 Arm 互补度最高（感知 vs 精度两个极端），
#           UAV_1 与 UAV_2 互补度为 0（完全同质，冗余）

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

# ❌ 错误 1：不归一化就直接算差异
def complementarity_WRONG_1(Q):
    N = Q.shape[0]
    C = np.zeros((N, N))
    for i in range(N):
        for k in range(N):
            C[i, k] = np.sum(np.abs(Q[i] - Q[k]))  # 未归一化！
    return C
# 问题：payload 列量级 ~50，aerial_range 列量级 ~150，precision 列 ~100，
#      traversability 列 ~1。不归一化时，互补度几乎完全由 aerial_range 主导，
#      traversability 维度的差异被淹没——结果毫无意义。
#      根本原因：异构能力的不同维度量纲天差地别，必须先归一化到可比尺度。

# ❌ 错误 2：把不可累加能力当可累加，用于联盟判断
def coalition_capability_WRONG(Q, members):
    return Q[members].sum(axis=0)   # 所有维度一律求和！
# 问题：对 aerial_range（不可累加）求和 = 把两台无人机的视野"相加"，
#      得出"两台 150m 视野的无人机合起来有 300m 视野"——物理上荒谬。
#      两台无人机一起看，覆盖面积可能更大，但单点最远感知距离仍是 150m。
#      正确做法见 Step 4：可累加维用 sum，不可累加维用 max。

# ❌ 错误 3：把能力矩阵的行列方向搞反
Q_wrong = Q.T   # 现在是 (U×N)，行变成了能力维度
# 问题：后续所有 "Q[i] = 第 i 个机器人的能力" 的代码全错位，
#      且不会报错（只要 N≠U 维度对不上才报错，N==U 时静默出错）。
#      自检：永远在注释里固定"行=机器人、列=能力"，并 assert Q.shape[0]==len(ROBOT_NAMES)

Step 4: 给出不同实现方式的对比（联盟能力的正确聚合）

def coalition_capability(Q, members, cumulative=CUMULATIVE):
    """正确的联盟能力聚合：可累加维求和，不可累加维取最大。
       这是异构联盟分配的核心运算——§6.2 会反复用到。"""
    sub = Q[members]                       # (len(members), U)
    agg = np.where(cumulative,
                   sub.sum(axis=0),        # 可累加：相加（如总载重）
                   sub.max(axis=0))        # 不可累加：取最强者（如最高视野）
    return agg

# 验证：两台四足组成联盟搬运
team = [2, 3]   # Quad_1 + Quad_2
print("联盟 {Quad_1, Quad_2} 的聚合能力:", coalition_capability(Q, team))
# payload: 30+30=60（可累加，两机合搬 60kg）✅
# aerial_range: max(20,20)=20（不可累加，视野不会因人多而变远）✅

# === 三种"联盟能力聚合"方式的对比 ===
# | 方式            | payload(可累加) | aerial_range(不可累加) | 正确性 |
# |-----------------|----------------|----------------------|--------|
# | 全部 sum        | 60 ✅          | 40 ❌（视野翻倍，荒谬） | 错     |
# | 全部 max        | 30 ❌（搬不动重物）| 20 ✅              | 错     |
# | 按 cumulative 分 | 60 ✅          | 20 ✅                | 对     |
# 结论：异构联盟聚合必须区分能力的可累加性，没有"一刀切"的正确做法。

这段代码里，coalition_capability 函数是整章的一个关键工具——它把"多个异构机器人合起来能做什么"这个问题，归约为一个区分可累加性的向量聚合。STRATA 框架的核心贡献正是形式化了这个 cumulative / non-cumulative 的区分，而这几行 np.where 就是它的最小实现。

⚠️ 常见陷阱¶

编程陷阱：不可累加能力被错误累加

错误做法：判断联盟能否完成任务时，对所有能力维度一律求和
现象：算法认为"两台最高飞 60m 的无人机组队能飞到 120m"，
     于是把"飞到 100m 拍照"的任务派给了两台 60m 无人机的联盟——
     执行时无人机怎么也飞不到 100m，任务永久失败
根本原因：把不可累加特质（non-cumulative trait，如最高高度、最高分辨率、
         最快单点速度）当成了可累加特质。可累加性是能力的内在属性，
         不是所有能力都满足"人多力量大"
正确做法：为每个能力维度标注 cumulative 布尔位，
         聚合时可累加维用 sum、不可累加维用 max（见 Step 4）
自检方法：对每个能力维度问一句"两个机器人一起做，这个指标会翻倍吗？"——
         载重会（搬两份），但最高速度/最高高度/最高精度都不会

概念误区："机型越多，异构系统能力越强"

错误理解：往队伍里多加几种机型，系统就更强大
现象：加了第三种、第四种机型，系统复杂度（通信、协调、维护）暴涨，
     但能完成的任务种类没有相应增加——因为新机型的能力和已有机型重叠
根本原因：异构红利来自能力的"覆盖性"和"互补性"，不是机型数量。
         如果新机型的能力向量与已有机型高度相似（互补矩阵该行该列接近 0），
         它只是增加了冗余，没有扩展能力边界
正确理解：判断"该不该加这种机型"要看它是否填补了能力矩阵某一列的空缺，
         或在某个关键能力上提供了更高的强度。
         能力覆盖全 + 互补结构丰富 > 机型数量多

思维陷阱：把"参数异构"当成"结构异构"，或反之

错误做法（之一）：看到两个机器人最大速度不同，就为它们设计完全不同的协同协议
错误做法（之二）：看到两个机器人都"会移动"，就用同一套轨迹喂给差速车和四旋翼
现象（之一）：过度设计——本来加个增益调度就行，却拆成两套独立架构，维护成本翻倍
现象（之二）：欠设计——同一条含横移段的轨迹喂给差速车，直接违反非完整约束，车走不了
根本原因：没区分"参数异构"（同一模型不同参数，如 max_v）和
         "结构异构"（不同模型，如全向 vs 非完整 vs 欠驱动）。
         参数异构是同构控制的范畴（增益调度即可），
         结构异构必须分别设计控制器再在高层协调
正确做法：面对异构队伍，先沿三轴判断每个轴是参数异构还是结构异构——
         能力轴几乎总是"强度差异"（可统一在能力矩阵里处理），
         动力学轴要看运动模型方程是否同形（同形=参数、异形=结构），
         感知轴要看模态/视角/坐标系是否同类

练习¶

（实现 + 分析） 用本节的 complementarity_matrix 计算正文那支"2 UAV + 2 四足 + 1 臂"队伍的完整 $5\times5$ 互补矩阵。然后回答：(a) 哪一对机器人互补度最高？为什么？(b) 如果预算只允许保留 3 台机器人，你会保留哪 3 台来最大化"能力列覆盖"？给出你的判据，并讨论"最大化互补度之和"和"最大化能力覆盖"这两个目标是否一致——构造一个反例说明它们可能冲突。
（设计题） 为"灾后建筑结构评估"任务设计能力维度（至少 5 维）和任务需求向量。任务包括：高空裂缝拍摄、室内梁柱检测、承重测试（需要施加已知力）、有毒气体检测。对每个能力维度，标注它是 cumulative 还是 non-cumulative，并解释你的判断依据。然后说明：哪些任务必须由联盟完成（单机能力不足）、哪些可单机完成？
（跨章综合题，串联第 1、3 章） 第 1 章定义了通信图 $G$ 与代数连通性 $\lambda_2$，第 3 章的 CBBA 在这张图上做分布式分配。现在考虑一支异构队伍：无人机通信半径大（500 m）、地面车通信半径小（100 m）。请论证：(a) 异构的通信半径如何让通信图 $G$ 本身变成"异构图"（边的存在取决于哪类节点）？(b) 如果无人机失效，通信图的 $\lambda_2$ 可能如何变化、甚至图可能不连通？(c) 这对在该图上运行的 CBBA 收敛性有什么影响？把你的分析和 §6.3 即将讲的"中继型架构单点故障"联系起来。

§6.2 异构任务分配：能力感知的 CBBA 扩展 ⭐⭐⭐¶

这一节解决什么问题：第 3 章的 CBBA 假设所有 agent 同质——任何机器人都能做任何任务，区别只在代价。现实的异构队伍不是这样：搬运任务地面车能做、无人机做不了；高空侦察反过来。这一节把 CBBA 系统地异构化，加入三样东西——能力硬约束（谁有资格竞标）、联盟任务（一个任务需要多种能力拼合）、能量约束（续航短的机型分配时要算油耗）。核心信息是：CBBA 的两阶段骨架原封不动，变的是喂给它的问题定义——但加入联盟任务后，CBBA 赖以保证 50% 最优界的 DMG 条件可能被破坏，这是异构分配付出的理论代价。

动机：当"谁都能做"的假设崩塌¶

回顾第 3 章，CBBA 解决的是一个干净的问题：$N$ 个同质机器人、$M$ 个任务，每个机器人对每个任务有一个代价 $c_{ij}$，目标是分配使总效用最大。它的优雅之处在于完全分布式——每个机器人本地构建任务包（bundle）、靠共识消解冲突，最终拿到不差于最优 50% 的解。

但这个问题设定有一个隐含的同质假设：任何机器人都可以竞标任何任务。在异构队伍里，这个假设直接崩塌：

设想搜救场景：3 架无人机 + 2 台地面车，任务清单包括"航拍废墟全景"（需要飞行）、"钻进倒塌楼梯间搜寻"（需要地面越障 + 体型小）、"搬开挡路的预制板"（需要大载重）。让无人机去竞标"搬预制板"是没有意义的——它根本搬不动。让地面车竞标"航拍全景"同样荒谬——它飞不起来。

如果你天真地把这些不可行分配的代价设成一个大数（比如前面"如果跳过本章"场景一里的 $10^6$），会出现一个隐蔽而致命的 bug：在某些边角情况下，优化器为了"完成所有任务"会选择一个超贵但有限的不可行分配。比如所有地面车都已分配满，"搬预制板"无人认领，优化器一看"派无人机去搬，代价 $10^6$，总比留着任务不完成（可能被建模为无穷大惩罚）强"，于是把物理上不可能的任务派给了无人机。仿真里这表现为"无人机收到一个它永远无法完成的指令，卡在那里"。

这就引出了异构分配的第一个、也是最基础的改动：能力约束必须是硬的，不可行分配必须从竞标资格里彻底剔除，而不是用大代价软性惩罚。

如果用"大代价软惩罚"会怎样：硬约束 vs 软约束¶

让我们把这个区别讲透，因为它是异构分配最常见的坑，也体现了一个普适的工程原则。

软约束（大代价）的做法：保留所有 $(i,j)$ 对，把不可行的设成大代价 $c_{ij} = M_{\text{big}}$。优化目标里它仍是一个可选项，只是"很贵"。

硬约束（剔除）的做法：为每个任务 $j$ 维护一个**合格候选集** $\mathcal{A}_j = \{i : \mathbf{q}_i \succeq \mathbf{y}_j\}$（机器人 $i$ 的能力向量在所需维度上全部达标），只有 $\mathcal{A}_j$ 里的机器人能竞标任务 $j$。不在候选集里的 pair 根本不进入优化。

两者的区别在三个层面：

层面	软约束（大代价）	硬约束（剔除候选）
正确性	极端情况下不可行分配仍可能被选中	不可行分配永不出现（结构上保证）
数值	大代价污染出价数值，CBBA 的 $\varepsilon$-松弛、收敛阈值都被扭曲	出价数值干净，只在可行集上比较
效率	优化器要在包含大量"必不选"项的空间里搜索，浪费算力	搜索空间直接缩小到可行子集

本质洞察：硬约束优于软约束，背后是一个超越异构分配的普适原则——用约束的结构（可行集的形状）表达"绝对不可以"，用代价的数值表达"不太好但可以"。 把"绝对不可以"编码成"代价很大"，本质是把一个布尔事实（能/不能）降级成了一个连续量（贵/便宜），而连续量在优化中永远存在"被其他更糟的选择反超"的可能。这个原则在 §6.6 的不对称内力分配（接触约束是硬的）、§6.4 的避障（碰撞是硬的）里会反复出现。凡是物理上"绝对不可能"的，都应该编码进可行集而非代价函数。

历史：从同质拍卖到能力感知分配¶

CBBA（Choi, Brunet, How, IEEE T-RO 2009）本身是同质的。把它异构化的需求很早就出现了——Gerkey-Matarić 的 MRTA 分类法（2004）里的 SR/MR 维度（单机器人任务 vs 多机器人任务）就预示了"联盟任务"的存在。真正系统地处理异构能力分配的，是 STRATA（Ravichandar 等, ICRA 2019）这条线——它用连续特质向量和可累加性区分，把"哪些机器人组合能满足任务需求"形式化为一个优化问题。

在 CBBA 的具体扩展上，文献里有几条路线：(1) 给 CBBA 加能力约束（最简单，改候选集）；(2) 处理需要多机协作的耦合任务（coupled-constraint CBBA / CBBA with task coupling）；(3) 把能量/续航纳入代价（energy-aware MRTA，近年针对长航时任务的工作，如 2024 年针对长航时动态场景的异构 MRTA 框架，处理电量、充电、任务中继）。本节把这三条路线的核心思想压缩成对 CBBA 的三个具体改动，让你能在第 3 章实现的基础上增量地异构化。

理论：异构 CBBA 的三个改动¶

回顾第 3 章 CBBA 的两阶段骨架（这里重述以便你不翻回去）：

阶段一（bundle 构建）：每个机器人 $i$ 维护一个任务包 $b_i$（按加入顺序）和一条路径 $p_i$（按执行顺序）。它反复地把"边际收益最高"的任务加入包中，直到包满或没有正收益任务。边际收益定义为 $c_{ij}(p_i) = S_i^{p_i \oplus_n j} - S_i^{p_i}$，即把任务 $j$ 插入路径最优位置后的得分增量。
阶段二（冲突消解）：每个机器人维护一个"中标价向量" $\mathbf{y}_i$（每个任务当前已知的最高出价）和"中标者向量" $\mathbf{z}_i$。通过与邻居交换这两个向量做**带时间戳的最大值共识**，全网逐渐对"每个任务归谁"达成一致。被别人以更高价抢走的任务，连同其后所有任务，从自己的包里释放。

异构化就是在这个骨架上做三个改动。关键是：阶段二（共识冲突消解）完全不变——出价是标量，最大值共识对标量的运算与机器人是否异构无关。所有改动都在**阶段一（问题定义）**。

改动一：能力硬约束——改候选集¶

在 bundle 构建阶段，机器人 $i$ 考虑加入任务 $j$ 之前，先检查资格：

\[ i \in \mathcal{A}_j \iff \mathbf{q}_i \succeq \mathbf{y}_j \quad(\text{即 } q_{i,u} \geq y_{j,u},\ \forall u \in \text{所需维度}) \]

只有 $i \in \mathcal{A}_j$ 时，$i$ 才计算 $j$ 的边际收益并可能加入包。否则任务 $j$ 对 $i$ 的边际收益直接视为 $-\infty$（即不可加入）。这一步是纯本地的、$O(U)$ 的向量比较，几乎不增加计算量，却从根本上杜绝了不可行分配。

改动二：联盟任务——改 bundle 的原子单位¶

有些任务单机能力不足，必须由多机能力拼合。比如"搬运 60 kg 物体"，单台载重 30 kg 的四足做不了，需要两台组队。这类任务称为**联盟任务（coalition task）**，其能力需求 $\mathbf{y}_j$ 在某个可累加维度上超过任何单机的能力。

处理联盟任务有两种思路：

思路 A：任务分解（task decomposition）。把联盟任务拆成多个子任务（"四足_1 抬左端"、"四足_2 抬右端"）加时序/同步约束，再用扩展的 CBBA 分配子任务。优点是复用单任务 CBBA，缺点是要显式管理子任务间的同步（两台必须同时抬）。

思路 B：联盟竞标（coalition bidding）。让机器人对"加入某个联盟"出价，而非对"独自完成任务"出价。任务 $j$ 的合格联盟集合定义为：

\[ \mathcal{C}_j = \Big\{ S \subseteq \mathcal{R} : \text{agg}_{\text{cumulative}}\big(\{\mathbf{q}_i\}_{i \in S}\big) \succeq \mathbf{y}_j \Big\} \]

其中 agg 就是 §6.1 的 coalition_capability 函数（可累加维求和、不可累加维取 max）。机器人竞标的是"以最小代价满足 $\mathbf{y}_j$ 的联盟中的一席"。这更接近 STRATA 的视角，但组合复杂度高（联盟数随队伍规模指数增长），实践中要靠贪婪或启发式构造联盟。

对比性思维（反事实）：如果不引入联盟任务、坚持"每个任务必须单机完成"会怎样？那么所有"重于单机载重"的搬运任务、所有"需要侦察+操作两种能力"的复合任务，都会被判为**不可分配**（候选集为空）——异构系统最有价值的能力（用多机能力拼出单机做不到的事）就被白白浪费了。联盟任务不是 CBBA 的可选装饰，而是异构协同"1+1>2"红利的算法载体。没有联盟，异构就只是'各干各的、互不干涉'，谈不上真正的协同。

改动三：能量约束——改代价函数¶

续航短的机型（无人机 ~20 min vs 四足 ~2 h）在分配时必须考虑能耗，否则会出现"无人机接了一串远距离任务，飞到一半没电掉下来"。把能量纳入代价：

\[ c_{ij}^{\text{energy}} = \text{reward}_j - \alpha \cdot \text{energy\_cost}_i(\text{travel}_{ij}) - \beta \cdot \mathbb{1}[\text{energy}_i < \text{threshold}] \]

其中 $\text{energy\_cost}_i$ 是机型相关的能耗模型（无人机的悬停+移动功耗远高于地面车），最后一项是"低电量惩罚"。更精细的处理（如 2024 年长航时 MRTA 工作）还会显式建模充电站、任务中继（一个任务接力完成）。能量约束的本质是给代价函数加一个**机型相关的、状态相关的修正项**——这恰好用上了异构能力矩阵里的续航维度。

理论-工程桥接：50% 最优界何时失效¶

这是本节最重要的理论洞察，也是异构分配区别于同质 CBBA 的关键代价。

第 3 章告诉我们，CBBA 在 DMG（Diminishing Marginal Gain，边际收益递减） 条件下保证不差于最优 50%。DMG 的含义是：一个任务的边际收益不会因为先做了别的任务而增加——形式化地，对任意路径 $p \subseteq p'$，有 $c_{ij}(p) \geq c_{ij}(p')$（已有任务越多，新任务的边际收益越小或相等）。这是一个**子模性（submodularity）**条件，是顺序贪婪等价于集中式贪婪、从而拿到 50% 界的数学基础。

改动一（能力约束）不破坏 DMG：它只是缩小了候选集，每个机器人在自己的可行任务子集上仍满足 DMG。50% 界保持。

改动三（能量约束）一般不破坏 DMG：只要能耗是路径长度的、且"已有任务越多、新任务边际收益越小"仍成立（通常成立，因为电量越用越少、新任务越难加），DMG 保持。

改动二（联盟任务）可能破坏 DMG：这是危险所在。联盟任务引入了 agent 之间的**正向耦合**——任务 $j$ 对机器人 $i$ 的价值，依赖于是否有另一个机器人 $k$ 也加入这个联盟。如果 $i$ 单独无法完成 $j$（收益为 0），但 $i$ 和 $k$ 一起能完成（收益为正），那么"$k$ 加入"反而**增加**了 $i$ 的边际收益——这正是 DMG（边际收益只减不增）的反面！这种**超可加（super-additive）效用**违反子模性。

本质洞察：联盟任务把异构协同从"竞争性分配"（大家抢有限任务，多一个竞争者收益降低，子模）变成了"合作性分配"（大家凑能力完成任务，多一个合作者收益升高，超可加）。CBBA 的 50% 界是为竞争性分配设计的，一旦出现超可加的合作收益，这个界就可能失效。这不是 CBBA 实现的 bug，而是问题结构的根本变化——它把我们引向第 5 章协同搬运（合抬重物，效用超可加）那类"强协同"问题，那里需要的是另一套方法（联合优化、显式联盟形成），而非分布式拍卖。

这给了一个清晰的工程判据：

如果你的异构任务**都能单机完成**（只是能力筛选 + 能量加权）→ 异构 CBBA 安全，50% 界保持，放心用分布式拍卖。
如果存在**必须多机拼能力**的联盟任务（超可加）→ CBBA 的保证失效，要么接受"无保证的启发式"，要么对这部分任务上专门的联盟形成算法（如 STRATA 的优化、或第 5 章的联合方法）。

代码：在第 3 章 CBBA 上增量异构化¶

下面给出异构 CBBA 的核心改动代码（建立在你第 3 章已实现的 CBBA 之上，只展示新增/修改的部分）。

Step 1: 先讲为什么这样组织改动

为什么把三个改动都做在 bundle 构建阶段、保持冲突消解阶段不变？

因为 CBBA 的分布式正确性来自冲突消解阶段的"最大值共识"——
它保证全网对"每个任务归谁"达成一致，这个机制与任务/机器人是否异构无关
（出价是标量，max 共识对标量永远成立）。
异构性只影响"每个机器人本地认为自己对每个任务值多少"——这是 bundle 阶段的事。
所以正确的异构化策略是：把所有异构逻辑封装进"边际收益计算"和"候选资格判断"，
让冲突消解阶段对异构完全无感知。这是一种关注点分离（separation of concerns）。

Step 2: 给出正确写法

import numpy as np

def is_eligible(robot_caps, task_demand, required_dims):
    """改动一：能力硬约束。机器人能力在所需维度上全部达标才合格。"""
    return np.all(robot_caps[required_dims] >= task_demand[required_dims])

def marginal_score_hetero(robot_id, task_j, path, Q, task_demands,
                          required_dims, energy_state, energy_model,
                          alpha=1.0, lowE_penalty=1e3, lowE_thresh=0.2):
    """异构边际收益：先查资格（改动一），再算含能量修正的收益（改动三）。
       返回 -inf 表示不可加入。"""
    # 改动一：能力硬约束 —— 不合格直接 -inf，从竞标中剔除
    if not is_eligible(Q[robot_id], task_demands[task_j], required_dims[task_j]):
        return -np.inf, None

    # 基础边际收益（复用第 3 章：把 j 插入 path 最优位置的得分增量）
    base_gain, best_pos = best_insertion_gain(robot_id, task_j, path)

    # 改动三：能量修正 —— 机型相关的能耗 + 低电量惩罚
    e_cost = energy_model(robot_id, task_j, path, best_pos)   # 机型相关
    score = base_gain - alpha * e_cost
    if energy_state[robot_id] - e_cost < lowE_thresh:
        score -= lowE_penalty                                # 软性回避（不是硬剔除）

    return score, best_pos

def build_bundle_hetero(robot_id, tasks, Q, task_demands, required_dims,
                        energy_state, energy_model, bundle_capacity):
    """异构 bundle 构建：在合格且有正收益的任务里贪婪地加边际收益最高者。
       冲突消解阶段（未列出）与第 3 章完全一致，无需改动。"""
    bundle, path = [], []
    while len(bundle) < bundle_capacity:
        best_task, best_score, best_pos = None, 0.0, None
        for j in tasks:
            if j in bundle:
                continue
            score, pos = marginal_score_hetero(
                robot_id, j, path, Q, task_demands, required_dims,
                energy_state, energy_model)
            if score > best_score:          # 只加正收益、且最高的
                best_task, best_score, best_pos = j, score, pos
        if best_task is None:
            break                            # 没有可加的正收益任务
        bundle.append(best_task)
        path.insert(best_pos, best_task)
    return bundle, path

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

# ❌ 错误 1：用大代价软惩罚代替能力硬约束
def marginal_score_WRONG(robot_id, task_j, path, Q, task_demands, required_dims):
    base_gain, pos = best_insertion_gain(robot_id, task_j, path)
    if not is_eligible(Q[robot_id], task_demands[task_j], required_dims[task_j]):
        return base_gain - 1e6, pos     # ❌ 大惩罚而非剔除
    return base_gain, pos
# 问题：当某任务无人合格、且"不完成"被建模为更大惩罚时，
#      优化器仍会选这个 -1e6 的不可行分配。物理上不可能的任务被派出去了。
#      根本原因：把布尔约束（能/不能）降级成了连续代价（贵/便宜）。

# ❌ 错误 2：对联盟任务用单机 CBBA 直接分配，且默默假设 DMG 成立
#    若任务需求 y_j 的某可累加维 > 任何单机能力，is_eligible 对所有单机返回 False
#    → 候选集为空 → 任务被判不可分配，但代码不报警，任务静默丢失
# 问题：联盟任务需要专门处理（分解或联盟竞标），不能指望单机 CBBA 自动凑出联盟。
#      自检：分配结束后检查是否有任务未被任何机器人认领，
#           若有且其需求超过单机能力上限 → 它是漏处理的联盟任务。

# ❌ 错误 3：能量惩罚也用硬剔除（过度硬化）
#    if energy_state[i] - e_cost < thresh: return -inf   # ❌ 不该 -inf
# 问题：低电量是"不太好"而非"绝对不可以"——也许这是唯一能做该任务的机器人，
#      宁可让它做完再充电，也比任务不完成强。能量是软约束，能力才是硬约束。
#      根本原因：混淆了"物理不可能"（飞行 vs 不会飞，硬）和
#               "代价高/有风险"（电量低，软）。前者剔除，后者加惩罚。

Step 4: 给出不同处理联盟任务方式的对比

# === 三种处理联盟任务的方式 ===
# 任务示例：搬运 60kg 物体，单台四足载重 30kg → 需 2 台组队（可累加维 payload）

# 方式 A：任务分解（推荐工程落地）
#   把"搬 60kg"拆成 "抬左端"+"抬右端" 两个子任务 + 同步约束
#   优点：复用单任务 CBBA；缺点：需显式管理"两台必须同时抬"的时序同步
def decompose_coalition_task(task, n_split):
    return [SubTask(parent=task, role=r, sync_group=task.id)
            for r in range(n_split)]

# 方式 B：联盟竞标（接近 STRATA，理论优美）
#   枚举/贪婪构造满足 y_j 的联盟，机器人竞标"联盟中一席"
#   优点：直接优化联盟代价；缺点：联盟数指数增长，需启发式剪枝
def feasible_coalitions(Q, demand, cumulative, max_size=3):
    from itertools import combinations
    N = Q.shape[0]
    coalitions = []
    for size in range(1, max_size + 1):
        for S in combinations(range(N), size):
            agg = np.where(cumulative, Q[list(S)].sum(0), Q[list(S)].max(0))
            if np.all(agg >= demand):
                coalitions.append(S)
    return coalitions   # 取代价最小者

# 方式 C：忽略联盟、只做单机分配（仅当确无联盟任务时可用）
#   最简单，但遇到超单机能力的任务会静默失败

# 对比：
# | 方式      | 复杂度        | DMG/50%界 | 适用 |
# |-----------|--------------|-----------|------|
# | A 分解    | 低（复用CBBA） | 子任务间仍可能超可加，界不保证 | 工程首选 |
# | B 联盟竞标 | 高（指数，需剪枝）| 显式处理，可上专门保证 | 研究/小规模 |
# | C 忽略    | 最低          | 无联盟时保持，有联盟时失败 | 确认无联盟任务 |

⚠️ 常见陷阱¶

编程陷阱：能力约束用软惩罚导致不可行分配被选中

错误做法：把"机器人做不了任务"编码成大代价 c_ij = 1e6，而非剔除候选
现象：绝大多数实例正常，但在"某任务无合格机器人 + 不完成惩罚更大"的边角实例里，
     物理上不可能的任务（如让无人机搬 50kg）被分配出去，执行端卡死
根本原因：把硬约束（能力，布尔）降级成软约束（代价，连续）。
         连续代价永远可能被"其他更糟选择"反超而被选中
正确做法：维护合格候选集 A_j，不合格者的边际收益直接 -inf（结构上剔除）
自检方法：分配后断言"每个被分配的 (i,j) 都满足 q_i ≥ y_j"，
         任何违反立即暴露软约束泄漏

概念误区："异构 CBBA 也有 50% 最优界保证"

错误理解：CBBA 有 50% 界，我加了异构扩展，所以异构 CBBA 也有 50% 界
现象：在含联盟任务的实例上，异构 CBBA 给出的解远差于最优的一半，
     且无法解释为什么——明明"用的是有保证的 CBBA"
根本原因：50% 界依赖 DMG（边际收益递减/子模性）。联盟任务引入超可加效用
         （多一个合作者收益升高），直接违反 DMG，界因此失效
正确理解：异构 CBBA 的保证是分情况的——纯能力约束 + 能量约束保持 50% 界，
         联盟任务（超可加）破坏它。判断标准：你的任务是"抢"（竞争，子模，有界）
         还是"凑"（合作，超可加，无界）。"凑"的部分要换方法

思维陷阱：把"能量低"和"能力缺失"用同一种约束处理

错误做法：要么都用硬剔除（电量低也剔除），要么都用软惩罚（能力不足也只罚）
现象（都硬）：一架电量稍低但仍能完成任务的无人机被剔除，任务无人做（过度保守）
现象（都软）：不会飞的地面车被软性派去航拍（不可行分配泄漏）
根本原因：没区分约束的物理性质——能力缺失是"绝对不可能"（硬），
         能量低是"有风险但可能可以"（软）
正确做法：能力 = 硬约束（剔除候选）；能量 = 软约束（代价惩罚，留余地）。
         一般原则：物理上绝对做不到的进可行集（硬），代价高/有风险的进代价（软）

练习¶

（实现 + 实验） 在你第 3 章的 CBBA 实现上加入改动一（能力硬约束）和改动三（能量约束），构造一个"3 UAV（可飞/不可搬重）+ 2 UGV（可搬重/不可飞）"、任务含"侦察"（需飞行）和"搬运"（需载重 $\ge 20$kg）两类的实例。验证：(a) 所有侦察任务只分给 UAV、所有搬运任务只分给 UGV；(b) 故意把某 UAV 的初始电量设很低，观察能量惩罚是否让它避开远距离侦察任务。然后做一个对照实验：把能力约束改成"大代价软惩罚"，构造一个所有 UGV 满载的实例，复现"搬运任务被错误派给 UAV"的 bug。
（推导 + 反例） 严格论证：纯能力约束的异构 CBBA（改动一）保持 DMG 条件。提示：能力约束只是把每个机器人的可行任务集从全集缩小到子集，在子集上 DMG 是否保持？然后构造一个**含联盟任务**的最小实例（2 个机器人、1 个需要两机合作的任务），手动计算边际收益，证明 DMG 被破坏（即存在 $p \subseteq p'$ 使 $c_{ij}(p) < c_{ij}(p')$）。
（跨章综合题，串联第 2、3、6 章） 异构 CBBA 的冲突消解阶段沿用第 2 章的最大值共识。现在假设通信图是异构的（§6.1 练习 3：UAV 通信半径大、UGV 小），且 UAV 可能因续航耗尽而中途退出（节点消失）。请分析：(a) 当一个持有多个任务中标权的 UAV 突然退出，这些任务的中标信息如何在剩余网络中被"撤销"和"重新拍卖"？(b) 第 2 章的最大值共识在节点动态消失时还能保证一致吗？需要什么额外机制（如租约/超时）？(c) 把你的方案和 §6.3 即将讲的通信架构鲁棒性联系起来：哪种通信架构对"UAV 中途退出"最鲁棒？

§6.3 空地联合通信架构 ⭐⭐¶

这一节解决什么问题：异构队伍里，谁和谁能通信、信息怎么流动，比同构队伍复杂得多——因为节点本身异构（无人机通信半径大、续航短；地面车反之）。这一节给出空地异构系统（Air-Ground Heterogeneous System, AGHS）的三类通信架构（中继 / mesh / leader-follower），用"单点故障谱"这一条主线把它们串起来，并教你为搜救、巡检、物流三类场景选对架构。这是 §6.2 分配结果能否真正下发、§6.5 感知信息能否真正汇聚的物理前提。相比前两节，这是一个偏系统、偏工程的"轻松段"，但选错架构的代价在真机上是致命的。

动机：分配做完了，指令传不到¶

前两节解决了"谁干什么"。但有一个被默认成立、实际却很脆弱的前提：分配结果能下发到每个机器人、每个机器人的状态能汇聚回来。在同构队伍里这通常不是问题（大家通信能力一样、距离相近）。异构空地队伍里，这个前提随时可能崩：

林区搜救场景：地面车在树林里慢慢搜，无人机在 100 米高空快速巡。地面基站在山脚。问题来了——地面车被树木遮挡，和基站直接通信时断时续；无人机视野开阔、通信通畅，但它 20 分钟就要回来换电池。信息到底怎么从基站流到深山里的地面车？谁来当中间人？当无人机回去换电池的那几分钟，整个系统是不是就断联了？

这就是异构通信架构要回答的问题。它和第 1 章的"三大架构"（集中/分布/去中心）相关但不同：第 1 章关心的是**决策权**在哪（谁做决定），本节关心的是**信息流**怎么走（数据怎么传）。在异构系统里，信息流的设计必须考虑节点的异构性——哪类节点适合当中继（视野好、通信半径大）、哪类节点续航是瓶颈、哪类节点失效会导致全网瘫痪。

理论：三类架构与单点故障谱¶

把三类架构沿一条"单点故障脆弱性"的谱排列，是理解它们取舍的最佳视角。

架构 A：中继型（relay）——最简单，但最脆弱¶

   地面基站 ←──→ UAV（空中中继）←──→ 远端 UGV（深山/废墟内）
                  ↑
            视野好、通信半径大，
            天然适合当"空中信号塔"

无人机利用其高空视野和大通信半径，充当基站与远端地面车之间的中继。这是空地协同最自然的架构——无人机本来就在高处，让它顺带转发信号，几乎零额外成本。

优势：实现极简；通信距离大幅延伸（无人机当"移动信号塔"）；地面车不需要强通信能力。
致命弱点：单点故障。无人机一旦失效（撞树、没电、被干扰），基站和远端地面车立刻完全断联。前面搜救场景里"无人机回去换电池"那几分钟，整个链路就断了。

架构 B：Mesh 型（ad-hoc mesh）——最鲁棒，但延迟不可控¶

        UAV_1 ←──→ UAV_2
         ↑  ╲      ╱  ↑
         │   ╲    ╱   │       所有节点组成自组织 mesh 网络，
         ↓    ╲  ╱    ↓       消息多跳路由，任意单点失效都有备份路径
        UGV_1 ←──→ UGV_2 ←──→ 基站

所有节点（无人机、地面车、基站）组成一个自组织（ad-hoc）网状网络，消息通过多跳路由在网络中传播。这是去中心化思想在通信层的体现。

优势：无单点故障。任意一个节点失效，消息可绕道其他路径——这正好补上中继型的致命弱点。最适合需要高可靠性的场景（如军事、灾难现场）。
代价：延迟不确定（多跳路由，跳数随拓扑变化）；网络管理复杂（路由表维护、拓扑动态变化）；带宽利用率低（转发开销）。

架构 C：Leader-Follower 型——折中，决策集中¶

   UGV（leader，计算强、续航长）
      │ 下发目标
      ▼
   UAV_1, UAV_2（follower，侦察 + 跟随，把感知回传 leader）

选一个节点当 leader（通常是计算能力强、续航长的地面车），其他节点当 follower。Leader 负责决策和协调，follower 执行并回传感知。

优势：结构清晰、协调简单（决策集中在 leader）；适合"一个强节点 + 多个弱节点"的异构结构——这恰恰是很多空地队伍的真实形态（一台强地面车 + 几架轻无人机）。
弱点：leader 是单点故障（leader 失效则全队失去协调），但比中继型温和——follower 间可能还能局部协作，且可设计 leader 选举（leader election）让备份节点顶上。

三架构对比表¶

维度	中继型	Mesh 型	Leader-Follower 型
实现复杂度	最低	最高	中
单点故障	严重（中继挂=全断）	无	中（leader 挂，可选举恢复）
通信延迟	低（单跳/双跳）	不确定（多跳）	低（星型）
通信距离	大（中继延伸）	大（多跳接力）	中（受 leader 半径限）
适合的异构结构	远端弱通信节点 + 空中中继	节点能力相近、要高可靠	一强多弱（强地面车+轻无人机）
续航考量	中继节点续航是命门	分散，无单一命门	leader 续航是命门

本质洞察：这三种架构本质上是在**鲁棒性、延迟、复杂度**三者之间做权衡，而权衡的关键变量是异构节点的**能力分布**。中继型把所有鸡蛋放在一个高能力节点（中继无人机）的篮子里——能力越集中、效率越高但越脆弱；mesh 型把能力摊平到所有节点——越分散、越鲁棒但越低效。没有最好的架构，只有最匹配当前异构结构和任务可靠性需求的架构。 这与第 1 章"集中 vs 分布 vs 去中心"的权衡是同一个哲学在通信层的回响：集中带来效率与脆弱，分散带来鲁棒与开销。

理论-工程桥接：为不同场景选架构¶

把三类架构落到三个真实场景，看怎么选：

场景	关键约束	推荐架构	理由
灾后搜救	高可靠性优先（断联=救援失败）；环境恶劣	Mesh 型	任意节点失效都不能让系统瘫痪；可靠性 > 延迟
大范围巡检	通信距离大；地面车在远端弱通信区	中继型	无人机当空中中继延伸覆盖；环境相对可控，单点故障风险可接受
物流配送	一个调度中心 + 多个执行单元；结构稳定	Leader-Follower	调度中心天然是 leader；星型决策简单高效

注意这些不是死规则——实践中常**混合**使用。比如一个大型搜救系统可能在局部用 leader-follower（一个指挥车带几架无人机），多个局部之间用 mesh 互联。HetSwarm（2025，§6.4 详述）就是一个 leader-follower（UAV 引领、UGV 跟随）+ 局部阻抗链的混合架构。

ROS2 实现要点¶

异构系统的 ROS2 实现有几个关键约定（这部分在第 13 章 Mini-MultiBot 实战会展开，这里给骨架）：

1. 命名空间隔离：每类机器人一个命名空间
   /uav_0/, /uav_1/, /go2_0/, /go2_1/
   → 同类机器人话题/服务/参数自动隔离，避免串味

2. 跨类型通信走共享话题：
   /shared/target_pose, /shared/global_map
   → 异构节点通过订阅共享话题交换"公共语义"信息
     （注意：交换的必须是公共语义，如世界系位姿，不是各自的私有状态——
      呼应前置自测第 2 题：异构状态不能直接共享）

3. DDS 自动发现：DDS multicast 让节点动态加入/退出自动被感知
   → 天然支持 mesh 型的动态拓扑、leader 选举的成员变化
   → 但要配置 QoS：可靠性敏感的（指令）用 RELIABLE，
     高频感知（图像流）用 BEST_EFFORT 省带宽

4. 异构节点的时钟同步：UAV 和 UGV 可能时钟漂移不同
   → 跨模态融合（§6.5）对时间戳敏感，需 PTP/chrony 同步，
     否则 §6.5 的传感器配准会因时间错位失败

对比性思维（不是 X 而是 Y）：异构系统的跨类型通信**不是**"把各自的完整状态广播出去让别人解析"，而是"约定一个公共语义层（如世界系目标位姿），各节点只在这一层交换信息"。前者会逼迫每个节点理解所有其他机型的私有状态格式（无人机要懂四足的 12 维关节、四足要懂无人机的姿态四元数），耦合度爆炸；后者让每个节点只需把自己的私有状态投影到公共语义，解耦清晰。这正是前置自测第 2 题"异构状态不能直接共识"的工程对应——通信层必须先有公共语义层。

⚠️ 常见陷阱¶

编程陷阱：跨类型通信直接传私有状态，导致耦合爆炸

错误做法：让每类机器人把自己的完整状态（UAV 传 SE(3) 位姿+电机转速，
         四足传 12 关节角+足端力）广播到一个公共话题，让所有节点订阅
现象：每加一种新机型，所有已有节点的解析代码都要改（要能解析新机型的私有格式）；
     消息体积大、带宽爆炸；不同机型状态语义不同，订阅方常常误解
根本原因：没有公共语义层，把异构的私有状态当成了通信内容
正确做法：约定公共语义话题（世界系位姿、目标点、共享地图），
         每个节点只把自己的私有状态投影到公共语义后发布
自检方法：问"加一种全新机型，需要改几个已有节点的代码？"——
         理想答案是 0（新机型只需把自己投影到已有公共语义）

概念误区："mesh 网络最鲁棒，所以总是最优选择"

错误理解：既然 mesh 无单点故障，那就一律用 mesh
现象：在一个"一台强地面车 + 3 架轻无人机"的简单巡检任务里上了 full mesh，
     结果路由开销吃掉大量带宽、延迟抖动让无人机控制回路不稳，
     维护这套 mesh 的工程量远超任务本身
根本原因：把"鲁棒性"当成了唯一目标，忽略了延迟、复杂度、带宽的代价
正确理解：鲁棒性是有成本的。任务对可靠性要求不高、结构稳定、有天然 leader 时，
         leader-follower 或中继型更划算。架构选择是多目标权衡，不是单目标最大化

思维陷阱：忽略异构节点的续航差异，让续航短的节点当关键枢纽

错误做法：选通信能力最强的节点当中继/leader，不管它续航
现象：选了视野最好的无人机当唯一中继，结果它 20 分钟就要回去换电池，
     每次换电池整个系统断联 5 分钟，任务被反复打断
根本原因：只看了"通信能力"这一个异构维度，忽略了"续航"这个维度。
         异构节点的多个能力维度可能冲突（通信最强的恰好续航最短）
正确做法：选关键枢纽节点时综合考虑通信能力 AND 续航。
         若续航短的节点通信最强，要么用 mesh 分散依赖，
         要么设计"中继接力"（多架无人机轮班当中继）
自检方法：算一下"关键枢纽节点的续航 vs 任务时长"——
         若枢纽续航 << 任务时长，必须有接力或冗余机制

练习¶

（设计 + 仿真） 用 ROS2（或纯 Python 模拟）搭一个"1 强地面车 + 2 无人机"的 leader-follower 通信骨架：地面车当 leader 发布 /shared/target_pose，两架无人机订阅并把模拟的"侦察结果"发布到 /shared/observation。然后注入故障：(a) 模拟 leader 失效，观察系统行为；(b) 实现一个简单的 leader 选举（leader 失联超时后，follower 中 ID 最小者升为新 leader）。对比有无 leader 选举时系统的恢复能力。
（分析题） 对正文三类架构，分别画出"节点失效数 vs 系统连通性"的定性曲线（横轴是失效节点数，纵轴是仍能通信的节点比例）。中继型、mesh 型、leader-follower 型的曲线形状有什么本质不同？哪一类是"阶跃下降"（关键节点失效即崩）、哪一类是"渐进下降"（优雅降级）？这条曲线和你在 §6.1 练习 3 分析的"UAV 失效后通信图 $\lambda_2$ 变化"有什么联系？
（开放设计题） 设计一个**混合架构**应对这样的场景：一个大型仓库，分成 4 个区域，每个区域有 1 台 AGV（地面车）+ 2 架库存盘点无人机，4 个区域共享一个调度中心。请说明：区域内用什么架构、区域间用什么架构、调度中心如何接入？当某区域的 AGV 故障时，你的设计如何让该区域的无人机继续工作（被邻区接管？直连调度中心？）？画出你的拓扑图并标注每条链路的架构类型。

§6.4 异构编队：leader-follower 与阻抗引导 ⭐⭐⭐¶

这一节解决什么问题：第 4 章的分布式 MPC 编队让一群机器人保持队形，但它默认所有机器人动力学同构（同一模型模板、只是参数不同），一致性约束直接写在状态空间。当队伍里有全向点质量、非完整差速车、欠驱动四旋翼时，这套做法直接崩——它们的状态空间维度和约束都不同，没法逐分量比较。这一节讲透异构编队的核心原则：一致性必须建在公共输出空间（编队几何）而非状态空间，并通过 HetSwarm 的阻抗引导（impedance-based guidance），展示如何让"无人机能飞过但地面车要绕开"的可达集差异被优雅地吸收进编队控制。

动机：同一条参考轨迹，喂不进异构编队¶

第 4 章的编队很美：$N$ 个机器人用 ADMM 把各自的单体 MPC 耦合起来，邻居之间维持期望的相对位置 $p_i - p_j = d_{ij}$，整个队形像一个刚体一样移动。这套方法的前提是所有机器人共享同一个动力学模型模板——比如都是二阶积分 $\ddot{p}=u$，只是质量/推力上限不同。一致性约束 $p_i - p_j = d_{ij}$ 直接写在它们共有的位置状态上，干净利落。

现在把这个前提打破。考虑一个编队，里面有：

一架四旋翼（欠驱动，水平加速度靠倾斜产生，不能瞬间侧移）；
一台差速驱动地面车（非完整，瞬时速度方向锁在车头朝向，不能横移）；
一个全向移动平台（可任意方向移动）。

让它们保持一个三角形队形向前移动。问题立刻来了：

编队需要向左平移一小步。对全向平台，直接向左走即可。对四旋翼，它得先向左倾斜再产生向左加速度——位置能到，但有动态延迟。对差速车，它**根本不能直接向左走**——非完整约束禁止横移，它必须先转向再前进，走一条曲线才能到达左边的目标位置。

如果你硬套第 4 章的方法，把一致性约束写在状态空间，会发现：三种机器人的状态空间维度都不同（四旋翼 12 维、差速车 3 维、全向平台 4 维），"相对状态相等"这个约束**根本无法定义**——你没法让一个 12 维向量减一个 3 维向量。即便强行只比较位置子空间，约束 $p_i - p_j = d_{ij}$ 对差速车也是**瞬时不可满足**的（它不能瞬间到达任意相对位置）。

这就是异构编队的根本困难：异构动力学的状态空间不可比，可达集形状不同，第 4 章的状态空间一致性失效。

如果硬在状态空间做一致性会怎样¶

让我们把这个失败讲透，因为它揭示了一个深刻的设计原则。

假设你不信邪，强行在位置子空间写一致性约束 $p_i - p_j = d_{ij}$，喂给每个 agent 的局部 MPC。会发生三件事：

差速车的 MPC 不可行（infeasible）：在某些时刻，"达到期望相对位置"要求差速车瞬间横移，违反非完整约束，求解器报 infeasible，控制断流。
四旋翼的姿态被忽略：位置一致性完全不管姿态，但四旋翼的位置和姿态强耦合——只约束位置可能逼出剧烈的姿态机动，超出姿态控制能力。
整个编队被"最弱者"拖累且不自知：即便勉强可行，编队的可达运动被差速车的非完整约束限制，但状态空间的约束写法看不到这一点，导致规划出的队形机动差速车跟不上，队形持续散开。

根本原因是：状态空间一致性隐含假设"所有 agent 能瞬间达到任意相对状态"，这只对全向、全驱动系统成立。 异构编队里有非完整、欠驱动成员，这个假设崩了。

正确的出路是把一致性**上移一层**——不在状态空间约束，而在**输出空间（output space）**约束。

历史：从输出调节到异构编队¶

把多机协调建立在"输出"而非"状态"上，思想源头是控制理论里的**输出调节（output regulation）和**输出一致性（output consensus）。核心观念是：异构系统可以有不同的内部状态和动力学，但只要它们的**输出**（我们关心的可观测量，如机体在世界系的位置）能达成一致，协调目标就实现了。每个 agent 用自己的（异构的）动力学和控制器去驱动自己的输出，输出层面才做耦合。

在机器人编队里，这条线发展出"虚拟结构（virtual structure）"和"leader-follower 输出跟踪"等方法。近年的 HetSwarm（2025）则给出了一个特别优雅的工程实例：它不试图让异构成员保持刚性几何，而是用**阻抗（impedance）**把它们柔性地连起来——leader 在输出空间（位置）规划，follower 通过阻抗链跟随，且 follower 能根据自己的可达集对障碍做出与 leader 不同的响应。

理论：公共输出空间一致性¶

异构编队的设计分三步走，核心是"找到公共输出空间"。

第一步：定义公共输出空间¶

为每个 agent $i$ 定义一个输出映射 $h_i$，把它的（异构）状态 $x_i$ 投影到一个**所有 agent 共享的、同维同义的输出空间**：

\[ y_i = h_i(x_i) \in \mathbb{R}^p, \qquad \forall i \]

关键是 $h_i$ 因 agent 而异（异构），但输出 $y_i$ 同维同义（公共）。最常用的公共输出是**机体在世界系下的位置**（2D 地面编队取 $(x,y)$，3D 空地编队取 $(x,y,z)$）：

四旋翼：$h_{\text{uav}}(x) = $ 从 12 维状态取位置分量；
差速车：$h_{\text{ugv}}(x) = (x, y)$；
全向平台：$h_{\text{omni}}(x) = $ 位置分量。

现在所有 agent 的输出都是世界系位置，同维（2D 或 3D）、同义（都是位置）、可比较、可做共识。前置自测第 2 题的答案在这里落地：异构状态不能直接共识，但投影到公共输出后可以。

第二步：在输出空间写一致性¶

编队约束写在输出空间：

\[ y_i - y_j = d_{ij} \quad (\text{期望的输出相对几何，即队形}) \]

这个约束现在是良定义的（同维向量相减），且对所有 agent 一视同仁。它描述的是"我们想要的队形几何"，不涉及任何 agent 的内部动力学。

第三步：每个 agent 用各自动力学实现输出目标¶

一致性给出每个 agent 的**输出目标** $y_i^*$（为维持队形它应该到达的世界系位置），然后**每个 agent 用自己的（异构的）局部控制器**去实现这个输出目标：

四旋翼：用微分平坦 + SE(3) 控制生成可行轨迹到达 $y_i^*$；
差速车：用考虑非完整约束的轨迹跟踪（如 pure pursuit / 非完整 MPC）到达 $y_i^*$；
全向平台：直接位置控制。

本质洞察：异构编队的核心设计原则可以浓缩成一句话——"在公共输出空间协调，在私有状态空间执行"（coordinate in the shared output space, execute in the private state space）。协调层只关心"每个 agent 的输出应该在哪"（队形几何，所有 agent 同语言），执行层让每个 agent 用自己的动力学去实现（各说各话）。这是关注点分离在异构编队上的体现，也是它区别于第 4 章同构编队的根本之处：第 4 章协调和执行都在同一个状态空间（因为同构），异构编队必须把这两层劈开。这个"协调/执行分离"会在 §6.7 的异构 MARL（共享的高层 + 私有的低层动作）和 §6.8 的分层架构里再次出现——它是异构系统设计的元原则。

改进 APF 与 leader-follower 的结合¶

HetSwarm 给出的具体实现是 leader-follower + 改进人工势场（APF）：

**Leader（无人机）**用 APF 在输出空间（位置）规划：目标产生引力、障碍产生斥力，合力引导 leader 实时调整轨迹。APF 的好处是反应式、计算轻、天然处理动态障碍。
**Follower（地面车）**通过**阻抗链（impedance link）**跟随 leader：把 leader-follower 之间的期望相对位置当成一个虚拟弹簧-阻尼系统，follower 受到一个把它拉向期望队形位置的"阻抗力"。

阻抗模型把跟随写成一个二阶系统：

\[ M_d (\ddot{y}_i - \ddot{y}_i^*) + B_d (\dot{y}_i - \dot{y}_i^*) + K_d (y_i - y_i^*) = F_{\text{ext}} \]

其中 $M_d, B_d, K_d$ 是虚拟惯量/阻尼/刚度，$y_i^*$ 是 leader 给出的期望输出，$F_{\text{ext}}$ 是外部交互力（如局部避障）。这个柔性连接比刚性约束 $y_i - y_j = d_{ij}$ 宽容得多——它允许 follower 在遇到障碍时**暂时偏离**期望队形（弹簧被拉长），过后再回弹。

阻抗引导处理异构可达集差异¶

HetSwarm 最妙的一点，正好踩中异构编队的痛点。考虑一个矮障碍（比如地面上的一块石头）：

对无人机（leader），它在高空飞，这个矮障碍根本不在它的飞行高度——它可以无视。
对地面车（follower），这个矮障碍**挡在路上**，必须绕开。

如果用刚性队形约束，地面车绕障碍时会破坏队形，进而（通过刚性耦合）干扰无人机。HetSwarm 的做法是：让地面车对这类矮障碍建立一个**临时阻抗链（temporal impedance link）**——障碍施加一个临时斥力，地面车局部绕开（阻抗弹簧被拉伸），而无人机的轨迹完全不受影响（因为障碍不在它的可达集相关区域）。

对比性思维（不是 X 而是 Y）：异构编队的避障**不是**"整个编队一起绕开所有障碍"，而是"每个 agent 只对自己可达集相关的障碍做出反应"。无人机不该为地面的石头绕路（它能飞过去），地面车不该为高处的树枝绕路（它在树下）。刚性编队强迫所有成员对所有障碍统一反应，浪费且低效；阻抗引导让每个成员根据自己的可达集选择性地反应，柔性连接吸收了不一致。这把异构性从"必须统一妥协的负担"变成了"各扬所长的红利"——无人机的飞行能力让它免于地面绕障，正是异构的价值所在。

代码：异构 leader-follower 编队（最小实现）¶

下面给一个 2D 的最小实现：无人机（leader，全向近似）用 APF 规划，差速车（follower，非完整）用阻抗 + 非完整跟踪。

Step 1: 先讲为什么这样分层

为什么 leader 用 APF（反应式）、follower 用阻抗（柔性）、且都在输出空间？

输出空间（世界系位置）是 leader 和 follower 唯一能"对话"的公共语言——
leader 是全向近似（无人机水平面内可任意方向加速），follower 是非完整（差速车）。
它们的状态空间不可比，但世界系位置可比。
APF 给 leader：反应式、轻量、天然处理动态障碍，适合实时引领。
阻抗给 follower：柔性连接允许临时偏离队形去绕障碍，比刚性约束鲁棒。
非完整跟踪：阻抗给出的是"期望位置/速度"（输出空间目标），
           差速车再用自己的非完整控制器去实现——这就是"协调/执行分离"。

Step 2: 给出正确写法

import numpy as np

def apf_velocity(pos, goal, obstacles, k_att=1.0, k_rep=2.0, rep_radius=2.0):
    """Leader 的人工势场速度：目标引力 + 障碍斥力（在输出空间=世界系位置）。"""
    # 引力：指向目标，大小随距离（远处饱和避免过冲）
    to_goal = goal - pos
    dist = np.linalg.norm(to_goal) + 1e-9
    f_att = k_att * to_goal / dist * min(dist, 1.0)
    # 斥力：近距离障碍推开，超出影响半径则无作用
    f_rep = np.zeros(2)
    for obs in obstacles:
        diff = pos - obs
        d = np.linalg.norm(diff) + 1e-9
        if d < rep_radius:
            f_rep += k_rep * (1.0/d - 1.0/rep_radius) / (d**2) * (diff / d)
    return f_att + f_rep

def impedance_target(follower_pos, follower_vel, desired_pos, desired_vel,
                     local_obstacles, Md=1.0, Bd=4.0, Kd=6.0,
                     k_obs=3.0, obs_radius=1.0):
    """Follower 的阻抗：把它拉向期望队形位置（虚拟弹簧-阻尼），
       同时对【自己可达集相关的】局部障碍建临时阻抗（额外斥力）。
       返回期望加速度（再交给非完整跟踪器实现——执行层）。"""
    # 阻抗弹簧-阻尼：拉回期望队形位置
    f_imp = Kd * (desired_pos - follower_pos) + Bd * (desired_vel - follower_vel)
    # 临时阻抗链：仅对地面障碍（无人机会忽略的矮障碍）反应
    f_obs = np.zeros(2)
    for obs in local_obstacles:
        diff = follower_pos - obs
        d = np.linalg.norm(diff) + 1e-9
        if d < obs_radius:
            f_obs += k_obs * (1.0/d - 1.0/obs_radius) / (d**2) * (diff / d)
    return (f_imp + f_obs) / Md      # 期望加速度

def unicycle_track(state, acc_des, dt, k_v=1.0, k_w=2.0):
    """执行层：差速车用非完整控制器实现输出空间的期望加速度。
       state=(x,y,theta,v)，把期望加速度方向转成 (线加速, 角速度)。"""
    x, y, theta, v = state
    des_dir = np.arctan2(acc_des[1], acc_des[0])
    heading_err = np.arctan2(np.sin(des_dir - theta), np.cos(des_dir - theta))
    a = k_v * np.linalg.norm(acc_des) * np.cos(heading_err)  # 只沿车头方向加速
    omega = k_w * heading_err                                # 转向对齐期望方向
    v_new = v + a * dt
    theta_new = theta + omega * dt
    x_new = x + v_new * np.cos(theta_new) * dt               # 非完整运动学
    y_new = y + v_new * np.sin(theta_new) * dt
    return np.array([x_new, y_new, theta_new, v_new])

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

# ❌ 错误 1：在状态空间写刚性一致性，喂给非完整 follower
def rigid_consistency_WRONG(follower_state, leader_state, offset):
    # 直接要求 follower 瞬间到达 leader + offset 的位置
    target = leader_state[:2] + offset
    return target - follower_state[:2]   # 当成可瞬时达到的速度指令
# 问题：差速车不能瞬间横移。当 target 在车的侧方时，这个"速度指令"
#      要求横向速度——非完整约束下无法实现，跟踪器要么饱和要么发散。
#      根本原因：状态空间刚性约束假设全向可达，对非完整 agent 失效。
#      正确做法：阻抗（柔性）+ 非完整跟踪器（执行层用自己的动力学实现）。

# ❌ 错误 2：让 follower 对所有障碍都反应（包括无人机才需避的高障碍）
def impedance_WRONG(follower_pos, ..., ALL_obstacles):  # 传入全部障碍
    for obs in ALL_obstacles:   # ❌ 包括高空障碍
        ...
# 问题：地面车为高处的树枝绕路——但它在树下，本不该受影响。
#      浪费机动、破坏队形。根本原因：没按 agent 的可达集筛选相关障碍。
#      正确做法：每个 agent 只对自己可达集相关的障碍反应（地面车→地面障碍）。

# ❌ 错误 3：APF 斥力没有影响半径，全局生效
#   f_rep += k_rep / d**2 * (diff/d)   # 缺少 if d < rep_radius
# 问题：远处障碍仍施加微弱斥力，累积起来让 leader 轨迹持续偏移、
#      甚至陷入局部极小（被四面八方的微弱斥力锁住）。
#      正确做法：斥力必须有有限影响半径，超出则为零。

Step 4: 给出刚性 vs 柔性编队的对比

# === 异构编队：刚性约束 vs 阻抗（柔性）连接 对比 ===
# | 维度          | 刚性一致性 (y_i-y_j=d_ij) | 阻抗柔性连接 |
# |---------------|---------------------------|-------------|
# | 非完整 agent  | 不可行（要求瞬时横移）     | 可行（执行层用非完整跟踪）|
# | 遇障碍        | 整队妥协或求解失败         | 局部偏离（弹簧拉伸）后回弹 |
# | 异构可达集    | 被最弱者拖累              | 各按自己可达集反应 |
# | 队形精度      | 高（若可行）             | 略松（弹簧形变），但鲁棒 |
# | 计算          | 需联合优化（如第4章ADMM） | 轻量（局部反应式）|
# 结论：异构编队优先柔性连接——用一点队形精度换取对异构可达集和障碍的鲁棒。
#      第 4 章同构编队可用刚性（大家可达集相同，无需柔性吸收差异）。

⚠️ 常见陷阱¶

编程陷阱：把输出空间目标当状态空间指令直接下发给异构 agent

错误做法：一致性算出 follower 的期望世界系位置，直接当速度/位置指令喂给非完整车
现象：当期望位置在车侧方时，差速车无法直接横移，跟踪器饱和或发散，车原地打转
根本原因：跳过了"执行层"——输出空间目标必须经由 agent 自己的（异构）控制器
         转成它动力学可行的指令，不能绕过动力学直接当指令
正确做法：协调层只给"期望输出"（世界系位置/速度），
         每个 agent 用自己的控制器（非完整跟踪/微分平坦）去实现
自检方法：检查下发给 agent 的指令是否是它动力学的合法输入
         （差速车应是 (v,ω) 或可达加速度，不是任意方向的位置跳变）

概念误区："异构编队就是给不同机器人设不同增益的同构编队"

错误理解：第 4 章编队加个增益调度，对不同机型调不同 PID 参数，就成异构编队了
现象：增益怎么调，非完整车在侧向机动时都跟不上，队形持续散开
根本原因：增益调度只能处理"参数异构"（同一模型不同参数），
         处理不了"结构异构"（不同模型、不同约束、不同可达集）。
         非完整 vs 全向是结构差异，不是参数差异（呼应 §6.1 思维陷阱）
正确理解：异构编队的关键不是调参，而是架构改变——把一致性从状态空间
         上移到输出空间，劈成"协调层 + 各自执行层"。这是结构性改动，不是调参

思维陷阱：追求刚性队形精度，牺牲对异构可达集的适应

错误做法：为了队形"好看"，用刚性约束逼所有 agent 严格保持几何
现象：地面车为了维持与无人机的精确相对位置，被迫做非完整车做不好的机动，
     或为无人机能飞过的障碍绕远路，整体效率反而暴跌
根本原因：把"队形精度"当成了目标本身，忘了队形是为任务服务的手段。
         异构成员可达集不同，强求刚性精度等于强求"削足适履"
正确理解：异构编队应优先柔性（阻抗）——允许队形在遇障/机动时临时形变，
         换取每个成员都能扬长避短。队形是约束，不是目标

练习¶

（实现 + 实验） 用本节代码搭一个 2D 异构编队：1 个 leader（APF，全向近似）+ 2 个 follower（1 个差速车用 unicycle_track，1 个全向平台直接位置控制）。让编队穿过一个有"矮障碍"（只挡地面车）的走廊。验证：(a) 全向平台和差速车都绕开矮障碍，但 leader（无人机）轨迹不受影响；(b) 对比用刚性约束（错误写法 1）时差速车的行为——它能跟上吗？把阻抗刚度 $K_d$ 从小调到大，观察"柔性→刚性"过渡时差速车跟踪质量和队形保持的此消彼长。
（推导题） 证明：对一个全向、全驱动的 agent（$\ddot{p}=u$，输出 $y=p$），输出空间一致性 $y_i - y_j = d_{ij}$ 与状态空间一致性 $p_i - p_j = d_{ij}$ 等价。这说明第 4 章的同构编队是异构编队的特例（当所有 agent 全向全驱动时，输出空间退化为状态空间）。然后说明：对非完整 agent，为什么这个等价性破裂？（提示：考虑输出映射 $h$ 是否可逆、可达集是否满秩。）
（跨章综合题，串联第 2、4、6 章） 第 2 章的共识、第 4 章的 ADMM 编队、本节的输出空间一致性，本质上都是"让一群 agent 在某个量上达成协调"。请用一张表对比：(a) 它们各自"协调的对象"是什么（标量平均 / 状态相对位置 / 输出相对几何）？(b) 它们对 agent 同构性的要求分别是什么？(c) 当 agent 异构时，第 2 章的共识和第 4 章的 ADMM 各需要做什么改造才能用？把你的分析与本节"协调/执行分离"的元原则联系起来，说明这个原则如何统一地解释三者在异构情形下的改造方式。

§6.5 跨模态信息融合 ⭐⭐⭐⭐¶

这一节解决什么问题：异构队伍要协同，前提是它们对世界有**一致的认识**——无人机看到的目标和地面车看到的同一个目标，必须能对应到同一个世界坐标。但无人机鸟瞰、地面车平视，用的传感器还可能不同（相机 vs 激光），它们看到的"世界"在外观上几乎没有交集。这一节讲透异构感知融合的两道根本鸿沟——视角鸿沟（viewpoint gap）**和**模态鸿沟（modality gap）——为什么简单的"各自定位 + 位姿广播"彻底失效，并给出三条把异构感知拼成一致世界模型的技术路线：2.5D 高程图配准、深度学习描述子检索、半分布式相对定位。这是本章最硬核、也最能体现"异构 ≠ 同构拼接"的一节。

动机：各自定位很准，拼在一起全错位¶

"如果跳过本章"场景二已经埋了这个坑，现在正面拆解。

一个看似显然的方案：让无人机和地面车各自跑 SLAM（各自定位 + 建图），然后把各自估计的位姿广播出去，按位姿把两张地图拼到一起。在**同构**队伍里（两台一样的地面车），这个方案常常能凑合工作——它们视角相近、传感器相同，有大量共视特征，回环检测能把两个坐标系对齐。

但在**异构空地**队伍里，这个方案会惨败：

无人机在 80 米高空下视，看到的是屋顶、道路、树冠——一片俯视的纹理。地面车在地面平视，看到的是墙面、门窗、家具、行人腿——一片侧视的结构。这两套观测之间几乎没有任何共视特征：屋顶和墙面是同一栋楼的不同面，道路的俯视纹理和地面车看到的路面侧视完全不同。即便用最强的深度学习特征描述子，也匹配不上——因为同一个物理点在鸟瞰和平视下的外观差异，远超描述子的不变性范围。

没有共视特征 → 无法回环 → 两个独立 SLAM 的坐标系无法对齐 → 各自的位姿在各自的、漂移的、互不相干的坐标系里 → 广播出来的位姿拼不到一起。结果就是"各自都很准，拼起来差两米"。

这背后是异构感知的两道鸿沟，它们是这一节要攻克的核心。

两道鸿沟：视角鸿沟与模态鸿沟¶

视角鸿沟（viewpoint gap）：同一个物体/场景在鸟瞰和平视下的外观差异巨大，导致跨视角的特征匹配失效。这是几何层面的鸿沟——观测的是同一个三维世界，但投影视角差了近 90 度，二维外观几乎无重叠。

模态鸿沟（modality gap）：不同传感器（相机 vs 激光雷达 vs 热成像）产生的数据类型根本不同（图像 vs 点云 vs 温度场），无法直接比对。这是数据层面的鸿沟——连数据结构都不一样，谈何匹配。

这两道鸿沟可以单独出现，也可以叠加（无人机相机 vs 地面车激光，视角 + 模态双鸿沟）。它们的存在使得"位姿广播 + 地图拼接"这个对同构系统有效的方案，对异构系统**结构性地失效**。

本质洞察：同构多机的信息融合和异构多机的信息融合，难度不是量的差异而是质的差异。同构融合的核心挑战是"数据关联 + 一致性估计"（找到共视、融合估计），是一个已被充分研究的问题（多机 SLAM、协同定位）。异构融合在数据关联之前先要跨越"鸿沟"——当根本没有共视特征、连数据类型都不同时，传统数据关联无从下手。异构融合的真正难点在'鸿沟'，不在'融合'——一旦跨越鸿沟建立了对应，后面的融合反而是相对成熟的部分。这就是为什么这一节是 ⭐⭐⭐⭐：它要解决的是一类同构系统里根本不存在的问题。

历史：从协同 SLAM 到跨模态定位¶

多机感知融合的发展可以看作"逐步跨越鸿沟"的历史：

同构协同 SLAM（2010s）：多个相同/相近的机器人共享特征、联合优化位姿图。前提是大量共视——无鸿沟或弱鸿沟。代表如多机 ORB-SLAM、CoSLAM。
2.5D 地图配准（2010s 中后期）：意识到鸟瞰和平视无法直接特征匹配，转而在**几何中间表示**上对齐——把无人机视觉重建的 3D 点云和地面车激光的 3D 点云都投影成 2.5D 高程图（elevation map），用 ICP（Iterative Closest Point）对齐高程图。绕过了外观鸿沟，改在几何上对齐。代表如基于 2.5D 地图的空地协同定位（Sensors 2017 等）。
深度学习跨视角描述子（2020s）：用学习的方法把鸟瞰和平视图像编码到一个**共享的嵌入空间**，让同一地点的鸟瞰和平视描述子在嵌入空间里接近——直接攻克视角鸿沟。代表如跨视角地理定位（cross-view geo-localization）、空地 place recognition。
半分布式相对定位（2025）：最新思路是**解耦**——不强求所有机器人在一个全局坐标系里，而是估计机器人**两两之间的相对位姿**，且把相对定位与各自的状态估计解耦。UGV 用 LiDAR/相机/IMU 局部 BA 快速求相对位姿，配合深度描述子做增量回环。代表如"Semi-distributed Cross-modal Air-Ground Relative Localization"（2025）。

这条线的主线是：从"消除鸿沟"（找共视）走向"绕过鸿沟"（几何中间表示）再走向"跨越鸿沟"（学习共享嵌入）和"回避全局对齐"（只求相对位姿）。下面把三条主流技术路线展开。

路线一：2.5D 高程图配准——在几何上绕过外观鸿沟¶

核心思想：既然鸟瞰和平视的**外观**无法匹配，那就退到它们共有的**几何**上对齐——三维世界的几何结构（地形高度、建筑轮廓）是视角无关的。

流程：

各自生成 3D 点云：无人机用下视相机做多视图几何（或深度相机）生成场景 3D 点云；地面车用激光雷达（或滚动 2D 激光累积）生成 3D 点云。
投影成 2.5D 高程图：把各自的 3D 点云投影到水平网格，每个网格存高度值（"2.5D" = 2D 网格 + 每格一个高度，介于 2D 和完整 3D 之间）。高程图是视角无关的——无论从天上还是地面看，同一片地形的高度分布是一样的。
ICP 对齐：用 ICP 把地面车的高程图配准到无人机的高程图（或共同配准到一个全局高程图），求出两者坐标系之间的相对变换 $T$。
融合定位：有了 $T$，两个机器人的位姿就被锚定到同一坐标系，可以互相提供定位约束（如 GPS 拒止环境下，无人机的全局位姿通过 $T$ 传递给地面车）。

为什么有效：高程图把"外观匹配"问题转化为"几何对齐"问题。屋顶纹理和墙面纹理无法匹配，但"这片区域有一栋 10 米高的楼"这个几何事实，无人机和地面车都能观测到——几何是它们的公共语言。

多视角理解（类比）：2.5D 高程图配准的思路，和你在地理课上读"等高线地形图"高度相似——无论你站在山顶俯瞰还是站在山脚仰望，那张等高线图描述的"哪里高、哪里低"是同一个客观事实，与你的观察视角无关。高程图就是机器人版的等高线图：它剥离了"从哪个角度看"（外观随视角剧变），只保留"这片地形的高度分布"（几何随视角不变）。相似之处在于：都用一个视角无关的几何表示，让不同位置的观察者能对同一片地形达成一致。但这个类比的边界在于：等高线图是人预先测绘好的静态产物，而机器人的高程图是各自实时从点云投影出来的、带噪声和缺失（未观测区为 NaN）的估计——所以还需要 ICP 去对齐两份不完美的高程图，而不是像查同一张地图那样直接共享。不要把"高程图是公共语言"误解成"两个机器人查的是同一份地图"——它们查的是各自测的、需要配准才能对齐的两份地图。

局限：需要场景有足够的几何特征（高度变化）；平坦无特征区域（如空旷停车场）高程图退化，ICP 无法收敛；点云生成本身有误差。

理论-工程桥接：2.5D 高程图配准用 ICP，而 ICP 的本质是在两个点集间反复"找最近点对应 + 求最优刚体变换"（你在 SLAM 章学过）。这里复用得恰到好处——异构感知融合没有发明新的几何工具，而是把成熟的 ICP 用在了"高程图"这个精心选择的、视角无关的中间表示上。异构融合的工程艺术，很大程度上是'选对中间表示'的艺术——选一个让异构观测变得可比的公共表示（高程图就是这样的表示），后面的对齐就能复用成熟工具。

路线二：深度学习跨视角描述子——直接跨越视角鸿沟¶

核心思想：用神经网络学一个编码器，把鸟瞰图像和平视图像都映射到一个**共享嵌入空间**，使得同一地点的鸟瞰描述子和平视描述子在嵌入空间里距离近，不同地点的远。

流程：

训练跨视角编码器：用配对数据（同一地点的鸟瞰 + 平视图像对）训练，损失函数（如对比损失/三元组损失）拉近正样本对、推远负样本对。网络学会忽略视角差异、抓住地点本质。
描述子检索：地面车提取当前平视图像的全局描述子，在无人机建立的鸟瞰描述子数据库里检索最近邻——找到"地面车现在在无人机地图的哪个位置"。
位姿求解：检索到对应后，用几何方法（PnP / 配准）求精确相对位姿。

近期工作（2025 半分布式相对定位）进一步把这个思路做成增量式：UGV 和 UAV 各自提取基于深度学习的**关键点 + 全局描述子**，UGV 用局部 BA（LiDAR + 相机 + IMU）快速获得精确相对位姿，增量式回环检测维护和检索关键帧。关键创新是**把相对定位与所有 agent 的状态估计解耦**——不需要把所有机器人塞进一个联合优化，灵活性和精度都更好。

为什么有效：神经网络的表征能力可以学到"视角不变的地点特征"，直接在外观层面跨越视角鸿沟，不需要 2.5D 那样退到几何。

局限：需要训练数据（配对的跨视角图像，获取成本高）；泛化性受训练分布限制（换个城市/场景可能失效）；对模态鸿沟（相机 vs 激光）不直接适用，需额外设计。

路线三：相对定位解耦——回避全局对齐难题¶

核心思想：很多协同任务其实**不需要全局一致坐标系**，只需要知道机器人**两两之间的相对位姿**。比如"无人机引导地面车到目标"，只要无人机知道目标相对地面车的位姿即可，不需要二者都在一个全局系里。

把问题从"全局定位"降级为"相对定位"有两个好处：

避开全局对齐的病态：全局对齐要求消除所有 agent 的累积漂移并对齐到一个公共原点，异构鸿沟下极难；相对定位只需局部、瞬时的相对关系，宽容得多。
解耦状态估计：每个 agent 独立做自己的 SLAM（不互相耦合），相对定位作为一个**独立的、解耦的**模块，输入各自的 SLAM 输出和跨模态观测，输出相对位姿。这就是 2025 半分布式工作的"semi-distributed"——估计是分布式的（各自 SLAM），但相对定位有一个解耦的协调环节。

为什么有效：它承认并回避了异构全局对齐的根本困难，把目标降到"够用就好"——很多协同任务确实只需相对关系。这是一种务实的工程智慧。

三条路线对比¶

路线	攻克的鸿沟	核心工具	需要训练	局限
2.5D 高程图配准	视角（绕过到几何）	ICP + 高程图	否	需几何特征；平坦区退化
深度跨视角描述子	视角（直接跨越）	神经网络嵌入 + 检索	是	需配对数据；泛化受限
相对定位解耦	回避全局对齐	局部 BA + 解耦协调	部分	只给相对位姿，无全局系

对比性思维（不是 X 而是 Y）：异构感知融合的目标**不一定是**"建立一个所有机器人共享的、全局一致的世界地图"，而往往只是"让需要协作的机器人对它们协作所需的那部分世界达成一致"。前者是 SLAM 式的全局野心（在异构鸿沟下极难、常常过度），后者是任务驱动的务实（只对齐协作必需的相对关系）。想清楚"我的任务到底需要全局一致，还是只需相对一致"，是选对融合路线的第一步——很多时候，路线三（相对定位）就够了，根本不必去啃路线一、二的全局对齐硬骨头。

代码：跨模态融合的骨架（2.5D 配准 + 相对位姿）¶

下面给出 2.5D 高程图配准的核心骨架（简化的 2D ICP 演示思想，实际用 Open3D 的 3D ICP）。

Step 1: 先讲为什么选高程图作中间表示

为什么不直接 ICP 原始点云，而要先投影成 2.5D 高程图？

直接 ICP 两个原始点云（无人机视觉点云 vs 地面激光点云）会失败，因为：
(1) 密度天差地别：无人机视觉点云稀疏不均，地面激光稠密——ICP 的最近点对应被密度差污染；
(2) 覆盖不同：无人机看到屋顶（地面车看不到），地面车看到墙面下部（无人机看不到）——
    大量点根本无对应，ICP 被这些无对应点带偏。
高程图把两者归约到"每个水平网格的高度"——
密度被网格化抹平，只比较共同覆盖的水平区域的高度分布，
让 ICP 在一个干净的、可比的表示上工作。这就是"选对中间表示"。

Step 2: 给出正确写法

import numpy as np

def pointcloud_to_elevation(points, grid_res=0.5, bounds=None):
    """把 3D 点云投影成 2.5D 高程图：每个水平网格存最高点高度。
       points: (M,3) 的 [x,y,z]。返回 (H,W) 高程网格 + 原点。"""
    if bounds is None:
        xy_min = points[:, :2].min(axis=0)
        xy_max = points[:, :2].max(axis=0)
    else:
        xy_min, xy_max = bounds
    size = np.ceil((xy_max - xy_min) / grid_res).astype(int) + 1
    elev = np.full((size[1], size[0]), np.nan)        # NaN = 未观测
    idx = ((points[:, :2] - xy_min) / grid_res).astype(int)
    for (cx, cy), z in zip(idx, points[:, 2]):
        if np.isnan(elev[cy, cx]) or z > elev[cy, cx]:
            elev[cy, cx] = z                          # 取最高（屋顶/树冠）
    return elev, xy_min

def icp_2d(src, dst, max_iter=50, tol=1e-5):
    """2D ICP：把 src 点集配准到 dst，返回旋转 R、平移 t。
       (高程图配准的核心——求两坐标系的相对变换。实际 3D 用 Open3D。)"""
    from scipy.spatial import cKDTree
    R, t = np.eye(2), np.zeros(2)
    src_cur = src.copy()
    prev_err = np.inf
    for _ in range(max_iter):
        tree = cKDTree(dst)
        dists, idx = tree.query(src_cur)              # 最近点对应
        matched = dst[idx]
        # 求最优刚体变换（Procrustes / Kabsch）
        src_c = src_cur - src_cur.mean(0)
        dst_c = matched - matched.mean(0)
        H = src_c.T @ dst_c
        U, _, Vt = np.linalg.svd(H)
        R_step = Vt.T @ U.T
        if np.linalg.det(R_step) < 0:                 # 防止反射
            Vt[-1] *= -1
            R_step = Vt.T @ U.T
        t_step = matched.mean(0) - R_step @ src_cur.mean(0)
        src_cur = (R_step @ src_cur.T).T + t_step      # 更新
        R, t = R_step @ R, R_step @ t + t_step
        err = dists.mean()
        if abs(prev_err - err) < tol:                  # 收敛
            break
        prev_err = err
    return R, t   # 地面车系 → 无人机系 的相对变换

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

# ❌ 错误 1：直接广播位姿、按各自 SLAM 坐标拼接（无任何配准）
def naive_fusion_WRONG(uav_pose, ugv_pose, uav_map, ugv_map):
    # 假设两个 SLAM 的坐标系一致，直接拼
    return merge(uav_map, place(ugv_map, ugv_pose))   # ❌
# 问题：两个独立 SLAM 的坐标系原点、朝向、尺度都不同且各自漂移，
#      直接拼接错位严重。这就是"各自准、拼起来差两米"的根源。
#      根本原因：跳过了坐标系对齐（配准）这一异构融合的核心步骤。

# ❌ 错误 2：用图像特征描述子跨视角匹配（无视视角鸿沟）
def cross_view_orb_WRONG(uav_img, ugv_img):
    kp1, des1 = orb.detectAndCompute(uav_img, None)   # 鸟瞰
    kp2, des2 = orb.detectAndCompute(ugv_img, None)   # 平视
    matches = bf.match(des1, des2)                     # ❌ 几乎全是错匹配
    return matches
# 问题：ORB/SIFT 等手工描述子的视角不变性远不足以跨越 90° 视角差。
#      屋顶和墙面的描述子根本对不上，匹配全是噪声。
#      正确做法：用视角无关的几何（路线一）或学习的跨视角描述子（路线二）。

# ❌ 错误 3：ICP 直接配准原始点云（不投影高程图，密度/覆盖差异污染）
#   R, t = icp_2d(uav_raw_cloud, ugv_raw_cloud)  # ❌
# 问题：密度差和覆盖差让最近点对应大量错误，ICP 收敛到错误解或发散。
#      正确做法：先投影成高程图（密度归一、只比共同覆盖区）再配准。

Step 4: 给出三条路线的选择对比

# === 跨模态融合三路线的选择决策 ===
def choose_fusion_strategy(has_geometric_features, has_training_data,
                           needs_global_frame, modality_same):
    """按场景条件选融合路线（对应正文三条路线）。"""
    if not needs_global_frame:
        return "路线三：相对定位解耦（只需相对位姿，回避全局对齐）"
    if has_geometric_features and not modality_same:
        return "路线一：2.5D 高程图配准（几何中间表示，跨模态友好）"
    if has_training_data and modality_same:
        return "路线二：深度跨视角描述子（直接跨越视角鸿沟）"
    if has_geometric_features:
        return "路线一：2.5D 高程图配准（最通用、无需训练）"
    return "警告：无几何特征、无训练数据、需全局系——异构融合在此条件下极难，"\
           "考虑加人工标志（fiducial markers）或 GPS 锚点降低难度"

# 对比：
# | 条件                        | 推荐路线 |
# |-----------------------------|---------|
# | 只需相对位姿                 | 路线三  |
# | 需全局系 + 有几何 + 跨模态    | 路线一  |
# | 需全局系 + 有训练数据 + 同模态 | 路线二  |
# | 需全局系 + 有几何（通用兜底）  | 路线一  |
# | 啥都没有                     | 加 fiducial/GPS 锚点 |

⚠️ 常见陷阱¶

编程陷阱：用手工图像描述子做跨视角匹配

错误做法：对无人机鸟瞰图和地面车平视图各提 ORB/SIFT，用暴力匹配求对应
现象：匹配点几乎全是错的，求出的相对位姿完全错误，融合后地图错位更严重
根本原因：手工描述子（ORB/SIFT）的视角不变性是为"小视角变化"设计的，
         远不足以跨越鸟瞰 vs 平视的近 90° 视角鸿沟。同一物理点的鸟瞰和平视外观无重叠
正确做法：用视角无关的几何中间表示（2.5D 高程图 + ICP，路线一），
         或专门训练的跨视角学习描述子（路线二）
自检方法：可视化匹配点对——若匹配线杂乱无章（不符合任何刚体变换），即是跨视角失效

概念误区："异构融合就是把各自的位姿广播出来拼一拼"

错误理解：每个机器人都能自定位，广播位姿后按位姿拼地图即可
现象：各自定位精度都很高（cm 级），但拼接后两图错位米级，无法协作
根本原因：忽略了"坐标系对齐"——各自 SLAM 在各自的、独立漂移的坐标系里，
         位姿数值高精度但坐标系不共享，广播出来的数字没有公共基准
正确理解：异构融合的核心不是"融合估计值"，而是先"对齐坐标系"（跨越鸿沟）。
         没有共同坐标系，再准的位姿也拼不到一起。
         必须先配准（路线一/二）或改求相对位姿（路线三）

思维陷阱：不问任务需求，一律追求全局一致地图

错误做法：默认所有协同都需要一个全局一致的共享地图，不惜代价去做全局对齐
现象：在异构鸿沟下死磕全局对齐，耗费大量工程，而任务其实只需"无人机告诉地面车
     目标在哪"这种相对关系，全局地图是过度设计
根本原因：把"全局一致地图"当成了协同的必需品，没区分任务真正需要的一致性层级
正确理解：先问"我的协同任务到底需要全局一致还是相对一致？"
         引导/跟踪/相对避障类任务往往只需相对位姿（路线三，简单得多）；
         真正需要全局地图的（如多机协同建图）才上路线一/二。
         务实地匹配一致性层级与任务需求，避免在鸿沟上做无谓的全局对齐

练习¶

（实现 + 实验） 用本节代码（或 Open3D 的 3D ICP）做一个 2.5D 配准实验：生成两个有重叠的 3D 点云（模拟无人机视觉点云和地面激光点云，故意让它们密度不同、覆盖区域部分重叠），先各自投影成高程图，再用 ICP 配准。验证：(a) 配准后两个坐标系对齐；(b) 对照实验——直接 ICP 原始点云（错误写法 3），观察密度/覆盖差异如何让它失败。然后破坏场景的几何特征（把点云压平成一个平面），观察 ICP 如何退化（无法收敛或收敛到错误解），理解路线一"需几何特征"的局限。
（分析 + 设计） 对以下四个场景，分别判断存在哪些鸿沟（视角/模态/坐标系），并选择融合路线：(a) 两架无人机都用下视相机协同建图；(b) 无人机相机 + 地面车激光，GPS 拒止室内搜救；(c) 无人机引导地面车前往一个无人机看到的目标（户外）；(d) 无人机热成像 + 地面车 RGB 相机夜间协同。对每个场景说明你的判断依据和路线选择理由。
（跨章综合题，串联 SLAM + 第 6 章） 路线一的 ICP、路线三的局部 BA，都是你在 SLAM 章学过的几何工具。请说明：(a) 同构多机 SLAM 里这些工具怎么用（前提是什么）？(b) 异构融合里复用它们时，输入发生了什么关键改变（提示：从"原始观测"变成了"精选的中间表示"）？(c) 论证一个观点："异构感知融合没有发明新的几何优化工具，而是发明了让异构观测变得可比的'中间表示'和'问题降级'（全局→相对）"。结合 §6.4 的"输出空间一致性"，说明"找到合适的公共表示/公共空间"是不是贯穿整个异构协同的元方法——分配在能力空间、编队在输出空间、感知在几何中间表示，它们是不是同一种思想？

§6.6 异构协同操作：不对称 wrench cone 与内力分配 ⭐⭐⭐¶

这一节解决什么问题：第 5 章的协同搬运假设所有机器人在接触点的力可行集（摩擦锥）对称，内力均分。但异构操作里，一只手是夹爪（双边摩擦锥）、一只手是吸盘（只能拉）、一架无人机用绳索悬挂（只能沿绳拉）——它们能施加的力集合形状完全不同。这一节讲透异构接触如何让 grasp matrix 的内力分配从"对称均分"变成"在形状各异的 wrench cone 上做投影"，并连接到空中-地面联合抓取这类真实异构操作场景。这是把第 5 章的力分配理论异构化的关键一节。

动机：均分内力，吸盘却被要求"推"¶

第 5 章建立了协同操作的力学核心：$n$ 个机器人在物体上的接触点施加力/力矩，grasp matrix $G$ 把这些接触 wrench 映射到物体合力旋量 $w_{\text{obj}} = G f$。要让物体按期望运动，需要 $Gf = w_{\text{des}}$；这个方程通常欠定（接触自由度 > 物体 6 自由度），解空间是特解 + $G$ 零空间，零空间里的力是**内力（internal wrench）——不产生物体净运动，但维持抓取稳定（如对抗性的"夹紧力"）。第 5 章默认各 agent 力可行集对称，内力在各接触点**均分。

现在把接触异构化。考虑一个异构联合抓取：

三个 agent 共同搬一个箱子：左边是一台带夹爪的地面机械臂（夹爪能推能拉，双边摩擦锥），右边是一台带吸盘的机械臂（吸盘吸在箱子侧面，只能拉、不能推——你没法用吸盘"推"一个面），上方是一架用绳索吊住箱子的无人机（绳索**只能沿绳方向拉，且拉力必须为正**——绳子不能压）。

第 5 章的均分内力策略立刻出问题：内力的本质是"对抗性的夹紧"——一个 agent 推、对面 agent 推回来，互相抵消产生净零运动但夹紧物体。但在这个异构场景里：

吸盘**不能推**——如果均分策略要求它施加一个"推"的内力分量，它做不到（吸盘的力可行集里没有"推"这个方向）。
绳索吊的无人机**只能沿绳拉、且拉力为正**——如果均分要求它施加横向力或压力，它也做不到。

均分策略隐含假设"每个 agent 能施加任意方向的力"，这只对对称的双边摩擦锥成立。异构接触下，每个 agent 的力被限制在一个形状各异的可行锥里，均分会要求某些 agent 施加它们物理上无法施加的力。

如果沿用对称均分会怎样¶

把失败讲透。沿用第 5 章的均分内力，喂给异构接触，会发生：

吸盘端要求负压（推力）→ 物理不可实现：均分算出的吸盘内力含有指向物体的分量（推），但吸盘只能拉。控制器要么饱和（吸盘尽力但达不到要求），要么物体被另外两个 agent 的力推得偏离期望——抓取失稳。
绳索端要求侧向力或负张力 → 绳子做不到：绳子只能沿自身方向拉。均分要求的侧向分量绳子无法提供，负张力意味着"绳子推物体"（松弛），物体下坠。
物体净运动偏离期望：因为某些 agent 无法施加分配给它们的力，实际合力 $Gf \neq w_{\text{des}}$，物体不按计划动。

根本原因：均分把内力当成"可以任意分配的对抗力"，但异构接触下每个 agent 的可施加力被锁在各自的 wrench cone 里，分配必须尊重这些不对称约束。

历史：从对称抓取到异构联合操作¶

协同操作的力学（grasp matrix、内力、force closure）源自多指手抓取理论（Murray, Li, Sastry 的经典著作），那里假设各手指接触模型相同（点接触+摩擦、软接触等），可行力锥对称。机器人协同搬运（multi-robot cooperative manipulation）早期也沿用这个对称假设。

异构操作的需求随着空中操作和混合编队兴起。Michael 等（Auton. Robots 2014）的多四旋翼绳索悬挂协同搬运是一个标志——绳索接触是单边的（只能拉），这是协同操作里第一次大规模处理**不对称接触约束**。Tognon 等（RA-L 2019）的空中冗余操作进一步处理飞行基座的力约束。近年的工作（如 2025 的力自适应近-远机器人协同操作、多人形搬运）则把异构接触推向更复杂的组合（不同末端、不同基座动力学）。

这条线的核心进展是：从"对称可行锥下的内力均分"走向"不对称可行锥下的内力优化"——内力分配从一个解析的均分公式，变成一个带锥约束的优化问题。

理论：在不对称 wrench cone 上做内力分配¶

异构内力分配的正确表述是一个**带锥约束的二次规划（QP）**。

每个 agent 的 wrench cone¶

为每个 agent $i$ 定义它在接触点能施加的力/力矩集合——wrench cone $\mathcal{W}_i$：

夹爪（双边摩擦锥）：标准摩擦锥 $\{f : \|f_t\| \leq \mu f_n,\ f_n \geq 0\}$（法向可正可负取决于是否双边夹持，能推能拉）。
吸盘（单边 + 拉限）：只能拉，法向力 $f_n \leq 0$（指向远离物体），且切向受真空吸力限制 $\|f_t\| \leq \mu |f_n|$。
绳索悬挂（射线）：力只能沿绳方向 $\hat{d}$，且张力为正：$f = \lambda \hat{d},\ \lambda \geq 0$。这是最窄的可行集——一条射线。

这三个锥形状天差地别（实心圆锥 / 反向圆锥 / 一条射线），这正是异构的体现。

内力分配的 QP 表述¶

物体期望旋量 $w_{\text{des}}$，要求 $Gf = w_{\text{des}}$（$f$ 是所有接触 wrench 的堆叠）。内力分配问题：

\[ \begin{aligned} \min_{f} \quad & f^\top W f \quad (\text{最小化总力/能耗，或加权偏好}) \\ \text{s.t.} \quad & Gf = w_{\text{des}} \quad (\text{合力满足期望}) \\ & f_i \in \mathcal{W}_i, \quad \forall i \quad (\text{每个 agent 的力在各自可行锥内}) \end{aligned} \]

对比第 5 章：第 5 章的均分是这个 QP 在"所有 $\mathcal{W}_i$ 相同且对称"时的解析特例（对称性让最优解恰好均分）。异构时 $\mathcal{W}_i$ 各异、不对称，QP 没有解析均分解，必须数值求解（摩擦锥用二阶锥约束 SOCP，或多面体近似后用 QP）。

本质洞察：异构内力分配和第 6.2 节的能力硬约束、§6.4 的避障，本质上是同一个原则的不同实例——"物理上绝对不可以"的事情（吸盘不能推、绳子不能压、机器人不会飞）必须编码进可行集（约束的形状），而不是代价。 内力分配里，这个原则体现为"力必须落在 wrench cone 内"是硬约束，写进 QP 的约束而非目标。这就是为什么异构操作的内力分配必然是带锥约束的优化，而不能是一个均分公式——均分公式无法表达"某些方向的力绝对不可施加"这件事。整章反复出现的这个"硬约束进可行集"原则，在这里又一次统一了看似无关的问题。

异构带来的可行性问题¶

异构接触还引入一个同构操作里少见的问题：抓取可能不可行（infeasible）。如果各 agent 的 wrench cone 太窄、组合起来无法张成 $w_{\text{des}}$ 所需的方向，QP 无解——物体根本无法被这组异构接触按期望操作。

例如：三个 agent 全是绳索悬挂（都只能拉），它们能合成的力都是"向上拉"的组合，无法提供任何向下的净力——如果任务需要把物体往下压，这组抓取不可行。这就是为什么异构操作要检查**force closure（力封闭）**：各 agent 的 wrench cone 的组合（Minkowski 和）必须张成整个 wrench 空间（能抵抗任意方向的外部扰动）。前置自测第 4 题的答案在这里落地——异构接触的可行 wrench 空间是各 agent 可行集的 Minkowski 和。

对比性思维（不是 X 而是 Y）：异构协同操作的内力**不是**"对称分配给每个 agent 的对抗力"，而是"在各 agent 形状各异的可行锥约束下、为满足合力且保持抓取稳定而求出的优化解"。前者假设力可任意分配（同构对称），后者承认每个 agent 的力被锁在不对称的可行集里。把异构操作当对称操作处理，会要求 agent 施加它们物理上不可能的力，导致抓取失稳——这是异构操作最典型的失败模式。

代码：异构内力分配的 QP¶

下面给出异构内力分配的 QP 骨架（用简化的多面体锥近似 + 二次规划）。

Step 1: 先讲为什么必须用带约束 QP 而非均分公式

为什么不能像第 5 章那样用解析均分，必须解一个带锥约束的 QP？

第 5 章的均分能成立，依赖一个隐含条件：所有接触的力可行集相同且对称
（关于"推/拉"对称）。对称性让"最优内力恰好均分"成为解析结论。
异构接触下这个对称性消失——吸盘只能拉、绳子只能沿绳拉，可行集既不相同也不对称。
此时"均分"会落到某些 agent 的可行集之外（要求吸盘推、绳子压），物理不可实现。
所以必须显式地把"每个 agent 的力 ∈ 它的 wrench cone"写成约束，
在这些约束下优化——这只能是带约束 QP/SOCP，不存在解析均分公式。

Step 2: 给出正确写法

import numpy as np
from scipy.optimize import linprog, minimize

def build_wrench_cone_constraints(agent_type, contact_normal, mu=0.5, n_facets=4):
    """为每类异构接触构造 wrench cone 的线性（多面体近似）约束 A_i f_i <= 0。
       返回该 agent 力可行集的不等式约束矩阵。"""
    n = contact_normal / (np.linalg.norm(contact_normal) + 1e-9)
    if agent_type == "gripper":      # 双边摩擦锥：能推能拉，|f_t| <= mu|f_n|
        # 多面体近似：n_facets 个侧面 + 不限制法向符号（双边）
        return _friction_cone_facets(n, mu, n_facets, bilateral=True)
    elif agent_type == "suction":    # 吸盘：只能拉（f·n <= 0）+ 切向受限
        return _friction_cone_facets(n, mu, n_facets, bilateral=False, pull_only=True)
    elif agent_type == "cable":      # 绳索：力沿绳方向 d，张力 >= 0（一条射线）
        # 等式约束：f 平行于 n 且 f·n <= 0（拉），切向分量 = 0
        return _cable_ray_constraint(n)
    else:
        raise ValueError(f"unknown contact type {agent_type}")

def allocate_internal_force(G, w_des, agent_types, normals, W=None):
    """异构内力分配：min f'Wf s.t. Gf=w_des, f_i in wrench_cone_i。
       G: grasp matrix (6, sum_contact_dim)；返回各接触力 f。"""
    n_dim = G.shape[1]
    if W is None:
        W = np.eye(n_dim)              # 默认最小化总力平方
    # 组装每个 agent 的锥约束（不等式）
    A_ineq_blocks, b_ineq_blocks = [], []
    offset = 0
    for atype, nrm in zip(agent_types, normals):
        Ai, bi, dim = build_wrench_cone_constraints(atype, nrm)
        A_full = np.zeros((Ai.shape[0], n_dim))
        A_full[:, offset:offset+dim] = Ai     # 放到对应 agent 的列块
        A_ineq_blocks.append(A_full)
        b_ineq_blocks.append(bi)
        offset += dim
    A_ineq = np.vstack(A_ineq_blocks)
    b_ineq = np.concatenate(b_ineq_blocks)

    # QP: min 0.5 f'Wf  s.t. G f = w_des, A_ineq f <= b_ineq
    def obj(f):  return 0.5 * f @ W @ f
    def grad(f): return W @ f
    cons = [
        {"type": "eq",   "fun": lambda f: G @ f - w_des},          # 合力满足期望（硬）
        {"type": "ineq", "fun": lambda f: b_ineq - A_ineq @ f},    # 力在锥内（硬）
    ]
    res = minimize(obj, np.zeros(n_dim), jac=grad, constraints=cons, method="SLSQP")
    if not res.success:
        # 关键：异构抓取可能不可行——锥太窄无法合成 w_des
        raise RuntimeError("内力分配不可行：异构 wrench cone 无法张成期望旋量，"
                           "检查 force closure（各锥的 Minkowski 和是否覆盖所需方向）")
    return res.x

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

# ❌ 错误 1：沿用第 5 章的对称均分，不管接触类型
def equal_split_WRONG(G, w_des, n_contacts):
    f_particular = np.linalg.pinv(G) @ w_des       # 特解
    # 内力均分到各接触（第 5 章做法）
    null_basis = null_space(G)
    return f_particular + null_basis @ np.ones(null_basis.shape[1]) / n_contacts
# 问题：均分出的力可能落在吸盘/绳索的可行集之外（要求吸盘推、绳子压）。
#      物理不可实现 → 控制器饱和 → 抓取失稳。
#      根本原因：均分假设可行集对称，异构接触不满足。

# ❌ 错误 2：把锥约束当成软惩罚加进目标
def soft_cone_WRONG(G, w_des, ...):
    def obj(f):
        cone_violation = np.maximum(0, A_ineq @ f - b_ineq).sum()
        return 0.5 * f @ W @ f + 1e3 * cone_violation   # ❌ 软惩罚
    ...
# 问题：软惩罚下，优化器在"满足合力"和"不违反锥"之间权衡，
#      可能输出一个轻微违反锥的解（吸盘施加微小推力）——但物理上吸盘一点都推不了。
#      根本原因：力可行集是硬物理约束，必须硬约束（呼应整章"硬约束进可行集"）。

# ❌ 错误 3：不检查可行性，假设总有解
#   return minimize(...).x   # 不检查 res.success
# 问题：异构抓取可能根本不可行（如三根绳子无法提供向下力）。
#      不检查会返回一个无意义的失败解，物体操作失败且不知原因。
#      正确做法：检查 force closure / QP 可行性，不可行时明确报错。

Step 4: 给出同构 vs 异构内力分配的对比

# === 同构（第5章）vs 异构（本节）内力分配对比 ===
# | 维度          | 同构（对称摩擦锥）   | 异构（不对称 wrench cone） |
# |---------------|--------------------|--------------------------|
# | 内力分配      | 解析均分公式        | 带锥约束 QP/SOCP（数值）  |
# | 可行集        | 各 agent 相同、对称  | 各 agent 形状各异、不对称  |
# | 约束类型      | 对称摩擦锥          | 夹爪锥/吸盘半锥/绳索射线   |
# | 可行性        | 一般可行（对称足够） | 可能不可行（需查 force closure）|
# | "推/拉"对称   | 是                 | 否（吸盘只拉、绳子只拉）   |
# 结论：异构操作的内力分配是优化问题，不是公式；
#      且必须显式检查可行性——异构接触组合可能根本无法完成期望操作。

⚠️ 常见陷阱¶

编程陷阱：异构接触沿用对称内力均分

错误做法：用第 5 章的均分公式分配内力，不区分夹爪/吸盘/绳索
现象：吸盘被要求施加推力、绳子被要求施加压力或侧向力——物理不可实现，
     控制器饱和，物体偏离期望甚至脱落
根本原因：均分公式假设各接触力可行集对称，异构接触（单边/射线）违反此假设
正确做法：为每个 agent 构造其 wrench cone，解带锥约束的 QP/SOCP
自检方法：分配后检查每个力是否落在对应锥内（f_suction·n <= 0、f_cable 平行绳向且张力>0）

概念误区："只要有足够多的机器人，异构抓取总能完成任意操作"

错误理解：agent 越多，能合成的力越全，所以异构抓取总可行
现象：三根绳索吊一个箱子，任务要求把箱子向下压一下，怎么都做不到
根本原因：可行性取决于各 wrench cone 的 Minkowski 和能否张成所需方向，
         不取决于 agent 数量。三根只能拉的绳子，加再多根也提供不了向下的净力
正确理解：异构抓取的可行性由 force closure 决定（各锥的 Minkowski 和覆盖性），
         必须主动检查。某些方向若所有 agent 都无法提供，再多 agent 也不可行

思维陷阱：把内力分配的不可行当成求解器 bug

错误做法：QP 求解失败时，怀疑求解器配置/初值问题，反复调参重试
现象：换求解器、调初值、放松收敛阈值都没用，QP 始终无解
根本原因：可能不是数值问题，而是问题本身不可行——异构 wrench cone 组合
         无法满足 Gf=w_des。这是物理事实，不是求解器缺陷
正确理解：QP 不可行先检查 force closure（问题层面），而非纠结求解器（数值层面）。
         异构操作的"不可行"是常态而非异常，要在设计阶段就检查可行性
自检方法：单独验证"各锥的 Minkowski 和是否包含 w_des 方向"——
         若不包含，无论怎么调求解器都无解，需改变抓取配置（换接触点/加 agent 类型）

练习¶

（实现 + 实验） 用本节代码实现一个异构联合抓取的内力分配：1 夹爪 + 1 吸盘 + 1 绳索悬挂搬一个箱子。给定一个期望旋量（如水平移动 + 保持姿态），求解内力分配并验证：(a) 每个力落在各自锥内；(b) 合力满足期望。然后构造一个不可行实例（如要求一个所有 agent 都无法提供的方向的力），验证你的可行性检查正确报错。对比：把同一问题用第 5 章均分公式求解，检查均分结果违反了哪些锥约束。
（推导题） 对"三根绳索悬挂一个点质量"的情形，写出每根绳的 wrench cone（射线），求它们的 Minkowski 和。证明：这个和无法包含任何"向下的净力"方向（即无法把物体往下压），因此"向下压"任务对这组抓取不可行。然后说明：加入一个吸盘（吸在物体下表面，能向下拉物体即向上的反作用）后，可行 wrench 空间如何改变？
（跨章综合题，串联第 5、6 章 + §6.2/§6.4） 本节的"力进 wrench cone（硬约束）"、§6.2 的"能力硬剔除"、§6.4 的"避障硬约束"，都体现"物理不可能进可行集、代价高进目标函数"的原则。请：(a) 用一句话概括这个统一原则；(b) 对每个场景，指出什么是"物理不可能"（进约束）、什么是"代价高/有偏好"（进目标）；(c) 论证为什么把"物理不可能"错误地编码成"代价很大"在三个场景里都会导致同一类失败（优化器在边角情况下选中物理不可行解）。这个练习是为了让你建立一个跨越分配、编队、感知、操作的统一设计直觉。

§6.7 异构 MARL：HAPPO 与异构动作空间 ⭐⭐⭐⭐¶

这一节解决什么问题：前面几节用的是"模型驱动"的协同（分配、编队、操作都有显式模型）。但有些异构协同任务太复杂、交互太难建模，人们转向"学习驱动"——用多智能体强化学习（MARL）直接学策略。这立刻撞上一堵墙：无人机 4 维控制、四足 12 维关节，动作空间维度和语义都不同，标准的参数共享 MARL（如 MAPPO）根本搭不起网络。这一节讲透 HAPPO（Heterogeneous-Agent PPO）如何用**多智能体优势分解 + 顺序更新**处理异构动作空间，为什么它必须放弃参数共享（NoPS）才能保证单调改进，以及由此付出的样本效率代价。这是本章第二个硬核小节，也是异构协同在"学习"层面的核心。

动机：维度对不上，网络都搭不起来¶

回顾你学过的 MARL 基础（后续第 10 章会系统展开）。最常用的协作 MARL 算法是 MAPPO（Multi-Agent PPO），它在很多基准上表现优异。MAPPO 的一个关键工程实践是**参数共享（parameter sharing）**：所有 agent 共用一个策略网络 $\pi_\theta$，输入各自的观测、输出各自的动作。参数共享极大提升样本效率（所有 agent 的经验都用来训同一个网络），在**同构**多智能体里几乎是标配。

现在把 agent 异构化。一个"2 无人机 + 2 四足"的联合任务：

无人机的动作是 4 维（总推力 + 三轴力矩，或归一化的姿态+油门），四足的动作是 12 维（每条腿 3 个关节力矩）。你想用 MAPPO 的参数共享，让一个网络同时输出两类动作。问题立刻来了：网络的输出层维度该是 4 还是 12？ 4 维喂不动四足（缺 8 个关节指令），12 维对无人机多了 8 维（无意义）。动作空间维度不一致，参数共享的网络**结构上无法定义**。

即便你用 padding（把无人机动作补零到 12 维）勉强搭起网络，更深的问题暴露：无人机的第 5-12 维动作是无意义的填充，但网络会试图学习它们，梯度被污染；而且两类 agent 的动作**语义完全不同**（推力 vs 关节力矩），一个共享网络要同时学两套语义，训练几乎不收敛。

这就是异构 MARL 的根本困难：异构动作空间（维度 + 语义不一致）让参数共享失效。前置自测和"如果跳过本章"场景三都指向这里。

如果硬用参数共享会怎样¶

把失败讲透。强行参数共享 + padding 异构动作，会遇到：

填充维度的梯度污染：无人机被 padding 到 12 维，第 5-12 维是无效填充。但网络不知道它们无效，反向传播时这些维度也产生梯度，扰乱有效维度的学习。
语义冲突：同一个网络的同一个输出维度，对无人机是"推力"、对四足是"某关节力矩"——网络被迫在一个输出上叠加两种物理语义，学到的是两者的混乱平均。
不收敛或退化：实践中表现为训练曲线剧烈震荡或卡在低水平，因为网络在两套不兼容的动作语义间反复横跳。

根本原因：参数共享假设所有 agent 的策略可以用同一组参数表达，这要求它们的动作空间同构。异构动作空间打破了这个前提。

出路是**放弃参数共享**——让每个 agent（或每类 agent）有自己的策略网络，各自的输出维度匹配各自的动作空间。但这立刻带来一个新问题：多个独立更新的策略如何保证联合训练**稳定、单调改进**？这正是 HAPPO 要解决的。

历史：从 MAPPO 到 HAPPO¶

MARL 的协作算法发展有两条路线：

值分解路线（QMIX 等）：把联合 Q 分解为各 agent 的局部 Q，但有结构限制（单调性约束）。
策略梯度路线（MAPPO 等）：直接优化策略，参数共享提样本效率。在同构任务上 MAPPO 是强基线。

但 MAPPO 缺乏**理论上的单调改进保证**（它把单智能体 PPO 的 trick 套到多智能体，理论基础不牢），且参数共享假设同构。Kuba、Zhong 等（JMLR 2022 / ICLR 2022）提出的 Heterogeneous-Agent Reinforcement Learning（HARL） 框架——核心算法 HAPPO（和 HATRPO）——填补了这两个空白：

理论：证明了**多智能体优势分解引理（multi-agent advantage decomposition lemma），给出多智能体策略改进的**严格单调保证——这是 MAPPO 没有的。
异构：通过**顺序更新（sequential update scheme）+ 独立策略（无参数共享）**，天然支持异构 agent（每个 agent 有自己的策略，动作空间可以不同）。

后续工作（HAMDPO、HAA2C、HADDPG、HATD3 等）把这套框架扩展到连续/离散动作、确定性策略等。本节聚焦 HAPPO 的两个核心机制：优势分解和顺序更新。

理论：优势分解 + 顺序更新¶

HAPPO 的精妙之处在于：它把"同时更新所有 agent 策略"这个困难问题，转化为"按顺序逐个更新、每个 agent 更新时把前面 agent 的更新纳入考虑"。这让异构策略的联合优化既可行又有保证。

多智能体优势分解引理¶

核心理论工具。它把联合优势函数（joint advantage）分解为各 agent 的**顺序边际贡献**。对一个 agent 的排列 $i_1, i_2, \dots, i_n$，联合优势可以写成：

\[ A^{i_{1:n}}(s, a^{i_{1:n}}) = \sum_{m=1}^{n} A^{i_m}\big(s,\ a^{i_{1:m-1}},\ a^{i_m}\big) \]

直观地说：联合动作带来的总优势，等于"逐个 agent 加入时各自的边际优势之和"——agent $i_m$ 的边际优势是在**已知前面 $i_1, \dots, i_{m-1}$ 的动作**的条件下，它选择 $a^{i_m}$ 带来的额外优势。这和第 3 章 CBBA 的"边际收益"、§6.2 的联盟边际贡献是同一个"顺序边际"的思想——只不过这里是在策略空间里。

多视角理解（类比）：优势分解和 §6.2 异构 CBBA 的 bundle 构建在结构上高度相似——CBBA 里每个机器人按顺序把任务加入包，每个任务的价值是"在已有任务基础上的边际收益"；优势分解里每个 agent 按顺序更新策略，每个 agent 的改进是"在前面 agent 已更新基础上的边际优势"。相似之处在于：都用"顺序 + 边际"把一个耦合的联合问题拆成可逐个处理的序列。但不同之处在于：CBBA 的顺序是为了分布式可解（贪婪近似），优势分解的顺序是为了理论保证（严格单调改进）——前者是工程近似，后者是数学定理。理解这个"顺序边际"思想在分配和学习里的双重出现，是把握异构协同的一条暗线。

顺序更新方案¶

基于优势分解，HAPPO 的训练不是同时更新所有策略，而是**按顺序逐个更新**：

对每一轮训练：
  随机打乱 agent 顺序 i_1, i_2, ..., i_n
  设 M = 1（重要性比率累积量）
  for m = 1 to n:
      用 M（前面 agent 更新带来的联合策略变化）修正 agent i_m 的优势估计
      用 PPO 的裁剪目标更新 agent i_m 的独立策略 π^{i_m}
      把 agent i_m 的策略变化（重要性比率）累乘进 M，传给下一个 agent

关键点：

每个 agent 有独立策略 $\pi^{i_m}$（No Parameter Sharing, NoPS）——这天然支持异构动作空间，每个 $\pi^{i_m}$ 的输出维度匹配自己的动作空间（无人机的网络输出 4 维、四足的输出 12 维，各自独立）。
顺序更新 + 重要性修正：每个 agent 更新时，把前面 agent 已经做出的策略改变（通过累积重要性比率 $M$）纳入自己的优势估计——这保证了"我改进我的策略时，考虑了队友刚才的改进"，从而联合策略**单调改进**。

这两个机制配合，使得：(1) 异构动作空间被自然支持（独立策略）；(2) 联合训练有严格的单调改进保证（优势分解 + 顺序修正）。这正是 MAPPO（参数共享、无单调保证）做不到的。

NoPS vs ParPS 的根本张力¶

这是本节最深刻、也最实用的洞察。HAPPO 的单调改进保证**依赖 NoPS（无参数共享）**——优势分解和顺序更新的理论推导假设每个 agent 有独立策略。但实践中，纯 NoPS 有一个代价：

样本效率下降：每个 agent 单独学自己的策略，不能像参数共享那样"共享经验"。$n$ 个 agent 就要训 $n$ 个网络，每个只用自己的数据——样本效率远低于参数共享。

而**部分参数共享（Partial Parameter Sharing, ParPS）**——让同类 agent 共享部分参数——能恢复一些样本效率。但近期研究（如 2025 的相关工作）发现：直接把 ParPS 和 HAPPO 结合会引入**策略更新基线漂移（baseline drift）**问题——参数共享让 agent 的更新相互干扰，破坏了顺序更新赖以保证单调改进的独立性假设。

本质洞察：异构 MARL 面临一个深刻的张力——样本效率（要参数共享）与单调改进保证（要独立策略）不可兼得。参数共享让所有 agent 的经验汇集训练，样本效率高，但它假设策略同质、且破坏了 HAPPO 单调改进的理论前提；独立策略保证了单调改进和异构支持，但每个 agent 各学各的，样本效率低。这不是工程实现的缺陷，而是异构学习的**内在权衡**——你想要异构灵活性 + 理论保证，就得在样本效率上让步；你想要样本效率，就得牺牲一部分异构灵活性或理论保证。HAPPO 选择了前者（保证 + 异构，牺牲效率），这个选择在样本昂贵的真实机器人上是有代价的——这也是为什么"UAV + 四足的联合 HAPPO 训练至今没有大规模成功案例"。

另一条思路：Transformer 统一异构输入¶

除了 HAPPO 的"独立策略"路线，处理异构动作空间还有另一条思路：用 Transformer / attention 机制统一异构输入输出。核心想法是把每个 agent 的观测/动作表示成**变长的 token 序列**，用 attention 处理任意维度的输入，输出层用 agent 类型相关的 head 解码成各自的动作。这样一个模型能处理不同维度的 agent，又保留了一定的参数共享（attention 主干共享、解码 head 分类型）。

这两条思路的对比是本节的一个开放性话题（也是骨架原文留的思考题）：

思路	异构支持方式	参数共享	单调保证	样本效率	成熟度
HAPPO（独立策略）	每 agent 独立网络	无（NoPS）	有（理论证明）	低	较成熟
Transformer 统一	变长 token + 分类型 head	部分（主干共享）	无显式保证	较高	探索中

对比性思维（不是 X 而是 Y）：处理异构动作空间，HAPPO 的策略**不是**"想办法把异构 agent 塞进一个共享网络"，而是"承认它们本质不同，给每个独立的策略，再用顺序更新协调它们的学习"。Transformer 路线反过来，不是"放弃共享"，而是"用足够灵活的架构（attention + 分类型 head）让共享重新可能"。两条路线代表两种哲学：HAPPO 拥抱异构（独立 + 协调），Transformer 弥合异构（灵活共享）。哪条更好至今没有定论——这正是异构 MARL 最活跃的前沿。

代码：HAPPO 顺序更新的骨架¶

下面给出 HAPPO 顺序更新的核心逻辑骨架（伪实现，聚焦机制而非完整 RL 管线）。

Step 1: 先讲为什么是顺序更新 + 独立策略

为什么不并行更新所有 agent，而要顺序更新？为什么每个 agent 独立网络？

并行更新（所有 agent 同时改策略）的问题：每个 agent 基于"队友旧策略"计算自己的改进，
但所有 agent 同时改了 → 实际环境是"所有队友都用新策略"，
各自的改进假设失效 → 联合策略可能变差（无单调保证）。这是 MAPPO 的隐患。
顺序更新：agent 逐个改，每个改的时候把"前面 agent 已经改了"纳入考虑（重要性修正 M），
所以它的改进是基于"队友的真实当前策略"——这才能保证联合单调改进。
独立网络（NoPS）：异构动作空间维度不同，无法共享输出层；
且顺序更新的理论保证依赖各策略独立。所以每个 agent（或每类）一个网络。

Step 2: 给出正确写法

import numpy as np

class HeteroAgent:
    """异构 agent：独立策略网络，动作维度匹配自己的动作空间。"""
    def __init__(self, obs_dim, act_dim, agent_type):
        self.act_dim = act_dim          # UAV=4, 四足=12 —— 各自不同！
        self.agent_type = agent_type
        self.policy = build_policy_net(obs_dim, act_dim)   # 独立网络（NoPS）

    def action_logprob_ratio(self, obs, act, old_policy):
        """重要性采样比率 π_new(a|o)/π_old(a|o)，用于 PPO 裁剪和顺序修正。"""
        return self.policy.logprob(obs, act) - old_policy.logprob(obs, act)

def happo_sequential_update(agents, rollouts, clip=0.2, lr=3e-4):
    """HAPPO 顺序更新：随机顺序逐个更新独立策略，用累积比率 M 修正优势。
       agents: 异构 agent 列表（动作维度可不同）。"""
    order = np.random.permutation(len(agents))     # 随机打乱 agent 顺序
    M = np.ones(rollouts.batch_size)               # 累积联合策略变化（初值 1）
    old_policies = [a.policy.clone() for a in agents]

    for m in order:
        agent = agents[m]
        obs, act = rollouts.obs[m], rollouts.act[m]
        # 关键：用 M（前面 agent 更新带来的联合变化）修正本 agent 的优势
        advantage = rollouts.joint_advantage * M    # 顺序修正后的优势
        # PPO 裁剪目标（在修正后的优势上）
        ratio = np.exp(agent.action_logprob_ratio(obs, act, old_policies[m]))
        clipped = np.clip(ratio, 1 - clip, 1 + clip)
        loss = -np.minimum(ratio * advantage, clipped * advantage).mean()
        agent.policy.step(loss, lr)                 # 更新本 agent 独立策略
        # 把本 agent 的策略变化累乘进 M，传给下一个 agent
        new_ratio = np.exp(agent.action_logprob_ratio(obs, act, old_policies[m]))
        M = M * new_ratio                           # 累积，供后续 agent 修正
    return agents

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

# ❌ 错误 1：参数共享 + padding 异构动作
class SharedPolicyWRONG:
    def __init__(self, obs_dim, max_act_dim=12):   # 统一到最大维度
        self.policy = build_policy_net(obs_dim, max_act_dim)
    def act(self, obs, agent_type):
        full = self.policy(obs)                     # 总是输出 12 维
        if agent_type == "uav":
            return full[:4]                         # ❌ 取前 4 维，后 8 维白算
        return full
# 问题：UAV 的第 5-12 维输出无意义但仍产生梯度，污染学习；
#      UAV 和四足共用网络，推力和关节力矩两套语义在同一权重上冲突。
#      根本原因：参数共享假设动作空间同构，异构维度/语义打破前提。

# ❌ 错误 2：独立策略但并行更新（丢掉顺序修正）
def parallel_update_WRONG(agents, rollouts):
    for agent in agents:                            # 所有 agent 同时更新
        advantage = rollouts.joint_advantage        # ❌ 不做 M 修正
        agent.policy.step(ppo_loss(agent, advantage))
# 问题：每个 agent 基于"队友旧策略"算改进，但所有人同时改了，
#      联合策略实际变化未被各自考虑 → 无单调保证，训练可能发散。
#      根本原因：丢掉了顺序更新的核心——用 M 把队友的更新纳入自己的优势修正。

# ❌ 错误 3：顺序更新但用固定顺序（不打乱）
#   order = range(len(agents))   # ❌ 总是同一顺序
# 问题：固定顺序让排在前面的 agent 系统性占优（总是先改、影响后面），
#      引入偏置。HAPPO 要求每轮随机打乱顺序以保证公平和理论性质。

Step 4: 给出 HAPPO vs MAPPO vs Transformer 的对比

# === 异构 MARL 三种方案对比 ===
# | 方案            | 异构动作 | 参数共享 | 单调保证 | 样本效率 | 适用 |
# |-----------------|---------|---------|---------|---------|------|
# | MAPPO + 共享    | ❌不支持 | 是      | 无      | 高      | 仅同构 |
# | HAPPO（NoPS）   | ✅独立网络| 无      | 有(证明) | 低      | 异构、要保证 |
# | Transformer 统一 | ✅token  | 部分    | 无显式   | 较高    | 异构、探索中 |
# 选型：
# - 同构 agent → MAPPO（最省样本）
# - 异构 agent + 要单调改进保证 → HAPPO（牺牲样本效率换保证）
# - 异构 agent + 样本宝贵 + 可接受无保证 → Transformer 统一（探索性）
def choose_marl(homogeneous, need_guarantee, sample_budget):
    if homogeneous:
        return "MAPPO + 参数共享"
    if need_guarantee:
        return "HAPPO（NoPS，独立策略 + 顺序更新）"
    return "Transformer 统一架构（部分共享，前沿探索）"

⚠️ 常见陷阱¶

编程陷阱：异构动作空间用参数共享 + padding

错误做法：把所有 agent 动作 padding 到最大维度，共用一个策略网络
现象：训练剧烈震荡或卡在低水平不收敛；填充维度学到无意义的值
根本原因：参数共享假设动作空间同构；padding 引入无效维度（梯度污染）+
         不同 agent 的动作语义冲突（推力 vs 关节力矩共用权重）
正确做法：每个 agent（或每类）独立策略网络（NoPS），输出维度匹配各自动作空间；
         用 HAPPO 的顺序更新协调独立策略的联合训练
自检方法：检查网络输出维度是否对每类 agent 都恰好匹配其动作空间（无填充）

概念误区："HAPPO 只是支持异构的 MAPPO"

错误理解：HAPPO = MAPPO 加个异构支持，本质一样
现象：以为可以随便在 HAPPO 里加参数共享提效率，结果单调改进保证失效、训练不稳
根本原因：HAPPO 的核心不是"支持异构"，而是"优势分解 + 顺序更新带来的单调改进保证"，
         这个保证严格依赖 NoPS（独立策略）。加参数共享会引入基线漂移、破坏保证
正确理解：HAPPO 和 MAPPO 是不同理论基础的算法——MAPPO 无单调保证、靠参数共享提效率；
         HAPPO 有单调保证、靠独立策略 + 顺序更新。二者在"参数共享"上是对立的设计选择，
         不能简单混搭

思维陷阱：忽视异构 MARL 的样本效率代价，盲目上 HAPPO

错误做法：看到"异构"就上 HAPPO，不评估样本预算
现象：在真实机器人（样本极其昂贵）上训 HAPPO，n 个独立网络各学各的，
     样本需求是参数共享的数倍，训练周期长到不现实
根本原因：忽略了 NoPS 的样本效率代价。HAPPO 的单调保证和异构支持是用样本效率换的
正确理解：上 HAPPO 前先问"我有多少样本预算？"——
         样本充足（仿真海量采样）→ HAPPO 的效率劣势可接受；
         样本宝贵（真机）→ 考虑 Transformer 统一（部分共享）或先在仿真训好再迁移。
         异构 MARL 的方案选择必须把样本预算作为一等约束

练习¶

（实现 + 实验） 实现 HAPPO 顺序更新的核心逻辑（可用 PettingZoo 的简单异构环境，或自定义一个"2 个动作维度不同的 agent"的玩具环境）。验证：(a) 顺序更新 + M 修正时联合回报单调上升；(b) 对照——去掉 M 修正（错误写法 2，并行更新）时训练是否震荡/发散。然后做一个消融：用固定顺序 vs 随机打乱顺序，观察对训练稳定性和公平性（各 agent 最终性能）的影响。
（分析题） §6.2 的异构 CBBA bundle 构建和本节 HAPPO 的顺序更新，都用"顺序 + 边际"处理耦合问题。请详细对比：(a) 二者的"顺序"分别是为了什么（分布式可解 vs 理论保证）？(b) 二者的"边际量"分别是什么（任务边际收益 vs 策略边际优势）？(c) CBBA 的顺序贪婪是近似（50% 界），HAPPO 的顺序更新是精确单调改进——为什么一个是近似一个是精确？（提示：考虑各自处理的是离散组合还是连续策略空间，以及是否做了重要性修正。）
（开放设计题，串联 §6.1/§6.7） 为一个"2 UAV（4 维动作）+ 2 四足（12 维动作）"的联合搬运任务设计 MARL 方案。要求：(a) 用 §6.1 的能力互补矩阵说明为什么这个任务需要异构团队（单一机型做不到）；(b) 在 HAPPO（独立策略）和 Transformer 统一架构之间做选择，给出你的判据（样本预算、是否需要保证、工程成熟度）；(c) 讨论一个 §6.5 的连接点——异构 agent 的观测也是异构的（UAV 鸟瞰、四足平视），你的 MARL 方案如何处理异构观测（每个 agent 独立编码器？共享的跨模态编码器？）？这个练习把异构的"动作空间""观测空间""能力"三处串起来。

§6.8 异构 vs 同构的本质边界与分层协同架构 ⭐⭐⭐¶

这一节解决什么问题：前面七节逐层讲了异构如何改写协同的每一面。这一节把它们收口——用一张五层对比总表，系统地回答"异构系统到底比同构系统多了哪些本质问题、哪些前五章的工具可直接复用、哪些必须改写"。然后给出一个统一的**分层协同架构**，把分配、感知、编队、控制、学习在异构系统里的关系组织起来，并提供选型决策框架。这一节是全章的认知收口——读完它，你脑中应该有一张"异构协同的完整地图"。

动机：异构不是同构的并集¶

整章反复强调一句话：异构系统不是同构系统的简单拼接。现在到了系统论证它的时候。

一个诱人的错觉是："异构系统无非是几种同构子系统放在一起，我把每种机型当一个同构小组分别处理，再拼起来就行。" 这个想法在最表层（如各机型各自的底层控制）部分成立，但在协同的每一个真正耦合的层面都会崩溃——因为**协同的本质是跨个体的耦合，而异构让这种耦合发生在"不可直接比较的个体"之间**。

让我们沿五个层面（分配、感知、编队、操作、学习）逐一对比同构与异构，看异构在每一层引入了什么同构系统里根本不存在的新问题。

理论：五层对比总表¶

这是全章最重要的一张表——它把前七节的核心区别浓缩到一处。

协同层面	同构系统怎么做	异构系统的新问题	异构系统怎么解	对应节
任务分配	所有 agent 可竞标所有任务，区别仅在代价；CBBA 有 50% 界	能力约束（谁有资格）、联盟任务（需多机拼能力，超可加破坏 DMG）	候选集硬剔除 + 联盟竞标/分解 + 能量代价；联盟部分界失效	§6.2
通信	同质节点、相近能力，拓扑由距离定	节点异构（通信半径、续航差异）；哪类节点当中继/leader	中继/mesh/LF 架构按节点能力分布选；公共语义层	§6.3
编队/运动	同一动力学模板，一致性写在状态空间	动力学结构异构（全向/非完整/欠驱动），状态空间不可比	一致性上移到公共输出空间；协调/执行分离；阻抗柔性连接	§6.4
感知	视角/模态相近，大量共视，位姿广播可拼	视角鸿沟 + 模态鸿沟，无共视，坐标系无法对齐	2.5D 几何中间表示 / 跨视角学习描述子 / 相对定位解耦	§6.5
操作	对称摩擦锥，内力均分（解析公式）	接触异构（夹爪/吸盘/绳索），wrench cone 不对称，可能不可行	带锥约束 QP；force closure 可行性检查	§6.6
学习	同构动作空间，参数共享，MAPPO	异构动作空间（维度/语义不同），参数共享失效	独立策略 + 顺序更新（HAPPO）；样本效率代价	§6.7

本质洞察：通读这张表，一个统一的模式浮现——异构在每一层引入的"新问题"，本质上都是"个体不可直接比较"导致的。分配里能力不可比（要硬约束筛选）、编队里状态不可比（要投影到公共输出）、感知里观测不可比（要找几何中间表示）、操作里力可行集不可比（要在各自锥上优化）、学习里动作空间不可比（要独立策略）。同构系统的所有协同算法都隐含一个"个体可比"的前提（可平均、可相减、可共享参数），异构打破了这个前提。异构协同的所有技术，归根结底都是在回答同一个问题：当个体不可直接比较时，如何找到一个让它们变得可比的"公共空间/公共表示"，并在那里协调。

这个洞察给出了贯穿全章的元方法——寻找公共空间（finding the common ground）：

层面	不可比的东西	找到的公共空间/表示
分配	异构能力	能力向量空间（§6.1 的 trait 向量）
编队	异构状态	公共输出空间（世界系位置）
感知	异构观测	几何中间表示（高程图）/ 学习嵌入空间
操作	异构力可行集	物体 wrench 空间（grasp matrix 的值域）
学习	异构动作	独立策略 + 顺序协调（不强求统一动作空间）
通信	异构私有状态	公共语义层（共享话题）

这就是为什么 §6.5 练习 3 让你论证"找到合适的公共表示是不是贯穿整个异构协同的元方法"——答案是肯定的，这张表就是证据。

分层协同架构：把五层组织起来¶

前面每层是分开讲的，但一个真实异构系统里它们必须协同工作。把它们组织成一个**分层协同架构**：

┌─────────────────────────────────────────────────────────┐
│  L4 任务层：能力感知分配（§6.2）                          │
│      输入：能力矩阵 Q、任务需求 Y                         │
│      输出：谁做什么（含联盟）                             │
│      公共空间：能力向量空间                               │
└──────────────────────────┬──────────────────────────────┘
                           ▼ 任务目标下发
┌─────────────────────────────────────────────────────────┐
│  L3 协调层：编队几何 / 联合操作规划（§6.4, §6.6）         │
│      输入：任务目标、各 agent 当前输出                    │
│      输出：每个 agent 的输出空间目标（期望位置/力）       │
│      公共空间：输出空间（位置）/ 物体 wrench 空间         │
└──────────────────────────┬──────────────────────────────┘
                           ▼ 输出目标下发
┌─────────────────────────────────────────────────────────┐
│  L2 执行层：各 agent 私有控制器（异构！）                 │
│      UAV: 微分平坦+SE(3)  |  四足: WBC  |  臂: 逆运动学    │
│      输入：输出空间目标   输出：各自的动力学可行指令       │
│      （这一层每类 agent 各说各话，无公共空间——也无需）    │
└──────────────────────────┬──────────────────────────────┘
                           ▲ 状态/感知回传
┌─────────────────────────────────────────────────────────┐
│  L1 感知层：跨模态融合（§6.5）+ 通信（§6.3）              │
│      输入：各 agent 异构观测                              │
│      输出：一致世界模型 / 相对位姿                        │
│      公共空间：几何中间表示 / 学习嵌入                    │
└─────────────────────────────────────────────────────────┘

这个架构的核心是**"上层找公共空间协调、下层各自私有执行"**——这正是 §6.4 的"协调/执行分离"原则推广到整个系统：L4/L3/L1 都在各自的公共空间里协调（能力空间、输出空间、几何表示），只有 L2 执行层是每类 agent 私有的、异构的、无需公共空间的。

理论-工程桥接：这个分层架构不是凭空设计的，而是被异构性"逼"出来的。同构系统里，协调和执行可以在同一层（同一个状态空间），因为大家可比；异构系统里，可比性只在精心选择的公共空间里成立，而执行必然在各自的私有空间，所以协调层和执行层必须分开。架构的分层，是个体不可比性的直接产物——这也解释了为什么异构系统普遍比同构系统架构更复杂（多了"投影到公共空间"和"从公共空间目标到私有执行"两道转换）。

选型决策框架：我的问题该怎么处理¶

把整章的方法论收成一个决策框架，面对一个真实异构问题时按它走：

异构协同问题选型决策：

第 1 步：沿三轴诊断异构性（§6.1）
  - 能力轴异构？→ 任务分配要能力感知（§6.2）
  - 动力学轴异构？且是结构异构（非参数）？→ 编队要输出空间一致性（§6.4）
  - 感知轴异构？有视角/模态鸿沟？→ 需跨模态融合（§6.5）

第 2 步：判断任务耦合强度
  - 任务可单机完成（只是筛选+加权）→ 异构 CBBA 安全（保 50% 界）
  - 任务需多机拼能力（超可加联盟）→ CBBA 界失效，上联盟形成/联合优化
  - 任务需紧密物理协同（合抬重物，强耦合）→ 第 5 章协同搬运 + §6.6 异构内力

第 3 步：判断是否需要学习
  - 任务可建模（动力学清晰）→ 优先模型驱动（§6.2/§6.4/§6.6，可解释、无需训练）
  - 任务难建模（复杂交互）→ 上 MARL；异构动作空间 → HAPPO（§6.7）

第 4 步：选通信架构（§6.3）
  - 高可靠性优先 → mesh
  - 大范围+远端弱通信 → 中继
  - 一强多弱+结构稳定 → leader-follower

第 5 步：确定一致性层级（§6.5）
  - 需全局一致地图 → 2.5D 配准 / 跨视角描述子
  - 只需相对关系 → 相对定位解耦（更简单，优先考虑）

⚠️ 常见陷阱¶

概念误区："把每种机型当同构小组分别处理再拼起来 = 异构协同"

错误理解：异构系统 = 几个同构子系统的并集，分别处理各机型再拼接
现象：各机型的底层控制都正常，但一旦需要跨机型协同（联合搬运、共享地图、
     编队机动），系统就出问题——拼接处全是 bug
根本原因：协同的本质是跨个体耦合，异构让耦合发生在不可比个体间。
         "分别处理再拼"只在无耦合的底层（L2 执行）成立，
         在所有真正耦合的层面（分配/感知/编队/操作/学习）都需要专门的"公共空间"机制
正确理解：异构协同的难点恰恰在"拼接处"——即跨个体的协调层。
         必须为每个耦合层面找到让异构个体可比的公共空间（五层对比表），
         而非假设拼接是平凡的

思维陷阱：在错误的层面处理异构性

错误做法：试图在执行层（L2）统一异构（如逼所有机型用同一控制器），
         或在协调层（L3/L4）保留私有差异（如让分配器理解每类机型的底层动力学）
现象（前者）：用统一控制器逼非完整车做全向机动——做不到；
     （后者）：分配器代码里塞满各机型的动力学细节，耦合爆炸、无法维护
根本原因：把"该统一的层"和"该分异的层"搞反了。
         异构性应在执行层（L2）保留（各自私有控制器），
         在协调层（L3/L4/L1）通过公共空间消解（投影到可比表示）
正确理解：分层架构的纪律是——协调层找公共空间求可比（消解异构），
         执行层各自私有（保留异构）。在协调层强行保留私有差异、
         或在执行层强行统一，都是层次错位

思维陷阱：过度异构化（同构问题用异构重武器）

错误做法：明明是参数异构（同模型不同 max_v）的简单情形，却上输出空间一致性、
         HAPPO 独立策略等异构重武器
现象：系统复杂度暴涨，但收益甚微——因为问题本来用同构方法加增益调度就够了
根本原因：没区分"参数异构"（同构方法即可）和"结构异构"（才需异构方法）。
         §6.1 的三轴诊断第一步就是判断每个轴是参数还是结构异构
正确理解：异构方法是有成本的（架构更复杂、样本效率更低、可行性更难保证）。
         只对"结构异构"的轴用异构方法，"参数异构"的轴用同构方法 + 调度。
         先诊断（§6.1），再按需施治，避免杀鸡用牛刀

练习¶

（综合分析题） 给定三个系统，分别判断它们"是真异构还是伪异构"，并说明各轴该用同构还是异构方法：(a) 5 台同型号但最大速度不同的 AGV；(b) 3 台地面车 + 2 架无人机的搜救队；(c) 10 台同型号无人机但分两组、装了不同传感器（相机 vs 激光）。对每个系统，填写一张"三轴诊断表"（能力/动力学/感知各是参数异构还是结构异构、需不需要异构方法）。
（架构设计题） 为"四足 + 无人机 + 固定臂"的建筑检测系统画出完整的四层协同架构（L1-L4），标注每层的输入/输出、公共空间、以及哪些前五章的工具可直接复用、哪些必须改写。特别说明：L2 执行层为什么不需要公共空间，而 L1/L3/L4 为什么需要。
（跨章综合题，全章收口） 本章反复出现一个元方法——"为不可比的异构个体找公共空间"。请：(a) 把全章六个层面（分配/通信/编队/感知/操作/学习）各自的"不可比的东西"和"找到的公共空间"列成表（可参考正文的元方法表，但要用自己的话重述每一项为什么是公共空间）；(b) 论证这个元方法和 §6.4 的"协调/执行分离"、§6.8 的"分层架构"是同一思想的不同表述；(c) 反思：是否存在某种异构性，**找不到**合适的公共空间？如果有，那类问题该怎么办？（这是一个开放性的研究级问题，没有标准答案——尝试构造一个例子并讨论。）

§6.9 端到端案例：四足 + 无人机联合搜救 ⭐⭐¶

这一节解决什么问题：前面八节是分层拆解，这一节把它们重新组装成一个**端到端的、可运行的异构协同场景**——四足 + 无人机联合搜救。它的目的是让你看到三轴异构（能力/动力学/感知）如何在一个真实系统里同时出现、并被前面各节的方法逐一打通。这一节也是本章累积项目（Mini-MultiBot 的异构模块）的落地，把分散的代码片段串成一个完整管线。

场景设定¶

灾后建筑搜救：一栋部分倒塌的多层建筑，需要快速定位可能的幸存者。可用资源：

1 架无人机（UAV）：下视相机，鸟瞰感知范围大，续航 20 分钟，载重极低，可飞越废墟。
2 台四足机器人（Quad）：激光雷达 + RGB，地面视角，越障能力强，续航 2 小时，中等载重，可钻进倒塌区。
1 个地面基站：计算和通信枢纽。

任务清单：(T1) 全区航拍生成俯视地图（需飞行）；(T2) 标记可疑热点区域；(T3) 地面进入可疑区搜寻（需越障）；(T4) 搬开挡路的轻型障碍（需载重，可能需两台四足联盟）。

三轴异构如何在这个场景同时出现¶

轴	本场景的体现	用哪节的方法打通
能力异构	UAV 能飞不能搬、Quad 能搬能越障不能飞；T4 可能需两台 Quad 联盟	§6.1 能力矩阵 + §6.2 异构 CBBA（能力约束 + 联盟）
动力学异构	UAV 欠驱动飞行、Quad 足式步态——可达集结构不同	§6.4 输出空间协调 + 各自执行层（UAV 微分平坦、Quad WBC）
感知异构	UAV 鸟瞰相机 vs Quad 平视激光——视角 + 模态双鸿沟	§6.5 跨模态融合（2.5D 配准把鸟瞰地图与地面激光对齐）

通信上，无人机续航短（20 min）、基站固定、四足续航长——这是一个"远端续航长节点 + 短续航空中节点"的结构，§6.3 建议用**混合架构**：四足当局部 leader（续航长、计算够），无人机当空中侦察 follower，基站经四足中继接入。

端到端管线¶

把全章方法串成一条管线：

阶段 0 [感知-L1]：UAV 航拍 → 生成鸟瞰 3D 点云 → 投影 2.5D 高程图（§6.5 路线一）
                 Quad 激光 → 地面 3D 点云 → 2.5D 高程图
                 → ICP 配准两图 → 建立 UAV↔Quad 相对变换 T（坐标系对齐）
                           │
阶段 1 [任务-L4]：在对齐的地图上标记任务（T1-T4）
                 → 能力矩阵 Q + 任务需求 Y → 异构 CBBA（§6.2）
                 → T1/T2 派 UAV（需飞行），T3 派 Quad（需越障），
                   T4 若需 60kg→两台 Quad 联盟（§6.2 联盟任务）
                           │
阶段 2 [协调-L3]：UAV 飞航拍航线、Quad 编队进可疑区（§6.4）
                 → 输出空间（世界系位置）协调，阻抗柔性连接
                 → Quad 绕地面障碍（临时阻抗链），UAV 不受影响
                           │
阶段 3 [执行-L2]：UAV 微分平坦生成轨迹、Quad WBC 步态控制
                 → 各自私有控制器实现 L3 的输出目标
                           │
阶段 4 [回传-L1]：Quad 发现热点 → 经相对变换 T 把位置传给 UAV/基站
                 → 共享语义话题 /shared/target_pose（§6.3 公共语义层）

下面给出把这条管线串起来的顶层编排代码骨架（调用前面各节实现的模块）。

import numpy as np

def heterogeneous_sar_pipeline(uav, quads, base_station, tasks):
    """四足+无人机联合搜救端到端编排（串联 §6.1-§6.8 的模块）。"""

    # === 阶段 0：跨模态感知融合（§6.5）—— 对齐坐标系 ===
    uav_cloud = uav.aerial_survey()                    # 鸟瞰 3D 点云
    uav_elev, uav_origin = pointcloud_to_elevation(uav_cloud)   # 2.5D（§6.5）
    quad_clouds = [q.lidar_scan() for q in quads]
    quad_elev, quad_origin = pointcloud_to_elevation(np.vstack(quad_clouds))
    R_rel, t_rel = icp_2d(elev_to_points(quad_elev), elev_to_points(uav_elev))  # 配准
    frame_transform = (R_rel, t_rel)                   # UAV↔Quad 相对变换

    # === 阶段 1：能力感知任务分配（§6.1 + §6.2）===
    robots = [uav] + quads
    Q = np.array([r.capability_vector() for r in robots])       # 能力矩阵（§6.1）
    task_demands = np.array([t.demand_vector() for t in tasks])
    required_dims = [t.required_dims() for t in tasks]
    energy_state = np.array([r.battery() for r in robots])
    # 异构 CBBA：能力硬约束 + 能量代价 + 联盟任务（§6.2）
    assignment = hetero_cbba(robots, tasks, Q, task_demands, required_dims,
                             energy_state, energy_model=sar_energy_model,
                             enable_coalition=True)             # T4 可能触发联盟

    # === 阶段 2+3：协调（§6.4）+ 执行（L2）===
    for robot, assigned in assignment.items():
        for task in assigned:
            if robot.type == "uav":
                # UAV：输出空间目标 → 微分平坦轨迹（私有执行层）
                wp = task.target_in_frame(frame_transform, to="uav")
                robot.fly_differential_flat(wp)                 # §6.4 执行层
            else:
                # Quad：输出空间目标 → 阻抗编队 + WBC（私有执行层）
                wp = task.target_in_frame(frame_transform, to="quad")
                acc_des = impedance_target(robot.pos, robot.vel, wp, np.zeros(2),
                                           robot.local_ground_obstacles())  # §6.4
                robot.walk_wbc(acc_des)
                if task.needs_coalition:                        # T4 两台联盟搬运
                    f = allocate_internal_force(task.grasp_matrix, task.w_des,
                                                task.agent_types, task.normals)  # §6.6
                    robot.apply_contact_force(f[robot.contact_idx])

    # === 阶段 4：跨模态回传（§6.3 公共语义层）===
    for q in quads:
        if q.found_hotspot():
            pos_world = transform_to_world(q.hotspot_pos(), frame_transform)
            base_station.publish("/shared/target_pose", pos_world)  # 公共语义（§6.3）

本质洞察：这段编排代码最值得注意的，不是它调用了多少模块，而是**每个模块之间传递的都是"公共空间里的量"——阶段 0 输出的 frame_transform 是坐标系对齐（公共几何空间），阶段 1 输入的是能力向量（公共能力空间），阶段 2 传给执行层的是 wp/acc_des（公共输出空间目标），阶段 4 发布的是 pos_world（公共语义）。**整条管线就是"在各种公共空间之间转换 + 在每个公共空间里协调"——这把 §6.8 的"寻找公共空间"元方法落到了最具体的代码层面。异构系统的工程，本质就是管理这些公共空间和它们之间的转换。

前沿工作与开放问题¶

把本章方法放到最新研究的坐标系里，看还有哪些没解决的硬骨头。

LLM 驱动的异构协调（RoCo, Mandi 等, ICRA 2024）：RoCo 把每个机器人委托给一个 LLM agent，让它们用自然语言"对话式"地分工——LLM 负责高层任务分解，底层用运动规划执行。它按三个性质组织协作：子任务能否并行/串行、所有 agent 是否获得相同任务状态信息、执行时机器人间的邻近度。这给异构协调提供了一个全新的"语义层"——不再用能力向量的数值匹配，而用 LLM 对任务和能力的语义理解来分工。在 RoCoBench 上 GPT-4 达到 86.7% 成功率。开放问题是：LLM 的语义分工如何与底层的物理约束（§6.6 的 wrench cone、§6.4 的可达集）严格对接？语义层的"看似合理"如何保证物理可行？

力自适应近-远协同操作（Force-ANTS 类工作，2025）：针对一个机器人在远处、一个在近处协同操作的异构场景，做力自适应的协调。这把 §6.6 的异构内力分配推向动态、自适应的方向——可行锥不仅形状各异，还随机器人相对位置变化。

异构形态的 MARL（多人形搬运, Pandit 等, CoRL 2025）：在多个人形机器人搬运载荷的任务上，用去中心化策略学出异构形态的协调涌现。它部分回应了 §6.7 的开放问题，但仍局限于"同类形态"（多人形）；真正异构形态（UAV + 四足）的联合 MARL 训练至今没有大规模成功案例——这是 §6.7 留下的、本领域最硬的开放问题之一。HAPPO 理论上支持，但样本效率代价 + 异构观测/动作的工程复杂度，让它在真实异构机器人上的落地仍是空白。

对比性思维（不是 X 而是 Y）：异构协同的研究前沿，不是"把同构方法做得更好"，而是"在同构系统里根本不存在的新问题上开疆拓土"——LLM 语义分工（同构系统不需要，因为大家能力一样）、跨模态融合（同构系统无鸿沟）、异构动作空间学习（同构系统参数共享即可）。这再次印证全章主线：异构不是同构的难一点的版本，而是一个有自己独立问题谱系的领域。

练习¶

（综合实现题，累积项目） 把本章各节实现的模块（§6.1 能力矩阵、§6.2 异构 CBBA、§6.4 阻抗编队、§6.5 2.5D 配准、§6.6 内力分配）串成本节的端到端管线，在一个简化仿真（Gazebo 的 1 TurtleBot3 + 1 Crazyflie，或纯 Python）里跑通"无人机航拍 → 地面车进区搜寻"的最小闭环。验证三轴异构都被正确处理：(a) 任务正确分配（航拍给无人机、地面搜寻给地面车）；(b) 坐标系对齐（地面车上报的位置能正确显示在无人机地图上）；(c) 地面车绕开矮障碍而无人机不受影响。
（设计 + 批判题） 阅读 RoCo 的核心思想（LLM 对话式分工），为本节搜救场景设计一个"LLM 分工层"替代 §6.2 的异构 CBBA。然后**批判性地**分析：(a) LLM 分工相比能力向量数值分配，优势在哪（语义灵活性）、风险在哪（可能给出物理不可行的分工）？(b) 如何用 §6.6 的 force closure 检查、§6.4 的可达集检查给 LLM 的分工"兜底"（拒绝物理不可行的方案并要求重新分工）？这正是 RoCo 用"环境反馈 + 重新对话"机制处理的问题。
（开放研究题，全章终极综合） §6.7 指出"真正异构形态（UAV + 四足）的联合 MARL 至今无大规模成功案例"。请综合全章知识，分析这个开放问题难在哪、并提出一个研究方案草图：(a) 异构动作空间用 HAPPO 还是 Transformer（§6.7）？(b) 异构观测（鸟瞰 vs 平视）的编码器怎么设计（独立 vs 跨模态共享，§6.5）？(c) 奖励如何设计才能体现"能力互补"（§6.1）而非各自为战？(d) 样本效率问题怎么缓解（仿真预训练 + 真机微调？课程学习？）？这是一个真正的研究级问题——你的方案不必正确，但要体现你对全章"异构协同独立问题谱系"的整体把握。

本章常见误解汇总¶

误解	正确理解	出处
"异构就是型号不同"	异构是能力/动力学/感知三轴上的差异，且每轴还分"参数异构"（同模型不同参数）和"结构异构"（不同模型）。型号标签太粗，丢失能力连续性和可累加性	§6.1
"所有能力都能靠多机累加"	能力分可累加（载重，两机合搬更重）和不可累加（最高高度/分辨率，两机合不出更高）。混淆会导致"两台 60m 无人机被派去飞 100m"	§6.1
"机型越多，异构系统越强"	异构红利来自能力覆盖性和互补性，不是机型数量。能力雷同的新机型只增冗余不增能力边界	§6.1
"用大代价软惩罚表达'做不了'"	能力约束必须硬剔除（进可行集），不能软惩罚（进代价）。软惩罚下不可行分配在边角情况会被选中	§6.2
"异构 CBBA 也有 50% 最优界"	纯能力+能量约束保持 50% 界；联盟任务（超可加）破坏 DMG，界失效。要分情况	§6.2
"mesh 网络最鲁棒，所以总是最优"	鲁棒性有代价（延迟、复杂度、带宽）。结构稳定/有天然 leader 时，LF 或中继更划算。架构是多目标权衡	§6.3
"异构编队=同构编队加增益调度"	增益调度只处理参数异构；结构异构（全向/非完整/欠驱动）需把一致性从状态空间上移到输出空间，是架构改动	§6.4
"异构融合=把各自位姿广播拼接"	核心不是融合估计值，而是先对齐坐标系（跨越视角/模态鸿沟）。无共同坐标系，再准的位姿也拼不到一起	§6.5
"手工描述子能跨视角匹配"	ORB/SIFT 的视角不变性远不足以跨越鸟瞰 vs 平视的 90° 鸿沟。需几何中间表示或学习描述子	§6.5
"异构操作沿用对称内力均分"	均分假设可行集对称；异构接触（吸盘/绳索单边）的 wrench cone 不对称，需带锥约束 QP；还可能不可行	§6.6
"agent 够多，异构抓取总能完成任意操作"	可行性由各 wrench cone 的 Minkowski 和覆盖性（force closure）决定，不是 agent 数量。三根绳子加再多也提供不了向下力	§6.6
"异构动作空间用参数共享+padding"	参数共享假设动作空间同构；padding 引入梯度污染 + 语义冲突。需独立策略（NoPS）+ 顺序更新	§6.7
"HAPPO 只是支持异构的 MAPPO"	HAPPO 的核心是优势分解+顺序更新带来的单调改进保证，严格依赖 NoPS。加参数共享会破坏保证	§6.7
"把各机型当同构小组分别处理再拼=异构协同"	协同的本质是跨个体耦合，异构让耦合发生在不可比个体间。难点在拼接处（协调层），需为每层找公共空间	§6.8

本章小结¶

本章把前五章建立的"同构协同工具箱"系统地异构化。一条认知主线贯穿始终：异构系统不是同构系统的简单拼接，因为协同的本质是跨个体耦合，而异构让耦合发生在"不可直接比较的个体"之间。整章所有技术，归根结底都在回答同一个元问题——当个体不可比时，如何找到让它们变得可比的"公共空间/公共表示"，并在那里协调。

§6.1 用能力/动力学/感知三轴定义异构性，区分参数异构（同构方法即可）与结构异构（才需异构方法），并把能力写成 trait 向量、队伍写成能力矩阵，区分可累加/不可累加特质。这是全章的共同语言。
§6.2 把同构 CBBA 异构化（能力硬约束 + 联盟任务 + 能量约束），核心洞察是"物理不可能进可行集、代价高进目标"，以及联盟任务的超可加效用如何破坏 50% 界。
§6.3 给出空地通信的中继/mesh/leader-follower 三架构及其单点故障谱，强调跨类型通信必须走公共语义层。
§6.4 揭示异构编队的核心原则——一致性必须建在公共输出空间而非状态空间，协调/执行分离，阻抗柔性连接吸收可达集差异。
§6.5 攻克异构感知的视角鸿沟 + 模态鸿沟，给出 2.5D 几何配准、跨视角学习描述子、相对定位解耦三条路线，核心是"选对让异构观测可比的中间表示"。
§6.6 把内力分配从对称均分推广到不对称 wrench cone 上的带约束优化，强调 force closure 可行性检查。
§6.7 用 HAPPO 的优势分解 + 顺序更新处理异构动作空间，揭示"样本效率 vs 单调改进保证"的根本张力（NoPS vs ParPS）。
§6.8 用五层对比总表系统论证"异构 ≠ 同构并集"，提炼"寻找公共空间"元方法和分层协同架构。
§6.9 把全章方法组装成四足+无人机搜救的端到端管线，落地累积项目。

术语速查表¶

术语（中英）	一句话定义	首见
异构性三轴（capability/dynamics/perception heterogeneity）	沿能力、动力学、感知三个正交轴刻画机器人队伍的差异	§6.1
参数异构 vs 结构异构	同一模型不同参数（增益调度可解）vs 不同模型/约束（需专门方法）	§6.1
能力/特质向量（capability/trait vector）	用连续数值向量刻画一个机器人在各能力维度上的强度	§6.1
能力矩阵（capability matrix）$Q$	行=机器人、列=能力维度的稠密矩阵，异构分配的输入	§6.1
能力互补矩阵（complementarity matrix）$C^{\text{comp}}$	度量任意两机器人能力互补程度，越大越互补	§6.1
可累加/不可累加特质（cumulative/non-cumulative trait）	能否通过多机相加满足任务需求（载重可、最高高度不可）	§6.1
物种（species）	按特质聚类的机型组，物种内同构、物种间异构	§6.1
合格候选集（eligible set）$\mathcal{A}_j$	能力达标、有资格竞标任务 $j$ 的机器人集合	§6.2
联盟任务（coalition task）	单机能力不足、需多机能力拼合的任务（可累加维超单机上限）	§6.2
DMG（Diminishing Marginal Gain）	边际收益递减/子模性，CBBA 50% 界的前提	§6.2
超可加效用（super-additive utility）	多一个合作者收益升高（联盟任务），破坏 DMG	§6.2
AGHS（Air-Ground Heterogeneous System）	空地异构系统，UAV+UGV 协作	§6.3
单点故障（single point of failure）	某节点失效导致全系统瘫痪（中继型最严重）	§6.3
公共语义层（shared semantic layer）	异构节点交换信息的公共表示（世界系位姿等）	§6.3
公共输出空间（shared output space）	异构 agent 状态投影到的同维同义空间（世界系位置）	§6.4
协调/执行分离	在公共输出空间协调、在私有状态空间执行	§6.4
阻抗引导（impedance-based guidance）	用虚拟弹簧-阻尼柔性连接 follower，允许临时偏离队形	§6.4
视角鸿沟（viewpoint gap）	鸟瞰 vs 平视外观差异巨大，跨视角特征匹配失效	§6.5
模态鸿沟（modality gap）	不同传感器数据类型不同（图像 vs 点云），无法直接比对	§6.5
2.5D 高程图（elevation map）	2D 网格 + 每格高度，视角无关的几何中间表示	§6.5
相对定位解耦（semi-distributed relative localization）	只求两两相对位姿、与各自状态估计解耦，回避全局对齐	§6.5
wrench cone	一个 agent 在接触点能施加的力/力矩可行集	§6.6
force closure（力封闭）	各 wrench cone 的 Minkowski 和能否张成整个 wrench 空间	§6.6
HAPPO（Heterogeneous-Agent PPO）	用优势分解 + 顺序更新处理异构 agent 的 MARL 算法	§6.7
多智能体优势分解	联合优势=各 agent 顺序边际优势之和	§6.7
顺序更新（sequential update）	按随机顺序逐个更新策略，用累积比率修正优势	§6.7
NoPS / ParPS	无参数共享（HAPPO 需要）/ 部分参数共享（提效率但破坏保证）	§6.7
寻找公共空间（finding the common ground）	为不可比的异构个体找到可比的公共表示——全章元方法	§6.8

知识点总表¶

编号	知识点	核心要点	对应节	难度
6-1	异构性三轴定义	能力/动力学/感知；参数 vs 结构异构	§6.1	⭐⭐
6-2	能力矩阵与互补矩阵	trait 向量、$Q$、$C^{\text{comp}}$、可累加性	§6.1	⭐⭐
6-3	联盟能力聚合	可累加维 sum、不可累加维 max	§6.1	⭐⭐
6-4	异构 CBBA 三改动	能力硬约束 + 联盟任务 + 能量代价	§6.2	⭐⭐⭐
6-5	硬约束 vs 软约束	物理不可能进可行集、代价高进目标	§6.2	⭐⭐⭐
6-6	50% 界何时失效	联盟超可加破坏 DMG	§6.2	⭐⭐⭐⭐
6-7	三类通信架构	中继/mesh/LF 的单点故障谱	§6.3	⭐⭐
6-8	公共语义层	跨类型通信不传私有状态	§6.3	⭐⭐
6-9	输出空间一致性	异构动力学的一致性上移到输出空间	§6.4	⭐⭐⭐
6-10	协调/执行分离	公共空间协调 + 私有空间执行	§6.4	⭐⭐⭐
6-11	阻抗引导	柔性连接吸收异构可达集差异	§6.4	⭐⭐⭐
6-12	视角/模态鸿沟	异构感知融合的两道根本障碍	§6.5	⭐⭐⭐⭐
6-13	跨模态融合三路线	2.5D 配准 / 学习描述子 / 相对定位	§6.5	⭐⭐⭐⭐
6-14	不对称内力分配	带 wrench cone 约束的 QP	§6.6	⭐⭐⭐
6-15	force closure 可行性	Minkowski 和覆盖性决定可行	§6.6	⭐⭐⭐
6-16	异构动作空间	维度/语义不同，参数共享失效	§6.7	⭐⭐⭐⭐
6-17	HAPPO 优势分解+顺序更新	单调改进保证，依赖 NoPS	§6.7	⭐⭐⭐⭐
6-18	NoPS vs ParPS 张力	样本效率 vs 单调保证不可兼得	§6.7	⭐⭐⭐⭐
6-19	五层对比总表	异构 ≠ 同构并集	§6.8	⭐⭐⭐
6-20	寻找公共空间元方法	全章统一思想	§6.8	⭐⭐⭐
6-21	分层协同架构	L1-L4，上层协调下层执行	§6.8	⭐⭐⭐
6-22	端到端搜救管线	三轴异构在一个系统打通	§6.9	⭐⭐

累积项目：本章新增模块（Mini-MultiBot 异构层）¶

项目背景：Mini-MultiBot 是贯穿整个多机协作部分的累积项目（第 13 章综合实战会整合所有模块）。前五章你已经搭好：通信图与拓扑（Ch1）、共识协调器（Ch2）、CBBA 分配 + MAPF 规划（Ch3）、分布式 MPC 编队（Ch4）、协同搬运力控（Ch5）。本章给它加上**异构层**——让 Mini-MultiBot 从"一群相同的机器人"升级为"无人机 + 地面车的异构队伍"。

本章新增的项目模块（保存在 mini_multibot/heterogeneous/）：

模块文件	内容	对应节
`capability.py`	能力矩阵 $Q$、互补矩阵、联盟能力聚合（cumulative/non-cumulative）	§6.1
`hetero_cbba.py`	在 Ch3 的 `cbba.py` 上扩展：能力硬约束 + 联盟任务 + 能量代价	§6.2
`comm_arch.py`	中继/mesh/LF 三架构 + leader 选举 + 公共语义话题	§6.3
`hetero_formation.py`	在 Ch4 的编队上扩展：输出空间一致性 + 阻抗引导 + 非完整跟踪	§6.4
`cross_modal_fusion.py`	2.5D 高程图 + ICP 配准 + 相对位姿	§6.5
`hetero_grasp.py`	在 Ch5 的内力分配上扩展：wrench cone + 带约束 QP + force closure 检查	§6.6
`sar_pipeline.py`	§6.9 端到端搜救编排，串联以上所有模块	§6.9

与前面章节模块的接口：

hetero_cbba.py 复用 Ch3 的 cbba.py 的冲突消解（最大值共识）——只替换 bundle 构建阶段（§6.2 强调的"骨架不变、问题定义变"）。
hetero_formation.py 复用 Ch4 的单体 MPC/控制器作为"执行层"，只在上面加"协调层"（输出空间一致性）。
hetero_grasp.py 复用 Ch5 的 grasp matrix 计算，只替换内力分配（均分 → 带锥约束 QP）。

本章项目验收标准：

能力矩阵正确构造，联盟能力聚合区分可累加性（§6.1 练习 1）。
异构 CBBA 把飞行任务只派 UAV、搬运只派 UGV，且能复现"软惩罚导致不可行分配"的对照 bug（§6.2 练习 1）。
异构编队让地面车绕矮障碍而无人机不受影响（§6.4 练习 1）。
2.5D 配准对齐鸟瞰与地面坐标系（§6.5 练习 1）。
sar_pipeline.py 跑通最小搜救闭环，三轴异构都被正确处理（§6.9 练习 1）。

下一步（第 13 章）：Mini-MultiBot 综合实战会把本章异构层与前面所有模块整合，并接入 MARL（第 10-12 章）——届时 §6.7 的 HAPPO 异构 MARL 会成为一个可选的"学习式协调器"，与本章的模型驱动协调器（异构 CBBA + 阻抗编队）形成对比实验。

延伸阅读¶

异构任务分配（能力感知） - ⭐⭐ Gerkey & Matarić, "A Formal Analysis and Taxonomy of Task Allocation in Multi-Robot Systems," IJRR 2004 — MRTA 分类法奠基，理解 ST/MT、SR/MR、IA/TA 三维。 - ⭐⭐⭐ Choi, Brunet, How, "Consensus-Based Decentralized Auctions for Robust Task Allocation," IEEE T-RO 2009 — CBBA 原始论文，本章 §6.2 异构扩展的同构起点。 - ⭐⭐⭐ Ravichandar et al., "STRATA: A Unified Framework for Task Assignments in Large Teams of Heterogeneous Robots," ICRA 2019 — trait 向量、cumulative/non-cumulative、species 的来源，§6.1 的理论基础。 - ⭐⭐⭐ "Heterogeneous Multi-Robot Task Allocation for Long-Endurance Missions in Dynamic Scenarios," 2024 — 能量/续航约束、任务中继、充电的异构 MRTA，§6.2 能量约束的深化。

空地协同（UAV-UGV） - ⭐⭐ "Air-Ground Cooperative Systems: A Survey and Taxonomy"（Machines / RAS 综述, 2024）— AGHS 全栈梳理，§6.3 架构分类的来源。 - ⭐⭐⭐ Michael et al., "Cooperative Manipulation and Transportation with Aerial Robots," Auton. Robots 2014 — 空中协同搬运先驱，§6.6 不对称（绳索）接触的起点。 - ⭐⭐⭐ "Air-Ground Collaborative Robots for Fire and Rescue Missions"（2024）— UAV 建图 + UGV 导航的分层协调，§6.9 搜救场景的参考。

异构编队 - ⭐⭐⭐ HetSwarm, "Cooperative Navigation of Heterogeneous Swarm... through Impedance-based Guidance," 2025 — §6.4 阻抗引导的直接来源，UAV-APF 引领 + UGV 阻抗跟随。

跨模态融合 - ⭐⭐⭐ "A 2.5D Map-Based Mobile Robot Localization via Cooperation of Aerial and Ground Robots," Sensors 2017 — §6.5 路线一（2.5D 配准）的代表。 - ⭐⭐⭐⭐ "Semi-distributed Cross-modal Air-Ground Relative Localization," 2025 — §6.5 路线三（相对定位解耦）的最新工作，深度描述子 + 局部 BA。 - ⭐⭐⭐ "Tightly-Coupled Air-Ground Collaborative System for Autonomous UGV Navigation in GPS-Denied Environments," 2025 — GPS 拒止下的空地协同定位。

异构 MARL - ⭐⭐⭐⭐ Kuba/Zhong et al., "Heterogeneous-Agent Reinforcement Learning (HAPPO/HATRPO)," JMLR 2022 / ICLR 2022 — §6.7 的核心，多智能体优势分解 + 顺序更新。 - ⭐⭐⭐⭐ "Improving Monotonic Optimization in Heterogeneous MARL with Optimal Marginal Deterministic Policy Gradient," 2025 — NoPS vs ParPS 基线漂移问题的深化（§6.7 张力）。 - ⭐⭐⭐ Pandit et al., "Learning Decentralized Multi-Biped Control for Payload Transport," CoRL 2025 — 异构形态协调涌现。

LLM 驱动异构协调 - ⭐⭐⭐ Mandi et al., "RoCo: Dialectic Multi-Robot Collaboration with Large Language Models," ICRA 2024 — §6.9 前沿，LLM 对话式异构分工。

本章与后续章节的关系¶

后续章节	关系	本章铺垫的知识点
第 7 章双臂/多臂协同操作	异构操作的深化（不同臂、不同末端）	§6.6 异构 wrench cone、内力分配
第 8 章多足协同 Loco-Manipulation	移动+操作的异构协同	§6.4 输出空间协调、§6.6 力分配
第 9 章人形+四足+臂混合体	最复杂的异构形态协同	§6.1 三轴异构、§6.8 分层架构
第 10-12 章 MARL 系列	§6.7 异构 MARL 的系统展开	§6.7 HAPPO、优势分解、异构动作空间
第 13 章 Mini-MultiBot 综合实战	整合本章异构层 + MARL	全章模块（累积项目异构层）

承上：本章把 Ch1-Ch5 的同构协同工具（通信图、共识、CBBA、MPC 编队、grasp matrix）系统异构化，是从"同构理想"到"异构现实"的转折点。启下：第 7-9 章在操作任务上深化异构，第 10-12 章在学习上展开异构 MARL，第 13 章整合。本章是理解后续所有"真实异构系统"的地基。

🔧 故障排查手册¶

症状	可能原因	排查步骤	相关节
物理上做不了的任务被派出去（无人机被派搬重物）	能力约束用了大代价软惩罚而非硬剔除	1. 检查是否维护合格候选集 $\mathcal{A}_j$ 2. 断言每个分配的 $(i,j)$ 满足 $q_i \succeq y_j$ 3. 把软惩罚改为 $-\infty$/剔除	§6.2
联盟任务无人认领、静默丢失	需求超单机能力，is_eligible 对所有单机返回 False，但未触发联盟逻辑	1. 分配后检查未认领任务 2. 若其需求超单机上限→是漏处理的联盟任务 3. 启用联盟分解/竞标	§6.2
异构 CBBA 解远差于最优、无法解释	含联盟任务（超可加）破坏 DMG，50% 界失效	1. 判断任务是"抢"（竞争）还是"凑"（合作）2. 合作部分换联盟形成算法 3. 别指望分布式拍卖的界	§6.2
无人机回去换电池时整个系统断联	用了中继型架构，中继无人机是单点故障，且续航短	1. 改 mesh 分散依赖 2. 或多机轮班中继接力 3. 检查枢纽节点续航 vs 任务时长	§6.3
加一种新机型要改所有已有节点代码	跨类型通信传私有状态，无公共语义层	1. 约定公共语义话题（世界系位姿）2. 每节点只发布投影后的公共语义 3. 验证"加新机型改 0 个旧节点"	§6.3
差速车在异构编队里原地打转/跟不上	在状态空间写刚性一致性，喂给非完整 agent	1. 把一致性上移到输出空间 2. 协调层只给期望输出、执行层用非完整跟踪器 3. 改阻抗柔性连接	§6.4
异构编队遇障碍整队散开	用了刚性约束，某成员绕障破坏队形并干扰他人	1. 改阻抗（柔性）连接 2. 每个 agent 只对自己可达集相关障碍反应 3. 调小阻抗刚度 $K_d$	§6.4
各机器人自定位准、拼接后错位米级	跳过坐标系对齐，直接按各自 SLAM 坐标拼	1. 做配准（2.5D ICP 或跨视角描述子）2. 或改求相对位姿 3. 确认有共同坐标系基准	§6.5
跨视角特征匹配全是错的	用手工描述子（ORB/SIFT）跨鸟瞰-平视，视角鸿沟太大	1. 改 2.5D 几何中间表示 + ICP 2. 或用学习的跨视角描述子 3. 可视化匹配线确认杂乱	§6.5
ICP 配准不收敛	直接配原始点云（密度/覆盖差异）或场景无几何特征	1. 先投影 2.5D 高程图再配 2. 检查场景是否有高度变化 3. 平坦区考虑加 fiducial	§6.5
异构抓取吸盘被要求推、物体失稳	沿用对称内力均分，落到吸盘可行集之外	1. 为每 agent 构造 wrench cone 2. 解带锥约束 QP 3. 验证每个力落在对应锥内	§6.6
内力分配 QP 反复求解失败	可能不是数值问题，是异构抓取本身不可行	1. 先查 force closure（各锥 Minkowski 和是否覆盖 $w_{des}$ 方向）2. 不可行则改抓取配置，别纠结求解器	§6.6
异构 MARL 训练剧烈震荡/不收敛	参数共享+padding 异构动作（梯度污染+语义冲突）	1. 改独立策略（NoPS），输出维度匹配各自动作空间 2. 用 HAPPO 顺序更新 3. 检查无填充维度	§6.7
HAPPO 加了参数共享后保证失效、不稳	ParPS 引入基线漂移，破坏顺序更新的独立性假设	1. 退回纯 NoPS 2. 或用专门处理基线漂移的变体 3. 接受样本效率代价	§6.7
HAPPO 在真机上训练周期长到不现实	忽略 NoPS 的样本效率代价，n 个独立网络各学各的	1. 评估样本预算 2. 仿真预训练+真机微调 3. 样本宝贵考虑 Transformer 统一（部分共享）	§6.7
异构系统"拼接处"全是 bug	把每机型当同构小组分别处理再拼，忽略跨个体耦合	1. 识别哪些层有跨个体耦合 2. 为每个耦合层找公共空间（五层对比表）3. 按分层架构组织	§6.8

API 速查表¶

API / 工具	用途	出处
`complementarity_matrix(Q)`	能力互补矩阵；越大越互补，对角线 0	§6.1
`coalition_capability(Q, members, cumulative)`	联盟能力聚合：可累加维 sum、不可累加维 max	§6.1
`normalize_columns(Q)`	能力矩阵按列归一化，消除量纲差异（互补度计算前必做）	§6.1
`is_eligible(caps, demand, dims)`	能力硬约束：能力在所需维度全达标才合格	§6.2
`marginal_score_hetero(...)`	异构边际收益：先查资格（硬剔除）再算含能量修正的收益	§6.2
`build_bundle_hetero(...)`	异构 bundle 构建；冲突消解阶段复用 Ch3 不变	§6.2
`feasible_coalitions(Q, demand, cumulative)`	枚举满足需求的合格联盟（取代价最小者）	§6.2
`apf_velocity(pos, goal, obstacles)`	Leader 人工势场速度（输出空间）：目标引力 + 障碍斥力（须有影响半径）	§6.4
`impedance_target(...)`	Follower 阻抗：拉向期望队形位置 + 对局部障碍临时阻抗，返回期望加速度	§6.4
`unicycle_track(state, acc_des, dt)`	执行层：差速车非完整跟踪器，把期望加速度转成 $(a, \omega)$	§6.4
`pointcloud_to_elevation(points, grid_res)`	点云→2.5D 高程图（视角无关的几何中间表示）	§6.5
`icp_2d(src, dst)`	高程图配准，求两坐标系相对变换（实际 3D 用 Open3D）	§6.5
`choose_fusion_strategy(...)`	按场景条件选跨模态融合路线（几何/学习/相对定位）	§6.5
`build_wrench_cone_constraints(type, normal, mu)`	为夹爪/吸盘/绳索构造 wrench cone 线性约束	§6.6
`allocate_internal_force(G, w_des, types, normals)`	异构内力分配：带锥约束 QP；不可行则报错（查 force closure）	§6.6
`happo_sequential_update(agents, rollouts)`	HAPPO 顺序更新：随机顺序逐个更新独立策略 + 累积比率 M 修正优势	§6.7
`HeteroAgent(obs_dim, act_dim, type)`	异构 agent：独立策略网络，动作维度匹配各自动作空间（NoPS）	§6.7
`choose_marl(homogeneous, need_guarantee, budget)`	MARL 方案选型：MAPPO / HAPPO / Transformer	§6.7
`heterogeneous_sar_pipeline(...)`	四足+无人机搜救端到端编排，串联全章模块	§6.9
`scipy.optimize.minimize(method="SLSQP")`	带等式+不等式约束的内力分配 QP 求解	§6.6
`scipy.spatial.cKDTree`	ICP 最近点对应查询	§6.5
Open3D `registration_icp`	工程级 3D ICP（替代教学用的 `icp_2d`）	§6.5

研究实践建议¶

给初学者：务必**亲手把异构 CBBA、异构编队、2.5D 配准这三个跑通**——它们对应异构的三个轴（能力/动力学/感知），是本章区别于同构多机的最直接体现。重点踩三个坑，每个都对应一个"硬约束 vs 软约束"或"公共空间"的核心洞察：(1) 把能力约束从硬剔除改成大代价软惩罚，构造一个全 UGV 满载的实例，亲眼看到搬运任务被错误派给 UAV——理解"物理不可能必须进可行集"；(2) 把异构编队的一致性从输出空间改回状态空间，喂给差速车，看它原地打转——理解"异构状态不可比、必须投影到公共输出空间"；(3) 直接 ICP 原始点云（不投影高程图）或直接广播位姿拼接，看配准失败——理解"异构感知的核心是找可比的中间表示、对齐坐标系"。先把**模型驱动**的异构协同（§6.2/§6.4/§6.6）吃透，再碰 §6.7 的学习驱动。从一开始就把"三轴诊断"（§6.1）和"寻找公共空间"（§6.8）这两个元方法刻进脑子——它们是贯穿全章的主线。

给有经验者：异构系统设计按"三轴诊断 → 判断参数 vs 结构异构 → 按需施治"走（§6.8 决策框架）。第一原则是**别过度异构化**——参数异构（同模型不同参数）用同构方法 + 增益调度即可，只对结构异构上异构重武器，异构方法的成本（架构复杂、样本低效、可行性难保证）不容忽视。分配上，任务可单机完成就放心用异构 CBBA（保 50% 界），出现超可加联盟就换联盟形成/联合优化（界失效）。感知上，先问"需全局一致还是只需相对"——很多任务相对定位（§6.5 路线三）就够，别在异构鸿沟上死磕全局对齐。学习上，异构 MARL 把样本预算当一等约束：仿真海量采样可上 HAPPO（NoPS+顺序更新，有保证），真机样本宝贵考虑 Transformer 统一或仿真预训练+迁移。关注前沿：RoCo 系的 LLM 语义分工（如何与物理约束严格对接是开放问题）、跨模态相对定位、以及本领域最硬的空白——真正异构形态（UAV+四足）的联合 MARL。最后记住全章的元洞察：异构协同的所有技术，归根结底都是为不可比的个体寻找公共空间——分配在能力空间、编队在输出空间、感知在几何中间表示、操作在物体 wrench 空间、学习用独立策略+顺序协调。抓住这条主线，面对任何新的异构问题，第一反应就是"它们在哪不可比？公共空间在哪？"。

版本信息速查¶

本章涉及的工具与算法参考（截至 2026 年中）：

项目	版本 / 出处	说明
MRTA 分类法	Gerkey & Matarić 2004, IJRR 23(9)	§6.1 异构能力分类的源头
CBBA	Choi, Brunet, How 2009, IEEE T-RO 25(4)	§6.2 异构扩展的同构起点
STRATA	Ravichandar et al. 2019, ICRA	§6.1 trait 向量、cumulative/non-cumulative、species
长航时异构 MRTA	2024, arXiv (Heterogeneous MRTA for Long-Endurance Missions)	§6.2 能量/中继/充电约束
空中协同搬运	Michael et al. 2014, Auton. Robots	§6.6 不对称（绳索）接触先驱
空中冗余操作	Tognon et al. 2019, RA-L	§6.6 飞行基座力约束
AGHS 综述	2024, Machines / RAS (Air-Ground Cooperative Survey)	§6.3 架构分类
HetSwarm	2025, arXiv	§6.4 阻抗引导异构编队
2.5D 协同定位	2017, Sensors	§6.5 路线一（高程图配准）
跨模态相对定位	2025, arXiv (Semi-distributed Cross-modal Air-Ground)	§6.5 路线三（相对定位解耦）
HAPPO / HATRPO	Kuba/Zhong et al. 2022, JMLR / ICLR	§6.7 优势分解 + 顺序更新
ParPS 基线漂移	2025, arXiv (Monotonic Optimization in Heterogeneous MARL)	§6.7 NoPS vs ParPS 张力
多人形搬运	Pandit et al. 2025, CoRL	§6.9 异构形态协调涌现
RoCo	Mandi et al. 2024, ICRA	§6.9 LLM 驱动异构协调
NumPy / SciPy	≥ 1.24 / ≥ 1.10	能力矩阵、ICP、内力 QP（`linprog`/`minimize`/`cKDTree`）
Open3D	≥ 0.17	工程级 3D ICP（§6.5 配准）
ROS2	Humble / Jazzy	§6.3 命名空间隔离、共享话题、DDS 发现
PettingZoo / 自定义环境	≥ 1.24	§6.7 异构 MARL 实验环境

说明：论文的作者、年份、期刊以正文引用与延伸阅读为准；工具版本随上游更新，复现时以官方发行说明为准。部分 2025 工作以 arXiv 预印本为准，正式发表信息可能变化。

符号	含义	首见
\(\mathcal{R} = \{r_1, \dots, r_N\}\)	机器人集合（异构，可分多个 species）	§6.1
\(\mathcal{S} = \{s_1, \dots, s_K\}\)	species（机型）集合，每个 species 内部同构	§6.1
\(\mathbf{q}_i \in \mathbb{R}^U\)	机器人 \(i\) 的能力/特质向量（trait vector），\(U\) 维	§6.1
\(Q \in \mathbb{R}^{N \times U}\)	能力矩阵（行=机器人，列=能力维度）	§6.1
\(C^{\text{comp}}\)	能力互补矩阵（capability complementarity matrix）	§6.1
\(\mathbf{y}_j \in \mathbb{R}^U\)	任务 \(j\) 的能力需求向量	§6.1, §6.2
\(a_i\)	机器人 \(i\) 的动作（异构：维度 \(d_i\) 可不同）	§6.7
\(c_{ij}\)	机器人 \(i\) 执行任务 \(j\) 的代价	§6.2
\(\mathcal{A}_j\)	任务 \(j\) 的合格机器人候选集（能力筛选后）	§6.2
\(G\)	grasp matrix（接触力→物体旋量）	§6.6
\(\mathcal{W}_i\)	机器人 \(i\) 在接触点的可施加 wrench 锥	§6.6
\(T_W^{B_i}\)	机器人 \(i\) 本体系到世界系的变换	§6.5
\(A^{i_1:i_m}\)	多智能体联合优势函数（HAPPO）	§6.7
\(\pi^i\)	智能体 \(i\) 的独立策略（NoPS）	§6.7

机体	运动模型	状态维度	关键约束
二阶积分点质量	\(\ddot{p} = u\)	6（位置+速度）	无（全向）
差速驱动车（unicycle）	\(\dot{x}=v\cos\theta,\ \dot{y}=v\sin\theta,\ \dot\theta=\omega\)	3	非完整：不能横向移动
四旋翼（欠驱动）	\(SE(3)\) 上的刚体 + 单向推力	12	欠驱动：水平加速度必须靠倾斜产生

模块文件	内容	对应节
`capability.py`	能力矩阵 \(Q\)、互补矩阵、联盟能力聚合（cumulative/non-cumulative）	§6.1
`hetero_cbba.py`	在 Ch3 的 `cbba.py` 上扩展：能力硬约束 + 联盟任务 + 能量代价	§6.2
`comm_arch.py`	中继/mesh/LF 三架构 + leader 选举 + 公共语义话题	§6.3
`hetero_formation.py`	在 Ch4 的编队上扩展：输出空间一致性 + 阻抗引导 + 非完整跟踪	§6.4
`cross_modal_fusion.py`	2.5D 高程图 + ICP 配准 + 相对位姿	§6.5
`hetero_grasp.py`	在 Ch5 的内力分配上扩展：wrench cone + 带约束 QP + force closure 检查	§6.6
`sar_pipeline.py`	§6.9 端到端搜救编排，串联以上所有模块	§6.9

症状	可能原因	排查步骤	相关节
物理上做不了的任务被派出去（无人机被派搬重物）	能力约束用了大代价软惩罚而非硬剔除	1. 检查是否维护合格候选集 \(\mathcal{A}_j\) 2. 断言每个分配的 \((i,j)\) 满足 \(q_i \succeq y_j\) 3. 把软惩罚改为 \(-\infty\)/剔除	§6.2
联盟任务无人认领、静默丢失	需求超单机能力，is_eligible 对所有单机返回 False，但未触发联盟逻辑	1. 分配后检查未认领任务 2. 若其需求超单机上限→是漏处理的联盟任务 3. 启用联盟分解/竞标	§6.2
异构 CBBA 解远差于最优、无法解释	含联盟任务（超可加）破坏 DMG，50% 界失效	1. 判断任务是"抢"（竞争）还是"凑"（合作）2. 合作部分换联盟形成算法 3. 别指望分布式拍卖的界	§6.2
无人机回去换电池时整个系统断联	用了中继型架构，中继无人机是单点故障，且续航短	1. 改 mesh 分散依赖 2. 或多机轮班中继接力 3. 检查枢纽节点续航 vs 任务时长	§6.3
加一种新机型要改所有已有节点代码	跨类型通信传私有状态，无公共语义层	1. 约定公共语义话题（世界系位姿）2. 每节点只发布投影后的公共语义 3. 验证"加新机型改 0 个旧节点"	§6.3
差速车在异构编队里原地打转/跟不上	在状态空间写刚性一致性，喂给非完整 agent	1. 把一致性上移到输出空间 2. 协调层只给期望输出、执行层用非完整跟踪器 3. 改阻抗柔性连接	§6.4
异构编队遇障碍整队散开	用了刚性约束，某成员绕障破坏队形并干扰他人	1. 改阻抗（柔性）连接 2. 每个 agent 只对自己可达集相关障碍反应 3. 调小阻抗刚度 \(K_d\)	§6.4
各机器人自定位准、拼接后错位米级	跳过坐标系对齐，直接按各自 SLAM 坐标拼	1. 做配准（2.5D ICP 或跨视角描述子）2. 或改求相对位姿 3. 确认有共同坐标系基准	§6.5
跨视角特征匹配全是错的	用手工描述子（ORB/SIFT）跨鸟瞰-平视，视角鸿沟太大	1. 改 2.5D 几何中间表示 + ICP 2. 或用学习的跨视角描述子 3. 可视化匹配线确认杂乱	§6.5
ICP 配准不收敛	直接配原始点云（密度/覆盖差异）或场景无几何特征	1. 先投影 2.5D 高程图再配 2. 检查场景是否有高度变化 3. 平坦区考虑加 fiducial	§6.5
异构抓取吸盘被要求推、物体失稳	沿用对称内力均分，落到吸盘可行集之外	1. 为每 agent 构造 wrench cone 2. 解带锥约束 QP 3. 验证每个力落在对应锥内	§6.6
内力分配 QP 反复求解失败	可能不是数值问题，是异构抓取本身不可行	1. 先查 force closure（各锥 Minkowski 和是否覆盖 \(w_{des}\) 方向）2. 不可行则改抓取配置，别纠结求解器	§6.6
异构 MARL 训练剧烈震荡/不收敛	参数共享+padding 异构动作（梯度污染+语义冲突）	1. 改独立策略（NoPS），输出维度匹配各自动作空间 2. 用 HAPPO 顺序更新 3. 检查无填充维度	§6.7
HAPPO 加了参数共享后保证失效、不稳	ParPS 引入基线漂移，破坏顺序更新的独立性假设	1. 退回纯 NoPS 2. 或用专门处理基线漂移的变体 3. 接受样本效率代价	§6.7
HAPPO 在真机上训练周期长到不现实	忽略 NoPS 的样本效率代价，n 个独立网络各学各的	1. 评估样本预算 2. 仿真预训练+真机微调 3. 样本宝贵考虑 Transformer 统一（部分共享）	§6.7
异构系统"拼接处"全是 bug	把每机型当同构小组分别处理再拼，忽略跨个体耦合	1. 识别哪些层有跨个体耦合 2. 为每个耦合层找公共空间（五层对比表）3. 按分层架构组织	§6.8

第 6 章 异构多机协同——能力互补、空地联合与跨模态融合¶

前置自测¶

本章目标¶

本章知识导航¶

前置知识桥接¶

如果跳过本章会怎样¶

预计阅读时间¶

科研发展脉络¶

本章符号约定¶

§6.1 异构性的三轴定义与能力互补矩阵 ⭐⭐¶

动机：一个"显然"却没人说清的词¶

如果用最朴素的"型号标签"会怎样¶

历史：从型号分类到特质向量¶

理论：异构性的三轴定义¶

轴一：能力异构（capability heterogeneity）——影响"谁干什么"¶

轴二：动力学异构（dynamics heterogeneity）——影响"怎么动"¶

轴三：感知异构（perception heterogeneity）——影响"看到什么"¶

三轴的正交性与耦合¶

能力互补矩阵：把"互补"变成可计算的对象¶

⚠️ 常见陷阱¶

练习¶

§6.2 异构任务分配：能力感知的 CBBA 扩展 ⭐⭐⭐¶

动机：当"谁都能做"的假设崩塌¶

如果用"大代价软惩罚"会怎样：硬约束 vs 软约束¶

历史：从同质拍卖到能力感知分配¶

理论：异构 CBBA 的三个改动¶

改动一：能力硬约束——改候选集¶

改动二：联盟任务——改 bundle 的原子单位¶

改动三：能量约束——改代价函数¶

理论-工程桥接：50% 最优界何时失效¶

代码：在第 3 章 CBBA 上增量异构化¶

⚠️ 常见陷阱¶

练习¶

§6.3 空地联合通信架构 ⭐⭐¶

动机：分配做完了，指令传不到¶

理论：三类架构与单点故障谱¶

架构 A：中继型（relay）——最简单，但最脆弱¶

架构 B：Mesh 型（ad-hoc mesh）——最鲁棒，但延迟不可控¶

架构 C：Leader-Follower 型——折中，决策集中¶

三架构对比表¶

理论-工程桥接：为不同场景选架构¶

ROS2 实现要点¶

⚠️ 常见陷阱¶

练习¶

§6.4 异构编队：leader-follower 与阻抗引导 ⭐⭐⭐¶

动机：同一条参考轨迹，喂不进异构编队¶

如果硬在状态空间做一致性会怎样¶

历史：从输出调节到异构编队¶

理论：公共输出空间一致性¶

第一步：定义公共输出空间¶

第二步：在输出空间写一致性¶

第三步：每个 agent 用各自动力学实现输出目标¶

改进 APF 与 leader-follower 的结合¶

阻抗引导处理异构可达集差异¶

代码：异构 leader-follower 编队（最小实现）¶

⚠️ 常见陷阱¶

练习¶

§6.5 跨模态信息融合 ⭐⭐⭐⭐¶

动机：各自定位很准，拼在一起全错位¶

两道鸿沟：视角鸿沟与模态鸿沟¶

历史：从协同 SLAM 到跨模态定位¶

路线一：2.5D 高程图配准——在几何上绕过外观鸿沟¶

路线二：深度学习跨视角描述子——直接跨越视角鸿沟¶

路线三：相对定位解耦——回避全局对齐难题¶

三条路线对比¶

代码：跨模态融合的骨架（2.5D 配准 + 相对位姿）¶

⚠️ 常见陷阱¶

练习¶

§6.6 异构协同操作：不对称 wrench cone 与内力分配 ⭐⭐⭐¶

动机：均分内力，吸盘却被要求"推"¶

如果沿用对称均分会怎样¶

历史：从对称抓取到异构联合操作¶

理论：在不对称 wrench cone 上做内力分配¶

每个 agent 的 wrench cone¶

内力分配的 QP 表述¶

异构带来的可行性问题¶

代码：异构内力分配的 QP¶

⚠️ 常见陷阱¶

练习¶

§6.7 异构 MARL：HAPPO 与异构动作空间 ⭐⭐⭐⭐¶

第 6 章　异构多机协同——能力互补、空地联合与跨模态融合¶