面向工业机器人的位姿同步加减速算法

陈多多，李丽宏，原 钢

（1.太原理工大学电气与动力工程学院，太原 030024；2.中国煤炭科工集团太原研究院，太原 030006）

对工业机器人来说，最常见的操作要求之一是在笛卡尔空间中将其末端执行器从初始位姿按照指定的空间路径平滑地移动至目标位姿［1-3］。为实现上述过程，需进行位姿轨迹规划［4］，即计算出空间路径上每个点的位置坐标（位置轨迹规划），以及机器人在末端执行器位于这些点时的姿态（姿态轨迹规划），且要求位置轨迹与姿态轨迹光滑连续且同步运行［5-9］。

李振娜等［10］提出带约束S型曲线加减速控制方法对位置轨迹、姿态轨迹进行规划，根据总位移与加速段位移之间的关系等约束条件将S型速度曲线分为4种类型，算法实现较为复杂［11-14］，曲线首末速度为零，影响工业机器人作业执行效率［15］。李振娜等［10］虽然给出位置轨迹和姿态轨迹的规划过程，但未具体给出位姿同步规划的处理过程。王斌锐等［16-17］采用非对称S曲线加减速控制方法进行轨迹规划，该方法相较于带约束S型曲线加减速控制方法，曲线首末速度非必须为零，可实现工业机器人任务空间内圆弧段路径与直线段路径的衔接，提高工业机器人作业执行效率，但需考虑由于规划运行速度较大或运行路径较短等因素引起的匀加速段、匀速段或匀减速段缺失的情形，方法实现复杂。王斌锐等［16］同样只给出位置轨迹和姿态轨迹的规划过程，未具体给出位姿同步规划的实现过程，而罗钧等［17］提出的加减速控制方法对工业机器人轨迹规划具有一定的参考意义，但文献中主要将加减速控制方法集成于机床NC系统中，未提及机器人位姿同步规划的方案。黄忠明等［18］采用正弦波加速度曲线加减速控制方法分别规划位置轨迹和姿态轨迹，按照等时原则对2种轨迹进行了同步规划处理。正弦波加速度曲线加减速控制方法加速度-时间（a-t）曲线按正弦规律变化，表达式简单，加速度无阶跃变化，但考虑到机器人运动过程中需要实时地更新路径位置，而计算机计算三角函数的计算量较大，因此对机器人控制系统的实时性影响较大［19］，且等时原则同步规划处理方法只是实现了位置轨迹和姿态轨迹的等时处理，未实现同步对象同步加速、同步匀速、同步减速的效果。

本文提出一种面向工业机器人的位姿同步加减速算法。本算法首先从正弦波加速度曲线加减速控制方法出发，提出一种基于函数逼近的四次多项式加减速控制方法（quartic polynomial acceleration and deceleration control method based on function approximation，QPADFA），接着在 QPADFA方法的基础上采取完全同步控制策略给出机器人末端位置轨迹和姿态轨迹同步规划处理的方法。

较S型曲线加减速控制方法，QPADFA方法的控制过程简单，只需考虑初次规划得到的加速段位移Sa与减速段位移Sd之和大于给定的总位移ST一种情况下的再次规划问题，不必像S型曲线加减速控制方法那样考虑无匀加（减）速段、无匀速段或两者皆无的情况，易于编程实现。较正弦曲线加减速控制方法，QPADFA方法具有程序运行时间短、程序运行所占内存小的优势。较等时原则，完全同步控制策略可实现工业机器人末端执行器位置轨迹和姿态轨迹同步加速、同步匀速、同步减速的效果。

1 位姿同步加减速算法

1.1 QPADFA方法

本方法首先按照给定的匀速段运行速度、加速段最大加速度、减速段最大减速度和轨迹规划总位移进行初次规划，如果规划得到的加速阶段位移Sa与减速阶段位移Sd之和小于等于总位移ST，则轨迹规划完成，若不满足该条件，则按照S′a+S′d＝ST原则进行再次规划，S′a、S′d分别为再次规划的加速阶段位移、减速阶段位移。

1.1.1 按给定信息进行初次规划

正弦波加速度曲线加减速控制方法的加速度 -时间表达式［20］如下：

式中：ama、amd为给定参数，分别代表加速段最大加速度、减速段最大减速度；Ta、Td、T为待求参数，代表加速段持续时间、减速段持续时间及轨迹规划总时间，其中 t1＝Ta，t2＝T-Td。

使用多项式对式（1）中的加速度函数进行最佳平方逼近。将式（1）中加速段表达式正弦函数部分简单表示为 f（x）＝sin（π（x）），其中，x＝（t／Ta）∈［0，1］。使用线性无关函数族 φ0（x）＝1，φ1（x）＝x，φ2（x）＝x2进行最佳平方逼近，与函数族对应的正规方程组为

其中，（φ0，φ0）表示两函数 φ0（x）与 φ0（x）在区间［0，1］上的内积，其余表达式以此类推。由式（2）可解得 a0＝-0.05，a1＝4.122，a2＝-4.122，则所求最佳平方逼近多项式为

考虑到应使变速段能够达到给定最大加速度且变速段起始和终止时刻的加速度为零，所以将式（3）近似为下式：

同理，可求得减速段表达式最佳平方逼近多项式。则式（1）化简为：

式中，t′＝t-t2。

由式（5）及边界条件积分得速度-时间表达式：

式中：v0为给定的加速段起始速度；vm为给定的匀速段运行速度。

由式（6）及给定的减速段终止速度v2得加速段、减速段持续时间：

由式（6）（7）及边界条件积分得位移-时间表达式：

式中：Sa为加速阶段位移；Sc为匀速阶段位移。

1.1.2 初次规划失败后的再次规划

当按照给定的ama、amd、vm规划得到的加速段位移Sa与减速段位移Sd之和大于给定的总位移ST，即Sa+Sd＞ST时，上述规划中无匀速段，此时需进行再次规划。本方法重新规划的原则是：取消匀速段，先从起始速度v0加速到一个尽可能大的中间速度vt（vt≤vm），然后减速到减速段终止速度v2，使再次规划得到的加速段位移S′a与减速段位移S′d之和等于轨迹规划总位移ST，即：

由式（8）可得再次规划的位移-时间表达式为

式中：t′＝t-t1，S′a为 t＝t1时的位移值，即再次规划得到的加速段位移。中间速度vt由以下操作求得。

由式（7）可知，加速段持续时间与减速段持续时间均与中间速度有关，这里的中间速度是指加速段完成时的临界点速度，也就是减速段开始时的临界点速度。因此，当加速段与减速段之间的中间速度为vt时，有：

将式（11）代入式（10）可求得再次规划后的加速段位移 S′a减速段位移 S′d，再由式（9）可得：

QPADFA方法的规划流程如图1所示。

图1 QPADFA方法的规划流程框图

1.2 位姿同步规划处理方法

对机器人的笛卡尔运动而言，其末端执行器的位置轨迹规划和姿态轨迹规划需要同步进行。本文中，位置和姿态同步变化采取完全同步的控制策略，即首先按照给定位置轨迹参数和姿态轨迹参数分别对位置、姿态按前一节所述QPADFA方法进行轨迹规划，得到位置轨迹的加速段持续时间Taw和姿态轨迹的加速段持续时间Taz，比较两个加速段时间，选择时间较长的作为位姿同步规划中的加速段时间TaF。同样，得到位置轨迹的减速段持续时间Tdw和姿态轨迹的减速段持续时间Tdz，比较2个减速段时间，选择时间较长的作为位姿同步规划中的减速段时间TdF，如果位置轨迹和姿态轨迹中至少有一个存在匀速段，则按照同样的处理方法得到位姿同步规划中的匀速段时间TcF。

在确定了轨迹规划曲线各阶段的时间后，轨迹各变速段的加速度不再确定，此时需重新讨论确定新的位置轨迹和姿态轨迹的位移-时间表达式。

将QPADFA方法的加速段位置-时间、速度-时间、加速度-时间表达式写成多项式形式如下：

若给定加速时间 Ta，并已知 a（0）＝0、a（Ta）＝0、v（0）＝v0，设加速段完成时的临界点速度 v（Ta）＝vL。此时，式（13）中的加速段表达式各项系数为：

将t＝Ta代入式（13）求得加速段位移为：

同理，可求得减速段位移：

根据加速段位移Sa、匀速段位移Sc、减速段位移Sd之和等于总位移ST的原则，可求得

以上式子对再次规划的情况也是适用的，此时Tc＝0。

将式（17）代入式（14）可得加速段持续时间确定下的加速段位移-时间表达式，同理可得减速段持续时间确定下的减速段位移-时间表达式，最后得出位姿同步变化的位移-时间表达式如下：

其中 t1＝TaF，t2-t1＝TcF，T-t2＝TdF，t′＝t-t2。

2 仿真验证

2.1 多路径过渡实验

本实验进行典型多路径衔接模型的轨迹规划。实验目的为验证QPADFA方法根据路径约束条件进行初次规划或再次规划的功能，主要参考文献［16］给出的方法建立典型多路径衔接模型。

实验选取的典型多路径由目标点P0-P2组成。设定轨迹规划匀速段运行速度为80 mm／s，最大加速度100 mm／s2，圆弧过渡段速度为20 mm／s。给定路径信息如表1所示。

表1 路径信息

路径中，路径点P0为直线路径起始点，其速度为0 mm／s，半径调节参数为0；路径点P1为规划路径P0P1与P1P2的交点，因为直线路径之间采用圆弧路径过渡的方式进行，其速度不应为零，且小于等于给定匀速段运行速度，定义其为20 mm／s，定义半径调节参数为0.1，求得过渡圆弧路径的半径为56.5 mm；路径点P2为直线路径终止点，其速度为0 mm／s，半径调节参数为0。

按照图1给出的QPADFA方法规划流程图进行轨迹规划，得到的机器人末端轨迹、末端速度和加速度曲线分别如图2、3所示。图2中，Pi为机器人任务空间中的目标点，PGi为路径过渡衔接点，C1为过渡圆弧路径的圆心。根据表1给出的路径参数及文献［16］提供的计算方法可得：路径一P0PG1的总位移ST1＝59.40 mm，过渡圆弧路径PG1PG2的总位移ST2＝88.86 mm，路径二PG2P2的总位移ST3＝424.26 mm。

图2 机器人末端轨迹

图3 机器人末端速度及加速度曲线

由图3可以看出：路径一P0PG1初次规划失败，进行了再次规划，再次规划得到的最大运行速度为45.3 mm／s，加速阶段最大加速度为56.6 mm／s2，减速阶段最大减速度为 -42.2 mm／s2，且由于路径初始速度vp0与终止速度vpG1不一致，所以其速度曲线和加速度曲线是非对称的。机器人末端以给定速度20 mm／s匀速通过过渡圆弧路径PG1PG2。路径二PG2P2初次规划成功，加速阶段最大加速度达到了给定的100 mm／s2，加速阶段结束时机器人末端速度为给定的匀速运行速度80 mm／s，之后按该速度匀速运行，匀速段结束后，进入减速阶段，减速阶段最大减速度为-100 mm／s2，速度逐渐降至 0 mm／s。

由多路径过渡实验可知：本文轨迹规划方法QPADFA实现了速度的平滑过渡，并可在路径约束条件下进行速度和加速度的自行调整，验证了QPADFA方法的有效性。

2.2 加减速控制方法效果对比实验

本实验从规划运动时间、程序运行时间及对运动曲线平滑性的影响3个方面对QPADFA方法、非对称S曲线加减速控制方法、带约束S型曲线加减速控制方法及正弦波加速度曲线加减速控制方法进行对比。给定直线路径P0P1信息如表2所示。路径中，路径点P0为直线路径起始点，其速度应为0 mm／s，半径调节参数应为0；路径点P1为直线路径终止点，其速度应为0 mm／s，半径调节参数应为0。

表2 路径信息

由表2得位置总位移ST＝1 000 mm。给定加速段起始速度v0＝0 mm／s，匀速段运行速度vm＝500 mm／s，加速段最大加速度 ama＝1 000 mm／s2、减速段最大减速度amd＝-1 000 mm／s2，减速段终止速度 v2＝0 mm／s。

通过Matlab仿真，得到4种方法的程序运行时间及规划运动时间，如表3所示，QPADFA方法与非对称S曲线加减速控制方法的加速度曲线如图4、QPADFA方法与带约束S型曲线加减速控制方法的加速度曲线如图5、QPADFA方法与正弦波加速度曲线加减速控制方法的加速度曲线如图6。

表3 4种方法的程序运行时间及规划运动时间 ms

图4 QPADFA方法与非对称S曲线加减速控制方法的加速度曲线

图5 QPADFA方法与带约束S型曲线加减速控制方法的加速度曲线

图6 QPADFA方法与正弦波加速度曲线加减速控制方法的加速度曲线

由图4、表3可以看出：相对于非对称S曲线加减速控制方法，本文提出的QPADFA方法加速度曲线更加平滑，规划运动时间缩短5%，程序运行时间缩短48.9%。

由图5、表3可以看出：相对于带约束S型曲线加减速控制方法，本文提出的QPADFA方法加速度曲线更加平滑，规划运动时间缩短5%，程序运行时间缩短50.5%。

由图6、表3可以看出：虽然本文提出的QPADFA方法的加速度曲线和正弦波加速度曲线加减速控制方法接近，但规划运动时间缩短3.2%，程序运行时间缩短16.4%，在机器人实时控制系统中，更具实时性优势。

2.3 位姿同步规划实验

本实验目的为验证本文提出的位姿同步加减速算法可以实现工业机器人末端执行器位置和姿态的同步规划。

设定位置轨迹规划匀速段运行速度为80 mm／s，最大加速度 85 mm／s2；姿态轨迹规划的匀速段运行速度为0.15 rad／s，最大姿态角加速度0.15 rad／s2；实验起始点位姿矩阵AS和目标点位姿矩阵AE分别为：

矩阵AS、AE分别为本课题研究使用的6关节型机器人处于2种不同状态时的末端位姿矩阵。由矩阵AS、AE可得起始点与目标点之间的位置偏移为423.73 mm，姿态对应的四元数之间的夹角为0.82 rad。对位置和姿态按照QPADFA方法进行轨迹规划，分别得到2种规划的加速段时间Ta、匀速段时间Tc、减速段时间Td如表4所示。

表4 轨迹规划时间

由表4可以看出：位置轨迹和姿态轨迹单独规划时加速段时间、匀速段时间、减速段时间是不相同的，按照本文给出的完全同步控制策略，确定位姿同步规划的加速段时间TaF＝1.500 s，匀速段时间 TcF＝3.962 s，减速段时间 TdF＝1.500 s。确定各阶段时间后，按照1.2节所述位姿同步规划处理方法对位置轨迹和姿态轨迹进行同步规划处理。

经本文的位姿同步加减速算法得到的位置轨迹的位移-时间、速度-时间及加速度-时间曲线如图7所示，姿态轨迹的角位移-时间、角速度-时间及角加速度-时间曲线如图8所示。

图7 位置轨迹规划-时间曲线

图8 姿态轨迹规划-时间曲线

由图7、8可知：位置轨迹规划运动时间Tw＝6.962 s，姿态轨迹规划运动时间 Tz＝6.962 s，两种轨迹的加速段时间、匀速段时间、减速段时间均相同，该算法实现了位置轨迹和姿态轨迹的同步运行。由图7可知：位置轨迹规划中的匀速段速度不是设定的80 mm／s，而是 77.58 mm／s，这是进行位姿同步规划处理的结果。

图7、8得到的机器人末端执行器位置轨迹规划和姿态轨迹规划的位移-时间曲线、速度-时间曲线是光滑连续的，因此可以忽略工业机器人在运动过程中的刚性冲击；加速度-时间曲线无阶跃且光滑连续，因此可以忽略工业机器人在运动过程中的柔性冲击。本文提出的面向工业机器人的位姿同步加减速算法具有工程实用性。

3 结论

本文中提出了一种面向工业机器人的位姿同步加减速算法，包括QPADFA方法和位姿同步规划处理方法2部分。实验结果表明：相对于非对称S曲线加减速控制方法、带约束S型曲线加减速控制方法及正弦波加速度曲线加减速控制方法，QPADFA方法在规划运动时间、时间复杂度、加速度曲线平滑性等方面有一定优势；位姿同步规划处理方法实现了工业机器人末端执行器位置轨迹和姿态轨迹同步加速、同步匀速、同步减速的效果，具有工程实用价值。本文提出的位姿同步加减速算法是将机器人末端位置和姿态单独规划后进行同步处理的，为了进一步提高机器人轨迹规划的效率，下一步的工作为将位置和姿态统一规划插补以实现综合运算。