刘 尧， 胡 博， 张 辉， 郭 强， 张勇军*， 李宏毅

(1.北京科技大学 工程技术研究院,北京 100083; 2.中国石油集团长城钻探工程有限公司钻井一公司，辽宁 盘锦 124010)

轨迹规划一直都是工业机器人研究的重点，是国内外机器人应用领域研究的重点内容之一[1-2]。轨迹规划可以分为两个方面，一个是机器人运动过程中的路径选择，另一个是减少机器人关节运行时的振动与冲击[3-4]。Starcevic等[5]研究了机器人的空间路径规划过程，对机器人轨迹进行插值时结合了三次多项式和四次多项式，这种轨迹规划的方法对于机器人运行中的轨迹突变而引起的机器人运行振动等问题起到不错的改善作用。文献[6]用两种不同频率的正弦加速度函数进行轨迹规划，函数充分利用了正弦函数的优点，把插补段分为加速段、匀速段、减速段进行插补，降低了速度和加速度的峰值，使机器人运行更加稳定，但是该方法算法复杂、运算量大，且不能实现实时控制。

在运动控制领域中常用的加减速控制算法有梯形速度曲线控制算法和S型速度曲线控制算法[7]。S型速度曲线能够在一定程度上避免梯形速度曲线加速度不连续而导致的机械冲击或振动效应，是一种较为常用的机器人运动控制算法。但理想的S型速度曲线控制算法运动过程程序实现较为复杂，而且其加加速度不连续，存在软冲击。文献[8]等提出了S型速度曲线的新算法，有效地将7段S型曲线简化为5段S型曲线，简化了计算过程，但其加加速度不连续的情况依然存在。Sun等[9]提出了三角函数构造的柔性加减速曲线，使速度、加速度、加加速度连续可导，运动平稳性较高，但其计算过程中包含三角函数运算，增加了计算时间且丢失了部分精度。

本文以提高系统运行效率和安全裕度为目标，将多项式速度曲线控制算法与传统S型速度曲线控制算法相结合，提出一种改进的S型速度曲线控制算法，可减小加加速度突变次数，提高加速度连续变化平滑性，使速度连续变化更快且移动效率更高，然后对上下料系统典型运动轨迹进行优化，将机械手运行过程中的直角拐弯路径改为在拐角处以弧线方式过渡的路径。改进后S型速度曲线控制算法，有效减少发生冲击或振动的可能性，达到了提高系统工作稳定性、运行效率和控制精度的目标，并通过仿真与实际运行测试对该方法进行了验证。

1 常用加减速算法

现有的设备在某些加工要求不高的生产过程中，一般采用梯形加减速算法或指数型加减速算法。这2种算法的优点是实现简单、计算量小，但缺点是在加减速启停阶段，会出现速度和加速度突变，会对机械系统产生冲击[10]。在加工要求较高的生产过程中，一般采用7段S型加减速算法，相比于前两种方法，这种方法的优点是能够计算出平滑的速度和加速度曲线，其缺点是精度有限、快速响应依旧会产生突变。

1.1 梯形加减速算法

梯形加减速控制算法是指系统在加减速过程中，速度沿某个固定斜率直线变化，加速度保持恒定，速度均匀变化[11]，如图1所示。

图1 梯形加减速速度曲线变化图

梯形加减速速度曲线由以下3个部分构成。

① 加速阶段：速度沿固定斜率直线线性增加，公式为

vi=at

(1)

② 匀速阶段：速度保持恒定，公式为

vi=vc

(2)

③ 减速阶段：速度沿固定斜率线性减小，公式为

vi=ve-at

(3)

梯形加减速算法的优点是实现简单，运算量小，适用于低速慢变化、低成本的生产加工系统。但由于算法采用恒定的加速度，因而在加减速过程的起点和终点位置加速度有突变，这就导致在实际系统中，电机运行过程中会有各种振动和噪声，系统运行不够平滑。

1.2 指数型加减速算法

指数型加减速算法是指在加减速阶段速度按照指数规律上升或下降[12]，如图2所示。

图2 指数型加减速速度曲线变化图

指数型加减速速度曲线也分3个阶段，具体如下。

① 加速阶段：速度曲线按指数规律变化，公式为

vi=vc(1-e-i/T)

(4)

② 匀速阶段：速度保持不变，公式为

vi=vc

(5)

③ 减速阶段：速度沿指数曲线下降，公式为

vi=vcei-tc/T

(6)

相比于梯形加减速算法，指数型加减速算法平滑性有所提高，但运算量要比梯形加减速算法大，故导致动态响应变慢。此外，在加减速过程的起点和终点，依旧存在速度与加速度突变的情况。

1.3 S型速度曲线控制算法

S型速度曲线控制算法是一种在机电系统运动控制的加减速过程中对速度变化曲线进行柔性控制的方式[13-14]。与常见的梯形速度曲线控制算法相比，S型速度曲线控制算法加减速曲线变化平滑，加速度是连续、线性的，可以减少机械系统因加速度不连续而产生冲击[15]。如图3所示，S型速度曲线轨迹可以分为加加速、匀加速、减加速、匀速、加减速、匀减速、减减速共7个阶段。

图3 S型速度曲线轨迹图

若定义运动位移为s，时间为t，正向最大加速度为amax，正向最大加加速度为jmax，负向最大加速度为-amax，负向最大加加速度为-jmax，则S型速度曲线的轨迹可用以下方程表示：

(7)

(8)

(9)

(10)

在实际应用中，受上下料机器人机械部件的限制，手爪在进行横向或纵向移动时，其允许的最大加速度、最大速度已经确定，在加速度、速度、位移等参数已知的条件下，可计算出加速段、匀速段、减速段对应的时间，进而可以得到S型速度曲线的7个阶段对应的7个时间，并可进一步确定速度曲线的轨迹变化。

若系统最大加速度amax、最大加加速度jmax、初速度v0、末速度ve等条件已知，当系统到达最大加速度amax且无匀加速、匀减速、匀速段时，此时位移s、速度v、加速度a和加加速度j的变化如图4所示，图4中位移大小为s4，速度最大值为vmax。

图4 S型曲线变化图

根据s4与实际位移大小L以及vmax与系统设定的最大速度vs之间的大小关系，可对以下情况进行分析。

① 若Lvmax，L的长度不足以使系统增大至最大加速度amax，此时不存在匀加速段、减速段和匀速段，速度v与加速度a的变化关系如图5(a)所示。

② 若vs

③ 若L>s4，vs>vmax，L的长度足够长可以使加速度增大至amax，并且vmax没有超过系统最大速度vs，此时存在匀加速段、匀减速段和匀速段，速度v与加速度a的变化关系如图5(c)所示。

图5 速度与加速度变化曲线

可以看出，S型速度曲线控制算法提高了加减速过程中的稳定性，使运动过程中的加速度、速度变化连续，有较好的平滑性，但其参数较多、计算复杂，且存在加加速度不连续的问题，容易使机器人在运动过程中发生冲击或振动，系统稳定性还需进一步提高。

2 改进后S型速度曲线控制算法

为解决传统的S型速度曲线控制算法程序实现复杂，且加加速度不连续易造成软冲击的问题。在上述研究的基础上，将多项式速度曲线控制算法与S型速度曲线控制算法相结合，提出了一种改进的S型速度曲线控制算法，其速度、加速度、加加速度连续变化，平滑性好。

若定义O、s1、s2、s3分别为起始点、匀速起点、匀速终点、结束点的位移量，vs是起始点的速度，vm是匀速段的速度，vf是结束点的速度，ama和amb为实际情况下系统所允许的最大加速度、最大减速度。改进后S型速度曲线位移s、速度v、加速度a、加加速度j之间的变化关系如图6所示。图6中[0，t1]为加速区间，(t1，t2]为匀速区间，(t2，t3]为减速区间，区间[0，t1]和区间(t2，t3]的加加速度曲线为二次曲线，区间[0，t1]和区间(t2，t3]的加速度曲线为三次曲线。

当t∈[0，T1]时，加加速度j在t=0时有最大值，可令其函数表达式为

j(t)=b1t2+b2

(11)

式中，b1和b2为二次函数待定系数。根据位移、速度、加速度、加加速度之间的微积分关系，可推导出改进的S型速度曲线在加速区间t∈[0，t1]的运动方程为

(12)

实际运动过程中，需要减少加速段和减速段的时间来提高运动效率，所以加速度曲线a(t)与坐标轴t所包围的面积要尽可能地大，而加速度曲线应满足a(t)max≤ama，结合初始速度、初始加速度等条件，可建立如下已知条件：

(13)

其中ta∈[0，t1]，在ta时刻加速度a(t)取得最大值，此时j(ta)=0，联立式(12)和式(13)可解得：

(14)

将式(14)解得的值代入式(5)～式(8)，可求得加速段t∈[0，t1]的运动方程为

(15)

同理，可以推导出改进的S型速度曲线在匀速段t∈(t1,t2]的运动方程为

(16)

也可以推导出改进的S型速度曲线在减速段t∈(t2,t3]的运动方程为

(17)

在实际的运动系统中，最大速度即匀速段速度vm和目标位移sf一般是给定的，可以求得各个阶段的时间参数t1、t2、t3：

(18)

由改进的S型速度曲线运动方程可知，当时间参数t1、t2、t3确定好之后，便可以求得各个时间的位移、速度、加速度和加加速度。与传统的S型速度曲线控制算法相比，改进后的算法求解相对简单，实际运用中易于实现，且加速度、加加速度均连续，可以使运动系统具有更好的高速运动平稳性。

3 改进后S型速度曲线控制算法的实现

以某汽车零部件加工企业的数控机床自动上下料系统为背景，对改进前后S型速度曲线控制算法在桁架式机器人自动上下料流程中机械手爪运动路径规划问题进行研究。自动上下料系统要求桁架式机械人能够在两台数控机床之间完成连续取料、换料、放料的任务，代替人工操作实现待加工工件在两道工序之间的传递，现场布局示意图如图7所示。

图7 自动上下料系统现场布局示意图

自动上下料机器人系统的工作流程如图8所示。首先机器人手爪从取料料盘处取料，然后分别移动待加工工件经过两台数控机床，完成两次加工与换料，最后将加工好的工件放入放料料盘并经传送带传送到工件存储处。

图8 自动上下料机器人系统的工作流程示意图

两台数控机床的上下料轨迹基本类似，且下料过程是上料过程的逆运动，上料轨迹与下料的逆向轨迹也基本系统。本文仅对一台数控机床的上料轨迹进行分析分析，将机械手爪上料过程的运动轨迹划分为4段，即由AB、BC、CD、DE组成的直角拐弯路径，如图9虚线所示。但这种上料轨迹存在很明显的缺陷，主要表现在工作效率较低，且在拐角处易造成装置的抖动，会对设备造成损害。

图9 机械手爪上料运动轨迹对比

针对直角拐弯路径存在的缺点，将直角拐弯改成弧线过渡，规划后的轨迹如图9实线所示，这样可以提高系统的工作效率，并且使速度变化平稳，提高了系统的稳定性。假设AB长度为S1，A点到B点的时间为T1；BC长度为S2，B点到C点的时间为T2；CD长度为S3，C点到D点的时间为T3；DE长度为S4，D点到E点的时间为T4。已知在实际的上料过程中，S1S3，S3>S4，可以做出如下路径规划。

①t=0时刻为上升段S1的初始时刻。

4 仿真与实验验证

桁架式数控机床上下料机器人的机械手爪移动分为横向运动和纵向运动两个分运动，对于各分运动，分别采用梯形控制算法、S型速度曲线控制算法和改进后S型速度曲线控制算法对两个方向的机械手爪运动进行加减速控制，以验证算法的使用效果。

根据实际用于双数控机床上下料的桁架式机器人设计参数设定仿真过程中的数据，其中纵向位移S1=1000 mm，横向位移S2=2500 mm，纵向位移S3=800 mm，横向位移S4=600 mm，此外为避免机械臂受到过大冲击，加加速度最大上限不应超过6500 mm/s3。为了验证文中提出的改进的S型曲线控制算法的有效性，采用Matlab仿真软件对梯形控制算法、传统S型速度曲线控制算法和改进S型曲线控制算法分别对机械手爪的位移、速度、加速度、加加速度的运动进行仿真，仿真结果分别如图10和图11所示。

图10 不同算法横向、纵向位移与速度仿真结果

图11 不同算法横向、纵向加速度和加加速度仿真结果

梯形控制算法采用恒定的加速度，是一种较易使用的算法。由图10可知，相比于两种S型速度曲线控制算法，梯形控制算法的加减速过程中会出现明显的拐点、系统运行不够平滑，可能对系统造成冲击。从图11可知，在梯形控制算法最大加加速度大于两种S型速度曲线控制算法的情况下，梯形控制算法完成移动使用时间仍大于两种S型速度曲线控制算法。由此可以看出，两种S型速度曲线控制算法运行速度的稳定性方面要优于梯形控制算法。在此基础上，进一步探究两种S型速度曲线控制算法的性能。

两种S型速度曲线控制算法的位移、速度、加速度和加加速度具体参数如表1所示。从表1中可以看出，横向最终位移是3100 mm，纵向最终位移是200 mm，改进前后最大运动速度均为614 mm/s。其中，横向匀速运动时间从3.125 s增加为3.135 s，纵向匀速运动时间为从1.010 s增加为1.051 s；横向、纵向变速运动时间从3.84 s减小为3.6 s。在完成相同路程的前提下，匀速运动时间增加且变速运动时间减小，说明机器人可更快完成加减速过程进入平稳运行状态。结合位移和速度曲线可知，改进后运动轨迹中的位移和速度曲线相比于改进前，在最大运动速度和匀速运动时间保持一致的情况下，完成相同的动作位移所需时间要少0.3 s。说明改进后轨迹的加减速过程用时更短，在一定程度上可提升运行效率。

表1 两种S型速度曲线控制算法参数指标表

改进前的加加速度为定值3000 mm/s3，改进后的加加速度可连续变化，最大值为6020 mm/s3；改进前加速度最大值为1000 mm/s2，改进后加速度最大值为1020 mm/s2。对比加速度、加加速度曲线(如图12所示)，改进后的加加速度减少了阶跃变化的次数，使加速度曲线可以更加平滑，降低了软冲击次数。改进后加速度曲线相比于改进前，无明显的拐点并减少了不可导点数量，有效降低了发生冲击、振动或系统抖动的可能性。因此可以在保证系统工作稳定性、运行效率和控制精度的前提下，提高加加速度的上限，进一步减少运动时间。

在Matlab仿真基础上，将改进后S型速度曲线控制算法应用于新代机械臂驱控一体机SCD-80RA，测试结果如表2所示。与Matlab仿真相比，实际物理验证虽略有差异，但改进后控制算法能够很好地实现两台数控机床系统进行自动上下料这一任务要求，并能够在不影响系统的控制精度、稳定性的条件下获得更大的加加速度，减小加速度变化所需时间和整体运动时间，有效提升了工作效率。

表2 改进后S型速度曲线控制算法实际测试与仿真性能指标表

5 结束语

对于机械臂上下料等机器人应用场景，要求末端机构运动尽可能平稳，即系统加减速过程需要具有高度的柔性。传统的梯形加减速和指数型加减速算法在运动过程中存在突变冲击，不适于快响应系统。七段S型加减速算法虽有一定提升，但有时还无法满足某些系统平稳运行的要求。为此本文以桁架式上下料机器人机械手爪的运动控制为研究对象，将易于实现的多项式曲线融入到S型速度曲线控制算法中，使得改进后的S型速度曲线控制算法求解过程更易于程序实现，且加速度、加加速度均连续。通过对典型双机床桁架式上下料机器人的轨迹优化，将机械手爪运行过程中的直角拐弯路径改为在拐角处以弧线方式过渡的路径。Matlab仿真实验分析和物理验证结果表明，在改进后的S型速度曲线控制算法和带有弧线过渡路径的模式下，机械手爪的位移、速度、加速度可以随时间连续变化且运行平稳，从而达到缩短上下料过程中机械手的工作时间、提高系统运行效率和稳定性的目的。