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

剑映月

1 前言
- F- \& v: C/ k2 P" f3 G, `多轴联动的线性插补及其加减速算法是高档数控系统的核心技术。加减速处理分前加减速处理和后加减速处理。前加减速处理在插补之前，其优点在于对合成速度进行控制，不影响位置精度，但要进行减速点的预测。前加减速通常采用直线加减速。后加减速是对各插补轴分别进行加减速控制，由于各轴之间没有协调关系，因此，合成位置可能不准确。
( K! T! _% r! J$ a5 v; n2 DS曲线加减速通常用于后加减速处理，我们在开发基于开放式数控系统的多坐标联动纤维缠绕机时，将其应用于前加减速处理，取得了很好的效果。
+ X" ?: B# P+ A/ t! C3 Z% {2 多轴联动的线性插补S加减速
2 `  P: K$ ^# SS曲线加减速规划是指在加减速时，使其加速度的导数(Jerk) da/dt为常数，通过对加Jerk值的控制来最大限度地减小对机械系统造成的冲击。另外，通过对加速度和Jerk两个物理量的参数设定或编程设定，可实现柔性加减速控制，以适应不同种类机床的工况。
设在n维线性插补数控程序中，任意一段插补数据为
| P1, P2, …，Pn, F |其中：F为合成速度，P1～Pn为各插补轴当前段的位移。
根据线性插补原理，各插补轴的位移与速度比相等，则有应于各插补轴的分速度。令
|Pi|
, L# u1 S* d) z/ P" g* l=
P
=TSEP      i=1, 2, …, n
Fi
9 s; a' k1 _2 I% g1 SF
5 y+ A4 D, b) x6 W  I
图1 “S加减速”规划原理图
( }) r: a- l1 s# v式中：P=( Pi2)½表示合成位移；TSEP表示该线性插补段各轴同时到达终点所需时间；F1～F2为合成速度对应于各插补轴的分速度。
5 D  W: L! R$ G% V$ {0 ~. C令
+ Z$ U$ M& G' H3 dKi=
- s" ?0 D  @6 ~, c; l+ a& @2 W, pPi
5 W0 k& x3 k+ E3 Li=1, 2, …, n
$ t; y# L# l( a' D- KP
' P' r+ d. `, l. kFi=KiF      i=1, 2, …, n
(3)
+ E( |8 }/ y4 Y$ @% h& r5 Z8 `在前加减速处理时，对给定速度进行规划，如图1所示，整个加减速过程分为三段，即加速段(1, 2, 3区)，匀速段(4区)和速段(4, 5, 6区)。在加速段和减速段，又分别包括变加减速区(1, 3, 5, 7区)和恒加减速区(2, 6区)：变加减速区，|da/dt|=J, Jerk为恒值；恒加减速区，|a|=A，加速度为恒值，匀速段(4区)的速度为恒值Vc。
; z5 S/ m$ {7 M' V. h7 [1 x! N5 Z各轴的运动参量成比例
  Z- b( z) f( O' X' J# e对已规划的合成速度v(t)在te点幂级数展开，令∆t=t-tx，有
v(t)=v(tx)+a(tx)∆t+½J(te)∆t2
% E1 g( o# Z( N- e; @(4)
$ w2 f- r# e/ I& ^8 _1 a同理，各插补轴对应．点速度
vi(t)=vi(te)+ai(te)∆t+½Ji(tx)∆t2F      i=1, 2, …, n
(5)
根据线性插补原理，合成速度与各插补轴速度有下列比例关系：
9 x5 u5 k5 W2 y- T9 E! A! ^4 ], k0 A0 w0 Bvi(t)=Kiv(t)F      i=1, 2, …, n
(6)
对于上面恒等式，应有
vi(tx)=Kiv(tx), ai(tx)=Kia(tx), Ji(tx)=KiJ(tx)
(7)
由于tx为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
" S( L2 J4 ?7 N4 l- zS加减速的插补递推公式
设插补周期为T，则在第k个插补周期结束时的合成位移Sk为
1 o* M' C$ i# j* Q: b* z∫
; v$ Z, {; t  otk
v(t)dt=
∫
# v* E; y! ~9 Ntk-1
v(t)dt+
∫
tk-1+T
v(t)dt=Sk-1
∫
t
(vk-1+ak-1t+½Jt2)dt
% R6 G. L7 d- x) P! F& g=Sk-1+vk-1T+½ak-1T2+(1/6)JT3
9 F" M$ G) W& h' `- |0
: I: o+ X* s0 M, i( @) _+ D, b- B+ t0
; X8 a% |; p8 L2 Ntk-1
% N3 j& [/ Q: J# @: s2 u2 g0
第k个插补周期内的合成位移增量为
5 v% |  _* m" f& X/ v6 z) z& }: ]3 i∆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
0 F5 s/ h1 {' U3 h(9)
ak=ak-1+(1/3)JT
# |. L' q2 t; J$ r9 x5 G! o- R5 A- X+ E(10)
vk=vk-1+(1/2)akT
(11)
5 y- g' y; e) y9 X# {注意，上述递推公式是分区适应的，即
J=
{
J, T∈[t0，t1]∪t6，t7
0, t∈(t1，t2)∪(t3，t4)∪(t5，t6)
-J, t∈[t2，t3]∪[t4，t5]
* Q3 A; v: J* C! `; @* S9 ]只要初始条件ak-1和vk-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为
5 l6 c6 o! r5 W+ \7 p∆Pik=
# O; Q* G; Q# ePi
∆Sk=Ki∆Sk
P
/ N8 O* B+ Q7 t6 o- S) i8 P区间的判别
段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t0-t1)的初始速度和初始加速度为0，则在t1时刻的位移Pti=(1/6)Jt13，其加速度a1=A=Jts，速度Vt1=(1/2)At12=(1/2)Ats，则
$ ^/ \/ A* N) x0 [# Bts=t1=A/J
$ w+ V! ?  v8 ~$ @) A' [(14)
" P4 o3 G" B3 n7 u; n) A# S由图1中的加速度图线可以看出
V=(1/2)Ats+Atl+(1/2)Ats=A(ts+tl)
* Q& Q" U5 F: z(15)
tl=(V/A)-(A/J)
& t  }( B4 p1 f: j* H(16)
! m6 S% G1 T; }* Dta=2ts+tl=(V/A)+(A/J)
( x; i2 K$ p( h3 ~5 Q  y# `; C(17)

图2 插补计算流程框图
# y3 a% @6 D* c0 F! g( W∆S=P-
! {6 B+ P! `2 |6 Lk
∆Sk
9 f9 E4 J; P. Q- L( j! |* ~. Q∑
1
6 @+ n$ J' B1 u  [* M. _: Y5 h∆Si=Pi-
" R( c3 @# z4 C, R3 G$ zk
∆Pik
' Q2 |4 e- {5 R, j1 P∑
1