diff --git a/modules/planning/common/path_obstacle.h b/modules/planning/common/path_obstacle.h
index 27ac3c9c18fecdc68e6143674d44d9b861f69314..84034a38c2b6b01fbd3a915f5a11e79e1f9d4f66 100644
--- a/modules/planning/common/path_obstacle.h
+++ b/modules/planning/common/path_obstacle.h
@@ -131,6 +131,9 @@ class PathObstacle {
    **/
   static bool IsLateralDecision(const ObjectDecisionType& decision);
 
+  void SetBlockingObstacle(bool blocking) { is_blocking_obstacle_ = blocking; }
+  bool IsBlockingObstacle() const { return is_blocking_obstacle_; }
+
  private:
   FRIEND_TEST(MergeLongitudinalDecision, AllDecisions);
   static ObjectDecisionType MergeLongitudinalDecision(
@@ -155,6 +158,8 @@ class PathObstacle {
   ObjectDecisionType lateral_decision_;
   ObjectDecisionType longitudinal_decision_;
 
+  bool is_blocking_obstacle_ = false;
+
   struct ObjectTagCaseHash {
     std::size_t operator()(
         const planning::ObjectDecisionType::ObjectTagCase tag) const {
diff --git a/modules/planning/conf/planning_config.pb.txt b/modules/planning/conf/planning_config.pb.txt
index ab5152398b0edc7fe170b25f552553281c76c91f..793b8bf53937f699e19f35aa047d68b60c9c7356 100644
--- a/modules/planning/conf/planning_config.pb.txt
+++ b/modules/planning/conf/planning_config.pb.txt
@@ -158,6 +158,8 @@ em_planner_config {
         jerk_weight: 1.0
         obstacle_weight: 10.0
 
+        unblocking_obstacle_cost: 1e3
+
         st_boundary_config {
             boundary_buffer: 0.1
             high_speed_centric_acceleration_limit: 1.2
diff --git a/modules/planning/proto/poly_st_speed_config.proto b/modules/planning/proto/poly_st_speed_config.proto
index a00a110030157c926fc64003e287ba168d3d73d5..d6979948936eba45c04cf3c3efed7fca593106f4 100644
--- a/modules/planning/proto/poly_st_speed_config.proto
+++ b/modules/planning/proto/poly_st_speed_config.proto
@@ -16,5 +16,6 @@ message PolyStSpeedConfig {
   optional double speed_weight = 8;
   optional double jerk_weight = 9;
   optional double obstacle_weight = 10;
-  optional apollo.planning.StBoundaryConfig st_boundary_config = 11;
+  optional double unblocking_obstacle_cost = 11;
+  optional apollo.planning.StBoundaryConfig st_boundary_config = 12;
 }
diff --git a/modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc b/modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
index 9ae7be6ae8523dda4bce0dae8ee003828c91b122..2f9b74ea6f62482d7735cf013c3d7b8b9399de04 100644
--- a/modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
+++ b/modules/planning/tasks/poly_st_speed/poly_st_speed_optimizer.cc
@@ -87,6 +87,7 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
     DCHECK(path_obstacle->HasLongitudinalDecision());
   }
   // step 1 get boundaries
+  auto path_decision_copy = *path_decision;
   path_decision->EraseStBoundaries();
   if (boundary_mapper.CreateStBoundary(path_decision).code() ==
       ErrorCode::PLANNING_ERROR) {
@@ -94,10 +95,15 @@ Status PolyStSpeedOptimizer::Process(const SLBoundary& adc_sl_boundary,
                   "Mapping obstacle for qp st speed optimizer failed!");
   }
 
-  std::vector<const StBoundary*> boundaries;
   for (const auto* obstacle : path_decision->path_obstacles().Items()) {
+    auto id = obstacle->Id();
+    auto* mutable_obstacle = path_decision->Find(id);
+
     if (!obstacle->st_boundary().IsEmpty()) {
-      boundaries.push_back(&obstacle->st_boundary());
+      mutable_obstacle->SetBlockingObstacle(true);
+    } else {
+      path_decision->SetStBoundary(id,
+                                   path_decision_copy.Find(id)->st_boundary());
     }
   }
 
diff --git a/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc b/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
index 0ab0c1347861ab41bc07a43c6423c3459707877e..7b46f420045bcebba361cae752038d7f5ec20d62 100644
--- a/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
+++ b/modules/planning/tasks/poly_st_speed/speed_profile_cost.cc
@@ -85,7 +85,8 @@ double SpeedProfileCost::CalculatePointCost(
     if (t < boundary.min_t() || t > boundary.max_t()) {
       continue;
     }
-    if (boundary.IsPointInBoundary(STPoint(s, t))) {
+    if (obstacle->IsBlockingObstacle() &&
+        boundary.IsPointInBoundary(STPoint(s, t))) {
       return kInfCost;
     }
     double s_upper = 0.0;
@@ -106,6 +107,10 @@ double SpeedProfileCost::CalculatePointCost(
         cost += config_.obstacle_weight() *
                 std::pow((kSafeDistance + s_upper - s), 2);
       }
+    } else {
+      if (!obstacle->IsBlockingObstacle()) {
+        cost += config_.unblocking_obstacle_cost();
+      }
     }
   }
   cost += config_.speed_weight() * std::pow((v - speed_limit), 2);
diff --git a/modules/planning/tasks/poly_st_speed/speed_profile_cost.h b/modules/planning/tasks/poly_st_speed/speed_profile_cost.h
index f14213bbbff7f72ce97a943c2537eaf58ecbaabc..ec5a44348e6b7c6711d562148fa5d1f48de43a4a 100644
--- a/modules/planning/tasks/poly_st_speed/speed_profile_cost.h
+++ b/modules/planning/tasks/poly_st_speed/speed_profile_cost.h
@@ -15,7 +15,7 @@
  *****************************************************************************/
 
 /**
- * @file trajectory_cost.h
+ * @file
  **/
 
 #ifndef MODULES_PLANNING_TASKS_POLY_ST_SPEED_SPEED_PROFILE_COST_H_