CN114115271A - 一种改进rrt的机器人路径规划方法和*** - Google Patents

Abstract

本发明公开了一种改进RRT的机器人路径规划方法和***，通过自适应生成扩展节点，并优化起点到新生成扩展节点的路径，降低了RRT算法在路径规划过程中的时间复杂度；并对生成的随机扩展树进行剪枝处理，去除冗余节点，从而加快收敛速度，得到更优的路径，提高了路径规划的效率。

Description

一种改进RRT的机器人路径规划方法和***

技术领域

本发明属于机器人路径规划技术领域，具体涉及一种能够避障的机器人路径规划方法和***。

背景技术

路径规划是指在包含障碍物的给定区域内搜索到一条从起点到终点的安全无碰撞、可行的路径。基于随机点采样的快速探索随机树(rapidlyexploring random tree，RRT)是目前广泛应用于机器人路径规划的一种算法。RRT算法以一个初始点作为根结点，通过随机采样增加叶子结点的方式，生成一个随机扩展树，当随机树中的叶子结点包含了目标结点或进入了目标区域，便可以在随机树中找到一条由初始点到目标点的路径，该方法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径。

但也存在一些明显的缺点：对环境信息不敏感，当可行区域狭窄且复杂，或者存在大量不规则障碍物时，RRT算法收敛到最优解的效率会大打折扣，甚至探索失败。

发明内容

发明目的：本发明提供一种改进RRT的机器人路径规划方法和***，能够解决采用RRT算法进行路径规划时存在的收敛速度慢、易陷入局部最优解的问题，提高路径规划的效率。

技术方案：本发明一方面公开了一种改进RRT的机器人路径规划方法，包括：

S1、将机器人活动的二维场景分为可通行区域和不可通行的障碍物，建立二维栅格图；获取机器人在二维场景中的起始位置Q_start和目的位置Q_goal；

如果从Q_start到Q_goal的直线路径没有碰触障碍物，则规划路径为从Q_start到Q_goal的直线段；否则，将Q_start作为RRT算法中随机扩展树的根节点，初始化目标方向权重k＝1，根据步骤S2-S7确定规划路径；

S2、在机器人运动区域生成随机位置点Q_rand；如果Q_start与Q_rand的连线穿过障碍物，则丢弃该Q_rand，并再次生成新的Q_rand，直至Q_start与Q_rand的连线没有穿过障碍物；

S3、遍历当前随机扩展树，查找与Q_rand距离最近的点Q_near；

S4、计算Q_near到障碍物的最小距离D_ob，计算斥力因子h：

ρ为机器人运动步长；

计算Q_rand对Q_near的引力

Q_goal对Q_near的引力

障碍物对Q_near的斥力

计算Q_near处受到的合力

其中

w为引力权重，是预设的(0,1]范围内的常数；Q_ob为距离Q_near最近的障碍物位置；||为计算二维环境栅格图中两个位置点的距离；

以Q_near为父节点，根据合力

生成新节点Q_new：

判断新节点Q_new和Q_near的连线是否与障碍物有碰撞，如没有碰撞则将Q_new添加到随机扩展树上，如果有碰撞跳转至S2重新生成随机位置点；

S5、优化起点Q_start到Q_new的路径；

S6、重复步骤S3-S5，当新节点Q_new进入目的位置Q_goal的邻域内或为Q_goal本身时，停止随机扩展树的扩展；对随机扩展树进行剪枝处理；

S7、从目的位置Q_goal开始依次回溯父节点直到起始点Q_start结束，找到一条从Q_start到Q_goal的无障碍路径。

作为优选，所述步骤S4中还包括：根据F₁和F₂更新k：如果F₁＞F₂，k＝k+Δk；否则k＝k-Δk；Δk为预设的更新步长。

具体地，所述步骤S5具体包括：

S5.1、以Q_new为圆心，R_near为半径计算邻域V_near；R_near为预设的布线长度；

S5.2、随机扩展树中位于邻域V_near内的节点构成集合S_near，获取集合S_near中每个节点的第一外节点，组成集合S_pnear；节点s的第一外节点为随机扩展树中与节点s间的距离小于第一距离R₁的节点；s∈S_near，S_sum＝S_near∪S_pnear；

S5.3、遍历集合S_sum中的节点，判断Q_new的父节点替换成S_sum中的节点后，是否会缩短Q_start到Q_new的路径，如果缩短则进行Q_new的父节点替换操作，反之不替换；

S5.4、寻找Q_new的第二外节点的集合S_pnew，节点Q_new的第二外节点为随机扩展树中与Q_new间的距离小于第二距离R₂的节点；

S5.5、遍历S_near中的节点，判断如果其父节点更换为Q_new与S_pnew的并集内的节点后是否会减小Q_start到所述S_near中的节点的路径，如果路径有所减小，则进行所述S_near中的节点父节点的替换操作，反之不操作。

具体地，所述步骤S6中对随机扩展树进行剪枝处理具体为：

从随机扩展树的根节点Q_start开始，寻找能够无碰撞连接目的位置Q_goal的中间点P1，接着以P1为起点寻找能够无碰撞连接目的位置Q_goal的中间点P2，如此循环往复，直至找到Q_goal或进入目的位置Q_goal的邻域内；Q_start和所有中间点按序构成优化后的路径。

另一方面，本发明还公开了一种改进RRT的机器人路径规划***，包括：

二维场景建立模块，用于将机器人活动的二维场景分为可通行区域和不可通行的障碍物，建立二维栅格图；获取机器人在二维场景中的起始位置Q_start和目的位置Q_goal；

随机位置点Q_rand生成模块，用于随机位置点Q_rand；

最近点Q_near生成模块，用于遍历当前随机扩展树，查找与Q_rand距离最近的点Q_near；

新节点Q_new生成模块，用于生成新节点Q_new，生成步骤为：

计算Q_near到障碍物的最小距离D_ob，计算斥力因子h：

ρ为机器人运动步长；

计算Q_rand对Q_near的引力

Q_goal对Q_near的引力

障碍物对Q_near的斥力

计算Q_near处受到的合力

其中

w为引力权重，是预设的(0,1]范围内的常数；Q_ob为距离Q_near最近的障碍物位置；||为计算二维环境栅格图中两个位置点的距离；

以Q_near为父节点，根据合力

生成新节点Q_new：

判断新节点Q_new和Q_near的连线是否与障碍物有碰撞，如没有碰撞则将Q_new添加到随机扩展树上，如果有碰撞则随机位置点Q_rand生成模块重新生成随机位置点；

起点Q_start到Q_new的路径优化模块，用于优化起点Q_start到Q_new的路径；

停止扩展判断模块，用于判断是否停止随机扩展树的扩展；判断方法为：当新节点Q_new进入目的位置Q_goal的邻域内或为Q_goal本身时，停止随机扩展树的扩展；

剪枝处理模块，用于对随机扩展树进行剪枝处理；

路径生成模块，用于从目的位置Q_goal开始依次回溯父节点直到起始点Q_start结束，找到一条从Q_start到Q_goal的无障碍路径。

有益效果：本发明公开的机器人路径规划方法和***具有如下优点：

1、本发明在Qnew的扩展中引入引力法，并设计自适应函数调节随机点引力与目标点引力在新节点扩展过程中的占比，以此提高RRT算法路径搜索速度和避障能力；

2、在路径优化的过程中，引入了改进重布线法，减小了时间复杂度，使得本发明的改进RRT算法具有渐进优化性；

3、本发明在节点的处理中，引入了贪心算法进行剪枝处理，过滤了大量的冗余节点，在保持时间复杂度和空间复杂度相同的情况下，获得了更优的初始路径和更快的收敛速度。

附图说明

图1为本发明公开机器人路径规划方法流程图；

图2为二维场景示意图；

图3为生成新节点的示意图；

图4为优化路径的示意图；

图5为剪枝处理的示意图；

图6为本发明公开机器人路径规划***的组成示意图；

图7为实施例中仿真结果图；

图8为实施例中仿真结果经剪枝处理后的示意图；

图9为机器人运动方式示意图。

具体实施方式

下面结合附图和具体实施方式，进一步阐明本发明。

本发明公开了一种改进RRT的机器人路径规划方法，如图1所示，包括：

S1、将机器人活动的二维场景分为可通行区域和不可通行的障碍物，建立二维栅格图；获取机器人在二维场景中的起始位置Q_start和目的位置Q_goal；

本实施例中利用激光雷达、双目相机提取机器人活动场景的点云数据，结合ros平台，对点云数据的成像，进行可通行区域和障碍物的区分，所构建的二维场景如图2所示，障碍物为黑色部分，其余白色部分为机器人可通行的区域。

如果从Q_start到Q_goal的直线路径没有碰触障碍物，则规划路径为从Q_start到Q_goal的直线段；否则，将Q_start作为RRT算法中随机扩展树的根节点，初始化目标方向权重k＝1，根据步骤S2-S7确定规划路径；

S2、在机器人运动区域生成随机位置点Q_rand；如果Q_start与Q_rand的连线穿过障碍物，则丢弃该Q_rand，并再次生成新的Q_rand，直至Q_start与Q_rand的连线没有穿过障碍物；

S3、遍历当前随机扩展树，查找与Q_rand距离最近的点Q_near；

S4、计算Q_near到障碍物的最小距离D_ob，计算斥力因子h：

ρ为机器人运动步长；

计算Q_rand对Q_near的引力

Q_goal对Q_near的引力

障碍物对Q_near的斥力

计算Q_near处受到的合力

其中

w为引力权重，是预设的(0,1]范围内的常数；Q_ob为距离Q_near最近的障碍物位置；||||为计算二维环境栅格图中两个位置点的距离；

在

的计算式中添加与障碍物距离有关的自适应函数，以此调节斥力在合力中的占比，避免遇到连续障碍物时震荡不前，使随机树靠近障碍物区域时尽快往远离障碍物区域扩散，在无障碍区域时快速朝目标点扩展，缩短规划时间，减少采样点数。

为了避免随机树节点到达目标点时发生振荡，本实施例根据当前节点距离目标点的远近来调整目标方向权重k：如果F₁＞F₂，即当前位置距离目标位置较远，增大k的值，k＝k+Δk；否则减小k的值，k＝k-Δk；Δk为预设的更新步长，本实施例中Δk＝0.1。

以Q_near为父节点，根据合力

生成新节点Q_new：

如图3所示，为新节点Q_new与合力

的示意图；

判断新节点Q_new和Q_near的连线是否与障碍物有碰撞，如没有碰撞则将Q_new添加到随机扩展树上，如果有碰撞跳转至S2重新生成随机位置点；

S5、优化起点Q_start到Q_new的路径；具体包括：

S5.1、以Q_new为圆心，R_near为半径计算邻域V_near；R_near为预设的布线长度；

S5.2、随机扩展树中位于邻域V_near内的节点构成集合S_near，获取集合S_near中每个节点的第一外节点，组成集合S_pnear；节点s的第一外节点为随机扩展树中与节点s间的距离小于第一距离R₁的节点；s∈S_near；S_sum＝S_near∪S_pnear；

如图4所示，图中小圆圈表示随机扩展树中的节点，小圆圈中的数字为节点的编号，节点之间的连线表示路径，连线上的数字表示路径长度。图4(a)中，1号节点为起点Q_start，5号节点为前面步骤S3查找到的最近点Q_near，并根据S4生成新节点Q_new为10号节点。如图4(b)，以Q_new为圆心R_near为半径得到的邻域V_near为图中虚线圆内部区域；V_near内部的节点有编号为5、6、7、9、10的节点，构成了集合S_near；分别以编号为5、6、7、9、10的节点为圆心，第一距离R₁为半径画圆形，圆形内部的节点为圆心处节点的第一外节点。本实施例中R₁设置为3，则可以得到6号节点的第一外节点为8号节点；7号节点的第一外节点为11号节点；则得到S_pnear由编号为8、11的节点组成，进而得到集合S_near和S_pnear的并集S_sum。此处通过设置由第一距离R₁限定的邻域V_near内部节点的外节点集合，是为了能均匀的搜索V_near周边的范围，降低因随机点的扩展的不确定性而导致的搜索时间过长的概率，减小搜索时长，加快路径优化的速度，提升路径搜索的效率。

S5.3、遍历集合S_sum中的节点，判断Q_new的父节点替换成S_sum中的节点后，是否会缩短Q_start到Q_new的路径，如果缩短则进行Q_new的父节点替换操作，反之不替换；如图4(c)，遍历后将Q_new的父节点替换为6号节点；

S5.4、寻找Q_new的第二外节点的集合S_pnew，节点Q_new的第二外节点为随机扩展树中与Q_new间的距离小于第二距离R₂的节点；本实施例中设置R₂为4，如图4(d)，实线大圆内5、6、7、9、11构成了集合S_pnew。

S5.5、遍历S_near中的节点，判断如果其父节点更换为Q_new与S_pnew的并集内的节点后是否会减小Q_start到所述S_near中的节点的路径，如果路径有所减小，则进行所述S_near中的节点父节点的替换操作，反之不操作。如图4(e)所示，节点7的父节点更换为节点11。

S6、重复步骤S3-S5，当新节点Q_new进入目的位置Q_goal的邻域内或为Q_goal本身时，停止随机扩展树的扩展；对随机扩展树进行剪枝处理；剪枝处理具体为：

从随机扩展树的根节点Q_start开始，寻找能够无碰撞连接目的位置Q_goal的中间点P1，接着以P1为起点寻找能够无碰撞连接目的位置Q_goal的中间点P2，如此循环往复，直至找到Q_goal或进入目的位置Q_goal的邻域内；Q_start和所有中间点按序构成优化后的路径。如图5所示，圆形为起点，方形为目的点，根据随机扩展树得到的路径是图中L1所示的路径，经过剪枝处理后的路径为L2。

S7、从目的位置Q_goal开始依次回溯父节点直到起始点Q_start结束，找到一条从Q_start到Q_goal的无障碍路径。

本发明还公开了实现上述机器人路径规划方法的***，如图6所示，包括：

二维场景建立模块1，用于将机器人活动的二维场景分为可通行区域和不可通行的障碍物，建立二维栅格图；获取机器人在二维场景中的起始位置Q_start和目的位置Q_goal；

随机位置点Q_rand生成模块2，用于随机位置点Q_rand；

最近点Q_near生成模块3，用于遍历当前随机扩展树，查找与Q_rand距离最近的点Q_near；

新节点Q_new生成模块4，用于生成新节点Q_new，生成步骤为：

计算Q_near到障碍物的最小距离D_ob，计算斥力因子h：

ρ为机器人运动步长；

计算Q_rand对的引力F₁、Q_goal对Q_near的引力F₂、障碍物对Q_near的斥力F₃；计算Q_near处受到的合力

其中

w为引力权重，是预设的(0,1]范围内的常数；Q_ob为距离Q_near最近的障碍物位置；||||为计算二维环境栅格图中两个位置点的距离；

根据F₁和F₂更新k：如果F₁＞F₂，k＝k+Δk；否则k＝k-Δk；Δk为预设的更新步长。

以Q_near为父节点，根据合力

生成新节点Q_new：

判断新节点Q_new和Q_near的连线是否与障碍物有碰撞，如没有碰撞则将Q_new添加到随机扩展树上，如果有碰撞则随机位置点Q_rand生成模块重新生成随机位置点；

起点Q_start到Q_new的路径优化模块5，用于按步骤S5.1-S5.5优化起点Q_start到Q_new的路径；

停止扩展判断模块6，用于判断是否停止随机扩展树的扩展；判断方法为：当新节点Q_new进入目的位置Q_goal的邻域内或为Q_goal本身时，停止随机扩展树的扩展；

剪枝处理模块7，用于对随机扩展树进行剪枝处理；

路径生成模块8，用于从目的位置Q_goal开始依次回溯父节点直到起始点Q_start结束，找到一条从Q_start到Q_goal的无障碍路径。

本发明改进的RRT算法的路径规划的效果图如图7所示，经过剪枝处理后的路径规划效果图如图8所示，图中细线部分为每次扩展的步长，粗线部分为实际的搜索的路径，图中起点为(0，30)，终点为(46，20)。改进后的路径相比较传统的RRT算法规划出来的路径，其迭代的次数降低了27％，所用的时间降低了47％，规划出来的路径长度减少了58％，拐角数目减少了69％。

本实施例中，机器人的运动方式为八邻域运动，即可以前进、后退、左移、右移、向左前方45°移动、向右前方45°移动、向左后方45°移动、向右后方45°移动，如图9所示。

Claims (8)

1.一种改进RRT的机器人路径规划方法，其特征在于，包括：

S1、将机器人活动的二维场景分为可通行区域和不可通行的障碍物，建立二维栅格图；获取机器人在二维场景中的起始位置Q_start和目的位置Q_goal；

如果从Q_start到Q_goal的直线路径没有碰触障碍物，则规划路径为从Q_start到Q_goal的直线段；否则，将Q_start作为RRT算法中随机扩展树的根节点，初始化目标方向权重k＝1，根据步骤S2-S7确定规划路径；

S2、在机器人运动区域生成随机位置点Q_rand；如果Q_start与Q_rand的连线穿过障碍物，则丢弃该Q_rand，并再次生成新的Q_rand，直至Q_start与Q_rand的连线没有穿过障碍物；

S3、遍历当前随机扩展树，查找与Q_rand距离最近的点Q_near；

S4、计算Q_near到障碍物的最小距离D_ob，计算斥力因子h：

ρ为机器人运动步长；

计算Q_rand对Q_near的引力

Q_goal对Q_near的引力

障碍物对Q_near的斥力

计算Q_near处受到的合力

其中

w为引力权重，是预设的(0,1]范围内的常数；Q_ob为距离Q_near最近的障碍物位置；|| ||为计算二维环境栅格图中两个位置点的距离；

以Q_near为父节点，根据合力

生成新节点Q_new：

判断新节点Q_new和Q_near的连线是否与障碍物有碰撞，如没有碰撞则将Q_new添加到随机扩展树上，如果有碰撞跳转至S2重新生成随机位置点；

S5、优化起点Q_start到Q_new的路径；

S6、重复步骤S3-S5，当新节点Q_new进入目的位置Q_goal的邻域内或为Q_goal本身时，停止随机扩展树的扩展；对随机扩展树进行剪枝处理；

S7、从目的位置Q_goal开始依次回溯父节点直到起始点Q_start结束，找到一条从Q_start到Q_goal的无障碍路径。

2.根据权利要求1所述的机器人路径规划方法，其特征在于，所述步骤S4中还包括：根据F₁和F₂更新k：如果F₁＞F₂，k＝k+Δk；否则k＝k-Δk；Δk为预设的更新步长。

3.根据权利要求1所述的机器人路径规划方法，其特征在于，所述步骤S5具体包括：

S5.1、以Q_new为圆心，R_near为半径计算邻域V_near；R_near为预设的布线长度；

S5.2、随机扩展树中位于邻域V_near内的节点构成集合S_near，获取集合S_near中每个节点的第一外节点，组成集合S_pnear；节点s的第一外节点为随机扩展树中与节点s间的距离小于第一距离R₁的节点；s∈S_near；，S_sum＝S_near∪S_pnear；

S5.3、遍历集合S_sum中的节点，判断Q_new的父节点替换成S_sum中的节点后，是否会缩短Q_start到Q_new的路径，如果缩短则进行Q_new的父节点替换操作，反之不替换；

S5.4、寻找Q_new的第二外节点的集合S_pnew，节点Q_new的第二外节点为随机扩展树中与Q_new间的距离小于第二距离R₂的节点；

S5.5、遍历S_near中的节点，判断如果其父节点更换为Q_new与S_pnew的并集内的节点后是否会减小Q_start到所述S_near中的节点的路径，如果路径有所减小，则进行所述S_near中的节点父节点的替换操作，反之不操作。

4.根据权利要求1所述的机器人路径规划方法，其特征在于，所述步骤S6中对随机扩展树进行剪枝处理具体为：

从随机扩展树的根节点Q_start开始，寻找能够无碰撞连接目的位置Q_goal的中间点P1，接着以P1为起点寻找能够无碰撞连接目的位置Q_goal的中间点P2，如此循环往复，直至找到Q_goal或进入目的位置Q_goal的邻域内；Q_start和所有中间点按序构成优化后的路径。

5.一种改进RRT的机器人路径规划***，其特征在于，包括：

二维场景建立模块，用于将机器人活动的二维场景分为可通行区域和不可通行的障碍物，建立二维栅格图；获取机器人在二维场景中的起始位置Q_start和目的位置Q_goal；

随机位置点Q_rand生成模块，用于随机位置点Q_rand；

最近点Q_near生成模块，用于遍历当前随机扩展树，查找与Q_rand距离最近的点Q_near；

新节点Q_new生成模块，用于生成新节点Q_new，生成步骤为：

计算Q_near到障碍物的最小距离D_ob，计算斥力因子h：

ρ为机器人运动步长；

计算Q_rand对Q_near的引力

Q_goal对Q_near的引力

障碍物对Q_near的斥力

计算Q_near处受到的合力

其中

w为引力权重，是预设的(0,1]范围内的常数；Q_ob为距离Q_near最近的障碍物位置；|| ||为计算二维环境栅格图中两个位置点的距离；

以Q_near为父节点，根据合力

生成新节点Q_new：

判断新节点Q_new和Q_near的连线是否与障碍物有碰撞，如没有碰撞则将Q_new添加到随机扩展树上，如果有碰撞则随机位置点Q_rand生成模块重新生成随机位置点；

起点Q_start到Q_new的路径优化模块，用于优化起点Q_start到Q_new的路径；

停止扩展判断模块，用于判断是否停止随机扩展树的扩展；判断方法为：当新节点Q_new进入目的位置Q_goal的邻域内或为Q_goal本身时，停止随机扩展树的扩展；

剪枝处理模块，用于对随机扩展树进行剪枝处理；

路径生成模块，用于从目的位置Q_goal开始依次回溯父节点直到起始点Q_start结束，找到一条从Q_start到Q_goal的无障碍路径。

6.根据权利要求5所述的机器人路径规划***，其特征在于，所述新节点Q_new生成模块还用于：根据F₁和F₂更新k：如果F₁＞F₂，k＝k+Δk；否则k＝k-Δk；Δk为预设的更新步长。

7.根据权利要求5所述的机器人路径规划***，其特征在于，起点Q_start到Q_new的路径优化模块进行优化具体包括：

S5.1、以Q_new为圆心，R_near为半径计算邻域V_near；R_near为预设的布线长度；

S5.2、随机扩展树中位于邻域V_near内的节点构成集合S_near，获取集合S_near中每个节点的第一外节点，组成集合S_pnear；节点s的第一外节点为随机扩展树中与节点s间的距离小于第一距离R₁的节点；s∈S_near；，S_sum＝S_near∪S_pnear；

S5.3、遍历集合S_sum中的节点，判断Q_new的父节点替换成S_sum中的节点后，是否会缩短Q_start到Q_new的路径，如果缩短则进行Q_new的父节点替换操作，反之不替换；

S5.4、寻找Q_new的第二外节点的集合S_pnew，节点Q_new的第二外节点为随机扩展树中与Q_new间的距离小于第二距离R₂的节点；

S5.5、遍历S_near中的节点，判断如果其父节点更换为Q_new与S_pnew的并集内的节点后是否会减小Q_start到所述S_near中的节点的路径，如果路径有所减小，则进行所述S_near中的节点父节点的替换操作，反之不操作。

8.根据权利要求5所述的机器人路径规划***，其特征在于，剪枝处理模块对随机扩展树进行剪枝处理具体为：

从随机扩展树的根节点Q_start开始，寻找能够无碰撞连接目的位置Q_goal的中间点P1，接着以P1为起点寻找能够无碰撞连接目的位置Q_goal的中间点P2，如此循环往复，直至找到Q_goal或进入目的位置Q_goal的邻域内；Q_start和所有中间点按序构成优化后的路径。

Images