一种高效的基于快速随机扩展树的路径规划方法技术

本发明专利技术公开了一种高效的基于快速随机扩展树的路径规划方法，使用贪婪采样器在空间中采样样本点引导树的扩展，并且在遇到障碍物无法成功对树进行扩展时会判断局部环境的情况来引导树的扩展。规划器使用双树的方式搜索路径，在双树的交换方式上也采用更加灵活的方法。可以有效处理窄通道环境的规划问题。本发明专利技术使用贪婪采样器，加快了规划器探索空间的速度，从而减少了规划所需时间；环境判断过程使得树可以更容易的通过窄通道环境。环境判断过程会引导树向着计算出的可行方向扩展，树可以在障碍物附近有更好的扩展表现；两棵树的交换方式使得规划器可以更稳定的给出规划结果，不会长时间陷入一方无法扩展且无法交换的状态或是陷入频繁交换的状态。

【技术实现步骤摘要】
一种高效的基于快速随机扩展树的路径规划方法
本专利技术属于路径规划
，涉及一种高效的基于快速随机扩展树的路径规划方法。
技术介绍
采样规划算法作为运动规划算法中的一类自提出后就在无人机航线规划及多自由度机械臂路径规划等机器人运动规划任务中得到了大量的应用。这类规划算法可以更高效的应对高维空间及带有微分约束的运动规划问题。快速随机扩展树(RRT)，是采样规划方法中一种基于树的扩展来探索空间的规划方法，其改进后的应用广泛的RRT-Connect方法是基于双树的更快速的规划方法。此处需要对RRT及RRT-Connect进行简要描述。RRT在规划时首先将起始点设为树的根节点，之后通过在空间中随机采样来得到一个的样本点qrand。此时需要对样本点进行碰撞检测，若样本点被判断在障碍物中，则重新采样，反之，则计算树中和样本点距离最近的点qnear。接下来以一定的步长从qnear向qrand扩展得到qnew节点，若qnear至qnew的路径上没有障碍物则将qnew和他们之间的边加入树中并进入下一轮采样，若有障碍物则直接进入下一轮采样，扩展过程如图1所示。因为采样步骤中是在整个规划空间随机采样，因此在进行多次这样的采样扩展过程后，树将充满整个空间。此外，在扩展过程中，若目标点连接到树中则完成了规划，路径可从目标点逆向迭代到树根得到。RRT-Connect则是在RRT的基础上在目标点也生成一棵树，然后双树交替扩展。交替扩展是指每次扩展时，选择以起始点为根的树进行一次RRT的正常扩展，得到qnew，然后以目标点为根的树将qnew作为目标进行多次扩展，直到遇到障碍物时停止，最后交换双树，使得本轮迭代中的起始点和目标点分别为下一轮迭代中的目标点和起始点。这种交替扩展可以使规划器从两个方向相向搜索空间，提高了规划路径的效率。RRT类的算法虽然可以高效的规划路径，但是在遇到类似于窄通道的困难环境时，规划器的性能会大幅度下降。这是由于窄通道的区域相对于整个规划空间占比较小，采样过程短时间内很难在窄通道区域采样到有效的采样点来引导树的扩展，因此树难以快速的通过窄通道。有一些方法被提出来改进此问题，但其复杂度都较高且改进后的算法更依赖于解决窄通道环境，而对多障碍物的环境性能可能会明显下降，适应性通常较差。
技术实现思路
本专利技术的目的在于解决现有技术中的问题，提供一种高效的基于快速随机扩展树的路径规划方法为达到上述目的，本专利技术采用以下技术方案予以实现：一种高效的基于快速随机扩展树的路径规划方法，其特征在于，包括以下步骤：步骤1，以规划起始点和目标点为树根各初始化一个树结构，父节点置空，两棵树分别为树Ta和树Tb，并各自初始化描述树所占空间大小的参数；步骤2，初始化贪婪采样器；步骤3，使用贪婪采样器在规划空间中随机采样一个样本点；步骤4，找到树Ta中和样本点最接近的树节点，并计算得到此树节点和样本点的连线上与树节点有一定距离的扩展点，判断树节点和扩展点的连线是否与障碍物碰撞；步骤5，若检测结果为碰撞，则进入局部环境判断过程，根据判断的结果对步骤4中的树节点进行扩展，之后执行步骤8；若结果为无碰撞，则将扩展点和被检测的连线作为新的节点和边加入树Ta中；步骤6，以步骤5中新加入树Ta的树节点为目标，找到树Tb中距离最近的树节点，计算得到两点间与树Tb有一定距离的扩展点，同样计算扩展点和树Tb连线的碰撞情况，若检测无碰撞，则执行步骤7，若有碰撞则进入局部环境判断过程，并根据判断结果对树Tb的树节点进行扩展，然后执行步骤8；步骤7，将扩展点和被检测的连线加入树Tb中，并判断树Ta与树Tb是否可连接，若是，则连接两树，并分别回溯得到各自的路径然后整合成最终的规划路径，结束规划过程；若不可连接，则执行步骤6继续扩展树Tb；步骤8，根据两棵树的节点数大小判断是否交换，若树Ta的节点数小于树Tb的节点数，则交换，否则记录未交换的次数，若未交换次数大于设置的阈值，进行强制交换；交换后，树Ta代表原树Tb的树，树Tb代表原树Ta的树；返回步骤3进入下一轮迭代。本专利技术进一步的改进在于：所述步骤1中每棵树初始化一个描述树区域的参数R_T，参数R_T描述了树当前的外接超立方体，即参数R_T中存储了树的外接超立方体在每一维度上的区间范围，此超立方体的每一条边都与空间坐标系中的一条坐标轴平行；初始化时，参数R_T在每一维度上的区间范围都是根节点在相应维度上的坐标。所述步骤2具体如下：初始化贪婪采样器的两个参数Pt和Po，这两个参数将[0,1]分为了三个区间，[0,Pt]，(Pt,Po]和(Po,1]，分别代表采样器在目标区域采样，在树外区域采样及在树内区域采样。所述步骤3具体如下：使用随机函数得到一个在[0,1]内的随机值；若此值在[0,Pt]内，则返回目标点作为采样点；若随机值在(Po,1]内，则在参数R_T所表示的区域内采样，即返回树内区域中一个随机样本点；若随机值在(Pt,Po]内，则首先计算规划空间中每个维度上树外区间的长度，即每个维度上的总区间长度减去参数R_T中相应维度的长度；找出其中树外区间最长的维度记为dmax，然后计算dmax上树外区间长度与所有维度树外区间长度的和的比值Pm；随后，通过随机函数得到一个随机值，若随机值小于Pm则采样时dmax维度选择该维度上较大的树外区间作为该维度的采样范围，若随机值大于Pm，则在除了dmax的维度中任选一个含有树外区间的维度，采样时该维度以较大的树外区间作为该维度上的采样范围；除了依据Pm选择的维度，其余维度均使用空间中该维度的总区间作为采样范围，最终采样点将是参数R_T外的一个区域内的随机采样点，即树外区域的采样点；若参数R_T已经与整个规划空间重合，此时上述采样过程退化为以下方式：通过随机函数得到[0,1]内的一个随机值，若此值小于Pt，则返回目标点作为采样点，反之，则返回参数R_T内的任意一个采样点。所述步骤4具体如下：通过比较树Ta中每个节点与步骤3中得到的采样点qrand之间的欧式距离，找到树Ta中采样点的最近邻点qnear，然后计算出采样点和最近邻点连线上距最近邻点设定距离的点qnew，所述设定距离是规划器设定的扩展步长；然后对采样点的最近邻点qnear和距最近邻点设定距离的点qnew间的连线进行碰撞检测，检测方式为在线段上以设定间隔均匀的检测上面的点，若这些点都无碰撞，则认为连线无碰撞。所述步骤5和步骤6中的局部环境判断过程是指在树中最近邻点扩展失败时，通过在最近邻点周围采样一些样本点来判断所处局部环境类型，并根据不同的类型使用局部采样的样本点计算出的扩展方向，使得树能够在障碍物周围得到扩展。所述步骤5具体如下：若步骤4的结果为连线无碰撞，则将qnew和连线作为新的树节点和边加入树Ta中，继续步骤6；若有碰撞，则进行局部环境判断过程；所述局部环境判断过程具体如下：步骤5-1，局部采样用于描述空间信息的点局部采本文档来自技高网...

【技术保护点】
1.一种高效的基于快速随机扩展树的路径规划方法，其特征在于，包括以下步骤：/n步骤1，以规划起始点和目标点为树根各初始化一个树结构，父节点置空，两棵树分别为树Ta和树Tb，并各自初始化描述树所占空间大小的参数；/n步骤2，初始化贪婪采样器；/n步骤3，使用贪婪采样器在规划空间中随机采样一个样本点；/n步骤4，找到树Ta中和样本点最接近的树节点，并计算得到此树节点和样本点的连线上与树节点有一定距离的扩展点，判断树节点和扩展点的连线是否与障碍物碰撞；/n步骤5，若检测结果为碰撞，则进入局部环境判断过程，根据判断的结果对步骤4中的树节点进行扩展，之后执行步骤8；若结果为无碰撞，则将扩展点和被检测的连线作为新的节点和边加入树Ta中；/n步骤6，以步骤5中新加入树Ta的树节点为目标，找到树Tb中距离最近的树节点，计算得到两点间与树Tb有一定距离的扩展点，同样计算扩展点和树Tb连线的碰撞情况，若检测无碰撞，则执行步骤7，若有碰撞则进入局部环境判断过程，并根据判断结果对树Tb的树节点进行扩展，然后执行步骤8；/n步骤7，将扩展点和被检测的连线加入树Tb中，并判断树Ta与树Tb是否可连接，若是，则连接两树，并分别回溯得到各自的路径然后整合成最终的规划路径，结束规划过程；若不可连接，则执行步骤6继续扩展树Tb；/n步骤8，根据两棵树的节点数大小判断是否交换，若树Ta的节点数小于树Tb的节点数，则交换，否则记录未交换的次数，若未交换次数大于设置的阈值，进行强制交换；交换后，树Ta代表原树Tb的树，树Tb代表原树Ta的树；返回步骤3进入下一轮迭代。/n...

【技术特征摘要】
1.一种高效的基于快速随机扩展树的路径规划方法，其特征在于，包括以下步骤：
步骤1，以规划起始点和目标点为树根各初始化一个树结构，父节点置空，两棵树分别为树Ta和树Tb，并各自初始化描述树所占空间大小的参数；
步骤2，初始化贪婪采样器；
步骤3，使用贪婪采样器在规划空间中随机采样一个样本点；
步骤4，找到树Ta中和样本点最接近的树节点，并计算得到此树节点和样本点的连线上与树节点有一定距离的扩展点，判断树节点和扩展点的连线是否与障碍物碰撞；
步骤5，若检测结果为碰撞，则进入局部环境判断过程，根据判断的结果对步骤4中的树节点进行扩展，之后执行步骤8；若结果为无碰撞，则将扩展点和被检测的连线作为新的节点和边加入树Ta中；
步骤6，以步骤5中新加入树Ta的树节点为目标，找到树Tb中距离最近的树节点，计算得到两点间与树Tb有一定距离的扩展点，同样计算扩展点和树Tb连线的碰撞情况，若检测无碰撞，则执行步骤7，若有碰撞则进入局部环境判断过程，并根据判断结果对树Tb的树节点进行扩展，然后执行步骤8；
步骤7，将扩展点和被检测的连线加入树Tb中，并判断树Ta与树Tb是否可连接，若是，则连接两树，并分别回溯得到各自的路径然后整合成最终的规划路径，结束规划过程；若不可连接，则执行步骤6继续扩展树Tb；
步骤8，根据两棵树的节点数大小判断是否交换，若树Ta的节点数小于树Tb的节点数，则交换，否则记录未交换的次数，若未交换次数大于设置的阈值，进行强制交换；交换后，树Ta代表原树Tb的树，树Tb代表原树Ta的树；返回步骤3进入下一轮迭代。


2.根据权利要求1所述的高效的基于快速随机扩展树的路径规划方法，其特征在于，所述步骤1中每棵树初始化一个描述树区域的参数R_T，参数R_T描述了树当前的外接超立方体，即参数R_T中存储了树的外接超立方体在每一维度上的区间范围，此超立方体的每一条边都与空间坐标系中的一条坐标轴平行；初始化时，参数R_T在每一维度上的区间范围都是根节点在相应维度上的坐标。


3.根据权利要求2所述的基于快速随机扩展树的路径规划方法，其特征在于，所述步骤2具体如下：
初始化贪婪采样器的两个参数Pt和Po，这两个参数将[0,1]分为了三个区间，[0,Pt]，(Pt,Po]和(Po,1]，分别代表采样器在目标区域采样，在树外区域采样及在树内区域采样。


4.根据权利要求3所述的高效的基于快速随机扩展树的路径规划方法，其特征在于，所述步骤3具体如下：
使用随机函数得到一个在[0,1]内的随机值；若此值在[0,Pt]内，则返回目标点作为采样点；若随机值在(Po,1]内，则在参数R_T所表示的区域内采样，即返回树内区域中一个随机样本点；若随机值在(Pt,Po]内，则首先计算规划空间中每个维度上树外区间的长度，即每个维度上的总区间长度减去参数R_T中相应维度的长度；找出其中树外区间最长的维度记为dmax，然后计算dmax上树外区间长度与所有维度树外区间长度的和的比值Pm；随后，通过随机函数得到一个随机值，若随机值小于Pm则采样时dmax维度选择该维度上较大的树外区间作为该维度的采样范围，若随机值大于Pm，则在除了dmax的维度中任选一个含有树外区...

【专利技术属性】
技术研发人员：陈霸东，李炳辉，谢宇清，任鹏举，郑南宁，
申请(专利权)人：西安交通大学，
类型：发明
国别省市：陕西;61