feat(behavior_velocity): add jerk limit for occlusion spot deceleration

planning/behavior_velocity_planner/config/occlusion_spot.param.yaml

@@ -2,20 +2,16 @@
   ros__parameters:
     occlusion_spot:
       pedestrian_vel: 1.0                  # [m/s] assume pedestrian is dashing from occlusion at this velocity
-      safety_time_buffer: 0.1              # [s] delay for the system response
       threshold:
         detection_area_length: 100.0        # [m] the length of path to consider perception range
         stuck_vehicle_vel: 1.0              # [m/s] velocity below this value is assumed to stop
         lateral_distance: 1.5               # [m] maximum lateral distance to consider hidden collision
-      # road type parameters
-      public_road:
-        min_velocity: 1.5                   # [m/s] minimum velocity to ignore occlusion spot
-        ebs_decel: -2.5                     # [m/s^2] maximum deceleration to assume for emergency stops.
-        pbs_decel: -1.0                     # [m/s^2] deceleration to assume for predictive braking stops
-      private_road:
-        min_velocity: 1.5                   # [m/s] minimum velocity to ignore occlusion spot
-        ebs_decel: -2.0                     # [m/s^2] maximum deceleration to assume for emergency stops.
-        pbs_decel: -1.0                     # [m/s^2] deceleration to assume for predictive braking stops.
+      motion:
+        safety_ratio: 0.8                   # [-] jerk/acceleration ratio for safety
+        max_slow_down_acceleration: -1.5    # [m/s^2] minimum deceleration for safe deceleration.
+        min_allowed_velocity: 1.0       # [m/s] minimum velocity allowed
+        delay_time: 0.1                     # [s] safety time buffer for delay system response
+        safe_margin: 1.0                    # [m] maximum safety distance for any error
       sidewalk:
         min_occlusion_spot_size: 1.0     # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle.
         slice_size: 1.5     # [m] size of sidewalk slices in both length and distance relative to the ego trajectory.

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

@@ -71,24 +71,22 @@ struct Sidewalk
   double slice_size;               // [m] size of each slice
   double min_occlusion_spot_size;  // [m] minumum size to care about the occlusion spot
 };
-
-struct VehicleInfo
+struct Velocity
 {
-  double vehicle_width;      // [m]  vehicle_width from parameter server
-  double baselink_to_front;  // [m]  wheel_base + front_overhang
-};
-
-struct EgoVelocity
-{
-  double ebs_decel;     // [m/s^2] emergency braking system deceleration
-  double pbs_decel;     // [m/s^2] predictive braking system deceleration
-  double min_velocity;  // [m/s]   minimum allowed velocity not to stop
+  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_accel;   // [m/s^2] maximum allowed deceleration
+  double min_allowed_velocity;  // [m/s]   minimum allowed velocity not to stop
+  double a_ego;                 // [m/s^2] current ego acceleration
+  double v_ego;                 // [m/s]   current ego velocity
+  double delay_time;            // [s] safety time buffer for delay response
+  double safe_margin;           // [m] maximum safety distance for any error
 };
 
 struct PlannerParam
 {
   // parameters in yaml
-  double safety_time_buffer;     // [s]
   double detection_area_length;  // [m]
   double stuck_vehicle_vel;      // [m/s]
   double lateral_distance_thr;   // [m] lateral distance threshold to consider
@@ -98,17 +96,27 @@ struct PlannerParam
   double angle_thr;      // [rad]
   bool show_debug_grid;  // [-]
 
-  VehicleInfo vehicle_info;
-  EgoVelocity private_road;
-  EgoVelocity public_road;
+  // vehicle info
+  double half_vehicle_width;  // [m]  half vehicle_width from vehicle info
+  double baselink_to_front;   // [m]  wheel_base + front_overhang
+
+  Velocity v;
   Sidewalk sidewalk;
   grid_utils::GridParam grid;
 };
 
+struct SafeMotion
+{
+  double stop_dist;
+  double safe_velocity;
+};
+
 struct ObstacleInfo
 {
+  SafeMotion safe_motion;  // safe motion of velocity and stop point
   geometry_msgs::msg::Point position;
   double max_velocity;  // [m/s] Maximum velocity of the possible obstacle
+  double ttc;           // [s] time to collision with ego
 };
 
 /**
@@ -123,27 +131,24 @@ struct ObstacleInfo
  */
 struct PossibleCollisionInfo
 {
-  ObstacleInfo obstacle_info;  // For hidden obstacle
-  autoware_auto_planning_msgs::msg::PathPoint
-    collision_path_point;                              // For baselink at collision point
-  geometry_msgs::msg::Pose intersection_pose;          // For egp path and hidden obstacle
+  ObstacleInfo obstacle_info;                          // For hidden obstacle
+  PathPoint collision_with_margin;                     // For baselink at collision point
+  Pose collision_pose;                                 // only use this for debugging
+  Pose intersection_pose;                              // For egp path and hidden obstacle
   lanelet::ArcCoordinates arc_lane_dist_at_collision;  // For ego distance to obstacle in s-d
   PossibleCollisionInfo() = default;
   PossibleCollisionInfo(
-    const ObstacleInfo & obstacle_info,
-    const autoware_auto_planning_msgs::msg::PathPoint & collision_path_point,
-    const geometry_msgs::msg::Pose & intersection_pose,
-    const lanelet::ArcCoordinates & arc_lane_dist_to_occlusion)
+    const ObstacleInfo & obstacle_info, const PathPoint & collision_with_margin,
+    const Pose & intersection_pose, const lanelet::ArcCoordinates & arc_lane_dist_to_occlusion)
   : obstacle_info(obstacle_info),
-    collision_path_point(collision_path_point),
+    collision_with_margin(collision_with_margin),
     intersection_pose(intersection_pose),
     arc_lane_dist_at_collision(arc_lane_dist_to_occlusion)
   {
   }
 };
 
 lanelet::ConstLanelet toPathLanelet(const PathWithLaneId & path);
-
 // Note : consider offset_from_start_to_ego and safety margin for collision here
 inline void handleCollisionOffset(
   std::vector<PossibleCollisionInfo> & possible_collisions, double offset, double margin)

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

@@ -26,51 +26,69 @@ namespace behavior_velocity_planner
 {
 namespace occlusion_spot_utils
 {
-void applySafeVelocityConsideringPossibleCollison(
-  autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path,
-  std::vector<PossibleCollisionInfo> & possible_collisions, const double current_vel,
-  const EgoVelocity & ego, const PlannerParam & param);
+void applySafeVelocityConsideringPossibleCollision(
+  PathWithLaneId * inout_path, std::vector<PossibleCollisionInfo> & possible_collisions,
+  const PlannerParam & param);
 
 int insertSafeVelocityToPath(
   const geometry_msgs::msg::Pose & in_pose, const double safe_vel, const PlannerParam & param,
-  autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path);
+  PathWithLaneId * inout_path);
 
 // @brief calculates the maximum velocity allowing to decelerate within the given distance
-inline double calculatePredictiveBrakingVelocity(
-  const double ego_vel, const double dist2col, const double pbs_decel)
+inline double calculateMinSlowDownVelocity(
+  const double v0, const double len, const double a_max, const double safe_vel)
 {
-  return std::sqrt(std::max(std::pow(ego_vel, 2.0) - 2.0 * std::abs(pbs_decel) * dist2col, 0.0));
+  // if target velocity is inserted backward return current velocity as limit
+  if (len < 0) return safe_vel;
+  return std::sqrt(std::max(std::pow(v0, 2.0) - 2.0 * std::abs(a_max) * len, 0.0));
 }
 
 /**
- * @param: safety_time: safety time buffer for reaction
- * @param: dist_to_obj: distance to virtual darting object
- * @param: v_obs: relative  velocity for virtual darting object
- * @param: ebs_decel: emergency brake
- * @return safe velocity considering rpb
+ * @param: sv: ego velocity config
+ * @param: ttc: time to collision
+ * @return safe motion
  **/
-inline double calculateSafeRPBVelocity(
-  const double safety_time, const double dist_to_obj, const double v_obs, const double ebs_decel)
+inline SafeMotion calculateSafeMotion(const Velocity & v, const double ttc)
 {
-  const double t_vir = dist_to_obj / v_obs;
-  // min safety time buffer is at least more than 0
-  const double ttc_virtual = std::max(t_vir - safety_time, 0.0);
-  // safe velocity consider emergency brake
-  const double v_safe = std::abs(ebs_decel) * ttc_virtual;
-  return v_safe;
+  SafeMotion sm;
+  const double j_max = v.safety_ratio * v.max_stop_jerk;
+  const double a_max = v.safety_ratio * v.max_stop_accel;
+  const double t1 = v.delay_time;
+  double t2 = a_max / j_max;
+  double & v_safe = sm.safe_velocity;
+  double & stop_dist = sm.stop_dist;
+  if (ttc <= t1) {
+    // delay
+    v_safe = 0;
+    stop_dist = 0;
+  } else if (ttc <= t2 + t1) {
+    // delay + const jerk
+    t2 = ttc - t1;
+    v_safe = -0.5 * j_max * t2 * t2;
+    stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6;
+  } else {
+    const double t3 = ttc - t2 - t1;
+    // delay + const jerk + const accel
+    const double v2 = -0.5 * j_max * t2 * t2;
+    v_safe = v2 - a_max * t3;
+    stop_dist = v_safe * t1 - j_max * t2 * t2 * t2 / 6 + v2 * t3 - 0.5 * a_max * t3 * t3;
+  }
+  stop_dist += v.safe_margin;
+  return sm;
 }
 
-inline double getPBSLimitedRPBVelocity(
-  const double pbs_vel, const double rpb_vel, const double min_vel, const double original_vel)
+inline double calculateInsertVelocity(
+  const double min_allowed_vel, const double safe_vel, const double min_vel,
+  const double original_vel)
 {
   const double max_vel_noise = 0.05;
   // ensure safe velocity doesn't exceed maximum allowed pbs deceleration
-  double rpb_pbs_limited_vel = std::max(pbs_vel + max_vel_noise, rpb_vel);
+  double cmp_safe_vel = std::max(min_allowed_vel + max_vel_noise, safe_vel);
   // ensure safe path velocity is also above ego min velocity
-  rpb_pbs_limited_vel = std::max(rpb_pbs_limited_vel, min_vel);
+  cmp_safe_vel = std::max(cmp_safe_vel, min_vel);
   // ensure we only lower the original velocity (and do not increase it)
-  rpb_pbs_limited_vel = std::min(rpb_pbs_limited_vel, original_vel);
-  return rpb_pbs_limited_vel;
+  cmp_safe_vel = std::min(cmp_safe_vel, original_vel);
+  return cmp_safe_vel;
 }
 
 }  // namespace occlusion_spot_utils

planning/behavior_velocity_planner/occlusion-spot-design.md

@@ -53,28 +53,50 @@ Occlusion spot computation: searching occlusion spots for all cells in the occup
 
 Note that the accuracy and performance of this search method is limited due to the approximation.
 
+#### Occlusion Spot Common
+
+##### The Concept of Safe Velocity
+
+Safe velocity is calculated from below parameters of ego emergency braking system and time to collision.
+
+- jerk limit[m/s^3]
+- deceleration limit[m/s2]
+- delay response time[s]
+- time to collision of pedestrian[s]
+  with these parameters we can briefly define safe motion before occlusion spot for ideal environment.
+  ![occupancy_grid](./docs/occlusion_spot/safe_motion.svg)
+
+##### Safe Behavior After Passing Safe Margin Point
+
+This module defines safe margin consider ego distance to stop and collision path point geometrically.
+while ego is cruising from safe margin to collision path point ego keeps same velocity as occlusion spot safe velocity.
+
+![brief](./docs/occlusion_spot/behavior_after_safe_margin.svg)
+
 #### Module Parameters
 
-| Parameter            | Type   | Description                                                               |
-| -------------------- | ------ | ------------------------------------------------------------------------- |
-| `pedestrian_vel`     | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point |
-| `safety_time_buffer` | double | [m/s] time buffer for the system delay                                    |
+| Parameter        | Type   | Description                                                               |
+| ---------------- | ------ | ------------------------------------------------------------------------- |
+| `pedestrian_vel` | double | [m/s] maximum velocity assumed pedestrian coming out from occlusion point |
 
 | Parameter /threshold    | Type   | Description                                               |
 | ----------------------- | ------ | --------------------------------------------------------- |
 | `detection_area_length` | double | [m] the length of path to consider occlusion spot         |
 | `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 /(public or private)\_road | Type   | Description                                                          |
-| ------------------------------------ | ------ | -------------------------------------------------------------------- |
-| `min_velocity`                       | double | [m/s] minimum velocity to ignore occlusion spot                      |
-| `ebs_decel`                          | double | [m/s^2] maximum deceleration to assume for emergency braking system. |
-| `pbs_decel`                          | double | [m/s^2] deceleration to assume for predictive braking system         |
+| 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 /sidewalk       | Type   | Description                                                     |
 | ------------------------- | ------ | --------------------------------------------------------------- |
 | `min_occlusion_spot_size` | double | [m] the length of path to consider occlusion spot               |
+| `slice_size`              | double | [m] the distance of divided detection area                      |
 | `focus_range`             | double | [m] buffer around the ego path used to build the sidewalk area. |
 
 | Parameter /grid  | Type   | Description                                                     |
@@ -121,8 +143,12 @@ note right
   - occlusion spot is calculated by longitudinally closest point of unknown cells.
   - intersection point is where ego front bumper and darting object will crash.
   - collision path point is calculated by arc coordinate consider ego vehicle's geometry.
+  - safe velocity and safe margin is calculated from performance of ego emergency braking system.
+end note
+:calculate safe velocity and safe margin for possible collision;
+note right
+  - safe velocity and safe margin is calculated from performance of ego emergency braking system.
 end note
-
 }
 partition process_possible_collision {
 :filter possible collision by road type;
@@ -137,10 +163,10 @@ end note
 note right
 consider offset from path start to ego vehicle for possible collision
 end note
-:apply safe velocity consider possible collision;
+:apply safe velocity comparing with allowed velocity;
 note right
 calculated by
-- ebs deceleration [m/s] emergency braking system consider lateral distance to the occlusion spot.
+- safe velocity calculated from emergency brake performance.
 - maximum allowed deceleration [m/s^2]
 - min velocity [m/s] the velocity that is allowed on the road.
 - original_velocity [m/s]
@@ -186,12 +212,16 @@ note right
   - intersection point is where ego front bumper and darting object will crash.
   - collision path point is calculated by arc coordinate consider ego vehicle's geometry.
 end note
+:calculate safe velocity and safe margin for possible collision;
+note right
+  - safe velocity and safe margin is calculated from performance of ego emergency braking system.
+end note
 }
 partition process_possible_collision {
 :filter collision by road type;
 :calculate slow down points for possible collision;
 :handle collision offset;
-:calculate safe velocity consider lateral distance and safe velocity;
+:apply safe velocity comparing with allowed velocity;
 :insert safe velocity to path;
 }
 stop
@@ -236,12 +266,16 @@ note right
   - consider occlusion which is nearer than `lateral_distance_threshold`.
 end note
 :calculate collision path point and intersection point;
+:calculate safe velocity and safe margin for possible collision;
+note right
+  - safe velocity and safe margin is calculated from performance of ego emergency braking system.
+end note
 }
 partition handle_possible_collision {
 :filter collision by road type;
 :calculate slow down points for possible collision;
 :handle collision offset;
-:calculate safe velocity consider lateral distance and safe velocity;
+:apply safe velocity comparing with allowed velocity;
 :insert safe velocity to path;
 }
 stop