diff --git a/planning/autoware_freespace_planner/README.md b/planning/autoware_freespace_planner/README.md
index 6bf13a81c0413..e0fd41a70cd36 100644
--- a/planning/autoware_freespace_planner/README.md
+++ b/planning/autoware_freespace_planner/README.md
@@ -59,14 +59,15 @@ None
 | Parameter                 | Type   | Description                                        |
 | ------------------------- | ------ | -------------------------------------------------- |
 | `time_limit`              | double | time limit of planning                             |
-| `minimum_turning_radius`  | double | minimum turning radius of robot                    |
-| `maximum_turning_radius`  | double | maximum turning radius of robot                    |
+| `maximum_turning_ratio`   | double | max ratio of actual turning range to use           |
+| `turning_steps`           | double | number of turning steps within turning range       |
 | `theta_size`              | double | the number of angle's discretization               |
 | `lateral_goal_range`      | double | goal range of lateral position                     |
 | `longitudinal_goal_range` | double | goal range of longitudinal position                |
 | `angle_goal_range`        | double | goal range of angle                                |
 | `curve_weight`            | double | additional cost factor for curve actions           |
 | `reverse_weight`          | double | additional cost factor for reverse actions         |
+| `direction_change_weight` | double | additional cost factor for switching direction     |
 | `obstacle_threshold`      | double | threshold for regarding a certain grid as obstacle |
 
 #### A\* search parameters
@@ -75,7 +76,9 @@ None
 | --------------------------- | ------ | ------------------------------------------------------- |
 | `only_behind_solutions`     | bool   | whether restricting the solutions to be behind the goal |
 | `use_back`                  | bool   | whether using backward trajectory                       |
+| `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                     |
 
 #### 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 8692aa293cbf7..ee631f8639153 100644
--- a/planning/autoware_freespace_planner/config/freespace_planner.param.yaml
+++ b/planning/autoware_freespace_planner/config/freespace_planner.param.yaml
@@ -15,16 +15,16 @@
     # -- Configurations common to the all planners --
     # base configs
     time_limit: 30000.0
-    minimum_turning_radius: 9.0
-    maximum_turning_radius: 9.0
-    turning_radius_size: 1
+    max_turning_ratio: 0.5 # ratio of actual turning limit of vehicle
+    turning_steps: 1
     # search configs
     theta_size: 144
     angle_goal_range: 6.0
-    curve_weight: 1.2
-    reverse_weight: 2.0
     lateral_goal_range: 0.5
     longitudinal_goal_range: 2.0
+    curve_weight: 0.5
+    reverse_weight: 1.0
+    direction_change_weight: 1.5
     # costmap configs
     obstacle_threshold: 100
 
@@ -32,7 +32,9 @@
     astar:
       only_behind_solutions: false
       use_back: true
+      expansion_distance: 0.5
       distance_heuristic_weight: 1.0
+      smoothness_weight: 0.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 55068263f5d5b..d79a06ae52ead 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
@@ -245,6 +245,8 @@ FreespacePlannerNode::FreespacePlannerNode(const rclcpp::NodeOptions & node_opti
       autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo();
     vehicle_shape_.length = vehicle_info.vehicle_length_m;
     vehicle_shape_.width = vehicle_info.vehicle_width_m;
+    vehicle_shape_.base_length = vehicle_info.wheel_base_m;
+    vehicle_shape_.max_steering = vehicle_info.max_steer_angle_rad;
     vehicle_shape_.base2back = vehicle_info.rear_overhang_m;
   }
 
@@ -296,18 +298,16 @@ PlannerCommonParam FreespacePlannerNode::getPlannerCommonParam()
 
   // search configs
   p.time_limit = declare_parameter<double>("time_limit");
-  p.minimum_turning_radius = declare_parameter<double>("minimum_turning_radius");
-  p.maximum_turning_radius = declare_parameter<double>("maximum_turning_radius");
-  p.turning_radius_size = declare_parameter<int>("turning_radius_size");
-  p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius);
-  p.turning_radius_size = std::max(p.turning_radius_size, 1);
 
   p.theta_size = declare_parameter<int>("theta_size");
   p.angle_goal_range = declare_parameter<double>("angle_goal_range");
   p.curve_weight = declare_parameter<double>("curve_weight");
   p.reverse_weight = declare_parameter<double>("reverse_weight");
+  p.direction_change_weight = declare_parameter<double>("direction_change_weight");
   p.lateral_goal_range = declare_parameter<double>("lateral_goal_range");
   p.longitudinal_goal_range = declare_parameter<double>("longitudinal_goal_range");
+  p.max_turning_ratio = declare_parameter<double>("max_turning_ratio");
+  p.turning_steps = declare_parameter<int>("turning_steps");
 
   // costmap configs
   p.obstacle_threshold = declare_parameter<int>("obstacle_threshold");
@@ -517,7 +517,13 @@ void FreespacePlannerNode::planTrajectory()
 
   // execute planning
   const rclcpp::Time start = get_clock()->now();
-  const bool result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame);
+  std::string error_msg;
+  bool result = false;
+  try {
+    result = algo_->makePlan(current_pose_in_costmap_frame, goal_pose_in_costmap_frame);
+  } catch (const std::exception & e) {
+    error_msg = e.what();
+  }
   const rclcpp::Time end = get_clock()->now();
 
   RCLCPP_DEBUG(get_logger(), "Freespace planning: %f [s]", (end - start).seconds());
@@ -530,9 +536,8 @@ void FreespacePlannerNode::planTrajectory()
     prev_target_index_ = 0;
     target_index_ =
       getNextTargetIndex(trajectory_.points.size(), reversing_indices_, prev_target_index_);
-
   } else {
-    RCLCPP_INFO(get_logger(), "Can't find goal...");
+    RCLCPP_INFO(get_logger(), "Can't find goal: %s", error_msg.c_str());
     reset();
   }
 }
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 bca08f0d11c58..4f0a59f44a5d6 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
@@ -23,6 +23,7 @@
 
 #include <tf2/utils.h>
 
+#include <algorithm>
 #include <vector>
 
 namespace autoware::freespace_planning_algorithms
@@ -57,14 +58,21 @@ geometry_msgs::msg::Pose local2global(
 
 struct VehicleShape
 {
-  double length;     // X [m]
-  double width;      // Y [m]
+  double length;  // X [m]
+  double width;   // Y [m]
+  double base_length;
+  double max_steering;
   double base2back;  // base_link to rear [m]
 
   VehicleShape() = default;
 
-  VehicleShape(double length, double width, double base2back)
-  : length(length), width(width), base2back(base2back)
+  VehicleShape(
+    double length, double width, double base_length, double max_steering, double base2back)
+  : length(length),
+    width(width),
+    base_length(base_length),
+    max_steering(max_steering),
+    base2back(base2back)
   {
   }
 
@@ -72,6 +80,8 @@ struct VehicleShape
     const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0)
   : length(vehicle_info.vehicle_length_m + margin),
     width(vehicle_info.vehicle_width_m + margin),
+    base_length(vehicle_info.wheel_base_m),
+    max_steering(vehicle_info.max_steer_angle_rad),
     base2back(vehicle_info.rear_overhang_m + margin / 2.0)
   {
   }
@@ -82,18 +92,16 @@ struct PlannerCommonParam
   // base configs
   double time_limit;  // planning time limit [msec]
 
-  // robot configs
-  double minimum_turning_radius;  // [m]
-  double maximum_turning_radius;  // [m]
-  int turning_radius_size;        // discretized turning radius table size [-]
-
   // search configs
   int theta_size;                  // discretized angle table size [-]
   double curve_weight;             // curve moving cost [-]
   double reverse_weight;           // backward moving cost [-]
+  double direction_change_weight;  // direction change cost [-]
   double lateral_goal_range;       // reaching threshold, lateral error [m]
   double longitudinal_goal_range;  // reaching threshold, longitudinal error [m]
   double angle_goal_range;         // reaching threshold, angle error [deg]
+  double max_turning_ratio;        // factor of max turning range to use [-]
+  int turning_steps;               // number of turning steps [-]
 
   // costmap configs
   int obstacle_threshold;  // obstacle threshold on grid [-]
@@ -120,6 +128,8 @@ class AbstractPlanningAlgorithm
     const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape)
   : planner_common_param_(planner_common_param), collision_vehicle_shape_(collision_vehicle_shape)
   {
+    planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1);
+    collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio;
     is_collision_table_initialized = false;
   }
 
@@ -128,6 +138,8 @@ class AbstractPlanningAlgorithm
     const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double margin = 0.0)
   : planner_common_param_(planner_common_param), collision_vehicle_shape_(vehicle_info, margin)
   {
+    planner_common_param_.turning_steps = std::max(planner_common_param_.turning_steps, 1);
+    collision_vehicle_shape_.max_steering *= planner_common_param_.max_turning_ratio;
   }
 
   virtual void setMap(const nav_msgs::msg::OccupancyGrid & costmap);
@@ -142,8 +154,11 @@ class AbstractPlanningAlgorithm
   void computeCollisionIndexes(
     int theta_index, std::vector<IndexXY> & indexes,
     std::vector<IndexXY> & vertex_indexes_2d) const;
+  bool detectBoundaryExit(const IndexXYT & base_index) const;
   bool detectCollision(const IndexXYT & base_index) const;
-  inline bool isOutOfRange(const IndexXYT & index) const
+
+  template <typename IndexType>
+  inline bool isOutOfRange(const IndexType & index) const
   {
     if (index.x < 0 || static_cast<int>(costmap_.info.width) <= index.x) {
       return true;
@@ -153,16 +168,41 @@ class AbstractPlanningAlgorithm
     }
     return false;
   }
-  inline bool isObs(const IndexXYT & index) const
+
+  template <typename IndexType>
+  inline bool isWithinMargin(const IndexType & index) const
+  {
+    if (
+      index.x < nb_of_margin_cells_ ||
+      static_cast<int>(costmap_.info.width) - index.x < nb_of_margin_cells_) {
+      return false;
+    }
+    if (
+      index.y < nb_of_margin_cells_ ||
+      static_cast<int>(costmap_.info.height) - index.y < nb_of_margin_cells_) {
+      return false;
+    }
+    return true;
+  }
+
+  template <typename IndexType>
+  inline bool isObs(const IndexType & index) const
   {
     // NOTE: Accessing by .at() instead makes 1.2 times slower here.
     // Also, boundary check is already done in isOutOfRange before calling this function.
     // So, basically .at() is not necessary.
-    return is_obstacle_table_[index.y][index.x];
+    return is_obstacle_table_[indexToId(index)];
+  }
+
+  // compute single dimensional grid cell index from 2 dimensional index
+  template <typename IndexType>
+  inline int indexToId(const IndexType & index) const
+  {
+    return index.y * costmap_.info.width + index.x;
   }
 
   PlannerCommonParam planner_common_param_;
-  const VehicleShape collision_vehicle_shape_;
+  VehicleShape collision_vehicle_shape_;
 
   // costmap as occupancy grid
   nav_msgs::msg::OccupancyGrid costmap_;
@@ -174,7 +214,7 @@ class AbstractPlanningAlgorithm
   std::vector<std::vector<IndexXY>> vertex_indexes_table_;
 
   // is_obstacle's table
-  std::vector<std::vector<bool>> is_obstacle_table_;
+  std::vector<bool> is_obstacle_table_;
 
   // pose in costmap frame
   geometry_msgs::msg::Pose start_pose_;
@@ -185,6 +225,8 @@ class AbstractPlanningAlgorithm
 
   // result path
   PlannerWaypoints waypoints_;
+
+  int nb_of_margin_cells_;
 };
 
 }  // namespace autoware::freespace_planning_algorithms
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 a746ceea6f838..3f6c3935b8a42 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
@@ -34,6 +34,8 @@
 
 namespace autoware::freespace_planning_algorithms
 {
+using geometry_msgs::msg::Pose;
+
 enum class NodeStatus : uint8_t { None, Open, Closed };
 
 struct AstarParam
@@ -41,9 +43,11 @@ struct AstarParam
   // base configs
   bool only_behind_solutions;  // solutions should be behind the goal
   bool use_back;               // backward search
+  double expansion_distance;
 
   // search configs
   double distance_heuristic_weight;  // obstacle threshold on grid [0,255]
+  double smoothness_weight;
 };
 
 struct AstarNode
@@ -52,20 +56,30 @@ struct AstarNode
   double x;                              // x
   double y;                              // y
   double theta;                          // theta
-  double gc = 0;                         // actual cost
-  double hc = 0;                         // heuristic cost
+  double gc = 0;                         // actual motion cost
+  double fc = 0;                         // total node cost
+  double dir_distance = 0;               // distance traveled from last direction change
+  int steering_index;                    // steering index
   bool is_back;                          // true if the current direction of the vehicle is back
   AstarNode * parent = nullptr;          // parent node
 
-  double cost() const { return gc + hc; }
+  inline void set(
+    const Pose & pose, const double move_cost, const double total_cost, const double steer_ind,
+    const bool backward)
+  {
+    x = pose.position.x;
+    y = pose.position.y;
+    theta = tf2::getYaw(pose.orientation);
+    gc = move_cost;
+    fc = total_cost;
+    steering_index = steer_ind;
+    is_back = backward;
+  }
 };
 
 struct NodeComparison
 {
-  bool operator()(const AstarNode * lhs, const AstarNode * rhs)
-  {
-    return lhs->cost() > rhs->cost();
-  }
+  bool operator()(const AstarNode * lhs, const AstarNode * rhs) { return lhs->fc > rhs->fc; }
 };
 
 struct NodeUpdate
@@ -74,7 +88,7 @@ struct NodeUpdate
   double shift_y;
   double shift_theta;
   double distance;
-  bool is_curve;
+  int steering_index;
   bool is_back;
 
   NodeUpdate rotated(const double theta) const
@@ -120,43 +134,45 @@ class AstarSearch : public AbstractPlanningAlgorithm
       AstarParam{
         node.declare_parameter<bool>("astar.only_behind_solutions"),
         node.declare_parameter<bool>("astar.use_back"),
-        node.declare_parameter<double>("astar.distance_heuristic_weight")})
+        node.declare_parameter<double>("astar.expansion_distance"),
+        node.declare_parameter<double>("astar.distance_heuristic_weight"),
+        node.declare_parameter<double>("astar.smoothness_weight")})
   {
   }
 
-  void setMap(const nav_msgs::msg::OccupancyGrid & costmap) override;
-  bool makePlan(
-    const geometry_msgs::msg::Pose & start_pose,
-    const geometry_msgs::msg::Pose & goal_pose) override;
+  bool makePlan(const Pose & start_pose, const Pose & goal_pose) override;
 
   const PlannerWaypoints & getWaypoints() const { return waypoints_; }
 
   inline int getKey(const IndexXYT & index)
   {
-    return (index.theta + (index.y * x_scale_ + index.x) * y_scale_);
+    return indexToId(index) * planner_common_param_.theta_size + index.theta;
   }
 
 private:
+  void setTransitionTable();
+  void setCollisionFreeDistanceMap();
   bool search();
-  void clearNodes();
+  void expandNodes(AstarNode & current_node);
+  void resetData();
   void setPath(const AstarNode & goal);
   bool setStartNode();
   bool setGoalNode();
-  double estimateCost(const geometry_msgs::msg::Pose & pose) const;
+  double estimateCost(const Pose & pose, const IndexXYT & index) const;
   bool isGoal(const AstarNode & node) const;
-  geometry_msgs::msg::Pose node2pose(const AstarNode & node) const;
+  Pose node2pose(const AstarNode & node) const;
 
-  AstarNode * getNodeRef(const IndexXYT & index)
-  {
-    return &(graph_.emplace(getKey(index), AstarNode()).first->second);
-  }
+  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;
 
   // Algorithm specific param
   AstarParam astar_param_;
 
   // hybrid astar variables
   TransitionTable transition_table_;
-  std::unordered_map<uint, AstarNode> graph_;
+  std::vector<AstarNode> graph_;
+  std::vector<double> col_free_distance_map_;
 
   std::priority_queue<AstarNode *, std::vector<AstarNode *>, NodeComparison> openlist_;
 
@@ -166,8 +182,9 @@ class AstarSearch : public AbstractPlanningAlgorithm
   // distance metric option (removed when the reeds_shepp gets stable)
   bool use_reeds_shepp_;
 
-  int x_scale_;
-  int y_scale_;
+  double steering_resolution_;
+  double heading_resolution_;
+  double avg_turning_radius_;
 };
 }  // 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
new file mode 100644
index 0000000000000..0a59646f34d52
--- /dev/null
+++ b/planning/autoware_freespace_planning_algorithms/include/autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp
@@ -0,0 +1,73 @@
+// Copyright 2024 Tier IV, Inc. All rights reserved.
+//
+// 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.
+
+#ifndef AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_
+#define AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_
+
+#include <autoware/universe_utils/geometry/geometry.hpp>
+#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
+
+#include <tf2/utils.h>
+
+#include <vector>
+
+namespace autoware::freespace_planning_algorithms
+{
+namespace kinematic_bicycle_model
+{
+
+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;
+}
+
+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));
+  }
+  return pose;
+}
+
+}  // namespace kinematic_bicycle_model
+}  // namespace autoware::freespace_planning_algorithms
+
+#endif  // AUTOWARE__FREESPACE_PLANNING_ALGORITHMS__KINEMATIC_BICYCLE_MODEL_HPP_
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 8932fb277abe8..70de72f2bfffc 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,28 +114,30 @@ 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(
+        "expansion_distance", &freespace_planning_algorithms::AstarParam::expansion_distance)
       .def_readwrite(
         "distance_heuristic_weight",
-        &freespace_planning_algorithms::AstarParam::distance_heuristic_weight);
+        &freespace_planning_algorithms::AstarParam::distance_heuristic_weight)
+      .def_readwrite(
+        "smoothness_weight", &freespace_planning_algorithms::AstarParam::smoothness_weight);
   auto pyPlannerCommonParam =
     py::class_<freespace_planning_algorithms::PlannerCommonParam>(
       p, "PlannerCommonParam", py::dynamic_attr())
       .def(py::init<>())
       .def_readwrite("time_limit", &freespace_planning_algorithms::PlannerCommonParam::time_limit)
+      .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size)
       .def_readwrite(
-        "minimum_turning_radius",
-        &freespace_planning_algorithms::PlannerCommonParam::minimum_turning_radius)
-      .def_readwrite(
-        "maximum_turning_radius",
-        &freespace_planning_algorithms::PlannerCommonParam::maximum_turning_radius)
+        "max_turning_ratio", &freespace_planning_algorithms::PlannerCommonParam::max_turning_ratio)
       .def_readwrite(
-        "turning_radius_size",
-        &freespace_planning_algorithms::PlannerCommonParam::turning_radius_size)
-      .def_readwrite("theta_size", &freespace_planning_algorithms::PlannerCommonParam::theta_size)
+        "turning_steps", &freespace_planning_algorithms::PlannerCommonParam::turning_steps)
       .def_readwrite(
         "curve_weight", &freespace_planning_algorithms::PlannerCommonParam::curve_weight)
       .def_readwrite(
         "reverse_weight", &freespace_planning_algorithms::PlannerCommonParam::reverse_weight)
+      .def_readwrite(
+        "direction_change_weight",
+        &freespace_planning_algorithms::PlannerCommonParam::direction_change_weight)
       .def_readwrite(
         "lateral_goal_range",
         &freespace_planning_algorithms::PlannerCommonParam::lateral_goal_range)
@@ -150,9 +152,11 @@ PYBIND11_MODULE(autoware_freespace_planning_algorithms_pybind, p)
   auto pyVehicleShape =
     py::class_<freespace_planning_algorithms::VehicleShape>(p, "VehicleShape", py::dynamic_attr())
       .def(py::init<>())
-      .def(py::init<double, double, double>())
+      .def(py::init<double, double, double, double, double>())
       .def_readwrite("length", &freespace_planning_algorithms::VehicleShape::length)
       .def_readwrite("width", &freespace_planning_algorithms::VehicleShape::width)
+      .def_readwrite("base_length", &freespace_planning_algorithms::VehicleShape::base_length)
+      .def_readwrite("max_steering", &freespace_planning_algorithms::VehicleShape::max_steering)
       .def_readwrite("base2back", &freespace_planning_algorithms::VehicleShape::base2back);
 
   py::class_<freespace_planning_algorithms::AbstractPlanningAlgorithm>(
diff --git a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
index c0cdf8c2ec6ba..93e3acbc345b6 100644
--- a/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
+++ b/planning/autoware_freespace_planning_algorithms/scripts/example/example.py
@@ -28,14 +28,14 @@
 planner_param = fp.PlannerCommonParam()
 # base configs
 planner_param.time_limit = 30000.0
-planner_param.minimum_turning_radius = 9.0
-planner_param.maximum_turning_radius = 9.0
-planner_param.turning_radius_size = 1
+planner_param.max_turning_ratio = 0.5
+planner_param.turning_steps = 1
 # search configs
 planner_param.theta_size = 144
 planner_param.angle_goal_range = 6.0
 planner_param.curve_weight = 1.2
-planner_param.reverse_weight = 2.0
+planner_param.reverse_weight = 1.0
+planner_param.direction_change_weight = 2.0
 planner_param.lateral_goal_range = 0.5
 planner_param.longitudinal_goal_range = 2.0
 # costmap configs
@@ -46,7 +46,9 @@
 astar_param = fp.AstarParam()
 astar_param.only_behind_solutions = False
 astar_param.use_back = True
+astar_param.expansion_distance = 0.4
 astar_param.distance_heuristic_weight = 1.0
+astar_param.smoothness_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 c434635ff1401..7ec594d389de3 100644
--- a/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/abstract_algorithm.cpp
@@ -14,6 +14,8 @@
 
 #include "autoware/freespace_planning_algorithms/abstract_algorithm.hpp"
 
+#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp"
+
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/normalization.hpp>
 
@@ -35,16 +37,16 @@ geometry_msgs::msg::Pose transformPose(
 
 int discretizeAngle(const double theta, const int theta_size)
 {
-  const double one_angle_range = 2.0 * M_PI / theta_size;
-  return static_cast<int>(std::rint(normalizeRadian(theta, 0.0) / one_angle_range)) % theta_size;
+  const double angle_resolution = 2.0 * M_PI / theta_size;
+  return static_cast<int>(std::round(normalizeRadian(theta, 0.0) / angle_resolution)) % theta_size;
 }
 
 IndexXYT pose2index(
   const nav_msgs::msg::OccupancyGrid & costmap, const geometry_msgs::msg::Pose & pose_local,
   const int theta_size)
 {
-  const int index_x = pose_local.position.x / costmap.info.resolution;
-  const int index_y = pose_local.position.y / costmap.info.resolution;
+  const int index_x = std::round(pose_local.position.x / costmap.info.resolution);
+  const int index_y = std::round(pose_local.position.y / costmap.info.resolution);
   const int index_theta = discretizeAngle(tf2::getYaw(pose_local.orientation), theta_size);
   return {index_x, index_y, index_theta};
 }
@@ -57,8 +59,8 @@ geometry_msgs::msg::Pose index2pose(
   pose_local.position.x = index.x * costmap.info.resolution;
   pose_local.position.y = index.y * costmap.info.resolution;
 
-  const double one_angle_range = 2.0 * M_PI / theta_size;
-  const double yaw = index.theta * one_angle_range;
+  const double angle_resolution = 2.0 * M_PI / theta_size;
+  const double yaw = index.theta * angle_resolution;
   pose_local.orientation = createQuaternionFromYaw(yaw);
 
   return pose_local;
@@ -105,20 +107,15 @@ double PlannerWaypoints::compute_length() const
 void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
 {
   costmap_ = costmap;
-  const auto height = costmap_.info.height;
-  const auto width = costmap_.info.width;
 
+  const uint32_t nb_of_cells = costmap_.data.size();
   // Initialize status
-  std::vector<std::vector<bool>> is_obstacle_table;
-  is_obstacle_table.resize(height);
-  for (uint32_t i = 0; i < height; i++) {
-    is_obstacle_table.at(i).resize(width);
-    for (uint32_t j = 0; j < width; j++) {
-      const int cost = costmap_.data[i * width + j];
-
-      if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) {
-        is_obstacle_table[i][j] = true;
-      }
+  std::vector<bool> is_obstacle_table;
+  is_obstacle_table.resize(nb_of_cells);
+  for (uint32_t i = 0; i < nb_of_cells; ++i) {
+    const int cost = costmap_.data[i];
+    if (cost < 0 || planner_common_param_.obstacle_threshold <= cost) {
+      is_obstacle_table[i] = true;
     }
   }
   is_obstacle_table_ = is_obstacle_table;
@@ -133,6 +130,10 @@ void AbstractPlanningAlgorithm::setMap(const nav_msgs::msg::OccupancyGrid & cost
     }
     is_collision_table_initialized = true;
   }
+
+  const double base2front = collision_vehicle_shape_.length - collision_vehicle_shape_.base2back;
+  nb_of_margin_cells_ = std::ceil(
+    std::hypot(0.5 * collision_vehicle_shape_.width, base2front) / costmap_.info.resolution);
 }
 
 void AbstractPlanningAlgorithm::computeCollisionIndexes(
@@ -201,16 +202,12 @@ void AbstractPlanningAlgorithm::computeCollisionIndexes(
   addIndex2d(back, left, vertex_indexes_2d);
 }
 
-bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const
+bool AbstractPlanningAlgorithm::detectBoundaryExit(const IndexXYT & base_index) const
 {
-  if (coll_indexes_table_.empty()) {
-    std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl;
-    return false;
-  }
-
+  if (isWithinMargin(base_index)) return false;
   const auto & vertex_indexes_2d = vertex_indexes_table_[base_index.theta];
   for (const auto & vertex_index_2d : vertex_indexes_2d) {
-    IndexXYT vertex_index{vertex_index_2d.x, vertex_index_2d.y, 0};
+    IndexXY vertex_index{vertex_index_2d.x, vertex_index_2d.y};
     // must slide to current base position
     vertex_index.x += base_index.x;
     vertex_index.y += base_index.y;
@@ -218,11 +215,21 @@ bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) con
       return true;
     }
   }
+  return false;
+}
+
+bool AbstractPlanningAlgorithm::detectCollision(const IndexXYT & base_index) const
+{
+  if (coll_indexes_table_.empty()) {
+    std::cerr << "[abstract_algorithm] setMap has not yet been done." << std::endl;
+    return false;
+  }
+
+  if (detectBoundaryExit(base_index)) return true;
 
   const auto & coll_indexes_2d = coll_indexes_table_[base_index.theta];
   for (const auto & coll_index_2d : coll_indexes_2d) {
-    int idx_theta = 0;  // whatever. Yaw is nothing to do with collision detection between grids.
-    IndexXYT coll_index{coll_index_2d.x, coll_index_2d.y, idx_theta};
+    IndexXY coll_index{coll_index_2d.x, coll_index_2d.y};
     // must slide to current base position
     coll_index.x += base_index.x;
     coll_index.y += base_index.y;
diff --git a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
index 1a75c8277ebfd..4ead3cb5a7423 100644
--- a/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/astar_search.cpp
@@ -14,11 +14,15 @@
 
 #include "autoware/freespace_planning_algorithms/astar_search.hpp"
 
+#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp"
+
 #include <autoware/universe_utils/geometry/geometry.hpp>
 #include <autoware/universe_utils/math/unit_conversion.hpp>
 
 #include <tf2/utils.h>
 
+#include <limits>
+
 #ifdef ROS_DISTRO_GALACTIC
 #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 #else
@@ -29,8 +33,7 @@
 
 namespace autoware::freespace_planning_algorithms
 {
-double calcReedsSheppDistance(
-  const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, double radius)
+double calcReedsSheppDistance(const Pose & p1, const Pose & p2, double radius)
 {
   const auto rs_space = ReedsSheppStateSpace(radius);
   const ReedsSheppStateSpace::StateXYT pose0{
@@ -45,8 +48,7 @@ void setYaw(geometry_msgs::msg::Quaternion * orientation, const double yaw)
   *orientation = autoware::universe_utils::createQuaternionFromYaw(yaw);
 }
 
-geometry_msgs::msg::Pose calcRelativePose(
-  const geometry_msgs::msg::Pose & base_pose, const geometry_msgs::msg::Pose & pose)
+Pose calcRelativePose(const Pose & base_pose, const Pose & pose)
 {
   tf2::Transform tf_transform;
   tf2::convert(base_pose, tf_transform);
@@ -62,55 +64,6 @@ geometry_msgs::msg::Pose calcRelativePose(
   return transformed.pose;
 }
 
-AstarSearch::TransitionTable createTransitionTable(
-  const double minimum_turning_radius, const double maximum_turning_radius,
-  const int turning_radius_size, const double theta_size, const bool use_back)
-{
-  // Vehicle moving for each angle
-  AstarSearch::TransitionTable transition_table;
-  transition_table.resize(theta_size);
-
-  const double dtheta = 2.0 * M_PI / theta_size;
-
-  // Minimum moving distance with one state update
-  // arc  = r * theta
-  const auto & R_min = minimum_turning_radius;
-  const auto & R_max = maximum_turning_radius;
-  const double step_min = R_min * dtheta;
-  const double dR = (R_max - R_min) / std::max(turning_radius_size - 1, 1);
-
-  // NodeUpdate actions
-  std::vector<NodeUpdate> forward_node_candidates;
-  const NodeUpdate forward_straight{step_min, 0.0, 0.0, step_min, false, false};
-  forward_node_candidates.push_back(forward_straight);
-  for (int i = 0; i < turning_radius_size; ++i) {
-    double R = R_min + i * dR;
-    double step = R * dtheta;
-    const double shift_x = R * sin(dtheta);
-    const double shift_y = R * (1 - cos(dtheta));
-    const NodeUpdate forward_left{shift_x, shift_y, dtheta, step, true, false};
-    const NodeUpdate forward_right = forward_left.flipped();
-    forward_node_candidates.push_back(forward_left);
-    forward_node_candidates.push_back(forward_right);
-  }
-
-  for (int i = 0; i < theta_size; i++) {
-    const double theta = dtheta * i;
-
-    for (const auto & nu : forward_node_candidates) {
-      transition_table[i].push_back(nu.rotated(theta));
-    }
-
-    if (use_back) {
-      for (const auto & nu : forward_node_candidates) {
-        transition_table[i].push_back(nu.reversed().rotated(theta));
-      }
-    }
-  }
-
-  return transition_table;
-}
-
 AstarSearch::AstarSearch(
   const PlannerCommonParam & planner_common_param, const VehicleShape & collision_vehicle_shape,
   const AstarParam & astar_param)
@@ -119,66 +72,134 @@ AstarSearch::AstarSearch(
   goal_node_(nullptr),
   use_reeds_shepp_(true)
 {
-  transition_table_ = createTransitionTable(
-    planner_common_param_.minimum_turning_radius, planner_common_param_.maximum_turning_radius,
-    planner_common_param_.turning_radius_size, planner_common_param_.theta_size,
-    astar_param_.use_back);
+  steering_resolution_ =
+    collision_vehicle_shape_.max_steering / planner_common_param_.turning_steps;
+  heading_resolution_ = 2.0 * M_PI / planner_common_param_.theta_size;
 
-  y_scale_ = planner_common_param.theta_size;
+  const double avg_steering =
+    steering_resolution_ + (collision_vehicle_shape_.max_steering - steering_resolution_) / 2.0;
+  avg_turning_radius_ =
+    kinematic_bicycle_model::getTurningRadius(collision_vehicle_shape_.base_length, avg_steering);
+
+  setTransitionTable();
 }
 
-void AstarSearch::setMap(const nav_msgs::msg::OccupancyGrid & costmap)
+void AstarSearch::setTransitionTable()
 {
-  AbstractPlanningAlgorithm::setMap(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));
+    }
 
-  x_scale_ = costmap_.info.height;
+    if (astar_param_.use_back) {
+      for (const auto & transition : forward_transitions) {
+        transition_table_[i].push_back(transition.reversed().rotated(theta));
+      }
+    }
+  }
 }
 
-bool AstarSearch::makePlan(
-  const geometry_msgs::msg::Pose & start_pose, const geometry_msgs::msg::Pose & goal_pose)
+bool AstarSearch::makePlan(const Pose & start_pose, const Pose & goal_pose)
 {
+  resetData();
+
   start_pose_ = global2local(costmap_, start_pose);
   goal_pose_ = global2local(costmap_, goal_pose);
 
-  clearNodes();
-  graph_.reserve(100000);
+  if (!setGoalNode()) {
+    throw std::logic_error("Invalid goal pose");
+  }
+
+  setCollisionFreeDistanceMap();
 
   if (!setStartNode()) {
-    return false;
+    throw std::logic_error("Invalid start pose");
   }
 
-  if (!setGoalNode()) {
-    return false;
+  if (!search()) {
+    throw std::logic_error("HA* failed to find path to goal");
   }
 
-  return search();
+  return true;
 }
 
-void AstarSearch::clearNodes()
+void AstarSearch::resetData()
 {
   // clearing openlist is necessary because otherwise remaining elements of openlist
   // point to deleted node.
   openlist_ = std::priority_queue<AstarNode *, std::vector<AstarNode *>, NodeComparison>();
+  const int nb_of_grid_nodes = costmap_.info.width * costmap_.info.height;
+  const int total_astar_node_count = nb_of_grid_nodes * planner_common_param_.theta_size;
+  graph_ = std::vector<AstarNode>(total_astar_node_count);
+  col_free_distance_map_ =
+    std::vector<double>(nb_of_grid_nodes, std::numeric_limits<double>::max());
+}
 
-  graph_ = std::unordered_map<uint, AstarNode>();
+void AstarSearch::setCollisionFreeDistanceMap()
+{
+  using Entry = std::pair<IndexXY, double>;
+  struct CompareEntry
+  {
+    bool operator()(const Entry & a, const Entry & b) { return a.second > b.second; }
+  };
+  std::priority_queue<Entry, std::vector<Entry>, CompareEntry> heap;
+  std::vector<bool> closed(col_free_distance_map_.size(), false);
+  auto goal_index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size);
+  col_free_distance_map_[indexToId(goal_index)] = 0.0;
+  heap.push({IndexXY{goal_index.x, goal_index.y}, 0.0});
+
+  Entry current;
+  std::array<int, 3> offsets = {1, 0, -1};
+  while (!heap.empty()) {
+    current = heap.top();
+    heap.pop();
+    const int id = indexToId(current.first);
+    if (closed[id]) continue;
+    closed[id] = true;
+
+    const auto & index = current.first;
+    for (const auto & offset_x : offsets) {
+      const int x = index.x + offset_x;
+      for (const auto & offset_y : offsets) {
+        const int y = index.y + offset_y;
+        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;
+        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;
+        col_free_distance_map_[n_id] = dist;
+        heap.push({n_index, dist});
+      }
+    }
+  }
 }
 
 bool AstarSearch::setStartNode()
 {
   const auto index = pose2index(costmap_, start_pose_, planner_common_param_.theta_size);
 
-  if (detectCollision(index)) {
-    return false;
-  }
+  if (detectCollision(index)) return false;
 
   // Set start node
-  AstarNode * start_node = getNodeRef(index);
-  start_node->x = start_pose_.position.x;
-  start_node->y = start_pose_.position.y;
-  start_node->theta = 2.0 * M_PI / planner_common_param_.theta_size * index.theta;
-  start_node->gc = 0;
-  start_node->hc = estimateCost(start_pose_);
-  start_node->is_back = false;
+  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->status = NodeStatus::Open;
   start_node->parent = nullptr;
 
@@ -191,29 +212,18 @@ bool AstarSearch::setStartNode()
 bool AstarSearch::setGoalNode()
 {
   const auto index = pose2index(costmap_, goal_pose_, planner_common_param_.theta_size);
-
-  if (detectCollision(index)) {
-    return false;
-  }
-
-  return true;
+  return !detectCollision(index);
 }
 
-double AstarSearch::estimateCost(const geometry_msgs::msg::Pose & pose) const
+double AstarSearch::estimateCost(const Pose & pose, const IndexXYT & index) const
 {
-  double total_cost = 0.0;
+  double total_cost = col_free_distance_map_[indexToId(index)];
   // Temporarily, until reeds_shepp gets stable.
   if (use_reeds_shepp_) {
-    const double radius = (planner_common_param_.minimum_turning_radius +
-                           planner_common_param_.maximum_turning_radius) *
-                          0.5;
-    total_cost +=
-      calcReedsSheppDistance(pose, goal_pose_, radius) * astar_param_.distance_heuristic_weight;
-  } else {
-    total_cost += autoware::universe_utils::calcDistance2d(pose, goal_pose_) *
-                  astar_param_.distance_heuristic_weight;
+    total_cost =
+      std::max(total_cost, calcReedsSheppDistance(pose, goal_pose_, avg_turning_radius_));
   }
-  return total_cost;
+  return astar_param_.distance_heuristic_weight * total_cost;
 }
 
 bool AstarSearch::search()
@@ -232,6 +242,7 @@ bool AstarSearch::search()
     // Expand minimum cost node
     AstarNode * current_node = openlist_.top();
     openlist_.pop();
+    if (current_node->status == NodeStatus::Closed) continue;
     current_node->status = NodeStatus::Closed;
 
     if (isGoal(*current_node)) {
@@ -240,45 +251,81 @@ bool AstarSearch::search()
       return true;
     }
 
-    // Transit
-    const auto index_theta = discretizeAngle(current_node->theta, planner_common_param_.theta_size);
-    for (const auto & transition : transition_table_[index_theta]) {
-      const bool is_turning_point = transition.is_back != current_node->is_back;
+    expandNodes(*current_node);
+  }
+
+  // Failed to find path
+  return false;
+}
+
+void AstarSearch::expandNodes(AstarNode & current_node)
+{
+  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
+    if (
+      current_node.parent != nullptr && transition.is_back != current_node.is_back &&
+      transition.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 auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size);
 
-      const double move_cost = is_turning_point
-                                 ? planner_common_param_.reverse_weight * transition.distance
-                                 : transition.distance;
+    if (isOutOfRange(next_index) || isObs(next_index)) continue;
 
-      // Calculate index of the next state
-      geometry_msgs::msg::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 auto next_index = pose2index(costmap_, next_pose, planner_common_param_.theta_size);
+    AstarNode * next_node = &graph_[getKey(next_index)];
+    if (next_node->status == NodeStatus::Closed || detectCollision(next_index)) continue;
 
-      if (detectCollision(next_index)) {
-        continue;
-      }
+    const bool is_direction_switch =
+      (current_node.parent != nullptr) && (transition.is_back != current_node.is_back);
 
-      // Compare cost
-      AstarNode * next_node = getNodeRef(next_index);
-      if (next_node->status == NodeStatus::None) {
-        next_node->status = NodeStatus::Open;
-        next_node->x = next_pose.position.x;
-        next_node->y = next_pose.position.y;
-        next_node->theta = tf2::getYaw(next_pose.orientation);
-        next_node->gc = current_node->gc + move_cost;
-        next_node->hc = estimateCost(next_pose);
-        next_node->is_back = transition.is_back;
-        next_node->parent = current_node;
-        openlist_.push(next_node);
-        continue;
-      }
+    double total_weight = 1.0;
+    total_weight += getSteeringCost(transition.steering_index);
+    if (transition.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);
+    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->dir_distance =
+        transition.distance + (is_direction_switch ? 0.0 : current_node.dir_distance);
+      next_node->parent = &current_node;
+      openlist_.push(next_node);
+      continue;
     }
   }
+}
 
-  // Failed to find path
-  return false;
+double AstarSearch::getSteeringCost(const int steering_index) const
+{
+  return planner_common_param_.curve_weight *
+         (abs(steering_index) / planner_common_param_.turning_steps);
+}
+
+double AstarSearch::getSteeringChangeCost(
+  const int steering_index, const int prev_steering_index) const
+{
+  double steering_index_diff = abs(steering_index - prev_steering_index);
+  return astar_param_.smoothness_weight * steering_index_diff /
+         (2.0 * planner_common_param_.turning_steps);
+}
+
+double AstarSearch::getDirectionChangeCost(const double dir_distance) const
+{
+  return planner_common_param_.direction_change_weight * (1.0 + (1.0 / (1.0 + dir_distance)));
 }
 
 void AstarSearch::setPath(const AstarNode & goal_node)
@@ -293,30 +340,12 @@ void AstarSearch::setPath(const AstarNode & goal_node)
   // From the goal node to the start node
   const AstarNode * node = &goal_node;
 
-  // push exact goal pose first
-  {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.header = header;
-    pose.pose = local2global(costmap_, goal_pose_);
-
-    PlannerWaypoint pw;
-    pw.pose = pose;
-    pw.is_back = node->is_back;
-    waypoints_.waypoints.push_back(pw);
-  }
-
+  geometry_msgs::msg::PoseStamped pose;
+  pose.header = header;
   // push astar nodes poses
   while (node != nullptr) {
-    geometry_msgs::msg::PoseStamped pose;
-    pose.header = header;
     pose.pose = local2global(costmap_, node2pose(*node));
-
-    // PlannerWaypoint
-    PlannerWaypoint pw;
-    pw.pose = pose;
-    pw.is_back = node->is_back;
-    waypoints_.waypoints.push_back(pw);
-
+    waypoints_.waypoints.push_back({pose, node->is_back});
     // To the next node
     node = node->parent;
   }
@@ -359,9 +388,9 @@ bool AstarSearch::isGoal(const AstarNode & node) const
   return true;
 }
 
-geometry_msgs::msg::Pose AstarSearch::node2pose(const AstarNode & node) const
+Pose AstarSearch::node2pose(const AstarNode & node) const
 {
-  geometry_msgs::msg::Pose pose_local;
+  Pose pose_local;
 
   pose_local.position.x = node.x;
   pose_local.position.y = node.y;
diff --git a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp
index 6b0018c517d4a..0ef31959e65fa 100644
--- a/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp
+++ b/planning/autoware_freespace_planning_algorithms/src/rrtstar.cpp
@@ -14,6 +14,8 @@
 
 #include "autoware/freespace_planning_algorithms/rrtstar.hpp"
 
+#include "autoware/freespace_planning_algorithms/kinematic_bicycle_model.hpp"
+
 namespace autoware::freespace_planning_algorithms
 {
 rrtstar_core::Pose poseMsgToPose(const geometry_msgs::msg::Pose & pose_msg)
@@ -29,6 +31,7 @@ RRTStar::RRTStar(
     planner_common_param, VehicleShape(
                             original_vehicle_shape.length + 2 * rrtstar_param.margin,
                             original_vehicle_shape.width + 2 * rrtstar_param.margin,
+                            original_vehicle_shape.base_length, original_vehicle_shape.max_steering,
                             original_vehicle_shape.base2back + rrtstar_param.margin)),
   rrtstar_param_(rrtstar_param),
   original_vehicle_shape_(original_vehicle_shape)
@@ -36,9 +39,6 @@ RRTStar::RRTStar(
   if (rrtstar_param_.margin <= 0) {
     throw std::invalid_argument("rrt's collision margin must be greater than 0");
   }
-  if (planner_common_param_.maximum_turning_radius != planner_common_param.minimum_turning_radius) {
-    throw std::invalid_argument("Currently supports only single radius in rrtstar.");
-  }
 }
 
 bool RRTStar::makePlan(
@@ -50,8 +50,8 @@ bool RRTStar::makePlan(
   goal_pose_ = global2local(costmap_, goal_pose);
 
   const auto is_obstacle_free = [&](const rrtstar_core::Pose & pose) {
-    const int index_x = pose.x / costmap_.info.resolution;
-    const int index_y = pose.y / costmap_.info.resolution;
+    const int index_x = std::round(pose.x / costmap_.info.resolution);
+    const int index_y = std::round(pose.y / costmap_.info.resolution);
     const int index_theta = discretizeAngle(pose.yaw, planner_common_param_.theta_size);
     return !detectCollision(IndexXYT{index_x, index_y, index_theta});
   };
@@ -60,7 +60,8 @@ bool RRTStar::makePlan(
   const rrtstar_core::Pose hi{
     costmap_.info.resolution * costmap_.info.width, costmap_.info.resolution * costmap_.info.height,
     M_PI};
-  const double radius = planner_common_param_.minimum_turning_radius;
+  const double radius = kinematic_bicycle_model::getTurningRadius(
+    collision_vehicle_shape_.base_length, collision_vehicle_shape_.max_steering);
   const auto cspace = rrtstar_core::CSpace(lo, hi, radius, is_obstacle_free);
   const auto x_start = poseMsgToPose(start_pose_);
   const auto x_goal = poseMsgToPose(goal_pose_);
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 79898ed57327b..31c551e1906db 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
@@ -37,7 +37,10 @@ namespace fpa = autoware::freespace_planning_algorithms;
 
 const double length_lexus = 5.5;
 const double width_lexus = 2.75;
-const fpa::VehicleShape vehicle_shape = fpa::VehicleShape(length_lexus, width_lexus, 1.5);
+const double base_length_lexus = 3.0;
+const double max_steering_lexus = 0.7;
+const fpa::VehicleShape vehicle_shape =
+  fpa::VehicleShape(length_lexus, width_lexus, base_length_lexus, max_steering_lexus, 1.5);
 const double pi = 3.1415926;
 const std::array<double, 3> start_pose{5.5, 4., pi * 0.5};
 const std::array<double, 3> goal_pose1{8.0, 26.3, pi * 1.5};   // easiest
@@ -170,15 +173,14 @@ fpa::PlannerCommonParam get_default_planner_params()
 {
   // set problem configuration
   const double time_limit = 10 * 1000.0;
-  const double minimum_turning_radius = 9.0;
-  const double maximum_turning_radius = 9.0;
-  const int turning_radius_size = 1;
+  const double max_turning_ratio = 0.5;
+  const int turning_steps = 1;
 
   const int theta_size = 144;
 
-  // Setting weight to 1.0 to fairly compare all algorithms
-  const double curve_weight = 1.0;
+  const double curve_weight = 0.5;
   const double reverse_weight = 1.0;
+  const double direction_change_weight = 1.5;
 
   const double lateral_goal_range = 0.5;
   const double longitudinal_goal_range = 2.0;
@@ -187,15 +189,15 @@ fpa::PlannerCommonParam get_default_planner_params()
 
   return fpa::PlannerCommonParam{
     time_limit,
-    minimum_turning_radius,
-    maximum_turning_radius,
-    turning_radius_size,
     theta_size,
     curve_weight,
     reverse_weight,
+    direction_change_weight,
     lateral_goal_range,
     longitudinal_goal_range,
     angle_goal_range,
+    max_turning_ratio,
+    turning_steps,
     obstacle_threshold};
 }
 
@@ -203,16 +205,18 @@ std::unique_ptr<fpa::AbstractPlanningAlgorithm> configure_astar(bool use_multi)
 {
   auto planner_common_param = get_default_planner_params();
   if (use_multi) {
-    planner_common_param.maximum_turning_radius = 14.0;
-    planner_common_param.turning_radius_size = 3;
+    planner_common_param.turning_steps = 3;
   }
 
   // configure astar param
   const bool only_behind_solutions = false;
   const bool use_back = true;
-  const double distance_heuristic_weight = 1.0;
-  const auto astar_param =
-    fpa::AstarParam{only_behind_solutions, use_back, distance_heuristic_weight};
+  const double expansion_distance = 0.4;
+  const double distance_heuristic_weight = 2.0;
+  const double smoothness_weight = 0.5;
+  const auto astar_param = fpa::AstarParam{
+    only_behind_solutions, use_back, expansion_distance, distance_heuristic_weight,
+    smoothness_weight};
 
   auto algo = std::make_unique<fpa::AstarSearch>(planner_common_param, vehicle_shape, astar_param);
   return algo;
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
index e6f84e7eece0c..61908b524c253 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
@@ -90,9 +90,8 @@
           velocity: 1.0
           vehicle_shape_margin: 1.0
           time_limit: 3000.0
-          minimum_turning_radius: 5.0
-          maximum_turning_radius: 5.0
-          turning_radius_size: 1
+          max_turning_ratio: 0.7
+          turning_steps: 1
           # search configs
           search_configs:
             theta_size: 144
diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
index b86bbbca7d98b..8a4f3ab202a67 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/manager.cpp
@@ -201,17 +201,10 @@ void GoalPlannerModuleManager::init(rclcpp::Node * node)
     p.vehicle_shape_margin = node->declare_parameter<double>(ns + "vehicle_shape_margin");
     p.freespace_parking_common_parameters.time_limit =
       node->declare_parameter<double>(ns + "time_limit");
-    p.freespace_parking_common_parameters.minimum_turning_radius =
-      node->declare_parameter<double>(ns + "minimum_turning_radius");
-    p.freespace_parking_common_parameters.maximum_turning_radius =
-      node->declare_parameter<double>(ns + "maximum_turning_radius");
-    p.freespace_parking_common_parameters.turning_radius_size =
-      node->declare_parameter<int>(ns + "turning_radius_size");
-    p.freespace_parking_common_parameters.maximum_turning_radius = std::max(
-      p.freespace_parking_common_parameters.maximum_turning_radius,
-      p.freespace_parking_common_parameters.minimum_turning_radius);
-    p.freespace_parking_common_parameters.turning_radius_size =
-      std::max(p.freespace_parking_common_parameters.turning_radius_size, 1);
+    p.freespace_parking_common_parameters.max_turning_ratio =
+      node->declare_parameter<double>(ns + "max_turning_ratio");
+    p.freespace_parking_common_parameters.turning_steps =
+      node->declare_parameter<int>(ns + "turning_steps");
   }
 
   //  freespace parking search config
@@ -590,19 +583,10 @@ void GoalPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, ns + "time_limit", p->freespace_parking_common_parameters.time_limit);
     updateParam<double>(
-      parameters, ns + "minimum_turning_radius",
-      p->freespace_parking_common_parameters.minimum_turning_radius);
-    updateParam<double>(
-      parameters, ns + "maximum_turning_radius",
-      p->freespace_parking_common_parameters.maximum_turning_radius);
+      parameters, ns + "max_turning_ratio",
+      p->freespace_parking_common_parameters.max_turning_ratio);
     updateParam<int>(
-      parameters, ns + "turning_radius_size",
-      p->freespace_parking_common_parameters.turning_radius_size);
-    p->freespace_parking_common_parameters.maximum_turning_radius = std::max(
-      p->freespace_parking_common_parameters.maximum_turning_radius,
-      p->freespace_parking_common_parameters.minimum_turning_radius);
-    p->freespace_parking_common_parameters.turning_radius_size =
-      std::max(p->freespace_parking_common_parameters.turning_radius_size, 1);
+      parameters, ns + "turning_steps", p->freespace_parking_common_parameters.turning_steps);
   }
 
   //  freespace parking search config
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
index cb34f9962c479..851e96f7a265c 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/config/start_planner.param.yaml
@@ -62,9 +62,8 @@
         velocity: 1.0
         vehicle_shape_margin: 1.0
         time_limit: 3000.0
-        minimum_turning_radius: 5.0
-        maximum_turning_radius: 5.0
-        turning_radius_size: 1
+        max_turning_ratio: 0.7
+        turning_steps: 1
         # search configs
         search_configs:
           theta_size: 144
diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
index facb39c821764..23817ee081501 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/manager.cpp
@@ -137,17 +137,10 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
     p.vehicle_shape_margin = node->declare_parameter<double>(ns + "vehicle_shape_margin");
     p.freespace_planner_common_parameters.time_limit =
       node->declare_parameter<double>(ns + "time_limit");
-    p.freespace_planner_common_parameters.minimum_turning_radius =
-      node->declare_parameter<double>(ns + "minimum_turning_radius");
-    p.freespace_planner_common_parameters.maximum_turning_radius =
-      node->declare_parameter<double>(ns + "maximum_turning_radius");
-    p.freespace_planner_common_parameters.turning_radius_size =
-      node->declare_parameter<int>(ns + "turning_radius_size");
-    p.freespace_planner_common_parameters.maximum_turning_radius = std::max(
-      p.freespace_planner_common_parameters.maximum_turning_radius,
-      p.freespace_planner_common_parameters.minimum_turning_radius);
-    p.freespace_planner_common_parameters.turning_radius_size =
-      std::max(p.freespace_planner_common_parameters.turning_radius_size, 1);
+    p.freespace_planner_common_parameters.max_turning_ratio =
+      node->declare_parameter<double>(ns + "max_turning_ratio");
+    p.freespace_planner_common_parameters.turning_steps =
+      node->declare_parameter<int>(ns + "turning_steps");
   }
   //  freespace planner search config
   {
@@ -473,19 +466,10 @@ void StartPlannerModuleManager::updateModuleParams(
     updateParam<double>(
       parameters, ns + "time_limit", p->freespace_planner_common_parameters.time_limit);
     updateParam<double>(
-      parameters, ns + "minimum_turning_radius",
-      p->freespace_planner_common_parameters.minimum_turning_radius);
-    updateParam<double>(
-      parameters, ns + "maximum_turning_radius",
-      p->freespace_planner_common_parameters.maximum_turning_radius);
+      parameters, ns + "max_turning_ratio",
+      p->freespace_planner_common_parameters.max_turning_ratio);
     updateParam<int>(
-      parameters, ns + "turning_radius_size",
-      p->freespace_planner_common_parameters.turning_radius_size);
-    p->freespace_planner_common_parameters.maximum_turning_radius = std::max(
-      p->freespace_planner_common_parameters.maximum_turning_radius,
-      p->freespace_planner_common_parameters.minimum_turning_radius);
-    p->freespace_planner_common_parameters.turning_radius_size =
-      std::max(p->freespace_planner_common_parameters.turning_radius_size, 1);
+      parameters, ns + "turning_steps", p->freespace_planner_common_parameters.turning_steps);
   }
   {
     const std::string ns = "start_planner.freespace_planner.search_configs.";