T5 多智能体时空协调:N 个智能体如何在共享时空中互不冲突¶

前置自测¶

开始前,先回答下面 5 个问题。这一章从"单个智能体怎么规划"跨到"N 个智能体怎么互不冲突地一起规划"——是 T1-T4 的自然延伸,也是仓储物流、多无人机、多机器人协作的底层算法。答不出 2 题以上,建议先回 T2/T3 补一补。

T2 的 A* / SIPP 在时空图上搜索单个智能体的路径。如果有 N 个智能体,最朴素的做法是把它们看成一个"联合智能体"(状态是所有 agent 位置的笛卡尔积)再 A*。这样做的状态空间会怎样爆炸?(答不出 → 回 T2)
T2 的 SIPP(Safe Interval Path Planning)用"安全时间区间"压缩时空图。它如何把"某个格子在哪些时刻被占用"编码成区间?(答不出 → 回 T2 §T2.2)
什么是"完备性(completeness)"和"最优性(optimality)"?一个算法完备但不最优,意味着什么?(答不出 → 这一章会反复用到)
T3 的 MINCO / 走廊 QP 怎么把单架无人机的轨迹优化成平滑、安全、动力学可行的?(答不出 → 回 T3)
什么是"去中心化(decentralized)"?为什么大规模多智能体系统倾向去中心化而非中心化统一规划?(没想过 → 这一章会讲)

参考答案要点(先自己答,再对照): 1. 联合智能体的状态空间是单 agent 的 N 次方:若单 agent 有 |V| 个可能位置,N 个 agent 的联合状态有 |V|^N 个——10 个 agent、100 个格子就是 100^10 = 10^20,A* 根本搜不动。这就是为什么需要 CBS/LaCAM 这类"不把问题压成联合智能体"的专门算法。 2. SIPP 不把时间离散成一格一格,而是把"某格子的时间轴"切成若干**安全区间**(没有障碍占用的连续时段)和不安全区间——搜索的状态是"(格子, 安全区间)"而非"(格子, 时刻)",大幅压缩状态数。这正是 CBS 低层搜索的高效底座。 3. 完备 = 有解必能找到(无解能报告无解);最优 = 找到的解代价最小。完备但不最优 = 一定能找到一个可行解,但不保证是最好的(如最短的)——很多大规模方法(PIBT、ORCA)牺牲最优换速度/规模。 4. MINCO 用航点 + 段时间的低维参数化,走廊把安全约束变成凸约束,L-BFGS / QP 优化出平滑轨迹。 T5 的无人机部分(EGO-Swarm/MADER)正是在 T3 单机轨迹优化的基础上,加"多机互斥"约束。 5. 去中心化 = 每个智能体自己做决策(基于局部信息/通信),没有一个中心统一规划所有 agent。大规模系统倾向去中心化,因为中心化的计算量随 agent 数爆炸、且中心是单点故障;去中心化每个 agent 只算自己,可扩展、鲁棒。 ORCA、EGO-Swarm、MADER 都是去中心化的。

本章目标¶

学完本章,你应该能够:

理解 多智能体路径规划(MAPF)的核心难点:为什么不能把 N 个智能体当成一个联合智能体直接搜,以及"在共享时空中找互不冲突路径使总代价最优"这个问题的本质。
掌握 CBS(Conflict-Based Search)的双层搜索:高层在冲突树上搜索约束集合、低层每个 agent 在约束下独立用 A*/SIPP 搜索,以及它为什么最优。
理解 **LaCAM 的惰性后继生成**如何把 MAPF 推到 10000 agents 的规模,以及它和 PIBT(配置生成器)的关系。
掌握 ORCA(最优互惠避碰)的速度空间半平面:每对 agent 各承担一半避碰责任、每 agent 解一个低维 LP,实现 O(n) 去中心化无通信避碰。
理解 EGO-Swarm 的 B-spline 凸包互斥**和 **MADER 的 MINVO 分离超平面——两种无人机时空互斥机制的数学差异与工程取舍。
理解 RMADER 的通信延迟鲁棒性:为什么通信延迟会破坏多机安全、delay-check + 两阶段发布如何在最大延迟下保证递归可行(100% 无碰撞)。
建立 "离散图搜索 → 连续速度空间 → 连续轨迹优化"的算法谱系认知,理解不同方法在最优性、规模、通信、动力学上的取舍。

这一章和 T1-T4 的最大不同:前四章都是**单个智能体**的规划(一辆车、一架无人机怎么走);这一章是**多个智能体**的协调(N 个怎么互不冲突地一起走)。多出来的核心难点只有一个词——冲突(conflict):智能体 A 的最优路径可能正好撞上智能体 B 的最优路径,如何在保证各自尽量优的同时让所有路径互不冲突?围绕这个难点,本章会展示三类截然不同的思路:离散图上的搜索(CBS/LaCAM)、连续速度空间的局部避碰(ORCA)、连续轨迹空间的优化互斥(EGO-Swarm/MADER)。

知识导航¶

小节	主题	难度	一句话
§T5.1	CBS 冲突基搜索	⭐⭐⭐	双层搜索:高层冲突树加约束、低层单 agent A*,最优
§T5.2	LaCAM 规模革命	⭐⭐⭐	惰性后继生成 + PIBT,10000 agents 秒级
§T5.3	ORCA 速度空间避碰	⭐⭐⭐	半平面 + 各担一半责任,O(n) 去中心化无通信
§T5.4	EGO-Swarm 凸包互斥	⭐⭐⭐	B-spline 控制点凸包重叠 → 软惩罚推开
§T5.5	MADER 分离超平面	⭐⭐⭐	MINVO 最小体单纯形 + 分离超平面硬约束 + check-recheck
§T5.6	谱系与选型	⭐⭐	离散→速度→轨迹三类方法的统一视角与取舍

三条阅读线:想做仓储/多机器人调度(离散 MAPF)的,重点 §T5.1-§T5.2;想做多机器人/人群局部避碰的,重点 §T5.3;想做多无人机集群飞行的,重点 §T5.4-§T5.5。但 §T5.6(谱系与选型)建议都读——它把三类方法串成一张图,回答"什么场景用什么"。

前置知识桥接¶

回顾 T2-T3(本章在它们之上做"多智能体"扩展):T2 给了时空图上的搜索(A*/SIPP)——CBS 的低层正是用它给单个 agent 在约束下搜路径;T3 给了连续轨迹优化(MINCO/走廊 QP/OBCA)——EGO-Swarm/MADER 的轨迹后端正是这些单机优化器,只是多加了"机间互斥"约束。所以本章不是从零开始,而是把 T2/T3 的单智能体工具,升级到多智能体协调。

具体对应:CBS 低层的"单 agent 在约束下搜最短路" = T2 的 A*/SIPP + 额外的时空约束;EGO-Swarm 的"每架无人机优化自己的 B-spline 轨迹" = T3 的 B-spline/L-BFGS 轨迹优化 + 机间凸包排斥项;MADER 的"每架无人机的轨迹优化" = T3 的轨迹优化 + MINVO 分离超平面约束。换句话说,T5 = T2/T3 的单机算法 + 多机冲突处理。

前向预告:本章会出现一组多智能体专有名词(MAPF、冲突树、配置、半平面、互惠、分离超平面……),它们背后是同一个朴素问题:N 条路径/轨迹如何互不碰撞。读的时候,始终抓住"冲突怎么定义、怎么消解"这条主线。三类方法消解冲突的方式不同:CBS/LaCAM 用"加约束重新搜"、ORCA 用"在速度空间切一刀"、EGO-Swarm/MADER 用"在轨迹优化里加排斥/分离约束"——本章末尾的谱系图会把它们串起来。

如果跳过本章会怎样¶

跳过 T5,你会在两个地方卡住。 场景一:多个机器人各自规划,结果互相撞或死锁。 你用 T1-T3 学的单机规划,给仓库里 50 台 AGV 各自规划了到货架的最短路——结果它们在窄通道里迎面相遇、互相堵死,或者在交叉口同时进入而碰撞。单机规划只保证"我这条路对我最优",完全不管别人;多个智能体共享空间时,必须有专门的协调机制(CBS/LaCAM 算无冲突路径、ORCA 局部避碰),否则各自最优 = 集体死锁。没有 T5,你做不出能让一群机器人高效协作的系统。 场景二:多架无人机编队飞行,要么撞机要么不敢靠近。 你想让 10 架无人机穿过一片树林——用单机轨迹优化,每架只顾自己避开树木和飞向目标,彼此之间没有避让,密集时必然撞机;或者你保守地给每架留巨大安全球,结果它们不敢靠近、编队松散、过不了窄缝。多机轨迹必须显式建模"机间互斥"(EGO-Swarm 凸包排斥、MADER 分离超平面),才能既密集又安全。没有 T5,你的多机系统要么危险要么低效。这两个场景的共同点:单机规划(T1-T4)解决"一个智能体怎么走得好",但**多个智能体共享时空时,会产生单机规划处理不了的冲突**——这正是 T5 要解决的。跳过它,你会停在"会规划单个智能体但做不出多智能体协作系统"的状态,而仓储、集群、多机协作恰恰是机器人最有价值的应用方向。

预计阅读时间¶

模式	时长	适合
精读	8-12 小时	第一次系统学多智能体协调:逐节读动机→理论→代码,亲手编译运行 demo(CBS 双层搜索、ORCA 半平面 LP 等),做完每节练习。建议分 5-6 次,每次一个方法。
速读	2-3 小时	有规划基础、想建立全局图景:读章首问题定义、每节的"动机"和"理论"核心、§T5.6 谱系与选型,跳过代码细节和陷阱。
速查	20-40 分钟	已学过、回来查特定方法:直接定位到对应 §T5.x,看该方法的核心机制 + "带数字走一遍";或查章末故障排查、API 速查、谱系对照表。

无论哪种模式,§T5.1(CBS)的"理论"部分都建议读——它把"多智能体冲突如何定义和消解"这个全章核心讲得最透,是理解后面所有方法的地基。

本章符号约定¶

记号	含义	首见
\(N\) / agent	智能体数量 / 单个智能体	全章
MAPF	Multi-Agent Path Finding,多智能体路径规划(离散图上)	§T5.1
冲突 (conflict)	两 agent 在同一时刻占同一位置(点冲突)或交换位置(边冲突)	§T5.1
CT	Conflict Tree,冲突树(CBS 高层搜索的树)	§T5.1
约束 (constraint)	\((a_i, v, t)\):禁止 agent \(i\) 在时刻 \(t\) 处于位置 \(v\)	§T5.1
SIC / SOC	Sum of Individual Costs / Sum Of Costs,所有 agent 路径代价之和	§T5.1
配置 (configuration)	所有 agent 位置的联合向量 \(Q=(v_1,\dots,v_N)\)	§T5.2
PIBT	Priority Inheritance with Backtracking,优先级继承+回溯	§T5.2
\(v^{opt}\)	agent 的优化速度(ORCA 中用于推断责任划分)	§T5.3
半平面 (half-plane)	ORCA 中每对 agent 在速度空间施加的线性约束	§T5.3
凸包 (convex hull)	B-spline 控制点张成的、包含轨迹段的凸多面体	§T5.4
MINVO	最小体积外包单纯形的多项式基	§T5.5
分离超平面	\(n^\top p \ge d\):把两 agent 的占据体分到两侧的平面	§T5.5

怎么读这章:§T5.1 先建立 MAPF 和冲突的概念(CBS),§T5.2 看规模突破(LaCAM);§T5.3 转到连续速度空间(ORCA);§T5.4-§T5.5 到连续轨迹空间(无人机集群);§T5.6 把三类方法统一成谱系。建议按顺序读,因为"离散→速度→轨迹"是一条由简到繁、由格子到连续的清晰主线。

§T5.1 CBS:冲突基搜索 ⭐⭐⭐¶

动机:N 个智能体一起规划,难在哪?¶

T1-T4 教的都是单个智能体的规划。现在把场景换成:一个仓库里有 N 台 AGV(自动导引车),每台有自己的起点和目标,要同时为所有车规划路径,既让每台尽量走最短、又让任何两台不在同一时刻占同一个格子(不相撞)。这就是多智能体路径规划(MAPF,Multi-Agent Path Finding)——仓储物流、多机器人协作、游戏 NPC 群体移动的核心问题。难点在哪? 最朴素的想法是:把 N 个 agent 看成一个"联合智能体",它的状态是所有 agent 位置的组合(笛卡尔积),然后对这个联合智能体跑 A*。但这立刻撞上**维度灾难**:若单个 agent 有 |V| 个可能位置,N 个 agent 的联合状态空间有 |V|^N 个——10 个 agent、100 个格子,联合状态就是 100^10 = 10^20,A* 连状态都存不下,更别说搜索。另一个朴素想法是:每个 agent 各自独立 A* 算最短路,不管别人。这快是快,但算出来的路径之间**必然有冲突**(两台车的最短路可能正好在某时刻穿过同一个格子),直接执行就撞车。所以 MAPF 的核心矛盾是:既要避免联合搜索的指数爆炸,又要消解各自独立规划带来的冲突。 CBS(Conflict-Based Search)给出了一个漂亮的答案——它既不联合搜索,也不放任冲突,而是用"按需消解冲突"的双层结构,在两者之间找到了高效又最优的路。

如果不这样做会怎样(反面)¶

把两种朴素做法的失败看清,才懂 CBS 的价值。 联合 A* 的失败:状态空间 |V|^N 指数爆炸,内存和时间都撑不住。即便用各种剪枝,联合搜索的分支因子也是单 agent 的 N 次方(每个 agent 每步有若干选择,联合起来要枚举所有 agent 选择的组合),agent 一多就彻底卡死。这是"把多智能体当一个大智能体"的根本困境。 独立 A* + 事后修补的失败:各自算最短路、发现冲突再"打补丁"(让其中一个等一步、绕一下)。问题是,局部打补丁会引发新冲突(A 让了 B,可能又撞上 C),补丁摞补丁,既不保证能消解所有冲突,更不保证总代价最优,最终常常陷入死锁或得到很差的解。这是"忽视冲突的全局性"的困境。 优先级法的局限(一个更聪明但仍不完美的做法):给 agent 排个优先级,高优先级的先规划、低优先级的把已规划的当障碍躲开。这比事后修补好,但它**不完备**——某些必须靠"高优先级让低优先级"才能解的实例,固定优先级会找不到解(后面 §T5.2 的 PIBT 用"优先级继承"缓解了这点)。 CBS 的突破在于:它**不预先固定任何 agent 的路径或优先级**,而是先让每个 agent 各自走最优,然后**只在真正发生冲突的地方,按需地、最小化地施加约束**,逼着冲突的 agent 之一改道,反复直到无冲突。这种"按需加约束"既避免了联合搜索的爆炸,又通过系统性的搜索保证了最优——下面看它怎么做到。

历史:从联合搜索到冲突基搜索¶

MAPF 的求解思路有清晰的演进。早期(2000 年代)主流是把 MAPF 归约成单智能体问题:要么联合智能体 A*(爆炸),要么用 A* 的变体(如 Standley 的 Operator Decomposition、Independence Detection)缓解爆炸,但本质仍是在联合空间里搜。 2011-2012 年,Sharon 等人提出了一系列"不归约成联合智能体"的新思路,其中影响最大的是 CBS(Conflict-Based Search)——Guni Sharon、Roni Stern、Ariel Felner、Nathan Sturtevant 的《Conflict-based search for optimal multi-agent pathfinding》(先发表于 AAAI 2012,完整版载于 Artificial Intelligence, 2015, 219:40-66,DOI 10.1016/j.artint.2014.11.006,约 1500+ 引用,是 MAPF 领域引用最高的奠基工作之一)。 CBS 的核心洞察是:把 N 个 agent 的耦合,降维成"两两之间的冲突"来处理——大部分时候 agent 之间没有冲突(各走各的最短路就行),只有少数时空点发生冲突,对这些冲突逐个施加约束、重新规划即可。这避免了对"所有 agent 的所有组合"的枚举。此后 CBS 衍生出庞大的家族:ICBS / CBSH(加入冲突优先级和启发式,加速高层搜索);MA-CBS(允许把频繁冲突的 agent 合并成元 agent);有界次优的 ECBS(Barer et al. 2014)和其 SOTA 后继 EECBS(Jiaoyang Li et al. AAAI 2021,用 focal search + explicit estimation 在保证次优界的同时大幅提速)。 CBS 至今仍是"最优 MAPF"的标杆和众多算法的基础,理解它是理解整个 MAPF 领域的入口。

理论:双层搜索——高层冲突树 + 低层单 agent 搜索¶

整体结构:两层搜索。 CBS 的精髓是把 MAPF 拆成两层:

高层(High-Level):在"冲突树"(Constraint Tree, CT)上搜索
   每个 CT 节点 = 一组约束 + 在这组约束下每个 agent 的最优路径 + 总代价
   ┌─ 根节点:无任何约束,每个 agent 各自最优路径(可能有冲突)
   │
   ├─ 检测冲突:扫描所有 agent 路径,找第一个冲突 (a_i, a_j, v, t)
   │            (agent i 和 j 在时刻 t 都在位置 v)
   │
   └─ 分裂(split):产生两个子节点,各加一条约束消解这个冲突
        左子:给 agent i 加约束"t 时刻不能在 v"
        右子:给 agent j 加约束"t 时刻不能在 v"
        (子节点继承父节点的所有约束,再各加一条)

低层(Low-Level):给定一个 agent 的约束集合,用 A*/SIPP 搜它的最优路径
   (把"t 时刻不能在 v"这类约束编码进时空 A* 的状态扩展)

高层:在冲突树上搜索。 高层把每个可能的"约束集合"看成搜索树(CT)的一个节点。根节点没有约束,每个 agent 用低层各自搜最短路——这些路径合起来可能有冲突。检测冲突:扫描所有 agent 的路径,找出第一个冲突,形如 \((a_i, a_j, v, t)\)——agent \(i\) 和 agent \(j\) 在时刻 \(t\) 都占据位置 \(v\)(这叫**点冲突**;还有**边冲突**:两 agent 在 \(t\) 到 \(t{+}1\) 交换了位置,即对穿)。消解冲突:CBS 在这个冲突上**分裂**出两个子节点。一个子节点给 \(a_i\) 加约束"时刻 \(t\) 不能在 \(v\)",另一个给 \(a_j\) 加同样的约束。关键洞察:这个冲突要消解,要么 \(i\) 让开、要么 \(j\) 让开,两个子节点正好覆盖这两种可能——所以分裂不会漏掉任何解,这是 CBS 完备和最优的基础。每个子节点继承父节点的全部约束,再加这一条新约束,然后**只对被加约束的那个 agent** 重新跑低层搜索(其他 agent 的路径不变,省计算)。高层按节点的总代价(SIC,Sum of Individual Costs,所有 agent 路径长度之和)做 best-first 搜索(优先展开当前代价最小的 CT 节点)——这保证第一个找到的"无冲突"节点就是最优解。

低层:带约束的单 agent 搜索。 低层的任务很纯粹:给定一个 agent 和它的一组约束(一串"某时刻不能在某位置"),搜出满足所有约束的最短路径。这就是 T2 的时空 A*(或 SIPP)——把约束编码进状态扩展:扩展到状态 \((v, t)\) 时,若存在约束"\(t\) 时刻不能在 \(v\)",就跳过这个状态。用 SIPP 可以更高效:把每个格子的时间轴按约束切成安全区间,搜索 \((格子, 安全区间)\) 而非 \((格子, 时刻)\),状态数大幅压缩(这正是 T2 §T2.2 讲的)。低层是 CBS 反复调用的子程序——每分裂一个 CT 节点,就调用一次低层给某个 agent 重新规划。

为什么 CBS 是最优的? 直觉是:高层在 CT 上做的是 best-first 搜索,按 SIC 从小到大展开节点;每个冲突的分裂(给 \(i\) 加约束 / 给 \(j\) 加约束)穷尽了消解该冲突的所有方式、不漏解;所以当高层第一次展开到一个"无冲突"的节点时,它的 SIC 一定是所有无冲突解里最小的——即最优解。形式化地,CBS 的 CT 搜索是完备的(有解必找到),且因 best-first + SIC 单调,找到的第一个无冲突解最优。

复杂度。 最坏情况下,CT 可能指数增长:约束的组合是 \(O(b^{n^2 T})\) 量级(\(n\) 个 agent、\(T\) 时间步、\(b\) 分支)——理论上很可怕。但**实践中 CBS 远好于最坏界**,原因是真实场景里**冲突是稀疏的**:大多数 agent 各走各的不冲突,只有少数几个时空点需要消解冲突,CT 实际只展开很少的节点。这种"最坏指数、实际高效"的特性,正是 CBS 在中小规模(几十到上百 agent)MAPF 上实用的原因。当 agent 数上千,CBS 的高层搜索才会真正吃力——那时就需要 §T5.2 的 LaCAM 了。

多视角对照:CBS 的双层结构,从三个熟悉的角度看,各自照亮它的一面。 从"分支定界"看:高层冲突树就是一棵分支定界树——每个冲突的分裂是"分支"(两种消解方式),按 SIC best-first 展开是"定界"(优先探索下界最小的节点),第一个无冲突解就是最优。这个视角告诉你 CBS 为什么最优:它本质是在"约束组合"空间做分支定界,和整数规划的求解套路同源。 从"懒惰求解(lazy)"看:CBS 不预先处理所有 agent 的所有交互,而是先假设大家互不干扰(各自最短路),等冲突真的出现了才处理它。这个视角告诉你 CBS 为什么高效:它把"潜在的 N² 对交互"推迟到"实际发生的少数冲突"才计算——和数据库的惰性求值、编译器的按需计算是同一种智慧(不做用不到的功)。 从"关注点分离"看:低层只懂"单 agent 怎么搜路径"(对多 agent 一无所知),高层只懂"怎么协调冲突"(对路径怎么搜一无所知)。这个视角告诉你 CBS 为什么可扩展:换低层(A*→SIPP)不影响高层,改高层(加冲突优先级 ICBS/CBSH)不影响低层——和软件工程的模块解耦同理。三个视角不矛盾:分支定界解释最优性、惰性求解解释高效性、关注点分离解释可扩展性。把这三个你在别处熟悉的思想套到 CBS 上,它就从"一个 MAPF 专用算法"变成了"几个通用工程思想在多智能体问题上的组合"——这也是为什么懂分支定界、懂惰性求值的工程师理解 CBS 特别快。

代码实现(为什么 → 正确 → 错误 → 对比)¶

Step 1 — 为什么这样写。 完整的 CBS 涉及高层冲突树搜索 + 低层带约束 A*,但核心逻辑可以在一份**可直接编译运行**的代码里讲清。下面这份 demo 实现了 CBS 的双层结构:低层是带时空约束的 A*(把"某时刻不能在某格"编码进状态扩展),高层维护冲突树(检测冲突→分裂出两个子节点各加一条约束→best-first 按 SIC 展开)。场景设计成两个 agent 迎面对穿(目标互换),它们的独立最短路必然在中点冲突——正好演示 CBS 如何检测并消解。只用标准库,g++ -std=c++17 即编。

// 最简但完整的 CBS:2D 网格 MAPF,高层冲突树 + 低层带约束 A*
// 高层检测冲突→分裂加约束,低层在约束下单 agent A*。纯标准库。
#include <vector>
#include <queue>
#include <map>
#include <set>
#include <cstdio>
#include <cmath>
#include <algorithm>

struct Cell { int x, y; bool operator==(const Cell&o)const{return x==o.x&&y==o.y;} };

struct Constraint { int agent, x, y, t; };   // agent a 在时刻 t 不能在 (x,y)

// 低层 A*:给定起点/终点 + 该 agent 的约束,搜时空最短路
std::vector<Cell> lowLevelAStar(Cell start, Cell goal, int agent,
                                const std::vector<Constraint>& cons,
                                int W, int H, int maxT) {
  std::set<std::tuple<int,int,int>> blocked;          // 该 agent 的 (t,x,y) 禁止集
  for (auto& c : cons) if (c.agent == agent) blocked.insert({c.t, c.x, c.y});
  auto h = [&](Cell c){ return std::abs(c.x-goal.x)+std::abs(c.y-goal.y); };
  struct Node { Cell c; int t, g, f; };
  auto cmp=[](const Node&a,const Node&b){return a.f>b.f;};
  std::priority_queue<Node,std::vector<Node>,decltype(cmp)> open(cmp);
  std::map<std::tuple<int,int,int>,int> gbest;
  std::map<std::tuple<int,int,int>,std::tuple<int,int,int>> parent;
  open.push({start,0,0,h(start)}); gbest[{start.x,start.y,0}]=0;
  int dx[5]={0,1,-1,0,0}, dy[5]={0,0,0,1,-1};         // 含原地等待

  while(!open.empty()){
    Node cur=open.top(); open.pop();
    if(cur.c==goal){                                   // 回溯路径
      std::vector<Cell> path; std::tuple<int,int,int> s={cur.c.x,cur.c.y,cur.t};
      while(true){ auto[x,y,t]=s; path.push_back({x,y});
        auto it=parent.find(s); if(it==parent.end())break; s=it->second; }
      std::reverse(path.begin(),path.end()); return path;
    }
    if(cur.t>=maxT) continue;
    for(int d=0;d<5;d++){
      Cell nc{cur.c.x+dx[d], cur.c.y+dy[d]}; int nt=cur.t+1;
      if(nc.x<0||nc.x>=W||nc.y<0||nc.y>=H) continue;
      if(blocked.count({nt,nc.x,nc.y})) continue;      // 违反约束→跳过
      int ng=cur.g+1; auto key=std::make_tuple(nc.x,nc.y,nt);
      if(gbest.count(key)&&gbest[key]<=ng) continue;
      gbest[key]=ng; parent[key]={cur.c.x,cur.c.y,cur.t};
      open.push({nc,nt,ng,ng+h(nc)});
    }
  }
  return {};
}

struct Conflict { int i,j,x,y,t; };
Conflict detectConflict(const std::vector<std::vector<Cell>>& paths){
  int maxLen=0; for(auto&p:paths) maxLen=std::max(maxLen,(int)p.size());
  auto at=[&](int a,int t)->Cell{ auto&p=paths[a]; return t<(int)p.size()?p[t]:p.back(); };
  for(int t=0;t<maxLen;t++)
    for(int i=0;i<(int)paths.size();i++)
      for(int j=i+1;j<(int)paths.size();j++)
        if(at(i,t)==at(j,t)) return {i,j,at(i,t).x,at(i,t).y,t};   // 点冲突
  return {-1,-1,-1,-1,-1};
}

struct CTNode { std::vector<Constraint> cons; std::vector<std::vector<Cell>> paths; int cost; };

int main(){
  int W=5,H=3,maxT=20;
  std::vector<Cell> starts={{0,1},{4,1}}, goals={{4,1},{0,1}};   // 迎面对穿→必冲突
  auto cmp=[](const CTNode&a,const CTNode&b){return a.cost>b.cost;};
  std::priority_queue<CTNode,std::vector<CTNode>,decltype(cmp)> open(cmp);

  CTNode root; root.cons={};                            // 根节点:无约束
  for(int a=0;a<2;a++) root.paths.push_back(lowLevelAStar(starts[a],goals[a],a,root.cons,W,H,maxT));
  root.cost=0; for(auto&p:root.paths) root.cost+=(int)p.size()-1;
  open.push(root);

  int expanded=0;
  while(!open.empty()){
    CTNode node=open.top(); open.pop(); expanded++;
    Conflict cf=detectConflict(node.paths);
    if(cf.t==-1){                                       // 无冲突→最优解
      std::printf("CBS 找到无冲突解! 展开 %d 个 CT 节点, SIC=%d\n",expanded,node.cost);
      for(int a=0;a<2;a++){
        std::printf("  agent %d: ",a);
        for(auto&c:node.paths[a]) std::printf("(%d,%d) ",c.x,c.y);
        std::printf("\n");
      }
      return 0;
    }
    std::printf("展开%d: 冲突 a%d&a%d 在(%d,%d)@t=%d→分裂\n",expanded,cf.i,cf.j,cf.x,cf.y,cf.t);
    for(int who : {cf.i, cf.j}){                         // 对 i、j 各加约束分裂
      CTNode child=node; child.cons.push_back({who,cf.x,cf.y,cf.t});
      auto np=lowLevelAStar(starts[who],goals[who],who,child.cons,W,H,maxT);
      if(np.empty()) continue;
      child.paths[who]=np; child.cost=0;
      for(auto&p:child.paths) child.cost+=(int)p.size()-1;
      open.push(child);
    }
  }
  std::printf("无解\n"); return 0;
}
// 运行输出:
//   展开1: 冲突 a0&a1 在(2,1)@t=2→分裂
//   CBS 找到无冲突解! 展开 2 个 CT 节点, SIC=9
//     agent 0: (0,1) (1,1) (1,1) (2,1) (3,1) (4,1)    ← 在(1,1)等一步让行
//     agent 1: (4,1) (3,1) (2,1) (1,1) (0,1)

Step 2 — 正确要点。 这份 demo 抓住了 CBS 的三个本质。其一,双层分离:低层(lowLevelAStar)只管"单 agent 在约束下搜最短路",高层(main 的循环)只管"检测冲突、分裂加约束、best-first 展开"——两层职责清晰,低层对"多 agent"一无所知,高层对"怎么搜路径"一无所知。其二,约束编码进低层:约束"\(t\) 时刻不能在 \((x,y)\)"通过 blocked 集合编码进 A* 的状态扩展——扩展到被禁的时空点就跳过。这是 CBS 把"全局协调"下放成"单 agent 带约束搜索"的关键。其三,分裂的完备性:检测到冲突 \((i,j,v,t)\) 后,对 \(i\) 和 \(j\) **各加一条约束**分裂出两个子节点——这两个子节点穷尽了"让 \(i\) 改道"和"让 \(j\) 改道"两种消解方式,不漏解;配合 best-first(按 SIC 展开),保证第一个无冲突解最优。运行结果印证:agent 0 在 (1,1) 等一步让 agent 1 先过,SIC=9 是最优。真实 CBS 还要处理边冲突(对穿)、用 SIPP 加速低层、加冲突优先级/启发式(ICBS/CBSH)加速高层,但"双层 + 约束编码 + 分裂消解"的骨架是一致的。

Step 3 — 一个常见的错误写法。 实现 CBS 时,最常见的致命错误是**低层 A* 忘记"原地等待"动作**:

// 反例:低层 A* 的邻居只有上下左右四个移动,没有"原地等待"
int dx[4]={1,-1,0,0}, dy[4]={0,0,1,-1};   // 错:缺少 (0,0) 等待动作!
for(int d=0;d<4;d++){ /* 只扩展移动,不能停 */ }
// 问题:CBS 消解冲突的核心手段之一就是"让一个 agent 等一步"(如上面 agent 0
//       在 (1,1) 停一拍让 agent 1 先过)。如果 agent 不能原地等待,
//       很多冲突就无法消解——明明"等一下"就能解决的,算法却报告无解。
//       结果:CBS 在大量本可解的实例上失败(不完备)。

这个错误把"agent 可以停留等待"这个 MAPF 的基本假设给丢了。 MAPF 里 agent 不仅能移动,还能**停在原地等待**——这是消解冲突最常用的方式。漏掉等待动作,CBS 就失去了"以时间换空间"的能力。

Step 4 — 对比。 正确版的低层 A* 邻居含 (0,0) 等待动作(dx[5]={0,1,-1,0,0}),agent 能停下让行,CBS 可消解"对穿""路口抢占"等需要等待的冲突。错误版只有四个移动方向,agent 不能停,大量需要"等一步"才能解的实例会被误报无解。这个对比的通用要点:在时空搜索里,"等待"和"移动"同等重要——状态扩展必须包含原地等待。无论 CBS 低层、SIPP 还是任何时空规划,漏掉等待动作都会让算法在本可解的实例上失败。

⚠️ 常见陷阱¶

💡 编程陷阱:低层 A* 漏掉"原地等待"动作 - 错误想法:路径规划嘛,agent 上下左右移动就行,扩展四个邻居。 - 现象 / 后果:CBS 在大量本可解的实例上报告无解——明明"让一个 agent 等一步"就能消解的冲突,因为 agent 不能停而无法解决。 - 根本原因:MAPF 里 agent 不仅能移动,还能**停在原地等待**(以时间换空间),这是消解冲突最常用的手段;漏掉等待动作,CBS 就失去了这个能力。 - 正确做法:低层 A* 的状态扩展必须包含"原地等待"(如 dx[5]={0,1,-1,0,0} 含 (0,0));每个时间步,agent 可以移动到相邻格,也可以停在当前格。

⚠️ 概念陷阱:以为 CBS 是把所有 agent 联合起来搜索 - 错误想法:CBS 既然能保证最优,应该是在某种联合空间里穷举所有 agent 的组合。 - 现象 / 后果:理解不了 CBS 为什么能避免 |V|^N 的爆炸、为什么低层是"单 agent"搜索。 - 根本原因:CBS 的核心恰恰是**不联合搜索**——高层只在"冲突约束"的空间里搜(冲突稀疏,这个空间实际很小),低层是独立的单 agent 搜索。它把"N 个 agent 的耦合"降维成"少数几个两两冲突"来处理。 - 正确做法:理解 CBS 是"分而治之 + 按需耦合":默认各 agent 独立(分),只在检测到冲突时才施加约束让它们间接协调(按需耦合)。最优性来自高层 best-first 搜索的完备性,而非联合穷举。

🧠 思维陷阱:以为 CBS 最坏复杂度是指数,所以实际一定慢 - 错误想法:CBS 最坏 \(O(b^{n^2T})\) 指数复杂度,那它一定不实用。 - 现象 / 后果:因为看到吓人的最坏界就放弃 CBS,或反过来以为它能解任意规模(上万 agent)。 - 根本原因:最坏复杂度和实际性能可能差很远——CBS 的实际效率取决于**冲突的稀疏程度**:冲突少(大多数实例),CT 只展开很少节点,很快;冲突极密(罕见的拥堵实例),才接近最坏。所以 CBS 在中小规模(几十到上百 agent、冲突不太密)很实用,但 agent 上千、冲突密集时会吃力。 - 正确做法:理解"最坏指数、实际看冲突密度"。 CBS 适合中小规模最优 MAPF;要上万 agent,用 §T5.2 的 LaCAM(放弃最优换规模)。评估算法别只看最坏界,要看它在你的实际场景(冲突密度)下的表现。

练习¶

[实现] 给上面的 CBS demo 加入**边冲突(对穿)检测**:两 agent 在 \(t\) 到 \(t{+}1\) 交换了位置(agent \(i\) 从 \(u\) 到 \(v\)、agent \(j\) 从 \(v\) 到 \(u\))也是冲突。修改 detectConflict 检测它,并在分裂时加边约束(禁止某 agent 在 \(t\) 走某条边)。
[调试] 把 demo 改成 3 个 agent(如起点 (0,0)(0,1)(0,2)、目标 (4,2)(4,1)(4,0)),运行观察 CT 展开节点数随 agent 增多如何变化。再人为构造一个高冲突场景(所有 agent 挤过一个窄口),观察展开节点数的爆炸——体会"冲突密度决定 CBS 效率"。
[设计] 当前低层用普通时空 A*。请把它换成 SIPP(T2 §T2.2):把每个格子的约束转成"安全区间",搜索 \((格子,安全区间)\)。分析在约束很多时,SIPP 相比时空 A* 能省多少状态。这正是真实 CBS 用 SIPP 做低层的原因。

§T5.2 LaCAM:10000 智能体的规模革命 ⭐⭐⭐¶

动机:CBS 上千 agent 就吃力,怎么办?¶

§T5.1 的 CBS 优雅且最优,但它有个硬伤:规模。 CBS 在几十到上百 agent 时高效,但 agent 一上千、冲突一密集,高层冲突树就会指数膨胀——CT 节点爆炸,内存和时间都撑不住。可现实需求恰恰在大规模:亚马逊的仓库里有**上千甚至上万**台搬运机器人同时跑;大型自动化港口、密集编队,都是成百上千个 agent。这些场景对算法的要求是:几千上万 agent、秒级出解——哪怕解不是最优(次优就行),只要快、能扩展。 CBS 那种"保证最优但规模受限"的路子,在这里走不通。需要一种**为规模而生**的算法:放弃最优性,换取把 agent 数推到上万的能力。 LaCAM(Lazy Constraints Addition search for MAPF)就是这个突破——它用一个巧妙的"惰性"思想,把 MAPF 的求解规模从几百 agent 直接推到了 10000 agent、秒级求解,比 CBS 快上千倍。

如果不这样做会怎样(反面)¶

不用 LaCAM 这类规模化方法,大规模 MAPF 会卡在几个地方。 CBS 高层爆炸:上千 agent 时冲突极多,CT 每检测到一个冲突就分裂两个子节点,冲突一多,树指数膨胀,几秒内就耗尽内存。 CBS 的最优性在这里成了负担——为了保证最优,它必须系统地搜索所有约束组合。 联合搜索更不可能:|V|^N 在上万 agent 时是天文数字,想都别想。 优先级法不完备:固定优先级的方法(高优先级先规划、低优先级避让)快,但**不完备**——很多稠密场景下,固定优先级会找不到解(明明有解,却因为优先级顺序不对而失败)。比如两个 agent 在窄道里需要"低优先级的先退一步让高优先级过",固定优先级做不到这种灵活让步。那么,能不能既有优先级法的速度、又不丢完备性? LaCAM 的答案是:用 PIBT 做"一步配置生成"(快、且通过优先级继承缓解了固定优先级的死板),外面套一层惰性的图搜索(保证完备)。它把"快速生成下一步"和"系统搜索保完备"两件事拆开,前者用 PIBT(毫秒级生成下一配置),后者用惰性的配置空间搜索——既快又完备。这正是 LaCAM 能把规模推到上万的关键设计。

历史:PIBT 与 LaCAM¶

LaCAM 的故事要从它的"引擎"PIBT 讲起。 PIBT(Priority Inheritance with Backtracking,优先级继承+回溯)由 Keisuke Okumura、Manao Machida、Xavier Défago、Yasumasa Tamura 提出(IJCAI 2019,pp.535-542;期刊版 Artificial Intelligence, 2022, 103752,DOI 10.1016/j.artint.2022.103752)。 PIBT 解决的是"每一步,让所有 agent 同时往各自目标挪一格、且不撞"——它给每个 agent 一个优先级,高优先级先选下一格,低优先级避让;若低优先级被逼得无路可走,就触发**优先级继承**(让它临时获得高优先级、反过来请高优先级的那个让一让)和**回溯**(撤销冲突的选择重来)。 PIBT 极快(毫秒级处理上千 agent)、可去中心化,但有个关键局限:它对一般 MAPF 不完备——它保证的是在 biconnected(双连通)图上所有 agent 有限时间到达,但在一般图或有死端的地图上,可能卡住、找不到解。 LaCAM(Keisuke Okumura,Tokyo Institute of Technology,后在 AIST + University of Cambridge;AAAI 2023, 37(10):11655-11662,arXiv 2211.13432)正是为了"补上 PIBT 的完备性"而生。它的核心是 lazy successor generation(惰性后继生成):用 PIBT 快速生成"下一个配置"作为候选,但外面套一层完备的图搜索——一旦 PIBT 卡住,搜索会回溯、换一个后继重试,从而保证完备。 LaCAM 一举把 MAPF 的规模推到惊人的程度:实验里它能解 1491×656 的大网格、10000 个 agent,而且秒级出解。后续 LaCAM*(Okumura,IJCAI 2023,arXiv 2305.03632)加入 anytime 机制,让解的质量随时间收敛到最优;Engineering LaCAM*(Okumura,AAMAS 2024,pp.1501-1509)进一步做实时化、大规模、近最优的工程优化。 LaCAM 系列已成为大规模 MAPF 的事实标准,也是仓储机器人调度竞赛(League of Robot Runners)里的主力方法。

理论:配置空间搜索 + 惰性后继生成 + PIBT¶

核心对象:配置(configuration)。 LaCAM 不像 CBS 那样在"约束"空间搜,而是直接在**配置空间**搜索。一个**配置** \(Q = (v_1, v_2, \dots, v_N)\) 是所有 agent 在某一时刻的位置组成的联合向量。 MAPF 的解,就是一串配置序列 \(Q_{start} \to Q_1 \to Q_2 \to \dots \to Q_{goal}\),使得相邻配置间每个 agent 移动合法、且任何配置内无碰撞。 LaCAM 在这个配置序列上做搜索(类似在"配置图"上找从起始配置到目标配置的路径)。

双层结构。 LaCAM 也是双层,但和 CBS 不同: 高层:在配置空间做搜索(DFS 为主,带回溯),维护一个搜索树,节点是配置。低层:给定当前配置 + 一些约束(指定某些 agent 下一步去哪),用 PIBT 生成一个合法的后继配置(一步内让所有 agent 各挪一格且不撞)。所以 PIBT 在这里的角色是"配置生成器"——给定当前配置,它快速吐出一个下一配置。

关键:惰性后继生成(lazy successor generation)。 这是 LaCAM 的灵魂,也是它名字的由来。朴素的配置空间搜索有个致命问题:一个配置的后继数量是**指数级**的——N 个 agent、每个有 \(b\) 个移动选择,后继配置就有 \(O(b^N)\) 个。若像普通图搜索那样"展开一个节点时枚举它的所有后继",立刻爆炸。 LaCAM 的破解办法是**惰性**:展开一个配置时,不**一次性枚举所有 \(O(b^N)\) 个后继,而是**每次只用 PIBT 生成一个**后继(按需、一个一个地生成)。具体地,LaCAM 给每个高层节点维护一个"约束队列",每次从中取一组约束(指定部分 agent 的下一步),调用 PIBT 在这组约束下生成一个后继配置;如果这个后继不行(走不通),下次再取另一组约束生成另一个后继。这样,分支因子从 \(O(b^N)\) 被压缩到**每次只生成一个——把指数级的后继枚举,变成了"按需逐个生成"。这就是"惰性"的含义:不预先算所有可能,用到才算。

为什么这能保证完备又高效? 高效:因为 PIBT 生成单个后继是毫秒级的,而惰性避免了枚举指数个后继——大多数情况下,PIBT 生成的第一个后继就推进了搜索,根本不需要枚举其他后继。完备:因为外层是带回溯的系统搜索——如果 PIBT 生成的后继走进死胡同,搜索会回溯、让 LaCAM 取下一组约束生成不同的后继,最终穷尽所有可能(理论上)。所以即使 PIBT 本身不完备(会卡住),LaCAM 的回溯机制补上了完备性:PIBT 卡住时,LaCAM 换个后继重试。这正是"PIBT 快但不完备 + 外层搜索保完备"的巧妙结合:用 PIBT 的速度推进常见情况,用搜索的回溯兜底罕见的卡住情况。

规模突破与代价。 靠这套机制,LaCAM 把 MAPF 推到 10000 agent、秒级——比 CBS 快约 1000 倍。代价是**放弃了最优性**:LaCAM(原版)找到的是可行解、不保证最优(它是 sub-optimal 的)。 LaCAM* 用 anytime 机制弥补:先快速出一个可行解,然后在剩余时间里持续改进,解的质量逐步收敛到最优——这样既有 LaCAM 的速度(快速出解),又能在时间允许时逼近最优。所以 LaCAM 系列的定位很清晰:当 CBS 因规模算不动时,用 LaCAM 换规模(牺牲最优);若还想要质量,用 LaCAM* 在时间允许内逼近最优。这是 MAPF 从"中小规模最优"迈向"大规模可扩展"的关键一步。

本质洞察:LaCAM 的"惰性后继生成"揭示了一个远超 MAPF 的普适原理——当一个问题的候选空间大得无法枚举时,关键不是"如何更快地枚举",而是"如何不枚举"。朴素的配置空间搜索面对 \(O(b^N)\) 个后继束手无策(再快的枚举也扛不住指数);LaCAM 的破局不是优化枚举,而是**根本不枚举**——用 PIBT 直接"猜"一个大概率正确的后继,猜错了(走进死路)再用回溯换一个。这把问题从"在指数空间里搜索"变成了"用一个好的生成器逐个产生候选 + 回溯兜底"。这个思路在计算机科学里反复出现:SAT 求解器不枚举 \(2^n\) 个赋值,而是 DPLL 按需赋值 + 冲突回溯;大语言模型不枚举所有可能的句子,而是逐 token 生成;游戏 AI 不展开整棵博弈树,而是用启发式引导 + 剪枝。它们的共同本质是:有一个"足够好的生成器/启发式"把搜索引向正确方向,再用"回溯/剪枝"兜住生成器的错误——生成器负责快(覆盖常见情况),回溯负责全(保证不漏解)。 LaCAM = PIBT(生成器)+ 惰性搜索(回溯兜底),正是这个范式在 MAPF 上的实例。理解了"用生成器替代枚举 + 回溯保完备",你就掌握了应对一切指数级候选空间的通用钥匙——这比记住 LaCAM 的具体流程重要得多。

代码实现(为什么 → 正确 → 错误 → 对比)¶

Step 1 — 为什么这样写。 完整的 LaCAM 是"惰性配置搜索 + PIBT 配置生成 + 回溯",较复杂。但 LaCAM 的引擎——PIBT(优先级继承 + 回溯)如何一步生成下一个无碰撞配置——可以独立演示,而且它是理解 LaCAM 的关键。下面这份**可直接编译运行**的代码,实现 PIBT 的核心:给定当前配置,按优先级给每个 agent 选下一格;若某 agent 想去的格被占,就递归"请"占位者先让路(优先级继承)。场景是 3 个 agent 路径交叉,看 PIBT 如何无碰撞地化解。只用标准库,g++ -std=c++17 即编。

// 最简 PIBT:LaCAM 的配置生成器核心——给定当前配置,一步生成下一无碰撞配置
// 优先级继承:agent 想去的格被占→递归"请"占位者先让→成功则连锁让路。纯标准库。
#include <vector>
#include <algorithm>
#include <cstdio>
#include <cmath>

struct Cell { int x, y; bool operator==(const Cell&o)const{return x==o.x&&y==o.y;} };
int W=5, H=3;
std::vector<Cell> goal, cur, next_;
std::vector<int> decided, prio;

std::vector<Cell> candidates(Cell c, int i){       // 候选下一格(4邻+原地),按离目标近排序
  std::vector<Cell> cs; int dx[5]={0,1,-1,0,0}, dy[5]={0,0,0,1,-1};
  for(int d=0;d<5;d++){ Cell n{c.x+dx[d],c.y+dy[d]};
    if(n.x>=0&&n.x<W&&n.y>=0&&n.y<H) cs.push_back(n); }
  std::sort(cs.begin(),cs.end(),[&](Cell a,Cell b){
    return std::abs(a.x-goal[i].x)+std::abs(a.y-goal[i].y)
         < std::abs(b.x-goal[i].x)+std::abs(b.y-goal[i].y); });
  return cs;
}

bool pibt(int i, int rj){                          // 为 agent i 定下一格;rj=请 i 让路的更高优先级 agent
  for(Cell c : candidates(cur[i], i)){
    bool occ=false;
    for(int k=0;k<(int)cur.size();k++)
      if(decided[k]==1 && next_[k]==c){ occ=true; break; }
    if(occ) continue;                              // c 已被他人占用
    if(rj!=-1 && c==cur[rj]) continue;             // 不和请求者对穿
    next_[i]=c; decided[i]=1;
    int j=-1;
    for(int k=0;k<(int)cur.size();k++)
      if(k!=i && decided[k]==0 && cur[k]==c){ j=k; break; }
    if(j==-1) return true;                         // c 空,成功
    if(pibt(j,i)) return true;                     // 优先级继承:请占位者 j 让路
    decided[i]=0;                                  // j 让不开,撤销,试 i 的下一候选
  }
  return false;
}

int oneStep(){                                     // 用 PIBT 生成一个后继配置
  next_.assign(cur.size(),Cell{-1,-1});
  decided.assign(cur.size(),0);
  std::vector<int> order(cur.size()); for(int i=0;i<(int)cur.size();i++) order[i]=i;
  std::sort(order.begin(),order.end(),[&](int a,int b){ return prio[a]>prio[b]; });
  for(int i:order) if(decided[i]==0) pibt(i,-1);
  for(int i=0;i<(int)cur.size();i++) prio[i]=(cur[i]==goal[i])?0:prio[i]+1;  // 越久没到越优先
  cur=next_;
  int arrived=0; for(int i=0;i<(int)cur.size();i++) if(cur[i]==goal[i]) arrived++;
  return arrived;
}

int main(){
  cur ={{0,0},{0,1},{0,2}};                        // 3 agent,路径交叉
  goal={{4,2},{4,1},{4,0}};
  prio={3,2,1};
  for(int step=1; step<=12; step++){
    int arr=oneStep();
    std::printf("step %2d: a0(%d,%d) a1(%d,%d) a2(%d,%d)  [%d/3 到达]\n",
      step,cur[0].x,cur[0].y,cur[1].x,cur[1].y,cur[2].x,cur[2].y,arr);
    if(arr==3){ std::printf("全部到达! PIBT 每步只生成一个配置,无碰撞推进。\n"); break; }
  }
  return 0;
}
// 运行输出(节选):
//   step  1: a0(1,0) a1(1,1) a2(1,2)  [0/3 到达]
//   step  4: a0(4,0) a1(4,1) a2(4,2)  [1/3 到达]
//   step  9: a0(4,2) a1(4,1) a2(4,0)  [3/3 到达]   ← 经侧让/腾挪,全部到达

Step 2 — 正确要点。 这份 demo 抓住了 PIBT 的本质,也就是 LaCAM 配置生成器的核心。其一,一步生成一个配置:oneStep 给所有 agent 同时定下一格,产出一个完整的下一配置——这正是 LaCAM 高层每次向 PIBT 要的"一个后继"。 PIBT 快(每步线性时间),所以生成单个后继是毫秒级的。其二,优先级继承(priority inheritance):pibt(i, rj) 里,当 agent \(i\) 想去的格 \(c\) 被未决定的 agent \(j\) 占着,就递归调用 pibt(j, i) 让 \(j\) 先让开——\(j\) 临时"继承"了 \(i\) 的紧迫性。这化解了"低优先级挡了高优先级的路"的死锁(优先级反转)。运行中 step 5-9 能看到这种连锁让路。其三,回溯(backtracking):若 \(j\) 让不开(它的所有候选都失败),pibt 撤销 \(i\) 的选择(decided[i]=0)、试 \(i\) 的下一个候选——这是 PIBT 名字里的"回溯"。 LaCAM 正是把 PIBT 当配置生成器:每次要一个后继配置,PIBT 毫秒级吐一个;LaCAM 在外层用带回溯的搜索组织这些配置,PIBT 卡住时换个约束让它生成不同后继——补上 PIBT 缺失的完备性。

Step 3 — 一个常见的错误写法。 实现 PIBT 时,最危险的错误是**优先级继承时不防"对穿交换"**:

// 反例:pibt 里允许 agent i 移动到"请求它让路的 agent rj 的当前位置"
bool pibt(int i, int rj){
  for(Cell c : candidates(cur[i], i)){
    // ... 检查 c 没被已定的他人占用 ...
    // 错:没有 "if(rj!=-1 && c==cur[rj]) continue;" 这一句!
    next_[i]=c; decided[i]=1;
    // ...
  }
}
// 问题:rj 请 i 让路时,i 可能"让"到了 rj 当前所在的格——而 rj 正打算离开它、
//       两者正好交换位置(对穿)。在格子世界里,对穿意味着两 agent 在通道里
//       擦身而过、实际会碰撞(它们同时占据了中间的边)。
//       结果:生成的"无碰撞配置"其实有边冲突,后续执行时撞车。

这个错误漏掉了"边冲突(对穿)"的防范。点冲突(同格)容易想到,但**对穿**(两 agent 交换位置)同样是碰撞——尤其在 PIBT 的让路过程中很容易发生:A 让给 B 的位置,恰好是 B 想离开的位置。

Step 4 — 对比。 正确版有 if(rj!=-1 && c==cur[rj]) continue;——禁止 \(i\) 让到请求者 \(rj\) 的当前位置,杜绝对穿。错误版漏掉这句,会生成带对穿(边冲突)的配置,执行时撞车。这个对比和 §T5.1 的"边冲突检测"呼应:在格子世界里,点冲突(同格)和边冲突(对穿)都是碰撞,任何 MAPF 算法都必须同时防范。 PIBT 在优先级继承的让路过程中尤其要小心对穿——这是实现 PIBT 时最易踩的坑。

带数字走一遍¶

把 PIBT demo 的输出摊开,能看清"优先级继承如何无碰撞地推进所有 agent"。场景:3 个 agent 在 5×3 网格,起点 a0(0,0)/a1(0,1)/a2(0,2),目标 a0(4,2)/a1(4,1)/a2(4,0)——注意 a0 和 a2 的目标 y 坐标互换了,它们的路径会交叉。输出:step 1-3 三个 agent 齐头并进向右(各自 x+1),step 4 a2 先到 (4,2),step 5-9 在右侧腾挪让路,step 9 全部到达。逐步读它的含义。 为什么 step 1-3 能齐步走、不打架? 因为这三步里,三个 agent 都在各自的行(y=0,1,2)向右,彼此不抢同一格——PIBT 给每个 agent 选"离目标最近的下一格"(都是右邻格),三者互不冲突,直接推进。这说明 PIBT 在无冲突时就是简单的贪心前进,开销极小。 为什么 step 5-9 要腾挪? 因为 a0(要去 y=2)和 a2(要去 y=0)的目标行相反,它们到达右侧后必须换行——这就产生了对同一格的争抢。 PIBT 此时启动**优先级继承**:谁优先级高谁先占目标格,被挡的低优先级 agent 触发"请占位者让路"的递归。输出里 step 5-9 的位置变化(a0、a2 在右侧几格内来回腾挪),正是这个让路过程。 为什么最终 9 步全部到达、全程无碰撞? 因为 PIBT 每步生成的都是一个无碰撞配置(点冲突和对穿都防了),且优先级 + 优先级继承化解了换行的争抢。 9 步比理论最短(直线 4 步 + 换行几步)略多,因为腾挪让路有额外开销——这正是 sub-optimal 的体现(可行但不最优)。 把场景改成 1D 走廊(H=1)、两 agent 对冲(目标互换)再跑,你会看到 PIBT 震荡(在两格间来回、永不到达)——因为 1D 无侧让空间,优先级继承无处腾挪。这正是练习 2 的点,也是 PIBT 不完备的铁证:它需要 LaCAM 的外层回溯来跳出这种死循环。

⚠️ 常见陷阱¶

💡 编程陷阱:PIBT 优先级继承时漏防"对穿交换" - 错误想法:只要保证两个 agent 不进同一个格子(点冲突),配置就无碰撞了。 - 现象 / 后果:生成的配置有边冲突(两 agent 交换位置/对穿),执行时它们在通道里擦身碰撞。 - 根本原因:格子世界里,两 agent 交换位置意味着同时占用它们之间的边——这是碰撞。 PIBT 让路时,A 被请去的格恰好是 B 想离开的格,极易发生对穿。 - 正确做法:pibt(i, rj) 里禁止 \(i\) 移动到请求者 \(rj\) 的当前位置(if(rj!=-1 && c==cur[rj]) continue;);更完整的实现要检测所有 agent 对的位置交换。点冲突和边冲突都要防。

⚠️ 概念陷阱:以为 PIBT 是完备的、能解所有 MAPF - 错误想法:PIBT 又快又能让路,应该能解所有 MAPF 实例。 - 现象 / 后果:在窄道对冲、死端地图等场景,PIBT 会卡住或震荡(本节 1D 走廊对冲就会来回震荡),却以为是 bug。 - 根本原因:PIBT 对一般 MAPF 不完备——它只保证在 biconnected(双连通)图上所有 agent 有限时间到达;一般图/有死端的地图上可能找不到解。这是 PIBT 的本质局限,不是 bug。 - 正确做法:理解 PIBT 是"快但不完备"的配置生成器;要完备,必须像 LaCAM 那样在外层套带回溯的搜索——PIBT 卡住时,搜索回溯、换一个后继重试。 LaCAM = PIBT 的速度 + 搜索的完备。

🧠 思维陷阱:以为"惰性"是偷懒、会漏解 - 错误想法:LaCAM 惰性后继生成"每次只生成一个后继,不枚举全部",听起来像偷懒,会不会漏掉某些解? - 现象 / 后果:怀疑 LaCAM 不可靠、不敢用,或不理解它为什么仍完备。 - 根本原因:惰性 ≠ 放弃。 LaCAM 不是"只生成一个后继就完事",而是"先生成一个,不行再生成下一个"——配合回溯,它**最终能穷尽所有后继**(理论上),只是把"一次性枚举指数个"改成了"按需逐个生成"。绝大多数情况第一个后继就推进了搜索,无需枚举其他。 - 正确做法:理解"惰性"是一种**计算顺序的优化**(用到才算),不改变完备性(回溯保证最终穷尽)。这是把指数级分支因子 \(O(b^N)\) 压成"每次 \(O(1)\) 生成"的关键——既高效又不漏解。

练习¶

[实现] 给 PIBT demo 加入"对穿(边冲突)的完整检测":不只防 \(i\) 让到 \(rj\) 的位置,而是检测任意两 agent \(a,b\) 是否 next_[a]==cur[b] && next_[b]==cur[a](交换)。构造一个会触发对穿的场景验证。
[观察] 把 demo 改成 1D 走廊(H=1)、两 agent 对冲(目标互换),运行观察 PIBT 的**震荡/死锁**(在两格间来回)。这正是 PIBT 不完备的体现。思考:LaCAM 的回溯如何能跳出这种震荡?(提示:换不同的后继配置)
[设计] LaCAM 的惰性后继生成,每次让 PIBT 在"一组约束"下生成一个后继。请设计这个"约束"长什么样(指定哪些 agent 的下一步)、LaCAM 如何枚举不同的约束来获得不同的后继。这是 LaCAM 完备性的关键机制。

§T5.3 ORCA:速度空间里的互惠避碰 ⭐⭐⭐¶

动机:中心化 MAPF 不适合实时局部避碰¶

§T5.1-§T5.2 的 CBS/LaCAM 都是**离散、中心化**的:在格子图上、由一个中心规划器算出所有 agent 的完整路径。这套方法适合"提前规划好的、格子化的"场景(仓库 AGV 按预定路径跑)。但很多场景不是这样: 机器人在连续空间里实时移动,环境动态变化(别的机器人、行人随时改变方向),没有格子、也来不及中心化地重算所有 agent 的全局路径。比如:一群送餐机器人在商场里穿行,每个只知道自己的目标和周围邻居的位置/速度,要**实时、各自决定怎么走才能不撞**——没有中心、没有格子、没有预先规划。这种"连续空间、动态、去中心化、实时局部避碰"的需求,CBS/LaCAM 处理不了(它们是离散、中心化、规划完整路径的)。需要一种**连续速度空间的、去中心化的、每个 agent 自己实时算的**避碰方法。 ORCA(Optimal Reciprocal Collision Avoidance,最优互惠避碰)正是为此而生——每个 agent 只看周围邻居,在**速度空间**里实时算出一个"既朝目标、又保证不撞"的速度,完全去中心化、无需通信、每步只解一个低维线性规划。

如果不这样做会怎样(反面)¶

不用 ORCA 这类速度空间方法,连续空间的多机避碰会遇到几个问题。 离散方法水土不服:把连续空间强行格子化再跑 CBS/LaCAM?格子粒度粗了不精确(机器人卡在格子间)、细了状态爆炸;而且动态环境里要不停重新规划完整路径,中心化算不过来。离散方法的前提(格子、中心、完整路径)在这里都不成立。 朴素速度避碰的震荡(reciprocal dance):一个直觉做法是"看到对面 agent 要撞,就往旁边躲"。但如果两个 agent 都这么想、同时往同一边躲,会**再次对上**;然后又同时往另一边躲,再对上——来回震荡,像两个人在走廊里礼貌性地反复让到同一侧(英文叫 reciprocal dance)。这是因为每个 agent 都假设"对方不动、全靠我躲",导致过度反应。 速度障碍(VO)的责任不清:更进一步,可以用"速度障碍(Velocity Obstacle,VO)"——把会导致碰撞的速度集合算出来、避开它。但朴素 VO 假设对方速度不变(把对方当动态障碍),没考虑"对方也是会避让的智能体",于是责任不清:要么两边都全力躲(过度,还是震荡),要么都指望对方躲(不够,撞上)。 ORCA 的突破在"互惠(reciprocal)"二字:它让每对 agent **各承担一半**避碰责任——A 知道 B 也会躲一半,所以 A 只需躲自己那一半。这样既不过度(不震荡)、也不欠缺(不碰撞),还无需通信(双方各自按"一半责任"算,自然协调)。下面看它怎么用半平面精确实现这个"各担一半"。

历史:从 RVO 到 ORCA¶

速度空间避碰有一条清晰的演进线,都出自 UNC Chapel Hill 的 GAMMA 实验室(Dinesh Manocha 组)。 VO(Velocity Obstacle,速度障碍)(Fiorini & Shiller, 1998)是起点:把"会在未来某时刻撞上障碍"的所有相对速度,刻画成速度空间里的一个锥形区域(VO),agent 只要选 VO 之外的速度就安全。但 VO 把对方当成速度固定的障碍,处理"对方也会动"的智能体时会震荡。 RVO(Reciprocal Velocity Obstacle,互惠速度障碍)(van den Berg, Lin, Manocha, ICRA 2008)引入互惠思想:不再假设对方不动,而是假设"对方也会避让一半",取自己当前速度和 VO 边界速度的平均——缓解了震荡,但仍不够干净(责任划分是启发式的)。 ORCA(van den Berg, Stephen J. Guy, Ming Lin, Dinesh Manocha,《Reciprocal n-Body Collision Avoidance》,ISRR 2011,Springer Tracts in Advanced Robotics vol.70, pp.3-19)是集大成者:它用**半平面**精确地划分责任——每对 agent 的"许可速度"被一条半平面边界严格界定,A 和 B 各承担一半,数学上保证无碰撞、且不过度反应。 ORCA 的工程影响巨大:选最优速度被归约成一个**低维线性规划(LP)**(2D 里就是 2 维 LP),每个 agent 每步解一个 LP,毫秒级;完全去中心化、无需通信;能处理上千 agent。它被广泛用于人群仿真、游戏、机器人集群,开源实现 RVO2 库(snape/RVO2,Apache-2.0)是事实标准。

理论:速度障碍、半平面、各担一半责任¶

第一步:速度障碍(VO)。 考虑 agent \(A\) 和 \(B\),半径分别 \(r_A, r_B\),当前位置 \(p_A, p_B\)。 速度障碍 \(VO_{A|B}^\tau\) 是这样一个集合:所有"会导致 A 和 B 在未来 \(\tau\) 时间内碰撞"的**相对速度** \(v_A - v_B\)。几何上,\(VO_{A|B}^\tau\) 是一个截断的锥形(从 \(p_B - p_A\) 方向、张角由半径和 \(\tau\) 决定)——若相对速度落在这个锥里,A、B 会在 \(\tau\) 内相撞。所以,只要让相对速度 \(v_A - v_B\) 落在 \(VO\) 之外,就能在 \(\tau\) 时间内避免碰撞。

第二步:责任划分——半平面。 朴素做法是 A 独自把相对速度调出 VO(假设 B 不动),但这会震荡(B 也在调)。 ORCA 的关键:让 A 和 B 各承担一半**调整责任。具体地,设当前相对速度 \(v_A^{opt} - v_B^{opt}\)(\(v^{opt}\) 是 agent 的优化速度,通常取当前速度)。计算 \(u\):从 \(v_A^{opt} - v_B^{opt}\) 到 **VO 边界上最近点**的向量——\(u\) 就是"为脱离碰撞,相对速度最少要改变多少、往哪改"。 ORCA 规定:A 承担 \(u\) 的一半。于是 A 的**许可速度集合**是一个**半平面:

\[ ORCA_{A|B}^\tau = \left\{ v \;\middle|\; \left( v - \left(v_A^{opt} + \tfrac{1}{2}u\right) \right) \cdot \hat{n} \ge 0 \right\} \]

其中 \(\hat{n}\) 是 \(u\) 方向的单位向量(指向远离碰撞的方向)。直观理解:这个半平面由**过点 \(v_A^{opt} + \frac{1}{2}u\)、垂直于 \(u\) 的直线**界定——A 只要把自己的新速度选在这个半平面内(即在"自己那一半责任"的安全侧),就尽到了责任。 \(B\) 的半平面 \(ORCA_{B|A}^\tau\) 对称定义(B 承担另一半 \(u\))。 为什么各担一半就够? 因为 A 选在自己半平面内(贡献 \(\frac{1}{2}u\)),B 选在自己半平面内(贡献另 \(\frac{1}{2}u\)),两者相对速度的改变量加起来正好是 \(u\)——恰好脱离 VO,不多不少。这就是"互惠"的数学本质:双方各出一半力,合起来正好解决问题,无需通信协调。

第三步:多邻居 + 线性规划。 A 周围可能有多个邻居 \(B_1, B_2, \dots\),每个邻居给 A 一个半平面约束 \(ORCA_{A|B_k}\)。 A 要选一个新速度 \(v_A^{new}\),同时满足所有这些半平面约束(全部邻居都不撞),并且**尽量接近 A 的偏好速度** \(v_A^{pref}\)(朝目标的速度)。这是一个标准的优化问题:

\[ v_A^{new} = \arg\min_{v \in \bigcap_k ORCA_{A|B_k}^\tau} \| v - v_A^{pref} \| \]

目标是"在所有半平面的交集里,找离偏好速度最近的速度"。因为约束都是线性的(半平面)、目标是到一点的距离,这是一个**低维线性规划**(2D 里 2 维、3D 里 3 维)——van den Berg 等人给了一个 \(O(n)\) 的增量算法(\(n\) 是邻居数)逐个加入半平面求解,极快。所以每个 agent 每步:收集邻居 → 算每个邻居的半平面 → 解一个低维 LP → 得到新速度。全程只用局部信息(邻居的位置/速度,可观测无需通信),复杂度 \(O(n)\),毫秒级。

完备性兜底:LP 无解怎么办? 极密集时,所有半平面的交集可能为空(没有速度能同时避开所有邻居)——LP 无解。 ORCA 的处理:这时退而求其次,选一个"违反约束最少"的速度(最小化最大的约束违反量),即一个 3D 的线性规划(densest case)。这保证 ORCA 总能给出一个速度(尽量安全),不会卡死。这是 ORCA 工程鲁棒性的关键:即使在理论上无解的拥堵中,也能优雅降级、给出最不坏的选择。

ORCA 的定位。 ORCA 是**局部、反应式、去中心化**的避碰:它不规划完整路径(那是 CBS/LaCAM 的事),而是每个 agent 实时算"下一步速度"。优点:去中心化、无需通信、\(O(n)\) 极快、连续空间、处理动态环境。局限:它是**局部**的,只保证短期(\(\tau\) 内)不撞,不保证全局最优、甚至不保证一定到达目标(可能在复杂障碍中陷入局部死锁——这时需要配合全局路径引导)。所以 ORCA 常和全局规划配合:全局规划(可能用 MAPF 或单机路径)给出大致路径,ORCA 负责沿途的实时局部避碰。这是连续空间多机系统的典型架构:全局规划定大方向 + ORCA 管局部不撞。

多视角对照:ORCA 的"半平面 + 各担一半",从三个角度看,各有启发。 从"约束优化"看:每个邻居给一个半平面(线性不等式约束),agent 在所有约束的交集里找离偏好速度最近的点——这就是一个带线性约束的最小二乘/LP。这个视角告诉你 ORCA 为什么能解得快:它是低维(2D/3D)线性约束优化,有 \(O(n)\) 增量算法,毫秒级——和 SVM 找最大间隔超平面、QP 求解是同源的几何优化。 从"博弈/契约"看:"各担一半"是 agent 之间一个隐式的契约——我承诺躲一半,默认你也躲一半,谁都不必通信就能信任对方会履约(因为对方跑同样的 ORCA)。这个视角告诉你 ORCA 为什么无需通信:协调来自"共享同一套规则 + 对称的责任划分",像交通规则里"各让右侧"——大家都守规则,不必每次都商量。 从"分布式系统"看:每个 agent 是一个独立节点,只用本地可观测信息(邻居位置/速度)做决策,没有中心、没有消息传递。这个视角告诉你 ORCA 为什么鲁棒可扩展:无单点故障、无通信瓶颈、agent 数增加时每个只看局部 \(O(n)\) 邻居——和无中心的 P2P、群体智能(鸟群/鱼群的局部规则涌现全局行为)同理。三个视角对应三类思想(凸优化 / 博弈契约 / 分布式涌现):凸优化解释求解效率、博弈契约解释无需通信、分布式涌现解释鲁棒可扩展。 ORCA 之所以优雅,正是因为它用一个简单的几何构造(半平面 + ½)同时满足了这三方面——这也是它成为人群仿真、机器人集群事实标准的原因。

代码实现(为什么 → 正确 → 错误 → 对比)¶

Step 1 — 为什么这样写。 完整的 ORCA 要做时间推进模拟 + 精确的 VO 锥几何,代码量大且 VO 几何的边界情况繁琐。但 ORCA 的核心机制——给定两 agent 的位置/速度,构造"各担一半责任"的半平面,再解 LP 把速度调到安全侧——可以在一份**可直接编译运行**的代码里清晰演示,而且这是理解 ORCA 的关键。下面的 demo 做一次单步 ORCA:A 想朝右上冲(偏好速度会撞向 B),构造 ORCA 半平面,解 LP 求 A 的新速度,并**数学上验证**新速度落在半平面安全侧、且偏转最小。只用标准库,g++ -std=c++17 即编。

// 最简 ORCA:演示单步"速度空间半平面 + 各担一半 + LP 求解"核心机制
// 给定两 agent 位置/速度,构造 ORCA 半平面,解 LP 求 A 的新速度,验证在安全侧。纯标准库。
#include <vector>
#include <cmath>
#include <cstdio>

struct Vec{double x,y;};
Vec operator-(Vec a,Vec b){return{a.x-b.x,a.y-b.y};}
Vec operator+(Vec a,Vec b){return{a.x+b.x,a.y+b.y};}
Vec operator*(Vec a,double s){return{a.x*s,a.y*s};}
double dot(Vec a,Vec b){return a.x*b.x+a.y*b.y;}
double len(Vec a){return std::sqrt(dot(a,a));}
Vec norm(Vec a){double l=len(a);return l>1e-9?Vec{a.x/l,a.y/l}:Vec{0,0};}

struct Line{Vec point,n;};   // 半平面:dot(v - point, n) >= 0

// 由 A 对 B 构造 ORCA 半平面(VO 圆盘 + 各担一半)
Line orcaHalfPlane(Vec pA,Vec vA,double rA,Vec pB,Vec vB,double rB,double tau){
  Vec relP=pB-pA, relV=vA-vB;
  double R=rA+rB;
  Vec center=relP*(1.0/tau);              // 截断 VO 近端圆盘:中心 relP/tau
  double vor=R/tau;                       // 圆盘半径 R/tau
  Vec w=relV-center;                      // 相对速度相对圆盘中心的偏移
  double wl=len(w);
  Vec dirn=norm(w);
  Vec u=dirn*(vor-wl);                    // 脱离碰撞所需的相对速度改变量
  Line L; L.n=norm(u);                    // 半平面法向沿 u
  L.point=vA+u*0.5;                       // 各担一半:过点 vA + 0.5u
  return L;
}

// LP:在所有半平面交集中找离 pref 最近的速度(迭代投影,2D 教学版)
Vec solveLP(Vec pref,const std::vector<Line>&ls,double vmax){
  Vec v=pref; if(len(v)>vmax) v=norm(v)*vmax;
  for(int it=0;it<200;it++){
    bool ok=true;
    for(auto&L:ls){
      double d=dot(v-L.point,L.n);
      if(d<-1e-9){ v=v-L.n*d; ok=false; }   // 违反→投影回半平面边界
    }
    if(len(v)>vmax) v=norm(v)*vmax;
    if(ok) break;
  }
  return v;
}

int main(){
  Vec pA{0,0}, vA{1.0,0.4}, pB{4,1.2}, vB{-1.0,0};   // A、B 接近将撞
  double rA=0.5, rB=0.5, tau=3.0, vmax=1.5;
  Vec prefA{1.0,0.4};                                 // A 想朝右上(会撞向 B)

  Line L=orcaHalfPlane(pA,vA,rA,pB,vB,rB,tau);
  std::printf("A 对 B 的 ORCA 半平面: 过点(%.3f,%.3f), 法向(%.3f,%.3f)\n",
              L.point.x,L.point.y,L.n.x,L.n.y);
  double dpref=dot(prefA-L.point,L.n);
  std::printf("偏好速度(%.1f,%.1f) 代入: dot=%.3f %s\n",
              prefA.x,prefA.y,dpref, dpref<0?"<0 违反(会撞)→需调整":">=0 安全");
  Vec vnew=solveLP(prefA,{L},vmax);
  double dnew=dot(vnew-L.point,L.n);
  std::printf("LP 求得新速度(%.3f,%.3f): dot=%.3f (>=0 安全侧 ✓), 偏转=%.1f°\n",
              vnew.x,vnew.y,dnew, std::acos(dot(norm(prefA),norm(vnew)))*180/M_PI);
  return 0;
}
// 运行输出:
//   A 对 B 的 ORCA 半平面: 过点(0.833,0.400), 法向(-1.000,-0.000)
//   偏好速度(1.0,0.4) 代入: dot=-0.167 <0 违反(会撞)→需调整
//   LP 求得新速度(0.833,0.400): dot=-0.000 (>=0 安全侧 ✓), 偏转=3.8°

Step 2 — 正确要点。 这份 demo 抓住了 ORCA 的三个本质。其一,半平面"各担一半":orcaHalfPlane 里 L.point = vA + u*0.5——半平面过 \(v_A + \frac{1}{2}u\),即 A 只承担脱离碰撞所需改变量 \(u\) 的一半(另一半由 B 对称承担)。这就是"互惠(reciprocal)"的代码体现:不假设对方不动、也不全靠自己,而是各出一半。其二,违反检测 + LP 投影:偏好速度 (1.0,0.4) 代入半平面 dot=-0.167<0,说明它在危险侧(会撞);solveLP 把它投影回半平面边界,得到 (0.833,0.400)、dot≈0(落在边界上,即"最小调整到安全")。这正是 LP 的作用:在安全约束下,找离偏好最近的速度。其三,偏转最小:新速度只偏转了 3.8°——ORCA 不会过度反应(不像震荡那样大幅乱躲),而是恰好调整到安全边界、尽量保持原方向。这是"最优(optimal)"的体现。真实 ORCA 在此基础上:精确处理 VO 锥(不只圆盘)、多邻居叠加多个半平面、用 \(O(n)\) 增量 LP 算法、LP 无解时降级到"违反最小"的 3D LP。但"半平面各担一半 + LP 投影到安全侧"的核心一致。

Step 3 — 一个常见的错误写法。 实现 ORCA 时,最常见的概念性错误是**让 A 承担全部责任(忘了"各担一半")**:

// 反例:半平面过 vA + u(而非 vA + 0.5u),A 独自承担全部脱离碰撞的责任
Line L; L.n = norm(u);
L.point = vA + u;          // 错:应该是 vA + u*0.5!
// 问题:这等于假设"B 完全不动,全靠 A 躲开"。但 B 也是个会避让的 ORCA agent,
//       B 同样假设自己要躲——结果 A 躲了全程、B 也躲了全程,两者过度反应:
//       (1) 轨迹大幅绕远(各自多躲了一倍);
//       (2) 更糟的是会震荡(reciprocal dance)——A、B 都全力躲同一边,
//           再对上、再一起躲另一边,来回摆动。
//       这正是 RVO 之前的方法的老毛病,ORCA 用"各担一半"才根治。

这个错误丢掉了 ORCA 最核心的"互惠"思想。把 \(\frac{1}{2}u\) 写成 \(u\),看似只差一个系数,实则退回到了"假设对方不动、自己全责"的老路,导致过度避让和震荡。

Step 4 — 对比。 正确版 L.point = vA + u*0.5——A 担一半、B 担一半,合起来正好脱离碰撞,不多不少,无震荡。错误版 L.point = vA + u——A 担全责(隐含假设 B 不动),B 也担全责,双方过度反应、轨迹绕远、甚至震荡。这个对比是 ORCA 的灵魂:"互惠"= 各担一半 = 那个 \(\frac{1}{2}\)。它让多个 agent 在无通信的情况下自然协调——每个都只做自己那一半,合起来恰好解决问题。这也是 ORCA 比 VO/RVO 更优雅、更稳定的根本原因。

带数字走一遍¶

把 ORCA demo 的单步输出摊开,能看清"半平面如何把危险速度拉回安全侧"。场景:A 在 (0,0) 速度 (1.0,0.4),B 在 (4,1.2) 速度 (-1.0,0),两者接近会撞;A 的偏好速度是 (1.0,0.4)(想继续朝右上)。输出三行:半平面过点 (0.833,0.400)、法向 (-1.000,0);偏好速度代入 dot=-0.167<0(违反,会撞);LP 求得新速度 (0.833,0.400)、dot≈0(安全侧)、偏转 3.8°。逐个数字读它的含义。 为什么半平面法向是 (-1,0)? 因为这个场景里,A、B 的相对运动让"脱离碰撞所需的速度改变 \(u\)"主要在 -x 方向(A 要稍微减小向右的分量才能避开 B)。法向 (-1,0) 表示半平面的安全侧是"x 分量更小"那一侧——即 ORCA 建议 A 把向右的速度收一点。 为什么偏好速度 dot=-0.167<0? 因为 A 的偏好 (1.0,0.4) 的 x 分量(1.0)太大,落在了半平面的危险侧(会撞向 B)。负的 dot 值量化了"违反程度"——越负越危险。这就是 ORCA 检测到"按偏好走会撞"。 为什么 LP 把速度调到 (0.833,0.400)? 因为 LP 要"在半平面安全侧找离偏好最近的速度"。它把 x 分量从 1.0 压到 0.833(刚好到半平面边界,dot≈0),y 分量保持 0.4 不变——这是**最小调整**:只动了必须动的 x 分量,且只动到边界(不过度)。 为什么偏转只有 3.8°? 因为 (0.833,0.400) 和 (1.0,0.4) 方向只差一点点——ORCA 不会让 A 大幅转向乱躲,而是恰好调整到安全边界、最大限度保持原方向。这正是"最优(optimal)"和"不震荡"的体现:动作最小、不过度反应。 把 B 的速度改成径直冲向 A(更危险)再算,你会看到 dot 更负(违反更严重)、\(u\) 更大、新速度偏转更大——危险程度直接决定了 ORCA 让 A 调整的幅度。这正是练习 2 要体会的:越危险,半平面约束越强,偏转越大。

⚠️ 常见陷阱¶

💡 编程陷阱:半平面忘了"各担一半",让 A 承担全部责任 - 错误想法:A 要避开 B,那就让 A 把相对速度完全调出碰撞区(半平面过 \(v_A + u\))。 - 现象 / 后果:A、B 都全力避让(各自以为全靠自己),轨迹大幅绕远;更糟的是震荡(reciprocal dance)——两者反复躲向同一边、再对上、再一起躲另一边。 - 根本原因:这隐含假设"对方不动、自己全责",忽略了 B 也是会避让的 agent。 ORCA 的互惠思想恰恰是"对方也会躲一半,我只需躲一半"。 - 正确做法:半平面过 \(v_A + \frac{1}{2}u\)(L.point = vA + u*0.5)——A 担一半、B 对称担另一半。这个 \(\frac{1}{2}\) 是 ORCA 的灵魂,合起来正好脱离碰撞、不过度、无震荡。

⚠️ 概念陷阱:以为 ORCA 规划完整路径、保证到达目标 - 错误想法:ORCA 既然能避碰,应该像 CBS 那样算出完整无冲突路径、保证每个 agent 到达目标。 - 现象 / 后果:在有障碍/复杂场景里 agent 陷入局部死锁、到不了目标,却以为 ORCA 出了 bug。 - 根本原因:ORCA 是**局部、反应式**的——它只算"下一步速度"、只保证短期(\(\tau\) 内)不撞,不规划完整路径、不保证全局到达(可能困在障碍后的局部极小)。它和 CBS/LaCAM 是不同层次的工具。 - 正确做法:理解 ORCA 的定位是"局部实时避碰",通常要配合全局路径引导:全局规划(如 MAPF 或单机路径)给大方向,ORCA 负责沿途实时不撞。" 全局定方向 + ORCA 管局部"是连续空间多机系统的典型架构。

🧠 思维陷阱:以为去中心化、无通信就一定不如中心化 - 错误想法:ORCA 每个 agent 各自算、不通信,肯定不如中心化统一规划"全局协调"得好。 - 现象 / 后果:在该用去中心化的大规模/实时场景硬上中心化,算不过来或单点故障。 - 根本原因:中心化能全局最优,但计算量随 agent 数爆炸、且中心是单点故障、还需要把所有 agent 状态汇总到中心(通信开销大);去中心化每个 agent 只算自己(\(O(n)\) 邻居)、无中心瓶颈、鲁棒。 ORCA 的"互惠"设计让去中心化也能协调(各担一半自然不撞),无需通信。 - 正确做法:理解中心化和去中心化各有适用——小规模、需全局最优、可中心化算用 CBS/LaCAM;大规模、实时、动态、要鲁棒用 ORCA 这类去中心化方法。" 去中心化 + 互惠"在合适场景下又快又鲁棒,不是中心化的劣化版。

练习¶

[实现] 给 ORCA demo 加入第二个邻居(三个 agent),A 同时受 B、C 两个半平面约束。修改 solveLP 让它在两个半平面的交集里找最优速度,观察 A 如何同时避开两者。
[分析] demo 里 A 的偏转只有 3.8°(很小)。如果把 B 的速度改成直接冲向 A(更危险),偏转角会怎么变?如果 A、B 距离很近(快撞上了),\(u\) 和半平面会怎么变?体会"越危险、半平面约束越强、偏转越大"。
[设计] ORCA 在极密集时所有半平面交集可能为空(LP 无解)。论文的处理是"选违反约束最少的速度"(3D LP)。请设计这个降级方案:当 solveLP 找不到满足所有约束的速度时,如何选一个"最不坏"(违反量最小)的速度?为什么这比"随便选一个"或"停下"更好?

§T5.4 EGO-Swarm:连续轨迹空间的梯度互斥 ⭐⭐⭐¶

动机:从"格子/速度"到"完整轨迹"¶

§T5.1-§T5.2 在离散格子上规划路径(CBS/LaCAM),§T5.3 在速度空间算下一步速度(ORCA)。但无人机集群有更高的要求:它们要的是**完整的、平滑的、动力学可行的飞行轨迹**——不是格子路径(无人机不沿格子飞),也不只是下一步速度(无人机要规划未来几秒的连续轨迹来保证平滑和动力学约束)。回顾 T3:单架无人机的轨迹用 MINCO/B-spline 参数化、用梯度优化求平滑安全的轨迹。现在场景是**一群**无人机在杂乱环境(树林、室内)里飞,每架不仅要避开障碍物,还要**互相避让**——而且要去中心化(每架自己算)、实时(毫秒级)、能在密集编队里穿行。这就需要把 T3 的单机轨迹优化,升级成多机:每架优化自己的连续轨迹,同时加入"和其他无人机的轨迹不冲突"的约束。 EGO-Swarm 给出的方案是:在梯度轨迹优化里,把"机间碰撞风险"当成一个惩罚项——和避障、平滑、动力学约束一起,放进同一个非线性优化,用梯度下降求解。它去中心化、毫秒级,而且不需要可靠通信。

如果不这样做会怎样(反面)¶

不用 EGO-Swarm 这类连续轨迹互斥方法,多无人机集群会遇到几个问题。 离散/速度方法不够:CBS/LaCAM 给的是格子路径,无人机不能直接飞(不平滑、不满足动力学);ORCA 给的是下一步速度,没有未来几秒的完整轨迹,无人机高速飞行时光靠"下一步速度"来不及保证平滑和安全(尤其要做出复杂机动避开树木时)。无人机需要的是**前瞻一段时间的完整轨迹**。 单机轨迹优化各飞各的会撞:用 T3 的方法让每架无人机各自优化轨迹(只避障、不管别的无人机),密集时必然撞机——因为每架的优化目标里没有"避开其他无人机"这一项。 保守留大安全球太低效:为了不撞,给每架无人机留一个巨大的安全球(谁也不准进)?那编队会很松散、过不了窄缝(树林里的缝隙容不下大安全球),失去了集群的密集协同优势。 依赖可靠通信不现实:如果要求所有无人机实时共享精确轨迹、且通信永不丢包?现实中无线通信会丢包、延迟,强依赖可靠通信的方案在真实飞行中会失效。 EGO-Swarm 的思路是:每架无人机优化自己的轨迹,把"和邻居轨迹的碰撞风险"作为优化的一个惩罚项——既不需要中心化、也不需要可靠通信(用一个"不可靠轨迹共享网络",丢包也能工作),还能在密集环境里飞出平滑紧凑的编队。下面看它怎么做。

历史:ESDF-free 梯度规划到集群¶

EGO-Swarm 出自浙江大学 FAST Lab(高飞团队),是一条清晰的演进。 EGO-Planner(Xin Zhou, Zhepei Wang, Chao Xu, Fei Gao, RA-L 2021)是基础:它提出 ESDF-free(无欧氏符号距离场)**的梯度局部规划——传统梯度规划要先建一个 ESDF(整个空间每点到最近障碍的距离场)来算避障梯度,建 ESDF 很耗时;EGO-Planner 跳过 ESDF,直接在轨迹与障碍发生碰撞处局部地估计排斥方向,大幅提速。 **EGO-Swarm(Xin Zhou, Jiangchao Zhu, Hongyu Zhou, Chao Xu, Fei Gao,ICRA 2021,pp.4101-4107,arXiv 2011.04183)把 EGO-Planner 扩展到集群:在梯度优化框架里,把**碰撞风险(含障碍和其他无人机)作为非线性优化的惩罚项**;用轻量的**拓扑轨迹生成**逃离局部极小(梯度法易陷局部极小,多条不同拓扑的候选轨迹增加找到好解的机会);每架无人机用一个**不可靠的轨迹共享网络**广播自己的轨迹(丢包也能工作);并用**深度图里的无人机检测**纠正机间相对定位漂移。整个系统去中心化、异步、仅用机载资源、毫秒级。 Swarm of micro flying robots in the wild(Xin Zhou, Xiangyong Wen, Jiangchao Zhu, Hongyu Zhou, Zhepei Wang, ... Fei Gao 等,Science Robotics 2022, 7(66):eabm5954)是其巅峰展示:一群巴掌大的微型无人机,在**密林**中自主穿行、保持编队——完全机载、去中心化,登上 Science Robotics 封面,是无人机集群领域的标志性成果。代码开源在 ZJU-FAST-Lab/ego-planner-swarm(~1.9k star),是研究多机轨迹规划的重要参考。

理论:B-spline 凸包 + 碰撞惩罚 + 拓扑逃逸¶

轨迹表示:均匀 B-spline。 EGO 系列用**均匀 B-spline**表示轨迹——一条 B-spline 由一串**控制点** \(\{Q_0, Q_1, \dots, Q_M\}\) 决定。 B-spline 有一个对碰撞检测极其有用的性质——凸包性质(convex hull property):B-spline 曲线的每一段,都完全落在它对应的几个控制点张成的**凸包**内。这意味着:要判断/控制一段轨迹是否安全,不需要检查曲线上无穷多个点,只需检查那几个控制点——把"曲线和障碍/其他轨迹的关系",转化成"控制点和障碍/其他轨迹的关系"。这是 B-spline 用于轨迹优化的关键便利。

机间碰撞:控制点凸包的关系。 两架无人机各有自己的 B-spline 轨迹(各自一串控制点)。在同一时间段,无人机 \(i\) 的轨迹段落在它那几个控制点的凸包里,无人机 \(j\) 的轨迹段落在它的凸包里。若两个凸包**不相交**,则这段时间两架轨迹不可能碰撞(安全);若凸包**重叠**,则有碰撞风险。所以机间避碰的核心,变成了:让对应时间段的控制点凸包互不重叠(或保持安全距离)。

碰撞作为惩罚项(soft constraint)。 EGO-Swarm 不把"凸包不重叠"作为硬约束(硬约束会让优化问题难解、且不平滑),而是作为**惩罚项**放进目标函数。直观地,无人机 \(i\) 的轨迹优化目标大致是:

\[ \min_{\{Q\}} \; \lambda_s J_{smooth} + \lambda_c J_{collision} + \lambda_d J_{dynamics} + \lambda_f J_{fitness} \]

其中 \(J_{smooth}\) 是平滑度(jerk/snap 积分,T3 学过),\(J_{dynamics}\) 是动力学可行性(速度/加速度限幅),\(J_{fitness}\) 是贴合参考路径,而 \(J_{collision}\) 是**碰撞惩罚**——包含和静态障碍的碰撞风险、以及**和其他无人机轨迹的碰撞风险**。机间碰撞惩罚的具体形式:当无人机 \(i\) 的某控制点 \(Q_k^i\) 和无人机 \(j\) 对应时段的轨迹/控制点距离小于安全阈值时,产生一个随距离减小而增大的惩罚(如距离差的平方),其**梯度**把 \(Q_k^i\) 往远离 \(j\) 的方向推。所以优化时,梯度下降会自动把"快撞上别的无人机"的控制点**推开**——这就是"梯度互斥":不是硬性禁止,而是用惩罚的梯度,把轨迹柔和地推离碰撞。

ESDF-free 的排斥方向。 传统方法靠 ESDF 提供"每点往哪躲"的梯度,但建 ESDF 慢。 EGO 的 ESDF-free 技巧:当检测到某控制点的轨迹段和障碍(或其他无人机)碰撞时,**局部地**生成一个"锚点 + 排斥方向"(anchor point and repulsion direction)——直接给出该点应该往哪个方向移动来脱离碰撞,无需全局 ESDF。这大幅减少了计算量,是 EGO 毫秒级的关键。

逃离局部极小:拓扑轨迹生成。 梯度下降有个老问题:容易陷入**局部极小**(被障碍挡住、推来推去出不去)。 EGO-Swarm 的对策:生成**多条不同拓扑类**的候选轨迹(比如"从障碍左边绕"和"从右边绕"是不同拓扑),分别优化,选最好的。这样即使某条轨迹陷入局部极小,其他拓扑的候选可能找到更好的解——大幅提高在复杂环境里找到可行优质轨迹的成功率。这呼应了 T2/T3 反复出现的"不同 homotopy/拓扑类"思想。

去中心化 + 不可靠通信。 每架无人机:优化自己的轨迹 → 通过网络广播给邻居 → 接收邻居的轨迹作为避碰惩罚的输入 → 下一轮重新优化。关键:这个轨迹共享网络是**不可靠的**(允许丢包、延迟)——即使某次没收到邻居的最新轨迹,无人机也能用上一次的、或保守估计继续飞,不会崩溃。加上用深度相机检测视野里的其他无人机来纠正相对定位漂移,EGO-Swarm 实现了**纯机载、去中心化、异步、对通信不苛求**的集群飞行——这正是它能在真实密林里飞起来的工程基础。

EGO-Swarm 的定位。 EGO-Swarm 是**连续轨迹、去中心化、软约束(惩罚)的多机方法。优点:输出完整平滑轨迹(无人机能直接飞)、去中心化、毫秒级、对通信不苛求、密集环境能穿行。代价:碰撞是**软约束(惩罚)——理论上不像硬约束那样**严格保证**无碰撞(惩罚权重不够大、或优化没收敛好时,可能有残余碰撞风险);它靠"惩罚足够大 + 实际工程调优"来保证安全,而非数学上的硬保证。这正是 §T5.5 的 MADER 要改进的——MADER 用**硬约束(分离超平面)**严格保证无碰撞。

代码实现(为什么 → 正确 → 错误 → 对比)¶

Step 1 — 为什么这样写。 完整的 EGO-Swarm 是一个完整的梯度轨迹优化器(B-spline + L-BFGS + 多项惩罚 + 拓扑生成),代码量很大。但它机间避碰的核心机制——用 B-spline 控制点的凸包判断两机轨迹是否冲突,冲突则用惩罚梯度把控制点推开——可以独立演示,这正是理解 EGO-Swarm 多机协同的关键。下面的 demo:给两架无人机各一段轨迹的控制点,检测它们的凸包是否重叠(用分离轴),若重叠则按"碰撞惩罚梯度"把无人机 \(i\) 的控制点推离 \(j\),并验证推开后间隙增大。只用标准库,g++ -std=c++17 即编。

// 最简 EGO-Swarm 机间避碰核心:B-spline 控制点凸包重叠检测 + 碰撞惩罚梯度
// 检测两机轨迹段凸包是否重叠,重叠则用惩罚梯度推开控制点。纯标准库。
#include <vector>
#include <cmath>
#include <cstdio>
#include <algorithm>

struct Vec { double x, y; };
Vec operator-(Vec a,Vec b){return{a.x-b.x,a.y-b.y};}
Vec operator+(Vec a,Vec b){return{a.x+b.x,a.y+b.y};}
Vec operator*(Vec a,double s){return{a.x*s,a.y*s};}
double dot(Vec a,Vec b){return a.x*b.x+a.y*b.y;}
double len(Vec a){return std::sqrt(dot(a,a));}

// 用连心线做分离轴,返回两凸包沿该轴的间隙(>0 分离/安全, <=0 重叠/碰撞)
double convexGap(const std::vector<Vec>& A, const std::vector<Vec>& B){
  Vec ca{0,0}, cb{0,0};
  for(auto&p:A) ca=ca+p;
  ca=ca*(1.0/A.size());
  for(auto&p:B) cb=cb+p;
  cb=cb*(1.0/B.size());
  Vec axis = cb - ca;                          // 连心线方向(分离轴)
  double l=len(axis); if(l<1e-9) return -1;
  axis = axis*(1.0/l);
  double aMin=1e9,aMax=-1e9,bMin=1e9,bMax=-1e9;  // 两点集在 axis 上的投影区间
  for(auto&p:A){ double d=dot(p,axis); aMin=std::min(aMin,d); aMax=std::max(aMax,d); }
  for(auto&p:B){ double d=dot(p,axis); bMin=std::min(bMin,d); bMax=std::max(bMax,d); }
  return bMin - aMax;                           // A 在前、B 在后:间隙 = bMin - aMax
}

int main(){
  std::vector<Vec> Qi = {{0,0},{0.5,0.3},{1.0,0.2},{1.5,0.0}};     // i 的控制点
  std::vector<Vec> Qj = {{1.2,0.1},{1.6,0.3},{2.0,0.2},{2.4,0.0}}; // j 的控制点(靠近)
  double safeDist = 0.6;

  double gap = convexGap(Qi, Qj);
  std::printf("两机轨迹段凸包间隙 = %.3f (安全距离 %.2f)\n", gap, safeDist);
  if(gap < safeDist){
    std::printf("间隙 < 安全距离 → 碰撞风险! 用惩罚梯度推开 i 的控制点\n");
    Vec cj{0,0}; for(auto&p:Qj) cj=cj+p; cj=cj*(1.0/Qj.size());
    for(size_t k=0;k<Qi.size();k++){
      Vec away = Qi[k] - cj; double d=len(away);
      if(d < safeDist*2){                        // 离 j 近的控制点才推
        Vec dir = away*(1.0/std::max(d,1e-6));
        double push = (safeDist - gap)*0.5;       // 推动量随违反程度增大
        Qi[k] = Qi[k] + dir*push;
      }
    }
    std::printf("推开后间隙 = %.3f (增大,碰撞风险降低)\n", convexGap(Qi,Qj));
  }
  return 0;
}
// 运行输出:
//   两机轨迹段凸包间隙 = -0.298 (安全距离 0.60)
//   间隙 < 安全距离 → 碰撞风险! 用惩罚梯度推开 i 的控制点
//   推开后间隙 = 0.117 (增大,碰撞风险降低)

Step 2 — 正确要点。 这份 demo 抓住了 EGO-Swarm 机间避碰的本质。其一,凸包性质把"查曲线"变成"查控制点":convexGap 只用两组控制点(各 4 个点)就判断了两段 B-spline 轨迹是否可能碰撞——靠的是 B-spline"曲线落在控制点凸包内"的性质。不用在曲线上密集采样、检查无穷多个点,只看控制点凸包是否分离。这是 B-spline 用于高效碰撞检测的关键便利。其二,碰撞作软约束(惩罚梯度):检测到凸包重叠(gap < safeDist)后,不是硬性禁止,而是给 \(i\) 离 \(j\) 近的控制点施加一个"远离 \(j\)"的位移(惩罚的负梯度方向),推动量 (safeDist - gap) 随违反程度增大。运行结果:推开后间隙从 -0.298 增大到 0.117——梯度把轨迹柔和地推离碰撞。这就是"梯度互斥"。其三,去中心化的输入:\(i\) 优化时把 \(j\) 的控制点当作避碰惩罚的输入(\(j\) 的轨迹通过不可靠网络广播而来)——每架无人机只需邻居的轨迹、各自优化,无需中心。真实 EGO-Swarm 在此之上:用完整的 B-spline + L-BFGS 优化、多项惩罚联合(平滑/动力学/障碍/机间)、拓扑生成逃局部极小、ESDF-free 局部排斥方向。但"凸包判碰 + 惩罚梯度推开"的核心一致。

Step 3 — 一个常见的错误写法。 用 B-spline 做轨迹优化时,一个常见错误是**在曲线上密集采样点来检测碰撞,而不用凸包性质**:

// 反例:在 B-spline 曲线上采样几十个点,逐点检查碰撞(没用凸包性质)
for(double t=0; t<=1.0; t+=0.02){              // 沿曲线采样 50 个点
  Vec pt = evaluateBSpline(Qi, t);             // 求曲线在 t 处的点(要算 B-spline 基函数)
  for(double s=0; s<=1.0; s+=0.02){
    Vec pt2 = evaluateBSpline(Qj, s);
    if(len(pt-pt2) < safeDist) { /* 碰撞 */ }   // 50x50=2500 次距离检查
  }
}
// 问题:(1) 慢——每条轨迹采样几十点、两两比较是平方级(O(采样数^2)),
//           还要反复求 B-spline 曲线值(算基函数),远慢于查几个控制点凸包;
//       (2) 不严格——采样是离散的,两个采样点之间的曲线段可能碰撞却没被采到(漏检);
//       (3) 求梯度难——对采样点求碰撞梯度,要链式法则穿过 B-spline 基函数,繁琐。
//       凸包性质直接用控制点,既快、又严格(凸包包住整段曲线、不漏)、梯度还简单
//       (直接对控制点求)。

这个错误放弃了 B-spline 最有价值的凸包性质,退回到"密集采样 + 两两比较",既慢、又可能漏检(采样间隙)、求梯度还麻烦。

Step 4 — 对比。 正确版用凸包性质(convexGap),只查几个控制点就判碰撞——快、严格(凸包包住整段曲线)、梯度对控制点直接可求。错误版在曲线上密集采样两两比较——慢(平方级 + 反复求曲线值)、可能漏检(采样间隙)、梯度难求。这个对比是用 B-spline 做轨迹优化的通用要点:充分利用凸包性质,把曲线层面的问题(碰撞、约束)下放到控制点层面——这是 B-spline(以及 Bézier、MINCO 等多项式表示)在轨迹优化里高效的根本原因。下一节 MADER 会把这个思想推到极致:用比 B-spline 凸包更紧的 MINVO 单纯形。

带数字走一遍¶

把 EGO demo 的输出摊开,能看清"凸包判碰 + 梯度推开"如何运转。场景:无人机 \(i\) 的控制点凸包大致在 x∈[0,1.5]、\(j\) 的在 x∈[1.2,2.4],两者在 x≈1.2~1.5 处靠近;安全距离 0.6。输出:两机凸包沿连心线的间隙 = -0.298(负,重叠);推开 \(i\) 离 \(j\) 近的控制点后,间隙增大到 0.117。逐个数字读它的含义。 为什么间隙是 -0.298(负值)? 因为 \(i\) 的凸包右端(x≈1.5)伸进了 \(j\) 的凸包左端(x≈1.2)——两者在连心线方向上的投影**重叠**了 0.298。负的间隙量化了"重叠多深",即碰撞风险有多大。这就是凸包判碰:间隙 < 安全距离(这里 -0.298 < 0.6)就有风险。 为什么只推 Q2、Q3 两个控制点? 因为代码里 if(d < safeDist*2) 只推"离 \(j\) 近"的控制点。 \(i\) 的 Q0(0,0)、Q1(0.5,0.3) 离 \(j\) 远,不用动;Q2(1.0,0.2)、Q3(1.5,0.0) 离 \(j\) 近,是碰撞风险的来源,把它们推开。这体现了惩罚梯度的**局部性**:只调整真正接近碰撞的那部分轨迹,不动其余——这也是 EGO-Swarm ESDF-free 的精神(局部地处理碰撞,不全局重算)。 为什么推开后间隙变成正的 0.117? 因为 Q2、Q3 被沿"远离 \(j\) 质心"方向推动后,\(i\) 的凸包整体后缩,不再伸进 \(j\) 的凸包——间隙从重叠(-0.298)变成分离(+0.117)。这就是梯度互斥的效果:惩罚的梯度把轨迹柔和地推离碰撞,直到分离。 注意 0.117 仍 < 安全距离 0.6——说明一次推动没完全解决,还需要继续优化(真实 EGO-Swarm 是迭代优化,多轮梯度下降直到惩罚足够小)。这正是软约束的特点:逐步逼近安全,而非一步到位的硬保证。若把推动量调大、或多迭代几轮,间隙会继续增大到安全距离以上。这也呼应了"软约束安全靠调优/收敛"的陷阱——权重和迭代不够,可能留有残余风险。

⚠️ 常见陷阱¶

💡 编程陷阱:不用凸包性质,在 B-spline 曲线上密集采样检测碰撞 - 错误想法:检测轨迹碰撞,就在曲线上多采样些点、逐点查距离。 - 现象 / 后果:慢(采样点两两比较是平方级,还要反复求 B-spline 曲线值);可能漏检(两个采样点之间的曲线段碰撞却没采到);对碰撞求梯度麻烦(要穿过 B-spline 基函数)。 - 根本原因:放弃了 B-spline 的凸包性质——曲线每段落在对应控制点的凸包内,本可以只查控制点凸包。 - 正确做法:用凸包性质,把"曲线碰撞检测"转成"控制点凸包是否分离"(如分离轴检测)。只查几个控制点,既快、又严格(凸包包住整段曲线不漏检)、梯度还能直接对控制点求。

⚠️ 概念陷阱:以为 EGO-Swarm 的碰撞惩罚能严格保证无碰撞 - 错误想法:EGO-Swarm 有碰撞惩罚项,优化后就一定不会撞。 - 现象 / 后果:在惩罚权重不够大、或优化没充分收敛时,出现残余的机间穿插/擦碰,却以为算法保证了安全。 - 根本原因:EGO-Swarm 的碰撞是**软约束(惩罚项)——它把碰撞风险加到目标函数里、靠梯度推开,但**不是硬约束,理论上不保证严格无碰撞(惩罚和其他项权衡,极端情况可能容忍小碰撞换取其他目标)。它靠"惩罚足够大 + 工程调优"实践中保证安全。 - 正确做法:理解 EGO-Swarm 是软约束方法(快、平滑、但安全靠调优而非数学保证);若需要**严格**的无碰撞保证,用硬约束方法——如 §T5.5 的 MADER(分离超平面作硬约束)。两者是"软 vs 硬"的取舍。

🧠 思维陷阱:以为去中心化集群必须依赖可靠通信 - 错误想法:多架无人机要协同避碰,肯定得实时、可靠地共享彼此的精确轨迹,丢一次包就会撞。 - 现象 / 后果:设计出强依赖可靠通信的系统,真实无线环境(丢包、延迟)下频繁失效或不敢飞。 - 根本原因:EGO-Swarm 用**不可靠轨迹共享网络**——允许丢包/延迟:某次没收到邻居最新轨迹,就用上一次的或保守估计继续;再配合深度相机检测邻机纠正定位漂移。系统对通信不苛求,而非强依赖。 - 正确做法:理解鲁棒的集群设计要**容忍通信不完美**(丢包/延迟是常态),用"过期轨迹兜底 + 感知补偿"而非"假设通信完美"。这也是为什么 §T5.5 的 RMADER 要专门处理通信延迟——通信不可靠是真实多机系统必须正视的问题。

练习¶

[实现] 当前 convexGap 只用连心线一个分离轴判断,可能误判(两个凸包实际重叠但连心线轴上分离的情况较少,但理论存在)。请改进:用分离轴定理(SAT)的完整版,测试两个凸包所有边的法向作为候选分离轴,只要存在一个轴上投影分离就判不重叠。
[分析] demo 里推动量是 (safeDist - gap)*0.5。如果两架无人机迎面高速逼近(gap 快速变负),这个推动量会怎样?如果安全距离 safeDist 设得很大,编队会变松还是变紧?体会惩罚权重/安全距离对编队密集度的影响。
[设计] EGO-Swarm 用拓扑轨迹生成逃局部极小。设想两架无人机迎面飞、中间有根柱子:梯度优化可能让两架都想从柱子同一侧绕(陷入次优)。请说明"生成多条不同拓扑(从柱子左/右绕)的候选轨迹"如何帮助找到更好的解(如一架左绕、一架右绕)。

§T5.5 MADER 与 RMADER:分离超平面的硬约束保证 ⭐⭐⭐¶

动机:从"软惩罚"到"硬保证"¶

§T5.4 的 EGO-Swarm 用碰撞惩罚(软约束)——快、平滑,但不严格保证无碰撞。有些场景对安全的要求是**绝对的**:不能"大概率不撞",必须"数学上保证不撞"。比如载人/载货无人机、贵重设备编队、安全攸关任务——一次碰撞的代价无法接受,需要的是**硬保证(hard guarantee):只要算法说安全,就一定不撞。怎么做到硬保证?核心思路是**分离超平面(separating hyperplane):如果能找到一个平面,把无人机 \(i\) 的占据空间完全放在一侧、无人机 \(j\) 完全放在另一侧,那它们**一定不相交**(这是凸集分离定理的直接应用)。把"两条轨迹不碰撞"转化成"存在一个分离平面",再把这个平面作为**硬约束**放进优化——优化必须满足它,从而严格保证无碰撞。 MADER(Multi-Agent and Dynamic EnviRonment trajectory planner)正是这个思路:对每对无人机的每个时间段,引入一个分离超平面作为优化的决策变量和硬约束,数学上保证无碰撞;并用一种比 B-spline 更紧的多项式表示(MINVO)让这个保证更不保守。

如果不这样做会怎样(反面)¶

不用 MADER 的硬约束 + 紧表示,严格安全的多机轨迹会遇到几个问题。 软约束不够安全:EGO-Swarm 的惩罚项(§T5.4)在安全攸关场景不够——惩罚权重和其他目标权衡,极端情况可能容忍小碰撞,无法给出"一定不撞"的保证。 凸包太松导致过度保守:要硬保证不撞,得用轨迹的外包凸集(把曲线包起来)做分离。但 B-spline 的凸包**比曲线本身大很多**(凸包是控制点张成的,曲线只占其中一小部分)——用这么松的外包做分离约束,会把两架本可以靠很近的无人机判成"会撞",导致编队过度保守、过不了窄缝。表示越松,安全保证越保守、越浪费空间。 异步通信下的安全漏洞:多机去中心化时,各自异步地规划、广播轨迹。若 \(i\) 刚改了轨迹还没广播出去,\(j\) 用的还是 \(i\) 的旧轨迹来做分离——两者基于不一致的信息,可能各自"自认为安全"却实际会撞。异步 + 通信延迟会撕开安全保证的漏洞。 MADER 用三招应对:分离超平面作硬约束(严格保证不撞)、MINVO 基(比 B-spline 紧得多的外包,减少保守)、check-recheck 协议(应对异步:优化后再检查一次,若期间收到新轨迹就重新规划)。而 RMADER 进一步处理通信延迟。下面看细节。

历史:MADER 与 MINVO,以及 RMADER¶

MADER 出自 MIT 的 ACL 实验室(Jonathan How 组)。 MADER(Jesus Tordesillas, Jonathan P. How,《MADER: Trajectory Planner in Multi-Agent and Dynamic Environments》,IEEE Transactions on Robotics, 2022,arXiv 2010.11061)是核心:一个 3D 去中心化、异步**的无人机轨迹规划器,能在有静态障碍、动态障碍、其他规划 agent 的环境里生成无碰撞轨迹。它的做法:对每对轨迹区间做**外包多面体(outer polyhedral)**表示,然后**把分离这对多面体的平面作为优化的决策变量;并用 check-recheck 方案**保证对其他 agent 的安全。 **MINVO basis(Jesus Tordesillas, Jonathan P. How,《MINVO Basis: Finding Simplexes with Minimum Volume Enclosing Polynomial Curves》,Computer-Aided Design,DOI 10.1016/j.cad.2022.103341)是 MADER 的关键工具:它找一个**最小体积的单纯形(simplex)来外包多项式曲线**。论文给出了惊人的数字——用 MINVO 基得到的外包体积,比常用的 Bernstein 基小 2.36 倍、比 B-spline 基小 254.9 倍!这意味着 MADER 的分离约束**紧得多**(外包贴合曲线),保守性大幅降低——两架无人机能安全地靠得更近,编队更紧凑、能过更窄的缝。 RMADER(Robust MADER)(Kota Kondo, Jesus Tordesillas, Reinaldo Figueroa, Juan Rached, Joseph Merkel, Parker C. Lusk, Jonathan P. How,《Robust MADER: Decentralized and Asynchronous Multiagent Trajectory Planner Robust to Communication Delay》,ICRA 2023,pp.1687-1693,DOI 10.1109/ICRA48891.2023.10161244,arXiv 2303.06222)解决通信延迟问题:原版 MADER 假设通信无延迟,但真实环境通信会延迟,可能破坏安全。 RMADER 用三招——(1)Delay Check 步(在提交新轨迹前,检查通信延迟期间是否有冲突)、(2)两步轨迹发布方案、(3)轨迹存储与检查方法——证明了在异步去中心化、有通信延迟的情况下的**递归可行性(recursive feasibility),实现 **100% 无碰撞成功率(而此前最好的异步去中心化方法做不到)。 RMADER 基于 convex MADER,后端用 Gurobi 求解,代码开源在 mit-acl/rmader。

理论:MINVO 外包 + 分离超平面 + check-recheck¶

第一步:用 MINVO 单纯形外包轨迹段。 和 EGO-Swarm 用 B-spline 凸包类似,MADER 也要把轨迹段"包起来"做碰撞判断。但 MADER 用 MINVO 基:对每段多项式轨迹,找一个**体积最小的单纯形(2D 是三角形、3D 是四面体)**把它完全包住。为什么追求"最小体积"?因为外包越紧(越贴合曲线),用它做分离约束就越不保守——两架无人机的外包能安全地靠得更近。对比:B-spline 凸包是控制点张成的,往往比曲线大很多(松);MINVO 单纯形是"最小体积外包",紧得多——**比 B-spline 小 254.9 倍**的体积差距,意味着 MADER 能让无人机贴得更近而仍保证安全。这是 MADER 相比 EGO-Swarm 在"紧凑性"上的关键优势。

第二步:分离超平面作为决策变量(硬约束)。 这是 MADER 的核心创新。考虑无人机 \(i\) 和 \(j\) 在某时间段,各自的轨迹段被 MINVO 单纯形外包(一组顶点)。 凸集分离定理:两个不相交的凸集,一定存在一个超平面把它们分开。 MADER 把这个**分离超平面 \(n^\top x = d\) 的参数 \((n, d)\) 作为优化的决策变量**,并加入硬约束(设 \(V_i\) 是无人机 \(i\) 的 MINVO 顶点集、\(V_j\) 是 \(j\) 的):

\[ \begin{cases} n^\top v \le d - 1, & \forall\, v \in V_i \\ n^\top v \ge d + 1, & \forall\, v \in V_j \end{cases} \]

直观:这组约束要求无人机 \(i\) 的所有 MINVO 顶点(\(v \in V_i\))在超平面一侧(\(n^\top v \le d-1\))、无人机 \(j\) 的所有顶点(\(v \in V_j\))在另一侧(\(n^\top v \ge d+1\))。只要优化能找到满足这组约束的 \((n,d)\) 和轨迹,就**数学上保证**两段轨迹被超平面分开、一定不相交(硬保证)。关键:超平面 \((n,d)\) 和轨迹控制点**一起被优化**——优化器自动找"既让轨迹尽量好(平滑/快/贴目标)、又能被某个超平面分开(不撞)"的解。这把"避碰"从软惩罚升级成了优化必须满足的硬约束。 (注:这是非凸问题——超平面和轨迹耦合。原版 MADER 用非凸求解;后续 convex MADER/RMADER 做了凸化处理,用 Gurobi 求解。)

第三步:check-recheck 协议(应对异步)。 去中心化异步规划有个安全隐患:\(i\) 优化自己的轨迹时,用的是"当前已知的其他 agent 轨迹";但在 \(i\) 优化的这段时间里,别的 agent 可能也改了轨迹并广播了——\(i\) 优化完成时,它依据的信息可能已过期,新轨迹可能和别人的新轨迹冲突。 MADER 的 check-recheck: check:\(i\) 优化出新轨迹后,先**检查**它和"优化开始时已知的所有 agent 轨迹"无冲突(优化保证了这点)。 recheck:在正式提交(广播)这条新轨迹前,再检查一次——看优化期间是否收到了其他 agent 的新轨迹;若收到了且与自己的新轨迹冲突,就**放弃这次结果、用新信息重新规划**。只有通过 recheck(确认和最新信息也无冲突),才提交新轨迹。这个"算完再核对一遍最新情况"的机制,堵住了异步带来的信息过期漏洞。

第四步:RMADER 处理通信延迟。 check-recheck 假设"消息一发就到"(无延迟)。但真实通信有延迟:\(i\) 提交轨迹后,\(j\) 可能要过一会儿才收到——这段延迟里,\(j\) 还在用 \(i\) 的旧轨迹,可能与 \(i\) 的新轨迹冲突。 RMADER 的三招: Delay Check(延迟检查):在提交新轨迹前,额外检查"在最大通信延迟时间内,我的新轨迹会不会和别人冲突"——把延迟期间的不确定性纳入检查。 两步轨迹发布:不是一步切换到新轨迹,而是分两步发布,给通信延迟留出安全过渡。 轨迹存储与检查:存储自己和他人的轨迹历史,在延迟窗口内交叉检查。靠这三招,RMADER 证明了递归可行性(每一步都能找到安全轨迹、且这个性质递归保持)——即使在异步、去中心化、有通信延迟的真实环境里,也能 100% 无碰撞。这是从"理想通信下安全"到"真实通信下也安全"的关键一步。

MADER/RMADER 的定位。 MADER 系列是**连续轨迹、去中心化、硬约束**的多机方法。优点:严格保证无碰撞(分离超平面硬约束);MINVO 外包紧、不保守(可密集编队);RMADER 还能扛通信延迟。代价:计算更重(分离超平面是额外的决策变量,问题更大、非凸/需凸化,用 Gurobi 这类商业求解器);比 EGO-Swarm 的软惩罚慢。所以 MADER 和 EGO-Swarm 的取舍很清晰:要严格安全保证、可接受更重计算 → MADER(硬约束);要极致速度、可接受软约束安全 → EGO-Swarm(惩罚)。这是连续轨迹多机方法里"硬 vs 软"的典型分野,§T5.6 会把它放进完整谱系。

代码实现(为什么 → 正确 → 错误 → 对比)¶

Step 1 — 为什么这样写。 完整的 MADER 是一个带分离超平面决策变量的非凸/凸轨迹优化(用 Gurobi 求解),无法在几十行里完整重现。但它的核心几何——给两架无人机的占据点集,求一个分离超平面,并验证它确实把两者分到两侧(从而硬保证不碰撞)——可以独立演示,这正是理解 MADER"硬约束保证"的关键。下面的 demo:给无人机 \(i\)、\(j\) 各一组占据顶点(模拟 MINVO 外包),求分离平面 \((n,d)\),验证 \(i\) 的所有顶点在一侧、\(j\) 的在另一侧。只用标准库,g++ -std=c++17 即编。

// 最简 MADER 核心:给两架无人机的占据点集(MINVO 顶点),求一个分离超平面
// 演示"分离超平面作硬约束":找 (n,d) 使 i 的点都在一侧、j 的点都在另一侧。纯标准库。
#include <vector>
#include <cmath>
#include <cstdio>
#include <algorithm>

struct Vec { double x, y; };
Vec operator-(Vec a,Vec b){return{a.x-b.x,a.y-b.y};}
Vec operator+(Vec a,Vec b){return{a.x+b.x,a.y+b.y};}
Vec operator*(Vec a,double s){return{a.x*s,a.y*s};}
double dot(Vec a,Vec b){return a.x*b.x+a.y*b.y;}
double len(Vec a){return std::sqrt(dot(a,a));}
Vec norm(Vec a){double l=len(a);return l>1e-9?Vec{a.x/l,a.y/l}:Vec{0,0};}

struct Plane { Vec n; double d; double margin; };
Plane separatingPlane(const std::vector<Vec>& Vi, const std::vector<Vec>& Vj){
  Vec ci{0,0}, cj{0,0};
  for(auto&p:Vi) ci=ci+p;
  ci=ci*(1.0/Vi.size());
  for(auto&p:Vj) cj=cj+p;
  cj=cj*(1.0/Vj.size());
  Vec n = norm(cj - ci);                       // 法向:从 i 指向 j
  double iMax=-1e9, jMin=1e9;                   // i 在 n 上投影最大值、j 的最小值
  for(auto&p:Vi) iMax=std::max(iMax, dot(p,n));
  for(auto&p:Vj) jMin=std::min(jMin, dot(p,n));
  Plane pl; pl.n=n;
  pl.d=(iMax+jMin)/2.0;                         // 平面放在间隙正中
  pl.margin=jMin-iMax;                          // 间隙(>0 可分离)
  return pl;
}

bool verify(const Plane& pl, const std::vector<Vec>& Vi, const std::vector<Vec>& Vj){
  for(auto&p:Vi) if(dot(p,pl.n) > pl.d + 1e-6) return false;   // i 应都 <= d
  for(auto&p:Vj) if(dot(p,pl.n) < pl.d - 1e-6) return false;   // j 应都 >= d
  return true;
}

int main(){
  std::vector<Vec> Vi = {{0,0},{1,0},{1,1},{0,1}};         // i 占据 [0,1]x[0,1]
  std::vector<Vec> Vj = {{2,0.3},{3,0.3},{3,1.3},{2,1.3}}; // j 占据 [2,3]x[0.3,1.3]

  Plane pl = separatingPlane(Vi, Vj);
  std::printf("分离平面: 法向 n=(%.3f,%.3f), d=%.3f, 间隙 margin=%.3f\n",
              pl.n.x, pl.n.y, pl.d, pl.margin);
  if(pl.margin > 0){
    bool ok = verify(pl, Vi, Vj);
    std::printf("验证: i 顶点都在 n·x<=d 侧, j 都在 n·x>=d 侧 → %s\n",
                ok ? "成立 ✓ (硬保证不碰撞)" : "不成立 ✗");
    std::printf("  i 各顶点 n·x: ");
    for(auto&p:Vi) std::printf("%.2f ", dot(p,pl.n));
    std::printf("(都 <= %.2f)\n", pl.d);
    std::printf("  j 各顶点 n·x: ");
    for(auto&p:Vj) std::printf("%.2f ", dot(p,pl.n));
    std::printf("(都 >= %.2f)\n", pl.d);
  }
  return 0;
}
// 运行输出:
//   分离平面: 法向 n=(0.989,0.148), d=1.580, 间隙 margin=0.885
//   验证: i 顶点都在 n·x<=d 侧, j 都在 n·x>=d 侧 → 成立 ✓ (硬保证不碰撞)
//     i 各顶点 n·x: 0.00 0.99 1.14 0.15 (都 <= 1.58)
//     j 各顶点 n·x: 2.02 3.01 3.16 2.17 (都 >= 1.58)

Step 2 — 正确要点。 这份 demo 抓住了 MADER 硬约束保证的本质。其一,分离超平面 = 不碰撞的充分条件:separatingPlane 找到的 \((n,d)\),只要验证通过(i 的所有顶点 \(n^\top x \le d\)、j 的所有 \(n^\top x \ge d\)),就**数学上保证**两个占据集不相交——这是凸集分离定理的直接应用。运行结果:i 的顶点投影都 ≤1.58、j 的都 ≥1.58,被平面 \(d=1.58\) 严格分开,硬保证不撞。其二,margin 量化分离的余量:margin = jMin - iMax = 0.885 > 0 表示两集在法向上投影不重叠、有 0.885 的间隙——可分离。 margin > 0 是"能硬保证不撞"的判据;若 margin ≤ 0,说明这个法向分不开(占据集可能相交),优化需要调整轨迹。其三,这是硬约束、不是惩罚:MADER 把 \((n,d)\) 作为优化的决策变量、把"i 在一侧、j 在另一侧"作为硬约束——优化**必须**满足它才算可行解。这和 EGO-Swarm 的软惩罚(可以违反、只是代价高)本质不同:硬约束下,输出的轨迹**一定**被某平面分开、一定不撞。真实 MADER 在此之上:用 MINVO 单纯形(比这里的方块更紧的外包)、超平面和轨迹联合优化(非凸/凸化)、Gurobi 求解、check-recheck 应对异步。但"分离超平面作硬约束保证不碰"的核心一致。

Step 3 — 一个常见的错误写法。 理解 MADER 时,一个常见的概念错误是**用外包的"中心距离"判断碰撞,而非"分离超平面"**:

// 反例:用两个外包中心的距离判断是否碰撞(而非找分离超平面)
Vec ci = centroid(Vi), cj = centroid(Vj);
if(len(cj - ci) > someThreshold) { /* 判为不碰撞 */ }
// 问题:(1) 中心距离大不代表不碰撞——两个细长的外包,中心离得远,
//           但可能一端交叠(像两把交叉的剪刀);中心距离会漏判这种碰撞。
//       (2) 阈值难定——外包形状/朝向各异,没有一个统一的"安全中心距离"。
//       (3) 不是硬保证——中心距离是个启发式,不像分离超平面那样
//           由凸集分离定理严格保证"不相交"。
//   分离超平面才是正确的:存在分离平面 ⟺ 两凸集不相交(充要),这是数学保证。

这个错误用"中心距离"这个启发式代替了"分离超平面"这个严格判据。中心距离会在细长/特定朝向的外包上漏判碰撞,且无法给出硬保证。

Step 4 — 对比。 正确版(分离超平面)找一个平面把两个占据集分到两侧——存在这样的平面 ⟺ 两凸集不相交(凸集分离定理,充要条件),这是**严格的数学保证**,不漏判、不依赖阈值。错误版(中心距离)用启发式阈值判断——会在细长/交叉的外包上漏判碰撞,阈值还难定,不是硬保证。这个对比是 MADER 硬约束的核心:"不碰撞"的严格判据是"存在分离超平面",不是"中心够远"。把分离超平面作为优化的决策变量和硬约束,才能让优化器产出"数学上保证不撞"的轨迹——这是 MADER 区别于软约束方法(EGO-Swarm)的根本,也是它能用于安全攸关场景的原因。

带数字走一遍¶

把 MADER demo 的输出摊开,能看清"分离超平面如何硬保证不碰撞"。场景:无人机 \(i\) 的占据顶点是方块 [0,1]×[0,1],\(j\) 的是方块 [2,3]×0.3,1.3。输出:分离平面法向 n=(0.989,0.148)、d=1.580、间隙 margin=0.885;验证 \(i\) 的顶点投影都 ≤1.58、\(j\) 的都 ≥1.58,成立。逐个数字读它的含义。 为什么法向是 (0.989,0.148)? 因为它是从 \(i\) 质心(0.5,0.5)指向 \(j\) 质心(2.5,0.8)的单位向量——主要朝 +x(0.989)、略朝 +y(0.148),因为 \(j\) 在 \(i\) 的右侧偏上。这个法向定义了"分离的方向":沿这个方向,\(i\) 在前、\(j\) 在后。 为什么 margin=0.885 > 0? margin = "\(j\) 顶点在法向上的最小投影" − "\(i\) 顶点的最大投影" = 0.885。正值意味着:\(i\) 的占据(投影最大到 jMax 处)和 \(j\) 的占据(投影最小从 jMin 处)之间,有 0.885 的间隙——两者沿法向不重叠,可以被一个平面分开。 margin > 0 是"能硬保证不撞"的判据。 为什么平面 d=1.580? 因为平面放在间隙正中:d = (iMax + jMin)/2。 \(i\) 的顶点投影最大约 1.14、\(j\) 的最小约 2.02,中点 ≈1.58。平面放中间,给两侧都留余量,最稳妥。 为什么验证能"硬保证"? 看输出的两行:\(i\) 的 4 个顶点投影是 0.00/0.99/1.14/0.15,全部 ≤ 1.58;\(j\) 的是 2.02/3.01/3.16/2.17,全部 ≥ 1.58。这意味着平面 \(n^\top x = 1.58\) 把 \(i\) 完全放在一侧、\(j\) 完全放在另一侧——由凸集分离定理,两个占据集**一定不相交**。这不是"大概率不撞",而是数学上的**充要保证**:存在分离平面 ⟺ 不相交。这就是 MADER"硬约束"的含义。 把 \(j\) 的方块左移到 [0.8,1.8]×... 让它和 \(i\) 交叠再跑,你会看到 margin 变负(无法分离)——此时 MADER 的优化会**拒绝**这样的轨迹(硬约束不满足),强制调整轨迹直到能找到分离平面。这正是硬约束的作用:不满足就不是可行解,从根本上杜绝碰撞。

⚠️ 常见陷阱¶

💡 编程陷阱:用外包"中心距离"代替"分离超平面"判碰撞 - 错误想法:两个轨迹外包离得够远就不会撞,用中心距离加个阈值判断就行。 - 现象 / 后果:细长或特定朝向的外包,中心距离很远却可能一端交叠(像交叉的剪刀),中心距离漏判这种碰撞;阈值还难统一确定。 - 根本原因:中心距离是启发式,不等价于"不相交";"两凸集不相交"的严格充要条件是"存在分离超平面"(凸集分离定理)。 - 正确做法:用分离超平面——找 \((n,d)\) 使一个占据集所有点 \(n^\top x \le d\)、另一个所有点 \(n^\top x \ge d\)。存在这样的平面才严格保证不相交。 MADER 正是把这个平面作为优化的决策变量和硬约束。

⚠️ 概念陷阱:以为外包越大越安全 - 错误想法:把轨迹外包(凸包/单纯形)取大一点,留足裕度,更安全。 - 现象 / 后果:外包过大(如 B-spline 凸包很松),把两架本可安全靠近的无人机判成"会撞",编队过度保守、过不了窄缝——安全是"安全"了,但失去了密集协同的能力。 - 根本原因:外包是"轨迹一定在里面"的保证,但外包越大、越保守(越容易误判碰撞)。安全保证的质量不在"外包大",而在"外包紧贴曲线"——紧外包既保证轨迹在内(安全)、又不浪费空间(不保守)。 - 正确做法:追求**最小体积外包**。 MADER 用 MINVO 单纯形(比 B-spline 凸包小 254.9 倍、比 Bernstein 小 2.36 倍),外包紧贴曲线——既硬保证安全、又能让无人机贴得很近、编队密集。紧表示是"既安全又不保守"的关键。

🧠 思维陷阱:以为有了硬约束保证就万事大吉,忽略通信延迟 - 错误想法:MADER 用分离超平面硬约束保证了不碰撞,那多机系统就绝对安全了。 - 现象 / 后果:在真实异步、有通信延迟的环境部署,仍出现碰撞——因为各 agent 基于过期/不一致的他机轨迹做分离,硬约束建立在错误信息上。 - 根本原因:硬约束保证的前提是"各 agent 对彼此轨迹的信息一致且最新"。异步规划 + 通信延迟会破坏这个前提:\(i\) 改了轨迹,\(j\) 还没收到,两者基于不一致信息各自"自认为安全"。再强的硬约束,建立在过期信息上也失效。 - 正确做法:理解"算法层的硬保证"还需"系统层的信息一致性"配合。 MADER 用 check-recheck(提交前再核对最新信息)应对异步;RMADER 进一步用 Delay Check + 两步发布 + 轨迹存储应对通信延迟,证明了真实通信下的递归可行性。安全是算法 + 通信协议共同保证的。

练习¶

[实现] 当前 separatingPlane 只用质心连线作法向,可能找不到存在的分离平面(法向选得不好)。请改进:当质心连线法向的 margin ≤ 0 时,尝试其他候选法向(如两点集所有边的法向,即 SAT 的思路),找到使 margin > 0 的那个。
[对比] 把同一对轨迹段,分别用"B-spline 凸包"(较大的外包)和"MINVO 单纯形"(紧外包,可手动缩小顶点模拟)做分离,观察:紧外包下两机能靠多近(margin 仍 > 0)?松外包下呢?体会 MINVO"小 254.9 倍"如何让编队更密。
[设计] RMADER 的 Delay Check:在提交新轨迹前,检查"最大通信延迟时间内会不会和别人冲突"。请设计这个检查:已知最大延迟 \(\Delta\)、自己的新轨迹、他机的当前(可能过期 \(\Delta\))轨迹,如何判断延迟窗口内是否安全?为什么这能堵住"\(j\) 还没收到我的新轨迹"的漏洞?

§T5.6 谱系与选型:三类方法的统一视角 ⭐⭐¶

讲完五个方法,这一节把它们串成一张图——不是罗列,而是揭示它们背后的统一脉络和取舍逻辑,回答"什么场景用什么"。

动机:为什么要建立谱系?¶

CBS、LaCAM、ORCA、EGO-Swarm、MADER——五个方法,看起来各不相同(格子搜索、配置生成、速度空间、轨迹惩罚、分离超平面)。但如果只是孤立地记住五个方法,你会在面对一个新场景时手足无措:该用哪个? 真正的理解,是看清它们**背后的统一脉络**:它们都在解同一个问题(N 个智能体在共享时空中互不冲突),只是**在不同的空间、用不同的冲突消解方式**。建立这个谱系,你才能:面对新场景,根据"离散还是连续、中心还是去中心、要不要硬保证、规模多大",快速定位该用哪类方法;也才能理解这些方法之间的**演进逻辑**(为什么会从 CBS 发展到 LaCAM、从 ORCA 发展到 MADER)。这一节就是把五个方法放进一个统一框架,让它们从"五个孤立的算法"变成"一张有逻辑的地图"。

理论:三类方法的统一框架¶

统一视角:都在解"共享时空无冲突",区别在三个维度。 所有方法都回答同一个问题:N 个智能体如何在共享时空中互不冲突。它们的区别可以归到三个维度: 维度一:在什么空间求解? 离散格子(CBS/LaCAM)、连续速度(ORCA)、连续轨迹(EGO-Swarm/MADER)。 维度二:中心化还是去中心化? 中心化(CBS/LaCAM,一个规划器算所有 agent)、去中心化(ORCA/EGO-Swarm/MADER,每个 agent 自己算)。 维度三:冲突消解的方式? 加约束重搜(CBS/LaCAM)、速度空间切半平面(ORCA)、轨迹优化加惩罚/分离约束(EGO-Swarm 软 / MADER 硬)。

谱系图:从离散到连续的演进。

    问题:N 个智能体在共享时空中互不冲突,总代价尽量优
                         │
    ┌────────────────────┼────────────────────────┐
    │                    │                         │
 离散图搜索           连续速度空间              连续轨迹优化
 (格子路径)           (下一步速度)             (完整轨迹)
    │                    │                         │
 中心化               去中心化                  去中心化
    │                    │                         │
 ┌──┴───┐               ORCA              ┌────────┴────────┐
 CBS   LaCAM         (半平面,            EGO-Swarm        MADER
(最优,  (规模,        各担一半,           (软约束:          (硬约束:
 几百)  10000)        O(n)无通信)         碰撞惩罚梯度)     分离超平面)
    │      │                                  │                │
 冲突树  惰性后继                          B-spline凸包      MINVO单纯形
 +约束   +PIBT                             +惩罚推开        +分离平面硬约束
                                                          RMADER(+通信延迟鲁棒)

  消解冲突的方式:
  CBS/LaCAM:  在离散空间"加约束、重新搜索"
  ORCA:       在速度空间"切一个半平面,各担一半责任"
  EGO-Swarm:  在轨迹空间"加碰撞惩罚,梯度推开"(软)
  MADER:      在轨迹空间"加分离超平面,硬约束保证"(硬)

逐对比较,看清演进逻辑。 CBS → LaCAM(同是离散,最优 → 规模):都在格子上搜路径。 CBS 保证最优但规模受限(几百 agent);LaCAM 放弃最优、用惰性后继生成 + PIBT,把规模推到 10000。演进逻辑:当最优性的代价(规模受限)无法接受时,牺牲最优换规模。 离散 → ORCA(中心规划路径 → 去中心实时避速):CBS/LaCAM 中心化地规划完整格子路径;ORCA 去中心化地实时算下一步速度。演进逻辑:从"提前规划好的离散路径"转向"连续空间实时反应式避碰",适应动态、无格子、要去中心的场景。 ORCA → EGO-Swarm(速度 → 完整轨迹):ORCA 只给下一步速度,无完整轨迹;EGO-Swarm 优化完整的平滑轨迹(无人机能直接飞)。演进逻辑:无人机高速飞行需要前瞻的完整轨迹(保证平滑、动力学可行),而非只有下一步速度。 EGO-Swarm → MADER(软约束 → 硬约束):都优化连续轨迹。 EGO-Swarm 用碰撞惩罚(软,快但不严格保证);MADER 用分离超平面(硬,严格保证但更重)。演进逻辑:当需要严格的无碰撞保证(安全攸关)时,从软惩罚升级到硬约束。 MADER → RMADER(理想通信 → 真实通信):MADER 假设通信无延迟;RMADER 处理通信延迟,保证真实环境下的递归可行。演进逻辑:从"理想通信假设"走向"真实通信鲁棒"。这条演进链不是"后者全面优于前者",而是**不同需求催生的不同取舍**——理解了取舍,就理解了为什么有这么多方法。

本质洞察:这五个方法,本质上是**同一个问题在不同抽象层次上的解**——而抽象层次的选择,由"问题离散到什么程度、需要保证到什么程度"决定。把空间从"格子"放松到"连续速度"再到"连续轨迹",换来的是越来越精细的运动表达(格子路径→下一步速度→完整平滑轨迹),代价是越来越难求解(离散搜索→LP→非凸轨迹优化)。把保证从"软"收紧到"硬",换来的是越来越强的安全性(惩罚→分离超平面硬约束),代价是越来越重的计算。 没有一个方法能同时占据"表达最精细、保证最强、计算最轻、规模最大"的四个角——这是一个不可能三角(实际是四角)的权衡。 CBS 占"最优 + 严格(离散)",牺牲规模;LaCAM 占"规模",牺牲最优;ORCA 占"轻 + 去中心",牺牲完整轨迹和全局性;EGO-Swarm 占"轻 + 完整轨迹",牺牲硬保证;MADER 占"硬保证 + 完整轨迹",牺牲计算。看清这个"四角权衡",你就不会再问"哪个最好",而会问"我的场景最在乎哪个角"——这才是选型的正确姿势,也是理解一切工程方法谱系的通用框架:每个方法都是在某个权衡空间里占据一个点,没有全能的银弹。

多视角对照:"消解冲突"这件事,从三个视角看是三种截然不同的操作,但目标一致。 从"约束满足"视角看:CBS/LaCAM 把冲突当成"违反的约束",消解 = 加约束 + 重新求解(满足新约束的解自然无冲突)。这个视角下,多智能体协调是一个**约束满足问题(CSP)——不断加约束直到所有冲突消除。适合离散、可枚举约束的场景。 **从"空间分割"视角看:ORCA 和 MADER 把冲突当成"两个集合的交叠",消解 = 在空间里切一刀分开它们(ORCA 在速度空间切半平面、MADER 在位置空间切分离超平面)。这个视角下,避碰是一个**几何分割问题**——找一个超平面把"我"和"你"分到两侧。适合连续、可用凸几何刻画的场景。 从"势能优化"视角看:EGO-Swarm 把冲突当成"高势能(碰撞风险)",消解 = 沿势能梯度下降(惩罚项的梯度把轨迹推离碰撞)。这个视角下,避碰是一个**连续优化问题**——把碰撞风险编码成代价,梯度下降自然避开。适合连续、要平滑可微的场景。三个视角对应三种数学工具(约束求解 / 凸几何 / 连续优化),各有适用:离散可枚举约束→约束满足(CBS);连续要硬保证→空间分割(MADER);连续要平滑快速→势能优化(EGO-Swarm)。理解了"消解冲突有这三种范式",你看任何多智能体方法,都能一眼归类它属于哪种、为什么这么设计——这比记住具体算法更本质。

选型对照表¶

方法	空间	中心化	冲突消解	最优性	规模	通信	严格无碰撞	适用场景
CBS	离散格子	中心	加约束重搜	最优	几十~几百	中心汇总	是(离散)	中小规模最优 MAPF、需要最优解
LaCAM	离散格子	中心	惰性后继+PIBT	次优(LaCAM*近最优)	~10000	中心汇总	是(离散)	大规模仓储调度、上万 agent
ORCA	连续速度	去中心	半平面各担一半	局部	上千	无需	短期保证	实时局部避碰、人群/机器人、动态
EGO-Swarm	连续轨迹	去中心	碰撞惩罚(软)	局部	数十~上百	不可靠即可	软(靠调优)	多无人机集群、密集飞行、毫秒级
MADER	连续轨迹	去中心	分离超平面(硬)	局部	数十	需要(可异步)	硬保证	安全攸关多机、需严格不撞
RMADER	连续轨迹	去中心	分离超平面+延迟检查	局部	数十	抗延迟	硬保证	真实通信环境、安全攸关

怎么用这张表选型。 先问几个问题,逐步缩小范围: 问题 1:格子化还是连续? 场景能格子化(仓库 AGV 按网格走)→ CBS/LaCAM;必须连续(无人机/机器人在连续空间)→ ORCA/EGO-Swarm/MADER。 问题 2(若离散):要最优还是要规模? 几百 agent、要最优解 → CBS;上千上万 agent、次优可接受 → LaCAM(要近最优用 LaCAM*)。 问题 3(若连续):要完整轨迹还是只要避碰? 只需实时局部避碰(配合全局规划)→ ORCA;要完整平滑轨迹(无人机飞行)→ EGO-Swarm/MADER。 问题 4(若要轨迹):软约束够还是要硬保证? 追求速度、软约束安全可接受 → EGO-Swarm;安全攸关、要严格保证不撞 → MADER;还要抗通信延迟 → RMADER。关键:没有"最好"的方法,只有"最适合场景"的方法。这张表的价值,是把"五个方法的差异"整理成可按场景需求(离散/连续、最优/规模、软/硬、通信)快速定位的决策依据。

一个常见的组合:分层使用。 实践中,这些方法常常**组合**而非单用。一个典型架构是**分层**:用离散 MAPF(CBS/LaCAM)算全局的、粗粒度的无冲突路径(走哪条大致路线),再用连续方法(ORCA/EGO-Swarm)做局部的实时避碰和轨迹平滑。比如仓储:LaCAM 算所有 AGV 的全局路径调度,每台 AGV 用局部避碰处理临时的、计划外的相遇。比如无人机集群:全局规划定大致航线和编队,EGO-Swarm/MADER 做局部的实时轨迹优化和机间避碰。这种"全局离散规划定方向 + 局部连续方法管实时避碰"的分层,结合了两类方法的优点——全局的协调性 + 局部的实时性和连续性。这也是为什么本章要把这些方法都学透:真实系统往往不是用一个,而是组合多个。

退一步看:为什么 MAPF 这么难?¶

理解了这些方法,值得回头看一个根本问题:为什么多智能体路径规划需要这么多专门算法,不能直接套单机方法? 答案是:最优 MAPF 是 NP-hard 的。具体地,"找到使总代价(SIC)或完工时间(makespan)最小的无冲突路径集合"这个优化问题,已被证明是 NP-hard(Yu & LaValle, 2013 等)——这意味着(在 P≠NP 的假设下)不存在多项式时间的最优算法,问题规模一大,最优求解的时间就指数增长。难的根源在**耦合**:每个 agent 的最优路径依赖于其他 agent 怎么走(要避开它们),而其他 agent 的路径又依赖于这个 agent——所有 agent 的决策是耦合的,无法简单地各自独立求解。这种耦合让解空间随 agent 数指数膨胀(回顾 §T5.1 的 |V|^N)。理解了 MAPF 的 NP-hard 本质,本章这些方法的设计逻辑就全通了: CBS 不硬解这个 NP-hard 问题,而是利用"冲突稀疏"的实际结构(大多 agent 不冲突),只在冲突处付出代价——所以它最坏指数(逃不开 NP-hard)、但实际常常很快。 LaCAM 直接放弃最优(NP-hard 的最优解求不起),换多项式级的快速次优解——用 sub-optimal 绕开 NP-hard 的诅咒。 ORCA/EGO-Swarm/MADER 是局部/反应式的,根本不追求全局最优——它们解的是"局部不撞"这个可处理的子问题,把全局协调交给上层或留给涌现。所以本章方法的多样性,本质是**对一个 NP-hard 问题的不同妥协方式**:CBS 妥协规模(保最优、限规模)、LaCAM 妥协最优(保规模、弃最优)、连续方法妥协全局性(保实时局部、弃全局最优)。没有哪个方法能既最优、又大规模、又实时——因为这违背 NP-hard 的本质。看清这一点,你就理解了为什么 MAPF 领域会有这么多方法,以及它们各自在"妥协什么"。

⚠️ 常见陷阱¶

⚠️ 概念陷阱:以为这五个方法是互相替代、只能选一个 - 错误想法:CBS、ORCA、MADER 是竞争关系,做系统时选一个最好的就行。 - 现象 / 后果:在需要"全局调度 + 局部避碰"的系统里,硬用单一方法——用 CBS 它处理不了连续实时避碰,用 ORCA 它没有全局协调,顾此失彼。 - 根本原因:这些方法处于**不同层次**(全局离散规划 vs 局部连续避碰),解决问题的不同方面,很多时候是**互补**而非替代。 - 正确做法:理解它们可以**分层组合**——全局用离散 MAPF(CBS/LaCAM)定大方向,局部用连续方法(ORCA/EGO-Swarm/MADER)管实时避碰。真实系统常常组合多个,而非单选。

🧠 思维陷阱:以为"更新、更复杂"的方法一定更好 - 错误想法:MADER/RMADER 比 CBS 新、比 ORCA 复杂,那它一定更好,应该优先用。 - 现象 / 后果:在简单场景(如纯仓储格子调度)硬上 MADER 的连续轨迹 + 分离超平面 + Gurobi,杀鸡用牛刀,计算开销巨大、还不如 LaCAM 直接。 - 根本原因:方法的"新/复杂"不等于"适合你的场景"。每个方法有它的适用边界:CBS 适合中小规模最优、LaCAM 适合大规模离散、ORCA 适合实时局部避碰、MADER 适合安全攸关连续轨迹。用错场景,再先进的方法也是负担。 - 正确做法:按场景需求选(离散/连续、规模、软/硬、通信),而非按"新不新"。简单场景用简单方法(格子调度用 LaCAM),复杂需求才用复杂方法(安全攸关连续飞行用 MADER)。匹配场景,才是最优选择。

练习¶

[设计] 给三个场景选方法并说明理由:(a) 一个仓库 2000 台 AGV 按预设货架网格调度;(b) 一群送餐机器人在商场连续空间穿行避开行人;(c) 10 架载货无人机在城市楼宇间编队飞行、绝不能撞。
[分析] 本章的方法都在处理"冲突"。请用一句话概括每个方法消解冲突的方式(CBS/LaCAM/ORCA/EGO-Swarm/MADER),并指出哪些是"加约束"、哪些是"加惩罚"、哪些是"切空间"。
[跨章综合] 设计一个"全局 LaCAM + 局部 EGO-Swarm"的分层多无人机系统:LaCAM 在粗格子上给每架无人机算全局路径,EGO-Swarm 沿全局路径做局部轨迹优化和机间避碰。说明两层如何衔接(全局路径如何引导局部优化、局部如何处理全局没预见的临时冲突),以及这种分层结合了两类方法的什么优点。

本章常见误解汇总¶

误解	实情	出处
把 N 个 agent 当一个联合智能体直接 A*	状态空间 \|V\|^N 指数爆炸;CBS 用双层避免联合搜索	§T5.1
CBS 是在联合空间穷举所有 agent 组合	CBS 不联合搜索:高层只搜冲突约束、低层是单 agent 搜索	§T5.1
CBS 低层 A* 只需上下左右四个移动	必须含"原地等待",否则很多需等待消解的冲突解不了	§T5.1
CBS 最坏指数所以一定慢	最坏指数,实际看冲突密度;中小规模冲突稀疏时很快	§T5.1
PIBT 完备、能解所有 MAPF	PIBT 对一般 MAPF 不完备(只在 biconnected 图保证到达);LaCAM 用回溯补完备	§T5.2
只防点冲突(同格)就够了	还要防边冲突(对穿/交换位置),它也是碰撞	§T5.1, §T5.2
LaCAM 惰性是偷懒、会漏解	惰性是计算顺序优化(用到才算)+回溯保证最终穷尽,不漏解	§T5.2
ORCA 让 A 承担全部避碰责任	ORCA 各担一半(半平面过 v_A + ½u),这个 ½ 是互惠的灵魂	§T5.3
ORCA 规划完整路径、保证到达目标	ORCA 是局部反应式避碰,只保证短期不撞,需配合全局规划	§T5.3
去中心化、无通信一定不如中心化	去中心化每 agent 只算自己、无单点瓶颈、鲁棒;合适场景更优	§T5.3
EGO-Swarm 碰撞惩罚能严格保证不撞	惩罚是软约束,不严格保证;严格保证用 MADER 硬约束	§T5.4
在 B-spline 曲线上密集采样检测碰撞	用凸包性质查控制点凸包,更快、更严格、梯度好求	§T5.4
轨迹外包越大越安全	外包越大越保守(误判碰撞);追求最小体外包(MINVO 比 B-spline 小254.9×)	§T5.5
用外包中心距离判碰撞	中心距离会漏判(细长/交叉外包);严格判据是"存在分离超平面"	§T5.5
有了硬约束保证就绝对安全	还需通信信息一致;异步/延迟会破坏前提,需 check-recheck/RMADER	§T5.5
五个方法互相替代、选一个就行	处于不同层次,常分层组合(全局离散+局部连续)	§T5.6
越新越复杂的方法越好	按场景选;简单场景用简单方法,复杂需求才用复杂方法	§T5.6

本章小结¶

本章从"单智能体规划"跨到"多智能体协调",核心是**冲突**:N 个智能体共享时空时,如何互不冲突地一起规划。围绕这个问题,本章展示了三类思路。 离散图搜索(中心化、格子路径):CBS(§T5.1)用双层搜索(高层冲突树加约束、低层单 agent A*)保证最优;LaCAM(§T5.2)用惰性后继生成 + PIBT 把规模推到 10000 agent。 连续速度空间(去中心化、实时避碰):ORCA(§T5.3)用半平面 + 各担一半责任,每 agent 解低维 LP,O(n) 无通信。 连续轨迹优化(去中心化、完整轨迹):EGO-Swarm(§T5.4)把碰撞作软惩罚、梯度推开(快但软);MADER(§T5.5)把分离超平面作硬约束(严格保证不撞),RMADER 进一步抗通信延迟。 §T5.6 把三类方法串成谱系:它们都解"共享时空无冲突",区别在"什么空间、中心还是去中心、怎么消解冲突(加约束/切空间/加惩罚)";没有最好的方法,只有最适合场景的,真实系统常分层组合(全局离散 + 局部连续)。贯穿全章的主线:多智能体的难点是冲突,而冲突的消解,在离散空间靠加约束重搜、在速度空间靠切半平面、在轨迹空间靠加排斥/分离约束——同一个目标,三种范式。

术语速查¶

术语	英文/全称	一句话定义
MAPF	Multi-Agent Path Finding	多智能体在离散图上找无冲突路径
点冲突/边冲突	vertex / edge conflict	同时占同一格 / 交换位置(对穿)
CBS	Conflict-Based Search	双层:高层冲突树加约束、低层单 agent A*
冲突树	Constraint Tree (CT)	CBS 高层搜索的树,节点是约束集合
SIC/SOC	Sum of (Individual) Costs	所有 agent 路径代价之和
LaCAM	Lazy Constraints Addition search	惰性后继生成的大规模 MAPF 算法
配置	configuration	所有 agent 位置的联合向量
PIBT	Priority Inheritance with Backtracking	优先级继承+回溯的一步配置生成器
惰性后继生成	lazy successor generation	按需逐个生成后继,不枚举指数个
ORCA	Optimal Reciprocal Collision Avoidance	速度空间半平面、各担一半责任的避碰
速度障碍	Velocity Obstacle (VO)	会导致碰撞的相对速度集合
互惠	reciprocal	每对 agent 各承担一半避碰责任
凸包性质	convex hull property	B-spline 曲线落在控制点凸包内
MINVO	minimum-volume simplex basis	最小体积外包多项式曲线的单纯形基
分离超平面	separating hyperplane	把两 agent 占据体分到两侧的平面
递归可行性	recursive feasibility	每步都能找到安全解、且性质递归保持

知识点总表¶

知识点	核心结论	关联
MAPF 难点	联合搜索 \|V\|^N 爆炸,需专门算法	§T5.1
CBS 双层	高层冲突树加约束 + 低层单 agent A*,最优	§T5.1
点/边冲突	同格 + 对穿都要防	§T5.1, §T5.2
LaCAM 规模	惰性后继 + PIBT,10000 agent 秒级	§T5.2
PIBT 不完备	只在 biconnected 图保证到达,靠 LaCAM 回溯补	§T5.2
ORCA 半平面	各担一半责任,低维 LP,O(n) 去中心无通信	§T5.3
EGO-Swarm 软约束	碰撞作惩罚、梯度推开,快但不严格保证	§T5.4
MADER 硬约束	分离超平面严格保证不撞,MINVO 紧外包	§T5.5
RMADER 抗延迟	delay-check + 两步发布,真实通信下递归可行	§T5.5
三类方法谱系	离散/速度/轨迹,加约束/切空间/加惩罚	§T5.6

累积项目:多智能体协调器¶

延续 T1-T4 的累积项目(单机规划),本章把它扩展到多智能体——给一组智能体做无冲突协调。

本章新增模块¶

// T 线累积项目:多智能体协调层(承接 T1-T3 单机规划,本章加多机协调)
// 分层架构:全局离散 MAPF 定无冲突路径 + 局部连续避碰管实时

// 1. 统一的"冲突"抽象(贯穿离散和连续)
struct Conflict {
  int agent_i, agent_j;        // 冲突的两个 agent
  enum Type { VERTEX, EDGE, TRAJECTORY } type;  // 点/边/轨迹冲突
  double x, y, t;              // 冲突的时空位置
};

// 2. 全局层:离散 MAPF(CBS 或 LaCAM)
class GlobalMAPF {
 public:
  // 给所有 agent 算无冲突的格子路径
  virtual std::vector<Path> solve(const std::vector<Start>& starts,
                                  const std::vector<Goal>& goals) = 0;
  // 实现:CBS(中小规模最优)或 LaCAM(大规模)
};

// 3. 局部层:连续避碰(ORCA 或 EGO-Swarm/MADER)
class LocalAvoidance {
 public:
  // 给定全局路径 + 邻居状态,算本 agent 的安全速度/轨迹
  virtual Velocity computeVelocity(const Agent& self,
                                   const std::vector<Agent>& neighbors,
                                   const Path& globalPath) = 0;
  // 实现:ORCA(速度,无通信)或 EGO-Swarm/MADER(轨迹)
};

// 4. 分层协调器:全局定方向 + 局部管实时
class HierarchicalCoordinator {
  GlobalMAPF* global_;         // 全局离散规划
  LocalAvoidance* local_;      // 局部连续避碰
 public:
  void coordinate(std::vector<Agent>& agents) {
    auto paths = global_->solve(/*...*/);          // 全局无冲突路径
    for (auto& a : agents)
      a.velocity = local_->computeVelocity(a, neighbors(a), paths[a.id]);
    // 全局路径引导大方向,局部避碰处理临时/计划外的相遇
  }
};
// 关键:T1-T3 是"单机怎么走",本章是"多机怎么互不冲突地一起走"——
//   统一冲突抽象 + 全局离散 MAPF + 局部连续避碰 = 分层多智能体协调

验收标准¶

冲突抽象统一:点冲突、边冲突、轨迹冲突用统一的 Conflict 表达,贯穿离散和连续层。
全局无冲突:全局 MAPF(CBS/LaCAM)产出的格子路径两两无冲突(点冲突 + 边冲突都检测)。
局部实时避碰:局部层(ORCA/EGO-Swarm)能处理全局没预见的临时相遇,实时算安全速度/轨迹。
分层衔接:全局路径引导局部优化(局部沿全局大方向走),局部避碰不破坏全局可达性(不把 agent 带偏到无法到达目标)。
可切换实现:全局层可在 CBS(最优)和 LaCAM(规模)间切换,局部层可在 ORCA(速度)和 EGO-Swarm/MADER(轨迹)间切换,适配不同场景。

评价指标¶

多智能体协调的质量,从几个维度衡量。

维度	指标	含义
最优性	SIC / makespan	所有 agent 路径代价之和 / 最晚到达时间
成功率	求解成功率	多少实例能找到无冲突解(完备性的实际体现)
规模	可处理 agent 数	在时间预算内能协调多少个 agent
安全	碰撞率 / 最小机间距	执行中是否真的无碰撞(硬保证 vs 软约束的差异)
实时性	单次规划/重规划耗时	是否满足实时(尤其去中心化局部避碰)
通信	通信量 / 抗延迟性	需多少通信、能否容忍丢包延迟

不同方法侧重不同指标:CBS 重最优性、LaCAM 重规模、ORCA 重实时性和无通信、EGO-Swarm 重实时性、MADER 重安全(硬保证)、RMADER 重抗延迟。选型就是看你最在乎哪个指标(呼应 §T5.6 的"四角权衡")。

FAQ¶

Q:CBS、LaCAM、ORCA、MADER 这么多,我到底该学哪个、用哪个? 都要理解(它们覆盖不同场景),但用哪个看场景。仓储/格子调度、要最优 → CBS;上万 agent → LaCAM;连续空间实时局部避碰 → ORCA;多无人机集群飞行 → EGO-Swarm(快)或 MADER(严格安全)。 §T5.6 的选型对照表和"四个问题"能帮你快速定位。关键不是记住一个,而是理解它们的取舍,按场景选。

Q:MAPF(CBS/LaCAM)和连续避碰(ORCA/EGO-Swarm)是竞争还是互补? 主要是互补,处于不同层次。 MAPF 是全局、离散、规划完整路径;连续避碰是局部、连续、实时反应。真实系统常**分层组合**:全局用 MAPF 定无冲突的大致路线,局部用连续避碰处理实时的、计划外的相遇。比如仓储用 LaCAM 调度 + 局部避碰兜底,无人机集群用全局航线 + EGO-Swarm 局部优化。

Q:为什么 ORCA 不用通信也能避碰,而 MADER 要通信? 因为它们的"协调机制"不同。 ORCA 靠**互惠假设**:每个 agent 假设"对方也会按 ORCA 躲一半",于是各自算各自的半平面、各担一半,合起来自然不撞——这个假设让协调隐含在算法里,无需显式通信(只需能观测到邻居的位置/速度)。 MADER 要让两机的轨迹被**同一个分离超平面**分开,这需要双方对彼此的轨迹有一致认知,所以要通信(广播轨迹);RMADER 进一步处理通信延迟带来的认知不一致。

Q:"完备"和"最优"到底差在哪?为什么有的算法放弃最优? 完备 = 有解必能找到(无解能报告);最优 = 找到的解代价最小。 CBS 既完备又最优,但代价是规模受限(最坏指数)。 LaCAM 完备但不最优(sub-optimal)——它放弃最优,换来 10000 agent 的规模。原因:大规模场景下,"快速找到一个可行解"比"慢慢找到最优解"实用得多;LaCAM* 用 anytime 机制弥补(先出可行解,再逐步逼近最优)。这是"最优性 vs 规模"的典型取舍。

Q:软约束(EGO-Swarm)和硬约束(MADER)的安全性差多少?实际该用哪个? 软约束(惩罚)不严格保证无碰撞——靠惩罚权重足够大 + 优化充分收敛,实践中很安全,但理论上极端情况可能有残余碰撞。硬约束(分离超平面)数学上严格保证不撞。实际选择:一般的集群飞行(可接受极小残余风险、要极致速度)→ EGO-Swarm;安全攸关(载人/载货/贵重设备,绝不能撞)→ MADER。这是"速度 vs 严格安全"的取舍。

Q:这章和 T1-T4、和后续 Part 什么关系? T1-T3 给单机规划(Frenet/ST、走廊搜索、轨迹优化),T4 给工业架构,本章 T5 给多机协调——把单机算法扩展到多智能体。 CBS 低层用 T2 的 A*/SIPP,EGO-Swarm/MADER 的轨迹后端用 T3 的 B-spline/优化。往后:Part-Multi 会更深入多机器人系统(任务分配、编队、协作 SLAM 等),本章的协调算法是其基础;而这些多机方法最终也要落进 T4 那样的工程架构才能部署。

延伸阅读¶

核心论文:Sharon, Stern, Felner, Sturtevant, Conflict-Based Search for Optimal Multi-Agent Pathfinding, Artificial Intelligence 2015(CBS 奠基);Okumura, LaCAM: Search-Based Algorithm for Quick MAPF, AAAI 2023(大规模 MAPF);van den Berg, Guy, Lin, Manocha, Reciprocal n-Body Collision Avoidance, ISRR 2011(ORCA);Zhou et al., EGO-Swarm, ICRA 2021 与 Swarm of micro flying robots in the wild, Science Robotics 2022(无人机集群);Tordesillas & How, MADER, T-RO 2022 与 Kondo et al., Robust MADER, ICRA 2023(硬约束 + 抗延迟)。

开源实现:whoenig/libMultiRobotPlanning(CBS/SIPP 等的 C++ 模板实现,MIT,适合学 MAPF);Kei18/lacam 系列(LaCAM/LaCAM*/Engineering LaCAM*,作者本人实现);Jiaoyang-Li/EECBS(有界次优 CBS);snape/RVO2(ORCA 的事实标准库,Apache-2.0);ZJU-FAST-Lab/ego-planner-swarm(EGO-Swarm,~1.9k star);mit-acl/mader 与 mit-acl/rmader(MADER/RMADER 官方实现)。

前沿:League of Robot Runners(ICAPS 2024+ 的大规模 MAPF 竞赛,上万 agent);MAPF-GPT(AAAI 2025,用 Transformer 学 MAPF 策略);学习驱动的多智能体规划(PRIMAL 系列等);异构多智能体(地面 + 空中)协调。

与本书其他部分:T2(A*/SIPP)是 CBS 低层基础;T3(B-spline/MINCO/轨迹优化)是 EGO-Swarm/MADER 后端基础;Part-Multi 深入多机器人系统;T4(工业架构)是这些方法落地的工程框架。

本章与后续的关系¶

后续主题	本章提供的基础	衔接点
多机器人任务分配	多机无冲突路径规划	先分配任务(谁去哪),再用 MAPF 算无冲突路径
编队控制	多机协调与避碰	编队是带队形约束的多机协调
协作 SLAM	多机相对定位(EGO-Swarm 的深度检测)	多机协同建图与定位
学习驱动多智能体	经典方法作为基线/专家	MAPF-GPT 等学习方法模仿 LaCAM 等专家
工业部署	协调算法	落进 T4 那样的架构、ROS 多机系统

🔧 故障排查手册¶

现象	可能原因	排查 / 修复
CBS 在本可解的实例上报告无解	低层 A* 漏了"原地等待"动作	状态扩展加入等待 (0,0)(§T5.1 编程陷阱)
CBS 算出的"无冲突"路径执行时撞车	只检测了点冲突,漏了边冲突(对穿)	加边冲突检测(两 agent 交换位置)(§T5.1 练习)
CBS 上千 agent 时内存爆炸/超时	冲突密集,CT 指数膨胀;CBS 规模受限	换 LaCAM(大规模);或减小问题规模(§T5.1 思维陷阱)
PIBT/LaCAM 生成的配置执行时对穿撞车	优先级继承让路时漏防交换	禁止让到请求者当前位置 + 检测所有 agent 对交换(§T5.2 编程陷阱)
PIBT 在窄道对冲时来回震荡、不收敛	PIBT 对一般 MAPF 不完备,无侧让空间会卡	用 LaCAM(外层回溯跳出震荡)(§T5.2 概念陷阱)
ORCA 多机出现震荡(reciprocal dance)	半平面没"各担一半",A 担了全责	半平面过 v_A + ½u(那个 ½)(§T5.3 编程陷阱)
ORCA 的 agent 卡在障碍后到不了目标	ORCA 是局部避碰,会陷局部极小	配合全局路径引导(全局定方向 + ORCA 局部)(§T5.3 概念陷阱)
EGO-Swarm 优化后仍有机间擦碰	碰撞是软约束,惩罚权重不够/没收敛	增大碰撞惩罚权重、多迭代;或换 MADER 硬约束(§T5.4 概念陷阱)
多机轨迹判碰很慢	在曲线上密集采样两两比较	用 B-spline 凸包性质查控制点(§T5.4 编程陷阱)
编队过度保守、过不了窄缝	轨迹外包太松(如 B-spline 凸包)	用更紧的外包(MINVO 比 B-spline 小254.9×)(§T5.5 概念陷阱)
MADER 判分离漏判(细长外包交叉)	用了中心距离而非分离超平面	用分离超平面(凸集分离定理的严格判据)(§T5.5 编程陷阱)
MADER 硬约束保证了,真实飞行仍撞	通信延迟导致各机轨迹认知不一致	用 check-recheck / RMADER 的 delay-check(§T5.5 思维陷阱)

API 速查表¶

以教学示意接口为主,实际库(libMultiRobotPlanning/RVO2/ego-planner-swarm/mader)的命名与签名以各自代码为准。

功能	示意接口	说明
CBS 高层	`CBS::search(starts, goals)`	冲突树 best-first,返回无冲突路径
CBS 低层	`lowLevelAStar(start, goal, constraints)`	单 agent 带约束时空 A*/SIPP
冲突检测	`detectConflict(paths)`	返回第一个点/边冲突
LaCAM	`LaCAM::solve(starts, goals)`	惰性配置搜索,大规模 MAPF
PIBT 一步	`PIBT::step(config)`	给当前配置生成一个无碰撞后继配置
ORCA 半平面	`computeORCALine(agentA, agentB, tau)`	一对 agent 的速度空间半平面约束
ORCA 求解	`linearProgram(prefVel, orcaLines)`	低维 LP:满足半平面+最近偏好速度
EGO 轨迹优化	`EGOPlanner::optimize(initTraj, neighbors)`	B-spline + 碰撞惩罚梯度优化
MADER 规划	`MADER::replan(neighbors_trajs)`	分离超平面硬约束轨迹优化
分离超平面	`separatingPlane(polyA, polyB)`	找分开两凸集的超平面(硬约束)

研究实践建议¶

按目标分三层。

入门(理解算法、能跑开源)。 先选一类深入。想做离散 MAPF,从 whoenig/libMultiRobotPlanning 入手——它有 CBS、SIPP、ECBS 的清晰 C++ 模板实现,对照 §T5.1 的双层结构读代码,跑几个 benchmark(MAPF 有标准测试集,如 Stern et al. 2019 的地图)。想做连续避碰,从 snape/RVO2(ORCA)入手,跑圆周对冲、人群仿真的例子,对照 §T5.3 的半平面理解。想做无人机集群,搭 ZJU-FAST-Lab/ego-planner-swarm 的仿真,看多机在杂乱环境飞行。这一阶段重点是把"理论里的双层/半平面/惩罚"和"代码里的实现"对上号。

进阶(能改进、能对比)。 在开源基础上做有针对性的实验:给 CBS 加冲突优先级/启发式(ICBS/CBSH)看高层加速;对比 CBS 和 EECBS 在同一批实例上的成功率和速度;给 ORCA 调 tau(时间视野)看保守性变化;在 EGO-Swarm 里调碰撞惩罚权重看编队密集度和安全性的权衡。这一阶段重点练两个能力:一是**读懂方法的关键参数如何影响行为**(优先级、tau、惩罚权重),二是**理解不同方法的取舍**(对照 §T5.6 谱系,在同一场景跑不同方法对比)。建议复现一两篇论文的核心实验。

研究(能创新、能融合)。 盯住几个前沿方向:大规模 MAPF(LaCAM 系列、League of Robot Runners 竞赛,如何把规模和质量同时推高);学习驱动的多智能体规划(MAPF-GPT 等,如何用学习突破经典方法的瓶颈、或加速经典方法);异构多智能体协调(地面车 + 无人机,不同动力学/能力的协同);lifelong MAPF(agent 不断有新任务,如何高效在线重规划)。这些方向往往需要融合本章的多类方法(如学习 + 搜索、离散 + 连续),以及结合 T2/T3 的底层算法。研究的起点,常常是在复现中发现现有方法的局限(规模、质量、实时、鲁棒的某个短板),那就是创新的切入点。

版本信息速查¶

项目	版本 / 状态(截至本章撰写)	备注
CBS	AIJ 2015 经典,持续衍生(ICBS/EECBS 等)	libMultiRobotPlanning 有实现
LaCAM / LaCAM*	AAAI 2023 / IJCAI 2023 / AAMAS 2024	大规模 MAPF 主力,作者持续优化
PIBT	AIJ 2022(IJCAI 2019 会议版)	LaCAM 的配置生成器
ORCA / RVO2	ISRR 2011,RVO2 库稳定	速度空间避碰事实标准
EGO-Swarm	ICRA 2021 + Science Robotics 2022	ZJU-FAST-Lab 开源,~1.9k star
MADER / RMADER	T-RO 2022 / ICRA 2023	MIT-ACL 开源,后端 Gurobi
MAPF 测试集	Stern et al. 2019 benchmark	标准地图与实例,用于对比

版本随时间变化,落地前请以各项目官方仓库为准。

前沿工作与开放问题¶

大规模 MAPF 的极限。 LaCAM 系列把规模推到 10000+ agent,League of Robot Runners 竞赛(ICAPS 2024 起)推动着大规模、高密度 MAPF 的工程极限(竞赛实例可达上万 agent、97%+ 密度)。开放问题:如何在上万 agent 时同时保证规模、质量(接近最优)、和在线重规划的实时性?

学习驱动的多智能体规划。 MAPF-GPT(AAAI 2025)用 Transformer 学习 MAPF 策略,从 LaCAM 等专家的解中学;PRIMAL 系列用强化学习 + 模仿学习做去中心化 MAPF。开放问题:学习方法能否突破经典搜索的瓶颈(如超大规模的实时性)、或与搜索融合(学习引导搜索)?目前学习方法在解质量上仍常落后于专门的搜索算法。为什么学习方法目前还难以全面超越搜索?因为 MAPF 的最优性来自系统的搜索(回溯、定界、完备性保证),而神经网络的前向推理是"一次性"的、没有回溯——它学到的是"专家解的模式",在分布内(和训练数据相似的场景)表现好,但遇到训练时没见过的拥堵格局、或需要精巧让步的实例,容易给出次优甚至冲突的解,且不像搜索那样能保证完备。学习方法真正的吸引力在**速度和泛化**:一旦训练好,推理是常数时间(不随冲突密度爆炸)、且能零样本迁移到新地图。所以最有前景的方向可能不是"学习取代搜索",而是"学习加速搜索"——用学习的策略给搜索提供好的初始解、好的分支顺序、好的启发式(让搜索更快收敛),搜索则用回溯兜住学习的错误、保证完备和质量。这又回到了 §T5.2 那个普适范式:好的生成器(这里是学习的策略)+ 回溯兜底(这里是搜索)——学习负责快和泛化,搜索负责全和最优。

离散与连续的统一。 CBS/LaCAM(离散 MAPF)和 EGO-Swarm/MADER(连续轨迹)是两套体系,实践中靠分层拼接。开放问题:能否有一个统一框架,既有离散 MAPF 的全局协调性、又有连续方法的轨迹平滑和动力学可行?

异构与真实约束。 真实多机系统是异构的(地面车 + 无人机,不同速度/动力学/传感)、有真实约束(通信延迟、定位误差、动态环境)。 RMADER 处理了通信延迟,但更一般的异构协调、不确定性下的多机规划仍是开放问题。

开放问题小结。 大规模在线 MAPF 的实时重规划;学习与搜索的有效融合;离散全局规划与连续轨迹优化的统一;异构多智能体在真实约束(通信、定位、动力学)下的协调。这些是把本章内容往研究推进的入口。

拓展练习(综合 / 项目级)¶

[实现 + 实验] 用 whoenig/libMultiRobotPlanning 跑 CBS 和 ECBS,在同一批 MAPF benchmark 实例上对比:成功率、求解时间、解质量(SIC)随 agent 数增加的变化。画出曲线,体会"最优(CBS) vs 有界次优(ECBS)"的取舍。
[实现 + 可视化] 用 snape/RVO2 实现一个多 agent 圆周对冲(N 个 agent 从圆周各点走到对面),可视化轨迹,验证全程无碰撞、观察 agent 如何在中心区域互相避让。再增大 N,观察拥堵时的行为。
[仿真] 搭建 ZJU-FAST-Lab/ego-planner-swarm 仿真,让一组无人机穿过有障碍的环境,观察机间避碰和编队;尝试调整碰撞惩罚权重,看编队密集度和安全性如何变化。
[设计] 设计一个"全局 LaCAM + 局部 ORCA"的仓储 AGV 系统:LaCAM 在货架网格上为所有 AGV 算全局无冲突路径,ORCA 处理执行中临时的、计划外的相遇(如某 AGV 故障停车)。说明两层如何衔接、局部避碰如何不破坏全局调度。
[思考 · 跨 T 线] 把 T1-T5 串起来:T1 解耦/Frenet/ST、T2 走廊/搜索、T3 轨迹优化、T4 工业架构、T5 多机协调。请说明一个"多无人机在城市环境协同配送"的系统会如何用到 T1-T5 的知识:单机轨迹(T3)、单机的时空表示(T1/T2)、多机协调(T5,如 MADER)、整体工程架构(T4),以及哪里体现了"冲突消解"这条 T5 主线。

速记卡:一句话记住每个要点¶

复习时用这几张卡片快速唤起本章核心。

CBS——"冲突树上加约束,逼着让路"。 记忆锚点:双层 + 冲突树。高层维护冲突树,检测到冲突就分裂(对两个 agent 各加一条"此时此地不准来"的约束),低层让被约束的 agent 重新 A* 搜路。一句话:不联合搜索,而是默认各走各的、只在冲突处加约束重搜,best-first 保最优。

LaCAM——"PIBT 快速生成,惰性搜索保完备"。 记忆锚点:惰性后继 + PIBT。在配置空间搜索,每次只用 PIBT 生成一个后继配置(不枚举指数个),外层回溯保完备。一句话:用 PIBT 的速度推进、用惰性 + 回溯把规模推到 10000 agent,牺牲最优换规模。

ORCA——"速度空间切一刀,各担一半责任"。 记忆锚点:半平面 + ½。每对 agent 在速度空间画一个半平面(过 v + ½u),A 担一半、B 担一半,各自解低维 LP 选速度。一句话:去中心化、无通信,靠"互惠各担一半"让所有 agent 实时避碰不震荡。

EGO-Swarm——"碰撞当惩罚,梯度推开"。 记忆锚点:B-spline 凸包 + 软惩罚。轨迹用 B-spline,机间碰撞风险作为优化的惩罚项,梯度把控制点推离邻机。一句话:连续轨迹、去中心、毫秒级,碰撞作软约束、梯度互斥——快但不严格保证。

MADER——"找个平面分两边,硬保证不撞"。 记忆锚点:分离超平面 + MINVO。把分离两机占据体的超平面作为优化的决策变量和硬约束,MINVO 外包紧(比 B-spline 小254.9×)。一句话:连续轨迹、硬约束严格保证不撞,RMADER 还抗通信延迟——安全攸关首选。

把五张卡片连起来:同样是"多智能体不冲突",CBS/LaCAM 在离散格子上加约束重搜,ORCA 在速度空间切半平面各担一半,EGO-Swarm/MADER 在连续轨迹上加惩罚(软)或分离超平面(硬)。记住"离散加约束、速度切半平面、轨迹加排斥/分离"这三种冲突消解范式,就记住了这一章的精髓。

一页纸选型决策树¶

把全章的选型逻辑浓缩成一条可以照着走的决策路径,方便你在真实项目里快速定位该用哪个方法。读这棵决策树时,不必死记每个分支的结论,关键是体会每一问背后在权衡什么——空间的离散还是连续、规模的大还是小、要不要可证明最优、安全是软约束够还是必须硬保证、通信能不能依赖。把这些权衡想清楚,选型自然水到渠成。

第一问,你的智能体在什么空间里活动。如果可以离散成格子(比如仓库里 AGV 沿预定网格走、停靠点是离散的货位),走离散 MAPF 这条线;如果必须在连续空间里运动(无人机在三维空间飞、机器人在开阔地面跑、动力学约束重要),走连续这条线。

走离散这条线,接着问规模和最优性。如果智能体只有几十到一两百个,而且你需要可证明最优的解(比如要严格比较不同调度方案的代价、或论文里要 baseline),用 CBS;如果智能体成百上千甚至上万(大型仓库、自动化港口),最优解既求不起也不必,用 LaCAM,它能秒级出可行解,要更接近最优就用 LaCAM 星号版的 anytime 机制在时间允许内逐步精修。

走连续这条线,先问你需要的是"下一步速度"还是"完整轨迹"。如果有一个全局规划器已经给了大方向、你只需要在执行时做实时的局部避碰(尤其是和行人、和其他不受你控制的智能体),用 ORCA,它去中心化、不需要通信、每个智能体只解一个低维线性规划、毫秒级。如果你要为每个智能体规划未来几秒的完整平滑轨迹(无人机高速飞行必须前瞻、要保证动力学可行),再往下问安全要求。

到了"要完整轨迹"这一层,最后一问是安全保证的强度。如果追求极致速度、能接受"惩罚足够大时实践上很安全但理论不绝对保证",用 EGO-Swarm,它的碰撞惩罚 + 梯度推开又快又能在密集杂乱环境里穿行;如果安全是绝对的、一次碰撞的代价无法接受(载人、载贵重货物、关键任务),用 MADER,它的分离超平面硬约束从数学上保证不撞;如果还要在真实的、有通信延迟的无线环境里部署,用 RMADER,它在 MADER 基础上加了延迟检查,在最大延迟下仍保证递归可行、百分百无碰撞。

最后记住一条贯穿始终的提醒:真实的大型系统很少只用一个方法,而是分层组合。一个常见的成熟架构是,上层用离散 MAPF(CBS 或 LaCAM)算全局的、粗粒度的无冲突路线,下层用连续方法(ORCA 或 EGO-Swarm 或 MADER)沿着全局路线做实时的局部避碰和轨迹平滑。这样既拿到了全局规划的协调性,又拿到了局部方法的实时性和连续性。所以这张决策树不是让你"二选一",而是帮你想清楚每一层该放什么。

结语:你现在站在哪里¶

本章带你从"单个智能体"跨到了"一群智能体"。 T1-T4 让你掌握了单个机器人/车/无人机怎么规划出好的时空轨迹,而本章——N 个智能体如何在共享的时空里互不冲突地一起规划——让你进入了多智能体协调的世界。这是仓储物流、无人机集群、多机器人协作这些最有价值的机器人应用的底层算法。

你现在手里有了三类工具和一张谱系图:离散图搜索(CBS 保最优、LaCAM 上规模)、连续速度避碰(ORCA 去中心无通信)、连续轨迹优化(EGO-Swarm 软约束求快、MADER 硬约束保安全、RMADER 抗延迟)。更重要的是,你理解了它们背后的统一主线——多智能体的核心难点是冲突,而消解冲突有三种范式:离散空间加约束重搜、速度空间切半平面、轨迹空间加排斥/分离约束;也理解了选型的"四角权衡"(表达精细度、安全保证、计算开销、规模无法兼得),以及真实系统常用的分层组合(全局离散 + 局部连续)。

但你也该看清这套方法的**边界**。本章的方法大多假设:智能体是**合作的**(都遵守同一套协调规则,如都跑 ORCA、都广播轨迹),且环境里的"其他智能体"行为是**可预测或可获知的**(知道邻居的位置/速度/轨迹)。但真实世界里,有些智能体是**非合作甚至对抗的**(对方未必让你、甚至会利用你的避让),它们的意图是**不确定的**(你不知道对方下一步要干什么)。合作假设处理不了博弈,可获知假设处理不了不确定。这两个边界,正是后续 Part-G(博弈规划:对抗/非合作的多智能体)和 Part-U(不确定性规划:对方意图不确定)要突破的。

T1-T5 为你建立了从单机到多机的完整时空规划能力:你既懂单个智能体怎么算好轨迹(T1-T3)、怎么工程化部署(T4),也懂多个智能体怎么互不冲突地协调(T5)。带着这套能力和它的边界,你已经准备好去面对更复杂的世界——多智能体不再只是"互相避让的合作者",而可能是"意图不明的博弈对手"。

回头看这一章走过的路,你会发现一条清晰的递进:从把多个智能体当成一个庞然大物去硬搜(行不通),到学会让它们各自规划、只在冲突处协调(CBS),到为了规模而放弃最优、用生成器加回溯的巧思(LaCAM),到走出离散格子、在连续的速度空间里用一个半平面和"各担一半"的默契实现无通信协调(ORCA),再到在连续轨迹空间里用惩罚的梯度柔和地推开(EGO-Swarm)、或用分离超平面硬生生地保证不撞(MADER)。每一步都是对同一个问题的更深理解,也是对"冲突"这个核心难点的一种新的回应方式。当你以后遇到任何"多个个体共享有限资源、要协调而不冲突"的问题——不只是机器人,也可能是网络流量调度、多任务资源分配、甚至社会协作——你都会想起这一章教给你的那几种范式:加约束、切空间、加惩罚,以及那个贯穿始终的智慧——好的生成器负责快,回溯或约束负责对。这,就是多智能体协调留给你的、可以带走的思维财富。