路径搜索方法、装置、终端设备及存储介质

技术领域

本申请涉及计算机技术领域，具体而言，涉及一种路径搜索方法、装置、终端设备及存储介质。

背景技术

机器人避障是可移动机器人在移动过程中的重要功能。目前，机器人避障方法有多种,而采用基于搜索的运动规划方法来避障被广泛应用，路径搜索算法包括迪杰斯特拉(Dijstra)算法、A*(Astar)算法和跳点搜索(Jump Point Search，JPS)算法等。

在相关技术中，A*算法在工程实践时，由于目标点在障碍物内或者没有到达目标点的路径等多种原因，导致搜索不到路径而使得机器人无法绕过眼前的障碍。此时，A*算法会一直循环直至到达目标点，如果一直没有有效路径就会因搜索路径点过多而影响搜索效率的问题。

发明内容

本申请实施例的目的在于提供一种路径搜索方法、装置、终端设备及存储介质，旨在解决目前路径搜索方法中存在搜索效率低的问题。

第一方面，本申请实施例提供了一种路径搜索方法，包括：

将路径起点和拓展节点加入到预设的优先级队列，拓展节点为与路径起点或其他拓展节点相邻的节点；

基于优先级队列中的节点，进入预设的路径搜索循环；

若优先级队列为空或者路径搜索循环的循环次数达到预设循环次数，则检测是否存在关键节点和关键节点的父节点，关键节点为基于路径搜索循环搜索到的能够绕障的关键位置；

若存在关键节点和关键节点的父节点，则确定起点与关键节点之间的路径为目标路径，并退出路径搜索循环。

在本实施例中，通过预设优先级队列，将路径节点加入到优先级队列进行路径搜索循环的路径搜索，并在优先级队列为空或者路径搜索循环的循环次数达到预设循环次数时，检测是否存在关键节点和关键节点的父节点，从而将路径搜索循环算法的运行时间限制在一定范围内，避免出现路径搜索点过多而降低搜索效率的情况，保证算法运行流畅；若存在关键节点和关键节点的父节点，则确定起点与关键节点之间的路径为目标路径，以实现路径的高效搜索。

在一实现方式中，基于优先级队列中的节点，进入路径搜索循环之后，还包括：

若优先级队列不为空以及路径搜索循环的循环次数未达到预设循环次数，则将优先级队列中代价函数f(n)取值最小的节点作为第一节点，其中f(n)＝g(n)+h(n)，g(n)为路径起点到当前位置实际走过的距离，h(n)为当前位置与目标位置之间的欧式距离；

若第一节点不在障碍区域内以及第一节点与路径起点的距离大于预设距离，则将第一节点作为关键节点。

在上述实现方式中，代价函数取值最小时，说明路径起点、第一节点、目标位置三者组成的路径最短，以及第一节点不在障碍区内，说明第一节点的位置能够绕开路障，以实现避障路径搜索，第一节点与路径起点的距离大于预设距离，说明第一节点没有偏离路径，因此将第一节点作为绕过障碍并通往目标位置的关键节点。

在一实现方式中，若优先级队列不为空以及路径搜索循环的循环次数未达到预设循环次数，则将优先级队列中代价函数f(n)取值最小的节点作为第一节点之后，还包括：

检测第一节点是否为目标位置；

若第一节点为目标位置，则退出路径搜索循环。

在上述实现方式中，若第一节点为目标位置，说明第一节点就是终点，因此退出路径循环，以降低系统运行内存和减少线程占用，从而提高路径搜索效率和导航效率。

在一实现方式中，若优先级队列不为空以及路径搜索循环的循环次数未达到预设循环次数，则将优先级队列中代价函数f(n)取值最小的节点作为第一节点之后，还包括：

将第一节点加入到优先级队列。

在上述实现方式中，将第一节点加入到优先级队列，以便于及时利用路径搜索循环算法对第一节点进行避障识别，保证不会遗漏第一节点和提高路径搜索效率。

在一实现方式中，若第一节点不在障碍区域内以及第一节点与路径起点的距离大于预设距离，则将第一节点作为关键节点之后，还包括：

对第一节点进行节点拓展，得到与第一节点相邻的多个第二节点；

更新第二节点的代价函数f(M)。

在上述实现方式中，此时第一节点为关键节点，所以为了使无人机顺利到达目标位置，确定关键节点的下一个节点，因此需要对与第一节点相邻的第二节点进行识别，具体为通过更新第二节点的代价函数确定关键节点的下一个节点，其中代价函数越小，说明路径越短，确定为下一个节点的优先级越大。

在一实现方式中，更新第二节点的代价函数f(M)，包括：

将第二节点的父节点更新为第一节点；

若第二节点的理论g(M)值为无穷大，或者大于g(A)与MA之和，则确定第二节点的实际g(M)值为g(A)与MA值之和，g(M)为路径起点到第二节点应走的距离，g(A)为路径起点到第一节点实际走过的距离，MA为第一节点与第二节点之间的距离；

确定第二节点的代价函数f(M)为实际g(M)值与h(M)值之和，h(M)为第二节点与目标位置之间的欧式距离。

在上述实现方式中，通过在无人机移动过程中，不断更新第二节点的代价函数，从而实时确定最优的路径节点，优化路径搜索过程。

在一实现方式中，若第二节点的理论g(M)值为无穷大，则还将第二节点加入到优先级队列。

在上述实现过程中，由于存在路障，所以无人机无法计算出第二节点的理论g(M)值，因而使得第二节点的理论g(M)值会出现无穷大的情况，但是第二节点的父节点为第一节点，第一节点为关键节点，所以实际上第二节点是能够使无人机通过的，因此将第二节点加入到优先级队列进行运算，保证不会遗漏第二节点。

第二方面，本申请实施例提供了一种路径搜索装置，包括：

加入模块，用于将路径起点和拓展节点加入到预设的优先级队列，拓展节点为与路径起点或其他拓展节点相邻的节点；

进入模块，用于基于优先级队列中的节点，进入预设的路径搜索循环；

检测模块，用于若优先级队列为空或者路径搜索循环的循环次数达到预设循环次数，则检测是否存在关键节点和关键节点的父节点，关键节点为基于路径搜索循环搜索到的能够绕障的关键位置；

确定模块，用于若存在关键节点和关键节点的父节点，则确定起点与关键节点之间的路径为目标路径，并退出路径搜索循环。

第三方面，本申请实施例提供了一种终端设备，包括存储器及处理器，存储器用于存储计算机程序，处理器运行计算机程序以使终端设备执行第一方面的路径搜索方法。

第四方面，本申请实施例提供了一种计算机可读存储介质，其存储有计算机程序，计算机程序被处理器执行时实现第一方面的路径搜索方法。

可以理解的是，上述第二至第四方面的有益效果可参见第一方面的相关描述，在此不再赘述。

附图说明

为了更清楚地说明本申请实施例的技术方案，下面将对本申请实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本申请的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1为本申请一实施例提供的路径搜索方法的流程示意图；

图2为本申请另一实施例提供的路径搜索方法的流程示意图；

图3为本申请实施例提供的路径搜索装置的结构示意图；

图4为本申请实施例提供的终端设备的结构示意图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行描述。

应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。同时，在本申请的描述中，术语“第一”、“第二”等仅用于区分描述，而不能理解为指示或暗示相对重要性。

如背景技术相关记载，由于目标点在障碍物内或者没有到达目标点的路径等多种原因，导致搜索不到路径而使得机器人无法绕过眼前的障碍，而A*算法会一直循环直至到达目标点，如果一直没有有效路径就会因搜索路径点过多而影响搜索效率的问题。

针对上述现有技术中的问题，本申请提供了一种路径搜索方法，通过预设优先级队列，将路径节点加入到优先级队列进行路径搜索循环的路径搜索，并在优先级队列为空或者路径搜索循环的循环次数达到预设循环次数时，检测是否存在关键节点和关键节点的父节点，从而将路径搜索循环算法的运行时间限制在一定范围内，避免出现路径搜索点过多而降低搜索效率的情况，保证算法运行流畅；若存在关键节点和关键节点的父节点，则确定起点与关键节点之间的路径为目标路径，以实现路径的高效搜索。

实施例一

参见图1，图1示出了本申请实施例提供的一种路径搜索方法的实现流程图。本申请实施例中下述的路径搜索方法可应用于终端设备，终端设备包括但不限于无人机、扫地机器人、巡检机器人等可移动智能设备，为便于描述，终端设备以无人机为例。本申请实施例的路径搜索方法，包括步骤S101至S104，详述如下：

步骤S101，将路径起点和拓展节点加入到预设的优先级队列，拓展节点为与路径起点或其他拓展节点相邻的节点。

在本实施例中，路径起点为无人机出发时的起始点，可以该点作为坐标原点构建坐标系，记录无人机的移动过程。优先级队列用于路径搜索循环筛选路径节点。通过将路径起点和拓展节点加入到预设的优先级队列，以一定优先级进行路径点搜索，从而只要搜索到目标节点，就不再进行路径点搜索，提高搜索效率。

可选地，将成为目标节点的概率大的拓展节点设置更高的运算优先级，例如，对于路径起点的拓展节点，可以将与路径起点最近的拓展节点视为运算优先级最高的节点。应理解，上述优先级的设置方式仅用作示例，在其他实施例中还可以采用其他设置方式设置优先级队列中节点的优先级。

步骤S102，基于优先级队列中的节点，进入预设的路径搜索循环。

在本实施例中，路径搜索循环为基于A*算法的路径搜索循环过程，采用优先级队列进行路径搜索，以有目的性的对节点进行运算，从而减少无效节点的运算，提高路径搜索效率。

步骤S103，若优先级队列为空或者路径搜索循环的循环次数达到预设循环次数，则检测是否存在关键节点和关键节点的父节点，关键节点为基于路径搜索循环搜索到的能够绕障的关键位置。

在本实施例中，优先级队列为空，表示路径搜索循环已经对所有可能是关键节点的拓展节点进行运算；路径搜索循环的循环次数达到预设循环次数，表示循环次数已达上限，继续循环将影响运算。可以理解的是，有些障碍物是暂时的，例如无人机前方有人经过，所以在优先级队列内或预设循环次数内能够使无人机查找出关键节点，因此当优先级队列为空或者路径搜索循环的循环次数达到预设循环次数时，检测是否存在关键节点和关键节点的父节点。当不存在关键节点和关键节点的父节点时，则说明无法绕开障碍物，所以可以使无人机原路返回路径起点，可以重新规划路径。

本实施例通过对优先级队列和路径搜索循环次数进行判断，以将路径搜索循环限制至一定的运算范围内，避免过多的无效运算，提高后续节点的运算效率，同时又避免运算次数太少，导致未绕过暂时的障碍物而原路返回。

步骤S104，若存在关键节点和关键节点的父节点，则确定起点与关键节点之间的路径为目标路径，并退出路径搜索循环。

在本实施例中，当存在关键节点和关键节点的父节点时，说明无人机以搜索到绕过障碍物的路径点，所以更新局部运动路径，并退出路径搜索循环，以减少无效的运算量。

实施例二

在图1实施例的基础上，图2示出了本申请另一实施例提供的路径搜索方法的流程示意图。应理解，与图1实施例相同的步骤，此处不再赘述。

如图2所示，本实施例的路径搜索方法在步骤S102之后还包括S201至S202，详述如下：

步骤S201，若优先级队列不为空以及路径搜索循环的循环次数未达到预设循环次数，则将优先级队列中代价函数f(n)取值最小的节点作为第一节点，其中f(n)＝g(n)+h(n)，g(n)为路径起点到当前位置实际走过的距离，h(n)为当前位置与目标位置之间的欧式距离；

步骤S202，若第一节点不在障碍区域内以及第一节点与路径起点的距离大于预设距离，则将第一节点作为关键节点。

在上述步骤S201和S202中，代价函数取值最小时，说明路径起点、第一节点、目标位置三者组成的路径最短；第一节点不在障碍区内，说明第一节点的位置能够绕开路障，以实现避障路径搜索；第一节点与路径起点的距离大于预设距离，说明第一节点没有偏离路径，因此将第一节点作为绕过障碍并通往目标位置的关键节点。

进一步地，若第一节点在障碍区域内或第一节点与路径起点的距离不大于预设距离，则对其他拓展节点进行运算，以识别出其他拓展节点是否为关键节点。

如图2所示，本实施例的路径搜索方法在步骤S201之后还包括步骤S203，详述如下：

S203，检测第一节点是否为目标位置，若第一节点为目标位置，则退出路径搜索循环。

在上述步骤S203中，若第一节点为目标位置，说明第一节点就是终点，因此退出路径循环，以降低系统运行内存和减少线程占用，从而提高路径搜索效率和导航效率。示例性地，将第一节点的位置坐标与目标位置的位置坐标进行匹配，若两者匹配时，则判定第一节点为目标位置。

如图2所示，本实施例的路径搜索方法在步骤S201之后还包括步骤S204，详述如下：

S204，将第一节点加入到优先级队列。

在本实施例中，将第一节点加入到优先级队列，以便于及时利用路径搜索循环算法对第一节点进行避障识别，保证不会遗漏第一节点和提高路径搜索效率。

如图2所示，本实施例的路径搜索方法在步骤S202之后还包括步骤S205和S206，详述如下：

S205，对第一节点进行节点拓展，得到与第一节点相邻的多个第二节点；

S206，更新第二节点的代价函数f(M)。

在上述步骤S205和S206中，此时第一节点为关键节点，所以为了使无人机顺利到达目标位置，确定关键节点的下一个节点，因此需要对与第一节点相邻的第二节点进行识别，具体为通过更新第二节点的代价函数确定关键节点的下一个节点，其中代价函数越小，说明路径越短，确定为下一个节点的优先级越大。

示例性地，将在第一节点的预设距离范围内多个角度的节点作为拓展节点。应理解，在其他实施例中，节点拓展可以采用其他拓展方式。

如图2所示，本实施例的路径搜索方法在步骤S206包括步骤S207和S209，详述如下：

S207，将第二节点的父节点更新为第一节点；

S208，若第二节点的理论g(M)值为无穷大，或者大于g(A)与MA值之和，则确定第二节点的实际g(M)值为g(A)与MA值之和，g(M)为路径起点到第二节点应走的距离，g(A)为路径起点到第一节点实际走过的距离，MA为第一节点与第二节点之间的距离；

S209，确定第二节点的代价函数f(M)为实际g(M)值与h(M)值之和，h(M)为第二节点与目标位置之间的欧式距离。

在上述步骤S207至S209中，由于无人机路径中存在障碍物，所以第二节点的理论g(M)值可能存在无穷大的情况，也存在因绕障而使路径变长，导致理论g(M)值大于g(A)与MA之和的情况，因此为便于实时更新第二节点真实的代价函数，将第二节点的实际g(M)值确定为g(A)与MA值之和。本实施例通过在无人机移动过程中，不断更新第二节点的代价函数，从而实时确定最优的路径节点，优化路径搜索过程。

进一步地，若第二节点的理论g(M)值小于g(A)与MA值之和，则忽略该第二节点。若第二节点的理论g(M)值等g(A)与MA值之和，则确定第二节点的代价函数f(M)为实际g(M)值与h(M)值之和。

在一实现方式中，若第二节点的理论g(M)值为无穷大，则还将第二节点加入到优先级队列。

在上述实现方式中，由于存在路障，所以无人机无法计算出第二节点的理论g(M)值，因而使得第二节点的理论g(M)值会出现无穷大的情况，但是第二节点的父节点为第一节点，第一节点为关键节点，所以实际上第二节点是能够使无人机通过的，因此将第二节点加入到优先级队列进行运算，保证不会遗漏第二节点。

实施例三

为了执行上述方法实施例对应的方法，以实现相应的功能和技术效果，下面提供一种路径搜索装置。参见图3，图3是本申请实施例提供的一种路径搜索装置的结构框图。本实施例中该装置包括的各模块/单元用于执行图1或图2对应的实施例中的各步骤，具体参见图1或图2以及图1或图2所对应的实施例中的相关描述。为了便于说明，仅示出了与本实施例相关的部分，本申请实施例提供的路径搜索装置，包括：

加入模块301，用于将路径起点和拓展节点加入到预设的优先级队列，拓展节点为与路径起点或其他拓展节点相邻的节点；

进入模块302，用于基于优先级队列中的节点，进入预设的路径搜索循环；

检测模块303，用于若优先级队列为空或者路径搜索循环的循环次数达到预设循环次数，则检测是否存在关键节点和关键节点的父节点，关键节点为基于路径搜索循环搜索到的能够绕障的关键位置；

确定模块304，用于若存在关键节点和关键节点的父节点，则确定起点与关键节点之间的路径为目标路径，并退出路径搜索循环。

在一实现方式中，上述装置还包括：

第一判定模块，用于若优先级队列不为空以及路径搜索循环的循环次数未达到预设循环次数，则将优先级队列中代价函数f(n)取值最小的节点作为第一节点，其中f(n)＝g(n)+h(n)，g(n)为路径起点到当前位置实际走过的距离，h(n)为当前位置与目标位置之间的欧式距离；

第二判定模块，用于若第一节点不在障碍区域内以及第一节点与路径起点的距离大于预设距离，则将第一节点作为关键节点。

在一实现方式中，上述检测模块303，还用于检测第一节点是否为目标位置；

上述装置还包括退出模块，用于若第一节点为目标位置，则退出路径搜索循环。

在一实现方式中，加入模块301，还用于将第一节点加入到优先级队列。

在一实现方式中，上述装置还包括：

拓展模块，用于对第一节点进行节点拓展，得到与第一节点相邻的多个第二节点；

更新模块，用于更新第二节点的代价函数f(M)。

在一实现方式中，更新模块还包括更新单元，用于将第二节点的父节点更新为第一节点；

确定模块304，还用于若第二节点的理论g(M)值为无穷大，或者大于g(A)与MA之和，则确定第二节点的实际g(M)值为g(A)与MA值之和，g(M)为路径起点到第二节点实际走过的距离，g(A)为路径起点到第一节点实际走过的距离，MA为第一节点与第二节点之间的距离；

确定模块304，还用于确定第二节点的代价函数f(M)为实际g(M)值与h(M)值之和，h(M)为第二节点与目标位置之间的欧式距离。

在一实现方式中，加入模块301，还用于若第二节点的理论g(M)值为无穷大，则还将第二节点加入到优先级队列。

实施例四

图4为本申请一实施例提供的终端设备的结构示意图。如图4所示，该实施例的终端设备4包括：至少一个处理器40(图4中仅示出一个)处理器、存储器41以及存储在存储器41中并可在至少一个处理器40上运行的计算机程序42，处理器40执行计算机程序42时实现上述任意方法实施例中的步骤。

终端设备4可以是无人机、扫地机器人、巡检机器人等可移动智能设备。该终端设备可包括但不仅限于处理器40、存储器41。本领域技术人员可以理解，图4仅仅是终端设备4的举例，并不构成对终端设备4的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如还可以包括输入输出设备、网络接入设备等。

所称处理器40可以是中央处理单元(Central Processing Unit，CPU)，该处理器40还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

存储器41在一些实施例中可以是终端设备4的内部存储单元，例如终端设备4的硬盘或内存。存储器41在另一些实施例中也可以是终端设备4的外部存储设备，例如终端设备4上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，存储器41还可以既包括终端设备4的内部存储单元也包括外部存储设备。存储器41用于存储操作系统、应用程序、引导装载程序(BootLoader)、数据以及其他程序等，例如计算机程序的程序代码等。存储器41还可以用于暂时地存储已经输出或者将要输出的数据。

另外，本申请实施例还提供一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现可实现上述任意方法实施例中的步骤。

本申请实施例提供了一种计算机程序产品，当计算机程序产品在终端设备上运行时，使得终端设备执行时实现可实现上述各个方法实施例中的步骤。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，也可以通过其它的方式实现。以上所描述的装置实施例仅仅是示意性的，例如，附图中的流程图和框图显示了根据本申请的多个实施例的装置、方法和计算机程序产品的可能实现的体系架构、功能和操作。流程图或框图中的每个方框可以代表一个模块、程序段或代码的一部分，所述模块、程序段或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现方式中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个连续的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或动作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

另外，在本申请各个实施例中的各功能模块可以集成在一起形成一个独立的部分，也可以是各个模块单独存在，也可以两个或两个以上模块集成形成一个独立的部分。

所述功能如果以软件功能模块的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本申请的实施例而已，并不用于限制本申请的保护范围，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。应注意到：相似的标号和字母在下面的附图中表示类似项，因此，一旦某一项在一个附图中被定义，则在随后的附图中不需要对其进行进一步定义和解释。

以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应所述以权利要求的保护范围为准。

需要说明的是，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。