diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp
index 7b7feefd364ba..086bd66acba2d 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/path_with_lane_id.hpp
@@ -201,13 +201,18 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
  * @return index of insert point
  */
 template <>
-inline size_t insertTargetPoint(
+inline boost::optional<size_t> insertTargetPoint(
   const size_t seg_idx, const geometry_msgs::msg::Point & p_target,
   std::vector<autoware_auto_planning_msgs::msg::PathPointWithLaneId> & points,
   const double overlap_threshold)
 {
   validateNonEmpty(points);
 
+  // invalid segment index
+  if (seg_idx + 1 >= points.size()) {
+    return {};
+  }
+
   const auto p_front = getPoint(points.at(seg_idx));
   const auto p_back = getPoint(points.at(seg_idx + 1));
 
@@ -215,6 +220,7 @@ inline size_t insertTargetPoint(
     validateNonSharpAngle(p_front, p_target, p_back);
   } catch (const std::exception & e) {
     std::cerr << e.what() << std::endl;
+    return {};
   }
 
   const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold;
diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp
index 316b171928d2c..b77f52a964ef7 100644
--- a/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp
+++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/trajectory/trajectory.hpp
@@ -610,12 +610,17 @@ inline boost::optional<geometry_msgs::msg::Pose> calcLongitudinalOffsetPose(
  * @return index of insert point
  */
 template <class T>
-inline size_t insertTargetPoint(
+inline boost::optional<size_t> insertTargetPoint(
   const size_t seg_idx, const geometry_msgs::msg::Point & p_target, T & points,
   const double overlap_threshold = 1e-3)
 {
   validateNonEmpty(points);
 
+  // invalid segment index
+  if (seg_idx + 1 >= points.size()) {
+    return {};
+  }
+
   const auto p_front = getPoint(points.at(seg_idx));
   const auto p_back = getPoint(points.at(seg_idx + 1));
 
@@ -623,6 +628,7 @@ inline size_t insertTargetPoint(
     validateNonSharpAngle(p_front, p_target, p_back);
   } catch (const std::exception & e) {
     std::cerr << e.what() << std::endl;
+    return {};
   }
 
   const auto overlap_with_front = calcDistance2d(p_target, p_front) < overlap_threshold;
diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp
index 6c56ed700d6a8..b1c25d2424e90 100644
--- a/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp
+++ b/common/tier4_autoware_utils/test/src/trajectory/test_path_with_lane_id.cpp
@@ -410,7 +410,8 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -418,7 +419,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -447,7 +448,8 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -455,7 +457,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -484,7 +486,8 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -492,7 +495,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -521,7 +524,8 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -537,7 +541,8 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -555,12 +560,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
     EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
-    EXPECT_EQ(insert_idx, base_idx + 1);
-    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
-
-    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
-      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
-    }
+    EXPECT_EQ(insert_idx, boost::none);
   }
 
   // Invalid target point(Behind of end point)
@@ -573,12 +573,7 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
     EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
-    EXPECT_EQ(insert_idx, base_idx + 1);
-    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
-
-    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
-      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
-    }
+    EXPECT_EQ(insert_idx, boost::none);
   }
 
   // Invalid target point(Huge lateral offset)
@@ -591,12 +586,17 @@ TEST(trajectory, insertTargetPoint_PathWithLaneId)
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
     EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
-    EXPECT_EQ(insert_idx, base_idx + 1);
-    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+    EXPECT_EQ(insert_idx, boost::none);
+  }
 
-    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
-      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
-    }
+  // Invalid base index
+  {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(10.0, 0.0, 0.0);
+    const auto insert_idx = insertTargetPoint(9U, p_target, traj_out.points);
+
+    EXPECT_EQ(insert_idx, boost::none);
   }
 
   // Empty
@@ -630,7 +630,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
     const auto insert_idx =
       insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -639,7 +640,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -669,7 +670,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
     const auto insert_idx =
       insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
 
-    EXPECT_EQ(insert_idx, base_idx);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -687,7 +689,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold_PathWithLaneId)
     const auto insert_idx =
       insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
diff --git a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp
index 8fee29d0524e1..4025e5838841b 100644
--- a/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp
+++ b/common/tier4_autoware_utils/test/src/trajectory/test_trajectory.cpp
@@ -1493,7 +1493,8 @@ TEST(trajectory, insertTargetPoint)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -1501,7 +1502,7 @@ TEST(trajectory, insertTargetPoint)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -1530,7 +1531,8 @@ TEST(trajectory, insertTargetPoint)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -1538,7 +1540,7 @@ TEST(trajectory, insertTargetPoint)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -1567,7 +1569,8 @@ TEST(trajectory, insertTargetPoint)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -1575,7 +1578,7 @@ TEST(trajectory, insertTargetPoint)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(-30.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -1604,7 +1607,8 @@ TEST(trajectory, insertTargetPoint)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -1620,7 +1624,8 @@ TEST(trajectory, insertTargetPoint)
     const size_t base_idx = findNearestSegmentIndex(traj.points, p_target);
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -1638,12 +1643,7 @@ TEST(trajectory, insertTargetPoint)
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
     EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
-    EXPECT_EQ(insert_idx, base_idx + 1);
-    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
-
-    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
-      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
-    }
+    EXPECT_EQ(insert_idx, boost::none);
   }
 
   // Invalid target point(Behind of end point)
@@ -1656,12 +1656,7 @@ TEST(trajectory, insertTargetPoint)
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
     EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
-    EXPECT_EQ(insert_idx, base_idx + 1);
-    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
-
-    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
-      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
-    }
+    EXPECT_EQ(insert_idx, boost::none);
   }
 
   // Invalid target point(Huge lateral offset)
@@ -1674,12 +1669,17 @@ TEST(trajectory, insertTargetPoint)
     const auto insert_idx = insertTargetPoint(base_idx, p_target, traj_out.points);
 
     EXPECT_STREQ(testing::internal::GetCapturedStderr().c_str(), "Sharp angle.\n");
-    EXPECT_EQ(insert_idx, base_idx + 1);
-    EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
+    EXPECT_EQ(insert_idx, boost::none);
+  }
 
-    for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
-      EXPECT_TRUE(calcDistance2d(traj_out.points.at(i), traj_out.points.at(i + 1)) > 1e-3);
-    }
+  // Invalid base index
+  {
+    auto traj_out = traj;
+
+    const auto p_target = createPoint(10.0, 0.0, 0.0);
+    const auto insert_idx = insertTargetPoint(9U, p_target, traj_out.points);
+
+    EXPECT_EQ(insert_idx, boost::none);
   }
 
   // Empty
@@ -1713,7 +1713,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
     const auto insert_idx =
       insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size() + 1);
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -1722,7 +1723,7 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
     }
 
     {
-      const auto p_insert = getPose(traj_out.points.at(insert_idx));
+      const auto p_insert = getPose(traj_out.points.at(insert_idx.get()));
       const auto ans_quat = createQuaternionFromRPY(deg2rad(0.0), deg2rad(0.0), deg2rad(0.0));
       EXPECT_EQ(p_insert.position.x, p_target.x);
       EXPECT_EQ(p_insert.position.y, p_target.y);
@@ -1752,7 +1753,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
     const auto insert_idx =
       insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
 
-    EXPECT_EQ(insert_idx, base_idx);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {
@@ -1770,7 +1772,8 @@ TEST(trajectory, insertTargetPoint_OverlapThreshold)
     const auto insert_idx =
       insertTargetPoint(base_idx, p_target, traj_out.points, overlap_threshold);
 
-    EXPECT_EQ(insert_idx, base_idx + 1);
+    EXPECT_NE(insert_idx, boost::none);
+    EXPECT_EQ(insert_idx.get(), base_idx + 1);
     EXPECT_EQ(traj_out.points.size(), traj.points.size());
 
     for (size_t i = 0; i < traj_out.points.size() - 1; ++i) {