基于CISSA的冗余机械臂逆运动学求解*
时间：2023-06-25 12:40:04 来源：雅意学习网 本文已影响 人 
周文辉，贾振堂，王 磊
(上海电力大学电子与信息工程学院，上海 200120)
机器人运动学有正逆两部分，是关于机械臂关节角与末端位姿转换的关系。根据给定的各个关节的角度确定机械臂末端的位置姿态是机械臂正运动学，结果是唯一确定的；
根据机械臂末端的位姿反过来求解机械臂对应的关节角是机械臂逆运动学，其解通常不唯一。目前常见的方法有几何法[1]、解析法[2]、数值法[3]和人工智能方法[4]等。
冗余机械臂在运行的过程中能够躲避奇异位形，防止运动关节超限，有很好的避障能力，同时具备很高的灵活性，但与此同时冗余机械臂的逆运动学求解也变得更加复杂。因为冗余机械臂的机械结构不符合Pieper准则，所以传统意义上的解析法与几何法无法求得冗余机械臂对应的封闭逆解，而数值法的雅可比矩阵迭代法，求解过程比较复杂，过于依赖种子的设置，求解质量难以得到保证[5]。冗余机械臂的逆运动学方程是强耦合性的，非线性的方程组，因此可以将机械臂求逆解问题转化为群智能优化问题[6]。这类算法包括粒子群算法[7]、遗传算法[8]、麻雀搜索算法[9]以及神经网络算法[10]等。
吕鑫等[11]提出改进麻雀搜索算法，受鸟群算法中飞行行为的启发，对鸟群中发现者的位置进行更新；
张伟康等[12]提出的自适应t分布与黄金正弦改进的麻雀搜索算法，通过黄金正弦算法改进发现者的位置更新方式。段玉先等[13]提出基于sobol序列和纵横交叉策略的麻雀搜索算法。上述文献从多个方面对麻雀搜索算法进行改进，虽然在函数初始化过程中增加了种群的多样性，在一定程度上防止寻优函数过早陷入局部最优，但是收敛效果不明显，全局与局部搜索不协调的问题仍旧没有明显改善。
本文针对传统麻雀搜索算法的特性与不足之处，提出了一种结合改进Tent混沌扰动初始化策略，自适应参数调整策略，柯西-Tent扰动策略的综合改进麻雀搜索算法，最后利用MATLAB软件对算法进行仿真实验，验证CISSA算法在冗余机械臂运动学求逆解的有效性。
1.1 标准麻雀搜索算法
麻雀搜索算法(SSA)是受麻雀作为群居动物进行觅食并对捕食者作出反捕食行为得到启发而提出的群智能算法[14]。群体在觅食过程中有探索者、跟随者和预警者3种角色；
探索者担任觅食和导向的职责；
跟随者采取跟随策略来获取食物；
预警者负责警戒。
(1)探索者的位置更新公式为：
(1)
式中，常数量有itermax、L与ST； (2)追随者的位置更新公式为： (2) 式中，常量N为种群总数； (3)预警者的位置更新公式为： (3) 式中，常数量有ε，设置它是为了避免分母为0； (1)基于混沌Tent映射的种群初始化。在群体智能算法寻优性能指标中，寻优性能与初始种群相关紧密。若初始化种群均匀地分布在解空间中，这有利于快速地锁定全局最优解[15]。麻雀搜索算法采用随机策略生成初始化种群，这种策略生成的种群满足随机性，但存在分布不均，遍历性不够的缺点，在算法初期不能保证搜索范围的广度。基于此，在种群初始化过程中引用改进的混沌模型替代随机生成策略，利用Tent映射[16]遍历性，随机性的特点初始化种群，减弱随机种群遍历性不够的影响，进而提高SSA算法的全局搜索性能。 改进Tent映射的数学表达式为： (4) 式中，i为种群规模，i=1,2,3,…N； 图1 混沌映射与随机映射直方图对比 (2)改进探索者位置更新公式。群智能算法在进化过程中可分为两部分，全局定位搜索与局部细化。在算法的初始化阶段，主要依靠全局搜索来定位最优解的大致范围，但同时也极易陷入局部最优，为了杜绝这种情况的发生，可以让种群中的探索者，参考上一代的最优解。由于在算法运算过程中探索者引导着整个麻雀群体的流动，所以对群体中探索者位置的改进具有重要的意义。因此引入了动态权重W1、W2，这两个动态参数随着种群搜索发生相应的变化，在全局搜索时保持较大的值，增强全局搜索能力，在局部搜索时，下降到相应较小的值，加大局部搜索能力。 (5) 式中，p∈[0,1]，q∈[-0.5,0.5]服从均匀分布，d=(Wmax-Wmin)。 (a) 权重W1 (b) 权重W2图2 麻雀搜索算法与综合改进麻雀搜索算法权重变化对比 改进后探索者的位置更新公式为： (6) (3)种群探索者数量自适应调整。在SSA算法中，在算法最初就确定了探索者与跟随着在种群中的比例。探索者在种群中的比例过大导致算法在迭代后期无法快速收敛，不能进行精确的局部搜索； (7) 式中，r为探索者在群体中的比例； (4)柯西变异和Tent扰动。柯西变异[18]来源于柯西分布。柯西分布特征为：两端具有较长尾翼，这种分布使得个体具有更高的概率跳跃至更好位置，脱离局部最优。将柯西算子引入当前最优解的位置更新中，利用柯西算子的调节功能，避免算法陷入局部最优。具体表示为： Xnew=cauchy×Xbest (8) 式中，cauchy为服从柯西分布的柯西算子； Tent扰动基于Tent混沌映射，在种群中的麻雀适应度都较高，且有向适应度低的方向发散的趋势的时候，此时要对种群中的个体进行Tent扰动。利用式(4)对个体进行混沌扰动。 柯西变异与Tent扰动针对于两种情况，对低适应度的个体进行柯西变异，对高适应度的个体在原有位置的基础上进行扰动，使得算法不至于陷入局部最优，并且在算法的后期能不断提升收敛精度。 (5)越界处理。对于越界的个体，通常的做法是让他的值等于边界值，这种做法降低了算法的搜索能力。为了避免这种情况发生，将越界的种群个体按式(4)再次放回种群中，通过对越界量的弹性处理，解决了算法在迭代过程中种群丰富度降低的问题，加快了算法的寻优效率。 基于CISSA的算法的运行步骤如下： 步骤1：对麻雀搜索算法中麻雀种群的初始数量，种群中探索者和跟随者的比例，预警值，安全值，随机值等参数变量进行初始化赋值； 步骤2：利用改进的Tent混沌映射生成初始化种群，确定种群中成员的初始位置； 步骤3：计算个体的适应度，然后获得最高适应度与最低适应度麻雀的位置信息； 步骤4：确定种群中发现者的数量，然后根据式(6)确定探索者更新之后的位置信息； 步骤5：确定麻雀种群中跟随者的数量，根据式(2)计算其更新过后的位置； 步骤6：根据式(7)，随着迭代次数的增加，更新发现者与跟随者的数量随之更新； 步骤7：确定种群中预警者的数量，然后根据式(3)确定预警者在更新之后的位置信息； 步骤8：对于越界的麻雀个体，根据式(4)重新放入种群中； 步骤9：计算每只麻雀的适应度值，并将之和种群的平均适应度值进行比较。若适应度值比平均值低，则进行柯西变异，变异后个体更优则更新； 步骤10：在更新之后，确定种群中适应度值最高和最低的麻雀的位置信息； 步骤11：当算法迭代次数达到设定的最大值时，便输出适应度最高的个体的位置信息； 图3 算法流程图 仿真硬件设备：CPU处理器：Intel®CoreTMi5-8300H 2.3 GHz； 仿真测试中用3个标准测试函数对CISSA与SSA进行对比分析，这3个标准测试函数的定义为： (1)Sphere函数 (9) (2)Rosebrock函数 (10) (3)Step函数 (11) 改进麻雀搜索算法(CISSA)和标准麻雀搜索算法(SSA)算法测试结果，如图4所示。 (a) Sphere函数 (b) Rosenbrock函数 从图4的3个测试函数可以看出，在收敛速度与收敛精度这两方面来说，CISSA的表现优于SSA，这主要是因为CISSA在迭代过程中探索者的位置更新公式会自适应调整，探索者的数量也会根据迭代次数进行调整，在算法初始阶段对全局进行充分的搜索，在算法的后期加强局部精确搜索，使得到的最优解精确度更高，并加快收敛的速度。 为了进一步证明CISSA的性能，在之前3个标准测试函数的基础上，增加了3个测试函数。同时为了证明CISSA的优越性，选取传统的群智能算法进行对比，对比算法选取粒子群算法(PSO)[19]、鲸鱼优化算法(WOA)[20]以及灰狼优化算法(GWO)[21]。 测试函数如下： (1)Ackley函数 (12) (2)Salomon函数 (13) (3)Griewangk函数 (14) 上述群智能算法在测试函数上独立运行60次，选取最优收敛精度，平均收敛精度与平均收敛时间作为性能指标，算法内部设置的最大进化次数为1000，对比数据如表1所示。 表1 算法对比数据 表1数据显示，在最优收敛精度与平均收敛精度方面，CISSA的性能明显优于传统群智能算法。比精度最高的灰狼优化算法高4～5个数量级； 图5为1类7自由度冗余机械臂的结构简图，利用标准D-H法[22]进行正运动学建模，机械臂各关节D-H参数如表2所示。 图5 七自由度机械臂结构简图 表2 冗余机械臂各关节D-H参数 机械臂相邻两个连杆的坐标系齐次变换矩阵如下： (15) 式中，sθ是sinθ的简写； (16) P=(α,β,γ,PxPy,Pz) (17) 式中，R3×3为姿势矩阵； 为了减少输入变量的个数，本文采用欧拉角表示机械臂末端的位姿，按照式(16)将机械臂的姿态矩阵进行欧拉变换得到α、β、γ三个对应的欧拉角。欧拉变化将之前机械臂末端位姿矩阵的12个变量转换为6个独立变量。变换公式如下： (18) P=(α,β,γ,PxPy,Pz) (19) 式中，P为位姿矢量。 将机械臂的目标位姿与当前位姿的作差值并求出平方和来构建最终的适应度函数，α1=(α-α0)2，β1=(β-β0)2，γ1=(γ-γ0)2，P1=(Px-Px0)2，P2=(Py-Py0)2，P3=(Pz-Pz0)2。 f1=(α,β,γ,P1,P2,P3)=α1+β1+γ1+P1+P2+P3 (20) 冗余机械臂在实际求逆解的过程中存在着不止以一组解。每次式(18)只是求得了一组符合条件的解，要想机械臂能够在空间中平滑柔顺地运行，可以定义如下函数： (21) 式中，是机械臂第i个关节角当前时刻与该关节角前一时刻关节角的差值。 f3=min(f1+ρf2) (22) 式中，f1为逆运动其中一组解； 为检验CISSA在求冗余机械臂逆运动学解中 的可行性与有效性，通过计算机建立仿真实验将CISSA、SSA、GWO、WOA、PSO分别应用于空间点到点运动的工况展开对比验证。 本文选取基本麻雀搜索算法(SSA)、鲸鱼算法(WOA)、灰狼算法(GWO)、粒子群算法(PSO)作为对照，为保证实验的公平性，4种算法的种群规模设置为30，最大迭代次数为200，其他参数设置如表3所示。 表3 算法参数 给定一组关节角θ(rad)=[1,1,1,1,1,1,1]，代入式(15)求得正运动矩阵T。 (23) 针对末端位姿，5种算法分别在工作空间内对冗余机械臂求逆解，结果如表4所示。 表4 5种算法各自求得一组机械臂逆解 图6为5种算法位姿求解曲线对比图，结果表明：CISSA明显优于其他算法。CISSA达到收敛条件时的速度比SSA提高了40%左右，CISSA最终收敛的精度也明显高于其他算法。为了验证算法的稳定性，同时避免结果的偶然性，将5种算法独立对位姿求解200次，结果如图7所示。 图6 5种算法位姿求解曲线图对比 (a) WOA算法 (b) PSO算法 可以看出，CISSA的算法精度是高于其他算法，从收敛曲线来看，CISSA的曲线波动比较小，最终的位姿误差维持在10-7数量级。 针对冗余机械臂求逆解问题，本文提出了一种综合改进麻雀搜索算法。在算法初期，利用改进Tent混沌序列，增加种群遍历性，增强了算法整体寻优定位的能力；
itermax为最大迭代次数；
L为一个d维的单位矩阵；
ST∈[0.5,1]为探索者在野外探索的安全阈值；
随机变量有α、Q；
α∈(0,1]增加探索者在探索过程中的随机性与遍历性；
Q服从正态分布；
t为当前迭代数；
Xi,j为种群中第j维，第i只麻雀的状态信息。当R2
变量有A，是一个d维的矩阵，矩阵中的元素随机取1、-1，A满足A+=AT(AAT)-1；
Xp、Xworst分别为探索者当前能探索的最优位与最差位置。当i>N/2时表示种群中第i只追随者获取食物不足，它要尽可能远离位置最差的那只麻雀；
其他麻雀依然是在最优位置的麻雀附近寻找食物。
随机变量有β和K，β为一个服从正态分布的步长控制参数；
K∈[-1,1]，调节预警者的位置信息；
Xbest与Xworst分别为种群的最优位置和最差位置；
fi为第i只麻雀的适应度值；
fg、fw分别为最高与最低的适应度值。当fi≠fg表示该麻雀离群，易被偷袭，当fi=fg表示处于种群中间的麻雀意识到了危险，需要向身边伙伴靠近以避免偷袭。1.2 改进麻雀搜索算法(CISSA)
j对应于个体空间维度；
μn为混沌参数；
N为种群总数，确定混沌参数并对式(4)中X取相应值后，可得到初始种群，然后映射至搜索空间。对麻雀种群进行混沌初始化与随机初始化的对比如图1所示。由图1可以看出，改进后的Tent映射分布更加均匀。
相对地，探索者在种群中比例过低会使得算法在迭代的前期无法对全局进行彻底的搜索[17]，降低了函数宽范围全局搜索的能力。所以为了能够让算法在迭代初期能够充分进行探索，迭代后期能够快速地收敛。提出了探索者数量自适应调整策略：该策略在算法迭代初期，探索者占种群的大多数，随着算法迭代次数的增加，种群中探索者相应地减少，直至到一个临界值。这种策略能够很好地兼顾全局搜索与局部搜索的优点，在算法的初期，不注重收敛精度，能很快地锁定全局最优解的范围，使得算法快速收敛；
在算法的中后期，在前期限定的范围内，强化局部搜索，提高算法的收敛精度。种群中探索者数目地调整公式为：
Nmax为探索者在种群中设置的最大数量；
Nmin为探索者在种群中设置的最小数量，相应地PNum，PNum=r×N。PNum为种群中探索者的数量。
Xbest为当前适应度最优的个体；
Xnew为经过柯西变异的最优个体。1.3 改进麻雀搜索算法执行步骤
反之，利用Tent混沌映射对种群中的个体进行扰动，扰动后个体更优则更新；
否则接着从第4步开始继续执行。
1.4 有效性测试
RAM内存：8 G ddr3 1600；
硬盘：1 TB；
软件平台：MATLAB 2016b。
但在平均计算时间指标上，CISSA算法稍落后于其他对比算法，因为他相比于传统算法在进化过程中多了一些处理步骤。上述实验表明，CISSA算法明显优于其他常用的群智能算法。2.1 建立机械臂坐标系
2.2 运动学分析
cθ是cosθ的简写。根据式(15)和表1的DH参数，将机械臂7个齐次变换矩阵依次顺序相乘便可得到最终机械臂末端位姿矩阵：
p为位置矢量。2.3 欧拉变换
2.4 构建适应度函数
f2为关节运动最小变化量；
f3为最终的适应度函数。3.1 参数设置
3.2 实验结果分析
在算法迭代的过程中，又引入了柯西变异与Tent扰动增强了算法的抗停滞能力；
在算法的中后期，对SSA的位置更新公式进行了优化，对种群中探索者数量进行自适应调整，大大提升了算法的精准度与收敛速度。基准测试函数的结果表明CISSA相比于SSA与其他传统算法在求解稳定性与求解准确度上有显著的进步，验证了改进算法的有效性。运用 CISSA对冗余机械臂求逆解，相比于PSO、WOA 、GWO等传统算法在平均值与标准差上有明显的提高。SSA算法的提出时间虽然较短，但是相比于其他传统算法有一定的优越性，后继可以继续融合其他算法的优点对SSA算法进行继续改进，并更广泛地运用到实践中来。
