基于组合优化算法的6R机器人逆运动学求解

吉阳珍 侯 力 罗 岚 罗 培 刘旭槟 梁 爽

四川大学机械工程学院，成都，610065

0 引言

工业机器人是现代制造业中一种重要的机电一体化设备，已被广泛应用于各个行业,其中，6R机器人因具有工作空间大、运动灵活、非冗余等特点而成为应用和研究的首要目标[1]。逆运动学求解是根据给定的末端执行器位姿(位置和姿态)求取各关节变量，其方程组具有高维、非线性的特点，求解复杂且不易求出[2]。逆解问题是机器人研究的重点问题之一，是机器人轨迹规划与运动控制的基础。

目前6R机器人逆运动学求解的方法主要分为解析法[3]、数值解法[4]与人工智能法[5-8]。这些方法虽得到了一些成功的应用，但仍存在一定的局限性。解析法的缺点是运算量大，涉及符号运算，求解过程复杂，对工程人员的数学基础有很高的要求，且受限于机器人构型，不具备通用性，只有在其几何结构满足Pieper准则[9]时才可求得封闭解。数值解法对机器人构型没有特别要求，通用性很高，其缺点是对初始条件敏感，若迭代初值设置不合理，可能会出现无解的情况。智能算法应用于解决逆解问题时存在计算成本高、随机性强、精度达不到要求等不足，在应用中受到了很大的限制，如神经网络算法需要大量已知样本进行训练，泛化能力较弱，而遗传算法实现过程复杂，求解效率低。

相比其他元启发式算法，鲸鱼优化算法[10](whale optimization algorithm,WOA)具有原理简单、参数设置少、易于编程实现、不需要梯度信息以及寻优性能强等[11]优点，已被成功应用于求解多种优化问题[12-13]。目前，还没有学者将鲸鱼算法与机器人逆运动学求解相结合进行研究。

由于利用智能优化算法求得的逆解本质上也属于数值解，故本文将改进鲸鱼算法与Newton-Raphson数值迭代法结合得到一种通用的组合优化算法(combinatorial optimization algorithm，COA) ，并应用于6R工业机器人的逆解问题，最后在MATLAB环境下开展了4组仿真试验，测试了改进鲸鱼算法和组合算法求逆解的性能。

1 机器人逆运动学分析

1.1 机器人模型

本文以IRB 4600-40/2.55型工业机器人为主要研究对象，它是符合Pieper准则的六自由度关节型机械臂。根据表1所示的机器人关节参数，采用标准D -H参数法[14]建立的连杆坐标系如图1所示，其中，Σr(orxryrzr)和Σe(oexeyeze)分别是固连于基座和末端执行器的参考坐标系。

表1 IRB 4600型机器人的关节参数

图1 IRB 4600-40/2.55型机器人连杆坐标系Fig.1 Linkage coordinate system of IRB4600-40/2.55 robot

从Σi-1(第i-1个坐标系)到Σi(第i个坐标系)的通用齐次变换矩阵为

i-1Ti=

(1)

将6个变换矩阵从左到右依次相乘，即可得到机器人末端坐标系相对于基坐标系的位姿矩阵0T6。对于给定的机器人结构，末端位姿T是关节变量θ=(θ1,θ2,θ3,θ4,θ5,θ6)的函数，有

(2)

式(2)即为机器人的正运动学方程。其中，R3×3、p分别为末端执行器坐标系相对于基坐标系的姿态矩阵和位置矢量。

1.2 逆运动学求解目标函数

组合优化算法求机器人逆运动学解的思想是将逆解问题转化为优化控制问题来解决，以最小化位姿误差为优化目标，其目标函数的构造过程如下。

假设机械人末端执行器的期望位姿矩阵为

(3)

假设当前关节角度值为θc，代入式(2)可求出当前位姿矩阵:

(4)

为了使末端执行器达到期望位姿，需要使当前位姿矩阵与期望位姿矩阵相等，即应满足

‖Tc-Td‖=0

(5)

此时满足条件的关节角度值即为运动学逆解。

各关节变量的边界条件表示为

θil≤θi≤θiui=1,2,…,6

(6)

式中，θiu、θil分别为第i个关节对应关节变量的上边界和下边界。

考虑工程应用实际，需要限制多解，且末端执行器从当前位姿运动到目标位姿的过程中，各关节转角变化应平稳连续，没有突变。假设机器人在连续轨迹规划过程中所经过的节点为Pk(k=1,2,…)，经求逆得到该点位姿对应的关节矢量值为θk，为确保当前解始终与上一组解是最接近的，应满足

min ‖Δθ*‖=‖θk+1-θk‖k=1,2,…

(7)

从而将逆解问题转化为多目标约束优化问题，则经组合算法求得的逆解将会是满足多约束条件的唯一解，不再具有随机性。优化模型如下：

(8)

式(8)即为本文机器人逆解问题的目标函数，包含两个非线性矢量方程和一个约束方程。

1.3 适应度函数的设计

由于上述构造的目标函数为复杂的方程组，不适合直接代入鲸鱼算法进行计算，故采用如下方式设计一个求解最小值的适应度函数。

将式(8)中的第一个非线性矢量方程表示为函数形式并分解成姿态误差函数f1(θ)和位置误差函数f2(θ)：

f1(θ)=‖ΔR3×3‖=‖R3×3,c-R3×3,d‖

(9)

f2(θ)=‖Δp‖=‖pc-pd‖

(10)

式中， ΔR3×3为姿态误差，表示机器人末端执行器实际姿态与期望姿态的差值；Δp为位置误差，表示末端执行器实际位置与期望位置的差值。

将式(8)中的第二个非线性矢量方程表示为函数形式：

f3(θ)=‖Δθ*‖=‖θk+1-θk‖

(11)

借鉴线性加权评价函数法[15]处理多目标优化问题的思想，分别赋予f1(θ)、f2(θ)和f3(θ)独立的权值系数α、β和γ，将三者进行线性组合。以式(6)作为变量定义域，最终构造的适应度函数如下：

F=α‖ΔR3×3‖+β‖Δp‖+γ‖Δθ*‖

(12)

由机器人变换矩阵式(1)可看出，姿态矩阵主要由正余弦函数组成，因此‖ΔR3×3‖的数量级为100；由机器人连杆参数可知，位置误差的欧氏范数数量级在103；计算过程中关节转角采用弧度单位制，而相邻两次转角之差变化较小，故‖Δθ*‖的数量级在100。由此，需通过对各权值系数取不同的值来协调求解过程中适应度函数各子项的收敛速度，避免因数量级差别过大导致某些子项的变化被忽略而对计算造成影响。实际取值根据具体的机器人参数确定。

2 鲸鱼优化算法的改进

2.1 标准鲸鱼优化算法

在WOA中，群体中的任一个体在搜索空间中的位置都代表了优化问题的一个可行解。假设鲸鱼种群数量为N，第t次迭代时的种群X(t)={Xi(t),i=1,2,…，N}，Xi(t)为种群中第i个鲸鱼个体的位置，其3种位置更新方式描述如下。

(1)包围猎物阶段的位置更新公式为

Xi(t+1)=Xbest(t)-AD

(13)

D=|CXbest(t)-Xi(t)|

A=2ar1-a

C=2r2

式中，D为当前鲸鱼个体与最优位置之间的距离；A、C为系数变量；Xbest(t)为当前群体最优位置；r1、r2为[0,1]范围内的随机数；a为收敛因子，按a=2(1-t/T)更新；T为最大迭代次数。

(2)发泡网攻击阶段，以相同概率选择两种方式更新。①收缩包围机制：按式(13)进行位置更新。②螺旋更新位置公式为

Xi(t+1)=D′eblcos(2πl)+Xbest(t)

(14)

D′=|Xbest(t)-Xi(t)|

式中，D′为鲸鱼个体与当前最优位置之间的距离；b为对数螺旋形状常数；l为[-1,1]上的随机数。

(3)随机搜索阶段的位置更新公式为

Xi(t+1)=Xrand(t)-A|CXrand(t)-Xi(t)|

(15)

式中，Xrand(t)为当前种群中随机一个鲸鱼个体的位置。

2.2 鲸鱼算法的改进

针对鲸鱼算法求解高维、非线性、多峰值复杂优化问题时存在的收敛速度慢、易陷入局部最优、收敛精度低等缺点，本文提出了4点改进措施，以改善WOA的寻优性能，并将经混合策略改进后的鲸鱼算法命名为MSWOA (improved whale optimization algorithm based on mixed strategy)。

2.2.1混沌映射初始化种群

利用混沌映射来初始化种群，增加种群的多样性和分布均匀性，可在一定程度上改善解的质量及算法的寻优性能。文献[16]使用多种混沌映射改进鲸鱼算法，结果表明Tent混沌映射对WOA性能提高的效果最好，故本文采用Tent映射来初始化鲸鱼种群的位置，其表达式如下：

(16)

φn∈[0,1]

2.2.2收敛因子非线性更新策略

WOA中参数A的值在很大程度上依赖于收敛因子a的变化，a随迭代次数的增加线性递减，不能较好地体现出实际的寻优过程，因此在调节全局探索和局部开发能力方面效果较差[17]。

本文对收敛因子a采用非线性时变更新策略，使得a在迭代初期具有较大的衰减速度，有利于算法快速收敛，随着迭代次数增加，a的衰减速度逐渐变慢，有利于后期进行精细的局部搜索。保持a的取值范围不变，更新公式为

(17)

a随迭代次数的变化趋势见图2(取最大迭代次数T=500)。

图2 不同更新策略下a的变化趋势Fig.2 The change trends of a under differentupdate strategies

2.2.3自适应惯性权重位置调整

设计合理的惯性权重是避免算法陷入局部最优并高效搜索的关键[18]。本文在WOA中引入权重因子w，并依据早熟收敛程度和适应度对其值进行自适应调整。

设定鲸鱼个体Xi(t)的适应度为fi，分别计算鲸鱼群的平均适应度favg，优于或等于favg的鲸鱼个体适应度平均值favg1，差于favg的个体适应度平均值favg2，依据favg1、favg2将群体分为3个子群，分别选择不同的惯性权重取值。

(1)fi≤favg1，表示当前个体的适应度优于本轮迭代中较优群体的平均适应度，该个体位置较优，此时应减小其惯性权重，有利于提高局部搜索能力，w取值为

(18)

(2)favg1

(3)fi≥favg2，表示当前个体的适应度差于本轮迭代中较差群体的平均适应度，该个体位置不理想，此时需跳出当前区域，应增大惯性权重，有利于增强全局寻优能力，w取最大值，w=wmax。

将权重因子w引入式(13)～式(15)后，得到新的鲸鱼个体位置更新公式分别为

Xi(t+1)=wXbest(t)-AD

(19)

Xi(t+1)=D′eblcos(2πl)+wXbest(t)

(20)

Xi(t+1)=wXrand(t)-A|CXrand(t)-Xi(t)|

(21)

2.2.4模拟退火策略

本文将模拟退火策略[19]引入WOA，使得其在搜索时增加适当的随机因素，以一定的概率接受使目标函数变差的解，可以有效地避免陷入局部最优，改善全局寻优能力,具体操作如下。

在每轮迭代中，当WOA求解完成之后，产生一个新的鲸鱼种群用于退火处理。为了区别于产生初始种群的方法，采用自相关性较好的Chebyshev混沌序列[20]来初始化新种群，其表达式为

yn+1=cos(ψarccosyn)yn∈[-1,1]

(22)

式中，ψ为Chebyshev映射的阶数，值取5。

(23)

温度tc的更新公式为

tc=μtc

(24)

式中，μ为退温系数。

3 机器人逆运动学组合优化算法

3.1 逆运动学数值迭代算法

本文采用一种基于Newton-Raphson迭代法的数值解法(简称“N-R算法”)[14]，其核心思想是：假设机器人位姿与关节转角的映射关系在关节转角的空间邻域内是线性的，利用机器人的雅可比矩阵J(θ)建立机器人微分运动矢量和关节转角增量之间的关系，从而构造迭代表达式。对于迭代过程中可能产生雅可比矩阵奇异的情况，可利用其广义逆矩阵J*(θn)代替逆矩阵J-1(θn)的方法解决。算法步骤如下：

(1)设置机器人的期望末端位姿Td、初始关节角θ0、初始化迭代次数n=0、最大迭代次数N0以及关节角θcn=θc(n)=θ0；设定迭代收敛条件ε，即末端位置和姿态误差的阈值εp、εq。

(2)根据正运动学方程计算对应于θcn的末端位姿矩阵Tc(θcn)以及雅可比矩阵J(θcn)。

(3)根据式(25)、式(26)计算从Tc(θcn)到Td的位置运动增量Δpn和姿态运动增量Δqn：

Δp(θc)=pc-pd

(25)

(26)

(4)判断迭代条件‖Δpn‖≤εp且‖Δqn‖≤εq是否满足，若条件满足或迭代次数已达N0则循环结束，转步骤(8)；否则，继续下面的步骤。

(5)计算雅可比矩阵J(θcn)的广义逆矩阵J*(θcn)，根据下式计算关节角增量：

(27)

(6)按下式计算下一个迭代周期的关节角：

θc(n+1)=θc(n)+Δθn

(28)

(7)令n←n+1，转步骤(2)。

(8)输出θc(n)，即为对应于初始关节角θ0及期望的末端位姿Td的运动学反解。算法结束。

3.2 逆运动学组合优化算法

机器人逆运动学问题本质上属于多元多峰函数最优解求取问题，由于其目标函数是带约束条件的复杂非线性方程组，若根据前文设计的适应度函数直接利用改进鲸鱼算法在解空间中进行寻优搜索，需要较大的种群数量和较多的迭代次数才可以收敛到较高质量的解，所需的计算成本将非常高，无法满足机器人实时控制的要求，又由于鲸鱼算法在求解高维、非线性、多峰复杂问题时存在收敛精度低的固有缺陷，因此在很大概率上无法搜索到可以直接满足机器人控制精度要求的解。

利用N-R算法求逆运动学解，优势在于程序简单且收敛速度快(收敛阶数为2)，缺点在于局部收敛性强，对迭代初值的要求很高，且不能提供算法收敛的先验条件。

逆运动学组合优化算法旨在将上述两种算法的优势有机结合，在充分协调计算成本的前提下，先利用MSWOA搜索到一个相对接近精确解或具有收敛趋势的初始解，以此作为N-R算法的迭代初值，再通过少数几次迭代后收敛到高精度的运动学逆解，极大地缩短求解时间。组合算法求机器人逆解的流程描述如下：

(1)输入机器人结构参数；

(2)输入机器人末端执行器的期望位姿Td；

(3)输入当次求解的参考关节矢量θk；

(4)设置MSWOA及N-R算法的基本参数；

(5)设置N-R算法的迭代收敛条件ε；

(6)将适应度函数代入MSWOA求解逆运动学初始解；

(7)将上一步得到的初始解赋值给N-R算法的初始关节角θ0；

(8)利用N-R算法迭代求解；

(9)输出θc，逆运动学求解结束。

需说明的是：对于连续轨迹起点或单点求逆时，参考矢量θk可随机指定，通常可取θk=(0°,0°,0°,0°,0°,0°)，如有其他已知条件，可相应选取合适的θk值；N-R算法的迭代收敛条件ε决定了最终解的精度；若要求得某一位姿对应的所有逆解，需指定不同的参考矢量θk并进行多次运算。

4 试验验证及结果分析

为检验改进鲸鱼算法及组合优化算法求运动学逆解的性能，在MATLAB仿真环境下开展了4组试验。其中，试验1用来分析改进鲸鱼算法的性能；试验2～4用来分析组合优化算法的性能。试验的硬件环境为Inter(R) Core(TM) i7-4800MQ CPU @ 2.70GHz，运行内存8GB，操作系统为Windows 7(64位)旗舰版，仿真环境为MATLAB R2017b。

4.1 试验1

本组试验直接利用WOA、基于自适应权重和模拟退火的鲸鱼优化算法(W-SA-WOA)[21]与MSWOA三种鲸鱼算法分别对两段连续轨迹进行求逆，以对比三种算法的收敛速度及求解稳定性差异，同时通过计算轨迹上各位姿点的姿态误差与位置误差，对比三种算法在求解精度方面的优劣。

三种算法的公共参数取值如表2所示。经预先多次试验后确定MSWOA的其他参数最优取值为：惯性权重因子上限wmax=1.6，下限wmin=0.4，模拟退火阶段初始温度tc=100，退温系数μ=0.95，常数b=1。WOA和W-SA-WOA算法的其他参数按对应参考文献设置(W-SA-WOA中，模拟退火初始温度同取t=100)。

表2 算法对比试验公共参数取值

试验所用的两段连续轨迹均是利用Robotics Toolbox机器人工具箱在笛卡儿空间下生成的，操作如下：在机器人工作空间内任取四点A、B、C、D，分别以A、C点作为起点位置，B、D点作为终点位置规划运动轨迹，使用jtraj()函数生成50个插值点，形成两段五次多项式轨迹，如图3所示。各端点对应的位置矢量及一组整数关节矢量解具体为

pA=(1454.47 mm,-961.222 mm，479.38 mm)T
θA=(-30°,15°,-55°,-20°,45°,120°)
pB=(-1669.65 mm，824.10 mm,-94.34 mm)T
θB=(165°,75°,-15°,-75°,120°,-135°)
pC=(-1356.31 mm，1502.29 mm,471.52 mm)T
θC=(123°,32°,10°,-99°,-48°,76°)
pD=(-963.99 mm,-2132.97 mm,-901.73 mm)T
θD=(-113°,105°,-39°,-172°,63°,108°)

图3 试验所用两段五次多项式轨迹Fig.3 The quintic polynomial interpolation trajectoriesused in the tests

适应度函数均采用式(11)的形式，为平衡各子项收敛速度的差异，三个子项与各自权值系数的乘积应保持数量级一致。先取权值系数α=1，经100组测试计算得出‖ΔR3×3‖/‖Δp‖的均值为0.000 967，‖ΔR3×3‖/‖Δθ*‖的均值为0.8834，并发现权值系数取值在一定范围内的波动不影响算法求解的性能和结果，因此，为简化计算，确定其余两系数取值为：β=0.001，γ=1。

由于求解对象为连续轨迹，故在计算过程中，可将上一点的求解结果作为当次插值点计算时的参考关节矢量θk。为更好地保证各关节角度值平稳变化，可将每次计算时的算法搜索空间作实时动态调整，即以上一点求解得到的关节矢量为中心，将各关节角度值分别向上和向下扩张一定角度范围Δθk后作为当次插值点计算时的搜索空间。为方便计算，设置统一取值为Δθk=10°，从而每点计算时的搜索空间表示为[θki-10°,θki+10°](i=1,2,…，6)。搜索空间的实时动态调整策略极大地缩小了算法搜索范围，可在很大程度上提高求解效率，同时使得最终的适应度值更小，可在一定程度上提高求解精度。

考虑对比试验的公平性，对于轨迹起点，参考关节矢量θk统一指定为θAi+15°及θCi+15°，各关节角的搜索空间范围均设置为[-180°,180°]。采用三种算法分别对两段轨迹的50个位姿点进行求逆运算，每种算法独立重复计算5次，每次记为一组。记录每组求解结果中各插值点(除起点外)对应的适应度值的最优值、最差值，并求取49个适应度值的平均值及标准差。由于轨迹起点在求解时存在的差异性，故将起点适应度值单列。其中最优值和最差值反映求解的质量，平均值反映算法求解的精度，标准差反映算法的鲁棒性和稳定性。计算结果如表3所示。

通过对比表3结果可以看出，对于连续轨迹的逆运动学求解，利用改进的鲸鱼算法MSWOA在相同的参数条件下可以收敛到更小的适应度值，且均在1以下，而WOA与W-SA-WOA收敛到的适应度值较大，存在远大于1的情况；在适应度平均值上，MSWOA表现最好，均值相对最小，每组试验均保持在0.3附近，WOA表现最差，均值较大且波动大；由适应度值标准差可看出，MSWOA求解结果的离散程度最小，说明其寻优稳定性最好，鲁棒性更好。

为进一步对比三种算法收敛速度的差异，将更具代表性的轨迹起点作为比较对象，选取所有试验中每种算法起点适应度最大值与最小值对应的收敛曲线并根据轨迹的不同分别绘制于图4中。

由图4可以看出，WOA收敛速度较慢，且会出现适应度值较大的情况，这时可能已经出现了“早熟”收敛现象使得算法较早收敛于局部最优解；W-SA-WOA虽可以收敛到较小的适应度值，但收敛速度较慢，且跳出局部最优时所需的迭代周期较长，如图中多段长“台阶”形曲线所示；MSWOA可以收敛到更小的适应度值，当陷入局部最优时通过少数几次迭代便可跳出，且在前期具有较快的收敛速度，在最大迭代次数的一半附近即可收敛到较小适应度值。

为了更加直观地评价三种算法在求解精度方面的优劣，将经过求逆运算的两轨迹上各插值点的姿态精度和位置精度进行对比，分别以姿态误差ΔR3×3和位置误差Δp的欧氏范数‖ΔR3×3‖及‖Δp‖作为误差评价指标。由于每种算法的多组求解具有相似性，为方便统计，对5次试验中各插值点的姿态误差与位置误差分别求平均值后绘制于同一坐标系下，对比结果如图5所示。

表3 三种鲸鱼算法求逆性能对比

图4 三种鲸鱼算法适应度值收敛曲线Fig.4 The convergence curves of fitness values ofthe three algorithms

由图5可看出，直接将三种算法应用于机器人求逆时，相比之下，MSWOA求得的解整体上具有更小的姿态误差及位置误差，说明其求解精度更高。

图5 三种鲸鱼算法求逆结果误差对比Fig.5 The error comparison of the results of thethree algorithms

通过以上对比分析可知， MSWOA不仅解决了基本WOA容易出现的“早熟”及局部收敛等现象，同时比其余两种算法具有更高的求解精度及更好的鲁棒性，且在收敛速度方面优势显著。

4.2 试验2

由试验1结果分析可知MSWOA算法具有较快的收敛速度，在相同的迭代次数下可以收敛到更优解，故将其应用于逆运动学组合优化算法。

本组试验利用组合优化算法分别对两条连续轨迹进行求逆，与试验1的结果形成对比，以说明组合算法求逆时在计算成本与精度方面的优势。

经大量测试发现组合算法在求逆解方面具有非常高的性能，为充分节约计算成本，将本组试验参数设置为：MSWOA种群数量N=30，最大迭代次数T=30，N-R算法的最大迭代次数N0=30，迭代收敛条件为ε≤10-8，其余参数设置同前。利用组合算法对两条轨迹分别进行5次求逆运算，记录每组试验中经MSWOA计算后的各插值点适应度值，性能指标设置同前；记录各点经N-R算法迭代的次数，统计迭代次数的起点值、最优值、最差值及平均值。试验结果如表4所示。

表4 组合优化算法求逆性能分析

由表4结果可看出，虽然轨迹起点适应度值偏大，但经N-R算法少数几次迭代后仍可收敛到满足精度要求的运动学逆解；其余各点适应度最差值均不超过0.41，每组适应度平均值均不超过0.25，标准差均小于0.08，这是由于每次经组合算法计算后的逆解具有较高的精度，以其作为下一点计算时的参考矢量及搜索空间中点，不易引起误差的累积，从而使得除起点外其余各插值点的适应度值分布较为均匀，具有较高的求解稳定性；N-R算法迭代次数均不超过5，最优值为3，各组平均值均在4次左右，收敛速度极快，求解效率高。

为分析组合算法的求解精度，对两轨迹各插值点5次试验的姿态误差与位置误差求平均值，结果绘制于图6中。同时统计四种算法对两轨迹所有试验点在全部试验中的姿态误差及位置误差平均值，对比结果记录于表5中。

对比图6和表5结果可以看出，利用组合算法进行机器人逆运动学求解时，姿态误差达到了10-16，位置误差达到了10-13mm，均远远高于单独的鲸鱼优化算法直接进行求逆的精度水平，说明组合算法在求解精度方面有着巨大的优势。

图6 组合算法求逆结果误差分析Fig.6 Error analysis of COA

表5 四种算法逆运动学求解精度对比

经组合算法计算得到的连续轨迹各位姿点的逆运动学解曲线见图7。从图中可看出，各关节角随轨迹上插值点位置的变化曲线是连续且光滑的，可以满足机器人运动控制的平稳性要求。

图7 组合算法求逆的关节角度变化曲线Fig.7 The change curves of joint angles of COA

4.3 试验3

为检验组合算法对机器人奇异位姿点求逆的有效性，在工作空间内选取两奇异位姿点进行试验。两奇异点对应的一组逆解分别为

θP=(50°,-45°,-12°,60°,0°,-66°)
θQ=(-15°,24.5°,-81.5°,85°,0°,24°)

将其代入robot.jacob0()函数计算机器人雅可比矩阵，秩为5，行列式为0，均为奇异矩阵，所以该两位姿点为奇异点。参考关节矢量均设置为θk=(0°,0°,0°,0°,0°,0°)，各关节角的搜索空间范围均为[-180°,180°]，其余试验参数设置同试验2，利用组合算法分别对两点独立求解100次，运算结果统计如表6所示。

表6 组合算法对奇异点求逆结果

由表6可看出，组合算法对奇异点求逆稳定性略差，适应度平均值和N-R算法迭代次数平均值偏大，姿态精度与位置精度虽较连续轨迹相比稍差，但求解仍是有效的。分析其原因，在于参考关节矢量选取不够合理，MSWOA寻优计算量不足，如果合理选取参考矢量并调整搜索空间范围，适当增加MSWOA的计算成本，即增加种群数量或迭代次数，各项性能指标值将会有一定的改善，求解成功率也会有所提高。

4.4 试验4

为检验组合算法对不满足Pieper准则的一般6R机器人求逆的有效性，本文将文献[22]所讨论的一般6R机器人作为对象开展仿真试验。该机器人的D-H参数如表7所示。经先验测试，确定适应度函数3个权值系数取值同前。

表7 一般6R机器人的D-H参数

将关节转角θ=(-14°,-104°,-116°,2°,8°,-78°)对应的末端位姿作为期望位姿，在其他15组逆解中任取5组作为参照，结果如表8所示，利用每组解分别进行100次独立计算并统计结果，算法参数设置同试验2。

每次试验时，将该组解的6个关节角度值经四舍五入取整后分别增加一个[-30°,30°]范围内的随机角度值后得到的新的关节矢量作为θk，各关节角搜索空间范围均为[-180°,180°]。试验结果如表9所示，其中成功率S定义为

S=z′/z

式中，z为总的试验次数；z′为算法求解结果与参照解有效数字完全一致的试验次数。

θk产生示例如下：

θk=(-98°,-6°,-180°,37°,54°,-38°)+r×30°

式中，r=(r1,r2,…,r6),r1～r6为[-1,1]上的随机数。

由表9结果可知，求解成功率均达到95%以上，说明使用组合算法对一般6R机器人的求逆是有效的；每组试验中N-R算法迭代次数都未超过最大值30，说明所有求解结果均达到了收敛条件所要求的精度，对于未成功的几次试验，算法也收敛到了其他15组逆解中的一组，仅与参照解不同。各组适应度平均值均小于1.5，N-R算法迭代次数平均值均小于6；若适当增加MSWOA的计算成本或合理选择参考关节矢量θk并缩小搜索空间范围，理论上求解成功率可继续提高。

表8 试验4所用5组逆运动学解

表9 组合算法对一般6R机器人求逆结果

5 结论

(1)提出了一种用于六自由度关节型机器人逆运动学求解的组合优化算法。从机器人正运动学出发，结合工程应用实际，建立了求逆解问题的目标函数，并设计了用于组合算法求解的适应度函数。

(2)对基本鲸鱼算法提出了4种改进策略，提高了算法的收敛速度、收敛精度及稳定性，改善了其全局寻优性能；组合算法将智能优化算法搜索到的解作为数值法的初值进行迭代继续求解，有效地结合了两者的优势。

(3)在MATLAB环境下进行了多组仿真试验，结果表明：逆运动学组合算法能够保证逆解的唯一性，所求关节角变化平稳；相比于直接利用鲸鱼算法搜索机器人数值解，组合算法在满足较高精度的前提下具有更快的求解速度和稳定性，且不受机械臂构型限制，能够有效解决6R机器人逆运动学求解的问题。下一步将在机器人实验平台上进行该算法的实验研究，并针对实际的机器人系统开展轨迹规划的应用研究。