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

剑映月

1 前言
5 j) |! ~9 f) E& B; m多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
9 F! W+ w- S; s) U" tS曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
2 多轴联动的线性插补S加减速
6 f4 x0 D0 ~# |! dS曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
% X3 P- T8 @3 @0 E# `|Pi|
! L6 h- x7 H8 J0 f( M3 g, }=
1 N; r- N/ w) P& {, {* b' PP
=TSEP      i=1, 2, …, n
3 y, r; s7 x1 r3 E! L8 j4 ^/ [0 qFi
F
2 ^6 |- K2 \. w. t0 e
图1 “S加减速”规划原理图
' J6 b+ ^4 N) s6 ~& @式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
令
Ki=
Pi
& }" H' y5 ?% U4 w, t) gi=1, 2, …, n
P
Fi=KiF      i=1, 2, …, n
' }' S1 m+ h, m  A6 @(3)
' s1 j* _! u; ~; ?+ g3 j在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
3 q6 M+ o8 U" P' Z各轴的运动参量成比例
对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
5 [6 w: h$ A5 fv(t)=v(tx)+a(tx)∆t+½J(te)∆t2
) i3 n0 P+ w  F; R, J(4)
同理，各插补轴对应．点速度
vi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
% d0 Q: a1 n; C+ v; g(5)
* s0 {* ?; Q0 z根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
3 `2 r: }6 Q( o. Xvi(t)=Kiv(t)F      i=1, 2, …, n
! G: [- J" q: F1 u4 ]& e(6)
- C2 t2 q( O: }( w  i# J& S对于上面恒等式，应有
vi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
(7)
由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
S加减速的插补递推公式
设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
∫
6 k; B: ?5 u, }# C' [/ jtk
; Z9 O$ \9 A) m: h8 V. y- yv(t)dt=
  J/ ]# X) k+ t8 W  o. P% y∫
  h, T: q) L) W7 c$ u3 Ytk-1
9 U" s  c: O5 t7 r: L: t1 Z; fv(t)dt+
1 s# q4 M1 L- `/ d& k1 a# T7 y∫
' m) p& H: l) c' v% f2 {tk-1+T
v(t)dt=Sk-1
% W5 k( `9 P" [' U/ ]∫
. @& `! M. T- j# o; J% G8 ut
. h/ ?  E2 t8 r' }(vk-1+ak-1t+½Jt2)dt
8 p! o$ y9 V  y/ A=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
0
6 X0 b+ @7 y" g0 k8 p7 _0
0 W! N' O/ r% M/ z' T- J. Ltk-1
0
第k个插补周期内的合成位移增量为
6 R2 l$ f$ z. @) `∆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)
* p4 M; `7 ~/ Gak=ak-1+(1/3)JT
0 ?- C' N( T# W) a5 d7 G+ b(10)
' ]: @; g: B6 z: r( X  N! wvk=vk-1+(1/2)akT
2 _6 w6 ?( y* d" i( I  h& ~+ p& P(11)
; K: V' L1 Y4 [* S" d3 A0 U4 E: v注意，上述递推公式是分区适应的，即
7 }' D3 d+ }  x2 K  W. fJ=
, F3 F- P+ Y3 a% N- v1 b# h{
0 h! v$ m$ |0 s" X! k8 aJ, T∈[t0，t1]∪t6，t7
2 \% |8 A& H+ `, T0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
+ l) X3 h6 H; c) I-J, t∈[t2，t3]∪[t4，t5]
只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
" p- Z% F- F! D+ {/ ]∆Pik=
Pi
∆Sk=Ki∆Sk
P
区间的判别
( j3 z3 @3 n1 j7 O1 K' x段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
7 {) [. o& Q6 q# @1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
2 V! [0 `& N$ {8 L% z5 P; ots=t1=A/J
(14)
, ^# i3 @& H) q3 b6 q0 P, u由图1中的加速度图线可以看出
" w, \! X' H7 S; p$ }( p8 LV=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
8 u" U% b7 H9 Z: n0 c, N(15)
tl=(V/A)-(A/J)
(16)
" V1 X3 I+ u9 m0 N1 T1 e0 {ta=2ts+tl=(V/A)+(A/J)
(17)
& Y, h% S3 H' n- u" |
图2 插补计算流程框图
∆S=P-
k
1 i* D! R- p  U5 t∆Sk
∑
. ?4 [- b" w) d# D1
: @% h+ S" }. L$ L∆Si=Pi-
k
# P' n- f2 z' d; b0 ?' K- i∆Pik
∑
$ }3 x9 ~% U( d7 a1