diff --git a/planning/behavior_velocity_intersection_module/README.md b/planning/behavior_velocity_intersection_module/README.md
index c47e6d89db02e..0565d0b974914 100644
--- a/planning/behavior_velocity_intersection_module/README.md
+++ b/planning/behavior_velocity_intersection_module/README.md
@@ -86,6 +86,14 @@ The parameters `collision_detection.collision_start_margin_time` and `collision_
 
 If collision is detected, the state transits to "STOP" immediately. On the other hand, the state does not transit to "GO" unless safe judgement continues for a certain period `collision_detection.state_transit_margin` to prevent the chattering of decisions.
 
+Currently, the intersection module uses `motion_velocity_smoother` feature to precisely calculate ego vehicle velocity profile along the intersection lane under longitudinal/lateral constraints. If the flag `collision_detection.use_upstream_velocity` is true, the target velocity profile of the original path is used. Otherwise the target velocity is set to `common.intersection_velocity`. In the trajectory smoothing process the target velocity at/before ego trajectory points are set to ego current velocity. The smoothed trajectory is then converted to an array of (time, distance) which indicates the arrival time to each trajectory point on the path from current ego position. You can visualize this array by adding the lane id to `debug.ttc` and running
+
+```bash
+ros2 run behavior_velocity_intersection_module ttc.py --lane_id <lane_id>
+```
+
+![ego ttc profile](./docs/ttc.gif)
+
 #### Stop Line Automatic Generation
 
 If a stopline is associated with the intersection lane on the map, that line is used as the stopline for collision detection. Otherwise the path is interpolated at a certain intervals (=`common.path_interpolation_ds`), and the point which is `stop_line_margin` meters behind the attention area is defined as the position of the stop line for the vehicle front.
diff --git a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
index 266ab5a581b3a..13662b7f0a32d 100644
--- a/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
+++ b/planning/behavior_velocity_intersection_module/config/intersection.param.yaml
@@ -21,19 +21,21 @@
         timeout_private_area: 3.0 # [s] cancel stuck vehicle stop in private area
 
       collision_detection:
-        state_transit_margin_time: 1.0
+        state_transit_margin_time: 0.5
         min_predicted_path_confidence: 0.05
         minimum_ego_predicted_velocity: 1.388 # [m/s]
         fully_prioritized:
           collision_start_margin_time: 2.0
           collision_end_margin_time: 0.0
         partially_prioritized:
-          collision_start_margin_time: 2.0
+          collision_start_margin_time: 3.0
           collision_end_margin_time: 2.0
         not_prioritized:
-          collision_start_margin_time: 4.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
-          collision_end_margin_time: 6.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
+          collision_start_margin_time: 3.0 # [s] this + state_transit_margin_time should be higher to account for collision with fast/accelerating object
+          collision_end_margin_time: 2.0 # [s] this + state_transit_margin_time should be higher to account for collision with slow/decelerating object
         keep_detection_vel_thr: 0.833 # == 3.0km/h. keep detection if ego is ego.vel < keep_detection_vel_thr
+        use_upstream_velocity: true # flag to use the planned velocity profile from the upstream module
+        minimum_upstream_velocity: 0.01 # [m/s] minimum velocity to avoid null division for the stop line from the upstream velocity
 
       occlusion:
         enable: false
@@ -57,7 +59,7 @@
           maximum_peeking_distance: 6.0 # [m]
         attention_lane_crop_curvature_threshold: 0.25
         attention_lane_curvature_calculation_ds: 0.5
-        static_occlusion_with_traffic_light_timeout: 3.5
+        static_occlusion_with_traffic_light_timeout: 0.5
 
       enable_rtc:
         intersection: true # If set to true, the scene modules require approval from the rtc (request to cooperate) function. If set to false, the modules can be executed without requiring rtc approval
diff --git a/planning/behavior_velocity_intersection_module/docs/ttc.gif b/planning/behavior_velocity_intersection_module/docs/ttc.gif
new file mode 100644
index 0000000000000..bb797f7df59a1
Binary files /dev/null and b/planning/behavior_velocity_intersection_module/docs/ttc.gif differ
diff --git a/planning/behavior_velocity_intersection_module/package.xml b/planning/behavior_velocity_intersection_module/package.xml
index 1cbecb636b457..0c9b3407d0f38 100644
--- a/planning/behavior_velocity_intersection_module/package.xml
+++ b/planning/behavior_velocity_intersection_module/package.xml
@@ -9,6 +9,7 @@
   <maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
   <maintainer email="tomoya.kimura@tier4.jp">Tomoya Kimura</maintainer>
   <maintainer email="shumpei.wakabayashi@tier4.jp">Shumpei Wakabayashi</maintainer>
+  <maintainer email="kyoichi.sugahara@tier4.jp">Kyoichi Sugahara</maintainer>
 
   <license>Apache License 2.0</license>
 
diff --git a/planning/behavior_velocity_intersection_module/scripts/ttc.py b/planning/behavior_velocity_intersection_module/scripts/ttc.py
new file mode 100755
index 0000000000000..6d1593d95f055
--- /dev/null
+++ b/planning/behavior_velocity_intersection_module/scripts/ttc.py
@@ -0,0 +1,290 @@
+#!/usr/bin/env python3
+
+# Copyright 2023 TIER IV, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import argparse
+from dataclasses import dataclass
+from itertools import cycle
+import math
+from threading import Lock
+import time
+
+import imageio
+import matplotlib
+import matplotlib.pyplot as plt
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from tier4_debug_msgs.msg import Float64MultiArrayStamped
+
+matplotlib.use("TKAgg")
+
+
+@dataclass
+class NPC:
+    x: float
+    y: float
+    th: float
+    width: float
+    height: float
+    speed: float
+    dangerous: bool
+    ref_object_enter_time: float
+    ref_object_exit_time: float
+    collision_start_time: float
+    collision_start_dist: float
+    collision_end_time: float
+    collision_end_dist: float
+    pred_x: list[float]
+    pred_y: list[float]
+
+    def __init__(self, data: list[float]):
+        self.x = data[0]
+        self.y = data[1]
+        self.th = data[2]
+        self.width = data[3]
+        self.height = data[4]
+        self.speed = data[5]
+        self.dangerous = bool(int(data[6]))
+        self.ref_object_enter_time = data[7]
+        self.ref_object_exit_time = data[8]
+        self.collision_start_time = data[9]
+        self.collision_start_dist = data[10]
+        self.collision_end_time = data[11]
+        self.collision_end_dist = data[12]
+        self.first_collision_x = data[13]
+        self.first_collision_y = data[14]
+        self.last_collision_x = data[15]
+        self.last_collision_y = data[16]
+        self.pred_x = data[17:58:2]
+        self.pred_y = data[18:58:2]
+
+
+class TTCVisualizer(Node):
+    def __init__(self, args):
+        super().__init__("ttc_visualizer")
+        self.ttc_dist_pub = self.create_subscription(
+            Float64MultiArrayStamped,
+            "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/ego_ttc",
+            self.on_ego_ttc,
+            1,
+        )
+        self.ttc_time_pub = self.create_subscription(
+            Float64MultiArrayStamped,
+            "/planning/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/debug/intersection/object_ttc",
+            self.on_object_ttc,
+            1,
+        )
+        self.args = args
+        self.lane_id = args.lane_id
+        self.ego_ttc_data = None
+        self.object_ttc_data = None
+        self.npc_vehicles = []
+        self.images = []
+        self.last_sub = time.time()
+
+        self.plot_timer = self.create_timer(0.2, self.on_plot_timer)
+        self.fig = plt.figure(figsize=(13, 6))
+        self.ttc_ax = self.fig.add_subplot(1, 2, 1)
+        self.ttc_vel_ax = self.ttc_ax.twinx()
+        self.world_ax = self.fig.add_subplot(1, 2, 2)
+        self.lock = Lock()
+        self.color_list = [
+            "#e41a1c",
+            "#377eb8",
+            "#4daf4a",
+            "#984ea3",
+            "#ff7f00",
+            "#ffff33",
+            "#a65628",
+            "#f781bf",
+        ]
+        plt.ion()
+        plt.show(block=False)
+
+    def plot_ttc(self):
+        self.ttc_ax.cla()
+        self.ttc_vel_ax.cla()
+
+        n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size)
+        ego_ttc_time = self.ego_ttc_data.data[n_ttc_data : 2 * n_ttc_data]
+        ego_ttc_dist = self.ego_ttc_data.data[2 * n_ttc_data : 3 * n_ttc_data]
+
+        self.ttc_ax.grid()
+        self.ttc_ax.set_xlabel("ego time")
+        self.ttc_ax.set_ylabel("ego dist")
+        time_dist_plot = self.ttc_ax.plot(ego_ttc_time, ego_ttc_dist, label="time-dist", c="orange")
+        self.ttc_ax.set_xlim(
+            min(ego_ttc_time) - 2.0,
+            min(max(ego_ttc_time) + 3.0, self.args.max_time),
+        )
+        # self.ttc_ax.set_ylim(min(ego_ttc_dist) - 2.0, max(ego_ttc_dist) + 3.0)
+        for npc, color in zip(self.npc_vehicles, cycle(self.color_list)):
+            t0, t1 = npc.collision_start_time, npc.collision_end_time
+            d0, d1 = npc.collision_start_dist, npc.collision_end_dist
+            self.ttc_ax.fill(
+                [t0, t0, t1, t1, 0, 0],
+                [d0, 0, 0, d1, d1, d0],
+                c=color,
+                alpha=0.2,
+            )
+        dd = [d1 - d0 for d0, d1 in zip(ego_ttc_dist, ego_ttc_dist[1:])]
+        dt = [t1 - t0 for t0, t1 in zip(ego_ttc_time, ego_ttc_time[1:])]
+        v = [d / t for d, t in zip(dd, dt)]
+        self.ttc_vel_ax.yaxis.set_label_position("right")
+        self.ttc_vel_ax.set_ylabel("ego velocity")
+        # self.ttc_vel_ax.set_ylim(0.0, max(v) + 1.0)
+        time_velocity_plot = self.ttc_vel_ax.plot(ego_ttc_time[1:], v, label="time-v", c="red")
+        lines = time_dist_plot + time_velocity_plot
+        labels = [line.get_label() for line in lines]
+        self.ttc_ax.legend(lines, labels, loc="upper left")
+
+    def plot_world(self):
+        detect_range = self.args.range
+        self.world_ax.cla()
+        n_ttc_data = int(self.ego_ttc_data.layout.dim[1].size)
+        ego_path_x = self.ego_ttc_data.data[3 * n_ttc_data : 4 * n_ttc_data]
+        ego_path_y = self.ego_ttc_data.data[4 * n_ttc_data : 5 * n_ttc_data]
+        self.world_ax.set_aspect("equal")
+        self.world_ax.scatter(ego_path_x[0], ego_path_y[0], marker="x", c="red", s=15)
+        min_x, max_x = min(ego_path_x), max(ego_path_x)
+        min_y, max_y = min(ego_path_y), max(ego_path_y)
+        x_dir = 1 if ego_path_x[-1] > ego_path_x[0] else -1
+        y_dir = 1 if ego_path_y[-1] > ego_path_y[0] else -1
+        min_x = min_x - detect_range if x_dir == 1 else min_x - 20
+        max_x = max_x + detect_range if x_dir == -1 else max_x + 20
+        min_y = min_y - detect_range if y_dir == 1 else min_y - 20
+        max_y = max_y + detect_range if y_dir == -1 else max_y + 20
+        self.world_ax.set_xlim(min_x, max_x)
+        self.world_ax.set_ylim(min_y, max_y)
+        arrows = [
+            (x0, y0, math.atan2(x1 - x0, y1 - y0))
+            for (x0, y0, x1, y1) in zip(
+                ego_path_x[0:-1:5],
+                ego_path_y[0:-1:5],
+                ego_path_x[4:-1:5],
+                ego_path_y[4:-1:5],
+            )
+        ]
+        for x, y, th in arrows:
+            self.world_ax.arrow(
+                x,
+                y,
+                math.sin(th) * 0.5,
+                math.cos(th) * 0.5,
+                head_width=0.1,
+                head_length=0.1,
+            )
+
+        for npc, color in zip(self.npc_vehicles, cycle(self.color_list)):
+            x, y, th, w, h = npc.x, npc.y, npc.th, npc.width, npc.height
+            bbox = np.array(
+                [
+                    [-w / 2, -h / 2],
+                    [+w / 2, -h / 2],
+                    [+w / 2, +h / 2],
+                    [-w / 2, +h / 2],
+                    [-w / 2, -h / 2],
+                ]
+            ).transpose()
+            Rth = np.array([[math.cos(th), -math.sin(th)], [math.sin(th), math.cos(th)]])
+            bbox_rot = Rth @ bbox
+            self.world_ax.fill(bbox_rot[0, :] + x, bbox_rot[1, :] + y, color, alpha=0.5)
+            self.world_ax.plot(
+                [npc.first_collision_x, npc.last_collision_x],
+                [npc.first_collision_y, npc.last_collision_y],
+                c=color,
+                linewidth=3.0,
+            )
+            if npc.dangerous:
+                self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linewidth=1.5)
+            else:
+                self.world_ax.plot(npc.pred_x, npc.pred_y, c=color, linestyle="--")
+
+        self.world_ax.plot(ego_path_x, ego_path_y, c="k", linestyle="--")
+
+    def cleanup(self):
+        if self.args.save:
+            kwargs_write = {"fps": self.args.fps, "quantizer": "nq"}
+            imageio.mimsave("./" + self.args.gif + ".gif", self.images, **kwargs_write)
+        rclpy.shutdown()
+
+    def on_plot_timer(self):
+        with self.lock:
+            if (not self.ego_ttc_data) or (not self.object_ttc_data):
+                return
+
+            if not self.last_sub:
+                return
+
+            now = time.time()
+            if (now - self.last_sub) > 1.0:
+                print("elapsed more than 1sec from last sub, exit/save fig")
+                self.cleanup()
+
+            self.plot_ttc()
+            self.plot_world()
+            self.fig.canvas.flush_events()
+
+            if self.args.save:
+                image = np.frombuffer(self.fig.canvas.tostring_rgb(), dtype="uint8")
+                image = image.reshape(self.fig.canvas.get_width_height()[::-1] + (3,))
+                self.images.append(image)
+
+    def on_ego_ttc(self, msg):
+        with self.lock:
+            if int(msg.data[0]) == self.lane_id:
+                self.ego_ttc_data = msg
+                self.last_sub = time.time()
+
+    def parse_npc_vehicles(self):
+        self.npc_vehicles = []
+        n_npc_vehicles = int(self.object_ttc_data.layout.dim[0].size)
+        npc_data_size = int(self.object_ttc_data.layout.dim[1].size)
+        for i in range(1, n_npc_vehicles):
+            data = self.object_ttc_data.data[i * npc_data_size : (i + 1) * npc_data_size]
+            self.npc_vehicles.append(NPC(data))
+
+    def on_object_ttc(self, msg):
+        with self.lock:
+            if int(msg.data[0]) == self.lane_id:
+                self.object_ttc_data = msg
+                self.parse_npc_vehicles()
+                self.last_sub = time.time()
+
+
+if __name__ == "__main__":
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "--lane_id",
+        type=int,
+        required=True,
+        help="lane_id to analyze",
+    )
+    parser.add_argument(
+        "--range",
+        type=float,
+        default=60,
+        help="detect range for drawing",
+    )
+    parser.add_argument("--max_time", type=float, default=100, help="max plot limit for time")
+    parser.add_argument("-s", "--save", action="store_true", help="flag to save gif")
+    parser.add_argument("--gif", type=str, default="ttc", help="filename of gif file")
+    parser.add_argument("--fps", type=float, default=5, help="fps of gif")
+    args = parser.parse_args()
+
+    rclpy.init()
+    visualizer = TTCVisualizer(args)
+    rclpy.spin(visualizer)
diff --git a/planning/behavior_velocity_intersection_module/src/manager.cpp b/planning/behavior_velocity_intersection_module/src/manager.cpp
index c2761e2cefb86..f2f0951d65357 100644
--- a/planning/behavior_velocity_intersection_module/src/manager.cpp
+++ b/planning/behavior_velocity_intersection_module/src/manager.cpp
@@ -100,6 +100,10 @@ IntersectionModuleManager::IntersectionModuleManager(rclcpp::Node & node)
     ns + ".collision_detection.not_prioritized.collision_end_margin_time");
   ip.collision_detection.keep_detection_vel_thr =
     node.declare_parameter<double>(ns + ".collision_detection.keep_detection_vel_thr");
+  ip.collision_detection.use_upstream_velocity =
+    node.declare_parameter<bool>(ns + ".collision_detection.use_upstream_velocity");
+  ip.collision_detection.minimum_upstream_velocity =
+    node.declare_parameter<double>(ns + ".collision_detection.minimum_upstream_velocity");
 
   ip.occlusion.enable = node.declare_parameter<bool>(ns + ".occlusion.enable");
   ip.occlusion.occlusion_attention_area_length =
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
index 1be9bdf311ea6..5f9461546bd2d 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.cpp
@@ -1081,7 +1081,9 @@ IntersectionModule::DecisionResult IntersectionModule::modifyPathVelocityDetail(
                                       collision_state_machine_.getDuration());
 
   const bool has_collision = checkCollision(
-    *path, &target_objects, path_lanelets, closest_idx, time_to_restart, traffic_prioritized_level);
+    *path, &target_objects, path_lanelets, closest_idx,
+    std::min<size_t>(occlusion_stop_line_idx, path->points.size() - 1), time_to_restart,
+    traffic_prioritized_level);
   collision_state_machine_.setStateWithMarginTime(
     has_collision ? StateMachine::State::STOP : StateMachine::State::GO,
     logger_.get_child("collision state_machine"), *clock_);
@@ -1281,8 +1283,8 @@ util::TargetObjects IntersectionModule::generateTargetObjects(
 bool IntersectionModule::checkCollision(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
-  const int closest_idx, const double time_delay,
-  const util::TrafficPrioritizedLevel & traffic_prioritized_level)
+  const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+  const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level)
 {
   using lanelet::utils::getArcCoordinates;
   using lanelet::utils::getPolygonFromArcLength;
@@ -1290,9 +1292,11 @@ bool IntersectionModule::checkCollision(
   // check collision between target_objects predicted path and ego lane
   // cut the predicted path at passing_time
   const auto time_distance_array = util::calcIntersectionPassingTime(
-    path, planner_data_, associative_ids_, closest_idx, time_delay,
-    planner_param_.common.intersection_velocity,
-    planner_param_.collision_detection.minimum_ego_predicted_velocity);
+    path, planner_data_, associative_ids_, closest_idx, last_intersection_stop_line_candidate_idx,
+    time_delay, planner_param_.common.intersection_velocity,
+    planner_param_.collision_detection.minimum_ego_predicted_velocity,
+    planner_param_.collision_detection.use_upstream_velocity,
+    planner_param_.collision_detection.minimum_upstream_velocity);
   const double passing_time = time_distance_array.back().first;
   util::cutPredictPathWithDuration(target_objects, clock_, passing_time);
 
diff --git a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
index ecd9ccf877965..8c09636678a01 100644
--- a/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
+++ b/planning/behavior_velocity_intersection_module/src/scene_intersection.hpp
@@ -94,6 +94,8 @@ class IntersectionModule : public SceneModuleInterface
         double collision_end_margin_time;    //! end margin time to check collision
       } not_prioritized;
       double keep_detection_vel_thr;  //! keep detection if ego is ego.vel < keep_detection_vel_thr
+      bool use_upstream_velocity;
+      double minimum_upstream_velocity;
     } collision_detection;
     struct Occlusion
     {
@@ -300,8 +302,8 @@ class IntersectionModule : public SceneModuleInterface
   bool checkCollision(
     const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
     util::TargetObjects * target_objects, const util::PathLanelets & path_lanelets,
-    const int closest_idx, const double time_delay,
-    const util::TrafficPrioritizedLevel & traffic_prioritized_level);
+    const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+    const double time_delay, const util::TrafficPrioritizedLevel & traffic_prioritized_level);
 
   OcclusionType getOcclusionStatus(
     const nav_msgs::msg::OccupancyGrid & occ_grid,
diff --git a/planning/behavior_velocity_intersection_module/src/util.cpp b/planning/behavior_velocity_intersection_module/src/util.cpp
index 8f2889726d365..6d55956a27a3c 100644
--- a/planning/behavior_velocity_intersection_module/src/util.cpp
+++ b/planning/behavior_velocity_intersection_module/src/util.cpp
@@ -33,6 +33,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <limits>
 #include <list>
 #include <memory>
 #include <set>
@@ -1273,18 +1274,34 @@ void cutPredictPathWithDuration(
 TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const std::set<int> & associative_ids,
-  const int closest_idx, const double time_delay, const double intersection_velocity,
-  const double minimum_ego_velocity)
+  const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+  const double time_delay, const double intersection_velocity, const double minimum_ego_velocity,
+  const bool use_upstream_velocity, const double minimum_upstream_velocity)
 {
-  double dist_sum = 0.0;
+  const double current_velocity = planner_data->current_velocity->twist.linear.x;
+
   int assigned_lane_found = false;
 
   // crop intersection part of the path, and set the reference velocity to intersection_velocity
   // for ego's ttc
   PathWithLaneId reference_path;
-  for (size_t i = closest_idx; i < path.points.size(); ++i) {
+  std::optional<size_t> upstream_stop_line{std::nullopt};
+  for (size_t i = 0; i < path.points.size() - 1; ++i) {
     auto reference_point = path.points.at(i);
-    reference_point.point.longitudinal_velocity_mps = intersection_velocity;
+    // assume backward velocity is current ego velocity
+    if (i < closest_idx) {
+      reference_point.point.longitudinal_velocity_mps = current_velocity;
+    }
+    if (
+      i > last_intersection_stop_line_candidate_idx &&
+      std::fabs(reference_point.point.longitudinal_velocity_mps) <
+        std::numeric_limits<double>::epsilon() &&
+      !upstream_stop_line) {
+      upstream_stop_line = i;
+    }
+    if (!use_upstream_velocity) {
+      reference_point.point.longitudinal_velocity_mps = intersection_velocity;
+    }
     reference_path.points.push_back(reference_point);
     bool has_objective_lane_id = hasLaneIds(path.points.at(i), associative_ids);
     if (assigned_lane_found && !has_objective_lane_id) {
@@ -1296,18 +1313,46 @@ TimeDistanceArray calcIntersectionPassingTime(
     return {{0.0, 0.0}};  // has already passed the intersection.
   }
 
+  std::vector<std::pair<double, double>> original_path_xy;
+  for (size_t i = 0; i < reference_path.points.size(); ++i) {
+    const auto & p = reference_path.points.at(i).point.pose.position;
+    original_path_xy.emplace_back(p.x, p.y);
+  }
+
   // apply smoother to reference velocity
   PathWithLaneId smoothed_reference_path = reference_path;
-  smoothPath(reference_path, smoothed_reference_path, planner_data);
+  if (!smoothPath(reference_path, smoothed_reference_path, planner_data)) {
+    smoothed_reference_path = reference_path;
+  }
 
   // calculate when ego is going to reach each (interpolated) points on the path
   TimeDistanceArray time_distance_array{};
-  dist_sum = 0.0;
+  double dist_sum = 0.0;
   double passing_time = time_delay;
   time_distance_array.emplace_back(passing_time, dist_sum);
-  for (size_t i = 1; i < smoothed_reference_path.points.size(); ++i) {
-    const auto & p1 = smoothed_reference_path.points.at(i - 1);
-    const auto & p2 = smoothed_reference_path.points.at(i);
+
+  // NOTE: `reference_path` is resampled in `reference_smoothed_path`, so
+  // `last_intersection_stop_line_candidate_idx` makes no sense
+  const auto smoothed_path_closest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    smoothed_reference_path.points, path.points.at(closest_idx).point.pose,
+    planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold);
+
+  const std::optional<size_t> upstream_stop_line_idx_opt = [&]() -> std::optional<size_t> {
+    if (upstream_stop_line) {
+      const auto upstream_stop_line_point = path.points.at(upstream_stop_line.value()).point.pose;
+      return motion_utils::findFirstNearestIndexWithSoftConstraints(
+        smoothed_reference_path.points, upstream_stop_line_point,
+        planner_data->ego_nearest_dist_threshold, planner_data->ego_nearest_yaw_threshold);
+    } else {
+      return std::nullopt;
+    }
+  }();
+  const bool has_upstream_stopline = upstream_stop_line_idx_opt.has_value();
+  const size_t upstream_stopline_ind = upstream_stop_line_idx_opt.value_or(0);
+
+  for (size_t i = smoothed_path_closest_idx; i < smoothed_reference_path.points.size() - 1; ++i) {
+    const auto & p1 = smoothed_reference_path.points.at(i);
+    const auto & p2 = smoothed_reference_path.points.at(i + 1);
 
     const double dist = tier4_autoware_utils::calcDistance2d(p1, p2);
     dist_sum += dist;
@@ -1315,14 +1360,20 @@ TimeDistanceArray calcIntersectionPassingTime(
     // use average velocity between p1 and p2
     const double average_velocity =
       (p1.point.longitudinal_velocity_mps + p2.point.longitudinal_velocity_mps) / 2.0;
-    passing_time +=
-      (dist / std::max<double>(
-                minimum_ego_velocity,
-                average_velocity));  // to avoid zero-division
+    const double passing_velocity = [=]() {
+      if (use_upstream_velocity) {
+        if (has_upstream_stopline && i > upstream_stopline_ind) {
+          return minimum_upstream_velocity;
+        }
+        return std::max<double>(average_velocity, minimum_ego_velocity);
+      } else {
+        return std::max<double>(average_velocity, minimum_ego_velocity);
+      }
+    }();
+    passing_time += (dist / passing_velocity);
 
     time_distance_array.emplace_back(passing_time, dist_sum);
   }
-
   return time_distance_array;
 }
 
diff --git a/planning/behavior_velocity_intersection_module/src/util.hpp b/planning/behavior_velocity_intersection_module/src/util.hpp
index e7ba2b00c749a..d1daf2a6e9114 100644
--- a/planning/behavior_velocity_intersection_module/src/util.hpp
+++ b/planning/behavior_velocity_intersection_module/src/util.hpp
@@ -141,8 +141,9 @@ void cutPredictPathWithDuration(
 TimeDistanceArray calcIntersectionPassingTime(
   const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
   const std::shared_ptr<const PlannerData> & planner_data, const std::set<int> & associative_ids,
-  const int closest_idx, const double time_delay, const double intersection_velocity,
-  const double minimum_ego_velocity);
+  const size_t closest_idx, const size_t last_intersection_stop_line_candidate_idx,
+  const double time_delay, const double intersection_velocity, const double minimum_ego_velocity,
+  const bool use_upstream_velocity, const double minimum_upstream_velocity);
 
 double calcDistanceUntilIntersectionLanelet(
   const lanelet::ConstLanelet & assigned_lanelet,
diff --git a/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
new file mode 100644
index 0000000000000..7fe8612cc6858
--- /dev/null
+++ b/planning/behavior_velocity_planner_common/src/utilization/trajectory_utils.cpp
@@ -0,0 +1,124 @@
+// Copyright 2023 TIER IV, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+// #include <behavior_velocity_planner_common/utilization/boost_geometry_helper.hpp>
+#include <behavior_velocity_planner_common/utilization/trajectory_utils.hpp>
+#include <motion_utils/trajectory/trajectory.hpp>
+#include <motion_velocity_smoother/trajectory_utils.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#include <autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp>
+#include <geometry_msgs/msg/quaternion.hpp>
+
+#include <tf2/utils.h>
+
+#ifdef ROS_DISTRO_GALACTIC
+#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#else
+#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+#endif
+
+#include <memory>
+
+namespace behavior_velocity_planner
+{
+using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
+using autoware_auto_planning_msgs::msg::PathWithLaneId;
+using autoware_auto_planning_msgs::msg::Trajectory;
+using autoware_auto_planning_msgs::msg::TrajectoryPoint;
+using TrajectoryPoints = std::vector<TrajectoryPoint>;
+using geometry_msgs::msg::Quaternion;
+using TrajectoryPointWithIdx = std::pair<TrajectoryPoint, size_t>;
+
+TrajectoryPoints convertPathToTrajectoryPoints(const PathWithLaneId & path)
+{
+  TrajectoryPoints tps;
+  for (const auto & p : path.points) {
+    TrajectoryPoint tp;
+    tp.pose = p.point.pose;
+    tp.longitudinal_velocity_mps = p.point.longitudinal_velocity_mps;
+    // since path point doesn't have acc for now
+    tp.acceleration_mps2 = 0;
+    tps.emplace_back(tp);
+  }
+  return tps;
+}
+PathWithLaneId convertTrajectoryPointsToPath(const TrajectoryPoints & trajectory)
+{
+  PathWithLaneId path;
+  for (const auto & p : trajectory) {
+    PathPointWithLaneId pp;
+    pp.point.pose = p.pose;
+    pp.point.longitudinal_velocity_mps = p.longitudinal_velocity_mps;
+    path.points.emplace_back(pp);
+  }
+  return path;
+}
+
+Quaternion lerpOrientation(const Quaternion & o_from, const Quaternion & o_to, const double ratio)
+{
+  tf2::Quaternion q_from, q_to;
+  tf2::fromMsg(o_from, q_from);
+  tf2::fromMsg(o_to, q_to);
+
+  const auto q_interpolated = q_from.slerp(q_to, ratio);
+  return tf2::toMsg(q_interpolated);
+}
+
+//! smooth path point with lane id starts from ego position on path to the path end
+bool smoothPath(
+  const PathWithLaneId & in_path, PathWithLaneId & out_path,
+  const std::shared_ptr<const PlannerData> & planner_data)
+{
+  const geometry_msgs::msg::Pose current_pose = planner_data->current_odometry->pose;
+  const double v0 = planner_data->current_velocity->twist.linear.x;
+  const double a0 = planner_data->current_acceleration->accel.accel.linear.x;
+  const auto & external_v_limit = planner_data->external_velocity_limit;
+  const auto & smoother = planner_data->velocity_smoother_;
+
+  auto trajectory = convertPathToTrajectoryPoints(in_path);
+  const auto traj_lateral_acc_filtered = smoother->applyLateralAccelerationFilter(trajectory);
+
+  const auto traj_steering_rate_limited =
+    smoother->applySteeringRateLimit(traj_lateral_acc_filtered, false);
+
+  // Resample trajectory with ego-velocity based interval distances
+  auto traj_resampled = smoother->resampleTrajectory(
+    traj_steering_rate_limited, v0, current_pose, planner_data->ego_nearest_dist_threshold,
+    planner_data->ego_nearest_yaw_threshold);
+  const size_t traj_resampled_closest = motion_utils::findFirstNearestIndexWithSoftConstraints(
+    traj_resampled, current_pose, planner_data->ego_nearest_dist_threshold,
+    planner_data->ego_nearest_yaw_threshold);
+  std::vector<TrajectoryPoints> debug_trajectories;
+  // Clip trajectory from closest point
+  TrajectoryPoints clipped;
+  TrajectoryPoints traj_smoothed;
+  clipped.insert(
+    clipped.end(), traj_resampled.begin() + traj_resampled_closest, traj_resampled.end());
+  if (!smoother->apply(v0, a0, clipped, traj_smoothed, debug_trajectories)) {
+    std::cerr << "[behavior_velocity][trajectory_utils]: failed to smooth" << std::endl;
+    return false;
+  }
+  traj_smoothed.insert(
+    traj_smoothed.begin(), traj_resampled.begin(), traj_resampled.begin() + traj_resampled_closest);
+
+  if (external_v_limit) {
+    motion_velocity_smoother::trajectory_utils::applyMaximumVelocityLimit(
+      traj_resampled_closest, traj_smoothed.size(), external_v_limit->max_velocity, traj_smoothed);
+  }
+  out_path = convertTrajectoryPointsToPath(traj_smoothed);
+  return true;
+}
+
+}  // namespace behavior_velocity_planner