diff --git a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
index 8ba933fd2db40..b316bc08e063b 100644
--- a/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
+++ b/planning/behavior_velocity_planner/src/scene_module/occlusion_spot/occlusion_spot_utils.cpp
@@ -157,8 +157,9 @@ void clipPathByLength(
   const PathWithLaneId & path, PathWithLaneId & clipped, const double max_length)
 {
   double length_sum = 0;
-  for (int i = 0; i < static_cast<int>(path.points.size()) - 1; i++) {
-    length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
+  clipped.points.emplace_back(path.points.front());
+  for (int i = 1; i < static_cast<int>(path.points.size()); i++) {
+    length_sum += tier4_autoware_utils::calcDistance2d(path.points.at(i - 1), path.points.at(i));
     if (length_sum > max_length) return;
     clipped.points.emplace_back(path.points.at(i));
   }
diff --git a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp
index 7513346ed93d2..3139fc2847649 100644
--- a/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp
+++ b/planning/behavior_velocity_planner/src/utilization/path_utilization.cpp
@@ -106,6 +106,13 @@ bool splineInterpolate(
     output->points.front().point.pose.orientation = output->points.at(1).point.pose.orientation;
     output->points.back().point.pose.orientation = output->points.at(l - 2).point.pose.orientation;
   }
+
+  // insert final point on path if its position is not same point as interpolated
+  const auto & op = output->points.back().point.pose.position;
+  const auto & ip = input.points.back().point.pose.position;
+  if (std::hypot(op.x - ip.x, op.y - ip.y) > ep) {
+    output->points.emplace_back(input.points.back());
+  }
   return true;
 }