基于改进粒子群优化的乒乓球发球机械手轨迹规划

王 成，王朝立，赵忆文

（1上海理工大学 光电信息与计算机工程学院，上海 200093；2中国科学院 沈阳自动化研究所，沈阳 110016）

0 引 言

目前，国内外研发出许多不同结构的乒乓球发球机械手，通过分析发现，绝大多数的发球机械手存在发球不精准、发球稳定性差、发球速度慢的缺点，使得其无法完美代替真人发球来帮助乒乓球爱好者开展训练。为了解决这些问题，需要采用更有效的轨迹规划方法对机械手的运动轨迹进行规划，使得机械手运动时位置、速度、加速度连续，进而让乒乓球发球机械手能够快速、平滑且准确地发球；否则，不充分平滑的轨迹规划容易导致机械系统产生谐振，这不仅会让轨迹误差大幅度增加，也会缩短机械手驱动部件的使用寿命。

针对这一问题，Piazza等学者提出基于三次样条插值的轨迹规划方法，这是属于位置、速度、加速度连续且阶次最低的多项式轨迹方法，虽然计算简单，但是规划出的速度曲线波动太大，并且对多个插值点进行轨迹规划时，机械手关节的加速度容易出现突变现象；朱世强等学者采用七次B样条曲线插值法规划机械手轨迹以及孙亮等学者引入五次多项式插值法对机械手进行轨迹规划，虽然这些方法可以避免机械手振动，但是却都提高了规划函数的阶次或者复杂度，增加轨迹规划时计算量。为解决这一问题，李白雅等学者根据分段多项式计算量小、易于理解且插值出的轨迹更加平滑等优点，提出了4-3-4多项式插值法来规划轨迹，不仅防止了速度和加速度突变，也让机械手的运动轨迹更加连续、平滑、精确。虽然该方法很好地保证机械手运动平滑度、准确度，但是仍不能解决机械手工作效率低这一关键性问题。王学琨等学者将差分进化（DE）算法和3-5-3多项式插值法相结合，用于优化机械手轨迹运行时间，但因对机械手各个关节的约束条件考虑得不全面，过于优化了机械手轨迹运行时间。曾关平采用三次B样条方法对七自由度乒乓球机械手进行轨迹规划，同时在充分考虑到运动学约束下，利用改进的遗传算法优化出了理想的机械手运动轨迹，虽然提高了乒乓球机械手的运动效率，也保证了其运动轨迹的平滑性，但改进的遗传算法需进行浮点编码、解码操作，导致优化过程复杂且优化时间过长，所以在求解复杂的约束问题上有一定的困难。而文献［8-10］中提到粒子群算法具有结构简单、参数设置少的优点，相比其他的解决约束优化问题的方法具有一定的优势。

针对以上轨迹规划方法的优缺点，并在考虑机械手各个关节运动约束的同时，本文提出基于改进粒子群算法的4-3-4多项式插值法对乒乓球发球机械手进行时间最优轨迹规划。该方法利用改进的粒子群算法不仅解决了传统多项式插值轨迹规划方法效率低的问题，还保证了在时间达到最优的同时使得机械手运动轨迹更加连续、平滑、准确，并且通过在优化目标空间直接搜索最值，省略了构造粒子群算法计算自变量和因变量映射的步骤，减小粒子群寻优的复杂性和困难性。最后，通过Matlab的仿真，证明了该方法的可行性和有效性。

1 五自由度乒乓球发球机械手的运动学分析

1.1 正运动学

本文所研究的乒乓球发球机械手是由5个旋转关节串联连接的模块化机械臂，如图1所示。根据此特点，使用D-H表示法确定各个旋转关节坐标系，如图2所示。D-H参数见表1。

图1 五自由度乒乓球发球机械手Fig.1 5-DOF table tennis serving manipulator

图2 五自由度乒乓球发球机械手D-H坐标系Fig.2 D-H coordinate system of 5-DOF table tennis serving manipulator

在表1中，a表示从z轴到z轴的距离，α表示从z轴到z轴的扭转角，d表示从x轴到x轴的距离，θ表示从x轴到x轴的转角。

表1 五自由度乒乓球发球机械手的D-H参数表Tab.1 D-H parameter table of 5-DOF table tennis serving manipulator

其中，s＝sinθ，c＝cosθ，sα＝sinα，cα＝cosα。

将表1中的D-H参数代入公式（1）中，可以推导出5个关节之间的变换矩阵：

由式（2）中变换矩阵的连乘，可得机械手末端执行器中心点的位姿相对于基坐标系的变换矩阵为：

1.2 逆运动学求解

针对本文所研究五自由度乒乓球机械手，当机械手末端执行器中心点位置与姿态已知时，即已知，根据式（4）求解关节角度θ：

利用矩阵变换与反变换方法可以得出：

其中，N＝c a+s a，N＝s a-c a，M＝c o+s o，L＝c n+s n。

通过上面求出的逆运动学解，可以发现解并不是唯一的。所以，本文将根据规划任务以及机械手关节约束来选择合适的机械手运动姿态。

本文为了充分模拟机械手完整的发球过程，在初始位置和终止位置之间选择了2个路径点、来完成进行发球时的准备过程、抬拍过程、引拍过程，从而使得发球过程更加顺畅。4个路径点信息见表2。

表2 4个路径点的位置与姿态信息Tab.2 Position and attitude information of four waypoints

根据表2中4个点在笛卡尔空间的信息，利用推导的逆运动学解析法将其转换到关节空间中每个关节所对应的角度θ（表示路径点的序号，0，1，2，3），并选取出合适的关节角度，见表3。

表3 4个路径点对应的各个关节角度 Tab.3 Joint angles corresponding to the four path points（°）

在已知机械手在4个路径点的各个关节角度后，下面将根据起始、终止条件利用多项式插值法规划出每个关节理想曲线。

2 分段多项式插值轨迹规划方法

根据分段多项式计算量小、插值出的轨迹在速度和加速度上无突变的优点，同时为完成使得发球更加流畅而进行的准备过程、抬拍过程、引拍过程，本文利用4-3-4多项式插值法来规划发球机械手的运动轨迹。

4-3-4多项式的通式如下：

其中，L（t）、L（t）、L（t）是关节（1，2，3，4，5）在第一、二、三段轨迹的表达式，a是第个关节轨迹在第（1，2，3）段插值多项式中的第个未知系数，t、t、t分别表示关节在第一、二、三段轨迹的插值时间。

为了使得轨迹更加连续、平滑，发球机械手的运动位置、速度和加速度约束条件为：

（1）起始点的速度和加速度为0。

（2）终止点的速度为，加速度为0。

（3）各个路径点之间的位置、速度、加速度连续。即：

根据以上条件，可以推导出多项式未知系数a和4个路径点之间的关系式，由此推得式（12）～（14）。

当插值时间t、t、t都不为0时，式（12）中的矩阵是可逆的。所以当插值时间已知时，通过式（12）～（14）可以求出各段多项式轨迹函数，即可知乒乓球发球机械手各个关节的运动曲线。式（12）～（14）的数学表达式可写为：

3 改进粒子群算法

3.1 粒子群算法基本原理

粒子群算法（PSO）的主要思想就是把待求解问题（目标函数）的解看作搜索空间中的粒子，每个粒子都具有位置（x）和速度（v）两个属性，位置属性决定粒子所处的位置，速度属性决定粒子移动的方向和距离。在解空间里初始化一组随机解（粒子），并且每个粒子都有一个由目标函数所决定的适应度值，根据式（15）和式（16）不断迭代更新粒子，通过粒子适应度值的变化来搜索目标函数的最优值。具体数学公式如下：

3.2 粒子群算法的改进

在PSO中的大小主要是用来调节当前粒子的速度，较大时，则算法全局搜索的能力较强；较小时，则局部收敛能力较强。、的大小分别决定了粒子本身的认知能力和粒子相互之间的信息共享能力，较大时，粒子自我认知能力强；较大时，粒子全局信息共享能力强。但标准PSO中惯性权重和学习因子都是固定值，其缺乏有效的机制调整算法前、后期、、的大小来平衡算法前、后期搜索范围的着重程度，从而导致算法存在收敛速度慢、不易获得全局最优的缺点。

线性PSO通过调整惯性权重线性变换来改善传统PSO的缺点。线性惯性权重曲线路如图3所示。从图3可以看出，线性变化的在算法初期时，较大的时间短，易于导致算法前期全局搜索能力弱；由于变化幅度不变，导致算法后期不能迅速变小，使得算法后期局部搜索能力弱，这样一来算法也仍然存在前后期搜索范围不平衡，从而影响算法收敛速度和优化能力。并且线性PSO没有考虑学习因子对算法寻优能力的影响。

图3 线性惯性权重曲线Fig.3 Linear inertia weight curve

所以，本文将利用非线性函数对惯性权重和学习因子进行改进，并通过3个参数之间相互配合来进一步提高算法收敛速度以及优化能力。

为调整PSO算法前、后期全局和局部搜索能力的着重程度，本文将通过指数函数来调节随迭代次数的增加而非线性地减小，可见式（17）：

其中，和分别表示惯性权重的最大值和最小值；表示最大迭代次数；表示当前迭代次数。

为了提高算法的收敛速度与优化能力，即：使得算法前期注重粒子自我认知能力，算法后期注重粒子全局信息共享能力。本文采用余弦、正弦函数分别对、进行异步非线性调整，可见（18）：

其中，、、、分别表示学习因子、的最大值和最小值。

图4、图5分别是惯性权重、学习因子曲线对比图。从对比图中可以看出：利用非线性函数对、、改进后，在算法初期时，较大、较大、较小的时间更长，会使得算法全局搜索能力增强。并随着迭代次数的变化，对、、的变化幅度也不一样，在算法后期时，、能迅速变小、能迅速变大，使得算法局部搜索能力增强，从而让算法的全局和局部搜索能力更加平衡，进而算法的收敛速度得到了提高，也更易于获得全局最优值。

图4 惯性权重对比曲线Fig.4 Comparison curve of inertia weight

图5 学习因子对比曲线Fig.5 Comparison curve of learning factors

4 基于改进的粒子群算法优化轨迹规划问题

4.1 优化目标

在乒乓球运动中，快速、连续、准确的发球会更容易让对手失分。所以当使用乒乓球发球机械手来模拟真人进行发球时，在保证机械手的运动连续、平滑、准确的同时，需要缩短发球机械手的运动时间。相比通过对多项式的系数进行优化来优化时间，本文将直接根据约束条件对每个关节轨迹的3段运行时间进行优化，可以大大降低优化变量的维度和算法的复杂度，从而使得算法能够更加快速、准确地寻找到最优值。第个关节的时间优化目标函数为：

为保证乒乓球发球机械手运动连续以及安全使用，需要对发球机械手每个关节的位置、速度、加速度进行约束。约束条件为：

（1）已知各个关节在4个路径点处的关节角度（θ），参见表3。

（2）各个关节的速度、加速度边界条件，详见式（11）。

（3）各个关节在3段轨迹中的位置、速度最大限制，详见式（20）、式（21）。

4.2 改进的粒子群算法优化4-3-4多项式轨迹规划

采用改进PSO来优化乒乓球发球机械手关节的运行时间的分步过程如下：

在关节的3段插值时间的搜索空间中为种群P、P、P分别随机生成个粒子，然后初始化3个粒子的位置和速度。

将组插值时间变量代入式（12）（14）中计算多项式的未知系数a。

将步骤2中求解出的多项式系数a代入式（10）中，根据时间计算关节的实时位置；然后对式（10）求导，计算关节的实时速度。

计算每个粒子的适应度值。在步骤3中，若在3段轨迹中，关节的任意一段位置、速度大于位置、速度约束值，则将此粒子的适应度值设定为极大常数B（以便通过适应值的比较，使得此粒子将不会被选为最优粒子）；反之，则根据目标函数计算粒子的适应度值。

将步骤4计算出的每个粒子的适应度值进行比较，选择出每个粒子自身所经历过的最好位置（个体最优值）以及整个种群粒子所经历过的最好位置（群体最优值）。

根据式（17）、（18）更新每个粒子的位置和速度，生成新的种群。

根据终止条件判断算法是否结束。满足终止条件，则输出关节的运行最优时间；反之，则返回到步骤2。

关节的最优运行时间优化流程可见图6。

图6 第i个关节进行最优时间轨迹规划程序流程图Fig.6 Flow chart of optimal time trajectory planning for the i-th joint

5 实验与仿真结果分析

5.1 乒乓球发球机械手模型的建立

根据表1中乒乓球发球机械手的D-H参数，其中92 cm，28 cm，48 cm，在Matlab中利用Robotics Toolbox工具箱中的函数和函数构建发球机械手的三维模型，如图7所示。

图7 五自由度乒乓球发球机械手三维模型Fig.7 Three dimensional model of 5-DOF table tennis serving manipulator

5.2 乒乓球发球机械手工作空间仿真

考虑到乒乓球发球机械手在实际工作中进行轨迹规划时，会存在奇异点（不可到达的区域），导致机械手无法运动。本文基于蒙特卡罗法理论基础上，在Matlab环境下，使用（，1）随机函数对机械手5个关节的角度随机采样进行工作空间仿真分析。合理选取30 000，发球机械手的工作空间仿真结果如图8所示。

观察图8可知，发球机械手的工作空间存在少许空穴、空洞现象，因此在轨迹规划仿真时应避免这些奇异点，以防后续无法进行实际发球操作。

图8 五自由度乒乓球发球机械手的工作空间Fig.8 Workspace of a 5-DOF table tennis serving manipulator

5.3 笛卡尔空间速度与关节空间速度的线性映射

根据表3中终止点位置（θ），在Matlab中利用Robotics Toolbox工具箱中函数和函数求解出五自由度发球机械手的雅可比矩阵以及逆雅克比矩阵，然后根据式（22）求解出关节空间速度，于是可得：

根据发球任务（发球机械手在终止点处的笛卡尔空间速度为［100 200 200 0 0 0］，即机械手末端线速度为300 cm／s，角速度为0，利用式（22）求解出5个关节在终止点时的关节空间速度为：

5.4 轨迹规划仿真对比实验

通过上文仿真出的机械手工作空间、表3中各个关节的位置以及及终止点的速度，在3段轨迹的插值时间都为2 s时，单独使用4-3-4多项式插值法规划各个关节的位置、速度和加速度曲线。参见图9～图11。

图9 优化前5个关节的角位置变化曲线Fig.9 The angular position curves of the first five joints

图10 优化前5个关节的角速度变化曲线Fig.10 The angular velocity curves of the first five joints

图11 优化前5个关节的角加速度变化曲线Fig.11 The angular acceleration curves of the first five joints

从图9～图11中可以看出，单独使用4-3-4多项式插值法规划机械手轨迹时，虽然机械手各个关节的位置、速度和加速度曲线是连续的，且全程都无突变，但是各个关节的运行速度都没趋向关节最大限制速度（80°／s），加速度也较小。可见该方法并没有充分利用机械手的性能，从而导致机械手运动效率低，不能够快速地完成发球动作。

为验证基于改进PSO的4-3-4轨迹规划法可行性与有效性，本文以求解关节1的轨迹最佳运行时间为例，分别使用基于标准PSO（SPSO）、线性PSO（PSO-I）、遗传算法（GA）、差分进化算法（DE）、改进PSO（NPSO）的4-3-4轨迹规划法对其进行优化，可见图12。表4为5种算法优化效率对比结果。粒子群算法的参数设置为：种群数20，粒子取值范围为［0，2］，、分别为0.9、0.4，学习因子、的变化范围为［0.02，0.05］，最大迭代次数为100。

图12 关节1的运行时间迭代收敛对比图Fig.12 Comparison diagram of running time iteration convergence of joint 1

表4 5种算法求解关节1最优时间的效率对比结果Tab.4 The efficiency comparison results of five algorithms to solve the optimal time of joint 1 s

从图12中可以看出，SPSO优化效果最差，但收敛速度比DE较快。DE、SPSO、GA、PSO-I分别在第85代、第78代、第66代和第56代收敛，而NPSO在第21代收敛。并且NPSO将关节1运行时间最终收敛到1.948 5 s，相比其他4种算法，得出了最短的运行时间。从表4中可以看出NPSO处理轨迹优化的时间为3.886 s，可知NPSO的计算效率更加快速。因此，本文提出的NPSO相比其他4种算法具有更好的收敛速度和寻找最优值的能力。换言之，即：基于改进PSO的4-3-4轨迹规划法可以更好地在满足运动约束下（最大化机械手性能）对机械手轨迹的运行时间进行优化。

在关节速度限制为［-80，80］（°／s）的情况下，根据4.2小节中的步骤1～步骤7，利用基于改进PSO算法的4-3-4轨迹规划法获得5个关节在各段轨迹的最优运行时间，见表5。

表5 各个关节在速度［-80，80］（°／s）的最优运行时间Tab.5 Optimal running time of each joint while the velocity is between［-80，80］（°／s） s

由于机器人的关节运动具有时间同步性，因此需选择5个关节在各段轨迹的运行时间的最大值，即：

可以得出3段轨迹运行的最优时间分别为0.683 1 s，0.618 0 s，0.695 6 s，比未优化时缩短了4.003 3 s。

图13～图15分别显示了采用基于改进粒子群算法的4-3-4多项式插值法所规划出的乒乓球发球机械手5个关节运动的位置、速度和加速度曲线。图16是乒乓球发球机械手末端运动轨迹。

图13 优化后5个关节的角位置变化曲线Fig.13 The angular position curve of the five joints after optimization

图14 优化后5个关节的角速度变化曲线Fig.14 The angular velocity curves of the five joints after optimization

图15 优化后5个关节的角加速度变化曲线Fig.15 The angular acceleration curves of the five joints after optimization

图16 乒乓球发球机械手的末端运动轨迹Fig.16 End motion trajectory of table tennis serving manipulator

从图13～图15中可以清晰看出，在满足运动约束的情况下，优化后的机械手各个关节的位置、速度、加速度曲线更加连续、平滑。相比未优化前的各个关节运动曲线图，各个关节的运动速度都得到提高，并趋向关节最大限制速度，而且各个关节的加速度也有所提高且无突变现象。图16中也显示出了乒乓球发球机械手能够平滑地通过给定的路径点完成发球任务。

6 结束语

本文以五自由度乒乓球发球机械手为研究对象，根据其各个关节的结构特性，利用D-H参数法建立发球机械手的三维模型。通过机械手在关节空间的位姿变换理论推导出发球机械手的运动数学模型，并针对机械手运动过程可能出现的奇异现象，利用蒙特卡罗法分析发球机械手的工作空间；然后，在满足乒乓球发球机械手运动学约束下，为使得机械手能够快速、连续、准确地完成发球动作，提出了一种基于改进PSO的4-3-4多项式插值轨迹规划方法；最后，通过Matlab仿真对比实验，不仅证明了利用非线性变换的惯性权重和学习因子能够提高粒子群算法的收敛速度和优化能力，还证明了基于改进PSO的4-3-4多项式插值法能够大大缩短机械手的运行时间，同时也确保了各个关节运动的位置、速度、加速度更加连续、平滑，从而提高了乒乓球发球机械手末端运动的精度以及机械手的使用寿命。