diff --git a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml b/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
index 078305a1b7375..7ccda0468bac6 100644
--- a/planning/behavior_velocity_planner/config/occlusion_spot.param.yaml
+++ b/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
diff --git a/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg
new file mode 100644
index 0000000000000..e970eb3f87845
--- /dev/null
+++ b/planning/behavior_velocity_planner/docs/occlusion_spot/maximum_slowdown_velocity.svg
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
index 260fdb40357be..dc6349dd12793 100644
--- a/planning/behavior_velocity_planner/include/scene_module/occlusion_spot/occlusion_spot_utils.hpp
+++ b/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.
diff --git a/planning/behavior_velocity_planner/include/utilization/util.hpp b/planning/behavior_velocity_planner/include/utilization/util.hpp
index 4926e53542ba0..24c71d056ba2f 100644
--- a/planning/behavior_velocity_planner/include/utilization/util.hpp
+++ b/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(
diff --git a/planning/behavior_velocity_planner/occlusion-spot-design.md b/planning/behavior_velocity_planner/occlusion-spot-design.md
index 7dc1cb6330cd1..5d97f298106ff 100644
--- a/planning/behavior_velocity_planner/occlusion-spot-design.md
+++ b/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,6 +60,17 @@ 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.
+##### 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]
+
+
+
##### Safe Behavior After Passing Safe Margin Point
This module defines safe margin to consider ego distance to stop and collision path point geometrically.
@@ -67,7 +78,7 @@ While ego is cruising from safe margin to collision path point, ego vehicle keep
-##### 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 |
| ------------------------- | ------ | --------------------------------------------------------------------- |
diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp
index 739fa9169c461..1ad0f5970c3f2 100644
--- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/manager.cpp
+++ b/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);
diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp
index ce6df41e80338..e2427b90ae055 100644
--- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/risk_predictive_braking.cpp
+++ b/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;
diff --git a/planning/behavior_velocity_planner/src/utilization/util.cpp b/planning/behavior_velocity_planner/src/utilization/util.cpp
index 82c75705e06b5..b2f73a60d452d 100644
--- a/planning/behavior_velocity_planner/src/utilization/util.cpp
+++ b/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: "< 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: "< 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;
diff --git a/planning/behavior_velocity_planner/test/src/test_utilization.cpp b/planning/behavior_velocity_planner/test/src/test_utilization.cpp
index 4d16c285d0564..f2c557454b273 100644
--- a/planning/behavior_velocity_planner/test/src/test_utilization.cpp
+++ b/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;
+ }
+ }
+}