三角函数柔性加减速算法

公式推导

 

三段速度公式为

 
$$\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 plt

Vm = 500
Am = 1000
Jm = 5000
S = 1
Vs = 60
Ve = 40

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

if Sall > S: 
    tmd = np.pi*(Vs-Ve)/(2*Am)
    Sall = (Vs + Ve) / 2 * tmd
    if Sall > S:
        Vs = (4*Am*S/np.pi+Ve**2)**0.5
        Vm = Vs
    else:
        Vm = (2*Am*S/np.pi+Vs**2/2+Ve**2/2)**0.5


time = []
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 = 0

for t in np.arange(0,tma,0.001):
    time.append(t)
    dt = t*np.pi/tma
    v = (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)/Vm
    time.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 / 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)

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()