feat(trajectory_follower): extend mpc trajectory for terminal yaw (au…

…towarefoundation#2447)

* feat(trajectory_follower): extend mpc trajectory for terminal yaw

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* make mpc min vel param

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add mpc extended point after smoothing

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* Revert "make mpc min vel param"

This reverts commit 02157b6.

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add comment and hypot

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* remove min vel

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add flag for extending traj

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* add extend param to default param

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix typo

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix from TakaHoribe review

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* fix typo

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

* refactor

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

control/trajectory_follower/design/mpc_lateral_controller-design.md

@@ -93,6 +93,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving.
 | path_smoothing_times                         | int    | number of times of applying path smoothing filter                                                                                                 | 1             |
 | curvature_smoothing_num_ref_steer            | double | index distance of points used in curvature calculation for reference steer command: p(i-num), p(i), p(i+num). larger num makes less noisy values. | 35            |
 | curvature_smoothing_num_traj                 | double | index distance of points used in curvature calculation for trajectory: p(i-num), p(i), p(i+num). larger num makes less noisy values.              | 1             |
+| extend_trajectory_for_end_yaw_control        | bool   | trajectory extending flag for end yaw control.                                                                                                    | true          |
 | steering_lpf_cutoff_hz                       | double | cutoff frequency of lowpass filter for steering output command [hz]                                                                               | 3.0           |
 | admissible_position_error                    | double | stop vehicle when following position error is larger than this value [m].                                                                         | 5.0           |
 | admissible_yaw_error_rad                     | double | stop vehicle when following yaw angle error is larger than this value [rad].                                                                      | 1.57          |

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -400,7 +400,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
     const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
     const double traj_resample_dist, const bool enable_path_smoothing,
     const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
-    const int curvature_smoothing_num_ref_steer);
+    const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control);
   /**
    * @brief set the vehicle model of this MPC
    */

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -97,6 +97,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   //!< @brief path resampling interval [m]
   double m_traj_resample_dist;
 
+  //!< @brief flag of traj extending for terminal yaw
+  bool m_extend_trajectory_for_end_yaw_control;
+
   /* parameters for stop state */
   double m_stop_state_entry_ego_speed;
   double m_stop_state_entry_target_speed;

control/trajectory_follower/include/trajectory_follower/mpc_utils.hpp

@@ -179,6 +179,21 @@ TRAJECTORY_FOLLOWER_PUBLIC bool calcNearestPoseInterp(
 //  */
 TRAJECTORY_FOLLOWER_PUBLIC double calcStopDistance(
   const autoware_auto_planning_msgs::msg::Trajectory & current_trajectory, const int origin);
+
+/**
+ * @brief extend terminal points
+ * Note: The current MPC does not properly take into account the attitude angle at the end of the
+ * path. By extending the end of the path in the attitude direction, the MPC can consider the
+ * attitude angle well, resulting in improved control performance. If the trajectory is
+ * well-defined considering the end point attitude angle, this feature is not necessary.
+ * @param [in] terminal yaw
+ * @param [in] extend interval
+ * @param [in] flag of forward shift
+ * @param [out] extended trajectory
+ */
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj);
+
 }  // namespace MPCUtils
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/src/mpc.cpp

@@ -180,7 +180,7 @@ void MPC::setReferenceTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & trajectory_msg,
   const double traj_resample_dist, const bool enable_path_smoothing,
   const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj,
-  const int curvature_smoothing_num_ref_steer)
+  const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control)
 {
   trajectory_follower::MPCTrajectory mpc_traj_raw;        // received raw trajectory
   trajectory_follower::MPCTrajectory mpc_traj_resampled;  // resampled trajectory
@@ -217,6 +217,17 @@ void MPC::setReferenceTrajectory(
     }
   }
 
+  /* extend terminal points
+   * Note: The current MPC does not properly take into account the attitude angle at the end of the
+   * path. By extending the end of the path in the attitude direction, the MPC can consider the
+   * attitude angle well, resulting in improved control performance. If the trajectory is
+   * well-defined considering the end point attitude angle, this feature is not necessary.
+   */
+  if (extend_trajectory_for_end_yaw_control) {
+    trajectory_follower::MPCUtils::extendTrajectoryInYawDirection(
+      mpc_traj_raw.yaw.back(), traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
+  }
+
   /* calculate yaw angle */
   trajectory_follower::MPCUtils::calcTrajectoryYawFromXY(&mpc_traj_smoothed, m_is_forward_shift);
   trajectory_follower::MPCUtils::convertEulerAngleToMonotonic(&mpc_traj_smoothed.yaw);

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -65,6 +65,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
   m_mpc.m_admissible_yaw_error_rad = node_->declare_parameter<double>("admissible_yaw_error_rad");
   m_mpc.m_use_steer_prediction = node_->declare_parameter<bool>("use_steer_prediction");
   m_mpc.m_param.steer_tau = node_->declare_parameter<double>("vehicle_model_steer_tau");
+  m_extend_trajectory_for_end_yaw_control =
+    node_->declare_parameter<bool>("extend_trajectory_for_end_yaw_control");
 
   /* stop state parameters */
   m_stop_state_entry_ego_speed = node_->declare_parameter<double>("stop_state_entry_ego_speed");
@@ -302,7 +304,8 @@ void MpcLateralController::setTrajectory(
 
   m_mpc.setReferenceTrajectory(
     *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num,
-    m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer);
+    m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer,
+    m_extend_trajectory_for_end_yaw_control);
 
   // update trajectory buffer to check the trajectory shape change.
   m_trajectory_buffer.push_back(*m_current_trajectory_ptr);

control/trajectory_follower/src/mpc_utils.cpp

@@ -265,10 +265,9 @@ bool calcMPCTrajectoryTime(MPCTrajectory & traj)
   traj.relative_time.clear();
   traj.relative_time.push_back(t);
   for (size_t i = 0; i < traj.x.size() - 1; ++i) {
-    const double dx = traj.x.at(i + 1) - traj.x.at(i);
-    const double dy = traj.y.at(i + 1) - traj.y.at(i);
-    const double dz = traj.z.at(i + 1) - traj.z.at(i);
-    const double dist = std::sqrt(dx * dx + dy * dy + dz * dz);
+    const double dist = std::hypot(
+      traj.x.at(i + 1) - traj.x.at(i), traj.y.at(i + 1) - traj.y.at(i),
+      traj.z.at(i + 1) - traj.z.at(i));
     const double v = std::max(std::fabs(traj.vx.at(i)), 0.1);
     t += (dist / v);
     traj.relative_time.push_back(t);
@@ -404,6 +403,31 @@ double calcStopDistance(
   return stop_dist;
 }
 
+void extendTrajectoryInYawDirection(
+  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj)
+{
+  // set terminal yaw
+  traj.yaw.back() = yaw;
+
+  // get terminal pose
+  autoware_auto_planning_msgs::msg::Trajectory autoware_traj;
+  autoware::motion::control::trajectory_follower::MPCUtils::convertToAutowareTrajectory(
+    traj, autoware_traj);
+  auto extended_pose = autoware_traj.points.back().pose;
+
+  constexpr double extend_dist = 10.0;
+  constexpr double extend_vel = 10.0;
+  const double x_offset = is_forward_shift ? interval : -interval;
+  const double dt = interval / extend_vel;
+  const size_t num_extended_point = static_cast<size_t>(extend_dist / interval);
+  for (size_t i = 0; i < num_extended_point; ++i) {
+    extended_pose = tier4_autoware_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);
+    traj.push_back(
+      extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(),
+      extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt);
+  }
+}
+
 }  // namespace MPCUtils
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/test/test_mpc.cpp

@@ -84,6 +84,7 @@ class MPCTest : public ::testing::Test
   int curvature_smoothing_num_ref_steer = 35;
   bool enable_path_smoothing = true;
   bool use_steer_prediction = true;
+  bool extend_trajectory_for_end_yaw_control = true;
 
   void SetUp() override
   {
@@ -175,7 +176,8 @@ class MPCTest : public ::testing::Test
     // Init trajectory
     mpc.setReferenceTrajectory(
       dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
-      path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
+      path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
+      extend_trajectory_for_end_yaw_control);
   }
 };  // class MPCTest
 
@@ -233,7 +235,8 @@ TEST_F(MPCTest, InitializeAndCalculateRightTurn)
   initializeMPC(mpc);
   mpc.setReferenceTrajectory(
     dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
-    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
+    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
+    extend_trajectory_for_end_yaw_control);
 
   // Calculate MPC
   AckermannLateralCommand ctrl_cmd;
@@ -251,7 +254,8 @@ TEST_F(MPCTest, OsqpCalculate)
   initializeMPC(mpc);
   mpc.setReferenceTrajectory(
     dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
-    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
+    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
+    extend_trajectory_for_end_yaw_control);
 
   const std::string vehicle_model_type = "kinematics";
   std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
@@ -282,7 +286,8 @@ TEST_F(MPCTest, OsqpCalculateRightTurn)
   initializeMPC(mpc);
   mpc.setReferenceTrajectory(
     dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
-    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
+    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
+    extend_trajectory_for_end_yaw_control);
 
   const std::string vehicle_model_type = "kinematics";
   std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =
@@ -327,7 +332,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculate)
   // Init trajectory
   mpc.setReferenceTrajectory(
     dummy_straight_trajectory, traj_resample_dist, enable_path_smoothing,
-    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
+    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
+    extend_trajectory_for_end_yaw_control);
   // Calculate MPC
   AckermannLateralCommand ctrl_cmd;
   Trajectory pred_traj;
@@ -344,7 +350,8 @@ TEST_F(MPCTest, KinematicsNoDelayCalculateRightTurn)
   initializeMPC(mpc);
   mpc.setReferenceTrajectory(
     dummy_right_turn_trajectory, traj_resample_dist, enable_path_smoothing,
-    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer);
+    path_filter_moving_ave_num, curvature_smoothing_num_traj, curvature_smoothing_num_ref_steer,
+    extend_trajectory_for_end_yaw_control);
 
   const std::string vehicle_model_type = "kinematics_no_delay";
   std::shared_ptr<trajectory_follower::VehicleModelInterface> vehicle_model_ptr =

control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml

@@ -12,6 +12,9 @@
     curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
     curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
 
+    # -- trajectory extending --
+    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
+
     # -- mpc optimization --
     qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp)
     mpc_prediction_horizon: 50 # prediction horizon step

launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml

@@ -13,6 +13,9 @@
     curvature_smoothing_num_traj: 15        # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num)
     curvature_smoothing_num_ref_steer: 15   # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num)
 
+    # -- trajectory extending --
+    extend_trajectory_for_end_yaw_control: true  # flag of trajectory extending for terminal yaw control
+
     # -- mpc optimization --
     qp_solver_type: "osqp"                       # optimization solver option (unconstraint_fast or osqp)
     mpc_prediction_horizon: 50                   # prediction horizon step