CN114489051B - 一种机器人的路径规划方法及设备 - Google Patents
一种机器人的路径规划方法及设备 Download PDFInfo
- Publication number
- CN114489051B CN114489051B CN202111654927.3A CN202111654927A CN114489051B CN 114489051 B CN114489051 B CN 114489051B CN 202111654927 A CN202111654927 A CN 202111654927A CN 114489051 B CN114489051 B CN 114489051B
- Authority
- CN
- China
- Prior art keywords
- elevator
- robot
- path
- vertex
- destination
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 49
- 230000033001 locomotion Effects 0.000 claims description 13
- 238000003825 pressing Methods 0.000 claims description 9
- 238000004364 calculation method Methods 0.000 claims description 4
- 230000004044 response Effects 0.000 claims description 3
- 230000009191 jumping Effects 0.000 claims description 2
- 230000008569 process Effects 0.000 abstract description 5
- 230000015654 memory Effects 0.000 description 15
- 238000004891 communication Methods 0.000 description 9
- 238000012546 transfer Methods 0.000 description 8
- 238000004422 calculation algorithm Methods 0.000 description 6
- 238000004590 computer program Methods 0.000 description 4
- 230000003993 interaction Effects 0.000 description 4
- 230000005291 magnetic effect Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 1
- QVFWZNCVPCJQOP-UHFFFAOYSA-N chloralodol Chemical compound CC(O)(C)CC(C)OC(O)C(Cl)(Cl)Cl QVFWZNCVPCJQOP-UHFFFAOYSA-N 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000010845 search algorithm Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000003936 working memory Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Manipulator (AREA)
Abstract
本申请的目的是提供一种机器人的路径规划方法及设备,本申请通过获取所述机器人的当前位置和待抵达的目的地;获取与所述机器人的当前位置所属场景对应的地图文件;基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图;基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,其中,所述最优全局路径的起点为所述当前位置,实现了将机器人乘电梯的流程纳入至路径规划中来,并结合机器人所属场景对应的地图文件及其对应的带权重无向图,实现了对机器人的全局路径的最优规划,能够适用于任何多机器人、多电梯的乘梯环境。
Description
技术领域
本申请涉及计算机技术领域,尤其涉及一种机器人的路径规划方法及设备。
背景技术
随着机器人技术的日趋完善,各种服务机器人开始深入人类生活,尤其是在一些商用环境中,机器人面临的一大挑战就是如何乘坐电梯或者如何换乘电梯等。受限于运算性能和运动模型,服务机器人一般只能在同一楼层中进行路径规划,在进行跨楼层运动时,往往都需要通过地图中标记的电梯信息,进行地图和坐标的转换,再通过一套额外的子系统进行乘电梯。
现有技术中对于机器人乘梯的研究重点往往在于机器人和电梯的一对一交互以及电梯状态的获取上,对于乘电梯方案,尤其是需要在不同楼层、不同建筑中进行换乘,以及多机器人、多电梯的部署环境,目前尚未提出普适性的解决方案,大多是通过枚举场景来实现有限的换乘。但在一些复杂的场景中,比如有多部电梯分别前往低楼层和高楼层,或者分别停靠奇数层和偶数层,以及多幢大楼通过大厅或者空中连廊联通起来,这样的系统就难以满足机器人的乘电梯或者换乘需求。
发明内容
本申请的一个目的是提供一种机器人的路径规划方法及设备,实现了针对多电梯和多机器人的环境下的,机器人的乘梯路径的全局规划。
根据本申请的一个方面,提供了一种机器人的路径规划方法,其中,所述方法包括:
获取所述机器人的当前位置和待抵达的目的地;
获取与所述机器人的当前位置所属场景对应的地图文件;
基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图;
基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,其中,所述最优全局路径的起点为所述当前位置。
进一步地,上述方法中,所述地图文件分层存储,包括任意数量和类型的地图层,不同的地图层之间相互独立,每个地图层包含地图用途、楼层信息、电梯信息及数据格式。
进一步地,上述方法中,所述基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图,包括:
在所述地图文件中,根据所述地图文件中的楼层信息和电梯信息,以电梯的停靠点作为节点,将能直达所述地图文件中的其他层的电梯的路径和在同一楼层内的任意两个电梯间的路径均作为边,并预置每条边对应的权重值,以构造出与所述地图文件对应的带权重无向图。
进一步地,上述方法中,所述预置每条边对应的权重值,包括:
当所述边为电梯能直达所述地图文件中的其他楼层的电梯的路径,则将跨楼层的电梯连通的边的权重值设置为第一权重阈值;
当所述边中的任一节点对应的电梯被占用时,则将包含有被占用电梯所属的边的权重值设置为第二权重阈值,其中,所述第二权重阈值大于所述第一权重阈值;
当所述边为同一楼层内的任意两个电梯间的路径,则根据同一楼层内的两个电梯间的路径的实际距离,确定所述同一楼层内的两个电梯间的路径形成的边的权重值,其中,所述同一楼层内的两个电梯间的路径形成的边的权重值大于零且小于所述第一权重阈值。
进一步地,上述方法中,所述根据同一楼层内的两个电梯间的路径的实际距离,确定所述同一楼层内的两个电梯间的路径形成的边的权重值的计算公式为:
其中,threshold表示同一楼层内的两个电梯间的路径的距离阈值,w1为所述第一权重阈值,x为同一楼层内的两个电梯间的路径的实际距离。
进一步地,上述方法中,所述基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,包括:
步骤一、构造一个数组G用于表示从起点s到带权重无向图中的每个顶点v的路径和距离,其中,距离为起点s到每个顶点v之间的权重值相加,v.d表示从起点s到顶点v的距离,v.p表示到从起点s到顶点v需要经过的上一个顶点,当顶点为s时,s.d为0,s.p为空,其他顶点的d属性为无穷大,p属性为空;
步骤二、初始化一个空集S,用于存储已确定的最优全局路径的各个顶点;
步骤三、初始化一个用带权重无向图中的顶点v的d属性进行比较的最小优先队列Q,用于存储待访问的顶点;初始时,把起点s放入Q中,此时,Q中只包含起点s;
步骤四、如果Q为空,跳到步骤五;
否则,开始第一次循环,从Q中弹出起点s,放入S,对于和起点s相邻的顶点,更新d属性和p属性,然后将与起点s相邻的全部的顶点压入Q,Q中的每个顶点的键值为起点s到各个顶点之间的最短距离;
第二次循环,从Q中弹出距离最小的一个顶点u1,放入S,若顶点u1为目的地且距离小于等于第一权重阈值,则循环结束,起点s到顶点u1之间的路径为所述机器人规划抵达所述目的地的最优全局路径,否则对于和u1相邻的所有不在S中的顶点v1,如果v1.d>u1.d+w(u1,v1),其中,w(u1,v1)为顶点u1与顶点v1之间的边的权重值,则v1.d=u1.d+w(u1.v),v1.p=u1,然后把v1压入Q,此时,Q中包含和s以及u1相邻的所有顶点;
第三次循环,从Q中弹出距离最小的一个顶点u2,放入S,若顶点u2为目的地且距离小于等于所述第一权重阈值,则循环结束,根据u2.p依次回溯形成最优全局路径,可能的路径有2种s->u1->u2,或者s->u2,若顶点u2不是目的地,对于和顶点u2相邻的所有不在S中的顶点v2,若v2.d>u2.d+w(u2,v2),w(u2,v2)为顶点u2与顶点v2之间的边的权重值,则v2.d=u2.d+w(u2.v2),v2.p=u2,然后把v2压入Q;
按照上述第三次循环的循环执行逻辑继续循环,每次循环都从Q中弹出距离最小的一个顶点放到S,同时把和每次从Q中弹出的距离最小的顶点相邻的其他顶点放入Q,直到Q为空或者放入S的顶点为目的地,则循环结束;
步骤五、在数组G中找到所述目的地所在的顶点t,如果t.d为无穷大,则表示路径搜路失败,否则依次回溯带权重无向图中的每个顶点的p属性并存入数组P,然后,反转数组P就可以得到完整的路径,其中的每个元素表示沿途需要经过的顶点。
进一步地,上述方法中,所述方法还包括:
基于所述最优全局路径,控制所述机器人从所述当前位置移动至所述目的地。
进一步地,上述方法中,所述最优全局路径包括至少一个按序的阶段路径,每一所述阶段路径包括对应的目标楼层和目标地点,当前的阶段路径中的目的地点为下一阶段路径中的出发地点;其中,所述基于所述最优全局路径,控制所述机器人从所述当前位置移动至所述目的地,包括:
基于所述最优全局路径,从所述机器人的当前位置出发,重复如下步骤,依次完成每一所述阶段路径,直至控制所述机器人移动至所述最优全局路径中的最后一阶段路径中的目标地点:
若所述机器人需乘坐电梯,控制所述机器人前往当前位置所属的初始电梯的停靠点;
对所述初始电梯发出呼叫指令以呼叫电梯,若所述初始电梯返回的状态为忙碌,则更新所述带权重无向图,并更新所述最优全局路径,否则持续查询所述初始电梯的状态并等待;
所述初始电梯到达后,向所述初始电梯发出进电梯开门指令,以控制所述机器人进所述初始电梯;若进电梯失败,则回到所述初始电梯的停靠点,并重新呼叫和进电梯,直至进电梯成功;
在所述初始电梯内,发出控制所述初始电梯至当前阶段路径中的目标楼层的运行指令,并持续查询所述初始电梯的状态;
所述初始电梯到达所述目标楼层后,向所述初始电梯发出出电梯开门指令,以控制所述机器人移动出所述初始电梯;若出电梯失败,则重新呼叫和出电梯直至出电梯成功;
响应于对所述机器人进行的移动指令,控制所述机器人移动至当前阶段路径中的目标地点。
根据本申请的另一方面,还提供了一种非易失性存储介质,其上存储有计算机可读指令,所述计算机可读指令可被处理器执行时,使所述处理器实现如上述机器人的路径规划方法。
根据本申请的另一方面,还提供了一种机器人的路径规划设备,其中,该设备包括:
一个或多个处理器;
计算机可读介质,用于存储一个或多个计算机可读指令,
当所述一个或多个计算机可读指令被所述一个或多个处理器执行,使得所述一个或多个处理器实现如上述机器人的路径规划方法。
与现有技术相比,本申请通过获取所述机器人的当前位置和待抵达的目的地;获取与所述机器人的当前位置所属场景对应的地图文件;基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图;基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,其中,所述最优全局路径的起点为所述当前位置,实现了将机器人乘电梯的流程纳入至路径规划中来,并结合机器人所属场景对应的地图文件及其对应的带权重无向图,实现了对机器人的全局路径的最优规划,能够适用于任何多机器人、多电梯的乘梯环境。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述,本申请的其它特征、目的和优点将会变得更明显:
图1示出根据本申请一个方面的一种机器人的路径规划方法的流程示意图;
图2示出根据本申请一个方面的一种机器人的路径规划方法在实际应用场景中的地图文件的示例图;
图3示出根据本申请一个方面的一种机器人的路径规划方法在实际应用场景中的带权重无向图的示例图。
附图中相同或相似的附图标记代表相同或相似的部件。
具体实施方式
下面结合附图对本申请作进一步详细描述。
在本申请一个典型的配置中,终端、服务网络的设备和可信方均包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器,随机存取存储器(RAM)和/或非易失性内存等形式,如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括,但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带,磁带磁盘存储或其他磁性存储设备或任何其他非传输介质,可用于存储可以被计算设备访问的信息。按照本文中的界定,计算机可读介质不包括非暂存电脑可读媒体(transitory media),如调制的数据信号和载波。
如图1所示,本申请的一个方面提出了一种机器人的路径规划方法,所述方法包括步骤S11、步骤S12、步骤S13和步骤S14,具体包括如下步骤:
步骤S11,获取所述机器人的当前位置和待抵达的目的地;
步骤S12,获取与所述机器人的当前位置所属场景对应的地图文件;在此,所述机器人的当前位置所属场景可以包括但不限于办公大楼、商场、学校及会展等可通过机器人移动到达的区域,当然,为了适用范围更广,所述机器人的当前位置所属场景中可以包括一部或多部电梯,以供机器人通过电梯到达不同的楼层。
步骤S13,基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图;
步骤S14,基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,其中,所述最优全局路径的起点为所述当前位置。在此,若所述目的地与所述机器人的当前位置在不同的楼层,所述最优全局路径还包含换乘电梯的阶段路径。
在上述步骤S11至步骤S14中,通过获取所述机器人的当前位置和待抵达的目的地;获取与所述机器人的当前位置所属场景对应的地图文件;基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图;基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,其中,所述最优全局路径的起点为所述当前位置,实现了将机器人乘电梯的流程纳入至路径规划中来,并结合机器人所属场景对应的地图文件及其对应的带权重无向图,实现了对机器人的全局路径的最优规划,能够适用于任何多机器人、多电梯的乘梯环境。
在此,本申请的一种机器人的路径规划方法适用于普适性、完备的机器人换乘电梯的规划,包括地图管理器、跨楼层路径规划器、电梯交互模块及机器人运动规划系统构成。其中,地图管理器负责加载并解析地图文件,地图文件采用层级结构,每一层包含一个楼层的地图数据、若干个电梯编号、楼层名称、建筑名称等;地图管理器加载地图文件后,以电梯在每层的停靠点为节点,以电梯以及同楼层电梯间的路径为边,构造出与地图文件对应的带权重无向图;在与地图文件对应的带权重无向图的基础上,跨楼层路径规划器通过最短路径搜索算法(比如Dijkstra算法等)找出最合理的电梯换乘对应的全局路径;电梯交互模块用于呼叫电梯和控制电梯门,同时支持云端通信和近场通信方式,该电梯交互模块可以通过梯控的反馈得知电梯是否被别的机器人占用,从而更新这部电梯的权重,使得跨楼层路径规划器优先选择别的电梯;机器人运动规划系统实现了在近似平面上的移动机器人运动控制算法。在跨楼层路径规划器得到整体的最优全局路径的规划后,该系统负责实现每一阶段的运动控制,包括进电梯、出电梯、在同楼层内的路径规划和运动控制等。
接着本申请的上述实施例,所述步骤S12中的所述地图文件分层存储,包括任意数量和类型的地图层,不同的地图层之间相互独立,每个地图层包含地图用途、楼层信息、电梯信息及数据格式。例如,为了保持良好的扩展性和兼容性,本申请实施例中的地图文件采用分层存储的文件格式,地图文件中包含一段元数据和若干数量的地图层,每个地图层代表实际应用场景中的不同类型,比如街道、建筑、公园及绿地等类型。元数据为若干字符串格式的键值对,允许任意扩展,比如,每个地图层包括该地图层的地图用途、数据格式、楼层信息及电梯信息等元数据,以及各电梯和楼层的位置信息等数据,不同的地图层之间相互独立,即地图文件可以包含任意数量和类型的地图层。
在本申请一优选实施例中,以从机器人加载地图到完成一次跨楼层运动为例,详细描述本申请的机器人的路径规划方法中的初始化运行环境,机器人启动时就默认运行了地图管理器,地图管理器根据机器人当前位置从云端下载地图文件并解析其内容,如图2所示为实际应用场景中的一个地图文件的示例,其中,1楼包含了一个激光探索的栅格地图,一个矩形区域地图,矩形区域地图的每个元素为一个矩形区域及其包含的若干元数据,作为楼层之间的连接纽带的电梯,则也是其中的一种矩形区域,以实现对机器人的路径规划中的初始化运行环境的设置和调用。
接着本申请的上述实施例,所述步骤S13中的所述基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图,具体包括:
通过地图管理器加载完与所述机器人的当前位置所属场景对应的地图文件后,在所述地图文件中,根据所述地图文件中的楼层信息和电梯信息,以电梯的停靠点作为节点,将能直达所述地图文件中的其他层的电梯的路径和在同一楼层内的任意两个电梯间的路径均作为边,并预置每条边对应的权重值,以构造出与所述地图文件对应的带权重无向图,以便后续能够在该所述地图文件对应的带权重无向图的基础上,通过最短路径搜索算法,可以得到最优的电梯换乘的最优全局路径。
接着本实施例,在构建与地图文件对应的带权重无向图的步骤S13中,所述预置带权重无向图中的每条边对应的权重值,具体包括:
当所述边为电梯能直达所述地图文件中的其他楼层的电梯的路径,则将跨楼层的电梯连通的边的权重值设置为第一权重阈值;在此,所述第一权重阈值用于指示途径跨楼层的电梯连通的边的代价值。所述第一权重阈值可以取任意取值的权重值,在本申请一优选实施例中,可以将所述第一权重阈值优选为10。
当所述边中的任一节点对应的电梯被占用时,则将包含有被占用电梯所属的边的权重值设置为第二权重阈值,其中,所述第二权重阈值大于所述第一权重阈值;在此,所述第二权重阈值用于指示途径包含有被占用电梯所属的边的代价值,由于存在电梯被占用的情况,故而,想要途径包含有被占用电梯所属的边的代价值一定比可以直接跨楼层的电梯连通的边的代价值高,以便在形成带权重无向图时,包含有被占用电梯所属的边的权重值比跨楼层的电梯连通的边的权重值高,使得在进行路径规划时,优先考虑可以直接电梯连通的边,而非被占用电梯所属的边。即当某部电梯被别的机器人占用时,则增大该电梯所在边的权重值,同样也可以按照时间段动态调整不同电梯的权重值,如此使用同样的权重值调整的方式就可以得到不同的用于电梯换乘的最优全局路径。需要说明的是,所述第二权重阈值可以取任意大于第一权重阈值的取值,在本申请一优选实施例中,可以将所述第二权重阈值优选为20。
当所述边为同一楼层内的任意两个电梯间的路径,则根据同一楼层内的两个电梯间的路径的实际距离,确定所述同一楼层内的两个电梯间的路径形成的边的权重值,其中,所述同一楼层内的两个电梯间的路径形成的边的权重值大于零且小于所述第一权重阈值。在此,所述同一楼层内的两个电梯间的路径形成的边的权重值可以取在大于零且小于第一权重阈值之间的任意取值,在本申请一优选实施例中,可以将所述同一楼层内的两个电梯间的路径形成的边的权重值优选为大于0且小于第一权重阈值:10。
在本申请的实施例中,所述根据同一楼层内的两个电梯间的路径的实际距离,确定所述同一楼层内的两个电梯间的路径形成的边的权重值的计算公式为:
其中,threshold表示同一楼层内的两个电梯间的路径的距离阈值,w1为所述第一权重阈值,x为同一楼层内的两个电梯间的路径的实际距离。
在本申请的优选实施例中,以从机器人加载地图到完成一次跨楼层运动为例,详细描述本申请的机器人的路径规划方法中的构建包含电梯信息和楼层信息的带权重无向图的过程:以每个电梯在每层的停靠点作为节点,不同的节点间有电梯能够直达时,就加一条边,如果是同一层且连通的,也加一条边。在本优选实施例中,通过电梯连通的边的权重设置为10,当电梯被别的机器人占用时,被占用的机器人所在的边的权重设置为20,同一楼层内的任意两个电梯之间的路径所形成的边的权重值根据两个电梯之间的实际距离取处于0~10之间的权重值,计算公式为:
w为同一楼层内的任意两个电梯之间的路径所形成的边的权重值,threshold表示表示同一楼层内的两个电梯间的路径的距离阈值,该距离阈值可以人为任意配置,当实际距离超过该距离阈值时,同一楼层内的任意两个电梯之间的路径所形成的边的权重值设为10,这样的话,机器人绕远路乘电梯的总代价是前往远处电梯的代价+乘电梯代价+回到目标楼层的当前位置=30,大于乘坐当前电梯的代价,也就避免了机器人绕远路换乘电梯的情况发生;实际距离小于所述距离阈值时,根据实际距离确定的权重值在0~10之间且不包含10,此时在当前电梯被占时,如果别的电梯不太远,则机器人换乘电梯,否则继续等待。如图3所示为一个带权重无向图的示例,图3中表示有A、B两座建筑,通过1楼大厅和5楼走廊连通,A座2部电梯分别停靠奇数层和偶数层,B座1部电梯停靠全部楼层,具体A、B两座建筑所形成的场景对应的地图文件的带权重无向图为图3所示。
接着本申请的上述实施例,机器人在每次执行任务前,进行全局的快楼层路径规划,在构造的带权重无向图的基础上,执行最短路径算法,结合该场景对于路径权重具有预定义的范围,本申请在最短路径搜索算法(比如Dijkstra算法等)基础上进行路径规划和优化,以减少搜索空间,其中,所述步骤S14基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,具体包括:
步骤一、构造一个数组G用来表示从起点s到带权重无向图中的每个顶点v的路径和距离,其中,距离为起点s到每个顶点v之间的权重值相加,v.d表示从起点s到顶点v的距离,v.p表示到从起点s到顶点v需要经过的上一个顶点,当顶点为s时,s.d为0,s.p为空,其他顶点的d属性为无穷大,p属性为空;
步骤二、初始化一个空集S,用于存储已确定的最优全局路径的各个顶点;
步骤三、初始化一个用带权重无向图中的顶点v的d属性进行比较的最小优先队列Q,用于存储待访问的顶点;初始时,把起点s放入Q中,此时,Q中只包含起点s;
步骤四、如果Q为空,则执行步骤五;否则,开始第一次循环,从Q中弹出起点s,放入S,对于和起点s相邻的顶点,更新d属性和p属性,然后将与起点s相邻的全部的顶点压入Q,Q中的每个顶点的键值为起点s到各个顶点之间的最短距离;
第二次循环,从Q中弹出距离最小的一个顶点u1,放入S,若顶点u1为目的地且距离小于等于第一权重阈值,则循环结束,起点s到顶点u1之间的路径为所述机器人规划抵达所述目的地的最优全局路径,否则对于和u1相邻的所有不在S中的顶点v1,如果v1.d>u1.d+w(u1,v1),其中,w(u1,v1)为顶点u1与顶点v1之间的边的权重值,则v1.d=u1.d+w(u1.v),v1.p=u1,将顶点u1放入数组P,然后把v1压入Q,此时,Q中包含和s以及u1相邻的所有顶点;
第三次循环,从Q中弹出距离最小的一个顶点u2,放入S,若顶点u2为目的地且距离小于等于所述第一权重阈值,则循环结束,根据u2.p依次回溯形成最优全局路径,可能的路径有2种s->u1->u2,或者s->u2,若顶点u2不是目的地,对于和顶点u2相邻的所有不在S中的顶点v2,若v2.d>u2.d+w(u2,v2),w(u2,v2)为顶点u2与顶点v2之间的边的权重值,则v2.d=u2.d+w(u2.v2),v2.p=u2,将顶点u2依序放入数组P,然后把v2压入Q;
按照上述第三次循环的循环执行逻辑继续循环,每次循环都从Q中弹出距离最小的一个顶点放到S,同时把和每次从Q中弹出的距离最小的顶点相邻的其他顶点放入Q,直到Q为空或者放入S的顶点为目的地,则循环结束;
步骤五、在数组G中找到所述目的地所在的顶点t,如果t.d为无穷大,则表示路径搜路失败,否则依次回溯带权重无向图中的每个顶点的p属性并依序存入数组P,反转数组P中的所有顶点,就可以得到为所述机器人规划的从起点s抵达所述目的地的最优全局路径,其中,数组P中的每个元素表示沿途需要经过的顶点,使得通过上述步骤一至步骤五,结合构建出的与地图文件对应的带权重无向图,为所述机器人规划出抵达所述目的地的最优全局路径。
接着本申请的上述实施例,本申请一个方面提供的一种机器人的路径规划方法还包括:
基于所述最优全局路径,控制所述机器人从所述当前位置移动至所述目的地。
例如,在进行机器人与电梯之间的交互时,机器人可以通过规划出的最优全局路径,控制该机器人从当前位置移动至目的地,完成机器人前往目的地的移动。当机器人与电梯进行交互时,存在多模式的交互方式,比如,机器人可以同时通过云端服务和近场通信的方式呼叫和控制电梯;与云端服务通信可以通过机器人自带的移动网络或者无线连接网络WIFI,近场通信则支持Lora或者蓝牙BLE等,机器人与电梯之间的多模式通信方式可以确保机器人在电梯内信号不好时也能与电梯保持通信的通畅,从而控制机器人能够顺利达到目的地。
机器人在呼叫电梯时会上传自身的设备ID给电梯,若电梯当前空闲或者在执行当前的机器人的指令,则接受机器人的呼叫指令并作为新指令,若在执行别的机器人的指令,则拒绝当的前机器人的呼梯指令并返回电梯忙碌的状态,此时,当前呼叫电梯的机器人可以更新忙碌状态的电梯构成的边的权重值,然后重新为当前的机器人规划出一条新的最优全局路径。
接着本申请的上述实施例,若最优全局路径为跨楼层移动路径,则所述最优全局路径包括至少一个按序的阶段路径,每一所述阶段路径包括对应的目标楼层和目标地点,当前的阶段路径中的目的地点为下一阶段路径中的出发地点;其中,所述基于所述最优全局路径,控制所述机器人从所述当前位置移动至所述目的地,具体包括:
基于所述最优全局路径,从所述机器人的当前位置出发,重复如下步骤,依次完成每一所述阶段路径,直至控制所述机器人移动至所述最优全局路径中的最后一阶段路径中的目标地点:
若所述机器人需乘坐电梯,控制所述机器人前往当前位置所属的初始电梯的停靠点;
对所述初始电梯发出呼叫指令以呼叫电梯,若所述初始电梯返回的状态为忙碌,则更新所述带权重无向图,并更新所述最优全局路径,否则持续查询所述初始电梯的状态并等待;
所述初始电梯到达后,向所述初始电梯发出进电梯开门指令,以控制所述机器人进所述初始电梯;若进电梯失败,则回到所述初始电梯的停靠点,并重新呼叫和进电梯,直至进电梯成功;
在所述初始电梯内,发出控制所述初始电梯至当前阶段路径中的目标楼层的运行指令,并持续查询所述初始电梯的状态;
所述初始电梯到达所述目标楼层后,向所述初始电梯发出出电梯开门指令,以控制所述机器人移动出所述初始电梯;若出电梯失败,则重新呼叫和出电梯直至出电梯成功;
响应于对所述机器人进行的移动指令,控制所述机器人移动至当前阶段路径中的目标地点。
根据本申请的另一方面,还提供了一种非易失性存储介质,其上存储有计算机可读指令,所述计算机可读指令可被处理器执行时,使所述处理器实现如上述机器人的路径规划方法。
根据本申请的另一方面,还提供了一种机器人的路径规划设备,其中,该设备包括:
一个或多个处理器;
计算机可读介质,用于存储一个或多个计算机可读指令,
当所述一个或多个计算机可读指令被所述一个或多个处理器执行,使得所述一个或多个处理器实现如上述机器人的路径规划方法。
在此,所述机器人的路径规划设备中的各实施例的详细内容,具体可参见上述机器人的路径规划方法的实施例的对应部分,在此,不再赘述。
综上所述,本申请通过获取所述机器人的当前位置和待抵达的目的地;获取与所述机器人的当前位置所属场景对应的地图文件;基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图;基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,其中,所述最优全局路径的起点为所述当前位置,实现了将机器人乘电梯的流程纳入至路径规划中来,并结合机器人所属场景对应的地图文件及其对应的带权重无向图,实现了对机器人的全局路径的最优规划,能够适用于任何多机器人、多电梯的乘梯环境。
需要注意的是,本申请可在软件和/或软件与硬件的组合体中被实施,例如,可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中,本申请的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地,本申请的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中,例如,RAM存储器,磁或光驱动器或软磁盘及类似设备。另外,本申请的一些步骤或功能可采用硬件来实现,例如,作为与处理器配合从而执行各个步骤或功能的电路。
另外,本申请的一部分可被应用为计算机程序产品,例如计算机程序指令,当其被计算机执行时,通过该计算机的操作,可以调用或提供根据本申请的方法和/或技术方案。而调用本申请的方法的程序指令,可能被存储在固定的或可移动的记录介质中,和/或通过广播或其他信号承载媒体中的数据流而被传输,和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此,根据本申请的一个实施例包括一个装置,该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器,其中,当该计算机程序指令被该处理器执行时,触发该装置运行基于前述根据本申请的多个实施例的方法和/或技术方案。
对于本领域技术人员而言,显然本申请不限于上述示范性实施例的细节,而且在不背离本申请的精神或基本特征的情况下,能够以其他的具体形式实现本申请。因此,无论从哪一点来看,均应将实施例看作是示范性的,而且是非限制性的,本申请的范围由所附权利要求而不是上述说明限定,因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本申请内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外,显然“包括”一词不排除其他单元或步骤,单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一,第二等词语用来表示名称,而并不表示任何特定的顺序。
Claims (7)
1.一种机器人的路径规划方法,其中,所述方法包括:
获取所述机器人的当前位置和待抵达的目的地;
获取与所述机器人的当前位置所属场景对应的地图文件;
基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图;
基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,其中,所述最优全局路径的起点为所述当前位置;
其中,所述基于所述地图文件中的楼层信息和电梯信息,构造与所述地图文件对应的带权重无向图,包括:在所述地图文件中,根据所述地图文件中的楼层信息和电梯信息,以电梯的停靠点作为节点,将能直达所述地图文件中的其他层的电梯的路径和在同一楼层内的任意两个电梯间的路径均作为边,并预置每条边对应的权重值,以构造出与所述地图文件对应的带权重无向图;
其中,所述预置每条边对应的权重值,包括:当所述边为电梯能直达所述地图文件中的其他楼层的电梯的路径,则将跨楼层的电梯连通的边的权重值设置为第一权重阈值;当所述边中的任一节点对应的电梯被占用时,则将包含有被占用电梯所属的边的权重值设置为第二权重阈值,其中,所述第二权重阈值大于所述第一权重阈值;当所述边为同一楼层内的任意两个电梯间的路径,则根据同一楼层内的两个电梯间的路径的实际距离,确定所述同一楼层内的两个电梯间的路径形成的边的权重值,其中,所述同一楼层内的两个电梯间的路径形成的边的权重值大于零且小于所述第一权重阈值;
其中,所述根据同一楼层内的两个电梯间的路径的实际距离,确定所述同一楼层内的两个电梯间的路径形成的边的权重值的计算公式为:
其中,threshold表示同一楼层内的两个电梯间的路径的距离阈值,w1为所述第一权重阈值,x为同一楼层内的两个电梯间的路径的实际距离。
2.根据权利要求1所述的方法,其中,所述地图文件分层存储,包括任意数量和类型的地图层,不同的地图层之间相互独立,每个地图层包含地图用途、楼层信息、电梯信息及数据格式。
3.根据权利要求1所述的方法,其中,所述基于所述带权重无向图,为所述机器人规划抵达所述目的地的最优全局路径,包括:
步骤一、构造一个数组G用于表示从起点s到带权重无向图中的每个顶点v的路径和距离,其中,距离为起点s到每个顶点v之间的权重值相加,v.d表示从起点s到顶点v的距离,v.p表示到从起点s到顶点v需要经过的上一个顶点,当顶点为s时,s.d为0,s.p为空,其他顶点的d属性为无穷大,p属性为空;
步骤二、初始化一个空集S,用于存储已确定的最优全局路径的各个顶点;
步骤三、初始化一个用带权重无向图中的顶点v的d属性进行比较的最小优先队列Q,用于存储待访问的顶点;初始时,把起点s放入Q中,此时,Q中只包含起点s;
步骤四、如果Q为空,跳到步骤五;
否则,开始第一次循环,从Q中弹出起点s,放入S,对于和起点s相邻的顶点,更新d属性和p属性,然后将与起点s相邻的全部的顶点压入Q,Q中的每个顶点的键值为起点s到各个顶点之间的最短距离;
第二次循环,从Q中弹出距离最小的一个顶点u1,放入S,若顶点u1为目的地且距离小于等于第一权重阈值,则循环结束,起点s到顶点u1之间的路径为所述机器人规划抵达所述目的地的最优全局路径,否则对于和u1相邻的所有不在S中的顶点v1,如果v1.d>u1.d+w(u1,v1),其中,w(u1,v1)为顶点u1与顶点v1之间的边的权重值,则v1.d=u1.d+w(u1.v),v1.p=u1,然后把v1压入Q,此时,Q中包含和s以及u1相邻的所有顶点;
第三次循环,从Q中弹出距离最小的一个顶点u2,放入S,若顶点u2为目的地且距离小于等于所述第一权重阈值,则循环结束,根据u2.p依次回溯形成最优全局路径,可能的路径有2种s->u1->u2,或者s->u2,若顶点u2不是目的地,对于和顶点u2相邻的所有不在S中的顶点v2,若v2.d>u2.d+w(u2,v2),w(u2,v2)为顶点u2与顶点v2之间的边的权重值,则v2.d=u2.d+w(u2.v2),v2.p=u2,然后把v2压入Q;
按照上述第三次循环的循环执行逻辑继续循环,每次循环都从Q中弹出距离最小的一个顶点放到S,同时把和每次从Q中弹出的距离最小的顶点相邻的其他顶点放入Q,直到Q为空或者放入S的顶点为目的地,则循环结束;
步骤五、在数组G中找到所述目的地所在的顶点t,如果t.d为无穷大,则表示路径搜路失败,否则依次回溯带权重无向图中的每个顶点的p属性并依序存入数组P,反转数组P中的所有顶点,得到为所述机器人规划的从起点抵达所述目的地的最优全局路径。
4.根据权利要求1所述的方法,其中,所述方法还包括:
基于所述最优全局路径,控制所述机器人从所述当前位置移动至所述目的地。
5.根据权利要求4所述的方法,其中,所述最优全局路径包括至少一个按序的阶段路径,每一所述阶段路径包括对应的目标楼层和目标地点,当前的阶段路径中的目的地点为下一阶段路径中的出发地点;其中,所述基于所述最优全局路径,控制所述机器人从所述当前位置移动至所述目的地,包括:
基于所述最优全局路径,从所述机器人的当前位置出发,重复如下步骤,依次完成每一所述阶段路径,直至控制所述机器人移动至所述最优全局路径中的最后一阶段路径中的目标地点:
若所述机器人需乘坐电梯,控制所述机器人前往当前位置所属的初始电梯的停靠点;
对所述初始电梯发出呼叫指令以呼叫电梯,若所述初始电梯返回的状态为忙碌,则更新所述带权重无向图,并更新所述最优全局路径,否则持续查询所述初始电梯的状态并等待;
所述初始电梯到达后,向所述初始电梯发出进电梯开门指令,以控制所述机器人进所述初始电梯;若进电梯失败,则回到所述初始电梯的停靠点,并重新呼叫和进电梯,直至进电梯成功;
在所述初始电梯内,发出控制所述初始电梯至当前阶段路径中的目标楼层的运行指令,并持续查询所述初始电梯的状态;
所述初始电梯到达所述目标楼层后,向所述初始电梯发出出电梯开门指令,以控制所述机器人移动出所述初始电梯;若出电梯失败,则重新呼叫和出电梯直至出电梯成功;
响应于对所述机器人进行的移动指令,控制所述机器人移动至当前阶段路径中的目标地点。
6.一种非易失性存储介质,其上存储有计算机可读指令,所述计算机可读指令可被处理器执行时,使所述处理器实现如权利要求1至5中任一项所述的方法。
7.一种机器人的路径规划设备,其中,该设备包括:
一个或多个处理器;
计算机可读介质,用于存储一个或多个计算机可读指令,
当所述一个或多个计算机可读指令被所述一个或多个处理器执行,使得所述一个或多个处理器实现如权利要求1至5中任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111654927.3A CN114489051B (zh) | 2021-12-30 | 2021-12-30 | 一种机器人的路径规划方法及设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111654927.3A CN114489051B (zh) | 2021-12-30 | 2021-12-30 | 一种机器人的路径规划方法及设备 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN114489051A CN114489051A (zh) | 2022-05-13 |
CN114489051B true CN114489051B (zh) | 2024-05-03 |
Family
ID=81507616
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111654927.3A Active CN114489051B (zh) | 2021-12-30 | 2021-12-30 | 一种机器人的路径规划方法及设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114489051B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115857515B (zh) * | 2023-02-22 | 2023-05-16 | 成都瑞华康源科技有限公司 | 一种agv机器人路线规划方法、系统及存储介质 |
CN117095536B (zh) * | 2023-08-21 | 2024-02-09 | 江苏智慧优视电子科技有限公司 | 一种基于大数据的重卡换电处理系统及方法 |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107450535A (zh) * | 2017-07-31 | 2017-12-08 | 中南大学 | 一种智能运载机器人最优路径混合图论控制规划方法 |
CN110533209A (zh) * | 2018-05-25 | 2019-12-03 | 南京理工大学 | 一种人车分流的停车场多楼层路径规划方法 |
CN111680351A (zh) * | 2020-06-09 | 2020-09-18 | 科大讯飞(苏州)科技有限公司 | 建筑内最短距离确定方法、装置、设备及存储介质 |
CN112698647A (zh) * | 2020-12-14 | 2021-04-23 | 深圳市普渡科技有限公司 | 跨楼层路径规划方法、装置、计算机设备及存储介质 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107289923A (zh) * | 2016-04-01 | 2017-10-24 | 奥的斯电梯公司 | 楼宇内乘梯导航终端、系统及方法 |
-
2021
- 2021-12-30 CN CN202111654927.3A patent/CN114489051B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107450535A (zh) * | 2017-07-31 | 2017-12-08 | 中南大学 | 一种智能运载机器人最优路径混合图论控制规划方法 |
CN110533209A (zh) * | 2018-05-25 | 2019-12-03 | 南京理工大学 | 一种人车分流的停车场多楼层路径规划方法 |
CN111680351A (zh) * | 2020-06-09 | 2020-09-18 | 科大讯飞(苏州)科技有限公司 | 建筑内最短距离确定方法、装置、设备及存储介质 |
CN112698647A (zh) * | 2020-12-14 | 2021-04-23 | 深圳市普渡科技有限公司 | 跨楼层路径规划方法、装置、计算机设备及存储介质 |
Also Published As
Publication number | Publication date |
---|---|
CN114489051A (zh) | 2022-05-13 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN114489051B (zh) | 一种机器人的路径规划方法及设备 | |
CN107644273B (zh) | 一种导航路径规划方法及设备 | |
US10054447B2 (en) | Lazier graph-based path planning for autonomous navigation | |
KR102235168B1 (ko) | 계층적 아키텍처를 갖는 실시간 디바이스 제어 시스템 및 이를 이용한 실시간 로봇 제어 시스템 | |
US10593110B2 (en) | Method and device for computing a path in a game scene | |
KR101621026B1 (ko) | 인코딩된 라우터빌리티 그래프 디스크립션의 정션에서 방향 전환 가능성의 결정 | |
US10107632B2 (en) | Assisted roadmap generation | |
JP5302016B2 (ja) | 空間情報管理システム、及び地図情報サーバ装置、並びに、プログラム | |
MX2009000586A (es) | Control de software basado en ubicacion de dispositivos moviles. | |
CN108806320A (zh) | 反向寻车方法、装置及计算机可读存储介质 | |
CN111650931A (zh) | 多个移动设备的路径确定的方法、装置与存储介质 | |
CN108121330A (zh) | 一种调度方法、调度系统及地图路径规划方法 | |
CN109048909A (zh) | 枝节式路径调度方法、装置、后台服务端及第一机器人 | |
RU2296368C2 (ru) | Способ отсечения линии и способ отображения трехмерного изображения на основе этого способа | |
CN113568400A (zh) | 一种机器人控制方法、装置、电子设备及存储介质 | |
CN114608552A (zh) | 一种机器人建图方法、系统、装置、设备及存储介质 | |
Yayan et al. | Development of indoor navigation software for intelligent wheelchair | |
US10200814B1 (en) | Voronoi diagram-based algorithm for efficient progressive continuous k-nearest neighbor query for moving objects | |
Weyns et al. | Environments for situated multi-agent systems: Beyond infrastructure | |
US11432164B2 (en) | Computer system for providing service consumption plan for efficient service migration in mobile edge computing environments, and method thereof | |
US20220196405A1 (en) | Systems for Generating Indications of Traversable Paths | |
Steuer | High precision 3D indoor routing on reduced visibility graphs | |
Winter et al. | Indoor localization and navigation independent of sensor based technologies | |
CN117086868B (zh) | 机器人及其控制方法、装置、存储介质 | |
CN104634349A (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 | ||
PE01 | Entry into force of the registration of the contract for pledge of patent right |
Denomination of invention: A Path Planning Method and Equipment for Robots Granted publication date: 20240503 Pledgee: Shanghai Rural Commercial Bank Co.,Ltd. Putuo Sub branch Pledgor: SHANGHAI SLAMTEC Co.,Ltd. Registration number: Y2024310000544 |