CN108876024A - 路径规划、路径实时优化方法及装置、存储介质 - Google Patents

Abstract

本发明实施例提供一种路径规划方法，初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点；随机采样，得到第一随机点X_rand；在树T_a中寻找距离第一随机点X_rand最近的点X_nearest；判断第一随机点X_rand与X_nearest之间的路径是否可行，若可行，则将第一随机点X_rand作为X_new；将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点；判断X_new到所述最佳父节点的路径是否可行，若可行，则判断树T_a、T_b是否相连，若相连，则获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。本发明实施例还提供一种路径实时优化方法及装置、路径规划装置及存储介质。利用本发明实施例可快速找到一个可行路径，并实时优化路径。

Description

路径规划、路径实时优化方法及装置、存储介质

技术领域

本发明涉及数据处理领域，尤其涉及一种路径规划方法及装置、路径实时优化方法及装置、存储介质。

背景技术

本部分旨在为权利要求书及具体实施方式中陈述的本发明实施例的实施方式提供背景或上下文。此处的描述不因为包括在本部分中就承认是现有技术。

随着机器人行业的不断发展，机器人的运动规划越来越受到人们的广泛关注，并已应用于机器臂操作、移动机器人和自动驾驶汽车等多个领域。基本的运动规划问题就是在起点和终点之间找到一个无碰撞且路径代价最小的路径。路径规划算法已经研究了很长一段时间，人们提出了很多的改进算法来加快路径规划搜索速率、提高算法收敛速率(逼近、找到最优解的速度)以及节省计算所需内存。但是，大多数的算法都是针对静态的环境，在动态环境中不适应。机器人往往会处于较复杂的环境中，障碍物或目标不是一直静止的。有一些算法针对动态环境，但是算法搜索速度和收敛速度较慢。

发明内容

鉴于此，有必要提供一种路径规划方法及装置、路径实时优化方法及装置、存储介质，能够快速找到一个可行路径，并实时优化路径。

本发明实施例一方面提供一种路径规划方法，所述路径规划方法包括：

步骤1，初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点；

步骤2，在预设空间内随机采样，得到第一随机点X_rand；

步骤3，在树T_a中寻找距离第一随机点X_rand最近的点X_nearest；

步骤4，利用贪婪搜索策略，判断第一随机点X_rand与X_nearest之间的路径是否可行，若判断结果为是，则进入步骤5；

步骤5，将第一随机点X_rand作为X_new；

步骤6，将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点；

步骤7，判断X_new到所述最佳父节点的路径是否可行，若判断结果为是，则进入步骤8；

步骤8，判断树T_a、T_b是否相连，若判断结果为是，则进入步骤9；

步骤9，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。

进一步的，在本发明实施例提供的上述路径规划方法中，所述判断第一随机点X_rand与X_nearest之间的路径是否可行还包括：

若判断结果为否，则从X_nearest向第一随机点X_rand扩展预设步长，扩展节点为X_new；

判断X_new是否可行；

若X_new不可行，则返回步骤2在预设空间内进行重新随机采样，若X_new可行，则进入步骤6。

进一步的，在本发明实施例提供的上述路径规划方法中，所述判断X_new到所述最佳父节点的路径是否可行还包括：

若判断结果为否，则放弃节点X_new，并返回步骤2在预设空间内进行重新随机采样。

进一步的，在本发明实施例提供的上述路径规划方法中，所述判断树T_a、T_b是否相连还包括：

若判断结果为否，则判断X_b是否等于X_rand，若判断结果为是，则进入步骤11，否则，进入步骤12；

步骤11，将树T_a、T_b进行交换，并返回步骤2；

步骤12，在树T_b中找到距离X_new最近的点X_b；

步骤13，将X_rand赋值为X_b，并返回步骤3。

本发明实施例再一方面还提供一种路径规划装置，所述路径规划装置包括：

初始化模块，用于初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点；

随机采样模块，用于在预设空间内随机采样，得到第一随机点X_rand；

最近点获取模块，在树T_a中获取距离第一随机点X_rand最近的点X_nearest；

第一判断模块，用于利用贪婪搜索策略，判断第一随机点X_rand与X_nearest之间的路径是否可行；

标记模块，用于在判断结果为是时，将第一随机点X_rand作为X_new；

代价计算模块，用于将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点；

第二判断模块，用于判断X_new到所述最佳父节点的路径是否可行；

第三判断模块，用于在判断结果为是时，判断树T_a、T_b是否相连；

路径获取模块，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。

本发明实施例另一方面还提供一种应用于上述的路径实时优化方法，所述路径实时优化方法包括：

步骤1，初始化树T，并以起点作为根节点；

步骤2，获取从起点到目标点的路径；

步骤3，判断所述路径上的两个节点之间的距离是否大于预设值，若判断结果为是，则进入步骤4；

步骤4，在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树T_path中；

步骤5，在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P₀，若判断结果为是，则进入步骤6；

步骤6，在树T_path的所有节点中随机选取一个节点作为第二随机点X_rand；

步骤7，在树T_path中，获取第二随机点X_rand的父节点、

步骤8，判断树T_path中路径节点是否发生变化，若判断结果为是，则返回步骤2。

进一步的，在本发明实施例提供的上述路径实时优化方法中，所述判断p值是否大于常数P₀还包括：

若判断结果为否，则在预设空间内任意选取一个节点作为第三随机点X_rand，并在树T中利用RRT*算法扩展所述第三随机节点X_rand，进入步骤8。

进一步的，在本发明实施例提供的上述路径实时优化方法中，所述获取第二随机点X_rand的父节点包括：

在树T_path中，将第二随机点X_rand赋值给X₀；

依次将X₀的父节点赋值给X₀，并计算X₀与X_rand的路径代价；

若当前路径代价小于上一次计算的路径代价，则令X₀为X_rand的父节点。

进一步的，在本发明实施例提供的上述路径实时优化方法中，所述判断树T_path中路径节点是否发生变化还包括：

若判断结果为否，则返回步骤5。

本发明实施例再一方面还提供一种路径实时优化装置，所述路径实时优化装置包括：

初始化模块，用于初始化树T，并以起点作为根节点；

路径获取模块，用于获取从起点到目标点的路径；

第一判断模块，用于判断所述路径上的两个节点之间的距离是否大于预设值；

节点保存模块，用于在判断结果为是时，在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树T_path中；

第二判断模块，用于在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P₀；

随机选取模块，用于在判断结果为是时，在树T_path的所有节点中随机选取一个节点作为第二随机点X_rand；

父节点获取模块，用于在树T_path中，获取第二随机点X_rand的父节点；

第三判断模块，用于判断树T_path中路径节点是否发生变化。

本发明实施例再一方面还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任意一项所述的路径规划方法的步骤以及上述任意一项所述的路径实时优化方法的步骤。

本发明实施例再一方面还提供一种终端装置，所述终端装置包括处理器，所述处理器用于执行存储器中存储的计算机程序时实现上述任意一项所述的路径规划方法的步骤以及上述任意一项所述的路径实时优化方法的步骤。

本发明实施例提供的路径规划方法及装置、路径实时优化方法及装置和计算机可读存储介质，利用路径规划方法可以找到任意一条可行的路径，继而使用路径实时优化方法能够对当前可行的路径进行实时优化，从而输出优化路径。利用本发明实施例能够以更快的速度搜索出一条无碰撞的可行路径。本发明实施例提供的方法收敛速度快，在短时间内迅速收敛逼近最优路径，实时优化路径。本发明不需要太多的内存来存储节点信息，可以在高维空间如机械臂规划上实现。

附图说明

图1为本发明实施例提供的路径规划方法的流程图。

图2为本发明实施例提供的路径实时优化方法的流程图。

图3为本发明一实施方式的终端的结构示意图。

图4为图3所示的终端的示例性的功能模块图。

图5为本发明另一实施方式的终端的结构示意图。

图6为图5所示的终端的示例性的功能模块图。

主要元件符号说明

终端	1
		存储器	10
显示屏	20
		处理器	30
路径规划装置	100
		路径实时优化装置	200
初始化模块	11、21
		随机采样模块	12
最近点获取模块	13
		第一判断模块	14、23
标记模块	15
		代价计算模块	16
第二判断模块	17、25
		第三判断模块	18、28
路径获取模块	19、22
		节点保存模块	24
随机选取模块	26
		父节点获取模块	27

如下具体实施方式将结合上述附图进一步说明本发明实施例。

具体实施方式

为了能够更清楚地理解本发明实施例的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行详细描述。需要说明的是，在不冲突的情况下，本申请的实施方式中的特征可以相互组合。

在下面的描述中阐述了很多具体细节以便于充分理解本发明实施例，所描述的实施方式仅仅是本发明一部分实施方式，而不是全部的实施方式。基于本发明中的实施方式，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施方式，都属于本发明实施例保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本发明实施例的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施方式的目的，不是旨在于限制本发明实施例。

请参阅图1，图1为本发明实施例提供的路径规划方法的流程图。如图1所示，所述路径规划方法可以包括如下步骤：

S101：初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点。

本实施方式中，连通而不含回路的无向树称为树。树的初始化目的是为了建立一颗空树，初始化树T_a、T_b是指建立空树T_a、T_b。树是n个节点(元素的有限集合)，当n＝0时，称这棵树为空树。树是有且仅有一个树根节点，树的根节点是同一颗树中除本身外所有节点的祖先。将起点作为树T_a的根节点，将目标点作为树T_b的根节点。

S102：在预设空间内随机采样，得到第一随机点X_rand。

本实施方式中，在预设空间内随机选取节点，所述预设空间是指与实际物理空间相互映射的抽象空间，建立预设空间的目的是建立一个便于计算机进行路径规划所使用的空间模型。在预设空间内随机选取的节点作为第一随机点X_rand。

S103：在树T_a中寻找距离第一随机点X_rand最近的点X_nearest。

本实施方式中，在二维平面上，所述在树T_a中寻找距离第一随机点X_rand最近的点X_nearest就是找到与第一随机点X_rand的直线距离最短的点，作为X_nearest。可以理解，在更高维空间中，还可以用欧氏距离来表示。

S104：利用贪婪搜索策略，判断第一随机点X_rand与X_nearest之间的路径是否可行，如果是，则进入步骤S105，否则，执行步骤S1041。

本实施方式中，所述贪婪搜索策略是指所求问题的整体最优解可以通过一系列局部最优的选择来达成。所述贪婪搜索策略的基本思路是从问题的某一个初始解出发一步一步地进行，根据某个优化测度，每一步都要确保能获得局部最优解。每一步只考虑一个数据，它的选取应该满足局部优化的条件。若下一个数据和部分最优解连在一起不再是可行解时，就不把该数据添加到部分解中，直到把所有数据枚举完成，或者不能再添加算法位置。所述判断第一随机点X_rand与X_nearest之间的路径是否可行也就是判断第一随机点X_rand与X_nearest之间是否存在碰撞，若存在碰撞，则表明第一随机点X_rand与X_nearest之间的路径是不可行的。

S1041：从X_nearest向第一随机点X_rand扩展预设步长，扩展节点作为X_new，并判断X_new是否可行，如果判断结果为是，则进入步骤S106，否则，返回步骤S102。

本实施方式中，若第一随机点X_rand与X_nearest之间的路径不可行，则可以选定第一随机点X_rand为一个扩展方向，从X_nearest向第一随机点X_rand方向扩展预设步长，扩展节点作为X_new。所述预设步长可以是本领域技术人员根据经验设定的值。判断X_new是否可行，也即判断X_new是否与障碍物发生碰撞，若发生碰撞，则放弃本次生长，返回步骤S102。若未发生碰撞，则进入步骤S106，将X_new添加到树T_a中。

S105：将第一随机点X_rand作为X_new。

本实施方式中，若第一随机点X_rand与X_nearest之间的路径是可行的，则可以直接将所述第一随机点X_rand作为节点X_new。

S106：将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点。

本实施方式中，在树T_a中寻找与X_new距离在预设范围内的点，并计算出使得预设范围内的点到X_new路径代价最小的点，作为X_new的最佳父节点。在二维平面上，所述路径代价最小也即与X_new的直线距离最小。在更高维空间上，所述路径代价最小也即X_new的欧氏距离最小。所述预设范围可以是本领域技术人员根据经验设定的值。具体的，可以构建关于X_new的代价函数，在距离X_new预设范围内获取使得所述代价函数的函数值最小的点即为使得X_new路径代价最小的点。所述父节点是指若一个节点含有子节点，则这个节点称为其子节点的父节点。

S107：判断X_new到所述最佳父节点的路径是否可行，如果是，则进入步骤S108，否则，进入步骤S1071。

本实施方式中，可以理解，所述X_new到所述最佳父节点的路径是否可行，也即判断X_new到所述最佳父节点的路径是否存在碰撞，若存在碰撞，则表明X_new到所述最佳父节点的路径不可行。若不存在碰撞，则表明X_new到所述最佳父节点的路径可行。

S1071：放弃节点X_new，并返回步骤S102。

S108：判断树T_a、T_b是否相连，如果判断结果为是，则进入步骤S109，否则，进入步骤S1081。

本实施方式中，判断树T_a、T_b是否相连，若树T_a、T_b相连，则可以找到从起点到目标点的可行路径。可以理解，在树T_a的第一次扩展(第一次是指在预设空间内随机采样)之后，此时的树T_a、T_b不相连。在判断得出树T_a、T_b不相连之后，进入步骤S1081以对树T_a进行第二次扩展(第二次是指在树T_b中找到距离X_new最近的点X_b)。在树T_a进行第二次扩展之后，此时树T_a、T_b可能相连，也可能不相连。

S1081：判断X_b是否等于X_rand，若判断结果为是，则进入步骤S1082，否则，进入步骤S1083。

本实施方式中，X_b是指树T_b中最新生成的点。可以理解，在树T_a的第一次扩展(第一次是指在预设空间内随机采样)之后，此时的X_b未被赋值，所以X_b的值为空，因而判断得出X_b不等于X_rand。在判断结果为否时，进入步骤S1083以对X_b进行赋值。

S1082：将树T_a、T_b进行交换，并返回步骤S102。

本实施方式中，将树T_a、T_b进行交换，对树T_a、T_b进行交换也即将变量T_a的名字改为T_b，同时将变量T_b的名字改为T_a。对树T_a、T_b进行交换之后，在本方法中只需要处理树T_a即可，因而可以提高处理速度。

S1083：在树T_b中找到距离X_new最近的点X_b。

本实施方式中，可以理解，在二维空间上，所述在树T_b中找到距离X_new最近的点X_b也即在树T_b中找到与X_new的直线距离最小的点X_b。在更高维空间上，所述在树T_b中找到距离X_new最近的点X_b也即在树T_b中找到与X_new的欧氏距离最小的点X_b。所述在树T_b中找到距离X_new最近的点X_b也即所述对树T_a进行第二次扩展。

S1084：将X_rand赋值为X_b，并返回步骤S103。

本实施方式中，将X_rand赋值为X_b，并返回步骤S103，此时判断树T_a、T_b是否相连，也即判断X_b与X_new之间是否存在碰撞(也即是否存在障碍)，若存在碰撞，则树T_a、T_b不相连。若不存在碰撞，则树T_a、T_b相连，因而可以获得连接起点与目标点的可行路径。

S109：获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。

本发明实施例提供的路径规划方法，能够提高在起点与目标点之间找到任意一条可行路径的速率，且适用于各种障碍物分布的环境，能够用于高维空间内的规划，如机械臂规划。

请参阅图2，图2为本发明实施例提供的路径实时优化方法的流程图。如图2所示，所述路径实时优化方法可以包括如下步骤：

S201：初始化树T，并以起点作为根节点。

本实施方式中，初始化树T，也即建立空树T，将起点作为树T的根节点。

S202：获取从起点到目标点的路径。

本实施方式中，获取从起点到目标点的路径，所述路径是指连接起点位置和目标点位置的序列点或曲线。

S203：判断路径上的两个节点之间的距离是否大于预设值，如果是，则进入步骤S204，否则继续执行本步骤S203或者结束流程。

本实施方式中，所述预设值可以是本领域技术人员根据经验而设定的值。所述两个节点可以为两个相邻的节点。可以理解，在二维空间上，两个相邻节点之间的距离是指直线距离，判断直线距离的值是否大于预设值。在更高维空间上，两个相邻节点之间的距离可以指欧氏距离，判断欧氏距离的值是否大于预设值。

S204：在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树T_path中。

本实施方式中，线性插值是指插值函数为一次多项式的插值方式，其在插值节点上的插值误差为零。若判断得出路径上的两个节点之间的距离大于预设值，则在两个节点之间进行线性插值处理，并将经过线性插值处理后的所有节点保存在树T_path中。可以理解，树T_path中只包含从起点到目标点的可行路径上的节点。而树T中包含了所有的节点信息，包括不在可行路径上的节点。

S205：在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P₀，如果是，则进入步骤S206，否则，进入步骤S2061。

本实施方式中，在基本RRT算法基础上采用一种目标偏向采样策略，提前设定一个目标偏向概率阈值，也即常数P₀。在预设空间内进行随机采样时根据均匀概率分布在区间(0，1)上随机获得一个概率值p，将p值与常数P₀的取值进行比较，进而获得第二/三随机点X_rand。常数P₀为一个(0，1)区间的值，常数P₀是根据本领域技术人员根据经验而设定的值。本实施方式中，选取的常数P₀的值为0.7。

S2061：在预设空间内任意选取一个节点作为第三随机点X_rand，并在树T中利用RRT*算法扩展所述第三随机节点X_rand，进入步骤S208。

本实施方式中，所述在树T中利用RRT*算法扩展所述第三随机节点X_rand可以包括：选定第三随机点X_rand为一个扩展方向，并向第三随机点X_rand的扩展方向扩展一定步长，扩展节点为X₁。判断树T_path中路径节点是否发生变化，若判断结果为是，则返回步骤S202，否则，返回步骤S205。

S206：在树T_path中随机选取一个节点作为第二随机点X_rand。

S207：在树T_path中，获取第二随机点X_rand的父节点。

本实施方式中，所述获取第二随机点X_rand的父节点的方法可以包括：在树T_path中，将第二随机点X_rand赋值为X₀，依次将X₀的父节点赋值给X₀，并计算X₀与X_rand的路径代价；若当前路径代价小于上一次计算的路径代价，则令X₀为X_rand的父节点。

S208:判断树T_path中路径节点是否发生变化，如果是，则返回步骤S202，否则，则返回步骤S205。

本发明实施例提供的路径实时优化方法中，，对于路径的插值处理可以保证路径上节点的数量，有利于路径的优化处理；以一定概率选取路径上的随机节点可以加速路径收敛，且以一定概率选取预设空间的随机点，可以使得路径不会陷入局部最小值。

以上是对本发明实施例所提供的方法进行的详细描述。根据不同的需求，所示流程图中方块的执行顺序可以改变，某些方块可以省略。下面对本发明实施例所提供的路径规划装置以及路径实时优化装置进行描述。

本发明实施例还提供一种终端装置，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述任一实施方式中所述的路径规划方法以及路径实时优化方法的步骤。

图3是本发明一实施方式的终端的结构示意图，如图3所示，终端1包括存储器10，存储器10中存储有路径规划装置100。所述的终端1可以是个人数字助理等具有应用显示功能的终端。所述路径规划装置100可以初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点；在预设空间内随机采样，得到第一随机点X_rand；在树T_a中寻找距离第一随机点X_rand最近的点X_nearest；利用贪婪搜索策略，判断第一随机点X_rand与X_nearest之间的路径是否可行，若判断结果为是，则将第一随机点X_rand作为X_new；将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点；判断X_new到所述最佳父节点的路径是否可行，若判断结果为是，则判断树T_a、T_b是否相连，若判断结果为是，则获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。利用本发明实施例，可以快速找到一个可行路径。

终端1还可以包括显示屏20及处理器30。存储器10、显示屏20可以分别与处理器30电连接。

所述的存储器10可以是不同类型存储设备，用于存储各类数据。例如，可以是终端1的存储器、内存，还可以是可外接于该终端1的存储卡，如闪存、SM卡(Smart Media Card，智能媒体卡)、SD卡(Secure Digital Card，安全数字卡)等。此外，存储器10包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(SmartMedia Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。存储器10存储各类数据，例如，所述终端1中安装的各类应用程序(Applications)、应用上述路径规划方法而设置、获取的数据等信息。

显示屏20安装于终端1，用于显示信息。

处理器30用于执行所述路径规划方法以及所述终端1内安装的各类软件，例如操作系统及应用显示软件等。处理器30包含但不限于处理器(Central Processing Unit，CPU)、微控制单元(Micro Controller Unit，MCU)等用于解释计算机指令以及处理计算机软件中的数据的装置。

所述的路径规划装置100可以包括一个或多个的模块，所述一个或多个模块被存储在终端1的存储器10中并被配置成由一个或多个处理器(本实施方式为一个处理器30)执行，以完成本发明实施例。例如，参阅图4所示，所述路径规划装置100可以包括初始化模块11、随机采样模块12、最近点获取模块13、第一判断模块14、标记模块15、代价计算模块16、第二判断模块17、第三判断模块18、路径获取模块19。本发明实施例所称的模块可以是完成一特定功能的程序段，比程序更适合于描述软件在处理器中的执行过程。

可以理解的是，对应上述路径规划方法中的各实施方式，终端1可以包括图4中所示的各功能模块中的一部分或全部，各模块的功能将在以下具体介绍。需要说明的是，以上路径规划方法的各实施方式中相同的名词相关名词及其具体的解释说明也可以适用于以下对各模块的功能介绍。为节省篇幅及避免重复起见，在此就不再赘述。

初始化模块11可以用于初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点。

随机采样模块12可以用于在预设空间内随机采样，得到第一随机点X_rand。

最近点获取模块13可以用于在树T_a中获取距离第一随机点X_rand最近的点X_nearest。

第一判断模块14可以用于利用贪婪搜索策略，判断第一随机点X_rand与X_nearest之间的路径是否可行。

标记模块15可以用于在判断结果为是时，将第一随机点X_rand作为X_new。

代价计算模块16可以用于将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点。

第二判断模块17可以用于判断X_new到所述最佳父节点的路径是否可行。

第三判断模块18可以用于在判断结果为是时，判断树T_a、T_b是否相连，若判断结果为是，则进入步骤9中。

路径获取模块19可以用于获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。

图5是本发明另一实施方式的终端的结构示意图。如图5所示，终端1包括存储器10，存储器10中存储有路径实时优化装置200。终端1还可以包括显示屏20及处理器30。存储器10、显示屏20可以分别与处理器30电连接。所述的路径实时优化装置200可以包括一个或多个的模块，所述一个或多个模块被存储在终端1的存储器10中并被配置成由一个或多个处理器(本实施方式为一个处理器30)执行，以完成本发明实施例。例如，参阅图6所示，所述路径实时优化装置200可以包括初始化模块21、路径获取模块22、第一判断模块23、节点保存模块24、第二判断模块25、随机选取模块26、父节点获取模块27、第三判断模块28。可以理解的是，对应上述路径实时优化方法中的各实施方式，终端1可以包括图6中所示的各功能模块中的一部分或全部，各模块的功能将在以下具体介绍。

初始化模块21可以用于初始化树T，并以起点作为根节点。

路径获取模块22可以用于获取从起点到目标点的路径。

第一判断模块23可以用于判断所述路径上的两个节点之间的距离是否大于预设值。

节点保存模块24可以用于在判断结果为是时，在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树T_path中。

第二判断模块25可以用于在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P₀。

随机选取模块26可以用于在判断结果为是时，在树T_path的所有节点中随机选取一个节点作为第二随机点X_rand。

父节点获取模块27可以用于在树T_path中，获取第二随机点X_rand的父节点。

第三判断模块28可以用于判断树T_path中路径节点是否发生变化。

本发明实施例还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一实施方式中的路径规划方法以及路径实时优化方法的步骤。

所述路径规划装置/路径实时优化装置/终端/计算机设备集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施方式方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读存储介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。

所称处理器可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等，所述处理器是所述路径规划装置/路径实时优化装置/终端的控制中心，利用各种接口和线路连接整个路径规划装置/路径实时优化装置/终端的各个部分。

所述存储器用于存储所述计算机程序和/或模块，所述处理器通过运行或执行存储在所述存储器内的计算机程序和/或模块，以及调用存储在存储器内的数据，实现所述路径规划装置/路径实时优化装置/终端的各种功能。所述存储器可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序(比如声音播放功能、图像播放功能等)等；存储数据区可存储根据手机的使用所创建的数据(比如音频数据、电话本等)等。此外，存储器可以包括高速随机存取存储器，还可以包括非易失性存储器，例如硬盘、内存、插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)、至少一个磁盘存储器件、闪存器件、或其他易失性固态存储器件。

在本发明所提供的几个具体实施方式中，应该理解到，所揭露的终端和方法，可以通过其它的方式实现。例如，以上所描述的系统实施方式仅仅是示意性的，例如，所述模块的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。

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

以上实施方式仅用以说明本发明实施例的技术方案而非限制，尽管参照以上较佳实施方式对本发明实施例进行了详细说明，本领域的普通技术人员应当理解，可以对本发明实施例的技术方案进行修改或等同替换都不应脱离本发明实施例的技术方案的精神和范围。

Claims (10)

1.一种路径规划方法，其特征在于，所述路径规划方法包括：

步骤1，初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点；

步骤2，在预设空间内随机采样，得到第一随机点X_rand；

步骤3，在树T_a中获取距离第一随机点X_rand最近的点X_nearest；

步骤4，利用贪婪搜索策略，判断第一随机点X_rand与X_nearest之间的路径是否可行，若判断结果为是，则进入步骤5；

步骤5，将第一随机点X_rand作为X_new；

步骤6，将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点；

步骤7，判断X_new到所述最佳父节点的路径是否可行，若判断结果为是，则进入步骤8；

步骤8，判断树T_a、T_b是否相连，若判断结果为是，则进入步骤9；

步骤9，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。

2.根据权利要求1所述的路径规划方法，其特征在于，所述判断第一随机点X_rand与X_nearest之间的路径是否可行还包括：

若判断结果为否，则从X_nearest向第一随机点X_rand扩展预设步长，扩展节点为X_new；

判断X_new是否可行；

若X_new不可行，则返回步骤2在预设空间内进行重新随机采样，若X_new可行，则进入步骤6。

3.根据权利要求1所述的路径规划方法，其特征在于，所述判断X_new到所述最佳父节点的路径是否可行还包括：

若判断结果为否，则放弃节点X_new，并返回步骤2在预设空间内进行重新随机采样。

4.根据权利要求1所述的路径规划方法，其特征在于，所述判断树T_a、T_b是否相连还包括：

若判断结果为否，则判断X_b是否等于X_rand，若判断结果为是，则进入步骤11，否则，进入步骤12；

步骤11，将树T_a、T_b进行交换，并返回步骤2；

步骤12，在树T_b中找到距离X_new最近的点X_b；

步骤13，将X_rand赋值为X_b，并返回步骤3。

5.一种路径规划装置，其特征在于，所述路径规划装置包括：

初始化模块，用于初始化树T_a、T_b，并分别以起点和目标点作为树T_a、T_b的根节点；

随机采样模块，用于在预设空间内随机采样，得到第一随机点X_rand；

最近点获取模块，在树T_a中获取距离第一随机点X_rand最近的点X_nearest；

第一判断模块，用于利用贪婪搜索策略，判断第一随机点X_rand与X_nearest之间的路径是否可行；

标记模块，用于在判断结果为是时，将第一随机点X_rand作为X_new；

代价计算模块，用于将X_new添加到树T_a中，并在树T_a中获取与X_new距离在预设范围内的点，将计算出预设范围内的点到X_new路径代价最小的点作为X_new的最佳父节点；

第二判断模块，用于判断X_new到所述最佳父节点的路径是否可行；

第三判断模块，用于在判断结果为是时，判断树T_a、T_b是否相连；

路径获取模块，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。

6.一种应用于权利要求1中所述的路径实时优化方法，其特征在于，所述路径实时优化方法包括：

步骤1，初始化树T，并以起点作为根节点；

步骤2，获取从起点到目标点的路径；

步骤3，判断所述路径上的两个节点之间的距离是否大于预设值，若判断结果为是，则进入步骤4；

步骤4，在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树T_path中；

步骤5，在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P₀，若判断结果为是，则进入步骤6；

步骤6，在树T_path的所有节点中随机选取一个节点作为第二随机点X_rand；

步骤7，在树T_path中，获取第二随机点X_rand的父节点；

步骤8，判断树T_path中路径节点是否发生变化，若判断结果为是，则返回步骤2。

7.根据权利要求6所述的路径实时优化方法，其特征在于，所述判断p值是否大于常数P₀还包括：

若判断结果为否，则在预设空间内任意选取一个节点作为第三随机点X_rand，并在树T中利用RRT*算法扩展所述第三随机节点X_rand，进入步骤8。

8.根据权利要求6所述的路径实时优化方法，其特征在于，所述获取第二随机点X_rand的父节点包括：

在树T_path中，将第二随机点X_rand赋值给X₀；

依次将X₀的父节点赋值给X₀，并计算X₀与X_rand的路径代价；

若当前路径代价小于上一次计算的路径代价，则令X₀为X_rand的父节点。

9.根据权利要求6所述的路径实时优化方法，其特征在于，所述判断树T_path中路径节点是否发生变化还包括：

若判断结果为否，则返回步骤5。

10.一种路径实时优化装置，其特征在于，所述路径实时优化装置包括：

初始化模块，用于初始化树T，并以起点作为根节点；

路径获取模块，用于获取从起点到目标点的路径；

第一判断模块，用于判断所述路径上的两个节点之间的距离是否大于预设值；

节点保存模块，用于在判断结果为是时，在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树T_path中；

第二判断模块，用于在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P₀；

随机选取模块，用于在判断结果为是时，在树T_path的所有节点中随机选取一个节点作为第二随机点X_rand；

父节点获取模块，用于在树T_path中，获取第二随机点X_rand的父节点；

第三判断模块，用于判断树T_path中路径节点是否发生变化。