基于GB-APFB-RRT*算法的机械臂路径规划*

尹庆文

(昆明理工大学机电工程学院,昆明 650500)

0 引言

随着智能制造技术的不断发展,机械臂广泛应用于工业发展,太空探索,医疗辅助等行业,减轻了人为因素对工作的影响,提高了工作效率,给制造业带来了许多便利[1-3]。然而,当前机械臂的自动化和智能化水平依旧发展缓慢,在多数环境狭小作业场合,需要依赖人为的示教,工作人员难以直接参与该环境,导致了机械臂发展受到局限性。因此,机械臂能自主的、高效的规划出一条可行路径成为当前的研究热点。

机械臂路径规划算法主要有A*[4]算法,蚁群算法[5],人工势场法[6],快速随机树算法(rapidly-exploring random trees,RRT)[7]等,前两种算法直观易修改,但是存在不稳定性,取决于具体问题和设计者的经验;人工势场法是一种反馈控制策略,对控制和传感误差具有一定鲁棒性,但是很容易陷入局部最小值;RRT适用于高维度空间,它们以概率完备性来代替完备性,从而提高搜索效率,但是存在盲目性高,导向性差等特点。本文综合考虑研究热点和机械臂所处的高纬度空间,选取RRT算法作为基础算法进行研究。许多学者对该算法提出了改进思路,张振等[8]提出了一种基于采样的RRT算法,改进稀疏点产生机制,减少重复性采样,提出动态采样策略,提高了扩展方向的确定性,但是保留了RRT算法的部分缺陷,规划的路径不为最优;魏武等[9]提出了一种双树Quick-RRT*算法,起点和终点分别生成一棵随机树,起点树和终点树轮流生长,提高了路径收敛速度,但是该学者只运用于二维环境移动机器人,对机械臂所属于的高纬度空间未进行分析;陈法法等[10]设计了一种多次根节点快速扩展随机树算法,采用多个有限扩展次数的次根节点生成局部连通路径,减少了采样点落在障碍物的情况次数,该算法解决了采样点生成的情况,未解决RRT本身盲目性的问题;张婷婷等[11]提出一种目标导向和冗余删除策略,使路径更加平滑,但是在复杂环境中,容易陷入局部最小值,无法逃离障碍物环境。

本文基于RRT算法本身存在的盲目性差,导向性不明显提出了一种基于人工势场引导的概率偏置双向最优RRT算法(GB-APFB-RRT*)。首先,把单树搜索改为双向搜索,在采样时,利用目标偏置阈值控制采样点方向,再重选父节点重布线,保证每次迭代点为最优;其次,加入人工势场思想引导新节点成长,控制新节点方向接近终点并且避开障碍物;最后,删除冗余点用三次B样条曲线对路径进行拟合,使得机械臂在关节空间下运动平稳。

1 机械臂运动学分析及碰撞检测技术

1.1 机械臂运动学分析及模型建立

本文采用埃夫特ERF-900六轴六自由度机械臂为研究对象如图1a所示,在SolidWorks中装配关系如图1b所示,其改进型D-H模型坐标系如图1c所示,D-H参数表如表1所示。

(a) 六自由度机械臂 (b) SolidWorks中装配关系

(c) 改进型D-H模型坐标系图1 机械臂图

表1 改进型D-H参数表

表中:i为第i个关节,αi-1为连杆扭转角,ai-1为连杆长度,di为连杆偏距,θi为关节角。

根据改进型D-H参数表,可得机械臂相邻两关节齐次变换矩阵为:

(1)

式中:cθi表示cos(θi),sθi表示sin(θi),cαi-1表示cos(αi-1),sαi-1表示sin(αi-1)。

(2)

式中:n、o、α分别表示机械臂末端坐标系X、Y、Z相对于基坐标姿态的单位矢量,p表示末端坐标系相对于基座的位置矢量。

1.2 碰撞检测分析

在工业环境中,障碍物模型具有不规则,多排列,密集分布等特点。因此要根据机械臂机身尺寸与障碍物物理信息建立数学模型。针对障碍物物理信息建立数学模型方法中,主要有包络球[12],包络盒[13]等。两种建立数学模型方法核心思想类似,用大体积的模型集合来代替本体模型。针对本文机械臂结构形态和障碍物为空间紧密分布的方体,为确保碰撞检测技术的准确性,采用包围盒包络法来描述障碍物简化数学模型。

在此基础上,为了提高碰撞检测技术的稳定性,使用圆柱体包络盒来简化机械臂连杆模型和包围球简化机械臂关节,包围球和圆柱体半径均为ri,并且以包围球半径ri对障碍物模型进行膨胀处理,数学模型如图2所示。定义机械臂包络盒数学模型为R{L1,L2,L3,L4,L5,L6},膨胀后的障碍物数学模型6个面为C{O1,O2,O3,O4,O5,O6},只要满足两个数学模型没有交集则机械臂障碍物不会发生碰撞,如式(3)所示。

图2 机械臂碰撞检测模型

C{O1,O2,O3,O4,O5,O6}∩C{O1,O2,O3,O4,O5,O6}=∅

(3)

2 GB-APFB-RRT*算法原理

2.1 采样方式和最优扩展

在RRT算法中加入了双树思想,在起始点xstart和目标点xgoal生成两颗随机树Tree1和Tree2,同时进行迭代扩展[14]。如图3所示。

图3 双向扩展图

在搜索空间内采用目标偏置策略来控制随机树采样点xrand1,xrand2的生长,引导随机树扩展的大概生长方向。当随机概率点Prandom小于目标偏置阈值xvalve时,随机点xrand的生长偏向于未知区域扩展,即在空间中随机选取采样点,增加随机树向位置区域的遍历。如果概率点大于等于阈值点,则随机树的扩展方向为另一颗随机树的起点。目标偏置策略公式为:

(4)

在此基础上,加入RRT*扩展思想,重新为新枝叶点xnew重新选择父节点,以节点xnew为圆心定义半径为r(r一般选取迭代步长的3～5倍)做圆,寻找最优点,作为替换xnew父节点的备选,依次计算xnew到邻域r内所有节点的路径代价,如果存在某一节点xmin到达xnew路径代价小于xnearest到xnew的路径代价最小并且不发生碰撞,则删除xnew与xnearest的连线,将xmin作为新父节点,并且重新画线路径线,使得路径代价降低,重布线示意图如图4所示。

图4 最优扩展

基于目标偏置双向RRT*算法保留了RRT算法的整体性和扩展性,并且对随机xrand点的生成增加了偏置,通过偏置阈值来引导随机点的生成,当环境复杂且障碍物较多时提高阈值xvalve使随机树向未知区域进行遍历,反之降低xvalve的数值,然后对xnew点增加了重选最优点重布线过程,使得到的路径接近最短路径。但是该算法存在复杂环境下收敛速度过慢,搜索时间较长等问题。

2.2 新节点扩展策略

针对上文算法存在搜索目的性较差的特点,加入人工势场思想[15]控制新节点的生成。

人工势场的引力势函数为:

Uatt(q)=1/2ξd2(q,qgoal)

(5)

人工势场的引力函数为:

Fatt(q)=▽Uatt(q)=ξd(q,qgoal)

(6)

式中:ξ为引力常数,q和qgoal分别为规定区域中起点位置和目标位置,d(q,qgoal)表示两个位置之间的欧氏距离,为设定的距离阈值。

人工势场的斥力力势函数为:

(7)

人工势场的斥力函数为:

(8)

人工势场法的合力势函数为:

U(q)=Uatt(q)+Urep(q)

(9)

人工势场法的合力函数为:

F(q)=Fatt(q)+Frep(q)

(10)

新节点Xnew的生长方式原理图如图5所示。图中,xnew为点xnearest沿xrand方向扩展一个步长得到的初步新节点,Frep为障碍物对点xnearest的排斥力向量,Fatt为目标点xgoal对点xnearest的引力向量,F为点xnearest在搜索区域内所受的人工势场和向量,Frand代表由点xnearest指向xrand的向量,F1为F和Frand的合向量。新节点的生长方向由父节点xnearest的人工势场合向量和指向向量的合力方向确定,在该方向上扩展一个步长xstepsize则得到新节点Xnew,如式(11)所示。

图5 新节点扩展原理图

(11)

式中:xstepsize为偏置RRT*算法的迭代步长,λ为人工势场的迭代步长,λ

2.3 路径优化

本文算法得出的路径点P={P1,…,Pi,…,Pk}不是最优路径,充满了冗余点,需要对冗余点进行去除。冗余点去除的思想为若Pi与Pk-1(k-1-i>1)间的连线与障碍物没有发生碰撞则把Pi与Pk-1中的冗余点去除,如图6所示。

图6 冗余点删除示意图

在此基础上,路径中的冗余点已经全部删除,但是存在大量拐点,不是一条平滑的曲线。机械臂在跟随路径运动时会造成抖动,不稳定等特点,因此使用三次B样条曲线对路径进行平滑处理[16]。

K次B样条曲线方程为:

(12)

式中:K表示B样条的次数,K+1表示曲线阶数,i表示B样条的序号,控制点di个数为n+1,Ni,k(u)b表示i个K次B样条基函数,使用Cox-deBoor递推公式获取:

(13)

因此三次均匀B样条曲线为:

好的。雷果子还告诉我说只有屎克郎有点惨，当兵头一年就上了南边战场，被敌人坦克辗死了，像辗一只屎克郎那样辗得骨头渣都没剩了，真是惨哩。我听后心里也怪不是味儿的。这样沉默了好一会儿，我说明我的来意，我说我已经在天地人大酒店君子兰厅订了一桌饭，请他、螃蟹、屁斑虫和茄子几个一起吃。他说可以，还说螃蟹、屁斑虫和茄子几个没忘了他这个小学老师，时常请他吃饭的。我说想请他以他的名义请他们几个。他问为啥。我说长时间没回来，他们几个不一定还认得我，还是他出面请为好。他狠想了想，答应了。我说声不见不散，然后告辞了。

p0,3(u)=d0*N0,3(u)+d1*N1,3(u)+
d2*N2,3(u)+d3*N3,3(u)u∈[0,1]

(14)

当给定控制点di后,利用上式就可以求出一条三次B样式的平缓曲线点。

3 算法实现

本文算法流程图如图7所示。

图7 算法流程图

步骤1:初始化栅格地图,初始化障碍物环境,定义初始参数,起末坐标点和起末两颗随机树;

步骤2:利用概率偏置策略控制采样点生成,并向采样点方向扩展一个步长得到初步新节点,在次基础上重选父节点重布线,使每次迭代的新节点路径长度最短;

步骤3:引入人工势场思想,算出父节点的人工势场合力向量,并且和父节点指向初步新节点方向求出指向向量,利用式(11)求出最终新节点的位置;

步骤4:判断新节点与障碍物是否发生碰撞,若发生碰撞则去除新节点,重新通过目标概率阈值选取采样点重新迭代;否则进行两个新节点的位置判断,若不为同一个点,再次进入目标概率阈值选取采样点重新迭代,若为同一个点,则进入下一步;

步骤5:合并两棵树节点为路径点,进行路径优化处理,先删除冗余点后通过三次B样条进行曲线拟合;

4 算法仿真实验分析

为了验证本文算法的高效性和应用性,设置两组仿真实验。第一组实验把机械臂看成一个质点寻找避障路径,通过与传统RRT算法、B-RRT*算法、APF-RRT算法的对比,来证实本文算法高效、准确的特点;第二组实验把本文算法应用在具体的工程环境中,考虑机械臂放置工件时机械臂与障碍物的碰撞检测,规划出一条平缓的路径。两种实验分析均在同一台计算机上进行,计算机型号为80WW,在MATLAB 2021a上进行实验仿真。

4.1 算法高效性仿真实验

基于本文算法高效性仿真实验分析,设置搜索区域大小为[0 1000;0 1000;0 1000],为搜索区域建立栅格地图,起始点为原点[0 0 0],目标点为[1000 1000 1000],自定义参数迭代步长xstepsize为5,目标偏置概率为0.8,取20次数据的平均值做表,如表2所示。障碍物为形状不同,大小不一的方体,立体和圆柱体,布局环境为不规则布局。其中,方体障碍物颜色为透明度是0.3的浅灰色,球体障碍物颜色为透明度是0.3的黑色,圆柱体障碍物颜色为透明度是0.3的深灰色,仿真实验如图8所示。

(a) RRT (b) B-RRT*

(c) APF-RRT(d) 本文算法图8 算法对比图

表2 算法数据对比图

实验结果表明,本文算法规划时间、迭代步长、节点数、路径长度皆为最优,证明了本文算法的准确性和高效性。分析表格数据可得,本文算法与传统RRT算法相比规划时间减少了95%,迭代次数降低了98%,路径节点数降低了32%,路径长度减少了20%;与Goal-bias BRRT*算法相比比较指标分别减少了89%、98%、5%、12%;与APF-RRT算法相比比较指标分别减少了54%、88%、38%、15%。

4.2 机械臂算法仿真实验分析

首先,在MATLAB平台搭建运动规划场景。起始点为机械臂末端执行器位置,坐标[0.68,0,0.75],终点为数控铣床工作台工件装夹定位中心[0.15,0.9,0.54]。在此基础上,根据真实情况再设置两个机械臂必经坐标点,第一个坐标点为机械臂面向铣刀位置点[-0.15,0.4,0.58],第二个坐标点为机械臂到达放置工件区域位置初始点[-0.15 0.9 0.58],以上4点均用红点表示。障碍物环境为数控铣床长方体防护挡板,大小为[0.30,0.03,0.72]和[0.52,0.03,0.72],夹具4个装夹工件[0.08,0.02,0.02]和铣刀,如图9所示。

图9 仿真环境图

其次,ERF-900机械臂在关节空间中采样规划,采用包围盒的方式对机械臂与障碍物进行碰撞检测,将本文算法规划出的路径进行冗余点删除和三次B样条曲线优化得到最终平滑路径。

最后,记录了机械臂通过本文算法规划路径中的4个运行位姿,分别是:面向铣床防护板位姿图10a,到达第一个路径点位姿,机械臂末端执行器X轴坐标面向铣床夹具;通过铣床防护板位姿图10b,机械臂与铣床防护板无碰撞;夹具上方姿态图10c,到达第二个路径点,机械臂到工件放置区域初始位置,准备放入夹具;机终点位姿图10d,机械臂到达夹具装夹工件位置,完成工件的放置。机械臂4个运行姿态如图10所示。

(a) 面向铣床防护板位姿图 (b) 铣床防护板位姿图

(c) 夹具上方姿态图 (d) 机终点位姿图图10 机械臂4个运行姿态图

5 结论

为了解决路径规划中机械臂RRT算法存在盲目性高,耗时长等问题,本文提出了一种GB-APFB-RRT*算法。仿真实验表明,本文算法搜索时间与RRT算法相比缩短了95%,迭代次数减少了98%,路径节点数减少了32%,路径长度缩短了了20%,验证了本文算法的高效性。本文具体改进思路如下:

(1)从单向搜索改为双向搜索,极大程度上缩短了搜索时间。

(2)加入目标阈值控制采样点生成,减少了搜索的盲目性。

(3)新节点的生长引入人工势场思想,增强了枝叶节点扩展的目的性,使新节点避障障碍物,又接近目标点。

本文提出的GB-APFB-RRT*算法,能极大缩短机械臂路径搜索所需要的时间,提高机械臂的路径规划效率,这为机械臂对路径搜索时长有要求的场景方面提供有益参考。