工业机器人的实时轨迹插补算法李天友 ,孟正大 ,陈勍奇（东南大学自动化学院，江苏南京 210096）摘要：提出了一种实现工业机器人实时轨迹插补的规划算法。
该算法既能满足时间上的实时性，又能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态。
而对于不同插补类型，只要找准对应线长的表示，不需要对算法本身进行修改，就可以完成相应的轨迹插补。
本算法应用于“昆山一号”焊接机器人中，表明其满足焊接实时性和可调速性要求。
关键词：工业机器人；实时插补；算法；轨迹规划示教再现方式下的轨迹插补算法是工业机器人的一个传统课题[1]，技术和方法比较成熟有效。
文献[2-4]分别解决了直线、圆弧、样条曲线等单一类型的轨迹插补，文献[5,6]讨论了复杂曲线在编程时用分段直线或圆弧进行拟合插补的方法，文献[7]研究了关节空间和笛卡儿空间的通用插补算法，把插补段分为加速段、匀速段、减速段进行插补, 但算法复杂，运算量大，且不能进行实时控制。
此外，时间上满足实时性的轨迹插补方法也得到了研究[3,4]。
但是既满足实时性要求又能够进行平滑调速并且能够同时完成关节空间和笛卡儿空间各种类型插补的通用轨迹插补算法却比较少见。
本文介绍工业机器人的实时轨迹插补算法。
它是为满足“昆山一号”焊接机器人的实时性而设计的，实时性包含两层涵义，一是满足时间上的实时性，即在一个采样周期内能够完成一次轨迹插补，多数算法能够满足这层要求；而实时性第二层涵义是系统能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态，本文的算法很好地完成了这层实时性的要求。
并且这种算法能够完成PTP （点到点）、多点关节空间、直线、圆弧、样条曲线、FlyBy[8,9]等多种类型的轨迹插补。
本文首先介绍实时轨迹插补算法的提出背景，然后给出了算法基本原理的详细说明，并且运用流程图进一步明确了基本算法，对算法的实时性和调速控制进行了分析，最后阐述了算法在多种插补类型中的实际应用。
1 实时轨迹插补算法1.1 提出背景“昆山一号”机器人是我国第一台自主研制的全自动工业焊接机器人，其核心技术“自动控制软件系统”第一版在轨迹插补方面，不仅实现了常见的PTP、直线、圆弧这些插补方式，而且实现了样条曲线插补和多点笛卡尔坐标空间插补等新型插补方式，丰富了机器人的轨迹插补功能，这些插补算法在实际系统运行中，操作简单方便，机器人运行平稳[10]。
先前的算法是把插补段分为加速段、匀速段、减速段，分别对位置和姿态进行定时插补计算，然后把所有的插补点位姿全部求解出来形成位置和姿态链表。
通过对插补点位置和姿态链表进行依次读取执行，来完成各种类型的插补。
这种方法虽然能够完成姿态的插补，但是在已知插补类型的情况下，不能对机器人运动轨迹与状态进行控制，也就是不能满足实时性第二层内容的要求，当然也就不能满足“昆山一号”焊接机器人在实际应用中的电弧跟踪实基金项目：江苏省科技成果转化专项资金项目（BA2007058）和国家高技术研究发展计划资助项目(2006AA040202)作者简介：李天友（1983-），男，河南民权人，硕士生，litianyouseu@时偏差补偿和插补过程中进行调速的需要，所以我们提出了实时轨迹插补算法。
1.2 基本算法机器人控制系统计算点列并形成轨迹的过程叫插补，插补工作有三个要素：插补的参数、插补的方程、插补形成的轨迹（点列）。
只有插补的参数、插补的方程都合乎操作者的需求时，产生的点列才是符合要求的。
其中把插补的参数分成两部分：第一部分参数一般包括示教点的位姿如始点位姿和终点位姿、示教点间的插补方式如直线、圆弧等，一般能够规定插补的轨迹方程，第二部分参数一般包括设定的匀速段速度、变速段加速度、始点速度和终点速度等，一般能够规定机器人工具手末端的运动状态。
为方便说明问题，我们定义空间两点始点A和终点B以及它们之间的线段为直线段（实际上两点之间可以为圆弧、样条曲线、FlyBy等任意形状的线段），算法原理图如图1所示。
图1 实时轨迹插补算法原理示意图图2 实时轨迹插补算法关键步骤说明示意图由图1可知第一部分参数有：始点A位姿startAuxiT6，终点B位姿endAuxiT6，该图例表示的插补类型为直线；第二部分参数有：始点A的速度为Vstart、终点B的速度为Vend、始点A和终点B之间的匀速段速度velo和变速段加速度accel。
定义当前插补点的位姿为currentAuxiT6、速度为Vcurrent和距离A的线长Scurrent，下一个插补点的位姿为nextAuxiT6、速度Vnext和距离A的线长Snext。
定义A和B总线长为S，SA为A附近的变速距离（Vstart按设定加速度变化到velo所需要的线长），SB为B附近的变速距离（velo按设定加速度变化到Vend所需要的线长），SAB为总的变速距离。
定义SAECE为当前插补点处于末段变速状态时所需的变速距离（Vcurrent按设定加速度变化到Vend所需要的线长），SCE为当前插补点到B的距离。
输入变量有：startAuxiT6、 Vstart、endAuxiT6、Vend、currentAuxiT6、Vcurrent、Scurrent （以上三个变量由上次插补算法计算得到）、velo、accel、调速控制量Vrate（通过控制该变量完成机器人的调速，集中反映了该算法通过控制输入变量对机器人的插补状态实行控制）。
输出变量有： Vnext、Snext、nextAuxiT6(以上三个量作为下次轨迹插补的输入控制量)。
算法的核心思想：由以上实时性的分析，可以通过控制输入变量对当前插补点状态施加控制，这样就把整段的连续插补变为可以控制插补状态（如插补的速度和姿态）的单步插补。
在完成当前插补点轨迹插补的同时输出Vnext和Snext，通过计算得到nextAuxiT6，以上三个变量作为下次轨迹插补的输入变量对下次轨迹插补的状态进行控制。
所以现在讨论的问题就变为如何通过控制当前插补点输入变量求解下一插补点的Vnext和Snext上。
在两个插补点之间，存在两个关键判断点：一是比较SCE和SAECE，若SCE不大于SAECE，当前插补点进入末段变速段；若SCE大于SAECE，则进入非末段的变速段；这就相当于直观的把整条线段分成了两个部分，末段变速段和非末段变速段。
第二个关键判断点是速度的对比。
若当前插补点进入末段变速段，首先比较velo和Vend，确定变速段是加速状态还是减速状态，然后比较Vcurrent和Vend，确定当前插补要进行相应的运动还是匀速运动。
而当前插补点处于非末段变速段的处理方法与处于末段变速段类似，首先比较velo 和Vstart ，确定本段是加速状态还是减速状态，然后比较Vcurrent 和Vstart ，确定当前插补要进行相应的运动还是匀速运动。
其关键步骤说明示意图见图2。
所以实时轨迹插补算法处理流程如下：step1：初始化操作，进行速度、加速度、距离合法性检验。
求解SA 和SB ，求和得SAB ；比较SAB 与S 的大小，判别S 的合法性；对插补点末点进行处理；step2：求解处理关键判断点一。
求解SAECE 和SCE ；比较SCE 和SAECE ；若SCE 不大于SAECE ，则进入末段的变速段，进入step3；若SCE 大于SAECE ，则进入非末段的变速段，进入step6；accel Vend Vcurrent fabs SAECE 2/)(22−= (1) step3：处理末段变速段的关键判断点二。
比较velo 和Vend ，若velo 小于Vend ，则本段为加速段，进入step4；若velo 大于Vend ，则本段为减速段，进入step5；step4：比较Vcurrent 和Vend 。
若Vcurrent 小于Vend ，加速操作，算出Vnext 、Snext ；Ts accel Vcurrent Vnext *+= (2)2**5.0*Ts accel Ts Vcurrent Scurrent Snext ++= (3) 其它情况，加速过程提前到达末点速度，则做匀速运动；Vend Vnext = (4) Ts Vcurrent Scurrent Snext *+= (5) step5：比较Vcurrent 和Vend 。
若Vcurrent 大于Vend ，减速操作，算出Vnext 、Snext ；Ts accel Vcurrent Vnext *−= (6)2**5.0*Ts accel Ts Vcurrent Scurrent Snext −+= (7)其它情况，减速过程中提前达到末点速度，则做匀速运动，计算公式同(4)(5)。
step6：处理非末段变速段的关键判断点二。
比较velo 和Vstart ，若Vstart 小于velo ，则本段为加速段，进入step7；若Vstart 不小于velo ，则本段为减速段，进入step8；图3 实时轨迹插补算法的流程示意图 图4 直线在线实时插补的仿真效果示意图 step7：比较Vcurrent 和velo 。
若Vcurrent 小于velo ，加速操作，算出Vnext 、Snext ，计算公式同(2)(3)。
若Vcurrent 达到或暂时超过velo ，则需要将速度保持或调整为velo ，计算公式如下：velo Vnext = (8) Ts Vcurrent Scurrent Snext *+= (9) step8：比较Vcurrent 和velo 。
若Vcurrent 大于velo ，减速操作，算出Vnext 、Snext ，计算公式同(6)(7)。
若Vcurrent 达到或暂时超过velo ，则需要将速度保持或调整为velo ，计算公式同 (8)(9)。
step9：实时轨迹插补算法结束。
根据以上的分析推导过程可以得到如图3所示的实时轨迹插补算法流程图。
1.3 算法的实时性和调速控制分析要实现调速控制，引入调速控制量Vrate （其中有效范围：0-100%），作为插补点的输入变量，对轨迹插补的速度进行控制。
由Sstep Scurrent Snext += (10) 其中Sstep 为单步插补步长；要实现速度的调节，既把V 调整为V*Vrate ；设Ssteprate 为速度调整后的步长,由dt V Sstept t ∫=21 (11)调整为 Vrate dt V dt Vrate V Ssteprate t t t t *)()*(2121∫∫== (12)所以对速度V 的调节转化为对步长Sstep 的调节，得到引入Vrate 后的控制方程：Vrate Sstep Scurrent Snext *+= (13) 由以上推理可知，要实现速度的平滑调节，以公式(3)为例，计算公式如下：Vrate Ts accel Ts Vcurrent Scurrent Snext *)**5.0*(2++= (14) 要实现电弧跟踪实时偏差补偿，由以上算法得到Vnext 和Snext ，计算得到nextAuxiT6，再由电弧跟踪得到的偏差补偿量进行姿态调整，求得补偿后的新的nextAuxiT6位姿量，然后把这三个量作为中间变量，等待下一步调用，这样就完成了电弧跟踪偏差的实时补偿。