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

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

Path Planning, Real-time Path Optimization, Devices and Storage Media

The embodiment of the present invention provides a path planning method, which initializes the tree Ta and Tb, and takes the starting point and the target point as the root nodes of the tree Ta and Tb respectively; randomly sampled to obtain the first random point Xrand; searched for the nearest point Xnearest to the first random point Xrand in the tree Ta; and judged the path between the first random point Xrand and Xnearest rest. Whether it is feasible or not, the first random point Xrand is regarded as Xnew; Xnew is added to the tree Ta, and the points with the distance from Xnew in the preset range are obtained in the tree Ta, and the point with the least cost of calculating the preset range to the Xnew path is regarded as the best parent node of Xnew; whether the path from Xnew to the best parent node is feasible or not is judged. If feasible, then judge whether the tree Ta and Tb are connected, if connected, then obtain the feasible path, and use the real-time path optimization method to optimize the feasible path in real time. The embodiment of the invention also provides a real-time path optimization method and device, a path planning device and a storage medium. The embodiment of the invention can quickly find a feasible path and optimize the path in real time.

【技术实现步骤摘要】
路径规划、路径实时优化方法及装置、存储介质
本专利技术涉及数据处理领域，尤其涉及一种路径规划方法及装置、路径实时优化方法及装置、存储介质。
技术介绍
本部分旨在为权利要求书及具体实施方式中陈述的本专利技术实施例的实施方式提供背景或上下文。此处的描述不因为包括在本部分中就承认是现有技术。随着机器人行业的不断发展，机器人的运动规划越来越受到人们的广泛关注，并已应用于机器臂操作、移动机器人和自动驾驶汽车等多个领域。基本的运动规划问题就是在起点和终点之间找到一个无碰撞且路径代价最小的路径。路径规划算法已经研究了很长一段时间，人们提出了很多的改进算法来加快路径规划搜索速率、提高算法收敛速率(逼近、找到最优解的速度)以及节省计算所需内存。但是，大多数的算法都是针对静态的环境，在动态环境中不适应。机器人往往会处于较复杂的环境中，障碍物或目标不是一直静止的。有一些算法针对动态环境，但是算法搜索速度和收敛速度较慢。
技术实现思路
鉴于此，有必要提供一种路径规划方法及装置、路径实时优化方法及装置、存储介质，能够快速找到一个可行路径，并实时优化路径。本专利技术实施例一方面提供一种路径规划方法，所述路径规划方法包括：步骤1，初始化树Ta、Tb，并分别以起点和目标点作为树Ta、Tb的根节点；步骤2，在预设空间内随机采样，得到第一随机点Xrand；步骤3，在树Ta中寻找距离第一随机点Xrand最近的点Xnearest；步骤4，利用贪婪搜索策略，判断第一随机点Xrand与Xnearest之间的路径是否可行，若判断结果为是，则进入步骤5；步骤5，将第一随机点Xrand作为Xnew；步骤6，将Xnew添加到树Ta中，并在树Ta中获取与Xnew距离在预设范围内的点，将计算出预设范围内的点到Xnew路径代价最小的点作为Xnew的最佳父节点；步骤7，判断Xnew到所述最佳父节点的路径是否可行，若判断结果为是，则进入步骤8；步骤8，判断树Ta、Tb是否相连，若判断结果为是，则进入步骤9；步骤9，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。进一步的，在本专利技术实施例提供的上述路径规划方法中，所述判断第一随机点Xrand与Xnearest之间的路径是否可行还包括：若判断结果为否，则从Xnearest向第一随机点Xrand扩展预设步长，扩展节点为Xnew；判断Xnew是否可行；若Xnew不可行，则返回步骤2在预设空间内进行重新随机采样，若Xnew可行，则进入步骤6。进一步的，在本专利技术实施例提供的上述路径规划方法中，所述判断Xnew到所述最佳父节点的路径是否可行还包括：若判断结果为否，则放弃节点Xnew，并返回步骤2在预设空间内进行重新随机采样。进一步的，在本专利技术实施例提供的上述路径规划方法中，所述判断树Ta、Tb是否相连还包括：若判断结果为否，则判断Xb是否等于Xrand，若判断结果为是，则进入步骤11，否则，进入步骤12；步骤11，将树Ta、Tb进行交换，并返回步骤2；步骤12，在树Tb中找到距离Xnew最近的点Xb；步骤13，将Xrand赋值为Xb，并返回步骤3。本专利技术实施例再一方面还提供一种路径规划装置，所述路径规划装置包括：初始化模块，用于初始化树Ta、Tb，并分别以起点和目标点作为树Ta、Tb的根节点；随机采样模块，用于在预设空间内随机采样，得到第一随机点Xrand；最近点获取模块，在树Ta中获取距离第一随机点Xrand最近的点Xnearest；第一判断模块，用于利用贪婪搜索策略，判断第一随机点Xrand与Xnearest之间的路径是否可行；标记模块，用于在判断结果为是时，将第一随机点Xrand作为Xnew；代价计算模块，用于将Xnew添加到树Ta中，并在树Ta中获取与Xnew距离在预设范围内的点，将计算出预设范围内的点到Xnew路径代价最小的点作为Xnew的最佳父节点；第二判断模块，用于判断Xnew到所述最佳父节点的路径是否可行；第三判断模块，用于在判断结果为是时，判断树Ta、Tb是否相连；路径获取模块，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。本专利技术实施例另一方面还提供一种应用于上述的路径实时优化方法，所述路径实时优化方法包括：步骤1，初始化树T，并以起点作为根节点；步骤2，获取从起点到目标点的路径；步骤3，判断所述路径上的两个节点之间的距离是否大于预设值，若判断结果为是，则进入步骤4；步骤4，在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树Tpath中；步骤5，在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P0，若判断结果为是，则进入步骤6；步骤6，在树Tpath的所有节点中随机选取一个节点作为第二随机点Xrand；步骤7，在树Tpath中，获取第二随机点Xrand的父节点、步骤8，判断树Tpath中路径节点是否发生变化，若判断结果为是，则返回步骤2。进一步的，在本专利技术实施例提供的上述路径实时优化方法中，所述判断p值是否大于常数P0还包括：若判断结果为否，则在预设空间内任意选取一个节点作为第三随机点Xrand，并在树T中利用RRT*算法扩展所述第三随机节点Xrand，进入步骤8。进一步的，在本专利技术实施例提供的上述路径实时优化方法中，所述获取第二随机点Xrand的父节点包括：在树Tpath中，将第二随机点Xrand赋值给X0；依次将X0的父节点赋值给X0，并计算X0与Xrand的路径代价；若当前路径代价小于上一次计算的路径代价，则令X0为Xrand的父节点。进一步的，在本专利技术实施例提供的上述路径实时优化方法中，所述判断树Tpath中路径节点是否发生变化还包括：若判断结果为否，则返回步骤5。本专利技术实施例再一方面还提供一种路径实时优化装置，所述路径实时优化装置包括：初始化模块，用于初始化树T，并以起点作为根节点；路径获取模块，用于获取从起点到目标点的路径；第一判断模块，用于判断所述路径上的两个节点之间的距离是否大于预设值；节点保存模块，用于在判断结果为是时，在两个节点之间进行线性插值，并将所述线性插值处理后的所有节点保存在树Tpath中；第二判断模块，用于在区间(0，1)上对p进行随机取值，并判断p值是否大于常数P0；随机选取模块，用于在判断结果为是时，在树Tpath的所有节点中随机选取一个节点作为第二随机点Xrand；父节点获取模块，用于在树Tpath中，获取第二随机点Xrand的父节点；第三判断模块，用于判断树Tpath中路径节点是否发生变化。本专利技术实施例再一方面还提供一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任意一项所述的路径规划方法的步骤以及上述任意一项所述的路径实时优化方法的步骤。本专利技术实施例再一方面还提供一种终端装置，所述终端装置包括处理器，所述处理器用于执行存储器中存储的计算机程序时实现上述任意一项所述的路径规划方法的步骤以及上述任意一项所述的路径实时优化方法的步骤。本专利技术实施例提供的路径规划方法及装置、路径实时优化方法及装置和计算机可读存储介质，利用路径规划方法可以找到任意一条可行的路径，继而使用路径实时优化方法能够对当前可行的路径进行实时优化，从而输出优化路径。利用本专利技术实施例能够以更快的速度搜索出一条无碰撞的可本文档来自技高网...

【技术保护点】
1.一种路径规划方法，其特征在于，所述路径规划方法包括：步骤1，初始化树Ta、Tb，并分别以起点和目标点作为树Ta、Tb的根节点；步骤2，在预设空间内随机采样，得到第一随机点Xrand；步骤3，在树Ta中获取距离第一随机点Xrand最近的点Xnearest；步骤4，利用贪婪搜索策略，判断第一随机点Xrand与Xnearest之间的路径是否可行，若判断结果为是，则进入步骤5；步骤5，将第一随机点Xrand作为Xnew；步骤6，将Xnew添加到树Ta中，并在树Ta中获取与Xnew距离在预设范围内的点，将计算出预设范围内的点到Xnew路径代价最小的点作为Xnew的最佳父节点；步骤7，判断Xnew到所述最佳父节点的路径是否可行，若判断结果为是，则进入步骤8；步骤8，判断树Ta、Tb是否相连，若判断结果为是，则进入步骤9；步骤9，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。

【技术特征摘要】
1.一种路径规划方法，其特征在于，所述路径规划方法包括：步骤1，初始化树Ta、Tb，并分别以起点和目标点作为树Ta、Tb的根节点；步骤2，在预设空间内随机采样，得到第一随机点Xrand；步骤3，在树Ta中获取距离第一随机点Xrand最近的点Xnearest；步骤4，利用贪婪搜索策略，判断第一随机点Xrand与Xnearest之间的路径是否可行，若判断结果为是，则进入步骤5；步骤5，将第一随机点Xrand作为Xnew；步骤6，将Xnew添加到树Ta中，并在树Ta中获取与Xnew距离在预设范围内的点，将计算出预设范围内的点到Xnew路径代价最小的点作为Xnew的最佳父节点；步骤7，判断Xnew到所述最佳父节点的路径是否可行，若判断结果为是，则进入步骤8；步骤8，判断树Ta、Tb是否相连，若判断结果为是，则进入步骤9；步骤9，获取可行路径，并利用路径实时优化方法对所述可行路径进行实时优化。2.根据权利要求1所述的路径规划方法，其特征在于，所述判断第一随机点Xrand与Xnearest之间的路径是否可行还包括：若判断结果为否，则从Xnearest向第一随机点Xrand扩展预设步长，扩展节点为Xnew；判断Xnew是否可行；若Xnew不可行，则返回步骤2在预设空间内进行重新随机采样，若Xnew可行，则进入步骤6。3.根据权利要求1所述的路径规划方法，其特征在于，所述判断Xnew到所述最佳父节点的路径是否可行还包括：若判断结果为否，则放弃节点Xnew，并返回步骤2在预设空间内进行重新随机采样。4.根据权利要求1所述的路径规划方法，其特征在于，所述判断树Ta、Tb是否相连还包括：若判断结果为否，则判断Xb是否等于Xrand，若判断结果为是，则进入步骤11，否则，进入步骤12；步骤11，将树Ta、Tb进行交换，并返回步骤2；步骤12，在树Tb中找到距离Xnew最近的点Xb；步骤13，将Xrand赋值为Xb，并返回步骤3。5.一种路径规划装置，其特征在于，所述路径规划装置包括：初始化模块，用于初始化树Ta、Tb，并分别以起点和目标点作为树Ta、Tb的根节点；随机采样模块，用于在预设空间内随机采样，得到第一随机点Xrand；最近点获取模块，在树Ta中获取距离第一随机点Xrand最近的点Xnearest；第一判断模块，用于利用贪婪搜索策略，判断第一随机点Xrand与Xnearest之间的路径是否可行；标记模块，用于在判断结果为是时，将第一随机点Xrand作为Xnew；代价计算模块，用于将Xnew添加到树Ta中，并在树Ta中获取与Xnew距离在预设范围内的点，将计算出预设范围内的点到Xnew...

【专利技术属性】
技术研发人员：刘厚德，刘思成，朱晓俊，梁斌，王学谦，高学海，
申请(专利权)人：清华大学深圳研究生院，
类型：发明
国别省市：广东,44