diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
index b9136bc4002e3..7c9b5a5378ab6 100644
--- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
+++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp
@@ -607,8 +607,12 @@ double calcLateralOffset(
     return std::nan("");
   }
 
-  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx));
-  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(seg_idx + 1));
+  const auto p_indices = overlap_removed_points.size() - 2;
+  const auto p_front_idx = (p_indices > seg_idx) ? seg_idx : p_indices;
+  const auto p_back_idx = p_front_idx + 1;
+
+  const auto p_front = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_front_idx));
+  const auto p_back = tier4_autoware_utils::getPoint(overlap_removed_points.at(p_back_idx));
 
   const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0.0};
   const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0.0};