CN109668572A - 一种基于floyd算法的激光叉车路径搜索方法 - Google Patents

Abstract

本发明适用于自动控制技术领域，提供了一种基于floyd算法的激光叉车路径搜索方法，包括：S1、获取离激光叉车的当前位置p最近的站点S_st，基于站点S_st来确定位置p在环境拓扑地图中的起始路段；S2、基于floyd算法搜索站点S_st到目标站点S_end的最短路径roadList；S3、判断起始路段与最短路径roadList中的首个路段是否重合；S4、若不重合，则在最短路径roadList中的首个路段前插入起始路段；S5、遍历路段roadList，将相同行驶方向的相邻路段进行合并，得到激光叉车行驶路径上的路径名集合roadNameList。即使激光叉车的当前位置不在环境拓扑地图的站点上，也能基于floyd算法来进行路的规划。

Description

一种基于floyd算法的激光叉车路径搜索方法

技术领域

本发明属于自动控制技术领域，提供了一种基于floyd算法的激光叉车路径搜索方法。

背景技术

随着柔性制造系统和智能仓库系统等智能生产系统的发展，AGV作为物料输送系统中的重要组成部门，在制造业运输环节得到了广泛的应用。激光叉车相比其它AGV具有明显的优势，转弯半径小，路径更改方便，在人工驾驶叉车使用的场景中能得到直接应用。由于工厂中设备较多，还会存在不同工种的操作人员，为了生产安全，需要通过软件提前规划好激光叉车的行驶路线，生成拓扑地图，地图中包含不同站点和不同路段。

常用的静态最短路径搜索方法有floyd算法和A*算法，其中A*算法属于单源最短路径搜索方法，floyd算法属于多源最短路径搜索算法。激光叉车运行的环境拓扑地图在生产中不会实时发生变动，因此采用多源最短路径搜索算法可以在初始化就搜索到所有站点之间的最短路径，后续的路径搜索简单，速度快。

标准的floyd算法只能搜索站点到站点之间经过的站点，不能根据叉车控制系统的特点提供导航路径；此外，当激光叉车不位于站点上时，标准的floyd算法无法提供叉车行走的路径。

发明内容

本发明实施例提供一种基于floyd算法的激光叉车路径搜索方法，基于floyd算法来为不处于站点位置的激光叉车进行路径规划。

为了实现上述目的，本发明提供了一种基于floyd算法的激光叉车路径搜索方法，所述方法包括如下步骤：

S1、获取离激光叉车的当前位置p最近的站点S_st，基于站点S_st来确定位置p在环境拓扑地图中的起始路段R1；

S2、基于floyd算法搜索站点S_st到目标站点S_end的最短路径roadList；

S3、判断起始路段R1与最短路径roadList中的首个路段是否重合；

S4、若不重合，则在最短路径roadList中的首个路段前插入起始路段R1；

S5、遍历路段roadList，将相同行驶方向的相邻路段进行合并，得到激光叉车行驶路径上的路径名集合roadNameList。

进一步的，步骤S1具体包括如下步骤：

S11、判断站点S_st是否与位置p重合，若判断结果为否，则执行步骤S12，若判断结果为是，则执行步骤S13；

S12、在环境拓扑地图中查找路段r，将路段r作为起始路段R1，路段r为经过站点S_st，且距位置p最近的路段；

S13、判断站点S_st与目标站点S_end是否重合，若判断结果为是，则将通过站点S_st且与叉车车头朝向夹角最小的路段作为起始路段R1。

进一步的，步骤S12中，在环境拓扑地图中存在至少两个路段r距离相等，则分别计算p-p_st向量与p_end-p_st向量之间的夹角，将夹角最小的路段确定为起始路段R1；

其中，p-p_s向量由位置p至r路段起点p_st组成的向量，p_end-p_st向量为r路段终点p_end至r路段起点p_st组成的向量。

进一步的，在步骤S2中，若站点S_st与目标站点S_end重合，则起始路段R1放入路径名集合roadNameList。

进一步的，采用路段名来标识行驶方向。

进一步的，基于路段的起点站点及终止站点来判断路段是否重合。

基于距激光叉车当前位置最近的站点，来确定在环境拓扑地图中的起始路段，同时以距叉车当前位置最近的站点作为floyd算法的起始站点，去查找从起始站点到目标站点的最短行驶路径，在将起始路段与最短行驶路径进行融合，构成激光叉车当前位置对应的行驶路径，即使激光叉车的当前位置不在环境拓扑地图的站点上，也能基于floyd算法来进行路径的规划。

附图说明

图1为本发明实施例提供的基于floyd算法的激光叉车路径跟踪方法流程图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明要解决的是激光叉车在给定的拓扑地图中，能正确的搜索到达目的地所通过的路径的问题。需要先建立环境拓扑地图，激光叉车能通过激光雷达实时获取自身的位姿，拓扑地图由站点和路段组成，站点的结构为Site{id,name,pos}，其中id为站点在站点集合中的序号，name为站点名，pos为站点在地图坐标系中的坐标；路段的结构为Road{roadName,startSite,endSite,dist,id}，其中roadName为路段名，路段名用于标识行驶方向，假定路段A与路段B位于相同的行驶方向，则路段A与路段B的路段名roadName一样，相同路段A与路段B在正向行驶时的路段名与反向行驶时的路段名不同，路段startSite为起始站点，endSite为终止站点，dist为路段距离，id为路段在路段集合中的序号。

在路径搜索中，需要用到floyd算法搜索拓扑地图中经过的节点，floyd算法的步骤如下：初始化距离矩阵distMat，将不同站点之间的距离设为∞，相同站点之间的距离设为0；根据地图中的路段和站点信息，对distMat进行更新，已知路段R起点S1的id为i，终点S2的id为j，则distMat[i,j]就等于路段R的dist；对于每一对顶点u和v，查看是否存在一个顶点w使得从u到w再到v比已知的路径更短，如果有，则令distMat[u,v]＝distMat[u,w]+distMat[w,v]，令路径矩阵pathMat[u,v]＝w；遍历distMat，得到所有站点之间的路径矩阵pathMat，里面包含了所有站点之间的最短路径，在激光叉车调度中，一般是将激光叉车从当前位置调度到某一个节点，所以路径搜索可以分站点到站点的路径搜索，以及位置到站点的路径搜索。

基于距激光叉车当前位置最近的站点，来确定在环境拓扑地图中的起始路段，同时以距叉车当前位置最近的站点作为floyd算法的起始站点，去查找从起始站点到目标站点的最短行驶路径，在将起始路段与最短行驶路径进行融合，构成激光叉车当前位置对应的行驶路径，即使激光叉车的当前位置不在环境拓扑地图的站点上，也能基于floyd算法来进行路径规划。

图1为本发明实施例提供的基于floyd算法的激光叉车路径跟踪方法流程图，该方法包括如下步骤：

S1、获取离激光叉车的当前位置p最近的站点S_st，基于站点S_st来确定位置p在环境拓扑地图中的起始路段R1；

在本发明实施例中，步骤S1具体包括如下步骤：

S11、判断站点S_st是否与位置p重合，若判断结果为否，则执行步骤S12，若判断结果为是；则执行步骤S13；

S12、在环境拓扑地图中路段r，路段r定义为环境拓扑地图中经过站点S_st，且距位置p最近的路段，若环境拓扑地图中只存在一个路段r，则将该路段r作为起始路段R1，若环境拓扑地图中至少存在两个路段r距离相等，则分别计算向量(p-p_st)与向量(p_end-p_st)之间的夹角，将夹角最小的路段确定为起始路段R1，其中，p_st为路段r的起点、p_end为路段r的终点，p-p_st向量由位置p至r路段起点p_st组成的向量，p_end-p_st向量为r路段终点p_end至r路段起点p_st组成的向量。

S13、判断站点S_st与目标站点S_end是否重合，若判断结果为是，则将通过站点S_st且与叉车车头朝向夹角最小的路段作为起始路段R1。

S2、基于floyd算法搜索最近站点S_st到目标站点S_end的最短路径roadList；

在本发明实施例中，若站点S_st与目标站点S_end重合，则直接将起始路段放入路径名集合roadNameList，若站点S_st与目标站点S_end不重合，则基于步骤S3至步骤S4来获取路段名集合roadNameList。

S3、判断起始路段R1与路径roadList中的首个路段是否重合；

在本发明实施例中，基于待判定路段的起点及终点来坐标来判断待判断路段时是否重合，重合的路段具有相同的起点及终点坐标。

S4、若不重合，则在路径roadList中的首个路段前插入起始路段；

在本发明实施例中，若起始路段与路径roadList中的首个路段重合，则不将起始路段放入路径roadList中，即保持路径roadList不变，直接执行步骤S5，若起始路段与路径roadList中的首个路段重合，则在路径roadList中的首个路段前插入起始路段，在执行步骤S5。

S5、遍历路段roadList，将相同行驶方向的相邻路段进行合并，得到激光叉车行驶路径上的路段名集合roadNameList。

在本发明实施例中，采用路段名来标识行驶方向，即相同行驶方向上的各路段具有相同的路段名，相同路段在不同行驶方向上具有不同的路段名，将具有相同路段名的路段进行合并，即构成激光叉车行驶路径上的路段名集合roadNameList。

基于距激光叉车当前位置最近的站点，来确定在环境拓扑地图中的起始路段，同时以距叉车当前位置最近的站点作为floyd算法的起始站点，去查找从起始站点到目标站点的最短行驶路径，在将起始路段与最短行驶路径进行融合，构成激光叉车当前位置对应的行驶路径，即使激光叉车的当前位置不在环境拓扑地图的站点上，也能基于floyd算法来进行路径的规划。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种于floyd算法的激光叉车路径搜索方法，其特征在于，所述方法包括如下步骤：

S1、获取离激光叉车的当前位置p最近的站点S_st，基于站点S_st来确定位置p在环境拓扑地图中的起始路段R1；

S2、基于floyd算法搜索站点S_st到目标站点S_end的最短路径roadList；

S3、判断起始路段R1与最短路径roadList中的首个路段是否重合；

S4、若不重合，则在最短路径roadList中的首个路段前插入起始路段R1；

S5、遍历路段roadList，将相同行驶方向的相邻路段进行合并，得到激光叉车行驶路径上的路径名集合roadNameList。

2.如权利要求1所述基于floyd算法的激光叉车路径搜索方法，其特征在于，步骤S1具体包括如下步骤：

S11、判断站点S_st是否与位置p重合，若判断结果为否，则执行步骤S12，若判断结果为是，则执行步骤S13；

S12、在环境拓扑地图中查找路段r，将路段r作为起始路段R1，路段r为经过站点S_st，且距位置p最近的路段；

S13、判断站点S_st与目标站点S_end是否重合，若判断结果为是，则将通过站点S_st且与叉车车头朝向夹角最小的路段作为起始路段R1。

3.如权利要求1所述基于floyd算法的激光叉车路径搜索方法，其特征在于，步骤S12中，在环境拓扑地图中存在至少两个路段r距离相等，则分别计算p-p_st向量与p_end-p_st向量之间的夹角，将夹角最小的路段确定为起始路段R1；

其中，p-p_s向量由位置p至r路段起点p_st组成的向量，p_end-p_st向量为r路段终点p_end至r路段起点p_st组成的向量。

4.如权利要求1至3任一权利要求所述基于floyd算法的激光叉车路径搜索方法，其特征在于，在步骤S2中，若站点S_st与目标站点S_end重合，则起始路段R1放入路径名集合roadNameList。

5.如权利要求1至3任一权利要求所述基于floyd算法的激光叉车路径搜索方法，其特征在于，采用路段名来标识行驶方向。

6.如权利要求1至3任一权利要求所述基于floyd算法的激光叉车路径搜索方法，其特征在于，基于路段的起点站点及终止站点来判断路段是否重合。