Frenet坐标系相关知识系统学习
一、Frenet坐标系简介
二、Frenet坐标系与全局坐标系的转换
可以基于Frenet坐标系，报据自动驾驶车辆的始末状态，利用五次多项式建立自动驾驶车辆轨迹规划模型，并建立各个场景下的轨迹质量评估函数。
三、深入学习
《硕士论文-基于Frenet坐标系采样的自动驾驶轨迹规划算法研究》
《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame》
《无人驾驶汽车系统入门（二十一）——基于Frenet优化轨迹的无人车动作规划方法》
《Apollo项目坐标系研究》
《第三期 预测——Frenet 坐标》
维基百科：Frenet–Serret formulas
四、代码学习
Trajectory Planning in the Frenet Space
------基于论文《Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame》
链接：Trajectory Planning in the Frenet Space - fjp.github.io
代码
-
''' -
https://fjp.at/posts/optimal-frenet/ -
http://fileadmin.cs.lth.se/ai/Proceedings/ICRA2010/MainConference/data/papers/1650.pdf -
https://blog.csdn.net/AdamShan/article/details/80779615 -
''' -
import pdb -
import time -
import pylab as pl -
from IPython import display -
import matplotlib -
import numpy as np -
import matplotlib.pyplot as plt -
import copy -
import math -
from cubic_spline_planner import * -
class quintic_polynomial: -
def __init__(self, xs, vxs, axs, xe, vxe, axe, T): -
# calc coefficient of quintic polynomial -
self.xs = xs -
self.vxs = vxs -
self.axs = axs -
self.xe = xe -
self.vxe = vxe -
self.axe = axe -
self.a0 = xs -
self.a1 = vxs -
self.a2 = axs / 2.0 -
A = np.array([[T ** 3, T ** 4, T ** 5], -
[3 * T ** 2, 4 * T ** 3, 5 * T ** 4], -
[6 * T, 12 * T ** 2, 20 * T ** 3]]) -
b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T ** 2, -
vxe - self.a1 - 2 * self.a2 * T, -
axe - 2 * self.a2]) -
x = np.linalg.solve(A, b) -
self.a3 = x[0] -
self.a4 = x[1] -
self.a5 = x[2] -
def calc_point(self, t): -
xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ -
self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5 -
return xt -
def calc_first_derivative(self, t): -
xt = self.a1 + 2 * self.a2 * t + \ -
3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4 -
return xt -
def calc_second_derivative(self, t): -
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3 -
return xt -
def calc_third_derivative(self, t): -
xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2 -
return xt -
class quartic_polynomial: -
def __init__(self, xs, vxs, axs, vxe, axe, T): -
# calc coefficient of quintic polynomial -
self.xs = xs -
self.vxs = vxs -
self.axs = axs -
self.vxe = vxe -
self.axe = axe -
self.a0 = xs -
self.a1 = vxs -
self.a2 = axs / 2.0 -
A = np.array([[3 * T ** 2, 4 * T ** 3], -
[6 * T, 12 * T ** 2]]) -
b = np.array([vxe - self.a1 - 2 * self.a2 * T, -
axe - 2 * self.a2]) -
x = np.linalg.solve(A, b) -
self.a3 = x[0] -
self.a4 = x[1] -
def calc_point(self, t): -
xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ -
self.a3 * t ** 3 + self.a4 * t ** 4 -
return xt -
def calc_first_derivative(self, t): -
xt = self.a1 + 2 * self.a2 * t + \ -
3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 -
return xt -
def calc_second_derivative(self, t): -
xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 -
return xt -
def calc_third_derivative(self, t): -
xt = 6 * self.a3 + 24 * self.a4 * t -
return xt -
class Frenet_path: -
def __init__(self): -
self.t = [] -
self.d = [] -
self.d_d = [] -
self.d_dd = [] -
self.d_ddd = [] -
self.s = [] -
self.s_d = [] -
self.s_dd = [] -
self.s_ddd = [] -
self.cd = 0.0 -
self.cv = 0.0 -
self.cf = 0.0 -
self.x = [] -
self.y = [] -
self.yaw = [] -
self.ds = [] -
self.c = [] -
# Parameter -
MAX_SPEED = 50.0 / 3.6 # maximum speed [m/s] -
MAX_ACCEL = 2.0 # maximum acceleration [m/ss] -
MAX_CURVATURE = 1.0 # maximum curvature [1/m] -
MAX_ROAD_WIDTH = 7.0 # maximum road width [m] -
D_ROAD_W = 1.0 # road width sampling length [m] -
DT = 0.2 # time tick [s] -
MAXT = 5.0 # max prediction time [m] -
MINT = 4.0 # min prediction time [m] -
TARGET_SPEED = 30.0 / 3.6 # target speed [m/s] -
D_T_S = 5.0 / 3.6 # target speed sampling length [m/s] -
N_S_SAMPLE = 1 # sampling number of target speed -
ROBOT_RADIUS = 2.0 # robot radius [m] -
# cost weights -
KJ = 0.1 -
KT = 0.1 -
KD = 1.0 -
KLAT = 1.0 -
KLON = 1.0 -
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): -
print('di={0},Ti={1}'.format(di,Ti)) -
fp = Frenet_path() -
lat_qp = quintic_polynomial(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] -
# Loongitudinal 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) #not tfp=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 -
faTrajX = [] -
faTrajY = [] -
def calc_global_paths(fplist, csp): -
# faTrajX = [] -
# faTrajY = [] -
for fp in fplist: -
# calc global positions -
for i in range(len(fp.s)): -
ix, iy = csp.calc_position(fp.s[i]) -
if ix is None: -
break -
iyaw = csp.calc_yaw(fp.s[i]) -
di = fp.d[i] -
fx = ix + di * math.cos(iyaw + math.pi / 2.0) -
fy = iy + di * math.sin(iyaw + math.pi / 2.0) -
fp.x.append(fx) -
fp.y.append(fy) -
# Just for plotting -
faTrajX.append(fp.x) -
faTrajY.append(fp.y) -
# calc yaw and ds -
for i in range(len(fp.x) - 1): -
dx = fp.x[i + 1] - fp.x[i] -
dy = fp.y[i + 1] - fp.y[i] -
fp.yaw.append(math.atan2(dy, dx)) -
fp.ds.append(math.sqrt(dx ** 2 + dy ** 2)) -
fp.yaw.append(fp.yaw[-1]) -
fp.ds.append(fp.ds[-1]) -
# calc curvature -
for i in range(len(fp.yaw) - 1): -
fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i]) -
return fplist -
faTrajCollisionX = [] -
faTrajCollisionY = [] -
faObCollisionX = [] -
faObCollisionY = [] -
def check_collision(fp, ob): -
# pdb.set_trace() -
for i in range(len(ob[:, 0])): -
# Calculate the distance for each trajectory point to the current object -
d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2) -
for (ix, iy) in zip(fp.x, fp.y)] -
# Check if any trajectory point is too close to the object using the robot radius -
collision = any([di <= ROBOT_RADIUS ** 2 for di in d]) -
if collision: -
# plot(ft.x, ft.y, 'rx') -
faTrajCollisionX.append(fp.x) -
faTrajCollisionY.append(fp.y) -
# plot(ox, oy, 'yo'); -
# pdb.set_trace() -
if ob[i, 0] not in faObCollisionX or ob[i, 1] not in faObCollisionY: -
faObCollisionX.append(ob[i, 0]) -
faObCollisionY.append(ob[i, 1]) -
return True -
return False -
# faTrajOkX = [] -
# faTrajOkY = [] -
def check_paths(fplist, ob): -
okind = [] -
for i in range(len(fplist)): -
if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check -
continue -
elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check -
continue -
elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check -
continue -
elif check_collision(fplist[i], ob): -
continue -
okind.append(i) -
return [fplist[i] for i in okind] -
fpplist = [] -
def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob): -
# pdb.set_trace() -
fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0) -
fplist = calc_global_paths(fplist, csp) -
fplist = check_paths(fplist, ob) -
# fpplist = deepcopy(fplist) -
fpplist.extend(fplist) -
# find minimum cost path -
mincost = float("inf") -
bestpath = None -
for fp in fplist: -
if mincost >= fp.cf: -
mincost = fp.cf -
bestpath = fp -
return bestpath -
from cubic_spline_planner import * -
def generate_target_course(x, y): -
csp = Spline2D(x, y) -
s = np.arange(0, csp.s[-1], 0.1) -
rx, ry, ryaw, rk = [], [], [], [] -
for i_s in s: -
ix, iy = csp.calc_position(i_s) -
rx.append(ix) -
ry.append(iy) -
ryaw.append(csp.calc_yaw(i_s)) -
rk.append(csp.calc_curvature(i_s)) -
return rx, ry, ryaw, rk, csp -
show_animation = True -
# show_animation = False -
# way points -
wx = [0.0, 10.0, 20.5, 35.0, 70.5] -
wy = [0.0, -6.0, 5.0, 6.5, 0.0] -
# obstacle lists -
ob = np.array([[20.0, 10.0], -
[30.0, 6.0], -
[30.0, 8.0], -
[35.0, 8.0], -
[50.0, 3.0] -
]) -
tx, ty, tyaw, tc, csp = generate_target_course(wx, wy) -
# initial state -
c_speed = 10.0 / 3.6 # current speed [m/s] -
c_d = 2.0 # current lateral position [m] -
c_d_d = 0.0 # current lateral speed [m/s] -
c_d_dd = 0.0 # current latral acceleration [m/s] -
s0 = 0.0 # current course position -
area = 20.0 # animation area length [m] -
fig = plt.figure() -
plt.ion() -
faTx = tx -
faTy = ty -
faObx = ob[:, 0] -
faOby = ob[:, 1] -
faPathx = [] -
faPathy = [] -
faRobotx = [] -
faRoboty = [] -
faSpeed = [] -
for i in range(100): -
path = frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob) -
s0 = path.s[1] -
c_d = path.d[1] -
c_d_d = path.d_d[1] -
c_d_dd = path.d_dd[1] -
c_speed = path.s_d[1] -
if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0: -
print("Goal") -
break -
faPathx.append(path.x[1:]) -
faPathy.append(path.y[1:]) -
faRobotx.append(path.x[1]) -
faRoboty.append(path.y[1]) -
faSpeed.append(c_speed) -
if show_animation: -
plt.cla() -
plt.plot(tx, ty, animated=True) -
plt.plot(ob[:, 0], ob[:, 1], "xk") -
plt.plot(tx,ty,'-',color='crimson') -
plt.plot(path.x[1], path.y[1], "vc") -
for (ix, iy) in zip(faTrajX, faTrajY): -
# pdb.set_trace() -
plt.plot(ix[1:], iy[1:], '-', color=[0.5, 0.5, 0.5]) -
faTrajX = [] -
faTrajY = [] -
for (ix, iy) in zip(faTrajCollisionX, faTrajCollisionY): -
# pdb.set_trace() -
plt.plot(ix[1:], iy[1:], 'rx') -
faTrajCollisionX = [] -
faTrajCollisionY = [] -
# pdb.set_trace() -
for fp in fpplist: -
# pdb.set_trace() -
plt.plot(fp.x[1:], fp.y[1:], '-g') -
fpplist = [] -
# pdb.set_trace() -
for (ix, iy) in zip(faObCollisionX, faObCollisionY): -
# pdb.set_trace() -
plt.plot(ix, iy, 'oy') -
faObCollisionX = [] -
faObCollisionY = [] -
plt.plot(path.x[1:], path.y[1:], "-ob") -
print('len:{}'.format(len(path.x[1:]))) -
plt.xlim(path.x[1] - area, path.x[-1] + area) -
plt.ylim(path.y[1] - area, path.y[-1] + area) -
plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4]) -
plt.grid(True) -
plt.pause(0.00001) -
plt.show() -
# display.clear_output(wait=True) -
# display.display(pl.gcf()) -
plt.pause(0.1) -
print("Finish") -
plt.ioff()
运行结果（片段）
(73条消息) Frenet坐标系相关知识系统学习_David's Tweet-CSDN博客_frenet坐标系