1. 多智能体路径规划到底在解决什么问题多个机器人、无人机或者AGV小车在同一个空间里跑最怕的不是单个跑不动而是凑在一起就乱套。你让三台仓储机器人同时从A区出发去B区取货如果各自按最短路径走大概率会在某个十字路口撞成一团。多智能体路径规划Multi-Agent Path FindingMAPF要解决的核心问题就是给每个智能体规划一条从起点到终点的路径同时保证它们在整个运动过程中不会发生碰撞。这件事听起来简单做起来极其棘手。假设有N个智能体每个智能体有M种可能的走法那总的联合状态空间就是M的N次方。10个智能体、每个10种选择搜索空间就是100亿级别。暴力搜索根本不可行所以必须引入协调机制。模式协调Pattern Coordination是近年来在这个领域比较受关注的一个思路。它的核心思想是不把所有智能体放在一起做联合搜索而是先识别出哪些智能体之间存在潜在的冲突关系把它们分成若干组也就是“模式”在组内做精细协调组间则通过更高层的调度来解耦。这样做的好处是搜索空间被大幅压缩同时还能保证解的完备性——只要协调策略设计得当不会漏掉可行解。我这次做的项目就是基于模式协调的思路用MATLAB实现了一套完整的多智能体路径规划系统。代码覆盖了环境建模、冲突检测、模式分组、协调搜索、路径平滑这几个核心环节。适合做机器人调度、仓储物流仿真、无人机编队研究的朋友参考也适合刚接触MAPF的研究生拿来改改用。2. 为什么选模式协调而不是其他方案2.1 几种主流MAPF方案的对比在动手写代码之前我花了不少时间对比现有的几种主流方案。这里把核心思路和各自的适用场景整理一下方便你判断自己的项目该走哪条路。方案核心思路优点缺点适用场景联合A*所有智能体一起做A*搜索最优解状态空间指数爆炸智能体数量≤5优先级规划按优先级逐个规划后来者避让实现简单、速度快不完备可能无解松散耦合场景CBS冲突搜索高层检测冲突底层加约束重规划最优且完备冲突多时约束树膨胀快中等规模、冲突适中模式协调分组协调组间调度搜索空间可控、可扩展分组策略影响解质量大规模、结构化环境强化学习训练策略网络做决策在线响应快训练成本高、泛化难动态环境我最终选模式协调主要基于两个判断。第一我的目标场景是仓储环境通道结构比较规整智能体之间的冲突往往集中在交叉口和窄通道天然适合分组处理。第二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 i1 map(i-1,j)0, nb [nb; i-1, j]; end if i30 map(i1,j)0, nb [nb; i1, j]; end if j1 map(i,j-1)0, nb [nb; i, j-1]; end if j30 map(i,j1)0, nb [nb; i, j1]; end neighbors{i,j} nb; end end end3.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{t1}) ... isequal(path_i{t1}, path_j{t}) conflict true; return; end end % 处理路径长度不一致的情况 if length(path_i) T for t T1: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{end1} component; else subGroups splitComponent(component, conflictMatrix, maxGroupSize); groups [groups, subGroups]; end end end end4.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 []; % 无解 end4.3 组间调度与资源协商组内规划完成后每个组会输出一组带时间戳的路径。组间调度器要做的事情是检查不同组的路径之间是否有资源竞争如果有就调整。调整策略有三种按优先级依次尝试延迟出发让某个组的所有智能体晚几个时间步出发避开冲突窗口。路径微调在冲突点附近让某个智能体的路径绕一个格子。重新分组如果前两种都解决不了把冲突的智能体重新分到同一组重新做组内协调。实际跑下来大部分组间冲突用延迟出发就能解决。延迟的步数可以通过计算冲突窗口的长度来确定——如果两个组在时间t到tk之间存在资源竞争延迟k1步通常就够了。注意延迟出发会增大所有智能体的总完成时间。如果系统对完成时间有硬性要求需要在调度时加一个约束确保延迟后的完成时间不超过上限。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 i1 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]; end5.2 仿真验证的关键指标仿真跑起来之后不能只看“有没有撞”还要看几个量化指标指标含义期望值成功率所有智能体都到达终点的比例100%总完成时间最后一个智能体到达的时间越短越好平均延迟实际路径长度/最短路径长度接近1规划耗时从输入到输出路径的计算时间秒级以内冲突次数规划过程中检测到的冲突总数用于分析瓶颈我跑了一组对比实验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步还解决不了就重新分组。这些值不是绝对的需要根据你的具体场景调整。建议先用默认值跑通然后根据仿真结果逐步优化。