feat(lane_change): update path generation logic to consider lateral j…

…erk and lateral acceleration (autowarefoundation#2428)

* [lane_change] update path generation to handle lateral acceleration limit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused code & fix precommit

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update docs

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* move path_shifter implementation to cpp

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* Update planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* Update planning/behavior_path_planner/behavior_path_planner_path_generation.md

Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>

* update doc link

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* update doc

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* remove unused code

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

* add common min distance computation

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* rearrange config and rework min distance

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* revert some changes

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* remove warning

Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

* Update planning/behavior_path_planner/behavior_path_planner_path_generation.md

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* Update planning/behavior_path_planner/behavior_path_planner_path_generation.md

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* fix spell check

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com>
Co-authored-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Signed-off-by: kminoda <koji.minoda@tier4.jp>

planning/behavior_path_planner/README.md

@@ -155,6 +155,8 @@ If there are multiple avoidance targets and the lateral distances of these are c
 
 The path generation is computed in Frenet coordinates. The shift length profile for avoidance is generated by four segmental constant jerk polynomials, and added to the original path. Since the lateral jerk can be approximately seen as a steering maneuver, this calculation yields a result similar to a Clothoid curve.
 
+For more detail, see [behavior-path-planner-path-generation](./behavior_path_planner_path_generation.md).
+
 <!-- <p align="center"> <img src="./image/path_shifter.png" width="1000"/> </p> -->
 
 ![path_shifter](./image/path_shifter.png)

planning/behavior_path_planner/behavior_path_planner_path_generation.md

@@ -0,0 +1,97 @@
+## Path Generation
+
+This document explains how the path is generated for lane change and avoidance, etc. The implementation can be found in [path_shifter.hpp](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp).
+
+### Overview
+
+The base idea of the path generation in lane change and avoidance is to smoothly shift the reference path, such as the center line, in the lateral direction. This is achieved by using a constant-jerk profile as in the figure below. More details on how it is used can be found in [README](https://github.com/autowarefoundation/autoware.universe/blob/main/planning/behavior_path_planner/README.md). It is assumed that the reference path is smooth enough for this algorithm.
+
+The figure below explains how the application of a constant lateral jerk $l^{'''}(s)$ can be used to induce lateral shifting. In order to comply with the limits on lateral acceleration and velocity, zero-jerk time is employed in the figure ( $T_a$ and $T_v$ ). In each interval where constant jerk is applied, the shift position $l(s)$ can be characterized by a third-degree polynomial. Therefore the shift length from the reference path can then be calculated by combining spline curves.
+
+<p align="center">
+  <img src="./image/path_shifter.png" width="800">
+</p>
+
+Note that, due to the rarity of the $T_v$ in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation.
+
+### Mathematical Derivation
+
+By applying simple integral operations, the following analytical equations can be derived to describe the shift distance $l(t)$ at each time under lateral jerk, acceleration, and velocity constraints.
+
+```math
+\begin{align}
+l_1&= \frac{1}{6}jT_j^3\\[10pt]
+l_2&= \frac{1}{6}j T_j^3 + \frac{1}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j\\[10pt]
+l_3&= j  T_j^3 + \frac{3}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j\\[10pt]
+l_4&= j T_j^3 + \frac{3}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j + j(T_a + T_j)T_j T_v\\[10pt]
+l_5&= \frac{11}{6} j T_j^3 + \frac{5}{2} j T_a T_j^2 + \frac{1}{2} j T_a^2 T_j + j(T_a + T_j)T_j T_v \\[10pt]
+l_6&= \frac{11}{6} j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j + j(T_a + T_j)T_j T_v\\[10pt]
+l_7&= 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j + j(T_a + T_j)T_j T_v
+\end{align}
+```
+
+These equations are used to determine the shape of a path. Additionally, by applying further mathematical operations to these basic equations, the expressions of the following subsections can be derived.
+
+#### Calculation of Maximum Acceleration from transition time and final shift length
+
+In the case where there are no limitations on lateral velocity and lateral acceleration, the maximum lateral acceleration during the shifting can be calculated as follows. The constant-jerk time is given by $T_j = T_{\rm total}/4$ because of its symmetric property. Since $T_a=T_v=0$, the final shift length $L=l_7=2jT_j^3$ can be determined using the above equation. The maximum lateral acceleration is then given by $a_{\rm max} =jT_j$. This results in the following expression for the maximum lateral acceleration:
+
+```math
+\begin{align}
+a_{\rm max}  = \frac{8L}{T_{\rm total}^2}
+\end{align}
+```
+
+#### Calculation of Ta, Tj and jerk from acceleration limit
+
+In the case where there are no limitations on lateral velocity, the constant-jerk and acceleration times, as well as the required jerk can be calculated from the acceleration limit, total time, and final shift length as follows. Since $T_v=0$, the final shift length is given by:
+
+```math
+\begin{align}
+L = l_7 = 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j
+\end{align}
+```
+
+Additionally, the velocity profile reveals the following relations:
+
+```math
+\begin{align}
+a_{\rm lim} &= j T_j\\
+T_{\rm total} &= 4T_j + 2T_a
+\end{align}
+```
+
+By solving these three equations, the following can be obtained:
+
+```math
+\begin{align}
+T_j&=\frac{T_{\rm total}}{2} - \frac{2L}{a_{\rm lim} T_{\rm total}}\\[10pt]
+T_a&=\frac{4L}{a_{\rm lim} T_{\rm total}} - \frac{T_{\rm total}}{2}\\[10pt]
+jerk&=\frac{2a_{\rm lim} ^2T_{\rm total}}{a_{\rm lim} T_{\rm total}^2-4L}
+\end{align}
+```
+
+where $T_j$ is the constant-jerk time, $T_a$ is the constant acceleration time, $j$ is the required jerk, $a_{\rm lim}$ is the acceleration limit, and $L$ is the final shift length.
+
+#### Calculation of Required Time from Jerk and Acceleration Constraint
+
+In the case where there are no limitations on lateral velocity, the total time required for shifting can be calculated from the jerk and acceleration limits and the final shift length as follows. By solving the two equations given above:
+
+```math
+L = l_7 = 2 j T_j^3 + 3 j T_a T_j^2 + j T_a^2 T_j,\quad a_{\rm lim} = j T_j
+```
+
+we obtain the following expressions:
+
+```math
+\begin{align}
+T_j &= \frac{a_{\rm lim}}{j}\\[10pt]
+T_a &= \frac{1}{2}\sqrt{\frac{a_{\rm lim}}{j}^2 + \frac{4L}{a_{\rm lim}}} - \frac{3a_{\rm lim}}{2j}
+\end{align}
+```
+
+The total time required for shifting can then be calculated as $T_{\rm total}=4T_j+2T_a$.
+
+### Limitation
+
+- Since $T_v$ is zero in almost all cases of lane change and avoidance, $T_v$ is not considered in the current implementation.

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -2,11 +2,8 @@
   ros__parameters:
     backward_path_length: 5.0
     forward_path_length: 200.0
-    backward_length_buffer_for_end_of_lane: 5.0
     backward_length_buffer_for_end_of_pull_over: 5.0
     backward_length_buffer_for_end_of_pull_out: 5.0
-    minimum_lane_change_length: 12.0
-    minimum_lane_change_prepare_distance: 2.0 # [m]
     minimum_pull_over_length: 16.0
 
     refine_goal_search_radius_range: 7.5

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -2,19 +2,28 @@
   ros__parameters:
     lane_change:
       lane_change_prepare_duration: 4.0         # [s]
-      lane_changing_duration: 8.0               # [s]
-      minimum_lane_change_prepare_distance: 4.0 # [m]
+      lane_changing_safety_check_duration: 8.0  # [s]
+
+      minimum_lane_change_prepare_distance: 2.0 # [m]
+      minimum_lane_change_length: 16.5          # [m]
+      backward_length_buffer_for_end_of_lane: 2.0 # [m]
       lane_change_finish_judge_buffer: 3.0      # [m]
-      minimum_lane_change_velocity: 5.6         # [m/s]
+
+      double lane_changing_lateral_jerk: 0.5    # [m/s3]
+      double lane_changing_lateral_acc: 0.5     # [m/s2]
+
+      minimum_lane_change_velocity: 2.778       # [m/s]
       prediction_time_resolution: 0.5           # [s]
       maximum_deceleration: 1.0                 # [m/s2]
       lane_change_sampling_num: 10
+
       abort_lane_change_velocity_thresh: 0.5
       abort_lane_change_angle_thresh: 10.0      # [deg]
       abort_lane_change_distance_thresh: 0.3    # [m]
       prepare_phase_ignore_target_speed_thresh: 0.1 # [m/s]
+
       enable_abort_lane_change: true
-      enable_collision_check_at_prepare_phase: false
+      enable_collision_check_at_prepare_phase: true
       use_predicted_path_outside_lanelet: true
-      use_all_predicted_path: true
+      use_all_predicted_path: false
       publish_debug_marker: false

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp

@@ -26,7 +26,9 @@ namespace behavior_path_planner
 struct LaneChangeParameters
 {
   double lane_change_prepare_duration{2.0};
-  double lane_changing_duration{4.0};
+  double lane_changing_safety_check_duration{4.0};
+  double lane_changing_lateral_jerk{0.5};
+  double lane_changing_lateral_acc{0.5};
   double lane_change_finish_judge_buffer{3.0};
   double minimum_lane_change_velocity{5.6};
   double prediction_time_resolution{0.5};

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp

@@ -15,7 +15,9 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__UTIL_HPP_
 
+#include "behavior_path_planner/parameters.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_module.hpp"
+#include "behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/scene_module/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/utilities.hpp"
 
@@ -31,6 +33,7 @@
 #include <memory>
 #include <string>
 #include <unordered_map>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::lane_change_utils
@@ -53,18 +56,18 @@ double getExpectedVelocityWhenDecelerate(
   const double & current_velocity, const double & expected_acceleration,
   const double & lane_change_prepare_duration);
 
-double getDistanceWhenDecelerate(
-  const double & velocity, const double & expected_acceleration, const double & duration,
-  const double & minimum_distance);
+std::pair<double, double> calcLaneChangingSpeedAndDistanceWhenDecelerate(
+  const double velocity, const double shift_length, const double deceleration,
+  const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param,
+  const LaneChangeParameters & lc_param);
 
 std::optional<LaneChangePath> constructCandidatePath(
   const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment,
   const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line,
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  const double & acceleration, const double & prepare_distance, const double & prepare_duration,
-  const double & prepare_speed, const double & minimum_prepare_distance,
-  const double & lane_change_distance, const double & lane_changing_duration,
-  const double & minimum_lane_change_velocity);
+  const double acceleration, const double prepare_distance, const double prepare_duration,
+  const double prepare_speed, const double lane_change_distance, const double lane_changing_speed,
+  const BehaviorPathPlannerParameters & params, const LaneChangeParameters & lane_change_param);
 
 LaneChangePaths getLaneChangePaths(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
@@ -125,10 +128,8 @@ PathWithLaneId getLaneChangePathPrepareSegment(
 
 PathWithLaneId getLaneChangePathLaneChangingSegment(
   const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
-  const Pose & current_pose, const double & forward_path_length, const double & prepare_distance,
-  const double & lane_change_distance, const double & minimum_lane_change_length,
-  const double & lane_change_distance_buffer, const double & lane_changing_duration,
-  const double & minimum_lane_change_velocity);
+  const Pose & current_pose, const double prepare_distance, const double lane_change_distance,
+  const double lane_changing_speed, const BehaviorPathPlannerParameters & common_param);
 bool isEgoWithinOriginalLane(
   const lanelet::ConstLanelets & current_lanes, const Pose & current_pose,
   const BehaviorPathPlannerParameters & common_param);

planning/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp

@@ -26,8 +26,8 @@
 
 #include <algorithm>
 #include <string>
+#include <utility>
 #include <vector>
-
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathPointWithLaneId;
@@ -72,6 +72,16 @@ class PathShifter
    */
   void setPath(const PathWithLaneId & path);
 
+  /**
+   * @brief  Set velocity used to apply a lateral acceleration limit.
+   */
+  void setVelocity(const double velocity);
+
+  /**
+   * @brief  Set acceleration limit
+   */
+  void setLateralAccelerationLimit(const double acc);
+
   /**
    * @brief  Add shift point. You don't have to care about the start/end_idx.
    */
@@ -122,65 +132,23 @@ class PathShifter
   ////////////////////////////////////////
 
   static double calcLongitudinalDistFromJerk(
-    const double lateral, const double jerk, const double velocity)
-  {
-    const double j = std::abs(jerk);
-    const double l = std::abs(lateral);
-    const double v = std::abs(velocity);
-    if (j < 1.0e-8) {
-      return 1.0e10;  // TODO(Horibe) maybe invalid arg?
-    }
-    return 4.0 * std::pow(0.5 * l / j, 1.0 / 3.0) * v;
-  }
-
-  static double calcJerkFromLatLonDistance(
-    const double lateral, const double longitudinal, const double velocity)
-  {
-    constexpr double ep = 1.0e-3;
-    const double lat = std::abs(lateral);
-    const double lon = std::max(std::abs(longitudinal), ep);
-    const double v = std::abs(velocity);
-    return 0.5 * lat * std::pow(4.0 * v / lon, 3);
-  }
+    const double lateral, const double jerk, const double velocity);
 
-  double getTotalShiftLength() const
-  {
-    double sum = base_offset_;
-    for (const auto & l : shift_lines_) {
-      sum += l.end_shift_length;
-    }
-    return sum;
-  }
-
-  double getLastShiftLength() const
-  {
-    if (shift_lines_.empty()) {
-      return base_offset_;
-    }
+  static double calcShiftTimeFromJerkAndJerk(
+    const double lateral, const double jerk, const double acc);
 
-    const auto furthest = std::max_element(
-      shift_lines_.begin(), shift_lines_.end(),
-      [](auto & a, auto & b) { return a.end_idx < b.end_idx; });
+  static double calcJerkFromLatLonDistance(
+    const double lateral, const double longitudinal, const double velocity);
 
-    return furthest->end_shift_length;
-  }
+  double getTotalShiftLength() const;
 
-  boost::optional<ShiftLine> getLastShiftLine() const
-  {
-    if (shift_lines_.empty()) {
-      return {};
-    }
+  double getLastShiftLength() const;
 
-    const auto furthest = std::max_element(
-      shift_lines_.begin(), shift_lines_.end(),
-      [](auto & a, auto & b) { return a.end_idx > b.end_idx; });
-
-    return *furthest;
-  }
+  boost::optional<ShiftLine> getLastShiftLine() const;
 
   /**
    * @brief  Calculate the theoretical lateral jerk by spline shifting for current shift_lines_.
-   * @return Jerk array. THe size is same as the shift points.
+   * @return Jerk array. The size is same as the shift points.
    */
   std::vector<double> calcLateralJerk() const;
 
@@ -194,13 +162,25 @@ class PathShifter
   // The amount of shift length to the entire path.
   double base_offset_{0.0};
 
+  // Used to apply a lateral acceleration limit
+  double velocity_{0.0};
+
+  // lateral acceleration limit considered in the path planning
+  double acc_limit_{-1.0};
+
   // Logger
   mutable rclcpp::Logger logger_{
     rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")};
 
   // Clock
   mutable rclcpp::Clock clock_{RCL_ROS_TIME};
 
+  std::pair<std::vector<double>, std::vector<double>> calcBaseLengths(
+    const double arclength, const double shift_length, const bool offset_back) const;
+
+  std::pair<std::vector<double>, std::vector<double>> getBaseLengthsWithoutAccelLimit(
+    const double arclength, const double shift_length, const bool offset_back) const;
+
   /**
    * @brief Calculate path index for shift_lines and set is_index_aligned_ to true.
    */

planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp

@@ -482,11 +482,12 @@ bool isSafeInFreeSpaceCollisionCheck(
 
 bool checkPathRelativeAngle(const PathWithLaneId & path, const double angle_threshold);
 
-double calcTotalLaneChangeDistanceWithBuffer(const BehaviorPathPlannerParameters & common_param);
+double calcTotalLaneChangeDistance(
+  const BehaviorPathPlannerParameters & common_param, const bool include_buffer = true);
 
 double calcLaneChangeBuffer(
   const BehaviorPathPlannerParameters & common_param, const int num_lane_change,
-  const double length_to_intersection);
+  const double length_to_intersection = 0.0);
 }  // namespace behavior_path_planner::util
 
 #endif  // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_

planning/behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -209,14 +209,14 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
   p.backward_path_length = declare_parameter("backward_path_length", 5.0) + backward_offset;
   p.forward_path_length = declare_parameter("forward_path_length", 100.0);
   p.backward_length_buffer_for_end_of_lane =
-    declare_parameter("backward_length_buffer_for_end_of_lane", 5.0);
+    declare_parameter("lane_change.backward_length_buffer_for_end_of_lane", 5.0);
   p.backward_length_buffer_for_end_of_pull_over =
     declare_parameter("backward_length_buffer_for_end_of_pull_over", 5.0);
   p.backward_length_buffer_for_end_of_pull_out =
     declare_parameter("backward_length_buffer_for_end_of_pull_out", 5.0);
-  p.minimum_lane_change_length = declare_parameter("minimum_lane_change_length", 8.0);
+  p.minimum_lane_change_length = declare_parameter("lane_change.minimum_lane_change_length", 8.0);
   p.minimum_lane_change_prepare_distance =
-    declare_parameter("minimum_lane_change_prepare_distance", 2.0);
+    declare_parameter("lane_change.minimum_lane_change_prepare_distance", 2.0);
 
   p.minimum_pull_over_length = declare_parameter("minimum_pull_over_length", 15.0);
   p.refine_goal_search_radius_range = declare_parameter("refine_goal_search_radius_range", 7.5);
@@ -367,7 +367,9 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam()
 
   LaneChangeParameters p{};
   p.lane_change_prepare_duration = dp("lane_change_prepare_duration", 2.0);
-  p.lane_changing_duration = dp("lane_changing_duration", 4.0);
+  p.lane_changing_safety_check_duration = dp("lane_changing_safety_check_duration", 4.0);
+  p.lane_changing_lateral_jerk = dp("lane_changing_lateral_jerk", 0.5);
+  p.lane_changing_lateral_acc = dp("lane_changing_lateral_acc", 0.5);
   p.lane_change_finish_judge_buffer = dp("lane_change_finish_judge_buffer", 3.0);
   p.minimum_lane_change_velocity = dp("minimum_lane_change_velocity", 5.6);
   p.prediction_time_resolution = dp("prediction_time_resolution", 0.5);