冗余机械臂逆运动学问题的多策略贪婪蜂群求解

任云晖，贾仙鱼

（1.江海职业技术学院，江苏 扬州 225101；
2.扬州大学，江苏 扬州 225000）

冗余机械臂逆运动学求解是指根据末端执行器在笛卡尔空间的位姿求解相应的关节空间角［1］，求解过程具有高度非线性、高度耦合性的特点。机械臂运动学逆解是机械臂进行关节空间轨迹规划和轨迹跟踪控制的基础，逆运动学求解精度决定了机械臂对笛卡尔空间轨迹的跟踪精度［2］，会进一步影响机械臂的定位精度和生产质量等。

因此，研究机械臂逆运动学求解问题具有重要意义。冗余机械臂逆运动学求解方法可大致分为3类，分别为封闭解法、数值解法和智能求解法［3］。

封闭解法也称为几何法，该方法依据空间几何知识进行逆运动学求解，具有求解精度高、求解速度快的特点，但是此方法通用性差切只适用于满足Pieper准则的机械臂结构。文献［4］将几何法和坐标投影法相结合进行逆运动学求解，经分析得出了逆运动学最多8组封闭解的结论。数值求解法包括雅可比矩阵法、梯度投影法等。雅可比矩阵法在每一次迭代都要有一次求导和求逆，计算过程较为复杂。梯度投影法是利用迭代法求逆运动学近似解的过程。文献［5］以目标函数作为约束对任务空间进行增广，并构造增广雅可比矩阵，使用分解计算策略得到求解结果。文献［6］对比了雅可比矩阵转置法和雅可比矩阵求逆法，并提出了计算量小、可靠性高的Levenberg-Marquardt逆解法。

智能求解法是通过编码和适应度函数的设置将运动学逆解问题转化为寻优问题，常用方法包括神经网络、粒子群算法、遗传算法等。文献［7］针对不满足Pieper准则的机械臂，以位置误差和姿态误差作为优化目标，使用人工蜂群算法进行求解，得到了逆运动学的最优解。文献［8］将布谷鸟算法和帝国主义算法进行混合，混合算法的搜索性能好于任何单一算法，使用混合算法求解机械臂逆运动学问题，得到了较优解。启发式搜索算法容易陷入局部最优，难以搜索空间最优解，从而影响逆解质量，因此仍有较大研究空间。

这里研究了冗余机械臂逆运动学问题的求解方法，建立了以减小逆解的位置误差和姿态误差为目标的优化模型，并对标准蜂群算法进行改进提出了多策略贪婪蜂群算法，使用多策略贪婪蜂群算法进行逆运动学求解，实现了提高求解精度的目的。

2.1 机械臂介绍

这里研究对象为KUKA LBR iiwa 七自由度机械臂，其实物图及运动学示意图，如图1所示。图1（b）中θ1～θ7为机械臂关节转角。

图1 KUKA LBR iiwa机械臂Fig.1 KUKA LBR iiwa Manipulator

机械臂的D-H运动学参数及各关节活动范围，如表1所示。

表1 KUKA LBR iiwa机械臂D-H参数Tab.1 KUKA LBR iiwa Manipulator D-H Parameters

表中：αi—连杆扭角；
di—连杆偏距；
ai—连杆长度；
Δθi—关节角活动范围，也是这里中关节角θi的搜索范围。

2.2 逆运动学问题描述与建模

机械臂逆运动学问题描述为：给定机械臂末端执行器位姿(xex，yex，zex，αex，βex，γex)，其中(xex，yex，zex)为末端执行器质点在极坐标系的位置，(αex，βex，γex)为末端执行器在极坐标系的欧拉角，要求求解出该位姿对应的一个关节空间点{θi}，i= 1～7。

基于启发式智能算法的求解过程为：算法每更新一个关节空间点，则使用正运动学方程求解出对应的末端执行器位姿，并计算末端执行器位姿误差，位姿误差最小的解即为最优解。

由以上求解过程可以看出，仍有两个问题需要解决：

（1）正运动学方程的建立；

（2）位姿误差评价模型的建立。

基于欧拉角法建立坐标轴之间的转换矩阵，坐标系之间通过平移使原点重合，并通过3次旋转可以使3个坐标轴重合。根据上述过程，假设机械臂的当前关节角为{θi}，i= 1～7，则坐标系i到坐标系i-1的转换矩阵为［9］：

式中：(px，py，pz)—末端执行器质心在基坐标系的实际位置，末端执行器在基坐标系的姿态可以依据矩阵T07的元素求出，如式（3）所示。

式中：αre、βre、γre—末端执行器在基坐标系的实际欧拉角。

根据给定的末端执行器位姿和关节空间点求解的末端执行器实际位姿，得到位置和姿态误差分别为：

式中：ΔP—末端执行器位置误差；
ΔR-姿态误差。

使用加权法将2个目标统一为1个优化目标，为：

式中：fobj—优化目标；
wp、wr—位置误差系数和姿态误差系数。在式（5）中，位置误差与连杆长度、连杆偏距相关，而姿态误差由于使用弧度制表示，也与连杆长度相关，因此位置误差和姿态误差具有一定的相关性和倍数关系。为了平衡位置误差与姿态误差，经分析与实验，参数设置为：wp= 1、wr=180 π。

3.1 人工蜂群算法

在标准人工蜂群算法中，蜜蜂分为引领蜂、跟随蜂和侦察蜂等3类。算法的实现大致包括蜜蜂初始化、引领蜂搜索阶段、跟随蜂搜索阶段和侦察蜂搜索阶段等步骤［10］。

（1）蜜蜂初始化。初始时刻，所有蜜蜂使用随机方式进行位置初始化：

式中：i∈[1，N]—蜜蜂编号；
N—蜜蜂总数；
j∈[1，J]—位置维度，J—位置维度；
xij—蜜蜂i的初始化位置；
rand(0，1)—随机数；

（2）引领蜂搜索阶段。所有蜜蜂进行位置适应度排序，适应度靠前的NB个蜜蜂被设置为引领蜂，其余蜜蜂设置为跟随蜂。引领蜂采用交叉方式进行位置更新［11］，即：

（3）跟随蜂搜索阶段。引领蜂位置适应度通过摇摆舞传递给跟随蜂，跟随蜂根据引领蜂各自适应度确定选择概率，为：

式中：pi—跟随蜂选择引领蜂i的概率；
引领蜂i的位置适应度值；
引领蜂i的目标函数。

跟随蜂选择完引领蜂后，与引领蜂一起进行蜜源搜索，蜜源搜索方式与式（7）一致。

（4）侦察蜂搜索阶段。当引领蜂和跟随蜂在某一位置搜索次数超过阈值，且蜜源适应度没有明显提高时，蜜蜂放弃此位置，转化为侦察蜂后进行大范围搜索，方法为：

分析标准人工蜂群算法可知：（1）跟随蜂依据式（7）确定对引领蜂的选择概率，但是当引领蜂中出现适应度极大的“超级蜜蜂”时，其超强竞争力会给其他蜜蜂产生较大竞争压力，跟随蜂会以极大概率选择“超级蜜蜂”，蜜蜂多样性必然遭到严重破坏；
（2）跟随蜂根据式（7）进行蜜源位置更新，更新方式单一且缺少群体经验引导，是一种低效的位置更新方法。下文针对上述两点分析和缺陷进行改进。

3.2 选择策略优化

为了削弱适应度极大的“超级蜜蜂”对算法多样性的影响，减小不同引领蜂之间的被选概率差值，这里使用基础概率与适应度差值概率求和的方式，将引领蜂i的被选概率pi设置为：

3.3 多搜索方式-贪婪选择策略

为了解决跟随蜂搜索方式单一且缺少群体经验引导的问题，参考粒子群算法和差分算法中的更新方法，设置3种位置更新方法，分别为：

最优蜜蜂；
xa、xb、xm、xn—从种群中随机选择的蜜蜂。

式（11）中给出的多搜索方式中，第一式为受粒子群算法启发的更新方法，第二式和第三式分别类似于差分算法的DE/best/1和DE/best/2的更新方法。

跟随蜂同时使用式（11）中的3种方式进行位置更新，而后计算原蜜源位置、3种更新位置的蜜源适应度，使用贪婪策略确定更新的蜜源位置。

3.4 基于多策略贪婪蜂群算法的逆运动学求解

根据上述分析及改进措施原理，制定基于多策略贪婪蜂群算法的逆运动学求解步骤为：

（1）设置多策略贪婪蜂群算法参数，包括蜜蜂总数N、位置维度J、引领蜂数量NB、单位置搜索次数阈值；

（2）所有蜜蜂进行位置初始化，并对蜜蜂适应度值进行排序，适应度靠前的NB个蜜蜂设置为引领蜂；

（3）跟随蜂按照式（10）计算对引领蜂的选择概率；

（4）引领蜂与跟随蜂按照式（11）的3种方式进行蜜源搜索，并使用贪婪准则确定更新蜜源位置；

（5）当引领蜂与跟随蜂在某位置搜索次数大于阈值次数，且蜜源适应度未得到提高时，蜜蜂转化为侦察蜂按照式（9）进行蜜源搜索；

（6）判断是否到达最大迭代次数，若否则转至（4），若是则输出最优蜜蜂位置，算法结束。

4.1 求解结果与算法性能分析

实验的操作系统为Windows 10系统，硬件条件为Intel Core i7-8700K @ 3.7GHz，内存大小为16GB。以图1 的KUKA LBR iiwa 机械臂为研究对象，将机械臂末端执行器期望位姿设置为(400，500，600，π 2，π 4，π 3)。

为了比较算法的收敛精度、收敛速度、性能稳定性等参数，分别使用标准人工蜂群算法、多策略贪婪蜂群算法对前文建立的优化模型进行求解，算法的参数设置为：蜜蜂总数N= 50、位置维度J= 7、引领蜂数量NB= 10、单位置搜索次数阈值为7。

为了减小随机因素影响，两种算法各自独立求解10次，选择10组中的最优解进行比较，两种蜂群算法对最优解的迭代过程，如图2所示。

图2 求解迭代过程Fig.2 Solving Iteration Process

由图2所示结果可知，多策略贪婪算法迭代至32次时算法收敛完毕，标准蜂群算法迭代至61次时算法收敛完毕。

从收敛精度看，多策略贪婪算法收敛值为1.4×10-5，求得的关节空间点为（0.1277，0.9453，1.1840，1.3314，-1.4050，-1.1484，1.8500），对应的实际位姿为（399.9999，500.0001，600.0000，1.5708，0.7854，1.0472），与设定位姿做差可知，位置误差仅为10-4mm，姿态误差为0；
标准蜂群算法收敛值为1.59，逆解对应的实 际 位 姿 为（399.9416，500.0091，599.9324，1.1780，0.9425，1.0473），与设定位姿做差可知，位置误差在接受范围内，但是姿态误差较大，无法满足求解要求。

由上述分析可以看出，多策略贪婪蜂群算法的收敛精度和速度均好于标准蜂群算法，这是因为改进算法中的跟随蜂使用了多种位置更新策略，并使用贪婪规则确定更新位置，提高了蜜蜂的局部搜索能力；

另外，改进算法有效避免了“超级蜜蜂”对算法多样性的破坏，有效维持了蜜蜂多样性。因此多策略贪婪风趣算法的收敛精度和收敛速度均好于标准蜂群算法。

4.2 多点求解精度测试

为了验证这里算法对连续多点的求解精度，在蝴蝶曲线上按固定步长选取500个点，蝴蝶曲线表达式为：

式中：t—时间。

机械臂末端执行器的质心位置即为蝴蝶曲线的采样位置，末端执行器的姿态规定为：X轴向与曲线切向一致，Y轴向在蝴蝶曲线平面内与切向垂直，Z轴向成右手定则。

依据多策略贪婪蜂群算法对上述500个位姿点求运动学逆解，得到对应的关节空间点，实际轨迹与求解位置，如图3所示。

图3 标准轨迹与计算轨迹Fig.3 Standard Trajectory and Computed Trajectory Points

统计图3中500个位姿点的逆解误差，末端执行器质心位置的最大误差为2.3×10-4mm，最小误差为1.2×10-6mm，平均误差为1.3×10-5mm；
姿态最大误差为1.4×10-4rad，最小误差为2.2×10-5rad，平均误差为6.7×10-4rad。

由上述数据可知，多策略贪婪蜂群算法对连续多位姿点的逆运动学问题也具有较高的求解精度。以上实验结果和分析进一步验证了多策略贪婪蜂群算法在机械臂逆运动学求解问题中的优越性。

这里研究了冗余机械臂逆运动学的求解问题，建立了以减小逆解的位置误差和姿态误差为目标的优化模型，并提出了多策略贪婪蜂群算法的模型求解方法。

经验证得出以下结论：

（1）多策略贪婪蜂群算法的收敛精度和收敛速度好于标准人工蜂群算法；

（2）在机械臂逆运动学求解中，多策略贪婪蜂群算法的求解质量高于标准蜂群算法；

（3）在连续多个位姿点的逆运动学求解中，多策略贪婪蜂群算法求解的位置精度和姿态精度均较高，验证了该方法在逆运动学求解中的优越性。

猜你喜欢位姿执行器运动学基于MATLAB的6R机器人逆运动学求解分析河北省科学院学报(2020年1期)2020-05-25双级执行器系统的离散滑模控制测控技术(2018年12期)2018-11-25基于D-H法的5-DOF串并联机床运动学分析制造技术与机床(2018年11期)2018-11-23飞机装配预连接紧固件自动化安装末端执行器设计制造技术与机床(2017年9期)2017-11-27基于共面直线迭代加权最小二乘的相机位姿估计光学精密工程(2016年5期)2016-11-07基于CAD模型的单目六自由度位姿测量光学精密工程(2016年4期)2016-11-07考虑执行器饱和的改进无模型自适应控制自动化学报(2016年8期)2016-04-16一类具有执行器饱和的非线性系统抗饱和方法研究自动化学报(2016年5期)2016-04-16小型四旋翼飞行器位姿建模及其仿真湖北工业大学学报(2016年5期)2016-02-27基于运动学原理的LBI解模糊算法海军航空大学学报(2015年1期)2015-11-11