多轴联动线性插补及其“S加减速”规划算法

剑映月

1 前言
多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
S曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
2 多轴联动的线性插补S加减速
8 g, B" U) _. I! G% OS曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
1 _8 N( g  I  v3 d| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
/ L3 |$ _) T- O: P) I根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
+ y- B3 I: t/ @: v/ L4 N( _|Pi|
1 ~! |: n6 r  ^1 _0 i=
& r$ Q/ c1 ~( f- |, p  Z0 |P
=TSEP      i=1, 2, …, n
; e0 L+ H- l2 a2 _: l$ EFi
F

图1 “S加减速”规划原理图
式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
! v+ R! @/ o( [& P' v  u8 ~2 W1 r令
$ b0 f( l. ]1 R2 t" NKi=
7 k0 |0 `$ i6 ?, Q+ j5 IPi
i=1, 2, …, n
; l! @; b2 U0 M2 S& M/ Z  uP
Fi=KiF      i=1, 2, …, n
(3)
在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
, @$ X7 c6 e  K7 W7 r+ s各轴的运动参量成比例
4 |6 I+ x! x- e: n2 M0 O对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
v(t)=v(tx)+a(tx)∆t+½J(te)∆t2
5 w5 F4 M/ H6 W: ^(4)
2 |) }2 U) f; i1 d同理，各插补轴对应．点速度
  C+ {. }" a5 D* t- s6 S: y, k2 rvi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
(5)
4 m8 q5 v" g: z1 z& P8 \6 U根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
vi(t)=Kiv(t)F      i=1, 2, …, n
(6)
9 p$ {0 O0 m1 D6 w) S1 Z% }0 T- A* s对于上面恒等式，应有
vi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
(7)
3 T9 n9 T+ c5 _* X由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
5 c3 U1 a* P, d9 ?' |6 {7 `S加减速的插补递推公式
9 h2 S1 C, W' M4 q; r设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
tk
v(t)dt=
∫
tk-1
' j0 m: U; @$ i! e$ o0 wv(t)dt+
% J( g  r. s( e9 Z2 F∫
5 V1 U' c/ c  B% Y  ttk-1+T
* N( l3 |. J' a( \' M* \6 M6 B- qv(t)dt=Sk-1
∫
$ l  d3 P0 q$ i" S9 b  rt
(vk-1+ak-1t+½Jt2)dt
! h1 m, ^! z6 Q# X1 F0 c=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
0
0
tk-1
0
第k个插补周期内的合成位移增量为
∆Sk=vk-1T+(1/2)ak-1T2+(1/6)JT3=vk-1T+(1/2)(ak-1+(1/3)JT)T2=vk-1+(1/2)akT2=(vk-1+(1/2)akT)=vkT
& b/ W9 G* Z+ g( o5 n(9)
1 z# [# w) |& E3 X% Bak=ak-1+(1/3)JT
$ M+ k/ l+ ?; O9 V, a3 d( Y5 w(10)
' F& p# ^1 W' s! m, s, uvk=vk-1+(1/2)akT
1 t* |/ F) p9 \+ L4 o, V- r(11)
' x& s+ @0 K2 V6 d4 A" B1 p3 G1 R/ v注意，上述递推公式是分区适应的，即
J=
{
! Z" ]2 h1 I3 qJ, T∈[t0，t1]∪t6，t7
  N9 b6 Z/ Y2 O6 |! v# K0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
8 G1 g% F0 o$ M% e' E1 f( f-J, t∈[t2，t3]∪[t4，t5]
. Z: M$ t4 G) u( ^# }$ O) B0 c只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
∆Pik=
' Y+ q- S( e, ?1 L& S* V2 ?Pi
* N6 S# U7 z; [1 a# z∆Sk=Ki∆Sk
P
区间的判别
. o- C4 H  H( L- I  j/ o段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
: ^2 Z6 v7 j6 S# _( _, {ts=t1=A/J
( u- X" X+ W% L(14)
由图1中的加速度图线可以看出
9 N: l# h, l2 r' H( _V=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
4 D% V; A9 R5 I7 ^3 L) u" ^9 C/ U(15)
tl=(V/A)-(A/J)
(16)
! `- R. U- F$ V! k8 nta=2ts+tl=(V/A)+(A/J)
) w- F$ l0 {. D' U5 O. A(17)
$ c0 a6 G6 H& B
3 U4 B8 J: `6 Y: s2 e3 S图2 插补计算流程框图
% ?) s- [1 `3 ?: P" Q∆S=P-
k
∆Sk
∑
  q" y' Z! M) y3 _6 E& j, t1
∆Si=Pi-
k
* A$ C+ `# B) D8 @∆Pik
∑
1