From 008b5c7bb951150692d8f8cbefba0fd314df4e73 Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Tue, 24 Dec 2024 00:47:33 +0800 Subject: [PATCH 1/4] feat: Add third derivative and curvature rate calculations to CubicSpline classes --- .../CubicSpline/cubic_spline_planner.py | 48 +++++++++++++++++++ 1 file changed, 48 insertions(+) diff --git a/PathPlanning/CubicSpline/cubic_spline_planner.py b/PathPlanning/CubicSpline/cubic_spline_planner.py index 7cc7bedc79..5710117089 100644 --- a/PathPlanning/CubicSpline/cubic_spline_planner.py +++ b/PathPlanning/CubicSpline/cubic_spline_planner.py @@ -137,6 +137,27 @@ def calc_second_derivative(self, x): ddy = 2.0 * self.c[i] + 6.0 * self.d[i] * dx return ddy + def calc_third_derivative(self, x): + """ + Calc third derivative at given x. + + if x is outside the input x, return None + + Returns + ------- + dddy : float + third derivative for given x. + """ + if x < self.x[0]: + return None + elif x > self.x[-1]: + return None + + i = self.__search_index(x) + dx = x - self.x[i] + dddy = 6.0 * self.d[i] + return dddy + def __search_index(self, x): """ search data segment index @@ -287,6 +308,33 @@ def calc_curvature(self, s): k = (ddy * dx - ddx * dy) / ((dx ** 2 + dy ** 2)**(3 / 2)) return k + def calc_curvature_rate(self, s): + """ + calc curvature rate + + Parameters + ---------- + s : float + distance from the start point. if `s` is outside the data point's + range, return None. + + Returns + ------- + k : float + curvature rate for given s. + """ + dx = self.sx.calc_first_derivative(s) + dy = self.sy.calc_first_derivative(s) + ddx = self.sx.calc_second_derivative(s) + ddy = self.sy.calc_second_derivative(s) + dddx = self.sx.calc_third_derivative(s) + dddy = self.sy.calc_third_derivative(s) + a = dx * ddy - dy * ddx + b = dx * dddy - dy * dddx + c = dx * ddx + dy * ddy + d = dx * dx + dy * dy + return (b * d - 3.0 * a * c) / (d * d * d) + def calc_yaw(self, s): """ calc yaw From fde87a081e69f3db94a151b7a9374a47f5af819d Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Tue, 24 Dec 2024 00:48:09 +0800 Subject: [PATCH 2/4] feat: Extend frenet_optimal_trajectory to support more scenarios --- .../cartesian_frenet_converter.py | 144 +++++ .../frenet_optimal_trajectory.py | 556 +++++++++++++----- tests/test_frenet_optimal_trajectory.py | 38 +- 3 files changed, 585 insertions(+), 153 deletions(-) create mode 100644 PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py diff --git a/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py new file mode 100644 index 0000000000..a62eb2b6fb --- /dev/null +++ b/PathPlanning/FrenetOptimalTrajectory/cartesian_frenet_converter.py @@ -0,0 +1,144 @@ +""" + +A converter between Cartesian and Frenet coordinate systems + +author: Wang Zheng (@Aglargil) + +Ref: + +- [Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame] +(https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf) + +""" + +import math + + +class CartesianFrenetConverter: + """ + A class for converting states between Cartesian and Frenet coordinate systems + """ + + @ staticmethod + def cartesian_to_frenet(rs, rx, ry, rtheta, rkappa, rdkappa, x, y, v, a, theta, kappa): + """ + Convert state from Cartesian coordinate to Frenet coordinate + + Parameters + ---------- + rs: reference line s-coordinate + rx, ry: reference point coordinates + rtheta: reference point heading + rkappa: reference point curvature + rdkappa: reference point curvature rate + x, y: current position + v: velocity + a: acceleration + theta: heading angle + kappa: curvature + + Returns + ------- + s_condition: [s(t), s'(t), s''(t)] + d_condition: [d(s), d'(s), d''(s)] + """ + dx = x - rx + dy = y - ry + + cos_theta_r = math.cos(rtheta) + sin_theta_r = math.sin(rtheta) + + cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx + d = math.copysign(math.sqrt(dx * dx + dy * dy), cross_rd_nd) + + delta_theta = theta - rtheta + tan_delta_theta = math.tan(delta_theta) + cos_delta_theta = math.cos(delta_theta) + + one_minus_kappa_r_d = 1 - rkappa * d + d_dot = one_minus_kappa_r_d * tan_delta_theta + + kappa_r_d_prime = rdkappa * d + rkappa * d_dot + + d_ddot = (-kappa_r_d_prime * tan_delta_theta + + one_minus_kappa_r_d / (cos_delta_theta * cos_delta_theta) * + (kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa)) + + s = rs + s_dot = v * cos_delta_theta / one_minus_kappa_r_d + + delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa + s_ddot = (a * cos_delta_theta - + s_dot * s_dot * + (d_dot * delta_theta_prime - kappa_r_d_prime)) / one_minus_kappa_r_d + + return [s, s_dot, s_ddot], [d, d_dot, d_ddot] + + @ staticmethod + def frenet_to_cartesian(rs, rx, ry, rtheta, rkappa, rdkappa, s_condition, d_condition): + """ + Convert state from Frenet coordinate to Cartesian coordinate + + Parameters + ---------- + rs: reference line s-coordinate + rx, ry: reference point coordinates + rtheta: reference point heading + rkappa: reference point curvature + rdkappa: reference point curvature rate + s_condition: [s(t), s'(t), s''(t)] + d_condition: [d(s), d'(s), d''(s)] + + Returns + ------- + x, y: position + theta: heading angle + kappa: curvature + v: velocity + a: acceleration + """ + if abs(rs - s_condition[0]) >= 1.0e-6: + raise ValueError( + "The reference point s and s_condition[0] don't match") + + cos_theta_r = math.cos(rtheta) + sin_theta_r = math.sin(rtheta) + + x = rx - sin_theta_r * d_condition[0] + y = ry + cos_theta_r * d_condition[0] + + one_minus_kappa_r_d = 1 - rkappa * d_condition[0] + + tan_delta_theta = d_condition[1] / one_minus_kappa_r_d + delta_theta = math.atan2(d_condition[1], one_minus_kappa_r_d) + cos_delta_theta = math.cos(delta_theta) + + theta = CartesianFrenetConverter.normalize_angle(delta_theta + rtheta) + + kappa_r_d_prime = rdkappa * d_condition[0] + rkappa * d_condition[1] + + kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) * + cos_delta_theta * cos_delta_theta) / one_minus_kappa_r_d + rkappa) * \ + cos_delta_theta / one_minus_kappa_r_d + + d_dot = d_condition[1] * s_condition[1] + v = math.sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d * + s_condition[1] * s_condition[1] + d_dot * d_dot) + + delta_theta_prime = one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa + + a = (s_condition[2] * one_minus_kappa_r_d / cos_delta_theta + + s_condition[1] * s_condition[1] / cos_delta_theta * + (d_condition[1] * delta_theta_prime - kappa_r_d_prime)) + + return x, y, theta, kappa, v, a + + @ staticmethod + def normalize_angle(angle): + """ + Normalize angle to [-pi, pi] + """ + a = math.fmod(angle + math.pi, 2.0 * math.pi) + if a < 0.0: + a += 2.0 * math.pi + return a - math.pi diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 331df36309..9fc84bea07 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -17,43 +17,312 @@ import numpy as np import matplotlib.pyplot as plt import copy -import math import sys import pathlib + sys.path.append(str(pathlib.Path(__file__).parent.parent)) -from QuinticPolynomialsPlanner.quintic_polynomials_planner import \ - QuinticPolynomial +from QuinticPolynomialsPlanner.quintic_polynomials_planner import QuinticPolynomial from CubicSpline import cubic_spline_planner -SIM_LOOP = 500 +from enum import Enum, auto +from FrenetOptimalTrajectory.cartesian_frenet_converter import ( + CartesianFrenetConverter, +) + + +class LateralMovement(Enum): + HighSpeed = auto() + LowSpeed = auto() + + +class LongitudinalMovement(Enum): + MergingAndStopping = auto() + VelocityKeeping = auto() + # Parameter + +LATERAL_MOVEMENT = LateralMovement.HighSpeed +LONGITUDINAL_MOVEMENT = LongitudinalMovement.VelocityKeeping + MAX_SPEED = 50.0 / 3.6 # maximum speed [m/s] -MAX_ACCEL = 2.0 # maximum acceleration [m/ss] +MAX_ACCEL = 5.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] MAX_T = 5.0 # max prediction time [m] MIN_T = 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 K_J = 0.1 K_T = 0.1 +K_S_DOT = 1.0 K_D = 1.0 +K_S = 1.0 K_LAT = 1.0 K_LON = 1.0 +SIM_LOOP = 500 show_animation = True -class QuarticPolynomial: +if LATERAL_MOVEMENT == LateralMovement.LowSpeed: + MAX_ROAD_WIDTH = 1.0 # maximum road width [m] + D_ROAD_W = 0.2 # road width sampling length [m] + TARGET_SPEED = 3.0 / 3.6 # maximum speed [m/s] + D_T_S = 0.5 / 3.6 # target speed sampling length [m/s] + # Waypoints + WX = [0.0, 2.0, 4.0, 6.0, 8.0, 10.0] + WY = [0.0, 0.0, 1.0, 0.0, -1.0, -2.0] + OBSTACLES = np.array([[3.0, 1.0], [5.0, -0.0], [6.0, 0.5], [8.0, -1.5]]) + ROBOT_RADIUS = 0.5 # robot radius [m] + + # Initial state parameters + INITIAL_SPEED = 1.0 / 3.6 # current speed [m/s] + INITIAL_ACCEL = 0.0 # current acceleration [m/ss] + INITIAL_LAT_POSITION = 0.5 # current lateral position [m] + INITIAL_LAT_SPEED = 0.0 # current lateral speed [m/s] + INITIAL_LAT_ACCELERATION = 0.0 # current lateral acceleration [m/s] + INITIAL_COURSE_POSITION = 0.0 # current course position + + ANIMATION_AREA = 5.0 # Animation area length [m] + + STOP_S = 4.0 # Merge and stop distance [m] + D_S = 0.3 # Stop point sampling length [m] + N_STOP_S_SAMPLE = 3 # Stop point sampling number +else: + MAX_ROAD_WIDTH = 7.0 # maximum road width [m] + D_ROAD_W = 1.0 # road width sampling length [m] + TARGET_SPEED = 30.0 / 3.6 # target speed [m/s] + D_T_S = 5.0 / 3.6 # target speed sampling length [m/s] + # Waypoints + WX = [0.0, 10.0, 20.5, 35.0, 70.5] + WY = [0.0, -6.0, 5.0, 6.5, 0.0] + # Obstacle list + OBSTACLES = np.array( + [[20.0, 10.0], [30.0, 6.0], [30.0, 8.0], [35.0, 8.0], [50.0, 3.0]] + ) + ROBOT_RADIUS = 2.0 # robot radius [m] + + # Initial state parameters + INITIAL_SPEED = 10.0 / 3.6 # current speed [m/s] + INITIAL_ACCEL = 0.0 # current acceleration [m/ss] + INITIAL_LAT_POSITION = 2.0 # current lateral position [m] + INITIAL_LAT_SPEED = 0.0 # current lateral speed [m/s] + INITIAL_LAT_ACCELERATION = 0.0 # current lateral acceleration [m/s] + INITIAL_COURSE_POSITION = 0.0 # current course position + + ANIMATION_AREA = 20.0 # Animation area length [m] + STOP_S = 25.0 # Merge and stop distance [m] + D_S = 2 # Stop point sampling length [m] + N_STOP_S_SAMPLE = 4 # Stop point sampling number + + +class LateralMovementStrategy: + def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti): + """ + Calculate the lateral trajectory + """ + raise NotImplementedError("calc_lateral_trajectory not implemented") + + def calc_cartesian_parameters(self, fp, csp): + """ + Calculate the cartesian parameters (x, y, yaw, curvature, v, a) + """ + raise NotImplementedError("calc_cartesian_parameters not implemented") + + +class HighSpeedLateralMovementStrategy(LateralMovementStrategy): + def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti): + tp = copy.deepcopy(fp) + s0_d = fp.s_d[0] + s0_dd = fp.s_dd[0] + # d'(t) = d'(s) * s'(t) + # d''(t) = d''(s) * s'(t)^2 + d'(s) * s''(t) + lat_qp = QuinticPolynomial( + c_d, c_d_d * s0_d, c_d_dd * s0_d**2 + c_d_d * s0_dd, di, 0.0, 0.0, Ti + ) + + tp.d = [] + tp.d_d = [] + tp.d_dd = [] + tp.d_ddd = [] + + # Calculate all derivatives in a single loop to reduce iterations + for i in range(len(fp.t)): + t = fp.t[i] + s_d = fp.s_d[i] + s_dd = fp.s_dd[i] + + s_d_inv = 1.0 / (s_d + 1e-6) + 1e-6 # Avoid division by zero + s_d_inv_sq = s_d_inv * s_d_inv # Square of inverse + + d = lat_qp.calc_point(t) + d_d = lat_qp.calc_first_derivative(t) + d_dd = lat_qp.calc_second_derivative(t) + d_ddd = lat_qp.calc_third_derivative(t) + + tp.d.append(d) + # d'(s) = d'(t) / s'(t) + tp.d_d.append(d_d * s_d_inv) + # d''(s) = (d''(t) - d'(s) * s''(t)) / s'(t)^2 + tp.d_dd.append((d_dd - tp.d_d[i] * s_dd) * s_d_inv_sq) + tp.d_ddd.append(d_ddd) + + return tp + + def calc_cartesian_parameters(self, fp, csp): + # calc global positions + for i in range(len(fp.s)): + ix, iy = csp.calc_position(fp.s[i]) + if ix is None: + break + i_yaw = csp.calc_yaw(fp.s[i]) + i_kappa = csp.calc_curvature(fp.s[i]) + i_dkappa = csp.calc_curvature_rate(fp.s[i]) + s_condition = [fp.s[i], fp.s_d[i], fp.s_dd[i]] + d_condition = [ + fp.d[i], + fp.d_d[i], + fp.d_dd[i], + ] + x, y, theta, kappa, v, a = CartesianFrenetConverter.frenet_to_cartesian( + fp.s[i], ix, iy, i_yaw, i_kappa, i_dkappa, s_condition, d_condition + ) + fp.x.append(x) + fp.y.append(y) + fp.yaw.append(theta) + fp.c.append(kappa) + fp.v.append(v) + fp.a.append(a) + return fp + + +class LowSpeedLateralMovementStrategy(LateralMovementStrategy): + def calc_lateral_trajectory(self, fp, di, c_d, c_d_d, c_d_dd, Ti): + s0 = fp.s[0] + s1 = fp.s[-1] + tp = copy.deepcopy(fp) + # d = d(s), d_d = d'(s), d_dd = d''(s) + # * shift s range from [s0, s1] to [0, s1 - s0] + lat_qp = QuinticPolynomial(c_d, c_d_d, c_d_dd, di, 0.0, 0.0, s1 - s0) + + tp.d = [lat_qp.calc_point(s - s0) for s in fp.s] + tp.d_d = [lat_qp.calc_first_derivative(s - s0) for s in fp.s] + tp.d_dd = [lat_qp.calc_second_derivative(s - s0) for s in fp.s] + tp.d_ddd = [lat_qp.calc_third_derivative(s - s0) for s in fp.s] + return tp + + def calc_cartesian_parameters(self, fp, csp): + # calc global positions + for i in range(len(fp.s)): + ix, iy = csp.calc_position(fp.s[i]) + if ix is None: + break + i_yaw = csp.calc_yaw(fp.s[i]) + i_kappa = csp.calc_curvature(fp.s[i]) + i_dkappa = csp.calc_curvature_rate(fp.s[i]) + s_condition = [fp.s[i], fp.s_d[i], fp.s_dd[i]] + d_condition = [fp.d[i], fp.d_d[i], fp.d_dd[i]] + x, y, theta, kappa, v, a = CartesianFrenetConverter.frenet_to_cartesian( + fp.s[i], ix, iy, i_yaw, i_kappa, i_dkappa, s_condition, d_condition + ) + fp.x.append(x) + fp.y.append(y) + fp.yaw.append(theta) + fp.c.append(kappa) + fp.v.append(v) + fp.a.append(a) + return fp + + +class LongitudinalMovementStrategy: + def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0): + """ + Calculate the longitudinal trajectory + """ + raise NotImplementedError("calc_longitudinal_trajectory not implemented") + + def get_d_arrange(self, s0): + """ + Get the d sample range + """ + raise NotImplementedError("get_d_arrange not implemented") + + def calc_destination_cost(self, fp): + """ + Calculate the destination cost + """ + raise NotImplementedError("calc_destination_cost not implemented") + + +class VelocityKeepingLongitudinalMovementStrategy(LongitudinalMovementStrategy): + def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0): + fplist = [] + for tv in np.arange( + TARGET_SPEED - D_T_S * N_S_SAMPLE, TARGET_SPEED + D_T_S * N_S_SAMPLE, D_T_S + ): + fp = FrenetPath() + lon_qp = QuarticPolynomial(s0, c_speed, c_accel, tv, 0.0, Ti) + fp.t = [t for t in np.arange(0.0, Ti, DT)] + 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] + fplist.append(fp) + return fplist + + def get_d_arrange(self, s0): + return np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W) + + def calc_destination_cost(self, fp): + ds = (TARGET_SPEED - fp.s_d[-1]) ** 2 + return K_S_DOT * ds + + +class MergingAndStoppingLongitudinalMovementStrategy(LongitudinalMovementStrategy): + def calc_longitudinal_trajectory(self, c_speed, c_accel, Ti, s0): + if s0 >= STOP_S: + return [] + fplist = [] + for s in np.arange( + STOP_S - D_S * N_STOP_S_SAMPLE, STOP_S + D_S * N_STOP_S_SAMPLE, D_S + ): + fp = FrenetPath() + lon_qp = QuinticPolynomial(s0, c_speed, c_accel, s, 0.0, 0.0, Ti) + fp.t = [t for t in np.arange(0.0, Ti, DT)] + 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] + fplist.append(fp) + return fplist + + def get_d_arrange(self, s0): + # Only if s0 is less than STOP_S / 3, then we sample the road width + if s0 < STOP_S / 3: + return np.arange(-MAX_ROAD_WIDTH, MAX_ROAD_WIDTH, D_ROAD_W) + else: + return [0.0] + + def calc_destination_cost(self, fp): + ds = (STOP_S - fp.s[-1]) ** 2 + return K_S * ds + + +if LATERAL_MOVEMENT == LateralMovement.HighSpeed: + LATERAL_MOVEMENT_STRATEGY = HighSpeedLateralMovementStrategy() +else: + LATERAL_MOVEMENT_STRATEGY = LowSpeedLateralMovementStrategy() + +if LONGITUDINAL_MOVEMENT == LongitudinalMovement.VelocityKeeping: + LONGITUDINAL_MOVEMENT_STRATEGY = VelocityKeepingLongitudinalMovementStrategy() +else: + LONGITUDINAL_MOVEMENT_STRATEGY = MergingAndStoppingLongitudinalMovementStrategy() + +class QuarticPolynomial: def __init__(self, xs, vxs, axs, vxe, axe, time): # calc coefficient of quartic polynomial @@ -61,29 +330,25 @@ def __init__(self, xs, vxs, axs, vxe, axe, time): self.a1 = vxs self.a2 = axs / 2.0 - A = np.array([[3 * time ** 2, 4 * time ** 3], - [6 * time, 12 * time ** 2]]) - b = np.array([vxe - self.a1 - 2 * self.a2 * time, - axe - 2 * self.a2]) + A = np.array([[3 * time**2, 4 * time**3], [6 * time, 12 * time**2]]) + b = np.array([vxe - self.a1 - 2 * self.a2 * time, 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 + 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 + 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 + xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t**2 return xt @@ -94,111 +359,85 @@ def calc_third_derivative(self, t): class FrenetPath: - def __init__(self): self.t = [] self.d = [] - self.d_d = [] - self.d_dd = [] - self.d_ddd = [] + self.d_d = [] # d'(s) + self.d_dd = [] # d''(s) + self.d_ddd = [] # d'''(t) in low speed / d'''(s) in high speed self.s = [] - self.s_d = [] - self.s_dd = [] - self.s_ddd = [] - self.cd = 0.0 - self.cv = 0.0 + self.s_d = [] # s'(t) + self.s_dd = [] # s''(t) + self.s_ddd = [] # s'''(t) self.cf = 0.0 self.x = [] self.y = [] self.yaw = [] + self.v = [] + self.a = [] self.ds = [] self.c = [] - -def calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0): + def pop_front(self): + self.x.pop(0) + self.y.pop(0) + self.yaw.pop(0) + self.v.pop(0) + self.a.pop(0) + self.s.pop(0) + self.s_d.pop(0) + self.s_dd.pop(0) + self.s_ddd.pop(0) + self.d.pop(0) + self.d_d.pop(0) + self.d_dd.pop(0) + self.d_ddd.pop(0) + + +def calc_frenet_paths(c_s_d, c_s_dd, 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() - - # 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 = QuarticPolynomial(s0, c_speed, c_accel, 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 = 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) + for Ti in np.arange(MIN_T, MAX_T, DT): + lon_paths = LONGITUDINAL_MOVEMENT_STRATEGY.calc_longitudinal_trajectory( + c_s_d, c_s_dd, Ti, s0 + ) + + for fp in lon_paths: + for di in LONGITUDINAL_MOVEMENT_STRATEGY.get_d_arrange(s0): + tp = LATERAL_MOVEMENT_STRATEGY.calc_lateral_trajectory( + fp, di, c_d, c_d_d, c_d_dd, Ti + ) + + Jp = sum(np.power(tp.d_ddd, 2)) # square of jerk + Js = sum(np.power(tp.s_ddd, 2)) # square of jerk + + lat_cost = K_J * Jp + K_T * Ti + K_D * tp.d[-1] ** 2 + lon_cost = ( + K_J * Js + + K_T * Ti + + LONGITUDINAL_MOVEMENT_STRATEGY.calc_destination_cost(tp) + ) + tp.cf = K_LAT * lat_cost + K_LON * lon_cost + frenet_paths.append(tp) return frenet_paths def calc_global_paths(fplist, csp): - 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 - i_yaw = csp.calc_yaw(fp.s[i]) - di = fp.d[i] - fx = ix + di * math.cos(i_yaw + math.pi / 2.0) - fy = iy + di * math.sin(i_yaw + 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] - fp.yaw.append(math.atan2(dy, dx)) - fp.ds.append(math.hypot(dx, dy)) - - 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 + return [ + LATERAL_MOVEMENT_STRATEGY.calc_cartesian_parameters(fp, csp) for fp in fplist + ] def check_collision(fp, ob): for i in range(len(ob[:, 0])): - d = [((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2) - for (ix, iy) in zip(fp.x, fp.y)] + d = [ + ((ix - ob[i, 0]) ** 2 + (iy - ob[i, 1]) ** 2) + for (ix, iy) in zip(fp.x, fp.y) + ] - collision = any([di <= ROBOT_RADIUS ** 2 for di in d]) + collision = any([di <= ROBOT_RADIUS**2 for di in d]) if collision: return False @@ -207,38 +446,41 @@ def check_collision(fp, ob): def check_paths(fplist, ob): - ok_ind = [] + path_dict = { + "max_speed_error": [], + "max_accel_error": [], + "max_curvature_error": [], + "collision_error": [], + "ok": [], + } for i, _ in enumerate(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 + if any([v > MAX_SPEED for v in fplist[i].v]): # Max speed check + path_dict["max_speed_error"].append(fplist[i]) + elif any([abs(a) > MAX_ACCEL for a in fplist[i].a]): # Max accel check + path_dict["max_accel_error"].append(fplist[i]) + elif any([abs(c) > MAX_CURVATURE for c in fplist[i].c]): # Max curvature check + path_dict["max_curvature_error"].append(fplist[i]) elif not check_collision(fplist[i], ob): - continue - - ok_ind.append(i) + path_dict["collision_error"].append(fplist[i]) + else: + path_dict["ok"].append(fplist[i]) + return path_dict - return [fplist[i] for i in ok_ind] - -def frenet_optimal_planning(csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob): - fplist = calc_frenet_paths(c_speed, c_accel, c_d, c_d_d, c_d_dd, s0) +def frenet_optimal_planning(csp, s0, c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, ob): + fplist = calc_frenet_paths(c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, s0) fplist = calc_global_paths(fplist, csp) - fplist = check_paths(fplist, ob) + fpdict = check_paths(fplist, ob) # find minimum cost path min_cost = float("inf") best_path = None - for fp in fplist: + for fp in fpdict["ok"]: if min_cost >= fp.cf: min_cost = fp.cf best_path = fp - return best_path + return [best_path, fpdict] def generate_target_course(x, y): @@ -259,40 +501,39 @@ def generate_target_course(x, y): def main(): print(__file__ + " start!!") - # 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_accel = 0.0 # current acceleration [m/ss] - c_d = 2.0 # current lateral position [m] - c_d_d = 0.0 # current lateral speed [m/s] - c_d_dd = 0.0 # current lateral acceleration [m/s] - s0 = 0.0 # current course position - - area = 20.0 # animation area length [m] + tx, ty, tyaw, tc, csp = generate_target_course(WX, WY) + + # Initialize state using global parameters + c_s_d = INITIAL_SPEED + c_s_dd = INITIAL_ACCEL + c_d = INITIAL_LAT_POSITION + c_d_d = INITIAL_LAT_SPEED + c_d_dd = INITIAL_LAT_ACCELERATION + s0 = INITIAL_COURSE_POSITION + + area = ANIMATION_AREA + + last_path = None for i in range(SIM_LOOP): - path = frenet_optimal_planning( - csp, s0, c_speed, c_accel, c_d, c_d_d, c_d_dd, ob) + [path, fpdict] = frenet_optimal_planning( + csp, s0, c_s_d, c_s_dd, c_d, c_d_d, c_d_dd, OBSTACLES + ) + + if path is None: + path = copy.deepcopy(last_path) + path.pop_front() + if len(path.x) <= 1: + print("Finish") + break + last_path = path 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] - c_accel = path.s_dd[1] - + c_s_d = path.s_d[1] + c_s_dd = path.s_dd[1] if np.hypot(path.x[1] - tx[-1], path.y[1] - ty[-1]) <= 1.0: print("Goal") break @@ -301,15 +542,26 @@ def main(): plt.cla() # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect( - 'key_release_event', - lambda event: [exit(0) if event.key == 'escape' else None]) + "key_release_event", + lambda event: [exit(0) if event.key == "escape" else None], + ) plt.plot(tx, ty) - plt.plot(ob[:, 0], ob[:, 1], "xk") + plt.plot(OBSTACLES[:, 0], OBSTACLES[:, 1], "xk") + # for fp in fpdict["max_speed_error"]: + # plt.plot(fp.x[1:], fp.y[1:], "-m") + # for fp in fpdict["max_accel_error"]: + # plt.plot(fp.x[1:], fp.y[1:], "-y") + # for fp in fpdict["max_curvature_error"]: + # plt.plot(fp.x[1:], fp.y[1:], "-k") + # for fp in fpdict["collision_error"]: + # plt.plot(fp.x[1:], fp.y[1:], "-b") + # for fp in fpdict["ok"]: + # plt.plot(fp.x[1:], fp.y[1:], "-g") plt.plot(path.x[1:], path.y[1:], "-or") plt.plot(path.x[1], path.y[1], "vc") 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.title("v[km/h]:" + str(path.v[1] * 3.6)[0:4]) plt.grid(True) plt.pause(0.0001) @@ -320,5 +572,5 @@ def main(): plt.show() -if __name__ == '__main__': +if __name__ == "__main__": main() diff --git a/tests/test_frenet_optimal_trajectory.py b/tests/test_frenet_optimal_trajectory.py index 72bb7a8341..29238d8e0a 100644 --- a/tests/test_frenet_optimal_trajectory.py +++ b/tests/test_frenet_optimal_trajectory.py @@ -1,5 +1,9 @@ import conftest from PathPlanning.FrenetOptimalTrajectory import frenet_optimal_trajectory as m +from PathPlanning.FrenetOptimalTrajectory.frenet_optimal_trajectory import ( + LateralMovement, + LongitudinalMovement, +) def test1(): @@ -8,5 +12,37 @@ def test1(): m.main() -if __name__ == '__main__': +def test2(): + m.show_animation = False + m.LATERAL_MOVEMENT = LateralMovement.HighSpeed + m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.MergingAndStopping + m.SIM_LOOP = 5 + m.main() + + +def test3(): + m.show_animation = False + m.LATERAL_MOVEMENT = LateralMovement.HighSpeed + m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.VelocityKeeping + m.SIM_LOOP = 5 + m.main() + + +def test4(): + m.show_animation = False + m.LATERAL_MOVEMENT = LateralMovement.HighSpeed + m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.VelocityKeeping + m.SIM_LOOP = 5 + m.main() + + +def test5(): + m.show_animation = False + m.LATERAL_MOVEMENT = LateralMovement.HighSpeed + m.LONGITUDINAL_MOVEMENT = LongitudinalMovement.MergingAndStopping + m.SIM_LOOP = 5 + m.main() + + +if __name__ == "__main__": conftest.run_this_test(__file__) From 80f661be069a6788c876dc2155ca92bfdd98425a Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Wed, 25 Dec 2024 01:57:40 +0800 Subject: [PATCH 3/4] fix: frenet optimal trajectory type check --- .../FrenetOptimalTrajectory/frenet_optimal_trajectory.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py index 9fc84bea07..2a15b422c3 100644 --- a/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py +++ b/PathPlanning/FrenetOptimalTrajectory/frenet_optimal_trajectory.py @@ -310,6 +310,8 @@ def calc_destination_cost(self, fp): ds = (STOP_S - fp.s[-1]) ** 2 return K_S * ds +LATERAL_MOVEMENT_STRATEGY: LateralMovementStrategy +LONGITUDINAL_MOVEMENT_STRATEGY: LongitudinalMovementStrategy if LATERAL_MOVEMENT == LateralMovement.HighSpeed: LATERAL_MOVEMENT_STRATEGY = HighSpeedLateralMovementStrategy() From 2707e74cdfc5e77564e0376b00692589e23ba025 Mon Sep 17 00:00:00 2001 From: Aglargil <1252223935@qq.com> Date: Wed, 25 Dec 2024 02:07:48 +0800 Subject: [PATCH 4/4] fix: cubic spline planner code style check --- PathPlanning/CubicSpline/cubic_spline_planner.py | 1 - 1 file changed, 1 deletion(-) diff --git a/PathPlanning/CubicSpline/cubic_spline_planner.py b/PathPlanning/CubicSpline/cubic_spline_planner.py index 5710117089..73b5167ffc 100644 --- a/PathPlanning/CubicSpline/cubic_spline_planner.py +++ b/PathPlanning/CubicSpline/cubic_spline_planner.py @@ -154,7 +154,6 @@ def calc_third_derivative(self, x): return None i = self.__search_index(x) - dx = x - self.x[i] dddy = 6.0 * self.d[i] return dddy