from re import X
import numpy as np
import matplotlib.pyplot as plt
from scipy import integrate
import matplotlib.pyplot as plt
import numpy as np
q0 = 0
q1 = 10
vmax =8
max =5
v0 = 0
v1 = 0
amax = 30
jmax = 30
count=0
#加速段
if (vmax -v0)*jmax < amax**2:
print("pass")
if v0>vmax:
Tj1 = 0
Ta = 0
alima = 0
else:
Tj1= np.sqrt((vmax - v0)/jmax)
Ta = 2*Tj1
alima = Tj1 * jmax
print(Tj1)
else:
Tj1 = amax/jmax
print("pass1")
Ta = Tj1 + (vmax-v0)/amax
alima = amax
#第三段 减速
if (vmax -v1)*jmax < amax**2:
Tj2 = np.sqrt((vmax - v1)/jmax)
Td = 2*Tj1
alimd = Tj2* jmax
else:
Tj2 = amax/jmax
Td = Tj2 + (vmax-v1)/amax
alimd = amax
Tv = (q1 - q0)/vmax - Ta/2*(1+v0/vmax) - Td/2*(1+v1/vmax)##匀速段时间
T = Tv + Ta + Td#总时间
print(Tv)
p = []
vc = []
ac = []
jc = []
if Tv > 0:
vlim = vmax
T = Tv + Ta + Td
print(T)
time=np.arange(0, T, 0.01)
for t in time:
# 加速段轨迹
if t>=0 and t<Tj1:
v = v0 + jmax*t**2/2
vc.append(v)
a = jmax*t
ac.append(a)
elif t>= Tj1 and t<(Ta - Tj1):
v = v0 + alima*(t - Tj1/2)
vc.append(v)
a = alima
ac.append(a)
elif t>=(Ta - Tj1) and t < Ta:
v = vlim - jmax*(Ta - t)**2/2
vc.append(v)
a = jmax*(Ta - t)
ac.append(a)
elif t>=Ta and t<(Ta + Tv):
v = vlim
vc.append(v)
a = 0
ac.append(a)
print(len(ac))
print(len(time))
plt.plot(np.arange(0, 12.5, 0.1),ac)
plt.show()
纯跟踪 pure pursuit 算法
于 2023-08-19 18:12:06 首次发布