diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md index e1a914b488790..30b1d67a2b804 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md +++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md @@ -748,10 +748,16 @@ The envelope polygon is a rectangle box, whose size depends on the object's poly ![fig](./images/path_generation/envelope_polygon.png) -The module creates a one-shot envelope polygon by using the latest object pose and raw polygon in every planning cycle. On the other hand, the module uses the envelope polygon information created in the last planning cycle in order to update the envelope polygon according to the following logic. If the one-shot envelope polygon is not within the previous envelope polygon, the module creates a new envelope polygon. Otherwise, it keeps the previous envelope polygon. By doing this process, the envelope polygon size and pose will converge even if perception output includes noise in object pose or shape. +The module creates a one-shot envelope polygon by using the latest object pose and raw polygon in every planning cycle. On the other hand, the module uses the envelope polygon information created in the last planning cycle in order to update the envelope polygon with consideration of the pose covariance. + +If the pose covariance is smaller than the threshold, the envelope polygon would be updated according to the following logic. If the one-shot envelope polygon is not within the previous envelope polygon, the module creates a new envelope polygon. Otherwise, it keeps the previous envelope polygon. ![fig](./images/path_generation/polygon_update.png) +When the pose covariance is larger than the threshold, it is compared with the maximum pose covariance of each object. If the value is smaller, the one-shot envelope polygon is used directly as the envelope polygon. Otherwise, it keeps the previous envelope polygon. + +By doing this process, the envelope polygon size and pose will converge even if perception output includes noise in object pose or shape. + ### Relationship between envelope polygon and avoidance path The avoidance path has two shift sections, whose start or end point position depends on the envelope polygon. The end point of the avoidance shift section and start point of the return shift section are fixed based on the envelope polygon and the other side edges are dynamically changed based on ego speed, shift length, lateral jerk constraints, etc. diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png index 1503fe58382a5..e2b9c219733ac 100644 Binary files a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png and b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/images/path_generation/polygon_update.png differ