工业机器人的实时轨迹插补算法

1、工业机器人的实时轨迹插补算法李天友 ,孟正大 ,陈勍奇（东南大学自动化学院，江苏南京 210096）摘要：提出了一种实现工业机器人实时轨迹插补的规划算法。该算法既能满足时间上的实时性，又能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态。而对于不同插补类型，只要找准对应线长的表示，不需要对算法本身进行修改，就可以完成相应的轨迹插补。本算法应用于“昆山一号”焊接机器人中，表明其满足焊接实时性和可调速性要求。关键词：工业机器人；实时插补；算法；轨迹规划示教再现方式下的轨迹插补算法是工业机器人的一个传统课题1，技术和方法比较成熟有效。文献2

2、-4分别解决了直线、圆弧、样条曲线等单一类型的轨迹插补，文献5,6讨论了复杂曲线在编程时用分段直线或圆弧进行拟合插补的方法，文献7研究了关节空间和笛卡儿空间的通用插补算法，把插补段分为加速段、匀速段、减速段进行插补, 但算法复杂，运算量大，且不能进行实时控制。此外，时间上满足实时性的轨迹插补方法也得到了研究3,4。但是既满足实时性要求又能够进行平滑调速并且能够同时完成关节空间和笛卡儿空间各种类型插补的通用轨迹插补算法却比较少见。本文介绍工业机器人的实时轨迹插补算法。它是为满足“昆山一号”焊接机器人的实时性而设计的，实时性包含两层涵义，一是满足时间上的实时性，即在一个采样周期内能够完成一次轨迹插

3、补，多数算法能够满足这层要求；而实时性第二层涵义是系统能够在完成机器人当前轨迹插补的同时，实现在线调整插补参数，改变机器人当前插补方程，从而改变机器人运动轨迹与状态，本文的算法很好地完成了这层实时性的要求。并且这种算法能够完成PTP （点到点）、多点关节空间、直线、圆弧、样条曲线、FlyBy 8,9等多种类型的轨迹插补。本文首先介绍实时轨迹插补算法的提出背景，然后给出了算法基本原理的详细说明，并且运用流程图进一步明确了基本算法，对算法的实时性和调速控制进行了分析，最后阐述了算法在多种插补类型中的实际应用。1 实时轨迹插补算法1.1 提出背景“昆山一号”机器人是我国第一台自主研制的全自动工业焊接

4、机器人，其核心技术“自动控制软件系统”第一版在轨迹插补方面，不仅实现了常见的PTP 、直线、圆弧这些插补方式，而且实现了样条曲线插补和多点笛卡尔坐标空间插补等新型插补方式，丰富了机器人的轨迹插补功能，这些插补算法在实际系统运行中，操作简单方便，机器人运行平稳10。先前的算法是把插补段分为加速段、匀速段、减速段，分别对位置和姿态进行定时插补计算，然后把所有的插补点位姿全部求解出来形成位置和姿态链表。通过对插补点位置和姿态链表进行依次读取执行，来完成各种类型的插补。这种方法虽然能够完成姿态的插补，但是在已知插补类型的情况下，不能对机器人运动轨迹与状态进行控制，也就是不能满足实时性第二层内容的要求，

5、当然也就不能满足“昆山一号”焊接机器人在实际应用中的电弧跟踪实基金项目：江苏省科技成果转化专项资金项目（BA2007058）和国家高技术研究发展计划资助项目(2006AA040202作者简介：李天友（1983-），男，河南民权人，硕士生，litianyouseu时偏差补偿和插补过程中进行调速的需要，所以我们提出了实时轨迹插补算法。1.2 基本算法机器人控制系统计算点列并形成轨迹的过程叫插补，插补工作有三个要素：插补的参数、插补的方程、插补形成的轨迹（点列）。只有插补的参数、插补的方程都合乎操作者的需求时，产生的点列才是符合要求的。其中把插补的参数分成两部分：第一部分参数一般包括示教点的位姿如始

6、点位姿和终点位姿、示教点间的插补方式如直线、圆弧等，一般能够规定插补的轨迹方程，第二部分参数一般包括设定的匀速段速度、变速段加速度、始点速度和终点速度等，一般能够规定机器人工具手末端的运动状态。为方便说明问题，我们定义空间两点始点A 和终点B 以及它们之间的线段为直线段（实际上两点之间可以为圆弧、样条曲线、FlyBy 等任意形状的线段），算法原理图如图1所示。 图1 实时轨迹插补算法原理示意图 图2 实时轨迹插补算法关键步骤说明示意图 由图1可知第一部分参数有：始点A 位姿startAuxiT6，终点B 位姿endAuxiT6，该图例表示的插补类型为直线；第二部分参数有：始点A 的速度为Vst

7、art 、终点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 为当前插补点处于末段变速状态时所需的变速距离（Vcur

8、rent 按设定加速度变化到Vend 所需要的线长），SCE 为当前插补点到B 的距离。输入变量有：startAuxiT6、 Vstart、endAuxiT6、Vend 、currentAuxiT6、Vcurrent 、Scurrent （以上三个变量由上次插补算法计算得到）、velo 、accel 、调速控制量Vrate （通过控制该变量完成机器人的调速，集中反映了该算法通过控制输入变量对机器人的插补状态实行控制）。输出变量有： Vnext、Snext 、nextAuxiT6(以上三个量作为下次轨迹插补的输入控制量 。 算法的核心思想：由以上实时性的分析，可以通过控制输入变量对当前插补点状态

9、施加控制，这样就把整段的连续插补变为可以控制插补状态（如插补的速度和姿态）的单步插补。在完成当前插补点轨迹插补的同时输出Vnext 和Snext ，通过计算得到nextAuxiT6，以上三个变量作为下次轨迹插补的输入变量对下次轨迹插补的状态进行控制。所以现在讨论的问题就变为如何通过控制当前插补点输入变量求解下一插补点的Vnext 和Snext 上。在两个插补点之间，存在两个关键判断点：一是比较SCE 和SAECE ，若SCE 不大于SAECE ，当前插补点进入末段变速段；若SCE 大于SAECE ，则进入非末段的变速段；这就相当于直观的把整条线段分成了两个部分，末段变速段和非末段变速段。第二个

10、关键判断点是速度的对比。若当前插补点进入末段变速段，首先比较velo 和Vend ，确定变速段是加速状态还是减速状态，然后比较Vcurrent 和Vend ，确定当前插补要进行相应的运动还是匀速运动。而当前插补点处于非末段变速段的处理方法与处于末段变速段类似，首先比较velo 和Vstart ，确定本段是加速状态还是减速状态，然后比较Vcurrent 和Vstart ，确定当前插补要进行相应的运动还是匀速运动。其关键步骤说明示意图见图2。所以实时轨迹插补算法处理流程如下：step1：初始化操作，进行速度、加速度、距离合法性检验。求解SA 和SB ，求和得SAB ；比较SAB 与S 的大小，判别

11、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

12、小于Vend ，加速操作，算出Vnext 、Snext ；Ts accel Vcurrent Vnext *+= (22*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 *= (62*5. 0*Ts accel Ts Vcurre

13、nt 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 ，则需要

14、将速度保持或调整为 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%

15、），作为插补点的输入变量，对轨迹插补的速度进行控制。由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

16、为例，计算公式如下：Vrate Ts accel Ts Vcurrent Scurrent Snext * *5. 0*(2+= (14 要实现电弧跟踪实时偏差补偿，由以上算法得到Vnext 和Snext ，计算得到nextAuxiT6，再由电弧跟踪得到的偏差补偿量进行姿态调整，求得补偿后的新的nextAuxiT6位姿量，然后把这三个量作为中间变量，等待下一步调用，这样就完成了电弧跟踪偏差的实时补偿。插补的实时性在时间上要求一次计算过程必须在一个插补采样周期内完成。为了测试时间上的实时性，设定插补速度为0.1m/s，用Visual C+语言编程实现算法实时性的测试。在CPU 为P4，主频为2.

17、0GHz ，内存为768MB ，以Windows XP 作操作系统的计算机上进行实时性测试，测得进行10000次插补用时16ms ，既每次插补用时1600ns 。因为目前饲服系统的采样周期一般为1ms 11以上，而“昆山一号”机器人要求采样周期为10ms ，所以本算法能够满足实时性运算时间上的要求。实时性和调速控制分析表明，本文研究的算法能满足机器人控制的实时性和可调速性要求，本研究成果已成功应用于“昆山一号”焊接机器人。2 算法应用与分析本文算法应用于“昆山一号”机器人的PTP 、多点关节空间、直线、圆弧、样条曲线、FlyBy 的在线实时插补算法中。以上类型在线实时插补算法的实现统一为三步：

18、求解总线长（PTP 对应关节角度变化量，直线对应两点之间线段长度，圆弧对应两点之间总的弧长，样条曲线对应两点之间的弦长，FlyBy 对应弗格森曲线9线长）；调用1.2描述的实时轨迹插补算法，对于不同的插补类型，其核心插补算法是相同的；对位置和姿态进行插补。以直线为例，说明实时轨迹插补算法的应用及算法如何实现电弧跟踪实时偏差补偿和调速控制。直线的在线实时插补算法首先在第一步求解A 和B 之间的线段长度S ；第二步调用1.2中提出的实时轨迹插补算法，得到Vnext 和Snext ；第三步对位置和姿态分别进行插补，求解得到nextAuxiT6，计算公式为：6.px startAuxiT -.px (

19、endAuxiT6*(Snext/S+6.px startAuxiT =.px nextAuxiT6 (15 因为直线的位置和姿态都呈线性变化关系，所以式(15中px 既可以为表示位置的x 、y 、z ，同时也可以为表示姿态的fi 、theta 、pusi 。要实现电弧跟踪偏差的实时补偿，只需根据偏差量对nextAuxiT6进行调整即可，而对速度调节的实现只需要对调速控制量Vrate 进行操作即可，具体实现方法见1.3描述。应用了该算法的直线在线实时插补的仿真效果图如图4所示。3 结论根据本文提出的工业机器人实时轨迹插补算法，只需要找到与轨迹类型相对应的线长S 的表示，即可通过本文提出的算法规

20、划出相应的插补轨迹，并满足实时性和可调速性的要求。本文给出的实时轨迹插补算法处理流程清晰，编码简单，易于实现，代码重复利用率高。该算法用于“昆山一号”焊接机器人轨迹规划中，满足了焊接工艺要求，验证了该算法的可行性。参考文献：1 付京逊. 机器人学M. 北京: 中国科学技术出版杜, 1989. 110-142.2 叶伯生. 机器人空间三点圆弧功能的实现J. 华中科技大学学报（自然科学版）, 2007, 35(8:5-8. 3 邓 勇. B样条曲线的实时插补J. 机械与电子, 2001, (4:23-24.4 吴光琳, 林建平, 李从心, 等. B样条曲线的快速实时插补算法J. 模具工业, 200

21、0, (10:14-16. 5 Vic B, Red E. Inaccuracy compensation and piecewise circular approximation of parameter curves J. Robotics, 1993, 11(5:413-425.6 肖海荣, 周应兵, 周凤余, 等. 基于插补算法的大型喷漆机器人轨迹规划A.中国控制与决策学术年会论文集C, 2001. 1099-1104.7 刘晓平, 田西勇, 庄 未. 基于非对称组合正弦函数的机器人规划方法J. 电子机械工程, 2008, 24(1:56-58.8 David Kincaid, War

22、d Cheney数值分析(英文版M北京：机械工业出版社, 2005. 308-377. 9 柳朝阳, 周晓平计算机图形学：图形的计算与显示原理M 西安：西安电子科技大学出版社, 2005. 158-203;223-227.10 唐创奇焊接机器人系统运动规划方法的研究与软件实现D. 南京: 东南大学, 2008. 7-27. 11 边玉超, 张莉彦, 戴莺莺, 等. CNC系统中NURBS 曲线实时插补算法研究J. 机械制造与自动化, 2003, (6:36-39.A Real-time Interpolation Algorithm for Trajectory Planning of Industrial RobotLI Tian-you，MENG Zheng-da，CHEN Qing-qi(School of Automation，SouthEast University，Nanj