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

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

Info

Publication number
CN109668572B
CN109668572B CN201811629622.5A CN201811629622A CN109668572B CN 109668572 B CN109668572 B CN 109668572B CN 201811629622 A CN201811629622 A CN 201811629622A CN 109668572 B CN109668572 B CN 109668572B
Authority
CN
China
Prior art keywords
road section
station
road
path
section
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201811629622.5A
Other languages
English (en)
Other versions
CN109668572A (zh
Inventor
陈智君
李超
陈双
张阳
陆雪强
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Wuhu Hit Robot Technology Research Institute Co Ltd
Original Assignee
Wuhu Hit Robot Technology Research Institute Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Wuhu Hit Robot Technology Research Institute Co Ltd filed Critical Wuhu Hit Robot Technology Research Institute Co Ltd
Priority to CN201811629622.5A priority Critical patent/CN109668572B/zh
Publication of CN109668572A publication Critical patent/CN109668572A/zh
Application granted granted Critical
Publication of CN109668572B publication Critical patent/CN109668572B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3446—Details of route searching algorithms, e.g. Dijkstra, A*, arc-flags, using precalculated routes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Traffic Control Systems (AREA)

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 (5)

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;
步骤S1具体包括如下步骤：
S11、判断站点S_st是否与位置p重合，若判断结果为否，则执行步骤S12，若判断结果为是，则执行步骤S13；
S12、在环境拓扑地图中查找路段r，将路段r作为起始路段R1，路段r为经过站点S_st，且距位置p最近的路段；
S13、判断站点S_st与目标站点S_end是否重合，若判断结果为是，则将通过站点S_st且与叉车车头朝向夹角最小的路段作为起始路段R1。
2.如权利要求1所述基于floyd算法的激光叉车路径搜索方法，其特征在于，步骤S12中，在环境拓扑地图中存在至少两个路段r距离相等，则分别计算p-p_st向量与p_end-p_st向量之间的夹角，将夹角最小的路段确定为起始路段R1；
其中，p-p_st向量由位置p至r路段起点p_st组成的向量，p_end-p_st向量为r路段终点p_end至r路段起点p_st组成的向量。
3.如权利要求1至2任一权利要求所述基于floyd算法的激光叉车路径搜索方法，其特征在于，在步骤S2中，若站点S_st与目标站点S_end重合，则起始路段R1放入路径名集合roadNameList。
4.如权利要求1至2任一权利要求所述基于floyd算法的激光叉车路径搜索方法，其特征在于，采用路段名来标识行驶方向。
5.如权利要求1至2任一权利要求所述基于floyd算法的激光叉车路径搜索方法，其特征在于，基于路段的起点站点及终止站点来判断路段是否重合。
CN201811629622.5A 2018-12-28 2018-12-28 一种基于floyd算法的激光叉车路径搜索方法 Active CN109668572B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201811629622.5A CN109668572B (zh) 2018-12-28 2018-12-28 一种基于floyd算法的激光叉车路径搜索方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201811629622.5A CN109668572B (zh) 2018-12-28 2018-12-28 一种基于floyd算法的激光叉车路径搜索方法

Publications (2)

Publication Number Publication Date
CN109668572A CN109668572A (zh) 2019-04-23
CN109668572B true CN109668572B (zh) 2022-05-17

Family

ID=66146984

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201811629622.5A Active CN109668572B (zh) 2018-12-28 2018-12-28 一种基于floyd算法的激光叉车路径搜索方法

Country Status (1)

Country Link
CN (1) CN109668572B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111159319B (zh) * 2019-11-27 2023-07-25 北京中交兴路信息科技有限公司 一种基于大数据的寻路方法和系统
CN111024088B (zh) * 2019-12-27 2023-04-07 芜湖哈特机器人产业技术研究院有限公司 一种激光叉车路径规划方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106094834A (zh) * 2016-07-19 2016-11-09 芜湖哈特机器人产业技术研究院有限公司 基于已知环境下的移动机器人路径规划方法
CN106681334A (zh) * 2017-03-13 2017-05-17 东莞市迪文数字技术有限公司 基于遗传算法的自动搬运小车调度控制方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE10334829B4 (de) * 2003-07-30 2005-09-08 Siemens Ag Verfahren zur Klassifikation von Routensegmenten eines Navigationssystems und ein entsprechendes Navigationssystem
US9508263B1 (en) * 2015-10-20 2016-11-29 Skycatch, Inc. Generating a mission plan for capturing aerial images with an unmanned aerial vehicle
CN106525047B (zh) * 2016-10-28 2019-07-02 重庆交通大学 一种基于floyd算法的无人机路径规划方法
CN107036618A (zh) * 2017-05-24 2017-08-11 合肥工业大学(马鞍山)高新技术研究院 一种基于最短路径深度优化算法的agv路径规划方法
CN107560631B (zh) * 2017-08-30 2020-02-14 国网智能科技股份有限公司 一种路径规划方法、装置和巡检机器人
CN108007470B (zh) * 2017-11-30 2021-06-25 深圳市隐湖科技有限公司 一种移动机器人地图文件格式和路径规划系统及其方法
CN108664022B (zh) * 2018-04-27 2023-09-05 湘潭大学 一种基于拓扑地图的机器人路径规划方法及系统
CN108614554A (zh) * 2018-05-03 2018-10-02 南京理工大学 一种基于区域限制的机器人多源最短路径规划方法
CN109029478A (zh) * 2018-06-20 2018-12-18 华南理工大学 一种基于改进Floyd算法的智能车辆路径规划方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106094834A (zh) * 2016-07-19 2016-11-09 芜湖哈特机器人产业技术研究院有限公司 基于已知环境下的移动机器人路径规划方法
CN106681334A (zh) * 2017-03-13 2017-05-17 东莞市迪文数字技术有限公司 基于遗传算法的自动搬运小车调度控制方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于 Floyd 算法的移动机器人最短路径规划研究;石为人，王楷;《仪器仪表学报》;20091031;第30卷(第10期);2088-2092 *

Also Published As

Publication number Publication date
CN109668572A (zh) 2019-04-23

Similar Documents

Publication Publication Date Title
CN109724612B (zh) 一种基于拓扑地图的agv路径规划方法及设备
CN111007862B (zh) 一种多agv协同工作的路径规划方法
CN111026128B (zh) 一种多激光agv的避让方法
CN113821029B (zh) 一种路径规划方法、装置、设备及存储介质
CN109668572B (zh) 一种基于floyd算法的激光叉车路径搜索方法
CN110554688B (zh) 用于生成拓扑地图的方法和装置
CN112325873A (zh) 一种环境地图自主更新方法、设备及计算机可读存储介质
Asghar et al. Vehicle localization based on visual lane marking and topological map matching
CN112146668A (zh) 基于ros的无人车自主导航方法
CN104535068A (zh) 盲区路线导航方法及系统
CN109993813B (zh) 创建地图的方法、装置、车辆及可读存储介质
CN204883363U (zh) 激光制导地图构建的agv搬运机器人导航系统
CN116700298B (zh) 路径规划方法、系统、设备及存储介质
US20230273621A1 (en) Information processing apparatus, information processing method, and program
CN112197763B (zh) 地图构建方法、装置、设备和存储介质
CN114661047A (zh) 一种基于时间窗的多agv实时调度的路径优化方法
US20220205792A1 (en) Method and device for creating a first map
CN113515117A (zh) 一种基于时间窗的多agv实时调度的冲突消解方法
CN115587151A (zh) 用于车辆的使用共享slam地图的方法和装置
Riman et al. A Priority-based Modified A∗ Path Planning Algorithm for Multi-Mobile Robot Navigation
CN112578782A (zh) 自动导引运输车任务路径规划方法及装置
KR102345826B1 (ko) 맵 변경 기반 자율주행 장치 및 방법
KR0185101B1 (ko) 무인운반차의 운행경로 탐색방법
CN114415696B (zh) 一种交管危险区域的控制方法
CN117670162A (zh) 一种场内智能物流解决方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant