Primitive-Swarm¶

性质：算法工程教学 | 难度跨度：⭐⭐ ~ ⭐⭐⭐⭐ | 预计精读：15-20 小时(1.5 周) | 定位：❌ 纯无人机

一句话定位:当单机已经能在密林里贴树高速穿行(D1-D9),把它复制成 10 架、100 架、1000 架时,新的核心矛盾出现了——每架机只看得到局部、通信会延迟会丢包、碰撞代价是坠毁,却要在毫秒级凑出彼此不撞的轨迹。本章完整讲透两条对立的技术路线:ZJU 的**软惩罚+速度优先**(EGO-Swarm → Swarm-Formation → Primitive-Swarm,1 ms 重规划扩展到千机但无形式保证)与 MIT 的**硬约束+安全优先**(MADER → RMADER,Gurobi QP 慢但可证 100% 无碰),并把支撑它们的数学引擎(B 样条/MINVO 凸包、图 Laplacian、check-recheck 不变量、CBF 安全集)逐一拆开。

本部分(Part 5)定位:D1-D9 聚焦**单机能力**的极致化;D10(本章)讲多机如何**协同而不碰撞**;D11 讲 2024-2025+ 的**新兴方向**(VLM 任务层、3DGS 多机、非标构型、事件相机、安全保证),不求深度求视野。

前置自测¶

开始前先回答下面 6 个问题。答不出 2 题以上,建议先回前置章节补齐——本章的安全性证明、编队代价、扩展性分析全部建立在这些基础之上,欠了账会在 D10.4 的归纳证明处彻底卡住。

B 样条的凸包性质(Convex Hull Property)是什么? 对一条 $p$ 阶均匀 B 样条,某个节点区间 $[t_k, t_{k+1})$ 内的曲线段落在哪几个控制点的凸包内?这个性质为什么能把"曲线避碰"转化为"控制点避碰"? (答不出 → 回 D4 50_B样条轨迹优化.md,凸包性质一节)
图 Laplacian 矩阵 $L = D - A$ 的代数连通性 $\lambda_2$(Fiedler 值)衡量什么? 为什么 $\lambda_2 > 0$ 等价于图连通?共识协议 $\dot{x} = -Lx$ 的收敛速率与 $\lambda_2$ 是什么关系? (答不出 → 回 04/50_多机器人协作/30_共识算法与分布式优化.md §2.1)
微分平坦(Differential Flatness)如何把四旋翼规划从 12 维状态空间降到 4 维平坦输出空间? 平坦输出是哪四个量?为什么这让"用一条多项式/样条曲线表示轨迹"成为可能? (答不出 → 回 D1 微分平坦 + D3 多项式轨迹生成)
凸优化中,"软惩罚项"(penalty)和"硬约束"(hard constraint)的本质区别是什么? 一个加权惩罚 $w \cdot \max(0, d_{\text{safe}} - d)^3$ 与一个不等式约束 $d \geq d_{\text{safe}}$,在"求解器找不到安全解时会发生什么"上有何不同? (答不出 → 回 Part 0 优化基础 + D3 §D3.3 受约束 QP)
MINCO 轨迹表示相比 B 样条的核心优势是什么? 为什么它能把"时间分配 $\{T_i\}$"也变成可优化变量?$\partial J/\partial c_j$ 和 $\partial J/\partial T_i$ 的解析梯度为什么重要? (答不出 → 回 D5 60_MINCO轨迹表示与安全走廊.md)
CBBA(基于共识的捆绑算法)的两个阶段是什么? 为什么它是"去中心化"的任务分配?收敛轮数和通信图直径有什么关系? (答不出 → 回 04/50_多机器人协作/40_任务分配与路径规划.md §3.2)

参考答案要点(先自己答,再对照):

凸包性质:$p$ 阶均匀 B 样条在 $[t_k, t_{k+1})$ 内的曲线段落在 $\{Q_{k-p}, \ldots, Q_k\}$ 这 $p+1$ 个控制点的凸包内。因为曲线是控制点的凸组合(基函数非负且和为 1),所以控制点凸包是曲线的一个外包络。于是"两条曲线对应段距离 $\geq d_{\text{safe}}$"可由"两组控制点凸包距离 $\geq d_{\text{safe}}$"**充分**保证——这是 EGO-Swarm 软惩罚的几何基础。
$\lambda_2$ 是 $L$ 的第二小特征值($\lambda_1 = 0$ 恒成立)。$\lambda_2 > 0 \iff$ 图连通(零特征值的重数 = 连通分量数)。共识收敛速率 $\|x(t) - \bar{x}\mathbf{1}\| \leq e^{-\lambda_2 t}\|x(0) - \bar{x}\mathbf{1}\|$,$\lambda_2$ 越大收敛越快。
微分平坦让全部 12 维状态和 4 维控制都能由平坦输出 $\sigma = (x, y, z, \psi)$ 及其有限阶导数**纯代数地**恢复,无需积分动力学。于是规划只需在 4 维 $\sigma$ 空间设计一条足够光滑的曲线(导数有界即动力学可行),用多项式/B 样条/MINCO 参数化即可。
软惩罚:把不安全"加价"但仍允许——权重 $w$ 太小则不安全,太大则过保守,且"足够大"无法事先量化,所以**没有形式保证**;求解器永远返回一个解(可能危险)。硬约束:不安全直接"不可行"——求解器宁可返回"无解"也不给危险解,这是安全保证的基础(MADER 路线)。
MINCO 用"段端点的位置/导数 + 段时间"作为最小参数,系数由这些量唯一闭式确定。优势:(a) 时间 $\{T_i\}$ 直接是决策变量 → 可时空联合优化(窄缝减速、开阔加速);(b) 代价对系数和对时间的梯度 $\partial J/\partial c_j$、$\partial J/\partial T_i$ 都解析可算 → L-BFGS 高效收敛。
CBBA 两阶段:① Bundle 构建(本地贪心,按边际收益递减把任务加入任务包);② 共识消解(邻居间交换任务包,用共识协议解决多机竞标同一任务的冲突)。去中心化体现在每机只与邻居通信、无中央协调者。收敛轮数 $\propto$ 通信图直径。

本章目标¶

学完本章后,你应该能够:

严格论证 EGO-Swarm 的凸包互斥为什么"控制点对满足安全半径 → 连续轨迹天然无碰"——从 B 样条凸包性质出发,并说清这是**充分非必要**条件、为什么因此只是"经验性安全"而非"形式化安全"
从零讲透 MADER 的检查-复核(check-recheck)协议——把"已提交轨迹集合 CommittedSet"作为**归纳不变量**,用归纳法证明异步、不同频率下仍可证不碰;并能说出它与数据库乐观并发控制(OCC)的结构同构
复现 RMADER 的递归可行性证明——延迟检查窗口 $\delta_{\max}$ + 候选/提交两步发布如何堵住 MADER 在通信延迟下的漏洞,使"若初始无碰则永远无碰"
解释 MINVO 基为什么比 B 样条/Bernstein 更适合多机碰撞检测——3 次缩小 2.36×(vs Bernstein)/254.9×(vs B 样条),7 次缩小 ~$10^{21}$×,以及更紧凸包如何换来 33.9% 更短飞行时间
分析 Primitive-Swarm 的扩展性突破——为什么基于优化的方法在 ~50 机时因邻居代价叠加而卡壳,运动原语如何把 $O(\text{NLP})$ 降为 $O(\text{lookup})$,以及代价是什么
建立通信拓扑与一致性的数学框架——$\lambda_2$ 决定编队收敛速率、切换拓扑的联合连通性、时延阈值 $\tau^* = \pi/(2\lambda_N)$,并与 Swarm-Formation 的图 Laplacian 编队代价打通
系统分类 ORCA / SFC / CBF / BVC 四类安全间距保证机制的原理、复杂度、适用场景,并能为给定任务选型
综合设计 一个完整的多机系统:从架构选择(集中/分布/混合)→ 任务分配(CBBA/VRP)→ 轨迹规划(软惩罚/硬约束/原语)→ 安全机制 → 通信协议,为每个选择给出理由

本章知识导航¶

本章的知识结构是一棵以"如何让 N 架无人机协同而不碰撞"为根的树。树干是三个递进的核心问题——用什么架构组织、用什么数学保证不碰、怎么扩展到大规模——树枝是每个问题下的具体方法与工程实现。⭐ 标注难度。

如何让 N 架机协同而不碰撞?
    │
    ├─① 用什么架构组织?  (§D10.1 架构 ⭐⭐)
    │      集中式 / 分布式 / 混合式 ── 决策树
    │      │
    │      └─→ 通信怎么传、何时收敛?  (§D10.2 通信拓扑与一致性 ⭐⭐⭐)
    │             图 Laplacian λ₂ / 切换拓扑 / 时延阈值 τ*
    │             │
    │             └─→ 图 Laplacian 编队代价 (§D10.6 Swarm-Formation ⭐⭐⭐)
    │
    ├─② 用什么数学保证不碰?
    │      │
    │      ├─ 软惩罚路线(ZJU):凸包互斥惩罚  (§D10.3 EGO-Swarm ⭐⭐⭐)
    │      │     B 样条凸包 → 控制点对斥力 → 经验性安全
    │      │
    │      ├─ 硬约束路线(MIT):MINVO+分离超平面 (§D10.4 MADER ⭐⭐⭐⭐)
    │      │     最紧凸包 + check-recheck 不变量 → 异步可证无碰
    │      │     │
    │      │     └─→ 通信延迟鲁棒化 (§D10.5 RMADER ⭐⭐⭐⭐)
    │      │            δ_max 窗口 + 两步发布 → 递归可行性
    │      │
    │      ├─ 闭环路线:DMPC 耦合约束 (§D10.8 DMPC ⭐⭐⭐)
    │      │     本地 MPC + 邻居轨迹约束 + 终端代价 → 递归可行
    │      │
    │      └─ 即插即用安全层 (§D10.9 ORCA/SFC/CBF/BVC ⭐⭐⭐)
    │            速度障碍 / 凸走廊 / 屏障函数 / Voronoi
    │
    └─③ 怎么扩展到大规模?
           │
           ├─ 运动原语替代优化 (§D10.7 Primitive-Swarm ⭐⭐⭐⭐)
           │     TOPP-RA 原语库 + swept volume → O(K·N_nbr)
           │
           ├─ 仿生群集 (§D10.10 Reynolds/Vasarhelyi ⭐⭐)
           │     三规则涌现 → 1000 机
           │
           ├─ 任务分配与路由 (§D10.11 CBBA/TSP/VRP ⭐⭐⭐)
           │
           └─ 工程实践:定位精度-安全裕度 (§D10.10 ⭐⭐)
                  │
                  ▼
           里程碑系统全景 (§D10.12 Science Robotics 2022 ⭐⭐)

小节	主题	难度	一句话
§D10.1	集群架构	⭐⭐	集中/分布/混合——单点故障 vs 全局最优的权衡
§D10.2	通信拓扑与一致性	⭐⭐⭐	$\lambda_2$ 决定收敛,$\tau^*$ 决定稳定
§D10.3	EGO-Swarm 软惩罚	⭐⭐⭐	凸包互斥惩罚,1 ms 重规划,经验性安全
§D10.4	MADER 硬约束	⭐⭐⭐⭐	MINVO 最紧凸包 + check-recheck 不变量
§D10.5	RMADER 延迟鲁棒	⭐⭐⭐⭐	$\delta_{\max}$ 窗口 + 两步发布,递归可行
§D10.6	Swarm-Formation	⭐⭐⭐	归一化 Laplacian 编队代价,MINCO 可微
§D10.7	Primitive-Swarm	⭐⭐⭐⭐	原语库替代 NLP,$O(K \cdot N_{\text{nbr}})$,千机
§D10.8	DMPC 编队控制	⭐⭐⭐	本地 MPC + 邻居约束,终端代价保递归可行
§D10.9	安全间距保证机制	⭐⭐⭐	ORCA/SFC/CBF/BVC 四方法系统对比
§D10.10	大规模群飞工程	⭐⭐	定位精度→安全裕度;仿生群集
§D10.11	任务分配与路由	⭐⭐⭐	CBBA/TSP/CVRP → LKH/OR-Tools
§D10.12	Science Robotics 2022 深度解读	⭐⭐	端到端里程碑系统全景

三条阅读线:

工程线(把集群飞起来):§D10.1 → §D10.3 → §D10.10 → §D10.12,重点是架构、最熟悉的 EGO-Swarm、工程参数。
理论线(理解安全证明):§D10.2 → §D10.4 → §D10.5 → §D10.9,重点是不变量、归纳证明、安全集。
研究线(找前沿方向):§D10.7 → §D10.6 → §D10.8 → 末尾"前沿工作与开放问题",重点是扩展性与统一框架。

无论哪条线,§D10.1(架构)和 §D10.3(EGO-Swarm)都是必读——它们是理解所有后续方法的坐标系。

前置知识桥接¶

回顾 D4(B 样条轨迹优化):D4 建立了 B 样条的**凸包性质**——曲线落在局部控制点的凸包内。本章 D10.3 的 EGO-Swarm 把这个单机用来"避静态障碍"的性质,直接搬到多机:两架机的对应控制点凸包只要分得开,两条连续轨迹就分得开。换句话说,D4 教你"控制点离障碍远",D10 教你"控制点离邻居的控制点远"——同一把锤子,换了颗钉子。

回顾 D5(MINCO 轨迹表示与安全走廊):D5 的 MINCO 用"端点导数 + 段时间"作最小参数,且代价对系数和时间的梯度都解析可算。本章 D10.6(Swarm-Formation)和 D10.12(EGO-Planner-v2)正是把"编队相似性代价""邻居互斥代价"作为新的代价项,叠加进 MINCO 的解析梯度框架——因为梯度解析,这些新代价项可以无缝接入 L-BFGS。D5 还教过 SFC(安全飞行走廊),D10.9 会把它从单机扩展到多机的"相对安全走廊 RSFC"。

回顾 04/50_多机器人协作(通用多机基础):50 系列是**与平台无关的多机地基**。其第 2 章(共识算法与分布式优化)提供 Laplacian 矩阵 $L$、代数连通性 $\lambda_2$、ADMM 的数学引擎;第 3 章(任务分配与路径规划)提供 CBBA / CBS / MAPF 的离散决策层。本章 D10 站在这两章地基上,专攻**空中多机**的特有难题:3D 高速运动中的**连续时间**碰撞检测(不是 2D 栅格上的顶点/边冲突)、欠驱动微分平坦约束下的轨迹参数化(B 样条 / MINCO)、通信不可靠条件下的异步安全保证。建议读者先完成 04/50_多机器人协作/30_共识算法与分布式优化.md 和 04/50_多机器人协作/40_任务分配与路径规划.md,再来读本章——本章的 D10.2、D10.6、D10.11 会大量复用那里的结论(并在引用处 2-3 行重述,不要求你翻回去)。

前向预告:本章把"多机协同"作为单机能力的横向扩展。下一章(D11)会进一步引入**任务级智能**(VLM 指挥集群)、新型感知(事件相机、3DGS 多机重建)与**形式化安全**(可达性分析、控制综合)——那时你会发现 D10 的 check-recheck、CBF 只是"形式化安全"这棵大树最早的两根枝。现在只需记住一个分水岭:软惩罚换速度、硬约束换保证,没有免费的午餐。

如果跳过本章会怎样¶

跳过 D10,你会卡在三个具体的地方。

场景一:"两机仿真好好的,加到第八架就开始撞。" 你照搬 EGO-Swarm 的软惩罚,3 机穿障碍完美。但 8 机在窄通道里,多个邻居的斥力梯度同时把某架机"拉向不同方向",合力为零——它卡在原地,后面的机追尾。根本原因是软惩罚没有形式保证,"权重足够大"无法事先量化。没有 D10.3 对"经验性 vs 形式化安全"的剖析,你只会盲目调 w_sw,越调越乱。

场景二:"通信一延迟就出事,但我不知道为什么。" 你实现了 MADER 的 check-recheck,0 ms 延迟时 100% 无碰。但一旦 WiFi 延迟到 100 ms,碰撞率掉到 89%。你知道"延迟有害"却说不清机理——是 recheck 时邻居刚 commit 的轨迹还在路上,没被检查到。没有 D10.5 的递归可行性证明和 $\delta_{\max}$ 窗口设计,你无法在工程中堵住这个漏洞。

场景三:"50 机以下都行,再多就规划不动了。" 你的优化器在 50 机时每周期 5 ms,100 机时变成 50 ms,规划频率从 100 Hz 掉到 20 Hz,安全裕度骤降。你知道"变慢了"但不知道是邻居代价 $O(N_{\text{nbr}} \times N_{\text{ctrl}} \times \text{NLP\_iter})$ 超线性增长,也不知道 Primitive-Swarm 如何用原语查表把它降成线性。没有 D10.7,你会以为"千机集群"只是算力问题,而看不到它是**算法范式**问题。

科研发展脉络¶

年份	论文	Venue	Stars	核心贡献
2017	Preiss 等(USC), "Crazyswarm: A Large Nano-Quadcopter Swarm"	ICRA	~360	硬件基础设施:Crazyflie 集群化;广播上传+机载存储;Python 仿真直接调用机载 C 固件
2021	Zhou 等(ZJU FAST Lab), "EGO-Swarm: A Fully Autonomous and Decentralized Quadrotor Swarm System in Cluttered Environments"	ICRA	~1.9k	去中心化 B 样条:凸包互斥惩罚;不可靠 ROS topic 广播;~1 ms 重规划
2022	Tordesillas & How(MIT ACL), "MADER: Trajectory Planner in Multi-Agent and Dynamic Environments"	TRO	~450	MINVO 基+检查-复核协议:最紧凸包包络;分离超平面硬约束;异步安全保证
2022	Zhou 等(ZJU FAST Lab), "Swarm of Micro Flying Robots in the Wild" (EGO-Planner-v2)	Science Robotics	~630	10 机林地群飞:MINCO+去中心化广播;VINS-Fusion+UWB;GPS/mocap 全无
2023	Kondo 等(MIT ACL), "Robust MADER: Decentralized Multiagent Trajectory Planner Robust to Communication Delay"	RA-L	~114	通信延迟鲁棒:延迟检查窗口 δ_max;两步发布(候选→提交);递归可行性证明;100% 无碰
2023	Quan 等(ZJU FAST Lab), "Formation Flight in Dense Environments" (Swarm-Formation)	TRO	~513	图拉普拉斯编队代价:归一化 Laplacian 矩阵刻画几何相似性;MINCO 可微
2025	Hou 等(ZJU FAST Lab), "Primitive-Swarm: An Ultra-lightweight and Scalable Planner for Large-scale Aerial Swarms"	TRO	~173	运动原语替代在线优化:TOPP-RA 预计算库;O(K·N_nbr) 线性扫描;仿真 1000 机;真机 40 机

两条技术路线的分野: - ZJU 路线(软惩罚+速度优先):EGO-Swarm → Swarm-Formation → Primitive-Swarm。代价:凸包软惩罚,无形式安全保证;优势:1-10 ms 重规划,扩展到千机 - MIT 路线(硬约束+安全优先):MADER → RMADER。代价:10-100 ms 重规划(Gurobi QP);优势:分离超平面硬约束,检查-复核协议**可证无碰**

额外里程碑(本表未列入但在正文中深入讨论):

年份	论文 / 系统	核心贡献
1987	Reynolds, "Flocks, Herds, and Schools" (Boids)	三规则: Separation / Alignment / Cohesion——仿生群集的开山之作
2004	Olfati-Saber & Murray, "Consensus Problems in Networks of Agents with Switching Topology"	连续/离散时间共识协议;切换拓扑下的收敛条件
2011	van den Berg 等, "Reciprocal n-Body Collision Avoidance" (ORCA)	速度障碍→线性规划;O(N) 无碰保证;千体实时
2018	Vasarhelyi 等, "Optimized Flocking of Autonomous Drones in Confined Environments"	30 机户外真飞群集;进化优化参数;Science Robotics
2020	Luis 等, "Online Trajectory Generation with Distributed MPC for Multi-Robot Motion Planning"	DMPC 编队:每机解本地 QP + 邻居轨迹约束;递归可行性

本章符号约定¶

符号	含义	首见
$N$	集群中无人机总数	§D10.1
$N_{\text{nbr}}$	通信/碰撞范围内的邻居数	§D10.1
$\tau_i$	代理 $i$ 的轨迹(B 样条/MINCO/原语)	§D10.1
$Q_k^i$	代理 $i$ 第 $k$ 个 B 样条控制点	§D10.3
$\mathbf{m}_k$	MINVO 基控制点	§D10.4
$d_{\text{safe}}$	安全间距	§D10.1
$r_{\text{safe}}$	控制点对安全半径	§D10.3
$\mathcal{G}(t), \mathcal{E}(t)$	通信图与边集(时变)	§D10.2
$L, \mathcal{L}$	图 Laplacian / 归一化 Laplacian	§D10.2
$\lambda_2$	代数连通性(Fiedler 值)	§D10.2
$\tau, \tau^*$	通信延迟 / 临界时延阈值	§D10.2
$\delta_{\max}$	RMADER 通信延迟上界	§D10.5
$(\mathbf{n}, d)$	分离超平面法向与偏移	§D10.4
$h_{ij}(x)$	控制屏障函数	§D10.9
$\hat{\tau}_{-i}$	代理 $i$ 收到的邻居轨迹(可能延迟/丢包)	§D10.3

§D10.1 集群架构:集中式 vs 分布式 vs 混合 ⭐⭐¶

交叉引用(2-3 行重述): 04/50_多机器人协作/20_多机系统全景.md 把多机系统抽象为"集中式 / 分布式 / 混合式"三大范畴,并从信息流和决策权角度刻画它们。本节是那个抽象框架在**空中集群**中的具化——同样的三分法,但代入真实无人机的约束(3D、欠驱动、毫秒级、坠毁代价),你会看到每种架构的扩展性上限和故障模式都变得**可量化**。

动机:为什么架构选择是集群设计的第一道题¶

你可能会想:"规划算法才是核心,架构不就是'谁算谁'的工程细节吗?"——恰恰相反。架构是**第一道题**,因为它决定了后面所有算法的可行边界:能扩展到几架、单点故障会不会全军覆没、安全保证能否形式化。选错架构,再好的轨迹优化器也救不回来。

为什么对无人机集群尤其如此?因为多旋翼集群的约束与地面机器人**本质不同**。在地面多机器人系统中,碰撞后果通常是"停下来等一等"——速度低(< 2 m/s)、质量大(惯性大、制动快)、二维空间中躲避选项多。但多旋翼集群面临:

三维空间:障碍物与邻居从上下左右各方向逼近;避碰需要完整的 3D 凸包 / 包络检测,而非 2D 栅格冲突。
欠驱动:四旋翼只有 4 个推力输入控制 6 自由度——加速度方向受限于推力矢量,不能像全向车那样瞬间横移。
速度快、碰撞后果严重:3-10 m/s 飞行速度下,两机对飞 closing speed 可达 20 m/s;碰撞 = 坠毁 = 物理损失。
通信延迟与丢包:WiFi/Zigbee 在户外多径环境下延迟 10-500 ms 不等;丢包率在遮挡环境下可达 30%+。
算力受限:机载计算平台(如 Jetson NX、STM32)的算力远低于工作站。

这些约束使得架构选择直接决定了集群的安全性、扩展性和实时性。

反面:如果忽视架构、直接用单机思路堆机会怎样? 设想你把单机 EGO-Planner 复制 50 份,各自独立飞、互不通信。结果是:每架机把其他 49 架当成"突然出现的动态障碍",靠深度相机临时反应。但深度相机有视场角盲区(背后看不见)、有延迟(感知到反应 ~100 ms)、3-10 m/s 下 100 ms 就是 0.3-1 m 的位移。两机从盲区对冲时,等看见已经来不及——这正是"无架构"的代价:你失去了**预测性协同**,只剩下**被动反应**,而被动反应在高速 3D 中不够快。架构的本质,就是用通信换预测、用协调换安全裕度。

类比(带边界):集群架构 ≈ 公司的组织结构。 - 像的地方:集中式 = 老板拍板所有决策(信息全、最优,但老板一倒全瘫、人多了管不过来);分布式 = 每个员工自主决策只跟邻座沟通(抗单点故障、能无限扩招,但容易各自为政、目标不一致);混合式 = 高层定战略、各部门自治执行(战略层慢而稳、执行层快而灵)。 - 不像的地方:公司里"决策慢一点"顶多损失效率,无人机集群里"决策慢一点 = 坠毁";公司沟通用自然语言可以反复澄清,集群通信是单向广播、丢了就丢了、没有"再说一遍";公司的"员工"不会以 20 m/s 的相对速度撞向彼此。所以别把这个类比推到"安全性"维度——组织学里没有"递归可行性证明",但集群里有(见 §D10.5)。

本质洞察:架构选择的底层是"信息的集中度"与"故障的传染性"之间的对偶。 你掌握的全局信息越多(集中式),决策越优、协调越彻底,但这份"全局信息"必须汇聚到一个点,于是这个点的故障会传染给所有人。反之,信息越分散(分布式),任何单点故障都被隔离,但每个决策者只看得到局部、只能凑出局部最优。没有"既掌握全局又无单点故障"的免费午餐——混合式不是第三种魔法,而是在"任务级(低频、可集中)"和"轨迹级(高频、必分布)"之间画了一条分界线,让每一层各取所长。后文所有方法的分野,归根到底都是这条"集中度光谱"上的不同取点。

D10.1.2 三种架构详解¶

集中式架构(Centralized)

                   ┌──────────────┐
                   │  中央规划器   │
                   │ (地面站/领航机) │
                   └──────┬───────┘
                          │ 广播全局轨迹
              ┌───────────┼───────────┐
              ▼           ▼           ▼
           ┌─────┐    ┌─────┐    ┌─────┐
           │ UAV1│    │ UAV2│    │ UAV3│
           └─────┘    └─────┘    └─────┘
               (仅执行跟踪控制,不做规划)

工作方式:中央节点收集所有无人机的状态(位置、速度、电量),求解一个联合优化问题(如联合 QP 或 MILP),将分配好的轨迹下发给每架无人机;每机仅做底层跟踪控制(如 PID / 几何控制器)。

数学描述: $$ \min_{\tau_1, \tau_2, \ldots, \tau_N} \sum_{i=1}^{N} J_i(\tau_i) + \sum_{i<j} J_{ij}^{\text{couple}}(\tau_i, \tau_j) $$ $$ \text{s.t.} \quad \tau_i \in \mathcal{F}i \quad \forall i, \quad |\tau_i(t) - \tau_j(t)| \geq d \quad \forall i \neq j, \forall t $$}

其中 $J_i$ 是单机代价(平滑性、时间、能量),$J_{ij}^{\text{couple}}$ 是耦合代价(编队误差、协同时序),$\mathcal{F}_i$ 是动力学可行域,$d_{\text{safe}}$ 是安全间距。

优势: - 全局最优:中央节点掌握全信息,可求解全局最优(或已知次优界) - 冲突消解彻底:所有碰撞约束在一个问题里处理,无需多轮协商 - 编队精度高:适合需要精确几何编队的场景(灯光秀、编队表演)

劣势: - 单点故障:中央节点宕机 → 全群失控 - O(N³) 扩展性瓶颈:联合 QP 的决策变量数 = N × 单机变量数,约束数 = O(N²) 碰撞对;N > 20 时实时性告急 - 通信瓶颈:N 架无人机的状态回传 + 轨迹下发 = O(N) 双向流量,全部经由中央节点 → 带宽饱和 - 延迟敏感:状态回传→规划→下发的闭环延迟限制了最大飞行速度

典型代表: - Intel/Nova Sky Stories 灯光秀:预编排 GPS 航点 + 集中上传,数千机但**非自主**(无机间通信、无实时避障、无机载 SLAM) - Crazyswarm 1.0 (Preiss et al., ICRA 2017):集中式广播上传 + 机载存储执行;mocap 定位

关键洞察——灯光秀 ≠ 自主群飞:

Intel 千机灯光秀虽然壮观,但在技术维度上与 ZJU 林地 10 机群飞完全不同:

维度	Intel 灯光秀	ZJU Science Robotics 2022
轨迹来源	预编排(离线设计)	在线实时规划
定位	RTK-GPS(厘米级)	VINS-Fusion + UWB(无 GPS/mocap)
避障	无(假设空旷场地)	深度相机 + 3D 占据栅格
机间通信	无(仅与地面站通信)	去中心化 ROS topic 广播
故障处理	中央监控 + 返航	机载自主悬停 / 重规划
复杂度核心	编排美学(动画设计)	感知-规划-控制全栈自主

分布式架构(Decentralized / Distributed)

    ┌─────┐         ┌─────┐         ┌─────┐
    │ UAV1│◄───────►│ UAV2│◄───────►│ UAV3│
    └──┬──┘         └──┬──┘         └──┬──┘
       │               │               │
       ▼               ▼               ▼
   本地感知          本地感知          本地感知
   本地规划          本地规划          本地规划
   本地控制          本地控制          本地控制

工作方式:每架无人机拥有完整的感知-规划-控制栈;通过邻居间(peer-to-peer)通信交换自身轨迹参数;每机独立求解本地优化问题,将邻居轨迹作为约束或惩罚项。

数学描述(以 EGO-Swarm 为例): $$ \text{Agent } i: \quad \min_{\tau_i} \underbrace{J_s(\tau_i)}{\text{平滑}} + \underbrace{J_c(\tau_i)}}} + \underbrace{J_d(\tau_i){\text{动力学}} + \underbrace{J}}(\tau_i; \hat{\tau{-i})} $$}

其中 $\hat{\tau}_{-i}$ 是从邻居广播中接收到的轨迹(可能有延迟 / 丢包)。注意:每机解的是**单机**优化问题,邻居信息以**参数**形式注入,而非决策变量。

优势: - 无单点故障:任何一架退出不影响其余 - 扩展性好:每机计算量与邻居数(而非总机数)成正比 - 通信开销分散:仅与通信范围内的邻居交换

劣势: - 全局最优性缺失:局部信息 → 局部最优;可能出现死锁、振荡 - 安全性难以形式化保证:软惩罚不等于硬约束;消息丢失可能导致"我以为你不在这里" - 一致性收敛需要时间:切换拓扑下共识可能暂时发散

典型代表: - EGO-Swarm (Zhou et al., ICRA 2021) - EGO-Planner-v2 (Zhou et al., Science Robotics 2022) - Primitive-Swarm (Hou et al., TRO 2025) - Vasarhelyi et al. 30 机户外群集 (Science Robotics 2018)

混合架构(Hybrid)

                ┌───────────────┐
                │  全局任务分配   │  (集中式层)
                │  TSP/CVRP/CBBA│
                └───────┬───────┘
                        │ 任务目标 + 全局约束
            ┌───────────┼───────────┐
            ▼           ▼           ▼
    ┌───────────┐ ┌───────────┐ ┌───────────┐
    │ 子集群 A  │ │ 子集群 B  │ │ 子集群 C  │
    │ 去中心化  │ │ 去中心化  │ │ 去中心化  │
    │ 轨迹规划  │ │ 轨迹规划  │ │ 轨迹规划  │
    └───────────┘ └───────────┘ └───────────┘
     (分布式层)    (分布式层)    (分布式层)

工作方式: - 上层(集中式):全局任务分配、区域划分、高层路由——这些是**低频、离散**决策,对实时性要求不高,集中式可以接受 - 下层(分布式):每个子集群内部做去中心化轨迹规划与避碰——这些是**高频、连续**决策,必须本地快速求解

典型代表: - RACER (Zhou et al., TRO 2023):集中式多机探索任务分配(OR-Tools CVRP)+ 分布式 FUEL 探索规划 - MUI-TARE (Cao et al., RA-L 2023):集中式子区域分配 + 分布式 TARE 探索 - mrs_uav_system (CTU MRS):集中式任务层 + 分布式控制层;DARPA SubT Challenge 获奖系统

三种架构对比总表:

维度	集中式	分布式	混合式
最优性	全局最优(可行时)	局部最优	上层全局 + 下层局部
扩展性	O(N³),~20 机上限	O(N_nbr),~1000 机	上层 O(N),下层 O(N_nbr)
鲁棒性	单点故障	无单点故障	上层故障可降级为纯分布式
通信需求	O(N) 集中流量	O(N_nbr) 分散流量	上层低频 + 下层分散
实时性	受限于中央规划周期	每机独立,~1-10 ms	上层慢(~1 Hz)+ 下层快(~10-100 Hz)
安全保证	容易(全局约束)	困难(需特殊协议)	混合
适用场景	灯光秀、小规模精确编队	大规模自主群飞	多机探索、异构编组

D10.1.3 架构选择决策树¶

你的任务需要多少架无人机?
├── ≤ 5 架 且 有可靠中央通信
│   └── 集中式(简单、全局最优)
├── 5-50 架 且 需要精确编队
│   └── 混合式(集中分配 + 分布式执行)
├── 5-50 架 且 自主探索/搜索
│   └── 混合式(集中区域划分 + 分布式规划)
├── 50-1000 架 且 群集迁移/覆盖
│   └── 分布式(仿生群集 + 局部避碰)
└── 任何规模 且 要求形式化安全保证
    └── 分布式 + MADER/RMADER 类协议

本节常见陷阱¶

陷阱 1:把"灯光秀千机"当成"自主集群千机"的证据。 - 错误描述:看到 Intel/大疆数千架无人机灯光秀,得出"千机全自主群飞已经成熟"的结论,据此低估自主集群的技术难度。 - 现象/后果:在项目立项或论文 related work 中误判技术成熟度;以为"扩展到千机"只是买更多机的钱的问题,而非算法问题。 - 根本原因:混淆了"集中式预编排"与"分布式在线自主"两种**本质不同**的架构。灯光秀是离线设计 GPS 航点 + 集中上传,无机间通信、无实时避障、无机载 SLAM——它的"千机"扩展性来自**没有任何在线协同**(每架机只是回放预录轨迹),而自主集群的难度恰恰全在在线协同。 - 正确做法:评估一个集群系统时,问五个鉴别问题:轨迹是在线生成还是离线预录?有无机间通信?有无实时避障?有无机载 SLAM(还是依赖外部定位)?故障如何处理?(对照 §D10.1.2 的"灯光秀 ≠ 自主群飞"表)五项全"在线/有/机载"的才是自主集群。

陷阱 2:在需要扩展的场景里默认选集中式。 - 错误描述:因为集中式"全局最优、好实现、好调试",在 30 机以上的探索/搜索任务里仍选集中式联合优化。 - 现象/后果:仿真里 10 机跑得动,真机 30 机时中央规划器求解时间超过控制周期,轨迹下发滞后,或中央节点一掉线全群悬停/失控。 - 根本原因:集中式联合 QP 的决策变量 $\propto N$、碰撞约束 $\propto N^2$,求解复杂度约 $O(N^3)$;$N > 20$ 时实时性告急。这是**算法复杂度的硬墙**,不是换更快 CPU 能绕过的。 - 正确做法:按 §D10.1.3 决策树选型——需要扩展(> 20 机)时用混合式(集中分配 + 分布式执行)或纯分布式;只在 ≤ 5 机且有可靠中央通信时用集中式。把"全局最优"让位给"可扩展 + 无单点故障"。

本节练习¶

[思考题] 在一个 100 机集群中,如果有 5% 的无人机突然失联(电池耗尽或通信故障),对集中式、分布式、混合式架构分别有什么影响?哪种架构的降级最优雅?提示:从"失联节点是否承担全局信息"和"故障传染范围"两个角度分析。
[设计题] 你需要为 30 架四旋翼设计一个在 200m×200m 区域搜索地面目标的系统。仅就**架构层**给出选择(集中/分布/混合)并用一句话说明理由,再指出你的选择在"中央节点失效"时的降级行为。
[A 型·架构对比仿真] 用任意多机仿真框架,搭建一个 10 机"位置互换"场景,分别用集中式(一个进程解联合 QP)和分布式(每机独立 + 广播)实现,测量并对比:① 规划总耗时随机数增长的曲线;② 人为杀掉"中央进程/某架机"后的系统行为。

§D10.2 通信拓扑与一致性协议 ⭐⭐⭐¶

交叉引用(2-3 行重述): 04/50_多机器人协作/30_共识算法与分布式优化.md §2.1 证明了线性共识协议 $\dot{x} = -Lx$ 在固定连通图上收敛到初始均值,速率由代数连通性 $\lambda_2$ 决定;§2.3 推导了引入通信时延 $\tau$ 后系统稳定的阈值 $\tau^* = \pi/(2\lambda_N)$。本节直接复用这两个结论,但代入空中集群的现实——$L$ 不是常数,因为无人机在飞、通信范围在变,图的边集 $\mathcal{E}(t)$ 时刻在增删。我们要回答:当 $L$ 不断切换时,共识还收敛吗?

动机:为什么编队问题绕不开图论¶

你已经知道单机怎么飞。现在 10 架机要保持一个三角队形飞过森林——这件事为什么不能各飞各的? 因为"队形"是一个**相对**概念:第 3 架要待在第 1、2 架连线的中点上方,它必须知道 1、2 在哪。可它只看得到通信范围内的邻居,而且这个范围随飞行不断变化。于是问题变成:每架机只和邻居交换信息,如何让整个集群对"我们现在是什么队形、该往哪调整"达成一致? 这正是"共识(consensus)"——而描述"谁能和谁通信"的数学对象,就是图。一旦把通信关系写成图,Laplacian 矩阵 $L$ 的谱(特别是 $\lambda_2$)就直接告诉你"信息传得多快、队形收敛多快、延迟多大会崩"。这就是为什么集群编队绕不开图论:图论是"局部通信→全局协同"这个跳跃的唯一通用语言。

反面:如果不用图论、改用"全员广播 + 平均"会怎样? 设想每架机把自己的位置广播给所有人,各自取平均当作队形中心。问题有三:① 全员广播是 $O(N^2)$ 通信,100 机时带宽爆炸(见 §D10.10);② 远处的机根本收不到你的广播(超出通信半径),"全员"是假设不是事实;③ 你无法分析"信息传到对角那架机要几跳、要多久"——而这恰恰决定了队形能多快响应扰动。图论用 $\lambda_2$ 一个标量就量化了这一切,还能处理"图在变"的真实情况(切换拓扑,见 §D10.2.3)。抛开图论,你既算不出扩展性,也证不出收敛性。

类比(带边界):图 Laplacian $L$ ≈ 离散版的"扩散方程算子"。 - 像的地方:连续介质里热量从高温流向低温,扩散方程 $\dot{u} = \Delta u$(拉普拉斯算子 $\Delta$)让温度场趋于均匀;集群里共识 $\dot{x} = -Lx$ 让每个节点的状态趋于邻居平均,信息像热量一样从"高"处流向"低"处,最终全场均匀(达成共识)。$L$ 就是图上的 $-\Delta$。 - 不像的地方:连续扩散的速率由材料导热系数决定,处处可调;图上的扩散速率由**拓扑结构**($\lambda_2$)决定,你只能通过增删边(改变谁和谁通信)来调,不能连续微调;而且图会**突然断开**(两架机飞出通信范围),这在连续介质里没有对应——热传导不会"忽然某两点之间不导热了"。所以别把扩散方程的"平滑、连续"直觉用到切换拓扑上——那里收敛性靠的是"联合连通性"(§D10.2.3),是离散事件的拼接,不是光滑演化。

D10.2.1 通信图基础回顾¶

给定 $N$ 架无人机,定义通信图 $\mathcal{G}(t) = (\mathcal{V}, \mathcal{E}(t))$:

节点集 $\mathcal{V} = \{1, 2, \ldots, N\}$
边集 $\mathcal{E}(t) = \{(i,j) : \|p_i(t) - p_j(t)\| \leq R_{\text{comm}}\}$,其中 $R_{\text{comm}}$ 是通信半径

对应的矩阵定义: - 邻接矩阵 $A(t)$:$a_{ij}(t) = 1$ 若 $(i,j) \in \mathcal{E}(t)$,否则为 0 - 度矩阵 $D(t) = \text{diag}(d_1(t), \ldots, d_N(t))$,其中 $d_i(t) = \sum_j a_{ij}(t)$ - Laplacian 矩阵 $L(t) = D(t) - A(t)$

关键谱性质: - $L$ 半正定,最小特征值 $\lambda_1 = 0$,对应特征向量 $\mathbf{1}$ (全 1 向量) - 图连通 $\iff$ $\lambda_2(L) > 0$(代数连通性 / Fiedler 值) - $\lambda_2$ 越大 → 共识收敛越快;$\lambda_N$ 越大 → 时延容忍越差

D10.2.2 连续时间共识与编队¶

基本共识协议: $$ \dot{x}i(t) = -\sumi(t)} (x_i(t) - x_j(t)) = -\sum(t) x_j(t) $$}^{N} L_{ij

对固定连通拓扑,收敛速率: $$ |x(t) - \bar{x}\mathbf{1}|_2 \leq e^{-\lambda_2 t} |x(0) - \bar{x}\mathbf{1}|_2 $$

其中 $\bar{x} = \frac{1}{N}\sum_i x_i(0)$ 是初始均值。

编队控制:将共识协议推广到位移(displacement-based)编队: $$ \dot{p}i = -\sum \left[ (p_i - p_j) - (p_i^d - p_j^d) \right] $$}_i

其中 $p_i^d$ 是代理 $i$ 在期望编队中的位置。收敛到编队 $\iff$ $p_i - p_j \to p_i^d - p_j^d$ 对所有边 $(i,j)$。

三维编队的特殊性:在 3D 中,需要对 $(x, y, z)$ 三个分量分别运行共识,但由于四旋翼的欠驱动特性,垂直方向(z)的跟踪带宽通常低于水平方向——这在编队控制器设计中需要区分对待。

D10.2.3 切换拓扑下的收敛¶

在实际飞行中,通信拓扑随时间变化(无人机移动导致 $\mathcal{E}(t)$ 变化)。经典结果:

定理(联合连通性,Jadbabaie et al. 2003): 如果存在时间间隔序列 $[t_k, t_{k+1})$,使得每个间隔内的**联合图**(将该间隔内所有瞬时图的边集取并)是连通的,则共识收敛。

对集群的含义:即使某个瞬间图断开(两组无人机暂时超出通信范围),只要在有限时间内能重新连接,编队信息仍然可以传播到整个集群。

量化:设切换周期为 $T_{\text{switch}}$,联合图的 $\lambda_2^{\text{joint}}$ > 0,则收敛速率 $\approx e^{-\lambda_2^{\text{joint}} t / T_{\text{switch}}}$,比固定拓扑慢 $T_{\text{switch}}$ 倍。

D10.2.4 通信时延对集群稳定性的影响¶

交叉引用: 04/50_多机器人协作/30_共识算法与分布式优化.md §2.3 推导了时延阈值 $\tau^* = \pi / (2\lambda_N)$。

考虑延迟 $\tau$ 的共识协议: $$ \dot{x}i(t) = -\sum (x_i(t) - x_j(t - \tau)) $$}_i

临界时延阈值: $$ \tau^* = \frac{\pi}{2\lambda_N(L)} $$

$\tau < \tau^*$:系统稳定,共识收敛
$\tau > \tau^*$:系统可能振荡甚至发散

对集群设计的工程含义:

参数	典型值	影响
WiFi 延迟	5-50 ms(室内),50-200 ms(室外多径)	中等
Zigbee/UWB 延迟	1-10 ms	低
5G/LTE 延迟	10-50 ms(理论),50-500 ms(实测)	高
机载处理延迟	5-20 ms(Jetson),< 1 ms(STM32 简单运算)	低-中
端到端闭环延迟	通信 + 处理 + 执行 = 50-300 ms	关键

设计准则: 1. 选择 $R_{\text{comm}}$ 使得 $\lambda_N$ 不太大(稀疏但连通的图) 2. 使用 UWB 而非纯 WiFi 以降低 $\tau$ 3. 在 $\tau$ 已知上界的条件下,使用 RMADER 类延迟感知协议

D10.2.5 集群中的通信协议设计¶

广播模式(EGO-Swarm / EGO-Planner-v2):

每机以固定频率(如 20 Hz)广播:
  消息 = {agent_id, timestamp, B-spline/MINCO 参数}
接收方:
  对每个 agent_id 维护最新轨迹缓存
  若 Δt > timeout(如 500 ms)则标记该邻居为 "lost",回退悬停

优势:简单、低带宽(每条消息 ~200-500 bytes)、无握手开销
劣势:无确认 → 发送方不知道谁收到了;消息可能乱序到达

请求-响应模式(MADER / RMADER):

规划器想发布新轨迹:
  1. [Check] 请求所有邻居发送当前 committed 轨迹
  2. 收到响应后,求解优化
  3. [Recheck] 再次请求,检查期间是否有新 commit
  4. 若无冲突 → 广播 commit;若有 → 放弃,保留旧轨迹

优势:可提供安全保证
劣势:需要可靠通信(或已知延迟上界);每次规划需要多轮往返

混合模式(Primitive-Swarm):

广播:原语索引 + 时间戳(极小数据量,~20 bytes)
本地:在原语库中查询碰撞(无需等待确认)

优势:带宽极低,延迟容忍性好
劣势:原语库有限 → 轨迹质量受限于库的丰富度

D10.2.6 Swarm-Formation 中的图 Laplacian 编队代价¶

交叉引用: 本节直接建立在 04/50_多机器人协作/30_共识算法与分布式优化.md §2.4(编队控制)和 04/50_多机器人协作/50_分布式MPC多足编队.md 的数学工具之上。

Swarm-Formation (Quan et al., TRO 2023) 提出了一种用**归一化 Laplacian 矩阵**量化编队相似性的方法:

核心思想:给定当前编队位置 $\{p_i\}$ 和期望编队 $\{p_i^d\}$,构造各自的交互图 $\mathcal{G}$ 和 $\mathcal{G}^d$,其归一化 Laplacian 分别为 $\mathcal{L}$ 和 $\mathcal{L}^d$。编队相似性代价:

\[ J_{\text{form}} = \|\mathcal{L} - \mathcal{L}^d\|_F^2 \]

其中 $\|\cdot\|_F$ 是 Frobenius 范数。

归一化 Laplacian 的优势(vs 普通 Laplacian): - 旋转不变性:编队整体旋转不改变 $\mathcal{L}$ - 平移不变性:编队整体平移不改变 $\mathcal{L}$ - 尺度不变性:编队整体缩放不改变归一化后的 $\mathcal{L}$

这意味着编队只要保持相对几何关系,就可以自由地旋转、平移、缩放以适应环境(如穿过窄缝时缩小队形,通过后恢复)。

可微性:$J_{\text{form}}$ 对每个 $p_i$ 的梯度可以解析计算,因此可以直接嵌入 MINCO 的梯度下降优化框架中。这是 Swarm-Formation 的关键工程突破——把图论指标变成了可微的优化代价项。

本质洞察:$\lambda_2$ 是集群的"带宽",$\tau^*$ 是集群的"采样定理"。 把集群想成一个信号处理系统:你要把"队形指令""扰动响应"这些信号在节点间传递。$\lambda_2$ 决定了这个系统的**带宽**——它越大,信息传得越快、队形收敛越急、对扰动响应越灵敏(收敛速率正比于 $\lambda_2$)。而 $\tau^* = \pi/(2\lambda_N)$ 决定了**最大可容忍延迟**——它像采样定理一样划出一条红线:延迟超过这条线,反馈就从"稳定校正"变成"过冲振荡"直至发散。深刻之处在于这两者**此消彼长**:加密通信图(增边)能提升 $\lambda_2$ 加快收敛,但同时抬高 $\lambda_N$ 压缩了延迟容忍度。所以集群通信图的设计不是"连得越多越好",而是在"收敛快"和"抗延迟"之间找平衡——**稀疏但连通**往往优于"全连接"。这条对偶贯穿后面所有去中心化方法:EGO-Swarm 限制邻居数(隐式控制 $\lambda_N$)、RMADER 显式建模 $\delta_{\max}$,本质都在这条红线两侧操作。

本节常见陷阱¶

陷阱 1:在切换拓扑下要求"每一时刻都连通"。 - 错误描述:设计通信策略时,强行要求任意时刻通信图都连通(如加大发射功率、缩小编队),否则认为共识必然失败。 - 现象/后果:为维持瞬时连通而过度收紧编队或耗尽通信功率;或在某机短暂飞出范围时误判"集群已失效"触发不必要的全体悬停。 - 根本原因:误用了"固定拓扑连通"的结论。Jadbabaie 等(2003)的联合连通性定理表明:即使每个瞬间图都**断开**,只要在有限时间窗口内**所有瞬时图的边并集**连通,共识依然收敛——信息可以"接力"传播,不需要任何单一时刻全连通。 - 正确做法:用"联合连通性"而非"瞬时连通性"作为设计准则(§D10.2.3)。允许拓扑短暂断开,只要保证在 $T_{\text{switch}}$ 窗口内联合图连通即可;收敛会慢约 $T_{\text{switch}}$ 倍,但不会失败。把功率/队形预算花在"保证联合连通"而非"保证瞬时全连通"上。

陷阱 2:忽视通信延迟,把共识增益调到最大求快。 - 错误描述:为了让编队收敛更快,无脑增大共识/编队控制增益,或加密通信图提升 $\lambda_2$。 - 现象/后果:仿真(零延迟)里收敛飞快,真机上却出现编队"抖动""过冲",甚至低频振荡发散——尤其在 WiFi 户外多径(50-200 ms 延迟)下。 - 根本原因:增益越大、图越密,$\lambda_N$ 越大,临界时延阈值 $\tau^* = \pi/(2\lambda_N)$ 越小;真实延迟 $\tau$ 一旦超过 $\tau^*$,负反馈变正反馈。这是连续时延系统的根本稳定性边界,不是调参玄学。 - 正确做法:先测出端到端延迟 $\tau$ 的上界,反推 $\lambda_N$ 的容许上界,据此限制增益和图密度;延迟大时改用 UWB(1-10 ms)降低 $\tau$,或采用 RMADER 类延迟感知协议(§D10.5)。记住"稀疏但连通"的图既省带宽又抗延迟。

本节练习¶

[B 型·通信拓扑仿真] 编写一个 2D 共识仿真:① N=20 个代理随机分布;② 通信半径 $R_{\text{comm}}=2.0$;③ 运行 $\dot{x}_i = -\sum_{j\in\mathcal{N}_i}(x_i-x_j)$;④ 绘制 $\|x-\bar{x}\mathbf{1}\|$ vs $t$;⑤ 改变 $R_{\text{comm}}$,观察 $\lambda_2$ 和收敛速率的关系;⑥ 加入通信延迟 $\tau$,找到使系统发散的临界 $\tau^*$,与 $\pi/(2\lambda_N)$ 对比。
[计算题] 给定通信图 Laplacian $L = \begin{bmatrix} 2 & -1 & -1 & 0 \\ -1 & 2 & 0 & -1 \\ -1 & 0 & 2 & -1 \\ 0 & -1 & -1 & 2 \end{bmatrix}$:(a) 画出对应通信图;(b) 计算 $\lambda_2$;(c) 若延迟 $\tau = 0.5$ s,系统是否稳定?(算 $\tau^* = \pi/(2\lambda_N)$)
[思考题] Swarm-Formation 用归一化 Laplacian 而非普通 Laplacian 刻画编队,从而获得旋转/平移/尺度不变性。为什么这三个不变性对"穿窄缝"任务至关重要?如果改用普通 Laplacian(对绝对位置敏感),编队穿缝时会发生什么?

§D10.3 EGO-Swarm——软惩罚去中心化 ⭐⭐⭐¶

GitHub: ZJU-FAST-Lab/ego-planner-swarm, ~1.9k★(论文:Zhou et al., ICRA 2021)。

动机:为什么是"软惩罚"¶

前面 §D10.1 论证了大规模集群必须去中心化:每机只解自己的优化问题,把邻居当约束。现在的核心问题是——邻居避碰这个约束,怎么写进单机优化里? 最直接的想法是写成硬约束"我和每个邻居的距离 ≥ $d_{\text{safe}}$"。但这有两个麻烦:① 连续轨迹上"任意时刻任意邻居"的距离约束是无穷多个,没法直接喂给优化器;② 硬约束一旦不可行,求解器直接报错——可飞行中你**必须**输出一条轨迹,哪怕略保守,也不能"无解然后掉下来"。

EGO-Swarm 的答案是:把避碰写成可微的惩罚项,叠加进本就在解的梯度优化里。它复用了 D4 学过的 B 样条凸包性质——曲线落在控制点凸包内,所以"控制点对离得够远"就**充分**保证"曲线段离得够远"。这一步把"无穷维的曲线距离约束"降成了"有限个控制点对的距离惩罚",而且因为 B 样条优化本来就是对控制点做 L-BFGS,这个惩罚项的梯度可以无缝加进去。代价是:惩罚不是硬约束,安全性只是"经验性"的——这正是它和后面 MADER(§D10.4)的根本分野。

反面:如果用硬约束 + 实时求解器(MADER 路线)替代软惩罚,在 EGO-Swarm 的目标场景里会怎样? EGO-Swarm 要的是 ~1 ms 重规划、贴树高速穿林。硬约束需要 Gurobi 解 MIQP(分离超平面 + 整数变量),单次 10-100 ms——慢了 1-2 个数量级,无法支撑高速。而且硬约束在极窄通道里可能直接"不可行",此时 MADER 保留旧轨迹等下一周期,但高速飞行下"等一周期"可能已经撞上。软惩罚用"放弃形式保证"换来了"永远有解 + 毫秒级",这对"速度优先"的 ZJU 路线是正确的取舍。反过来,在"安全优先、可以慢"的场景(如载人区域上方),软惩罚的"经验性"就不可接受——该选 MADER。

类比(带边界):软惩罚 ≈ 弹簧斥力,硬约束 ≈ 刚性墙。 - 像的地方:软惩罚像在邻居周围放一个弹簧——越靠近斥力越大($(\cdot)^3$ 惩罚),把你"推开";墙(硬约束)则是绝对不可穿越的边界。两者都让你远离邻居。 - 不像的地方:弹簧可以被"压穿"——只要别的代价项(比如赶时间)的拉力足够大,你仍可能进入安全距离内(只是要付出很高代价);墙不行,一毫米都过不去。这正是"经验性 vs 形式化"安全的物理直觉:软惩罚的安全裕度取决于权重和其他力的相对大小,无法事先保证;硬约束的安全裕度是绝对的。 所以别把"权重调大点就安全了"当成保证——弹簧再硬也是弹簧。

D10.3.1 系统架构¶

┌────────────────────────────────────────────────────────┐
│                    每架无人机机载                        │
│                                                        │
│  ┌──────────┐    ┌──────────┐    ┌──────────┐          │
│  │ 深度相机  │──►│  ESDF    │──►│  前端    │          │
│  │ (d435i)  │    │ 占据栅格  │    │ A*+拓扑  │          │
│  └──────────┘    └──────────┘    └────┬─────┘          │
│                                       │ 引导路径        │
│  ┌──────────┐                   ┌─────▼─────┐          │
│  │ VIO/UWB  │──────────────────►│ B-spline  │          │
│  │ 状态估计  │    状态           │ 后端优化   │          │
│  └──────────┘                   └─────┬─────┘          │
│                                       │ 优化轨迹        │
│  ┌──────────┐                   ┌─────▼─────┐          │
│  │ ROS topic│◄──广播──────────►│ 轨迹执行   │          │
│  │ 邻居轨迹  │    my_traj        │ 几何控制器  │          │
│  └──────────┘                   └───────────┘          │
│                                                        │
│       ▲ 接收邻居轨迹                                    │
│       │                                                │
└───────┼────────────────────────────────────────────────┘
        │
   WiFi / Zigbee / UWB
        │
┌───────┼───────┐
│  其他无人机    │
│  (同构架构)   │
└───────────────┘

D10.3.2 多机互斥代价详解¶

多机互斥代价:

J_swarm = Σ_{j∈neighbors} Σ_{k} max(0, r_safe - ‖Q_k^self - Q_k^j‖)³

其中 Q_k 是 B 样条控制点。由凸包性质:控制点安全 → 曲线安全。

为什么用立方而非二次?

二次惩罚 $(\cdot)^2$ 在边界处梯度为零——当两机恰好在安全距离边缘时,优化器"不急着让开"
立方惩罚 $(\cdot)^3$ 在边界处梯度也为零但二阶导不为零——更温和的激活但更强的驱动力
实际上 EGO-Swarm 使用的是**三阶惩罚**(cubic penalty),在控制点间距小于 $r_{\text{safe}}$ 时提供递增的排斥力

梯度推导:

对代理 $i$ 的第 $k$ 个控制点 $Q_k^i$: $$ \frac{\partial J_{\text{swarm}}}{\partial Q_k^i} = \sum_{j \in \mathcal{N}i} 3 \cdot \max(0, r $$}} - |Q_k^i - Q_k^j|)^2 \cdot \frac{Q_k^i - Q_k^j}{|Q_k^i - Q_k^j|

这个梯度指向"远离邻居"的方向,大小与侵入深度的平方成正比。

凸包安全性论证:

B 样条的凸包性质保证曲线 $\mathbf{p}(t)$ 落在控制点的凸包内。对于 $p$ 阶均匀 B 样条,每个时间区间 $[t_k, t_{k+1})$ 内的曲线段包含在 $\{Q_{k-p}, Q_{k-p+1}, \ldots, Q_k\}$ 这 $p+1$ 个控制点的凸包内。

因此,如果任意两个**对应时间**的控制点组之间的最小距离 $\geq r_{\text{safe}}$,则对应时间的曲线段之间的距离也 $\geq r_{\text{safe}}$。

注意:这是充分条件而非必要条件——控制点凸包之间的距离是曲线实际距离的一个保守下界。这正是"软惩罚"方法的性质:它可能过于保守(让无人机绕更远的路),但**在惩罚权重足够大的前提下**能有效避碰。然而,权重"足够大"这个条件无法事先精确量化——这就是为什么 EGO-Swarm 没有**形式化**的安全保证。

D10.3.3 通信模式与消息格式¶

通信模式:每机以 20 Hz 广播自己的 B 样条参数(阶次+节点+控制点)到 ROS topic。无握手、无确认——消息丢失由 20 Hz 安全定时器处理(回退到悬停)。

消息结构:

// 简化的 ROS 消息结构
struct BsplineMsg {
    int32    drone_id;         // 代理 ID
    float64  start_time;       // 轨迹起始时间戳
    int32    order;            // B 样条阶次(通常 = 3)
    float64  knot_span;        // 均匀节点间隔 Δt
    Point3D  control_points[]; // 控制点序列(x,y,z)
};
// 单条消息大小:~200-500 bytes(取决于控制点数)
// 20 Hz × 500 bytes = 10 KB/s per agent(带宽需求很低)

安全定时器机制:

// 在 ego_replan_fsm.cpp 的主循环中(简化逻辑)
void EgoReplanFSM::execFSMCallback() {
    for (auto& [id, info] : received_trajs_) {
        double dt = ros::Time::now() - info.last_recv_time;
        if (dt > NEIGHBOR_TIMEOUT) {  // 通常 0.5s
            // 标记该邻居为 "lost"
            info.is_valid = false;
            // 若该邻居距离本机 < DANGER_DIST → 紧急悬停
            if (distance_to(info.last_known_pos) < DANGER_DIST) {
                trigger_emergency_hover();
            }
        }
    }
}

D10.3.4 死锁破对称¶

死锁破对称:have_recv_pre_agent_ 旗标——ID 较小的代理拥有优先权,ID 较大的需要让路。简单但有效。

死锁场景分析:

场景:两机对飞(head-on)
                ←───── UAV2 (id=2)
UAV1 (id=1) ─────►

两机同时检测到对方,同时规划让路:
  若都往左让 → 仍然对头
  若都往右让 → 仍然对头
  → 死锁(livelock)

EGO-Swarm 的解法:
  UAV1 (id=1):我 ID 小,我有优先权,我维持原路线
  UAV2 (id=2):我 ID 大,我需要让路,我绕行
  → 对称破除,UAV2 单方面绕行

实现:在 swarmTrajCallback 中,收到 ID < self_id 的邻居轨迹后,提高该邻居在 $J_{\text{swarm}}$ 中的权重——使本机更倾向于让路。

局限性:当三机或更多机形成环形死锁时,ID 优先序可能不够用——这时需要依赖拓扑引导路径的多样性来打破。

D10.3.5 代码路径¶

代码路径:plan_manage/src/ego_replan_fsm.cpp 中的 swarmTrajCallback → 接收邻居轨迹 → bspline_optimizer.cpp 中的 calcSwarmCost → 组装 J_swarm 及其梯度。

关键函数调用链:

ego_replan_fsm.cpp::swarmTrajCallback(msg)
  │
  ├── 解析 BsplineMsg → 存入 swarm_trajs_buf_[drone_id]
  ├── 更新 last_recv_time
  └── 设置 have_recv_pre_agent_[drone_id] = true
      (用于死锁优先级)

ego_replan_fsm.cpp::callReboundReplan()
  │
  ├── 构造优化问题:
  │   J = w_s * J_smooth + w_c * J_collision + w_d * J_dynamic
  │       + w_f * J_feasibility + w_sw * J_swarm
  │
  └── bspline_optimizer.cpp::optimize()
      │
      ├── calcSmoothnessCost()      → ∂J_s/∂Q
      ├── calcDistanceCost()         → ∂J_c/∂Q  (ESDF 避障)
      ├── calcFeasibilityCost()      → ∂J_d/∂Q  (动力学约束)
      └── calcSwarmCost()            → ∂J_sw/∂Q (多机互斥)
          │
          ├── for each neighbor j in swarm_trajs_buf_:
          │   for each control point pair (Q_k^self, Q_k^j):
          │     dist = ‖Q_k^self - Q_k^j‖
          │     if dist < r_safe:
          │       cost += (r_safe - dist)^3
          │       grad += 3*(r_safe - dist)^2 * (Q_k^self - Q_k^j) / dist
          │
          └── 总梯度 → L-BFGS 更新控制点

本节常见陷阱¶

陷阱 1:把惩罚权重 $w_{\text{sw}}$ 越调越大,以为就能"调出安全"。 - 错误描述:仿真出现穿插或贴得过近,直接把 $w_{\text{sw}}$ 从 1000 提到 100000,期望"权重够大就不会撞"。 - 现象/后果:近距离确实变少,但轨迹开始大幅绕行、急刹,甚至在多邻居处合力抵消而卡死(livelock);某些边界情形仍偶发碰撞。 - 根本原因:软惩罚是**经验性**而非形式化安全(见 §D10.3.2 末)。$J_{\text{swarm}}$ 只是与其他代价项(平滑、避障、时间)竞争的一项加权和;"足够大"无法事先量化,且权重过大会把代价景观推得更病态,L-BFGS 更易陷入差解。 - 正确做法:权重用于"通常情况下拉开距离",不是用于"保证"。需要保证时叠加一层硬机制——CBF 安全滤波(§D10.9.3)或切到 MADER 硬约束(§D10.4)。把 $w_{\text{sw}}$ 留在 §E3 给的 $[100, 10000]$ 区间,优先靠 $r_{\text{safe}}$ 留裕度。

陷阱 2:对应控制点配对要求"相同节点向量",却用了不同重规划相位的邻居轨迹。 - 错误描述:直接拿邻居最近一次广播的 B 样条控制点,与本机控制点逐个 $Q_k^i$-$Q_k^j$ 配对算斥力,忽略两条样条的节点向量/起始时刻可能不一致。 - 现象/后果:凸包互斥的"对应时间段"前提被破坏,惩罚作用在错位的时间段上;表现为"明明在斥力,却仍在真实时刻贴近"。 - 根本原因:凸包安全论证(附录 M1)要求两条 B 样条**同节点向量**才能保证"对应控制点凸包分离 ⇒ 对应时刻曲线分离"。异步重规划下相位不一致会让配对失去时间对齐意义。 - 正确做法:统一重规划频率与节点间隔(EGO-Swarm 用 20 Hz + 固定 $\Delta t$ 近似满足),或在配对前按时间戳把邻居轨迹重采样/对齐到本机节点;对齐误差应纳入 $r_{\text{safe}}$ 裕度。

本节练习¶

[思考题] 立方惩罚 $(\cdot)^3$ 在 $d = r_{\text{safe}}$ 处一阶、二阶导都为零。这种"软启动"对避碰有什么好处和坏处?如果换成在边界处梯度不连续的硬铰链(hinge),优化器行为会怎样变化?
[A 型·权重扫描] 在 ego-planner-swarm 的 3 机对飞场景里,固定 $r_{\text{safe}}$,把 $w_{\text{sw}}$ 在 $\{10^2,10^3,10^4,10^5\}$ 上扫描,记录:最小机间距、平均绕行增量、是否出现 livelock。画出"安全 vs 通行效率"的权衡曲线。
[B 型·凸包可视化] 在 RViz 中画出某架机相邻 $p+1$ 个控制点的凸包,与邻居对应凸包并排显示,直观确认"控制点凸包分离 ⇒ 曲线分离"是**充分非必要**(凸包分离时曲线一定分离,但曲线分离时凸包未必分离)。

§D10.4 MADER——MINVO 基与检查-复核协议 ⭐⭐⭐⭐¶

GitHub: mit-acl/mader, ~450★, BSD。

动机:为什么需要"硬约束 + 协议"两件套¶

§D10.3 的 EGO-Swarm 用软惩罚换来了毫秒级重规划,代价是没有形式化安全保证。但有些场景——载人区域上空、贵重载荷、对飞实验——碰撞不可接受,"经验性安全"不够。这时核心问题变成两个层层递进的难题:① 如何把"两条连续轨迹不相交"写成求解器能处理的硬约束?② 在每架机异步、不同频率、各自重规划的去中心化系统里,如何保证"我以为安全"和"实际安全"一致?

MADER 的回答正好是两件套。对问题①:用**分离超平面**把"两段轨迹不碰"表达为线性不等式约束——但这要求"曲线在某一侧"能由"控制点在某一侧"保证,于是凸包越紧约束越不保守,这就是为什么 MADER 先要造 MINVO 基(最紧凸包)。对问题②:即便每架机各自解出无碰轨迹,异步发布时仍可能"我规划时你还没动,等我提交时你已挪到我新轨迹上"——MADER 用 **check-recheck 协议**把"已提交轨迹集合"维护成一个归纳不变量来堵住它。所以本节是"几何引擎(MINVO)+ 并发协议(check-recheck)"的合体,缺一不可。

反面:如果只有硬约束、却没有 check-recheck 协议会怎样? 设想每架机都用分离超平面解出"与当前已知邻居轨迹都分离"的新轨迹,然后立即广播提交。问题在于"当前已知"是个**过时快照**:A 在 $t_0$ 读到邻居轨迹、$t_1$ 解完、$t_2$ 提交,而 B 可能在 $t_0$ 到 $t_2$ 之间提交了一条新轨迹——A 的解相对这条新轨迹并不安全。结果是"每架机各自都满足硬约束,合起来却撞了"。这说明**硬约束只保证"相对我看到的世界安全",并不保证"相对真实世界安全"**;后者需要一个并发控制协议在提交前再确认一次世界没变——这正是 check-recheck(§D10.4.4)存在的理由,也是它与数据库乐观并发控制(OCC)结构同构的根源。

类比(带边界):分离超平面 ≈ 法庭里的"隔离带",check-recheck ≈ 银行转账的"两阶段确认"。 - 像的地方:分离超平面像在两段轨迹之间插一块无限大的隔板,法向 $\mathbf{n}$ 指明"你在这边、我在那边",只要双方的凸包都不越过隔板,就物理上不可能碰——这是确定性的几何隔离,不是"大概率分开"。check-recheck 像转账时"先冻结、再确认余额没变、最后扣款":提交前的 recheck 就是那次"确认期间账户没被别的交易动过"。 - 不像的地方:法庭隔离带是静止的,而分离超平面要随两段轨迹的相对运动**每对、每段重新求解**($\mathbf{n}, d$ 是优化变量,不是常数);银行转账可以加锁串行化、失败重试无代价,而无人机"重试"意味着这一周期没出新轨迹、得继续飞旧轨迹,高速下'保留旧轨迹'本身可能不够安全——所以别把 OCC 的"回滚零成本"直觉照搬过来,MADER 的"reject 则保旧轨迹"之所以安全,前提是旧轨迹本就在不变量里(见归纳证明),这一点比数据库回滚更微妙。

D10.4.1 MINVO 基——为什么凸包大小至关重要¶

MINVO 基:在所有凸包覆盖 n 次多项式曲线的基中,MINVO 给出**最小体积包络单纯形**: - 3 次多项式:比 Bernstein 缩小 2.36×,比 B 样条缩小 254.9× - 7 次多项式:比 B 样条缩小 ~3×10²¹×

直觉理解:

考虑一条 3D 中的 3 次多项式曲线段 $\mathbf{p}(t) = \sum_{k=0}^{3} c_k \phi_k(t)$,其中 $\phi_k$ 是某组基函数。在不同基下:

Bernstein 基:
  凸包 = 4 个 Bernstein 控制点的凸包
  ┌─────────────────────┐
  │    ╱‾‾╲              │  ← 凸包大,曲线只占一小部分
  │   ╱ ╭─╮╲             │
  │  ╱  │曲│ ╲            │
  │ ╱   │线│  ╲           │
  │╱    ╰─╯   ╲          │
  └─────────────────────┘

B 样条基:
  凸包 = 4 个 B 样条控制点的凸包
  ┌────────────────────────────┐
  │                            │  ← 凸包更大(B 样条控制点
  │      ╭──╮                  │     可以离曲线很远)
  │      │曲│                  │
  │      │线│                  │
  │      ╰──╯                  │
  └────────────────────────────┘

MINVO 基:
  凸包 = 4 个 MINVO 控制点的凸包
  ┌──────────┐
  │  ╭──╮    │  ← 凸包最小,紧贴曲线
  │  │曲│    │
  │  │线│    │
  │  ╰──╯    │
  └──────────┘

物理含义:更紧的凸包 → 更少的虚假碰撞报警 → 代理可以更近地飞过 → 33.9% 更短飞行时间、88.8% 更少保守停顿。

D10.4.2 基变换的数学¶

任何基都可以通过线性变换相互转换。设 Bernstein 基下的控制点为 $\mathbf{b}_k$,MINVO 基下的控制点为 $\mathbf{m}_k$,则:

\[ \begin{bmatrix} \mathbf{m}_0 \\ \mathbf{m}_1 \\ \vdots \\ \mathbf{m}_n \end{bmatrix} = M_{n}^{-1} B_n \begin{bmatrix} \mathbf{b}_0 \\ \mathbf{b}_1 \\ \vdots \\ \mathbf{b}_n \end{bmatrix} \]

其中 $B_n$ 和 $M_n$ 分别是 Bernstein 基和 MINVO 基的基矩阵。$M_n$ 的构造来自一个**非凸优化问题**:

\[ M_n^* = \arg\min_{M} \text{Vol}(\text{ConvexHull}(\{M^{-1}B_n \mathbf{b}_k\}_{k=0}^{n})) \]

这个问题由 Tordesillas & How 在 MADER 论文中通过数值优化求解,并将结果硬编码为常数矩阵。

D10.4.3 分离超平面决策变量¶

分离超平面决策变量:每对轨迹段之间引入分离超平面系数 (n, d) 作为优化变量:

n^T · MINVO_hull(seg_i) ≤ d      (我的轨迹段在平面一侧)
n^T · MINVO_hull(seg_j) ≥ d + ε  (邻居的轨迹段在另一侧)

→ 硬可行性约束,不是惩罚。求解器:Gurobi MIQP(混合整数用于 Octopus 搜索)。

为什么是"硬约束"而非"惩罚"?

在 EGO-Swarm 中,$J_{\text{swarm}}$ 是一个加权惩罚项——权重 $w_{\text{sw}}$ 需要手调,太小则不安全,太大则过于保守。而 MADER 直接将分离条件作为优化的**不等式约束**:

\[ \text{minimize} \quad J(\tau_i) \quad \text{subject to} \quad \mathbf{n}_{ij}^T \mathbf{m}_k^i \leq d_{ij} \quad \forall k, \quad \mathbf{n}_{ij}^T \mathbf{m}_l^j \geq d_{ij} + \epsilon \quad \forall l \]

如果优化器找不到满足所有分离约束的解,它会返回"不可行"而不是返回一个"有点危险但代价还行"的解——这是安全保证的基础。

D10.4.4 检查-复核协议详解¶

检查-复核协议的归纳不变量:

定义: CommittedSet = 所有代理当前已提交的轨迹集合
不变量: CommittedSet 中任意两条轨迹在时空中不相交

归纳步骤:
  代理 i 想发布新轨迹 τ_i^new:
  1. [Check] 读取当前 CommittedSet \ {τ_i^old}
  2. 求解 NLP:τ_i^new 与 CommittedSet 中所有轨迹分离
  3. [Recheck] 检查 Check 和 Recheck 之间是否有新的 committed 轨迹到达
     - 若无 → commit τ_i^new(不变量保持)
     - 若有 → 放弃 τ_i^new,保留 τ_i^old(不变量保持)

关键性质:即使代理异步运行(不同频率、不同延迟),只要每个代理执行 check-recheck,CommittedSet 永远无碰。

状态机详解:

                          邻居有新 commit
                     ┌──────────────────────┐
                     ▼                      │
    ┌──────┐    ┌────────┐    ┌────────────┐│   ┌───────────┐
    │ IDLE │───►│ CHECK  │───►│ OPTIMIZING ││──►│ RECHECK   │
    │      │    │        │    │            ││   │           │
    └──────┘    └────────┘    └────────────┘│   └─────┬─────┘
        ▲                                  │         │
        │                                  │    ┌────┴────┐
        │                                  │    │         │
        │      ┌──────────┐                │    ▼         ▼
        │      │ REJECTED │◄───────────────┘  无冲突    有冲突
        │      │(保留旧轨迹)│                   │         │
        │      └─────┬────┘                    ▼         │
        │            │                  ┌──────────┐     │
        └────────────┘                  │COMMITTED │     │
        └──────────────────────────────►│(广播新轨迹)│◄────┘
                                        └──────────┘   (不可能:
                                                        recheck发现
                                                        冲突则reject)

归纳证明要点:

基础情况:初始时,所有代理的初始轨迹(悬停点)互不碰撞,CommittedSet 满足不变量
归纳假设:在某时刻,CommittedSet 满足不变量
归纳步骤:代理 $i$ 执行 check-recheck:
若 recheck 通过:$\tau_i^{\text{new}}$ 与 check 时刻的 CommittedSet 分离(由优化保证);check 到 recheck 期间无新 commit(由 recheck 检查保证) → $\tau_i^{\text{new}}$ 与**当前** CommittedSet 分离 → 不变量保持
若 recheck 失败:保留 $\tau_i^{\text{old}}$(已在 CommittedSet 中且安全) → 不变量保持

与乐观并发控制(OCC)的类比:

概念	数据库 OCC	MADER check-recheck
读阶段	读取相关行的快照	Check:读取邻居 committed 轨迹
计算阶段	在本地计算新值	求解 NLP:在本地计算新轨迹
验证阶段	检查快照是否仍有效	Recheck:检查是否有新 commit
提交/回滚	验证通过 → 提交;失败 → 回滚	无冲突 → commit;有冲突 → reject
不变量	可串行化	CommittedSet 无碰

D10.4.5 Octopus 搜索——前端路径多样性¶

MADER 的前端使用 "Octopus" 搜索:在多智能体环境中生成多条拓扑不同的路径候选(如"从邻居上方绕过"vs"从下方绕过"vs"从左侧绕过"),然后对每条候选分别运行后端优化,选择代价最小的可行解。

这解决了单一前端路径导致的"永远选择同侧绕行→死锁"问题。

本节常见陷阱¶

陷阱 1:把 check-recheck 当成"加锁串行化",以为它消除了所有并发。 - 错误描述:认为只要做了 check-recheck,多机就像数据库加了全局锁一样被"串起来"执行,任何时刻只有一架机在改轨迹。 - 现象/后果:据此假设系统吞吐被严重低估,或反过来误以为"协议保证了全局最优时序";真机上发现多架机其实在并发提交,行为与"串行化"想象不符。 - 根本原因:check-recheck 是**乐观**并发控制(OCC),不是悲观锁。它允许所有机并发地 check-optimize,只在提交瞬间验证"我 check 后世界没变";变了就 reject(回滚到旧轨迹),没变才 commit。它保证的是**不变量**(CommittedSet 无碰),不是"串行执行"。 - 正确做法:用 OCC 的心智模型(§D10.4.4 对照表)理解它——并发执行、提交时验证、冲突回滚。性能分析按"冲突率"而非"锁等待"来估;高冲突(密集对飞)时 reject 增多、有效重规划率下降,这才是真正的瓶颈。

陷阱 2:误以为 MINVO 的"最紧凸包"提供了更强的安全性。 - 错误描述:看到"MINVO 比 B 样条凸包小 254.9×",以为凸包越小越安全。 - 现象/后果:在选型论证里把 MINVO 列为"更安全",混淆了"保守度"与"安全性"两个正交维度。 - 根本原因:凸包大小决定的是**保守度**(虚假碰撞报警的多少),不是安全性。无论凸包多大,"控制点凸包分离 ⇒ 曲线分离"都成立(都安全);凸包越紧只是越**少误报**——让无人机敢飞得更近(33.9% 更短飞行时间),而不是"更不会撞"。安全性来自分离超平面是**硬约束** + check-recheck 协议,与凸包紧致度无关。 - 正确做法:把"紧凸包"理解为**降低保守度、提升通行效率**的手段;安全性归因于硬约束 + 协议。两者解耦后,选型时才能正确权衡"效率(MINVO)"与"保证(check-recheck)"。

本节练习¶

[思考题] MADER 的 check-recheck 与数据库 OCC 结构同构(§D10.4.4)。OCC 在"高冲突负载"下性能急剧下降(频繁回滚)。把这个结论迁移到 MADER:什么样的多机场景对应"高冲突"?此时 MADER 的有效重规划率会怎样?这是否解释了它"~20 机上限"?
[B 型·去掉 recheck 的反例构造] 在 mader.cpp 中注释掉 recheck(),构造一个两机并发提交场景(可人为加发布延迟),记录碰撞率从 0% 上升;再恢复 recheck,确认回到 0%。把时间线画成 §D10.5.1 那样的 check/commit 序列图。
[计算题] 3 次多项式在 3D 中,MINVO 凸包比 B 样条小 254.9×。若某算法用凸包体积作为"虚假碰撞"代理指标,在同样的障碍密度下,从 B 样条换到 MINVO,虚假碰撞报警期望下降多少倍?这对"敢飞多近"意味着什么(给出定性结论)?

§D10.5 RMADER——通信延迟下的递归可行性 ⭐⭐⭐⭐¶

GitHub: mit-acl/rmader, ~114★。

D10.5.1 MADER 在延迟下的失败模式¶

MADER 的 check-recheck 假设:在 recheck 时刻,所有邻居在 check 和 recheck 之间发布的 commit 都已到达本机。如果通信有延迟,这个假设不成立:

时间线:
  t=0: Agent A 执行 check,读取 CommittedSet
  t=1: Agent B commit 了新轨迹 τ_B^new(但消息在路上)
  t=2: Agent A 执行 recheck——但 τ_B^new 还没到达!
       → A 的 recheck 看到"没有新 commit" → A 也 commit 了 τ_A^new
  t=3: τ_B^new 到达 A——但 A 已经 commit 了!
       → τ_A^new 和 τ_B^new 可能碰撞!

D10.5.2 RMADER 的三重防御机制¶

RMADER 假设通信延迟上界 δ_max 已知。额外机制: - 延迟检查:recheck 阶段不仅检查"新到达的轨迹",还检查"δ_max 窗口内可能延迟到达的轨迹" - 两步发布:先广播 candidate(非 binding),等待 δ_max 后再 commit - 时间戳轨迹缓存:保留 δ_max 窗口内的所有收到轨迹

两步发布协议详解:

Agent i 的规划流程:

1. [Check] t = t_check
   读取 CommittedSet ∪ CandidateSet(包括候选轨迹)

2. [Optimize]
   求解 NLP:τ_i^new 与所有读取的轨迹分离

3. [Publish Candidate] t = t_candidate
   广播:CANDIDATE(τ_i^new, t_candidate)
   含义:"我**打算**执行这条轨迹,但还没正式承诺"

4. [Wait] 等待 δ_max 时间
   在此期间接收可能延迟到达的邻居 commit/candidate

5. [Delay Check] t = t_candidate + δ_max
   检查:从 t_check 到现在,是否有新的 committed 轨迹
   与 τ_i^new 冲突?
   (关键:即使某条轨迹在 t_check 之前就 committed,
    但因为延迟在 t_check 之后才到达,也会被检查到)

6. [Commit / Reject]
   若无冲突 → 广播:COMMITTED(τ_i^new)
   若有冲突 → 保留 τ_i^old,回到步骤 1

D10.5.3 递归可行性证明¶

形式化证明:若初始 CommittedSet 无碰,则在任何 ≤δ_max 的延迟下,CommittedSet 永远无碰。

证明要点(简化版):

引理 1(延迟窗口覆盖):若通信延迟 $\leq \delta_{\max}$,则任何在时刻 $t$ 之前发布的 commit 消息,在时刻 $t + \delta_{\max}$ 之前必定到达所有代理。

引理 2(候选锁定):代理 $i$ 在发布 candidate 后,不会在 delay check 之前修改或撤回它。

主定理:假设在某时刻 CommittedSet 无碰(归纳假设)。代理 $i$ 尝试 commit $\tau_i^{\text{new}}$:

在 $t_{\text{check}}$ 时刻,读取了 CommittedSet
在 $t_{\text{candidate}}$ 时刻,发布了 candidate
在 $t_{\text{candidate}} + \delta_{\max}$ 时刻,执行 delay check:
由引理 1:所有在 $t_{\text{check}}$ 之前发布的 commit 都已到达 → 它们在 check 阶段已被考虑
对于在 $t_{\text{check}}$ 到 $t_{\text{candidate}} + \delta_{\max}$ 之间发布的 commit:由引理 1,它们在 delay check 时刻已到达 → 在 delay check 中被检查
因此:与 $\tau_i^{\text{new}}$ 可能冲突的**所有** committed 轨迹都被检查了 → 若 delay check 通过,则 $\tau_i^{\text{new}}$ 与所有 committed 轨迹安全 → 不变量保持

D10.5.4 实验验证¶

实验:10 代理,50-300 ms 延迟,RMADER 100% 无碰;最佳异步基线仅 83%。

实验设置	RMADER	MADER	EGO-Swarm (模拟)
0 ms 延迟	100% 无碰	100% 无碰	99.2% 无碰
50 ms 延迟	100% 无碰	96% 无碰	97.5% 无碰
100 ms 延迟	100% 无碰	89% 无碰	93.1% 无碰
200 ms 延迟	100% 无碰	79% 无碰	85.3% 无碰
300 ms 延迟	100% 无碰	68% 无碰	76.8% 无碰

硬件验证: 6 架 UAV + 2 个动态障碍物,mesh 网络通信,平均延迟 49.8 ms(最大 483 ms),最大速度 5.6 m/s,100% 无碰。

代价:RMADER 的规划周期比 MADER 长 $\delta_{\max}$(因为等待步骤);在 $\delta_{\max} = 200$ ms 时,每次规划至少需要 200 ms + 优化时间。这限制了最大飞行速度——代理需要提前更多时间规划,无法像 EGO-Swarm 那样 1 ms 重规划、高速穿越。

本质洞察:RMADER 的 $\delta_{\max}$ 是把"未知的异步"赎买成"已知的延迟"的赎金。 MADER 在延迟下失效的根因不是"延迟大",而是"延迟不可知"——recheck 不知道还有多少消息在路上,于是可能漏检。RMADER 没有消除延迟,而是**假设延迟有已知上界 $\delta_{\max}$,然后用"多等 $\delta_{\max}$ 再提交"把这个上界买断:等满 $\delta_{\max}$ 后,引理 1 保证"所有该到的 commit 都到了",漏检窗口被彻底关闭。这揭示了一个普适的分布式系统真理——**你无法在完全异步且无时间假设的系统里既保安全又保活性(FLP 不可能性的影子);一旦给系统一个时间上界,问题就从"不可能"变成"只是慢一点"。$\delta_{\max}$ 就是这笔交易的价格:你用每周期多花 $\delta_{\max}$ 的时间(从而限制最大飞行速度),换来"若初始无碰则永远无碰"的递归可行性。RMADER 与 MADER 的全部差别,本质就是这一笔"用时间赎买确定性"的交易。

本节常见陷阱¶

陷阱 1:把 $\delta_{\max}$ 设得偏小以求快,赌"延迟通常没那么大"。 - 错误描述:实测平均延迟 50 ms,就把 $\delta_{\max}$ 设成 50 ms 甚至更小,理由是"等太久飞不快"。 - 现象/后果:大部分时候没事,但偶发的 200-480 ms 长尾延迟(WiFi 多径、拥塞)一来,delay check 漏掉那条迟到的 commit,递归可行性的前提被破坏,出现碰撞。 - 根本原因:递归可行性证明的引理 1 要求 $\delta_{\max}$ 是延迟的**上界**,不是均值。延迟分布长尾,均值 50 ms 不代表没有 480 ms 的尾巴;一旦真实延迟 > $\delta_{\max}$,"该到的都到了"这一步就不成立。 - 正确做法:$\delta_{\max}$ 按延迟的**实测上界/高分位(如 P99.9)+ 余量**设定,而非均值(对照硬件实验:平均 49.8 ms 但最大 483 ms,$\delta_{\max}$ 应覆盖 483 ms)。若上界太大拖慢飞行,应改善通信(UWB/mesh)压低尾延迟,而不是调小 $\delta_{\max}$。

陷阱 2:用 RMADER 追求 EGO-Swarm 级别的飞行速度。 - 错误描述:既要 RMADER 的 100% 无碰保证,又期望它像 EGO-Swarm 一样毫秒级重规划、高速贴树穿林。 - 现象/后果:发现规划周期被 $\delta_{\max}$ 拖到几百毫秒,最大安全速度远低于 EGO-Swarm;强行提速则规划跟不上,安全裕度骤降。 - 根本原因:两步发布的"等 $\delta_{\max}$"是安全保证的内在代价(见上方本质洞察),不是实现不够优化。最大飞行速度受"提前规划时间"约束,与 $\delta_{\max}$ 直接挂钩。 - 正确做法:按"没有免费午餐"选型(§D10.13 选型指南)——要 100% 保证就接受较低速度(RMADER/MADER);要高速穿越就接受经验性安全(EGO-Swarm)。不要指望单一方法同时拿满"保证"和"速度"两端。

本节练习¶

[计算题] $\delta_{\max} = 150$ ms,优化求解 80 ms。算一次完整规划周期最短时间(check→optimize→publish candidate→wait $\delta_{\max}$→delay check→commit),以及最大规划频率。若 $v_{\max} = 3$ m/s,一个周期内最多飞多远?这段距离对安全裕度 $d_{\text{safe}}$ 的设定意味着什么?
[思考题] RMADER 假设延迟有已知上界 $\delta_{\max}$。如果通信会**永久断开**(某机彻底失联,延迟 = ∞),递归可行性证明还成立吗?工程上应如何兜底(提示:超时 → 降级行为)?
[B 型·时间线推演] 在纸上画两机时间线,人为让 B 的 commit 延迟 $0.5\delta_{\max}$ 到达,逐步推演 A 的 delay check 是否捕获它;再把延迟改成 $1.5\delta_{\max}$,展示漏检如何发生,从而直观理解"$\delta_{\max}$ 必须是上界"。

§D10.6 Swarm-Formation——图 Laplacian 编队在密集环境中的飞行 ⭐⭐⭐¶

GitHub: ZJU-FAST-Lab/Swarm-Formation, ~513★。

D10.6.1 问题定义¶

传统编队控制(如位移基编队)在开阔空间中有效,但在密集障碍环境中,刚性编队可能无法通过:

期望编队:△(三角形)       障碍:两棵树之间的窄缝
                           │  │
  UAV1                     │  │
   /\                      │  │  ← 间距 < 编队宽度
  /  \                     │  │
UAV2──UAV3                 │  │

Swarm-Formation 的解决方案:允许编队**弹性变形**(缩放、旋转、局部变形)以适应环境,同时保持拓扑相似性。

D10.6.2 归一化 Laplacian 编队代价¶

给定 $N$ 架无人机的当前位置 $\{p_i\}$ 和期望编队 $\{p_i^d\}$:

构造交互图:以 k-近邻或距离阈值连接
计算边权:$w_{ij} = \exp(-\|p_i - p_j\|^2 / (2\sigma^2))$(高斯核)
构造归一化 Laplacian:$\mathcal{L} = D^{-1/2} L D^{-1/2}$
编队相似性代价:$J_{\text{form}} = \|\mathcal{L} - \mathcal{L}^d\|_F^2$

梯度链式法则:

\[ \frac{\partial J_{\text{form}}}{\partial p_i} = \frac{\partial J_{\text{form}}}{\partial \mathcal{L}} \cdot \frac{\partial \mathcal{L}}{\partial w_{ij}} \cdot \frac{\partial w_{ij}}{\partial p_i} \]

每一步都是可微的: - $\partial J_{\text{form}} / \partial \mathcal{L} = 2(\mathcal{L} - \mathcal{L}^d)$ - $\partial \mathcal{L} / \partial w_{ij}$ 涉及 $D^{-1/2}$ 的导数(较复杂但解析可求) - $\partial w_{ij} / \partial p_i = -\frac{w_{ij}}{\sigma^2}(p_i - p_j)$

D10.6.3 MINCO 联合优化¶

将编队代价嵌入 MINCO 优化框架:

\[ \min_{\tau_1, \ldots, \tau_N} \sum_{i=1}^{N} \left[ J_{\text{smooth}}(\tau_i) + J_{\text{obs}}(\tau_i) + J_{\text{dyn}}(\tau_i) \right] + w_f \cdot J_{\text{form}}(\tau_1, \ldots, \tau_N) \]

关键技术细节: - $J_{\text{form}}$ 在多个离散时间点采样评估(如每 0.1 s) - 通过 MINCO 的时间分配变量,编队可以在窄通道前减速、缩小、通过后加速恢复 - 编队权重 $w_f$ 的自适应调整:在开阔区域加大 $w_f$ 以维持精确编队;在窄缝处减小 $w_f$ 以允许变形

D10.6.4 实验结果¶

场景	无人机数	编队类型	环境	特点
柱状障碍林	6	三角形	仿真	编队在障碍间弹性变形后恢复
随机森林	9	方阵	仿真	自动缩放穿过窄缝
竹林	6	线阵	真机	与 EGO-Planner-v2 同平台验证

§D10.7 Primitive-Swarm——千机扩展性 ⭐⭐⭐⭐¶

GitHub: ZJU-FAST-Lab/Primitive-Planner, ~173★。

D10.7.1 为什么基于优化的方法无法扩展¶

EGO-Swarm 类方法的计算瓶颈分析:

每机每周期的计算:
  J_swarm = Σ_{j∈N_nbr} Σ_{k∈ctrl_pts} cost(Q_k^self, Q_k^j)

  时间复杂度:O(N_nbr × N_ctrl × NLP_iter)

  典型值:N_nbr=10, N_ctrl=20, NLP_iter=50
  → 每周期 10,000 次代价/梯度评估

  当 N_nbr=50(100 机场景):
  → 每周期 50,000 次 → 1 ms 变成 5 ms
  → 加上 L-BFGS 收敛变慢(更多冲突约束) → 10-50 ms
  → 100 Hz 规划降为 20-100 Hz → 安全裕度骤降

更根本的问题:随着邻居数增多,代价景观(landscape)变得更加复杂(更多局部极小值),L-BFGS 可能陷入差解或不收敛。

D10.7.2 运动原语库设计¶

核心思想:把在线 NLP 替换为离线原语库选择:

离线阶段:
  用 TOPP-RA 在体速度坐标系中生成 K 个时间最优原语
  每个原语缓存其时空占据足迹(swept volume)

在线阶段(每个代理每个周期):
  for prim in library:
    for nbr in K_nearest_neighbors:
      if prim.swept_volume ∩ nbr.committed_primitive ≠ ∅:
        reject prim
    if not rejected:
      commit prim; break

TOPP-RA 原语生成详解:

TOPP-RA (Time-Optimal Path Parameterization via Reachability Analysis) 在给定几何路径上求解时间最优参数化:

\[ \min_{\dot{s}(s)} \int_0^1 \frac{1}{\dot{s}(s)} ds \quad \text{s.t.} \quad \tau_{\min} \leq M(q)\ddot{q} + C(q,\dot{q})\dot{q} \leq \tau_{\max} \]

对于四旋翼,约束包括: - 最大推力:$\|f\| \leq f_{\max}$ - 最大角速度:$\|\omega\| \leq \omega_{\max}$ - 最大加加速度(jerk)限制

在体坐标系(body frame)中生成原语 → 旋转到当前航向后使用 → 一套原语适用于所有朝向。

原语库参数:

参数	典型值	含义
K(原语数)	100-500	库大小;越大选择越灵活但查询越慢
速度范围	0-5 m/s	覆盖悬停到最高速
转向角范围	-60° 到 +60°	覆盖急转到直飞
原语时长	0.5-2.0 s	规划视界
Swept volume 分辨率	0.2 m 体素	碰撞检测精度

D10.7.3 快速碰撞检测¶

Swept volume 预计算: 每个原语的 swept volume 被预计算为一组 3D 体素(以体坐标系表示):

prim[k].swept_voxels = {(dx, dy, dz) : 原语 k 经过的所有体素偏移}

在线碰撞检测:

def check_collision(my_pos, my_heading, prim_id, neighbor_prims):
    """O(|voxels| × N_nbr) 碰撞检测"""
    my_voxels = transform(prim_library[prim_id].swept_voxels,
                          my_pos, my_heading)
    for nbr in neighbor_prims:
        nbr_voxels = transform(prim_library[nbr.prim_id].swept_voxels,
                               nbr.pos, nbr.heading)
        if my_voxels.intersects(nbr_voxels):  # 哈希集合 O(1) 查询
            return True  # 碰撞!
    return False

复杂度:O(K · N_nbr),线性扫描。对比 EGO-Swarm:O(NLP_iter × N_nbr × N_control_points),超线性。

D10.7.4 扩展性实验¶

结果:仿真 1000 代理;真机 40 架位置互换。

规模	环境	平均规划时间	碰撞率	成功率
50 机	随机障碍	0.3 ms	0%	100%
100 机	随机障碍	0.5 ms	0%	100%
500 机	随机障碍	1.2 ms	0.1%	99.8%
1000 机	随机障碍	2.1 ms	0.3%	99.5%
40 机(真机)	室内	0.4 ms	0%	100%

与其他方法的规模对比:

                  规划时间(ms) vs 代理数
  100 ┤
      │           ╱ EGO-Swarm(NLP)
   50 ┤          ╱
      │         ╱
   10 ┤        ╱
      │       ╱
    5 ┤      ╱      ╱ MADER(QP)
      │     ╱      ╱
    1 ┤────╱──────╱─────────────────── Primitive-Swarm(lookup)
      │   ╱     ╱
  0.1 ┼───┼─────┼───┼───┼───┼───┼───
      10  20   50  100  200  500  1000
                     代理数 N

D10.7.5 原语方法的局限性¶

轨迹质量:离散原语库无法生成任意形状的轨迹;在需要精确编队或精细避障的场景中,原语的"粗粒度"会导致次优路径
库设计依赖先验:原语库需要针对特定动力学模型和运行条件设计;换平台时需要重新生成
无形式化安全保证:虽然碰撞检测是确定性的,但 swept volume 的体素化引入了离散化误差
异构困难:不同型号的无人机需要不同的原语库;异构集群中的碰撞检测更复杂

本质洞察:Primitive-Swarm 的扩展性突破,本质是把"在线求解"换成"离线枚举 + 在线查表"。 EGO-Swarm 类方法每周期都现场解一个 NLP,复杂度被 L-BFGS 迭代次数 × 邻居数 × 控制点数三重相乘,且邻居越多代价景观越病态(更多局部极小)——这是**超线性**的,50 机就开始卡。Primitive-Swarm 看穿了一件事:飞行器在体坐标系下的"可行动作"其实是有限且可预先穷举的。于是它把"求一条最优轨迹"(无限搜索空间、在线)降维成"从 $K$ 条预计算原语里挑一条不撞的"(有限候选、离线已算好 swept volume)。在线只剩 $O(K \cdot N_{\text{nbr}})$ 的线性扫描 + 哈希查表。这是计算机科学里反复出现的同一招——用空间(预计算的原语库)换时间(在线求解),与查找表、记忆化、预编译同源。代价也正是这一招的通病:库是离散的、有限的,所以轨迹质量受限于库的粒度(局限性 1-2),换平台要重造库(局限性 3-4)。"千机"不是算力堆出来的,是这次**范式切换**(优化→枚举)换来的。

本节常见陷阱¶

陷阱 1:把"1000 机"理解为算力问题,以为加 GPU/换更快 CPU 就能让 EGO-Swarm 也上千机。 - 错误描述:看到 Primitive-Swarm 仿真 1000 机,以为只要硬件够强,基于优化的 EGO-Swarm 也能扩到千机。 - 现象/后果:在项目里盲目堆算力,结果 EGO-Swarm 在 ~50-100 机就因 NLP 超线性增长 + 代价景观恶化而卡死,钱花在 CPU 上却没换来扩展性。 - 根本原因:这是**算法范式**问题,不是算力问题。优化方法的瓶颈是 $O(N_{\text{nbr}} \times N_{\text{ctrl}} \times \text{NLP\_iter})$ 的超线性增长 + 邻居增多导致的局部极小(§D10.7.1),换 CPU 只能线性提速,压不住超线性曲线,更治不了"不收敛"。 - 正确做法:扩展性瓶颈先判性质——是常数因子(可堆算力)还是复杂度阶数/收敛性(须换范式)。要千机就切到查表式范式(Primitive-Swarm)或仿生群集(§D10.10.4),用空间换时间;基于优化的方法定位在"几十机、要轨迹质量"的区间。

陷阱 2:无脑增大原语库 $K$ 以追求轨迹质量,忽视在线查询代价。 - 错误描述:嫌原语轨迹"粗",把 $K$ 从 200 加到几千,期望逼近优化方法的平滑度。 - 现象/后果:在线扫描时间 $O(K \cdot N_{\text{nbr}})$ 线性上升,规划频率下降;swept volume 内存占用膨胀;扩展性优势被吃掉。 - 根本原因:$K$ 直接进在线复杂度。原语库的"离散粒度"是范式的固有代价(本质洞察),靠加 $K$ 无限逼近连续是事倍功半——库再大也填不满连续轨迹空间,却线性拖慢每次查询。 - 正确做法:$K$ 取在"够用"区间(§D10.7.2 给的 100-500),靠**体坐标系生成 + 按航向旋转复用**让一套原语覆盖所有朝向,而不是靠堆 $K$;需要精细编队/避障时,叠加"编队偏好评分"挑原语(见 §C 型思考题),或在关键段局部切回优化,而非全程加大 $K$。

本节练习¶

[计算题] $K=200$、每个 swept volume 含 $V=50$ 体素、$N_{\text{nbr}}=8$、单次哈希查表 $t_{\text{hash}}=10$ ns。算一次完整原语选择(遍历所有原语 × 所有邻居 × 所有体素)的理论最坏耗时。与 EGO-Swarm 每周期上万次代价/梯度评估对比,差几个数量级?
[思考题] Primitive-Swarm 用"空间换时间"。还有哪些经典算法用了同一招(提示:查找表、记忆化、预编译、Cooley-Tukey 之外的预存旋转因子)?它们的"换取"和"牺牲"分别是什么?这个权衡在什么场景下不划算?
[A 型·库粒度实验] 在 Primitive-Planner 中分别用 $K=100$ 和 $K=500$ 跑同一密集场景,记录:平均规划时间、轨迹平滑度(jerk 积分)、成功率。验证"$K$ 越大越平滑但越慢",找到你的场景下的甜点。

§D10.8 DMPC 编队控制 ⭐⭐⭐¶

交叉引用: 本节扩展 04/50_多机器人协作/50_分布式MPC多足编队.md 的 DMPC 框架到空中平台。地面多足编队与空中编队共享 DMPC 的数学结构,但空中编队面临更严苛的实时性需求(碰撞后果)和动力学约束(欠驱动)。

D10.8.1 DMPC 基本框架¶

分布式模型预测控制(Distributed Model Predictive Control, DMPC)是一种将大规模集中式 MPC 分解为多个耦合小规模 MPC 的方法。

集中式 MPC(不可扩展):

\[ \min_{u_1, \ldots, u_N} \sum_{i=1}^{N} \sum_{k=0}^{H-1} \left[ \|x_i(k) - x_i^{\text{ref}}(k)\|_Q^2 + \|u_i(k)\|_R^2 \right] + \sum_{i<j} J_{ij}^{\text{couple}} $$ $$ \text{s.t.} \quad x_i(k+1) = f(x_i(k), u_i(k)), \quad u_i \in \mathcal{U}_i, \quad \|x_i(k) - x_j(k)\| \geq d_{\text{safe}} \]

决策变量维度:$N \times H \times n_u$;碰撞约束:$\binom{N}{2} \times H$ 个非凸约束。

DMPC 分解:每机解本地 MPC,将邻居轨迹视为**已知参数**(而非决策变量):

\[ \text{Agent } i: \quad \min_{u_i} \sum_{k=0}^{H-1} \left[ \|x_i(k) - x_i^{\text{ref}}(k)\|_Q^2 + \|u_i(k)\|_R^2 \right] $$ $$ \text{s.t.} \quad x_i(k+1) = f(x_i(k), u_i(k)), \quad u_i \in \mathcal{U}_i, \quad \|x_i(k) - \hat{x}_j(k)\| \geq d_{\text{safe}} \quad \forall j \in \mathcal{N}_i \]

其中 $\hat{x}_j(k)$ 是代理 $j$ 上一轮广播的预测轨迹(假设不变)。

D10.8.2 递归可行性与稳定性¶

递归可行性:如果在时刻 $t$ 找到了可行解,能否保证在时刻 $t+1$ 仍然能找到可行解?

终端代价与终端约束:标准 MPC 稳定性工具——在预测视界末端加入: - 终端代价 $V_f(x_i(H))$:用 LQR 代价近似无穷视界 - 终端约束 $x_i(H) \in \mathcal{X}_f^i$:终端不变集

对空中编队的特殊考虑: - 终端约束通常设为"在编队目标点附近的小球":$\|x_i(H) - x_i^d\| \leq r_f$ - 安全间距约束可以用 SFC (Safe Flight Corridor) 线性化——将非凸碰撞约束转化为一系列线性不等式 - 每个 DMPC 周期后,代理将新的预测轨迹广播给邻居,作为下一轮的 $\hat{x}_j$

D10.8.3 ADMM 加速 DMPC¶

交叉引用: 04/50_多机器人协作/30_共识算法与分布式优化.md §2.5(ADMM)。

当碰撞约束 $\|x_i - x_j\| \geq d_{\text{safe}}$ 不能简单地用"固定邻居轨迹"处理时(例如需要更高的全局最优性),可以用 ADMM 迭代:

\[ \text{引入一致性变量} \quad z_{ij} = x_i, \quad z_{ji} = x_j \]

增广拉格朗日: $$ L_\rho = \sum_i J_i(x_i) + \sum_{(i,j)} \left[ y_{ij}^T(x_i - z_{ij}) + \frac{\rho}{2}|x_i - z_{ij}|^2 \right] $$

ADMM 迭代: 1. x-更新(并行):每机解本地 QP,包含 $J_i + $ 对偶项 2. z-更新(并行):求解碰撞约束投影(将 $z_{ij}, z_{ji}$ 投影到安全距离集合) 3. y-更新(通信):交换对偶变量并更新

收敛性:对凸问题,ADMM 在 $O(1/\epsilon)$ 轮内收敛到 $\epsilon$-最优。对非凸碰撞约束,需要 SFC 线性化后才是凸的。

D10.8.4 DMPC vs 轨迹优化:何时选哪个?¶

维度	DMPC	轨迹优化(EGO-Swarm 类)
反馈性	闭环(每步重优化)	开环(规划一条轨迹后跟踪)
模型精度	需要准确的动力学模型	模型通过代价函数隐式编码
约束处理	显式(输入/状态约束)	惩罚 or SFC 硬约束
频率	50-200 Hz(实时 QP)	5-20 Hz(NLP 较慢)
适用场景	精确编队跟踪、已知环境	未知环境探索、快速穿越
与感知的耦合	弱(假设已知地图)	强(可直接用 ESDF 梯度)

§D10.9 安全间距保证机制 ⭐⭐⭐¶

D10.9.1 ORCA——速度障碍方法¶

背景: ORCA (Optimal Reciprocal Collision Avoidance) 由 van den Berg 等人于 2011 年提出,是多智能体碰撞避免的经典算法。

核心思想:对于两个以速度 $v_A, v_B$ 运动的圆形代理(半径 $r_A, r_B$),定义**速度障碍**(Velocity Obstacle):

\[ VO_{A|B} = \{v : \exists t > 0, \|p_A + v \cdot t - p_B - v_B \cdot t\| \leq r_A + r_B\} \]

即:如果 $A$ 以速度 $v$ 运动,将在某个未来时刻与 $B$ 碰撞。

ORCA 的互惠分担: - 每对代理各承担 50% 的避碰责任 - 将 $VO_{A|B}$ 在速度空间中"截半",得到 ORCA 半平面 - 每个代理在自己的 ORCA 半平面集合的交集中,选择最接近期望速度的速度

求解:$N$ 个代理 → 每个代理求解一个**线性规划**(2D/3D 凸多边形中的最近点):

\[ v_A^* = \arg\min_{v \in \bigcap_{B \neq A} ORCA_{A|B}} \|v - v_A^{\text{pref}}\|^2 \]

复杂度:每个代理 $O(N_{\text{nbr}})$;全局 $O(N \cdot N_{\text{nbr}})$;实测可以在毫秒内处理**数千代理**。

ORCA 在空中集群中的应用与局限:

优势	局限
极快(< 1 ms per agent)	假设单积分器动力学(不考虑加速度约束)
可扩展到千体	不考虑环境障碍(仅处理代理间碰撞)
有碰撞自由保证(在假设下)	四旋翼是欠驱动系统,不能瞬间改变速度
无需通信(仅需知道邻居位置/速度)	不考虑轨迹平滑性和动力学可行性

改进方向: - 3D-ORCA:将 2D 速度障碍扩展到 3D(多一个自由度用于垂直避碰) - 加速度受限 ORCA:在速度选择时加入 $\|a\| \leq a_{\max}$ 约束 - ORCA + 轨迹优化混合:用 ORCA 生成安全速度指令 → 作为轨迹优化的约束 → 同时满足安全性和平滑性

D10.9.2 安全飞行走廊(SFC)¶

交叉引用: 60_MINCO轨迹表示与安全走廊.md 中 SFC 的单机版本。这里讨论多机场景下的 SFC 扩展。

单机 SFC 回顾: - 在自由空间中生成一系列凸多面体(如轴对齐包围盒 AABB 或通用凸多面体) - 轨迹约束在每个时间段落入对应的凸多面体内 - 非凸"避障"约束 → 凸"包含"约束

多机 SFC——Relative Safe Flight Corridor (RSFC):

对于一对代理 $(i, j)$,在**相对坐标系** $\Delta p = p_i - p_j$ 中构造安全走廊:

\[ \Delta p(t) \in \text{RSFC}_{ij}(t) \quad \iff \quad \|\Delta p(t)\| \geq d_{\text{safe}} \]

RSFC 将"不碰撞"约束转化为"相对位置落在某个凸集(安全距离外)"中。

线性化:在当前相对位置 $\Delta p_0$ 处线性化: $$ \mathbf{n}{ij}^T \Delta p(t) \geq d $$}}, \quad \text{where} \quad \mathbf{n}_{ij} = \frac{\Delta p_0}{|\Delta p_0|

这是一个**半平面约束**(与 MADER 的分离超平面类似,但在相对坐标系中表述)。

D10.9.3 控制屏障函数(CBF)¶

Control Barrier Functions (CBF) 提供了一种从控制论角度保证安全的方法:

定义安全集 $\mathcal{C} = \{x : h(x) \geq 0\}$,其中 $h$ 是屏障函数。对于多机系统: $$ h_{ij}(x) = |p_i - p_j|^2 - d_{\text{safe}}^2 $$

CBF 条件(确保安全集正不变): $$ \dot{h}{ij}(x, u) \geq -\alpha(h(x)) $$

其中 $\alpha$ 是一个扩展类 $\mathcal{K}$ 函数(如 $\alpha(s) = \gamma s$)。

QP 形式的安全滤波器:给定名义控制器输出 $u^{\text{nom}}$(如编队控制器),通过 QP 修正为安全控制: $$ u^* = \arg\min_u |u - u^{\text{nom}}|^2 \quad \text{s.t.} \quad \dot{h}{ij}(x, u) \geq -\alpha(h_i $$}(x)) \quad \forall j \in \mathcal{N

CBF 在空中集群中的优势: - 控制频率(~100-1000 Hz),远高于规划频率(~10-100 Hz)→ 可作为"最后一道防线" - 与任何上层规划器兼容:EGO-Swarm 规划 + CBF 安全滤波 = 软惩罚 + 硬安全 - 可以形式化证明安全性(在模型准确的前提下)

CBF 的局限: - 需要准确的动力学模型和状态估计 - 对 MIMO 系统(多输入)的高阶 CBF 设计复杂 - CBF 约束可能导致"安全但卡住"(safety vs liveness)

D10.9.4 Buffered Voronoi Cell (BVC)¶

BVC 是 Crazyswarm 中使用的轻量级避碰方法:

计算 Voronoi 划分:每个代理分到距其最近的空间区域
向内收缩(buffer):将 Voronoi cell 边界向内收缩 $r_{\text{safe}}/2$
约束:每个代理的下一步位置必须落在其 Buffered Voronoi Cell 内

原始 Voronoi:        Buffered Voronoi:
┌─────┬─────┐        ┌─────┬─────┐
│     │     │        │ ┌──┐│┌──┐ │
│  A  │  B  │   →    │ │A ││ │B│ │
│     │     │        │ └──┘│└──┘ │
└─────┴─────┘        └─────┴─────┘
                     (边界内缩 r_safe/2)

性质:如果每个代理都待在自己的 BVC 内,则任意两个代理之间的距离 $\geq r_{\text{safe}}$(因为两个 BVC 之间的间隙 $\geq 2 \times r_{\text{safe}}/2 = r_{\text{safe}}$)。

在 Crazyflie 上的实现:BVC 计算运行在 STM32 上(< 1 ms);仅需知道邻居位置(通过 mocap 广播)。

D10.9.5 安全机制对比总表¶

方法	安全保证	计算复杂度	动力学考虑	环境障碍	通信需求
ORCA	有(单积分器)	O(N_nbr) LP	无(单积分器)	无	仅位置/速度
SFC/RSFC	有(凸约束)	QP	有(轨迹约束)	有	轨迹参数
CBF	有(模型准确时)	QP(~100 Hz)	有(全动力学)	可扩展	位置/速度
BVC	有(几何)	Voronoi O(N log N)	无(位置约束)	无	仅位置
MADER 分离超平面	有(check-recheck)	MIQP	有(MINVO 包络)	有	轨迹+协议
EGO-Swarm 软惩罚	无(经验性)	L-BFGS	有(B 样条)	有(ESDF)	轨迹参数

本质洞察:ORCA/SFC/CBF/BVC 四者的区别,是"在哪个空间里画安全边界"。 表面上四种方法风格迥异,但都在做同一件事——把"不碰撞"这个非凸约束,转化为某个空间里的凸约束,差别只在选了哪个空间:ORCA 在**速度空间**画半平面(选下一步速度);SFC/RSFC 在**位置(或相对位置)空间**画凸多面体(轨迹落在走廊内);CBF 在**控制空间**画线性约束(修正控制量使安全集正不变);BVC 在**位置空间**用 Voronoi 划分(下一步落在缓冲单元内)。一旦看穿这点,选型就有了主轴:你的决策变量是什么、约束最自然落在哪个空间,就用哪个方法——规划层出轨迹用 SFC,控制层滤控制量用 CBF,只知邻居位置/速度用 ORCA,要几何确定且超轻量用 BVC。它们不是竞争关系而是**可叠层**:EGO-Swarm 规划(位置空间软惩罚)+ CBF 滤波(控制空间硬约束)= 软惩罚的灵活 + 硬保证的底线,这正是工程中"最后一道防线"的来源。

本节常见陷阱¶

陷阱 1:把 ORCA 的"千体无碰保证"直接用到四旋翼上。 - 错误描述:看到 ORCA "毫秒级处理数千代理、有碰撞自由保证",直接拿来当四旋翼集群的避碰层,期望同样的保证。 - 现象/后果:仿真(质点)漂亮,真机四旋翼却出现"指令速度突变跟不上"、贴近甚至碰撞——尤其高速急转时。 - 根本原因:ORCA 的保证建立在**单积分器**假设($\dot p = v$,速度可瞬间改变)上。四旋翼是欠驱动二阶系统($\ddot p = u$,$u$ 受推力约束),不能瞬间改速度;ORCA 选出的"安全速度"可能根本不可达,保证随假设一起失效。 - 正确做法:用"加速度受限 ORCA"(把下一步速度限制在当前速度 + 最大加速度的可达球内,见 §C 型思考题),或 ORCA 出速度指令 → 喂给轨迹优化/CBF 做动力学可行化;切勿把质点模型的保证当作四旋翼的保证。

陷阱 2:以为加了 CBF 安全滤波就"绝对安全",忽视模型/状态误差与活性问题。 - 错误描述:在名义控制器后接一层 CBF-QP,就宣称系统"形式化安全、不可能撞"。 - 现象/后果:状态估计有漂移或动力学模型不准时仍会越界;或 CBF 约束太紧导致无人机"安全但卡住"(僵在原地,任务做不下去)。 - 根本原因:CBF 的安全性是"模型准确 + 状态准确"前提下的形式化保证,前提破了保证就破;且 CBF 只管 safety 不管 liveness——过保守的 $\alpha$ 或冲突的多约束会让 QP 把无人机锁死(safety vs liveness 矛盾)。 - 正确做法:把 CBF 当"最后一道防线"而非唯一防线——配合可靠状态估计、在 $h_{ij}$ 里预留模型/估计误差裕度;调 $\alpha$(扩展类 $\mathcal K$ 函数)平衡保守度与活性,必要时引入高阶 CBF 或与规划层协同解死锁。

本节练习¶

[思考题] 四方法各在"哪个空间画凸约束"(速度/位置/控制/位置-Voronoi)?给定三个任务——(a) 已知地图的精确编队跟踪、(b) 只有邻居位置/速度的开阔空域避让、(c) STM32 上的超轻量纳米机避碰——分别选哪个,为什么?
[B 型·ORCA 加速度约束扩展] 用 RVO2 跑 100 代理圆周对穿,再手动加一层"可达速度球"约束(下一步速度 ∈ 当前速度 + $a_{\max}\Delta t$ 球 ∩ ORCA 半平面交集),对比加约束前后的碰撞率与轨迹平滑度。
[A 型·EGO-Swarm + CBF 叠层] 在 EGO-Swarm 输出轨迹的跟踪控制器后接一个 CBF-QP 安全滤波器(双积分器近似),人为把 $r_{\text{safe}}$ 调小制造贴近,观察 CBF 如何在控制层"兜底"修正;再把 $\alpha$ 调到很保守,复现"安全但卡住"。

§D10.10 大规模群飞:10-100-1000 机工程实践 ⭐⭐¶

D10.10.1 规模谱系——每个量级的核心瓶颈¶

┌──────────┬──────────────────────────────────┬───────────────────┐
│ 规模     │ 核心瓶颈                          │ 典型方案           │
├──────────┼──────────────────────────────────┼───────────────────┤
│ 2-5 机   │ 算法验证(概念验证阶段)             │ 集中式 MPC / MADER │
│          │ → 瓶颈是"做出来"而非"做快"        │                   │
├──────────┼──────────────────────────────────┼───────────────────┤
│ 5-10 机  │ 状态估计一致性                     │ EGO-Planner-v2    │
│(ZJU 真机) │ VINS-Fusion 漂移 → UWB 矫正      │ + VINS-Fusion+UWB │
│          │ 机间相对定位精度 ~10-30 cm         │                   │
├──────────┼──────────────────────────────────┼───────────────────┤
│ 10-50 机 │ 通信可靠性 + 规划扩展性            │ Primitive-Swarm   │
│(Primitive │ WiFi 广播域饱和;NLP 变慢          │ / 仿生群集         │
│ 真机)    │ → 需要低带宽协议或原语方法          │                   │
├──────────┼──────────────────────────────────┼───────────────────┤
│ 50-100 机│ 电池管理 + 充电轮换               │ 混合架构           │
│          │ 100 × 250g × 4S LiPo = 25 kg电池  │ + 自动充电站       │
│          │ 同时空中最大数量受限于充电轮换      │                   │
├──────────┼──────────────────────────────────┼───────────────────┤
│ 100-1000+│ 地面站 + 空域管理 + 法规           │ Primitive-Swarm   │
│(仿真)    │ 真机验证的后勤保障成本巨大          │ (仿真验证)         │
│          │ → 当前仅有仿真达到此规模            │                   │
└──────────┴──────────────────────────────────┴───────────────────┘

D10.10.2 通信架构设计¶

小规模(< 20 机)——单跳广播:

所有无人机在同一个 WiFi 网络中
每机广播 ROS topic → 所有机接收
带宽:20 Hz × 500 bytes × 20 机 = 200 KB/s(可接受)
延迟:WiFi 单跳 < 10 ms(理想条件)
问题:隐藏节点效应;多径干扰

中规模(20-100 机)——Mesh 网络:

无人机形成自组织 Mesh 网络
每机仅与通信范围内的邻居通信
消息通过多跳转发到达远处代理
协议:802.11s / 自定义 TDMA / UWB
带宽:每机 ~10 KB/s(仅邻居信息)
延迟:每跳 ~5-20 ms;3 跳 = 15-60 ms

大规模(100+ 机)——分层架构:

层 1:全局(地面站→子集群首领,~1 Hz,任务级指令)
层 2:子集群(首领→成员,~10 Hz,编队参数)
层 3:邻居(成员↔成员,~20 Hz,轨迹参数)

D10.10.3 定位精度与安全裕度¶

安全间距 $d_{\text{safe}}$ 的设定必须考虑定位误差:

\[ d_{\text{safe}} = d_{\text{physical}} + 2\sigma_{\text{pos}} \times k_{\sigma} + v_{\text{max}} \times \Delta t_{\text{react}} \]

其中: - $d_{\text{physical}}$:物理碰撞距离(机架对角线) - $\sigma_{\text{pos}}$:定位标准差 - $k_{\sigma}$:安全系数(通常 3 → 99.7% 置信) - $v_{\text{max}}$:最大速度 - $\Delta t_{\text{react}}$:反应时间(感知→规划→执行延迟)

典型数值:

定位方式	$\sigma_{\text{pos}}$	$d_{\text{physical}}$ (250mm 机架)	$d_{\text{safe}}$ (3σ, 3m/s, 100ms)
Mocap (Vicon)	1 mm	0.35 m	0.36 m
RTK-GPS	2 cm	0.35 m	0.77 m
VINS-Fusion	10 cm	0.35 m	1.25 m
VIO (无矫正)	30 cm	0.35 m	2.45 m

关键洞察:定位精度直接决定了集群的密度上限。VINS-Fusion 的 10 cm 误差 → 安全间距 > 1 m → 10 机在 5×5 m 空间中就很拥挤了。这就是为什么 ZJU 的 Science Robotics 2022 工作使用 UWB 辅助矫正——将漂移控制在可接受范围。

D10.10.4 仿生群集——Reynolds 规则与 Vasarhelyi 模型¶

背景: Craig Reynolds 1987 年提出的 Boids 模型是仿生群集(flocking)的开山之作,用三条简单规则产生逼真的群集行为。

Reynolds 三规则:

Separation(分离):远离过近的邻居 $$f_{\text{sep},i} = -\sum_{j: \|p_j - p_i\| < r_s} \frac{p_j - p_i}{\|p_j - p_i\|^2}$$
Alignment(对齐):速度趋向邻居平均速度 $$f_{\text{ali},i} = \frac{1}{|\mathcal{N}_i|}\sum_{j \in \mathcal{N}_i} v_j - v_i$$
Cohesion(聚集):位置趋向邻居质心 $$f_{\text{coh},i} = \frac{1}{|\mathcal{N}_i|}\sum_{j \in \mathcal{N}_i} p_j - p_i$$

控制输入: $$u_i = w_s f_{\text{sep},i} + w_a f_{\text{ali},i} + w_c f_{\text{coh},i}$$

Vasarhelyi 等(Science Robotics 2018)的增强模型:

在 Reynolds 规则基础上加入: - 速度匹配项(更精确的 alignment,考虑延迟) - 障碍排斥项(环境中的静态障碍物) - 自驱动项(维持目标速度) - 高度分层(减少 3D 中的碰撞风险) - 进化优化参数:6 个权重参数由进化算法在仿真中优化

结果:30 架真机户外群集飞行(无中央控制、无 GPS 以外的全局定位);仿真扩展到 1000 机 @ 115 km/h。

与优化方法的对比:

维度	仿生群集	优化方法(EGO-Swarm 类)
计算量	O(N_nbr),极低	O(NLP),中-高
轨迹质量	无平滑性保证	平滑、动力学可行
安全性	经验性(参数依赖)	经验性(惩罚权重依赖)或硬约束
环境适应	需要额外的障碍排斥项	原生支持 ESDF 避障
编队精度	低(涌现行为难以精确控制)	高(可指定精确编队)
扩展性	极好(~1000+)	中等(~50-1000)
美学效果	自然、有机(像鸟群)	精确、机械(像编队飞行)

§D10.11 多机任务分配与路由 ⭐⭐⭐¶

交叉引用: 本节是 04/50_多机器人协作/40_任务分配与路径规划.md 中 CBBA / TSP / CVRP 在空中多机探索中的应用实例。

D10.11.1 问题建模¶

多机任务分配的核心问题:$N$ 架无人机、$M$ 个目标点(如探索前沿点、覆盖区域、搜索目标),如何将目标分配给无人机,使得总代价(时间/距离/能量)最小?

分类(Gerkey-Matarić): - ST-SR-IA(单任务-单机器人-即时分配):最简单,等价于二分图匹配 → 匈牙利算法 $O(n^3)$ - ST-MR-IA(单任务-多机器人-即时分配):需要协同的任务(如协同搬运) - MT-SR-IA(多任务-单机器人-即时分配):每机依序访问多个目标 → TSP/VRP

在无人机集群中的常见形式:

系统	任务类型	OR 问题	求解器
FUEL/FALCON (ZJU)	单机多前沿探索	ATSP	LKH
RACER (ZJU)	多机协同探索	CVRP(容量 = 电池)	OR-Tools
MUI-TARE	多机区域划分探索	MD-VRP	OR-Tools
搜救	多机目标搜索	Set Cover + TSP	贪婪 + LKH

D10.11.2 CBBA——分布式拍卖分配¶

交叉引用: 04/50_多机器人协作/40_任务分配与路径规划.md §3.2 对 CBBA 有完整推导。这里聚焦空中集群的特殊考量。

CBBA (Consensus-Based Bundle Algorithm) 的两阶段:

Bundle 构建(本地):每机贪心地将任务加入自己的任务包,按边际收益递减(DMG)排序
共识消解(通信):邻居间交换任务包,用共识协议解决冲突(同一任务被多机竞标)

空中集群中的 CBBA 适配: - 代价函数:不仅是距离,还包括高度变化(垂直飞行能耗更大)、风场影响、电池余量 - 时间窗口约束:某些任务有时间限制(如"10 分钟内到达目标区域") - 异构能力:不同无人机携带不同传感器 → 任务-代理兼容性矩阵 - 通信拓扑:CBBA 的收敛轮数 $\propto$ 通信图直径;稀疏图 → 更多轮次 → 但空中集群的通信图通常较密(通信范围 > 编队间距)

D10.11.3 TSP/VRP 求解器在轨迹层的衔接¶

TSP/VRP 求解器输出的是**访问顺序**(离散路由),还需要转化为可执行的**连续轨迹**:

TSP 输出:目标访问顺序 [G3, G1, G7, G5, G2]
                     │
                     ▼
轨迹规划:对每段 (G3→G1, G1→G7, ...) 分别规划
  - 用 A* / RRT 生成引导路径
  - 用 MINCO / B-spline 优化为平滑轨迹
  - 加入避障 + 动力学约束
                     │
                     ▼
执行:跟踪控制器(几何控制器 / MPC)

注意:TSP/VRP 的代价矩阵通常用**欧氏距离**(快速但不精确)或**最短路径距离**(精确但计算量大)。对无人机,垂直距离的代价应适当放大(垂直飞行能耗约为水平飞行的 1.5-2 倍)。

§D10.12 EGO-Planner-v2 与 Science Robotics 2022 深度解读 ⭐⭐¶

这一节深入分析 Zhou 等人 "Swarm of Micro Flying Robots in the Wild" (Science Robotics, 2022) 的完整系统,因为它是当前**自主无人机群飞的里程碑工作**。

D10.12.1 系统全景¶

┌──────────────────────────────────────────────────────────────┐
│                     单机硬件                                  │
│  ┌─────────┐  ┌──────────┐  ┌──────────┐  ┌──────────────┐ │
│  │Intel    │  │Intel     │  │UWB       │  │STM32 飞控    │ │
│  │RealSense│  │NUC i7   │  │Nooploop  │  │(PX4)        │ │
│  │D435i    │  │(计算平台)│  │DWM1000   │  │              │ │
│  └────┬────┘  └────┬─────┘  └────┬─────┘  └──────┬───────┘ │
│       │ 深度图     │ 处理       │ 测距          │ 控制     │
│       ▼           ▼            ▼              ▲            │
│  ┌───────────────────────────────────┐       │            │
│  │        机载软件栈                   │       │            │
│  │                                   │       │            │
│  │  VINS-Fusion(双目+IMU)            │       │            │
│  │       + UWB 松耦合矫正             │       │            │
│  │            │                      │       │            │
│  │            ▼ 位姿估计               │       │            │
│  │  占据栅格 + ESDF                   │       │            │
│  │            │                      │       │            │
│  │            ▼ 环境表示               │       │            │
│  │  前端:A* + 拓扑路径搜索            │       │            │
│  │            │                      │       │            │
│  │            ▼ 引导路径               │       │            │
│  │  后端:MINCO 轨迹优化              │       │            │
│  │    + 邻居广播轨迹互斥代价          │       │            │
│  │            │                      │       │            │
│  │            ▼ 优化轨迹       ───────┘       │            │
│  │  几何控制器 → PX4 指令             │                    │
│  └───────────────────────────────────┘                    │
│                                                            │
│  WiFi 广播:MINCO 参数(去中心化)                            │
└──────────────────────────────────────────────────────────────┘

D10.12.2 VINS-Fusion + UWB 松耦合¶

为什么不能只用 VIO?

VIO (Visual-Inertial Odometry) 是相对定位——长时间飞行会累积漂移。在单机场景中,漂移的绝对位置误差不影响避障(障碍物也在同一坐标系中)。但在多机场景中:

每架无人机有自己的 VIO 坐标系
即使初始对齐(同一起飞点),几分钟后坐标系可能偏移 1+ 米
如果 A 以为 B 在 (3, 0, 1) 但 B 实际在 (4, 0.5, 1) → 虚假安全

UWB 矫正: - UWB (Ultra-Wideband) 提供机间测距(ranging):$d_{ij} = \|p_i - p_j\| + n$,噪声 $n \sim \mathcal{N}(0, \sigma_{\text{uwb}}^2)$,$\sigma_{\text{uwb}} \approx 10$ cm - 通过机间测距,可以校正 VIO 坐标系之间的相对偏移 - 松耦合:VIO 提供高频(200 Hz)位姿;UWB 提供低频(10 Hz)相对距离约束;用 EKF 融合

D10.12.3 MINCO 去中心化轨迹优化¶

从 EGO-Swarm(B 样条)升级到 MINCO 的优势: - 时间分配优化:MINCO 的时间分配变量 $\{T_i\}$ 可以被优化 → 自动调整速度(窄缝减速、开阔加速) - 更高效的梯度:MINCO 的 $\partial J / \partial c_j$ 和 $\partial J / \partial T_i$ 可以解析计算 - 与 SFC 更好的兼容性:MINCO 段在 SFC 凸多面体内的约束更容易处理

D10.12.4 林地群飞实验¶

10 架四旋翼在竹林中自主群飞: - 无 GPS:纯 VINS-Fusion + UWB - 无 Mocap:户外自然环境 - 无预先建图:在线感知 + 在线规划 - 最大速度:约 2 m/s(受限于 VIO 鲁棒性和安全裕度) - 安全间距:~1.5 m(由定位精度决定) - 规划频率:~5 Hz(MINCO NLP) - 通信:WiFi 广播 MINCO 参数

这是为什么它发在 Science Robotics 而不是普通会议上:它首次在完全自然的户外环境中(非实验室、非开阔场地),实现了无任何外部基础设施辅助的自主多机群飞。

§D10.13 通信模式与安全保证对比 ⭐⭐⭐¶

系统	广播内容	冲突消解	形式保证	重规划耗时	扩展上限
EGO-Swarm	B 样条控制点	软惩罚(凸包)	经验性	~1 ms	~50 机
EGO-Planner-v2	MINCO 参数	软惩罚	经验性	~5 ms	~10 机(真机验证)
MADER	MINVO 系数+时戳	分离超平面硬约束	异步无碰(check-recheck)	10-100 ms	~20 机
RMADER	MINVO+候选/提交	延迟检查 δ_max	递归可行,100% 无碰	更慢	~10 机
Swarm-Formation	MINCO + 图 Laplacian	软惩罚	经验性	~10 ms	~20 机
Primitive-Swarm	原语索引+时戳	集合交集	经验性	O(K·N_nbr)	1000 机(仿真)
ORCA	位置+速度	速度障碍半平面	有(单积分器)	< 0.1 ms	1000+ 机
Vasarhelyi 群集	位置+速度	Reynolds 规则	经验性	< 0.1 ms	1000+ 机
DMPC	预测轨迹	耦合约束 QP	递归可行(凸时)	5-50 ms	~20 机

选型指南:

需要形式化安全保证?
├── 是 → 通信延迟可控?
│   ├── < 50 ms → MADER
│   └── 50-500 ms → RMADER
└── 否 → 多少架?
    ├── < 20 且需要精确编队 → Swarm-Formation / DMPC
    ├── 20-100 → Primitive-Swarm
    └── 100+ → 仿生群集 + ORCA

前沿工作与开放问题¶

mrs_uav_system(CTU MRS, ~584★):研究级可复现多机飞行栈;DARPA SubT 获奖;UVDAR 紫外互定位(无 GNSS/mocap)
Crazyswarm2(~130★, ROS2):Buffered Voronoi Cell 机载避碰 + Nav2/SLAM Toolbox 集成——Crazyflie 集群的现代化
开放问题:千机全自主(SLAM+避障+去中心化)——当前上限约 10 机真机;异构空-地编组的跨视角位置识别;纳米无人机群(<50g)的通信带宽与算力瓶颈

更多前沿方向:

千机全自主群飞的核心挑战¶

挑战	当前状态	可能的突破路径
机载 SLAM 在大群中的一致性	~10 机(ZJU),VIO+UWB	去中心化 SLAM(CFL/分布式后端优化)
机间通信带宽	WiFi 广播域~20 机	UWB mesh / 5G sidelink / 光通信
规划扩展性	~1000 机仿真(Primitive-Swarm)	分层规划(全局稀疏 + 局部密集)
安全保证	~10 机(RMADER)	CBF + 学习(RL 训练安全策略)
电池续航	单机 ~15-25 min	自动充电站 + 轮换策略
法规与空域	受限	Beyond Visual Line of Sight (BVLOS) 法规演进

异构空-地协同¶

空中无人机 + 地面机器人的协同编组面临独特挑战: - 维度不匹配:无人机 3D + 地面机器人 2.5D(带地形) - 速度不匹配:无人机 ~5 m/s vs 地面机器人 ~1 m/s - 通信不对称:无人机有更好的 LoS(视线通信),地面机器人容易被遮挡 - 互补性:无人机提供全局视野 + 快速侦察;地面机器人提供持久力 + 载重能力

交叉引用: 04/50_多机器人协作/70_异构多机协同.md 深入讨论了异构协同的算法框架。

学习驱动的集群控制¶

**多智能体强化学习(MARL)**用于集群:去中心化策略 + 集中式训练(CTDE 范式)
**图神经网络(GNN)**处理变拓扑通信图:每个节点(代理)的策略共享,边(通信)上传递消息
Sim-to-Real:在仿真中训练数千代理的策略,迁移到真机(~10 架)

交叉引用: 04/50_多机器人协作/120_MARL多机运动协调.md 有 MARL 的完整理论框架。

项目精读清单¶

ego-planner-swarm:plan_manage/src/ego_replan_fsm.cpp——swarmTrajCallback:如何接收并存储邻居轨迹
ego-planner-swarm:bspline_opt/src/bspline_optimizer.cpp——calcSwarmCost:凸包互斥代价及其梯度
MADER:mader/src/mader.cpp——check-recheck 协议的实现:找到 check() 和 recheck() 函数
RMADER:rmader/src/rmader.cpp——延迟检查窗口 + 候选/提交两步发布
Swarm-Formation:src/——图 Laplacian 编队代价的 MINCO 可微实现
Primitive-Planner:src/——原语库选择 + swept volume 碰撞检测
Crazyswarm2:crazyflie/scripts/——Python API:如何一行代码控制 10+ 架 Crazyflie 编队飞行

精读建议与路径:

精读顺序(由浅入深)¶

Week 1:
  Day 1-2: EGO-Swarm 仿真 + 代码精读(最熟悉的 B 样条框架)
  Day 3-4: MADER 论文 + check-recheck 状态机手绘
  Day 5:   MADER vs EGO-Swarm 对比总结

Week 2:
  Day 1-2: RMADER 延迟分析 + 递归可行性证明复现
  Day 3:   Swarm-Formation 图 Laplacian 梯度推导
  Day 4:   Primitive-Swarm 原语库 + 扩展性分析
  Day 5:   综合对比 + 选型决策练习

代码精读检查清单¶

EGO-Swarm: - [ ] 找到 swarmTrajCallback 中邻居轨迹的缓存数据结构 - [ ] 理解 calcSwarmCost 中 r_safe 的含义和单位 - [ ] 找到死锁优先级的实现位置 - [ ] 修改 r_safe 为原值的 1.5 倍,观察行为变化 - [ ] 在 RViz 中可视化邻居轨迹的凸包

MADER: - [ ] 找到 check() 和 recheck() 函数 - [ ] 理解 CommittedSet 的数据结构 - [ ] 找到分离超平面 $(n, d)$ 在优化问题中的位置 - [ ] 注释掉 recheck(),观察碰撞率变化 - [ ] 理解 Gurobi 调用的接口和参数

Primitive-Planner: - [ ] 找到原语库的生成代码(离线阶段) - [ ] 理解 swept volume 的体素化表示 - [ ] 找到在线碰撞检测的核心循环 - [ ] 统计 K=100 和 K=500 时的规划时间差异 - [ ] 理解 TOPP-RA 的输入和输出接口

实战练习¶

A 型练习(动手仿真)¶

[A 型·EGO-Swarm 仿真] 编译 ego-planner-swarm,在仿真中运行 3 机编队穿越障碍场。观察 RViz 中的:三条 B 样条轨迹(不同颜色)、安全半径圆柱。手动制造"死锁"场景(两机对飞)——观察 ID 优先序如何破对称
[A 型·MADER 仿真] 编译 MADER(需要 Gurobi 学术许可)。运行 5 机场景。对比有/无 recheck 的碰撞率(在代码中注释掉 recheck 步骤)
[A 型·Crazyswarm2 入门] 安装 Crazyswarm2,在仿真模式下运行 4 机编队。用 Python API 编写一个简单的"位置互换"脚本:4 机分别飞到对角位置。验证内置 Buffered Voronoi Cell 避碰
[A 型·ORCA 2D 仿真] 使用 RVO2 库(C++ 或 Python 绑定),编写一个 100 代理的 2D ORCA 仿真:所有代理从圆周出发飞向对面。可视化速度障碍和选择的速度。测量不同代理密度下的碰撞率和通过时间
[A 型·仿生群集] 实现 Reynolds 三规则(Separation / Alignment / Cohesion)的 2D/3D 仿真。用滑动条调整三个权重,观察:
$w_s$ 过大 → 群体散开
$w_c$ 过大 → 群体坍缩(所有代理聚到一点)
$w_a$ 过大 → 群体保持间距但方向一致
加入一个"目标吸引"项 → 群体迁移
[A 型·DMPC 编队] 实现一个简单的 3 机 DMPC 编队控制器:
动力学:双积分器 $\ddot{p}_i = u_i$,$\|u_i\| \leq a_{\max}$
目标:三角形编队跟踪一条直线轨迹
碰撞约束:$\|p_i - p_j\| \geq d_{\text{safe}}$,用 SFC 线性化
求解器:OSQP(开源 QP)
对比:有/无碰撞约束的编队精度和通过时间

B 型练习(代码精读与分析)¶

[B 型·MADER check-recheck 精读] 精读 mader.cpp 中的 check() 和 recheck() 函数。画出状态机:IDLE → CHECKED → OPTIMIZING → RECHECKED → COMMITTED / REJECTED。标注:每个状态转移的条件和数据依赖
[B 型·MINVO vs Bernstein 凸包] 实现一个 2D 对比:给定同一条 3 次多项式曲线,分别用 MINVO 基和 Bernstein 基计算凸包,可视化并测量面积差异——预期 MINVO 小 ~2.36×
[B 型·归一化 Laplacian 编队代价] 对 Swarm-Formation 的编队代价做数值验证:
给定 5 个点的期望编队(正五边形)
对当前编队施加旋转/平移/缩放,验证 $J_{\text{form}}$ 不变
对当前编队施加形变(拉伸一个点),验证 $J_{\text{form}}$ 增大
计算 $\partial J_{\text{form}} / \partial p_i$,验证梯度方向指向恢复编队
[B 型·RMADER 延迟分析] 在纸上推导:如果 $\delta_{\max} = 200$ ms,规划一条新轨迹至少需要多长时间?画出时间线:check → optimize → publish candidate → wait $\delta_{\max}$ → delay check → commit。计算最大可能的规划频率
[B 型·通信拓扑仿真] 编写一个简单的 2D 共识仿真:
N=20 个代理随机分布
通信半径 $R_{\text{comm}}$ = 2.0
运行 $\dot{x}_i = -\sum_{j \in \mathcal{N}_i}(x_i - x_j)$
绘制 $\|x - \bar{x}\mathbf{1}\|$ vs $t$ 曲线
改变 $R_{\text{comm}}$,观察 $\lambda_2$ 和收敛速率的变化
加入通信延迟 $\tau$,找到使系统发散的临界 $\tau^*$

C 型练习(思考题与论文联系)¶

[思考题] MADER 的 check-recheck 协议和数据库系统中的"乐观并发控制(OCC)"有什么结构性相似?两者都在"提交前检查冲突"——如果冲突则回滚。SLAM 中有类似的乐观并发模式吗?
[思考题] 为什么 Intel/Nova Sky Stories 的千机灯光秀**不算**自主群飞?提示:预编排 GPS 航点、无机间通信、无实时避障、无机载 SLAM——与 ZJU 林地群飞在技术维度上完全不同
[思考题] EGO-Swarm 的软惩罚方法在什么条件下会失败?设计一个场景:3 架无人机在狭窄通道中,使得软惩罚无法同时满足所有安全约束。提示:当惩罚梯度从多个邻居"拉向不同方向"且通道太窄时
[思考题] ORCA 假设单积分器动力学($\dot{p} = v$),但四旋翼是欠驱动的二阶系统($\ddot{p} = u$,且 $u$ 受推力约束)。如何将 ORCA 扩展到考虑加速度约束?提示:在速度空间中加入"可达速度集"约束——下一步的速度不仅要在 ORCA 半平面交集中,还要在当前速度 + 最大加速度的可达球内
[思考题] Primitive-Swarm 用原语替代在线优化,牺牲了什么、换取了什么?如果需要在原语方法中加入编队保持能力(类似 Swarm-Formation),你会怎么设计?提示:可以在原语库中加入"编队偏好评分",选择原语时不仅看碰撞,还看与期望编队位置的距离
[思考题] 在一个 100 机集群中,如果有 5% 的无人机突然失联(电池耗尽或通信故障),对集中式、分布式、混合式架构分别有什么影响?哪种架构的降级最优雅?
[思考题] 仿生群集(Reynolds 规则)和基于优化的方法(EGO-Swarm)分别对应人工智能中的哪种范式?提示:前者是"涌现行为"(简单规则→复杂行为),后者是"最优控制"(显式目标→最优决策)。在什么条件下涌现行为优于显式优化?
[思考题] DMPC 的递归可行性依赖于终端代价和终端约束的正确设计。在空中编队中,如果某架无人机的电池即将耗尽(需要紧急着陆),终端约束应该如何修改?这会影响其他无人机的递归可行性吗?
[思考题] 考虑以下场景:10 架无人机需要在一个 50m × 50m 的区域内执行覆盖搜索任务。环境中有随机分布的树木(密度约 0.1 棵/m²)。设计一个完整的系统架构:选择集中/分布/混合架构、选择轨迹规划方法、选择安全保证机制、选择通信协议。为你的每个选择提供理由。

数学附录¶

M1. B 样条凸包性质与多机安全的关系¶

定理(B 样条凸包):对于 $p$ 阶均匀 B 样条曲线 $\mathbf{c}(t) = \sum_k Q_k N_{k,p}(t)$,在任意节点区间 $[t_k, t_{k+1})$ 内: $$ \mathbf{c}(t) \in \text{Conv}({Q_{k-p}, Q_{k-p+1}, \ldots, Q_k}) $$

推论(多机安全):如果对所有对应时间区间 $[t_k, t_{k+1})$,代理 $i$ 和 $j$ 的局部控制点凸包之间的最小距离 $\geq d_{\text{safe}}$: $$ \min_{\mathbf{a} \in \text{Conv}i, \mathbf{b} \in \text{Conv}_j} |\mathbf{a} - \mathbf{b}| \geq d $$ 则曲线之间的距离也 }$\geq d_{\text{safe}}$: $$ |\mathbf{c}i(t) - \mathbf{c}_j(t)| \geq d \quad \forall t $$}

注意:上述"对应时间区间"要求两条 B 样条具有**相同的节点向量**——EGO-Swarm 通过统一的重规划频率(20 Hz)和固定的节点间隔来近似满足这一条件。

M2. MINVO 体积最优性证明概要¶

对于 $n$ 次多项式 $p(t) = \sum_{k=0}^{n} c_k \phi_k(t)$,在基 $\{\phi_k\}$ 下的控制点凸包体积为: $$ V(\phi) = \text{Vol}(\text{Conv}({c_k}_{k=0}^{n})) $$

MINVO 基 $\{\phi_k^*\}$ 满足: $$ V(\phi^*) = \min_{\phi} V(\phi) \quad \text{s.t.} \quad \mathbf{p}(t) \in \text{Conv}({c_k}) \quad \forall t \in [0, 1] $$

即:在所有满足凸包包含性质的基中,MINVO 基给出的凸包体积最小。

证明思路(Tordesillas & How): 1. 将问题参数化为基变换矩阵 $M$ 的选择 2. 凸包包含性质等价于 $M$ 的行非负和为 1 的约束 3. 体积目标是 $\det(M)$ 的函数(非凸) 4. 使用数值全局优化(多起点+局部优化)求解

M3. ORCA 半平面推导¶

对于代理 $A$(位置 $p_A$,速度 $v_A$,半径 $r_A$)和代理 $B$($p_B, v_B, r_B$):

相对位置:$\Delta p = p_B - p_A$
相对速度:$\Delta v = v_A - v_B$
碰撞锥:$\text{CC} = \{v : \exists t > 0, \|\Delta p + v \cdot t\| \leq r_A + r_B\}$
截断碰撞锥(时间视界 $\tau$):$\text{CC}_\tau = \{v : \exists t \in (0, \tau], \|\Delta p + v \cdot t\| \leq r_A + r_B\}$
ORCA 半平面:将 $\Delta v$ 投影到 $\text{CC}_\tau$ 的边界,取投影方向的一半作为约束:

\[ \text{ORCA}_{A|B} = \left\{v : \left(v - \left(v_A + \frac{1}{2}\mathbf{u}\right)\right) \cdot \hat{\mathbf{n}} \geq 0 \right\} \]

其中 $\mathbf{u}$ 是从 $\Delta v$ 到 $\partial \text{CC}_\tau$ 的最短向量,$\hat{\mathbf{n}}$ 是 $\mathbf{u}$ 的单位方向。

M4. CBF 安全滤波器的 QP 形式¶

对于多机系统,安全集: $$ \mathcal{C} = \bigcap_{i < j} {x : h_{ij}(x) \geq 0}, \quad h_{ij}(x) = |p_i - p_j|^2 - d_{\text{safe}}^2 $$

CBF 条件(每对代理): $$ \frac{\partial h_{ij}}{\partial x} f(x, u) \geq -\gamma h_{ij}(x) $$

展开(双积分器 $\ddot{p}_i = u_i$): $$ 2(p_i - p_j)^T(v_i - v_j) + 2(v_i - v_j)^T(u_i - u_j) \geq -\gamma(|p_i - p_j|^2 - d_{\text{safe}}^2) $$

这是关于 $u_i$ 的**线性约束** → 可以嵌入 QP 安全滤波器。

M5. Laplacian 谱与编队收敛速率¶

对于 $N$ 个代理在连通无向图 $\mathcal{G}$ 上运行位移基编队协议: $$ \dot{e}i = -\sum(e_i - e_j), \quad e_i = p_i - p_i^d $$}_i

向量形式:$\dot{e} = -(L \otimes I_3) e$

收敛速率: $$ |e(t)| \leq e^{-\lambda_2 t} |e(0)| $$

编队误差以指数速率 $\lambda_2$ 收敛。$\lambda_2$ 越大,编队形成越快。

优化通信图以最大化 $\lambda_2$(Fiedler 值优化): - 给定 $N$ 个节点和最多 $M$ 条边的预算 - 求解:$\max_{\mathcal{E}, |\mathcal{E}| \leq M} \lambda_2(L(\mathcal{E}))$ - 这是一个组合优化问题,可以用半正定规划(SDP)松弛近似求解

工程实践指南¶

E1. 从零搭建 3 机集群的步骤清单¶

Phase 1:单机验证(1-2 周)
  □ 搭建硬件:飞控(PX4) + 计算平台(Jetson/NUC) + 深度相机 + UWB
  □ 单机飞行测试:手动/半自主模式
  □ VIO 状态估计验证:VINS-Fusion 或 ORB-SLAM3
  □ 单机自主避障验证:EGO-Planner 单机版
  □ 单机 ESDF 建图验证

Phase 2:通信基础设施(1 周)
  □ WiFi AP 搭建 + 带宽/延迟测试
  □ ROS 多机通信配置(ROS_MASTER_URI / DDS)
  □ 邻居轨迹广播/接收测试(无飞行,纯通信)
  □ UWB 机间测距校准

Phase 3:多机仿真(1 周)
  □ EGO-Swarm 仿真:3 机穿越障碍
  □ 参数调优:r_safe, 惩罚权重, 规划频率
  □ 死锁场景测试
  □ 丢包鲁棒性测试(在仿真中人为丢弃消息)

Phase 4:真机多机飞行(1-2 周)
  □ 先在 mocap 环境中验证(精确定位 → 排除定位误差的影响)
  □ 低速(0.5 m/s)3 机编队飞行
  □ 逐步提速 → 1 m/s → 2 m/s
  □ 切换到 VIO + UWB → 验证户外可行性
  □ 故障恢复测试:飞行中关掉一架,观察其余两架行为

E2. 故障诊断手册(症状 → 可能原因 → 排查步骤 → 相关章节)¶

这是本章的结构化排查表。遇到现象时**从"症状"列定位**,按"排查步骤"逐项排除,"相关章节"给出该故障背后的原理出处——排查不只是改参数,更要理解为什么。覆盖碰撞、卡死、编队失稳、扩展性、延迟、通信六大类共 9 个高频场景。

#	症状	可能原因(按概率排序)	排查步骤	相关章节
1	两机碰撞(软惩罚路线)	① $r_{\text{safe}}$ 太小;② 惩罚权重 $w_{\text{sw}}$ 太低;③ 定位漂移使"我以为的邻居位置"≠ 真实位置	a. 核对 $r_{\text{safe}} = d_{\text{phys}} + 3\sigma_{\text{pos}} + v_{\max}\Delta t_{\text{react}}$;b. 查 VIO 漂移(对飞几分钟看坐标系偏移);c. 适度增大 $w_{\text{sw}}$ 但别指望"调出安全",必要时叠 CBF	§D10.3.2、§D10.10.3、§D10.9.3
2	两机碰撞(延迟相关,硬约束路线)	① 延迟 > $\delta_{\max}$(长尾);② $\delta_{\max}$ 按均值而非上界设;③ recheck/delay-check 被误关	a. 抓包测延迟 P99.9 与最大值;b. 把 $\delta_{\max}$ 提到覆盖最大延迟;c. 确认 recheck 未被注释	§D10.5.1、§D10.5.3、§D10.4.4
3	一机悬停不动	① 收不到邻居轨迹 → 安全定时器触发;② 该邻居被标 lost	a. `rostopic hz` 查广播是否到达;b. 查 WiFi 信号/丢包;c. 核对 `NEIGHBOR_TIMEOUT` 是否过短误触发	§D10.3.3、§D10.2.5
4	编队散开/收不拢	① 共识收敛太慢($\lambda_2$ 太小);② 通信拓扑暂时断开;③ 用了普通 Laplacian 对绝对位置敏感	a. 算当前图 $\lambda_2$;b. 查是否满足"联合连通"(可断开但窗口内并集连通);c. 改用归一化 Laplacian 取得旋转/平移/尺度不变	§D10.2.2、§D10.2.3、§D10.6.2
5	编队抖动/低频振荡(真机有、仿真无)	① 通信延迟 $\tau > \tau^* = \pi/(2\lambda_N)$;② 共识增益/图密度过大抬高了 $\lambda_N$	a. 测端到端延迟反推 $\lambda_N$ 容许上界;b. 降增益或稀疏化图;c. 换 UWB 压低 $\tau$	§D10.2.4、§D10.2.6
6	轨迹振荡(两机反复互相让路)	① 延迟 + 软惩罚导致让路决策来回切换;② 死锁破对称失效	a. 增大 ID 优先级权重让低 ID 维持原路;b. 引入拓扑路径多样性;c. 安全优先场景切 MADER	§D10.3.4、§D10.4.5
7	规划频率随机数增大而下降	① 邻居数 $N_{\text{nbr}}$ 增多 → NLP 超线性变慢;② 代价景观恶化致 L-BFGS 不收敛	a. 限制最大邻居数(只取最近 5-10 个);b. 确认瓶颈是阶数而非常数因子;c. 50+ 机切 Primitive-Swarm 查表范式	§D10.7.1、§D10.7.2
8	全部紧急悬停	① WiFi AP 中断 → 所有安全定时器同时触发;② 广播域饱和(机数超 WiFi 容量)	a. 查 WiFi AP/信道拥塞;b. 增加冗余通道(UWB mesh);c. 20+ 机改 Mesh、100+ 机改分层通信	§D10.10.2、§D10.3.3
9	CBF/安全层介入后"安全但卡住"	① $\alpha$(类 $\mathcal K$ 函数)过保守;② 多 CBF 约束冲突;③ 状态估计误差吃掉裕度	a. 放宽 $\alpha$ 平衡 safety/liveness;b. 检查约束是否互相打架;c. 在 $h_{ij}$ 留模型/估计误差裕度	§D10.9.3、§D10.10.3

E3. 性能调优清单¶

□ r_safe:设为物理碰撞距离 + 3σ_pos + v_max × Δt_react
  → 太小:碰撞风险;太大:通行能力下降

□ 规划频率:EGO-Swarm ~10-20 Hz;MADER ~2-5 Hz
  → 太低:响应慢;太高:计算饱和

□ 邻居数上限:通常 5-10 个最近邻居
  → 太少:可能遗漏即将碰撞的邻居;太多:计算量爆炸

□ 通信频率:轨迹广播 ~20 Hz
  → 太低:邻居信息过时;太高:带宽饱和

□ 安全定时器超时:通常 0.3-1.0 s
  → 太短:频繁误触发悬停;太长:故障响应慢

□ 惩罚权重(EGO-Swarm):w_sw ∈ [100, 10000]
  → 太小:安全裕度不足;太大:轨迹质量下降(过于保守)

术语表¶

缩写/术语	全称	含义
ORCA	Optimal Reciprocal Collision Avoidance	速度障碍方法,通过半平面约束实现互惠避碰
SFC	Safe Flight Corridor	安全飞行走廊,自由空间的凸多面体序列
RSFC	Relative Safe Flight Corridor	相对安全飞行走廊,在相对坐标系中描述碰撞自由
CBF	Control Barrier Function	控制屏障函数,从控制论角度保证安全集正不变
BVC	Buffered Voronoi Cell	缓冲 Voronoi 单元,轻量级分布式避碰
DMPC	Distributed Model Predictive Control	分布式模型预测控制
ADMM	Alternating Direction Method of Multipliers	交替方向乘子法,分布式优化工具
MINVO	Minimum Volume	最小体积基,凸包最紧的多项式基
TOPP-RA	Time-Optimal Path Parameterization via Reachability Analysis	基于可达性分析的时间最优路径参数化
CBBA	Consensus-Based Bundle Algorithm	基于共识的捆绑算法,分布式任务分配
VRP	Vehicle Routing Problem	车辆路由问题
CVRP	Capacitated VRP	有容量约束的 VRP
TSP/ATSP	(Asymmetric) Traveling Salesman Problem	(非对称)旅行商问题
MAPF	Multi-Agent Path Finding	多智能体路径查找
CTDE	Centralized Training, Decentralized Execution	集中训练、去中心化执行(MARL 范式)
ESDF	Euclidean Signed Distance Field	欧氏符号距离场
UWB	Ultra-Wideband	超宽带通信/测距技术
VIO	Visual-Inertial Odometry	视觉惯性里程计
LoS	Line of Sight	视线(通信中指直视路径)
BVLOS	Beyond Visual Line of Sight	超视距(无人机法规术语)

预计学习时间¶

1.5 周(15-20 小时)

时间分配建议:

模块	建议时间	内容
D10.1-D10.2 架构与通信	2-3 小时	三种架构对比;通信拓扑;时延分析
D10.3 EGO-Swarm	3-4 小时	论文精读 + 代码 + 仿真
D10.4 MADER	3-4 小时	MINVO 基 + check-recheck 协议精读
D10.5 RMADER	2-3 小时	延迟分析 + 递归可行性证明
D10.6 Swarm-Formation	1-2 小时	图 Laplacian 编队代价
D10.7 Primitive-Swarm	1-2 小时	原语库 + 扩展性分析
D10.8-D10.9 DMPC + 安全机制	2-3 小时	DMPC 框架;ORCA/SFC/CBF/BVC 对比
D10.10-D10.11 大规模群飞 + 任务分配	1-2 小时	工程实践;CBBA/VRP
练习	3-5 小时	选做 2-3 个 A 型 + 1-2 个 B 型 + 思考题

知识图谱:本章核心概念关系¶

                        ┌──────────────────────────────────┐
                        │        集群协同规划               │
                        └───────────────┬──────────────────┘
                                        │
              ┌─────────────────────────┼─────────────────────────┐
              │                         │                         │
    ┌─────────▼────────┐    ┌──────────▼──────────┐   ┌─────────▼────────┐
    │    架构选择       │    │    轨迹规划方法       │   │    安全保证机制    │
    │ (D10.1)          │    │                      │   │ (D10.9)          │
    └─────┬────────────┘    └──────────┬───────────┘   └────────┬─────────┘
          │                            │                         │
    ┌─────┼──────┐           ┌─────────┼──────────┐       ┌─────┼──────┐
    │     │      │           │         │          │       │     │      │
 集中式 分布式 混合式     软惩罚    硬约束     原语     ORCA  CBF   BVC
    │     │      │        (EGO)    (MADER)  (Primitive)   │     │      │
    │     │      │           │         │          │       │     │      │
    │     │      │           └─────────┼──────────┘       └─────┼──────┘
    │     │      │                     │                        │
    │     │      │              ┌──────▼───────┐         ┌──────▼───────┐
    │     │      │              │ 通信拓扑      │         │  定位精度     │
    │     │      │              │ (D10.2)      │         │  (D10.10)    │
    │     │      │              └──────────────┘         └──────────────┘
    │     │      │
    │     │      └──── 任务分配 (D10.11) ──── CBBA / TSP / CVRP
    │     │
    │     └──── DMPC (D10.8) ──── 递归可行性 + 终端代价
    │
    └──── 编队控制 ──── 图 Laplacian (D10.6) ──── Swarm-Formation

阅读路径建议:

初学者路径(快速上手):
  D10.1(架构概览) → D10.3(EGO-Swarm 仿真) → D10.10(工程实践)

进阶路径(理论深入):
  D10.2(通信拓扑) → D10.4(MADER) → D10.5(RMADER) → D10.9(安全保证)

研究路径(前沿探索):
  D10.7(Primitive-Swarm) → D10.6(Swarm-Formation) → D10.8(DMPC) → 前沿开放问题

本章与教程其他部分的交叉引用总表¶

本章节	引用目标	关系
D10.1 集群架构	`50_多机器人协作/20_多机系统全景.md`	三大架构的具化
D10.2 通信拓扑	`50_多机器人协作/30_共识算法与分布式优化.md` §2.1, §2.3	Laplacian、时延阈值的直接应用
D10.3 EGO-Swarm	`50_B样条轨迹优化.md` (D4)	B 样条凸包性质
D10.4 MADER	`60_MINCO轨迹表示与安全走廊.md` (D5)	MINCO 轨迹表示基础
D10.6 Swarm-Formation	`50_多机器人协作/30_共识算法与分布式优化.md` §2.4	图 Laplacian 编队控制
D10.6 Swarm-Formation	`50_多机器人协作/50_分布式MPC多足编队.md`	DMPC 编队的数学工具
D10.7 Primitive-Swarm	`80_感知引导规划与自主探索.md` (D7)	RACER 多机探索
D10.8 DMPC	`50_多机器人协作/50_分布式MPC多足编队.md`	地面→空中 DMPC 的推广
D10.9 安全保证	`60_MINCO轨迹表示与安全走廊.md` SFC 部分	单机→多机 SFC 扩展
D10.11 任务分配	`50_多机器人协作/40_任务分配与路径规划.md`	CBBA / TSP / CVRP 应用
前沿:异构协同	`50_多机器人协作/70_异构多机协同.md`	空-地异构编组
前沿:MARL 集群	`50_多机器人协作/120_MARL多机运动协调.md`	学习驱动的集群控制

自测题(学完本章后应能回答)¶

概念题:解释 EGO-Swarm 为什么是"经验性安全"而非"形式化安全"。具体说明"软惩罚"在什么条件下可能失效。
概念题:MADER 的 check-recheck 协议中,如果去掉 recheck 步骤(只保留 check),画出一个两机并发场景使得碰撞发生。
计算题:给定通信图 Laplacian 矩阵: $$L = \begin{bmatrix} 2 & -1 & -1 & 0 \\ -1 & 2 & 0 & -1 \\ -1 & 0 & 2 & -1 \\ 0 & -1 & -1 & 2 \end{bmatrix}$$ (a) 画出对应的通信图; (b) 计算 $\lambda_2$; (c) 如果通信延迟 $\tau = 0.5$ s,系统是否稳定?(提示:计算 $\tau^* = \pi/(2\lambda_N)$)
计算题:在 RMADER 中,如果 $\delta_{\max} = 150$ ms,优化求解时间为 80 ms,计算一次完整规划周期的最短时间。如果最大飞行速度 $v_{\max} = 3$ m/s,在这个规划周期内无人机最多飞行多远?这对安全裕度有什么影响?
设计题:你需要为 30 架四旋翼设计一个协同搜索系统(在 200m × 200m 的区域中搜索地面目标)。选择:(a) 架构(集中/分布/混合);(b) 任务分配算法;(c) 轨迹规划方法;(d) 安全保证机制;(e) 通信协议。为每个选择提供一句话理由。
分析题:Primitive-Swarm 声称可以扩展到 1000 机。分析:如果每机的原语库大小 $K = 200$,每个 swept volume 包含 $V = 50$ 个体素,最近邻居数 $N_{\text{nbr}} = 8$,单次碰撞检测(哈希查表)耗时 $t_{\text{hash}} = 10$ ns,那么一次完整的原语选择(遍历所有原语 × 所有邻居 × 所有体素)的理论最坏时间是多少?
论述题:比较"check-recheck + 分离超平面"(MADER)和"CBF 安全滤波器"两种安全保证机制。它们分别在**规划层**和**控制层**提供安全保证。讨论:能否将两者结合使用?如果可以,描述一个合理的组合架构。

参考文献¶

Zhou, X., et al. "EGO-Swarm: A Fully Autonomous and Decentralized Quadrotor Swarm System in Cluttered Environments." ICRA 2021.
Tordesillas, J. & How, J.P. "MADER: Trajectory Planner in Multiagent and Dynamic Environments." IEEE TRO, 2022.
Zhou, X., et al. "Swarm of Micro Flying Robots in the Wild." Science Robotics, 2022.
Kondo, K., et al. "Robust MADER: Decentralized Multiagent Trajectory Planner Robust to Communication Delay." IEEE RA-L, 2023.
Quan, L., et al. "Robust and Efficient Trajectory Planning for Formation Flight in Dense Environments." IEEE TRO, 2023.
Hou, Y., et al. "Primitive-Swarm: An Ultra-lightweight and Scalable Planner for Large-scale Aerial Swarms." IEEE TRO, 2025.
Preiss, J.A., et al. "Crazyswarm: A Large Nano-Quadcopter Swarm." ICRA 2017.
van den Berg, J., et al. "Reciprocal n-Body Collision Avoidance." ISRR 2011.
Reynolds, C. "Flocks, Herds, and Schools: A Distributed Behavioral Model." SIGGRAPH 1987.
Vasarhelyi, G., et al. "Optimized Flocking of Autonomous Drones in Confined Environments." Science Robotics, 2018.
Olfati-Saber, R. & Murray, R. "Consensus Problems in Networks of Agents with Switching Topology and Time-Delays." IEEE TAC, 2004.
Luis, C.E., et al. "Online Trajectory Generation with Distributed Model Predictive Control for Multi-Robot Motion Planning." IEEE RA-L, 2020.
Hoenig, W., et al. "Multi-Agent Path Finding with Kinematic Constraints." ICAPS 2016.
Choi, H.L., et al. "Consensus-Based Decentralized Auctions for Robust Task Allocation." IEEE TRO, 2009.
Jadbabaie, A., et al. "Coordination of Groups of Mobile Autonomous Agents Using Nearest Neighbor Rules." IEEE TAC, 2003.
Ames, A.D., et al. "Control Barrier Functions: Theory and Applications." ECC 2019.
Park, J., et al. "Online Distributed Trajectory Planning for Quadrotor Swarm with Feasibility Guarantee using Linear Safe Corridor." IEEE RA-L, 2022.
Baca, T., et al. "The MRS UAV System: Pushing the Frontiers of Reproducible Research, Real-world Deployment, and Education with Autonomous Unmanned Aerial Vehicles." JINT, 2021.
Walter, V., et al. "UVDAR System for Visual Relative Localization with Application to Leader-Follower Formations of Multirotor UAVs." IEEE RA-L, 2019.
Augugliaro, F., et al. "The Flight Assembled Architecture Installation: Cooperative Construction with Flying Machines." IEEE Control Systems, 2014.

变更日志¶

日期	版本	变更内容
2025-01	v0.1	175 行骨架:科研脉络 + 核心知识点 + 练习
2026-06	v1.0	扩展至完整教学文档:新增 D10.1 集群架构三种范式对比、D10.2 通信拓扑与一致性协议(含 Laplacian 谱分析与时延阈值)、D10.8 DMPC 编队控制框架、D10.9 安全间距保证机制(ORCA/SFC/CBF/BVC 四方法对比)、D10.10 大规模群飞工程实践(定位精度-安全裕度关系、仿生群集 Reynolds/Vasarhelyi 模型)、D10.11 多机任务分配(CBBA/VRP)、D10.12 Science Robotics 2022 系统深度解读;新增数学附录(M1-M5)、工程实践指南(E1-E3)、知识图谱、交叉引用总表、自测题;与 04/50_多机器人协作全面交叉引用
2026-06	v1.1	规范合规改造:统一全部 §D10.4-§D10.13 标题层级(`## §` + ⭐ 难度标记);为 D10.4(MADER)补"动机→反面→类比";新增 5 处本节常见陷阱(D10.3/D10.4/D10.5/D10.7/D10.9,各含错误描述/现象后果/根本原因/正确做法四要素)及配套练习;新增 4 处本质洞察(>引用块:RMADER 时间赎买、Primitive 空间换时间、安全机制空间划分);E2 故障排查表重构为"症状→可能原因→排查步骤→相关章节"四列、扩至 9 个场景

符号	含义	首见
\(N\)	集群中无人机总数	§D10.1
\(N_{\text{nbr}}\)	通信/碰撞范围内的邻居数	§D10.1
\(\tau_i\)	代理 \(i\) 的轨迹(B 样条/MINCO/原语)	§D10.1
\(Q_k^i\)	代理 \(i\) 第 \(k\) 个 B 样条控制点	§D10.3
\(\mathbf{m}_k\)	MINVO 基控制点	§D10.4
\(d_{\text{safe}}\)	安全间距	§D10.1
\(r_{\text{safe}}\)	控制点对安全半径	§D10.3
\(\mathcal{G}(t), \mathcal{E}(t)\)	通信图与边集(时变)	§D10.2
\(L, \mathcal{L}\)	图 Laplacian / 归一化 Laplacian	§D10.2
\(\lambda_2\)	代数连通性(Fiedler 值)	§D10.2
\(\tau, \tau^*\)	通信延迟 / 临界时延阈值	§D10.2
\(\delta_{\max}\)	RMADER 通信延迟上界	§D10.5
\((\mathbf{n}, d)\)	分离超平面法向与偏移	§D10.4
\(h_{ij}(x)\)	控制屏障函数	§D10.9
\(\hat{\tau}_{-i}\)	代理 \(i\) 收到的邻居轨迹(可能延迟/丢包)	§D10.3

#	症状	可能原因(按概率排序)	排查步骤	相关章节
1	两机碰撞(软惩罚路线)	① \(r_{\text{safe}}\) 太小;② 惩罚权重 \(w_{\text{sw}}\) 太低;③ 定位漂移使"我以为的邻居位置"≠ 真实位置	a. 核对 \(r_{\text{safe}} = d_{\text{phys}} + 3\sigma_{\text{pos}} + v_{\max}\Delta t_{\text{react}}\);b. 查 VIO 漂移(对飞几分钟看坐标系偏移);c. 适度增大 \(w_{\text{sw}}\) 但别指望"调出安全",必要时叠 CBF	§D10.3.2、§D10.10.3、§D10.9.3
2	两机碰撞(延迟相关,硬约束路线)	① 延迟 > \(\delta_{\max}\)(长尾);② \(\delta_{\max}\) 按均值而非上界设;③ recheck/delay-check 被误关	a. 抓包测延迟 P99.9 与最大值;b. 把 \(\delta_{\max}\) 提到覆盖最大延迟;c. 确认 recheck 未被注释	§D10.5.1、§D10.5.3、§D10.4.4
3	一机悬停不动	① 收不到邻居轨迹 → 安全定时器触发;② 该邻居被标 lost	a. `rostopic hz` 查广播是否到达;b. 查 WiFi 信号/丢包;c. 核对 `NEIGHBOR_TIMEOUT` 是否过短误触发	§D10.3.3、§D10.2.5
4	编队散开/收不拢	① 共识收敛太慢(\(\lambda_2\) 太小);② 通信拓扑暂时断开;③ 用了普通 Laplacian 对绝对位置敏感	a. 算当前图 \(\lambda_2\);b. 查是否满足"联合连通"(可断开但窗口内并集连通);c. 改用归一化 Laplacian 取得旋转/平移/尺度不变	§D10.2.2、§D10.2.3、§D10.6.2
5	编队抖动/低频振荡(真机有、仿真无)	① 通信延迟 \(\tau > \tau^* = \pi/(2\lambda_N)\);② 共识增益/图密度过大抬高了 \(\lambda_N\)	a. 测端到端延迟反推 \(\lambda_N\) 容许上界;b. 降增益或稀疏化图;c. 换 UWB 压低 \(\tau\)	§D10.2.4、§D10.2.6
6	轨迹振荡(两机反复互相让路)	① 延迟 + 软惩罚导致让路决策来回切换;② 死锁破对称失效	a. 增大 ID 优先级权重让低 ID 维持原路;b. 引入拓扑路径多样性;c. 安全优先场景切 MADER	§D10.3.4、§D10.4.5
7	规划频率随机数增大而下降	① 邻居数 \(N_{\text{nbr}}\) 增多 → NLP 超线性变慢;② 代价景观恶化致 L-BFGS 不收敛	a. 限制最大邻居数(只取最近 5-10 个);b. 确认瓶颈是阶数而非常数因子;c. 50+ 机切 Primitive-Swarm 查表范式	§D10.7.1、§D10.7.2
8	全部紧急悬停	① WiFi AP 中断 → 所有安全定时器同时触发;② 广播域饱和(机数超 WiFi 容量)	a. 查 WiFi AP/信道拥塞;b. 增加冗余通道(UWB mesh);c. 20+ 机改 Mesh、100+ 机改分层通信	§D10.10.2、§D10.3.3
9	CBF/安全层介入后"安全但卡住"	① \(\alpha\)(类 \(\mathcal K\) 函数)过保守;② 多 CBF 约束冲突;③ 状态估计误差吃掉裕度	a. 放宽 \(\alpha\) 平衡 safety/liveness;b. 检查约束是否互相打架;c. 在 \(h_{ij}\) 留模型/估计误差裕度	§D10.9.3、§D10.10.3