一种求解冗余机械臂逆运动学的自适应动态差分进化算法

东辉， 李晨， 姚立纲， 郭金泉， 孙浩

(福州大学机械工程及自动化学院, 福建 福州 350108)

0 引言

冗余机械臂逆运动学求解是利用机械臂末端执行器位置及姿态反向求解各关节角的过程， 具有非线性、 高耦合的特点， 直接求解存在难度. 目前， 相关求解方法主要分为三类： 几何法、 代数数值解法和智能算法.

几何法通常是通过分析机械臂几何结构， 结合空间几何知识与逆运动学方程来求解逆运动学解. 针对S-R-S构型七自由度机械臂， 胡奎等[1]引入臂角变量， 推导出含有臂角的逆运动学封闭解， 并将关节限位与臂角可行范围进行映射， 定义一种避免关节限位目标优化函数， 最后将臂角和关节角关系进行线性化展开， 提出一种冗余机械臂逆运动学优化算法. Kofinas等[2]把整个NAO类机械臂划分为头部、 两只手、 两只脚一共5个部分， 使用Denavit-Hartenberg(D-H)参数， 对系统非线性方程以及身体和关节对称性进行分析， 提出一种高效、 准确并且能够避奇异的逆运动学求解方法. Faria等[3]通过求取自运动范围边界与当前自运动位置距离最小值来实现避关节限位并求逆运动学解， 可实现避奇异以及轨迹规划. 霍希建等[4]把七自由度冗余机械臂简化结构为肩、 肘、 腕， 新定义避关节限位评价函数， 提出一种关节角评价体制和逆运动学求解算法. 然而， 几何法虽求解速度快、 精度高且不存在误差积累， 但对不同构型机械臂， 几何算法需重新推导计算， 其通用性存在一定局限.

代数数值解法包括雅可比矩阵伪逆法、 增广雅可比法、 梯度投影法、 加权最小范数法等. Ignacy等[5]以雅可比矩阵为基础， 对比雅可比矩阵转置法和雅可比矩阵伪逆法， 此外还基于接近目标点短距离进行评估， 改进Levenberg-Marquardt方法， 提出了可靠且计算量较小的算法. Ayusawa等[6]利用迭代法去迭代梯度向量得到近似逆运动学解， 精度较高且求解速度较快. 东辉等[7-8]首先利用平面冗余机械臂结构参数结合傅里叶变换和卷积定理， 构建非线性工作空间密度函数， 再以该密度函数作为目标函数， 提出一种迭代的逆运动学求解算法. Wan等[9]在原有最小二乘范数法基础上增加一个弹性场函数， 提出一种改进最小二乘范数法. 阳方平等[10]基于加权最小范数法， 推导出一种避免计算雅可比矩阵伪逆优化方法， 通过七自由度冗余机械臂证明了该算法在精度、 避免关节限位等方面效果. 代数数值法尽管精度高且较为可靠， 但计算量巨大且求解过程复杂， 其所需的雅克比伪逆矩阵计算困难.

随着计算机技术发展， 许多智能算法也被应用到冗余机械臂逆运动学求解问题上， 其中很多源于自然生物规律， 比如神经网络算法(NN)、 遗传算法(GA)、 蚁群算法(ACO)等. Köker等[11]利用10组神经网络和对应数据组对一冗余机械臂进行逆运动学求解， 提出一种基于神经网络算法的冗余机械臂逆运动学求解机制. 此外， Köker还分别融合使用遗传算法[12]与模拟退火算法[13]对神经网络算法进行优化并用于冗余机械臂逆运动学求解. Parker等[14]较早使用遗传算法求解逆运动学问题. 谢习华等[15]利用改进差分进化算法求解冗余机械臂逆运动学， 并与基本差分进化算法进行比较. Huang等[16]利用粒子群算法求解七自由度冗余机械臂逆运动学解， 展示了粒子群算法强大粒子间合作及更新能力. Bayati等[17]提出基于布谷鸟算法和帝国主义算法的混合方法解决机械臂逆解问题. 智能算法具有精度高且速度较快等优势. 但是， 对于冗余机械臂， 常规启发式搜索算法易陷入局部搜索， 难以寻得最优解， 最终将影响逆运动学求解精度. 由此可知， 尽管多种方法成功应用于冗余机械臂逆运动学求解中， 但仍有完善空间.

本研究针对差分进化算法易陷入局部搜索等问题， 在基本差分进化算法模型基础上引入自适应变异操作与随机变动交叉操作, 改进了逆运动学算法适应度函数与边界处理方法， 根据迭代进程调整搜索强度. 同时, 兼顾机械臂末端执行器位置与姿态两类误差， 借助改进后的自适应动态差分进化算法在关节空间范围内进行可行解搜索， 以达到高效求解冗余机械臂逆运动学的研究目标. 应用KUKA LBR iiwa七自由度机械臂模型， 采用自适应动态差分进化算法对其进行逆运动学求解， 在仿真实验中重点研究和定量评价了求解精度和稳定性. 计算结果表明， 位置求解精度最高可达10-7mm， 姿态误差不超过10-5rad， 200次迭代计算仅需0.93 s. 在30次重复定位实验中， 最低位姿综合误差和最高位姿综合误差分别是10-7和2.5×10-5， 平均位姿综合误差为1.6×10-5. 求解精度和稳定性结果数据验证了方法可行性. 此外， 本方法具有良好普适性， 仅由机械臂D-H参数即可求解机械臂逆运动学问题， 而无需机械臂结构分解与处理. 因此， 本方法可推广至多领域内相关机械臂运动分析与控制研究.

1 机械臂运动学分析

机械臂运动学包含正运动学以及逆运动学， 正运动学是利用已知的各关节角值求取机械臂末端执行器位姿. 使用D-H方法建立机械臂模型， 以KUKA LBR iiwa 七自由度机械臂为研究对象， 如图1所示， 其D-H参数如表1所示.

图1 KUKA LBR iiwa机械臂及其运动学简图Fig.1 KUKA LBR iiwa manipulator and the schematic diagram

表1 机械臂D-H参数

在机械臂每个关节处建立坐标系， 转角α表示绕Z轴转动量， 关节角θ代表绕X轴旋转量， 偏距d和连杆长度a分别代表沿X，Z轴移动量， 相邻连杆位姿变换可用齐次变换矩阵i-1Ti表示：

(1)

其中： 末端执行器位姿由所有关节共同作用产生， 故机械臂末端执行器在基座标中的位姿0T7可以表示为：

(2)

式中：Px，Py，Pz分别表示末端执行器在基坐标系中X，Y，Z方向的位置.为简便表示姿态， 采用Z-Y-Z欧拉角进行定义， 即按顺序围绕基坐标系中Z，Y，Z轴分别旋转α，β，γ三个欧拉角得到最终姿态， 欧拉角计算方法如下式所示.若已知机械臂各关节角度值， 代入运动学方程中即可求得末端执行器在此关节角组合情况下的位姿 (x,y,z,α,β,γ).

(3)

逆运动学求解则是根据末端执行器位置与姿态反向求解关节角， 与正运动学不同， 冗余机械臂逆运动学解并不具有唯一性. 由公式(2)、 (3)机械臂自由度至少含有七个自由度， 则方程数目至少为7， 故运动学方程中自变量数量大于因变量数目， 从而导致逆运动学多解现象， 给冗余机械臂逆运动学的求解造成一定难度.

2 差分进化算法

2.1 基本差分进化算法

差分进化算法(differential evolution algorithm， DE) 由Storn等学者于1995年提出， 属于一种仿生算法， 采用基于群体的全局搜索策略， 利用实数编码， 具有较强全局收敛能力以及鲁棒性， 常用于解决非线性、 不可微等类型的问题. 目前差分进化算法已经广泛应用在化工、 电力、 信号处理、 机械设计等领域. 差分进化算法是基于实数的演化算法， 主要步骤包括种群初始化、 变异、 交叉、 选择， 其主要思想是在变化量范围内随机生成新的种群并选择若干个体作为交叉、 变异操作对象， 产生新个体， 接着对新个体进行适应度计算， 直至找到适应度值最符合要求的个体， 从而获得最优解. 算法进化流程可分为以下几个步骤.

1)种群初始化. 种群由一定数量个体组成， 每个个体可以看作是问题的一个解， 并且个体维度与所求问题维度需保持一致. 假设待解问题包含m个自变量， 则个体维度也为m.若要生成一个包含N个个体种群， 且其中每个个体都是有m维， 种群初始化操作如下所示：

X(i,j)=Xj, min+(Xj, max-Xj, min)×rand(0, 1)

(4)

其中：i=1, …,N;j=1, …,m;X(i,j)表示种群中第i个个体第j维;Xj, min和Xj, max分别表示j维度取值上限与下限; rand(0, 1)表示在[0, 1]内随机取值.

2)变异. 变异目的是产生与初始种群不同个体， 增加种群个体多样性， 可增加寻找最到优解的概率和速度. 变异是在已有个体中随机取若干各异个体进行相互运算， 从而产生新个体， 采用DE/best/1/bin变异机制， 其变异方式为：

Vi=Xbest+F(X1-X2)

(5)

式中：Vi表示第i代一个新个体；Xbest是当前种群中最符合适应度要求的个体；X1，X2是与Xbest相异的个体；F是变异因子， 用于控制偏差大小， 一般有0 ≤F≤ 2.虽然每个个体都是在取值范围内随机生成， 但经过变异操作后有些个体可能已经超出了取值范围， 所以针对每个个体的每个维度都需要进行边界处理， 常规边界处理一般是以超出边界粒子的最近边界值替代， 具体操作为：

(6)

3)交叉. 变异是基于个体整体进行的运算操作， 可以一定程度上增加种群多样性， 但这种方法仍然不够灵活. 为进一步扩大种群多样性， 差分进化算法还可以在个体维度层面进行交叉互换， 通过改变个体一部分的方式来产生新个体， 交叉的控制更依赖于交叉因子设置， 将其与随机数进行对比， 更具有随机性， 可有效增加个体多样性， 交叉操作如下所示：

(7)

式中： CR为交叉因子， 取值区间为[0, 1]；Ui, j，Vi, j，Xi, j分别是三个不同个体在i个体第j维度值.

4)选择. 差分进化算法根据贪婪原则， 从每个种群中选择出最佳个体， 再从每次迭代的最佳个体中选择出最优个体， 最终在指定迭代次数内或者精度要求范围内得到最优解. 选择依据是个体适应度函数值大小， 在最小化问题中一般选择适应度函数值最小个体作为最优解， 最大化问题则通常筛选出适应度函数值最大个体. 以最小化问题为例， 适应度函数值越小， 说明越符合需求， 选择方式如下：

(8)

2.2 自适应动态差分进化算法

在实际情况中， 基本差分进化算法易于陷入局部搜索， 其原因在于算法搜索方式一成不变， 随着迭代进行， 算法一直按照固定方式进行搜索， 以至于最终可能陷入局部最优而找不到全局最优解位置. 所以本研究引入了自适应变异因子以及随机变动交叉因子， 使算法随着迭代进行而调整搜索强度， 提出一种自适应动态差分进化算法.

2.2.1自适应变异因子

变异因子F是控制种群多样性以及收敛性的重要参数， 基本算法变异因子通常取值为常数， 导致算法不够灵活.若F取值过大， 则会导致算法收敛速度变慢且收敛精度降低； 若F过小， 则种群多样性不够， 容易出现收敛过早现象.为取得良好收敛效果， 应该使算法先进行大跨度搜索再小范围搜索， 即变异因子应随着迭代进程减小， 逐渐逼近最优解， 故引入如下随迭代进行而变化的自适应变异因子：

(9)

其中：F0为变异因子初始值；G为最大迭代次数；g为当前迭代次数.

2.2.2随机变动交叉因子

交叉因子CR控制个体各维参与交叉操作程度以及全局种群多样性， 基本算法交叉因子通常取固定值， 引入随机变化机制可增加交叉随机性， 从而增加种群多样性. 随机变动交叉因子操作如下：

CR=0.5×(1+rand(0, 1))

(10)

综上所述， 自适应动态差分进化算法流程如图2所示.

图2 自适应动态差分进化算法流程图Fig.2 Flow chart of the adaptive dynamic differential evolution algorithm

3 冗余机械臂逆运动学求解

冗余机械臂逆运动学问题具有高度非线性、 高耦合等特点， 机械臂每个关节都有其自身可运动范围， 需要根据逆运动学求解问题实际特点， 对差分进化算法进行适当修改. 通过分析使用自适应动态差分进化算法求解七自由度机械臂逆运动学实际效果， 发现以下两个问题： 1) 位置误差和姿态误差两者收敛效果差距大， 往往位置收敛精度比姿态精度高出4～5倍， 即出现了两种误差收敛不平衡现象， 同时由于各欧拉角之间存在耦合， 导致姿态收敛发生漂移. 2) 常用边界处理方法使得最后得出的结果中有些维度取值是边界极值， 不符合机械臂实际使用要求， 当关节角取极值时容易造成机械臂抖动， 导致机械臂工作不稳定. 针对这些问题， 对适应度函数以及边界处理进行改进， 适应度函数主要针对平衡对位置误差和姿态误差， 边界处理将避免关节限位纳入考虑.

3.1 适应度函数设计

冗余机械臂逆运动学问题可视为末端执行器位姿误差最小化问题， 位置误差用ΔP表示， 姿态误差用ΔR表示：

(11)

其中： Δx， Δy， Δz分别为X，Y，Z方向位置误差值； Δα， Δβ， Δγ为姿态误差.

位置误差与连杆长度有关， 姿态误差与关节角度相关联， 关节角度采用弧度制， 所以弧度制与机械臂连杆长度具有一定倍数关系. 正是这种数量关系的存在， 使两种误差收敛不平衡， 分别为均衡位置误差与姿态误差收敛. 改进适应度函数采用加权处理方法：

f=ωp·ΔP+ωr·ΔR

(12)

分析可知弧度制与连杆单位具有一定倍数关系， 当设置ωp=1，ωr=180/π时对位置误差和姿态误差具有良好平衡效果. 对KUKA LBR iiwa机械臂模型实验结果进行分析， 由于表示冗余机械臂姿态的欧拉角之间存在耦合关系， 导致欧拉角α的拟合值始终包含0.43 rad的固定漂移增量， 故在计算过程中对欧拉角α进行漂移量补偿：

(13)

3.2 关节边界处理

常用差分进化算法对超出边界的个体是用边界值替代， 实际情况中， 机械臂关节角取边界值不利于机械臂控制和有载工作， 会造成机械臂异常抖动使得工作稳定性下降， 所以为尽可能避免解中包含关节角边界值， 对于迭代过程中超出边界的个体进行处理， 使其重新从取值范围中点开始取值， 从而尽可能地避免边界取值， 关节边界处理操作如下：

(14)

4 仿真实验

为验证算法各方面性能表现， 以KUKA LBR iiwa 七自由度机械臂为实验对象， 实验平台基于Windows 10系统， 硬件条件为Intel Core i7-8700K @ 3.7 GHz， 内存大小为16 GB. 利用自适应动态差分进化算法进行逆运动学求解， 基于Matlab进行了三类仿真测试： 算法收敛性测试、 算法稳定性测试， 算法对比测试， 所得结果证明了算法在收敛性以及稳定性方面都具有良好效果， 适合用于冗余机械臂逆运动学求解.

4.1 算法比较测试

为验证自适应动态差分进化算法改进后的收敛性和稳定性， 把本算法与基本差分进化算法进行比较， 对同一点位姿进行逆运动学求解. 适应度函数由位置误差与姿态误差组成， 为体现综合性能， 以适应度函数值为评价标准， 分别从收敛性与稳定性两方面进行比对.

4.1.1收敛性比对

对于位姿(400, 500, 600, π/2, π/4, π/3)， 设置自适应动态差分进化算法初始变异因子F0为2， 分别使用两种算法迭代200次进行逆运动学求解， 对比两种方法适应度函数值如图3所示. 可以发现, 自适应动态差分进化算法最终收敛曲线位于基本差分进化算法下方， 适应度函数值最终收敛至1.64， 得出解为(399.941 3, 500.015 6, 599.957 7, -0.138 8, 1.114 0, 1.047 3)， 与目标位姿相差较大， 尤其是姿态误差最大. 改进后算法适应度函数收敛值为1.53×10-5， 得出解为(399.999 9, 500.000 1, 600.000 0, 1.570 8, 0.785 4, 1.047 2). 将两者结果进行对比可发现， 本算法在精度方面明显优于基本差分进化算法. 说明改进后算法在收敛性方面优于基本差分进化算法， 收敛曲线下降趋势更大， 且基本差分进化算法收敛速度一直慢于自适应动态差分进化算法， 体现了本算法优越性.

图3 收敛性对比曲线Fig.3 Convergence plots of the two algorithms

4.1.2稳定性对比

稳定性测试采用重复定位的方法， 同样对位姿(400, 500, 600, π/2, π/4, π/3)求解， 分别使用两种算法迭代30次计算， 记录每次定位的适应度函数收敛值并对比. 基本差分进化算法最大和最小收敛值分别是2.45和1.12， 平均收敛值为1.65， 收敛值变化幅度非常大. 本算法适应度函数收敛值则一直保持在1×10-5附近， 两者收敛值曲线对比如图4所示. 由图4可见， 基本差分进化算法不仅收敛值高于本算法， 收敛精度波动幅度也更大， 证明了改进后方法具有更好的稳定性.

图4 稳定性对比曲线Fig.4 Stability curves of the two algorithms

4.2 自适应差分进化算法性能测试

4.2.1求解精度测试

对于KUKA LBR iiwa机械臂末端执行器在工作空间内的目标位姿 (400, 500, 600, π/2, π/4, π/3)，设置自适应动态差分进化算法初始变异因子F0为2， 迭代次数G为200次， 对这一位姿进行逆运动学求解. 计算得到的一组解为(0.127 7， 0.945 3， 1.184 0， 1.331 4， -1.405 0， -1.148 4， 1.850 0)， 适应度函数值为1.4×10-5. 为验证结果准确性， 把这组关节角带入到正运动学方程中， 得到此时末端执行器位姿为(399.999 9, 500.000 1, 600.000 0, 1.570 8, 0.785 4, 1.047 2)， 位置最大误差仅为0.000 1 mm， 姿态误差为0 rad， 说明本方法具有较高逆运动学求解精度与计算效率. 计算过程中适应度函数值以及误差曲线如图5～7所示. 分别使用ε，η代表位置误差和姿态误差, 假设工作场合所需位姿综合精度为10-3(图中用红色虚线表示)， 则全局在第73次迭代就已满足位姿精度要求， 单独位置精度和姿态精度分别在第82次和第43次达到要求精度. 说明改进后的算法不仅提高了姿态收敛精度， 也加快了收敛速度.

图5 适应度函数收敛曲线 图6 位置误差收敛曲线 图7 姿态误差收敛曲线

4.2.2稳定性测试

差分进化算法属于随机搜索算法， 其搜索过程具有一定随机性， 故算法计算效果也具有相应随机变动， 这种计算精度波动情况可用算法稳定性来衡量， 稳定性越好， 多次计算平均精度更低且不同次数间的计算精度差别更为合理. 为检验本算法稳定性， 实施了重复定位实验， 对(400, 500, 600, π/2, π/4, π/3)这一位姿进行了30次重复定位计算并比较适应度函数值变化情况. 在30次计算中， 最低和最高适应度函数值分别为1.0×10-7和2.5×10-5， 平均适应度值是1.6×10-5， 适应度函数值变化曲线如图8所示， 在多次重复定位测试下， 适应度函数值波动小， 未发生较大数值偏差或陷入局部搜索现象， 稳定性表现良好.

图8 重复定位适应度曲线Fig.8 Fitness curve of repeated positioning

4.2.3多点轨迹跟踪测试

实际工作中， 机械臂末端执行器通过掠过由多个点组成的工作路径来完成动作， 为测试算法对连续多点的计算效果， 选定螺旋线和蝴蝶曲线两种不同曲线用于轨迹跟踪实验， 函数表达式分别为:

x=cos(t)+400；y=sin(t)+500；z=t

(15)

{y=sin(t)(ecos(t)-2cos(4t)-sin5t12)+500

z=cos(t)(ecos(t)-2cos(4t)-sin5t12)+500

0≤t≤12

首先在曲线上按固定步长选取若干计算点， 螺旋曲线和蝴蝶曲线分别均匀选取100个和500个计算点， 把所有计算点代入计算， 将输出计算结果点与标准轨迹进行对比. 计算结果表明， 螺旋线拟合精度最低为2×10-4mm， 最高精度1×10-6mm， 平均精度为1×10-5mm， 蝴蝶线轨迹计算平均精度为1×10-5mm. 对比图如图9， 10所示， 两种曲线计算结果点全部落在标准轨迹上， 表明算法对于轨迹上多点的位置计算精度高， 轨迹拟合能力较强.

图9 空间螺旋线预设轨迹与计算点Fig.9 Preset trajectory and calculated points of a spatial spiral

图10 “蝴蝶”形曲线预设轨迹与计算点Fig.10 Preset trajectory and calculated points of a butterfly-shaped curve

4.3 与其他逆运动学算法对比

目前冗余机械臂逆运动学求解算法众多， 每种方法都有其自身特点. 为综合对比不同算法效果， 列举了几种逆运动学算法性能表现, 如表2所示. 文献[18]使用改进人工蜂群算法求解七自由度冗余机械臂逆运动学， 位置计算精度可达10-3mm. 文献[19]采用粒子群算法、 遗传算法、 量子粒子群算法以及引力搜索算法求解四自由度机械臂逆运动学问题， 求解位置精度较高可达10-5mm， 但求解时间长， 耗时约为2 s. 文献[20]使用神经网络算法， 对七自由度机械臂单次求解仅需0.1 s， 但位置精度仅为5 mm， 计算速度较快但精度不高. 而同样针对七自由度冗余机械臂， 本算法位置计算精度可达10-7mm， 求解时间也较短， 仅需0.93 s.

表2 几种逆运动学求解方法对比

5 结语

采用自适应动态差分进化算法求解冗余机械臂逆运动学. 引入自适应变异因子以及随机变动交叉因子， 随迭代进行自适应调整算法搜索强度， 避免算法过早陷入姿态局部搜索， 提高了算法收敛性及稳定性. 改进含加权操作的适应度函数和关节边界处理操作， 平衡了位置误差及姿态误差收敛. 最后， 完成了一系列数值仿真实验， 测试和评价算法收敛性、 稳定性， 并与基本差分进化算法和其他逆运动学算法完成了比对分析. 结果表明， 机械臂位置和姿态误差均得到了良好收敛， 方法可普适于冗余机械臂逆运动学求解.