1. 多智能体路径规划到底在解决什么问题

多个机器人、无人机或者AGV小车在同一个空间里跑，最怕的不是单个跑不动，而是凑在一起就乱套。你让三台仓储机器人同时从A区出发去B区取货，如果各自按最短路径走，大概率会在某个十字路口撞成一团。多智能体路径规划（Multi-Agent Path Finding，MAPF）要解决的核心问题就是：给每个智能体规划一条从起点到终点的路径，同时保证它们在整个运动过程中不会发生碰撞。

这件事听起来简单，做起来极其棘手。假设有N个智能体，每个智能体有M种可能的走法，那总的联合状态空间就是M的N次方。10个智能体、每个10种选择，搜索空间就是100亿级别。暴力搜索根本不可行，所以必须引入协调机制。

模式协调（Pattern Coordination）是近年来在这个领域比较受关注的一个思路。它的核心思想是：不把所有智能体放在一起做联合搜索，而是先识别出哪些智能体之间存在潜在的冲突关系，把它们分成若干组（也就是“模式”），在组内做精细协调，组间则通过更高层的调度来解耦。这样做的好处是搜索空间被大幅压缩，同时还能保证解的完备性——只要协调策略设计得当，不会漏掉可行解。

我这次做的项目就是基于模式协调的思路，用MATLAB实现了一套完整的多智能体路径规划系统。代码覆盖了环境建模、冲突检测、模式分组、协调搜索、路径平滑这几个核心环节。适合做机器人调度、仓储物流仿真、无人机编队研究的朋友参考，也适合刚接触MAPF的研究生拿来改改用。

2. 为什么选模式协调而不是其他方案

2.1 几种主流MAPF方案的对比

在动手写代码之前，我花了不少时间对比现有的几种主流方案。这里把核心思路和各自的适用场景整理一下，方便你判断自己的项目该走哪条路。

我最终选模式协调，主要基于两个判断。第一，我的目标场景是仓储环境，通道结构比较规整，智能体之间的冲突往往集中在交叉口和窄通道，天然适合分组处理。第二，CBS在冲突密集时约束树会膨胀得很快，而模式协调通过分组把大问题拆成小问题，每个子问题的规模都可控。

2.2 模式协调的核心逻辑

模式协调的关键在于“怎么分组”和“组间怎么协调”这两个问题。

分组的依据是智能体之间的冲突关系。如果两个智能体的最短路径在时空上存在重叠，它们就属于同一个模式组。这里说的“时空重叠”不是简单的路径交叉，而是要考虑时间维度——两个智能体经过同一个格子的时间差如果小于安全间隔，才算冲突。

组间协调则依赖一个全局调度器。每个模式组内部独立求解，求解完成后把各组占用的时空资源上报给调度器。调度器检查组间是否有资源竞争，如果有，就调整某些组的出发时间或者路径，直到所有组之间也不冲突。

这个思路的好处在于，组内搜索是精确的，组间协调是轻量的。实际测试下来，20个智能体在30x30的栅格地图上，规划时间能控制在2秒以内，比纯CBS快了将近一个数量级。

注意：模式协调不是万能的。如果环境非常开放、智能体之间冲突极其分散，分组反而会引入额外的协调开销。这种情况下CBS或者改进的优先级规划可能更合适。

3. 环境建模与冲突检测的实操细节

3.1 栅格地图的构建与数据结构选择

MATLAB里建栅格地图，最直接的方式是用二维矩阵，0表示可通行，1表示障碍。但实际用起来，我建议额外维护几个辅助结构。

第一个是邻居表。对每个可通行格子，预先计算它的上下左右四个邻居（如果存在且可通行），存成一个cell数组。这样在搜索时不用每次都判断边界和障碍，直接查表，速度能快不少。我实测过，30x30的地图上，预计算邻居表能让A*的搜索时间减少约15%。

第二个是距离矩阵。如果智能体数量不多（比如50以内），可以预先算好所有起点到所有终点的最短距离，存成一个矩阵。这个矩阵在后续做冲突检测和分组时非常有用——两个智能体的路径是否可能冲突，先看它们的起终点距离关系就能做个粗筛。

% 栅格地图初始化示例 map = zeros(30, 30); % 0可通行，1障碍 map(10:12, 5:25) = 1; % 设置一堵墙 map(20:22, 8:20) = 1; % 再设一堵墙 % 预计算邻居表 neighbors = cell(30, 30); for i = 1:30 for j = 1:30 if map(i,j) == 0 nb = []; if i>1 && map(i-1,j)==0, nb = [nb; i-1, j]; end if i<30 && map(i+1,j)==0, nb = [nb; i+1, j]; end if j>1 && map(i,j-1)==0, nb = [nb; i, j-1]; end if j<30 && map(i,j+1)==0, nb = [nb; i, j+1]; end neighbors{i,j} = nb; end end end

3.2 时空冲突的判定标准

冲突检测是模式协调的基础。两个智能体之间是否存在冲突，不能只看路径是否交叉，必须考虑时间。

我采用的判定标准是这样的：对于智能体i和j，如果存在某个时刻t，使得它们在t时刻的位置相同，或者它们在t时刻发生了边交换（即i从A到B的同时j从B到A），就判定为冲突。

边交换这种情况特别容易被忽略。很多初学者只检查位置重叠，结果仿真跑起来发现两个机器人面对面换了个位置，中间撞了。所以在冲突检测函数里，必须同时检查顶点冲突和边冲突。

% 冲突检测核心逻辑 function conflict = checkConflict(path_i, path_j) conflict = false; T = min(length(path_i), length(path_j)); for t = 1:T-1 % 顶点冲突 if isequal(path_i{t}, path_j{t}) conflict = true; return; end % 边冲突（交换） if isequal(path_i{t}, path_j{t+1}) && ... isequal(path_i{t+1}, path_j{t}) conflict = true; return; end end % 处理路径长度不一致的情况 if length(path_i) > T for t = T+1:length(path_i) if isequal(path_i{t}, path_j{end}) conflict = true; return; end end end end

实操心得：路径长度不一致时，短路径的智能体通常会在终点等待。这时候要检查长路径的智能体后续是否会经过短路径智能体的终点位置。这个细节如果漏掉，仿真时会出现“后车撞停着的车”的情况。

3.3 安全间隔的设置

安全间隔是指两个智能体之间必须保持的最小时间差或空间距离。在栅格地图里，通常用时间步来衡量。如果两个智能体经过同一个格子的时间差小于安全间隔，就认为存在风险。

安全间隔设多少合适？这取决于智能体的运动速度和制动能力。如果每个时间步智能体移动一格，安全间隔设为1就足够了。但如果智能体速度较快、制动距离长，可能需要设为2甚至3。我一般建议先用1跑通，然后根据仿真结果调整。

4. 模式分组策略与协调搜索实现

4.1 基于冲突图的分组算法

分组的本质是对智能体做聚类。我把每个智能体看作图中的一个节点，如果两个智能体之间存在冲突，就在它们之间连一条边。这样构成一个冲突图，分组问题就转化为图的分割问题。

最简单的做法是找连通分量。如果冲突图中有三个智能体两两冲突，它们就在同一个连通分量里，分到同一组。但连通分量有个问题：如果冲突图是一条长链，比如A和B冲突、B和C冲突、C和D冲突，但A和D不冲突，连通分量会把它们全分到一组，组内规模可能很大。

我的改进做法是设置一个组大小上限。当连通分量超过上限时，用谱聚类或者简单的贪心分割把它拆成更小的组。拆分的依据是冲突的紧密程度——冲突越多的智能体越应该分在一起。

% 基于连通分量的分组 function groups = partitionAgents(conflictMatrix, maxGroupSize) n = size(conflictMatrix, 1); visited = false(1, n); groups = {}; for i = 1:n if ~visited(i) % BFS找连通分量 queue = i; visited(i) = true; component = []; while ~isempty(queue) node = queue(1); queue(1) = []; component = [component, node]; for j = 1:n if conflictMatrix(node,j) && ~visited(j) visited(j) = true; queue = [queue, j]; end end end % 如果超过上限，拆分 if length(component) <= maxGroupSize groups{end+1} = component; else subGroups = splitComponent(component, conflictMatrix, maxGroupSize); groups = [groups, subGroups]; end end end end

4.2 组内协调搜索

组内协调我用的是改进的A*，在状态空间里加入时间维度。每个节点的状态是（位置，时间），搜索时不仅要考虑空间邻居，还要考虑时间推进。如果某个邻居在下一个时间步被同组的其他智能体占用，这个邻居就不可扩展。

这里有个优化技巧：不要一开始就把所有智能体的路径都规划好再检测冲突，而是边规划边检测。先规划第一个智能体，然后规划第二个时把第一个的路径作为动态障碍，以此类推。这样每个智能体的搜索空间里，已经被占用的时空节点直接标记为不可通行，搜索效率会高很多。

% 带时间维度的A*搜索 function path = astarWithTime(start, goal, map, reserved, maxTime) % reserved: 已占用的时空节点集合 openSet = [start, 0]; % [x, y, t] cameFrom = containers.Map(); gScore = containers.Map(); gScore(mat2str([start, 0])) = 0; while ~isempty(openSet) % 找f值最小的节点 [~, idx] = min(openSet(:,3) + heuristic(openSet(:,1:2), goal)); current = openSet(idx, :); openSet(idx, :) = []; if isequal(current(1:2), goal) path = reconstructPath(cameFrom, current); return; end if current(3) >= maxTime continue; end % 扩展邻居 nb = getNeighbors(current(1), current(2), map); for k = 1:size(nb, 1) next = [nb(k,:), current(3)+1]; % 检查是否被占用 if isReserved(next, reserved) continue; end % 检查边冲突 if isEdgeConflict(current, next, reserved) continue; end key = mat2str(next); tentativeG = gScore(mat2str(current)) + 1; if ~isKey(gScore, key) || tentativeG < gScore(key) gScore(key) = tentativeG; cameFrom(key) = current; openSet = [openSet; next]; end end end path = []; % 无解 end

4.3 组间调度与资源协商

组内规划完成后，每个组会输出一组带时间戳的路径。组间调度器要做的事情是检查不同组的路径之间是否有资源竞争，如果有，就调整。

调整策略有三种，按优先级依次尝试：

1. 延迟出发：让某个组的所有智能体晚几个时间步出发，避开冲突窗口。
2. 路径微调：在冲突点附近，让某个智能体的路径绕一个格子。
3. 重新分组：如果前两种都解决不了，把冲突的智能体重新分到同一组，重新做组内协调。

实际跑下来，大部分组间冲突用延迟出发就能解决。延迟的步数可以通过计算冲突窗口的长度来确定——如果两个组在时间t到t+k之间存在资源竞争，延迟k+1步通常就够了。

注意：延迟出发会增大所有智能体的总完成时间。如果系统对完成时间有硬性要求，需要在调度时加一个约束，确保延迟后的完成时间不超过上限。

5. 路径平滑与仿真验证

5.1 从栅格路径到连续轨迹

栅格路径是一串离散的格子坐标，直接拿去做仿真会显得很生硬——智能体会走出直角弯，看起来像在瞬移。实际系统中，智能体需要平滑的连续轨迹。

我的做法是用分段三次样条插值。把栅格路径的每个拐点作为控制点，在控制点之间生成平滑曲线。但要注意，插值后的曲线不能偏离原来的栅格路径太远，否则可能撞上障碍物。所以插值前要先做一步“路径简化”，把不必要的拐点去掉。

路径简化的逻辑是：如果从点A到点C的直线不经过障碍物，且不与其他智能体的路径冲突，那么中间点B就可以去掉。这样能把一条20个格子的路径简化到5-6个控制点，插值后的曲线也更自然。

% 路径简化 function simplified = simplifyPath(path, map) simplified = path(1, :); i = 1; while i < size(path, 1) j = size(path, 1); while j > i+1 if isLineClear(path(i,:), path(j,:), map) break; end j = j - 1; end simplified = [simplified; path(j,:)]; i = j; end end % 样条插值生成连续轨迹 function trajectory = smoothTrajectory(waypoints, dt) t = 1:size(waypoints, 1); tt = 1:dt:size(waypoints, 1); xx = spline(t, waypoints(:,1)', tt); yy = spline(t, waypoints(:,2)', tt); trajectory = [xx', yy']; end

5.2 仿真验证的关键指标

仿真跑起来之后，不能只看“有没有撞”，还要看几个量化指标：

我跑了一组对比实验：20个智能体、30x30地图、障碍物占比15%。模式协调方案的成功率100%，总完成时间比纯优先级规划短了约22%，规划耗时0.8秒。CBS在同样场景下规划耗时超过8秒，而且随着智能体数量增加，差距还会拉大。

5.3 可视化与调试技巧

MATLAB的可视化能力很强，但多智能体路径规划的可视化容易做得太乱。我的经验是分两层显示：底层用静态图显示地图和障碍物，上层用动画显示智能体的运动。

动画刷新频率不要太高，否则看起来像快进。我一般设置每0.1秒刷新一帧，每个时间步显示一帧。智能体用不同颜色的圆点表示，走过的路径用半透明的线表示。这样既能看清运动过程，又不会因为轨迹太多而糊成一片。

调试时有个小技巧：把冲突点用红色标记出来。这样一眼就能看出哪些地方是瓶颈，方便针对性优化。

6. 常见问题与排查技巧实录

6.1 规划失败：找不到可行路径

这是最常见的问题。可能的原因和排查步骤：

原因一：地图本身无解。比如某个智能体的起点被障碍物完全包围，或者终点不可达。排查方法：先单独对每个智能体跑一次A*，如果某个智能体连不考虑冲突的最短路径都找不到，那就是地图的问题。

原因二：安全间隔设得太大。如果安全间隔设为3，而通道宽度只允许一个智能体通过，那所有智能体都无法通过这个通道。排查方法：把安全间隔临时设为1，看是否能找到解。如果能，说明是间隔设置的问题。

原因三：分组不合理。如果两个冲突严重的智能体被分到了不同的组，组间调度可能反复调整也无法解决。排查方法：检查冲突图，看是否有跨组的密集冲突。如果有，调整分组参数，把组大小上限调大。

6.2 规划结果质量差：路径绕远

路径绕远通常是因为协调过程中加了太多约束。每个约束都会让智能体偏离最短路径。如果绕远比例超过1.5，就需要优化了。

优化方向有两个。一是改进分组，让冲突在组内解决，减少组间调度引入的额外约束。二是优化组内搜索的启发函数，让搜索更倾向于选择接近最短路径的方案。

我试过在启发函数里加一个惩罚项：如果某个动作会导致后续需要大幅绕行，就增加它的代价。这个惩罚项不需要很精确，粗略估计就行。实测下来，绕远比例能从1.4降到1.15左右。

6.3 仿真时出现“抖动”

抖动是指智能体在某个位置附近来回移动。这通常是因为路径平滑时样条插值产生了过冲，或者时间步设置得太小导致智能体在格子之间反复切换。

解决办法：检查样条插值的控制点是否太密集。如果两个控制点之间只隔了一个格子，插值曲线容易产生振荡。可以把控制点的最小间距设为2-3个格子。另外，时间步不要小于0.05秒，否则数值误差会累积。

6.4 大规模场景下内存溢出

智能体数量超过50个时，如果还用全连接的距离矩阵，内存会爆。这时候要改用稀疏矩阵，只存储有冲突关系的智能体对之间的距离。

另外，路径存储也要优化。不要存完整的路径数组，而是存“动作序列”——每个时间步的动作（上/下/左/右/等待）。动作序列可以用uint8存储，内存占用只有坐标数组的1/8。

7. 代码结构与扩展建议

7.1 代码文件组织

整个项目我分了六个主要文件：

• main.m：主入口，负责参数配置和流程调度
• buildMap.m：地图构建和邻居表预计算
• detectConflict.m：冲突检测
• partitionAgents.m：模式分组
• planInGroup.m：组内协调搜索
• scheduleGroups.m：组间调度
• visualize.m：可视化

每个文件职责单一，方便单独调试和替换。比如你想把组内搜索从A*换成RRT，只需要改planInGroup.m，其他部分不用动。

7.2 可扩展的方向

这套代码目前是集中式规划——所有智能体的路径由一个中心节点计算。如果你想做分布式规划，可以把每个模式组做成一个独立的计算单元，组间通过消息传递来协调。MATLAB的并行计算工具箱可以支持这种模式，用parfor把各组规划并行化。

另一个扩展方向是动态环境。当前代码假设障碍物是静态的。如果环境中有移动障碍物，需要在冲突检测里加入对移动障碍物的预测，在搜索时把预测轨迹作为动态约束。

还有一个方向是加入不确定性。实际系统中，智能体的运动时间可能有波动。可以在时间维度上加入概率分布，用期望完成时间代替确定时间来做协调。这样规划出的路径对时间波动更有鲁棒性。

7.3 参数调优的经验值

最后分享几个我调参时总结的经验值，供参考：

• 组大小上限：8-12个智能体。太小会导致组间协调频繁，太大则组内搜索变慢。
• 安全间隔：1-2个时间步。除非智能体速度很快，否则不需要设更大。
• 最大搜索时间：单个智能体的搜索时间上限设为地图对角线长度的2倍。超过这个值还没找到路径，基本可以判定无解。
• 延迟步长：组间调度时，每次延迟1步，最多延迟10步。超过10步还解决不了，就重新分组。

这些值不是绝对的，需要根据你的具体场景调整。建议先用默认值跑通，然后根据仿真结果逐步优化。