4. 路径规划基础

AGV 的路径规划本质上是在一张“可通行关系图”上寻找从起点到终点的最优或可行路径。理解这一点的关键，是先分清两种地图表达：栅格地图把环境切成等大小方格，每个格子标记可通行或障碍；拓扑地图则把环境抽象为节点和边，节点通常对应站点、路口、充电位、装卸点，边表示两节点之间可通行的通道。栅格地图信息完整、适合局部避障和精细路径，但搜索规模大；拓扑地图结构紧凑、搜索快，适合站点间调度，但依赖建图时对通道的抽象是否准确。

无论哪种地图，路径规划都先转化为图搜索问题。设图为 G=(V,E)，V 是节点集合，E 是边集合，每条边有代价 w(u,v)。在 AGV 场景中，代价通常不是单纯的几何距离，而是“距离 ÷ 允许速度”得到的时间，或再叠加转弯惩罚、拥堵权重、单行限制。例如一条 12 m 的直道允许 1.2 m/s，则基础代价为 10 s；若该边包含一个 90° 转弯，可加 2 s 转弯惩罚，总代价 12 s。这样规划出的路径才更贴近实际运行时间，而不是只看谁更短。

Dijkstra：从起点向外均匀扩展

Dijkstra 算法维护一个“起点到各节点的已知最短代价”表，每次从未确定节点中取出代价最小者，松弛它的所有邻居。伪代码如下：

dist[s] = 0, 其余 dist = ∞
未确定集合 Q = 所有节点
while Q 非空:
    u = Q 中 dist 最小的节点
    将 u 移出 Q
    for 每条边 (u, v, w):
        if dist[u] + w < dist[v]:
            dist[v] = dist[u] + w
            prev[v] = u

它的特点是“按代价从近到远一圈圈扩展”，只要边权非负，第一次取出终点时得到的就是全局最优路径。缺点是它不知道终点在哪，会向四周均匀搜索，节点多时计算量大。对 100×100 的栅格，最坏情况要处理上万个格子，实时性会受影响。

A*：用启发函数把搜索拉向终点

A* 在 Dijkstra 基础上增加启发函数 h(v)，用 f(v)=g(v)+h(v) 排序，其中 g(v) 是起点到 v 的实际代价，h(v) 是 v 到终点的估计代价。只要 h 不高估真实代价（可采纳性），A* 仍能保证最优。栅格地图常用两种启发：

• 曼哈顿距离：h=|Δx|+|Δy|，适合只能上下左右移动的四邻域栅格；
• 欧几里得距离：h=√(Δx²+Δy²)，适合允许斜向移动的八邻域栅格。

若把单位格子边长设为 1 m、AGV 速度 1 m/s，则 h 可直接以秒为单位。A* 的搜索范围通常明显小于 Dijkstra，因为启发项不断把扩展方向拉向终点。需要注意：如果 h 被放大（加权 A*），搜索更快但可能不再最优，工程上要明确“要最快还是最优”。

栅格地图上的路径生成

栅格地图规划通常分三步：栅格化、搜索、路径平滑。栅格化时，每个格子边长要匹配 AGV 尺寸与安全余量。例如 AGV 车宽 0.8 m、安全余量 0.2 m，则障碍膨胀半径至少 0.5 m；若格子边长 0.2 m，膨胀约 3 格。膨胀不足会导致规划路径贴障碍太近，实际运行易剐蹭；膨胀过大则可能把窄通道封死，需要现场权衡。

搜索时，四邻域只允许上下左右，路径更规整但转弯多；八邻域允许斜向，路径更短，但斜向穿越障碍角点可能不安全，通常要禁止“穿角”。搜索完成后，栅格路径是折线，需做平滑：常用方法是拉绳法（string pulling），在路径点之间尝试直线连接，若连线不穿障碍就删除中间点。平滑后还要检查最小转弯半径，AGV 若为差速驱动可原地转向，对转弯半径要求低；若为舵轮或叉车式，必须保证每段圆弧半径不小于车辆最小转弯半径。

拓扑地图上的路径生成

拓扑地图的路径生成更接近“站点级调度”。节点是站点，边是通道，边权是通行时间。规划时直接对节点图跑 Dijkstra 或 A*，得到节点序列，例如 S→D→E→B→C。随后要把节点序列展开为可执行轨迹：在节点处按通道方向对齐，在边内按通道中心线行驶，在转弯节点插入圆弧或原地旋转动作。拓扑地图的优点是搜索极快，适合几十到几百个站点的大规模调度；缺点是节点间通道必须提前建好，临时障碍或通道封闭需要动态改权或临时删边。

关键参数与判断依据

判断规划结果是否可用，至少看三点：路径是否连续无断点、是否与障碍保持安全距离、每段曲率是否满足车辆运动约束。若路径穿过膨胀区、出现尖角或转弯半径不足，即使代价最优也不能直接下发。实际调试时，可先在小规模栅格上验证 Dijkstra 与 A* 的代价一致性，再逐步加入转弯惩罚和动态障碍，观察路径是否按预期绕行。

总结来说，栅格地图适合表达“哪里能走”，拓扑地图适合表达“按什么顺序走”；Dijkstra 保证最优但扩展大，A* 用启发函数加速但需保证 h 可采纳。路径生成不是搜完就结束，还要经过膨胀、平滑、转弯半径校验和代价复核，才能成为 AGV 真正可执行的路径。