一种工业机器人连续轨迹规划过渡算法

许 健， 梅江平， 段晓斌， 罗振军， 陈落根

(1.天津大学 机构理论与装备设计教育部重点实验室， 天津 300355；2.杭州娃哈哈集团有限公司 机电研究院， 浙江 杭州 310020)

一种工业机器人连续轨迹规划过渡算法

许 健1， 梅江平1， 段晓斌1， 罗振军2， 陈落根2

(1.天津大学 机构理论与装备设计教育部重点实验室， 天津 300355；2.杭州娃哈哈集团有限公司 机电研究院， 浙江 杭州 310020)

提出一种用于工业机器人操作空间连续轨迹规划的平滑曲线过渡算法，以提高工业机器人在连续轨迹过渡时的运动速度.该算法的主要目的是在保证连续轨迹过渡精度和工业机器人物理约束的条件下充分发挥工业机器人的能力，尽量提高过渡区域的速度，减少过渡所需时间，同时尽可能地使计算过程简便，以便用于在线规划，提高算法的通用性.该算法以给定速度为零的路径衔接点为基准，根据过渡半径参数得到2个速度不为零的过渡节点，在过渡节点之间采用一种有限项的正弦级数进行曲线拟合.而在非过渡区域只需采取经典的起止速度及加速度满足约束值的直线轨迹规划算法或圆弧轨迹规划算法.通过MATLAB仿真和一种两自由度高速并联机械手的实验验证了该算法的效果.该算法可以实现典型路径的在线轨迹规划，且轨迹通常不随速度参数的更改而变化，计算过程简便，故具有较大的应用价值和发展空间.

连续轨迹规划； 操作空间； 工业机器人； 路径过渡

工业机器人轨迹规划算法是保证工业机器人实现稳定运动的核心技术.对于弧焊等机器人来说，通常要求末端执行器尽量以给定速度沿着指定示教路径前进，并保证在相邻路径之间能够自动平滑过渡.为解决路径平滑过渡这一难题，最早采用的是基于样条曲线插补等方式的关节空间轨迹规划算法[1-3]，此类算法具有约束条件少和计算速度快等优势，但同时存在空间轨迹不直观和轨迹形状会随速度不同而改变等缺点，因此逐渐被机器人厂商弃用.近年来，国内外学者大多转向了操作空间轨迹规划算法的研究.常见的工作空间轨迹规划算法主要有3类：第1类是卷积类算法，该方法通常是给定混叠时间参数，对路径进行均匀离散化后采用卷积计算来实现加减速规划，但此类算法存在着很多与关节空间轨迹规划算法类似的缺点[4].第2类则是混叠类算法，包括速度混叠算法[5]和位置混叠算法[6-7]，此类算法一般需要给定加减速时间，然后通过线性、多项式或者摆线等函数形式的同伦变形获得速度曲线或者位置曲线，缺点是加速度约束涉及时间变量，需要复杂的处理流程，其中文献[7]是对ABB公司TrueMove技术的一次不完全尝试.最后一类算法是规划类算法，包括速度规划算法[8]和位置规划算法[9-10]，这一类算法通过设计过渡曲线的数学表达式来实现.其中速度规划算法计算流程复杂，且难以保证轨迹不随速度参数的更改而变化.而位置规划算法的研究中， Siciliano等将关节空间的位置规划算法扩展到笛卡尔空间，获得了抛物线形状的过渡轨迹，使得加速度约束不再取决于时间常数，但是仍存在加速度矢量方向突变的问题[9].林仕高等提出五次多项式位置规划过渡算法，但是因加速度约束是涉及时间变量的非线性函数而未能找到加速度约束处理方案[10].此外，还有一些研究通过先选取过渡路径，然后将其细分为微小路径段，最后采用前瞻算法实现速度平滑和轨迹修形，然而此类算法计算复杂，难以保证在高速运动条件下的实时性[4，11].

为此，提出一种计算效率高的基于有限项正弦级数的新型位置规划算法来解决上述问题.

1 节点配置

操作空间中典型的连续轨迹包括直线段与直线段、直线段与圆弧段以及圆弧段与圆弧段间的衔接.如图1所示，为简化论述，取平面直线段Path1与直线段Path2间的衔接为代表进行本文算法的论述，而空间连续轨迹可以很轻松地从平面算法中导出.

图1 带过渡的典型轨迹Fig.1 Typical trajectory with transition

其中：S0为Path1的起始点，S1为Path1和Path2的衔接点，S2为Path2的结束点；r1为S1处的过渡半径；P1和P2分别为以S1为圆心、r1为半径的圆与Path1和Path2相交而得到的过渡节点；v1和v2则分别为沿Path1和Path2的巡行速度；k1为过渡节点处待优化的速度比例系数，且一般要求0

首先设t为完成过渡所需要的时间，点P1和P2的位置矢量分别用p0和pT表示.

采用各轴分别进行平滑过渡的算法，故将相对位置、速度和加速度矢量分别向坐标轴投影以获得分量.该算法假设所有过渡节点的切向加速度均为0，从而使后续推导大为简化.过渡节点P1处的相对位置、速度和加速度的坐标轴分量配置如下：

同理，可得过渡节点P2处的相对位置、速度和加速度的坐标轴分量配置为

由上述可知，在确定k1之后，过渡节点的实际配置将随之确定.对于非过渡区域的轨迹只需采用经典的起止点的速度和加速度为给定值的直线段或圆弧段的轨迹规划算法[9-10，12-17]即可.由此可见本文过渡算法的关键在于确定k1.以下将具体阐述算法的原理和计算步骤.

2 算法原理

对于每一个坐标轴方向，引入基于有限项正弦级数的位置规划函数：

(1)

则过渡节点的运动学约束方程组如下：

(2)

为保证上述方程组解的确定性，方程数量与待定系数数量需相同，选取ns=3，ne=5.

考察待优化的速度比例系数k1，易知其与过渡总时间T成反相关，定义为

(3)

式中s=4r1(v1+v2)-1.将式(3)代入式(2)的约束方程组，借助正弦函数的特点可消去k1，得到

Z=CV，

(4)

式中：

oi，n=2-n(i-1)un，u=πs-1，

pi，n=oi，nsin 2-(i-1)π，qi，n=oi，ncos 2-(i-1)π，

vij=Aijsin φij，wij=Aijcos φij.

故由V=C-1Z可解得向量V.而由向量V可得

(5)

下面进一步考虑加速度性能约束.

将式(1)进行二次求导可得加速度表达式为

取φ=k1ut/16，有φ∈[0，π/16]，可得

(6)

其中：

可以看出，在给定|aj(φ)|max的允许值ajmax后，对于每一个坐标轴j，分别存在一个k1满足式(6)，将其记为k1j，则对应有

(7)

为获得k1j，并避免繁杂的数值判断，可采用近似计算方法计算|ρj(φ)|max.将ρj(φ)表达式中的正弦和余弦函数用下面的幂级数前3项近似：

可得近似表达式为

(8)

上式为五次多项式，其系数bjl为常量，求导可得四次多项式.可以通过解析法得到式(8)的极值，考虑边界情况，进而可获得ρj(φ)的近似最大值和近似最小值，分别记为ρjmax和ρjmin.为确认上述近似方法的有效性，需评估幂级数截断所产生的近似误差，定义为

(9)

可得近似误差如图2所示.

图2 幂级数近似公式的误差Fig.2 Errors of approximate power series

(10)

由此解得

(11)

则k1值由下式取得：

(12)

3 算法步骤

基于上述理论可以建立过渡区域的过渡算法，步骤如下：

1)根据式(4)和式(5)求解φij和Aij.

2)根据式(8)求解ρjmax和ρjmin.

3)根据式(11)和式(12)分别求解k1j和k1.

4)计算基本过渡曲线为

(13)

由式(11)可知，若对v1和v2进行同比变化，则k1与速度成反比，使得过渡节点配置不变，进而可知幅值系数与相位系数均不变，因此过渡轨迹的路径和速度剖面形状等均不改变.

5)如果(v1+v2)过小或者r1过大，可能造成k1>1.若k1>1，则可对式(13)进行时间缩放处理：

(14)

即：将过渡时间扩大k1倍，使速度缩小k1倍,而过渡轨迹的路径形状不发生改变.

6)将所得相对位置矢量变换成绝对位置矢量即可得到最终过渡曲线.

但需注意，如果k1≫1，则上述方法会使得过渡速度最小值过小，不利于保持运动过程中速度的相对稳定.因此在实际机器人控制器中仍需提供类似于文献[4，8，11]的分段细化和速度前瞻算法以处理(v1+v2)过小的情况.而对于r1过大的情况，通常意味着示教程序质量不高，需要进行一些局部调整，例如插入圆弧或增加示教点等.

4 仿真与实验

采用辰星(天津)自动化设备有限公司的D2-350两自由度高速并联机械手[18-19]为工业机器人模型，并以门式轨迹(bw=305 mm，hw=25 mm)[19]为典型轨迹在MATLAB软件上进行算法的仿真测试.

D2-350两自由度高速并联机械手如图3所示[19]，其操作空间为水平和铅锤方向所形成的平面，具体参数见表1[19].

图3 D2-350机械手Fig.3 The D2-350 manipulator

长度/mm高度/mm最大速度/(mm/s)最大加速度/(mm/s2)3501502004000

给定门式轨迹如图4所示，其基本参数见表2，参考D2-350机械手设定过渡参数,见表3.

图4 给定门式轨迹Fig.4 The given gantry trajectory

b/mmbw/mmh/mmhw/mm35030515025

表3 门式轨迹过渡参数

由于该算法主要用于过渡节点的选取以及过渡曲线的生成，为更直观地分析该算法的特性，以下主要对P1点处过渡区域进行详细分析.

经过计算可得k1=0.36，则P1点处过渡区域的相对位置、速度、加速度和过渡轨迹的曲线分别如图5、图6、图7和图8所示.

图5 P1点处过渡区域相对位置-时间曲线Fig.5 Relative position-time curve in P1 transition section

图6 P1点处过渡区域速度-时间曲线Fig.6 Velocity-time curve in P1 transition section

图7 P1点处过渡区域加速度-时间曲线Fig.7 Acceleration-time curve in P1 transition section

从P1过渡区域加速度曲线可以看出，用该算法求取的过渡曲线的加速度完全满足所施加的物理约束，说明该算法可以达到预期效果.

图8 P1点处过渡轨迹Fig.8 The P1 transition trajectory

为验证仿真结果，进行实验测试.本测试采用摆线加减速方式实现非过渡区域轨迹规划，同时，由于高速运动需要很高的采样频率，故采用编码器反馈的关节转角进行运动学正解计算得到实际过渡轨迹.实际过渡轨迹、速度和加速度曲线分别如图9、图10和图11所示，采样时间为1 ms.

图9 实际过渡轨迹与计算过渡轨迹对比Fig.9 The contrast of actual transition trajectory and computed transition trajectory

图10 过渡区域实际速度-时间曲线Fig.10 The actual velocity-time curve in transition section

图11 过渡区域实际加速度-时间曲线Fig.11 The actual acceleration-time curve in transition section

由图9可知实际过渡轨迹与计算过渡轨迹基本吻合，而由图11可知其加速度满足机械手物理约束条件，故该算法得到验证.

同时，用户可以通过调整过渡半径r1，得到不同的过渡轨迹，图12所示过渡轨迹簇中的过渡轨迹分别为r1=2，3，4，5 mm时所生成的过渡轨迹.

图12 过渡轨迹簇Fig.12 The transition trajectory cluster

如图13所示：过渡轨迹1为v1=v2=200 mm/s时的过渡轨迹，而过渡轨迹2为v1=v2=100 mm/s时的过渡轨迹，两者完全重合，证明在速度等比例变化时，所得过渡轨迹不变.

图13 速度等比例变化过渡轨迹对比Fig.13 Comparison of transition trajectories with proportion change of velocities

5 结 论

本文给出了一种工业机器人操作空间连续轨迹规划的过渡算法，该算法的主要目的是在保证连续轨迹过渡精度和工业机器人物理约束的条件下充分发挥工业机器人的能力，尽量提高过渡区域的速度，减少过渡所需时间，同时尽可能地使计算过程简便，以便用于在线规划，提高算法的通用性.通过MATLAB仿真和两自由度高速并联机械手的实验验证，证实了该算法能够在保证连续轨迹平滑过渡的同时，保证其物理约束条件.此外，该算法具备在速度等比例变化的条件下过渡轨迹不变的特性，这种特性使其能够更广泛地应用于机器人应用领域.此外，该算法是位置规划方法，容易计入位置约束条件，因此其可扩展性良好.后续研究将对姿态配置进行类似处理，进而实现包含空间姿态的轨迹过渡，推动位置规划类连续轨迹规划方法的创新和发展.

[1] FU K S, GONZALEZ R C, LEE C S G. Robotics: control, sensing, vision, and intelligence[M]. Blacklick, Ohio: McGraw-Hill Book Company, 1987: 149-200.

[2] 刘承立，栾楠. 关节空间样条的连续点位运动规划算法[J]. 机械与电子，2014(8)：74-78． LIU Cheng-li， LUAN Nan. Continuous point to point motion planning algorithm based on splines in joint space[J]. Machinery & Electronics, 2014(8): 74-78．

[3] 张斌. 基于多约束的机器人关节空间轨迹规划[J]. 机械工程学报，2011，47(21): 1-6． ZHANG Bin. Joint-space trajectory planning for robots under multiple constraint[J]. Journal of Mechanical Engineering, 2011， 47(21): 1-6．

[4] SUH S-H， KANG S-K. Theory and design of CNC systems[M]. London: Springer-Verlag, 2008: 107-156.

[5] VOLPE R. Task space angular velocity blending for real-time trajectory generation: US 5602968[P]. 1997-02-11．

[6] LLOYD J， HAYWARD V. Real-time trajectory generation using blend functions [C]//Proceedings of IEEE International Conference on Robotics and Automation. Sacramento， CA, Apr.9-11， 1991: 784-789．

[7] USTYAN T， JÖNSSON V. Implementation of a generic virtual robot controller [D]. Sweden: Chalmers University of Technology, Department of Signals and Systems, Division of Automatic Control, Automation and Mechatronics, 2011: 8-31.

[8] KOICHI Funaya. Method of and apparatus for robot tip trajectory control: US 5740327A[P]. 1998-04-14．

[9] SICILIANO B， SCIAVICCO L， VILLANI L. Robotics: modelling， planning and control [M]. 3th ed. London: Springer-Verlag, 2009: 161-190.

[10] 林仕高，刘晓麟，欧元贤. 机械手笛卡尔空间轨迹规划研究[J]. 机械设计与制造，2013(3)：49-52． LIN Shi-gao， LIU Xiao-lin， OU Yuan-xian. The study of trajectory planning of manipulators in Cartesian space[J]. Machinery Design & Manufacture， 2013(3):49-52．

[11] 王允航. 连续轨迹的 S 型速度规划的研究[D].深圳：哈尔滨工业大学深圳研究生院，2010：5-46． WANG Yun-hang. Research on S-shaped continuous trajectory planning [D]. Shenzhen: Harbin Institute of Technology, Graduate School at Shenzhen, 2010: 5-46.

[12] SONJA Macfarlane， ELIZABETH A Croft. Jerk-bounded manipulator trajectory planning: design for real-time applications[J]. IEEE Transactions on Robotics and Automation, 2003, 19(1):42-52．

[13] LUIGI Biagiotti， CLAUDIO Melchiorri. Trajectory planning for automatic machines and robots[M]. Berlin: Springer-Verlag, 2008: 341-414.

[14] JEON J W， HA Y Y. A generalized approach for the acceleration and deceleration of industrial robotics and CNC machine tools[J]. IEEE Transaction on Industrial Electronics， 2000， 47(1): 133-139．

[15] CUI J， CHU Z Y. An improved approach for the acceleration and deceleration of industrial robots and CNC machine tools[C]//Proceedings of the 2005 IEEE International Conference on Industrial Technology. Hong Kong, Dec. 14-17, 2005: 1269-1273.

[16] JEON J W. Efficient acceleration and deceleration technique for short distance movement in industrial robots and CNC machine tools[J]. Electronics Letters, 2000,36(8): 766-768．

[17] NG Y K， XIE Y， WANG C， et al. Development of a new velocity profile generation for improvement of CNC Machining Efficiency[C]//Proceedings of the 2008 IEEE International Conference on Mechatronics and Automation. Takamatsu, Aug. 5-8, 2008: 313-318.

[18] HUANG Tian， LI Zhan-xian， LI Meng， et al. Conceptual design and dimensional synthesis of a novel 2-DOF translational parallel robot for pick-and-place operations[J]. Journal of Mechanical Design, 2004， 126(3): 449-455．

[19] 辰星(天津)自动化设备有限公司. D2-350技术参数[EB/OL]. (2015-09-03)[2016-03-27]. http://www.cxrobot.com/gsby/1203_30.html. Chenxing (Tianjin) Automation Equipment Co., Ltd.. The technical parameters of D2-350[EB/OL]. (2015-09-03)[2016-03-27]. http://www.cxrobot.com/gsby/1203_30.html.

An algorithm for segment transition in continuous trajectory planning of industrial robot

XU Jian1， MEI Jiang-ping1， DUAN Xiao-bin1， LUO Zhen-jun2， CHEN Luo-gen2

(1. Key Laboratory of Mechanism Theory and Equipment Design of Ministry of Education, Tianjin University，Tianjin 300355， China; 2. Mechanical and Electrical Institute, Hangzhou Wahaha Group Co., Ltd.， Hangzhou 310020， China)

A new algorithm for obtaining concatenation curves between segments in operational space related to continuous trajectory planning of industrial robots is introduced, which is to improve motion speeds of industrial robots when they transit between segments. The main aim of the algorithm was to make full use of industrial robot, try to improve their speed and reduce the time needed in transition section with ensuring the precision in transition section and the physical constraints of industrial robot. And the computation procedure of the algorithm should be as simple as possible, so as to be easy for the online trajectory planning and improve the generality of the algorithm. The algorithm used the via-point as a reference to obtain two transition points according to the specified transition radius, and each transition point had a non-zero velocity. A novel finite sine series function was used to interpolate the trajectory between the transition points, while conventional linear or circular trajectory planning algorithms with boundary points satisfying velocity and acceleration constraints could be employed to interpolate the trajectory between transition points and other points. The effect of the algorithm was verified by simulation in MATLAB and experiment in a 2-DOFs high speed parallel manipulator. The algorithm can realize the online trajectory planning of typical robot paths. Its trajectory is fixed even when path velocity is modified, and its computation procedure is relatively simple, and hence has much potential in many applications.

continuous trajectory planning; operational space; industrial robot; path transition

2016-03-03.

本刊网址·在线期刊：http://www.zjujournals.com/gcsjxb.

国家自然科学基金资助项目(51135008).

许健(1991—)，男，天津人，硕士生，从事机器人控制系统研究，E-mail：kk53623@sina.com. http://orcid.org//0000-0003-2141-6104 通信联系人：罗振军(1976—)，男，湖南桂阳人，博士，总工程师，原天津大学副教授，硕士生导师，从事机器人机构理论、机器人控制系统以及机电产品数字化设计等研究，E-mail：zhenjunluo@hotmail.com. http://orcid.org//0000-0003-0648-4256

10.3785/j.issn. 1006-754X.2016.06.003

TP 242.2

A

1006-754X(2016)06-0537-07