2019, Vol. 34
2. 杭州新松机器人自动化有限公司，杭州 311200
2. Hangzhou SIASUN Robot & Automation Co., Ltd, Hangzhou 311200, China
轨迹规划是工业机器人运动控制的核心技术.搬运、喷绘等机器人的末端执行器应以给定速度沿着指定示教路径前进, 并保证在相邻路径段间能够自动平滑过渡.为解决路径平滑过渡难题, 最早采用的是基于样条曲线插补等方式的关节空间轨迹规划算法[1-3].此类算法具有约束条件少和计算速度快等优势, 但存在空间轨迹不直观和轨迹形状会随速度不同而改变等缺点.近年来, 任务空间轨迹规划算法是研究方向, 主要有卷积类算法和规划类算法等.卷积类算法通过给定混叠时间参数, 对路径进行均匀离散化后采用卷积计算来实现加减速控制, 但仍存在很多与关节空间轨迹规划算法类似的缺点[4].规划类算法包括位置规划算法和速度规划算法, 通过设计过渡曲线的数学表达式, 并结合加减速控制模型加以实现[5]. Liu等[6]将关节空间的位置规划算法扩展到笛卡尔空间, 使用三次样条使得加速度约束不再取决于时间常数, 但仍存在加速度矢量方向突变的问题.许健等[7]提出一种基于有限项正弦级数的新型位置规划算法来提高路径过渡速度, 但速度比例参数过大使得过渡速度过小, 不利于保持速度相对稳定.采用前瞻轨迹规划算法实现速度平滑过渡是目前研究热点.史中权等[8]设计了一种加速度自适应调整的前瞻处理算法, 但在拐角处未考虑增加光顺曲线, 速度方向会发生突变, 效率提升有限. Hu等[9]设计了基于梯型加减速控制的前瞻轨迹规划算法, 但梯形速度规划加速度不连续, 会引起机械冲击.罗钧等[10]设计了基于S型加减速自适应前瞻NURBS曲线轨迹规划算法, 但此算法未考虑路径长度约束, 速度可能超出允许范围.
已有前瞻算法多应用于数控加工领域, 且多数未能将路径光顺、前瞻速度规划、运动约束匹配等技术有效结合.本文通过建立圆弧转接模型进行路径拐角过渡, 结合非对称S加减速模型进行跨段前瞻速度规划, 通过增加路径段长度约束和始末速度约束条件, 实现路径衔接点速度和加速度的自适应调整.最后将算法集成应用于机器人实时控制系统中, 实现机器人任务空间多路径平滑过渡, 从而提高作业效率.
1 路径衔接模型及轨迹规化工业机器人任务空间的运动轨迹一般由圆弧段和直线段组成.本算法要实现的功能为:通过手动输入空间目标点、进给速度和半径调节系数, 算法可自动完成在相邻直线段间的圆弧过渡, 以及过渡衔接点的速度取优, 圆弧的半径大小可通过参数调节.
1.1 衔接模型如图 1所示, Pi、vi、ki分别为机器人任务空间的目标点、进给速度和半径调节系数, PTi、vTi、Ci分别为过渡衔接点、过渡衔接点的速度和圆心, Mi为两过渡衔接点的中点, li、θi分别为相邻路径段的角平分线和夹角, di为被分割后的小路径段长度.
|
图 1 通过圆弧过渡的路径段间衔接模型 |
以由P0、P1、P2三个已知目标点构成的路径段衔接模型为例, 求解步骤如下.
Step 1:求解拐角θ1及过渡衔接点坐标PT1、PT2.由已知条件可得
|
(1) |
设定半径
|
(2) |
由式(1)、(2)和几何关系可得
|
(3) |
Step 2:求解圆心坐标C1及分割的路径段长度di.由M1为PT1、PT2的中点且在角平分线l1上, 可得
|
(4) |
在三角形Δ P1PT1C1中, 由三角函数关系可得
|
(5) |
结合式(4)和(5)可求得C1和M1的坐标.
由Step 1和Step 2所求得的关键路径点及相关参数, 可得
|
(6) |
其余可以依此类推.
1.2 基于非对称S曲线加减速控制的轨迹规划梯形速度函数存在加速度不连续, 会产生冲击、降低工业机器人精度和使用寿命等缺陷[11].传统的7段S型加减速曲线首末速度为零, 影响工业机器人作业执行效率[12].本文采用基于非对称S型加减速控制方法, 并将S型加减速曲线函数按照拆分的路径段进行优化.理想状态下, 将目标路径轨迹对应的速度曲线拆分为9个阶段, 如图 2所示.
|
图 2 非对称S曲线加减速区段分析 |
图 2中: T1~T4对应路径段P0PT1, 分为加加速段、匀加速段、减加速段和匀速段; T5对应过渡圆弧PT1PT2, 为匀速运动; T6~T9对应路径段PT2PT3, 分为减加速段、匀减速段、减减速段和匀速段.其中T2、T4、T6的值可以取0, T5可为匀速运动(vT1=vT2)或S型减速运动(vT1>vT2), T6~T9可为S型加速运动(vT3>vT2).
由图 2速度规划曲线可知, 优化后的路径轨迹包括S型加速运动、S型减速运动和匀速运动.为提高算法的通用性, 将S曲线加速和减速速度函数进行优化统一, 令T1=T3, T6=T8.
P0PT1直线段S型加速速度时间函数为
|
(7) |
PT2PT3直线段S型减速速度时间函数为
|
(8) |
由图 2和式(7)、(8)的速度函数可知, S加速和减速时间函数只需将对应的加速度和加加速度取反, 并将对应的首末速度和加速度大小进行替换.故可将S型加减速规划统一为四段速度规划.引入方向判别
|
(9) |
其中: 
设该段路径运行总时间为Ttotal.若已知位移di, 首末速度vTi、vTi+1、加速度amax和J, 可得时间分段为
|
(10) |
|
(11) |
|
(12) |
|
(13) |
对速度函数进行分段积分, 可得4段位移时间函数
|
(14) |
其中
|
PT1PT2圆弧段分为匀速(vT1=vT2)和S型减速运动(vT1>vT2).设采样时刻tk, 若该段为匀速运动, 则有
|
(15) |
|
(16) |
|
(17) |
当ki和θi较小时, 圆弧长度较小, 考虑整段轨迹的运行效率, 采用降低过渡圆弧末端点速度vT2的方案, 并采用四段速度规划.
采用弧长增量法的插补技术, 在每个插补周期内, 按照进给速度用一个微小线段去逼近目标轮廓线.设插补周期为T0, 由式(10) ~(17)可得
|
(18) |
其中: N为该段轨迹的总插补点数, λk为归一化因子, λk∈[0, 1].
插补运动分为位置插补和姿态插补.
1) 位置插补运动.
以P0PT1直线段为例, P0、PT1、插补点坐标分别为(x0, y0, z0)、(xT1, yT1, zT1)、(xk, yk, zk), 各插补点坐标可表示为
|
(19) |
将式(18)求得的直线段λk函数值代入式(19), 可得直线段位置表达式.
以PT1PT2圆弧段为例, 采用基于局部坐标系的空间圆弧插补方法和齐次坐标变换原理, 可得
|
(20) |
将式(18)求得的圆弧段λk函数值代入式(20), 并经齐次坐标变换可求得圆弧段的位置表达式.
2) 姿态插补运动.
四元数具有无数据冗余、不存在万向节锁死等优点[13].设q=[s, (a, b, c)], 由机器人正运动学齐次矩阵可求得对应的两个互补的单位四元数
|
(21) |
设工业机器人的初始姿态和目标姿态分别为R1和R2, 根据式(21)可求出其对应的四元素q1和q2.由于每个姿态矩阵对应两个互补的四元数, 考虑到时间最优, 选择与前一姿态夹角较小的一组四元数.定义两个四元数夹角为Ω, 则有
|
(22) |
对于两点间的姿态可由Slerp公式[13]求出, 即
|
(23) |
前瞻控制技术最重要的两个部分是连续轨迹段间衔接速度处理和加减速控制[14].在前瞻控制算法中, 多路径段间过渡衔接点的速度处理是加减速控制的前提条件.
2.1 算法设计本文设计的前瞻算法如图 3所示.
|
图 3 前瞻算法流程 |
以计算路径段被分割后相邻的第i段和第i+1段路径轨迹间衔接速度为例(参考图 1衔接模型), 过渡衔接点速度求解过程如下:第i段路径的始末过渡衔接点速度为vTi-1, vTi; 第i+1段路径的始末过渡衔接点速度为vTi, vTi+1; vi为两段路径段间给定目标点的速度, 理想状态下vTi=vi=vTi+1.因为过渡点PTi处的速度需要保证路径段作业过程中有足够的距离达到点PTi, 且通过PTi点后有足够的距离到达点PTi+1, 需增加路径长度约束条件, 实现过渡衔接点速度的自适应调整.同理, 在始末速度约束条件下, 实现加速度的自适应调整.
2.2 多路径段长度约束下的衔接速度规划由第2.1节获得路径段始末速度vTi-1和vTi及位移di, 本节讨论在确定的位移di范围内能达到的最大速度.
1) 二段加速.如果di < 2vTi-1amax/J+amax3/J2, 则在位移si范围内不能达到最大加速度amax, 加速过程分为, 加加速阶段和减加速阶段两段, 时间为T1, 则有
|
(24) |
式(24)方程判别式为
|
(25) |
初始速度vTi-1、加加速度J均为非负值, 故满足Δ>0, 有
|
(26) |
进而求得
|
(27) |
2) 三段加速.不满足二段加速条件则在位移di范围内能达到最大加速度amax, 加速过程分为3段, 位移方程为
|
(28) |
解式(32)的二次方程得到
|
(29) |
根据本节推导的结论, 分别向前和向后判断位移条件下能达到的理论最大速度.如果该理论最大速度小于用户设置的速度, 则需将过渡衔接点速度调整为理论最大速度, 从而实现过渡衔接点速度的自适应调整.
2.3 多路径段速度约束下的速度规划将第2.2节自适应调整的速度进行始末速度约束下的三段速度规划和四段速度规划.以三段速度规划(包含无匀速运动情况)为例, 如果|vTi-vTi-1| ≤amax2/J, 则表明不需要达到最大加速度amax即可加速到最大速度vTi.最大加速度调整为
|
(30) |
分段用时分别为
|
(31) |
|
(32) |
若di-2vTi-1T1-(vTi-vTi-1)amax/J>0, 则存在匀速段
|
(33) |
否则不存在匀速段, 即T4 =0.三段位移函数为
|
其中
|
如果|vTi-vTi-1|>amax2/J, 则为四段速度规划, 四段位移函数与第2.2节同.
3 实验测试与结果分析自主研发的六自由度工业机器人控制系统采用基于IPC+实时扩展模块的开放式架构[15], 由示教器、嵌入式PC(x86架构)、LinuxCNC实时操作系统、机器人语言解析、运动控制和伺服驱动等模块组成.在嵌入式PC上利用C++编程实现控制算法, 控制器与伺服驱动器之间采用EtherCAT工业实时以太网进行通信.
实验1 典型路径过渡实验.
实验选取典型的路径由目标点P0~P4组成.设定系统参数amax=1 200 mm/s2, J=7 500 mm/s3, 插补周期2 ms.给定路径目标点和相关目标参数如表 1所示.
|
表 1 给定实验参数 |
路径过渡实验得到机器人末端插补轨迹、末端速度和加速度曲线分别如图 4和图 5所示. 图 4中:实线为前瞻插补轨迹, 虚线为给定目标点路径, 星号为过渡衔接点.由图 5可见, 本文前瞻算法根据给定参数值将目标路经分割为8段, n=i表示机器人在i段路径运行区间.
|
图 4 典型路径过渡机器人末端插补轨迹 |
|
图 5 典型路径过渡机器人末端速度及加速度 |
图 5中:当n=1时, 机器人末端最大速度只有235 mm/s, 未达到给定速度360 mm/s, 表明本文前瞻算法在路径长度约束下, 对速度进行自适应调整, 并符合四段速度规划; 当n=2时, 由于半径调节参数较小(k=0.01), 过渡圆弧较小, 保持235 mm/s匀速运动; 当n=3时, 机器人末端达到给定速度144 mm/s, 但未达到系统加速度值1 200 mm/s2, 表明本文前瞻算法在始末速度下, 对加速度进行了自适应调整, 并符合三段速度规划中的二段速度规划; 当n=4时, 为过渡圆弧段, 保持144 mm/s的匀速运动; 当n=5时, 未达到系统加速度值1 200 mm/s2, 表明本文前瞻算法在始末速度下, 对加速度进行了自适应调整, 并符合三段速度规划; 其余可以依此类推.
由典型路径过渡实验可知, 本文前瞻控制算法可实现速度的平滑过渡, 并可实现在路径约束下速度和加速的自适应调整, 验证了前瞻算法的有效性.
实验2 喷绘实验.
为进一步体现算法的优越性, 通过喷绘实验验证本文多路径段平滑过渡前瞻插补算法, 为保证路径轨迹喷绘均匀, 要求机器人在喷绘过程中尽量保持匀速运动.由于喷绘轨迹路径段相对较短, 考虑到加速度过大会对机器人造成冲击, 根据现场喷绘效果选取加速度和速度分别为600 mm/s2和54 mm/s, J=7 500 mm/s3, 插补周期为2 ms.喷绘目标路径为字母“SIASUN”.由于目标路径点较多, 本文仅选取“S”字母的路径段进行分析, 喷绘实验效果如图 6所示.
|
图 6 喷绘实验喷绘效果 |
为便于区分, 将实验得到的“S”字母插补轨迹依次沿Y轴正方向平移60 mm, 如图 7所示.截取的“S”路径段速度及加速度如图 8所示.
|
图 7 喷绘实验截取路径轨迹 |
|
图 8 截取“S”路径速度和加速度 |
当半径调节系数k=0时, 表示采用首末速度为0的传统加减速控制方法, 该路径段加速度有24次正负值变化, 耗时8.58 s.选取k值分别为0.01、0.15和0.23的前瞻算法时, 加速度仅有2次正负值变化, 中间过渡段轨迹均保持匀速运动, 速度为54 mm/s. 3种不同k值情况下耗时分别为6.15 s、6.48 s和6.69 s, 效率提高22.03 % ~ 28.32 %.可见该前瞻算法不仅能提高作业的执行效率, 还能减小频繁启停对机器人造成的冲击.
此外, 该前瞻算法还可以实现轨迹修形, 由图 5可得到如图 9所示的效果图. 图 9(a) ~ 图 9(d)分别对应k值为0、0.01、0.15和0.35时的喷绘效果图.对比可知, 图 9(a)在路径段衔接处喷墨量比图 9(b) ~ 图 9(d)三幅图多, 因为传统加减速控制算法首末速度为零, 速度不连续, 而前瞻算法可保持匀速运动.随着半径调节参数的增大, 过渡轨迹越来越圆滑, 用户可根据需求选择最佳的修形轨迹.
|
图 9 截取“S”路径喷绘轨迹 |
在传统约束条件基础上, 增加基于非对称S曲线加减速控制的路径长度约束条件, 使得在处理多路径段间衔接速度时, 可实现过渡速度的自适应调整, 提高路径过渡时期的速度.通过实验验证所提出的前瞻插补算法可实现多路径段间速度平滑过渡和轨迹的修形, 减少机器的频繁启停, 从而提高机器人的作业执行效率和使用寿命.本文设计的多路径段平滑过渡的自适应前瞻插补算法综合路径光顺技术、前瞻与非对称S型速度规划技术、位移约束下的速度自适应规划技术和实时插补技术, 可实现多路径段间速度平滑过渡和轨迹修形.
在下一步工作中, 将增加动力学、拐角误差等约束条件, 提高控制精度; 进一步优化控制系统的软硬件平台, 提高系统实时性, 从而提高前瞻路径段插补数目.
| [1] |
Giberti H, Sbaglia L, Urgo M. A path planning algorithm for industrial processes under velocity constraints with an application to additive manufacturing[J]. J of Manufacturing Systems, 2017, 43(1): 160-167. |
| [2] |
Fan W, Lee C H, Chen J H. A real-time curvature-smooth interpolation scheme and motion planning for CNC machining of short line segments[J]. Int J of Machine Tools and Manufacture, 2015, 96(1): 27-46. |
| [3] |
Zhao H, Zhu L M, Ding H. A real-time look-ahead interpolation methodology with curvature-continuous B-spline transition scheme for CNC machining of short line segments[J]. Int J of Machine Tools and Manufacture, 2013, 65(1): 88-98. |
| [4] |
Gasparetto A, Zanotto V. Optimal trajectory planning for industrial robots[J]. Advances in Engineering Software, 2010, 41(4): 548-556. DOI:10.1016/j.advengsoft.2009.11.001 |
| [5] |
Olabi A, Béarée R, Gibaru O, et al. Feedrate planning for machining with industrial six-axis robots[J]. Control Engineering Practice, 2010, 18(5): 471-482. DOI:10.1016/j.conengprac.2010.01.004 |
| [6] |
Liu H, Lai X, Wu W. Time-optimal and jerk-continuous trajectory planning for robot manipulators with kinematic constraints[J]. Robotics and Computer- Integrated Manufacturing, 2013, 29(2): 309-317. DOI:10.1016/j.rcim.2012.08.002 |
| [7] |
许健, 梅江平, 段晓斌, 等. 一种工业机器人连续轨迹规划过渡算法[J]. 工程设计学报, 2016, 23(6): 537-543. (Xu J, Mei J P, Duan X B, et al. An algorithm for segments transition in continuous trajectory planning of industrial robot[J]. Chinese J of Engineering Design, 2016, 23(6): 537-543.) |
| [8] |
史中权, 叶文华. 多轴联动条件下插补速度实时可调的前瞻控制算法[J]. 航空学报, 2014, 35(2): 582-592. (Shi Z Q, Ye W H. A look-ahead algorithm with adjustable real-time federate based on multi-axis synchronous interpolation[J]. Acta Aeronautica et Astronautica Sinica, 2014, 35(2): 582-592.) |
| [9] |
Hu J, Xiao L, Wang Y, et al. An optimal feedrate model and solution algorithm for a high-speed machine of small line blocks with look-ahead[J]. Int J of Advanced Manufacturing Technology, 2006, 28(9): 930-935. |
| [10] |
罗钧, 汪俊, 刘学明, 等. 基于S型加减速的自适应前瞻NURBS曲线插补算法[J]. 计算机集成制造系统, 2013, 19(1): 55-60. (Luo J, Wang J, Liu X M, et al. Adaptive NURBS interpolation algorithm with look-ahead funcyion based on S-shape acceleration/deceleration[J]. Computer Integrated Manufacturing Systems, 2013, 19(1): 55-60.) |
| [11] |
朱世强, 刘松国, 王宣银, 等. 机械手时间最优脉动连续轨迹规划算法[J]. 机械工程学报, 2010, 46(3): 47-52. (Zhu S Q, Liu S G, Wang X Y, et al. Time-optimal and jerk-continuous trajectory planning algorithm for manipulators[J]. J of Mechanical Engineering, 2010, 46(3): 47-52.) |
| [12] |
吴明月, 刘延杰, 蔡鹤皋. 基于最优S型曲线轨迹的硅片传输机器人末端振动抑制方法[J]. 机器人, 2014, 36(4): 446-454. (Wu M Y, Liu Y J, Cai H G. Method of vibration suppression of the end-effector of wafer handling robot based on optimal S-curve profile[J]. Robot, 2014, 36(4): 446-454.) |
| [13] |
李宏胜, 汪允鹤, 黄家才, 等. 工业机器人倍四元数轨迹规划算法的研究[J]. 中国机械工程, 2016, 27(20): 2711-2716. (Li H S, Wang Y H, Huang J C, et al. Research on trajectory planning of industrial robots with double quaternion[J]. China Mechanical Engineering, 2016, 27(20): 2711-2716. DOI:10.3969/j.issn.1004-132X.2016.20.003) |
| [14] |
潘海鸿, 杨增启, 陈琳, 等. 一种优化轨迹段间衔接速度的自适应前瞻控制[J]. 机械工程学报, 2015, 51(5): 151-159. (Pan H H, Yang Z Q, Chen L, et al. An adaptive look-ahead control algorithm using optimized transition velocity between two trajectory segments[J]. J of Mechanical Engineering, 2015, 51(5): 151-159.) |
| [15] |
Wings E, Müller M, Rochler M. Integration of real-time Ethernet in LinuxCNC[J]. Int J of Advanced Manufacturing Technology, 2015, 78(9/10/11/12): 1737-1846. |