CN113296496B - 基于多采样点的引力自适应步长双向rrt路径规划方法 - Google Patents

Abstract

基于多采样点的引力自适应步长双向RRT路径规划方法，包括以下步骤：初始化两棵随机树T₁、T₂，设置参数初始值；利用rand函数生成n个采样点，找到采样点x_rand；找到T₁中与x_rand最接近的节点x_near，加入引力分量计算得到新节点x_new；对采样点x_near与x_new之间的连线进行障碍检测，若无障碍则继续，若有障碍则返回第二步骤，并采用自适应步长的策略调整引力系数k的值；找到T₂中离x_new最近的节点x_near1，判断节点x_near1与x_new之间是否有障碍，若有则跳过下一步，若没有则继续；判断节点x_near1与x_new之间的距离是否小于连接阈值δ，若小于则执行最后一步；若距离大于则继续；将T₁、T₂交换进行扩展，重复循环第二步骤到本步骤至获得初始路径；对得到的初始路径进行优化。

Description

基于多采样点的引力自适应步长双向RRT路径规划方法

技术领域

本发明涉及机器人技术领域，涉及移动机器人路径的规划和效率改进，具体为一种基于多采样点的引力自适应步长双向RRT路径规划方法。

背景技术

近年来，移动机器人的应用场景越来越广泛、功能不断丰富，给人类的生产、生活起到重要的作用，而移动机器人的路径规划是机器人领域中的关键环节，最终目的就是找到一条由起始点到目标点的无障碍路径，并在此基础上，使得花费的时间尽可能少，走过的路径尽可能短。

常用的移动机器人路径规划算法，有人工势场法、遗传算法、快速扩展随机树RRT算法等。人工势场法通过引力和斥力共同作用来令机器人躲避障碍，效率较高但是容易使机器人陷入局部极小值。遗传算法不会出现局部极小值，但是它的计算量大，且不适用于高维环境中。

RRT算法因其不需要对环境建模而广泛应用于机器人的路径规划中，适用于各种复杂的环境中，但是因它的随机性太强导致效率比较慢而且规划出的路径欠佳。双向RRT算法是一种比较基本的改进的RRT算法，它是从起始点和目标点同时生成两棵随机树，两棵树相向扩展，使得搜索效率有所提高但是仍然不够理想。

发明内容

本发明的目的是提供一种基于多采样点的引力自适应步长双向RRT路径规划方法，通过引入多采样点比较选优、目标偏向以及自适应步长来改进双向RRT算法，加快双向RRT的执行速度并且优化路径，提高移动机器人的工作效率。

为了实现本发明的目的，采用以下技术方案：

基于多采样点的引力自适应步长双向RRT路径规划方法，所述路径规划方法包括以下步骤：

S1.从起始点和目标点同时初始化两棵随机树T₁、T₂，设置初始步长s、引力系数k、最大执行次数max、连接阈值δ、距离设定值σ及相关参数的初始值；

S2.生成n个采样点，运用多采样点比较选优原则找到采样点x_rand；

S3.找到随机树T₁中与x_rand最接近的节点x_near，加入引力分量计算得到新节点x_new；

S4.对采样点x_near与新节点x_new之间的连线进行障碍检测，若无障碍则执行步骤5，若有障碍则返回步骤S2，并采用引力自适应步长的策略调整引力系数k的值；

S5.找到随机树T₂中离新节点x_new最近的节点x_near1，判断节点x_near1与新节点x_new之间是否有障碍，若有则执行步骤S7；若没有则执行步骤S6；

S6.判断节点x_near1与新节点x_new之间的距离是否小于连接阈值δ，若小于连接阈值δ，则执行步骤S8，若距离大于连接阈值δ，则执行步骤S7；

S7.将随机树T₁、T₂交换进行扩展，重复循环步骤S2-S7直至获得初始路径；

S8.对初始路径进行优化。

为了进一步实现本发明的目的，还可以采用以下技术方案：

如上所述的基于多采样点的引力自适应步长双向RRT路径规划方法，所述步骤S2中运用多采样点比较选优原则找到采样点x_rand，利用rand函数生成n个候选采样点，计算这n个候选采样点与目标点之间的距离，找到离目标点最近的一个节点作为采样点x_rand。

如上所述的基于多采样点的引力自适应步长双向RRT路径规划方法，所述步骤S3中加入引力分量计算得到新节点x_new，计算公式如下：

其中，s为随机点方向的步长，k为引力系数，kgs为引力方向的步长，x_rand-x_near||、||x_goal-x_near||分别为随机点与最近点、目标点与最近点之间的欧氏距离。

如上所述的基于多采样点的引力自适应步长双向RRT路径规划方法，所述步骤S4中采用引力自适应步长的策略调整引力系数k的值，在移动机器人的路径规划中具体步骤为：

B1.如果移动机器人在运动过程中一路畅通，没有遇到障碍物，即两者之间的距离大于距离设定值σ时，应使目标引力方向的步长kgs变大以使机器人朝向目标点运动从而尽快到达目标点，此时应令引力系数k为初始值；

B2.当移动机机器人遇到障碍物时，应令随机点方向的步长s占比重变大以使机器人能够准确避开障碍物，此时自适应地减小引力系数k的值，再次执行步骤S2到步骤S8；若循环过程中再次遇到障碍物，则继续减小k值以保证避障的准确性。

如上所述的基于多采样点的引力自适应步长双向RRT路径规划方法，若循环过程中k小于0了，则将k赋予一个0～1之间的定值，继续执行步骤S2到步骤S8，引力系数k的具体取值公式如下：

如上所述的基于多采样点的引力自适应步长双向RRT路径规划方法，所述步骤S8中用Dijkstra算法来优化路径，具体步骤为：

C1.首先，将算法得到的初始路径所含点中除起始点外的其它点放到一个集合M中，并新建一个初始只含有起始点的最短路径点集合N；

C2.其次，通过计算路径代价找到矩阵M中离起始点最近的一个点，将其放入集合N中并更新集合M和N；

C3.最后，循环进行步骤C1、C2，直到遍历所有节点将集合M中的点全部放入集合N中，形成起始点到目标点的最短路径。

与现有技术相比，本发明的优点在于：

本发明在基于双向RRT路径规划方法的基础上，引入多采样点比较选优、目标偏向以及自适应步长来改进双向RRT算法，加快双向RRT的执行速度并且优化路径，提高移动机器人的工作效率。通过引入多采样点比较选优减小了采样点的强的随机性，利用目标偏向以及自适应步长的策略使得移动机器人的动作具有目标偏向性，加快了算法的执行速度，缩短了路径长度，能够令移动机器人迅速到达目标点，并能够实现有效避障。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单的介绍。

图1为本发明的流程图；

图2为不同采样点个数的仿真结果图；

图3为本发明的节点扩展示意图；

图4为简单环境下的仿真实验图，其中(a)为双向RRT对应的仿真实验图；(b)为双采样点双向RRT对应的仿真实验图；(c)为引力自适应步长双向RRT对应的仿真实验图；(d)为基于多采样点的引力自适应步长双向RRT对应的仿真实验图；

图5为复杂环境下的仿真实验图，其中(e)为双向RRT对应的仿真实验图；(f)为双采样点双向RRT对应的仿真实验图；(g)为引力自适应步长双向RRT对应的仿真实验图；(h)为基于多采样点的引力自适应步长双向RRT对应的仿真实验图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

如图1-图5所示，本实施例公开一种基于多采样点的引力自适应步长双向RRT路径规划方法，该路径规划方法的具体步骤如下：

步骤1、设置机器人的两种工作环境，环境规格均为500×500。如图4、图5所示，黑色图形代表障碍物，其余部分代表自由空间；

步骤2、设置好相关参数，步长s为25、引力系数k初始值为1.5、最大执行次数max为10000，连接阈值δ为20，距离设定值σ为10，起始点为[10,10]，目标点为[490,490]；

步骤3、从起始点和目标点同时扩展两棵随机树T₁和T₂；

步骤4、利用多采样点选优原则得到随机点x_rand；

实验开始前要确定多采样点的具体个数，因此在两种环境下分别测试采样点为1～10时双向RRT的运行时间，结果如图2所示，可见当采样点数目为2时，双向RRT在简单环境和复杂环境下运行所花费的时间最少，因此在后续实验验证中均采用采样点个数为2来进行，即同时生成两个采样点，选择离目标点最近的一个作为采样点x_rand；

找到T₁中与x_rand距离最近的节点x_near，加入目标引力分量计算得到新节点x_new，节点扩展示意图如图2所示，新节点的计算公式为式(1)；

上式中：s为扩展步长，固定不变，||x_rand-x_near||、||x_goal-x_near||分别代表随机点与最近点、目标点与最近点之间的欧氏距离。

k是引力势场系数，根据机器人与障碍物之间的距离按照式(2)自适应地调整；

仿真环境为Intel Core i5-6500，3.20GHZ处理器，内存为4GB，操作系统为Win10，软件为Matlab 2016b。

为了验证本发明提出的基于多采样点的引力自适应步长双向RRT路径规划方法的优点，在简单环境和复杂环境下将双向RRT(BIRRT)、双采样点双向RRT(DBIRRT)、引力自适应步长双向RRT(GBIRRT)以及基于多采样点的引力自适应步长双向RRT(DGBIRRT)分别运行30次，统计每次运行得到的节点个数、路径点个数、运行时间和路径长度，并计算出相应的平均值。

简单环境和复杂环境下得到的数据分别如下表1、表2所示。仿真实验结果参见图4、图5所示，细虚线和细点线分别为T₁和T₂扩展的随机树分支，实线为两棵树成功连接后找到的初始路径，而粗的虚线是利用Dijkstra算法优化之后得到的路径。

表1简单环境下算法性能对比

算法	节点个数	路径点个数	时间/s	路径长度
					BIRRT	62.8	36	2.09	869.93
DBIRRT	57.37	36.37	1.89	854.11
					GBIRRT	27.77	15.1	1.01	737.42
DGBIRRT	26.03	4	1.00	690.35

表2复杂环境下算法性能对比

算法	节点个数	路径点个数	时间/s	路径长度
					BIRRT	79.5	37.53	1.96	894.42
DBIRRT	58.13	37.6	1.78	886.62
					GBIRRT	40.87	16.97	1.32	775.41
DGBIRRT	37.5	9.03	1.30	733.04

仿真结果表明，本发明提出的基于多采样点的引力自适应步长双向RRT路径规划方法无论在简单环境还是在复杂环境下，在节点个数、路径点个数、算法运行时间以及路径长度四项指标上均优于其它三种方法。

本发明未详尽描述的技术内容均为公知技术。

Claims (4)

1.基于多采样点的引力自适应步长双向RRT路径规划方法，其特征在于，所述路径规划方法包括以下步骤：

S1.从起始点和目标点同时初始化两棵随机树T1、T2，设置初始步长s、引力系数k、最大执行次数max、连接阈值δ、距离设定值σ参数的初始值；

S2.生成多个采样点，运用多采样点比较选优原则找到采样点Xrand；

S3.找到随机树T1中与Xrand最接近的节点Xnear，加入引力分量计算得到新节点Xnew；

S4.对采样点Xnear与新节点Xnew之间的连线进行障碍检测，若无障碍则执行步骤S5，若有障碍则返回步骤S2，并采用自适应步长的策略调整引力系数k的值；

S5.找到随机树T2中离新节点Xnew最近的节点Xnear1，判断节点Xnear1与新节点Xnew之间是否有障碍，若有则执行步骤S7；若没有则执行步骤S6；

S6.判断节点Xnear1与新节点Xnew之间的距离是否小于连接阈值δ，若小于连接阈值δ，则执行步骤S8；若距离大于连接阈值δ，则执行步骤S7；

S7.将随机树T1、T2交换进行扩展，重复循环步骤S2-S7至获得初始路径；

S8.对初始路径进行优化；

所述步骤S4中采用自适应步长的策略调整引力系数k的值，在移动机器人的路径规划中具体步骤为：

B1.如果移动机器人在运动过程中一路畅通，没有遇到障碍物，即两者之间的距离大于距离设定值σ时，应使目标引力方向的步长k·s变大以使机器人朝向目标点运动从而尽快到达目标点，此时应令引力系数k为初始值；

B2.当移动机机器人遇到障碍物时，应令随机点方向的步长s占比重变大以使机器人能够准确避开障碍物，此时自适应地减小引力系数k的值，再次执行步骤S2到步骤S8；若循环过程中再次遇到障碍物，则继续减小k值以保证避障的准确性；

若循环过程中k小于0了，则将k赋予一个0～1之间的定值，继续执行步骤S2到步骤S8，引力系数k的具体取值公式如下：

2.根据权利要求1所述的基于多采样点的引力自适应步长双向RRT路径规划方法，其特征在于，所述步骤S2中运用多采样点比较选优原则找到采样点Xrand，利用rand函数生成n个候选采样点，计算这n个候选采样点与目标点之间的距离，找到离目标点最近的一个节点作为采样点Xrand。

3.根据权利要求1所述的基于多采样点的引力自适应步长双向RRT路径规划方法，其特征在于，所述步骤S3中加入引力分量计算得到新节点Xnew，计算公式如下：

其中，s为随机点方向的步长，k为引力系数，k·s为引力方向的步长，||x_rand-x_near||、||x_goal-x_near||分别为随机点与最近点、目标点与最近点之间的欧氏距离。

4.根据权利要求1所述的基于多采样点的引力自适应步长双向RRT路径规划方法，其特征在于，所述步骤S8中用Dijkstra算法来优化路径，具体步骤为：

C1.首先，将算法得到的初始路径所含点中除起始点外的其它点放到一个集合M中，并新建一个初始只含有起始点的最短路径点集合N；

C2.其次，通过计算路径代价找到矩阵M中离起始点最近的一个点，将其放入集合N中并更新集合M和N；

C3.最后，循环进行步骤C1、C2，直到遍历所有节点将集合M中的点全部放入集合N中，形成起始点到目标点的最短路径。

Images