From 217f3955ed6f9680f451021c477b3f66a8db145d Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 6 Jul 2022 16:17:33 +0900 Subject: [PATCH] refactor Signed-off-by: kosuke55 --- .../include/trajectory_follower/mpc.hpp | 24 ++-- control/trajectory_follower/src/mpc.cpp | 105 +++++++++--------- 2 files changed, 68 insertions(+), 61 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index 399f4dbde9c1b..4769d62aa016d 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/control/trajectory_follower/include/trajectory_follower/mpc.hpp @@ -227,23 +227,24 @@ class TRAJECTORY_FOLLOWER_PUBLIC MPC * @param [in] reference_trajectory used for linearization around reference trajectory */ MPCMatrix generateMPCMatrix( - const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t max_dt); + 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 float64_t max_dt); + 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, - trajectory_follower::MPCTrajectory * output, float64_t & max_dt, - const geometry_msgs::msg::Pose & current_pose) const; + 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 */ @@ -251,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 max_dt) 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 float64_t max_dt) const; + void addSteerWeightR(Eigen::MatrixXd * R, const float64_t predition_dt) const; /** * @brief add weights related to lateral_jerk, steering_rate, steering_acc into f */ - void addSteerWeightF(Eigen::MatrixXd * f, const float64_t max_dt) const; + void addSteerWeightF(Eigen::MatrixXd * f, const float64_t predition_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 2f6c6ee8387ad..3fdf10faacc8c 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/control/trajectory_follower/src/mpc.cpp @@ -67,19 +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; - float64_t max_dt; + const float64_t prediction_dt = + getPredictionDeletaTime(mpc_start_time, reference_trajectory, current_pose); if (!resampleMPCTrajectoryByTime( - mpc_start_time, reference_trajectory, &mpc_resampled_ref_traj, max_dt, current_pose)) { + 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, max_dt); + MPCMatrix mpc_matrix = generateMPCMatrix(mpc_resampled_ref_traj, prediction_dt); /* solve quadratic optimization */ Eigen::VectorXd Uex; - if (!executeOptimization(mpc_matrix, x0, &Uex, max_dt)) { + if (!executeOptimization(mpc_matrix, x0, prediction_dt, &Uex)) { RCLCPP_WARN_THROTTLE(m_logger, *m_clock, 1000 /*ms*/, "optimization failed."); return false; } @@ -90,9 +91,8 @@ bool8_t MPC::calculateMPC( /* set control command */ { - const auto & dt = max_dt; ctrl_cmd.steering_tire_angle = static_cast(u_filtered); - ctrl_cmd.steering_tire_rotation_rate = static_cast((Uex(1) - Uex(0)) / dt); + ctrl_cmd.steering_tire_rotation_rate = static_cast((Uex(1) - Uex(0)) / prediction_dt); } storeSteerCmd(u_filtered); @@ -396,40 +396,13 @@ void MPC::storeSteerCmd(const float64_t steer) } bool8_t MPC::resampleMPCTrajectoryByTime( - float64_t ts, const trajectory_follower::MPCTrajectory & input, - trajectory_follower::MPCTrajectory * output, float64_t & max_dt, - const geometry_msgs::msg::Pose & current_pose) const + const float64_t ts, const float64_t prediction_dt, + const trajectory_follower::MPCTrajectory & input, + trajectory_follower::MPCTrajectory * output) const { - size_t nearest_idx = - static_cast(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; - }(); - - const float64_t dt = - (target_time - ts) / (static_cast(m_param.prediction_horizon) - 1.0); - max_dt = std::max(dt, m_param.prediction_dt); - std::vector mpc_time_v; for (float64_t i = 0; i < static_cast(m_param.prediction_horizon); ++i) { - mpc_time_v.push_back(ts + i * max_dt); + mpc_time_v.push_back(ts + i * prediction_dt); } if (!trajectory_follower::MPCUtils::linearInterpMPCTrajectory( input.relative_time, input, mpc_time_v, output)) { @@ -540,12 +513,12 @@ trajectory_follower::MPCTrajectory MPC::applyVelocityDynamicsFilter( * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...]) */ MPCMatrix MPC::generateMPCMatrix( - const trajectory_follower::MPCTrajectory & reference_trajectory, const float64_t max_dt) + 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 = max_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(); @@ -646,7 +619,7 @@ MPCMatrix MPC::generateMPCMatrix( m.R2ex.block(i, i, 2, 2) += J; } - addSteerWeightR(&m.R1ex, max_dt); + addSteerWeightR(&m.R1ex, prediction_dt); return m; } @@ -674,7 +647,8 @@ MPCMatrix MPC::generateMPCMatrix( * [ -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 float64_t max_dt) + const MPCMatrix & m, const Eigen::VectorXd & x0, const float64_t prediction_dt, + Eigen::VectorXd * Uex) { using Eigen::MatrixXd; using Eigen::VectorXd; @@ -697,7 +671,7 @@ bool8_t MPC::executeOptimization( H.triangularView() += m.R1ex + m.R2ex; H.triangularView() = H.transpose(); MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex; - addSteerWeightF(&f, max_dt); + addSteerWeightF(&f, prediction_dt); MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N); for (int64_t i = 1; i < DIM_U_N; i++) { @@ -706,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 * max_dt); - VectorXd ubA = VectorXd::Constant(DIM_U_N, m_steer_rate_lim * max_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; @@ -732,10 +706,10 @@ bool8_t MPC::executeOptimization( return true; } -void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr, const float64_t max_dt) const +void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr, const float64_t prediction_dt) const { const int64_t N = m_param.prediction_horizon; - const float64_t DT = max_dt; + const float64_t DT = prediction_dt; auto & R = *R_ptr; @@ -777,13 +751,13 @@ void MPC::addSteerWeightR(Eigen::MatrixXd * R_ptr, const float64_t max_dt) const } } -void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr, const float64_t max_dt) const +void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr, const float64_t prediction_dt) const { if (f_ptr->rows() < 2) { return; } - const float64_t DT = max_dt; + const float64_t DT = prediction_dt; auto & f = *f_ptr; // steer rate for i = 0 @@ -803,10 +777,39 @@ void MPC::addSteerWeightF(Eigen::MatrixXd * f_ptr, const float64_t max_dt) const f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5; } -[[maybe_unused]] float64_t MPC::getPredictionTime(const float64_t max_dt) 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(m_param.prediction_horizon - 1) * max_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(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(m_param.prediction_horizon) - 1.0); + + return std::max(dt, m_param.prediction_dt); } bool8_t MPC::isValid(const MPCMatrix & m) const