feat(behavior_velocity): apply and add max slowdown calculation utils (…

…#388)

* feat(behavior_velocity): add util to get max slow down velocity at target position

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): apply slow down jerk accel limit

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): consider edge case

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): add behind ego case

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): update comments and todos

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): add discriminant as variable

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* ci(pre-commit): autofix

* chore(behavior_velocity): fix typo

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* doc(behavior_velocity): add docs

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): refactor

Signed-off-by: taikitanaka <ttatcoder@outlook.jp>

* chore(behavior_velocity): change max slowdonw brake strength to a bit lower than emergency brake

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): refactor min max

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -9,7 +9,8 @@
         lateral_distance: 1.5               # [m] maximum lateral distance to consider hidden collision
       motion:
         safety_ratio: 0.8                   # [-] jerk/acceleration ratio for safety
-        max_slow_down_acceleration: -1.5    # [m/s^2] minimum deceleration for safe deceleration.
+        max_slow_down_jerk: -0.7            # [m/s^3] minimum jerk deceleration for safe brake.
+        max_slow_down_accel: -2.0           # [m/s^2] minimum accel deceleration for safe brake.
         non_effective_jerk: -0.3            # [m/s^3] weak jerk for velocity planning.
         non_effective_acceleration: -1.0    # [m/s^2] weak deceleration for velocity planning.
         min_allowed_velocity: 1.0           # [m/s] minimum velocity allowed

planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp

@@ -77,6 +77,7 @@ struct Velocity
   double safety_ratio;          // [-] safety margin for planning error
   double max_stop_jerk;         // [m/s^3] emergency braking system jerk
   double max_stop_accel;        // [m/s^2] emergency braking system deceleration
+  double max_slow_down_jerk;    // [m/s^3] maximum allowed slowdown jerk
   double max_slow_down_accel;   // [m/s^2] maximum allowed deceleration
   double non_effective_jerk;    // [m/s^3] too weak jerk for velocity planning.
   double non_effective_accel;   // [m/s^2] too weak deceleration for velocity planning.

planning/behavior_velocity_planner/include/utilization/util.hpp

@@ -137,6 +137,14 @@ double calcJudgeLineDistWithJerkLimit(
   const double velocity, const double acceleration, const double max_stop_acceleration,
   const double max_stop_jerk, const double delay_response_time);
 
+double calcDecelerationVelocityFromDistanceToTarget(
+  const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel,
+  const double current_velocity, const double distance_to_target);
+
+double findReachTime(
+  const double jerk, const double accel, const double velocity, const double distance,
+  const double t_min, const double t_max);
+
 tier4_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason);
 
 void appendStopReason(

planning/behavior_velocity_planner/occlusion-spot-design.md

@@ -51,7 +51,7 @@ This module considers any occlusion spot around ego path computed from the occup
 
 ##### The Concept of Safe Velocity
 
-Safe velocity is calculated from the below parameters of ego emergency braking system and time to collision.
+The safe slowdown velocity is calculated from the below parameters of ego emergency braking system and time to collision.
 
 - jerk limit[m/s^3]
 - deceleration limit[m/s2]
@@ -60,14 +60,25 @@ Safe velocity is calculated from the below parameters of ego emergency braking s
   with these parameters we can briefly define safe motion before occlusion spot for ideal environment.
   ![occupancy_grid](./docs/occlusion_spot/safe_motion.svg)
 
+##### Maximum Slowdown Velocity
+
+The maximum slowdown velocity is calculated from the below parameters of ego current velocity and acceleration with maximum slowdown jerk and maximum slowdown acceleration in order not to slowdown too much.
+
+- $j_{max}$ slowdown jerk limit[m/s^3]
+- $a_{max}$ slowdown deceleration limit[m/s2]
+- $v_{0}$ current velocity[m/s]
+- $a_{0}$ current acceleration[m/s]
+
+![brief](./docs/occlusion_spot/maximum_slowdown_velocity.svg)
+
 ##### Safe Behavior After Passing Safe Margin Point
 
 This module defines safe margin to consider ego distance to stop and collision path point geometrically.
 While ego is cruising from safe margin to collision path point, ego vehicle keeps the same velocity as occlusion spot safe velocity.
 
 ![brief](./docs/occlusion_spot/behavior_after_safe_margin.svg)
 
-##### Detection area polygon
+##### DetectionArea Polygon
 
 Occlusion spot computation: searching occlusion spots for all cells in the occupancy_grid inside "max lateral distance" requires a lot of computational cost, so this module use only one most notable occlusion spot for each partition. (currently offset is from baselink to front for safety)
 The maximum length of detection area depends on ego current vehicle velocity and acceleration.
@@ -86,13 +97,16 @@ The maximum length of detection area depends on ego current vehicle velocity and
 | `stuck_vehicle_vel`     | double | [m/s] velocity below this value is assumed to stop        |
 | `lateral_distance`      | double | [m] maximum lateral distance to consider hidden collision |
 
-| Parameter /motion     | Type   | Description                                                  |
-| --------------------- | ------ | ------------------------------------------------------------ |
-| `safety_ratio`        | double | [-] safety ratio for jerk and acceleration                   |
-| `max_slow_down_accel` | double | [m/s^2] deceleration to assume for predictive braking system |
-| `v_min`               | double | [m/s] minimum velocity not to stop                           |
-| `delay_time`          | double | [m/s] time buffer for the system delay                       |
-| `safe_margin`         | double | [m] maximum error to stop with emergency braking system.     |
+| Parameter /motion            | Type   | Description                                              |
+| ---------------------------- | ------ | -------------------------------------------------------- |
+| `safety_ratio`               | double | [-] safety ratio for jerk and acceleration               |
+| `max_slow_down_jerk`         | double | [m/s^3] jerk for safe brake                              |
+| `max_slow_down_accel`        | double | [m/s^2] deceleration for safe brake                      |
+| `non_effective_jerk`         | double | [m/s^3] weak jerk for velocity planning.                 |
+| `non_effective_acceleration` | double | [m/s^2] weak deceleration for velocity planning.         |
+| `min_allowed_velocity`       | double | [m/s] minimum velocity allowed                           |
+| `delay_time`                 | double | [m/s] time buffer for the system delay                   |
+| `safe_margin`                | double | [m] maximum error to stop with emergency braking system. |
 
 | Parameter /detection_area | Type   | Description                                                           |
 | ------------------------- | ------ | --------------------------------------------------------------------- |

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp

@@ -93,7 +93,8 @@ OcclusionSpotModuleManager::OcclusionSpotModuleManager(rclcpp::Node & node)
   pp.v.safety_ratio = node.declare_parameter(ns + ".motion.safety_ratio", 1.0);
   pp.v.delay_time = node.declare_parameter(ns + ".motion.delay_time", 0.1);
   pp.v.safe_margin = node.declare_parameter(ns + ".motion.safe_margin", 1.0);
-  pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel", -1.5);
+  pp.v.max_slow_down_jerk = node.declare_parameter(ns + ".motion.max_slow_down_jerk", -0.7);
+  pp.v.max_slow_down_accel = node.declare_parameter(ns + ".motion.max_slow_down_accel", -2.5);
   pp.v.non_effective_jerk = node.declare_parameter(ns + ".motion.non_effective_jerk", -0.3);
   pp.v.non_effective_accel =
     node.declare_parameter(ns + ".motion.non_effective_acceleration", -1.0);

planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp

@@ -34,6 +34,8 @@ void applySafeVelocityConsideringPossibleCollision(
     return;
   }
   const double v0 = param.v.v_ego;
+  const double a0 = param.v.a_ego;
+  const double j_min = param.v.max_slow_down_jerk;
   const double a_min = param.v.max_slow_down_accel;
   const double v_min = param.v.min_allowed_velocity;
   for (auto & possible_collision : possible_collisions) {
@@ -44,9 +46,11 @@ void applySafeVelocityConsideringPossibleCollision(
     const double v_safe = possible_collision.obstacle_info.safe_motion.safe_velocity;
 
     // min allowed velocity : min allowed velocity consider maximum allowed braking
-    const double v_slow_down = calculateMinSlowDownVelocity(v0, l_obs, a_min, v_safe);
-
-    // coompare safe velocity consider EBS, minimum allowed velocity and original velocity
+    const double v_slow_down =
+      (l_obs < 0)
+        ? v_safe
+        : planning_utils::calcDecelerationVelocityFromDistanceToTarget(j_min, a_min, a0, v0, l_obs);
+    // compare safe velocity consider EBS, minimum allowed velocity and original velocity
     const double safe_velocity = calculateInsertVelocity(v_slow_down, v_safe, v_min, original_vel);
     possible_collision.obstacle_info.safe_motion.safe_velocity = safe_velocity;
     const auto & pose = possible_collision.collision_with_margin.pose;

planning/behavior_velocity_planner/src/utilization/util.cpp

@@ -244,6 +244,92 @@ double calcJudgeLineDistWithJerkLimit(
   return std::max(0.0, x1 + x2 + x3);
 }
 
+double findReachTime(
+  const double jerk, const double accel, const double velocity, const double distance,
+  const double t_min, const double t_max)
+{
+  const double j = jerk;
+  const double a = accel;
+  const double v = velocity;
+  const double d = distance;
+  const double min = t_min;
+  const double max = t_max;
+  auto f = [](const double t, const double j, const double a, const double v, const double d) {
+    return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d;
+  };
+  if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) {
+    std::logic_error("[behavior_velocity](findReachTime): search range is invalid");
+  }
+  const double eps = 1e-5;
+  const int warn_iter = 100;
+  double lower = min;
+  double upper = max;
+  double t;
+  int iter = 0;
+  for (int i = 0;; i++) {
+    t = 0.5 * (lower + upper);
+    const double fx = f(t, j, a, v, d);
+    // std::cout<<"fx: "<<fx<<" up: "<<upper<<" lo: "<<lower<<" t: "<<t<<std::endl;
+    if (std::abs(fx) < eps) {
+      break;
+    } else if (fx > 0.0) {
+      upper = t;
+    } else {
+      lower = t;
+    }
+    iter++;
+    if (iter > warn_iter)
+      std::cerr << "[behavior_velocity](findReachTime): current iter is over warning" << std::endl;
+  }
+  // std::cout<<"iter: "<<iter<<std::endl;
+  return t;
+}
+
+double calcDecelerationVelocityFromDistanceToTarget(
+  const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel,
+  const double current_velocity, const double distance_to_target)
+{
+  if (max_slowdown_jerk > 0 || max_slowdown_accel > 0) {
+    std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative");
+  }
+  // case0: distance to target is behind ego
+  if (distance_to_target <= 0) return current_velocity;
+  auto ft = [](const double t, const double j, const double a, const double v, const double d) {
+    return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d;
+  };
+  auto vt = [](const double t, const double j, const double a, const double v) {
+    return j * t * t / 2.0 + a * t + v;
+  };
+  const double j_max = max_slowdown_jerk;
+  const double a0 = current_accel;
+  const double a_max = max_slowdown_accel;
+  const double v0 = current_velocity;
+  const double l = distance_to_target;
+  const double t_const_jerk = (a_max - a0) / j_max;
+  const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0);
+  const double d_const_acc_stop = l - d_const_jerk_stop;
+
+  if (d_const_acc_stop < 0) {
+    // case0: distance to target is within constant jerk deceleration
+    // use binary search instead of solving cubic equation
+    const double t_jerk = findReachTime(j_max, a0, v0, l, 0, t_const_jerk);
+    const double velocity = vt(t_jerk, j_max, a0, v0);
+    return velocity;
+  } else {
+    const double v1 = vt(t_const_jerk, j_max, a0, v0);
+    const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1;
+    // case3: distance to target is farther than distance to stop
+    if (discriminant_of_stop <= 0) {
+      return 0.0;
+    }
+    // case2: distance to target is within constant accel deceleration
+    // solve d = 0.5*a^2+v*t by t
+    const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max;
+    return vt(t_acc, 0.0, a_max, v1);
+  }
+  return current_velocity;
+}
+
 tier4_planning_msgs::msg::StopReason initializeStopReason(const std::string & stop_reason)
 {
   tier4_planning_msgs::msg::StopReason stop_reason_msg;

planning/behavior_velocity_planner/test/src/test_utilization.cpp

@@ -38,3 +38,38 @@ TEST(is_ahead_of, nominal)
   is_ahead = isAheadOf(target, origin);
   EXPECT_TRUE(is_ahead);
 }
+
+TEST(smoothDeceleration, calculateMaxSlowDownVelocity)
+{
+  using behavior_velocity_planner::planning_utils::calcDecelerationVelocityFromDistanceToTarget;
+  const double current_accel = 1.0;
+  const double current_velocity = 5.0;
+  const double max_slow_down_jerk = -1.0;
+  const double max_slow_down_accel = -2.0;
+  const double eps = 1e-3;
+  {
+    for (int i = -8; i <= 24; i += 8) {
+      // arc length in path point
+      const double l = i * 1.0;
+      const double v = calcDecelerationVelocityFromDistanceToTarget(
+        max_slow_down_jerk, max_slow_down_accel, current_accel, current_velocity, l);
+      // case 0 : behind ego
+      if (i == -8) EXPECT_NEAR(v, 5.0, eps);
+      // case 1 : const jerk
+      else if (i == 0)
+        EXPECT_NEAR(v, 5.0, eps);
+      // case 1 : const jerk
+      else if (i == 8)
+        EXPECT_NEAR(v, 5.380, eps);
+      // case 2 : const accel
+      else if (i == 16)
+        EXPECT_NEAR(v, 2.872, eps);
+      // case 3 : after stop
+      else if (i == 24)
+        EXPECT_NEAR(v, 0.00, eps);
+      else
+        continue;
+      std::cout << "s: " << l << " v: " << v << std::endl;
+    }
+  }
+}