多轨迹段平滑过渡的前瞻插补算法①

王旭浩,张 华

(浙江理工大学 机械与自动控制学院,杭州 310018)

数控系统通常采用连续直线或圆弧对加工路径进行逼近来完成插补.因多轨迹段的衔接处存在过渡拐角尖锐的特点,所以对离散小线段加工时,采用首位速度为零的加减速控制方式,但该方法使整个插补过程中的速度方向连续突变,即驱动轴的频繁启停会影响运动定位的精度.针对以上问题,张晓辉等[1]建立拐角曲线过渡模型,保证轨迹衔接处的平滑性,该算法使加工效率得到有效的提升,但不能保证加工精度;Fan 等[2]采用了基于样条曲线插补的轨迹规划算法,此算法参数约束少且计算速度快,但存在轨迹形状随速度变化而不可控的缺点.许健等[3]提出一种基于有限项正弦级数的新位置规划算法来提高路径过渡的速度,但速度比例参数过大会使得过渡速度很小,不利于保持速度相对稳定.关于速度规划技术在插补算法中的应用,Hu 等[4]采用梯形加减速控制前瞻的进给速度,但存在速度和加速度曲线过渡不平滑的问题;Cao 等[5]将多路径段细分为直线段和圆弧小线段,并对直线和圆弧轨迹的衔接处进行建模,由轨迹的位移约束条件规划衔接点的最优速度,但前瞻处理速度时,采用对称S 形加减速控制算法,该算法在规划初末速度不同的路径段时缺乏灵活性;冷洪滨等[6]采用三次多项式曲线代替复杂的S 形曲线进行速度规划,在加减速过程中实现了加加速度的连续,但加加速度在加减速的始末位置仍然存在突变的情况.为了进一步减少系统的柔性冲击,王允森等[7]采用了四次多项式加减速算法、宁培志等[8]采用五次多项式加减速算法,但上述算法需要大量的时间进行参数的计算,实现较复杂;史中权等[9]提出了一种加速度自适应调整的前瞻处理算法,该算法在加速度的规划上更具灵活性,但在轨迹的拐角处未考虑平滑过渡的问题.

基于以上研究现状,提出了一种多轨迹段平滑过渡的前瞻插补算法.该算法通过建立圆弧过渡模型进行相邻轨迹段的衔接,并基于非对称S 形加减速算法进行前瞻速度规划.本文基于速度曲线拟合构造了一种新型柔性加减速算法,并基于此算法对插补运算中的归一化因子进行求解,使加速度过渡更加平稳.最后将算法在Matlab 端和伺服系统中均进行验证,结果表明,该算法可以使多轨迹段平滑过渡,有效优化电机运动的平稳性,提高末端执行器的精度和效率.

1 相邻轨迹段的圆弧过渡模型

对于连续轨迹而言,任意两直线段的连接处选用建立圆弧模型来进行拐角的平滑过渡处理[10].该算法可以根据特征点的坐标和半径调节等参数规划出过渡圆弧的圆心坐标、圆弧转接点坐标和拐角等参数.

如图1 所示,Qn,vn,rn(n=1,2,3,···)(下同)分别为初始设定的路径目标点、速度和过渡圆弧半径;QTn,vTn,On分别是圆弧转接点、转接点速度和圆心;Kn为两圆弧转接点连线的中点;αn和ln分别为拐角大小和角平分线;dn为分割后的小路径段的长度.

图1 相邻轨迹段的圆弧转接模型

由Q0,Q1,Q2构成的路径段转接模型为例进行求解,设Q1坐标为(x1,y1,z1),拐角α1存在如下函数关系:

由几何关系可得:

由上式可计算得出QT1的坐标(xT1,yT1,zT1)为:

同理可以求得QT2的坐标值.由Q0和QT1的坐标可求得直线长度d1的值,因K1为QT1QT2的中点,可得:

由式(5)可求得K1点坐标.由图1 中的几何关系可得:

将式(6)带入式(7)可求得O1点坐标.圆弧转接模型的所有参数均求解完毕,剩余转接模型可以此类推.

2 新型柔性加减速控制算法

传统的余弦加减速算法通过余弦函数来计算出加速、匀速和减速段的执行时间和位移,该算法采用的余弦函数的计算量较直线函数要大,并没有充分利用三角函数加减速算法和直线形加减速算法在轨迹规划阶段计算结果一致的特点来简化计算.对此提出一种新型柔加减速控制算法,在轨迹规划时,将直线形加减速算法代替余弦算法对加速段、匀速段和减速段的执行时间进行求解,即在插补控制时,将当前插补周期带入余弦加减速函数中进行曲线拟合[11].此算法能使速度和加速度的过渡更加连续平滑,并减少传统余弦加减速算法的计算量.

2.1 计算加减速段的执行时间

由图2 知,vs为初速度,vm为匀速运行时的速度,ve为末速度,设a 和d 分别为加速段和减速段的加速度.加速段时间Ta和运行位移Sa可分别表示为:

图2 直线形加减速算法速度曲线

减速段的时间Td和运行位移Sd可分别表示为:

匀速段的运行位移Sm和执行时间Tm可分别表示为:

由图2 可知:

2.2 构造各速度段的位移函数

由余弦加减速算法构造加速段的速度时间函数:

上式中,t /Ta∈[0,1],ka和kb的取值分别为:

因三角函数无限可导,所以联立式(12)和(13)连续求导得到的加速度和加加速度均连续且有界,从而提升系统的平稳性.各速度段位移函数S(t)如下：

将式(8),(9)和式(10)的计算结果,带入到(14)式中,拟合出各阶段对应的新位移函数:

对式(15)中的各段位移函数S(t)分别求导即可得出系统对应不同阶段的速度函数:

同理,对速度函数v(t)求导可得系统各段的加速度函数:

系统的稳定性分析:取式(15)～(17)中加速段的位移、速度和加速度函数构造一个非线性系统:取其李雅普诺夫函数V 为:

可知V 正定.沿系统的任意轨迹:令V 对t 求导:

显然上式是负定的,由李雅普诺夫稳定性定理可知:该系统是渐进稳定的.

由式(15),对各个阶段位移函数积分可得:

将上式与式(8)～(10)对比可知,式(20)的结果与直线形加减速算法中的位移公式相同.这就很大程度上简化了速度前瞻和轨迹规划时的运算量.

2.3 基于Matlab 的仿真对比

(1)在Matlab 程序中设定同一组参数,即位移为50 mm,初速度为0 mm/s,末速度为8 mm/s,最大速度为40 mm/s,最大加速度为110 mm/s2.对传统余弦算法和新型柔性算法的运算效率进行仿真对比.得到的运动学曲线如图3,各速度段执行时间如表1.

图3 余弦算法与新型柔性算法的速度曲线

表1 两种算法各速度段的运算时间(单位:s)

由图3 和表1 所示的仿真结果表明:同一组参数约束下,新型柔性算法运算总时间更短,可有效减小原余弦加减速算法的运算量,提升运算的实时性.

(2)对新型柔性加减速算法、直线形加减速算法及传统7 段S 形加减速算法进行仿真,得到的运动学曲线对比如图4 所示.

由图4 可知:相较于直线形加减速算法,新型柔性加减速算法的速度过渡更加平滑;相较于传统7 段S 形加减速算法,新型算法的加速度过渡的平滑性得到有效改善,算法构造的过程更简洁;新型加减速算法的速度曲线根据给定的参数约束条件不同而呈现非对称的状态,满足多路径段规划时的外界约束要求.

图4 3 种加减速算法的运动学曲线对比图

3 前瞻插补算法设计

前瞻控制技术的两个关键点是加减速控制和连续轨迹段间的衔接速度处理[12,13],算法设计如下.

3.1 加减速控制算法

轨迹规划中的速度控制部分由非对称S 形加减速算法[14,15]进行规划,该算法的运动学曲线如图5 所示.

图5 非对称S 形加减速算法的运动学曲线

由图5 所示的运动学曲线可知,时间ta～tg将整个速度曲线分为加加速、匀加速、减加速、匀速、减减速、匀减速和加减速7 个阶段;v0,vmax,v1分别为初速度,设定最大速度和末速度;amax和amin分别是加速度的最大值和最小值;jmax和jmin分别代表加加速度的最大值和最小值.下节将按照非对称S 形加减速算法对连续轨迹的细分小线段进行速度的规划.

3.2 轨迹衔接速度处理

以图1 中Q0Q1QT3构成的拐角为例,由非对称S 形加减速控制方法将轨迹段中的直线段Q0QT1和QT2QT3进行加减速规划,对衔接圆弧QT1QT2做匀速处理.为了使末端执行器高效率运行,将圆弧段的速度规划为相邻直线段的可达最大速度vlim.具体如下:

若满足:

则加速段的时间Tsa可以求为:

否则按下式求解:

若满足:

则减速段的时间Tsd按下式求解:

否则按下式求解:

匀速段时间Tsv可求解为:

上式中,q0和q1分别代表初末位置;若Tsv＞0,即存在匀速段,则系统的限制最大速度vlim取vmax;若Tsv≤0,则vlim＜vmax.

加速直线段Q0QT1的速度与时间如下函数关系:

减速直线段QT2QT3的速度时间存在以下函数关系:

将两直线段中间的衔接圆弧QT1QT2的速度规划为vlim,至此,连续轨迹段间的衔接速度规划完毕.

3.3 位置插补

以Q0QT1段为例,设起始点Q0的坐标为(x1,y1,z1).中间插值点的位置坐标(x,y,z)可以表示为:

式中,λ 为归一化因子,(△x,△y,△z)代表首末位置间的增量.故插补问题转变为对归一化因子λ 的求解.传统的插补算法基于抛物线过渡的方式对归一化因子进行求解,但规划得出的速度和加速度曲线的并不连续,这会加剧系统振荡.本文将采用新型柔性速度规划算法来进行归一化参数的求解.

为简化算法,根据第2 节内容将新型柔性加减速算法的速度做对称处理.设加速阶段的时间为T1,位移为S1,匀速阶段末端的时间为T2,位移为S2,总的运动时间为Tsum,总位移为Ssum.设末端执行器在匀速运动时的线速度为v2,基于直线形加减速算法求解各段速度的时间时,设加速度和减速度的值为a_n,则可以计算出加速阶段的时间和位移分别为:

总时间Tsum为:

将位移、时间和加速度参数进行归一化处理:

上式中,S1λ,T1λ,T2λ,aλ分别是S1,T1,T2,a_n的归一化参数,结合新型柔性型速度规划算法的位移公式,对归一化因子λ 进行求解:

上式中,t 代表当前时刻,且t=i/N,(0≤t≤1),i=1,2,3,···,N,其中N 代表总的插值点数,通过下式来计算：

式中,Pn是插值参数,用于增加插值点数;X 和Y 在直线插补中分别表示末端执行器的总位移和线速度;在圆弧插补中分别表示末端执行器的圆心角和角速度.每个插值点的时间值,都有一个λ 与之对应,且0≤λ≤1,结合式(30)即可得到各个插值点的位置坐标.

3.4 插补算法流程

插补算法流程如图6.

图6 插补算法流程图

4 实验测试与结果分析

4.1 基于Matlab 的多段轨迹过渡实验

设定目标曲线是由9 条线段组成的连续轨迹,给定如表2 的位置坐标、过渡圆弧段的速度参数及其半径参数;设定系统允许的最大加速度amax=600 mm/s2,最大加加速度jmax=3000 mm/s3,插补周期为0.002 s,将以上参数初始化并进行空间插补运算,对比传统算法和平滑过渡前瞻算法得到的轨迹效果及运动学曲线.

表2 实验一给定实验参数

方法一:传统算法采用多轨迹段初末速度为零的加减速控制方式.由方法一规划的空间轨迹效果如图7 所示;其的运动学曲线如图8 所示.

图7 传统算法规划的空间轨迹

图8 传统算法规划的速度和加速度曲线

由图8 中的速度曲线可知,传统的控制方式存在频繁启停的问题,容易引起振动并对运动轴造成冲击.基于方法一计算得到的总执行时间为5.2636 s.

方法二:采用多轨迹段平滑过渡的前瞻算法,过渡圆弧半径设为r=3 mm,其余参数同方法一.由方法二规划的空间轨迹效果如图9 所示;其运行学曲线如图10 所示.

图9 平滑过渡前瞻算法规划的空间轨迹

图10 平滑过渡的前瞻算法规划的运动学曲线

对比图7 和图9 可知,由方法二规划得到的轨迹轮廓过渡的更加平滑;对比图8 和图10 可知,速度前瞻插补算法将过渡圆弧的速度规划为相邻直线段能达到的最大速度,提升了末端执行器速度的连续性和平稳性;基于方法二计算得到的总执行时间为3.4877 s,相较于方法一,该算法将执行效率提升近33.8%.

4.2 基于工控机的测试与分析

实验平台(如图11)的上位机采用研华的工控机,处理器是intel(R)Core(TM)i5-4460,CPU 频率是3.20 GHz,系统内存为8 GB.上位机软件TwinCAT 通过EtherCAT总线实现TwinCAT 和伺服驱动器的实时通信,并可以完成人机界面的搭建.按表3 将规划后的位置坐标下发给伺服驱动器,速度和加速度数据由机械手末端电机的编码器进行采集,并在上位机界面中实时显示.

在上位机软件中进行程序编写和设定参数.设定最大加速度amax=600 mm/s2,最大加加速度jmax=3000 mm/s3,插补周期为0.002 s.实验通过限制拐点处不同的最大速度来检验加减速控制的平稳性,并分析过渡圆弧的半径参数对于插补效率的影响.由编码器采集的三组半径参数对应的数据绘制而成的速度和加速度曲线分别如图12 和图13 所示.

表3 初始化参数

由图13 可知,在设定最大加速度为600 mm/s2时,电机的最大加速度为378 mm/s2,表明本文前瞻算法在速度的约束下,对加速度做出了自适应的调整,证明了算法的可行性;由图12 和图13 可知,当拐角半径参数设定为1.6 mm 时,系统的运行时间为6.9251 s;当半径设定为4.2 mm 时,系统的运行时间为6.8409 s;当半径设定为7.3 mm 时,系统的执行时间为6.7273 s,系统的执行时间随设定半径参数的变化而变化.

图11 实验平台示意图

图12 规划的速度曲线

图13 规划的加速度曲线

本实验结果表明:

(1)在不同的速度约束下,前瞻插补算法可以自适应调整速度和加速度,保证了速度过渡的平稳性;

(2)因多轨迹段之间靠圆弧衔接,所以每段轨迹规划的初末速度不必减小为零,即不存在电机频繁启停的问题,有效的提升了加工效率,验证了该前瞻算法的对每小段轨迹衔接速度处理的有效性和优越性;

(3)本文算法对参数设定进行了简化,用户可在上位机界面设定半径参数对目标轨迹做理想的修形处理,也可根据插补效率随过渡圆弧半径参数的不同而不同的特点,设定最优的半径参数来使系统的加工效率最大化;给定的速度和加速度参数可作为算法应用硬件平台的最大限制速度,进而防止轨迹形状因速度变化而出现不可控的缺点.

5 结论

多轨迹段平滑过渡的前瞻插补算法能够使插补轮廓曲线更加圆滑,有效解决了传统多轨迹段初末速度为零的加减速控制方式带来的频繁启停问题,并且用户可以调节半径参数对轨迹进行修形处理;该算法采用的速度前瞻技术可以规划出多轨迹段衔接圆弧的最优速度,并实时计算下一插补周期的执行速度,使末端执行器的运动具有连续性,有助于提高机床的实际运行效率;在插补算法的设计中,相较于基于抛物线过渡和传统余弦过渡的插补算法,基于新型柔性加减速控制算法的空间插补算法对于速度和加速度的连续性均有稳定的提升,适用于精度要求较高的场合.