基于改进灰狼算法的冗余机械臂轨迹跟踪与避障

崔靖凯，周宇飞，贺顺锋，徐振邦，朱明超*

（1.中国科学院 长春光学精密机械与物理研究所，吉林 长春 130033；2.中国科学院大学，北京 100049）

1 引言

机械臂在工业自动化的进程中具有不可替代的作用，由于其灵活性高、稳定性强、成本低等优势，已被广泛应用于各个领域［1-2］。在工业生产中，机械臂常用于搬运、组装、焊接等任务。若机械臂的自由度数大于完成任务所需的最小自由度数，则称其为冗余机械臂［3］。冗余的自由度有助于实现避障等次要的任务目标。但同时，冗余也带来了诸多挑战，这体现在运动规划的计算量增大，以及逆运动学的多解性和奇异性等方面。使冗余机械臂在跟踪目标轨迹的同时避开障碍物，是精密制造的重要需求，而将轨迹跟踪和避障规划统一到同一框架，设计一种计算简单且通用的解决方法，是一项具有实际意义和挑战性的工作［4］。

机械臂的轨迹跟踪问题是指设计控制指令使机械臂的末端执行器沿指定的目标轨迹运动。若目标轨迹在笛卡尔工作空间给定，需要通过逆运动学将其映射到关节空间。对于冗余机械臂，逆运动学映射不是唯一的，即同一条笛卡尔轨迹将被反解为无限多条关节轨迹。传统方法使用雅可比矩阵伪逆（Jacobian-Matrix-PseudoInverse，JMPI）来解决冗余问题［5］。然而，JMPI 方法具有很多局限性。首先，它只适用于求解等式约束，因此无法考虑关节角的限制；其次，它无法解决避障问题，因为避障常被建模为不等式约束；此外，JMPI 方法的计算量很大，难以应对冗余机械臂的多解性和奇异性等挑战。

近年来，诸如遗传算法、粒子群算法等元启发式优化算法被广泛应用于轨迹跟踪。这些以优化为核心的方法能够在满足轨迹跟踪需求的前提下解决额外的不等式约束，且不依赖于机械臂的构型和关节数量，计算量小、通用性强。LOPEZ-FRANCO C［6］等提出了一种元启发式框架，通过最小化末端执行器的位置和方向误差完成冗余机械臂的轨迹跟踪任务；ROKBANI N［7］等基于β-SSA 算法设计了逆运动学求解器，并将其应用于冗余机械臂的圆形轨迹跟踪。然而，这些方法没有考虑到机械臂与周围环境可能存在的碰撞问题。

避障是冗余机械臂在工业应用中应该满足的重要需求，它能够保证生产的安全和稳定。传统的避障方法使用“人工势场”的概念，例如姜力［8］等利用虚拟排斥力实现了七自由度冗余机械臂的避障任务。元启发式优化算法凭借其灵活简单的优势，成为解决避障问题的新趋势。AGARWAL D［9］等使用改进的黏菌算法规划了自主移动机器人的无碰撞路径；ZHANG J X［10］等将障碍物视为圆，将机械臂连杆简化为直线，基于改进的粒子群算法实现了冗余机械臂的避障规划；常宁东［11］等基于改进的遗传算法，设计了野外环境下车辆的无碰撞路径。传统的优化方法通常将避障作为一系列不等式约束，这些约束不会鼓励优化器主动避障，而是被动地将解决方案标记为不可行的解。

本文提出了一种避障跟踪优化器，通过制定适应度函数来实现冗余机械臂的轨迹跟踪和避障规划。适应度函数有两个目标：首先是轨迹跟踪，即最小化机械臂末端执行器与目标轨迹点之间的位置误差；其次是避障规划，即最大化机械臂连杆与障碍物之间的距离。不同于传统方法，本文将避障设计为奖励项，而不是作为不等式约束。避障奖励项可以实时评估解决方案的优劣，以主动奖励优化器避开障碍物。因此，跟踪和避障的本质被简化为求解同一个优化问题。

GJK（Gilbert-Johnson-Keerthi）算法［12］是一种高效的碰撞检测算法，它可以计算任意形状的三维凸多边形之间的最小距离。在避障空间建模时，本文采用GJK 算法进行碰撞检测。该方法考虑了障碍物与机械臂连杆的几何形状，而不是将其简化为点或直线。因此，避障空间更切合实际情况，碰撞检测的准确度得以保证。

为了求解优化问题，本文采用了一种元启发式算法——灰狼算法［13］。灰狼算法模拟了自然界中灰狼的社会阶级和狩猎行为，结构简单、收敛性强。然而，经典灰狼算法存在过早收敛和种群多样性低的缺点［14］。本文通过引入随机分散策略提出了一种改进的灰狼算法，以增强算法的全局搜索能力。九自由度冗余机械臂被用于验证所提出方法的有效性和优越性。

2 避障跟踪优化器

本节提出了一种避障跟踪优化器，将冗余机械臂的轨迹跟踪和避障规划统一到一个优化框架中。首先，基于GJK 算法和包围盒法对避障空间进行了建模；然后，设计了适应度函数，阐述了优化器的基本框架。

2.1 避障空间的建模

在传统方法中，机械臂连杆通常被简化为直线，障碍物被简化为圆或球，通过判断两者的位置关系实现碰撞检测［14］。这些方法没有考虑机械臂与障碍物的三维几何形状。本文使用GJK算法进行碰撞检测，它的基本原理是计算任意形状的三维凸多边形之间的最小距离。

考虑三维空间中的两个凸多边形A和B，它们的顶点由矩阵VA∈RnA×3和VB∈RnB×3定义，其中R表示实数集，nA和nB是A和B的顶点数量。VA和VB的每一行表示对应多边形顶点的位置坐标。GJK 算法利用这些矩阵计算两个多边形顶点之间的最小距离：

其中：Vi：表示矩阵V的第i行；‖ · ‖2表示2-范数。

本文的研究对象是九自由度冗余机械臂，其构型和坐标系如图1 所示。坐标系使用MDH（Modified Denavit-Hartenberg）参数法构造，其中，基坐标系被描述为坐标系｛b｝或｛0｝，关节i（i=1-9）的坐标系被描述为坐标系｛i｝，末端执行器的坐标系被描述为坐标系｛e｝。xi，yi，zi分别是坐标系｛i｝的x轴、y轴和z轴。dij是从坐标系｛i｝的原点到坐标系｛j｝的原点的距离。机械臂由9 个转动关节和8 个连杆组成。关节1，3，5，7，9绕质心轴旋转，不影响避障。由于机械臂连杆不是凸多边形，本文使用包围盒法将其建模为立方体，以满足GJK 算法的使用条件。

图1 九自由度冗余机械臂构型图Fig.1 Configuration diagram of a 9-DOF redundant robotic manipulator

图2 展示了包围盒的建立过程。由图2（a）所示，连杆2-3，4-5，6-7 以及连杆8 被最小的圆柱体包围，得到4 个虚拟连杆L1-L4，其中，每个圆柱体的底面半径为Ri（i=1-4），虚拟连杆两端的圆心坐标J2，J4，J6，J8和Je分别对应关节2，4，6，8 和末端执行器的质心坐标。由图2（b）所示，使用最小的长方体将虚拟连杆包围，根据连杆的圆心坐标J以及半径R，可以得到初始构型下，长方体包围盒的8 个顶点坐标。同样地，将障碍物O用最小的长方体包围，使用式（1）可以求得机械臂与障碍物之间的最小距离dg，如图2（c）所示。

图2 避障空间的建模Fig.2 Model of obstacle avoidance space

需要注意的是，随着机械臂的运动，顶点的坐标会发生变化，即它是关节角θ的函数。设计如下的计算方法来实时获得顶点坐标：

其中：Vi(θ)∈表示第i个虚拟连杆的顶点矩阵，Vi(0) 是初始时刻的顶点信息；θ={θ1，θ2，…，θ9}是当前时刻的关节角向量，θ1-θ9是关节1-9 的角度；Ri(θ)和Ti(θ)表示第i个虚拟连杆的旋转和平移矩阵。

2.2 适应度函数的设计

为了将轨迹跟踪和避障规划统一到一个优化框架中，本文设计了如下所示的适应度函数：

其中：O∈代表障碍物的顶点矩阵，no是障碍物的顶点数量；xt代表目标轨迹点的位置坐标；θ代表关节角向量；ferr(xt，θ)是跟踪误差项，表示机械臂末端执行器与目标轨迹之间的位置误差，由式（4）计算，其中FK(·)是机械臂的正运动学方程；foa(O，θ)是避障奖励项，取机械臂与障碍物之间最小GJK 距离的倒数，如式（5）所示，其中μ是常参数，一般取μ=1；Λ是避障开关，用于控制避障任务在轨迹跟踪中所占的比例，如Λ=0 即关闭避障功能，仅进行轨迹跟踪。

式（3）的设计基于这样的动机：最小化末端执行器与目标轨迹的位置误差，同时最大化机械臂与障碍物的距离。因此，本文提出的优化框架可以描述为：

其中：θc，，分别代表关节位置、速度和加速度约束，t代表时间；dmin是机械臂与障碍物之间最小距离的约束。对于违反约束的解，采用文献［15］提出的约束处理方法，其原理是赋予这些解更差的适应度值。

通过求解式（6）～式（8）所述的优化问题，即可在不违反关节约束的前提下，获得同时满足机械臂轨迹跟踪和避障需求的最优关节角配置θ。值得注意的是，本文提出的优化框架可以将笛卡尔空间的目标轨迹映射到关节空间，而不需要计算雅可比矩阵的伪逆。这是由于式（4）所示的跟踪误差项只需要机械臂的正运动学方程。对于冗余机械臂，这减小了轨迹跟踪的计算量，同时避免了传统方法可能存在的奇异性问题。此外，本文引入的式（5）所示的避障奖励项能鼓励优化器主动避开障碍物，因为避障性能更好的解被额外赋予了更好的适应度。

3 灰狼算法与改进

为了求解第2 节构造的优化问题，本节提出了一种改进的灰狼算法。首先介绍了经典灰狼算 法（Grey Wolf Optimizer，GWO）［13］的基本 结构，包括社会阶级、包围猎物以及攻击猎物；然后，提出了随机分散策略，以改进GWO 的性能。

3.1 社会阶级

灰狼社会由四个阶级组成：阿尔法（α）、贝塔（β）、德尔塔（δ）和欧米伽（ω）。α，β和δ分别是当前种群中最优、第二优和第三优的解。它们代表狼群的首领，距离猎物（全局最优解）最近。剩余的狼用ω表示，它们的位置在每次迭代中根据α，β和δ而改变。

3.2 包围猎物

狩猎的第一步是包围猎物，这一行为由以下方程描述：

其中：Xp和X分别表示猎物和灰狼的位置；D代表灰狼和猎物之间的距离；t是当前的迭代次数；A和C为系数变量，计算公式如下：

其中：r1和r2是［0，1］中的随机数；在整个迭代过程中，a由2 线性减小到0，即：

其中，MaxIter表示最大迭代次数。

3.3 攻击猎物

第二步是攻击猎物，即狼群ω跟随首领α、β和δ向猎物移动。ω的位置更新方程如下：

其中：Xα，Xβ和Xδ分别表示α，β和δ的位置；X1，X2和X3分别表示由α，β和δ引起的位移；A1，A2和A3分别表 示α，β和δ对应的 决定灰 狼移动 距离的随机变量，由式（11）计算；C1，C2和C3分别表示α，β和δ对应的决定猎物位置在灰狼运动中所占比例的随机变量，由式（12）计算。

3.4 随机分散策略

在经典GWO 中，由式（17）可以看出，狼群的位置完全由首领α，β和δ决定，这导致了种群多样性的缺失，容易使算法过早收敛而陷入局部最优。本文提出了一种随机分散策略，引入随机个体来影响种群更新，描述如下：

其中：Xm和Xn是种群中随机挑选的两个不同的个体；ρ是［0，1］中的随机数，它用于决定随机分散的距离。

由式（18）引起的位置更新是随机的，这给个体逃离局部最优提供了可能。此外，Xm和Xn的引入增加了种群多样性，这对克服过早收敛具有重要作用。随机分散策略模拟了灰狼在自然界中为了应对食物短缺而分散觅食的行为。如果首领找到的食物不足以供应整个狼群，部分灰狼可能会迁移到食物更加丰富的新地区。

3.5 改进的灰狼算法

将随机分散策略引入到经典GWO，得到如下所示的种群更新方程：

其中：Xi(t+1)表示第i个灰狼个体的新位置；=(X1(t)+X2(t)+X3(t)和 式（17）一致，表示由领导者引起的运动;的计算如式（18），表示由随机个体引起的运动；ρ1，ρ2∈[0，1]是随机数，用于决定运动的距离；B1和B2是权重因子，用于自适应地平衡算法的局部搜索能力和全局搜索能力，计算如下：

其中，B1和B2是迭代次数t的线性变量。在搜索的初期，通过设置较大的B2使个体随机分散到整个搜索空间，有利于全局搜索；在搜索的末期，较大的B1可以使个体快速收敛到最优解，强调局部搜索。改进GWO 的流程图如图3 所示。

图3 改进灰狼算法的流程图Fig.3 Flowchart of improved grey wolf optimizer

3.6 计算复杂度分析

本节将评估经典GWO 与提出的改进GWO 的计算复杂度。首先，在种群初始化阶段，经典GWO 与改进GWO 的计算复杂度均为O（N），N是种群大小；对于迭代的主循环，两个算法都要更新每个个体的位置向量，其计算复杂度为O（N·D），D是位置向量的维度；更新种群后，在O（N）的时间内评估所有个体的适应度值。

因为种群更新和适应度计算都在迭代次数达到最大次数T时停止，所以上述计算复杂度需乘以T。截至此操作步骤，经典GWO 与改进GWO 的计算复杂度均为O（T·N·D）。在改进GWO 中，随机分散策略将部分个体分散到搜索空间中的新区域，其计算复杂度为O（N）。因此，改进GWO 的计算复杂度依然是O（T·N·D），与经典GWO 相同。

4 仿真与实验

本节使用九自由度冗余机械臂对提出的优化器进行了仿真和实验研究。对于经典GWO 和改进的GWO，种群大小被设置为30，最大迭代次数被设置为100，搜索的停止条件是当前迭代次数达到最大值。目标轨迹是由6 325 个点组成的半径为0.1 m 的圆，障碍物是边长为0.1 m 的立方体。

4.1 仿真1：关闭避障功能

为了验证式（5）所示的避障奖励项的有效性，首先设置式（3）中的Λ=0，即关闭优化器的避障功能。使用改进的GWO 对优化器进行求解。图4 展示了Λ=0 时，机械臂跟踪圆形轨迹的构型变化及其三视图。从图4 可以看出，机械臂与障碍物发生了明显的碰撞。这是因为优化器计算的关节运动轨迹仅致力于降低末端执行器的跟踪误差，而没有考虑机械臂与障碍物之间的距离。

图4 关闭避障功能时的构型变化Fig.4 Configuration changes when obstacle avoidance function is turned off

4.2 仿真2：开启避障功能

在仿真2 中，设置式（3）中的Λ=0.02，即开启避障功能。另外，设置式（8）中的dmin=0.02 m，以约束机械臂与障碍物之间的最小距离。使用改进的GWO 对优化器进行求解，机械臂的构型变化如图5 所示。由图5 可以看出，开启避障功能后，机械臂在完成轨迹跟踪的同时成功避开了障碍物。这是由于在避障奖励项的鼓励下，优化器选择了无碰撞的运动轨迹。

图5 开启避障功能时的构型变化Fig.5 Configuration changes when obstacle avoidance function is turned on

图6 展示了Λ=0.02 时，关节和末端执行器的运动轨迹。图6（a）展示了轨迹跟踪过程中，机械臂九个关节的位置变化。关节的运动轨迹存在一些轻微的抖动，这是由元启发式算法的随机性造成的。然而，这些抖动并不会影响末端执行器的跟踪精度，且幅度没有超出机械臂安全运行的限制。此外，考虑到元启发式算法对计算效率和寻优精度带来的显著提升，这些轻微的抖动是可以接受的。末端执行器在三个方向上位置分量的变化如图6（b）所示，该轨迹与目标圆完全重合。

图6 Λ=0.02 时的运动轨迹Fig.6 Motion trajectory for Λ=0.02

Λ=0.02 时，机械臂末端的轨迹跟踪误差以及机械臂连杆与障碍物之间的最小距离分别如图7 和图8 所示。由图7 可以看出，跟踪误差集中在［10-7m，10-3m］，这证明了算法的全局收敛性，即随迭代次数的增加，跟踪误差收敛到0。误差在0 附近的波动是由元启发式算法固有的随机性造成的。经计算，平均跟踪误差为0.21 mm，满足毫米级的跟踪要求。由图8 可知，轨迹跟踪过程中，机械臂与障碍物的最小距离不小于70 mm，且没有低于设定的最小限制dmin。较高的距离值降低了在障碍物位置不准确或机械臂模型有误差的情况下，发生碰撞的风险，这提高了避障的鲁棒性。

图7 Λ=0.02 时的轨迹跟踪误差Fig.7 Trajectory tracking error for Λ=0.02

图8 机械臂连杆与障碍物之间的最小距离Fig.8 Minimum distance between the links and the obstacle

图9 展示了Λ=0.02 时，优化器在目标轨迹的第3 000 个点处的收敛曲线。需要注意的是，本文研究的优化问题是最小化问题，即适应度值越小，解决方案越好。从图9 可以看出，优化器以很快的速度收敛到了很小的适应度值。在迭代次数为20 左右时，优化器陷入了局部最优；然而，随着迭代次数的增加，优化器突破局部最优停滞，继续向缩小适应度值的方向收敛。这说明改进GWO 中的随机分散策略能有效地克服过早收敛，增强算法的全局搜索能力。优化器在目标轨迹的其他点处的收敛过程与该点类似，由于篇幅限制，本文不再展示。

图9 优化器的收敛曲线Fig.9 Convergence curve of optimizer

需要注意的是，式（3）所示的适应度函数是由跟踪误差项和避障奖励项的和构成的，避障开关Λ的取值会影响优化器的跟踪性能和避障性能。即增大Λ的值，将获得更好的避障性能，但跟踪性能会变差，这体现在机械臂与障碍物之间的距离增加，但跟踪误差变大。本文中，Λ=0.02 是通过多次实验获得的能够平衡跟踪和避障性能的最佳值。在实际应用中，应综合考虑跟踪和避障的需求，选取合适的Λ。

为了验证改进GWO 的优越性，在相同的仿真条件下，使用经典GWO 对优化器进行求解。轨迹跟踪误差的对比结果如表1 所示。由表1 可以看出，相比于经典GWO，改进的GWO 使轨迹跟踪误差的平均值降低了13%，这证明了改进GWO 具有更优越的性能。

表1 跟踪误差的对比结果Tab.1 Comparison results of tracking errors （mm）

最后，本节定性分析了基于改进灰狼算法的避障跟踪优化器相比于传统方法的区别和优势，如表2 所示。由表2 可以看出，本文提出的方法计算量小、通用性强，具有一定的应用价值。

表2 本文方法与传统方法的比较Tab.2 Comparison between the proposed method and traditional methods

4.3 实验验证

为了进一步证明提出的避障跟踪优化器的可行性，使用9 自由度冗余机械臂，对仿真2 的结果进行实验验证。实验平台如图10 所示，采样时间设置为5 ms。

图10 9 自由度冗余机械臂的实验平台Fig.10 Experimental platform for the 9-DOF redundant robotic manipulator

将仿真2 获得的关节运动轨迹输入给上位机执行，以驱动机械臂进行轨迹跟踪和避障。障碍物由边长为0.1 m 的泡沫立方体模拟，使用塑胶杆固定。实验前，障碍物的位置使用激光跟踪仪进行了标定。将靶标固定在障碍物上，由激光跟踪仪发射激光，经靶标反射后，获得障碍物在跟踪仪坐标系中的球面极坐标。经过坐标变换，即可确定障碍物在机械臂坐标系中的三维坐标，以保证实验场景的设置与仿真场景一致。实验结果如图11 所示，图中1 到4 表示机械臂的运动顺序。由上位机测量的关节运动轨迹如图12（a）所示，该轨迹与图6（a）的仿真结果完全吻合。图12（b）展示了由图12（a）的关节轨迹，经机械臂的正运动学方程，求解的末端执行器的运动轨迹，该轨迹与图6（b）的仿真结果一致。由此可见，避障跟踪优化器生成的关节轨迹安全可行，机械臂能够准确地完成轨迹跟踪和避障任务。

图11 轨迹跟踪与避障的实验结果Fig.11 Photos of experimental results of trajectory tracking and obstacle avoidance

图12 实验测量的运动轨迹Fig.12 Motion trajectories obtained from the experiment

5 结论

本文提出了一种基于改进灰狼算法的避障跟踪优化器，以解决冗余机械臂的轨迹跟踪和避障问题。首先，使用包围盒法对避障空间进行了建模，基于GJK 算法获得机械臂连杆与障碍物的最小距离，该方法考虑了机械臂连杆与障碍物的三维几何形状，而不是将其简化为直线或点。其次，设计了适应度函数，将轨迹跟踪和避障任务统一到同一个优化问题。其中，跟踪误差项只基于正运动学实现笛卡尔空间的轨迹向关节空间的映射，而不需要雅可比逆矩阵；避障奖励项能积极地鼓励优化器避开障碍物，而不是将避障问题简化为不等式约束。然后，基于随机分散策略提出了改进的灰狼算法，该算法具有更强的全局搜索能力。最后，使用九自由度冗余机械臂对提出的避障跟踪优化器进行了仿真和实验验证。

结果表明，轨迹跟踪的平均误差为0.21 mm，机械臂与障碍物的最小距离不小于70 mm；相比于经典灰狼算法，改进灰狼算法使跟踪误差降低了13%。基于改进灰狼算法的避障跟踪优化器能安全、准确地实现9 自由度冗余机械臂的轨迹跟踪和避障任务，随机分散策略能有效提高经典灰狼算法的性能。