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

剑映月

1 前言
多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
' |3 F9 H7 J" ~0 t1 E+ D' M5 jS曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
! e5 K6 o( k7 E/ F/ Z+ C) Y( \2 多轴联动的线性插补S加减速
S曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
8 h# J3 J. ~4 O& G设在n维线性插补数控程序中，任意一段插补数据为
| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
" f5 W) t5 S5 Q- E$ E( d" B: q根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
& u% q# U; d% Z9 ^* m|Pi|
=
1 k' Y4 w- W2 z- c; N4 CP
/ k2 g$ v/ Q7 `5 f3 f# I. `& z8 ~+ {=TSEP      i=1, 2, …, n
Fi
% ?# u9 ?. g8 B! T5 z- d/ aF

9 \! O7 S; G$ B, L  ]) T, x图1 “S加减速”规划原理图
式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
令
Ki=
Pi
i=1, 2, …, n
P
Fi=KiF      i=1, 2, …, n
(3)
! o6 h2 e* T( p, W1 _1 _9 @4 X在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
1 S& v' a( k" k各轴的运动参量成比例
对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
v(t)=v(tx)+a(tx)∆t+½J(te)∆t2
(4)
同理，各插补轴对应．点速度
% z8 g% f, e7 E1 \vi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
9 Y* G3 ?) G6 d; h(5)
6 O% }) i2 ~& j) ]+ T根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
vi(t)=Kiv(t)F      i=1, 2, …, n
(6)
对于上面恒等式，应有
& }5 ~9 a4 P2 {9 \' b. svi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
" X! o1 p/ i" p3 y(7)
由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
S加减速的插补递推公式
6 N3 N" r, N4 p3 w0 @/ D7 i7 s设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
' v8 h. R- c: w+ Stk
v(t)dt=
+ T2 J" C5 {/ o* Q∫
9 L! L* k- _  o0 w2 t5 itk-1
7 ?% ^4 h! R. ]5 U5 Dv(t)dt+
∫
" m1 _/ ]! ]3 J/ C0 Stk-1+T
v(t)dt=Sk-1
∫
8 N+ Q  [2 G; _$ x  O* N) Zt
4 r6 ~8 z3 R: y4 n; Q' D(vk-1+ak-1t+½Jt2)dt
- ^( c' B" N0 ?4 V=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
) A2 f  ]) w* x% C0
0
8 U" K0 U( ~! g( gtk-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
(9)
6 f  _$ G  r2 B+ o! `4 Cak=ak-1+(1/3)JT
(10)
2 D. f3 x3 r0 B4 L. u0 ~vk=vk-1+(1/2)akT
  _9 h5 h: z2 j7 V9 e(11)
3 k- @7 F  j3 o- q$ L( g注意，上述递推公式是分区适应的，即
J=
{
J, T∈[t0，t1]∪t6，t7
3 A4 X$ t2 f9 l0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
" `) J5 T, w% s8 v/ P) x-J, t∈[t2，t3]∪[t4，t5]
只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
∆Pik=
( @1 J& [* d$ M7 A+ L$ K  Z3 vPi
∆Sk=Ki∆Sk
P
; N& l( ]6 }4 k" O$ ?& J" T) }- n( P区间的判别
段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
  v2 p* [, [' S% C- o1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
ts=t1=A/J
(14)
由图1中的加速度图线可以看出
2 N+ t' I# R' M3 t! M. WV=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
  p- z, \/ s, }0 s(15)
tl=(V/A)-(A/J)
, v9 x2 ?, ?* m& w(16)
2 h6 E& w" W0 q9 D, ]" Ita=2ts+tl=(V/A)+(A/J)
. f' B( e; L& l; N4 U(17)
: b) S; W3 `- M6 q( s
图2 插补计算流程框图
- l, L, x% y  K8 Q$ n3 n* F∆S=P-
2 `/ H4 W5 X3 Q6 y! Jk
∆Sk
4 E7 Y1 D  Y( N: A0 I! H, z; z4 i# s) s∑
9 {& I: @5 a& q7 M. Q9 n' y1
% s4 Z5 D- t0 [! V: J∆Si=Pi-
- Z) u% P  f, {4 S6 `4 Lk
∆Pik
% t) A- c/ I9 `+ S, Q+ l∑
) X) e3 p1 ^& ]1