一种改进RRT算法重连策略的路径规划方法

一种改进rrt算法重连策略的路径规划方法
技术领域
1.本发明涉及自主导航技术领域，尤其是使用了rrt路径规划相关算法，本发明涉及一种基于改进rrt算法的剪枝与重连策略的机器人路径规划方法。


背景技术：

2.近年来，自主移动机器人在安全巡检、应急救援、无人运输等领域发挥着重要作用。路径规划作为移动机器人的一个基础研究课题，受到了广泛的关注。在给定周围环境的部分信息的基础上，移动机器人需要在满足某些条件(如距离或平滑度)的两个指定位置之间自主规划最优或次优无碰撞路径。它对无人驾驶自主系统的性能至关重要，如无人驾驶飞行器、无人地面车辆和无人驾驶水下车辆。提高机器人路径规划水平，对提高机器人的智能化水平、加快工程化应用具有重要的意义。
3.由于路径规划应用的广泛性，随着研究的深入，涌现出了一系列的算法。基于栅格的搜索算法，如dijkstra算法、theta*算法、a*算法、d*-lite算法等；元启发式算法，如遗传算法(genetic algorithm,ga)、粒子群算法(particle swarm algorithm,pso)、蚁群算法(ant colony algorithm,aco)等，其受自然界生物群体智能现象的启发，通过模仿社会性动物的行为而提出，这类算法不需要目标函数与约束的连续性，非常适合解决np问题(non-deterministic polynomial problem，非确定性多项式问题)，对于数据中的不确定性有很强的适应能力，但一方面由于缺乏全局信息，往往容易陷入局部极小值，只能实现近似最优，另一方面，由于此类算法需要不断迭代收敛，使求解效率低下，耗时过长。
4.于是，基于随机采样的规划算法被提出，如快速探索随机树(rapidly-exploring random tree，rrt)算法和概率路线图(probabilistic roadmaps，prm)算法，不需要对环境进行精确建模，在复杂环境及高维环境下具有明显的优势。其中，rrt算法可快速得到一条无碰撞路径，rrt因其在实际应用中的效率更高而赢得了研究界的广泛关注。


技术实现要素：

5.本发明针对上述问题及技术需求，提出了一种改进rrt算法重连策略的路径规划方法，可以适配任意输出的二维地图进行路径规划，适用于服务机器人、无人驾驶玩具车、自主导航无人机，可将方法适配至ros机器人操作系统中，作为插件。
6.为了解决现有技术存在的技术问题，本专利的技术方案如下：
7.本发明的实施根据一个方面，提供了一种改进rrt算法重连策略的路径规划方法，包括如下步骤：
8.通过ros系统下slam建图算法等方式获取实验场景地图，主要分为简单环境、杂乱环境、狭窄通道环境三大块如图1所示，确定在所述的实验场景地图中的起始点和目标点；
9.将起始点作为树扩展的根节点，在全局范围内进行随机采样，同时为了保证整体往目标点前进，提出了目标导向采样策略，给予一定的概率将目标点作为采样点；
10.根据所述的随机采样点，利用目标偏置及障碍物偏置策略进行树的扩展结点的选
取；
11.根据所述的扩展结点，寻找可达的最远父结点；
12.针对所选可达最远父结点(简称可达结点)，在可达结点与可达结点的父结点之间利用二分法创建距离障碍物较近的结点；
13.根据所述的距离障碍物较近的结点，与扩展结点利用二分法进一步分割，同时确保距离障碍物较近的结点与父结点、扩展结点的连线为安全可通行路径，最终针对所述的扩展结点创建新的父结点；
14.在所述新的父结点附近创建一定半径范围的超体球，利用三角不等式规则判断超体球内其他结点与所述新的父结点进行连接的路径，是否为更优路径；若是，则进行连接更新路径表及结点列表；若否，跳过此结点；
15.重复所述过程，直至达到最大迭代或扩展至目标点结束。
16.优选地，所述的地图应用场景主要为二维环境，包括但不限于静态环境，内容如下：
17.工作空间为二维空间，且工作空间下仅有一个移动机器人、一个起始位置(sp)与目标位置(gp)。路径规划问题的目标是尽可能快地找到一条与障碍物无碰撞的最优或接近最优的路径(最安全、最短和最平滑)，若无可行路径返回失败。为此，提出以下假设：
18.假设1：工作空间中存在多个规则或不规则静态障碍物；
19.假设2：移动机器人为一个圆形实体；因此为考虑移动机器人实际尺寸，障碍物根据移动机器人的半径进行扩展，因此移动机器人可以认为是一个点；
20.假设3：不考虑移动机器人的运动学约束；
21.假设4：移动机器人的运动是全方位的，可以向任何方向移动；
22.令φ为定义的状态空间，φ
obs
为障碍空间，φ
expand
为扩展空间，φ
free
＝φ-φ
obs-φ
expand
为自由空间。目标区域被定义为半径为r的圆，可表述为：
23.r：φ
goal
＝{s||s-s
goal
||≤r}
24.本文研究的目标即为计算一条可行路径，可表述为：
25.σ：[0，t]
→
φ
free
[0026]
其中σ(0)＝s
init
为初始状态，σ(t)∈φ
goal
为目标状态。
[0027]
优选地，所述目标导向采样策略的函数公式为：
[0028][0029]
其中，x
rand
是随机采样点，x
goal
是目标点，p是预设概率值，p(x
rand
)是随机生成概率值。
[0030]
优选地，所述目标偏置策略与障碍物偏置策略内容如下：
[0031]
在人工势场法思想与bg-rrt的启发下，如图2(a)所示引入目标偏置因子(bias-goal factor)使得新生成结点具备一定的概率往目标点偏置。同时在一定半径范围内利用障碍物斥力偏置因子(bias-obstacle factorη)，使得新生成的结点往障碍物的切向量方向偏置，偏置策略参见图2。那么，新生成结点q
new
可重新表述为：
[0032][0033]
其中，t(
·
)表示求其垂直向量，dist(
·
)表示计算最近的距离。目标偏置因子(bias-goal factor)与斥力偏置因子(bias-obstacle factorη)可通过下式给出：
[0034][0035][0036]
其中函数sigmoid(
·
)可将返回值约束与-1与1之间，同时生成的数值较为平滑、易于求导。参考图3sigmoid函数图像可知自变量的范围在-5到5之间时变化最为明显，因此在式中将归一化的数值扩大5倍，使效果偏置效果更优。
[0037]
sigmoid(
·
)函数表达式如下：
[0038][0039]
优选地，所述剪枝与重连策略如下：
[0040]
所述方法在rrt*的rewire过程基础上，将chooseparent过程拆分成findreachest与createnode两个过程。
[0041]
以下图4(b)为例进行说明，其中findreachest为了提高算法效率，取消了超体球的概念，不再对每一个“邻近”结点都进行操作，转而分别计算全局范围内的结点与q
new
的距离，并找到距离最近的结点q
nearest
如图4(b)中标识结点所示，在此基础上，不断搜索q
nearest
结点的父结点并进行碰撞检测，直至寻找到可连接到的q
new
且无碰撞的父结点记作q
reachest
，至此findreachest过程结束。
[0042]
createnode过程通过二分法生成一个距离障碍物最近的结点q
create
，它可以同时连接q
new
与q
reachest
的父结点且无碰撞，为了防止无限二分，增加了一个d
dichotomy
参数。createnode的详细二分过程见图5所示，算法流程图见图6。此过程将在q
reachest
与parent(q
reachest
)之间不断循环生成中间结点，进而创建一个更加靠近障碍物的结点，缩短路径代价的同时，进一步提高算法效率。
[0043]
可见，从原理层面来说，在空旷环境下所述方法，会使得生成的路径只在障碍物区域出现转角，同时路径更加靠近障碍物使得路径代价更短，效率有大幅提升。但另一方面，由于增加的findreachest与createnode过程，会经过许多次的碰撞检测，使得算法时间复杂度有一定程度的提高，但相对rrt*算法获取相同代价的路径所需时长而言依旧具备显著优势。
[0044]
优选地，所述预设的距离为机器人的一个步长，但由于偏置策略的存在，步长并不会是一个定值。
[0045]
优选地，所述一种改进rrt算法重连策略的路径规划方法，适用于服务机器人、无人驾驶玩具车、自主导航无人机，可将方法适配至ros机器人操作系统中，作为插件。
[0046]
与现有技术相比，本发明的有益技术效果是：
[0047]
本发明公开了一种改进rrt算法重连策略的路径规划方法。该方法在传统rrt算法
的基础上引入目标导向采样策略提高采样效率，减少冗余采样，引入目标偏置与障碍物偏置策略提高rrt算法树的扩展效率，保证了算法实时性，改进剪枝与重连策略，使得路径规划具体更短、平滑度更高。
附图说明
[0048]
图1是本发明的三种初始化地图。
[0049]
图2是本发明的目标偏置与障碍物偏置策略示意图。
[0050]
图3是本发明的sigmod函数仿真图。
[0051]
图4是本发明的剪枝与重连策略chooseparent与rewire过程示意图。
[0052]
图5是本发明的剪枝与重连策略chooseparent二分法示意图。
[0053]
图6是本发明提出的一种改进rrt算法重连策略的路径规划方法的方法流程图。
[0054]
图7是本发明的简单环境下改进前后对比图。
[0055]
图8是本发明的杂乱环境下改进前后对比图。
[0056]
图9是本发明的狭窄通道环境下改进前后对比图。
具体实施方式
[0057]
下面结合附图对本发明的具体实施方式做进一步说明，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
[0058]
本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)，具有与本发明所属领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语，应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样被特定定义，否则不会用理想化或过于正式的含义来解释
[0059]
本发明公开了一种改进rrt算法重连策略的路径规划方法，如图6流程图所示，该方法包括：
[0060]
通过ros系统下slam建图算法等方式获取实验场景地图，主要分为简单环境如图1中(a)所示、杂乱环境如图1中(b)所示、狭窄通道环境如图1中(c)所示，三大块作为实验场景地图，确定在所述的实验场景地图中的起始点和目标点，起始点统一选取为左上角，目标点统一选取为右下角；
[0061]
将起始点作为树扩展的根节点，在全局范围内进行随机采样，同时为了保证整体往目标点前进，提出了目标导向采样策略，给予一定的概率将目标点作为采样点；
[0062]
根据所述的随机采样点，利用目标偏置及障碍物偏置策略进行树的扩展结点的选取，如图2(a)与图2(b)所示；其中图3为sigmod函数曲线，用于限制目标偏置与障碍物切向偏置的扩展步长；
[0063]
根据所述的扩展结点，寻找可达的最远父结点，如图4(c所示)；
[0064]
针对所选可达最远父结点(简称可达结点)，在可达结点与可达结点的父结点之间利用二分法创建距离障碍物较近的结点，如图5所示为二分法的分割示意图；
[0065]
根据所述的距离障碍物较近的结点，与扩展结点利用二分法进一步分割，同时确保距离障碍物较近的结点与父结点、扩展结点的连线为安全可通行路径，最终针对所述的扩展结点创建新的父结点，如图4(f)所示；
[0066]
在所述新的父结点附近创建一定半径范围的超体球，利用三角不等式规则判断超体球内其他结点与所述新的父结点进行连接的路径，是否为更优路径；若是，则进行连接更新路径表及结点列表；若否，跳过此结点；
[0067]
重复所述过程，直至达到最大迭代或扩展至目标点结束。
[0068]
其中，如图4中的(a)(b)为其他现存方法的chooseparent过程，(c)为本发明的chooseparent过程；
[0069]
图4中(d)(e)为其他现存方法的rewire过程，(f)为本发明的rewire过程；
[0070]
下面将使用python中对上述控制算法进行仿真。初始路径的有效性将大幅影响算法后续收敛速度，为此，本部分对比实验用于验证sof-rrt*算法在初始路径生成中的有效性，为了确保每个算法在规划出初始路径后不再进行迭代，需要对算法伪代码进行修改，在各个算法的graphupdate处增加break操作，确保算法不再继续运行。仿真环境地图为图1所示，所有地图大小均为500
×
500，同时起始点q
start
统一位于左上角，目标点q
goal
统一位于右下角，由于采样的随机性，每个算法将在同一组环境、同一组参数中运行50次。换言之，所有算法在每次比较实验中使用相同的采样序列。各算法参数设置如下表所示。其中参数名解释如下：
[0071]
expand_dis：rrt树的扩展步长；
[0072]
goal_sample_rate：采样到目标点的概率，为了防止树扩展不到目标点而设置，使得有一定的几率选取目标点为采样点；
[0073]
pgoal：目标偏置概率，主要针对具备启发式思想的算法，如bg-rrt与sof-rrt*
[0074]
max_iter：最大迭代次数，防止出现由于一直寻找不到路径而出现死循环；
[0075]
epsilon：探索因子的初始值，主要针对sof-rrt*算法中的空间概率权重策略，初始值越趋近于1表明探索能力越强；
[0076]
γ：探索因子epsilon的收敛速率，值越小则收敛速度越快，越趋近于1则收敛速度越慢；
[0077]
epsilonfloor：探索下限，使得随着算法的进行，能始终保持一定的探索概率；
[0078]
ddichotomy：主要针对f-rrt*与sof-rrt*算法，同时为了避免由于ddichotomy值偏低导致的算法效率低下，建议值不要太小；
[0079]qstart
：路径规划起始点坐标(左上角方点)；
[0080]qgoal
：路径规划目标点坐标(右下角方点)；
[0081]
weightgrade：sof-rrt*算法中空间概率权重采样的权重等级；
[0082][0083]
图7为本发明在简单环境下运行50次选最优的改进前后对比图，(a)为bg-rrt算法运行结果图，(b)为知情者rrt算法运行结果图，(d)为f-rrt算法运行结果图，(d)本发明改进算法运行结果图，从图中可以看出，虽然路径长度相差不多，本发明的方法运行次数最少，所用结点数最少，路径最平滑，说明在简单环境下效果不错；
[0084]
图8为本发明在杂乱环境下运行50次选最优的改进前后对比图，(a)为bg-rrt算法运行结果图，(b)为知情者rrt算法运行结果图，(d)为f-rrt算法运行结果图，(d)本发明改进算法运行结果图，从图中可以看出，本发明的方法规划的路径长度最短，运行次数最少，所用结点数最少，路径最平滑，说明在杂乱环境下效果不错；
[0085]
图9为本发明在狭窄通道环境下运行50次选最优的改进前后对比图，(a)为bg-rrt算法运行结果图，(b)为知情者rrt算法运行结果图，(d)为f-rrt算法运行结果图，(d)本发明改进算法运行结果图，从图中可以看出，本发明的方法规划的路径长度最短，运行次数最少，所用结点数最少，路径最平滑，运行最稳定，说明在狭窄通道环境下效果不错；
[0086]
以上三种环境下的运行对比图，反应了本发明方法的可行性。
[0087]
以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是本发明的原理，在不脱离本发明精神和范围的前提下本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明的范围内。本发明要求的保护范围由所附的权利要求书及其等同物界定。