基于改进克隆选择算法的机械臂运动学逆解

石建平,李培生,刘国平,刘 鹏

(1.贵阳学院 电子与通信工程学院，贵州 贵阳 550005；2.南昌大学 机电工程学院，江西 南昌 330031；3.河北地质大学 宝石与材料工艺学院，河北 石家庄 050031)

0 引言

机器人逆运动学问题是指已知机器人末端执行器的位姿，求到达该位姿所需要的各关节变量值，该问题是机器人轨迹规划、运动控制以及工作空间分析等问题的前提和基础。因为逆运动学问题是一个从笛卡尔空间到关节空间的复杂非线性一对多映射，所以运动学逆解的求解是机器人学领域的技术难题之一。求解运动学逆解的主要方法包括封闭解法和数值解法两大类[1]。

封闭解法通过代数或几何的方式直接获得关节变量的表达式，具有求解速度快、精度高且能够求出所有逆解的优势。但封闭解法受机器人结构构型的限制较大，只有少部分结构构型满足Pieper准则[1]的机器人的逆运动学问题才存在封闭解析解，方法的通用性较差。数值解法是求解复杂机器人运动学逆解的主要方法，常规的数值解法包括基于雅克比矩阵的迭代法、神经网络法等。雅克比矩阵迭代法计算过程复杂，且存在累积误差以及对于奇异位形无法求解的问题；神经网络法需要大量的数据样本来进行网络学习，而训练样本的获取并非易事。近年来，遗传算法[2-3]、粒子群优化算法[4-7]、差分进化算法[8-9]、类电磁机制优化算法[10]及萤火虫群算法[11]等智能优化算法被用于机器人运动学逆解的求解并取得了良好的研究进展，为解决机器人逆运动学问题提供了有效的选择方案。

克隆选择算法(Clonal Selection Algorithm, CSA)[12]是人工免疫系统领域的一个研究热点，CSA通过对免疫系统执行免疫应答过程中抗体的克隆选择与扩增、高频变异与受体编辑等免疫机理的模拟，使得其具有较强的自学习、自组织和自适应能力，从而在诸多工程领域得到了广泛应用。同其他群智能优化算法一样，CSA在解决复杂优化问题时也存在容易陷入局部最优而导致收敛质量不高的问题。针对该问题，广大科研工作者结合不同的应用领域对CSA进行了大量卓有成效的改进研究[13-18]。目前，CSA在机器人运动学逆解中的应用鲜有报道，本文在前期研究成果的基础上，以基于CSA的机器人运动学逆解求解方法为研究背景，对CSA的改进与应用展开研究。

1 运动学方程与优化目标函数

1.1 冗余机械臂的运动学方程

本文以6连杆平面冗余机械臂的运动学逆解求解为例展开算法的改进研究，机械臂模型以及建立的连杆坐标系如图1所示。

根据D-H法可得从坐标系{i-1}到坐标系{i}的齐次变换矩阵为

(1)

式中：i=1,2,…,7;cθi=cos(θi),sθi=sin(θi)，θi和li-1分别为机械臂的关节角和连杆长度(l0=0、θ7=0°)。

进一步可得冗余机械臂的运动学方程为

(2)

将通过式(1)得到的相邻坐标系间的各齐次变换矩阵代入式(2)可得

(3)

1.2 优化目标函数

由于有无穷多个逆解与一个给定的冗余机械臂末端位姿对应，若没有约束条件，则逆解的选择难以确定。实际应用中，除了对机械臂末端的位姿有精度上的要求外，通常还有机械臂从初始位姿运动到目标位姿的能耗性、柔顺性、舒适度和安全性等指标要求。本文将机械臂末端的位姿精度要求作为约束条件，以机械臂从初始位姿运动到目标位姿的能量消耗最小为优化目标，将机械臂的逆运动学问题转换成一个最小约束优化问题来解决。

对上述平面冗余机械臂而言，由式(3)可知，有ax=ay=nz=oz=pz=0及az=1恒成立。为了简便计算优化目标函数，不需要考虑位姿矩阵中的常数元素。设期望的机械臂末端位姿矩阵为

定义位姿误差函数为

(4)

式中λ为调整因子，用于平衡机械臂的姿态误差与位置误差。

(5)

式中θ=[θ1,θ2,θ3,θ4,θ5,θ6]。

于是，机械臂的逆运动学问题可转换成如下的最小约束优化问题来解决，

s.t.

g(θ)=Δ-Δmax≤0。

(6)

式中Δmax为位姿误差函数的最大值。

本文利用罚函数法将上述约束优化问题(6)转换为无约束优化问题进行处理，采用的约束处理方法为：

minf′(θ)=min(f(θ)+φ(θ)),

(7)

(8)

式中：f(θ)为原目标函数(即能耗最小优化目标函数);φ(θ)为罚函数;f′(θ)为使用罚函数φ(θ)修正后的目标函数(即惩罚适应度函数);α为惩罚系数。

由式(8)可知，机械臂末端执行器的位姿误差越大，对原目标函数的惩罚力度越大。

2 算法原理

在前期研究中，笔者提出了具有双层协同进化机制的改进CSA，即BCECSA算法(bilevel coevolutionary clonal selection algorithm)[18]，该算法在混沌系统的参数估计中获得较好的优化效果。为了将改进的CSA用于有效解决冗余机械臂的逆运动学问题，本文在BCECSA算法架构的基础上，对其底层进化策略和顶层进化策略分别进行了相关的改进研究，进一步提出了改进的BCECSA算法，即IBCECSA算法。

2.1 底层进化

设抗体i用Xi=[xi1,xi2,…,xiD]表示，抗体i对应D维优化问题的一个候选解。将目标函数视为抗原，用目标函数适应值表示抗体和抗原的亲和度。亲和度的大小反映了候选解的质量，对于最小优化问题，亲和度越小，则候选解的质量越高。IBCECSA算法的底层进化采用如下的进化方案：

(9)

(10)

其中：Pi表示抗体Xi经历的最佳历史抗体，即抗体Xi在演化过程中所经历的最佳候选解；Pbest表示整个抗体种群的最佳历史抗体，即种群迄今为止经历的最佳候选解；Pr1表示抗体Xr1经历的最佳历史抗体，且索引r1满足r1≠i≠best；r1、r2、r3、r4、r5为[0,1]内均匀分布的随机变量；ω为比例因子；ps1为选择概率；t为当前迭代代数，tmax为算法最大迭代代数。

根据式(9)，算法按概率从两个进化算子中选择其中一个来产生下一代抗体i。其中，第一个进化算子是以种群最优抗体Pbest和最佳历史抗体Pr1的加权和为中心向量，以2×r2×(Pr1(t)-Pi(t))为随机扰动向量在中心向量附近进行扰动搜索；该策略利用种群全局历史最优信息和随机选择抗体r1的个体历史最优信息来引导算法的寻优搜索，能够兼顾种群的收敛性与多样性，达到有效平衡算法的全局探索与局部开发的目的。第二个进化算子是以Pbest×r3为中心向量，以ω×(r4×Pr1(t)-Pbest)为扰动向量进行局部的精细搜索，该方案直接利用种群最优信息进行引导寻优，能够加速算法的搜索速度；随迭代代数增加而非线性递减的比例因子ω能够确保该策略逐渐加强在全局最优抗体附近的局部深度开发，有利于提高算法的收敛精度。综上所述，算法的底层进化策略采用了由两个进化算子构成的混合协同进化机制，两个进化算子之间形成优势互补，在寻优过程中能够有效兼顾种群的多样性与收敛性。

对按上述底层进化得到的下一代抗体Xi(t+1)的亲和度进行实时评估，即计算该抗体的适应值。用F(·)表示适应值函数，按式(11)和式(12)实时更新抗体i的历史最佳抗体Pi和种群的历史最佳抗体Pbest，使得算法的收敛速度得到进一步增强。

Pi(t+1)=

(11)

Pbest(t+1)=

(12)

2.2 顶层进化

与BCECSA算法一样，IBCECSA算法的顶层进化采用克隆选择进化方案。克隆选择进化是在最佳历史抗体群P(由所有的Pi构成)的基础上进行的，具体的免疫操作包含克隆选择、克隆扩增、抗体的高频变异或受体编辑。本文将抗体群P按亲和度从小到大进行排序(针对最小优化问题)，得到排序后的临时抗体群P′。为了不破坏优秀抗体的良好进化势态，选择亲和度较大的50%抗体(对应质量较差的部分候选解)进行克隆免疫操作，克隆规模为：

(13)

式中：m为种群规模;Si为临时抗体群P′中排序序号为i的抗体对应的克隆规模;mc为所有被选抗体的总克隆规模。

高频变异和受体编辑是提高克隆体质量的常用免疫操作。其中：高频变异使得克隆体能够在局部邻域内进行深度的开发，能够挖掘质量更高的候选解；而受体编辑则赋予抗体在搜索空间内进行全局探索的能力，从而有效避免算法早熟收敛现象的发生。为全面改善算法的搜索性能，高频变异与受体编辑应当混合使用，本文采用的具体方案如下：

(14)

(15)

在大多数改进的克隆选择算法中，常常针对亲和度较好的部分抗体执行免疫操作，这样能够加速算法的收敛速度，同时也使得抗体种群的多样性较快丧失，增加了算法陷入早熟收敛的可能性。本文通过对亲和度较差的部分抗体执行免疫进化操作，使得该部分抗体的亲和度趋于成熟的同时较好地保持了种群的多样性，降低了抗体种群陷入早熟收敛而导致进化停滞的概率。

2.3 IBCECSA与BCECSA的异同

IBCECSA与BCECSA算法具有相同的算法架构，且都受克隆选择学说的启发，但两者的具体进化算子与免疫优化思路有明显区别。

首先，两个算法的底层进化策略不同。IBCECSA算法采用双策略混合协同进化的底层进化机制，其平衡全局搜索与局部搜索的能力比BCECSA算法更强，有效兼顾了种群的多样性与收敛性。

其次，与BCECSA算法相比，IBCECSA算法的顶层克隆选择进化环节更加简化直接，且更加容易编程实现。此外，BCECSA算法采用对精英抗体执行克隆免疫操作的方式，而IBCECSA算法采用了对亲和度较差的部分抗体执行克隆免疫操作，前者强调算法收敛速度的提升，而后者在稳步改善抗体种群质量的同时能够更好地保持种群的多样性。因此，IBCECSA算法具有更强的全局搜索能力。

最后，BCECSA算法的主要参数为放大系数β，而影响IBCECSA算法收敛质量的主要参数为选择概率ps1和ps2。放大系数β的不同取值直接改变BCECSA算法的函数总评估次数，而选择概率ps1和ps2的不同取值不影响IBCECSA算法的函数总评估次数。因此，IBCECSA算法的计算成本不受参数设置的影响。

2.4 算法步骤

综上所述，IBCECSA算法的流程如图2所示，其具体进化步骤描述如下：

(1)算法的参数设置与种群初始化。设置种群规模m、最大迭代次数tmax、克隆规模Si、选择概率ps1和ps2等参数，在可行域内随机初始化抗体种群。

(2)根据式(9)和式(10)对抗体i执行底层进化操作，对于超出搜索空间边界的抗体分量，参照文献[18]执行越界处理操作，根据式(11)和式(12)实时更新种群的历史最优信息。

(3)所有抗体执行完底层进化操作则转入步骤(4)，否则按步骤(2)继续对下一个抗体(即i=i+1)执行底层进化操作。

(4)将最佳历史抗体群P按亲和度从小到大进行排序，根据式(13)选择抗体群P中亲和度较差的50%个抗体执行克隆扩增。

(5)根据式(14)对所有克隆体执行高频变异或受体编辑免疫操作，对于超出搜索空间边界的抗体分量，参照文献[18]执行越界处理操作，按式(15)实时更新抗体群P中对应抗体的最佳历史信息。

(6)若迭代次数大于tmax，则输出寻优结果并退出算法循环，否则返回步骤(2)继续下一代的迭代进化操作。

3 基准函数测试与分析

3.1 参数设置

为便于与BCECSA算法的收敛性能进行比较，本文采用文献[18]中的10个基准测试函数来验证IBCECSA算法的可行性和有效性，有关基准测试函数的详细描述请参考文献[15]或文献[18]。IBCECSA算法中需要设置的关键参数为两个选择概率，即ps1和ps2。本文选择基准测试函数f4和f8来讨论上述两个参数的设置问题。

设ps2(或ps1)取固定值0.5，ps1(或ps2)分别取0、0.1、0.2、0.3、0.4、0.5、0.6、0.7、0.8、0.9及1不同值；其余参数：种群规模m=30，最大迭代次数tmax=200，抗体的克隆规模Si=5，函数维度D=30。各函数分别独立运行30次，算法寻优的平均适应值(Mean)、最优适应值(Best)、最差适应值(Worst)、标准差(Std.)以及函数有效平均总评估次数(TNE)如表1所示。其中，有效平均总评估次数是指算法收敛于函数理论最优值时的最小评估次数的平均值(对于未能收敛于函数全局最优的情形，则根据算法结束循环迭代时的实际函数评估次数进行统计)。

表1 不同选择概率的测试结果

由表1可知：当概率ps1<0.9时，算法能够收敛于函数f4的理论最优值；当概率ps1≥0.9时，算法不能收敛于函数f4的理论最优值。随着概率ps1的增加，算法对函数f4的计算成本逐渐增加，但概率ps2的改变对函数f4的寻优收敛质量及计算成本影响不大。对于函数f8，算法不能收敛其理论最优值，当概率ps1<0.6时，算法的收敛精度与寻优稳定性不够理想，且随概率ps1的减小，算法的整体收敛质量呈逐渐下降的趋势；当概率ps2>0.4时，算法的收敛精度与寻优稳定性不够理想，且随概率ps2增大，算法的整体收敛质量呈逐渐下降趋势。综述所述，概率ps1的取值区间建议为[0.6,0.9]，概率ps2的取值区间建议为[0,0.4]。

根据上述分析，本文将IBCECSA算法的参数设置如下：种群规模m=30，最大迭代次数tmax=200，抗体的克隆规模Si=5，概率ps1=0.7,ps2=0.3。

3.2 测试结果与分析

为全面检测IBCECSA算法的寻优能力，考虑基准函数的维度D为30维和100维的情形，将IBCECSA算法的测试结果与CSA、FDCSA[15]及BCECSA算法的测试结果进行对比，各函数的试验结果如表2所示(CSA及FDCSA算法的数据来源于文献[15]，BCECSA算法的数据来源于文献[18])。IBCECSA算法对各函数的平均适应值收敛曲线如图3所示。为便于观察，图3中除了函数f10外，其余函数的平均适应值取以10为底的对数。

表2 不同算法的测试结果对比

由表2的测试结果可知：IBCECSA和BCECSA算法能够稳定收敛于7个基准函数(f1-f7)的理论最优值，FDCSA算法能够稳定收敛于3个函数(f1，f5，f6)的理论最优值，而CSA算法只有1个函数(f5)能够收敛于理论最优值；IBCECSA、BCECSA及FDCSA算法在f8获得了较高的收敛精度，但IBCECSA、BCECSA算法的寻优稳定性优于FDCSA算法；IBCECSA、BCECSA算法在f9获得了相同的收敛质量且都优于FDCSA算法；IBCECSA、BCECSA及FDCSA算法在f10获得了相近的收敛质量；IBCECSA、BCECSA及FDCSA算法在函数f8～f10上的整体收敛质量远优于CSA。综上可知，从算法的收敛质量来看，IBCECSA、BCECSA算法的收敛质量相当，两者都优于FDCSA算法；而IBCECSA、BCECSA及FDCSA算法的收敛质量都远优于CSA算法，说明3个改进算法的改进策略是可行有效的。图3进一步验证了IBCECSA算法具有收敛精度高、鲁棒性强的特点，且对高维复杂优化问题具有较强的处理能力。

根据表2，对于所有测试函数，IBCECSA和BCECSA算法的函数总评估次数远小于CSA及FDCSA算法，使得在相同计算成本的前提下，IBCECSA和BCECSA算法的收敛速度更快、搜索效率更高；除了函数f8～f10外，BCECSA算法在其余函数上的计算成本明显低于IBCECSA算法。

综合以上分析，与其他改进的CSA相比，IBCECSA算法在收敛速度、收敛精度、寻优稳定性及计算成本等方面都具备相当的竞争力。

4 机械臂的逆运动学问题求解

(16)

Pi(t+1)=

(17)

Pbest(t+1)=

(18)

其中：Pi表示抗体θi经历的最佳历史抗体;Pr1表示抗体θr1经历的最佳历史抗体;Pbest表示整个抗体种群的最佳历史抗体;其余参数与前述相同。

同样，IBCECSA算法顶层进化策略中的式(14)和式(15)分别修改如下

(19)

(20)

其中：Pi′为临时抗体群P′中排序序号为i的抗体;Pr2′、Pr3′分别为临时抗体群P′中排序序号为r2与r3的抗体;Pik*为经过高频变异或受体编辑免疫操作后得到的成熟抗体;θmax、θmin分别为关节空间的上边界向量和下边界向量;其余相关说明与前述相同。

给定机械臂末端执行器的期望位姿为

表3 IBCECSA和BCECSA算法的运动学逆解寻优性能对比

表4 IBCECSA和BCECSA算法的最优逆解比较

由表3可知，IBCECSA算法获得了100%的运动学逆解寻优成功率，与BCECSA算法相比，IBCECSA算法运动学逆解的各项寻优指标具有明显的优势；BCECSA算法的运动学逆解寻优成功率为0%，说明BCECSA算法每次试验的寻优结果都无法满足指定的位姿精度要求。由图4可知，IBCECSA算法的平均适应值收敛曲线获得了最低的下降位置，同样说明了IBCECSA算法的整体收敛质量优于BCECSA算法。由表4进一步可知，BCECSA算法最优逆解对应的g(θ)=9.438 541×10-5>0，即BCECSA算法的最优逆解违反了约束条件；而IBCECSA算法最优逆解对应的g(θ)=-4.305 329×10-7<0，即IBCECSA算法的最优逆解满足给定的位姿精度要求。从图5可以直观观察BCECSA和IBCECSA算法的最优逆解对应的机械臂实际位姿。

5 结束语

为有效解决冗余机械臂的逆运动学问题，受前期研究成果BCECSA算法双层协同进化架构的启发，本文进一步提出了改进的BCECSA算法，即IBCECSA算法。IBCECSA算法采用了双策略协同进化的底层进化方案以及与BCECSA算法不同的顶层免疫克隆选择进化思路，使得其能够更好地平衡算法的全局探索与局部开发。在基准函数仿真实验中，IBCECSA算法获得了与BCECSA算法相近的收敛精度与寻优稳定性，但在算法的计算成本与收敛速度方面略逊于BCECSA算法，这也说明IBCECSA算法保持种群多样性的能力更强。以6连杆平面冗余机械臂的逆运动学求解为例展开试验对比分析，结果表明IBCECSA算法获得了很好的运动学逆解寻优结果，可用于有效解决冗余机械臂的逆运动学问题。后续将研究基于IBCECSA算法的多目标优化问题求解，进一步拓展算法的应用范围。