diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp
index eadfb72a42afa..53ef91473fe24 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/drivable_area_expansion.hpp
@@ -40,7 +40,7 @@ void expandDrivableArea(
 /// @param[in] expansion_polygons polygons to add to the drivable area
 /// @return expanded drivable area polygon
 polygon_t createExpandedDrivableAreaPolygon(
-  const PathWithLaneId & path, const multipolygon_t & expansion_polygons);
+  const PathWithLaneId & path, const multi_polygon_t & expansion_polygons);
 
 /// @brief Update the drivable area of the given path with the given polygon
 /// @details this function splits the polygon into a left and right bound and sets it in the path
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp
index 33bccf90ffbe8..70cc8a8bc5925 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/expansion.hpp
@@ -36,7 +36,7 @@ namespace drivable_area_expansion
 /// @return distance limit
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
-  const multilinestring_t & limit_lines);
+  const multi_linestring_t & limit_lines);
 
 /// @brief Calculate the distance limit required for the polygon to not cross the limit polygons.
 /// @details Calculate the minimum distance from base_ls to an intersection of limit_polygons and
@@ -47,7 +47,7 @@ double calculateDistanceLimit(
 /// @return distance limit
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
-  const multipolygon_t & limit_polygons);
+  const multi_polygon_t & limit_polygons);
 
 /// @brief Create a polygon from a base line with a given expansion distance
 /// @param[in] base_ls base linestring from which the polygon is created
@@ -64,9 +64,9 @@ polygon_t createExpansionPolygon(
 /// @param[in] uncrossable_lines lines that should not be crossed by the expanded drivable area
 /// @param[in] params expansion parameters
 /// @return expansion polygons
-multipolygon_t createExpansionPolygons(
-  const PathWithLaneId & path, const multipolygon_t & path_footprints,
-  const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines,
+multi_polygon_t createExpansionPolygons(
+  const PathWithLaneId & path, const multi_polygon_t & path_footprints,
+  const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines,
   const DrivableAreaExpansionParameters & params);
 
 /// @brief Create polygons for the area where the drivable area should be expanded
@@ -76,9 +76,9 @@ multipolygon_t createExpansionPolygons(
 /// @param[in] predicted_paths polygons of the dynamic objects' predicted paths
 /// @param[in] params expansion parameters
 /// @return expansion polygons
-multipolygon_t createExpansionLaneletPolygons(
+multi_polygon_t createExpansionLaneletPolygons(
   const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler,
-  const multipolygon_t & path_footprints, const multipolygon_t & predicted_paths,
+  const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths,
   const DrivableAreaExpansionParameters & params);
 }  // namespace drivable_area_expansion
 
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp
index 25f061affaa48..e5e1c76f2521f 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/footprints.hpp
@@ -55,7 +55,7 @@ polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t
 /// @param [in] objects objects from which to create polygons
 /// @param[in] params expansion parameters containing extra offsets to add to the dynamic objects
 /// @return footprint polygons of the object's predicted paths
-multipolygon_t createObjectFootprints(
+multi_polygon_t createObjectFootprints(
   const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
   const DrivableAreaExpansionParameters & params);
 
@@ -63,7 +63,7 @@ multipolygon_t createObjectFootprints(
 /// @param[in] path the path for which to create a footprint
 /// @param[in] params expansion parameters defining how to create the footprint
 /// @return footprint polygons of the path
-multipolygon_t createPathFootprints(
+multi_polygon_t createPathFootprints(
   const PathWithLaneId & path, const DrivableAreaExpansionParameters & params);
 }  // namespace drivable_area_expansion
 #endif  // BEHAVIOR_PATH_PLANNER__UTILS__DRIVABLE_AREA_EXPANSION__FOOTPRINTS_HPP_
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp
index 33c4cc8a0eff3..4524bd2be2299 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp
@@ -28,7 +28,7 @@ namespace drivable_area_expansion
 /// @param[in] lanelet_map lanelet map
 /// @param[in] uncrossable_types types that cannot be crossed
 /// @return the uncrossable linestrings
-multilinestring_t extractUncrossableLines(
+multi_linestring_t extractUncrossableLines(
   const lanelet::LaneletMap & lanelet_map, const std::vector<std::string> & uncrossable_types);
 
 /// @brief Determine if the given linestring has one of the given types
diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp
index e56ef4961589d..daabfa97598fd 100644
--- a/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp
+++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/drivable_area_expansion/types.hpp
@@ -30,13 +30,13 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId;
 using geometry_msgs::msg::Point;
 
 using point_t = tier4_autoware_utils::Point2d;
-using multipoint_t = tier4_autoware_utils::MultiPoint2d;
+using multi_point_t = tier4_autoware_utils::MultiPoint2d;
 using polygon_t = tier4_autoware_utils::Polygon2d;
 using ring_t = tier4_autoware_utils::LinearRing2d;
-using multipolygon_t = tier4_autoware_utils::MultiPolygon2d;
+using multi_polygon_t = tier4_autoware_utils::MultiPolygon2d;
 using segment_t = tier4_autoware_utils::Segment2d;
 using linestring_t = tier4_autoware_utils::LineString2d;
-using multilinestring_t = tier4_autoware_utils::MultiLineString2d;
+using multi_linestring_t = tier4_autoware_utils::MultiLineString2d;
 
 struct PointDistance
 {
diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
index 4101ac6ab8c98..e6fb4167bac20 100644
--- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/drivable_area_expansion.cpp
@@ -19,6 +19,9 @@
 #include "behavior_path_planner/utils/drivable_area_expansion/map_utils.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/parameters.hpp"
 #include "behavior_path_planner/utils/drivable_area_expansion/types.hpp"
+#include "interpolation/linear_interpolation.hpp"
+
+#include <Eigen/Geometry>
 
 #include <boost/geometry.hpp>
 
@@ -32,7 +35,7 @@ void expandDrivableArea(
 {
   const auto uncrossable_lines =
     extractUncrossableLines(*route_handler.getLaneletMapPtr(), params.avoid_linestring_types);
-  multilinestring_t uncrossable_lines_in_range;
+  multi_linestring_t uncrossable_lines_in_range;
   const auto & p = path.points.front().point.pose.position;
   for (const auto & line : uncrossable_lines)
     if (boost::geometry::distance(line, point_t{p.x, p.y}) < params.max_path_arc_length)
@@ -60,7 +63,7 @@ Point convert_point(const point_t & p)
 }
 
 polygon_t createExpandedDrivableAreaPolygon(
-  const PathWithLaneId & path, const multipolygon_t & expansion_polygons)
+  const PathWithLaneId & path, const multi_polygon_t & expansion_polygons)
 {
   polygon_t original_da_poly;
   original_da_poly.outer().reserve(path.left_bound.size() + path.right_bound.size() + 1);
@@ -69,7 +72,7 @@ polygon_t createExpandedDrivableAreaPolygon(
     original_da_poly.outer().push_back(convert_point(*it));
   original_da_poly.outer().push_back(original_da_poly.outer().front());
 
-  multipolygon_t unions;
+  multi_polygon_t unions;
   auto expanded_da_poly = original_da_poly;
   for (const auto & p : expansion_polygons) {
     unions.clear();
@@ -82,85 +85,164 @@ polygon_t createExpandedDrivableAreaPolygon(
   return expanded_da_poly;
 }
 
-std::array<ring_t::const_iterator, 4> findLeftRightRanges(
-  const PathWithLaneId & path, const ring_t & expanded_drivable_area)
+void copy_z_over_arc_length(
+  const std::vector<geometry_msgs::msg::Point> & from, std::vector<geometry_msgs::msg::Point> & to)
+{
+  if (from.empty() || to.empty()) return;
+  to.front().z = from.front().z;
+  if (from.size() < 2 || to.size() < 2) return;
+  to.back().z = from.back().z;
+  auto i_from = 1lu;
+  auto s_from = tier4_autoware_utils::calcDistance2d(from[0], from[1]);
+  auto s_to = 0.0;
+  auto s_from_prev = 0.0;
+  for (auto i_to = 1lu; i_to + 1 < to.size(); ++i_to) {
+    s_to += tier4_autoware_utils::calcDistance2d(to[i_to - 1], to[i_to]);
+    for (; s_from < s_to && i_from + 1 < from.size(); ++i_from) {
+      s_from_prev = s_from;
+      s_from += tier4_autoware_utils::calcDistance2d(from[i_from], from[i_from + 1]);
+    }
+    if (s_from - s_from_prev != 0.0) {
+      const auto ratio = (s_to - s_from_prev) / (s_from - s_from_prev);
+      to[i_to].z = interpolation::lerp(from[i_from - 1].z, from[i_from].z, ratio);
+    } else {
+      to[i_to].z = to[i_to - 1].z;
+    }
+  }
+}
+
+void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area)
 {
+  const auto original_left_bound = path.left_bound;
+  const auto original_right_bound = path.right_bound;
   const auto is_left_of_segment = [](const point_t & a, const point_t & b, const point_t & p) {
     return (b.x() - a.x()) * (p.y() - a.y()) - (b.y() - a.y()) * (p.x() - a.x()) > 0;
   };
-  const auto is_left_of_path_start = [&](const point_t & p) {
-    return is_left_of_segment(
-      convert_point(path.points[0].point.pose.position),
-      convert_point(path.points[1].point.pose.position), p);
+  // prepare delimiting lines: start and end of the original expanded drivable area
+  const auto start_segment =
+    segment_t{convert_point(path.left_bound.front()), convert_point(path.right_bound.front())};
+  const auto end_segment =
+    segment_t{convert_point(path.left_bound.back()), convert_point(path.right_bound.back())};
+  point_t start_segment_center;
+  boost::geometry::centroid(start_segment, start_segment_center);
+  const auto path_start_segment =
+    segment_t{start_segment_center, convert_point(path.points[1].point.pose.position)};
+  point_t end_segment_center;
+  boost::geometry::centroid(end_segment, end_segment_center);
+  const auto path_end_segment =
+    segment_t{convert_point(path.points.back().point.pose.position), end_segment_center};
+  const auto segment_to_line_intersection =
+    [](const auto p1, const auto p2, const auto q1, const auto q2) -> std::optional<point_t> {
+    const auto line = Eigen::Hyperplane<double, 2>::Through(q1, q2);
+    const auto segment = Eigen::Hyperplane<double, 2>::Through(p1, p2);
+    const auto intersection = line.intersection(segment);
+    std::optional<point_t> result;
+    const auto is_on_segment =
+      (p1.x() <= p2.x() ? intersection.x() >= p1.x() && intersection.x() <= p2.x()
+                        : intersection.x() <= p1.x() && intersection.x() >= p2.x()) &&
+      (p1.y() <= p2.y() ? intersection.y() >= p1.y() && intersection.y() <= p2.y()
+                        : intersection.y() <= p1.y() && intersection.y() >= p2.y());
+    if (is_on_segment) result = point_t{intersection.x(), intersection.y()};
+    return result;
   };
-  const auto is_left_of_path_end = [&, size = path.points.size()](const point_t & p) {
-    return is_left_of_segment(
-      convert_point(path.points[size - 2].point.pose.position),
-      convert_point(path.points[size - 1].point.pose.position), p);
+  // find intersection between the expanded drivable area and the delimiting lines
+  const auto & da = expanded_drivable_area.outer();
+  struct Intersection
+  {
+    point_t intersection_point;
+    ring_t::const_iterator segment_it;
+    double distance = std::numeric_limits<double>::max();
+    explicit Intersection(ring_t::const_iterator it) : segment_it(it) {}
+    void update(const point_t & p, const ring_t::const_iterator & it, const double dist)
+    {
+      intersection_point = p;
+      segment_it = it;
+      distance = dist;
+    }
   };
-  const auto dist_to_path_start = [start = convert_point(path.points.front().point.pose.position)](
-                                    const auto & p) { return boost::geometry::distance(start, p); };
-  const auto dist_to_path_end = [end = convert_point(path.points.back().point.pose.position)](
-                                  const auto & p) { return boost::geometry::distance(end, p); };
-
-  double min_start_dist = std::numeric_limits<double>::max();
-  auto start_transition = expanded_drivable_area.end();
-  double min_end_dist = std::numeric_limits<double>::max();
-  auto end_transition = expanded_drivable_area.end();
-  for (auto it = expanded_drivable_area.begin(); std::next(it) != expanded_drivable_area.end();
-       ++it) {
-    if (is_left_of_path_start(*it) != is_left_of_path_start(*std::next(it))) {
-      const auto dist = dist_to_path_start(*it);
-      if (dist < min_start_dist) {
-        start_transition = it;
-        min_start_dist = dist;
-      }
+  Intersection start_left(da.end());
+  Intersection end_left(da.end());
+  Intersection start_right(da.end());
+  Intersection end_right(da.end());
+  for (auto it = da.begin(); it != da.end(); ++it) {
+    if (boost::geometry::distance(*it, start_segment.first) < 1e-3)
+      start_left.update(*it, it, 0.0);
+    else if (boost::geometry::distance(*it, start_segment.second) < 1e-3)
+      start_right.update(*it, it, 0.0);
+    else if (boost::geometry::distance(*it, end_segment.first) < 1e-3)
+      end_left.update(*it, it, 0.0);
+    else if (boost::geometry::distance(*it, end_segment.second) < 1e-3)
+      end_right.update(*it, it, 0.0);
+    const auto inter_start =
+      std::next(it) == da.end()
+        ? segment_to_line_intersection(*it, da.front(), start_segment.first, start_segment.second)
+        : segment_to_line_intersection(
+            *it, *std::next(it), start_segment.first, start_segment.second);
+    if (inter_start) {
+      const auto dist = boost::geometry::distance(*inter_start, path_start_segment);
+      const auto is_left_of_path_start = is_left_of_segment(
+        convert_point(path.points[0].point.pose.position),
+        convert_point(path.points[1].point.pose.position), *inter_start);
+      if (is_left_of_path_start && dist < start_left.distance)
+        start_left.update(*inter_start, it, dist);
+      else if (!is_left_of_path_start && dist < start_right.distance)
+        start_right.update(*inter_start, it, dist);
     }
-    if (is_left_of_path_end(*it) != is_left_of_path_end(*std::next(it))) {
-      const auto dist = dist_to_path_end(*it);
-      if (dist < min_end_dist) {
-        end_transition = it;
-        min_end_dist = dist;
-      }
+    const auto inter_end =
+      std::next(it) == da.end()
+        ? segment_to_line_intersection(*it, da.front(), end_segment.first, end_segment.second)
+        : segment_to_line_intersection(*it, *std::next(it), end_segment.first, end_segment.second);
+    if (inter_end) {
+      const auto dist = boost::geometry::distance(*inter_end, path_end_segment);
+      const auto is_left_of_path_end = is_left_of_segment(
+        convert_point(path.points.back().point.pose.position), end_segment_center, *inter_end);
+      if (is_left_of_path_end && dist < end_left.distance)
+        end_left.update(*inter_end, it, dist);
+      else if (!is_left_of_path_end && dist < end_right.distance)
+        end_right.update(*inter_end, it, dist);
     }
   }
-  const auto left_start =
-    is_left_of_path_start(*start_transition) ? start_transition : std::next(start_transition);
-  const auto right_start =
-    is_left_of_path_start(*start_transition) ? std::next(start_transition) : start_transition;
-  const auto left_end =
-    is_left_of_path_end(*end_transition) ? end_transition : std::next(end_transition);
-  const auto right_end =
-    is_left_of_path_end(*end_transition) ? std::next(end_transition) : end_transition;
-  return {left_start, left_end, right_start, right_end};
-}
-
-void updateDrivableAreaBounds(PathWithLaneId & path, const polygon_t & expanded_drivable_area)
-{
+  if (  // ill-formed expanded drivable area -> keep the original bounds
+    start_left.segment_it == da.end() || start_right.segment_it == da.end() ||
+    end_left.segment_it == da.end() || end_right.segment_it == da.end()) {
+    return;
+  }
+  // extract the expanded left and right bound from the expanded drivable area
   path.left_bound.clear();
   path.right_bound.clear();
-  const auto begin = expanded_drivable_area.outer().begin();
-  const auto end = std::prev(expanded_drivable_area.outer().end());
-  const auto & [left_start, left_end, right_start, right_end] =
-    findLeftRightRanges(path, expanded_drivable_area.outer());
-  // NOTE: clockwise ordering -> positive increment for left bound, negative for right bound
-  if (left_start < left_end) {
-    path.left_bound.reserve(std::distance(left_start, left_end));
-    for (auto it = left_start; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it));
-  } else {  // loop back
-    path.left_bound.reserve(std::distance(left_start, end) + std::distance(begin, left_end));
-    for (auto it = left_start; it != end; ++it) path.left_bound.push_back(convert_point(*it));
-    for (auto it = begin; it <= left_end; ++it) path.left_bound.push_back(convert_point(*it));
+  path.left_bound.push_back(convert_point(start_left.intersection_point));
+  path.right_bound.push_back(convert_point(start_right.intersection_point));
+  if (!boost::geometry::equals(start_right.intersection_point, *start_right.segment_it))
+    path.right_bound.push_back(convert_point(*start_right.segment_it));
+  if (start_left.segment_it < end_left.segment_it) {
+    for (auto it = std::next(start_left.segment_it); it <= end_left.segment_it; ++it)
+      path.left_bound.push_back(convert_point(*it));
+  } else {
+    for (auto it = std::next(start_left.segment_it); it < da.end(); ++it)
+      path.left_bound.push_back(convert_point(*it));
+    for (auto it = da.begin(); it <= end_left.segment_it; ++it)
+      path.left_bound.push_back(convert_point(*it));
   }
-  if (right_start > right_end) {
-    path.right_bound.reserve(std::distance(right_end, right_start));
-    for (auto it = right_start; it >= right_end; --it)
+  if (!boost::geometry::equals(end_left.intersection_point, *end_left.segment_it))
+    path.left_bound.push_back(convert_point(end_left.intersection_point));
+  if (start_right.segment_it < end_right.segment_it) {
+    for (auto it = std::prev(start_right.segment_it); it >= da.begin(); --it)
+      path.right_bound.push_back(convert_point(*it));
+    for (auto it = std::prev(da.end()); it > end_right.segment_it; --it)
+      path.right_bound.push_back(convert_point(*it));
+  } else {
+    for (auto it = std::prev(start_right.segment_it); it > end_right.segment_it; --it)
       path.right_bound.push_back(convert_point(*it));
-  } else {  // loop back
-    path.right_bound.reserve(std::distance(begin, right_start) + std::distance(right_end, end));
-    for (auto it = right_start; it >= begin; --it) path.right_bound.push_back(convert_point(*it));
-    for (auto it = end - 1; it >= right_end; --it) path.right_bound.push_back(convert_point(*it));
   }
+  if (!boost::geometry::equals(end_right.intersection_point, *std::next(end_right.segment_it)))
+    path.right_bound.push_back(convert_point(end_right.intersection_point));
+  // remove possible duplicated points
+  const auto point_cmp = [](const auto & p1, const auto & p2) {
+    return p1.x == p2.x && p1.y == p2.y;
+  };
+  std::unique(path.left_bound.begin(), path.left_bound.end(), point_cmp);
+  std::unique(path.right_bound.begin(), path.right_bound.end(), point_cmp);
+  copy_z_over_arc_length(original_left_bound, path.left_bound);
+  copy_z_over_arc_length(original_right_bound, path.right_bound);
 }
-
 }  // namespace drivable_area_expansion
diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
index 79e15bf4e68d7..3fcf68bac06a2 100644
--- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/expansion.cpp
@@ -22,10 +22,10 @@ namespace drivable_area_expansion
 
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
-  const multilinestring_t & limit_lines)
+  const multi_linestring_t & limit_lines)
 {
   auto dist_limit = std::numeric_limits<double>::max();
-  multipoint_t intersections;
+  multi_point_t intersections;
   boost::geometry::intersection(expansion_polygon, limit_lines, intersections);
   for (const auto & p : intersections)
     dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls));
@@ -34,11 +34,11 @@ double calculateDistanceLimit(
 
 double calculateDistanceLimit(
   const linestring_t & base_ls, const polygon_t & expansion_polygon,
-  const multipolygon_t & limit_polygons)
+  const multi_polygon_t & limit_polygons)
 {
   auto dist_limit = std::numeric_limits<double>::max();
   for (const auto & polygon : limit_polygons) {
-    multipoint_t intersections;
+    multi_point_t intersections;
     boost::geometry::intersection(expansion_polygon, polygon, intersections);
     for (const auto & p : intersections)
       dist_limit = std::min(dist_limit, boost::geometry::distance(p, base_ls));
@@ -50,7 +50,7 @@ polygon_t createExpansionPolygon(
   const linestring_t & base_ls, const double dist, const bool is_left_side)
 {
   namespace strategy = boost::geometry::strategy::buffer;
-  multipolygon_t polygons;
+  multi_polygon_t polygons;
   // set a non 0 value for the buffer as it sometimes causes no polygon to be returned by bg:buffer
   constexpr auto zero = 0.1;
   const auto left_dist = is_left_side ? dist : zero;
@@ -66,7 +66,7 @@ std::array<double, 3> calculate_arc_length_range_and_distance(
   const linestring_t & path_ls, const polygon_t & footprint, const linestring_t & bound,
   const bool is_left, const double path_length)
 {
-  multipoint_t intersections;
+  multi_point_t intersections;
   double expansion_dist = 0.0;
   double from_arc_length = std::numeric_limits<double>::max();
   double to_arc_length = std::numeric_limits<double>::min();
@@ -93,7 +93,7 @@ std::array<double, 3> calculate_arc_length_range_and_distance(
 
 polygon_t create_compensation_polygon(
   const linestring_t & base_ls, const double compensation_dist, const bool is_left,
-  const multilinestring_t uncrossable_lines, const multipolygon_t & predicted_paths)
+  const multi_linestring_t uncrossable_lines, const multi_polygon_t & predicted_paths)
 {
   polygon_t compensation_polygon = createExpansionPolygon(base_ls, compensation_dist, !is_left);
   double dist_limit = std::min(
@@ -106,9 +106,9 @@ polygon_t create_compensation_polygon(
   return compensation_polygon;
 }
 
-multipolygon_t createExpansionPolygons(
-  const PathWithLaneId & path, const multipolygon_t & path_footprints,
-  const multipolygon_t & predicted_paths, const multilinestring_t & uncrossable_lines,
+multi_polygon_t createExpansionPolygons(
+  const PathWithLaneId & path, const multi_polygon_t & path_footprints,
+  const multi_polygon_t & predicted_paths, const multi_linestring_t & uncrossable_lines,
   const DrivableAreaExpansionParameters & params)
 {
   linestring_t path_ls;
@@ -118,9 +118,32 @@ multipolygon_t createExpansionPolygons(
     path_ls.emplace_back(p.point.pose.position.x, p.point.pose.position.y);
   for (const auto & p : path.left_bound) left_ls.emplace_back(p.x, p.y);
   for (const auto & p : path.right_bound) right_ls.emplace_back(p.x, p.y);
+  // extend the path linestring to the beginning and end of the drivable area
+  if (!right_ls.empty() && !left_ls.empty() && path_ls.size() > 2) {
+    const auto left_proj_begin = point_to_line_projection(left_ls.front(), path_ls[0], path_ls[1]);
+    const auto right_proj_begin =
+      point_to_line_projection(right_ls.front(), path_ls[0], path_ls[1]);
+    const auto left_ls_proj_begin = point_to_linestring_projection(left_proj_begin.point, path_ls);
+    const auto right_ls_proj_begin =
+      point_to_linestring_projection(right_proj_begin.point, path_ls);
+    if (left_ls_proj_begin.arc_length < right_ls_proj_begin.arc_length)
+      path_ls.insert(path_ls.begin(), left_proj_begin.point);
+    else
+      path_ls.insert(path_ls.begin(), right_proj_begin.point);
+    const auto left_proj_end =
+      point_to_line_projection(left_ls.back(), path_ls[path_ls.size() - 2], path_ls.back());
+    const auto right_proj_end =
+      point_to_line_projection(right_ls.back(), path_ls[path_ls.size() - 2], path_ls.back());
+    const auto left_ls_proj_end = point_to_linestring_projection(left_proj_end.point, path_ls);
+    const auto right_ls_proj_end = point_to_linestring_projection(right_proj_end.point, path_ls);
+    if (left_ls_proj_end.arc_length > right_ls_proj_end.arc_length)
+      path_ls.push_back(left_proj_end.point);
+    else
+      path_ls.push_back(right_proj_end.point);
+  }
   const auto path_length = static_cast<double>(boost::geometry::length(path_ls));
 
-  multipolygon_t expansion_polygons;
+  multi_polygon_t expansion_polygons;
   for (const auto & footprint : path_footprints) {
     bool is_left = true;
     for (const auto & bound : {left_ls, right_ls}) {
@@ -160,12 +183,12 @@ multipolygon_t createExpansionPolygons(
   return expansion_polygons;
 }
 
-multipolygon_t createExpansionLaneletPolygons(
+multi_polygon_t createExpansionLaneletPolygons(
   const lanelet::ConstLanelets & path_lanes, const route_handler::RouteHandler & route_handler,
-  const multipolygon_t & path_footprints, const multipolygon_t & predicted_paths,
+  const multi_polygon_t & path_footprints, const multi_polygon_t & predicted_paths,
   const DrivableAreaExpansionParameters & params)
 {
-  multipolygon_t expansion_polygons;
+  multi_polygon_t expansion_polygons;
   lanelet::ConstLanelets candidates;
   const auto already_added = [&](const auto & ll) {
     return std::find_if(candidates.begin(), candidates.end(), [&](const auto & l) {
diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
index 77c8b7faa27eb..0c31093a83c0e 100644
--- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/footprints.cpp
@@ -39,11 +39,11 @@ polygon_t createFootprint(const geometry_msgs::msg::Pose & pose, const polygon_t
   return translatePolygon(rotatePolygon(base_footprint, angle), pose.position.x, pose.position.y);
 }
 
-multipolygon_t createObjectFootprints(
+multi_polygon_t createObjectFootprints(
   const autoware_auto_perception_msgs::msg::PredictedObjects & objects,
   const DrivableAreaExpansionParameters & params)
 {
-  multipolygon_t footprints;
+  multi_polygon_t footprints;
   if (params.avoid_dynamic_objects) {
     for (const auto & object : objects.objects) {
       const auto front = object.shape.dimensions.x / 2 + params.dynamic_objects_extra_front_offset;
@@ -62,7 +62,7 @@ multipolygon_t createObjectFootprints(
   return footprints;
 }
 
-multipolygon_t createPathFootprints(
+multi_polygon_t createPathFootprints(
   const PathWithLaneId & path, const DrivableAreaExpansionParameters & params)
 {
   const auto left = params.ego_left_offset + params.ego_extra_left_offset;
@@ -73,7 +73,7 @@ multipolygon_t createPathFootprints(
   base_footprint.outer() = {
     point_t{front, left}, point_t{front, right}, point_t{rear, right}, point_t{rear, left},
     point_t{front, left}};
-  multipolygon_t footprints;
+  multi_polygon_t footprints;
   // skip the last footprint as its orientation is usually wrong
   footprints.reserve(path.points.size() - 1);
   double arc_length = 0.0;
diff --git a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp
index 39a69fbd74914..ded67c251f7ae 100644
--- a/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp
+++ b/planning/behavior_path_planner/src/utils/drivable_area_expansion/map_utils.cpp
@@ -26,10 +26,10 @@
 
 namespace drivable_area_expansion
 {
-multilinestring_t extractUncrossableLines(
+multi_linestring_t extractUncrossableLines(
   const lanelet::LaneletMap & lanelet_map, const std::vector<std::string> & uncrossable_types)
 {
-  multilinestring_t lines;
+  multi_linestring_t lines;
   linestring_t line;
   for (const auto & ls : lanelet_map.lineStringLayer) {
     if (hasTypes(ls, uncrossable_types)) {
diff --git a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
index 7c4967be14bb8..b02df9753e0b5 100644
--- a/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
+++ b/planning/behavior_path_planner/test/test_drivable_area_expansion.cpp
@@ -165,13 +165,13 @@ TEST(DrivableAreaExpansionProjection, SubLinestring)
     for (auto i = 0lu; i < ls.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i], sub[i]));
   }
   {
-    // arc lengths equal to existing point: sublinestring with same points
+    // arc lengths equal to existing point: sub-linestring with same points
     const auto sub = sub_linestring(ls, 1.0, 5.0);
     ASSERT_EQ(ls.size() - 2lu, sub.size());
     for (auto i = 0lu; i < sub.size(); ++i) EXPECT_TRUE(boost::geometry::equals(ls[i + 1], sub[i]));
   }
   {
-    // arc lengths inside the original: sublinestring with some interpolated points
+    // arc lengths inside the original: sub-linestring with some interpolated points
     const auto sub = sub_linestring(ls, 1.5, 2.5);
     ASSERT_EQ(sub.size(), 3lu);
     EXPECT_NEAR(sub[0].x(), 1.5, eps);
@@ -259,13 +259,14 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea)
     params.max_path_arc_length = 0.0;     // means no limit
     params.extra_arc_length = 1.0;
     params.expansion_method = "polygon";
-    // 4m x 4m ego footprint
+    // 2m x 4m ego footprint
     params.ego_front_offset = 1.0;
     params.ego_rear_offset = -1.0;
     params.ego_left_offset = 2.0;
     params.ego_right_offset = -2.0;
   }
-  // we expect the expand the drivable area by 1m on each side
+  // we expect the drivable area to be expanded by 1m on each side
+  // BUT short paths, due to pruning at the edge of the driving area, there is no expansion
   drivable_area_expansion::expandDrivableArea(
     path, params, dynamic_objects, route_handler, path_lanes);
   // unchanged path points
@@ -274,18 +275,21 @@ TEST(DrivableAreaExpansionProjection, expandDrivableArea)
     EXPECT_NEAR(path.points[i].point.pose.position.x, i, eps);
     EXPECT_NEAR(path.points[i].point.pose.position.y, 0.0, eps);
   }
+
   // expanded left bound
-  ASSERT_EQ(path.left_bound.size(), 2ul);
+  ASSERT_EQ(path.left_bound.size(), 3ul);
   EXPECT_NEAR(path.left_bound[0].x, 0.0, eps);
-  EXPECT_NEAR(path.left_bound[0].y, 2.0, eps);
-  EXPECT_NEAR(path.left_bound[1].x, 2.0, eps);
-  EXPECT_NEAR(path.left_bound[1].y, 2.0, eps);
+  EXPECT_NEAR(path.left_bound[0].y, 1.0, eps);
+  EXPECT_NEAR(path.left_bound[1].x, 1.0, eps);
+  EXPECT_NEAR(path.left_bound[1].y, 1.0, eps);
+  EXPECT_NEAR(path.left_bound[2].x, 2.0, eps);
+  EXPECT_NEAR(path.left_bound[2].y, 1.0, eps);
   // expanded right bound
   ASSERT_EQ(path.right_bound.size(), 3ul);
   EXPECT_NEAR(path.right_bound[0].x, 0.0, eps);
-  EXPECT_NEAR(path.right_bound[0].y, -2.0, eps);
-  EXPECT_NEAR(path.right_bound[1].x, 2.0, eps);
-  EXPECT_NEAR(path.right_bound[1].y, -2.0, eps);
+  EXPECT_NEAR(path.right_bound[0].y, -1.0, eps);
+  EXPECT_NEAR(path.right_bound[1].x, 1.0, eps);
+  EXPECT_NEAR(path.right_bound[1].y, -1.0, eps);
   EXPECT_NEAR(path.right_bound[2].x, 2.0, eps);
   EXPECT_NEAR(path.right_bound[2].y, -1.0, eps);
 }
@@ -294,12 +298,12 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit)
 {
   using drivable_area_expansion::calculateDistanceLimit;
   using drivable_area_expansion::linestring_t;
-  using drivable_area_expansion::multilinestring_t;
+  using drivable_area_expansion::multi_linestring_t;
   using drivable_area_expansion::polygon_t;
 
   {
     const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
-    const multilinestring_t uncrossable_lines = {};
+    const multi_linestring_t uncrossable_lines = {};
     const polygon_t expansion_polygon = {
       {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};
     const auto limit_distance =
@@ -317,7 +321,7 @@ TEST(DrivableAreaExpansion, calculateDistanceLimit)
   }
   {
     const linestring_t base_ls = {{0.0, 0.0}, {10.0, 0.0}};
-    const multilinestring_t uncrossable_lines = {
+    const multi_linestring_t uncrossable_lines = {
       {{0.0, 2.0}, {10.0, 2.0}}, {{0.0, 1.5}, {10.0, 1.0}}};
     const polygon_t expansion_polygon = {
       {{0.0, -4.0}, {0.0, 4.0}, {10.0, 4.0}, {10.0, -4.0}, {10.0, -4.0}}, {}};