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

剑映月

1 前言
( }1 l& k# Z+ a4 P( }多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
S曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
0 x9 z$ C+ [# i1 N( `) _& }1 L2 多轴联动的线性插补S加减速
S曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
" p/ F( \# A" ^  b| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
( a3 G& ?$ h3 t5 r: o根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
|Pi|
4 z; U% |7 Q5 G+ R=
0 x, v6 Y; X. j: \6 P7 @6 `8 u# J+ kP
2 X( w' R% O2 S9 L' j, n0 D=TSEP      i=1, 2, …, n
Fi
F

图1 “S加减速”规划原理图
7 ~+ s& a) q; r) O+ H9 T7 z7 u式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
. _. Y1 L$ ?9 B- h: S令
& S( I& _7 {0 G: wKi=
+ j7 w- x7 |" ~, m. l# ?; f0 YPi
) U4 ?' ^  c5 _$ C& t- d- Ni=1, 2, …, n
P
Fi=KiF      i=1, 2, …, n
0 n1 V: L" R6 L(3)
9 ^) C8 O8 ~* K: T: G在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
- Y2 m, c* w  K. f6 J各轴的运动参量成比例
对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
) u: l6 r4 F9 b0 ?- m! vv(t)=v(tx)+a(tx)∆t+½J(te)∆t2
# B* B: y, H5 f: }' A, G(4)
同理，各插补轴对应．点速度
& s6 ~: b& |6 E) uvi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
) l1 A6 }9 D3 r& i  K$ n(5)
. e+ m, _2 H( `6 s7 |根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
vi(t)=Kiv(t)F      i=1, 2, …, n
* u9 X2 B5 p+ S: \/ B( P(6)
对于上面恒等式，应有
vi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
4 F# V; v" j; V9 r- t6 Z2 n1 m(7)
/ p- l: }* g" y* T4 A由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
S加减速的插补递推公式
) v. _; Y* ~: H! S' l2 B设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
tk
v(t)dt=
1 v% g7 _6 Q. [2 M∫
  ^  x* c5 g/ ?7 [7 s7 J4 X+ jtk-1
v(t)dt+
∫
2 M. t* `- Q0 [/ }* otk-1+T
# U- a. y5 h" |* Mv(t)dt=Sk-1
! R: b4 q% T4 r∫
t
(vk-1+ak-1t+½Jt2)dt
" E  r% y# |! F! |$ d=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
" D7 Z; L* a6 K6 ~( y+ w0
0
7 }. g7 F  u/ I' Q( stk-1
0
/ {* @3 {" y3 o第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
1 o$ ?# k2 {- _+ K0 Q3 F  A(9)
: p! x. e$ ^1 `/ B5 Jak=ak-1+(1/3)JT
7 C! W+ l1 ^" s  [(10)
vk=vk-1+(1/2)akT
1 t0 ?) T0 p  c/ ?3 v$ c: B9 S3 u(11)
注意，上述递推公式是分区适应的，即
J=
{
/ e- O$ ^# o$ X( H; O  W4 lJ, T∈[t0，t1]∪t6，t7
0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
-J, t∈[t2，t3]∪[t4，t5]
3 e' {8 x1 L5 I( q" E只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
∆Pik=
Pi
. o( C' [6 {" O; E+ g0 x∆Sk=Ki∆Sk
P
区间的判别
段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
5 \+ A4 w, ^; ~* x4 l1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
! `/ _: j) T' Z8 _/ }% yts=t1=A/J
/ M* R& r$ j5 p0 I. y(14)
: I+ `5 e- K2 R9 S# }) p7 U0 D由图1中的加速度图线可以看出
V=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
1 H/ r9 @! b* y. c: w: |(15)
tl=(V/A)-(A/J)
" v0 ]/ u/ \: ?) C2 Z(16)
( N9 S  j& \& Vta=2ts+tl=(V/A)+(A/J)
0 }5 o$ Y) Y- A# A* j7 `; n(17)
. A2 t! b# A4 X$ [
: y' s6 X# ]$ O9 o图2 插补计算流程框图
∆S=P-
k
: W3 K$ G( K2 Q8 _0 L∆Sk
∑
1
% c1 V' h- Z0 }( w0 l6 y1 H2 g∆Si=Pi-
k
∆Pik
( r/ j) c3 b% H& z. m* k∑
7 E! ^* a9 C. Y6 s1