CN113858210A - 基于混合算法的机械臂路径规划方法 - Google Patents

Abstract

本发明公开了一种基于混合算法的机械臂路径规划方法，其特征在于：首先初始化树节点并不断检测当前节点和障碍物之间的距离，若检测到当前节点与障碍物之间的最小距离大于两倍的步长时，则说明当前节点附近没有障碍物，采用改进的人工势场方法进行快速拓展，若检测到当前节点与障碍物之间的最小距离小于两倍的步长的时，则采用改进的RRT算法，充分利用RRT算法高效的避障能力，重复上述步骤直至到达目标点。该混合算法可以有效地提高路径搜索效率，解决人工势场法存在的当起始点存在障碍物时斥力很大造成路径曲折和当目标点有障碍物的时候目标不可达的现象，同时该混合算法也解决了改进的RRT算法存在的当障碍物较多时效率大幅较低的现象。

Description

基于混合算法的机械臂路径规划方法

技术领域

本发明涉及机器人运动规划领域，具体涉及一种基于混合算法的机械臂路径规划方法。

背景技术

在机械臂的运动规划领域中，人工势场法路径规划最早是由Khatib在1985年提出，其思想是利用虚拟力的思想使机器人在障碍物中穿行，该算法存在的缺点：算法易陷入局部极小值或震荡，当目标点附近有障碍物时，很难达到目标点。RRT算法最早由美国教授Steven M.LaValle于1998年提出来的，是一种概率完备且不最优、基于采样且高效的多维空间的路径规划算法。但RRT的随机性就导致了算法的盲目、效率低且最终生成的路径曲折且不够光滑，在狭窄区域里搜索速度缓慢等缺点。目前，大量的学者针对这两种算法做了不同的改进，Zheng,Y等人提出了一种新的最小准则，设计了一种改进的虚拟障碍物局部路径规划方法克服了人工势场算法易陷入局部极小点等缺点。Sun,J等人提出了使用动态窗口的方法改进人工势场方法解决局部最小点的问题。李耀仲等人提出一种基于稀疏节点RRT的移动机械臂运动规划方法，解决算法在局部空间中过度搜索的问题，减少扩展的无效节点。葛佳昊等人提出了一种基于动力学RRT*算法的FFSR轨迹规划方法能够以较快的速度生成可行的机器人移动轨迹。但还存在以下的问题：(1)改进的人工势场算法不能很好地解决搜索过程中存在的局部极小点的问题，针对局部极小点不能及时的做出调整，当目标点和起始点都存在障碍物的时候，不能有效的避开障碍物快速到达目标点，且生成的路径较为曲折；(2)针对改进的快速拓展随机树算法，不能快速的在复杂的多障碍物环境下快速的寻找到一条可靠的路径，对不同的环境条件，算法不能很好的做出调整，不利于算法效率的提升，且对生成的路径未做出很好的平滑处理，导致机械臂在实际运行中会受到冲击损坏，严重缩短机械臂的使用寿命。

发明内容

本发明的目的在于克服上述缺点而提出了一种基于混合算法的机械臂路径规划方法，生成的路径更短也更加平滑。

本发明的一种基于混合算法的机械臂路径规划方法，其中：该方法包括以下步骤:

步骤一:参数的初始化设置，定义障碍物环境、起始点、目标点、步长、目标采样率；

步骤二:判断当前节点与障碍物的距离，若当前节点和最近障碍物之间的距离大于两倍的步长时，则执行步骤三；若当前节点与障碍物之间的距离小于两倍的步长时，则执行步骤四；

步骤三:采用改进的人工势场法进行搜索，在目标物和障碍物的合力作用下前进：(1)计算引力和斥力；(2)判断当前节点所受的引力和斥力是否相等，如果相等则产生虚拟子目标，对机械臂末端执行器产生虚拟子引力打破此临界状态，引导机械臂末端执行器继续移动；如果不相等，则执行步骤(3)；(3)当前节点与目标点的距离小于步长，或新节点和最近障碍物的距离小于两倍的步长，则结束人工势场法搜索过程，将人工势场法得到的路径节点加入到规划路径中并返回规划路径和最新节点；否则跳转至步骤(1)；

步骤四：若当前节点与障碍物之间的距离小于两倍的步长时，则采用改进的RRT算法进行路径搜索：(1)初始化树，设置初始节点和目标点，定义步长、目标采样率和分割率；(2)迭代开始，在状态空间中进行采样，当随机概率大于目标采样率的时候则采样点选取目标点，若小于目标采样率则在空间中随机选取采样点；(3)根据采样节点选取树中与之最近的节点，并计算节点与最近障碍物之间的距离dist1；(4)判断距离dist1是否大于步长，如果距离dist1大于步长，则采用步长F₁(n)进行拓展；如果距离dist1小于步长先按照步长F₁(n)生成虚拟新节点，计算虚拟新节点与障碍物之间的距离dist2；(5)若dist2<dist1则证明新节点有偏向障碍物的趋势，去除虚拟新节点并采用步长F₂(n)进行拓展；若dist2>dist1则证明新节点有远离障碍物的趋势，则去除虚拟新节点并利用步长F₃(n)进行拓展；(6)判断新节点是否与障碍物碰撞，如果碰撞了，跳转至步骤(2)重新进行随机点采样；如果没碰撞，将新节点加入到树中，并将新节点的父节点赋予最近节点；(7)当新节点与目标点的距离小于步长，或新节点和最近障碍物的距离大于两倍的步长，则结束迭代过程；否则跳转至步骤(2)；(8)对生成的路径利用路径的碰撞检测方法进行去除冗余节点的处理得到处理后的路径；(9)将处理后的路径节点加入到规划路径中并返回规划路径和最新节点；

其中，所述步长F₁(n)定义为：

其中，ρ表示随机生长的步长，K_p表示引力系数，||x_goal-x_near||表示目标点的位置向量x_goal和最近节点的位置向量x_neaar的欧式距离的绝对值；

所述步长F₂(n)定义为：

其中dist₁表示最近节点与障碍物s之间的距离，dist₂表示虚拟新节点与障碍物s之间的距离，||x_rand-x_near||表示随机点的位置向量x_rand和最近节点的位置向量x_neaar的欧式距离的绝对值；

所述步长F₃(n)定义为：

步骤五:判断最新节点与目标点之间的距离是否小于步长，如果小于步长则到达目标点，连接新节点和目标点，输出规划路径图，得到一条从起始点到终止点的无碰撞路径，退出程序；否则，跳转至步骤二。

上述的基于混合算法的机械臂路径规划方法，其中：所述步骤四中计算节点与最近障碍物之间的距离，采用欧氏距离计算。

上述的基于混合算法的机械臂路径规划方法，其中：所述步骤四的(8)中对生成的路径利用路径的碰撞检测方法进行去除冗余节点的处理：采用去除冗余节点和平滑处理的操作，对随机树生成的由起始点到目标点的路径进行处理，从第一个节点x_init开始依次连接后续路径节点，将第二个节点忽略，连接第三个节点如果未与障碍物碰撞则删除路径中的第二个节点，若碰撞则保留，并以该碰撞点的父节点作为新的判断节点，直到到达目标点为止，将最终得到的一系列保留的节点保存，并连接得到去除冗余节点后的最终规划路径。

上述的基于混合算法的机械臂路径规划方法，其中：所述步骤五得到一条从起始点到终止点的无碰撞路径，采用贝塞尔曲线对路径进行平滑处理。

本发明与现有技术的相比，具有明显的有益效果，由以上方案可知，首先判断障碍物和新节点之间的距离，若检测到当前节点与障碍物之间的最小距离大于两倍的步长时，则说明当前节点附近没有障碍物，采用改进的人工势场方法进行快速拓展；若检测到当前节点与障碍物之间的最小距离小于两倍的步长时，则采用改进的RRT算法，充分利用RRT算法高效的避障能力进行路径规划，同时解决了当目标点存在障碍物时，人工势场法不能到达目标点的缺点，充分利用了RRT高效的避障能力，两种算法交替进行路径探索直至到达目标点。所述改进的人工势场方法引入了动态子目标的思想，解决易陷入局部极小点问题。所述改进的RRT算法包括引入目标偏向的思想，随机点的生成以一定的概率选择目标点，加快收敛速率；设计了自适应步长的策略，提出的虚拟新节点生成方法，有效地提高了算法的避障能力和效率；对生成的路径节点采用去除冗余节点操作，使得路径更短更光滑。所述对最终生成的路径利用贝赛尔曲线进行拟合，使得路径更加光滑，减少机械臂运行过程中的冲击损伤。

以下通过具体实施方式，进一步说明本发明的有益效果。

附图说明

图1为本发明的流程示意图；

图2为本发明的人工势场法原理图；

图3为本发明的RRT思维图；

图4为本发明与不同算法在障碍物和步长等相同的条件下生成路径对比图。

具体实施方式

以下结合附图及较佳实施例，对依据本发明提出的一种基于混合算法的机械臂路径规划方法的具体实施方式、特征及其功效，详细说明如后。

参见图1，本发明的一种基于混合算法的机械臂路径规划方法，其中：该方法包括以下步骤:

步骤一:参数的初始化设置，定义障碍物环境、起始点、目标点、步长、目标采样率；

步骤二:判断当前节点与障碍物的距离，若当前节点和最近障碍物之间的距离大于两倍的步长时，则执行步骤三；若当前节点与障碍物之间的距离小于两倍的步长时，则执行步骤四；

步骤三:采用改进的人工势场法进行搜索，在目标物和障碍物的合力作用下前进：(1)计算引力和斥力；(2)判断当前节点所受的引力和斥力是否相等，如果相等则产生虚拟子目标，对机械臂末端执行器产生虚拟子引力打破此临界状态，引导机械臂末端执行器继续移动；如果不相等，则执行步骤(3)；(3)当前节点与目标点的距离小于步长，或新节点和最近障碍物的距离小于两倍的步长，则结束人工势场法搜索过程，将人工势场法得到的路径节点加入到规划路径中并返回规划路径和最新节点；否则跳转至步骤(1)；

步骤四：若当前节点与障碍物之间的距离小于两倍的步长时，则采用改进的RRT算法进行路径搜索：(1)初始化树，设置初始节点和目标点，定义步长、目标采样率和分割率；(2)迭代开始，在状态空间中进行采样，当随机概率大于目标采样率的时候则采样点选取目标点，若小于目标采样率则在空间中随机选取采样点；(3)根据采样节点选取树中与之最近的节点，并计算节点与最近障碍物之间的距离dist1；(4)判断距离dist1是否大于步长，如果距离dist1大于步长，则采用步长F₁(n)进行拓展；如果距离dist1小于步长先按照步长F₁(n)生成虚拟新节点，计算虚拟新节点与障碍物之间的距离dist2；(5)若dist2<dist1则证明新节点有偏向障碍物的趋势，去除虚拟新节点并采用步长F₂(n)进行拓展；若dist2>dist1则证明新节点有远离障碍物的趋势，则去除虚拟新节点并利用步长F₃(n)进行拓展；(6)判断新节点是否与障碍物碰撞，如果碰撞了，跳转至步骤(2)重新进行随机点采样；如果没碰撞，将新节点加入到树中，并将新节点的父节点赋予最近节点；(7)当新节点与目标点的距离小于步长，或新节点和最近障碍物的距离大于两倍的步长，则结束迭代过程；否则跳转至步骤(2)；(8)对生成的路径利用路径的碰撞检测方法进行去除冗余节点的处理得到处理后的路径；(9)将处理后的路径节点加入到规划路径中并返回规划路径和最新节点；

其中，所述步长F₁(n)定义为：

其中，ρ表示随机生长的步长，K_p表示引力系数，||x_goal-x_near||表示目标点的位置向量x_goal和最近节点的位置向量x_neaar的欧式距离的绝对值；

所述步长F₂(n)定义为：

其中dist₁表示最近节点与障碍物s之间的距离，dist₂表示虚拟新节点与障碍物s之间的距离，||x_rand-x_near||表示随机点的位置向量x_rand和最近节点的位置向量x_neaar的欧式距离的绝对值；

所述步长F₃(n)定义为：

步骤五:判断最新节点与目标点之间的距离是否小于步长，如果小于步长则到达目标点，连接新节点和目标点，输出规划路径图，得到一条从起始点到终止点的无碰撞路径，退出程序；否则，跳转至步骤二。

其中：

1经典人工势场法原理

经典的人工势场算法是物体的初始点在一个较高的“山头”上，要到达的目标点在“山脚”下，这就形成了一种势场。物体在这种势场的引导下，能避开障碍物到达目标点。

人工势场法包括引力场和斥力场，引力场函数U_att(q)如下：

其中，ε是尺度因子，ρ(q,q_goal)表示目标物体q_goal和当前位置q之间的欧式距离，相应的目标物体的引力F_att(q)就是引力场的导数：

斥力场U_rep(q)函数如下：

其中η是斥力尺度因子，ρ(q,q_obs)表示障碍物q_obs与当前位置q之间的欧式距离，ρ₀表示每个障碍物的影响半径。

则斥力F_rep(q)为斥力场的导数：

其中

表示对ρ(q,q_obs)求导数,目标物体产生引力，引导物体朝向目标物体运动，障碍物产生斥力，达到绕开障碍物的目的，如图2所示，物体在场中的任意一点受到的合力场U(q)(合力F(q))等于目标物体对他的引力场(引力)和障碍物对它的总的斥力场(斥力)的合力。其公式如下：

2经典快速拓展随机树算法原理

经典的RRT算法的原理是以一个初始点作为根节点q_init，在之后的每一次拓展中在地图中随机生成一个随机节点x_rand，Nearest函数在已经存在的随机树上根据欧氏距离选择一个离x_rand最近的一个节点x_nearest，通过Extend函数以一个步长的距离向x_rand的方向拓展，得到一个新的节点x_new,并判断x_new是否与障碍物碰撞，若碰撞则放弃生长并去除x_new，重新生成随机节点，若没有碰撞则将x_new加入到随机树中，并将x_new的父节点赋予x_nearest，如图3所示。当随机树的节点与目标点的距离小于某一个具体的值m的时候，则终止程序，便可以得到一条从起始点到目标点的无碰撞路径，按照上述步骤直至到达目标点。

3算法改进

3.1改进的人工势场算法

由经典人工势场法原理可知该算法存在一些缺陷，当起点存在障碍物的时候，斥力会很大造成路径曲折，当目标点有障碍物的时候，则会导致物体很难到达目标点，当物体所受的引力和斥力相等的时候，物体会陷入局部极小点的现象。

针对算法易陷入局部极小点问题，提出一种动态子目标的思想，即判断当前节点是否陷入了局部极小值，若是则随机生成一个虚拟的子目标对物体产生虚拟子引力来打破这个临界条件，引导物体摆脱此陷阱，完成路径规划。

3.2改进的RRT算法(I-RRT)

由经典RRT算法原理可知该算法具有随机性和较强的避障能力，但这势必会增加搜索的时间，使得效率降低，且搜索的路径长且曲折，占用内存相对较大，当空间中存在大量的障碍物的时候，算法的效率会大幅降低，因此在经典RRT算法的基础上进行了改进，并改进的RRT算法定义为I-RRT，具体的改进内容如下。

3.2.1路径的碰撞检测

经典RRT算法针对节点做了碰撞检测，但是没有涉及对路径的碰撞检测，如果路径与障碍物发生碰撞则生成的路径也是不符合实际要求的，在Collision_check_line()函数中对路径做出了碰撞检测，具体方法是传入两个随机树中的节点，计算出两节点之间的三维空间的欧氏距离，并设置分割率(Discretepoint),分割的点数等于两点之间的距离除以分割率，将路径分割成许多点，并判断这些点是否与障碍物碰撞，最终达到判断路径是否与障碍物发生碰撞的目的。

3.2.2目标偏向策略

针对算法采用选取随机点的方式进行随机树的拓展，导致路径的搜索效率低，因此采用目标偏向的思想，随机点的生成以一定的概率P_{goalSampleRate}选择目标点，这样可以有效地降低算法的盲目性，当随机概率大于P_{goalSampleRate}的时候生成随机点，当小于的时候则随机点取为目标点，加快了收敛速率。

3.2.3自适应步长

经典算法在拓展中步长固定，当靠近障碍物的时候不能很好地避开，导致碰撞，占用了大量的计算空间，而且产生的步长是朝向随机点的方向生长没有方向性，均导致了算法的效率的降低，因此采用自适应步长的策略，当产生的随机节点x_rand对应的x_nearest与障碍物的最小距离大于步长时，将此障碍物标记为s,将人工势场法中的引力的思想引入到RRT算法的自适应步长中，引导随机树朝着目标方向生长，在原始RRT算法朝向随机点生长的基础上加上朝向目标的步长分量G(n)，使新节点有向目标点朝向的趋势，步长F(n)公式定义如下：

F(n)＝R(n)+G(n) (7)

其中F(n)表示第n个节点生长的牵引力步长，R(n)表示第n个节点随机生长产生的牵引力步长，G(n)表示第n个节点目标物体对节点产生的牵引力步长。由人工势场法知目标点对最近节点的引力势能U，定义如下：

其中，K_p表示引力系数，||x_goal-x_near||表示目标点的位置向量x_goal和最近节点的位置向量x_neaar的欧式距离的绝对值，则引力G为引力势能U的导数，即

G＝k_p*||x_goal-x_near|| (9)

则

其中ρ表示随机生长的步长，由此也可推出R(n)为

其中||x_rand-x_near||表示随机点的位置向量x_rand和最近节点的位置向量x_neaar的欧式距离的绝对值，将(10)(11)两式带入(7)式中得到最近节点所受到的牵引力步长为式(12)，定义为F₁(n)，公式如下：

当与障碍物的最小距离小于步长的时候，分两种情况：

情况1：先按照上面的步骤生成虚拟新节点并判断虚拟新节点与障碍物s的距离是否小于最近节点与障碍物之间的距离，如果小于最近节点与障碍物的距离，则证明虚拟新节点有向障碍物靠近的趋势，则去除虚拟新节点，改变新节点生长的步长，在步长F₁(n)的基础上缩小步长，定义为步长F₂(n),公式如下：

其中dist₁表示最近节点与障碍物s之间的距离，dist₂表示虚拟新节点与障碍物s之间的距离。并判断是否与障碍物碰撞，若碰撞则重新生成随机节点，若没有碰撞将新节点加入到随机树中。

情况2：虚拟新节点与障碍物s的距离如果大于最近节点与障碍物s之间的距离，则证明新节点有远离障碍物生长的趋势，则新节点的生成步长采取经典RRT算法的步长，定义为步长F₃(n)，公式如下：

并判断是否与障碍物碰撞，若碰撞则重新生成随机节点，若没有碰撞将新节点加入到随机树中。

3.2.4去除冗余节点操作

由于经典RRT算法的随机性导致路径可能出现震荡现象，采用去除冗余节点和平滑处理的操作，对随机树生成的由起始点到目标点的路径进行处理，从第一个节点x_init开始依次连接后续路径节点，将第二个节点忽略，连接第三个节点如果未与障碍物碰撞则删除路径中的第二个节点，若碰撞则保留，并以该碰撞点的父节点作为新的判断节点，按照上述步骤进行判断直到到达目标点为止，将最终得到的一系列保留的节点保存到Path2中并连接得到去除冗余节点后的最终路径，这里的碰撞指的是前述中提到的路径的碰撞检测。

4贝塞尔曲线路径平滑处理

针对算法生成的路径存在转折点且不够平滑的现象，导致机械臂在实际运行当中，由于加速度的原因产生损耗，采用贝塞尔曲线对路径进行平滑处理，平滑处理是在原始路径的基础上实现的，n+1个节点得到n阶贝塞尔曲线的公式：

其中p_i表示n+1个空间上的点，参数为u的权重系数B_n,i(u)为伯恩斯坦基函数其计算方法如下：

最终生成的曲线与n+1个点中的每个点都有关系，并决定了曲线的最终走向，这n+1个点被称为控制点，公式15中的贝塞尔阶数是n阶，由n+1个控制点控制。当u＝0的时候位于起点，当u＝1的时候为终点。

实验验证分析

为了验证算法对于多障碍物环境也能保持优异的效果，对不同障碍物环境、算法的有效性、算法的可靠性分别进行了实验，本实验利用python软件在实验室台式惠普电脑4GB内存Intel(R)Core(TM)i5-6500 [email protected] 3.19GHz的Win10环境下进行实验。在地图上以空间点[0,0,0]为起点，以[8,10,10]为终点，随机步长为1.0，目标偏向步长是0.9，目标采样率设为0.5，寻找一条路径光滑，且无碰撞的高效路径。

实验1进行不同个数障碍物之间的算法比较，为了验证改进的人工势场和RRT混合算法在较多障碍物的情况下依然会保持较好的搜索效果，在障碍物逐渐递增的情况下与经典RRT算法、经典人工势场法、改进的RRT算法(I-RRT)进行路径的优劣比较，每组实验进行200次，取结果的平均值，如表1所示，其中搜索成功率为搜索在100秒以内为搜索成功，100秒以外为搜索未成功。

表1不同障碍物个数各类算法的比较

根据表1可知，对算法分别进行搜索时间、路径长度、搜索成功率的比较，经典RRT算法在障碍物较多的情况下搜索时间会大大增加，并且搜索的成功率也会降低，经典人工势场法在障碍物个数为20、24两种情况下搜索失败，是因为此时的障碍物在起始点和终止点都存在了障碍物，导致震荡的产生且无法到达目标点。改进的RRT算法在障碍物较多的情况下也表现出了优良的搜索效果但是相比较改进的人工势场和RRT混合算法其自身的搜索时间还是比较高，混合算法在多障碍物环境下表现出了令人满意的搜索结果。

实验2进行算法有效性的分析，为了验证改进的人工势场法和RRT混合算法的有效性，在障碍物、步长等条件相同的条件下与各类算法进行比较，每组实验进行200次，如表2所示。

根据表2数据可知，经典人工势场法虽然搜索时间短，但是当起点存在障碍物的时候，障碍物的斥力会很大导致路径偏移，路经长且曲折，如图4a所示，障碍物较多时会出现搜索成功率低等现象。经典RRT算法在障碍物较少的时候，搜索时间较短但路径比较曲折，障碍物如果较多该算法就会出现搜索速度慢、成功率低、路径曲折且不平滑现象如图4b所示。改进的RRT算法比标准RRT算法规划时间更短，效率更高，且生成的路径较为平滑，如图4c所示，但是当障碍物增加的时候其搜索的效率会降低。改进的人工势场和RRT混合算法在多障碍物情况下表现出了优异的效果，搜索效率更高且路径更平滑、搜索成功率为100％，且克服了人工势场法当起始点和目标点存在障碍物的时候路径曲折和目标不可达的现象，在一定程度上减小了RRT算法本身的盲目性、充分利用了RRT算法的高效的避障能力，如图4d所示。

相比较RRT算法，混合算法的搜索时间减少了81.5％～89％，路径的平均长度减少了11.9％～22.9％。因为有人工势场法的参与，该混合算法的路径中的节点数会有所提高，但这并不会影响路径的光滑度也不会导致路径更加曲折，通过与其他算法的对比验证了算法的有效性。

表2相同条件下各类算法的比较

实验3进行算法可靠性的分析，通过改变混合算法中的RRT的步长来证明，每个步长进行200次试验，其实验结果如表3所示。

表3所示，步长为1.0的时候搜索时间最小，相比较其他的步长下降了49.5％-67.4％，步长为1.0的左端搜索时间成逐渐下降的趋势，而右端搜索的时间因为步长过大的原因呈上升的趋势，步长越长路径中的节点的数量越少，但是路径的长度并不是随着步长的增加而减少的，步长为1.0的情况下呈现出路径最短且时间最小。综上所述在步长为1.0的时候，算法达到最优的效果。因此步长的选取对该混合算法还是很重要的，同时该实验也验证了算法的可靠性。

表3不同步长的本发明对比

以上所述，仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，任何未脱离本发明技术方案内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (4)

1.一种基于混合算法的机械臂路径规划方法，其特征在于：该方法包括以下步骤:

步骤一:参数的初始化设置，定义障碍物环境、起始点、目标点、步长、目标采样率；

步骤二:判断当前节点与障碍物的距离，若当前节点和最近障碍物之间的距离大于两倍的步长时，则执行步骤三; 若当前节点与障碍物之间的距离小于两倍的步长时，则执行步骤四 ；

步骤三:采用改进的人工势场法进行搜索，在目标物和障碍物的合力作用下前进：（1）计算引力和斥力；（2）判断当前节点所受的引力和斥力是否相等，如果相等则产生虚拟子目标，对机械臂末端执行器产生虚拟子引力打破此临界状态，引导机械臂末端执行器继续移动；如果不相等，则执行步骤（3）；（3）当前节点与目标点的距离小于步长，或新节点和最近障碍物的距离小于两倍的步长，则结束人工势场法搜索过程，将人工势场法得到的路径节点加入到规划路径中并返回规划路径和最新节点；否则跳转至步骤（1）；

步骤四：若当前节点与障碍物之间的距离小于两倍的步长时，则采用改进的RRT算法进行路径搜索：（1）初始化树，设置初始节点和目标点，定义步长、目标采样率和分割率；（2）迭代开始，在状态空间中进行采样，当随机概率大于目标采样率的时候则采样点选取目标点，若小于目标采样率则在空间中随机选取采样点；（3）根据采样节点选取树中与之最近的节点，并计算节点与最近障碍物之间的距离dist1；（4）判断距离dist1是否大于步长，如果距离dist1大于步长，则采用步长

进行拓展；如果距离dist1小于步长先按照步长

生成虚拟新节点，计算虚拟新节点与障碍物之间的距离dist2；（5）若dist2小于dist1则证明新节点有偏向障碍物的趋势，去除虚拟新节点并采用步长

进行拓展；若dist2大于dist1则证明新节点有远离障碍物的趋势，则去除虚拟新节点并利用步长

进行拓展；（6）判断新节点是否与障碍物碰撞，如果碰撞了，跳转至步骤（2）重新进行随机点采样；如果没碰撞，将新节点加入到树中，并将新节点的父节点赋予最近节点；（7）当新节点与目标点的距离小于步长，或新节点和最近障碍物的距离大于两倍的步长，则结束迭代过程；否则跳转至步骤（2）；（8）对生成的路径利用路径的碰撞检测方法进行去除冗余节点的处理得到处理后的路径；（9）将处理后的路径节点加入到规划路径中并返回规划路径和最新节点；

其中，所述步长

定义为：

其中，ρ表示随机生长的步长，

表示引力系数，

表示目标点的位置向量

和最近节点的位置向量

的欧式距离的绝对值；

所述步长

定义为：

其中

表示最近节点与障碍物s之间的距离，

表示虚拟新节点与障碍物s之间的距离，

表示随机点的位置向量

和最近节点的位置向量

的欧式距离的绝对值；

所述步长

定义为：

；

步骤五:判断最新节点与目标点之间的距离是否小于步长，如果小于步长则到达目标点，连接新节点和目标点，输出规划路径图，得到一条从起始点到终止点的无碰撞路径，退出程序；否则，跳转至步骤二。

2.如权利要求1所述的基于混合算法的机械臂路径规划方法，其特征在于：所述步骤四中计算节点与最近障碍物之间的距离，采用欧氏距离计算。

3.如权利要求1所述的基于混合算法的机械臂路径规划方法，其特征在于：所述步骤四的（8）中对生成的路径利用路径的碰撞检测方法进行去除冗余节点的处理：采用去除冗余节点和平滑处理的操作，对随机树生成的由起始点到目标点的路径进行处理，从第一个节点

开始依次连接后续路径节点，将第二个节点忽略，连接第三个节点如果未与障碍物碰撞则删除路径中的第二个节点，若碰撞则保留，并以该碰撞点的父节点作为新的判断节点，直到到达目标点为止，将最终得到的一系列保留的节点保存，并连接得到去除冗余节点后的最终规划路径。

4.如权利要求1所述的基于混合算法的机械臂路径规划方法，其特征在于：所述步骤五得到一条从起始点到终止点的无碰撞路径，采用贝塞尔曲线对路径进行平滑处理。

Images