From 6c18f720258c882f0a8997b74faa03dc83b3eb50 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 18 Aug 2023 18:18:13 +0900 Subject: [PATCH 1/5] feat(mpc): 1d interpolate in steering rate limit calculation Signed-off-by: Takamasa Horibe --- .../include/mpc_lateral_controller/mpc.hpp | 6 ++ control/mpc_lateral_controller/src/mpc.cpp | 97 +++++++++++-------- 2 files changed, 65 insertions(+), 38 deletions(-) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 0d144726a0866..8702714b82988 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -379,6 +379,12 @@ class MPC const MPCMatrix & mpc_matrix, const AckermannLateralCommand & ctrl_cmd, const VectorXd & Uex, const Odometry & current_kinematics) const; + /** + * + */ + VectorXd calcSteerDiffLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const; + //!< @brief logging with warn and return false template inline bool fail_warn_throttle(Args &&... args) const diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 8e99880b68050..3f9ad83be0a23 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -573,47 +573,16 @@ std::pair MPC::executeOptimization( A(i, i - 1) = -1.0; } - const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; - const auto get_adaptive_steer_rate_lim = [&](const double curvature, const double velocity) { - if (is_vehicle_stopped) { - return std::numeric_limits::max(); - } - - double steer_rate_lim_by_curvature = m_steer_rate_lim_map_by_curvature.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_curvature) { - if (std::abs(curvature) <= steer_rate_lim_info.first) { - steer_rate_lim_by_curvature = steer_rate_lim_info.second; - break; - } - } - - double steer_rate_lim_by_velocity = m_steer_rate_lim_map_by_velocity.back().second; - for (const auto & steer_rate_lim_info : m_steer_rate_lim_map_by_velocity) { - if (std::abs(velocity) <= steer_rate_lim_info.first) { - steer_rate_lim_by_velocity = steer_rate_lim_info.second; - break; - } - } - - return std::min(steer_rate_lim_by_curvature, steer_rate_lim_by_velocity); - }; - + // steering angle limit 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(DIM_U_N); - VectorXd ubA(DIM_U_N); - for (int i = 0; i < DIM_U_N; ++i) { - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(i), traj.vx.at(i)); - const double adaptive_delta_steer_lim = adaptive_steer_rate_lim * prediction_dt; - lbA(i) = -adaptive_delta_steer_lim; - ubA(i) = adaptive_delta_steer_lim; - } - const double adaptive_steer_rate_lim = - get_adaptive_steer_rate_lim(traj.smooth_k.at(0), traj.vx.at(0)); - lbA(0, 0) = m_raw_steer_cmd_prev - adaptive_steer_rate_lim * m_ctrl_period; - ubA(0, 0) = m_raw_steer_cmd_prev + adaptive_steer_rate_lim * m_ctrl_period; + // steering angle rate limit + VectorXd steer_rate_limits = calcSteerDiffLimitOnTrajectory(traj, current_velocity); + VectorXd lbA = steer_rate_limits * prediction_dt; + VectorXd ubA = -steer_rate_limits * prediction_dt; + lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period; + ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; auto t_start = std::chrono::system_clock::now(); bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex); @@ -761,6 +730,58 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } +VectorXd MPC::calcSteerDiffLimitOnTrajectory( + const MPCTrajectory & trajectory, const double current_velocity) const +{ + const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { + std::vector reference, limits; + for (const auto & p : steer_rate_limit_map) { + reference.push_back(p.first); + limits.push_back(p.second); + } + + // If the speed is out of range of the reference, apply zero-order hold. + if (current <= reference.front()) { + return limits.front(); + } + if (current >= reference.back()) { + return limits.back(); + } + + // Apply linear interpolation + for (size_t i = 0; i < reference.size() - 1; ++i) { + if (reference.at(i) <= current && current <= reference.at(i + 1)) { + auto ratio = + (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5); + ratio = std::clamp(0.0, 1.0, ratio); + const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i)); + return interp; + } + } + + std::cerr << "MPC::calcSteerRateLimitOnTrajectory() interpolation logic is broken. Command " + "filter is not working. Please check the code." + << std::endl; + return reference.back(); + }; + + // when the vehicle is stopped, no steering rate limit. + const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01; + if (is_vehicle_stopped) { + return VectorXd::Zero(m_param.prediction_horizon); + } + + // calculate steering rate limit + VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon); + for (int i = 0; i < m_param.prediction_horizon; ++i) { + const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i)); + const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i)); + steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity); + } + + return steer_rate_limits; +} + bool MPC::isValid(const MPCMatrix & m) const { if ( From ef2ebb9255cb89eb489ff56fc72327f5c7f06a5a Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 18 Aug 2023 18:49:14 +0900 Subject: [PATCH 2/5] update Signed-off-by: Takamasa Horibe --- .../include/mpc_lateral_controller/mpc.hpp | 6 ++++-- control/mpc_lateral_controller/src/mpc.cpp | 12 +++++++----- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 8702714b82988..0854340ed24a5 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -380,9 +380,11 @@ class MPC const Odometry & current_kinematics) const; /** - * + * @brief calculate steering rate limit along with the target trajectory + * @param reference_trajectory The reference trajectory. + * @param current_velocity current velocity of ego. */ - VectorXd calcSteerDiffLimitOnTrajectory( + VectorXd calcSteerRateLimitOnTrajectory( const MPCTrajectory & trajectory, const double current_velocity) const; //!< @brief logging with warn and return false diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 3f9ad83be0a23..23870f6c17242 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -20,6 +20,8 @@ #include #include +// #define PRINT_MAT(m) std::cerr << #m << ": \n" << m << std::endl + namespace autoware::motion::control::mpc_lateral_controller { using tier4_autoware_utils::calcDistance2d; @@ -578,11 +580,11 @@ std::pair MPC::executeOptimization( VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle // steering angle rate limit - VectorXd steer_rate_limits = calcSteerDiffLimitOnTrajectory(traj, current_velocity); - VectorXd lbA = steer_rate_limits * prediction_dt; - VectorXd ubA = -steer_rate_limits * prediction_dt; - lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period; + VectorXd steer_rate_limits = calcSteerRatefLimitOnTrajectory(traj, current_velocity); + VectorXd ubA = steer_rate_limits * prediction_dt; + VectorXd lbA = -steer_rate_limits * prediction_dt; ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; + lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period; auto t_start = std::chrono::system_clock::now(); bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex); @@ -730,7 +732,7 @@ double MPC::calcDesiredSteeringRate( return steer_rate; } -VectorXd MPC::calcSteerDiffLimitOnTrajectory( +VectorXd MPC::calcSteerRateLimitOnTrajectory( const MPCTrajectory & trajectory, const double current_velocity) const { const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) { From d491fa6c9d8789a0620ea5fbc1205b8aa16b7b87 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 18 Aug 2023 18:50:05 +0900 Subject: [PATCH 3/5] fix Signed-off-by: Takamasa Horibe --- control/mpc_lateral_controller/src/mpc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 23870f6c17242..32696ffe8b938 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -580,7 +580,7 @@ std::pair MPC::executeOptimization( VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim); // max steering angle // steering angle rate limit - VectorXd steer_rate_limits = calcSteerRatefLimitOnTrajectory(traj, current_velocity); + VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity); VectorXd ubA = steer_rate_limits * prediction_dt; VectorXd lbA = -steer_rate_limits * prediction_dt; ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period; From e0db98651c0d8c6a66d50654e2f1965cdca227d6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 20 Aug 2023 13:34:15 +0900 Subject: [PATCH 4/5] fix clamp Signed-off-by: Takamasa Horibe --- control/mpc_lateral_controller/src/mpc.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 32696ffe8b938..31aed8ba1953f 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -755,7 +755,7 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory( if (reference.at(i) <= current && current <= reference.at(i + 1)) { auto ratio = (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5); - ratio = std::clamp(0.0, 1.0, ratio); + ratio = std::clamp(ratio, 0.0, 1.0); const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i)); return interp; } From e1ae0b578206c318a29efd943e706cffd2dc6ca6 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sun, 20 Aug 2023 13:35:03 +0900 Subject: [PATCH 5/5] remove unused funcs Signed-off-by: Takamasa Horibe --- control/mpc_lateral_controller/src/mpc.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index 31aed8ba1953f..643f8a6f91023 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -20,8 +20,6 @@ #include #include -// #define PRINT_MAT(m) std::cerr << #m << ": \n" << m << std::endl - namespace autoware::motion::control::mpc_lateral_controller { using tier4_autoware_utils::calcDistance2d; @@ -361,9 +359,6 @@ std::pair MPC::updateStateForDelayCompensation( MatrixXd x_curr = x0_orig; double mpc_curr_time = start_time; - // for (const auto & tt : traj.relative_time) { - // std::cerr << "traj.relative_time = " << tt << std::endl; - // } for (size_t i = 0; i < m_input_buffer.size(); ++i) { double k, v = 0.0; try {