改进RRT-人工势场法的机械臂堆垛运动方法

2022-03-16 03:58袁庆霓施辉城孙睿彤衣君辉

计算机技术与发展 2022年2期

王鑫，袁庆霓，江涛，施辉城，孙睿彤，白欢，衣君辉

(现代制造技术教育部重点实验室，贵州贵阳 550025)

0 引言

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

在机械臂的运动规划领域中，Mashayekhi R等人提出混合RRT、Informed RRT-Connect等算法，在算法中引入双树模型。Khan A T等人提出使用称为加权雅可比快速探索随机树(WJRRT)的智能算法对硬性和软性机械手的路径进行规划。Kai Mi等人提出了一种结合运动学控制和快速探索随机采样方法的运动规划算法。Sertac Karaman等人提出一种基于RRT的随时算法。Gitae Kang等人提出了一种机械手运动规划的目标导向(GO)采样方法。陈波芝等人提出基于改进RRT算法利用剪枝择优和设置目标区域的方式使算法快速收敛。王怀江等人提出基于遗传算法的机械臂优化方法为机械臂规划抓取的最短路径。李洋等人提出协同路径规划的引力自适应步长RRT算法，并在随机树的节点处引入引力函数提升算法的融合速度。丛明等人使用改进的遗传算法求解，为机械臂双臂传接过程选择最佳的传接位置。马慧丽等人在RRT的基础上引入目标引力，同时提出自适应步长避免路径算法陷入局部最优。刘建宇等人在目标偏向策略引入采样的基础上，采用梯度下降法优化搜索出的路径。周恒旭等人利用Dubins曲线策略处理Informed RRT算法，解决了轨迹不稳定和尖锐的两个缺点。

上述算法虽能有效解决路径规划问题，但对于双臂机器人或者特定场景，并不能有效地为机械臂堆垛场景规划最优路径。因此，该文提出一种改进RRT-人工势场法的路径规划方法，为机械臂末端执行器规划出一条无碰撞的货柜自动堆垛运动轨迹，并在Python和ROS对该方法进行验证。

1 算法原理

1.1 RRT算法基本原理

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

1.2 人工势场法基本原理

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

2 改进算法

2.1 障碍物建模

该文使用AABB包围盒和球体包围障碍物分别在改进RRT和改进人工势场法下进行障碍物建模，当路径点存在于障碍物模型内则表示规划路径与障碍物发生碰撞。

AABB包围盒数学描述为：

]

(1)

AABB包围盒表示为每一边都平行于坐标面的长方体。

球体包围盒数学描述为：

]

(2)

人工势场法需要考虑当前路径点与障碍物距离因素，因此，在AABB包围盒的基础上引入球体包围盒对障碍物作进一步描述。(

)为障碍物中心的世界坐标，

为球形半径，分别表示为：

(3)

(4)

2.2 改进RRT算法原理

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

具体算法描述如下：

(1)将始位置点

设置为随机树根节点

；(2)令

；(3)生成随机采样点

；(4)遍历随机树，搜索随机树距离

最近的随机树节点，将其命名为

；(5)由

沿

方向拓展生成新的枝叶节点

，且连接

和

并进行碰撞检测，若碰撞，返回步骤(2)，若无碰撞，令

为

子节点，并进入下一步；(6)连接

和末位置点

。并进行碰撞检测，若碰撞，返回步骤(2)，若无碰撞，进入下一步；(7)连接

和

，令

为

子节点，由末位置点

开始，根据随机树中父子关系搜索路径节点，生成路径PATH。其中

是随机树始位置点，

是随机树的末位置点，

是随机树新增枝叶节点，

是随机树上距离随机采样点

的最近节点，

是步长。

的计算如式(5)～式(7)所示。

(5)

=[(

),(

)]

(6)

‖

‖=

(7)

图1是随机树生长图，其中黑点是RRT随机树节点和随机采样点；

是随机树始位置点；

是随机树的末位置点；

是随机树新增枝叶节点；

是随机树上距离随机采样点

的最近节点；

是步长。改进RRT算法伪代码如算法1所示：

图1 随机树生长图

算法1: 改进RRT算法。

Input:始、末位置：

、

；工作空间(障碍空间)：

；步长：

；

Output:路径PATH。

Initialize all Parameters;

1.Add_vertex(tree,

)

2.While Connect=0 do:

3.For 1 To N:

←Sample_free(X);5.

←Get_nearest(tree,

);6.if Collision_free(

) then:7.Connect_to_point(tree,

);

8.End if

9.if Collision_free(

) then:

10.PATH←Get_path( );

11.Connect=1;

12.End if

13.End For

14.End While

15.Final

16.Return PATH

2.3 改进人工势场法原理

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

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

可定义为：

(

)

(8)

其中，

为机械臂末端执行器当前位置点，

(

)表示引力势场，

(

)表示斥力势场。相应的合力

为：

(

)

(9)

针对当前位置与末位置距离过大而导致受力不平衡的问题，以障碍物斥力范围作为标准给定阈值

，用于修正引力函数，防止引力远大于斥力而导致路径与障碍物发生碰撞。将

(

)表示为：

(10)

其中，

为常量；

(

)为当前位置到末位置的距离；

为给定的当前位置与末位置距离的阈值。

算法2：改进人工势场法。

Input:始、末位置：

、

；工作空间(障碍空间)：

；步长：

；

Output:路径Path。

Initialize all Parameters;

1.Path = [

];2.

;3.While Dis(

) > S do:4.

←Attractive( ) + Repulsion( );5.

←

.direction( )*

;6.path←Get_path(

);7.if Dis(

) <

then:8.path←Get_path(

);

9.End if

10.End While

11.Final

12.Return path

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

(11)

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

(

)表示为：

(12)

其中，num=

(

)。

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

(13)

(

)、

(

)分别为：

(14)

(15)

其中，

为斥力场常量;

(

)为当前位置到障碍物的距离；

为斥力势场范围；人工势场法伪代码如算法2所示。

2.4 非均匀B样条曲线

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

B样条曲线方程为：

(16)

其中，

为控制点；

,为

次B样条的基函数；

为区间数；

为样条曲线的次数。

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

(17)

从式(17)可知，第

个控制点对应的

次B样条基数只与[

++1]个参数域有关。该文定义的是3次4阶非均匀B样条曲线，即当路劲节点为

时，该样条曲线共有

+3个参数区间，其合法的定义区域为[

]。

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

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

图2 改进RRT-人工势场法流程图

算法描述如下:

(1)初始化机械臂工作空间及工作空间中环境信息，给定机械臂移动的始、末位置点

、

和机械臂移动步长

;

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

(3)将改进RRT规划全局路径PATH设为当前路径，并对当前路径进行判断。若存在冗余路径，将冗余节点设为人工势场法始、末位置点

、

，开始改进人工势场法局部路径规划；否则执行攻进RPT算法步骤6)；(4)将始、末位置点

、

为当前位置点和末位置点代入式(11)计算引力，式(13)计算斥力，并根据式(9)求得当前位置点合力大小及方向。由当前位置点沿合力方向拓展步长

生成新节点

。同理，重复上述步骤直到Dis(

?，令

为

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

3 实验

3.1 实验任务

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

3.2 Python实验

为验证文中所提算法是否具有改进意义，将改进RRT-人工势场法同人工势场法、RRT和Bi-RRT算法进行对比分析。实验在地图大小为100×100×100，障碍物数目

为30的随机障碍场景中进行，始位置点为(0, 0, 0)，末位置点为(100, 100, 100)。

图3是四种算法对比实验。细线为随机树，粗线为算法规划的路径曲线。通过对比分析可知，RRT与Bi-RRT规划路径时不能有效解决因随机策略产生的随机树生长无向性问题，人工势场法在规划路径时能够有效地靠近末位置点，改进RRT-人工势场法混合算法能够有效地将两种算法的优点进行融合，规划出一条逐渐靠近末位置点的无碰光滑路径。

图3 算法对比实验

当

=30，40，50时，对该算法实验50次，结果如表1所示。随着环境复杂程度的增加，其平均路径长度无明显变化。由此可知，改进RRT-人工势场法混合算法不受复杂程度影响，且该算法能够稳定有效地为机械臂规划出一条无碰撞的光滑路径。为验证该算法是否能够有效为机械臂在工作空间中规划路径，故使用Python中模拟机械臂实验场景。该场景包含一个模拟传送带AABB包围盒和由若干AABB包围盒组成的立式货架，具体如图4所示。

图4 路径规划场景模拟实验

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

图4(a)～图4(d)为不同末位置点路径规划图，始位置位于传送带上，且坐标为(10, 25, 11)。结果表明，提出的改进RRT-人工势场法混合算法能够为机械臂规划出由传送带至货架任意格子的无碰光滑路径。

3.3 ROS实验仿真

在Ubuntu16.04LTS、ROS Kinetic平台中使用Gazebo模拟现实仿真，如图5所示，主要包含:传送带、块状物、UR5机械臂、货架。要求UR5机械臂在运动状态下传送带上，抓取块状物放置在货架的指定位置，实现机械臂由抓取的初始位置，避开货架边缘侧壁，为机械臂抓取至放置运动规划出一条至指定目标位置的无碰撞路径。

图5 货柜堆垛运动

图5(a)为机械臂在运动状态的传送带上抓取块状物；图5(b)为机械臂将块状物送至货架指定位置的行进运动；图5(c)为机械臂将块状物放置指定位置的货柜堆垛运动。由图可知，该算可以完成货柜堆垛场景下的机械臂运动仿真，实现为机械臂抓取、货柜堆垛运动，验证了改进RRT-人工势场法的有效性与可靠性。

4 结束语

该文提出改进RRT-人工势场法混合算法对机械臂堆垛进行运动规划，其主要改进点如下：

(1)针对传统RRT算法存在的路径节点多、冗余路径长等问题，引入目标搜索。该方法能够有效探索空间，减少随机树无效生长，提高算法规划效率；

(2)针对人工势场法受力不平衡和目标不可达等问题，分别利用斥力范围作为阈值修正引力函数、当前位置点和末位置点距离作为影响参数修正斥力函数。

(3)利用RRT算法的概率完备性规划全局路径，利用人工势场法的有向性对RRT算法进行了补充。同时，RRT算法也为改进算法的避障能力提供了有效保证。

最后，将改进RRT-人工势场法混合算法分别在Python和ROS系统中对机械臂货柜堆垛运动进行了模拟仿真。验证结果表明，该算法能够为机械臂堆垛运动提供有效支持。