纯跟踪 pure pursuit 算法

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