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

剑映月

1 前言
$ W6 t4 N# E% q( c. E多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
S曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
$ N. H# W" u6 r: @7 B2 多轴联动的线性插补S加减速
S曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
3 c3 f# N- Y* v* n* Z/ q  p设在n维线性插补数控程序中，任意一段插补数据为
| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
|Pi|
=
6 E* Y9 L+ z; YP
=TSEP      i=1, 2, …, n
" a2 f3 c6 Y. J4 ?" T! b$ zFi
F

图1 “S加减速”规划原理图
. ]5 Z. Y* ^4 G8 T: c式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
令
3 r# Y& \' v: R0 F1 Z  e0 BKi=
Pi
i=1, 2, …, n
P
. n  S8 P+ y' w+ J8 L0 H& j) hFi=KiF      i=1, 2, …, n
(3)
. J% s3 l5 z8 M6 f/ B3 S在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
各轴的运动参量成比例
5 @& p( w+ Q# t4 }+ y% [对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
v(t)=v(tx)+a(tx)∆t+½J(te)∆t2
' M. h" H( {! |: F: \& w(4)
同理，各插补轴对应．点速度
vi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
(5)
7 h; Z# w, x' v$ T根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
vi(t)=Kiv(t)F      i=1, 2, …, n
(6)
2 Q) a1 g0 A& |( T- Z+ q9 n/ y% r- `对于上面恒等式，应有
vi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
& w  w$ t/ R- ?! d5 ~! U; I(7)
由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
% [9 {0 }1 C2 ]) T+ B; u( JS加减速的插补递推公式
设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
* L; z7 K, `1 k/ L6 f2 e∫
" i4 D4 h6 T; J8 }9 Otk
v(t)dt=
  \0 X0 M5 q' N8 R3 R$ m∫
tk-1
v(t)dt+
∫
0 O8 v+ A& S' }. ?( Xtk-1+T
2 O2 e& t  ]: Vv(t)dt=Sk-1
∫
4 k1 b, e' ]# ]( j9 P1 C# st
" V' d- U, q( k6 ^(vk-1+ak-1t+½Jt2)dt
: h4 p0 \; e/ r0 {8 z3 b7 M3 d=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
* ]: T7 O+ x% r- Q& g0
( [% g& N/ A5 C0
tk-1
5 {5 G' W$ J4 ?" X! v, J1 w: ~/ L; u0
第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
  z7 z9 t2 K9 B* E(9)
7 N! I0 Y8 W5 Z" jak=ak-1+(1/3)JT
(10)
vk=vk-1+(1/2)akT
(11)
注意，上述递推公式是分区适应的，即
1 Q/ P: l' z7 S7 r5 [) [J=
{
2 C% e3 r+ M1 T1 JJ, T∈[t0，t1]∪t6，t7
7 q( t$ D' f- `9 s5 _0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
& d8 u. n, L; R+ q& Y-J, t∈[t2，t3]∪[t4，t5]
1 x) @- J& F5 w只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
8 {9 k( F5 [8 N' C8 i9 k- d+ }∆Pik=
Pi
7 O! k3 p0 ~3 B: \6 g/ i4 @$ o1 }∆Sk=Ki∆Sk
6 N1 B: {$ K- U7 e6 sP
区间的判别
段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
ts=t1=A/J
  g5 M/ [" f& F* R(14)
0 S2 r2 d! q/ ~7 F: E8 W1 }由图1中的加速度图线可以看出
& J+ I& v% ~$ J9 U6 GV=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
1 Z  {: }5 R( W& R" i(15)
tl=(V/A)-(A/J)
+ ^) n6 g* [% L3 W9 r(16)
ta=2ts+tl=(V/A)+(A/J)
(17)
% g8 l  Q' Y" o. K
图2 插补计算流程框图
$ `" r' w& p! n, K) X! _∆S=P-
- Y$ f7 A, r, ]- d+ M; L6 Ok
" `$ n" ?5 q) ?6 {- _0 g∆Sk
: e7 r, U/ Q+ J  A∑
1
' U. V: O- \7 |! z∆Si=Pi-
k
6 l" O7 d+ K( q6 q, A( [∆Pik
∑
1