diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md
index e0fd41a70cd36..91c880924857d 100644
--- a/planning/autoware_freespace_planner/README.md
+++ b/planning/autoware_freespace_planner/README.md
@@ -76,9 +76,11 @@ None
 | --------------------------- | ------ | ------------------------------------------------------- |
 | `only_behind_solutions`     | bool   | whether restricting the solutions to be behind the goal |
 | `use_back`                  | bool   | whether using backward trajectory                       |
+| `adapt_expansion_distance`  | bool   | if true, adapt expansion distance based on environment  |
 | `expansion_distance`        | double | length of expansion for node transitions                |
 | `distance_heuristic_weight` | double | heuristic weight for estimating node's cost             |
 | `smoothness_weight`         | double | cost factor for change in curvature                     |
+| `obstacle_distance_weight`  | double | cost factor for distance to obstacle                    |
 
 #### RRT\* search parameters
 
diff --git a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml
index ee631f8639153..b4a1951514604 100644
--- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml
+++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml
@@ -32,9 +32,11 @@
     astar:
       only_behind_solutions: false
       use_back: true
+      adapt_expansion_distance: true
       expansion_distance: 0.5
-      distance_heuristic_weight: 1.0
+      distance_heuristic_weight: 1.5
       smoothness_weight: 0.5
+      obstacle_distance_weight: 1.5
 
     # -- RRT* search Configurations --
     rrtstar:
diff --git a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp
index d79a06ae52ead..b077f79f88071 100644
--- a/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp
+++ b/planning/autoware_freespace_planner/src/autoware_freespace_planner/freespace_planner_node.cpp
@@ -573,6 +573,7 @@ void FreespacePlannerNode::initializePlanningAlgorithm()
   extended_vehicle_shape.length += margin;
   extended_vehicle_shape.width += margin;
   extended_vehicle_shape.base2back += margin / 2;
+  extended_vehicle_shape.setMinMaxDimension();
 
   const auto planner_common_param = getPlannerCommonParam();
 
diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
index 4f0a59f44a5d6..cbf835df3b6c6 100644
--- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
+++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/abstract_algorithm.hpp
@@ -24,6 +24,7 @@
 #include <tf2/utils.h>
 
 #include <algorithm>
+#include <limits>
 #include <vector>
 
 namespace autoware::freespace_planning_algorithms
@@ -63,6 +64,8 @@ struct VehicleShape
   double base_length;
   double max_steering;
   double base2back;  // base_link to rear [m]
+  double min_dimension;
+  double max_dimension;
 
   VehicleShape() = default;
 
@@ -74,6 +77,7 @@ struct VehicleShape
     max_steering(max_steering),
     base2back(base2back)
   {
+    setMinMaxDimension();
   }
 
   explicit VehicleShape(
@@ -84,6 +88,19 @@ struct VehicleShape
     max_steering(vehicle_info.max_steer_angle_rad),
     base2back(vehicle_info.rear_overhang_m + margin / 2.0)
   {
+    setMinMaxDimension();
+  }
+
+  void setMinMaxDimension()
+  {
+    const auto base2front = length - base2back;
+    if (base2back <= base2front) {
+      min_dimension = std::min(0.5 * width, base2back);
+      max_dimension = std::hypot(base2front, 0.5 * width);
+    } else {
+      min_dimension = std::min(0.5 * width, base2front);
+      max_dimension = std::hypot(base2back, 0.5 * width);
+    }
   }
 };
 
@@ -156,6 +173,16 @@ class AbstractPlanningAlgorithm
     std::vector<IndexXY> & vertex_indexes_2d) const;
   bool detectBoundaryExit(const IndexXYT & base_index) const;
   bool detectCollision(const IndexXYT & base_index) const;
+  bool detectCollision(const geometry_msgs::msg::Pose & base_pose) const;
+
+  // cspell: ignore Toriwaki
+  /// @brief Computes the euclidean distance to the nearest obstacle for each grid cell.
+  /// @cite T., Saito, and J., Toriwaki "New algorithms for euclidean distance transformation of an
+  /// n-dimensional digitized picture with applications," Pattern Recognition 27, 1994
+  /// https://doi.org/10.1016/0031-3203(94)90133-3
+  /// @details first, distance values are computed along each row. Then, the computed values are
+  /// used to to compute the minimum distance along each column.
+  void computeEDTMap();
 
   template <typename IndexType>
   inline bool isOutOfRange(const IndexType & index) const
@@ -194,6 +221,12 @@ class AbstractPlanningAlgorithm
     return is_obstacle_table_[indexToId(index)];
   }
 
+  template <typename IndexType>
+  inline double getObstacleEDT(const IndexType & index) const
+  {
+    return edt_map_[indexToId(index)];
+  }
+
   // compute single dimensional grid cell index from 2 dimensional index
   template <typename IndexType>
   inline int indexToId(const IndexType & index) const
@@ -216,6 +249,9 @@ class AbstractPlanningAlgorithm
   // is_obstacle's table
   std::vector<bool> is_obstacle_table_;
 
+  // Euclidean distance transform map (distance to nearest obstacle cell)
+  std::vector<double> edt_map_;
+
   // pose in costmap frame
   geometry_msgs::msg::Pose start_pose_;
   geometry_msgs::msg::Pose goal_pose_;
diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
index f7434d912d5e0..cbdbb9d4c3fb3 100644
--- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
+++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/astar_search.hpp
@@ -43,11 +43,13 @@ struct AstarParam
   // base configs
   bool only_behind_solutions;  // solutions should be behind the goal
   bool use_back;               // backward search
+  bool adapt_expansion_distance;
   double expansion_distance;
 
   // search configs
   double distance_heuristic_weight;  // obstacle threshold on grid [0,255]
   double smoothness_weight;
+  double obstacle_distance_weight;
 };
 
 struct AstarNode
@@ -56,9 +58,11 @@ struct AstarNode
   double x;                              // x
   double y;                              // y
   double theta;                          // theta
-  double gc = 0;                         // actual motion cost
-  double fc = 0;                         // total node cost
-  double dir_distance = 0;               // distance traveled from last direction change
+  double gc = 0.0;                       // actual motion cost
+  double fc = 0.0;                       // total node cost
+  double dir_distance = 0.0;             // distance traveled from last direction change
+  double dist_to_goal = 0.0;             // euclidean distance to goal pose
+  double dist_to_obs = 0.0;              // euclidean distance to nearest obstacle
   int steering_index;                    // steering index
   bool is_back;                          // true if the current direction of the vehicle is back
   AstarNode * parent = nullptr;          // parent node
@@ -82,46 +86,9 @@ struct NodeComparison
   bool operator()(const AstarNode * lhs, const AstarNode * rhs) const { return lhs->fc > rhs->fc; }
 };
 
-struct NodeUpdate
-{
-  double shift_x;
-  double shift_y;
-  double shift_theta;
-  double distance;
-  int steering_index;
-  bool is_back;
-
-  NodeUpdate rotated(const double theta) const
-  {
-    NodeUpdate result = *this;
-    result.shift_x = std::cos(theta) * this->shift_x - std::sin(theta) * this->shift_y;
-    result.shift_y = std::sin(theta) * this->shift_x + std::cos(theta) * this->shift_y;
-    return result;
-  }
-
-  NodeUpdate flipped() const
-  {
-    NodeUpdate result = *this;
-    result.shift_y = -result.shift_y;
-    result.shift_theta = -result.shift_theta;
-    return result;
-  }
-
-  NodeUpdate reversed() const
-  {
-    NodeUpdate result = *this;
-    result.shift_x = -result.shift_x;
-    result.shift_theta = -result.shift_theta;
-    result.is_back = !result.is_back;
-    return result;
-  }
-};
-
 class AstarSearch : public AbstractPlanningAlgorithm
 {
 public:
-  using TransitionTable = std::vector<std::vector<NodeUpdate>>;
-
   AstarSearch(
     const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
     const AstarParam & astar_param);
@@ -134,12 +101,15 @@ class AstarSearch : public AbstractPlanningAlgorithm
       AstarParam{
         node.declare_parameter<bool>("astar.only_behind_solutions"),
         node.declare_parameter<bool>("astar.use_back"),
+        node.declare_parameter<bool>("astar.adapt_expansion_distance"),
         node.declare_parameter<double>("astar.expansion_distance"),
         node.declare_parameter<double>("astar.distance_heuristic_weight"),
-        node.declare_parameter<double>("astar.smoothness_weight")})
+        node.declare_parameter<double>("astar.smoothness_weight"),
+        node.declare_parameter<double>("astar.obstacle_distance_weight")})
   {
   }
 
+  void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override;
   bool makePlan(const Pose & start_pose, const Pose & goal_pose) override;
 
   const PlannerWaypoints & getWaypoints() const { return waypoints_; }
@@ -150,27 +120,26 @@ class AstarSearch : public AbstractPlanningAlgorithm
   }
 
 private:
-  void setTransitionTable();
   void setCollisionFreeDistanceMap();
   bool search();
-  void expandNodes(AstarNode & current_node);
+  void expandNodes(AstarNode & current_node, const bool is_back = false);
   void resetData();
   void setPath(const AstarNode & goal);
   bool setStartNode();
-  bool setGoalNode();
   double estimateCost(const Pose & pose, const IndexXYT & index) const;
   bool isGoal(const AstarNode & node) const;
   Pose node2pose(const AstarNode & node) const;
 
+  double getExpansionDistance(const AstarNode & current_node) const;
   double getSteeringCost(const int steering_index) const;
   double getSteeringChangeCost(const int steering_index, const int prev_steering_index) const;
   double getDirectionChangeCost(const double dir_distance) const;
+  double getObsDistanceCost(const double obs_distance) const;
 
   // Algorithm specific param
   AstarParam astar_param_;
 
   // hybrid astar variables
-  TransitionTable transition_table_;
   std::vector<AstarNode> graph_;
   std::vector<double> col_free_distance_map_;
 
@@ -185,6 +154,20 @@ class AstarSearch : public AbstractPlanningAlgorithm
   double steering_resolution_;
   double heading_resolution_;
   double avg_turning_radius_;
+  double min_expansion_dist_;
+  double max_expansion_dist_;
+
+  // the following constexpr values were found to be best by trial and error, through multiple
+  // tests, and are not expected to be changed regularly, therefore they were not made into ros
+  // parameters.
+
+  // expansion distance factors
+  static constexpr double base_length_max_expansion_factor_ = 0.5;
+  static constexpr double dist_to_goal_expansion_factor_ = 0.15;
+  static constexpr double dist_to_obs_expansion_factor_ = 0.3;
+
+  // cost free obstacle distance
+  static constexpr double cost_free_obs_dist = 5.0;
 };
 }  // namespace autoware::freespace_planning_algorithms
 
diff --git a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp
index 0a59646f34d52..b9b6fe8c79af0 100644
--- a/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp
+++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp
@@ -31,39 +31,26 @@ static constexpr double eps = 0.001;
 
 inline double getTurningRadius(const double base_length, const double steering_angle)
 {
-  return base_length / tan(steering_angle);
-}
-
-inline geometry_msgs::msg::Pose getPoseShift(
-  const double yaw, const double base_length, const double steering_angle, const double distance)
-{
-  geometry_msgs::msg::Pose pose;
-  if (abs(steering_angle) < eps) {
-    pose.position.x = distance * cos(yaw);
-    pose.position.y = distance * sin(yaw);
-    return pose;
-  }
-  const double R = getTurningRadius(base_length, steering_angle);
-  const double beta = distance / R;
-  pose.position.x = R * sin(yaw + beta) - R * sin(yaw);
-  pose.position.y = R * cos(yaw) - R * cos(yaw + beta);
-  pose.orientation = autoware::universe_utils::createQuaternionFromYaw(beta);
-  return pose;
+  return base_length / std::tan(steering_angle);
 }
 
 inline geometry_msgs::msg::Pose getPose(
   const geometry_msgs::msg::Pose & current_pose, const double base_length,
   const double steering_angle, const double distance)
 {
-  const double current_yaw = tf2::getYaw(current_pose.orientation);
-  const auto shift = getPoseShift(current_yaw, base_length, steering_angle, distance);
   auto pose = current_pose;
-  pose.position.x += shift.position.x;
-  pose.position.y += shift.position.y;
-  if (abs(steering_angle) > eps) {
-    pose.orientation = autoware::universe_utils::createQuaternionFromYaw(
-      current_yaw + tf2::getYaw(shift.orientation));
+  const double yaw = tf2::getYaw(current_pose.orientation);
+  if (std::abs(steering_angle) < eps) {
+    pose.position.x += distance * std::cos(yaw);
+    pose.position.y += distance * std::sin(yaw);
+    return pose;
   }
+
+  const double R = getTurningRadius(base_length, steering_angle);
+  const double beta = distance / R;
+  pose.position.x += (R * std::sin(yaw + beta) - R * std::sin(yaw));
+  pose.position.y += (R * std::cos(yaw) - R * std::cos(yaw + beta));
+  pose.orientation = autoware::universe_utils::createQuaternionFromYaw(yaw + beta);
   return pose;
 }
 
diff --git a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
index 70de72f2bfffc..40b16f71d5fe7 100644
--- a/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
+++ b/planning/autoware_freespace_planning_algorithms/scripts/bind/astar_search_pybind.cpp
@@ -114,13 +114,19 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p)
       .def_readwrite(
         "only_behind_solutions", &freespace_planning_algorithms::AstarParam::only_behind_solutions)
       .def_readwrite("use_back", &freespace_planning_algorithms::AstarParam::use_back)
+      .def_readwrite(
+        "adapt_expansion_distance",
+        &freespace_planning_algorithms::AstarParam::adapt_expansion_distance)
       .def_readwrite(
         "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance)
       .def_readwrite(
         "distance_heuristic_weight",
         &freespace_planning_algorithms::AstarParam::distance_heuristic_weight)
       .def_readwrite(
-        "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight);
+        "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight)
+      .def_readwrite(
+        "obstacle_distance_weight",
+        &freespace_planning_algorithms::AstarParam::obstacle_distance_weight);
   auto pyPlannerCommonParam =
     py::class_<freespace_planning_algorithms::PlannerCommonParam>(
       p, "PlannerCommonParam", py::dynamic_attr())
diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
index 93e3acbc345b6..28c505fbaae02 100644
--- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
+++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
@@ -46,9 +46,11 @@
 astar_param = fp.AstarParam()
 astar_param.only_behind_solutions = False
 astar_param.use_back = True
+astar_param.adapt_expansion_distance = True
 astar_param.expansion_distance = 0.4
 astar_param.distance_heuristic_weight = 1.0
 astar_param.smoothness_weight = 1.0
+astar_param.obstacle_distance_weight = 1.0
 
 astar = fp.AstarSearch(planner_param, vehicle_shape, astar_param)
 
diff --git a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp
index 7ec594d389de3..380eafb745062 100644
--- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp
@@ -120,6 +120,8 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost
   }
   is_obstacle_table_ = is_obstacle_table;
 
+  computeEDTMap();
+
   // construct collision indexes table
   if (is_collision_table_initialized == false) {
     for (int i = 0; i < planner_common_param_.theta_size; i++) {
@@ -136,6 +138,73 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost
     std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution);
 }
 
+void AbstractPlanningAlgorithm::computeEDTMap()
+{
+  const int height = costmap_.info.height;
+  const int width = costmap_.info.width;
+  const double resolution_m = costmap_.info.resolution;
+  const double diagonal_m = std::hypot(resolution_m * height, resolution_m * width);
+  std::vector<double> edt_map;
+  edt_map.reserve(costmap_.data.size());
+
+  std::vector<double> temporary_storage(width);
+  // scan rows
+  for (int i = 0; i < height; ++i) {
+    double distance = resolution_m;
+    bool found_obstacle = false;
+    // forward scan
+    for (int j = 0; j < width; ++j) {
+      if (isObs(IndexXY{j, i})) {
+        temporary_storage[j] = 0.0;
+        distance = resolution_m;
+        found_obstacle = true;
+      } else if (found_obstacle) {
+        temporary_storage[j] = distance;
+        distance += resolution_m;
+      } else {
+        temporary_storage[j] = diagonal_m;
+      }
+    }
+
+    distance = resolution_m;
+    found_obstacle = false;
+    // backward scan
+    for (int j = width - 1; j >= 0; --j) {
+      if (isObs(IndexXY{j, i})) {
+        distance = resolution_m;
+        found_obstacle = true;
+      } else if (found_obstacle && temporary_storage[j] > distance) {
+        temporary_storage[j] = distance;
+        distance += resolution_m;
+      }
+    }
+    edt_map.insert(edt_map.end(), temporary_storage.begin(), temporary_storage.end());
+  }
+
+  temporary_storage.clear();
+  temporary_storage.resize(height);
+  // scan columns;
+  for (int j = 0; j < width; ++j) {
+    for (int i = 0; i < height; ++i) {
+      int id = indexToId(IndexXY{j, i});
+      double min_value = edt_map[id] * edt_map[id];
+      for (int k = 0; k < height; ++k) {
+        id = indexToId(IndexXY{j, k});
+        double dist = resolution_m * std::abs(static_cast<double>(i - k));
+        double value = edt_map[id] * edt_map[id] + dist * dist;
+        if (value < min_value) {
+          min_value = value;
+        }
+      }
+      temporary_storage[i] = sqrt(min_value);
+    }
+    for (int i = 0; i < height; ++i) {
+      edt_map[indexToId(IndexXY{j, i})] = temporary_storage[i];
+    }
+  }
+  edt_map_ = edt_map;
+}
+
 void AbstractPlanningAlgorithm::computeCollisionIndexes(
   int theta_index, std::vector<IndexXY> & indexes_2d,
   std::vector<IndexXY> & vertex_indexes_2d) const
@@ -218,6 +287,12 @@ bool AbstractPlanningAlgorithm::detectBoundaryExit(const IndexXYT & base_index)
   return false;
 }
 
+bool AbstractPlanningAlgorithm::detectCollision(const geometry_msgs::msg::Pose & base_pose) const
+{
+  const auto base_index = pose2index(costmap_, base_pose, planner_common_param_.theta_size);
+  return detectCollision(base_index);
+}
+
 bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const
 {
   if (coll_indexes_table_.empty()) {
@@ -227,6 +302,13 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con
 
   if (detectBoundaryExit(base_index)) return true;
 
+  double obstacle_edt = getObstacleEDT(base_index);
+
+  // if nearest obstacle is further than largest dimension, no collision is guaranteed
+  // if nearest obstacle is closer than smallest dimension, collision is guaranteed
+  if (obstacle_edt > collision_vehicle_shape_.max_dimension) return false;
+  if (obstacle_edt < collision_vehicle_shape_.min_dimension) return true;
+
   const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta];
   for (const auto & coll_index_2d : coll_indexes_2d) {
     IndexXY coll_index{coll_index_2d.x, coll_index_2d.y};
@@ -247,9 +329,7 @@ bool AbstractPlanningAlgorithm::hasObstacleOnTrajectory(
 {
   for (const auto & pose : trajectory.poses) {
     const auto pose_local = global2local(costmap_, pose);
-    const auto index = pose2index(costmap_, pose_local, planner_common_param_.theta_size);
-
-    if (detectCollision(index)) {
+    if (detectCollision(pose_local)) {
       return true;
     }
   }
diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
index 9233ad212bfa5..fe87011273c1b 100644
--- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
@@ -33,6 +33,8 @@
 
 namespace autoware::freespace_planning_algorithms
 {
+using autoware::universe_utils::calcDistance2d;
+
 double calcReedsSheppDistance(const Pose & p1, const Pose & p2, double radius)
 {
   const auto rs_space = ReedsSheppStateSpace(radius);
@@ -81,37 +83,18 @@ AstarSearch::AstarSearch(
   avg_turning_radius_ =
     kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering);
 
-  setTransitionTable();
+  min_expansion_dist_ = astar_param_.expansion_distance;
+  max_expansion_dist_ = collision_vehicle_shape_.base_length * base_length_max_expansion_factor_;
 }
 
-void AstarSearch::setTransitionTable()
+void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
 {
-  const double distance = astar_param_.expansion_distance;
-  transition_table_.resize(planner_common_param_.theta_size);
-
-  std::vector<NodeUpdate> forward_transitions;
-  int steering_ind = -1 * planner_common_param_.turning_steps;
-  for (; steering_ind <= planner_common_param_.turning_steps; ++steering_ind) {
-    const double steering = static_cast<double>(steering_ind) * steering_resolution_;
-    Pose shift_pose = kinematic_bicycle_model::getPoseShift(
-      0.0, collision_vehicle_shape_.base_length, steering, distance);
-    forward_transitions.push_back(
-      {shift_pose.position.x, shift_pose.position.y, tf2::getYaw(shift_pose.orientation), distance,
-       steering_ind, false});
-  }
-
-  for (int i = 0; i < planner_common_param_.theta_size; ++i) {
-    const double theta = static_cast<double>(i) * heading_resolution_;
-    for (const auto & transition : forward_transitions) {
-      transition_table_[i].push_back(transition.rotated(theta));
-    }
+  AbstractPlanningAlgorithm::setMap(costmap);
 
-    if (astar_param_.use_back) {
-      for (const auto & transition : forward_transitions) {
-        transition_table_[i].push_back(transition.reversed().rotated(theta));
-      }
-    }
-  }
+  // ensure minimum expansion distance is larger then grid cell diagonal length
+  min_expansion_dist_ = std::max(astar_param_.expansion_distance, 1.5 * costmap_.info.resolution);
+  max_expansion_dist_ = std::max(
+    collision_vehicle_shape_.base_length * base_length_max_expansion_factor_, min_expansion_dist_);
 }
 
 bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose)
@@ -121,7 +104,7 @@ bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose)
   start_pose_ = global2local(costmap_, start_pose);
   goal_pose_ = global2local(costmap_, goal_pose);
 
-  if (!setGoalNode()) {
+  if (detectCollision(goal_pose_)) {
     throw std::logic_error("Invalid goal pose");
   }
 
@@ -180,6 +163,7 @@ void AstarSearch::setCollisionFreeDistanceMap()
         const IndexXY n_index{x, y};
         const double offset = std::abs(offset_x) + std::abs(offset_y);
         if (isOutOfRange(n_index) || isObs(n_index) || offset < 1) continue;
+        if (getObstacleEDT(n_index) < 0.5 * collision_vehicle_shape_.width) continue;
         const int n_id = indexToId(n_index);
         const double dist = current.second + (sqrt(offset) * costmap_.info.resolution);
         if (closed[n_id] || col_free_distance_map_[n_id] < dist) continue;
@@ -200,6 +184,8 @@ bool AstarSearch::setStartNode()
   AstarNode * start_node = &graph_[getKey(index)];
   start_node->set(start_pose_, 0.0, estimateCost(start_pose_, index), 0, false);
   start_node->dir_distance = 0.0;
+  start_node->dist_to_goal = calcDistance2d(start_pose_, goal_pose_);
+  start_node->dist_to_obs = getObstacleEDT(index);
   start_node->status = NodeStatus::Open;
   start_node->parent = nullptr;
 
@@ -209,12 +195,6 @@ bool AstarSearch::setStartNode()
   return true;
 }
 
-bool AstarSearch::setGoalNode()
-{
-  const auto index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size);
-  return !detectCollision(index);
-}
-
 double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const
 {
   double total_cost = col_free_distance_map_[indexToId(index)];
@@ -252,28 +232,29 @@ bool AstarSearch::search()
     }
 
     expandNodes(*current_node);
+    if (astar_param_.use_back) expandNodes(*current_node, true);
   }
 
   // Failed to find path
   return false;
 }
 
-void AstarSearch::expandNodes(AstarNode & current_node)
+void AstarSearch::expandNodes(AstarNode & current_node, const bool is_back)
 {
-  const auto index_theta = discretizeAngle(current_node.theta, planner_common_param_.theta_size);
-  for (const auto & transition : transition_table_[index_theta]) {
-    // skip transition back to parent
+  const auto current_pose = node2pose(current_node);
+  double distance = getExpansionDistance(current_node) * (is_back ? -1.0 : 1.0);
+  int steering_index = -1 * planner_common_param_.turning_steps;
+  for (; steering_index <= planner_common_param_.turning_steps; ++steering_index) {
+    // skip expansion back to parent
     if (
-      current_node.parent != nullptr && transition.is_back != current_node.is_back &&
-      transition.steering_index == current_node.steering_index) {
+      current_node.parent != nullptr && is_back != current_node.is_back &&
+      steering_index == current_node.steering_index) {
       continue;
     }
 
-    // Calculate index of the next state
-    Pose next_pose;
-    next_pose.position.x = current_node.x + transition.shift_x;
-    next_pose.position.y = current_node.y + transition.shift_y;
-    setYaw(&next_pose.orientation, current_node.theta + transition.shift_theta);
+    const double steering = static_cast<double>(steering_index) * steering_resolution_;
+    const auto next_pose = kinematic_bicycle_model::getPose(
+      current_pose, collision_vehicle_shape_.base_length, steering, distance);
     const auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size);
 
     if (isOutOfRange(next_index) || isObs(next_index)) continue;
@@ -281,27 +262,28 @@ void AstarSearch::expandNodes(AstarNode & current_node)
     AstarNode * next_node = &graph_[getKey(next_index)];
     if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue;
 
+    const double distance_to_obs = getObstacleEDT(next_index);
     const bool is_direction_switch =
-      (current_node.parent != nullptr) && (transition.is_back != current_node.is_back);
+      (current_node.parent != nullptr) && (is_back != current_node.is_back);
 
     double total_weight = 1.0;
-    total_weight += getSteeringCost(transition.steering_index);
-    if (transition.is_back) {
-      total_weight *= (1.0 + planner_common_param_.reverse_weight);
-    }
+    total_weight += getSteeringCost(steering_index);
+    if (is_back) total_weight *= (1.0 + planner_common_param_.reverse_weight);
 
-    double move_cost = current_node.gc + (total_weight * transition.distance);
-    move_cost += getSteeringChangeCost(transition.steering_index, current_node.steering_index);
+    double move_cost = current_node.gc + (total_weight * std::abs(distance));
+    move_cost += getSteeringChangeCost(steering_index, current_node.steering_index);
+    move_cost += getObsDistanceCost(distance_to_obs);
     if (is_direction_switch) move_cost += getDirectionChangeCost(current_node.dir_distance);
 
     double total_cost = move_cost + estimateCost(next_pose, next_index);
     // Compare cost
     if (next_node->status == NodeStatus::None || next_node->fc > total_cost) {
       next_node->status = NodeStatus::Open;
-      next_node->set(
-        next_pose, move_cost, total_cost, transition.steering_index, transition.is_back);
+      next_node->set(next_pose, move_cost, total_cost, steering_index, is_back);
       next_node->dir_distance =
-        transition.distance + (is_direction_switch ? 0.0 : current_node.dir_distance);
+        std::abs(distance) + (is_direction_switch ? 0.0 : current_node.dir_distance);
+      next_node->dist_to_goal = calcDistance2d(next_pose, goal_pose_);
+      next_node->dist_to_obs = distance_to_obs;
       next_node->parent = &current_node;
       openlist_.push(next_node);
       continue;
@@ -309,6 +291,17 @@ void AstarSearch::expandNodes(AstarNode & current_node)
   }
 }
 
+double AstarSearch::getExpansionDistance(const AstarNode & current_node) const
+{
+  if (!astar_param_.adapt_expansion_distance || max_expansion_dist_ <= min_expansion_dist_) {
+    return min_expansion_dist_;
+  }
+  double exp_dist = std::min(
+    current_node.dist_to_goal * dist_to_goal_expansion_factor_,
+    current_node.dist_to_obs * dist_to_obs_expansion_factor_);
+  return std::clamp(exp_dist, min_expansion_dist_, max_expansion_dist_);
+}
+
 double AstarSearch::getSteeringCost(const int steering_index) const
 {
   return planner_common_param_.curve_weight *
@@ -328,6 +321,12 @@ double AstarSearch::getDirectionChangeCost(const double dir_distance) const
   return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance)));
 }
 
+double AstarSearch::getObsDistanceCost(const double obs_distance) const
+{
+  return astar_param_.obstacle_distance_weight *
+         std::max(1.0 - (obs_distance / cost_free_obs_dist), 0.0);
+}
+
 void AstarSearch::setPath(const AstarNode & goal_node)
 {
   std_msgs::msg::Header header;
@@ -342,10 +341,27 @@ void AstarSearch::setPath(const AstarNode & goal_node)
 
   geometry_msgs::msg::PoseStamped pose;
   pose.header = header;
+
+  const auto interpolate = [this, &pose](const AstarNode & node) {
+    if (node.parent == nullptr || !astar_param_.adapt_expansion_distance) return;
+    const auto parent_pose = node2pose(*node.parent);
+    const double distance_2d = calcDistance2d(node2pose(node), parent_pose);
+    const int n = static_cast<int>(distance_2d / min_expansion_dist_);
+    for (int i = 1; i < n; ++i) {
+      const double dist = ((distance_2d * i) / n) * (node.is_back ? -1.0 : 1.0);
+      const double steering = node.steering_index * steering_resolution_;
+      const auto local_pose = kinematic_bicycle_model::getPose(
+        parent_pose, collision_vehicle_shape_.base_length, steering, dist);
+      pose.pose = local2global(costmap_, local_pose);
+      waypoints_.waypoints.push_back({pose, node.is_back});
+    }
+  };
+
   // push astar nodes poses
   while (node != nullptr) {
     pose.pose = local2global(costmap_, node2pose(*node));
     waypoints_.waypoints.push_back({pose, node->is_back});
+    interpolate(*node);
     // To the next node
     node = node->parent;
   }
diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp
index 0ef31959e65fa..20a59048e59d1 100644
--- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp
@@ -113,9 +113,7 @@ bool RRTStar::hasObstacleOnTrajectory(const geometry_msgs::msg::PoseArray & traj
 {
   for (const auto & pose : trajectory.poses) {
     const auto pose_local = global2local(costmap_, pose);
-    const auto base_index = pose2index(costmap_, pose_local, planner_common_param_.theta_size);
-
-    if (detectCollision(base_index)) {
+    if (detectCollision(pose_local)) {
       return true;
     }
   }
diff --git a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
index 31c551e1906db..7284a8dcffc44 100644
--- a/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
+++ b/planning/autoware_freespace_planning_algorithms/test/src/test_freespace_planning_algorithms.cpp
@@ -211,12 +211,14 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi)
   // configure astar param
   const bool only_behind_solutions = false;
   const bool use_back = true;
+  const bool adapt_expansion_distance = true;
   const double expansion_distance = 0.4;
   const double distance_heuristic_weight = 2.0;
   const double smoothness_weight = 0.5;
+  const double obstacle_distance_weight = 1.7;
   const auto astar_param = fpa::AstarParam{
-    only_behind_solutions, use_back, expansion_distance, distance_heuristic_weight,
-    smoothness_weight};
+    only_behind_solutions,     use_back,          adapt_expansion_distance, expansion_distance,
+    distance_heuristic_weight, smoothness_weight, obstacle_distance_weight};
 
   auto algo = std::make_unique<fpa::AstarSearch>(planner_common_param, vehicle_shape, astar_param);
   return algo;