CN114877905A - 一种双向动态生长的Informed-RRT*路径规划方法 - Google Patents

Abstract

本发明公开了一种双向动态生长的Informed‑RRT*路径规划方法，包括以下步骤：根据障碍物地图建模并二值化，确定起始点和目标点，设定初始参数；根据障碍物地图初始化两棵随机树；利用双向搜索融合P概率扇形约束生成采样点；利用生长转角偏置确定节点扩展方向；利用变步长生长确定节点动态扩展步长，产生新的扩展节点；进行碰撞检测并更新主、副树图集，输出初始路径；确定动态椭圆采样范围；在动态椭圆区域利用动态生长来产生新的扩展节点；规定范围内为新的扩展节点重新寻找父节点并重新连接，寻找代价最小路径。本发明可提高初始路径的生成效率，增加目标导向性和路径规划容错率，加快算法收敛速度，解决了因环境复杂而导致路径规划效率低下的问题。

Description

一种双向动态生长的Informed-RRT＊路径规划方法

技术领域

本发明涉及机器人路径规划领域，尤其涉及一种双向动态生长的Informed-RRT*路径规划方法。

背景技术

随着社会的快速发展，智能机器人在智慧工厂、智能巡检、家政服务和太空探索等领域地应用越来越广泛。然而，在机器人路径规划方面仍存在不足，例如搜索效率低、规划路径非最优等缺点。

常见的运动规划方法分为搜索、智能、采样这三大类。其中常见的Dijkstra和A*算法等属于搜索路径规划，蚁群算法、遗传算法等属于智能路径规划算法，以上两类算法需要建立空间环境障碍物模型，不适合在环境复杂的空间中路径规划。基于采样的路径规划算法快速探索随机树(Rapidly-exploring Random Trees，RRT)、 RRT*、Informed-RRT*等不仅能够广泛应用于复杂高维空间，而且无需对构型空间障碍物进行描述，适合应用于多自由度机器人在复杂环境下和动态环境中的路径规划的问题。

传统的Informed-RRT*算法是基于RRT、RRT*发展而来的。RRT算法是通过状态空间的随机采样导向空白区域，从而快速地找到一条可行的规划路径，但是其生成的路径非最优且环境复杂度会影响收敛速度。鉴于RRT 算法的上述缺点，RRT*通过对指定范围内为当前节点重新寻找父节点和范围内节点的重新连接两步来产生一个最优路径，但由于修剪冗余枝叶范围大会导致规划效率低。鉴于RRT*的上述缺点，Informed-RRT*在初始路径已经生成的基础上，在最后修剪枝叶的时候建立一个渐变椭圆来规定采样空间，提升规划效率。但是该算法在生成初始路径时并未对采样空间做出约束，在初始路径生成和修剪冗余枝叶时都未对节点扩展方式做出优化，导致规划效率不满足机器人在复杂环境下作业的时效性要求。因此，对于以上问题，急需提出一种能够引导 Informed-RRT*算法快速找到一条渐进最优的路径规划方法。

发明内容

本发明创造目的在于，提供一种双向动态生长的Informed-RRT*路径规划方法。采用一种初始路径双向搜索的方式，有效提高了初始路径的生成效率；采用一种P概率扇形约束采样的方法，增加了目标导向性和路径规划容错率；在节点扩展时采用生长转角偏置，有效加快算法收敛速度；采用变步长生长的扩展方式，解决了在路径规划过程中因环境复杂而导致的路径规划效率低下的问题。

本发明为达到上述发明目的，采用如下技术方案：

本发明旨在提供一种双向动态生长的Informed-RRT*路径规划方法，以提高现有技术在复杂环境下路径规划的规划效率，包括如下步骤：

S1、根据障碍物地图建模并二值化，确定起始点和目标点，设定初始参数；

S2、根据障碍物地图初始化两棵随机树；

S3、利用双向搜索融合P概率扇形约束的采样策略进行采样生成采样点；

S4、利用生长转角偏置策略来确定节点扩展时的方向；

S5、利用变步长生长策略来确定节点扩展时的动态扩展步长，产生最新的扩展节点；

S6、进行碰撞检测并更新主、副树图集，输出初始路径；

S7、确定动态椭圆采样范围；

S8、在动态椭圆区域利用动态生长策略来产生新的扩展节点；

S9、在新的扩展节点规定范围内重新寻找父节点和重新连接，寻找代价最小的路径。

本发明所述的一种双向动态生长的Informed-RRT*路径规划方法的特点也在于，所述步骤S1包括：

障碍物地图包括地图中的障碍物和空白区域，并对两者进行二值化处理，确定初始点q_init和目标点 q_goal。

所述步骤S2包括：

根据障碍物地图初始化两棵随机树中的顶点集V₁、V₂和边集E₁、E₂，并分别把顶点集V₁、V₂和边集E₁、E₂加入到主树和副树的图集G₁、G₂中。

所述步骤S3包括：

S31、生成P概率扇形约束采样随机值并和规定阈值B比较，若是小于阈值B则采用扇形采样，否则选择普通随机采样。产生采样点q_rand；

S311、扇形采样是指连接上一次更新节点q_new(i-1)和目标点q_goal作为扇形的中线，若是初始采样即以初始点q_init和目标点q_goal作为扇形的中线。以2θ作为扇形夹角，扇形区域可由中线和扇形夹角所确定，扇形采样的采样区即可由扇形区域和地图边界所交区域确定；

S312、普通随机采样的采样区域即由地图边界所确定；

S32、判断树Tree1顶点集V₁中顶点的个数numv1是否小于等于树Tree2顶点集V₂中顶点的个数 numv2，若是则把Tree1作为主树，Tree2作为副树。寻找主树上离采样点最近节点q_nearst，通过生长转角偏置和变步长生长完成新的扩展节点q_new的扩展。

所述步骤S4包括：

生长转角偏置策略是为了改变目标节点扩展的方向，夹角α为直线最近节点q_nearst和目标节点q_goal所得直线L(q_nearst,q_goal)与q_near1和采样节点q_rand形成的直线L(q_nearst,q_rand)之间的夹角，若是没有生长方向偏置则原则生长的节点的在线L(q_nearst,q_goal)上，通过添加可调偏置系数μ(0<μ<1)变可改变偏置方向。

所述步骤S5包括：

S51、设定初始固定步长为r，若已知采样节点q_rand和最近节点q_nearst，以q_nearst为半圆的圆心，作一个半圆，半圆直径大小为2M(M＞r)，方向与q_nearst到q_rand的方向垂直；

S52、半圆区域为q_nearst环境复杂度所考虑的范围，在范围内的所有节点做半径为N的圆，定义环境复杂度指数为η：

上式中S是范围内节点所构成圆和障碍物相交的个数，U和V是设置两个阈值等级(0＜U＜V)；

S53、通过每次采样可求得S的大小，根据环境复杂度定义动态扩展步长r_d如下：

通过步骤S4可以确定扩展方向，通过步骤S5可以确定动态扩展步长r_d，至此可以完成新的扩展节点q_new的扩展。

所述步骤S7包括：

动态椭圆的长轴是当前产生路径的长度c_best，起始点q_star和目标点q_goal分别为椭圆的两个焦点，其距离设为c_min，椭圆的短轴长度是

由两个焦点和已知的长短轴即可确定椭圆采样区域。

所述步骤S8包括：

在步骤S7确定椭圆区域采样后，利用步骤S31生成P概率，选取采样方式并采样，利用步骤S4，采用生长转角偏置确定扩展方向，利用步骤S5，采用变步长生长确定动态扩展步长r_d，最终生成新的扩展节点q_new。

所述步骤S9包括：

S91、对步骤S8中生成新的扩展节点q_new在规定的范围内重新选择父节点q_father；

S911、把离当前采样点最近的节点q_nearest暂存于代价最小的变量q_min中；

S912、根据当前新的扩展节点q_new规定一个范围，即修剪枝叶的范围，此规定范围一般以当前新的扩展节点q_new为圆心，1.5倍的基础步长r为半径的圆C_near。遍历规定范围C_near内的所有节点为新的扩展节点 q_new重新寻找父节点q_father；

S913、把新的扩展节点q_new和遍历的每一个节点作碰撞检测，若是通过碰撞检测的点q_near的代价加上新的扩展节点q_new和q_near之间的代价暂存于c'中，如果新构造父节点q_father时q_new的代价c'比原来q_new的代价小，则把当前遍历点q_near存入当前新的扩展节点q_new的最小代价父节点q_min中，这样遍历完就可以找到当前新的扩展节点q_new在规定范围C_near内的最小代价父节点q_min，更新边集E₁、E₂；

S92、在当前规定范围C_near内为所有节点重新连接；

S921、在规定范围C_near内(除了最小代价父节点q_min外的其他节点)的节点把新的扩展节点q_new重新作为父节点q_father比较之前的代价；

S922、遍历的这些点和新的扩展节点q_new若是通过碰撞检测，遍历点原来的路径代价比新连接的代价大则更新当前遍历点的父节点q_father，更新总图集G。

与现有技术相比，本发明具有以下有益效果：

发明旨在提供一种双向动态生长的Informed-RRT*路径规划方法。首先采用一种初始路径双向搜索的方式，有效提高了初始路径的生成效率；其次采用一种P概率扇形约束采样的方法，增加了目标导向性和路径规划容错率；接着在节点扩展时采用生长转角偏置，有效加快算法收敛速度；最后采用变步长生长的扩展方式，解决了在路径规划过程中因环境复杂而导致的路径规划效率低下的问题。

附图说明

图1为本发明中一种双向动态生长的Informed-RRT*路径规划方法的总体流程图；

图2为本发明P概率扇形约束采样原理示意图；

图3为本发明生长转角偏置原理示意图；

图4为本发明变步长生长原理示意图；

图5为本发明椭圆采样空间示意图；

图6为本发明测试不同算法路径规划效果图；

图7为本发明算法对比运行时间折线图。

具体实施方式

下面结合附图和实施例子对本发明作进一步详细的说明。本申请所描述的具体的实施例仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分。

本发明提出一种双向动态生长的Informed-RRT*路径规划方法，如图1所示，包括以下步骤：

步骤一、根据障碍物地图建模并二值化，障碍物地图包括地图中的障碍物和空白区域，确定初始点q_init和目标点q_goal。

步骤二、根据障碍物地图初始化两棵随机树中的顶点集V₁、V₂和边集E₁、E₂，并分别把顶点集V₁、 V₂和边集E₁、E₂加入到主树和副树的图集G₁、G₂中。

步骤三、利用双向搜索融合P概率扇形约束的采样策略在初始化地图空间进行采样生成采样点。生成 P概率扇形约束采样随机值并和规定阈值B比较，若是小于阈值B则采用扇形采样，否则选择普通随机采样。如图2所示，产生采样点q_rand。扇形采样是指连接上一次更新节点q_new(i-1)和目标点q_goal作为扇形的中线，若是初始采样即以初始点q_init和目标点q_goal作为扇形的中线。以2θ作为扇形夹角(θ＝60°)，扇形区域可由中线和扇形夹角所确定，扇形采样的采样区即可由扇形区域和地图边界所交区域确定。普通随机采样的采样区域即由地图边界所确定。判断树Tree1顶点集V₁中顶点的个数numv1是否小于等于树Tree2顶点集V₂中顶点的个数numv2，若是则把Tree1作为主树，Tree2作为副树。寻找主树上离采样点最近节点q_nearst，通过生长转角偏置和变步长生长完成新的扩展节点q_new的扩展。

步骤4：基于新产生的采样点利用生长转角偏置策略来确定节点扩展时的方向。生长转角偏置策略是为了改变目标节点扩展的方向，如图3所示，夹角α为最近节点q_nearst和目标节点q_goal所得直线L(q_near1,q_goal) 与q_near1和采样节点q_rand形成的直线L(q_near1,q_rand)之间的夹角，若是没有生长方向偏置则原则生长的节点的在线L(q_near1,q_goal)上，通过添加可调偏置系数μ(0<μ<1)变可改变偏置方向。

步骤5：利用变步长生长策略来确定节点扩展时的动态扩展步长r_d，产生最新的扩展节点q_new。如图4 所示，设定初始固定步长为r，若已知采样节点q_rand和最近节点q_nearst，以q_netsra为半圆的圆心，作一个半圆，半圆直径大小为2M(M＞r)，方向与q_nearst到q_rand的方向垂直。半圆区域为q_nearst环境复杂度所考虑的范围，在范围内的所有节点做半径为N的圆，定义环境复杂度指数为η：

上式中S是范围内节点所构成圆和障碍物相交的个数，U和V是设置两个阈值等级(0＜U＜V)。通过每次采样可求得S的大小，根据环境复杂度定义动态扩展步长r_d如下：

通过步骤4可以确定扩展方向，通过步骤5可以确定动态扩展步长r_d，至此可以完成新的扩展节点q_new的扩展。

步骤6：对新的扩展节点q_new进行碰撞检测并更新主、副树图集G₁、G₂，迭代后满足主副树最新的扩展节点阈值条件则输出初始路径。

步骤7：根据初始点、目标点以及规划的初始路径来产生动态椭圆采样范围。如图5动态椭圆的长轴是当前产生路径的长度c_best，起始点q_star和目标点q_goal分别为椭圆的两个焦点，其距离设为c_min，椭圆的短轴长度是

由两个焦点和已知的长短轴即可确定椭圆采样区域。

步骤8：在动态椭圆区域利用P概率扇形约束采样策略、生长转角偏置策略和变步长生长策略来产生新的扩展节点q_new。在步骤7确定椭圆区域采样后，利用步骤3生成P概率，选取采样方式并采样，利用步骤4，采用生长转角偏置确定扩展方向，利用步骤5，采用变步长生长确定动态扩展步长r_d，最终生成新的扩展节点q_new。

步骤9：对步骤8中生成新的扩展节点q_new在规定的范围内重新选择父节点q_father，把离当前采样点最近的节点q_nearest暂存于代价最小的变量q_min中，根据当前新的扩展节点q_new规定一个范围，即修剪枝叶的范围，此规定范围C_near一般以当前新的扩展节点q_new为圆心，1.5倍的基础步长r为半径的圆C_near。遍历规定范围C_near内的所有节点为新的扩展节点q_new重新寻找父节点q_father，把新的扩展节点q_new和遍历的每一个节点作碰撞检测，若是通过碰撞检测的点q_near的代价加上新的扩展节点q_new和q_near之间的代价暂存于c'中，如果新构造父节点q_father时q_new的代价c'比原来q_new的代价小，则把当前遍历点q_near存入当前新的扩展节点q_new q_new的最小代价父节点q_min中，这样遍历完就可以找到当前新的扩展节点q_new在规定范围C_near内的最小代价父节点q_min，更新边集E₁、E₂。在当前规定范围C_near内为所有节点重新连接，在规定范围C_near内 (除了最小代价父节点q_min外的其他节点)的节点把新的扩展节点q_new q_new重新作为父节点q_father比较之前的代价，遍历的这些点和新的扩展节点q_new若是通过碰撞检测，若是遍历点原来的路径代价比新连接的代价大则更新当前遍历点的父节点q_father，更新总图集G。完成规定迭代次数后输出最终规划路径。

为了验证本方法在求解路径规划问题的有效性，设计了两组测试实验，一组为在相同环境复杂度下针对不同路径规划算法作效果对比实验；第二组为针对算法性能指标的测试实验。，***环境为Windows 10 操作***和PyCharm 2020.3.3(Community Edition)。

实验一是相同基础步长2和迭代次数300下，对比相同环境复杂度下RRT、RRT*、Informed-RRT*和本发明提出一种双向动态生长的Informed-RRT*路径规划方法规划的路径质量对比。地图大小采用25*25 的，障碍物如图6所示黑色部分。对比结果如图6所示，改进算法的规划路径比其他路径长度和转弯幅度更小，且相比较于其他规划的路径，在采样次数有限且相同的情况下，改进算法对于复杂度较高的环境其适应性更强。

实验二是对算法耗时进行定量测试。首先在相同环境和基础步长2下对比传统Informed-RRT*算法和引入生长转角偏置后的改进算法进行20次实验，测试初始路径平均规划时间和总规划时间，地图大小采用25*25的，障碍物如图6所示黑色部分。可以看出无论在初始路径的规划过程中还是在总规划过程中总体性能都有一定的提升，且总规划性能提升相较于初始路径更加明显，具体实验结果如下表1所示：

表1引入生长转角α偏置性能比较表

然后在相同环境和基础步长2下对比传统Informed-RRT*算法和本发明改进的算法进行20次实验，如图7所示，可以看出随着采样迭代次数的增加，两种算法耗时差距拉大，本发明路径规划算法效率更高。

针对仿真实验，本发明采用路径长度、规划时间、规划成功率、初始路径迭代次数等指标。路径长度

其中m树上已规划边集E₁、E₂中边的数量，L_i表示规划的第i段边的长度；规划时间T表示为算法在仿真环境中运行的总时长；规划成功率

其中C表示在已知迭代轮次中现已规划成功的次数；初始路径迭代次数F表示第一次规划成功时的迭代轮次。除了上述四个指标，还提出平均转弯指数 (Average Turning Index，ATI)算法代价指标。平均转弯指数是指规划路径后在路径上连接的所有节点的角度的平均值。计算方法如式3所示。

上式(3)中n为树上已规划路径的总节点数，第i号节点q_i的坐标为(x_iy_i)，ATI为平均转弯指数。不同环境相同迭代次数下，分别进行20组实验得到的性能比较结果如下表2所示。

无论是在简单环境下还是在复杂环境下，平均规划路径长度、平均规划时间、初始路径迭代次数、规划成功率和ATI等参数性能都有所提升；初始路径平均迭代次数在复杂环境和简单环境性能提升差不多；除了初始路径平均迭代次数和规划成功率，其他参数的参数性能提升复杂环境都更优于简单环境，因为改进算法在生成初始路径后完成了对生成初始路径以后规划过程的优化。对于ATI参数，因为改进算法应用了变步长思想，本发明改进的算法对于复杂环境的性能提升更加明显，更加适用于复杂环境的路径规划。整体改进算法对于规划效率、规划成功率、规划路径的质量和环境适用性都有较大的提升。

表2算法性能比较表

以上所述实施例仅是充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术的技术人员在本发明的基础上所做的等同替换或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (9)

1.一种双向动态生长的Informed-RRT*路径规划方法，其特征在于，包括以下步骤：

S1、根据障碍物地图建模并二值化，确定起始点和目标点，设定初始参数；

S2、根据障碍物地图初始化两棵随机树；

S3、利用双向搜索融合P概率扇形约束的采样策略进行采样生成采样点；

S4、利用生长转角偏置策略来确定节点扩展时的方向；

S5、利用变步长生长策略来确定节点扩展时的动态扩展步长，产生最新的扩展节点；

S6、进行碰撞检测并更新主、副树图集，输出初始路径；

S7、确定动态椭圆采样范围；

S8、在动态椭圆区域利用动态生长策略来产生新的扩展节点；

S9、在新的扩展节点规定范围内重新寻找父节点和重新连接，寻找代价最小的路径。

2.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S1包括：

障碍物地图包括地图中的障碍物和空白区域，并对两者进行二值化处理，确定初始点q_init和目标点q_goal。

3.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S2包括：

根据障碍物地图初始化两棵随机树中的顶点集V₁、V₂和边集E₁、E₂，并分别把顶点集V₁、V₂和边集E₁、E₂加入到主树和副树的图集G₁、G₂中。

4.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S3包括：

S31、生成P概率扇形约束采样随机值并和规定阈值B比较，若是小于阈值B则采用扇形采样，否则选择普通随机采样。产生采样点q_rand；

S311、扇形采样是指连接上一次更新节点q_new(i-1)和目标点q_goal作为扇形的中线，若是初始采样即以初始点q_init和目标点q_goal作为扇形的中线，以2θ作为扇形夹角，扇形区域可由中线和扇形夹角所确定，扇形采样的采样区即可由扇形区域和地图边界所交区域确定；

S312、普通随机采样的采样区域即由地图边界所确定；

S32、判断树Tree1顶点集V₁中顶点的个数numv1是否小于等于树Tree2顶点集V₂中顶点的个数numv2，若是则把Tree1作为主树，Tree2作为副树。寻找主树上离采样点最近节点q_nearst，通过生长转角偏置和变步长生长完成新的扩展节点q_new的扩展。

5.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S4包括：

生长转角偏置策略是为了改变目标节点扩展的方向，夹角α为直线最近节点q_nearst和目标节点q_goal所得直线L(q_nearst,q_goal)与q_near1和采样节点q_rand形成的直线L(q_nearst,q_rand)之间的夹角，若是没有生长方向偏置则原则生长的节点的在线L(q_nearst,q_goal)上，通过添加可调偏置系数μ(0<μ<1)变可改变偏置方向。

6.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S5包括：

S51、设定初始固定步长为r，若已知采样节点q_rand和最近节点q_nearst，以q_nearst为半圆的圆心，作一个半圆，半圆直径大小为2M(M＞r)，方向与q_nearst到q_rand的方向垂直；

S52、半圆区域为q_nearst环境复杂度所考虑的范围，在范围内的所有节点做半径为N的圆，定义环境复杂度指数为η：

上式中S是范围内节点所构成圆和障碍物相交的个数，U和V是设置两个阈值等级(0＜U＜V)；

S53、通过每次采样可求得S的大小，根据环境复杂度定义动态扩展步长r_d如下：

通过步骤S4可以确定扩展方向，通过步骤S5可以确定动态扩展步长r_d，至此可以完成新的扩展节点q_new的扩展。

7.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S7包括：

动态椭圆的长轴是当前产生路径的长度c_best，起始点q_star和目标点q_goal分别为椭圆的两个焦点，其距离设为c_min，椭圆的短轴长度是

由两个焦点和已知的长短轴即可确定椭圆采样区域。

8.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S8包括：

在步骤S7确定椭圆区域采样后，利用步骤S31生成P概率，选取采样方式并采样，利用步骤S4，采用生长转角偏置确定扩展方向，利用步骤S5，采用变步长生长确定动态扩展步长r_d，最终生成新的扩展节点q_new。

9.根据权利要求1所述的一种双向动态生长的Informed-RRT*路径规划方法，其特征还在于，所述步骤S9包括：

S91、对步骤S8中生成新的扩展节点q_new在规定的范围内重新选择父节点q_father；

S911、把离当前采样点最近的节点q_nearest暂存于代价最小的变量q_min中；

S912、根据当前新的扩展节点q_new规定一个范围，即修剪枝叶的范围，此规定范围一般以当前新的扩展节点q_new为圆心，1.5倍的基础步长r为半径的圆C_near，遍历规定范围C_near内的所有节点为新的扩展节点q_new重新寻找父节点q_father；

S913、把新的扩展节点q_new和遍历的每一个节点作碰撞检测，若是通过碰撞检测的点q_near的代价加上新的扩展节点q_new和q_near之间的代价暂存于c'中，如果新构造父节点q_father时q_new的代价c'比原来q_new的代价小，则把当前遍历点q_near存入当前新的扩展节点q_new的最小代价父节点q_min中，这样遍历完就可以找到当前新的扩展节点q_new在规定范围C_near内的最小代价父节点q_min，更新边集E₁、E₂；

S92、在当前规定范围C_near内为所有节点重新连接；

S921、在规定范围C_near内(除了最小代价父节点q_min外的其他节点)的节点把新的扩展节点q_new重新作为父节点q_father比较之前的代价；

S922、遍历的这些点和新的扩展节点q_new若是通过碰撞检测，遍历点原来的路径代价比新连接的代价大则更新当前遍历点的父节点q_father，更新总图集G。

Images