feat(avoidance): limit acceleration during avoidance maneuver (autowarefoundation#6698)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -287,7 +287,8 @@
           nominal_jerk: 0.5                             # [m/sss]
           max_deceleration: -1.5                        # [m/ss]
           max_jerk: 1.0                                 # [m/sss]
-          max_acceleration: 1.0                         # [m/ss]
+          max_acceleration: 0.5                         # [m/ss]
+          min_velocity_to_limit_max_acceleration: 2.78  # [m/ss]
 
       shift_line_pipeline:
         trim:

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -143,6 +143,9 @@ struct AvoidanceParameters
   // To prevent large acceleration while avoidance.
   double max_acceleration{0.0};
 
+  // To prevent large acceleration while avoidance.
+  double min_velocity_to_limit_max_acceleration{0.0};
+
   // upper distance for envelope polygon expansion.
   double upper_distance_for_polygon_expansion{0.0};
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/helper.hpp

@@ -24,6 +24,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <utility>
 #include <vector>
 
 namespace behavior_path_planner::helper::avoidance
@@ -32,7 +33,10 @@ namespace behavior_path_planner::helper::avoidance
 using behavior_path_planner::PathShifter;
 using behavior_path_planner::PlannerData;
 using motion_utils::calcDecelDistWithJerkAndAccConstraints;
+using motion_utils::calcLongitudinalOffsetPose;
+using motion_utils::calcSignedArcLength;
 using motion_utils::findNearestIndex;
+using tier4_autoware_utils::getPose;
 
 class AvoidanceHelper
 {
@@ -61,6 +65,11 @@ class AvoidanceHelper
 
   geometry_msgs::msg::Pose getEgoPose() const { return data_->self_odometry->pose.pose; }
 
+  geometry_msgs::msg::Point getEgoPosition() const
+  {
+    return data_->self_odometry->pose.pose.position;
+  }
+
   static size_t getConstraintsMapIndex(const double velocity, const std::vector<double> & values)
   {
     const auto itr = std::find_if(
@@ -262,6 +271,20 @@ class AvoidanceHelper
     return *itr;
   }
 
+  std::pair<double, double> getDistanceToAccelEndPoint(const PathWithLaneId & path)
+  {
+    updateAccelEndPoint(path);
+
+    if (!max_v_point_.has_value()) {
+      return std::make_pair(0.0, std::numeric_limits<double>::max());
+    }
+
+    const auto start_idx = data_->findEgoIndex(path.points);
+    const auto distance =
+      calcSignedArcLength(path.points, start_idx, max_v_point_.value().first.position);
+    return std::make_pair(distance, max_v_point_.value().second);
+  }
+
   double getFeasibleDecelDistance(
     const double target_velocity, const bool use_hard_constraints = true) const
   {
@@ -367,6 +390,56 @@ class AvoidanceHelper
     return true;
   }
 
+  void updateAccelEndPoint(const PathWithLaneId & path)
+  {
+    const auto & p = parameters_;
+    const auto & a_now = data_->self_acceleration->accel.accel.linear.x;
+    if (a_now < 0.0) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    if (getEgoSpeed() < p->min_velocity_to_limit_max_acceleration) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    if (max_v_point_.has_value()) {
+      return;
+    }
+
+    const auto v0 = getEgoSpeed() + p->buf_slow_down_speed;
+
+    const auto t_neg_jerk = std::max(0.0, a_now - p->max_acceleration) / p->max_jerk;
+    const auto v_neg_jerk = v0 + a_now * t_neg_jerk + std::pow(t_neg_jerk, 2.0) / 2.0;
+    const auto x_neg_jerk = v0 * t_neg_jerk + a_now * std::pow(t_neg_jerk, 2.0) / 2.0 +
+                            p->max_jerk * std::pow(t_neg_jerk, 3.0) / 6.0;
+
+    const auto & v_max = data_->parameters.max_vel;
+    if (v_max < v_neg_jerk) {
+      max_v_point_ = std::nullopt;
+      return;
+    }
+
+    const auto t_max_accel = (v_max - v_neg_jerk) / p->max_acceleration;
+    const auto x_max_accel =
+      v_neg_jerk * t_max_accel + p->max_acceleration * std::pow(t_max_accel, 2.0) / 2.0;
+
+    const auto point =
+      calcLongitudinalOffsetPose(path.points, getEgoPosition(), x_neg_jerk + x_max_accel);
+    if (point.has_value()) {
+      max_v_point_ = std::make_pair(point.value(), v_max);
+      return;
+    }
+
+    const auto x_end = calcSignedArcLength(path.points, getEgoPosition(), path.points.size() - 1);
+    const auto t_end =
+      (std::sqrt(v0 * v0 + 2.0 * p->max_acceleration * x_end) - v0) / p->max_acceleration;
+    const auto v_end = v0 + p->max_acceleration * t_end;
+
+    max_v_point_ = std::make_pair(getPose(path.points.back()), v_end);
+  }
+
   void reset()
   {
     prev_reference_path_ = PathWithLaneId();
@@ -417,6 +490,8 @@ class AvoidanceHelper
   ShiftedPath prev_linear_shift_path_;
 
   lanelet::ConstLanelets prev_driving_lanes_;
+
+  std::optional<std::pair<Pose, double>> max_v_point_;
 };
 }  // namespace behavior_path_planner::helper::avoidance
 

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -334,6 +334,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
     p.max_deceleration = getOrDeclareParameter<double>(*node, ns + "max_deceleration");
     p.max_jerk = getOrDeclareParameter<double>(*node, ns + "max_jerk");
     p.max_acceleration = getOrDeclareParameter<double>(*node, ns + "max_acceleration");
+    p.min_velocity_to_limit_max_acceleration =
+      getOrDeclareParameter<double>(*node, ns + "min_velocity_to_limit_max_acceleration");
   }
 
   // constraints (lateral)

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/scene.hpp

@@ -233,6 +233,12 @@ class AvoidanceModule : public SceneModuleInterface
    */
   void insertPrepareVelocity(ShiftedPath & shifted_path) const;
 
+  /**
+   * @brief insert max velocity in output path to limit acceleration.
+   * @param target path.
+   */
+  void insertAvoidanceVelocity(ShiftedPath & shifted_path) const;
+
   /**
    * @brief calculate stop distance based on object's overhang.
    * @param stop distance.

planning/behavior_path_avoidance_module/src/manager.cpp

@@ -244,6 +244,19 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
     }
   }
 
+  {
+    const std::string ns = "avoidance.constraints.longitudinal.";
+
+    updateParam<double>(parameters, ns + "nominal_deceleration", p->nominal_deceleration);
+    updateParam<double>(parameters, ns + "nominal_jerk", p->nominal_jerk);
+    updateParam<double>(parameters, ns + "max_deceleration", p->max_deceleration);
+    updateParam<double>(parameters, ns + "max_jerk", p->max_jerk);
+    updateParam<double>(parameters, ns + "max_acceleration", p->max_acceleration);
+    updateParam<double>(
+      parameters, ns + "min_velocity_to_limit_max_acceleration",
+      p->min_velocity_to_limit_max_acceleration);
+  }
+
   {
     const std::string ns = "avoidance.shift_line_pipeline.";
     updateParam<double>(

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -686,6 +686,7 @@ void AvoidanceModule::updateEgoBehavior(const AvoidancePlanningData & data, Shif
   }
 
   insertPrepareVelocity(path);
+  insertAvoidanceVelocity(path);
 
   switch (data.state) {
     case AvoidanceState::NOT_AVOID: {
@@ -1725,6 +1726,52 @@ void AvoidanceModule::insertPrepareVelocity(ShiftedPath & shifted_path) const
     shifted_path.path.points, start_idx, distance_to_object);
 }
 
+void AvoidanceModule::insertAvoidanceVelocity(ShiftedPath & shifted_path) const
+{
+  const auto & data = avoid_data_;
+
+  // do nothing if no shift line is approved.
+  if (path_shifter_.getShiftLines().empty()) {
+    return;
+  }
+
+  // do nothing if there is no avoidance target.
+  if (data.target_objects.empty()) {
+    return;
+  }
+
+  const auto [distance_to_accel_end_point, v_max] =
+    helper_->getDistanceToAccelEndPoint(shifted_path.path);
+  if (distance_to_accel_end_point < 1e-3) {
+    return;
+  }
+
+  const auto start_idx = planner_data_->findEgoIndex(shifted_path.path.points);
+  for (size_t i = start_idx; i < shifted_path.path.points.size(); ++i) {
+    const auto distance_from_ego = calcSignedArcLength(shifted_path.path.points, start_idx, i);
+
+    // slow down speed is inserted only in front of the object.
+    const auto accel_distance = distance_to_accel_end_point - distance_from_ego;
+    if (accel_distance < 0.0) {
+      break;
+    }
+
+    const double v_target_square =
+      v_max * v_max - 2.0 * parameters_->max_acceleration * accel_distance;
+    if (v_target_square < 1e-3) {
+      break;
+    }
+
+    // target speed with nominal jerk limits.
+    const double v_target = std::max(getEgoSpeed(), std::sqrt(v_target_square));
+    const double v_original = shifted_path.path.points.at(i).point.longitudinal_velocity_mps;
+    shifted_path.path.points.at(i).point.longitudinal_velocity_mps = std::min(v_original, v_target);
+  }
+
+  slow_pose_ = motion_utils::calcLongitudinalOffsetPose(
+    shifted_path.path.points, start_idx, distance_to_accel_end_point);
+}
+
 std::shared_ptr<AvoidanceDebugMsgArray> AvoidanceModule::get_debug_msg_array() const
 {
   debug_data_.avoidance_debug_msg_array.header.stamp = clock_->now();