CN111141304B - 一种基于同心圆采样引导rrt算法的路径规划方法 - Google Patents

Abstract

本发明涉及一种基于同心圆采样引导RRT算法的路径规划方法，包括以下步骤：1、将路径规划的起始点作为随机树的根节点；2、由同心圆采样生成随机点；3、判断随机点是否在地图范围内，是则转步骤4，否则返回步骤2重新生成随机点；4、找出离随机点最近的节点作为邻近点；5、从邻近点向随机点方向以步长扩展生成新节点；6、判断新节点与邻近点之间是否存在障碍物，是则返回步骤2重新采样，否则将新节点加入随机树中；7、判断新节点是否在目标点区域内，是则将目标点加入到随机树中，从目标点回溯到根节点，得到规划路径的路径点，否则返回步骤2；8、对路径点进行拟合，得到最终的路径。该方法有利于提高寻路效率，快速找到规划路径。

Description

一种基于同心圆采样引导RRT算法的路径规划方法

技术领域

本发明涉及路径规划领域，具体涉及一种基于同心圆采样引导RRT算法的路径规划方法。

背景技术

在复杂的环境下，智能车如何快速有效的从起始位置安全到达目标位置，一直受到许多学者的关注。路径规划作为该问题的关键技术，其主要任务就是在空间中找到一条从初始状态点到目标状态点的连续无碰撞路径，同时满足相应的约束条件。

常用的路径规划算法有智能路径规划算法、图搜索算法和启发式搜索算法等。启发式搜索算法，如A*算法，该算法基于栅格地图，对每一个搜索到的位置进行评估，从而找到最好的位置，再从该位置进行搜索，直到搜索到目标点位置。

快速搜索随机树(rapidly-exploring random tree，RRT)算法主要用于机器人路径规划，是一种基于采样的规划算法，同时又是基于树结构的典型算法，通过对状态空间进行随机采样的策略使得搜索树向空白区域扩展，省去了对状态空间建模的过程。其生成的路径点之间可以经过运动学、动力学仿真生成可执行曲线，因此其可以用来解决含有运动学约束的路径规划问题。但由于RRT算法采用均匀采样的策略，缺乏引导信息，导致算法计算量大，影响算法速度；另外，对于将目标点以一定概率作为随机点出现在采样点中，这样虽然可以提高规划效率，但也可能导致陷入局部最小，最终在一定的时间或迭代次数下找不到路径。因此，对于以上问题，急需提出一种既能够引导RRT算法快速找到路径，又应当避免陷入局部最小风险的路径规划方法。

发明内容

本发明的目的在于提供一种基于同心圆采样引导RRT算法的路径规划方法，该方法有利于提高寻路效率，快速找到规划路径。

为了实现上述目的，本发明采用的技术方案是：一种基于同心圆采样引导RRT算法的路径规划方法，其特征在于，提供规划路径的起始点、目标点、障碍物位置和规划地图，按如下步骤进行路径规划：

步骤1：定义同心圆系数m及扩展步长eps，以路径规划的起始点作为随机树T的根节点q_start，并将其作为下一次扩展随机树节点的父节点；

步骤2：以目标点作为同心圆的圆心，规划地图的外接圆半径R加上目标点与外接圆圆心距离d为采样半径，在同心圆系数m及随机数n的控制下生成随机的同心圆半径r，在半径为r的同心圆上生成随机点q_rand；

步骤3：判断随机点是否在地图范围内，若不在地图范围内，则返回步骤2重新生成随机点，若在地图范围内，则转步骤4；

步骤4：遍历随机树T中的每个节点，通过计算随机树T中节点与随机点之间的距离，找出离随机点最近的节点作为邻近点q_near；

步骤5：从邻近点q_near向随机点q_rand方向扩展步长eps，得到新节点q_new；

步骤6：判断新节点q_new与邻近点q_near之间是否存在障碍物，若不存在，则将新节点q_new加入随机树T中，并转步骤7，若存在，则返回步骤2重新采样；

步骤7：判断新节点q_new是否在目标点q_goal的区域内，若不在目标点区域内，则返回步骤2重新采样，若在目标点区域内，则将新节点q_new作为目标点的父节点，并将目标点q_goal加入到随机树T中，从目标点q_goal回溯到根节点q_start，得到规划路径的路径点；

步骤8，对步骤7得到的路径点进行拟合，得到最终的路径。

进一步地，所述步骤2中，生成随机的同心圆半径r的公式如下：

上式中，n为0到1范围内的随机数，m为同心圆系数；

在半径为r的同心圆上生成随机点q_rand的公式如下：

上式中，

分别为目标点q_goal的横、纵坐标值，

分别为生成的随机点q_rand的横、纵坐标值。

进一步地，所述步骤3中，当地图是矩形时，通过判断随机点坐标是否小于地图的边界值x_max和y_max来判断随机点是否在地图范围内。

进一步地，所述步骤5中，扩展生成新节点q_new的方法为：

计算节点q_near与q_rand之间的距离dist：

上式中，

分别为邻近点q_near的横、纵坐标值，

分别为随机点q_rand的横、纵坐标值；

若dist小于eps，则q_new节点的坐标即为q_rand点的坐标，否则q_new节点的坐标由以下公式计算得出：

上式中，

分别为新节点q_new的横、纵坐标值。

进一步地，所述步骤7中，所述目标点q_goal的区域是指以q_goal为圆心，以eps为半径的圆形区域。

进一步地，所述步骤8中，采用三次B样条曲线对步骤7得到的路径点进行拟合。

相较于现有技术，本发明具有以下有益效果：提供一种基于同心圆采样引导RRT算法的路径规划方法，该方法引入同心圆采样，将目标点作为同心圆圆心，在同心圆上生成随机点，通过同心圆系数m来控制同心圆疏密程度，这种方法在不失采样点随机性的同时达到随机树扩展趋向目标点的目的，同时也能有效避免陷入地图的陷阱中，大大提高了寻路效率，能够快速稳定的规划出相应路径，具有很强的实用性和广阔的应用前景。

附图说明

图1是本发明实施例的方法实现流程图。

图2是本发明实施例中同心圆采样示意图。

图3是本发明实施例中RRT方法的路径规划图。

图4是本发明实施例中本发明方法的路径规划图。

图5是本发明实施例中RRT方法与本发明方法的实验数据对比图。

具体实施方式

下面结合附图及具体实施例对本发明作进一步的详细说明。

在本实施例中，设规划地图的范围为1000*1000，路径规划的起始点为(100,150)，目标点为(500,800)，障碍物位置如图4所示。

本发明提供的基于同心圆采样引导RRT算法的路径规划方法，如图1所示，基于上述规划路径的起始点、目标点、障碍物位置和规划地图，按如下步骤进行路径规划：

步骤1：定义同心圆系数m为1.5，定义扩展步长eps为40单位长度，以路径规划的起始点作为随机树T的根节点q_start，并将其作为下一次扩展随机树节点的父节点。

步骤2：如图2所示，以目标点作为同心圆的圆心，规划地图的外接圆半径R加上目标点与外接圆圆心距离d为采样半径，在同心圆系数m及随机数n的控制下生成随机的同心圆半径r，在半径为r的同心圆上生成随机点q_rana。

其中，生成随机的同心圆半径r的公式如下：

上式中，n为0到1范围内的随机数，m为同心圆系数，其取值范围为0到3；

在半径为r的同心圆上生成随机点q_rand的公式如下：

上式中，

分别为目标点q_goal的横、纵坐标值，

分别为生成的随机点q_rand的横、纵坐标值。

步骤3：判断随机点是否在地图范围内，若不在地图范围内，则返回步骤2重新生成随机点，若在地图范围内，则转步骤4。其中，当地图是矩形时，通过判断随机点坐标是否小于地图x方向和y方向的边界值x_max和y_max来判断随机点是否在地图范围内。

步骤4：遍历随机树T中的每个节点，通过计算随机树T中节点与随机点之间的距离，找出离随机点最近的节点作为邻近点q_near。其中，节点与随机点之间的距离采用欧几里得距离计算公式来计算。

步骤5：从邻近点q_near向随机点q_rand方向扩展步长eps，得到新节点q_new。

其中，扩展生成新节点q_new的方法为：

计算节点q_near与q_rand之间的距离dist：

其中，

分别为邻近点q_near的横、纵坐标值，

分别为随机点q_rand的横、纵坐标值；

若dist小于eps，则q_new节点的坐标即为q_rand点的坐标，否则q_new节点的坐标由以下公式计算得出：

其中，

分别为新节点q_new的横、纵坐标值。

步骤6：判断新节点q_new与邻近点q_near之间是否存在障碍物，若不存在，则将新节点q_new加入随机树T中，并转步骤7，若存在，则返回步骤2重新采样。

步骤7：判断新节点q_new是否在目标点q_goal的区域内，若不在目标点区域内，则返回步骤2重新采样，若在目标点区域内，则将新节点q_new作为目标点的父节点，并将目标点q_goal加入到随机树T中，从目标点q_goal回溯到根节点q_start，得到规划路径的路径点。其中，所述目标点q_goal的区域是指以q_goal为圆心，以eps为半径的圆形区域。

步骤8，采用三次B样条曲线对步骤7得到的路径点进行拟合，得到最终的路径。图4即为本发明方法的路径规划结果图。

为了验证本发明方法的效率和稳定性，分别对RRT路径规划方法和本发明方法进行实验验证。RRT路径规划方法得到的路径规划结果如图3所示。在相同的环境下，以20次运行结果做一次统计，统计十次。得到这两种路径规划方法在找到路径时的时间及路径长度对比关系如图5所示。由图5可知，本发明方法与RRT路径规划方法相比，在同样的环境中找到路径所需的时间及路径长度均有明显减少。十次统计实验数据平均值的对比分析如表1所示：

表1实验数据对比

以上是本发明的较佳实施例，凡依本发明技术方案所作的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。

Claims (4)

1.一种基于同心圆采样引导RRT算法的路径规划方法，其特征在于，提供规划路径的起始点、目标点、障碍物位置和规划地图，按如下步骤进行路径规划：

步骤1：定义同心圆系数m及扩展步长eps，以路径规划的起始点作为随机树T的根节点q_start，并将其作为下一次扩展随机树节点的父节点；

步骤2：以目标点作为同心圆的圆心，规划地图的外接圆半径R加上目标点与外接圆圆心距离d为采样半径，在同心圆系数m及随机数n的控制下生成随机的同心圆半径r，在半径为r的同心圆上生成随机点q_rand；

步骤3：判断随机点是否在地图范围内，若不在地图范围内，则返回步骤2重新生成随机点，若在地图范围内，则转步骤4；

步骤4：遍历随机树T中的每个节点，通过计算随机树T中节点与随机点之间的距离，找出离随机点最近的节点作为邻近点q_near；

步骤5：从邻近点q_near向随机点q_rand方向扩展步长eps，得到新节点q_new；

步骤6：判断新节点q_new与邻近点q_near之间是否存在障碍物，若不存在，则将新节点q_new加入随机树T中，并转步骤7，若存在，则返回步骤2重新采样；

步骤7：判断新节点q_new是否在目标点q_goal的区域内，若不在目标点区域内，则返回步骤2重新采样，若在目标点区域内，则将新节点q_new作为目标点的父节点，并将目标点q_goal加入到随机树T中，从目标点q_goal回溯到根节点q_start，得到规划路径的路径点；

步骤8，对步骤7得到的路径点进行拟合，得到最终的路径；

所述步骤2中，生成随机的同心圆半径r的公式如下：

上式中，n为0到1范围内的随机数，m为同心圆系数；

在半径为r的同心圆上生成随机点q_rand的公式如下：

上式中，

分别为目标点q_goal的横、纵坐标值，

分别为生成的随机点q_rand的横、纵坐标值；

所述步骤5中，扩展生成新节点q_new的方法为：

计算节点q_near与q_rand之间的距离dist：

上式中，

分别为邻近点q_near的横、纵坐标值，

分别为随机点q_rand的横、纵坐标值；

若dist小于eps，则q_new节点的坐标即为q_rand点的坐标，否则q_new节点的坐标由以下公式计算得出：

上式中，

分别为新节点q_new的横、纵坐标值。

2.根据权利要求1所述的一种基于同心圆采样引导RRT算法的路径规划方法，其特征在于，所述步骤3中，当地图是矩形时，通过判断随机点坐标是否小于地图的边界值x_max和y_max来判断随机点是否在地图范围内。

3.根据权利要求1所述的一种基于同心圆采样引导RRT算法的路径规划方法，其特征在于，所述步骤7中，所述目标点q_goal的区域是指以q_goal为圆心，以eps为半径的圆形区域。

4.根据权利要求1所述的一种基于同心圆采样引导RRT算法的路径规划方法，其特征在于，所述步骤8中，采用三次B样条曲线对步骤7得到的路径点进行拟合。

Images