feat(trajectory_follower): add min_prediction_length to mpc (tier4#1171)

* feat(trajectory_follower): Add min_prediction_length to mpc

Signed-off-by: kosuke55 <[email protected]>

* refactor

Signed-off-by: kosuke55 <[email protected]>

control/trajectory_follower/include/trajectory_follower/mpc.hpp

@@ -70,6 +70,8 @@ struct MPCParam
   float64_t acceleration_limit;
   //!< @brief for trajectory velocity calculation
   float64_t velocity_time_constant;
+  //!< @brief minimum prediction dist for low velocity
+  float64_t min_prediction_length;
   //!< @brief time constant for steer model
   float64_t steer_tau;
   // for weight matrix Q
@@ -179,6 +181,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
   float64_t m_sign_vx = 0.0;
   //!< @brief buffer of sent command
   std::vector<autoware_auto_control_msgs::msg::AckermannLateralCommand> m_ctrl_cmd_vec;
+  //!< @brief minimum prediction distance
+  float64_t m_min_prediction_length = 5.0;
 
   /**
    * @brief get variables for mpc calculation
@@ -222,20 +226,24 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    * @brief generate MPC matrix with trajectory and vehicle model
    * @param [in] reference_trajectory used for linearization around reference trajectory
    */
-  MPCMatrix generateMPCMatrix(const trajectory_follower::MPCTrajectory & reference_trajectory);
+  MPCMatrix generateMPCMatrix(
+    const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t predition_dt);
   /**
    * @brief generate MPC matrix with trajectory and vehicle model
    * @param [in] mpc_matrix parameters matrix to use for optimization
    * @param [in] x0 initial state vector
+   * @param [in] precition_dt predition deleta time
    * @param [out] Uex optimized input vector
    */
   bool8_t executeOptimization(
-    const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, Eigen::VectorXd * Uex);
+    const MPCMatrix & mpc_matrix, const Eigen::VectorXd & x0, const float64_t predition_dt,
+    Eigen::VectorXd * Uex);
   /**
    * @brief resample trajectory with mpc resampling time
    */
   bool8_t resampleMPCTrajectoryByTime(
-    float64_t start_time, const trajectory_follower::MPCTrajectory & input,
+    const float64_t start_time, const float64_t prediction_dt,
+    const trajectory_follower::MPCTrajectory & input,
     trajectory_follower::MPCTrajectory * output) const;
   /**
    * @brief apply velocity dynamics filter with v0 from closest index
@@ -244,17 +252,20 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
     const trajectory_follower::MPCTrajectory & trajectory,
     const geometry_msgs::msg::Pose & current_pose, const float64_t v0) const;
   /**
-   * @brief get total prediction time of mpc
+   * @brief get prediction delta time of mpc.
+   * If trajectory length is shorter than min_prediction length, adjust delta time.
    */
-  float64_t getPredictionTime() const;
+  float64_t getPredictionDeletaTime(
+    const float64_t start_time, const trajectory_follower::MPCTrajectory & input,
+    const geometry_msgs::msg::Pose & current_pose) const;
   /**
    * @brief add weights related to lateral_jerk, steering_rate, steering_acc into R
    */
-  void addSteerWeightR(Eigen::MatrixXd * R) const;
+  void addSteerWeightR(const float64_t predition_dt, Eigen::MatrixXd * R) const;
   /**
    * @brief add weights related to lateral_jerk, steering_rate, steering_acc into f
    */
-  void addSteerWeightF(Eigen::MatrixXd * f) const;
+  void addSteerWeightF(const float64_t predition_dt, Eigen::MatrixXd * f) const;
   /**
    * @brief check if the matrix has invalid value
    */

control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp

@@ -18,6 +18,8 @@
 #include "common/types.hpp"
 #include "trajectory_follower/visibility_control.hpp"
 
+#include "geometry_msgs/msg/point.hpp"
+
 #include <iostream>
 #include <vector>
 
@@ -66,6 +68,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPCTrajectory
    * @return true if the compensents sizes are all 0 or are inconsistent
    */
   inline bool empty() const { return size() == 0; }
+
+  std::vector<geometry_msgs::msg::Point> toPoints() const
+  {
+    std::vector<geometry_msgs::msg::Point> points;
+    for (size_t i = 0; i < x.size(); ++i) {
+      geometry_msgs::msg::Point point;
+      point.x = x.at(i);
+      point.y = y.at(i);
+      point.z = z.at(i);
+      points.push_back(point);
+    }
+    return points;
+  }
 };
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/src/mpc.cpp

@@ -67,17 +67,20 @@ bool8_t MPC::calculateMPC(
   /* resample ref_traj with mpc sampling time */
   trajectory_follower::MPCTrajectory mpc_resampled_ref_traj;
   const float64_t mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
-  if (!resampleMPCTrajectoryByTime(mpc_start_time, reference_trajectory, &mpc_resampled_ref_traj)) {
+  const float64_t prediction_dt =
+    getPredictionDeletaTime(mpc_start_time, reference_trajectory, current_pose);
+  if (!resampleMPCTrajectoryByTime(
+        mpc_start_time, prediction_dt, reference_trajectory, &mpc_resampled_ref_traj)) {
     RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "trajectory resampling failed.");
     return false;
   }
 
   /* generate mpc matrix : predict equation Xec = Aex * x0 + Bex * Uex + Wex */
-  MPCMatrix mpc_matrix = generateMPCMatrix(mpc_resampled_ref_traj);
+  MPCMatrix mpc_matrix = generateMPCMatrix(mpc_resampled_ref_traj, prediction_dt);
 
   /* solve quadratic optimization */
   Eigen::VectorXd Uex;
-  if (!executeOptimization(mpc_matrix, x0, &Uex)) {
+  if (!executeOptimization(mpc_matrix, x0, prediction_dt, &Uex)) {
     RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "optimization failed.");
     return false;
   }
@@ -88,9 +91,8 @@ bool8_t MPC::calculateMPC(
 
   /* set control command */
   {
-    const auto & dt = m_param.prediction_dt;
     ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
-    ctrl_cmd.steering_tire_rotation_rate = static_cast<float>((Uex(1) - Uex(0)) / dt);
+    ctrl_cmd.steering_tire_rotation_rate = static_cast<float>((Uex(1) - Uex(0)) / prediction_dt);
   }
 
   storeSteerCmd(u_filtered);
@@ -228,7 +230,7 @@ void MPC::setReferenceTrajectory(
   {
     auto & t = mpc_traj_smoothed;
     const float64_t t_ext = 100.0;  // extra time to prevent mpc calcul failure due to short time
-    const float64_t t_end = t.relative_time.back() + getPredictionTime() + t_ext;
+    const float64_t t_end = t.relative_time.back() + t_ext;
     const float64_t v_end = 0.0;
     t.vx.back() = v_end;  // set for end point
     t.push_back(
@@ -305,7 +307,9 @@ bool8_t MPC::getData(
   }
 
   /* check trajectory time length */
-  auto end_time = data->nearest_time + m_param.input_delay + getPredictionTime();
+  const float64_t max_prediction_time =
+    m_param.min_prediction_length / static_cast<float64_t>(m_param.prediction_horizon - 1);
+  auto end_time = data->nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time;
   if (end_time > traj.relative_time.back()) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(
       m_logger, *m_clock, 1000 /*ms*/, "path is too short for prediction.");
@@ -392,12 +396,13 @@ void MPC::storeSteerCmd(const float64_t steer)
 }
 
 bool8_t MPC::resampleMPCTrajectoryByTime(
-  float64_t ts, const trajectory_follower::MPCTrajectory & input,
+  const float64_t ts, const float64_t prediction_dt,
+  const trajectory_follower::MPCTrajectory & input,
   trajectory_follower::MPCTrajectory * output) const
 {
   std::vector<float64_t> mpc_time_v;
   for (float64_t i = 0; i < static_cast<float64_t>(m_param.prediction_horizon); ++i) {
-    mpc_time_v.push_back(ts + i * m_param.prediction_dt);
+    mpc_time_v.push_back(ts + i * prediction_dt);
   }
   if (!trajectory_follower::MPCUtils::linearInterpMPCTrajectory(
         input.relative_time, input, mpc_time_v, output)) {
@@ -493,7 +498,7 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
   trajectory_follower::MPCUtils::dynamicSmoothingVelocity(
     static_cast<size_t>(nearest_idx), v0, acc_lim, tau, output);
   const float64_t t_ext = 100.0;  // extra time to prevent mpc calculation failure due to short time
-  const float64_t t_end = output.relative_time.back() + getPredictionTime() + t_ext;
+  const float64_t t_end = output.relative_time.back() + t_ext;
   const float64_t v_end = 0.0;
   output.vx.back() = v_end;  // set for end point
   output.push_back(
@@ -507,12 +512,13 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter(
  * cost function: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex
  * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
  */
-MPCMatrix MPC::generateMPCMatrix(const trajectory_follower::MPCTrajectory & reference_trajectory)
+MPCMatrix MPC::generateMPCMatrix(
+  const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t prediction_dt)
 {
   using Eigen::MatrixXd;
 
   const int64_t N = m_param.prediction_horizon;
-  const float64_t DT = m_param.prediction_dt;
+  const float64_t DT = prediction_dt;
   const int64_t DIM_X = m_vehicle_model_ptr->getDimX();
   const int64_t DIM_U = m_vehicle_model_ptr->getDimU();
   const int64_t DIM_Y = m_vehicle_model_ptr->getDimY();
@@ -613,7 +619,7 @@ MPCMatrix MPC::generateMPCMatrix(const trajectory_follower::MPCTrajectory & refe
     m.R2ex.block(i, i, 2, 2) += J;
   }
 
-  addSteerWeightR(&m.R1ex);
+  addSteerWeightR(prediction_dt, &m.R1ex);
 
   return m;
 }
@@ -641,7 +647,8 @@ MPCMatrix MPC::generateMPCMatrix(const trajectory_follower::MPCTrajectory & refe
  * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
  */
 bool8_t MPC::executeOptimization(
-  const MPCMatrix & m, const Eigen::VectorXd & x0, Eigen::VectorXd * Uex)
+  const MPCMatrix & m, const Eigen::VectorXd & x0, const float64_t prediction_dt,
+  Eigen::VectorXd * Uex)
 {
   using Eigen::MatrixXd;
   using Eigen::VectorXd;
@@ -664,7 +671,7 @@ bool8_t MPC::executeOptimization(
   H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
   H.triangularView<Eigen::Lower>() = H.transpose();
   MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex;
-  addSteerWeightF(&f);
+  addSteerWeightF(prediction_dt, &f);
 
   MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N);
   for (int64_t i = 1; i < DIM_U_N; i++) {
@@ -673,8 +680,8 @@ bool8_t MPC::executeOptimization(
 
   VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim);  // min steering angle
   VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // max steering angle
-  VectorXd lbA = VectorXd::Constant(DIM_U_N, -m_steer_rate_lim * m_param.prediction_dt);
-  VectorXd ubA = VectorXd::Constant(DIM_U_N, m_steer_rate_lim * m_param.prediction_dt);
+  VectorXd lbA = VectorXd::Constant(DIM_U_N, -m_steer_rate_lim * prediction_dt);
+  VectorXd ubA = VectorXd::Constant(DIM_U_N, m_steer_rate_lim * prediction_dt);
   lbA(0, 0) = m_raw_steer_cmd_prev - m_steer_rate_lim * m_ctrl_period;
   ubA(0, 0) = m_raw_steer_cmd_prev + m_steer_rate_lim * m_ctrl_period;
 
@@ -699,10 +706,10 @@ bool8_t MPC::executeOptimization(
   return true;
 }
 
-void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr) const
+void MPC::addSteerWeightR(const float64_t prediction_dt, Eigen::MatrixXd * R_ptr) const
 {
   const int64_t N = m_param.prediction_horizon;
-  const float64_t DT = m_param.prediction_dt;
+  const float64_t DT = prediction_dt;
 
   auto & R = *R_ptr;
 
@@ -744,13 +751,13 @@ void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr) const
   }
 }
 
-void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr) const
+void MPC::addSteerWeightF(const float64_t prediction_dt, Eigen::MatrixXd * f_ptr) const
 {
   if (f_ptr->rows() < 2) {
     return;
   }
 
-  const float64_t DT = m_param.prediction_dt;
+  const float64_t DT = prediction_dt;
   auto & f = *f_ptr;
 
   // steer rate for i = 0
@@ -770,10 +777,39 @@ void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr) const
   f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5;
 }
 
-float64_t MPC::getPredictionTime() const
+float64_t MPC::getPredictionDeletaTime(
+  const float64_t start_time, const trajectory_follower::MPCTrajectory & input,
+  const geometry_msgs::msg::Pose & current_pose) const
 {
-  return static_cast<float64_t>(m_param.prediction_horizon - 1) * m_param.prediction_dt +
-         m_param.input_delay + m_ctrl_period;
+  // Calculate the time min_prediction_length ahead from current_pose
+  const size_t nearest_idx =
+    static_cast<size_t>(trajectory_follower::MPCUtils::calcNearestIndex(input, current_pose));
+  float64_t sum_dist = 0;
+  const float64_t target_time = [&]() {
+    const float64_t t_ext =
+      100.0;  // extra time to prevent mpc calculation failure due to short time
+    for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
+      const float64_t segment_dist =
+        std::hypot(input.x.at(i) - input.x.at(i - 1), input.y.at(i) - input.y.at(i - 1));
+      sum_dist += segment_dist;
+      if (m_param.min_prediction_length < sum_dist) {
+        const float64_t prev_sum_dist = sum_dist - segment_dist;
+        const float64_t ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
+        const float64_t relative_time_at_i = i == input.relative_time.size() - 1
+                                               ? input.relative_time.at(i) - t_ext
+                                               : input.relative_time.at(i);
+        return input.relative_time.at(i - 1) +
+               (relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
+      }
+    }
+    return input.relative_time.back() - t_ext;
+  }();
+
+  // Calculate deleta time for min_prediction_length
+  const float64_t dt =
+    (target_time - start_time) / static_cast<float64_t>(m_param.prediction_horizon - 1);
+
+  return std::max(dt, m_param.prediction_dt);
 }
 
 bool8_t MPC::isValid(const MPCMatrix & m) const

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -453,6 +453,8 @@ void MpcLateralController::declareMPCparameters()
   m_mpc.m_param.acceleration_limit = node_->declare_parameter<float64_t>("mpc_acceleration_limit");
   m_mpc.m_param.velocity_time_constant =
     node_->declare_parameter<float64_t>("mpc_velocity_time_constant");
+  m_mpc.m_param.min_prediction_length =
+    node_->declare_parameter<float64_t>("mpc_min_prediction_length");
 }
 
 rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback(
@@ -505,6 +507,7 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback(
     update_param(parameters, "mpc_zero_ff_steer_deg", param.zero_ff_steer_deg);
     update_param(parameters, "mpc_acceleration_limit", param.acceleration_limit);
     update_param(parameters, "mpc_velocity_time_constant", param.velocity_time_constant);
+    update_param(parameters, "mpc_min_prediction_length", param.min_prediction_length);
 
     // initialize input buffer
     update_param(parameters, "input_delay", param.input_delay);

control/trajectory_follower/test/test_mpc.cpp

@@ -95,6 +95,7 @@ class MPCTest : public ::testing::Test
     param.input_delay = 0.0;
     param.acceleration_limit = 2.0;
     param.velocity_time_constant = 0.3;
+    param.min_prediction_length = 5.0;
     param.steer_tau = 0.1;
     param.weight_lat_error = 1.0;
     param.weight_heading_error = 1.0;

control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml

@@ -38,6 +38,7 @@
     mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero
     mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration
     mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing
+    mpc_min_prediction_length: 5.0 # minimum prediction length
 
     # -- vehicle model --
     vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics

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

@@ -39,6 +39,7 @@
     mpc_zero_ff_steer_deg: 0.5                   # threshold that feed-forward angle becomes zero
     mpc_acceleration_limit: 2.0                  # limit on the vehicle's acceleration
     mpc_velocity_time_constant: 0.3              # time constant used for velocity smoothing
+    mpc_min_prediction_length: 5.0               # minimum prediction length
 
     # -- vehicle model --
     vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics