码垛机器人修正正弦函数插值的轨迹规划

乔 木，于天彪，李 明，王 博

（东北大学 机械工程与自动化学院，辽宁 沈阳 110819）

1 引言

广泛应用于医药包装生产线的码垛机器人是提升整个医疗行业自动化水平的关键装备，能够完美地实现重载、快速、准确和连续的作业要求[1]。随着医疗行业的高速发展和生产规模的扩大，对码垛机器人的工作速度提出了更高的要求[2]。因为码垛机器人在堆垛过程中需要频繁的启停和变速，这对码垛机器人本体和电机会造成较大振动和磨损，所以为了保证码垛机器人能够快速平稳的作业，轨迹规划过程就显得尤为重要。

轨迹规划是根据具体的任务要求设计机器人的各个关节位移、速度、加速度以及急动度随时间t的变化曲线[3]，是码垛机器人控制的前提和基础，其运动性能对码垛机器人的工效、能耗和使用寿命[4]具有重要意义，因此对轨迹规划的研究一直是码垛机器人领域的热点之一[5]。文献[6]考虑动态约束求解时间最优轨迹，但加速度曲线不连续，伴有柔性冲击。文献[7-8]采用三次样条曲线规划时间最优轨迹，只能确保速度、加速度连续，无法保证急动度的连续。国内学者田西勇在组合函数曲线方面亦做出突出贡献。

在实际生产中，主要采用多项式函数和正余弦函数进行规划，但是随着工作要求的不断提高，单一曲线已经无法满足要求[9]。通过三段修正正弦函数曲线在关节空间对码垛机器人进行轨迹规划，并利用遗传算法对修正参数进行优化，结果表明该方法能够有效提高码垛机器人的运动平稳性。

2 轨迹规划优良曲线特性分析与选取

医药码垛机器人的特点是空间四自由度串联关节型机器人，具有双平行四边形结构，完成对药箱从输送带到货架的简单搬运，而药箱姿态不发生变化。码垛机器人的三维实体模型，如图1所示。

图1 码垛机器人实体模型Fig.1 The Entity Model of Palletizing Robot

本研究是建立在位移函数、速度函数和加速度函数连续的基础上，以加速度函数为切入点，选取运动性能最优的加速度函数曲线。码垛机器人的关节变量在工作时间t=0时为θI，t=T时为θF，为了保证码垛机器人运动的平滑性，应该满足如下表所示初始条件：

设码垛机器人关节轨迹的插值函数为s（τ），则有0≤s（τ）≤1，0≤τ≤1，τ=

根据式可以得到加速度函数为正弦函数的位移、速度、加速度和急动度的方程为：

根据式得到加速度函数为余弦函数的位移、速度、加速度和急动度的方程为：

根据式得到加速度函数为线性函数的位移、速度、加速度和

根据式得到加速度函数为三次多项式的位移、速度、加速度

在码垛机器人运动过程中，位移如果产生突变，除不能完成既定任务外，工作空间会受到极大影响；速度如果产生突变，理论上加速度会无穷大，惯性力无穷大，产生刚性冲击；加速度如果发生突变，亦会产生较大有限惯性力；急动度如果发生突变，会使机器人系统产生残余振动。上述四种情形均会使码垛机器人的运行性能变差，寿命降低，应力求避免发生，同时还应降低最大速度、最大加速度以及最大急动度的峰值。为了能够直观的对这四种函数曲线的各项运动性能进行对比以及选出运动性能相对优良的函数曲线，如图2所示。为了进一步降低码垛机器人在运行过程中的最大速度和最大加速度的峰值，将加速度函数为正弦函数、余弦函数、线性函数和三次多项式函数放在同一个图中进行比较，其中正弦函数和余弦函数图像是重合的。从图2（a）可以得出，4条曲线都是连续平缓的，没有突变发生；从图2（b）中可以得出，只有线性函数在行程的终止位置存在突变，有较大刚性冲击，从速度的峰值考虑线性函数的速度峰值最大，其次是正弦函数、余弦函数和三次多项式函数，三者速度峰值差别不大；从图2（c）中可以得出，线性函数在行程终止位置发生突变，伴有柔性冲击，从加速度峰值的角度考虑线性函数最大，其次是正弦函数、余弦函数和三次多项式函数，三者加速度峰值很接近；从图2（d）中可以得出，4条曲线均存在突变，其中线性函数急动度峰值最小，其次是正弦函数和余弦函数，最大为三次多项式函数，较其余三条曲线峰值大很多。因此综合考虑轨迹规划曲线的各项特征指标，选择正弦函数或者余弦函数作为优良曲线，文中将针对正弦函数进行深入研究。

图2 四条函数曲线的性能比较图Fig.2 Performance Comparison of Four Function Curves Diagram

3 三段修正正弦函数法的轨迹规划

由三条部分正弦曲线S1、S2和S3的组合而成，其中S1和S3是周期的正弦曲线，而S2是周期的正弦曲线，如图3所示。该修正函数周期设为1，则S1、S3的周期为T1=T3=4/a，S2的周期为T2=2-4/a，频率 ω1=ω3=aπ/2，ω2=aπ（/a-1）。考虑到修正的目的，修正参数a应该满足∈（0，），即 a∈（2，+∞）。

图3 三段修正正弦函数曲线图Fig.3 Three Stage Modified Sine Function Curve Diagram

设第一段部分正弦曲线S1方程为：经推理第二段部分正弦曲线S2可以由正弦曲线S1先伸长倍，再向左平移得到，因此可以设正弦曲线S2的方程为：

经推理第三段部分正弦曲线S3可以由正弦曲线S1向右平移得到，因此设正弦曲线S的方程为：3

为了保证修正正弦曲线的连续性，在两条曲线的交点处应保持数值相等，同时还要满足初始条件，则有：

由公式计算可以得到修正正弦函数加速度曲线方程为：

由公式计算可以得到修正正弦函数速度曲线方程为：

由公式计算可以得到修正正弦函数位移曲线方程为：由公式计算可以得到修正正弦函数急动度曲线方程为：

4 基于遗传算法的多目标优化

4.1 目标函数的确定

为了解决多目标优化问题，本优化采用统一目标函数法中的目标规划法把多目标优化问题转化为单目标优化问题。目标规划法是一种相对较简洁的构造统一目标函数的方法，首先确定各目标函数在最优解时的最优函数值，然后再根据多目标问题的总体要求对这些最优值做出相应调整，最后确定各个目标的最优值fj（j=0，1，2，…，n），因此所构造的统一目标函数为：

在本优化模型中，当a趋近无穷大时，最大加速度取得最小值，即；当a趋近无穷大时，最大速度取得最小值，即f2=经过计算，当a=4时，最大急动度取得最小值，即f3=4π2，因此统一量纲之后的目标函数可以表示为：

其中，修正参数a∈（2，+∞）。

4.2 遗传算法工具箱的参数设置

（1）对适应度函数进行编程，命名为my_fit；（2）设置算法参数，设置 Solver为ga-GeneticAlgorithm，Fitnessfunction为@my_fit，Number of variable为1，为防止算法陷入局部解并有较高的运算速度，设置Populationsize为 50，CrossoverFraction 为0.85，Migration Fraction 为 0.2[10]；（3）选取需要绘制的图像如 Generation，Bestindividual等。

4.3 优化结果分析

经Matlab遗传算法工具箱的优化运算，返回的最佳修正参数和目标函数值为：Best a=4.735，Best Fval=0.1164，经过57代的遗传运算得到的优化结果图，如图4所示。将通过Matlab遗传算法工具箱得到的最优修正参数a=4.735分别带入到公式中，比较三段修正正弦加速曲线与普通正弦加速曲线的位移曲线、速度曲线、加速度曲线和急动度曲线的运动性能。

图4 遗传算法优化效果图Fig.4 Effect of Genetic Algorithm Optimization Diagram

图5 修正正弦函数和正弦函数性能对比图Fig.5 Comparison of the Performance of Modified Sine Function and Sine Function Diagram

从5（a）中可以看出，加速正弦函数曲线修正后较修正前，位移变化的更光滑平缓，修正前后的位移均未出现突变；从5（b）中可以看出，修正后的正弦加速曲线的速度与修正前同时达到峰值，但修正后的峰值明显降低，能有效提高码垛机器人的运动性能；从中5（c）可以看出，修正后的正弦加速曲线的加速度较修正前提前达到峰值，而且修正后的加速度峰值略有降低，提高了码垛机器人的动态性能；从5（d）中可以看出，在行程的始末位置正弦加速曲线修正前后均出现较大的突变，但是文中涉及的码垛机器人是轻量中速的，主要对速度和加速度要求较高，急动度的影响相对较小。表1清楚明了的显示了三段修正正弦法的修正效果。从表中容易看出，三段修正正弦函数曲线较正弦函数曲线的速度峰值降低了4.09%，加速度峰值亦降低了4.09%，但急动度峰值升高了12.002%。急动度表示加速度的变化快慢，在高速重载工况下，如果码垛机器人的急动度较大且在运行过程中存在频繁的启停和变化，会导致相应零件发生磨损和振动，甚至可能引起频跳造成受力不均匀、冲击过大和疲劳断裂等问题，所以三段修正正弦法适用于轻载中速的工况，在高速重载的工况下，应该注意适当降低急动度峰值。

表1 正弦加速曲线修正前后的性能比较表Tab.1 Comparison of the Performance Before and After the Correction of the Sine Acceleration Curve

5 结论

（1）通过比较加速度函数为线性函数、三次多项式函数、正弦函数和余弦函数的特性曲线，发现正弦函数的综合运动性能最佳，既无刚性冲击也无柔性冲击，而且不存在位置突变。（2）提出了一种三段修正正弦函数的轨迹规划算法，在关节空间对码垛机器人的运动轨迹进行规划，然后利用遗传算法工具箱以速度峰值、加速度峰值和急动度峰值为目标进行多目标参数优化，得到最佳的修正参数。结果表明，三段修正正弦函数曲线的速度峰值降低了4.09%，加速度峰值亦降低了4.09%，提高了码垛机器人的运动平稳性和运动精度、减少了振动和冲击，延长了码垛机器人的使用寿命。