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

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-)，男，河南民权人，硕士生，litia ny ouseu时偏差补偿和插补过程中进行调速的需要，所以我们提出了实时轨迹插补算1.2基本算法机器人控制系统计算点列并形成轨迹的过程叫插补，插补工作有三个要素：插 补的参数、插补的方程、插补形成的轨迹（点列）。只有插补的参数、插补的方程 都合乎操作者的需求时，产生的点列才是符合要求的。其中把插补的参数分成两部 分：第

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

7、该图例表示的插补类型为直线；第二部分参数有：始点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为当前插补点处于末 段变速状态时所需

8、的变速距离（Vcurrent按设定加速度变化到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。所以实时轨迹插补算法处理流程如下：stepl :初始化操作，进行速度、加速度、距离合法性检验。求解SA和SB ,求和得SAB ；比较SAB与S的大小，判别S的合法性；对插补

11、点末点进行处理；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，加速操作，算出 Vn

12、ext、Snext ；Ts accel Vcurre nt Vn ext *+= (22*5. 0*Ts accel Ts Vcurre nt Scurre nt Sn ext += (3 其它情况，加速过程提前到 达末点速度，则做匀速运动；Vend Vnext = (4 Ts Vcurrent Scurrent Snext *+= (5 step5 比较 Vcurrent 和Vend。若 Vcurrent 大于 Vend，减速操作，算出 Vnext、Snext ；Ts accel Vcurre nt Vn ext *-= (62*5. 0*Ts accel Ts Vcurre nt Scur

13、re nt Sn ext -+= (7其它情况，减速过程中提前达到末点速度，则做匀速运动，计算公式同(4(5。step6:处理非末段变速段的关键判断点二。比较velo和Vstart，若Vstart小于velo，则本段为加速段，进入step7；若Vstart不小于velo，则本段为减速段， 进入step8;2R2B图3实时轨迹插补算法的流程示意图图4直线在线实时插补的仿真效果示意图step7：比较Vcurrent和velo。若Vcurrent小于velo，加速操作，算出 Vnext、 Snext ,计算公式同(2(3。若Vcurrent达到或暂时超过velo，则需要将速度保持或调整为velo，计

14、算公式如下：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 Scurre nt Snext *+= (13由以上推理可知，要实现速度的平滑调节,以公式(3为例，计算公式如下：Vrate Ts a

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

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

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

19、.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.模具工业，2000, (10:14-16. 5 Vic B, Red E. In accuracy c

21、ompe nsati on and piecewise circularap proximation of parameter curves J. Robotics, 1993, 11(5:413-425.肖海荣,周应兵,周凤余,等.基于插补算法的大型喷漆机器人轨迹规划A. 中国控制与决策学术年会论文集C, 2001. 1099-1104.7 刘晓平,田西勇,庄未.基于非对称组合正弦函数的机器人规划方法J.电子 机械工程,2008, 24(1:56-58.8 David Kincaid, Ward Cheney .数值分析(英文版M.北京：机械工业出版 社，2005. 308-377. 9柳朝

22、阳，周晓平.计算机图形学：图形的计算与显示原理M.西安：西安电子科技大学出版社，2005. 158-203;223-227.10唐创奇.焊接机器人系统运动规划方法的研究与软件实现 D.南京:东南 大学，2008. 7-27. 11边玉超，张莉彦，戴莺莺，等.CNC系统中NURBS曲线实时插 补算法研究J.机械制造与自动化，2003, (6:36-39.A Real-time In terpolati on Algorithm for Trajectory Pla nning of In dustrial RobotLI Tian-you , MENG Zheng-da, CHEN Qing-qi(School of Automation, SouthEast University, Nanjing 210096, ChinaAbstract: An algor