diff --git a/config/planning/eufs.yaml b/config/planning/eufs.yaml index 158cb58f2..567f35553 100644 --- a/config/planning/eufs.yaml +++ b/config/planning/eufs.yaml @@ -25,7 +25,7 @@ planning: smoothing_spline_coeffs_ratio: 3.0 smoothing_min_path_point_distance: 0 smoothing_use_path_smoothing: true - smoothing_use_optimization: false + smoothing_use_optimization: true smoothing_car_width: 1.2 smoothing_safety_margin: 0.3 smoothing_curvature_weight: 100.0 @@ -34,10 +34,10 @@ planning: smoothing_max_iterations: 4000 smoothing_tolerance: 1e-5 #---------------------- Velocity Planning (vp_) ----------------------- - vp_minimum_velocity: 4.0 + vp_minimum_velocity: 2.0 vp_braking_acceleration: -3.0 - vp_acceleration: 8.0 - vp_normal_acceleration: 6.0 + vp_acceleration: 4.0 + vp_lateral_acceleration: 3.0 vp_use_velocity_planning: true vp_desired_velocity: 20.0 #---------------------- Simulation Config (simulation_) --------------------------- diff --git a/config/planning/pacsim.yaml b/config/planning/pacsim.yaml index f4b8e0796..de3760eec 100644 --- a/config/planning/pacsim.yaml +++ b/config/planning/pacsim.yaml @@ -25,7 +25,7 @@ planning: smoothing_spline_coeffs_ratio: 3.0 smoothing_min_path_point_distance: 0 smoothing_use_path_smoothing: true - smoothing_use_optimization: false + smoothing_use_optimization: true smoothing_car_width: 1.2 smoothing_safety_margin: 0.3 smoothing_curvature_weight: 100.0 @@ -34,16 +34,14 @@ planning: smoothing_max_iterations: 4000 smoothing_tolerance: 1e-5 #---------------------- Velocity Planning (vp_) ----------------------- - vp_minimum_velocity: 4.0 + vp_minimum_velocity: 2.0 vp_braking_acceleration: -3.0 - vp_acceleration: 8.0 - vp_normal_acceleration: 6.0 + vp_acceleration: 4.0 + vp_lateral_acceleration: 3.0 vp_use_velocity_planning: true vp_desired_velocity: 20.0 #---------------------- Simulation Config (simulation_) --------------------------- simulation_publishing_visualization_msgs: true - - # Parameters for EBS Test diff --git a/config/planning/vehicle.yaml b/config/planning/vehicle.yaml index cd4881774..a7f8fa38f 100644 --- a/config/planning/vehicle.yaml +++ b/config/planning/vehicle.yaml @@ -25,7 +25,7 @@ planning: smoothing_spline_coeffs_ratio: 3.0 smoothing_min_path_point_distance: 0 smoothing_use_path_smoothing: true - smoothing_use_optimization: false + smoothing_use_optimization: true smoothing_car_width: 1.2 smoothing_safety_margin: 0.3 smoothing_curvature_weight: 100.0 @@ -37,7 +37,7 @@ planning: vp_minimum_velocity: 2.0 vp_braking_acceleration: -3.0 vp_acceleration: 4.0 - vp_normal_acceleration: 6.0 + vp_lateral_acceleration: 5.0 vp_use_velocity_planning: true vp_desired_velocity: 20.0 #---------------------- Simulation Config (simulation_) --------------------------- diff --git a/src/planning/include/config/planning_config.hpp b/src/planning/include/config/planning_config.hpp index 4c86c303a..d4e83784c 100644 --- a/src/planning/include/config/planning_config.hpp +++ b/src/planning/include/config/planning_config.hpp @@ -52,7 +52,7 @@ struct PlanningParameters { double vp_minimum_velocity_; double vp_braking_acceleration_; double vp_acceleration_; - double vp_normal_acceleration_; + double vp_lateral_acceleration_; bool vp_use_velocity_planning_; double vp_desired_velocity_; @@ -94,7 +94,7 @@ struct PlanningConfig { params.simulation_using_simulated_se_), velocity_planning_(params.vp_minimum_velocity_, params.vp_desired_velocity_, params.vp_braking_acceleration_, params.vp_acceleration_, - params.vp_normal_acceleration_, params.vp_use_velocity_planning_), + params.vp_lateral_acceleration_, params.vp_use_velocity_planning_), skidpad_(params.skidpad_minimum_cones_, params.skidpad_tolerance_) {} }; diff --git a/src/planning/include/config/velocity_config.hpp b/src/planning/include/config/velocity_config.hpp index 406e3b4b1..c5e1013d1 100644 --- a/src/planning/include/config/velocity_config.hpp +++ b/src/planning/include/config/velocity_config.hpp @@ -25,9 +25,9 @@ struct VelocityPlanningConfig { double acceleration_; /** - * @brief The maximum normal acceleration. + * @brief Maximum lateral acceleration. */ - double normal_acceleration_; + double lateral_acceleration_; /** * @brief Flag to enable/disable velocity planning. @@ -42,7 +42,7 @@ struct VelocityPlanningConfig { desired_velocity_(5.0), braking_acceleration_(-4.0), acceleration_(7.0), - normal_acceleration_(7.0), + lateral_acceleration_(7.0), use_velocity_planning_(true) {} /** @@ -50,11 +50,11 @@ struct VelocityPlanningConfig { */ VelocityPlanningConfig(double minimum_velocity, double desired_velocity, double braking_acceleration, double acceleration, - double normal_acceleration, bool use_velocity_planning) + double lateral_acceleration, bool use_velocity_planning) : minimum_velocity_(minimum_velocity), desired_velocity_(desired_velocity), braking_acceleration_(braking_acceleration), acceleration_(acceleration), - normal_acceleration_(normal_acceleration), + lateral_acceleration_(lateral_acceleration), use_velocity_planning_(use_velocity_planning) {} }; \ No newline at end of file diff --git a/src/planning/include/planning/velocity_planning.hpp b/src/planning/include/planning/velocity_planning.hpp index aaa55d3ca..8ff1acd80 100644 --- a/src/planning/include/planning/velocity_planning.hpp +++ b/src/planning/include/planning/velocity_planning.hpp @@ -11,9 +11,13 @@ using PathPoint = common_lib::structures::PathPoint; * @brief Computes velocity profiles for a planned path based on curvature and dynamics constraints. * * The VelocityPlanning class generates a velocity profile along a path by: - * - Estimating curvature (via circle fitting between consecutive path points), + * - Estimating curvature using the Menger curvature formula (circle fitting through three points), * - Deriving maximum allowable velocities from lateral acceleration limits, - * - Propagating braking constraints backward along the path. + * - Applying a friction ellipse model to account for combined longitudinal and lateral tire forces, + * - Propagating acceleration constraints forward and braking constraints backward along the path. + * + * The velocity planner respects the friction circle constraint: a_x² + a_y² ≤ a_max², + * ensuring that the vehicle stays within tire grip limits during combined cornering and acceleration/braking. */ class VelocityPlanning { public: @@ -29,14 +33,14 @@ class VelocityPlanning { explicit VelocityPlanning(VelocityPlanningConfig config) : config_(config) {} /** - * @brief Assigns an ideal velocity to each point of the path. + * @brief Assigns an velocity to each point of the path. * * @param final_path Vector of path points to update with planned velocities. */ void set_velocity(std::vector &final_path); /** - * @brief Computes velocity for track driving scenarios with repeated smoothing. + * @brief Computes velocity for trackdrive scenarios. * * @param final_path Vector of path points to update with planned velocities. */ @@ -60,39 +64,51 @@ class VelocityPlanning { VelocityPlanningConfig config_; /** - * @brief function to calculate the radius of the circle that passes through 3 points - * - * @param point1 first point - * @param point2 second point - * @param point3 third point - * @return radius of the circle + * @brief Numerical tolerance for floating-point comparisons + */ + static constexpr double epsilon = 1e-9; + + /** + * @brief Gravitational acceleration constant (m/s²) */ - double find_circle_center(const PathPoint &point1, const PathPoint &point2, - const PathPoint &point3); + static constexpr double gravity = 9.81; /** - * @brief Applies a forward acceleration constraint to the velocity profile. + * @brief Computes the curvature at a point using the Menger curvature formula. * + * @param p1 Previous point + * @param p2 Current point where curvature is computed + * @param p3 Next point + * @return double Curvature value. Returns 0.0 for straight sections + * or nearly collinear points. + */ + double find_curvature(const PathPoint &p1, const PathPoint &p2, const PathPoint &p3); + + /** + * @brief Computes the maximum velocity at each point based on curvature constraints. * - * @param points The sequence of path points containing positions. - * @param velocities The velocity vector to be modified in-place. + * @param curvatures Vector of curvature values for each path point + * @param velocities Output vector to store the computed maximum velocities */ - void acceleration_limiter(const std::vector &points, - std::vector &velocities); + void point_speed(const std::vector &curvatures, std::vector &velocities); /** - * @brief function to limit the speed of the car according to braking constraints + * @brief Limits velocities based on forward acceleration constraints and friction ellipse. * - * @param points path points - * @param velocities velocities of the path points + * @param points Vector of path points (used for distance calculations) + * @param velocities Vector of velocities to be updated (input/output) + * @param curvatures Vector of curvature values (used to compute lateral acceleration) */ - void braking_limiter(std::vector &points, std::vector &velocities); + void acceleration_limiter(const std::vector &points, std::vector &velocities, + const std::vector &curvatures); /** - * @brief function to calculate the speed of the car according to the curvature of the path + * @brief Limits velocities based on backward braking constraints and friction ellipse. * - * @param radiuses radiuses vector of the path points - * @param velocities velocities vector of the path points + * @param points Vector of path points (used for distance calculations) + * @param velocities Vector of velocities to be updated (input/output) + * @param curvatures Vector of curvature values (used to compute lateral acceleration) */ - void point_speed(const std::vector &radiuses, std::vector &velocities); + void braking_limiter(std::vector &points, std::vector &velocities, + const std::vector &curvatures); }; \ No newline at end of file diff --git a/src/planning/src/planning/planning.cpp b/src/planning/src/planning/planning.cpp index b0c074a1d..4eae7e4e0 100644 --- a/src/planning/src/planning/planning.cpp +++ b/src/planning/src/planning/planning.cpp @@ -72,7 +72,7 @@ PlanningParameters Planning::load_config(std::string &adapter) { params.vp_minimum_velocity_ = planning_config["vp_minimum_velocity"].as(); params.vp_braking_acceleration_ = planning_config["vp_braking_acceleration"].as(); params.vp_acceleration_ = planning_config["vp_acceleration"].as(); - params.vp_normal_acceleration_ = planning_config["vp_normal_acceleration"].as(); + params.vp_lateral_acceleration_ = planning_config["vp_lateral_acceleration"].as(); params.vp_use_velocity_planning_ = planning_config["vp_use_velocity_planning"].as(); params.vp_desired_velocity_ = planning_config["vp_desired_velocity"].as(); @@ -307,7 +307,7 @@ void Planning::run_trackdrive() { double avg_vel = sum / smoothed_path_.size(); - RCLCPP_DEBUG(get_logger(), + RCLCPP_INFO(get_logger(), "[TRACKDRIVE] Velocity Stats - Avg: %.2f m/s | Max: %.2f m/s | Min: %.2f m/s", avg_vel, max_vel, min_vel); } diff --git a/src/planning/src/planning/velocity_planning.cpp b/src/planning/src/planning/velocity_planning.cpp index 1bd118a37..423d078e8 100644 --- a/src/planning/src/planning/velocity_planning.cpp +++ b/src/planning/src/planning/velocity_planning.cpp @@ -1,171 +1,149 @@ #include "planning/velocity_planning.hpp" -double VelocityPlanning::find_circle_center(const PathPoint &point1, const PathPoint &point2, - const PathPoint &point3) { - double x1 = point1.position.x; - double y1 = point1.position.y; - double x2 = point2.position.x; - double y2 = point2.position.y; - double x3 = point3.position.x; - double y3 = point3.position.y; - - PathPoint mid1 = PathPoint((x1 + x2) / 2, (y1 + y2) / 2, 0); - PathPoint mid2 = PathPoint((x2 + x3) / 2, (y2 + y3) / 2, 0); - - double slope1 = MAXFLOAT; - double slope2 = MAXFLOAT; - double slope1_perpendicular = 10'000; - double slope2_perpendicular = 10'000; - if (x2 != x1) { - slope1 = (y2 - y1) / (x2 - x1); +double VelocityPlanning::find_curvature(const PathPoint &p1, const PathPoint &p2, + const PathPoint &p3) { + // lengths of the sides of the triangle formed by the three points + double a = std::hypot(p2.position.x - p1.position.x, p2.position.y - p1.position.y); + double b = std::hypot(p3.position.x - p2.position.x, p3.position.y - p2.position.y); + double c = std::hypot(p3.position.x - p1.position.x, p3.position.y - p1.position.y); + + // area of the triangle using the determinant method + double area = 0.5 * std::abs(p1.position.x * (p2.position.y - p3.position.y) + + p2.position.x * (p3.position.y - p1.position.y) + + p3.position.x * (p1.position.y - p2.position.y)); + + // To avoid division by zero in case of near duplicate points + if (a * b * c < epsilon) { + return 0.0; } - if (x3 != x2) { - slope2 = (y3 - y2) / (x3 - x2); - } - - if (slope1 != 0) { - slope1_perpendicular = -1 / slope1; - } - - if (slope2 != 0) { - slope2_perpendicular = -1 / slope2; - } - - if (slope1_perpendicular == slope2_perpendicular) { - return 10'000; - } - - double center_x = (slope1_perpendicular * mid1.position.x - - slope2_perpendicular * mid2.position.x + mid2.position.y - mid1.position.y) / - (slope1_perpendicular - slope2_perpendicular); - double center_y = slope1_perpendicular * (center_x - mid1.position.x) + mid1.position.y; - double radius = std::sqrt(std::pow(center_x - x2, 2) + std::pow(center_y - y2, 2)); - return radius; + /* The Menger curvature is given by the formula: K = 4A / (abc), + where A is the triangle area and a,b,c are the side lengths*/ + return 4 * area / (a * b * c); } -void VelocityPlanning::point_speed(const std::vector &radiuses, +void VelocityPlanning::point_speed(const std::vector &curvatures, std::vector &velocities) { - for (const auto &radius : radiuses) { - double velocity = std::sqrt(abs(config_.normal_acceleration_ * radius)); - velocities.push_back(std::max(velocity, config_.minimum_velocity_)); + for (const auto &k : curvatures) { + // This is a straight line, there is no curvature limit on the velocity + if (std::abs(k) < epsilon) { + velocities.push_back(config_.desired_velocity_); + continue; + } + + double velocity = std::sqrt(config_.lateral_acceleration_ / std::abs(k)); + velocities.push_back(std::min(velocity, config_.desired_velocity_)); } + // The last point is always the minimum velocity for safety velocities.back() = config_.minimum_velocity_; - - return; } void VelocityPlanning::acceleration_limiter(const std::vector &points, - std::vector &velocities) { + std::vector &velocities, + const std::vector &curvatures) { velocities[0] = config_.minimum_velocity_; for (int i = 1; i < (int)points.size(); i++) { - // distance to previous point double dx = points[i].position.x - points[i - 1].position.x; double dy = points[i].position.y - points[i - 1].position.y; double d = std::sqrt(dx * dx + dy * dy); - // v_i^2 = v_(i-1)^2 + 2 * a_acc * d - double max_velocity = std::sqrt( - std::max(0.0, velocities[i - 1] * velocities[i - 1] + 2.0 * config_.acceleration_ * d)); + // lateral acceleration at previous point: v(i-1)^2 * curvature + double ay = velocities[i - 1] * velocities[i - 1] * std::abs(curvatures[i - 1]); + double ax_max = std::sqrt( + std::max(0.0, config_.lateral_acceleration_ * config_.lateral_acceleration_ - ay * ay)); + + // Cap by acceleration limit + ax_max = std::min(ax_max, config_.acceleration_); - // cap the curvature-based velocity by longitudinal acceleration + // v_i^2 = v_(i-1)^2 + 2 * a_x_available * d + double max_velocity = + std::sqrt(std::max(0.0, velocities[i - 1] * velocities[i - 1] + 2 * ax_max * d)); velocities[i] = std::min(velocities[i], max_velocity); } } void VelocityPlanning::braking_limiter(std::vector &points, - std::vector &velocities) { - for (int i = static_cast(points.size()) - 2; i >= 0; i--) { - double distance = 0; - double max_speed = velocities[i]; + std::vector &velocities, + const std::vector &curvatures) { + for (int i = static_cast(points.size()) - 2; i >= 0; i--) { // Calculate segment distance int j = i + 1; - distance = std::sqrt(std::pow(points[j].position.x - points[j - 1].position.x, 2) + - std::pow(points[j].position.y - points[j - 1].position.y, 2)); + double distance = std::hypot(points[j].position.x - points[i].position.x, + points[j].position.y - points[i].position.y); + + // Lateral acceleration at the next point: a = v(j)^2 * curvature + double ay = velocities[j] * velocities[j] * std::abs(curvatures[j]); + double ax_brake = std::sqrt(std::max(0.0, config_.lateral_acceleration_ * config_.lateral_acceleration_ - ay * ay)); + + // Cap by braking limit + ax_brake = std::min(ax_brake, config_.braking_acceleration_); // Correct kinematic speed calculation // v_f² = v_i² + 2ad - double max_terminal_speed = std::sqrt( - std::max(0.0, std::pow(velocities[j], 2) - 2 * config_.braking_acceleration_ * distance)); + double max_speed = + std::sqrt(std::max(0.0, std::pow(velocities[j], 2) - 2 * ax_brake * distance)); - max_speed = std::min(max_speed, max_terminal_speed); max_speed = std::min(max_speed, this->config_.desired_velocity_); - - velocities[i] = max_speed; + velocities[i] = std::min(max_speed, velocities[i]); } } -// Main function to set the velocity of the car void VelocityPlanning::set_velocity(std::vector &final_path) { - if ((config_.use_velocity_planning_) && (final_path.size() > 2)) { - std::vector radiuses; - radiuses.push_back(0); - for (int i = 1; i < static_cast(final_path.size()) - 1; i++) { - radiuses.push_back(find_circle_center(final_path[i - 1], final_path[i], final_path[i + 1])); - } - radiuses[0] = radiuses[1]; - radiuses.push_back(radiuses.back()); + if (!config_.use_velocity_planning_ || final_path.size() <= 2) { + for (auto &p : final_path){ + p.ideal_velocity = config_.minimum_velocity_; + } + return; + } - std::vector velocities; - point_speed(radiuses, velocities); - acceleration_limiter(final_path, velocities); - braking_limiter(final_path, velocities); + const int n = static_cast(final_path.size()); - for (int i = 0; i < static_cast(final_path.size()); i++) { - final_path[i].ideal_velocity = velocities[i]; - } + std::vector curvatures(n, 0.0); + for (int i = 1; i < n - 1; ++i){ + curvatures[i] = find_curvature(final_path[i - 1], final_path[i], final_path[i + 1]); } - - else { - for (auto &path_point : final_path) { - path_point.ideal_velocity = config_.minimum_velocity_; - } + + std::vector velocities; + point_speed(curvatures, velocities); + acceleration_limiter(final_path, velocities, curvatures); + braking_limiter(final_path, velocities, curvatures); + + for (int i = 0; i < n; ++i) { + velocities[i] = std::max(velocities[i], config_.minimum_velocity_); + velocities[i] = std::min(velocities[i], config_.desired_velocity_); + final_path[i].ideal_velocity = velocities[i]; } } void VelocityPlanning::trackdrive_velocity(std::vector &final_path) { if (!config_.use_velocity_planning_ || final_path.size() <= 2) { - for (PathPoint &path_point : final_path) { - path_point.ideal_velocity = config_.minimum_velocity_; - } + for (auto &p : final_path){ + p.ideal_velocity = config_.minimum_velocity_; + } return; } - int path_size = static_cast(final_path.size()) - 1; // exclude duplicate last point - // ---- triple the path ---- + int path_size = static_cast(final_path.size()) - 1; + + // Triple the path std::vector triple_path; triple_path.reserve(3 * path_size); - for (int lap = 0; lap < 3; ++lap) { for (int i = 0; i < path_size; ++i) { triple_path.push_back(final_path[i]); } } - // ---- curvature ---- - std::vector radiuses(triple_path.size()); - - for (size_t i = 1; i < triple_path.size()-1; ++i) { - radiuses[i] = find_circle_center(triple_path[i - 1], triple_path[i], triple_path[i + 1]); - } - - radiuses[0] = radiuses[1]; - radiuses.back() = radiuses[triple_path.size() - 2]; - - // ---- velocity planning ---- - std::vector velocities; - point_speed(radiuses, velocities); - acceleration_limiter(triple_path, velocities); - braking_limiter(triple_path, velocities); + set_velocity(triple_path); - int offset = path_size; // start of the center lap - + // Extract middle path velocities ---- + int offset = path_size; // middle lap start for (int i = 0; i < path_size; ++i) { - final_path[i].ideal_velocity = velocities[offset + i]; + final_path[i].ideal_velocity = triple_path[offset + i].ideal_velocity; } - // close loop explicitly + // Close loop explicitly final_path.back().ideal_velocity = final_path.front().ideal_velocity; }