def generate_frenet_path(c_d,c_d_d,c_d_dd,s0,c_s_d):
f_paths = []
for di in np.arange(-HALF_ROUND_WIDTH,HALF_ROUND_WIDTH+0.001,D_SAMPLE):
for Ti in np.arange(MIN_T,MAX_T,dt):
d_path = Frenet_Path()
lat_qp = QuinticPolynomial(c_d,c_d_d,c_d_dd,di,0,0,Ti) # d(t),五次多项式
d_path.t = [t for t in np.arange(0,Ti+dt,dt)]
d_path.d = [lat_qp.calc_point(t) for t in d_path.t]
d_path.d_d = [lat_qp.calc_first_derivative(t) for t in d_path.t]
d_path.d_dd = [lat_qp.calc_second_derivative(t) for t in d_path.t]
d_path.d_ddd = [lat_qp.calc_third_derivative(t) for t in d_path.t]
for s_d in np.arange(TARGET_SPEED-NUM_V*V_SAMPLE,TARGET_SPEED+NUM_V*V_SAMPLE,V_SAMPLE):
sd_path = copy.deepcopy(d_path) # d_path + s_path
lon_qp = Quartic_Polynomials(s0,c_s_d,0.0,s_d,0.0,Ti) # s(t),四次多项式
sd_path.s = [lon_qp.calc_positon(t) for t in sd_path.t]
sd_path.s_d = [lon_qp.calc_velocity(t) for t in sd_path.t]
sd_path.s_dd = [lon_qp.calc_accelerate(t) for t in sd_path.t]
sd_path.s_ddd = [lon_qp.calc_jerk(t) for t in sd_path.t]
Jd = sum(np.power(sd_path.d_ddd,2)) # square of jerk
Js = sum(np.power(sd_path.s_ddd,2)) # square of jerk
# square of diff from target speed
ds = (TARGET_SPEED-sd_path.s_d[-1])**2
sd_path.cd = K_J*Jd + K_T*Ti + K_D*sd_path.d[-1]**2
sd_path.cv = K_J*Js + K_T*Ti + K_D*ds
sd_path.cf = K_LAT*sd_path.cd + K_LON*sd_path.cv
f_paths.append(sd_path)
return f_paths
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
'''
将空间与时间离散化，横向采用五次多项式，纵向采用四次多项式生成不同Frenet坐标系下的轨迹
:param c_speed: current speed [m/s]
:param c_d: current lateral position [m]
:param c_d_d: current lateral speed [m/s]
:param c_d_dd: current lateral acceleration [m/s]
:param s0: current course position
:return: Frenet坐标系下的不同轨迹,轨迹包括[t,d,d_d,d_dd,d_ddd,s,s_s,s__ss,s_sss]以及成本函数[cd,cv,cf]
'''
frenet_paths = []
# generate path to each offset goal state space sampling！
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
# Lateral motion planning 用五次多项式插值
# Ti是每次规划的采样时间间隔，Ti属于[MIN_T,MAX_T]
for Ti in np.arange(MIN_T, MAX_T, DT):
fp = FrenetPath()
#lat_qp = QuarticPolynomial(c_d, c_d_d, 0, di, 0.0, Ti)
lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
# 时间序列
fp.t = [t for t in np.arange(0.0, Ti, DT)]
# 横向距离
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
# Longitudinal motion planning (Velocity keeping) 用四次多项式插值
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
tfp = copy.deepcopy(fp)
lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk
Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
# square of diff from target speed
ds = (TARGET_SPEED - tfp.s_d[-1])**2
# 损失函数，考虑舒适性（通过急动度jerk），横向误差等。但是公式目前还没看懂？
tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1]**2
tfp.cv = K_J * Js + K_T * Ti + K_D * ds
tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
frenet_paths.append(tfp)
return frenet_paths
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
frenet_paths = []
# generate path to each offset goal
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
# Lateral motion planning
for Ti in np.arange(MIN_T, MAX_T, DT):
fp = FrenetPath()
# calculate quintic polynomial from start position to (di,0,0) within Ti time
lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
# get time samples from 0 to Ti
fp.t = [t for t in np.arange(0.0, Ti, DT)]
# calculate trajectory point and their derivatives between start and goal position
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
# Longitudinal motion planning (Velocity keeping)
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
tfp = copy.deepcopy(fp)
# calculate quartic polynomial from start position to (tv,0) within Ti time, tv is velocity
lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk
Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
# square of diff from target speed
ds = (TARGET_SPEED - tfp.s_d[-1])**2
# cost function of lateral
tfp.cd = K_J * Jp + K_T * Ti + K_D * tfp.d[-1]**2
# cost function of longitudinal
tfp.cv = K_J * Js + K_T * Ti + K_D * ds
# sum cost function
tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv
frenet_paths.append(tfp)
return frenet_paths
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):
frenet_paths = []
# generate path to each offset goal
for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):
# Lateral motion planning
for Ti in np.arange(MINT, MAXT, DT):
fp = Frenet_path()
# lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
fp.t = [t for t in np.arange(0.0, Ti, DT)]
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
# Longitudinal motion planning (Velocity keeping)
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
tfp = copy.deepcopy(fp)
lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk
Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk
# square of diff from target speed
ds = (TARGET_SPEED - tfp.s_d[-1])**2
tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1]**2
tfp.cv = KJ * Js + KT * Ti + KD * ds
tfp.cf = KLAT * tfp.cd + KLON * tfp.cv
frenet_paths.append(tfp)
return frenet_paths
def calc_frenet_paths(c_speed, lateral_position, lateral_speed, acceleration, current_course_position):
frenet_paths = []
# meter of -road to road one by one
# calculate 하는 범위를 지정해 주는 곳 map_enable_lane
for di in np.arange(MAX_LEFT_WIDTH, MAX_RIGHT_WIDTH, D_ROAD_W):
# 횡방향 motion planning
for ti in np.arange(MIN_T, MAX_T, DT):
fp = FrenetPath() # Frenet Frame 초기 파라미터
# 미분할 계수들을 설정
lat_qp = QuinticPolynomial(lateral_position, lateral_speed, acceleration, di, 0.0, 0.0, ti)
fp.t = [t for t in np.arange(0.0, ti, DT)] # 시간 (sec)
fp.d = [lat_qp.calc_point(t) for t in fp.t] # 변위
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t] # 속도 (1차 미분)
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t] # 가속도 (2차 미분)
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t] # 가가속도 (3차 미분)
# 종방향 motion planning
for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE,
TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):
tfp = copy.deepcopy(fp) # 주소 복사가 아닌 데이터 복사로 fp가 변할때 같이 변하지 않도록 설정
lon_qp = QuarticPolynomial(current_course_position, c_speed, 0.0, tv, 0.0, ti)
# 위의 [시간 ~ 가가속도] 와 동일
tfp.s = [lon_qp.calc_point(t) for t in fp.t]
tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
# 가가속도 제곱의 합 구하기 Jerk
Jp = sum(np.power(tfp.d_ddd, 2)) # 횡
Js = sum(np.power(tfp.s_ddd, 2)) # 종
# 목표 Velocity와의 차의 제곱
ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2
# cost weight(상수: K_J, K_T, K_D)
tfp.cd = K_J * Jp + K_T * ti + K_D * tfp.d[-1] ** 2
tfp.cv = K_J * Js + K_T * ti + K_D * ds
tfp.cf = K_LAT * tfp.cd + K_LON * tfp.cv # Cost
frenet_paths.append(tfp)
return frenet_paths
def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0, Ti, di, di_d, tv,
path_params):
# # Parameter
DT = path_params['DT'] # time tick [s]
TARGET_SPEED = path_params['TARGET_SPEED']
# cost weights
K_J = path_params['K_J']
K_T = path_params['K_T']
K_D = path_params['K_D']
K_LAT = path_params['K_LAT']
K_LON = path_params['K_LON']
# Four parameters are sampled Ti, di, di_d, tv
fp = FrenetPath()
# lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)
lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, di_d, 0.0, Ti)
fp.t = [t for t in np.arange(0.0, Ti, DT)]
fp.d = [lat_qp.calc_point(t) for t in fp.t]
fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]
fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]
fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]
# Longitudinal motion planning (Velocity keeping)
lon_qp = QuarticPolynomial(s0, c_speed, 0.0, tv, 0.0, Ti)
fp.s = [lon_qp.calc_point(t) for t in fp.t]
fp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]
fp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]
fp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]
Jp = sum(np.power(fp.d_ddd, 2)) # square of jerk
Js = sum(np.power(fp.s_ddd, 2)) # square of jerk
# square of diff from target speed
ds = (TARGET_SPEED - fp.s_d[-1])**2
fp.cd = K_J * Jp + K_T * Ti + K_D * fp.d[-1]**2
fp.cv = K_J * Js + K_T * Ti + K_D * ds
fp.cf = K_LAT * fp.cd + K_LON * fp.cv
return fp