多AGV动态调度思路

在铲屎官最近负责一个多AGV转运项目，其中涉及到多AGV交通管制内容，之前也没接触过这个领域，着实是遇到不小的挑战，在网上也查阅了很多相关资料，也研究了OPENTcs，但感觉在项目实际可用性不高，索性就自己搞个试试。

先说说业务场景：在一个30*50左右的区域内，3台AGV负则搬运不同物料到各个接驳点，如下图AGV运行路线图，AGV需要从A0运物料到A1/A2/A3，从B0运物料到B1/B2/B3，从D0运物料到D1/D2，路线只允许一台AGV通过。

再说说思路：

 1、AGV开始运动前，匹配从当前位置到目标位置的行驶路径，并保存；

2、计算当前AGV与其他运动中的AGV是否存在公共路径，存在则保存公共路径信息（起点终点，同向行驶/相向行驶等）；

3、取出公共路径，计算两台AGV到公共路径起点的最近距离，距离最近的AGV，先申请公共路径，距离远的AGV排后申请；

4、AGV申请公共路径成功，则可以行驶，并对公共路径加锁；AGV申请公共路径失败，则暂停等待；

5、若存在多条公共路径，在计算出AGV距离公共路径的距离后，再进行排序，距离最近优先；

6、若存在距离相同的两台AGV，则需判断AGV的先后关系；

最后实现：