三角函数柔性加减速算法

公式推导

三段速度公式为

 
$$\begin{gathered} \{\begin{array}{l}v(t)=(v_s+v_m)/2-(v_m-v_s)cost{}^{\prime }/2t^{{}^{\prime }}=(t/t_1)\pi (1)\\ v(t)=v_m\\ v(t)=(v_e+v_m)/2-(v_e-v_m)cost{}^{\prime }/2t^{{}^{\prime }}=(t/t_2)\pi \end{array} \\ {t}_1为加速时间，t_2为减速时间 \end{gathered}$$

三段加速公式为

 
$$\{\begin{array}{l}a(t)=\frac{\pi }{2t_1}(v_m-v_s)sin{}^{\prime }/2t^{{}^{\prime }}=(t/t_1)\pi (2)\\ a(t)=0\\ a(t)=\frac{\pi }{2t_1}(v_e-v_m)sin{}^{\prime }/2t^{{}^{\prime }}=(t/t_2)\pi \end{array}$$

设A为系统的最大加速度，则
$$\begin{gathered} t_1\ge (v_m-v_s)\pi /(2A)(3) \\ {t}_2\ge (v_m-v_e)\pi /(2A)(4) \end{gathered}$$
考虑时间最优策略，取
$$\begin{gathered} t_1=(v_m-v_s)\pi /(2A)(5) \\ {t}_2=(v_m-v_e)\pi /(2A)(6) \end{gathered}$$

则得到加减速时间。

算法仿真

参数说明

S : 位 移 V m : 最 大 速 度 A m : 最 大 加 速 度 V s : 起 始 速 度 V e : 终 止 速 度 \begin{aligned} &S :位移\\ &V_m :最大速度\\ &A_m :最大加速度\\ &V_s :起始速度\\ &V_e :终止速度\\ \end{aligned} ​S:位移Vm​:最大速度Am​:最大加速度Vs​:起始速度Ve​:终止速度​

S足够大时，速度曲线包含加速、匀速、减速

S = 80 V m = 150 A m = 1000 V s = 50 V e = 40 \begin{aligned} &S =80\\ &V_m =150\\ &A_m =1000\\ &V_s =50\\ &V_e =40\\ \end{aligned} ​S=80Vm​=150Am​=1000Vs​=50Ve​=40​

S不够大，速度曲线只包含加速、减速

S = 10 V m = 500 A m = 1000 V s = 50 V e = 40 \begin{aligned}&S =10\\&V_m =500\\&A_m =1000\\&V_s =50\\&V_e =40\\\end{aligned} ​S=10Vm​=500Am​=1000Vs​=50Ve​=40​

S很小，速度曲线只包含加速或者减速

S = 1 V m = 500 A m = 1000 V s = 60 V e = 40 \begin{aligned}&S =1\\&V_m =500\\&A_m =1000\\&V_s =60\\&V_e =40\\\end{aligned} ​S=1Vm​=500Am​=1000Vs​=60Ve​=40​

代码如下，欢迎交流

import numpy as np
import matplotlib.pyplot as pltVm = 500
Am = 1000
Jm = 5000
S = 1
Vs = 60
Ve = 40tma = np.pi*(Vm-Vs)/(2*Am)
tmd = np.pi*(Vm-Ve)/(2*Am)
Sa = (Vm+Vs)/2*tma
Sd = (Vm+Ve)/2*tmd
Sall = Sa + Sdif Sall > S: tmd = np.pi*(Vs-Ve)/(2*Am)Sall = (Vs + Ve) / 2 * tmdif Sall > S:Vs = (4*Am*S/np.pi+Ve**2)**0.5Vm = Vselse:Vm = (2*Am*S/np.pi+Vs**2/2+Ve**2/2)**0.5time = []
dataV = []
dataA = []
tma = np.pi*(Vm-Vs)/(2*Am)
tmd = np.pi*(Vm-Ve)/(2*Am)
Sa = (Vm+Vs)/2*tma
Sd = (Vm+Ve)/2*tmd
Sall = Sa + Sd
tmc = 0for t in np.arange(0,tma,0.001):time.append(t)dt = t*np.pi/tmav = (Vs+Vm)/2 - (Vm-Vs)/2*np.cos(dt)a = np.pi/(2*tma)*(Vm-Vs)*np.sin(dt)dataV.append(v)dataA.append(a)if Sall <= S:tmc = (S - Sa - Sd)/Vmtime.append(tmc + tma)dataV.append(Vm)dataA.append(0)for t in np.arange(0,tmd,0.001):time.append(t+tmc+tma)dt = t * np.pi / tmdv = (Ve+Vm)/2 - (Ve-Vm)/2*np.cos(dt)a = np.pi / (2 * tmd) * (Ve - Vm) * np.sin(dt)dataV.append(v)dataA.append(a)time.append(tmd+tmc+tma)
dt = tmd * np.pi / tmd
v = (Ve+Vm)/2 - (Ve-Vm)/2*np.cos(dt)
a = np.pi / (2 * tmd) * (Ve - Vm) * np.sin(dt)
dataV.append(v)
dataA.append(a)plt.subplot(2,1,1)
plt.plot(time, dataV,label="vel")
plt.legend()
plt.subplot(2,1,2)
plt.plot(time, dataA,label="acc")
plt.legend()
plt.show()