在车间多无人车Multi-AGV/AMR的中央调度系统RCS中时间窗 A* 算法Time-Space A* / Time-Window A*是解决多车路径冲突、死锁和路口博弈的核心底层算法。传统的 A* 算法只在二维物理空间 $(X, Y)$ 寻找最短路径而时间窗 A* 算法巧妙地引入了时间轴 $T$将搜索空间升级为三维的“时空网格Time-Space Grid”。它不仅规划小车“走哪条路”更精确锁定了小车“在什么时间点通过哪个格子”从而在源头上实现了多车路径的完美解耦。一、 时间窗 A* 算法的核心数学原理算法将时间离散化为等步长的时空片Time Slots$t_0, t_1, t_2, \dots$。1. 状态表示State Representation传统 A* 的节点状态为 $N (x, y)$。时间窗 A* 的节点状态升级为$$N (x, y, t)$$这表示在 $t$ 时刻车辆占用了坐标为 $(x, y)$ 的地图网格。2. 代价函数Cost Function系统的启发式评估函数依然保持经典形式$$f(n) g(n) h(n)$$$g(n)$从起点到当前时空节点 $(x, y, t)$ 实际耗费的时间或距离。$h(n)$当前节点到终点目标的预估时空代价通常采用曼哈顿距离或欧氏距离。3. 核心创新点允许“原地等待”的状态转移传统 A* 算法在 $(x, y)$ 节点只能向四周邻居东、西、南、北拓扑转移。而时间窗 A* 在 $(x, y, t)$ 节点允许第 5 种转移行为——原地等待Wait动作 车辆不移动。状态转移 $(x, y, t) \longrightarrow (x, y, t 1)$物理意义 当前方路口有高优先级车辆通过时本车停在原地消耗一个时间片让出路权。二、 核心机制全局预约表Reservation Table时间窗 A* 能够防撞的秘密在于中央控制系统维护的全局预约表Reservation Table。1. 动态预约机制每当系统为一台新车辆成功规划路径后就会将这条路径上的所有时空节点写进预约表车 A 的预约记录$(Grid\_A, t1)$ - 被车A占用$(Grid\_B, t2)$ - 被车A占用$(Grid\_C, t3)$ - 被车A占用2. 冲突碰撞检测强约束拦截当轮到车 B 进行路径规划时它的 A* 算法在拓展邻居节点时必须实时查验预约表强制拦截以下两种冲突冲突类型 1节点冲突 (Vertex Conflict) 车 B 计划在 t2 时驶入 Grid_B。 检查预约表 ── 发现 (Grid_B, t2) 已被车 A 预约 【拦截该节点视为死障碍物车 B 必须绕行或在该时刻原地等待。】 冲突类型 2相向对头/对流冲突 (Edge Conflict) 在 t1 时车 A 在 Grid_1车 B 在 Grid_2。 在 t2 时车 A 计划去 Grid_2车 B 计划去 Grid_1两车在窄道对向换位。 【拦截检查发现边转移交叉该动作直接被算法熔断。】三、 算法运行实战演练十字路口两车博弈场景 这是一个车间十字路口车 A 计划从西向东横穿车 B 计划从南向北横穿。路口中心格子坐标为 $(3, 3)$。优先级 车 A 满载高优先级车 B 空车低优先级。第一步系统先为车 A 规划。车 A 一路畅通其时空轨迹为$(1,3, t_1) \rightarrow (2,3, t_2) \rightarrow \mathbf{(3,3, t_3)} \rightarrow (4,3, t_4)$。系统将这组状态锁死写入全局预约表。第二步系统为车 B 规划。车 B 的标准路线原本也是要在 $t_3$ 时刻进入路口中心 $(3, 3)$。但是当车 B 的 A* 算法搜索到 $(3, 2, t_2)$ 并试图向北拓展 $\mathbf{(3, 3, t_3)}$ 时碰撞检测拦截器触发 因为预约表显示 $(3,3, t_3)$ 属于车 A。第三步车 B 自动执行“原地等待”。车 B 放弃在 $t_3$ 进入路口转而拓展了等待节点$(3, 2, t_2) \longrightarrow (3, 2, t_3)$。在下一个时间片 $t_4$车 B 再次尝试拓展北侧节点 $(3, 3, t_4)$。此时车 A 已经通过路口 $(3,3)$车 A 此时在 $(4,3, t_4)$。冲突解除 车 B 成功预约 $(3, 3, t_4)$。最终效果 在物理画面上车 B 走到路口前精准地踩了一下刹车停下 1 秒钟礼让车 A 顺滑通过后车 B 再起步穿过路口。四、 局限性与工业优化变体虽然时间窗 A* 逻辑完美但在工业落地时存在一个致命死角计算量爆炸。随着车辆总数 $N$ 增加预约表会变得极长后规划的小车要在三维空间里避开前面所有车的时空节点搜索树会严重退化导致算法超时。为了在 50 台小车以上的车间落地业界通常采用以下改良变体1. 冲突搜索算法CBS, Conflict-Based Search不搞绝对的先后顺序。先让所有小车各自用普通 A* 规划出最快路径。然后中央系统检查它们之间在哪里发生了冲突。针对有冲突的两台车单独在那个冲突点建立时空约束Constraint重新局部规划。这种“分层按需冲突优化”是现代 Fleet Management 的核心。2. 滚动窗口时间窗Rolling Horizon Window不一次性规划小车未来 10 分钟的所有时空点因为现场随时会有工人打乱时间轴。中央系统只精细化规划未来 15 秒如 15 个时间片的时空预约。15 秒以外的区域只做粗略的空间路由导航。小车边开系统边滚动刷新时空窗大幅释放了服务器内存与 CPU 压力。用 Python 编写的时间窗 A*Time-Space A*算法核心节点拓展与全局预约表碰撞检测的工业级伪代码框架。该框架完整保留了时间轴 T引入了“原地等待Wait”的动作机制并实现了节点冲突与相向对头冲突的刚性拦截。Python 核心伪代码实现import heapq import math class TimeSpaceNode: def __init__(self, x, y, t, g_cost, h_cost, parentNone): self.x x self.y y self.t t # 时间步 / 时间片 self.g_cost g_cost # 从起点到当前节点的实际代价时间/距离 self.h_cost h_cost # 到终点的启发式预估代价 self.f_cost g_cost h_cost self.parent parent # 用于优先队列Min-Heap根据 f_cost 自动排序 def __lt__(self, other): return self.f_cost other.f_cost property def pos_time(self): return (self.x, self.y, self.t) property def pos(self): return (self.x, self.y) class TimeSpaceAStarPlanner: def __init__(self, grid_map, reservation_table): :param grid_map: 二维网格地图0代表空闲1代表静态墙壁/障碍物 :param reservation_table: 全局预约表由调度系统(RCS)全局维护 self.grid_map grid_map self.reservation_table reservation_table # 格式见下方说明 def heuristic(self, pos1, pos2): # 采用曼哈顿距离作为空间启发式估计 return abs(pos1[0] - pos2[0]) abs(pos1[1] - pos2[1]) def is_collision(self, current_node, next_x, next_y, next_t): 工业级核心拦截器查验全局预约表检测时空冲突 # 1. 静态障碍物及地图边界检查 if not (0 next_x len(self.grid_map) and 0 next_y len(self.grid_map[0])): return True if self.grid_map[next_x][next_y] 1: return True # 2. 节点冲突检测 (Vertex Conflict) # 检查在 next_t 时刻(next_x, next_y) 是否已被其他高优先级的小车预约 if self.reservation_table.is_vertex_reserved(next_x, next_y, next_t): return True # 3. 边/相向对头冲突检测 (Edge Conflict / Swap Conflict) # 检查是否存在本车计划从 A-B而另一台车在同一时间段计划从 B-A 相互对撞擦肩 curr_x, curr_y, curr_t current_node.x, current_node.y, current_node.t if self.reservation_table.is_edge_swapped(curr_x, curr_y, next_x, next_y, curr_t, next_t): return True return False def plan_path(self, start_pos, goal_pos, start_time0, max_time_steps200): 时间窗 A* 主搜索算法 :param start_pos: 起点坐标 (x, y) :param goal_pos: 终点坐标 (x, y) open_set [] # Close集必须引入时间轴表示为 (x, y, t) 的集合 closed_set set() # 初始化起点时空节点 (x, y, t) h_start self.heuristic(start_pos, goal_pos) start_node TimeSpaceNode(start_pos[0], start_pos[1], start_time, 0, h_start) heapq.heappush(open_set, start_node) # 定义 5 种动作东、西、南、北、以及核心的【原地等待】 # (dx, dy) actions [(0, 1), (0, -1), (1, 0), (-1, 0), (0, 0)] while open_set: current_node heapq.heappop(open_set) # 到达目标判断不仅空间坐标重合且需要确认在当前及未来一小段时间内目标点未被他人预约 if current_node.pos goal_pos: return self._reconstruct_path(current_node) if current_node.pos_time in closed_set: continue closed_set.add(current_node.pos_time) # 安全熔断防止时空搜索树过深导致内存溢出 if current_node.t max_time_steps: continue # 拓展邻居节点 for dx, dy in actions: next_x current_node.x dx next_y current_node.y dy next_t current_node.t 1 # 每一个动作必然消耗 1 个时间片 # 核心拦截器介入若发生 Vertex 或 Edge 冲突直接剪枝跳过 if self.is_collision(current_node, next_x, next_y, next_t): continue if (next_x, next_y, next_t) in closed_set: continue # 动作代价移动和等待的 g_cost 消耗通常设为 1 next_g current_node.g_cost 1 next_h self.heuristic((next_x, next_y), goal_pos) neighbor_node TimeSpaceNode(next_x, next_y, next_t, next_g, next_h, parentcurrent_node) heapq.heappush(open_set, neighbor_node) return None # 规划失败无可行时空路径可能前方道路被死锁或长时间占用 def _reconstruct_path(self, end_node): 从终点逆向回溯生成时空轨迹 path [] curr end_node while curr: path.append((curr.x, curr.y, curr.t)) curr curr.parent return path[::-1] # 翻转恢复正向时间轴三、 工业落地时配套的“全局预约表Reservation Table”逻辑设计为了让上述is_collision核心函数在数百台车交互时依然保持高效率全局预约表通常在内存数据库如 Redis或 RCS 调度核心中以下列双向字典哈希结构进行封装1. 节点预约哈希表 (Vertex Maps)用于秒级判定is_vertex_reserved。结构dict[time_step] - set(x, y)示例vertex_reservations { t12: {(3,3), (3,4), (5,1)}, # 12时刻这些格子已被占 t13: {(3,4), (4,4), (5,2)} }2. 边/对流换位哈希表 (Edge Maps)用于秒级判定is_edge_swapped。结构dict[time_step] - set((from_x, from_y, to_x, to_y))示例edge_reservations { t_to_t_plus_1 12: { (3, 3, 3, 4) }, # 12到13秒期间有一台车正在从(3,3)向(3,4)移动 }当另一台车在规划时如果试图在此时刻预约从(3,4) - (3,3)的动作通过对比该哈希集合系统将瞬间实现 O(1) 时间复杂度的对流冲突强行拦截。