From a585ac2e7e3e551cd70a4e93d1c6c751e11e710e Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Fri, 8 Jul 2022 00:42:37 +0900 Subject: [PATCH] feat(trajectory_follower): add min_prediction_length to mpc (#1171) * feat(trajectory_follower): Add min_prediction_length to mpc Signed-off-by: kosuke55 * refactor Signed-off-by: kosuke55 --- .../include/trajectory_follower/mpc.hpp | 25 ++++-- .../trajectory_follower/mpc_trajectory.hpp | 15 ++++ control/trajectory_follower/src/mpc.cpp | 84 +++++++++++++------ .../src/mpc_lateral_controller.cpp | 3 + control/trajectory_follower/test/test_mpc.cpp | 1 + .../lateral_controller_defaults.param.yaml | 1 + .../lateral_controller.param.yaml | 1 + 7 files changed, 99 insertions(+), 31 deletions(-) diff --git a/control/trajectory_follower/include/trajectory_follower/mpc.hpp b/control/trajectory_follower/include/trajectory_follower/mpc.hpp index 4d8ee20db8c1b..35fc2ea37a8aa 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc.hpp +++ b/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 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 */ diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp index e1d37a9b3d692..e9bd992baa7ea 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_trajectory.hpp +++ b/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 #include @@ -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 toPoints() const + { + std::vector 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 diff --git a/control/trajectory_follower/src/mpc.cpp b/control/trajectory_follower/src/mpc.cpp index f71bd4e7b65ca..72457fab526b5 100644 --- a/control/trajectory_follower/src/mpc.cpp +++ b/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(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); @@ -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(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 mpc_time_v; for (float64_t i = 0; i < static_cast(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(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() += 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); + 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(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(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); + + return std::max(dt, m_param.prediction_dt); } bool8_t MPC::isValid(const MPCMatrix & m) const diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index ea59f46cee43e..77a369c746602 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -453,6 +453,8 @@ void MpcLateralController::declareMPCparameters() m_mpc.m_param.acceleration_limit = node_->declare_parameter("mpc_acceleration_limit"); m_mpc.m_param.velocity_time_constant = node_->declare_parameter("mpc_velocity_time_constant"); + m_mpc.m_param.min_prediction_length = + node_->declare_parameter("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); diff --git a/control/trajectory_follower/test/test_mpc.cpp b/control/trajectory_follower/test/test_mpc.cpp index a4ab0b8e51396..13ebb7ba68f4a 100644 --- a/control/trajectory_follower/test/test_mpc.cpp +++ b/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; diff --git a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml b/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml index f08e39cab9829..7dbbd5f039311 100644 --- a/control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml +++ b/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 diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml index 79b131806a4d6..b4674b64b2507 100644 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ b/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