多AGV协同路径规划：时间窗模型原理与C++工程实现

1. 项目概述与核心价值

最近在做一个AGV（自动导引运输车）集群调度系统的仿真项目，其中路径规划模块是整个系统的核心。在实际的仓储或生产车间里，AGV不是孤立的，它们共享通道、路口等资源，如果规划不当，很容易发生死锁、碰撞或者拥堵。传统的A*、Dijkstra算法能解决单AGV的最短路径问题，但面对多AGV协同作业时，就显得力不从心了。这时就需要引入时间窗模型。

简单来说，时间窗模型就是在路径规划时，不仅考虑空间上的“能不能走”，还要考虑时间上的“什么时候能走”。它为路径上的每个路段或节点分配一个时间窗口，标明该资源被占用的时间段，后续的AGV规划路径时，必须避开这些已被占用的时间窗，从而在时间和空间两个维度上避免冲突。这个项目，就是深入研究这个模型，并用C++将其实现出来，最终目标是让多台AGV能在共享地图中高效、无碰撞地运行。

2. 核心思路与模型设计

2.1 问题定义与挑战分析

多AGV路径规划的核心矛盾在于资源（道路、节点）的独占性与AGV任务的并发性。想象一下十字路口，如果两辆AGV同时到达，必然发生冲突。时间窗模型的核心思想是将空间资源时间化。我们不再简单地将地图看作一个静态的网格或图，而是将其升级为一个“时空地图”。路径上的每个单元（比如一个网格或一条边）都附着一系列的时间窗口，标明何时被占用，何时空闲。

面临的几个核心挑战：

1. 冲突类型：主要分为节点冲突（多AGV同时到达同一节点）和边冲突（多AGV在同一时间段内相向而行于同一条边）。
2. 优化目标：不仅仅是总路径最短，还要考虑总任务完成时间（makespan）最小化、AGV等待时间最少、系统吞吐量最大等。
3. 计算复杂度：随着AGV数量和地图规模的增加，解空间呈指数级增长，属于NP-Hard问题，需要设计高效的搜索与优化策略。

2.2 时间窗模型详解

我们采用一种比较经典且实用的模型：基于边的时间窗模型。在这个模型里，我们将地图建模为一个无向图G=(V, E)，其中V是节点（如路口、工位），E是边（通道）。每条边e和每个AGVk都关联一个资源时间窗列表。

时间窗数据结构： 每个时间窗是一个三元组(t_start, t_end, agv_id)，表示从t_start到t_end时刻，该边被AGVagv_id占用。AGV在规划路径时，对于拟使用的每条边，需要找到一个空闲时间窗（或一段连续空闲时间）插入自己的占用计划。

路径表示： 一条路径不再是一系列节点的序列，而是一个时空轨迹。例如：[(v1, t0), (e1, t0-t1), (v2, t1), (e2, t1-t2), ...]。它清晰地描述了AGV在何时位于何处。

冲突检测： 当为AGVk规划路径时，需要检查其计划使用的每一条边e在计划使用的时间段[t_use_start, t_use_end]内，是否与已有时间窗重叠。重叠即意味着冲突，需要调整AGVk的出发时间或在当前边之前插入等待。

2.3 算法框架选型

直接为所有AGV做联合优化非常困难。我们采用一种基于优先级的两阶段规划框架，这也是工业界常见的做法：

1. 阶段一：静态路径规划。为每个AGV单独计算一条从起点到终点的静态最短路径（忽略其他AGV），使用A*算法。这条路径作为“理想路径”或“备选路径集合”的基础。
2. 阶段二：带时间窗的路径规划与冲突消解。按照预设的优先级（如任务下达顺序、AGV编号），依次为每个AGV规划路径。在规划时，考虑之前已规划AGV所产生的时间窗，通过在节点插入等待的方式来避免冲突。这本质上是将动态冲突消解问题转化为一系列带约束的单AGV路径规划问题。

为什么选择这个框架？

• 可行性高：将复杂的多智能体规划分解为多个单智能体问题，易于实现和调试。
• 实时性好：AGV可以依次进入系统，无需等待所有任务一起规划。
• 可扩展性强：可以方便地集成各种单AGV规划算法和冲突消解策略。

当然，它的缺点是可能因为优先级顺序导致解的质量不是全局最优。但在实际工程中，在效率和效果之间取得平衡更为重要。

3. 关键数据结构与C++实现

3.1 基础数据结构定义

首先，我们需要定义一些核心的类来支撑整个模型。

// 基础坐标或节点 struct Point { int x, y; Point(int x_ = 0, int y_ = 0) : x(x_), y(y_) {} bool operator==(const Point& other) const { return x == other.x && y == other.y; } // 为方便在unordered_map等容器中使用，需要定义哈希 struct Hash { size_t operator()(const Point& p) const { return std::hash<int>()(p.x) ^ (std::hash<int>()(p.y) << 1); } }; }; // 时间窗 struct TimeWindow { int agv_id; // 占用该时间窗的AGV ID double start_time; // 开始占用时间 double end_time; // 结束占用时间 TimeWindow(int id, double s, double e) : agv_id(id), start_time(s), end_time(e) {} // 判断两个时间窗是否重叠 bool isOverlap(const TimeWindow& other) const { return !(end_time <= other.start_time || start_time >= other.end_time); } }; // 地图中的一条边 struct Edge { Point from; Point to; double length; // 边的长度，可理解为通行时间（假设匀速） std::vector<TimeWindow> occupied_windows; // 该边上已有的时间窗列表 Edge(const Point& f, const Point& t, double len) : from(f), to(t), length(len) {} // 查找在时间段[start, end]内是否空闲 bool isAvailable(double start, double end, int exclude_agv = -1) const { for (const auto& win : occupied_windows) { if (win.agv_id == exclude_agv) continue; // 排除自身之前可能占用的窗口 if (win.end_time > start && win.start_time < end) { return false; // 存在重叠 } } return true; } // 添加一个时间窗 void addTimeWindow(const TimeWindow& tw) { occupied_windows.push_back(tw); // 简单维护：按开始时间排序，便于查找和后续优化（如合并相邻窗口） std::sort(occupied_windows.begin(), occupied_windows.end(), [](const TimeWindow& a, const TimeWindow& b) { return a.start_time < b.start_time; }); } };

3.2 地图与AGV状态管理

我们需要一个类来管理整个地图（图结构）和所有边上的时间窗。

class TimeWindowMap { private: std::unordered_map<Point, std::vector<Edge>, Point::Hash> adjacency_list; // 邻接表表示图 std::unordered_map<Point, std::vector<TimeWindow>, Point::Hash> node_occupancy; // 节点占用（可选，用于处理节点冲突） public: // 添加边 void addEdge(const Point& from, const Point& to, double length) { adjacency_list[from].emplace_back(from, to, length); // 如果是无向图，还需要添加反向边 adjacency_list[to].emplace_back(to, from, length); } // 获取两点之间的边（假设两点间只有一条无向边） Edge* getEdge(const Point& from, const Point& to) { auto it = adjacency_list.find(from); if (it != adjacency_list.end()) { for (auto& edge : it->second) { if (edge.to == to) { return &edge; } } } return nullptr; } // 核心函数：为AGV在一条边上预约时间段 bool reserveEdge(const Point& from, const Point& to, int agv_id, double start_time, double speed) { Edge* edge = getEdge(from, to); if (!edge) return false; // 边不存在 double travel_time = edge->length / speed; double end_time = start_time + travel_time; // 检查该时间段是否可用 if (edge->isAvailable(start_time, end_time, agv_id)) { edge->addTimeWindow(TimeWindow(agv_id, start_time, end_time)); // 同时，也需要占用节点（在离开节点from和到达节点to的时间点） // 这里简化处理，占用一个极小的时间段，避免节点冲突 double node_occupancy_duration = 0.1; // 一个小的时间值，表示在节点停留/经过的瞬间 reserveNode(from, agv_id, start_time, start_time + node_occupancy_duration); reserveNode(to, agv_id, end_time - node_occupancy_duration, end_time); return true; } return false; // 预约失败，时间窗冲突 } void reserveNode(const Point& node, int agv_id, double start, double end) { // 实现类似边的检查与添加... 略 } };

AGV类需要维护其状态和计划。

class AGV { public: int id; Point current_position; Point target_position; double speed; std::vector<std::pair<Point, double>> spatio_temporal_path; // 时空路径：(位置，到达时间) std::vector<Point> spatial_path; // 静态空间路径 AGV(int id_, const Point& start, double speed_ = 1.0) : id(id_), current_position(start), speed(speed_) {} // 规划静态路径（A*算法） bool planStaticPath(const Point& goal, const TimeWindowMap& map) { // 实现A*算法，将结果存入 spatial_path // 这里省略A*的具体实现，假设它能返回一条点序列 // spatial_path = AStar(current_position, goal, map); return !spatial_path.empty(); } };

3.3 带时间窗的路径规划器

这是整个系统的核心算法类。

class TimeWindowPlanner { private: TimeWindowMap& map; std::vector<AGV>& agvs; double default_speed; public: TimeWindowPlanner(TimeWindowMap& m, std::vector<AGV>& a, double speed = 1.0) : map(m), agvs(a), default_speed(speed) {} // 按顺序为所有AGV规划路径（带时间窗） void planPaths() { // 按某种优先级排序，这里简单按ID顺序 std::sort(agvs.begin(), agvs.end(), [](const AGV& a, const AGV& b) { return a.id < b.id; }); for (auto& agv : agvs) { if (!agv.spatial_path.empty()) { // 已经通过planStaticPath获得了空间路径 bool success = allocateTimeWindows(agv); if (!success) { std::cerr << "Failed to plan path for AGV " << agv.id << " due to unresolvable conflict.\n"; // 处理规划失败：可以尝试重新规划静态路径，或者让AGV在起点等待 } } } } private: // 为单个AGV的静态空间路径分配时间窗 bool allocateTimeWindows(AGV& agv) { if (agv.spatial_path.size() < 2) return false; agv.spatio_temporal_path.clear(); double current_time = 0.0; // 假设AGV从时刻0开始 // 起始点 agv.spatio_temporal_path.emplace_back(agv.spatial_path[0], current_time); for (size_t i = 0; i < agv.spatial_path.size() - 1; ++i) { const Point& from = agv.spatial_path[i]; const Point& to = agv.spatial_path[i + 1]; Edge* edge = map.getEdge(from, to); if (!edge) return false; double travel_time = edge->length / agv.speed; double planned_arrival = current_time + travel_time; // 关键：寻找一个可行的出发时间，使得边[from, to]在[current_time, planned_arrival]内可用 double actual_start_time = findFeasibleStartTime(*edge, agv.id, current_time, planned_arrival); double actual_arrival_time = actual_start_time + travel_time; // 插入等待（如果需要） if (actual_start_time > current_time) { // AGV需要在节点`from`等待一段时间 agv.spatio_temporal_path.back().second = actual_start_time; // 更新到达`from`的时间为等待结束时间 } // 预约边和节点 if (!map.reserveEdge(from, to, agv.id, actual_start_time, agv.speed)) { // 即使经过findFeasibleStartTime，仍可能失败（如与其他AGV的节点占用冲突） return false; } // 更新当前时间和路径 current_time = actual_arrival_time; agv.spatio_temporal_path.emplace_back(to, current_time); } agv.target_position = agv.spatial_path.back(); return true; } // 为一个AGV在一条边上寻找可行的开始时间 double findFeasibleStartTime(const Edge& edge, int agv_id, double earliest_start, double desired_arrival) { double candidate_start = earliest_start; // 这是一个简化的实现：不断向后试探，直到找到空闲窗口 // 更高效的实现应该直接扫描已有的occupied_windows，寻找空隙 const double time_step = 0.5; // 试探步长，可根据实际情况调整 while (true) { double candidate_end = candidate_start + (desired_arrival - earliest_start); if (edge.isAvailable(candidate_start, candidate_end, agv_id)) { // 还需要检查节点占用（这里简化，假设节点检查在reserveEdge中完成） return candidate_start; } candidate_start += time_step; // 避免无限循环，可以设置一个最大等待时间 if (candidate_start - earliest_start > 60.0) { // 最大等待60秒 return -1.0; // 表示未找到 } } } };

3.4 核心算法实现细节与优化

上面给出的findFeasibleStartTime函数是最简单的“向前试探”法，效率不高。在实际项目中，我们需要一个更高效的算法来查找时间窗空隙。

优化后的时间窗空隙查找算法：

double findFeasibleStartTimeOptimized(const Edge& edge, int agv_id, double earliest_start, double duration) { // duration: 需要占用该边的时长 const auto& windows = edge.occupied_windows; double current_start = earliest_start; // 遍历所有已存在的时间窗 for (const auto& win : windows) { if (win.agv_id == agv_id) continue; // 忽略自身之前可能产生的窗口 // 如果当前计划的开始时间早于此窗口的结束时间，且与此窗口重叠 if (current_start < win.end_time) { // 检查当前计划开始时间是否在窗口开始之前，且留下的空隙是否足够 if (current_start + duration <= win.start_time) { // 空隙足够，直接返回 return current_start; } else { // 空隙不足，将计划开始时间推迟到此窗口结束之后 current_start = win.end_time; } } // 如果current_start >= win.end_time，则检查下一个窗口 } // 检查最后一个时间窗之后的时间段 // 如果所有窗口都检查完，current_start就是可行的开始时间 return current_start; }

这个算法的思路是：将已有的时间窗看作一个个“障碍物”，我们从最早开始时间出发，顺序扫描这些时间窗。如果当前计划与某个时间窗重叠，且重叠部分无法通过提前（因为不能早于earliest_start）来避免，那么就将计划开始时间推迟到该时间窗结束之后。然后继续检查下一个时间窗。这个过程保证了我们找到的是最早可行的开始时间。

关于节点冲突的处理：上面的代码简化了节点冲突处理。严谨的做法是，将节点也视为一种资源，同样维护其时间窗列表。当AGV到达或离开一个节点时，需要占用该节点一个极短的时间（例如0.1秒），表示“正在通过”。在reserveEdge函数中，除了预约边，还需要预约起始节点和结束节点。在findFeasibleStartTime中，也需要同时检查边和两端节点的可用性。这会增加算法的复杂度，但能更严格地避免在节点处的碰撞。

4. 系统集成与仿真测试

4.1 主程序流程

一个完整的仿真程序主流程如下：

int main() { // 1. 初始化地图 TimeWindowMap map; // 添加边，构建一个简单的网格地图 int grid_size = 5; for (int i = 0; i < grid_size; ++i) { for (int j = 0; j < grid_size; ++j) { if (i + 1 < grid_size) map.addEdge(Point(i, j), Point(i+1, j), 1.0); if (j + 1 < grid_size) map.addEdge(Point(i, j), Point(i, j+1), 1.0); } } // 2. 初始化AGV群 std::vector<AGV> agvs; agvs.emplace_back(0, Point(0, 0), 1.0); agvs.emplace_back(1, Point(4, 0), 1.0); agvs[0].target_position = Point(4, 4); agvs[1].target_position = Point(0, 4); // 3. 为每个AGV规划静态路径 for (auto& agv : agvs) { agv.planStaticPath(agv.target_position, map); // 这里需要实现A*算法填充agv.spatial_path // 为示例，我们手动设置一条简单路径（实际项目用A*计算） if (agv.id == 0) { agv.spatial_path = {Point(0,0), Point(1,0), Point(1,1), Point(1,2), Point(1,3), Point(1,4), Point(2,4), Point(3,4), Point(4,4)}; } else { agv.spatial_path = {Point(4,0), Point(3,0), Point(3,1), Point(3,2), Point(3,3), Point(3,4), Point(2,4), Point(1,4), Point(0,4)}; } } // 4. 创建规划器并进行带时间窗的规划 TimeWindowPlanner planner(map, agvs, 1.0); planner.planPaths(); // 5. 输出规划结果 for (const auto& agv : agvs) { std::cout << "AGV " << agv.id << " path:\n"; for (const auto& step : agv.spatio_temporal_path) { std::cout << " Pos(" << step.first.x << "," << step.first.y << ") at time " << step.second << "\n"; } } // 6. （可选）可视化或进行离散事件仿真 return 0; }

4.2 可视化与调试技巧

在开发过程中，可视化是理解算法行为和调试问题的利器。

1. 文本日志输出：最基础的方法。详细输出每个AGV的规划步骤、时间窗预约情况、冲突检测结果。当规划失败时，打印出冲突的边、时间以及涉及的AGV ID。
2. 生成时序图：可以编写脚本，将spatio_temporal_path输出为CSV或JSON格式，然后用Python的Matplotlib或Gantt图工具绘制每个AGV在每条边上的占用时间段。这能一目了然地看到是否有时间窗重叠。
3. 地图状态快照：在仿真时钟的每个重要时刻（如AGV到达/离开节点），输出地图上每条边的占用状态。这有助于发现死锁或资源竞争的热点区域。
4. 使用ROS+Rviz：如果项目允许，集成ROS，用Rviz实时显示AGV在地图上的移动轨迹。时空信息可以用不同颜色或透明度来表示时间维度，非常直观。

4.3 性能优化考虑

当AGV数量增多或地图变大时，算法性能可能成为瓶颈。以下是一些优化方向：

1. 数据结构优化：

   • 使用std::map或std::set来存储边上的时间窗列表，并保持按start_time排序。这样在查找空隙时可以使用二分查找，将时间复杂度从O(n)降到O(log n)。
   • 考虑使用空间索引（如网格、四叉树）来快速查找某个位置附近的边和AGV，用于冲突预测。

2. 搜索策略优化：

   • 窗口合并：当同一个AGV在一条边上产生多个连续或相邻的时间窗时，可以将其合并为一个，减少列表长度。
   • 懒惰检查：不一定在规划每一步时都严格检查所有边。可以只检查路径上的“关键冲突点”，如共享路段、交叉路口。
   • 启发式等待：当找不到可行时间窗时，不一定是简单地在当前节点等待。可以尝试“绕行”策略，即重新规划静态路径，避开拥堵区域。这需要将时间窗信息以某种形式（如动态权重）反馈给A*算法。

3. 算法升级：

   • 冲突搜索（CBS）：这是目前多智能体路径规划的前沿算法。它在上层搜索冲突解决方案，在下层为每个AGV进行带约束的路径规划。虽然更复杂，但能获得更好的全局解。
   • 基于规则的策略：为常见冲突场景（如十字路口）制定优先通行规则，可以大大减少在线计算量。

5. 常见问题与实战心得

5.1 典型问题与排查

1. 死锁（Deadlock）：

   • 现象：两个或多个AGV互相等待对方释放资源，系统卡住。
   • 排查：检查规划结果中的时空路径，看是否存在环形等待依赖。例如，AGV A 等待 B 释放边 E1，而 B 又在等待 A 释放边 E2。
   • 解决：
     • 引入回退（Backtracking）：当为某个AGV规划失败时，不仅让它等待，可以尝试让优先级更高的AGV重新规划，或者让当前AGV尝试一条不同的备选路径。
     • 定义通行规则：例如，规定在十字路口，ID小的AGV优先，或主干道优先。
     • 预留资源：在规划时，不仅预约即将使用的边，也提前预约未来几个关键节点/边，防止被其他AGV“插队”。

2. 规划时间过长：

   • 现象：AGV数量超过20台，规划器计算缓慢。
   • 排查：使用性能分析工具（如gprof、Valgrind）定位热点函数。通常是findFeasibleStartTime中的循环或isAvailable检查耗时。
   • 解决：
     • 应用前面提到的数据结构优化（二分查找）。
     • 限制搜索深度或最大等待时间。
     • 考虑将地图分区，AGV只在区域内详细规划，跨区域时使用宏观路径。

3. 时间窗碎片化：

   • 现象：边上有很多短时间窗，导致虽然总空闲时间很多，但找不到一个足够长的连续时间段供新AGV通过。
   • 解决：
     • 实现时间窗合并算法，在添加新窗口后，合并相邻或重叠的属于同一AGV的窗口。
     • 在规划时，允许AGV“提前出发”或“延迟出发”一小段时间，以对齐空闲窗口。

5.2 实战心得与技巧

1. 时间精度问题：使用double表示时间可能会带来浮点数精度误差。在比较时间窗是否重叠时，使用一个小的容忍度（epsilon），例如if (end_time <= other.start_time + 1e-9)。更好的做法是使用整数表示时间（如毫秒），避免精度问题。

2. 速度与距离单位：保持单位一致至关重要。如果地图网格边长是米，速度是米/秒，那么时间就是秒。在定义Edge.length和AGV.speed时就要明确。混合单位是常见的错误来源。

3. 初始时间窗：地图上可能有固定的障碍物或单向通行区域，这些可以在初始化时以“虚拟AGV”（id=-1）的形式预先占用时间窗，实现禁行或限行。

4. 动态障碍处理：实际环境中可能有临时障碍。可以设计一个“动态时间窗”机制，允许在运行时插入新的占用窗口（代表临时封锁），并触发受影响的AGV重新规划局部路径。

5. 测试策略：先从小规模场景开始测试（如2-3台AGV，简单地图），确保基础逻辑正确。然后逐步增加复杂度。编写单元测试，特别是针对TimeWindow的重叠判断、findFeasibleStartTimeOptimized等核心函数。

6. 与调度系统集成：时间窗路径规划器通常是上层调度系统的一个组件。设计清晰的接口（如bool planPath(AGV& agv, const Point& goal)），使其易于被调度系统调用。规划结果（时空路径）也需要以一种高效的方式反馈给调度系统和AGV的执行控制器。

这个项目将经典的图搜索算法与时空约束结合起来，是机器人学和运筹学的一个有趣交叉点。实现过程中，对数据结构的理解、对并发问题的处理、对算法效率的优化，都是极大的锻炼。当看到多台AGV在仿真中流畅、无碰撞地运行时，那种成就感是对所有调试和优化工作的最好回报。