2 多轴联动的线性插补S加减速

1. 各轴的运动参量成比例
   对已规划的合成速度v(t)在t_e点幂级数展开，令t=t-t_x，有 v(t)=v(t_x)+a(t_x)t+½J(t_e)t²(4)同理，各插补轴对应．点速度 v_i(t)=v_i(t_e)+a_i(t_e)t+½J_i(t_x)t²F      i=1, 2, …, n(5)
   根据线性插补原理，合成速度与各插补轴速度有下列比例关系： v_i(t)=K_iv(t)F      i=1, 2, …, n(6)
   对于上面恒等式，应有 v_i(t_x)=K_iv(t_x), a_i(t_x)=K_ia(t_x), J_i(t_x)=K_iJ(t_x)(7)
   由于t_x为任意一点，此式表明段内加减速过程中各插补轴的速度、加速度和Jerk分别与合成的速度、加速度和Jerk对应成比例。当对合成速度按S曲线规划时，各插补轴在保证空间轨迹的同时，也按S曲线进行加减速，即S曲线加减速可用于前加减速控制。同时，上述关系可用于各插补轴的速度、加速度和Jerk的极限值检查。
2. S加减速的插补递推公式
   设插补周期为T，则在第k个插补周期结束时的合成位移S_k为 S_k=∫^t_kv(t)dt=∫^t_k-1v(t)dt+∫^t_k-1+Tv(t)dt=S_k-1∫^t(v_k-1+a_k-1t+½Jt²)dt=S_k-1+v_k-1T+½a_k-1T²+(1/6)JT³_00tk-10(8)
   第k个插补周期内的合成位移增量为 S_k=v_k-1T+(1/2)a_k-1T²+(1/6)JT³=v_k-1T+(1/2)(a_k-1+(1/3)JT)T²=v_k-1+(1/2)a_kT²=(v_k-1+(1/2)a_kT)=v_kT(9)a_k=a_k-1+(1/3)JT(10)v_k=v_k-1+(1/2)a_kT(11)注意，上述递推公式是分区适应的，即 J={J, T∈[t₀，t₁]∪t₆，t₇0, t∈(t₁，t₂)∪(t₃，t₄)∪(t₅，t₆)-J, t∈[t₂，t₃]∪[t₄，t₅](12)
   只要初始条件a_k-1和v_k-1给定，则可推导出各插补周期的合成位移增量。进而得到各插补轴在插补周期内的位移增量，其公式为 P_i^k=P_iS_k=K_iS_kP(13)
区间的判别
段内加减速时，每程序段伺服电动机速度总要减到零后再执行下一程序段。因此其加速段和减速段的位移相等，见图1。
1区(t₀-t₁)的初始速度和初始加速度为0，则在t₁时刻的位移P_ti=(1/6)Jt₁³，其加速度a₁=A=Jt_s，速度Vt₁=(1/2)At₁²=(1/2)At_s，则 t_s=t₁=A/J(14)
由图1中的加速度图线可以看出 V=(1/2)At_s+At_l+(1/2)At_s=A(t_s+t_l)(15)
则 t_l=(V/A)-(A/J)(16)t_a=2t_s+t_l=(V/A)+(A/J)(17)由t_s、t_l、t_m可计算出t₀～t₇的时间，并以此进行区间判别。
终点判别
终点距离S及各轴距终点距离S_i


图2 插补计算流程框图
S=P-kS_k∑1(18)S_i=P_i-kP_i^k∑1(19)

3 算法描述

1. 读入A、J、V、P_i。
2. 计算k_i，计算出的各轴的速度和加速度将与各轴的极限值比较，如果超出极限值，修正V。
3. 扫描速度倍率K，令V=KV。
4. 初始化，t₀=O，S_i=P_i。
5. 速度规划，计算t_l、t_s、t_m。
6. 置速度规划完成标志。
7. 插补。

4 结语