摘要：以四自由度码垛机器人为研究对象，基于关节空间提出一种新型多项式拟合插值的运动轨迹规划方法。该方法不仅能够保证其速度、加速度甚至冲击有界且连续，还可得到不同限制条件下机器人最短执行时间；同时根据码垛机器人的特殊结构和特定运动模式，基于机器人末端运行路径实现了对关节运动过程的进一步合并优化，使得编程控制简单化。基于MATLAB软件对拟合曲线实现了仿真，分析对比了对称型及非对称型拟合曲线各自的性能。最后，在TRIO Motion Perfect软件环境下实现了码垛机器人实验运动控制。结果表明，该运动轨迹规划方法准确可行且工作效率高。
关键词：码垛机器人；轨迹规划；多项式拟合；最短时间规划
码垛机器人作为一种常见的工业机器人，其功能是代替人工进行物件的转移和堆垛，从而提高劳动生产力，实现生产过程自动化。随着生产规模的扩大和自动化水平的提高，对机器人工作效率提出了更高的要求，同时还要保证整个机器人系统的稳定性和可靠性。因此，轨迹规划越来越重要。
轨迹规划是指操作臂在运动过程中的位移、速度和加速度的曲线轮廓规划[1]。在关节空间中进行轨迹规划是指将关节变量表示为时间的函数来描述操作臂预期的运动，在关节空间直接进行轨迹规划具有很好的实时性。国内外不少学者在这方面作了大量工作。Kim 等[2]和Lin等[3]利用3次多项式关节轨迹规划方法,需要解一个(n-2)×(n-2)的对角方程，其中n为轨迹的中间点数。这种方法的缺点是当n较大时计算量大。Bazaz等[4]在3次样条函数的基础上实现了在线轨迹规划，并对轨迹进行了时间优化设计，但这种方法不能满足轨迹点之间的加速度连续性。
机器人机械系统在运动过程中必须遵循一个原则，就是运行过程尽量平滑，突变的运动需要无穷大的动力来实现，而电机则因受物理限制不能提供上述动力[5]，这就需要系统函数具有连续的一阶导数、二阶导数甚至三阶导数，同时满足最大速度、加速度和冲击要非常小，以减小振动[6]。为设计出性能优越的、符合要求的轨迹规划曲线，本文引入高阶多项式拟合的概念，进而推导出满足速度、加速度和冲击等运动学约束条件的最短操作时间。同时将非对称这个概念引入到轨迹规划中来，将规划推理出的对称组合曲线进行非对称化，然后分析对比，力求找出满足不同要求的、性能优越的轨迹规划曲线。
1 样机结构
本文采用的码垛机器人实体样机如图1[7]所示，其机械系统主要由4个关节组成，能实现基座旋转、水平移动、垂直移动和手腕旋转4种运动。机器人空间运动范围构成一圆柱形空间，我们将机器人由底部至手腕的4个关节分别定义为关节A、B、C、D。根据各个关节的运动模式，码垛机器人坐标可以用M(R，θ，Z，Ψ)形式表示，其中R代表C的位移量，θ代表A旋转角度，Z代表B位移量，Ψ代表D旋转角度。
图1 码垛机器人实体样机
Fig.1 Prototype of the palletizing robot
2 多项式拟合轨迹算法实现
码垛机器人能否快速、精确、稳定地从起始位姿到达终止位姿，轨迹规划中控制曲线的选取至关重要[8]。在位置、速度和加速度甚至冲击等多约束的条件下，需要高阶式的多项式插值。因此，本文采用4次多项式与5次多项式以加减速过程1∶1等比分段拟合的方法，得到对称型分段拟合函数。即
(1)
其中
。
(2)
将这个多项式作为比例项和自变量，描述各个关节变量θ在其值域范围内的运动，从而有
，
(3)
其中θ0和θT分别是机器人在起始和终止位姿的关节变量，起始时间令t=0开始，工作时间为T。
可得
，
(4)
类似有
，
(5)
。
(6)
若起始和终止的关节速度为0，那么，在PTP运动模式条件下，整个运动过程应满足以下边界条件：
(7)
在各段函数的连接点处，需要建立临界条件，设定在连接点处当时，关节速度达到最大值，因而其关节加速度满足
。
(8)
所以将式(7)代入式(3)～(5)，得到
应该满足的边界条件
(9)
上式中s″0和s″1需满足限制条件s″0≥0，s″1≤0。为减少机械振动和冲击，对曲线提出更高的要求，即保证位置、速度、加速度以及冲击函数连续。那么，要求插值函数(1)及其一阶、二阶及三阶导数在连续，同时，由
以及式(9)可以求得各项系数分别为
。
(10)
对于较低共振频率的机器人关节，要实现动作又要不引起冲击、不产生残留振动需要对轨迹曲线进行非对称化[9]。取具有代表性的加减速比2：3，希望通过在减速段将加速度及速度控制在较小值，使机器人在停止时更加平稳，从而降低残余振动。得到的非对称型多项式拟合函数如下：
(11)
同理得到各项系数值如下：
。
(12)
若限制电机的最大转速，由式(4)转换可得给定操作的最短时间T为
。
(13)
同样，若限制关节允许的加速度，由式(5)可求得给定操作的最短时间T为
(14)
那么，在同时限制关节允许速度及加速度的情况下，求得给定操作的最短时间T为
，
(15)
其中的可以通过电机的最高转速及该关节的减速比确定，θ″max则通过该关节两点之间受到的最大转动惯量以及电机的速度——转矩特性来确定。若同时给出关节的冲击限制，可进一步结合式(6)求出最短时间T。
3 机器人末端运行轨迹的优化
码垛机器人在正常工作中运动模式为PTP运动，即只需确定各段运动轨迹的端点，端点之间的运动轨迹由四轴电机分别单独控制[10]。根据实际码垛过程中壁障要求，将机器人工作过程的运动分为四个阶段，如图2所示。
阶段一，B轴运动机器人抓手从生产线抓取位P1出发上升至一定高度至P2(低于最高点P3)；
阶段二，A、B、C、D四轴联动机器人上升至最高点P3;
阶段三，A、B、C、D四轴联动机器人下降至堆垛点上方P4；
阶段四，B轴运动机器人从码垛上方下降至目标点P5。
图2 末端运行轨迹
Fig.2 Motion track of robot end-effector
在上述四个阶段中，在阶段一和阶段四中只有B关节运动，而阶段二和阶段三4个关节同时运动，也就意味着B关节需要做4次加减速。为提高码垛效率，我们把B关节的4个加减速过程合并为两个加减速过程，即P1→P3和P3→P5两次运动，其中从P2→P4阶段四轴同时运动不变。
4 实验验证
以机器人运动学限制为前提，取s″0=3.5，s″1=0，并运用MATLAB软件编程，运行后得到图3所示的对称型多项式拟合插值运动轨迹曲线和非对称型多项式拟合插值运动轨迹曲线对比图。表1显示了机器人各个关节运动学数据限制。
从图3看出，两种曲线皆具有连续的速度、加速度及冲击，且曲线平滑过渡。相对于对称型多项式拟合插值，采用非对称型插值轨迹规划方法将曲线后半部分进行平缓扩大，虽然能够使得机器人关节达到快速运动、缓慢停止的效果，却是以增大速度的最值为代价的。根据式(13)，如果限定电机的最大转速，完成给定操作对称型拟合函数所用的时间较短，相对具有更高的工作效率。因此实际运行中可以综合考虑这两种轨迹，充分利用各自优点，采用最优的轨迹。
图3 插值多项式拟合曲线
Fig.3 Fitting curve of the interpolation polynomial
表1 各轴运动学约束
Table 1 Kinematics constraints for each joint
把本文的理论研究应用于四自由度机器人上，以非对称型多项式拟合算法为例，验证其正确性。以TRIO的EURO205x型控制器为系统控制核心，在TRIO Motion Perfect软件环境下进行运动指令编程，实现机器人抓包试验，运动过程中抓取点P1及放置点P2的关节坐标由控制示教点的位置确定。实验条件如下：末端采用50 kg的鱼饲料包作为负载，高度为20 cm；抓取点A、C、D关节坐标为(80 °，300 mm，10°)，放料托盘中心A、C、D关节坐标为(140°，600 mm，70 °)；采用相邻层 3×2 交错码垛方式；机器人上升高度h1为400 mm，下降高度h2为600 mm。Motion Perfect v3示波器实时追踪的各关节位置以及速度轨迹曲线如图4和图5所示。
图4 各关节位移轨迹曲线
Fig.4 Displacement curve for each joint
图5 各关节速度轨迹曲线
Fig. 5 Velocity curve for each joint
共进行10次测试，每次堆垛4层(搬运20个来回)。测得码垛过程中单包最高速度可达2.55 s ，总时间为39.56 s，平均速度为3.42 秒/包，这样码垛效率可达1 100包/时。通过关节运动轨迹图5及图6看出，各关节位移和速度曲线光滑连续且减速过程平缓，既保证了机器人运动的平稳性又满足了高效码垛工作的要求，从而验证了运动规划方法的可行性。
5 结论
(1)在关节空间内，通过4、5次多项式拟合运动轨迹规划保证了高阶导数连续，避免运动突变，同时得到多运动学约束条件下的最短操作时间；
(2)结合码垛机器人自身特点和实际运动需求设计了抓手基本路径规划，对关节运动作了合并优化，进一步提高了码垛速度，同时更有利于编程控制；
(3)跟据MATLAB仿真曲线结果，分析对比本文提出的对称及非对称型多项式拟合算法各自性能优缺点，提高了算法的适应性和可行性；
(4)采用ABT码垛机器人进行实验验证，由示波器实时显示的运动轨迹曲线表明，本文的规划方法运用于实际项目中效果显著。
