CN114536328B - 一种基于改进rrt算法的机械臂运动规划方法 - Google Patents

Abstract

本发明属于机械臂运动规划技术领域，具体涉及一种基于改进RRT算法的机械臂运动规划方法。本发明包括以下步骤：起点采样策略；目标点膨胀策略；起点与目标点之间采用概率目标偏向策略。通过本发明的上述方法，可减少无用区域的搜索和无效节点的出现，并且优化起点区域的采样，防止了起点附近节点采样导致的时间浪费，算法的收敛速度可得到显著提升。

Description

一种基于改进RRT算法的机械臂运动规划方法

技术领域

本发明属于机械臂运动规划技术领域，具体涉及一种基于改进RRT算法的机械臂运动规划方法。

背景技术

机械臂的运动规划场景中，传统的路径规划算法有多边形拟合法、栅格法、人工势场法、遗传算法等；但这些方法都需要在确定的空间内对障碍物进行建模，计算复杂度与机器人自由度呈指数关系，不适合解决多自由度机器人在复杂环境中的规划。新的基于快速扩展随机树(RRT)的路径规划算法，可通过对状态空间中的采样点进行碰撞检测，来避免对空间的建模，能够有效地解决高维空间和复杂约束的路径规划问题。RRT算法的特点是能够快速有效地搜索高维空间，通过状态空间的随机采样点，把搜索导向空白区域，从而寻找到一条从起始点到目标点的规划路径，适合解决多自由度机器人在复杂环境下和动态环境中的路径规划，因此被广泛应用于机械臂的运动规划场景；但同时，它也存在着收敛速度慢，搜索随机性太强，规划路径冗长且存在很多不必要的拐点导致路径曲折等问题，亟待解决。

发明内容

本发明的目的是克服上述现有技术的不足，提供一种基于改进RRT算法的机械臂运动规划方法，本方法首先在起点区域找出无障碍范围，进行无需再次碰撞检测的节点采样，然后在目标点附近，寻找无碰撞区域，对目标点区域进行膨胀，能够根据地图动态调整目标点区域；最后再使用具有目标偏向策略的随机采样进行节点树的扩展，判断节点是否到达目标点区域，以便找到一条可行的路径。通过本发明的上述方法，可减少无用区域的搜索和无效节点的出现，并且优化起点区域的采样，防止了起点附近节点采样导致的时间浪费，算法的收敛速度可得到显著提升。

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

一种基于改进RRT算法的机械臂运动规划方法，其特征在于包括以下步骤：

1)、起点采样策略

在路径规划之前，对起点进行碰撞检测，得到起点与障碍物的最短距离d_init；在地图范围内，在以起点为圆心，d_init为半径的圆上，以步长为间距进行等间距采样，所有采用点均以起点为父节点，路径代价为d_init；

2)、目标点膨胀策略

采用基于地图的动态距离来判断是否达到终点，包括：

采样前，采用与步骤1)相同的方法，计算终点与障碍物的最短距离d_goal，并以终点为圆心，距离d_goal为半径的圆形区域内为目标点膨胀区域；当合法节点到达该区域内时，无需进行碰撞检测，将终点加入节点树，并返回路径；

3)、起点与目标点之间采用概率目标偏向策略

按照以下公式，在起点与目标点之间的随机采样过程中，设置概率阈值p(p∈[0,1])；当p值小于阈值时，采样点为随机点，当p值大于等于阈值时，采样点为目标点；

本发明的有益效果在于：

1)、对于目标点而言，传统的RRT算法每次运行都是从起点开始进行采样，但是由于特定步长的限制，导致起点附近会出现大量曲折的采样路径出现，存在大量的无用节点和路径。本文采用起点采样优化策略，避免大量低效率节点的出现，所有采用点均以起点为父节点，路径代价为d_init，并且无需进行碰撞检测，因为他们都在障碍物之外。围绕起点的等距采样得到的所有节点，只进行了一次碰撞检测，减少了算法浪费在碰撞检测上的时间。同时，在后续的随机采样过程中，增加起点区域的判断，删除了无效节点，避免节点回归到起点附近。同时使得路径直接从圆上节点区域进行向外扩展，让RRT的随机采样直接进入障碍物区域进行路径探索，最大化的发挥了RRT算法在复杂空间的路径探索能力。

2)、在常规RRT算法中，针对最后是否到达目标点的问题，使用的是固定距离方法。通常是自定义一定长度或者是直接使用步长作为判断条件，当节点树上的最新节点与终点的距离满足小于或等于该长度的条件时，则进行碰撞检测，如果无障碍物，认定此时路径规划完成。将终点加入节点树路径中，父节点为该新节点，代价为两者的欧式距离，返回起点到终点的节点路径。固定距离判断条件使用时选定的固定距离与地图信息无关，不能随地图变化而变化，导致很多时候终点附近大量的节点浪费。同时出现很多已经到达终点附近的节点，但因不满足距离条件，无法直接收敛的问题。对于本发明而言，当合法节点到达该区域内时，无需进行碰撞检测，将终点加入节点树，返回路径。本发明的膨胀方法将目标点区域与地图信息结合起来，提高了算法效率，避免了目标点附近无效节点的采样，极大的提高了算法的收敛速度，节省了规划时间。

3)、当起点与目标点拟定后，两者的规划路径就至关重要。由于随机采样没有方向性，导致无用节点偏多，本文采用概率目标偏向策略，在不耽误目标点随机性的同时，对随机树增加了一定的方向性限制，保证了随机树始终朝着目标点的方向发展，避免了过多无用点的产生，成效显著。

附图说明

图1为本发明的起点采用示意图；

图2为本发明的目标点膨胀示意图；

图3为简单环境下的RRT实验图；

图4为简单环境下的RRT-star实验图；

图5为简单环境下的本发明的实验图；

图6为复杂环境下的RRT实验图；

图7为复杂环境下的RRT-star实验图；

图8为复杂环境下的本发明的实验图。

具体实施方式

为便于理解，此处结合图1-8，对本发明的具体结构及工作方式作以下进一步描述：

1.1采样优化

1.1.1概率目标偏向策略

随机采样没有方向性，导致无用节点偏多，本文采用概率目标偏向策略对新节点的方向性进行一定的启发，来优化采样过程。

本发明如上述公式所示，在随机采样过程中，设置概率阈值p(p∈[0,1])；当p值小于阈值时，采样点为随机点，当p值大于等于阈值时，采样点为目标点。通过使用该目标偏向策略，在不耽误目标点随机性的同时，对随机树增加了一定的方向性限制，保证了随机树始终朝着目标点的方向发展，避免了过多无用点的产生；具体参照图1所示。

1.2起点采样优化

RRT算法每次运行都是从起点开始进行采样，但是由于特定步长的限制，导致起点附近会出现大量曲折的采样路径出现，存在大量的无用节点和路径。本文采用起点采样优化策略，避免大量低效率节点的出现，具体流程如下。

本发明参照图2所示，首先，在路径规划之前，对起点进行碰撞检测，得到起点与障碍物的最短距离d_init；之后，在地图范围内，在以起点为圆心，d_init为半径的圆上，以步长为间距进行等间距采样，所有采用点均以起点为父节点，路径代价为d_init，并且无需进行碰撞检测，因为他们都在障碍物之外。围绕起点的等距采样得到的所有节点，只进行了一次碰撞检测，减少了算法浪费在碰撞检测上的时间。

而在后续的随机采样过程中，增加起点区域的判断，删除了无效节点，避免节点回归到起点附近。同时使得路径直接从圆上节点区域进行向外扩展，让RRT的随机采样直接进入障碍物区域进行路径探索，最大化的发挥了RRT算法在复杂空间的路径探索能力。

1.3目标点膨胀

1.3.1目标点到达判断

在常规RRT算法中，针对最后是否到达目标点的问题，使用的是固定距离方法。通常是自定义一定长度或者是直接使用步长作为判断条件，当节点树上的最新节点与终点的距离满足小于或等于该长度的条件时，则进行碰撞检测，如果无障碍物，认定此时路径规划完成。将终点加入节点树路径中，父节点为该新节点，代价为两者的欧式距离，返回起点到终点的节点路径。

固定距离判断条件使用时选定的固定距离与地图信息无关，不能随地图变化而变化，导致很多时候终点附近大量的节点浪费。同时出现很多已经到达终点附近的节点，但因不满足距离条件，无法直接收敛的问题。

1.3.2目标点膨胀

针对上述问题，本发明采用基于地图的动态距离，来判断是否达到终点。采样前，采用与起点采样优化相同的方法，计算终点与障碍物的最短距离d_goal，并以终点为圆心，距离d_goal为半径的圆形区域内为目标点膨胀区域；当合法节点到达该区域内时，无需进行碰撞检测，将终点加入节点树，并返回路径。本发明的该膨胀方法将目标点区域与地图信息结合起来，提高了算法效率，避免了目标点附近无效节点的采样，极大的提高了算法的收敛速度，节省了规划时间。

为了验证算法的效率和可行性，本发明通过仿真实验对算法进行验证，具体如下：

测试的计算机硬件环境为：Intel酷睿CPU，型号i5-9300H，2.4GHz，内存为16GB，显卡为UHD Graphics 630。

为了测试算法的综合性能，本文采用简单地图和复杂地图两种地图在二维环境下进行仿真，来验证本发明的优势性，如下：

仿真环境下测试时，设置以下相关参数：

地图大小为20×20，起始点为(0，0)，目标点为(12，15)，固定搜索步长为2.0，最大采样次数为200，判断机械臂是否达到的阈值为步长值，改进的目标偏执概率为0.1。由于RRT算法的随机性，所以在每个地图执行50次实验，记录对应的时间、节点数目，路径长度，并计算出平均数来测试算法的性能。

实施例1：简单环境测试

首先，在简单环境中测试，结果如图3-5所示。

图3-5中，黑色大圆形代表障碍物，黑色小圆点点位代表有效节点，黑色小圆点之间的连线代表路径和节点之间的关系，连线代表规划出的可行路径。由图3可以看出，RRT算法规划出的路径比较曲折，无用拐点很多，并且在起点和目标点位置存在多次采样，但始终不能快速收敛的问题。图4中的RRT-star算法规划出的路线虽然相对较优，但是仍存在大量的无用节点，碰撞检测时浪费时间，导致算法规划时间较长。相比之下，本发明的改进RRT算法所获得的图5，能够在起点和目标点附近去除无用节点，不需要反复进行碰撞检测，算法收敛速度较快，用时短，规划出的路径相对比较平滑。

表1

由表1可以看出，本发明的改进RRT算法的平均规划时间是1.853s，相对于RRT算法的11.917s和RRT-star算法的7.026秒缩短了84.4％和73.6％。平均节点个数由RRT算法的43.95个和RRT-star算法的39个较低到了17.7个。虽然在规划路径长度方面，RRT-star算法是最短的，但是它同时也消耗了大量的时间，而相比之下改进RRT算法的在时间最短的情况下获取了相对较优的路径长度。

实施例2：复杂环境测试

复杂环境测试结果参照图6-8所示，而复杂地图下算法性能对比则如下表2所示：

表2

由图6-8和表3可以看出，在障碍物较多的地图中，本发明的改进RRT算法节点个数和路径长度的优势有所下降，但是相比于剩余两种传统算法仍，无论是路径长度、节点个数还是平均规划时间均具有明显的优势。

通过以上仿真结果可知，无论是在简单地图中还是在复杂地图中，改进RRT算法在规划时间上一直都有较大的优势。虽然在路径长度上不如RRT-star规划效果好，但是结合规划时间和规划节点个数综合分析，改进RRT算法性能仍然是最佳的。

当然，对于本领域技术人员而言，本发明不限于上述示范性实施例的细节，而还包括在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现的相同或类似方法。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。

此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。

本发明未详细描述的技术均为公知技术。

Claims (1)

1.一种基于改进RRT算法的机械臂运动规划方法，其特征在于包括以下步骤：

1)、起点采样策略

在路径规划之前，对起点进行碰撞检测，得到起点与障碍物的最短距离d_init；在地图范围内，在以起点为圆心，d_init为半径的圆上，以步长为间距进行等间距采样，所有采用点均以起点为父节点，路径代价为d_init；

2)、目标点膨胀策略

采用基于地图的动态距离来判断是否达到终点，包括：

采样前，采用与步骤1)相同的方法，计算终点与障碍物的最短距离d_goal，并以终点为圆心，距离d_goal为半径的圆形区域内为目标点膨胀区域；当合法节点到达该区域内时，无需进行碰撞检测，将终点加入节点树，并返回路径；

3)、起点与目标点之间采用概率目标偏向策略

按照以下公式，在起点与目标点之间的随机采样过程中，设置概率阈值p_target,p_target∈[0,1]；当随机采样的p值小于概率阈值p_target时，采样点为随机点，当随机采样的p值大于等于概率阈值p_target时，采样点为目标点；