先说我最近在折腾的一个调度场景：一张不算大的室内地图，十几台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, t+1)。扩展时还要检查目标节点是否越界、是否碰静态障碍物、是否撞上其他机器人已经占用的时空节点。
我特意把“等待”当成一个正式动作放进来，因为它对死锁消解非常关键。两条机器人相向而行堵在一条窄道里时，如果没有等待动作，搜索过程完全无解；有了等待动作，就多了一个“等对方先过”的选项。
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星搜索会倾向于用绕路替代等待；反过来，如果把等待代价设成0，A星搜索会疯狂原地等待，把路径搞成大量驻留时间，任务时间全耗光。
所以等待动作的代价必须和移动动作保持相对合理的比例。我说一下我的处理：默认情况下移动和等待都算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、t+1时刻在B，另一个机器人t时刻在B、t+1时刻在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(i+1,1), path(i+1,2), path(i,3)) = true; end后续机器人在扩展子节点时，只要查两眼：目标时空节点是否被占、本时间步要经过的有向边是否被反向占用。这两查做完，整组机器人就是时空一致、无碰撞的。
我建议所有机器人的轨迹都保存三列数组格式：[x, y, t]，后面做可视化分析会非常方便，逐台回放时只需要按t索引坐标点就能画动画。
5. 实测调参：启发权重、等待策略和计算效率的取舍
5.1 启发权重对结果的影响
我在30×30的仿真地图里实验了不同启发权重，结论用一张对比表说明（表中数据为多次运行的代表性结果，单位是时间步）：
| 启发权重 | 平均路径耗时 | 总规划耗时 | 备注 |
|---|---|---|---|
| 1.0 | 126 | 4.8s | 严格最优，节点扩展最多 |
| 1.2 | 128 | 2.1s | 路径略差，速度明显提升 |
| 1.3 | 129 | 1.4s | 我实际项目选用的档位 |
| 1.5 | 133 | 0.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)，很多以前需要专门处理多机冲突的奇技淫巧都可以统一归并成搜索空间的扩展问题。最后再分享一个小技巧：把每台机器人的时空轨迹按时间对齐画成三维图，用颜色区分不同机器人，一眼就能看出哪段时间哪块区域发生了资源争抢，排查调度问题的效率会高很多。
