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

剑映月

1 前言
多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
S曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
7 i# F. e& j) K9 ?- W, P/ K% r! |! c2 多轴联动的线性插补S加减速
S曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
1 j3 X" u  N6 P4 {1 n根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
& d  N9 P, _9 F|Pi|
* b- {. K8 J# I=
P
9 ~& h' J$ k: f: T0 j=TSEP      i=1, 2, …, n
Fi
F

图1 “S加减速”规划原理图
* S8 c; m  K( J* w式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
令
Ki=
Pi
i=1, 2, …, n
8 z* o/ v* C. k3 Z( u( z  l1 I' o8 FP
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。
8 B6 j& Y# x9 w  R, a( h3 Q: a各轴的运动参量成比例
/ j% v( E3 E* x+ o2 @0 A# l$ S3 l对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
9 c' {* K0 g3 F' T% cv(t)=v(tx)+a(tx)∆t+½J(te)∆t2
4 D0 k  }% J* e+ U(4)
1 G8 O! x- P/ z. C- |( S) z同理，各插补轴对应．点速度
$ v6 x3 b+ z9 @% s5 J/ H7 c" lvi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
$ N; o: u0 y  D  |(5)
. I( ]/ o' O2 t根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
2 u. E& j4 ]" Yvi(t)=Kiv(t)F      i=1, 2, …, n
) I& D# ~. O) _1 V6 {: ?! J$ x(6)
2 c( P; [5 H4 R% z9 j) ~8 ]! M对于上面恒等式，应有
8 P5 c2 }* W3 D1 n3 gvi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
0 F( T' N! G0 _(7)
5 b1 o" S* u; X/ g5 \由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
  e" K0 }* n/ v# m. n) n# Y: VS加减速的插补递推公式
设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
  b# S: t* T3 x+ i8 b∫
tk
v(t)dt=
+ h* j5 O4 w1 O0 h$ t4 m3 z∫
tk-1
v(t)dt+
∫
  v2 Y& g6 f/ q. ftk-1+T
* a; B  \- p5 ^: W9 Tv(t)dt=Sk-1
∫
9 f, @% u6 D% m, Wt
6 j; J) |9 `9 Y, ~" @# N# ?(vk-1+ak-1t+½Jt2)dt
=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
% o* t/ U7 Q8 i0
, S% X& k9 r8 t8 G0
tk-1
; f( T' T" }" V2 H0
) ?$ [7 {% q+ u1 Y第k个插补周期内的合成位移增量为
4 c+ Y* {. a0 l& x∆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
3 z; F" ]8 \. O6 |(9)
1 i& d$ A7 C2 F8 c& Qak=ak-1+(1/3)JT
(10)
vk=vk-1+(1/2)akT
(11)
注意，上述递推公式是分区适应的，即
, a  z( f8 J" f8 {J=
{
) G. q: N. n' H. jJ, T∈[t0，t1]∪t6，t7
" o- Z; B& K) c2 }7 {. z) h0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
7 z& E/ d( v0 `  S-J, t∈[t2，t3]∪[t4，t5]
只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
∆Pik=
Pi
∆Sk=Ki∆Sk
P
3 }9 Y6 f3 i% w( M区间的判别
段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
4 W% t$ d* R6 ^4 uts=t1=A/J
0 n- _8 g1 f5 {7 v(14)
由图1中的加速度图线可以看出
: f4 t5 x4 b) c3 T0 ?& c7 DV=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
, w' `. J. K* q+ D/ S) n8 ^8 q(15)
4 ]" Z1 Z0 C( @6 n9 ttl=(V/A)-(A/J)
(16)
ta=2ts+tl=(V/A)+(A/J)
(17)
8 Y; D" v, o/ M0 K
" z: b6 f. g5 d& b+ H图2 插补计算流程框图
2 O$ x- T) B9 j# e# [∆S=P-
8 G4 r0 i: T) |" uk
' g* j3 F7 \6 `1 O% [∆Sk
% w2 u1 _, L2 E$ S* ~" k∑
! |3 {( W1 x  r; ?" E+ P1
7 ^6 S# M9 }) q8 i6 K8 F6 O∆Si=Pi-
k
5 Q* g, N5 K5 G( L! v" S( ^∆Pik
, [5 m/ A( |3 E' b" D( G) L∑
6 l7 ]) q- A$ N, ?, S7 E6 R1