基于GA-PSO算法的机器人手势交互轨迹规划研究

2019-09-18吕亚辉严雨灵

组合机床与自动化加工技术 2019年9期

吕亚辉，么娆，严雨灵

(上海工程技术大学航空运输学院，上海 201620)

0 引言

随着机器人技术与人机交互技术的发展，将手势识别应用于工业机器人的路径规划已经可以实现。传统机器人示教的过程复杂、操作繁琐对操作人员的要求较高，基于自然手势交互获得机器人轨迹具有简单、学习成本低使用方便的优点，并可以降低对操作人员的要求。如陈畅等[1]使用Leap Motion传感器获取手势坐标动态，以使机器人运动，实现移动、抓取和放置等动作。

而初始手势轨迹路径离散性较大，所得轨迹具有运行时间慢且冲击较大，为提高工业机器人的工作效率和稳定性，可以对基于手势所得运行轨迹进行时间-冲击最优轨迹规划。付荣等[2]使用遗传算法进行时间最优轨迹规划，使用两个适应度函数寻找最优3-5-3插值多项式，得到了时间最优的轨迹。但是该方法并未考虑冲击，其结果中可以发现加加速度有很大突变，具有较大的冲击，不仅对机器人的工作精度有影响，还会缩短使用寿命[3]。为了解决这种问题，文献[4]中进行最优冲击轨迹规划，采用S型曲线对B样条进行插补，冲击明显减小。除此之外为了在要求运动稳定性的前提下寻找最优时间轨迹。文献[5]研究了时间-冲击最优轨迹规划，首先通过最优时间轨迹规划得到最短的运行时间，并在最短的时间内用自适应遗传算法搜索最优冲击轨迹，该方法在保证速度的情况下减小了冲击。Lin[6]使用PSO算法和类聚合算法在关节空间进行规划，通过优化轨迹中所安插的节点求得最优冲击轨迹。尤田等[7]提出使用GA-PSO算法对机器人的焊接路径规划，证明了其较强的搜索能力。

针对手势轨迹问题，本文提出使用改进GA-PSO算法进行时间-冲击最优轨迹规划，改进手势轨迹，以更好地适用于工业机器人，算法采用自适应的惯性权重和交叉变异概率，相比于GA-PSO算法收敛速度更快。使用5-5-5插值多项式对保证机器人关节空间的速度、加速度、加加速度的连续，并于Epson机器人上进行验证，机器人轨迹在不降低时间的情况下冲击大幅减少。

1 手势获取坐标

本文使用Leap Motion(图1)进行手部运动捕捉，获取手和手指的位置手势和动作。本文取长100mm、宽为80mm的Z型线，以食指沿下半部分反Z型为轨迹，其轨迹信息如图1所示(由于设备为自下而上获取坐标，坐标信息为镜面反射关系)，其与获取坐标信息消除z轴如图2所示。

图1 Leap Motion手势提取设备

图2 提取手势坐标信息

由于Leap Motion的内置坐标系与机器人不同，故本文基于手势坐标的相对坐标确定机器人轨迹，以机器人原点作为手势第一点坐标出发进行相对运动，由下式1分别获取机器人x和y方向上坐标的相对位移。

(1)

其中，Xi、Yi为修改前坐标，Xt、Yt为修改后坐标，L和W分别为模板的长和宽，根据坐标范围比例以获得机器人坐标。

以EPsonC3机器人为模型，使用机器人工具箱仿真(相对于原模型10：1缩小)，将修改后坐标进行逆解可得连续坐标的六轴角度，可驱动机器人模型以手势轨迹运动，其结果路径如图3所示。

图3 机器人工具箱仿真

为使手势轨迹能够应用实际生产，需消除硬件设备的不稳定性和操作者的不连续性，一般情况下，可以对带有噪声的信息进行滤波[8]过滤产生误差的帧，以保证手势控制的精度。而随着机器人视觉的发展，通过机器人所获图像处理反馈到伺服控制器修改原有轨迹[9-10]可以完成复杂轨迹的再现。手势坐标再归一化后所获取坐标可直接应用于机器人运行，可以获得机器人沿手势所走路线的机器人运行轨迹。

对于工业生产中，如图2类似的生产任务需要高频率与速度，若按照未经优化的轨迹会产生振动和磨损，本文使用手势识别获取坐标，并根据生产需要选取关键点，对轨迹中关键点进行时间-冲击最优轨迹规划，最终获得可以通过关键点的高速低冲击的轨迹。本文轨迹以取起点、终点和两处拐点四点关键点为例进行轨迹规划，获取基于手势轨迹的时间-冲击最优轨迹。

2 最优轨迹规划

自然手势识别可以应用于机器人的自动化编程中，除上节完全应用手势路径外，有时还需要对于工业机器人点对点轨迹规划中，基于手势规划机器人关键路径点，并对其进行最优轨迹规划。在无特定路径要求下，通常根据始末位置进行插补算法以得到轨迹[11]。

2.1 插值函数的构造

在考虑时间与冲击最优情况下，5次多项式可以通过多阶导数获得曲线的速度、加速度、加加速度，为保证4处关键点的3段运行时间段连接处的冲击连续，本文采用5-5-5插值多项式曲线，并使用智能算法搜索满足冲击约束的多项式系数。付荣等[2]使用3-5-3多项式插值曲线对时间最优轨迹规划，但其两段时间相连接处的冲击未考虑在内，本文构造5-5-5插值多项式，其中5次多项式可以保证轨迹连接处的加速度连续，其多项式通式为：

(2)

(3)

(4)

其中，Xij为第i个轴的第j段的角度，在点对点的轨迹规划中起点和终点的角度、角速度、角加速度已知，且路径点Xi20、Xi30已知，则可根据时间t1、t2、t3获得多项式系数，并由角度位移的5次多项式求导计算该段时间内的速度、加速度和加加速度。

根据每段时间段的对应关系则可有下列矩阵与系数a的关系式，如式(5)～式(7)。其中a为5次多项式系数，r0、r1、r2、r3分别为该轴起点、第一点、第二点、终点的角度，由于矩阵的特性要求每步运算时需要考虑矩阵A的可逆性。

(5)

b=[0 0 0 0 0 0 0 0r3 0 0 0r0 0 0 0
r2r1]

(6)

A*a=b

(7)

2.2 GA-PSO算法

粒子群优化算法是一种基于群体智能的全局进化优化算法，模拟鸟群觅食行为，种群个体之间通过相互协作来寻求最优解，每个优化问题的潜在解都是搜索空间中的一个粒子。每个粒子具有位置和速度两个特征，粒子的位置坐标对应的目标函数值即可作为该粒子的适应度，算法通过适应度来衡量粒子的优劣。在每一次迭代中，粒子通过跟踪两个“极值”来更新自己。第一个极值就是粒子本身所找到的最优解，叫做个体极值pbest；另一个极值是整个种群目前找到的最优解，这个极值是全局极值gbest。在找到这两个最优值时，粒子根据如下公式更新自己的速度和位置：

vt+1=W*vt+C1*rand*(pbest-xt)+C2*rand*(gbest-xt)

(8)

xt+1=xt+vt+1

(9)

其中，W为惯性权重，是上次粒子速度对此次粒子速度的影响程度，Kennedy提出作为收缩因子，使其随迭代减小粒子震荡，从而加快收敛速度。对于惯性权重，本文采用随迭代次数而自适应变化的时变权重：

(10)

其中，i为当前迭代次数，T为总迭代次数，当k>0时，其次数比的平方为由0～1的凹曲线，即惯性权重W为由1～0.3的凸曲线，则惯性权重在迭代前期受上次粒子速度影响较大而在迭代后期影响程度迅速减小，可一定程度防止粒子群过早收敛，且不影响迭代后期的搜索精度。

为寻找最优时间-冲击轨迹规划，Gasparetto等[12]给予每个优化目标相应的权重，将其转化为单目，通过调整时间、冲击两个优化目标的权重，获得了保证冲击较小的同时保证了速度的提高，运动时间有较大缩短。文献[13]中将时间、加速度和冲击作为对象进行混合最优轨迹规划。本文对粒子的加加速度与最大加加速度约束的差值加权，并与总时间求和作为适应度函数，如式(11)所示。

(11)

而当该次迭代的全局最优解不满足约束，则需对该单维时间段进行修正调整，以满足约束条件的最短时间为全局最优解进入下次迭代，其修正函数如式(12)所示。

(12)

粒子群算法搜索速度快、精度高，但易陷入局部最优解，遗传算法使用选择、交叉、变异操作产生子代，在加入粒子群算法后，粒子群可以很好的保留粒子的多样性，有效避免陷入局部最优解。本文在粒子群优化算法后嵌入遗传算法交叉、变异操作保留粒子群的多样性。为了不影响迭代前期粒子群优化算法的寻优性能，采用随迭代次数增加而增加的自适应交叉变异概率。

(13)

式中，p为交叉或变异概率，在实验过程中粒子群算法一般在三分之一迭代处迅速趋于最优，故在此之前取较小的交叉、变异概率，并在之后逐步增加至所设定的最大交叉、变异概率。

具体算法流程如下：

(1) 初始化粒子；设定粒子数和其维数，粒子以3段时间t组成三维粒子；

(2)计算粒子的适应度；根据粒子的3段时间带入式(4)～式(6)可求出系数向量a，并由此带入式(1)～式(3)分别求出3段时间内的最大速度、最大加速度、最大加加速度，以式(11)为适应度函数求取个体极值pbest和全局极值gbest；

(3)判断全局最优解是否超出最大约束，若未超出最大约束，则以式(11)为适应度函数对3段时间以式(7)～式(8)进行迭代，求得新的粒子，并对新的粒子进行交叉、变异操作，并对所得结果与原粒子群所得结果的适应度值进行比较，选取适应度值较低的为当前迭代所得粒子群；若超出最大约束，则对超出的该单维时间段进行以式(12)为适应度函数迭代选取新的满足约束的全局最优解；

(4)判断是否达到终止条件，若否，则返回(2)；

(5)以满足pipper准则的机器人为例，前3轴可确定机械手末端位置，则根据路径规划是否有姿态变化选择6轴角度或3轴角度优化，并分别对所需求解的6个轴或3个轴求取最优解，并选取每段时间的最大值作为最终最优时间，以最终3段时间带入各轴轨迹点要求内，求得最优轨迹。

3 仿真实验

本文以EpsonC4机器人为对象仿真验证，以该机器人运动范围与约束为基础，寻找满足约束的最优时间-冲击轨迹，具体约束如表1所示。

表1 机器人约束范围

选取第一节中的轨迹拐点为例，即取坐标(241,-37,0)，(385,-34,0)，(200,38,0)，(400,36,0)。以第一点为起点，以最后一点为终点，其姿态坐标保持不变，对6轴关节角度寻优，以图2中机器人模型姿态保持4点姿态不变，可逆运动学求解得4点位姿状态下的6轴关节角度如表2所示，其中第4轴为主要控制Z轴旋转，该规矩未涉及Z轴旋转，故为0。

表2 关节空间坐标点

分别使用PSO算法、GA-PSO算法和改进GA-PSO算法对对该问题求解，均设置30个粒子，其中PSO算法设置惯性权重W=0.7；GA-PSO算法使用固定的惯性权重0.7、且其交叉变异概率不变，分别为0.7和0.5；改进GA-PSO算法，设置30个粒子，前期交叉变异概率使用0.1，满足条件后，增大概率，交叉概率最大为0.7，变异概率最大为0.5，并以式(9)设置惯性权重，其中设置k=1.5，在迭代次数为30的情况下其搜索第一轴最优结果如图4所示。

图4 迭代收敛效果

如图4所示，其中PSO算法所得机器人运行3段总时间的最优时间为1.6447s；GA-PSO算法所得最优时间为1.6181s；改进GA-PSO算法所得最优时间为1.5481s。由上图可知，PSO算法搜索速度快但单纯PSO算法不能搜索到全局最优值，易陷入局部最优，GA-PSO算法在PSO算法的基础上增加后期粒子的种群多样性，但前期寻优能力不如改进GA-PSO算法，由于后者在迭代前期减小交叉变异概率，充分发挥粒子群的速度优势，而在后期增加交叉变异概率，增加了种群多样性，使之拥有较好的寻优性能。

将改进GA-PSO算法应用于机器人6轴关节上，以速度为πrad/s、加速度为10 rad/s2、加加速度为50 rad/s3为约束，时间与冲击的权重比为Wt:Wv=3:7，对6轴3段时间进行寻优，分别求得6轴3段最优时间，算法寻优迭代后最终结果如图5所示，其中角度以弧度为单位表示。

(a) 前3轴角度、速度、加速度、加加速度最优曲线

(b) 后3轴角度、速度、加速度、加加速度最优曲线图5 约束下最优时6轴曲线

如图5所示6轴速度、加速度、加加速度均在约束内，每段时间选取6轴中最长时间，获得3段时间：0.8395s、0.7017s、1.1492s，总时间2.6903s。其中第4轴角度未变化，第3个关键点处加加速度有突变，但大小均未超过40rad/s3，冲击较小。

将所得3段最优时间带入式(5)～式(7)，求得6轴最终轨迹，其机器手末端轨迹路线如图6所示，其中图6a为原手势轨迹机器人末端空间路线，路线处于同一平面内；图6b为冲击优化后的轨迹，为空间曲线，Z轴存在5mm内的偏移，将曲线映射到机器人，镜面反射重新恢复Z型，并将空间曲线的路径在EPSON RC软件C3 Sample中仿真，结果如图6c所示。

(a) 原手势轨迹 (b) 冲击优化轨迹

由图6结果可知，在第3点处，牺牲部分冲击以节约时间，在关节空间中，6个轴的速度、加速度、加加速度在约束范围内得到2.69s的经过4处关键点的优化后的轨迹结果，结果表明所得轨迹平滑，不超过冲击约束。增加关键点可以增加与原轨迹匹配度但冲击优化效果降低，本文以4关键点为例，关键点数量应随实际情况而定。

4 结论

本文使用Leap Motion获取手势轨迹信息，除原轨迹运行外研究了对原轨迹选取关键点进行优化，并获取时间-冲击优化轨迹的方法。其中遗传粒子群算法结合粒子群的搜索速度和遗传算法解决离散问题的优点，以较快的速度收敛于全局最优解，本文在此基础上引入自适应惯性权重和交叉变异概率，在根据迭代次数与适应度值改变参数，应用于机器人时间-冲击最优轨迹规划中，获得了比一般GA-PSO算法更快的收敛速度和更短的约束内总时间。