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

剑映月

1 前言
" B1 k! b( u9 d: G3 _+ B多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
S曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
/ V0 v; L$ \$ I2 多轴联动的线性插补S加减速
S曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
# b5 V7 P4 m: v. Y& I' S设在n维线性插补数控程序中，任意一段插补数据为
| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
  D7 C! n" O# [1 f, F根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
|Pi|
8 R* V; F/ {: T: \$ G8 J0 Q=
9 T3 r3 q, P- ]P
: c, L4 ]; m" p; b! v=TSEP      i=1, 2, …, n
9 \8 S9 K0 b' O2 z. WFi
F

; }" y7 o6 ?7 M/ G( ]: S/ @( l图1 “S加减速”规划原理图
式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
令
8 ?/ j3 \9 ]: f# S9 jKi=
Pi
i=1, 2, …, n
% h; [' _. O' }3 Y. |P
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。
& f$ b/ i1 ^6 E: J4 u/ b# l* y- |各轴的运动参量成比例
0 I. K2 E0 K6 w' J9 r对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
v(t)=v(tx)+a(tx)∆t+½J(te)∆t2
" z' g* z4 O) i4 \  E( M(4)
同理，各插补轴对应．点速度
vi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
(5)
; }9 K# N$ n. I& ^根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
vi(t)=Kiv(t)F      i=1, 2, …, n
(6)
对于上面恒等式，应有
vi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
0 |' [( d: {  [! ]% E5 ?(7)
" z" N1 d  Q5 `7 M$ f  P由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
5 ^3 u0 _, c: M0 O% q$ H! fS加减速的插补递推公式
设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
tk
v(t)dt=
! P, T, @; {& t8 Y# x$ R! L0 M∫
7 P. g& p7 n: X; F/ btk-1
v(t)dt+
∫
- h! t6 F: H8 o) w/ R; u2 ?- ptk-1+T
: H4 g+ Z6 Q' F- L7 p( l% \5 M  zv(t)dt=Sk-1
2 R0 G- e! F) f5 J1 [∫
+ v/ g' Z/ I8 E: h- Z6 L6 C. Zt
(vk-1+ak-1t+½Jt2)dt
=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
0
+ M) g3 u: `( h  M% m0
* Q  H! H; i) l! K" m4 Ntk-1
3 c. y0 q; i5 c2 L1 Y0
第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)
, V6 _. ^* i  k" A% ]5 J" n# ~ak=ak-1+(1/3)JT
(10)
  z) k, Z: Y. i) A$ ovk=vk-1+(1/2)akT
(11)
注意，上述递推公式是分区适应的，即
' S$ P1 d4 G' z& P/ {8 ^* LJ=
{
1 l' w4 r$ `1 g9 zJ, T∈[t0，t1]∪t6，t7
. B8 {, z( E, \! A/ B0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
) ^3 m, z5 r) V( p0 a" i-J, t∈[t2，t3]∪[t4，t5]
% a$ B( i6 [" C只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
8 c3 V7 H5 \9 G* j' X4 ^2 M∆Pik=
7 x: r8 |9 Z0 k7 OPi
9 K8 E9 M7 J' r# q7 r7 r∆Sk=Ki∆Sk
5 w) t* R' H2 {" {P
区间的判别
9 y) s; b" ?& v; ]/ q段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
ts=t1=A/J
(14)
7 P- j2 s7 w) U% R  h' O2 j3 C4 C由图1中的加速度图线可以看出
& i2 `$ N; i; j5 C1 n9 ^5 W) tV=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
(15)
9 z5 H0 Y2 x7 j* O0 {tl=(V/A)-(A/J)
(16)
ta=2ts+tl=(V/A)+(A/J)
; L9 t# u8 G5 E* Y$ R# F( t(17)

图2 插补计算流程框图
; W$ j" |% o& L, L) w- \- U∆S=P-
$ V+ p2 T6 K( D9 Y& wk
# J5 }0 ~- d' N& D8 A1 w: k$ P0 U∆Sk
∑
1
4 q' a. Y3 |3 _) T( J∆Si=Pi-
k
2 i2 q' Z( E5 p, o: X! a  Q∆Pik
∑
1