feat(mpc): update steer rate calculation (tier4#1566)

* feat(mpc): update target steering rate calculation method Signed-off-by: Takamasa Horibe <[email protected]> * update Signed-off-by: Takamasa Horibe <[email protected]> Signed-off-by: Takamasa Horibe <[email protected]>
maxime-clem · Oct 20, 2022 · 5d11abd · 5d11abd
1 parent f70cb22
commit 5d11abd
Show file tree

Hide file tree

Showing 2 changed files with 33 additions and 2 deletions.
diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp
@@ -266,6 +266,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC
    * @brief add weights related to lateral_jerk, steering_rate, steering_acc into f
    */
   void addSteerWeightF(const float64_t prediction_dt, Eigen::MatrixXd * f) const;
+
+  /**
+   * @brief calculate desired steering rate.
+   */
+  float64_t calcDesiredSteeringRate(
+    const MPCMatrix & m, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+    const float64_t u_filtered, const float current_steer, const float64_t predict_dt) const;
+
   /**
    * @brief check if the matrix has invalid value
    */

diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp
@@ -94,8 +94,8 @@ bool8_t MPC::calculateMPC(
   /* set control command */
   {
     ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
-    ctrl_cmd.steering_tire_rotation_rate =
-      static_cast<float>((u_filtered - current_steer.steering_tire_angle) / prediction_dt);
+    ctrl_cmd.steering_tire_rotation_rate = static_cast<float>(calcDesiredSteeringRate(
+      mpc_matrix, x0, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));
   }
 
   storeSteerCmd(u_filtered);
@@ -818,6 +818,29 @@ float64_t MPC::getPredictionDeltaTime(
   return std::max(dt, m_param.prediction_dt);
 }
 
+float64_t MPC::calcDesiredSteeringRate(
+  const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
+  const float64_t u_filtered, const float current_steer, const float64_t predict_dt) const
+{
+  if (m_vehicle_model_type != "kinematics") {
+    // not supported yet. Use old implementation.
+    return (u_filtered - current_steer) / predict_dt;
+  }
+
+  // calculate predicted states to get the steering motion
+  const auto & m = mpc_matrix;
+  const Eigen::MatrixXd Xex = m.Aex * x0 + m.Bex * Uex + m.Wex;
+
+  const size_t STEER_IDX = 2;  // for kinematics model
+
+  const auto steer_0 = x0(STEER_IDX, 0);
+  const auto steer_1 = Xex(STEER_IDX, 0);
+
+  const auto steer_rate = (steer_1 - steer_0) / predict_dt;
+
+  return steer_rate;
+}
+
 bool8_t MPC::isValid(const MPCMatrix & m) const
 {
   if (