From 5d11abd9d53231963b0a5eaf950475457cb659b9 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 20 Oct 2022 20:12:20 +0900 Subject: [PATCH] feat(mpc): update steer rate calculation (#1566) * feat(mpc): update target steering rate calculation method Signed-off-by: Takamasa Horibe * update Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../include/trajectory_follower/mpc.hpp | 8 ++++++ control/trajectory_follower/src/mpc.cpp | 27 +++++++++++++++++-- 2 files changed, 33 insertions(+), 2 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index ef14a78db3464..2e8051d470faf 100644 --- 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 index 4d05f6ed19157..9940af690cdde 100644 --- 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(u_filtered); - ctrl_cmd.steering_tire_rotation_rate = - static_cast((u_filtered - current_steer.steering_tire_angle) / prediction_dt); + ctrl_cmd.steering_tire_rotation_rate = static_cast(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 (