CN113084811A

CN113084811A - 一种机械臂路径规划方法

Info

Publication number: CN113084811A
Application number: CN202110389354.XA
Authority: CN
Inventors: 袁庆霓; 王鑫; 江涛; 杨观赐; 吴杨东; 蓝伟文
Original assignee: Guizhou University
Current assignee: Guizhou University
Priority date: 2021-04-12
Filing date: 2021-04-12
Publication date: 2021-07-09
Anticipated expiration: 2041-04-12
Also published as: CN113084811B

Abstract

本发明公开了一种机械臂路径规划方法，其特征在于：首先，引入目标搜索，对传统RRT算法进行改进，并使用改进后的RRT算法进行全局路径规划；其次，分别以斥力势场范围为阈值和机械臂末端执行器至末位置点影响修正引力函数和斥力函数，改进人工势场法，使用改进的人工势场法对局部路径进行优化；然后，使用三次B样条曲线对路径进行平滑处理并得到最终路径。本发明利用RRT算法的概率完备性规划全局路径，利用了人工势场法的有向性对RRT算法进行了补充，同时，RRT算法也为改进算法的避障能力提供了有效保证。总之，本发明在机械臂路径规划中能有效地获得最优路径规划，且规划效率高。

Description

一种机械臂路径规划方法

技术领域

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

背景技术

电子商务的高速发展带动了我国物流业的自动化和智能化。随着互联网电商平台订单量的大幅增长以及产品种类的多元化，对仓库中货物的拣选、货柜堆垛的时效及工作量的要求越来越高，而传统的仓储***中，人工货柜堆垛和分拣货柜中物品已经成为仓储自动化的瓶颈，亟待解决的问题，其中货柜物品的自动货柜堆垛首先需解决机械臂的运动规划问题。

在机械臂的运动规划领域中，常用的有自由空间法、人工势场法、快速拓展随机树算法(RRT)、可视图法、栅格解耦法和拓扑法等运动规划方法。现有技术中，Mashayekhi,Reza等人提出混合RRT、Informed RRT*-Connect等算法，在算法中引入双树模型。Khan,AT提使用称为加权雅可比快速探索随机树(WJRRT)的智能算法对硬性和软性机械手的路径进行规划。Kai Mi等人提出了一种结合运动学控制和快速探索随机采样方法的运动规划算法。Sertac Karaman提出一种基于RRT*的随时算法。Gitae Kang提出了一种机械手运动规划的目标导向(GO)采样方法。陈波芝等提出基于改进RRT算法利用剪枝择优和设置目标区域的方式使算法快速收敛。王怀江等提出基于遗传算法的机械臂优化方法为机械臂规划抓取的最短路径。这方法虽能有效解决路径规划问题，但针对特定场景，并不能有效地获得最优路径规划。

发明内容

本发明的目的在于克服上述缺点而提出了一种改进RRT-人工势场法的机械臂路径规划方法，能有效地获得最优路径规划。

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

步骤一、初始化机械臂工作空间及工作空间中环境信息，给定机械臂移动的始位置点q_init、机械臂移动的末位置点q_goal和机械臂移动步长S；

步骤二、利用改进RRT算法为机械臂规划全局路径；

步骤三、将改进RRT规划的全局路径PATH设为当前路径，并对当前路径进行判断；若存在冗余路径，将冗余节点设为改进人工势场法的始位置点q_i、改进人工势场法的末位置点q_j，开始采用改进人工势场法进行局部路径规划，否则执行步骤六；

步骤四、将人工势场法的始位置点q_i、末位置点q_j设为当前位置点和末位置点代入公式1计算引力，公式2计算斥力，并根据公式3求得当前位置点合力；由当前位置点沿合力方向拓展步长S生成新节点q_new；同理，重复步骤四直到新节点q_new和机械臂移动的末位置点q_goal的距离小于步长S，即Dis(q_new,q_goal)<S，令q_goal为q_new；

所述公式1为：

其中，F_att为引力，ξ为常量；ρ(q,q_go_al)为当前位置到末位置的距离， D为给定的当前位置与末位置距离的阈值；

所述公式2为：

其中，F_rep为斥力，F_rep1(q)、F_rep2(q)分别为：

η为斥力场常量；ρ(q,q_obs)为当前位置到障碍物距离；ρ⁰斥力势场范围；

所述公式3为：

F_vec(q)＝F_att(q)+F_rep(q)

其中，F_vec为合力；

步骤五、当优化路径满足目标条件时，对优化路径进行碰撞检测。若碰撞，优化失败；若无碰撞发生，生成优化路径path，并替代原局部路径；

步骤六、采用三次B样条曲线光滑路径并生成最终路径。

上述机械臂路径规划方法，其中：所述改进RRT算法，包括以下步骤：

1)将始位置点q_init设置为随机树根节点T_init，并令新节点等于始位置点q_init；

2)生成随机采样点q_rand；

3)遍历随机树，搜索随机树距离q_rand最近随机树节点q_nearest；

4)由q_rand沿q_nearest方向拓展生成新的枝叶节点q_new，且连接q_rand和q_new并进行碰撞检测，若碰撞，返回步骤2)，若无碰撞发生，令q_new为q_nearest子节点；

5)连接q_new和末位置点q_goal，并进行碰撞检测，若碰撞，返回步骤2)，若无碰撞发生，执行步骤6)；

6)令q_goal为q_new子节点，由末位置点q_goal开始，根据随机树中父子关系搜索路径节点，生成路径PATH。

上述机械臂路径规划方法，其中：所述改进人工势场法，在传统的人工势场法基础上，以障碍物斥力范围作为标准给定阈值D，用于修正引力函数，防止引力远大于斥力而导致路径与障碍物发生碰撞；同时，在原有斥力场的基础上增加机械臂末端执行器与末位置距离的影响，机械臂末端执行器靠近末位置时，斥力随着位置靠近而相应减小，避免斥力远大于引力而导致目标不可达。

本发明与现有技术的相比，具有明显的有益效果，由以上方案可知，所述改进RRT算法，针对传统随机树生长方向具有无向性，随着路径采样点的增加，冗余路径也随之增加问题，本发明通过引入目标搜索对传统的 RRT算法进行改进，改进RRT算法能够有效减少随机树无用生长，减小冗余路径，提高算法规划效率。所述改进人工势场法，针对人工势场法受力不平衡和目标不可达等问题，分别使用引入斥力范围作为阈值修正引力函数、当前位置点和末位置点距离作为影响参数修正斥力函数。此外，本发明利用RRT算法的概率完备性规划全局路径，利用了人工势场法的有向性对RRT 算法进行了补充，同时，RRT算法也为改进算法的避障能力提供了有效保证。总之，本发明在机械臂路径规划中能有效地获得最优路径规划，且规划效率高。

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

附图说明

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

图2为本发明的改进RRT算法的随机树生长图；

图3为具体实施方式中四种不同末位置点q_goal路径规划仿真图。

具体实施方式

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

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

步骤二、利用改进RRT算法为机械臂规划全局路径；

所述公式1为：

其中F_att为引力，ξ为常量；ρ(q,q_go_al)为当前位置到末位置的距离， D为给定的当前位置与末位置距离的阈值；

所述公式2为：

其中F_rep为斥力，F_rep1(q)、F_rep2(q)分别为：

所述公式3为：

F_vec(q)＝F_att(q)+F_rep(q)

其中F_vec为合力；

步骤六、采用三次B样条曲线光滑路径并生成最终路径。

所述改进RRT算法，包括以下步骤：

2)生成随机采样点q_rand；

所述改进人工势场法，在传统的人工势场法基础上，以障碍物斥力范围作为标准给定阈值D，用于修正引力函数，防止引力远大于斥力而导致路径与障碍物发生碰撞；同时，在原有斥力场的基础上增加机械臂末端执行器与末位置距离的影响，机械臂末端执行器靠近末位置时，斥力随着位置靠近而相应减小，避免斥力远大于引力而导致目标不可达。

其中：

1.1传统RRT算法基本原理

快速拓展随机树算法(RRT)基本原理是将始位置点作为随机树的根位置点，然后，在工作空间内随机采样并找到随机树上距离采样点最近节点，由最近节点沿采样点方向拓展一个步长生成新的枝叶节点，直到末位置点成为随机树枝叶节点。最后，连接随机树枝叶节点生成一条连接始、末位置点的无规则路径。

1.2传统人工势场法基本原理

人工势场法最早是针对移动机器人避障规划。其算法的基本思想是将机器人转化为一个质点，将整个环境转化为一个人造势场，该势场包含末位置点对机器人的引力势场和障碍物对机器人的斥力势场，机器人受引力势场产生的引力和斥力势场产生的叠加斥力，沿引力和斥力的合力方向运动。

1.3改进RRT算法原理

针对传统随机树生长方向具有无向性，随着路径采样点的增加，冗余路径也随之增加。本发明通过引入目标搜索，该方法能够有效减少随机树无用生长，减小冗余路径，提高算法规划效率(如图2)。

具体算法描述如下：

1)将始位置点q_init设置为随机树根节点T_init，并令q_new＝q_init；

2)生成随机采样点q_rand；

3)遍历随机树，搜索随机树距离q_rand最近随机树节点，将其命名为 q_nearest；

4)由q_rand沿q_nearest方向拓展生成新的枝叶节点q_new，且连接q_rand和q_new并进行碰撞检测，若碰撞，返回步骤2)，若无碰撞发生，令q_new为q_nearest子节点，并进入下一步；

5)连接q_new和末位置点q_goal。并进行碰撞检测，若碰撞，返回步骤2)，若无碰撞发生，进入下一步；

其中q_init是随机树始位置点，q_goal是随机树的末位置点，q_new是随机树新增枝叶节点，q_nearest是随机树上距离随机采样点q_rand节点最近节点，S是步长。其中q_new的计算如下：

q_rand-q_nearest＝

((x_rand-x_nearest)，(y_rand-y_nearest)，(z_rand-z_nearest))

图2是随机树生长图。黑点是RRT随机树节点和随机采样点；q_init是随机树始位置点；q_goal是随机树的末位置点；q_new是随机树新增枝叶节点；q_nearest是随机树上距离随机采样点q_rand节点最近节点；S是步长。

1.4改进人工势场法原理

人工势场法全部信息来源引力势场和斥力势场，当机械臂远离末位置点时：引力远大于斥力，规划路径易与障碍物发生碰撞；当障碍物靠近末位置点时：斥力远大于引力，易发生目标不可达。针对上述问题，对人工势场法进行改进，解决路径与障碍碰撞和目标不可达等问题。

人工势场主要由末位置点对机械臂末端执行器产生的引力势场和障碍物产生的斥力势场组成。故人工势场U_vec可定义为：

U_vec(q)＝U_att(q)+U_rep(q)

其中,q为机械臂末端执行器当前位置点，U_att(q)表示引力势场,U_rep(q) 表示斥力势场。相应的合力F_vec为：

F_vec(q)＝F_att(q)+F_rep(q)

针对当前位置与末位置距离过大而导致受力不平衡问题，以障碍物斥力范围作为标准给定阈值D，用于修正引力函数，防止引力远大于斥力而导致路径与障碍物发生碰撞。将U_att(q)表示为：

ξ为常量；ρ(q,q_goal)为当前位置到末位置的距离；D为给定的当前位置与末位置距离的阈值；

由引力势场引发的引力相应表示为：

针对于目标不可达的问题，在原有斥力场的基础上增加机械臂末端执行器与末位置距离的影响，机械臂末端执行器靠近末位置时，斥力随着位置靠近而相应减小，避免斥力远大于引力而导致目标不可达。U_rep(q)表示为：

由斥力势场引发的斥力相应表示为：

F_rep1(q)、F_rep2(q)分别为：

其中，η为斥力场常量；ρ(q,q_obs)为当前位置到障碍物距离；ρ⁰斥力势场范围；

1.5非均匀B样条曲线

机械臂工作空间出现数目较多的随机障碍物时，为满足避障需求，规划路径往往会出现路径转折。在此情况下，机械臂会因为加速度突变而引起机体振动，对机体造成伤害。因此，本发明引入非均匀B样条曲线平滑路径，为机械臂在复杂的障碍环境中提供有效解决方案。

B样条曲线方程为：

其中，p_i控制点，N_(i,n)为n次B样条的基函数。M为区间数，n为样条曲线的次数。

B样条的基本函数Cox-deBoor递归公式定义：

从上面公式中可知，第j个控制点对应的n次B样条基数只与[t_j，t_(j+n+1)] 个参数参数域有关。

本发明定义的是n次n+1阶B样条曲线，既当路劲节点为m时，该样条曲线共有m+n个参数区间，其合法的定义区域为[U_n，U_m]。

1.6改进RRT-人工势场法混合算法

本发明提出的改进RRT-人工势场法混合算法，是利用改进RRT算法对全局路径进行规划，当RRT规划存在冗余路径时，使用人工势场法对局部进行优化，并用优化路径取代冗余路径。其具体流程如图1所示。

算法描述如下:

1)初始化机械臂工作空间及工作空间中环境信息，给定机械臂移动的始、末位置点q_init、q_goal和机械臂移动步长S；

2)利用改进RRT算法为机械臂规划全局路径，其具体流程如改进RRT 算法描述所示；

3)将改进RRT规划全局路径PATH设为当前路径，并对当前路径进行判断。若存在冗余路径，将冗余节点设为人工势场法始、末位置点q_i、q_j，开始改进人工势场法局部路径规划；否则执行步骤6)；

4)将始、末位置点q_i、q_j为当前位置点和末位置点代入计算引力公式、计算斥力公式，并求得当前位置点合力大小及方向。由当前位置点沿合力方向拓展步长S生成新节点q_new。同理，重复上述步骤直到Dis(q_new,q_goal)<S，令q_goal为q_new；

5)当优化路径满足目标条件时，对优化路径进行碰撞检测。若碰撞，优化失败；若无碰撞发生，生成优化路径path，并替代原局部路径；

6)使用三次B样条曲线光滑路径并生成最终路径；

实验验证分析

为了验证改进RRT-人工势场法混合算法的有效性和可行性，在Python 上对改进算法进行比较和仿真实验。所有实验均在16G内存的Intel i7-9750H DELL Y7000计算机上进行。

将改进RRT-人工势场法算法同人工势场法、RRT和Bi-RRT算法进行对比分析。实验进行在地图大小为100×100×100，障碍物数目N为50的随机障碍场景中，始位置点为(0,0,0)，末位置点为(100,100,100)。

当N＝30，40，50时，对该算法实验50次。实验结果如表1所示，随着环境复杂程度增加，其平均路径长度无明显变化。由此可知，改进RRT- 人工势场法混合算法不受复杂程度发生变化，且该算法能够稳定有效为机械臂规划出一条无碰撞的光滑路径。

表1障碍物数量不同时各算法数据分析

为验证本文所提出的算法是否能够有效为机械臂在工作空间中规划路径，故使用Python中模拟机械臂实验场景。该场景中，包含一个模拟传送带AABB包围盒和由若干AABB包围盒组成的立式货架，其具体如图3所示。图3(a)-(d)为四种不同末位置点路径规划图，始位置点位于传送带上，且坐标为(10,25,11)。结果表明，本发明提出的改进RRT-人工势场法混合算法能够为机械臂规划出由传送带至货架任意格子的无碰光滑路径。

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

Claims

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

步骤一、初始化机械臂工作空间及工作空间中障碍物环境信息，给定机械臂移动的始位置点q _init、机械臂移动的末位置点q _goal和机械臂移动步长S;

步骤二、利用改进快速拓展随机树算法RRT为机械臂规划全局路径；

步骤三、将改进RRT规划的全局路径PATH设为当前路径，并对当前路径进行判断；若存在冗余路径，将冗余节点设为改进人工势场法的始位置点q _i、改进人工势场法的末位置点q _j，开始采用改进人工势场法进行局部路径规划，否则执行步骤六；

步骤四、将改进人工势场法的始位置点q _i、末位置点q _j设为当前位置点和末位置点代入公式1计算引力、公式2计算斥力，并根据公式3求得当前位置点合力及方向；由当前位置点沿合力方向拓展步长S生成新节点q _new；判断新节点q _new和机械臂移动的末位置点q _goal的距离是否小于步长S，若大于和等于，即Dis(q _new ,q _goal )S，则执行步骤四；若小于，即Dis(q _new , q _goal )<S，若令q _goal为q _new，执行步骤五；

所述公式1为：

其中，

为引力，ξ为常量；ρ(q,q _goal )为当前位置点到末位置点q _goal的距离，D为给定的当前位置点与末位置点q _goal距离的阈值；

所述公式2为：

其中，

为斥力，F _rep1 (q)、F _rep2 (q)分别为：

η为斥力场常量；ρ(q,q _obs )为当前位置点到障碍物距离；ρ ⁰斥力势场范围；

所述公式3为：

其中，

为合力；

步骤五、进行碰撞检测，若碰撞，优化失败；若无碰撞发生，生成优化路径path，并替代原局部路径；

步骤六、采用三次B样条曲线光滑路径并生成最终路径。

2.如权利要求1所述的机械臂路径规划方法，其特征在于：所述改进RRT算法，包括以下步骤：

将始位置点q _init设置为随机树根节点T _init，并令新节点等于始位置点q _init；

生成随机采样点q _rand；

遍历随机树，搜索随机树中离q _rand最近随机树节点q _nearest；

由q _rand沿q _nearest方向拓展生成新的枝叶节点q _new，且连接q _rand和q _new并进行碰撞检测，若碰撞，返回步骤2）；若无碰撞发生，令q _new为q _nearest子节点；

连接q _new和末位置点q _goal，并进行碰撞检测，若碰撞，返回步骤2）；若无碰撞发生，执行步骤6）；

令q _goal为q _new子节点，由末位置点q _goal开始，根据随机树中父子关系搜索路径节点，生成路径PATH。

3.如权利要求1或2所述的机械臂路径规划方法，其特征在于：所述改进人工势场法，在传统的人工势场法基础上，以障碍物斥力范围作为标准给定阈值D，用于修正引力函数，防止引力远大于斥力而导致路径与障碍物发生碰撞；在原有斥力场的基础上增加机械臂末端执行器与末位置点q _goal距离的影响，机械臂末端执行器靠近末位置点q _goal时，斥力随着位置靠近而相应减小，避免斥力远大于引力而导致目标不可达。