煤矿救援机器人机械臂轨迹规划算法研究

韩涛， 李静， 黄友锐， 徐善永， 许家昌

(安徽理工大学 电气与信息工程学院，安徽 淮南 232001)

0 引言

恶劣的开采环境导致煤矿安全事故频发。煤矿救援机器人能在复杂危险的矿井灾害环境中灵活运动，从而提升救援效率，减少煤矿灾难造成的人员伤亡。煤矿救援机器人机械臂轨迹规划即在工作空间中寻找与时间相关的运动路线[1]。机械臂轨迹规划算法的性能直接影响机器人机械臂的运动效率，因此其研究具有重要意义。

根据规划空间不同，轨迹规划分为关节空间轨迹规划和笛卡尔空间轨迹规划[2-3]。关节空间轨迹规划以各关节角度为规划对象，求取关节角度、角速度、角加速度，而笛卡尔空间轨迹规划以末端位姿为规划对象，通常以位姿坐标形式描述关键点。笛卡尔空间轨迹规划方法存在奇异解问题，而关节空间轨迹规划方法不会出现奇异解问题且计算较简单，因此，关节空间轨迹规划应用更广泛。

常用的机械臂轨迹规划算法有多项式插值算法和样条插值算法，其中三次样条插值、B样条插值适用于有复杂连续任务要求的运动[4-5]，三次多项式和五次多项式插值算法被广泛应用于点到点运动和简单连续运动[6-8]。为了保证角速度和角加速度均在可控范围内，本文选用五次多项式插值算法在机械臂关节空间进行轨迹规划。

轨迹规划算法的最终目的是寻找机械臂运动时目标函数的最优值。随着智能仿生算法的发展，遗传算法[9-10]、粒子群算法[11-12]、蜂群算法等[13]被应用于机械臂轨迹规划中，但传统的智能优化算法存在搜索时间长、规划轨迹不够光滑、能量消耗高的问题。因此，研究者提出了基于改进智能优化算法的轨迹规划方法。李国洪等[14]通过聚类方法对遗传算子进行综合调控，并通过新的变异操作提高算法后期的寻优效率，从而有效缩短了轨迹规划时间。邓伟等[15]利用双种群遗传算法全局搜索能力强、进化速度快和混沌算法局部搜索能力强的优点，提出了一种基于混沌局部搜索的双种群遗传最优时间轨迹规划算法，使机械臂末端执行器运行轨迹平滑且时间最优。A.Khoukhi等[16]利用神经网络并行处理、分布存储的特点及良好的自学习能力，提出了一种求解逆运动学问题的神经模糊网络，通过对最小时间和相应力矩的预处理建立神经模糊控制器，生成最优时间轨迹力矩。Y.L.Hsu等[17]提出了一种利用永磁同步电动机对切换机构进行节能轨迹规划的方法，采用实时编码遗传算法确定多项式系数，最终实现了能量最优控制。

针对现有算法存在的轨迹规划不合理、算法收敛速度慢等问题，为了同时满足轨迹规划的高精度和高速率要求，本文在上述研究的基础上，提出了一种基于融合杜鹃搜索的灰狼优化(Grey Wolf Optimization with Cuckoo Search，CS-GWO)算法的煤矿救援机器人机械臂轨迹规划算法。该算法在灰狼优化(Grey Wolf Optimization，GWO)算法的位置更新中融入杜鹃搜索(Cuckoo Search，CS)算法中出现的2次扰动，成功解决了GWO算法容易陷入局部最优解的问题，增强了算法的全局搜索能力，使机械臂轨迹规划算法能够更快收敛，且各关节角位移、角速度和角加速度曲线均光滑、连续，提高了轨迹规划的精度和速度。

1 关节空间轨迹规划

设六维向量θ=[θ1,θ2,θ3,θ4,θ5,θ6]，θ1—θ6分别对应6个关节角，则五次多项式定义为

θ(t)=a0+a1t+a2t2+a3t3+a4t4+a5t5

(1)

式中：t为时间；a0—a5为多项式系数。

(2)

由五次多项式定义得

(3)

由式(2)和式(3)可以求出五次多项式的各项系数为

(4)

2 GWO算法

GWO算法源于灰狼种群的捕食机制。灰狼种群等级划分如图1所示。第1层是种群的领导者，称为α狼，主要负责狩猎、食物分配等决策事宜。第2层为β狼，主要协助α狼进行决策，当α狼淘汰或死亡时，会有相应的β狼来替补。β狼一方面将α狼的命令下达给其他灰狼，另一方面将其他灰狼的行动情况反馈给领导者α狼，起着重要的桥梁作用。第3层为δ狼,δ狼听从α狼和β狼的命令，主要负责放哨、侦查、看护等任务。最底层为ω狼，主要负责种群内部关系的平衡。

图1 灰狼种群等级划分

依据适应度大小将狼群分为4个等级，其中最好的前3只灰狼依次为α，β和δ，其余的记为ω。GWO算法的主要思想如下：确定适应度最好的3只灰狼α，β和δ，作为最了解猎物信息的灰狼组；在算法迭代时，整个狼群会在灰狼组α，β和δ的带领下更新位置信息；狼群逐渐包围攻击猎物，以获得最优解。

GWO算法步骤如下：

(1)初始化狼群位置，设置最大迭代次数nmax，并初始化系数A,C,b。其中A和C为系数向量，A决定了更新后的灰狼位置向目标物靠近的程度;C主要决定了更新后灰狼位置相对于目标物的方位；系数b的变化决定了A的取值范围，随着算法迭代次数的增加，b由2线性递减到0。计算狼群中每只灰狼的适应度值，确定适应度最好的3只灰狼为α，β，δ。

(2)根据灰狼位置更新规则计算灰狼更新后的位置。

(3)计算更新后的每只灰狼的适应度值。

(4)更新α，β，δ狼的位置Xα，Xβ，Xδ及参数b，A，C的值。

(5)判断是否满足迭代条件，若满足则返回到步骤(2)继续执行，否则输出Xα及其适应度值。

3 CS算法

CS算法是一种模仿杜鹃鸟寻窝产卵活动的群集智能优化算法。CS算法中，每只杜鹃的寄生巢位置代表待求问题的1个可行解。每只杜鹃每次只会在1个寄生巢中产1个卵。在搜索鸟巢过程中，最优秀的寄生巢才会被留到下一代。每个寄生巢的主人都有一定的概率察觉自己的巢中有外来卵而放弃该鸟巢。寄生巢被放弃后，杜鹃会重新随机选择1个鸟巢作为新的寄生巢。

为了生存繁衍，杜鹃有2种产卵策略：

(1)莱维飞行。杜鹃将通过莱维飞行所找到的寄生巢与之前的寄生巢对比，选择较优的寄生巢作为下一代的寄生巢。

(2)随机选择。每个寄生巢的主人都有一定概率发现自己的巢被寄生。发现后，杜鹃将随机选择1个新的鸟巢作为自己的寄生巢。

杜鹃搜索鸟巢的过程就是在当前寄生巢的位置上按照一定的飞行规则寻找新的寄生巢的过程。该过程可描述为

xi(l+1)=xi(l)+ψ⊕L(λ)

(5)

式中：xi(l+1)为第i个寄生巢更新后的位置，l为当前迭代次数；xi(l)为第i个寄生巢的当前位置；ψ>0为控制步长的变量；⊕表示点乘运算；L(λ)为服从当前迭代次数l的莱维随机搜索路径，其概率分布为

L～u1=l-λ, 1<λ<3

(6)

式中：u1为服从正态分布的变量；λ为指数变量。

4 CS-GWO算法

4.1 算法原理

灰狼的猎食过程可分为包围猎物、狩猎和攻击猎物3个阶段。包围猎物阶段的行为可描述为

D=|CXp(n)-X(n)|

(7)

X(n+1)=Xp(n)-AD

(8)

C=2r2

(9)

A=2br1-b

(10)

(11)

式中:D为当前灰狼与猎物的距离；Xp(n)为当前猎物的位置向量，n为迭代次数；X(n)和X(n+1)为第n代和n+1代灰狼的位置向量；r1，r2为[0,1]的随机向量。

狩猎时整个狼群会在α，β，δ的带领下更新位置信息，该过程可描述为

(12)

(13)

(14)

式中：Dα，Dβ，Dδ为当前灰狼与α，β，δ之间的距离；C1，C2，C3分别为Xα，Xβ，Xδ的随机权重，其作用是增强或减弱3只最优灰狼的影响，以改变灰狼的随机搜索行为，可通过式(9)计算；A1，A2，A3分别决定了更新后位置为X1(n)，X2(n)，X3(n)的灰狼向3只最优灰狼靠近的程度，可通过式(10)计算。

在GWO算法中，灰狼狩猎时对未知猎物的勘探能力主要取决于参数A和C。当|A|>1时，灰狼个体会远离猎物位置，对空间进行全局搜索；当|A|<1时，灰狼向猎物发起攻击，对附近的空间进行局部搜索。参数C为猎物提供了一个随机权重，当|C|>1时，会强化猎物影响，灰狼要靠近猎物更加困难，会对空间进行全局搜索；当|C|<1时，会弱化猎物影响，使灰狼更加容易接近猎物，会对周围空间进行局部搜索。

GWO算法虽具有较强的搜索能力，但存在以下缺点：① 始终跟随α,β,δ对狼群的位置进行更新，这种优秀灰狼组的引导导致算法很难跳出局部最优解，容易发生早熟现象。② 在迭代后期，|A|<1时，提供的主要是局部搜索能力，缺少全局搜索能力，较易陷入局部最优。

CS算法中有2种选择寄生巢策略：莱维飞行和随机选择。在GWO算法中结合莱维飞行方式进行搜索，可以扩大搜索代理群体的搜索范围，增加搜索代理个体搜索的随机性，使算法在运行过程中能跳出当前局部最优点，寻找更优解。采用Mantegna算法模拟莱维飞行模式，算法公式为

(15)

式中：s为莱维飞行的路径；γ为莱维指数，取值范围为[0,2]；u2，ν为服从正态分布的变量，其服从的正态分布为

(16)

(17)

式中Γ为Gamma函数，其定义为

(18)

将CS算法中出现的2次扰动过程融入到GWO算法的位置更新中，分2个部分进行：第1部分是先对GWO算法中适应度最好的α,β,δ的位置进行扰动，然后选择位置较好的灰狼，将莱维飞行方式应用到前3只灰狼的位置更新中，再通过GWO算法计算狼群位置；第2部分是采用预先设定的发现概率对灰狼种群位置进行扰动，并保留位置较好的灰狼，将CS算法中的鸟巢位置随机更新方式应用到整个狼群的位置更新中，使得狼群在向猎物逼近的过程中能够随机跳出局部搜索区域，扩大了搜索范围，避免算法陷入局部最优解，增强了GWO算法的全局搜索能力。

4.2 算法流程

CS-GWO算法流程如图2所示，具体实现步骤如下：

图2 CS-GWO算法流程

(1)初始化种群和算法参数，设置最大迭代次数nmax、参数b,A和C及狼群被发现概率，计算狼群适应度fitness，记录α，β，δ的初始位置。

(2)使用莱维飞行机制对α，β，δ的位置进行扰动，并保留较好的位置。

(3)根据式(14)更新每个灰狼个体的位置。

(4)判断灰狼个体是否被发现，若被发现，则对整个灰狼种群的位置进行扰动，对比更新前后位置，择优保留位置较好的灰狼；否则，保留上一代灰狼位置。

(5)计算种群中每个个体的适应度fitness,更新Xα,Xβ,Xδ，b，A和C。

(6)判断是否满足迭代条件，若不满足则输出Xα及其适应度fitness，若满足则返回到步骤(2)继续执行。

5 基于CS-GWO算法的机械臂轨迹规划

5.1 约束条件

表1 机器人关节角约束条件

5.2 目标函数

在优化目标的选择上，机器人工作效率和能量消耗一直是备受关注的问题。本文综合考虑时间和能量为目标函数，采用五次多项式插值法进行轨迹规划。对于1条由m个关键点H1,H2,…,Hm组成的轨迹，建立轨迹的能耗模型：

(19)

式中：E为消耗的能量；tj为点Hj与Hj+1之间的运动时间；τj为关节力矩；dqj为关节角位移；ω(t)为关节角速度。

通过加权系数法得到优化目标函数F(tf,E)：

(20)

式中：ξ1,ξ2为归一化权因子;η为弹性系数。

最终建立的适应度函数为

(21)

通过CS-GWO算法搜索适应度函数值最大时的tf与E，即得到所需的最优解。

6 仿真分析

6.1 仿真条件

为了验证CS-GWO算法的效果，使用Matlab中的Robotics Toolbox工具箱进行机械臂仿真建模。在仿真中，规划出1条起始点为H1、中间点为H2、终点为H3的闭合工作路径，给定点H1,H2,H3的位姿，根据逆运动学原理解得其对应关节角。指定H1,H2,H3的初始速度和加速度均为0，H1到H2、H2到H3、H3到H1的运动时间分别为t1，t2，t3。设H1，H2，H3的位置为X1=(1.4 m 01.583 m)，X2=(2.3 m -0.6 m 0.75 m)，X3=(2.3 m -0.6 m 0.75 m)；末端姿态为R1=(0 -3.142 0)，R2=(-2.793 1.484 4.712)，R3=(-2.793 1.484 4.625)；解得对应关节角：q1=(1.571 -1.571 0 0 1.571 -1.571)，q2=(1.189 -0.470 -0.655 1.483 1.4940.477)，q3=(1.721 -0.601 -0.356 1.8601.046 0.688)。

6.2 结果分析

设种群规模为120,最大迭代次数为50，分别用CS-GWO、GWO和CS算法进行轨迹规划。弹性系数η的值取决于时间和能量的增长速度的比率，本文设弹性系数η=0.003。ξ1,ξ2取经验值，ξ1=0.5，ξ2=0.5。仿真结果见表2，其中Std为最优目标函数的标准差。

从表2可知，CS-GWO算法寻得的最优时间序列为(1.613 7 s，1.623 9 s，1.498 1 s)，寻得的平均最优目标函数值为10.679 2。相较于CS算法和GWO算法，CS-GWO算法得到的平均最优目标函数值最小，搜索到的解最优，全局搜索能力最强。另外，CS-GWO算法的标准差最小，说明其稳定性最好。

表2 轨迹规划仿真结果

算法寻优曲线如图3所示。可以看出，3种算法的寻优曲线最终趋势一致，但从收敛速度来看，GWO算法和CS-GWO算法都能较快收敛到最优值，明显比CS算法的收敛速度快。

图3 寻优曲线对比

综合来看，CS-GWO算法能够有效提高CS算法的收敛速度，提高GWO算法的全局搜索能力，其稳定性更好，整体性能较优。优化后关节1—关节6轨迹规划仿真结果如图4—图9所示。从关节角位移仿真结果可以看出，机械臂末端轨迹角位移曲线均较平滑，说明仿真过程中6个关节能够在规定的路径点之间较平滑地进行轨迹运动。从角速度曲线可以看出，6个关节的角速度变化都比较平稳，说明运动过程中角加速度均没有发生突变，且均满足机械臂约束条件。光滑、连续变化的角速度和角加速度轨迹曲线说明机械臂在运动过程中所受的冲击力小，能够保证电动机平稳运行，提高了机器人的工作效率。仿真结果验证了将CS-GWO算法应用于机器人机械臂轨迹规划的可行性与有效性。

(a)关节角位移

(a)关节角位移

(a)关节角位移

(a)关节角位移

(a)关节角位移

(a)关节角位移

7 结语

为了使煤矿救援机器人机械臂有效规划出一条可行关节角轨迹，提出了一种基于CS-GWO算法的煤矿救援机器人机械臂轨迹规划算法。以五次多项式插值为基本轨迹规划方法，采用CS-GWO算法对得到的轨迹进行优化，利用CS算法中的莱维飞行模式和鸟巢位置随机更新方式对基本GWO算法中灰狼位置进行扰动，使得狼群能够跳出局部最优解，扩大搜索范围，增强全局搜索能力。仿真结果表明：与基本GWO算法和CS算法相比，CS-GWO算法能够以较快的速度收敛到最优值，且寻得的平均最优目标函数值更小；从各关节优化后的运动轨迹曲线可以看出，各关节角位移、角速度和角加速度曲线均光滑、连续，角速度和角加速度无突变且满足约束条件。仿真结果验证了CS-GWO算法在搜索能力方面的提升，以及将其应用在煤矿救援机器人机械臂轨迹规划上的可行性与有效性。