复杂多场景下机械臂避障运动规划方法研究

宋勇, 张蕾, 田荣, 王晓华

1.西安工程大学电子信息学院， 陕西 西安 710048；2.陕西人工智能联合实验室（西安工程大学分部）， 陕西 西安 710048

工业机械臂因自身定位精度高、稳定性强和执行效率高等优异性能,广泛应用在装配、焊接、分拣等工业生产领域中,是推动智能化工业生产的重要工具[1]。考虑到其工作环境复杂多样,研究多场景情况下的避障运动规划具有重要的工程价值。

机械臂在传统运行操作上依赖于工人的示教编程,避障规划效率低且适应性差。要解决这一问题,需要机械臂能够自主规划出一条无碰撞路径以适应不同的环境。机械臂运动规划是根据起始点到目标点的信息,搜索出一组满足碰撞检测条件的关节角度信息,驱动机械臂从初始点运动到目标点,顺利完成作业任务。采用高性能路径规划算法,是实现机械臂高效、安全、柔顺作业的关键。

目前,路径规划算法的研究主要分为2类,按照是否在构型空间中显示障碍物区域,分为组合运动规划算法和基于采样的运动规划算法。组合运动规划算法包括人工势场法[2]、蚁群算法[3]、遗传算法[4]、A*算法[5]、Dijkstra算法[6]等。其中人工势场法在全局路径规划上非常有效,但是存在局部极小值;蚁群与遗传算法运算量太大且耗时长;A*与Dijkstra算法需要较大空间存储环境信息,在低维空间中的路径规划这些算法有一定优势,但对于在高维空间中执行作业任务的多自由度机械臂则不太适用[7]。

基于采样的路径规划方法通过碰撞检测模块,不需要显式构建整个配置空间和边界,主要算法包含概率路图法(PRM)和快速扩展随机树算法(rapidly-exploring random tree,RRT)[8],适用于高维空间和复杂约束下的运动规划问题。其中最典型的是基于采样的快速扩展随机树,此算法具有搜索速度快、探索能力强、概率完备性等优点[9-10],且不需要对障碍环境进行建模,能有效解决复杂的高维空间规划问题,但也存在规划路径长度不是最优、随机性大等缺点。Karaman和Frazzoli于2011年提出渐进最优性(最短距离)的RRT*算法[11],该算法在RRT的基础上增加了父节点择优过程和邻节点回溯重构过程,使规划的路径具有渐进最优性,弊端是收敛速度慢,收敛时间逐步增长,产生的冗余节点过多[12],以及在避障过程中所规划出的路径表现为锯齿状,并不平滑,从而影响机械臂的平稳运行[13]。文献[14]中生成的新节点以一定的概率向目标方向扩展,提升节点扩展的导向性,同时对所规划的路径进行分段优化,优化了路径曲率。文献[15]提出改进的双树拓展策略RRT-connect算法,通过三角不等式的重接线方法,提高了搜索效率。文献[16]将双向人工势场与RRT*结合提出P-RRT*算法,缩短路径长度优化搜索效率,但是势场构建可能会导致局部最优。针对陷入局部最优问题,文献[17]引入回归机制策略与RRT算法结合,可以逃逸极小值点。上述研究极大推动了机械臂自主路径规划的进程,然而在未来可预见的复杂多场景下适应性受限。

综上所述,为了解决RRT*算法在不同复杂的二维和三维等多场景下,规划效率不高、冗余节点多、路径曲折程度大,导致机械臂运行不平稳的问题,本文提出一种启发式概率融合人工势场的改进型RRT*算法(PAPF-RRT*),并在剔除冗余节点后进行B样条插值平滑优化,规划出一条满足运动学约束的平滑路径,使机械臂可以平稳运行。

1 机械臂建模与碰撞检测

1.1 机械臂运动学原理及其模型构建

本文以工业六自由度机械臂puma560为研究对象,其URDF模型如图1所示,其D-H参数如表1所示。

表1 D-H参数表

图1 puma560机械臂模型

表1中各个参数的含义为当前关节标号为i,逐次递推其余关节标号为i-1,di表示2个相邻连杆之间的距离,ai-1代表连杆长度,αi-1代表连杆i的扭角,θi代表关节转角的范围,其中d2=148.09 mm,d4=433.07 mm,a2=431.8 mm,a3=20.32 mm。

连杆i相对于连杆i-1的坐标变换通式为

(1)

式中:根据D-H参数及公式(1)可以得出机械臂末端位姿变换矩阵如下所示。

(2)

式中:向量n,o,a代表机械臂末端姿态;x,y,z代表机械臂末端坐标位置;p代表三维坐标。

1.2 机械臂连杆与障碍物空间位置关系的描述

机械臂路径规划的前提是要具备精准有效的碰撞检测。但是在实际运行的环境中,机械臂本体和障碍物都是不规则的物体,两者之间的碰撞检测条件很难设定,因此需要对机械臂和障碍物进行模型简化[18-19]。本文用几何包络法简化机械臂与障碍之间的碰撞模型,用球型包围盒对障碍物进行包络,为了精确逼近空间障碍物的占位,包络球的直径定义为障碍物径向最长的线段距离,其表达式为

(3)

式中:(a,b,c)为包络球的球心坐标;r为包络球的半径;(x,y,z)为障碍物的任意一点坐标。

将机械臂连杆用圆柱体完全包裹,如图2所示,其径向最大包络半径为R1;将障碍物用球体进行完全包裹, 其径向最大包络半径为R2;进而将机械臂与障碍物的碰撞检测转化成判断规则几何体是否相交问题。经过包络化处理,机械臂各连杆与障碍物可以简化为线段与球体的空间位置关系,如图3所示。根据末端位姿的信息,通过逆运动学求出各个关节的最优逆解,再进行正解求出空间位置坐标,即连杆AB的状态方程。设点Q到线段AB的直线距离为dmin,设R=R1+R2,根据线段AB与球体的空间位置关系归纳如下几种情况:

图2 模型简化示意图

图3 碰撞检测判断示意图

1) 如图3a)所示,dmin>R,则不发生碰撞。

2) 如图3b)所示,dmin

3) 如图3c)所示,dminR并且|QB|>R,点A、B在点Q的两侧,有2个交点,则发生碰撞。

4) 如图3d)所示,dminR并且|QB|>R,点A、B在点Q的同侧,无交点,则不发生碰撞。

2 基本算法原理简介

2.1 RRT及RRT*算法原理简介

快速扩展随机树算法(RRT)是一种基于随机采样的单步查询搜索算法,对环境障碍物无需建模,因此很适合高维空间路径规划。

RRT*算法在RRT节点扩展的基础上,增加了路径代价函数,依靠父节点优选过程和邻节点回溯重构,重新计算新节点,具备渐进最优性,且收敛到当前最优,具体过程如图4所示。

图4 父节点优选过程

父节点择优选择过程,如图4所示,其目的是通过邻域r内某个备选节去替换新节点,从而降低新节点的路径代价,并更新其权重。

邻节点回溯重构过程,如图5所示,在没有发生碰撞的情况下,若存在其他节点以Xnew做为父节点并索引至根节点的代价减小,则用Xnew替换该节点作为新的父节点。

图5 邻节点回溯重构过程

2.2 改进型RRT*算法(PAPF-RRT*)

机械臂路径规划问题是满足自身运动学约束和工作空间的条件下,规划从指定起点到目标点的路径,该路径没有发生碰撞,使机械臂能高效完成任务。该问题特征总结为3类:

1) 在复杂受限等特殊环境下,RRT*算法规划效率低,产生过多冗余节点;

2) 尽可能优化规划时间和路径长度;

3) 规划出来的路径必须满足机械臂运动学约束,机械臂才能够平稳运行。

2.2.1 混合约束采样策略

针对RRT*算法在复杂受限环境下搜索效率低的问题以及避免无效区域的搜索,借鉴启发式思想,提出目标偏向采样策略来优化采样过程。预先设置一个目标偏向阈值Pbias(设置为0.3),然后由均匀概率分布随机产生一个概率值P,将随机树扩展分为2个阶段,分别为探索阶段和收敛阶段。核心思想是在随机树生长过程中,如果P≥Pbias,随机树处于探索阶段如图6a)所示,通过RandomState函数随机生成采样点Xsample;随着迭代次数的增加,如果P

图6 概率指数示意图

(4)

如果随机值小于预设的目标偏向阈值,则通过位置约束来优化采样点。在采样过程中建立采样区,每次采样时可将多组采样点放入采样区中,定义采样区中距离目标点最近且生成的节点不与障碍物发生碰撞的点为最优点。建立采样区过程示意图如图7所示。判断条件为:计算当前采样点与目标点的距离是否小于上一个采样点与目标点的距离。若是,则符合判断条件,采样成功;否则,继续循环采样直到满足条件。

图7 采样点优选示意图

图7设置第一个参考采样点为Xsample1,图中按照采样次序以及采样点到目标点的欧氏距离进行大小排序Xsample1>Xsample2>Xsample3

2.2.2 随机树生长优化策略

采样优化函数虽然能提高采样的导向性和收敛速度,但未改变新节点生长过程的盲目性,在环境复杂时可能陷入局部极小点和重复偏置问题,降低算法寻迹效率。本文融合人工势场法,将目标引力函数G(n)和障碍物斥力函数R(n)叠加到随机树节点Xnear上,则随机树节点生长受采样点函数P(n)的约束,同时也受到目标引力和障碍物斥力合力的约束。APF对随机树节点扩展的影响如图8所示,生长函数由(5)式表示。

图8 APF对节点生长的影响

F(n)=P(n)+G(n)+R(n)

(5)

人工势场法是局部路径规划方法,基本思想在目标点设置作用于全局的引力场,对扩展树节点产生引力作用,障碍物设置作用于局部范围的斥力场,对扩展树节点产生斥力作用,引力与斥力的合力方向决定随机树生长方向,从而降低随机树生长的盲目性,加快算法收敛速度。(6)～(7)式为引力场和斥力场势函数

(6)

(7)

Uatt代表引力函数,Ka代表引力常数,Xnear表示随机树扩展的当前节点,Xgoal为目标点,Urep表示斥力函数,Kr为斥力系数,l表示采样点与障碍物的距离,l0为斥力场作用的范围,‖Xgoal-Xnear‖=ρ。

目标点对随机树节点Xnear的引力为其对应引力场的负梯度

FA=-Uatt=-Ka‖Xgoal-Xnear‖Δρ

(8)

由(8)式构造出节点Xnear处的目标引力函数G(n),其中FA表示引力方向上的扩展系数。

(9)

同理,环境中障碍物对随机树节点Xnear产生的斥力为对应斥力场的负梯度

(10)

由(10)式构造出节点Xnear处的障碍物斥力函数R(n),其中P0表示环境中障碍物位置信息,FR表示斥力方向上的扩展系数。

(11)

通过引入人工势场法将斥力与引力叠加到随机树扩展方向上,对节点扩展方向进行修正,引导随机树生长的方向和扩展步长大小,促使每一次扩展过程更接近目标点,加快收敛,增加了算法搜索的灵活性。因此,新节点被重新配置如公式(12)所示。

(12)

式中:δ为扩展固定步长;Xnear为距离采样点最近邻节点;‖Xrand-Xnear‖代表Xrand与Xnear之间的欧氏距离。

2.2.3 自适应步长策略

基于随机树生长优化策略的算法没有有效利用扩展的节点信息,忽略了碰撞概率较小的方向,如果算法继续朝这个方向扩展,成功扩展的可能性会更大。此外,当随机树向目标方向生长时,如果扩展方向上存在障碍物,与随机扩展相比,该策略将显著降低算法的避障能力。因此,本文引入了一种自适应步长策略,新节点的生成方法配置为

(13)

当机械臂的操作环境比较复杂或者有较大障碍物时,融合了人工势场法的RRT*算法,若始终是固定步长进行搜索,则不能有效利用扩展过程中收集到的信息提高寻迹效率。因此,本文采用自适应步长策略,可以有效利用扩展过程中所收集到的障碍物和环境信息,更好适应节点的生长状况,使得算法更快地穿越障碍物较少的区域。在随机树进行拓展时首先采用初始步长,在每一次扩展生成新节点Xnew时,进行碰撞检测,如检测到附近没有障碍物,则调整步长。具体过程:

1) 预设一个φ值,取值范围在[0,1],初值设为零,表示随机树向通过采样获得的节点方向扩展。

2) 当机械臂操作环境比较简单,增大φ值以加快偏向目标生长。但是φ值不宜设置过大,以避免陷入局部最优,根据实际的任务场景,合理设置[20]。

3) 当机械臂操作环境复杂或需要跨过较大障碍物时,降低φ值,从而使扩展步长减小并保持扩展的随机性,使得随机树绕开障碍物生长。

2.2.4 路径优化策略

RRT*算法及其衍生算法在三维空间中搜索得到一条由离散点所组成的连续线段,其路径的形状比较曲折,平滑性极差,产生大量冗余节点,不利于机械臂平稳运行。假设RRT*算法产生的路径节点向量q[q1,q2,q3…qn],对节点qa和qb之间路径进行碰撞检测(a

图9 优化冗余节点原理图

删除冗余节点之后,得到的路径长度较短,路径曲率小,且转折点较少。但在转折点处还需要进行平滑处理[21],以提高机械臂运行时的柔顺性。三次B样条曲线具有局部可控的特点,能较好满足路径规划中对拐点的局部拟合,且不影响整条路径的基本形状,增加了轨迹平滑性,保证了各关节速度、加速度变化连续性,且不发生突变,提高轨迹柔顺性。因此,选用B样条函数构造轨迹,其K阶B样条曲线公式为

(14)

式中:di表示给定曲线的控制点;Ni,k(u)表示K阶B样条基函数。三次B样条基函数为

(15)

式中,u表示均匀节点向量,当给定控制顶点后,可以求出一些列满足B样条插值的空间序列点,从而构造出插值曲线。

3 算法仿真分析

为了验证算法PAPF-RRT*的有效性和可靠性,与RRT*、GB-RRT*算法分别在二维和三维场景下进行仿真与实验分析。

3.1 二维仿真

仿真环境设置为[100,100],起点设为[30,80],目标点为[70,10],随机设置障碍物,对RRT*、GB-RRT*和PAPF-RRT*算法进行100余次仿真实验,迭代次数设置为100,步长设置为5,3种算法的仿真结果和各项指标如图10和表2所示。

表2 二维场景中各算法性能对比

图10 二维环境中各算法性能对比

红色线条表示算法寻迹的最终路径,黑色线条表示拓展树分支。通过对比可以看出PAPF-RRT*算法相对于RRT*、GB-RRT*算法在复杂狭窄环境中生成的节点数较少。通过图12和表2数据对比可以看出PAPF-RRT*算法相比RRT*算法,扩展无效结点数变少,其节点使用率由11.89%提升到35.62%,因此占用的内存更少;在搜索时间层面上由2.874 s降低到1.243 s,降低了约56.75%,提高了路径规划效率;平均路径长度上从124.63 cm缩短到102.51 cm,减少了约17.74%,且路径转折程度较低,有利于路径平滑优化;相比GB-RRT*算法,PAPF-RRT*算法在路径代价上减少了16.41%,节点使用率提高了24.50%,搜索时间上减少了13.5%。相比以上2种算法,本文所提PAPF-RRT*算法在复杂多障碍环境中具有良好的路径规划能力。

3.2 三维仿真

为了验证本文所提算法PAPF-RRT*在三维环境中的有效性,在MATLAB环境下搭建三维避障环境,仿真实验中设置算法搜索步长为5,迭代次数设置为100,设置搜索起点为[20,55,10],目标点为[40,15,40],仿真环境地图设置为[60×60×60]的三维空间,考虑到算法的随机性,重复进行实验100余次,实验结果和各项指标如图11和表3所示。

表3 三维场景中各算法性能对比

图11 三维环境中各算法仿真结果

蓝色线条表示随机树的分支,红色线条表示最终寻迹的路径,如图11所示,本文所提算法PAPF-RRT*在三维空间的采样点比传统RRT*减少了很多,且规划的路径长度短、曲折度低。结合表3中的数据可得,本文所提算法相比基本RRT*算法平均路径长度由42.73 cm降低到32.93 cm,减少了约22.93%;平均运行时间由6.743 s降低到1.548 s,降低了约77.04%;节点使用率由9.54%提升到63.21%,从而说明规划效率的提升,且路径中折点数较少,曲线转折程度较低,有利于路径平滑优化。虽然GB-RRT*算法寻迹的路径代价相对较短,因为目标偏向方法在三维空间中起到良好作用,但是该算法易陷入局部极小。然而本文所提PAPF-RRT*算法,在搜索路径的同时提高了节点使用率,降低路径规划长度,且规划效率较高。

3.3 路径平滑性分析

为了进一步验证本文所提算法PAPF-RRT*规划出的路径是否满足机械臂的运动学约束,将机械臂模型导入,进行运动规划仿真实验。因为PAPF-RRT*算法所规划出来的路径是由空间一系列离散的采样点组成,路径平滑性差,所以,本文采用基于三次B样条对所规划出来的路径映射到关节空间进行插值优化,平滑优化效果如图12所示。

图12 机械臂末端轨迹优化效果

图12表示在局部空间内,机械臂末端路径规划图,其中黑色实线代表未进行插值的路径,红色实线代表经过三次B样条插值平滑处理后末端的路径。由仿真结果发现,插值拟合处理后末端轨迹光滑性明显增强,各关节运动学曲线如图13所示。

图13 各关节拟合后角度、速度及加速度曲线

图13为机械臂避障运行过程中各关节位移、速度、加速度变化曲线,其中各关节的起始速度、加速度和终止速度、加速度数值大小都为零,符合实际工作情况。在平滑处理机械臂末端轨迹的同时,机械臂各关节角度、速度和角加速度3项运动学参数曲线变化连续、稳定,无跳变,降低了各关节在运行过程中的冲击和抖动,保证了机臂在规划路径上的高效、平稳运行,并且各连杆与障碍之间拥有安全裕度,能够满足机械臂的实际应用需求。

为了进一步验证本文所提算法对不同环境的适应性,设置障碍物个数不同的3种环境。仿真结果如图14所示,红色线条表示本文所提算法插值拟合后规划出来的路径,黑色线条表示未进行路径优化的轨迹,可以看出改进后算法规划出来的路径明显光滑,路径中的震荡现象也有效得到改善;并且随着障碍物个数的增加,本文所提PAPF-RRT*依然能够引导机械臂顺利避障,证明了算法的有效性。

图14 算法对环境复杂程度的适应性实验

4 结 论

本文以六自由度工业机械臂在智能工厂操作业务为背景,在不同复杂程度的的多场景中,针对传统RRT*算法进行路径规划时存在随机性强、冗余节点过多、路径不平滑等问题进行改进,提出一种基于PAPF-RRT*结合B样条插值的机械臂运动规划算法。本文主要改进有如下三点:

1) 在随机树扩展过程中引入混合约束采样策略,在采样点偏向扩展的基础上,通过采样区优选策略,提高采样点的导向性与质量。

2) 将节点扩展方向与人工势场合力方向进行融合,使得扩展方向具备目标点吸引和障碍物排斥的趋势,同时引入自适应步长策略,减少扩展的盲目性,加快收敛速度。

3) 通过去除冗余节点并进行二次插值优化,改善路径平滑性,以满足机械臂运动学约束。

仿真结果表明PAPF-RRT*算法在有效避障的同时,提高了规划效率以及节点使用率,使规划的路径长度更优。同时在规划后的路径点间进行B样条插值构造轨迹,提升路径平滑性。导入机械臂模型进行仿真,关节空间的各项运动学参数变化连续、稳定,同时也优化了机械臂末端执行器的轨迹,降低了规划时间,使规划的轨迹更有利于机器人的跟踪执行,具有较好的发展应用前景。