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

剑映月

1 前言
3 N( K7 b& _/ {) ~& ^. r多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
S曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
, G; Z; Y4 F: B1 q2 多轴联动的线性插补S加减速
  K% F/ \" ~% J" Y5 Y$ zS曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
4 X& x1 F2 i; h| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
4 V! ~! T6 I- V! Y& U9 @7 ]( @根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
|Pi|
7 R% w3 l4 p, {=
P
' y0 {$ n; R! M" P4 e+ A# X, O=TSEP      i=1, 2, …, n
Fi
1 a" t! `2 d, `! {0 _F
' w1 p; l% l' g& w/ N" q
1 t6 g0 M# [8 [* A$ f图1 “S加减速”规划原理图
. r5 ?/ p) F) U& v/ R式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
令
Ki=
Pi
- r( ^: _9 u7 J1 N% R' o" ^) v6 u$ hi=1, 2, …, n
P
Fi=KiF      i=1, 2, …, n
& X" q2 \) J$ N4 y  i3 i+ P) m1 s(3)
' W* ^" r$ ^" R8 V/ t4 W& I在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
" }" Y, x* {2 b* ~1 y& Q各轴的运动参量成比例
* m4 n0 C1 X8 G/ ~. N对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
v(t)=v(tx)+a(tx)∆t+½J(te)∆t2
' T6 H; p, Y: X& m(4)
7 D' I# e3 p5 k/ m同理，各插补轴对应．点速度
" Z6 L0 Q; D3 x  _2 w* V& q" Yvi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
(5)
& {; I! S& I' }) G7 b8 }6 t# n根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
' Y  v. [. G$ L) Qvi(t)=Kiv(t)F      i=1, 2, …, n
(6)
# {+ }8 ~, K8 o$ _+ G) t; S对于上面恒等式，应有
. u& V; F6 E0 C& L0 j5 g( }: Jvi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
" Q9 S2 m! r, o! n. D8 F(7)
由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
: `" ^. w  y. M! \: n1 ^. a" m; yS加减速的插补递推公式
设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
0 R/ L' g& P2 O9 atk
0 b  V1 o, v3 rv(t)dt=
∫
3 k; _0 v( [. r; y% m* J! U4 Ztk-1
v(t)dt+
∫
: f3 X: c5 _$ U, f" m8 a, C& qtk-1+T
/ c( Y5 O2 q; V0 [! I2 iv(t)dt=Sk-1
∫
t
! a4 c. A, o' U) U8 K1 i(vk-1+ak-1t+½Jt2)dt
=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
0
) ^: k+ X$ W% ^! Z& O/ C0
tk-1
. U! P- m9 n3 ?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)
8 s- M. g+ i4 x8 n, Vak=ak-1+(1/3)JT
8 _1 Q3 L0 A- }- ]7 _/ E  e( d9 U- ~(10)
3 C4 X, P! U- v" c2 evk=vk-1+(1/2)akT
(11)
注意，上述递推公式是分区适应的，即
# t1 p; k) g+ qJ=
$ z# K7 N1 ?! P) J, X/ l' V3 \/ w{
$ B- k4 o7 R. H5 P" O( \J, T∈[t0，t1]∪t6，t7
0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
  z" {: m0 c' Z0 C-J, t∈[t2，t3]∪[t4，t5]
只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
∆Pik=
Pi
+ A6 m' R( j4 X$ e∆Sk=Ki∆Sk
P
区间的判别
' \% v! X/ ]$ G  o; A7 P5 b段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
7 Q. N4 P: j% v6 Uts=t1=A/J
2 x" N4 ?* U5 L. H0 {2 b(14)
: L' X- A$ U  ^& W. t0 q9 g7 h由图1中的加速度图线可以看出
V=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
8 s& X- a* K; D3 p(15)
tl=(V/A)-(A/J)
(16)
7 N4 V' J3 O. c8 R$ \9 M5 }; pta=2ts+tl=(V/A)+(A/J)
9 o8 I" M) [& B6 [2 ~(17)
  f0 S3 y3 w+ K% R5 F+ h; {
. n" ?. s' U$ T+ y* G- K图2 插补计算流程框图
8 b( Y0 O) s, \5 S- ]∆S=P-
4 I- ?9 O: V' q" {# ak
; W& n; M( _  `' Q∆Sk
" T) o( p$ T$ p6 U- b. Z1 h, R9 r∑
6 E3 |- A0 J! U  B' ~1
∆Si=Pi-
k
* s! l  w% z; q+ u1 J∆Pik
$ l8 b' t6 ^4 ~& Z∑
2 d/ n/ \9 ]( f0 H4 ]& r( ~' ^1