当前位置:   article > 正文

Frenet坐标系相关知识系统学习

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 

代码

 
  1. '''

  2. https://fjp.at/posts/optimal-frenet/

  3. http://fileadmin.cs.lth.se/ai/Proceedings/ICRA2010/MainConference/data/papers/1650.pdf

  4. https://blog.csdn.net/AdamShan/article/details/80779615

  5. '''

  6. import pdb

  7. import time

  8. import pylab as pl

  9. from IPython import display

  10. import matplotlib

  11. import numpy as np

  12. import matplotlib.pyplot as plt

  13. import copy

  14. import math

  15. from cubic_spline_planner import *

  16. class quintic_polynomial:

  17. def __init__(self, xs, vxs, axs, xe, vxe, axe, T):

  18. # calc coefficient of quintic polynomial

  19. self.xs = xs

  20. self.vxs = vxs

  21. self.axs = axs

  22. self.xe = xe

  23. self.vxe = vxe

  24. self.axe = axe

  25. self.a0 = xs

  26. self.a1 = vxs

  27. self.a2 = axs / 2.0

  28. A = np.array([[T ** 3, T ** 4, T ** 5],

  29. [3 * T ** 2, 4 * T ** 3, 5 * T ** 4],

  30. [6 * T, 12 * T ** 2, 20 * T ** 3]])

  31. b = np.array([xe - self.a0 - self.a1 * T - self.a2 * T ** 2,

  32. vxe - self.a1 - 2 * self.a2 * T,

  33. axe - 2 * self.a2])

  34. x = np.linalg.solve(A, b)

  35. self.a3 = x[0]

  36. self.a4 = x[1]

  37. self.a5 = x[2]

  38. def calc_point(self, t):

  39. xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \

  40. self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5

  41. return xt

  42. def calc_first_derivative(self, t):

  43. xt = self.a1 + 2 * self.a2 * t + \

  44. 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4

  45. return xt

  46. def calc_second_derivative(self, t):

  47. xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3

  48. return xt

  49. def calc_third_derivative(self, t):

  50. xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2

  51. return xt

  52. class quartic_polynomial:

  53. def __init__(self, xs, vxs, axs, vxe, axe, T):

  54. # calc coefficient of quintic polynomial

  55. self.xs = xs

  56. self.vxs = vxs

  57. self.axs = axs

  58. self.vxe = vxe

  59. self.axe = axe

  60. self.a0 = xs

  61. self.a1 = vxs

  62. self.a2 = axs / 2.0

  63. A = np.array([[3 * T ** 2, 4 * T ** 3],

  64. [6 * T, 12 * T ** 2]])

  65. b = np.array([vxe - self.a1 - 2 * self.a2 * T,

  66. axe - 2 * self.a2])

  67. x = np.linalg.solve(A, b)

  68. self.a3 = x[0]

  69. self.a4 = x[1]

  70. def calc_point(self, t):

  71. xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \

  72. self.a3 * t ** 3 + self.a4 * t ** 4

  73. return xt

  74. def calc_first_derivative(self, t):

  75. xt = self.a1 + 2 * self.a2 * t + \

  76. 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3

  77. return xt

  78. def calc_second_derivative(self, t):

  79. xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2

  80. return xt

  81. def calc_third_derivative(self, t):

  82. xt = 6 * self.a3 + 24 * self.a4 * t

  83. return xt

  84. class Frenet_path:

  85. def __init__(self):

  86. self.t = []

  87. self.d = []

  88. self.d_d = []

  89. self.d_dd = []

  90. self.d_ddd = []

  91. self.s = []

  92. self.s_d = []

  93. self.s_dd = []

  94. self.s_ddd = []

  95. self.cd = 0.0

  96. self.cv = 0.0

  97. self.cf = 0.0

  98. self.x = []

  99. self.y = []

  100. self.yaw = []

  101. self.ds = []

  102. self.c = []

  103. # Parameter

  104. MAX_SPEED = 50.0 / 3.6 # maximum speed [m/s]

  105. MAX_ACCEL = 2.0 # maximum acceleration [m/ss]

  106. MAX_CURVATURE = 1.0 # maximum curvature [1/m]

  107. MAX_ROAD_WIDTH = 7.0 # maximum road width [m]

  108. D_ROAD_W = 1.0 # road width sampling length [m]

  109. DT = 0.2 # time tick [s]

  110. MAXT = 5.0 # max prediction time [m]

  111. MINT = 4.0 # min prediction time [m]

  112. TARGET_SPEED = 30.0 / 3.6 # target speed [m/s]

  113. D_T_S = 5.0 / 3.6 # target speed sampling length [m/s]

  114. N_S_SAMPLE = 1 # sampling number of target speed

  115. ROBOT_RADIUS = 2.0 # robot radius [m]

  116. # cost weights

  117. KJ = 0.1

  118. KT = 0.1

  119. KD = 1.0

  120. KLAT = 1.0

  121. KLON = 1.0

  122. def calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0):

  123. frenet_paths = []

  124. # generate path to each offset goal

  125. for di in np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W):

  126. # Lateral motion planning

  127. for Ti in np.arange(MINT, MAXT, DT):

  128. print('di={0},Ti={1}'.format(di,Ti))

  129. fp = Frenet_path()

  130. lat_qp = quintic_polynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, Ti)

  131. fp.t = [t for t in np.arange(0.0, Ti, DT)]

  132. fp.d = [lat_qp.calc_point(t) for t in fp.t]

  133. fp.d_d = [lat_qp.calc_first_derivative(t) for t in fp.t]

  134. fp.d_dd = [lat_qp.calc_second_derivative(t) for t in fp.t]

  135. fp.d_ddd = [lat_qp.calc_third_derivative(t) for t in fp.t]

  136. # Loongitudinal motion planning (Velocity keeping)

  137. for tv in np.arange(TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S):

  138. tfp = copy.deepcopy(fp) #not tfp=fp

  139. lon_qp = quartic_polynomial(s0, c_speed, 0.0, tv, 0.0, Ti)

  140. tfp.s = [lon_qp.calc_point(t) for t in fp.t]

  141. tfp.s_d = [lon_qp.calc_first_derivative(t) for t in fp.t]

  142. tfp.s_dd = [lon_qp.calc_second_derivative(t) for t in fp.t]

  143. tfp.s_ddd = [lon_qp.calc_third_derivative(t) for t in fp.t]

  144. Jp = sum(np.power(tfp.d_ddd, 2)) # square of jerk

  145. Js = sum(np.power(tfp.s_ddd, 2)) # square of jerk

  146. # square of diff from target speed

  147. ds = (TARGET_SPEED - tfp.s_d[-1]) ** 2

  148. tfp.cd = KJ * Jp + KT * Ti + KD * tfp.d[-1] ** 2

  149. tfp.cv = KJ * Js + KT * Ti + KD * ds

  150. tfp.cf = KLAT * tfp.cd + KLON * tfp.cv

  151. frenet_paths.append(tfp)

  152. return frenet_paths

  153. faTrajX = []

  154. faTrajY = []

  155. def calc_global_paths(fplist, csp):

  156. # faTrajX = []

  157. # faTrajY = []

  158. for fp in fplist:

  159. # calc global positions

  160. for i in range(len(fp.s)):

  161. ix, iy = csp.calc_position(fp.s[i])

  162. if ix is None:

  163. break

  164. iyaw = csp.calc_yaw(fp.s[i])

  165. di = fp.d[i]

  166. fx = ix + di * math.cos(iyaw + math.pi / 2.0)

  167. fy = iy + di * math.sin(iyaw + math.pi / 2.0)

  168. fp.x.append(fx)

  169. fp.y.append(fy)

  170. # Just for plotting

  171. faTrajX.append(fp.x)

  172. faTrajY.append(fp.y)

  173. # calc yaw and ds

  174. for i in range(len(fp.x) - 1):

  175. dx = fp.x[i + 1] - fp.x[i]

  176. dy = fp.y[i + 1] - fp.y[i]

  177. fp.yaw.append(math.atan2(dy, dx))

  178. fp.ds.append(math.sqrt(dx ** 2 + dy ** 2))

  179. fp.yaw.append(fp.yaw[-1])

  180. fp.ds.append(fp.ds[-1])

  181. # calc curvature

  182. for i in range(len(fp.yaw) - 1):

  183. fp.c.append((fp.yaw[i + 1] - fp.yaw[i]) / fp.ds[i])

  184. return fplist

  185. faTrajCollisionX = []

  186. faTrajCollisionY = []

  187. faObCollisionX = []

  188. faObCollisionY = []

  189. def check_collision(fp, ob):

  190. # pdb.set_trace()

  191. for i in range(len(ob[:, 0])):

  192. # Calculate the distance for each trajectory point to the current object

  193. d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2)

  194. for (ix, iy) in zip(fp.x, fp.y)]

  195. # Check if any trajectory point is too close to the object using the robot radius

  196. collision = any([di <= ROBOT_RADIUS ** 2 for di in d])

  197. if collision:

  198. # plot(ft.x, ft.y, 'rx')

  199. faTrajCollisionX.append(fp.x)

  200. faTrajCollisionY.append(fp.y)

  201. # plot(ox, oy, 'yo');

  202. # pdb.set_trace()

  203. if ob[i, 0] not in faObCollisionX or ob[i, 1] not in faObCollisionY:

  204. faObCollisionX.append(ob[i, 0])

  205. faObCollisionY.append(ob[i, 1])

  206. return True

  207. return False

  208. # faTrajOkX = []

  209. # faTrajOkY = []

  210. def check_paths(fplist, ob):

  211. okind = []

  212. for i in range(len(fplist)):

  213. if any([v > MAX_SPEED for v in fplist[i].s_d]): # Max speed check

  214. continue

  215. elif any([abs(a) > MAX_ACCEL for a in fplist[i].s_dd]): # Max accel check

  216. continue

  217. elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check

  218. continue

  219. elif check_collision(fplist[i], ob):

  220. continue

  221. okind.append(i)

  222. return [fplist[i] for i in okind]

  223. fpplist = []

  224. def frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob):

  225. # pdb.set_trace()

  226. fplist = calc_frenet_paths(c_speed, c_d, c_d_d, c_d_dd, s0)

  227. fplist = calc_global_paths(fplist, csp)

  228. fplist = check_paths(fplist, ob)

  229. # fpplist = deepcopy(fplist)

  230. fpplist.extend(fplist)

  231. # find minimum cost path

  232. mincost = float("inf")

  233. bestpath = None

  234. for fp in fplist:

  235. if mincost >= fp.cf:

  236. mincost = fp.cf

  237. bestpath = fp

  238. return bestpath

  239. from cubic_spline_planner import *

  240. def generate_target_course(x, y):

  241. csp = Spline2D(x, y)

  242. s = np.arange(0, csp.s[-1], 0.1)

  243. rx, ry, ryaw, rk = [], [], [], []

  244. for i_s in s:

  245. ix, iy = csp.calc_position(i_s)

  246. rx.append(ix)

  247. ry.append(iy)

  248. ryaw.append(csp.calc_yaw(i_s))

  249. rk.append(csp.calc_curvature(i_s))

  250. return rx, ry, ryaw, rk, csp

  251. show_animation = True

  252. # show_animation = False

  253. # way points

  254. wx = [0.0, 10.0, 20.5, 35.0, 70.5]

  255. wy = [0.0, -6.0, 5.0, 6.5, 0.0]

  256. # obstacle lists

  257. ob = np.array([[20.0, 10.0],

  258. [30.0, 6.0],

  259. [30.0, 8.0],

  260. [35.0, 8.0],

  261. [50.0, 3.0]

  262. ])

  263. tx, ty, tyaw, tc, csp = generate_target_course(wx, wy)

  264. # initial state

  265. c_speed = 10.0 / 3.6 # current speed [m/s]

  266. c_d = 2.0 # current lateral position [m]

  267. c_d_d = 0.0 # current lateral speed [m/s]

  268. c_d_dd = 0.0 # current latral acceleration [m/s]

  269. s0 = 0.0 # current course position

  270. area = 20.0 # animation area length [m]

  271. fig = plt.figure()

  272. plt.ion()

  273. faTx = tx

  274. faTy = ty

  275. faObx = ob[:, 0]

  276. faOby = ob[:, 1]

  277. faPathx = []

  278. faPathy = []

  279. faRobotx = []

  280. faRoboty = []

  281. faSpeed = []

  282. for i in range(100):

  283. path = frenet_optimal_planning(csp, s0, c_speed, c_d, c_d_d, c_d_dd, ob)

  284. s0 = path.s[1]

  285. c_d = path.d[1]

  286. c_d_d = path.d_d[1]

  287. c_d_dd = path.d_dd[1]

  288. c_speed = path.s_d[1]

  289. if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0:

  290. print("Goal")

  291. break

  292. faPathx.append(path.x[1:])

  293. faPathy.append(path.y[1:])

  294. faRobotx.append(path.x[1])

  295. faRoboty.append(path.y[1])

  296. faSpeed.append(c_speed)

  297. if show_animation:

  298. plt.cla()

  299. plt.plot(tx, ty, animated=True)

  300. plt.plot(ob[:, 0], ob[:, 1], "xk")

  301. plt.plot(tx,ty,'-',color='crimson')

  302. plt.plot(path.x[1], path.y[1], "vc")

  303. for (ix, iy) in zip(faTrajX, faTrajY):

  304. # pdb.set_trace()

  305. plt.plot(ix[1:], iy[1:], '-', color=[0.5, 0.5, 0.5])

  306. faTrajX = []

  307. faTrajY = []

  308. for (ix, iy) in zip(faTrajCollisionX, faTrajCollisionY):

  309. # pdb.set_trace()

  310. plt.plot(ix[1:], iy[1:], 'rx')

  311. faTrajCollisionX = []

  312. faTrajCollisionY = []

  313. # pdb.set_trace()

  314. for fp in fpplist:

  315. # pdb.set_trace()

  316. plt.plot(fp.x[1:], fp.y[1:], '-g')

  317. fpplist = []

  318. # pdb.set_trace()

  319. for (ix, iy) in zip(faObCollisionX, faObCollisionY):

  320. # pdb.set_trace()

  321. plt.plot(ix, iy, 'oy')

  322. faObCollisionX = []

  323. faObCollisionY = []

  324. plt.plot(path.x[1:], path.y[1:], "-ob")

  325. print('len:{}'.format(len(path.x[1:])))

  326. plt.xlim(path.x[1] - area, path.x[-1] + area)

  327. plt.ylim(path.y[1] - area, path.y[-1] + area)

  328. plt.title("v[km/h]:" + str(c_speed * 3.6)[0:4])

  329. plt.grid(True)

  330. plt.pause(0.00001)

  331. plt.show()

  332. # display.clear_output(wait=True)

  333. # display.display(pl.gcf())

  334. plt.pause(0.1)

  335. print("Finish")

  336. plt.ioff()

运行结果(片段)

(73条消息) Frenet坐标系相关知识系统学习_David's Tweet-CSDN博客_frenet坐标系

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/喵喵爱编程/article/detail/1011236
推荐阅读
  

闽ICP备14008679号