diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
index ab9f506de00a7..acaa0be7291ef 100644
--- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
+++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -886,8 +886,18 @@ PathWithLaneId BehaviorPathPlannerNode::modifyPathForSmoothGoalConnection(
   const auto goal = planner_data_->route_handler->getGoalPose();
   const auto goal_lane_id = planner_data_->route_handler->getGoalLaneId();
 
+  Pose refined_goal{};
+  {
+    lanelet::ConstLanelet goal_lanelet;
+    if (planner_data_->route_handler->getGoalLanelet(&goal_lanelet)) {
+      refined_goal = util::refineGoal(goal, goal_lanelet);
+    } else {
+      refined_goal = goal;
+    }
+  }
+
   auto refined_path = util::refinePathForGoal(
-    planner_data_->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, goal,
+    planner_data_->parameters.refine_goal_search_radius_range, M_PI * 0.5, path, refined_goal,
     goal_lane_id);
   refined_path.header.frame_id = "map";
   refined_path.header.stamp = this->now();
diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp
index 339965bdea493..aaf3dce50f1fc 100644
--- a/planning/behavior_path_planner/src/utilities.cpp
+++ b/planning/behavior_path_planner/src/utilities.cpp
@@ -1098,7 +1098,14 @@ bool setGoal(
 
 const Pose refineGoal(const Pose & goal, const lanelet::ConstLanelet & goal_lanelet)
 {
+  // return goal;
   const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal.position);
+  const double distance = boost::geometry::distance(
+    goal_lanelet.polygon2d().basicPolygon(), lanelet::utils::to2D(lanelet_point).basicPoint());
+  if (distance < std::numeric_limits<double>::epsilon()) {
+    return goal;
+  }
+
   const auto segment = lanelet::utils::getClosestSegment(
     lanelet::utils::to2D(lanelet_point), goal_lanelet.centerline());
   if (segment.empty()) {