From db4f70d1d9cc891651b1384840ecc34f728dad24 Mon Sep 17 00:00:00 2001 From: Atsushi Sakai Date: Thu, 11 Jan 2018 15:36:51 -0800 Subject: [PATCH] add global positoon projection --- .../cubic_spline_planner.py | 239 ++++++++++++++++++ .../frenet_optimal_trajectory.py | 38 ++- 2 files changed, 271 insertions(+), 6 deletions(-) create mode 100644 PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py diff --git a/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py b/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py new file mode 100644 index 00000000..d1df728c --- /dev/null +++ b/PathPlanning/FrenetOptimalTrajectory/cubic_spline_planner.py @@ -0,0 +1,239 @@ +""" +cubic spline planner + +Author: Atsushi Sakai + +""" +import math +import numpy as np +import bisect + + +class Spline: + u""" + Cubic Spline class + """ + + def __init__(self, x, y): + self.b, self.c, self.d, self.w = [], [], [], [] + + self.x = x + self.y = y + + self.nx = len(x) # dimension of x + h = np.diff(x) + + # calc coefficient c + self.a = [iy for iy in y] + + # calc coefficient c + A = self.__calc_A(h) + B = self.__calc_B(h) + self.c = np.linalg.solve(A, B) + # print(self.c1) + + # calc spline coefficient b and d + for i in range(self.nx - 1): + self.d.append((self.c[i + 1] - self.c[i]) / (3.0 * h[i])) + tb = (self.a[i + 1] - self.a[i]) / h[i] - h[i] * \ + (self.c[i + 1] + 2.0 * self.c[i]) / 3.0 + self.b.append(tb) + + def calc(self, t): + u""" + Calc position + + if t is outside of the input x, return None + + """ + + if t < self.x[0]: + return None + elif t > self.x[-1]: + return None + + i = self.__search_index(t) + dx = t - self.x[i] + result = self.a[i] + self.b[i] * dx + \ + self.c[i] * dx ** 2.0 + self.d[i] * dx ** 3.0 + + return result + + def calcd(self, t): + u""" + Calc first derivative + + if t is outside of the input x, return None + """ + + if t < self.x[0]: + return None + elif t > self.x[-1]: + return None + + i = self.__search_index(t) + dx = t - self.x[i] + result = self.b[i] + 2.0 * self.c[i] * dx + 3.0 * self.d[i] * dx ** 2.0 + return result + + def calcdd(self, t): + u""" + Calc second derivative + """ + + if t < self.x[0]: + return None + elif t > self.x[-1]: + return None + + i = self.__search_index(t) + dx = t - self.x[i] + result = 2.0 * self.c[i] + 6.0 * self.d[i] * dx + return result + + def __search_index(self, x): + u""" + search data segment index + """ + return bisect.bisect(self.x, x) - 1 + + def __calc_A(self, h): + u""" + calc matrix A for spline coefficient c + """ + A = np.zeros((self.nx, self.nx)) + A[0, 0] = 1.0 + for i in range(self.nx - 1): + if i != (self.nx - 2): + A[i + 1, i + 1] = 2.0 * (h[i] + h[i + 1]) + A[i + 1, i] = h[i] + A[i, i + 1] = h[i] + + A[0, 1] = 0.0 + A[self.nx - 1, self.nx - 2] = 0.0 + A[self.nx - 1, self.nx - 1] = 1.0 + # print(A) + return A + + def __calc_B(self, h): + u""" + calc matrix B for spline coefficient c + """ + B = np.zeros(self.nx) + for i in range(self.nx - 2): + B[i + 1] = 3.0 * (self.a[i + 2] - self.a[i + 1]) / \ + h[i + 1] - 3.0 * (self.a[i + 1] - self.a[i]) / h[i] + # print(B) + return B + + +class Spline2D: + u""" + 2D Cubic Spline class + + """ + + def __init__(self, x, y): + self.s = self.__calc_s(x, y) + self.sx = Spline(self.s, x) + self.sy = Spline(self.s, y) + + def __calc_s(self, x, y): + dx = np.diff(x) + dy = np.diff(y) + self.ds = [math.sqrt(idx ** 2 + idy ** 2) + for (idx, idy) in zip(dx, dy)] + s = [0] + s.extend(np.cumsum(self.ds)) + return s + + def calc_position(self, s): + u""" + calc position + """ + x = self.sx.calc(s) + y = self.sy.calc(s) + + return x, y + + def calc_curvature(self, s): + u""" + calc curvature + """ + dx = self.sx.calcd(s) + ddx = self.sx.calcdd(s) + dy = self.sy.calcd(s) + ddy = self.sy.calcdd(s) + k = (ddy * dx - ddx * dy) / (dx ** 2 + dy ** 2) + return k + + def calc_yaw(self, s): + u""" + calc yaw + """ + dx = self.sx.calcd(s) + dy = self.sy.calcd(s) + yaw = math.atan2(dy, dx) + return yaw + + +def calc_spline_course(x, y, ds=0.1): + sp = Spline2D(x, y) + s = list(np.arange(0, sp.s[-1], ds)) + + rx, ry, ryaw, rk = [], [], [], [] + for i_s in s: + ix, iy = sp.calc_position(i_s) + rx.append(ix) + ry.append(iy) + ryaw.append(sp.calc_yaw(i_s)) + rk.append(sp.calc_curvature(i_s)) + + return rx, ry, ryaw, rk, s + + +def main(): + print("Spline 2D test") + import matplotlib.pyplot as plt + x = [-2.5, 0.0, 2.5, 5.0, 7.5, 3.0, -1.0] + y = [0.7, -6, 5, 6.5, 0.0, 5.0, -2.0] + + sp = Spline2D(x, y) + s = np.arange(0, sp.s[-1], 0.1) + + rx, ry, ryaw, rk = [], [], [], [] + for i_s in s: + ix, iy = sp.calc_position(i_s) + rx.append(ix) + ry.append(iy) + ryaw.append(sp.calc_yaw(i_s)) + rk.append(sp.calc_curvature(i_s)) + + flg, ax = plt.subplots(1) + plt.plot(x, y, "xb", label="input") + plt.plot(rx, ry, "-r", label="spline") + plt.grid(True) + plt.axis("equal") + plt.xlabel("x[m]") + plt.ylabel("y[m]") + plt.legend() + + flg, ax = plt.subplots(1) + plt.plot(s, [math.degrees(iyaw) for iyaw in ryaw], "-r", label="yaw") + plt.grid(True) + plt.legend() + plt.xlabel("line length[m]") + plt.ylabel("yaw angle[deg]") + + flg, ax = plt.subplots(1) + plt.plot(s, rk, "-r", label="curvature") + plt.grid(True) + plt.legend() + plt.xlabel("line length[m]") + plt.ylabel("curvature [1/m]") + + plt.show() + + +if __name__ == '__main__': + main() diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index f2747fbf..081855d0 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -9,6 +9,7 @@ import numpy as np import matplotlib.pyplot as plt import copy import math +import cubic_spline_planner class quinic_polynomial: @@ -160,12 +161,21 @@ def calc_frenet_paths(c_speed, c_d): return frenet_paths -def calc_global_paths(fplist): +def calc_global_paths(fplist, csp): for fp in fplist: - fp.x = fp.s - fp.y = fp.d + # calc global positions + for i in range(len(fp.s)): + ix, iy = csp.calc_position(fp.s[i]) + 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) + + # 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] @@ -198,10 +208,10 @@ def check_paths(fplist): return [fplist[i] for i in okind] -def frenet_optimal_planning(c_speed, c_d): +def frenet_optimal_planning(csp, c_speed, c_d): fplist = calc_frenet_paths(c_speed, c_d) - fplist = calc_global_paths(fplist) + fplist = calc_global_paths(fplist, csp) fplist = check_paths(fplist) for fp in fplist: @@ -211,10 +221,26 @@ def frenet_optimal_planning(c_speed, c_d): def main(): print(__file__ + " start!!") + x = [0.0, 10.0, 20.5, 35.0, 70.5] + y = [0.0, -6.0, 5.0, 6.5, 0.0] + + csp = cubic_spline_planner.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)) + c_speed = 10.0 / 3.6 # m/s c_d = 1.0 # [m] - frenet_optimal_planning(c_speed, c_d) + plt.plot(rx, ry) + + frenet_optimal_planning(csp, c_speed, c_d) plt.axis("equal") plt.grid(True)