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

剑映月

1 前言
多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
; j! {- J; y6 N# b4 U) YS曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
/ s8 k% W, v5 K% z  \" H0 `2 多轴联动的线性插补S加减速
S曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
) t+ g) d9 y% o& o! l5 b根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
7 r  n  _" d5 v/ D* ?  N|Pi|
=
P
=TSEP      i=1, 2, …, n
% B1 K" i7 V/ a1 v! pFi
F
0 @3 A. m3 y* x) H" l# `1 l# D3 {, e
) G0 y7 R9 _8 H! j+ @) D图1 “S加减速”规划原理图
式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
令
) c- R( J" o- F+ T4 b' kKi=
6 L" o8 I. e& B5 kPi
i=1, 2, …, n
P
6 A  D( B6 X% l- v2 ~Fi=KiF      i=1, 2, …, n
1 u6 M# n% X0 H: _( S; q  l% c0 G: Z(3)
在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
各轴的运动参量成比例
对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
$ [9 e1 l1 ^( X* E' ]" @% Iv(t)=v(tx)+a(tx)∆t+½J(te)∆t2
  ^9 @6 e0 U5 x( V% N4 v7 ~(4)
同理，各插补轴对应．点速度
vi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
0 @+ T+ ~! D8 r5 A9 ]! o: ^(5)
根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
vi(t)=Kiv(t)F      i=1, 2, …, n
(6)
对于上面恒等式，应有
vi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
/ ^+ P, b0 K' Q6 F(7)
由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
S加减速的插补递推公式
" Y1 n. T0 R, R$ E. M设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
0 H# j* @8 L( F' N4 ?. Ptk
2 y- E$ i$ S1 L8 S- u/ Vv(t)dt=
∫
tk-1
v(t)dt+
∫
tk-1+T
1 j9 w/ ?) i+ W2 T6 wv(t)dt=Sk-1
8 P; l6 n# T: F. }0 E0 l∫
+ Q6 W! [" O! {# g6 a( u( S6 k7 ft
. g4 ^* G# Q# J* M(vk-1+ak-1t+½Jt2)dt
=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
$ y6 r8 T2 N3 \0
% z4 Q" O5 J% T( C0
; u% ~) }) ^# K5 T) m% U/ \2 ftk-1
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)
  \( m2 \3 T! n; b8 u2 k% E4 c+ xak=ak-1+(1/3)JT
& r7 E8 _' }. `. ^6 t(10)
0 ~+ t( @9 j5 Q2 h( E8 s$ G( m" Bvk=vk-1+(1/2)akT
# ^: y/ @9 K( y- D(11)
注意，上述递推公式是分区适应的，即
- l& G! J0 q; ]' n3 x" H6 ]* o; GJ=
{
( _9 Y% D; V( D0 q. \) \) qJ, T∈[t0，t1]∪t6，t7
" n' g  E' r0 r- V& o: a* E% p0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
-J, t∈[t2，t3]∪[t4，t5]
只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
- L) p3 t& o; j1 M5 S$ r∆Pik=
+ D+ S8 O0 o' v1 UPi
5 f2 W' O6 b! i: x* E∆Sk=Ki∆Sk
P
区间的判别
* I0 n8 P1 x2 S) `段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
* N4 k  D* D' |7 l, E# M2 Qts=t1=A/J
(14)
由图1中的加速度图线可以看出
V=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
(15)
tl=(V/A)-(A/J)
. S& C) Q7 B  f! i(16)
9 N( a( v) ~( l% A' J+ O9 B+ {7 {ta=2ts+tl=(V/A)+(A/J)
(17)
7 S$ f1 l1 v3 ?$ k" J) _
0 U. L1 Z6 i5 Y% O, J( n  x% d图2 插补计算流程框图
6 B4 E1 g, ~, J( C0 ^! U  R∆S=P-
k
7 Y1 k& o$ i8 r∆Sk
5 T5 J2 i& i  p, ^5 h% T∑
1
∆Si=Pi-
k
" h" a# s" a7 c8 g∆Pik
∑
7 N7 u% o0 L$ w7 v1