CN115056222A - 一种基于改进rrt算法的机械臂路径规划方法 - Google Patents

Abstract

本发明公开了一种基于改进RRT算法的机械臂路径规划方法，首先以合向量的形式生成节点，有利于快速找到路径；其次使用动态步长一定程度上使得算法避免了在复杂障碍物环境下难以找到路径的问题，防随机树逆生长避免了后期节点过多而导致的大量计算；进一步，基于三角剪枝策略在初始路径的基础上有效减少了路径代价，并使用三次B样条曲线对剪枝后的路径光滑处理，降低了机械臂的抖动，减少机械臂运动过程中而产生的磨损。通过改进RRT算法降低了路径距离且优化了路径质量，避免盲目采样的同时提高了规划效率。

Description

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

技术领域

本发明涉及路径规划术领域，具体而言，涉及一种基于改进RRT算法的机械臂路径规划方法。

背景技术

路径规划技术于20世纪70年代被提出，至今仍是机器人领域的重要研究方向之一。经过50多年的快速发展，许多理论算法被相继提出，用于解决路径规划问题。

机械臂的避障规划目标是规划出一条满足各类指标的最优路径，针对这一问题，路径规划算法大致可以被归纳为以下几种：基于图论路径规划算法、人工势场法、基于生物启发路径规划算法、基于离散采样路径规划算法。其中，基于图论路径规划算法的复杂度依赖于网格分辨率，将其应用于三维空间时，计算复杂度高且耗时，不太适用于机械臂的工作环境。人工势场法寻找路径时经常陷入局部最小值，因此不能保证一定找到解。基于生物启发路径规划算法需要通过大量的训练获取算法参数，运算量大，耗时久、需要占用较大的内存空间。基于离散采样算法，参数少，结构简单，算法的复杂度不依赖于地图的复杂度，并且具备概率完备性。因此，离散采样算法比较适合应用于高维环境的路径规划。

而RRT路径规划算法作为离散采样算法的一种，不仅搜索能力强，且不需要对环境进行建模，易与其它算法结合解决高纬空间、复杂约束环境下的路径规划，不足之处在于算法的后期节点利用率偏低，计算量偏大，生成的路径不稳定。

现有技术优化后的RRT算法，例如专利CN 114115239 A虽加快了算法的搜索速度，但生成的路径具有较多的转折点，不满足机器人的运动学规律；专利CN 110497403 A采用双向RRT策略，没有考虑到路径平滑问题。纵观现有技术的优化后的RRT算法，只是单一地提升了某项性能，例如减少了规划时间、缩短了路径代价，并没有考虑到路径代价与规划时间两者之间的平衡；其次，规划出的路径常伴有较多的转折点，没有考虑到转折点处的路径点会对机械臂造成物理上的冲击，这种状况的出现将会加剧机械臂的磨损。

发明内容

有鉴于此，本发明实施例的目的在于提供一种基于改进RRT算法的机械臂路径规划方法，以提高机械臂路径规划效率的同时确保路径代价较小，且生成的路径光滑，满足机械臂的运动学规律。

本发明的第一方面提供了一种基于改进RRT算法的机械臂路径规划方法，所述方法包括：

S1，初始化树T，给定起始点x_init、目标点x_goal、扩展步长eps，最大迭代次数max、空间障碍物信息obs、目标点阈值m；

S2，采用概率偏置生成随机点x_rand；当rand＜bias时，采样点x_rand向目标点x_goal采样；否则，采样点x_rand随机采样；

S3，在树T中寻找距离随机点最近的近邻点x_near，判断x_near是否进入目标点阈值范围内，若进入则直接缩短步长为eps1，其中，eps1<m，反之采用原步长；沿着

与

合向量的方向以当前步长生成新节点x_new；

S4，当生成新节点x_new后，判断x_new节点是否进入目标点邻域，若进入目标点邻域，则直接连线x_new与x_goal，生成初始路径，退出循环，进行步骤S5；反之，以该点x_new为圆心，R为半径，计算圆内所有节点到x_new节点的距离，并选择距离x_new节点最近的节点作为新生成节点的父节点，并将x_new节点加入到树T中，再次返回步骤S2，直至找到初始路径或者达到最大迭代次数退出循环；

S5，对初始路径点使用三角剪枝策略，采用三角剪枝对初始路径处理，记输出的初始路径点为[T₁，T₂，T₃，...,T_N]，从目标点开始依次向上回溯节点，若两节点之间[T_i,T_m]满足碰撞检测，则将该两点之间的其余节点全部剔除；若发生碰撞，则以不满足碰撞检测的节点T_i作为起点，再依次向上回溯，重复上述操作，直至遍历完路径序列；

S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径。

进一步，步骤S3之后，以及步骤S4之前还包括：

S31，判断向量

与向量

间的夹角θ是否在

范围内，若满足则进行步骤S32，不满足则返回步骤S2；

S32，连线

判断连线是否与空间障碍物发生碰撞；若产生碰撞，则缩短步长eps，并返回步骤S2，当再次进入到步骤S3时，反之采用原步长则更改为采用缩短后的步长；若不发生碰撞，进行步骤S4。

进一步，步骤S3，还包括：

设定向量P₁为

其控制参数为动态步长λ，向量P₂为

权重系数为τ；在向量P₁与P₂的共同作用下，沿着P₁与P₂的合向量P₃的方向生成新节点x_new；

其中，当生成的新节点与障碍物发生碰撞，则缩短步长；当没有发生碰撞时，恢复为原始步长。

进一步，S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径，包括：

设置B样条的控制点表示为M_j，j＝0,1,2,3...，B样条的基函数表示为N_j,k(u)，则B样条的数学描述函数记为：

其中N_j,0(u)的取值满足以下关系：

三次B样条基函数为：

代入B样条的数学描述函数：

M_0,3(u)＝M₀×N_0,3(u)+M₁×N_1,3(u)+M₂×N_2,3(u)+M₃×N_3,3(u)u∈[0,1]

给定控制点M_j，利用上式求出满足要求的曲线点；B样条的起始点为线段M₁M₁″的三分之一处，终点位于M₂M₂″线段的三分之一处；

其中，起始点处的切线与M₁″M₂平行，终点处的切线平行于M₂″M₃；M₁M₁″与M₂M₂″分别为三角形M₀M₁M₂和三角形M₁M₂M₃的中线；

将分段曲线结合起来，形成一条完整且光滑的B样条曲线，作为最终路径。

此外，本发明的第二方面提供了一种电子装置，所述电子装置包括：一个或多个处理器，存储器，所述存储器用于存储一个或多个计算机程序；其特征在于，所述计算机程序被配置成由所述一个或多个处理器执行，所述程序包括用于执行如上所述的基于改进RRT算法的机械臂路径规划方法步骤。

此外，本本发明的第三方面提供了一种存储介质，所述存储介质存储有计算机程序；所述程序由处理器加载并执行以实现如权利要求1-4任一项所述的基于改进RRT算法的机械臂路径规划方法步骤。

本发明的方案中，通过首先以合向量的形式生成节点，有利于快速找到路径；其次使用动态步长一定程度上使得算法避免了在复杂障碍物环境下难以找到路径的问题，防随机树逆生长避免了后期节点过多而导致的大量计算，目标点区域防振荡有效解决了到达目标点范围内后，避免了因步长原因导致无法找到路径的问题；最后重选父节点过程一定程度上减少了路径代价。进一步，对初始路径二次优化；首先，三角剪枝策略在初始路径的基础上有效减少了路径代价，其次，并使用三次B样条曲线对剪枝后的路径光滑处理，降低了机械臂的抖动，减少机械臂运动过程中而产生的磨损。通过改进RRT算法降低了路径距离且优化了路径质量，避免盲目采样的同时提高了规划效率。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，应当理解，以下附图仅示出了本发明的某些实施例，因此不应被看作是对范围的限定，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他相关的附图。

图1本发明实施例公开的基于改进RRT算法的机械臂路径规划方法的流程示意图；

图2是本发明实施例公开的改进节点扩展示意图；

图3是本发明实施例公开的防随机树逆生长的向量示意图；

图4是本发明实施例公开的原始步长扩展方案与本实施例改进的RRT算法的步长扩展方案示意图；

图5是本发明实施例公开的三角剪枝路径处理示意图；

图6是本发明实施例公开的三次B样条曲线示意图；

图7是本发明实施例公开的改进的RRT算法与其他几种现有技术中RRT算法基于环境1执行的仿真结果图；

图8是本发明实施例公开的改进的RRT算法与其他几种现有技术中RRT算法基于环境2执行的仿真结果图。

具体实施方式

现在将参考附图更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本申请将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。

此外，所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施例中。在下面的描述中，提供许多具体细节从而给出对本申请的实施例的充分理解。然而，本领域技术人员将意识到，可以实践本申请的技术方案而没有特定细节中的一个或更多，或者可以采用其它的方法、组元、装置、步骤等。在其它情况下，不详细示出或描述公知方法、装置、实现或者操作以避免模糊本申请的各方面。

附图中所示的方框图仅仅是功能实体，不一定必须与物理上独立的实体相对应。即，可以采用软件形式来实现这些功能实体，或在一个或多个硬件模块或集成电路中实现这些功能实体，或在不同网络和/或处理器装置和/或微控制器装置中实现这些功能实体。

附图中所示的流程图仅是示例性说明，不是必须包括所有的内容和操作/步骤，也不是必须按所描述的顺序执行。例如，有的操作/步骤还可以分解，而有的操作/步骤可以合并或部分合并，因此实际执行的顺序有可能根据实际情况改变。

需要说明的是：在本实施例中提及的“多个”是指两个或两个以上。

本实施例，路径规划：在具有障碍物的环境中，给定机器人的起始点与目标点，机器人能自行规划出一条从起始点到目标点且不与空间障碍物发生碰撞的路径。

RRT算法：RRT算法为路径规划算法中的一种，全称为快速随机树算法。该算法在空间中随机撒点，再将可行点进行连接获取机器人的可行路径。

以下对本申请实施例的技术方案的实现细节进行详细阐述：

请参阅图1，图1是本发明实施例公开的一种基于改进RRT算法的机械臂路径规划方法的流程示意图。如图1所示，本发明实施例的一种基于改进RRT算法的机械臂路径规划方法，所述方法包括：

S1，初始化树T，给定起始点x_init、目标点x_goal、扩展步长eps，最大迭代次数max、空间障碍物信息obs、目标点阈值m；

具体地，本实施例，对变量进行初始化，包含起始点x_init，目标点x_goal，扩展步长eps，最大迭代次数max，空间障碍物信息obs、目标点阈值m。

S2，采用概率偏置生成随机点x_rand；当rand＜bias时，采样点x_rand向目标点x_goal采样；否则，采样点x_rand随机采样；

具体地，本实施例，在工作空间生成采样点，给定偏置变量bias(0＜bias＜1)，使用随机函数生成变量值记为rand(0≤rand≤1)，当rand＜bias时，采样点x_rand向目标点x_goal采样，其它情况下，采样点x_rand随机采样。

S3，在树T中寻找距离随机点最近的近邻点x_near，判断x_near是否进入目标点阈值范围内，若进入则直接缩短步长为eps1，其中，eps1<m，反之采用原步长；沿着

与

合向量的方向以当前步长生成新节点x_new；

需要说明的是，如图2所示为本实施例改进节点扩展示意图。新节点的生成x_new，将原始RRT算法的固定步长改为动态步长的扩展方式，并将动态步长的参数引入到合向量的表达式中，表达式如式(1)所示：

设定向量P₁为

其控制参数为动态步长λ，向量P₂为

权重系数为τ。在向量P₁与P₂的共同作用下，沿着合向量P₃的方向生成新节点x_new。其中，当生成的新节点与障碍物发生碰撞，则缩短步长；当没有发生碰撞时，恢复为原始步长。

进一步，步骤S3之后，以及步骤S4之前还包括：

S31，判断向量

与向量

间的夹角θ是否在

范围内，若满足则进行步骤S32，不满足则返回步骤S2；

S32，连线

判断连线是否与空间障碍物发生碰撞；若产生碰撞，则缩短步长eps，并返回步骤S2，当再次进入到步骤S3时，反之采用原步长则更改为采用缩短后的步长；若不发生碰撞，进行步骤S4。

具体地，本实施例，为避免采样过程中采样域偏离，采用如下策略避免随机树逆生长。如图3所示为本实施例防随机树逆生长的向量示意图。

向量

与向量

间的夹角记为θ：

当θ角在

范围内，将生成的x_new节点加入树中，如图3中的

其它情况下，则认为采样过程中出现采样域偏离，随机树逆向生长，这种情况下生成的节点则视为无效节点，如图3中的

节点。

进一步，执行目标点区域防振荡，原始RRT算法的近邻点x_near进入目标点x_goal的邻域范围内时，采用原始步长扩展得到x_new，这种扩展方式可能会导致节点x_new在目标点附近振荡。如图4所示为本实施例中，原始步长扩展方案与本实施例改进的RRT算法的步长扩展方案示意图。

给定邻域的半径记为r，若x_near位于邻域范围内，则缩短步长为λ₁(λ₁＜r)；若x_near在邻域范围之外，则采用原步长λ生成节点x_new。当生成新节点x_new后，以该点为圆心，R为半径，计算圆内所有节点到x_new节点的距离，并选择距离x_new节点最近的节点作为新生成节点的父节点。

S4，当生成新节点x_new后，判断x_new节点是否进入目标点邻域，若进入目标点邻域，则直接连线x_new与x_goal，生成初始路径，退出循环，进行步骤S5；反之，以该点x_new为圆心，R为半径，计算圆内所有节点到x_new节点的距离，并选择距离x_new节点最近的节点作为新生成节点的父节点，并将x_new节点加入到树T中，再次返回步骤S2，直至找到初始路径或者达到最大迭代次数退出循环。

S5，对初始路径点使用三角剪枝策略，采用三角剪枝对初始路径处理，记输出的初始路径点为[T₁，T₂，T₃，...,T_N]，从目标点开始依次向上回溯节点，若两节点之间[T_i,T_m]满足碰撞检测，则将该两点之间的其余节点全部剔除；若发生碰撞，则以不满足碰撞检测的节点T_i作为起点，再依次向上回溯，重复上述操作，直至遍历完路径序列；

具体地，本实施例，经过上述改进后，RRT算法规划出的初始路径仍存在部分冗余节点与转折点，为获得光滑路径，采用三角剪枝对初始路径处理，如图5所示为本实施例三角剪枝路径处理示意图。记输出的初始路径点为[T₁，T₂，T₃，...,T_N]，从目标点开始依次向上回溯节点，若两节点之间[T_i,T_m]满足碰撞检测，则将该两点之间的其余节点全部剔除；若发生碰撞，则以不满足碰撞检测的节点T_i作为起点，再依次向上回溯，重复上述操作，直至遍历完路径序列。经过初次优化后的路径拐点变少，路径代价变小。

S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径。

进一步，本实施例采用三次B样条平滑路径进行优化。由于经过三角剪枝后的路径在节点的连接处存在较大的转折，不满足机器人运动学规律。为获得平滑的轨迹，本实施例使用三次B样条曲线对剪枝后的路径平滑处理。三次B样条曲线的曲率具有一定的连续性，且修改曲线的局部线段时，对曲线的其余部分不产生影响，因此能够很好地控制曲线的平滑度。

具体地，S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径，包括：

设置B样条的控制点表示为M_j，j＝0,1,2,3...，B样条的基函数表示为N_j,k(u)，则B样条的数学描述函数记为：

其中N_j,0(u)的取值满足以下关系：

三次B样条基函数为：

代入B样条的数学描述函数：

M_0,3(u)＝M₀×N_0,3(u)+M₁×N_1,3(u)+M₂×N_2,3(u)+M₃×N_3,3(u)u∈[0,1] (7)

给定控制点M_j，利用上式可求出满足要求的曲线点。如图6所示为本实施例三次B样条曲线示意图。B样条的起始点为线段M₁M₁″的三分之一处，终点位于M₂M₂″线段的三分之一处；起始点处的切线与M₁″M₂平行，终点处的切线平行于M₂″M₃；M₁M₁″与M₂M₂″分别为三角形M₀M₁M₂和三角形M₁M₂M₃的中线。最后，将分段曲线结合起来，形成一条完整且光滑的B样条曲线，作为最终路径。

进一步，本实施例分别选择两种不同复杂程度的障碍物环境，环境1由不同球体组成，环境2由不同长方体组成。仿真环境：联想小新pro14，Matlab2018a。参数设置：最大迭代次数Max＝2000，初始扩展步长eps＝3，采样时的概率偏置参数bias＝0.3，权重系数τ＝0.1，目标点阈值为30，圆形搜索半径为20。环境1中起始点位置为(0，0，0)，目标点位置为(100，100，100)，环境2中起始点位置为(0，100，0)，目标点位置为(100，0，20)。

如图7所示为本实施例改进的RRT算法与其他几种现有技术中RRT算法基于环境1执行的仿真结果图。原始RRT算法与目标偏置RRT算法只是寻找到路径即可，并没有考虑路径代价与路径质量。因此两种算法规划出的路径代价较高，且路径存在较多冗余路径点，导致路径比较曲折。RRT*算法在寻找路径时不断地重选父节点与重布线，使得生成路径长度与路径质量均得到了提升，但耗费了大量的时间。本发明改进后的RRT算法不仅优化了路径也降低了时间，使路径与时间之间得到了良好平衡。为避免一次实验的偶然性，统计了四种算法60次实验的平均值，具体数据如上表1所示。

表1 环境1仿真实验数据

如图8所示为本实施例改进的RRT算法与其他几种现有技术中RRT算法基于环境2执行的仿真结果图。如图8所示，三维空间的复杂度远高于二维空间，因此空间计算的复杂度也随之提升。RRT*算法增加了重选父节点与重布线过程，这两个过程依赖于节点之间的大量计算，因此导致了RRT*算法用时最久。原始RRT算法缺乏目标导向性，采样时在空间随机撒点，因此规划出路径所消耗的时间也久。目标偏置RRT算法与本实施例改进RRT算法在采样时均向目标点靠近，且本实施例改进算法在新节点生成时也会受到目标点的吸引，避免了采样时的盲目性，大大降低了寻找路径所耗费的时间，且改进RRT算法生成的路径较其它三种算法生成的路径更为光滑，表2为环境2中四种算法60次仿真实验的平均结果。

表2 环境2仿真实验数据

此外，本实施例的第二方面提供了一种电子装置，所述电子装置包括：一个或多个处理器，存储器，所述存储器用于存储一个或多个计算机程序；其特征在于，所述计算机程序被配置成由所述一个或多个处理器执行，所述程序包括用于执行如上所述的基于改进RRT算法的机械臂路径规划方法步骤。

此外，本实施例的第三方面提供了一种存储介质，所述存储介质存储有计算机程序；所述程序由处理器加载并执行以实现如权利要求1-4任一项所述的基于改进RRT算法的机械臂路径规划方法步骤。

本实施例提出的一种基于改进RRT算法的机械臂路径规划方法。第一部分，生成初始路径。首先以合向量的形式生成节点，有利于快速找到路径；其次使用动态步长一定程度上使得算法避免了在复杂障碍物环境下难以找到路径的问题，防随机树逆生长避免了后期节点过多而导致的大量计算，目标点区域防振荡有效解决了到达目标点范围内后，避免了因步长原因导致无法找到路径的问题；最后重选父节点过程一定程度上减少了路径代价。第二部分，对初始路径二次优化。首先，三角剪枝策略在初始路径的基础上有效减少了路径代价，其次，并使用三次B样条曲线对剪枝后的路径光滑处理，降低了机械臂的抖动，减少机械臂运动过程中而产生的磨损。

本领域普通技术人员可以意识到，结合本实施例中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本申请所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口、装置或单元的间接耦合或通信连接，也可以是电的，机械的或其它的形式连接。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显本领域普通技术人员可以意识到，结合本实施例中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网格设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种基于改进RRT算法的机械臂路径规划方法，其特征在于，所述方法包括：

S1，初始化树T，给定起始点x_init、目标点x_goal、扩展步长eps，最大迭代次数max、空间障碍物信息obs、目标点阈值m；

S2，采用概率偏置生成随机点x_rand；当rand＜bias时，采样点x_rand向目标点x_goal采样；否则，采样点x_rand随机采样；

S3，在树T中寻找距离随机点最近的近邻点x_near，判断x_near是否进入目标点阈值范围内，若进入则直接缩短步长为eps1，其中，eps1<m，反之采用原步长；沿着

与

合向量的方向以当前步长生成新节点x_new；

S4，当生成新节点x_new后，判断x_new节点是否进入目标点邻域，若进入目标点邻域，则直接连线x_new与x_goal，生成初始路径，退出循环，进行步骤S5；反之，以该点x_new为圆心，R为半径，计算圆内所有节点到x_new节点的距离，并选择距离x_new节点最近的节点作为新生成节点的父节点，并将x_new节点加入到树T中，再次返回步骤S2，直至找到初始路径或者达到最大迭代次数退出循环；

S5，对初始路径点使用三角剪枝策略，采用三角剪枝对初始路径处理，记输出的初始路径点为[T₁，T₂，T₃，...,T_N]，从目标点开始依次向上回溯节点，若两节点之间[T_i,T_m]满足碰撞检测，则将该两点之间的其余节点全部剔除；若发生碰撞，则以不满足碰撞检测的节点T_i作为起点，再依次向上回溯，重复上述操作，直至遍历完路径序列；

S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径。

2.根据权利要求1所述的基于改进RRT算法的机械臂路径规划方法，其特征在于，步骤S3之后，以及步骤S4之前还包括：

S31，判断向量

与向量

间的夹角θ是否在

范围内，若满足则进行步骤S32，不满足则返回步骤S2；

S32，连线

判断连线是否与空间障碍物发生碰撞；若产生碰撞，则缩短步长eps，并返回步骤S2，当再次进入到步骤S3时，反之采用原步长则更改为采用缩短后的步长；若不发生碰撞，进行步骤S4。

3.根据权利要求2所述的基于改进RRT算法的机械臂路径规划方法，其特征在于，步骤S3，还包括：

设定向量P₁为

其控制参数为动态步长λ，向量P₂为

权重系数为τ；在向量P₁与P₂的共同作用下，沿着P₁与P₂的合向量P₃的方向生成新节点x_new；

其中，当生成的新节点与障碍物发生碰撞，则缩短步长；当没有发生碰撞时，恢复为原始步长。

4.根据权利要求3所述的基于改进RRT算法的机械臂路径规划方法，其特征在于，S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径，包括：

设置B样条的控制点表示为M_j，j＝0,1,2,3...，B样条的基函数表示为N_j,k(u)，则B样条的数学描述函数记为：

其中N_j,0(u)的取值满足以下关系：

三次B样条基函数为：

代入B样条的数学描述函数：

M_0,3(u)＝M₀×N_0,3(u)+M₁×N_1,3(u)+M₂×N_2,3(u)+M₃×N_3,3(u)u∈[0,1]

给定控制点M_j，利用上式求出满足要求的曲线点；B样条的起始点为线段M₁M₁″的三分之一处，终点位于M₂M₂″线段的三分之一处；

其中，起始点处的切线与M₁″M₂平行，终点处的切线平行于M₂″M₃；M₁M₁″与M₂M₂″分别为三角形M₀M₁M₂和三角形M₁M₂M₃的中线；

将分段曲线结合起来，形成一条完整且光滑的B样条曲线，作为最终路径。

S6，基于三次B样条平滑路径对剪枝后的路径平滑处理，获得最终路径。

5.一种电子装置，所述电子装置包括：一个或多个处理器，存储器，所述存储器用于存储一个或多个计算机程序；其特征在于，所述计算机程序被配置成由所述一个或多个处理器执行，所述程序包括用于执行如权利要求1-4任一项所述的基于改进RRT算法的机械臂路径规划方法步骤。

6.一种存储介质，所述存储介质存储有计算机程序；所述程序由处理器加载并执行以实现如权利要求1-4任一项所述的基于改进RRT算法的机械臂路径规划方法步骤。

Images