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

剑映月

1 前言
多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
S曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
2 多轴联动的线性插补S加减速
S曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
0 o! @, \' s+ L| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
2 W3 _4 m  J+ W+ n. M0 ]根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
|Pi|
=
P
=TSEP      i=1, 2, …, n
6 K; D8 _- w3 A2 mFi
% g  ^* b* v9 V: z4 Q0 c/ WF

9 C+ b& E) v' }1 N图1 “S加减速”规划原理图
式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
' c# g2 p3 j+ [% L' v令
/ h! _; i, B4 O4 Q5 PKi=
. `+ M" ?: l% L8 q0 ?Pi
! z8 U$ {" R; w0 }& A+ S. Y' zi=1, 2, …, n
# P6 ]. v  W  d) aP
' }; g: ]1 s; u7 f7 HFi=KiF      i=1, 2, …, n
- l& A0 S) y4 c(3)
在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
各轴的运动参量成比例
0 [/ F( k: x0 m1 [+ ~对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
v(t)=v(tx)+a(tx)∆t+½J(te)∆t2
- `7 r7 d4 H! D+ P; n(4)
同理，各插补轴对应．点速度
vi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
9 V# I7 q7 W; I) W(5)
4 c0 m, F( Q) ^. G$ ?6 F6 f根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
  E& H) ?6 Z; a, R5 M% o0 U9 ?vi(t)=Kiv(t)F      i=1, 2, …, n
- @+ g# o" m) k; Y5 @(6)
0 Y' o/ t% U2 S对于上面恒等式，应有
1 O/ G/ i* E8 C( K  ~& z% ^& Hvi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
/ i5 t" K  f8 {3 E2 T(7)
由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
& }5 e+ D; q) p, d  \. Y1 HS加减速的插补递推公式
/ N( x# `7 p. h) L设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
tk
9 z' v) @3 k4 L0 fv(t)dt=
$ m7 g2 b) o: s9 ]+ U∫
tk-1
v(t)dt+
∫
: r/ r1 H# k/ y6 b5 s% _" L/ ztk-1+T
$ Q  S# p- M$ I1 k- gv(t)dt=Sk-1
9 l4 u5 x$ t8 w; W∫
( Y/ f: O3 J" x9 @$ B+ bt
(vk-1+ak-1t+½Jt2)dt
) ]7 [& a+ I' Z: p+ Q0 g=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
, T- L- m# m* q7 A7 \0
0
1 M8 ?" Z; a- C/ [' T* ?7 K! F, A4 mtk-1
9 O, F1 a2 u0 u6 g! c+ c0
第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
  M- J9 e+ E& E2 p+ r* _6 A8 @(9)
ak=ak-1+(1/3)JT
(10)
4 E; Q  j  G6 b1 i% ]vk=vk-1+(1/2)akT
(11)
* b0 D% L. ^% ?, O! @) d  D注意，上述递推公式是分区适应的，即
6 k' }/ p) y& t" @  {J=
, {8 d8 p! n* S! r- S* x; s9 Y, ]{
5 f2 Q; {! D1 E* bJ, T∈[t0，t1]∪t6，t7
$ \. e5 u7 x- z9 c. X0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
-J, t∈[t2，t3]∪[t4，t5]
2 z0 Z8 o; g. Q1 `只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
9 I7 @; w* D- W) z∆Pik=
Pi
. O% |5 ^) @0 Q' d5 u* U∆Sk=Ki∆Sk
& Q6 M1 M8 X- O' ~P
区间的判别
段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
) ~: J' S9 ^1 J8 ets=t1=A/J
(14)
由图1中的加速度图线可以看出
V=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
0 x, ~$ |) q" l0 l  V(15)
4 l8 \. \  q- F/ b8 H7 J( }2 Ftl=(V/A)-(A/J)
2 g* ~: E8 U5 k8 n, y  P(16)
ta=2ts+tl=(V/A)+(A/J)
" _- d- b2 N2 e! }6 P(17)

图2 插补计算流程框图
∆S=P-
; C' m6 K, k; v* C  v- Z! |" jk
∆Sk
∑
1
∆Si=Pi-
k
) }+ P# S+ r+ F4 \∆Pik
∑
( h! m4 B0 F. y1