diff --git a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp
index fb49a7fa446be..d119acf09b6ea 100644
--- a/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp
+++ b/planning/obstacle_avoidance_planner/include/obstacle_avoidance_planner/utils/trajectory_utils.hpp
@@ -129,6 +129,8 @@ ReferencePoint convertToReferencePoint(const TrajectoryPoint & traj_point);
 std::vector<ReferencePoint> convertToReferencePoints(
   const std::vector<TrajectoryPoint> & traj_points);
 
+std::vector<ReferencePoint> sanitizePoints(const std::vector<ReferencePoint> & points);
+
 void compensateLastPose(
   const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
   const double delta_dist_threshold, const double delta_yaw_threshold);
diff --git a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
index 7057a2885fd52..94dc62b1335d4 100644
--- a/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
+++ b/planning/obstacle_avoidance_planner/src/mpt_optimizer.cpp
@@ -567,6 +567,9 @@ std::vector<ReferencePoint> MPTOptimizer::calcReferencePoints(
   ref_points = motion_utils::cropPoints(
     ref_points, p.ego_pose.position, ego_seg_idx, forward_traj_length + tmp_margin,
     backward_traj_length + tmp_margin);
+
+  // remove repeated points
+  ref_points = trajectory_utils::sanitizePoints(ref_points);
   SplineInterpolationPoints2d ref_points_spline(ref_points);
   ego_seg_idx = trajectory_utils::findEgoSegmentIndex(ref_points, p.ego_pose, ego_nearest_param_);
 
@@ -586,6 +589,7 @@ std::vector<ReferencePoint> MPTOptimizer::calcReferencePoints(
   // NOTE: This must be after backward cropping.
   //       New start point may be added and resampled. Spline calculation is required.
   updateFixedPoint(ref_points);
+  ref_points = trajectory_utils::sanitizePoints(ref_points);
   ref_points_spline = SplineInterpolationPoints2d(ref_points);
 
   // 6. update bounds
diff --git a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp
index 453d55bd7f770..651b11b28e8bc 100644
--- a/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp
+++ b/planning/obstacle_avoidance_planner/src/utils/trajectory_utils.cpp
@@ -79,6 +79,24 @@ std::vector<ReferencePoint> convertToReferencePoints(
   return ref_points;
 }
 
+std::vector<ReferencePoint> sanitizePoints(const std::vector<ReferencePoint> & points)
+{
+  std::vector<ReferencePoint> output;
+  for (size_t i = 0; i < points.size(); i++) {
+    if (i > 0) {
+      const auto & current_pos = points.at(i).pose.position;
+      const auto & prev_pos = points.at(i - 1).pose.position;
+      if (
+        std::fabs(current_pos.x - prev_pos.x) < 1e-6 &&
+        std::fabs(current_pos.y - prev_pos.y) < 1e-6) {
+        continue;
+      }
+    }
+    output.push_back(points.at(i));
+  }
+  return output;
+}
+
 void compensateLastPose(
   const PathPoint & last_path_point, std::vector<TrajectoryPoint> & traj_points,
   const double delta_dist_threshold, const double delta_yaw_threshold)