先说我最近在折腾的一个调度场景一张不算大的室内地图十几台AGV小车各自有装卸任务有的限定了最晚到达时间有的是固定时间窗内必须经过某个工位。一开始我直接用经典A星逐台规划单看每一台的路径都挺漂亮放到一起就开始闹矛盾你堵我我堵你车队绕圈、原地等死锁、任务超时轮着来。后来我把思路换成了基于时空A星算法Space-Time A*来求解带时间约束的多机器人路径规划问题才算是找到了一个能跑通的办法。这篇文章就把我的完整思考路径和Matlab实现框架写出来为什么单机A星放到多机器人场景会失灵时空A星到底改了什么多机器人之间怎么共享时空资源代码层面怎么落地以及我调试时碰到的那些坑。对正在做多AGV调度、仓储机器人路径规划或者刚接触多智能体路径规划MAPF的朋友应该能省不少试错时间。1. 多机器人路径规划的问题本质为什么普通A星会翻车1.1 单机A星很成熟多机为什么立刻失灵经典A星解决的是“单智能体在静态地图上的最短路径”问题。它假设地图上除了静态障碍物之外其他区域随时都可以走。这个假设在单机场景下没问题但一旦变成多机器人共享同一张地图情况就变了一台机器人的路径规划结果会变成另一台机器人眼里的“移动障碍物”。我最早踩的坑就是这样用经典A星给三台小车分别规划路径第一台的路径最短第二台也最短第三台也最短三条路径叠在一起结果三个机器人堵在同一个十字路口。本质原因是每个机器人都在贪心追求自己的最优解而全局冲突根本没有进入任何一台的搜索空间。在带时间约束的场景里这个问题更严重。任务有释放时间、有截止时间如果规划时只考虑空间最短路径完全不考虑“什么时间到什么位置”那么哪怕地图上有一条理论可行的路真正调度时也会因为两台车同时经过同一个瓶颈节点而被迫等待等待时间一长后面的deadline全面超时。1.2 时间维度的引入把二维地图变成三维时空网格时空A星的核心改动一句话就能说清楚把规划空间从“二维地图”扩展成“二维地图 时间轴”的三维时空网格。每一个搜索状态不再是单纯的(x, y)坐标而是(x, y, t)三元组表示“机器人在时刻t位于坐标(x, y)”。搜索的目标也不再是“找到一条从起点到终点的几何路径”而是“找到一条在时间轴上自洽、不与任何其他机器人轨迹冲突的时空轨迹”。这样做的好处是所有机器人之间的动态冲突包括对向迎面、十字交汇、通道追尾都可以被统一表达为“在某个时空节点上的占用冲突”。只要保证任意两个机器人不会出现在同一个(x, y, t)节点上也不会在同一时间步内经过同一条边的正反两个方向碰撞就天然被禁止了。我用一个生活化的类比来理解这件事单机A星就像一个人在完全没人的商场里找最短路线时空A星就像在早高峰的地铁站里找人不仅要考虑路怎么走还要考虑每一秒钟站台和闸机口有没有被别人占住。1.3 时间步长的设定看起来简单错了全盘皆乱引入时间轴之后第一个要决定的问题就是时间步长。这一步很多人会忽略但它直接决定整个搜索空间的大小和路径真实性。我的做法是把地图网格尺寸和时间步长绑定假设地图每个网格边长是1米机器人空载速度是1米/秒那么一个时间步就对应1秒每一步移动正好从一个网格中心走到相邻网格中心。这样“速度”这个物理量就不需要额外建模了运动学关系隐含在时空网格的粒度里。如果网格边长是1米但机器人速度是2米/秒那就需要设置成每个时间步走2格或者缩小时间步到0.5秒。我实测下来的经验是尽量保持“1时间步 1网格边距”的口径否则子节点扩展逻辑会变得很别扭而且多机器人之间判断“同一时间步是否擦肩而过”的时候也容易出错。时间步长本质上决定的是搜索的“时间分辨率”。时间步设得太大两个机器人可能正好在你追我赶时被判定为不冲突实际却贴脸撞上设得太小搜索空间按时间轴膨胀计算量会成倍增加。2. 时空A星的核心原理拆解状态、扩展、代价与启发2.1 时空节点与动作集合的重新定义在时空A星里每个搜索状态是(x, y, t)。起点状态是(startX, startY, releaseTime)终点不是一个坐标点而是一个“时空区域”t在某个时间窗口内且坐标等于(goalX, goalY)。从当前状态(x, y, t)扩展子节点时可选动作比经典A星多了一个“等待”。具体来说每个非终点状态有5种合法动作向上、下、左、右四个方向移动到相邻网格耗时1个时间步原地等待1个时间步位置不变。每种动作都产生一个新的时空状态(x_new, y_new, t1)。扩展时还要检查目标节点是否越界、是否碰静态障碍物、是否撞上其他机器人已经占用的时空节点。我特意把“等待”当成一个正式动作放进来因为它对死锁消解非常关键。两条机器人相向而行堵在一条窄道里时如果没有等待动作搜索过程完全无解有了等待动作就多了一个“等对方先过”的选项。2.2 代价函数设计时间成本就是g值终点约束变成剪枝条件在经典A星里g(n)通常表示从起点到当前节点走过的路径长度。在时空A星里由于每次动作包括移动和等待都消耗1个时间步所以g(n)直接取“当前节点的到达时刻t”就是最自然的做法。这样一来评价函数变成f(n) g(n) h(n) t h(n)其中t是当前状态对应的时刻h(n)是启发式估计的剩余时间成本。带时间约束的任务在这里处理起来非常顺如果某个任务的截止时间是T_deadline那么在扩展节点时只要t h(n) T_deadline说明这个节点即使沿着理论最优方向继续走也不可能按时到达终点直接剪枝不再入堆。这就是“时间约束”进入搜索过程的入口。它不是规划完之后再去校验而是作为剪枝条件嵌入到A星搜索的内部逻辑里效果上比先规划再校验要高效得多。2.3 启发函数怎么选曼哈顿距离和时间修正时空A星里最常见的启发函数就是曼哈顿距离h(n) |x - goalX| |y - goalY|因为每个时间步最多只能横向或纵向移动一格所以剩余的“最少时间步数”不可能少于曼哈顿距离这个启发函数满足一致性条件不会高估代价用起来是安全的。但我在实际项目里发现纯曼哈顿距离在多机器人环境里会偏乐观因为动态冲突的存在机器人经常需要绕行或等待实际花费的时间比曼哈顿距离大不少。于是我给启发函数加了一个权重系数w变成h(n) w × 曼哈顿距离w取1.0时是严格最优求解保证找到的是时空代价最小的路径w取1.2到1.5时是次优加速求解路径质量略降但搜索速度能快好几倍。我用权重1.3的时候在100×100的地图上规划10台机器人求解效率提升非常明显路径总耗时只多了2%左右完全够用。2.4 为什么wait动作必须参与代价计算有一个细节很多人会栽跟头如果只允许等待但等待的代价和移动的代价相同A星搜索会倾向于用绕路替代等待反过来如果把等待代价设成0A星搜索会疯狂原地等待把路径搞成大量驻留时间任务时间全耗光。所以等待动作的代价必须和移动动作保持相对合理的比例。我说一下我的处理默认情况下移动和等待都算1个时间步这样时间轴连续且均衡。如果某些场景希望机器人在交换区多等待、少绕路就把等待代价设为0.5到0.8但一定要配合deadline剪枝否则搜索会往“原地等待最优”的方向跑偏。如果你能接受一点次优解我建议把等待代价设成略低于移动代价并配合deadline上限这种组合在窄道多的地图里能明显减少绕路。注意这只是权衡型调整不是严格最优解。3. 多机器人协同与冲突消解时空占位表的设计与用法3.1 常用求解策略逐台规划 时空占位表共享时空A星解决的是“单台机器人在考虑动态障碍物后的路径规划”问题多机器人协同还需要外层策略来统筹。我用的方案是最经典的“逐台规划 时空占位表共享”。思路是这样的把所有机器人按某种顺序排好优先级第一台机器人用时空A星规划得到一条时空轨迹把轨迹经过的每个(x, y, t)节点以及每条边的占用信息写进一张共享的时空占位表第二台机器人规划时时空A星的冲突检查函数直接查这张占位表凡是已经被占用的时空节点都视为动态障碍物依次类推等所有机器人都规划完全局就得到一组互相无冲突的时空轨迹。这个方案的工程实现非常简单而且天然把“时间约束”带进了占位表后续机器人只会避开“特定时刻被占用的位置”而不是把整个格子永久禁行空间利用率比单纯给每个机器人划分禁区的做法高得多。3.2 顶点冲突与边交换冲突的检测方法在使用时空占位表时冲突检测要分两种看漏掉任何一种都会出问题第一种是顶点冲突也就是两台机器人同一时刻出现在同一个网格节点上。这个最直观检查后续计算出的时空节点(x, y, t)是否已经被占位表中的相同(x, y, t)占用即可。第二种是边交换冲突两台机器人在同一时间步内相向而行在一条边的两个端点上互换位置。从顶点看一个机器人t时刻在A、t1时刻在B另一个机器人t时刻在B、t1时刻在A两者不共享任何时空节点但如果只看顶点占用这个交换动作会在边的中间“头对头撞上”。我在第一版实现里就只查了顶点占用结果仿真动画里两车在窄道正中间擦肩而过模型里完全没报冲突。后来我在占位表里额外维护了一张“边占用表”记录每条有向边在某时间步是否被使用检查时同时看正向和反向才算真正把冲突堵死。3.3 优先级排序的经验按deadline还是按任务长度外层规划顺序对整个系统的质量影响很大。最早我用固定编号顺序后来发现任务紧急性完全错位一辆deadline很紧的车排在后面规划前面宽松的车先占了路后面那台就算绕路也没法按时到达。我的经验是优先级排序要综合考虑两个维度deadline紧迫度剩余可用时间越少优先级越高先规划任务路径长度路径越长后规划时越容易被别人堵住也应该适当优先。我采用的排序规则是把两者加权打分score deadline余量比例 预估路径长度比例越小越靠前。在项目里用不了多少计算量但整体任务按时完成率提升很明显。4. Matlab实现从地图建模到核心函数的最小可运行框架4.1 数据结构与初始化我用的Matlab实现整体分4个模块地图建模、任务定义、时空A星规划器、轨迹回放可视化。先说数据结构的定义。地图用一个二维0/1矩阵map表示1代表可通行0代表障碍物。机器人任务用一个结构体数组存储每个结构体包含起点坐标、终点坐标、释放时间、截止时间。% 地图与任务定义示例基于常见实践整理的最小版本 map ones(30, 30); % 30x30空地 map(10:15, 10:15) 0; % 中间放一块矩形障碍 robots(1).start [2, 2]; % 起点 [x, y] robots(1).goal [28, 28]; robots(1).releaseTime 0; robots(1).deadline 80; robots(2).start [28, 2]; robots(2).goal [2, 28]; robots(2).releaseTime 0; robots(2).deadline 90; % ... 更多机器人定义时空占位表我用一个三维逻辑数组occMap来维护维度是[nx, ny, T_max]T_max是所有任务允许的最大时间上限。这个数组在逐台规划过程中不断积累占用信息。4.2 时空A星主循环的Matlab化实现核心搜索过程我按经典A星框架改写过。这里给出最核心的框架代码展示状态扩展、代价计算、冲突检查和deadline剪枝是怎么织到一起的function path SpaceTimeAStar(map, occMap, start, goal, tStart, tDeadline) % 状态用 [x, y, t] 表示openList 用结构数组sort实现 openList(1).x start(1); openList(1).y start(2); openList(1).t tStart; openList(1).g tStart; openList(1).h abs(start(1)-goal(1)) abs(start(2)-goal(2)); openList(1).f openList(1).g 1.3 * openList(1).h; openList(1).parent 0; closedMap containers.Map(); % 记录已扩展的时空节点 idx 1; while ~isempty(openList) [~, minIdx] min([openList.f]); current openList(minIdx); openList(minIdx) []; % 到达终点且满足时间约束 if current.x goal(1) current.y goal(2) current.t tDeadline % 回溯路径 path reconstructPath(current, openList); return; end % 剪枝理论最优也无法按时到达 if current.g abs(current.x-goal(1)) abs(current.y-goal(2)) tDeadline continue; end % 扩展5个动作上下左右 等待 actions [1 0; -1 0; 0 1; 0 -1; 0 0]; for k 1:5 nx current.x actions(k,1); ny current.y actions(k,2); nt current.t 1; % 越界或静态障碍 if nx 1 || nx size(map,1) || ny 1 || ny size(map,2) continue; end if map(nx, ny) 0 continue; end % 时空占用冲突 if occMap(nx, ny, nt) continue; end % 边交换冲突需要在外部逻辑中一并检查 if isEdgeConflict(occMap, current.x, current.y, nx, ny, current.t) continue; end % 构造新节点并计算代价值 newIdx idx 1; openList(newIdx).x nx; openList(newIdx).y ny; openList(newIdx).t nt; openList(newIdx).g nt; openList(newIdx).h abs(nx-goal(1)) abs(ny-goal(2)); openList(newIdx).f openList(newIdx).g 1.3 * openList(newIdx).h; openList(newIdx).parent minIdx; idx newIdx; end end path []; % 无解 end这段代码我用的是结构数组加sort来实现openList排序对于几百个节点的搜索完全够用。如果你规划的地图在200×200以上建议换成最小堆或者用Matlab的PriorityQueue思路否则sort的O(N log N)在大规模搜索时会成为瓶颈。4.3 时空占用写入与后续机器人的避让流程每台机器人规划完成后立刻把轨迹写入occMap作为后续机器人的动态障碍物for i 1:size(path,1) - 1 occMap(path(i,1), path(i,2), path(i,3)) true; % 同时记录有向边占用用于边交换冲突检测 edgeMap(path(i,1), path(i,2), path(i1,1), path(i1,2), path(i,3)) true; end后续机器人在扩展子节点时只要查两眼目标时空节点是否被占、本时间步要经过的有向边是否被反向占用。这两查做完整组机器人就是时空一致、无碰撞的。我建议所有机器人的轨迹都保存三列数组格式[x, y, t]后面做可视化分析会非常方便逐台回放时只需要按t索引坐标点就能画动画。5. 实测调参启发权重、等待策略和计算效率的取舍5.1 启发权重对结果的影响我在30×30的仿真地图里实验了不同启发权重结论用一张对比表说明表中数据为多次运行的代表性结果单位是时间步启发权重平均路径耗时总规划耗时备注1.01264.8s严格最优节点扩展最多1.21282.1s路径略差速度明显提升1.31291.4s我实际项目选用的档位1.51330.9s速度快但绕路明显变多可以看到在带时间约束的多机器人问题里纯追求最优并不一定划算。因为后续机器人会受到前面机器人轨迹的影响第一台的“最优”很可能把窄道全占掉导致后面几台大绕路全局总耗时反而比微调权重后更大。所以我的经验是权重取1.2到1.3让第一台别把地图资源吃得太死。5.2 等待上限与deadline剪枝的边界条件引入等待动作后另一个要调的是“单次最长等待时间”。如果不设上限两台机器人可能在地图角落互相礼让谁都不先走形成逻辑上的活锁。我采用的策略是给每台机器人设置一个最大等待次数上限比如连续等待不得超过5个时间步。实现上在状态结构体里加一个waitCount字段扩展等待动作时检查当前连续等待次数超过上限就禁止继续等待。这样做在窄道死锁场景里效果很好几台车最多短暂观望几秒钟就会主动放弃等待、寻找绕行路线。deadline剪枝也要留一点余量。我的做法是把真实的截止时间减去2个时间步作为搜索剪枝条件也就是预留安全余量。否则按最理想情况剪枝实际运行中若遇到轻微阻塞任务就会贴着deadline擦线完成风险太大。5.3 运行规模估算地图、机器人数量、时间上限怎么匹配时空A星在工程上最大的顾虑是计算量。状态空间规模大约是“可通行网格数 × 时间上限”所以地图越大、时间上限越长搜索就越慢。我在项目里总结出一个粗略估算经验假设可通行网格数为M任务时间上限为T单次规划扩展的节点数量通常介于0.5MT到1.5MT之间决定具体数字的是地图拥挤程度和启发权重。在30×30地图上运行10台机器人总时间上限100一共规划10次用我的数据结构大概耗时10到20秒。如果要扩大规模我有三条建议一是采用加权A星降低节点数二是把地图按功能分区先做区域级路径规划再做区域内时空细化三是减少重复规划只对受影响的机器人增量重规划而不是每次全量重跑。6. 踩过的坑和后续扩展6.1 几个容易翻车的细节第一个坑是边交换冲突的漏检。这个问题我在前面已经详细说了它在建模阶段就隐藏得非常深仿真动画里甚至会看到两车“穿身而过”而不报错。排查方法很简单给每个机器人画时空轨迹逐时间步检查两两之间是否有同格占用和同边反向占用。第二个坑是时间网格越界问题。occMap的三维数组如果初始化时T_max设小了搜索过程会直接数组越界或者静默出错。我建议在初始化时统一加一个较大的保守上限比如根据所有机器人曼哈顿距离最大值的1.5倍加上总任务数的3倍来估算T_max。第三个坑是start时刻的占用冲突。第一台机器人规划时占用了起点周围的时空节点后续机器人如果起点和它相邻或相同可能一开工就没法扩展。我在规划前会先对相邻起点的机器人做一次初始时间错位用释放时间错开的方式规避。6.2 从离线规划走向动态避障的改造思路时空A星本质上是离线规划工具它假设所有任务信息在规划前是已知的机器人按轨迹执行即可。但真实调度中经常会出现临时加任务、机器人故障、行人闯入等动态事件这时静态的时空轨迹就不够用了。我的后续改进思路是“滚动时域重规划”按固定周期比如每20个时间步重新读取当前所有机器人的实际位置把未执行的轨迹段作废重新搜索新任务的加入也只是在下一个规划周期里自然衔接。因为时空A星本身支持设置任意起点重规划时把当前实际位置作为新的起点、deadline不变就能无缝续跑。如果你做的项目涉及实时性更强的场景还可以在时空A星基础上加入多边形安全距离约束把机器人的实际外形尺寸而不是点模型放进时空网格这部分我已经在另一些实验里验证过效果不错但计算量会再上一个台阶。我的体会是时空A星最大的价值不是某一段代码而是“把时间显式地纳入路径决策”这个思维转变。只要想通了状态从(x, y)变成(x, y, t)很多以前需要专门处理多机冲突的奇技淫巧都可以统一归并成搜索空间的扩展问题。最后再分享一个小技巧把每台机器人的时空轨迹按时间对齐画成三维图用颜色区分不同机器人一眼就能看出哪段时间哪块区域发生了资源争抢排查调度问题的效率会高很多。