CN109855622A

CN109855622A - 用于移动机器人的路径搜索方法及设备

Info

Publication number: CN109855622A
Application number: CN201910013242.7A
Authority: CN
Inventors: 倪江荣; 赵永华; 李宇翔
Original assignee: Shanghai Lan Bao Intelligent Technology Co Ltd
Current assignee: Shanghai Lan Bao Intelligent Technology Co Ltd
Priority date: 2019-01-07
Filing date: 2019-01-07
Publication date: 2019-06-07
Anticipated expiration: 2039-01-07
Also published as: CN109855622B

Abstract

本发明的目的是提供一种用于移动机器人的路径搜索方法及设备，利用快速扩展随机树对大型地图进行高效路径搜索的高级策略，用户可以实时在建图时创建RRT或是对大型已知地图进行RRT的创建，用于之后的路径搜索算法中，使得搜索时间大大减小。本发明能够从设定的起点和终点之间迅速搜索一条路径并提供给机器人上层的导航***使用，RRT的核心优势是时间极短，适合用在嵌入式设备中，解决了传统路径搜索算法耗时漫长的痛点。

Description

用于移动机器人的路径搜索方法及设备

技术领域

本发明涉及计算机领域，尤其涉及一种用于移动机器人的路径搜索方法及设备。

背景技术

随着人们生活水平的提高，近些年来各类家用服务型移动机器人呈井喷式涌现，比如扫地机、自动送餐机器人、银行柜员机器人等等。目前服务型机器人在完善基本功能的同时，更多地加入了移动式底盘技术，使产品能够自主移动并进行地图构建和导航。传统的移动机器人使用最短路径算法(Dijkstra)，优点是能够找到最优路径，但有着耗时过长，经常搜索失败等等弊端，不适合用于商业环境和大型家庭环境。

发明内容

本发明的一个目的是提供一种用于移动机器人的路径搜索方法及设备。

根据本发明的一个方面，提供了一种用于移动机器人的路径搜索方法，该方法包括：

获取环境的栅格地图，确定所述栅格地图中的避开障碍物区域的可行走区域；

获取待搜索的路径的起点和终点，基于所述起点和终点，尝试在所述栅格地图中的可行走区域中建立RRT树，其中，所述RRT树包括依次连接的多条边，每条边连接两个顶点，每个顶点位于所述可行走区域，每个顶点包含了所述栅格地图的坐标信息，所以顶点中其中两个顶点分别为起点和终点；

判断所述RRT树是否生成成功，若成功，对所述RRT树进行搜索，尝试得到所述RRT树中由所述起点至终点的一条路径。

进一步的，上述方法中，对所述RRT树进行搜索，尝试得到所述RRT树中由所述起点至终点的一条路径，包括：

依次搜索从起点开始的通过边联通的各个顶点，

如果当前搜索到包含终点的顶点，则停止搜索；

如果没有包含终点的顶点，则继续递归式搜索与所有已经搜索到的顶点通过边联通的其他顶点，如此往复直至遍历完整个RRT树或者搜索到包含终点的顶点。

进一步的，上述方法中，获取待搜索的路径的起点和终点，基于所述起点和终点，尝试在所述栅格地图中的可行走区域中建立RRT树，包括：

步骤S211，以所述起点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为RRT树的新生长的边，该新生长的边的其中一个顶点为所述起点，另一个顶点为当前新生成的顶点；

步骤S212，以当前新生成的顶点为开始点，以所述开始点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为RRT树的新生长的边，该新生长的边的其中一个顶点为所述开始点，另一个顶点为当前新生成的顶点；

重复步骤S212，直到所述RRT树的当前新生成的顶点的接近所述终点，则所述RRT树生长成功，将当前新生成的顶点与所述终点连接成线段，并将该线段保留为RRT树的新生长的边；或者，直到所述RRT树的边密度达到预设阈值，且所述RRT树的当前新生成的顶点的未接近所述终点，则所述RRT树生长失败。

步骤S221，以所述起点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第一RRT树的新生长的边，该新生长的边的其中一个顶点为所述起点，另一个顶点为当前第一新生成的顶点；同时，以所述终点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第二RRT树的新生长的边，该新生长的边的其中一个顶点为所述终点，另一个顶点为当前第二新生成的顶点；

步骤S222，以当前第一新生成的顶点为第一开始点，以所述第一开始点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第一RRT树的第一新生长的边，该第一新生长的边的其中一个顶点为所述第一开始点，另一个顶点为当前第一新生成的顶点；同时，以当前第二新生成的顶点为第二开始点，以所述第二开始点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第二RRT树的第二新生长的边，该第二新生长的边的其中一个顶点为所述第二开始点，另一个顶点为当前第二新生成的顶点。

重复步骤S222，直到所述第一RRT树和第二RRT树有相交的顶点，则所述RRT树生长成功，将所述第一RRT树和第二RRT树保留为所述RRT树；或者，直到所述第一RRT树和/或第二RRT树的边密度达到预设阈值，且所述第一RRT树和第二RRT没有树有相交的顶点，则所述RRT树生长失败。

进一步的，上述方法中，对所述RRT树进行搜索之前，还包括：

若所生成的RRT树中两条连接的边的夹脚大于预设阈值，其中，该第一条边有分别有顶点E和F，该第二条边分别有顶点F和G，F为两条边的公共顶点，

则用A*算法找到顶点E和G之间局部最优路径，并将该局部最优路径保留为所述RRT树中的边，同时，在所述RRT树中去除该夹脚大于预设阈值的两条边。

根据本发明的另一方面，还提供了一种用于移动机器人的路径搜索设备，该设备包括：

第一装置，用于获取环境的栅格地图，确定所述栅格地图中的避开障碍物区域的可行走区域；

第二装置，用于获取待搜索的路径的起点和终点，基于所述起点和终点，尝试在所述栅格地图中的可行走区域中建立RRT树，其中，所述RRT树包括依次连接的多条边，每条边连接两个顶点，每个顶点位于所述可行走区域，每个顶点包含了所述栅格地图的坐标信息，所以顶点中其中两个顶点分别为起点和终点；

第三装置，用于判断所述RRT树是否生成成功，若成功，对所述RRT树进行搜索，尝试得到所述RRT树中由所述起点至终点的一条路径。

进一步的，上述设备中，所述第三装置，用于依次搜索从起点开始的通过边联通的各个顶点，如果当前搜索到包含终点的顶点，则停止搜索；如果没有包含终点的顶点，则继续递归式搜索与所有已经搜索到的顶点通过边联通的其他顶点，如此往复直至遍历完整个RRT树或者搜索到包含终点的顶点。

进一步的，上述设备中，所述第二装置，包括：

第211装置，用于以所述起点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为RRT树的新生长的边，该新生长的边的其中一个顶点为所述起点，另一个顶点为当前新生成的顶点；

第212装置，用于以当前新生成的顶点为开始点，以所述开始点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为RRT树的新生长的边，该新生长的边的其中一个顶点为所述开始点，另一个顶点为当前新生成的顶点；

重复执行所述第212装置，直到所述RRT树的当前新生成的顶点的接近所述终点，则执行第213装置，用于反馈所述RRT树生长成功，将当前新生成的顶点与所述终点连接成线段，并将该线段保留为RRT树的新生长的边；或者，直到所述RRT树的边密度达到预设阈值，且所述RRT树的当前新生成的顶点的未接近所述终点，则执行第223装置，用于反馈所述RRT树生长失败。

进一步的，上述设备中，所述第二装置，包括：

第221装置，用于以所述起点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第一RRT树的新生长的边，该新生长的边的其中一个顶点为所述起点，另一个顶点为当前第一新生成的顶点；同时，以所述终点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第二RRT树的新生长的边，该新生长的边的其中一个顶点为所述终点，另一个顶点为当前第二新生成的顶点；

第222装置，用于以当前第一新生成的顶点为第一开始点，以所述第一开始点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第一RRT树的第一新生长的边，该第一新生长的边的其中一个顶点为所述第一开始点，另一个顶点为当前第一新生成的顶点；同时，以当前第二新生成的顶点为第二开始点，以所述第二开始点为中心，以预设步长为距离，在所述可行走区域随机生成一条线段，如果该线段碰到所述障碍物区域，则取消该线段，如果该线段未碰到所述障碍物区域，则线段生成成功，将该线段保留为第二RRT树的第二新生长的边，该第二新生长的边的其中一个顶点为所述第二开始点，另一个顶点为当前第二新生成的顶点。

重复执行第222，直到所述第一RRT树和第二RRT树有相交的顶点，则执行第223装置，用于反馈所述RRT树生长成功，将所述第一RRT树和第二RRT树保留为所述RRT树；或者，直到所述第一RRT树和/或第二RRT树的边密度达到预设阈值，且所述第一RRT树和第二RRT没有树有相交的顶点，则执行第223装置，用于反馈所述RRT树生长失败。

进一步的，上述设备中，所述第三装置，还用于若所生成的RRT树中两条连接的边的夹脚大于预设阈值，其中，该第一条边有分别有顶点E和F，该第二条边分别有顶点F和G，F为两条边的公共顶点，则用A*算法找到顶点E和G之间局部最优路径，并将该局部最优路径保留为所述RRT树中的边，同时，在所述RRT树中去除该夹脚大于预设阈值的两条边。

与现有技术相比，本发明利用快速扩展随机树(RRT,Rapidly-exploring RandomTree)对大型地图进行高效路径搜索的高级策略，用户可以实时在建图时创建RRT或是对大型已知地图进行RRT的创建，用于之后的路径搜索算法中，使得搜索时间大大减小。本发明能够从设定的起点和终点之间迅速搜索一条路径并提供给机器人上层的导航***使用，RRT的核心优势是时间极短，适合用在嵌入式设备中，解决了传统路径搜索算法耗时漫长的痛点。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1示出本发明一个实施例的在地图中生成的RRT树的示意图；

图2示本发明一个实施例在生成的RRT树中搜索出较优路径的示意图。

附图中相同或相似的附图标记代表相同或相似的部件。

具体实施方式

下面结合附图对本发明作进一步详细描述。

在本申请一个典型的配置中，终端、服务网络的设备和可信方均包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。

内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。

计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。

本发明提供一种用于移动机器人的路径搜索方法，包括：

步骤S1，获取环境的栅格地图，确定所述栅格地图中的避开障碍物区域的可行走区域；

步骤S2，获取待搜索的路径的起点和终点，基于所述起点和终点，尝试在所述栅格地图中的可行走区域中建立RRT树，其中，所述RRT树包括依次连接的多条边，每条边连接两个顶点，每个顶点位于所述可行走区域，每个顶点包含了所述栅格地图的坐标信息，所以顶点中其中两个顶点分别为起点和终点；

步骤S3，判断所述RRT树是否生成成功，若成功，对所述RRT树进行搜索，尝试得到所述RRT树中由所述起点至终点的一条路径。

在此，如图1所示，在地图中白色区域为可行走区域，黑色区域为障碍物区域。当移动机器人获取全局栅格地图或者在建图时获取局部栅格地图时，可以用栅格地图结构创建一个RRT树，RRT树中的每个顶点包含了地图的坐标信息，RRT树中的每条边包含了两个顶点的连接关系。当RRT树生成后，可以根据RRT树的数据结构进行搜索，形成一条路径。

所述顶点包含坐标信息，RRT树中的每条边为两个定点的连接关系，起点和终点也是RRT中的一个特殊的顶点。后续对所述RRT树进行搜索，得到所述RRT树中由所述起点至终点的一条路径，要以此RRT树中经过各个定点并按照RRT树每个定点的边的连接顺序确定。

本发明是一种利用快速扩展随机树(RRT,Rapidly-exploring Random Tree)对大型地图进行高效路径搜索的高级策略，用户可以实时在建图时创建RRT或是对大型已知地图进行RRT的创建，用于之后的路径搜索算法中，使得搜索时间大大减小。本发明能够从设定的起点和终点之间迅速搜索一条路径并提供给机器人上层的导航***使用，RRT的核心优势是时间极短，适合用在嵌入式设备中，解决了传统路径搜索算法耗时漫长的痛点。

另外，所述栅格地图的地图精度比视觉信息要高很多，定位误差也会有很大提升；栅格计算量会小，对嵌入式设备来说运算时间短很多；地图存储空间要小，可以在设备端或者云端记录更多的历史地图，传输地图的时候节省流量。所以基于环境的栅格地图，可以更高效和精确的建立RRT树。

所述栅格地图可以是激光的平面栅格地图。

本发明的用于移动机器人的路径搜索方法一实施例中，步骤S3中，对所述RRT树进行搜索，尝试得到所述RRT树中由所述起点至终点的一条路径，包括：

步骤S31，依次搜索从起点开始的通过边联通的各个顶点，

步骤S32，如果当前搜索到包含终点的顶点，则停止搜索；

步骤S33，如果没有包含终点的顶点，则继续递归式搜索与所有已经搜索到的顶点通过边联通的其他顶点，如此往复直至遍历完整个RRT树或者搜索到包含终点的顶点。

在此，如图2所示，当RRT树生成完毕后，从起点开始以所述RRT树结构的方式进行遍历搜索，即依次搜索从起点开始的通过边联通的各个顶点，如果当前搜索到包含终点的顶点则停止搜索，如果没有包含终点，则继续递归式搜索与所有已经搜索到的顶点通过边联通的其他顶点，如此往复直至遍历完整个RRT树或者搜索到包含终点的顶点，从而可以快速得到所述RRT树中由所述起点至终点的一条路径。

本发明的用于移动机器人的路径搜索方法一实施例中，步骤S2，获取待搜索的路径的起点和终点，基于所述起点和终点，尝试在所述栅格地图中的可行走区域中建立RRT树，包括：

在此，以起点为中心，以一定步长作为距离(该步长可控)，随机生成一条线段，如果该线段碰到障碍物则取消，如果线段内无障碍物则生成成功。如此往复相当于一个不断生长的随机树。直到特定的收敛条件：比如有一个线段(树枝)接近目标点，则把该树枝与目标点相连，或者树的树枝的密度达到一定程度停止生长且没有一个线段(树枝)接近目标点，生长失败。图1中每段线段(边)作为一个树枝，最后会有一条从起点到终点的多个线段的连线则为最终RRT树，从而可以快速的建立后续可供搜索的原始路径。

在此，分别以起点和终点为中心，进行双向RRT树的生长，生成两个树，直到两个树有相交则停止生长，如果树的密度大于一定值并且两个树没有交点，则认为无法找出一条路径并停止生长，这样能够更高效地得到完整的RRT树。

本发明的用于移动机器人的路径搜索方法一实施例中，步骤S3中，对所述RRT树进行搜索之前，还包括：

在此，对于两个顶点间的连线(边)，一般认为是一条直线路径，当两条直线的夹角比较尖锐的时候，则说明路径转角过大，则需要对路径做平滑处理，可以忽略这个两条边的公共顶点，并把前后两个顶点间使用A*算法从新找到局部最优路径，作为所述RRT树中修正后的边，从而后续可以根据修正后的RRT树跟搜索到更有的路径。

具体的，下表为本发明方法和传统的最短路径算法的效能比较：

在得到的路径效果上看，本发明的的方法在获取路径的结果上，如表中的800unit、836unit与现有方法的617unit、742unit比较接近，尤其本发明的经过平滑处理后的方案，得到的738unit和758unit，已经与现有方法的617unit、742unit相当接近；

而且，从上表可以看出，本发明的方法在获取路径的时间上，如表中的4.525秒、2.862秒，远远短于现有方法的354秒、667秒和788秒的时间，从而达到获取路径的效果与现有方案基本接近，但搜索时间大大缩短但效果。

进一步的，上述设备中，所述第二装置，包括：

本发明的各设备实施例的详细内容，具体可参见各方法实施例的对应部分，在此，不再赘述。

显然，本领域的技术人员可以对本申请进行各种改动和变型而不脱离本申请的精神和范围。这样，倘若本申请的这些修改和变型属于本申请权利要求及其等同技术的范围之内，则本申请也意图包含这些改动和变型在内。

需要注意的是，本发明可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中，本发明的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地，本发明的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本发明的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。

另外，本发明的一部分可被应用为计算机程序产品，例如计算机程序指令，当其被计算机执行时，通过该计算机的操作，可以调用或提供根据本发明的方法和/或技术方案。而调用本发明的方法的程序指令，可能被存储在固定的或可移动的记录介质中，和/或通过广播或其他信号承载媒体中的数据流而被传输，和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此，根据本发明的一个实施例包括一个装置，该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该装置运行基于前述根据本发明的多个实施例的方法和/或技术方案。

对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims

1.一种用于移动机器人的路径搜索方法，其中，该方法包括：

2.根据权利要求1所述的方法，其中，对所述RRT树进行搜索，尝试得到所述RRT树中由所述起点至终点的一条路径，包括：

依次搜索从起点开始的通过边联通的各个顶点，

如果当前搜索到包含终点的顶点，则停止搜索；

3.根据权利要求1所述的方法，其中，获取待搜索的路径的起点和终点，基于所述起点和终点，尝试在所述栅格地图中的可行走区域中建立RRT树，包括：

4.根据权利要求1所述的方法，其中，获取待搜索的路径的起点和终点，基于所述起点和终点，尝试在所述栅格地图中的可行走区域中建立RRT树，包括：

5.根据权利要求1～4任一项所述的方法，其中，对所述RRT树进行搜索之前，还包括：

6.一种用于移动机器人的路径搜索设备，其中，该设备包括：

7.根据权利要求6所述的设备，其中，所述第三装置，用于依次搜索从起点开始的通过边联通的各个顶点，如果当前搜索到包含终点的顶点，则停止搜索；如果没有包含终点的顶点，则继续递归式搜索与所有已经搜索到的顶点通过边联通的其他顶点，如此往复直至遍历完整个RRT树或者搜索到包含终点的顶点。

8.根据权利要求6所述的设备，其中，所述第二装置，包括：

9.根据权利要求6所述的设备，其中，所述第二装置，包括：

10.根据权利要求6～9任一项所述的设备，其中，所述第三装置，还用于若所生成的RRT树中两条连接的边的夹脚大于预设阈值，其中，该第一条边有分别有顶点E和F，该第二条边分别有顶点F和G，F为两条边的公共顶点，则用A*算法找到顶点E和G之间局部最优路径，并将该局部最优路径保留为所述RRT树中的边，同时，在所述RRT树中去除该夹脚大于预设阈值的两条边。