diff --git a/control/mpc_lateral_controller/README.md b/control/mpc_lateral_controller/README.md index efd6480e214e1..83c0e7af3e114 100644 --- a/control/mpc_lateral_controller/README.md +++ b/control/mpc_lateral_controller/README.md @@ -34,12 +34,15 @@ For the optimization, a Quadratic Programming (QP) solver is used and two option - unconstraint_fast : use least square method to solve unconstraint QP with eigen. -- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) algorithm (for more details see the related papers at the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): +- [osqp](https://osqp.org/): run the [following ADMM](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) + algorithm (for more details see the related papers at + the [Citing OSQP](https://web.stanford.edu/~boyd/papers/admm_distr_stats.html) section): ### Filtering Filtering is required for good noise reduction. -A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as input of the MPC as well as for +A [Butterworth filter](https://en.wikipedia.org/wiki/Butterworth_filter) is used for the yaw and lateral errors used as +input of the MPC as well as for the output steering angle. Other filtering methods can be considered as long as the noise reduction performances are good enough. @@ -105,6 +108,7 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | keep_steer_control_until_converged | bool | keep steer control until steer is converged | true | | new_traj_duration_time | double | threshold value of the time to be considered as new trajectory | 1.0 | | new_traj_end_dist | double | threshold value of the distance between trajectory ends to be considered as new trajectory | 0.3 | +| mpc_converged_threshold_rps | double | threshold value to be sure output of the optimization is converged, it is used in stopped state | 0.3 | (\*1) To prevent unnecessary steering movement, the steering command is fixed to the previous value in the stop state. @@ -143,11 +147,13 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. ### How to tune MPC parameters 1. Set appropriate vehicle kinematics parameters for distance to front and rear axle and max steer angle. - Also check that the input `VehicleKinematicState` has appropriate values (speed: vehicle rear-wheels-center velocity [km/h], angle: steering (tire) angle [rad]). + Also check that the input `VehicleKinematicState` has appropriate values (speed: vehicle rear-wheels-center + velocity [km/h], angle: steering (tire) angle [rad]). These values give a vehicle information to the controller for path following. Errors in these values cause fundamental tracking error. -2. Set appropriate vehicle dynamics parameters of `steering_tau`, which is the approximated delay from steering angle command to actual steering angle. +2. Set appropriate vehicle dynamics parameters of `steering_tau`, which is the approximated delay from steering angle + command to actual steering angle. 3. Set `weight_steering_input` = 1.0, `weight_lat_error` = 0.1, and other weights to 0. If the vehicle oscillates when driving with low speed, set `weight_lat_error` smaller. @@ -156,7 +162,8 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. One of the simple way for tuning is to increase `weight_lat_error` until oscillation occurs. If the vehicle is unstable with very small `weight_lat_error`, increase terminal weight : `weight_terminal_lat_error` and `weight_terminal_heading_error` to improve tracking stability. - Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a trade-off between computational costs. + Larger `prediction_horizon` and smaller `prediction_sampling_time` is effective for tracking performance, but it is a + trade-off between computational costs. Other parameters can be adjusted like below. - `weight_lat_error`: Reduce lateral tracking error. This acts like P gain in PID. @@ -165,8 +172,10 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. - `weight_steering_input`: Reduce oscillation of tracking. - `weight_steering_input_squared_vel_coeff`: Reduce oscillation of tracking in high speed range. - `weight_lat_jerk`: Reduce lateral jerk. -- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for stability. -- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` for stability. +- `weight_terminal_lat_error`: Preferable to set a higher value than normal lateral weight `weight_lat_error` for + stability. +- `weight_terminal_heading_error`: Preferable to set a higher value than normal heading weight `weight_heading_error` + for stability. ## References / External links diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp index 17d29d1542cde..732a7d8acdc78 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_lateral_controller.hpp @@ -99,16 +99,32 @@ class MpcLateralController : public trajectory_follower::LateralControllerBase double m_stop_state_entry_ego_speed; double m_stop_state_entry_target_speed; double m_converged_steer_rad; - double m_new_traj_duration_time; // check trajectory shape change - double m_new_traj_end_dist; // check trajectory shape change + double m_mpc_converged_threshold_rps; // max mpc output change threshold for 1 sec + double m_new_traj_duration_time; // check trajectory shape change + double m_new_traj_end_dist; // check trajectory shape change bool m_keep_steer_control_until_converged; + /* parameter to store the actual steering rate limit */ + double m_steer_rate_lim; + // trajectory buffer for detecting new trajectory std::deque m_trajectory_buffer; // MPC object MPC m_mpc; + // Check is mpc output converged + bool m_is_mpc_history_filled{false}; + + //!< @brief store the last mpc outputs for 1 sec + std::vector> + m_mpc_steering_history{}; + //!< @brief set the mpc steering output to history + void setSteeringToHistory( + const autoware_auto_control_msgs::msg::AckermannLateralCommand & steering); + //!< @brief check if the mpc steering output is converged + bool isMpcConverged(); + //!< @brief measured kinematic state nav_msgs::msg::Odometry m_current_kinematic_state; //!< @brief measured steering diff --git a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml index 30f74be8c2326..be2993dc0f3f7 100644 --- a/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml +++ b/control/mpc_lateral_controller/param/lateral_controller_defaults.param.yaml @@ -62,6 +62,7 @@ keep_steer_control_until_converged: true new_traj_duration_time: 1.0 new_traj_end_dist: 0.3 + mpc_converged_threshold_rps: 0.01 # threshold of mpc convergence check [rad/s] # steer offset steering_offset: diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 6b89a3325ad05..6f9d0ec5567a0 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -69,6 +69,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} node_->declare_parameter("keep_steer_control_until_converged"); m_new_traj_duration_time = node_->declare_parameter("new_traj_duration_time"); // [s] m_new_traj_end_dist = node_->declare_parameter("new_traj_end_dist"); // [m] + m_mpc_converged_threshold_rps = + node_->declare_parameter("mpc_converged_threshold_rps"); // [rad/s] /* mpc parameters */ const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*node_).getVehicleInfo(); @@ -76,7 +78,7 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} const double steer_rate_lim_dps = node_->declare_parameter("steer_rate_lim_dps"); constexpr double deg2rad = static_cast(M_PI) / 180.0; m_mpc.m_steer_lim = vehicle_info.max_steer_angle_rad; - m_mpc.m_steer_rate_lim = steer_rate_lim_dps * deg2rad; + m_steer_rate_lim = steer_rate_lim_dps * deg2rad; /* vehicle model setup */ const std::string vehicle_model_type = @@ -202,6 +204,17 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_is_ctrl_cmd_prev_initialized = true; } + const bool is_vehicle_stopped = std::fabs(m_current_kinematic_state.twist.twist.linear.x) < 0.01; + + // if the vehicle is stopped, set steering angle rate limit to a large value to get a proper + // steering value from mpc. + // TODO(someone): solve mpc cannot create output in low steering rate limit problem + if (is_vehicle_stopped) { + m_mpc.m_steer_rate_lim = std::numeric_limits::max(); + } else { + m_mpc.m_steer_rate_lim = m_steer_rate_lim; + } + const bool is_mpc_solved = m_mpc.calculateMPC( m_current_steering, m_current_kinematic_state.twist.twist.linear.x, m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values); @@ -213,6 +226,8 @@ trajectory_follower::LateralOutput MpcLateralController::run( // the actual steer angle, and it may make the optimization result unstable. if (!is_mpc_solved) { m_mpc.resetPrevResult(m_current_steering); + } else { + setSteeringToHistory(ctrl_cmd); } if (enable_auto_steering_offset_removal_) { @@ -225,10 +240,17 @@ trajectory_follower::LateralOutput MpcLateralController::run( publishPredictedTraj(predicted_traj); publishDebugValues(debug_values); - const auto createLateralOutput = [this](const auto & cmd) { + const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) { trajectory_follower::LateralOutput output; output.control_cmd = createCtrlCmdMsg(cmd); - output.sync_data.is_steer_converged = isSteerConverged(cmd); + // To be sure current steering of the vehicle is desired steering angle, we need to check + // following conditions. + // 1. At the last loop, mpc should be solved because command should be optimized output. + // 2. The mpc should be converged. + // 3. The steer angle should be converged. + output.sync_data.is_steer_converged = + is_mpc_solved && isMpcConverged() && isSteerConverged(cmd); + return output; }; @@ -239,7 +261,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( } // Use previous command value as previous raw steer command m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle; - return createLateralOutput(m_ctrl_cmd_prev); + return createLateralOutput(m_ctrl_cmd_prev, false); } if (!is_mpc_solved) { @@ -250,7 +272,7 @@ trajectory_follower::LateralOutput MpcLateralController::run( } m_ctrl_cmd_prev = ctrl_cmd; - return createLateralOutput(ctrl_cmd); + return createLateralOutput(ctrl_cmd, is_mpc_solved); } bool MpcLateralController::isSteerConverged( @@ -410,6 +432,61 @@ void MpcLateralController::publishDebugValues( m_pub_steer_offset->publish(offset); } +void MpcLateralController::setSteeringToHistory( + const autoware_auto_control_msgs::msg::AckermannLateralCommand & steering) +{ + const auto time = node_->now(); + if (m_mpc_steering_history.empty()) { + m_mpc_steering_history.emplace_back(steering, time); + m_is_mpc_history_filled = false; + return; + } + + m_mpc_steering_history.emplace_back(steering, time); + + // Check the history is filled or not. + if (rclcpp::Duration(time - m_mpc_steering_history.begin()->second).seconds() >= 1.0) { + m_is_mpc_history_filled = true; + // remove old data that is older than 1 sec + for (auto itr = m_mpc_steering_history.begin(); itr != m_mpc_steering_history.end(); ++itr) { + if (rclcpp::Duration(time - itr->second).seconds() > 1.0) { + m_mpc_steering_history.erase(m_mpc_steering_history.begin()); + } else { + break; + } + } + } else { + m_is_mpc_history_filled = false; + } +} + +bool MpcLateralController::isMpcConverged() +{ + // If the number of variable below the 2, there is no enough data so MPC is not converged. + if (m_mpc_steering_history.size() < 2) { + return false; + } + + // If the history is not filled, return false. + + if (!m_is_mpc_history_filled) { + return false; + } + + // Find the maximum and minimum values of the steering angle in the past 1 second. + double min_steering_value = m_mpc_steering_history[0].first.steering_tire_angle; + double max_steering_value = m_mpc_steering_history[0].first.steering_tire_angle; + for (size_t i = 1; i < m_mpc_steering_history.size(); i++) { + if (m_mpc_steering_history[i].first.steering_tire_angle < min_steering_value) { + min_steering_value = m_mpc_steering_history[i].first.steering_tire_angle; + } + if (m_mpc_steering_history[i].first.steering_tire_angle > max_steering_value) { + max_steering_value = m_mpc_steering_history[i].first.steering_tire_angle; + } + } + return (max_steering_value - min_steering_value) < m_mpc_converged_threshold_rps; +} + void MpcLateralController::declareMPCparameters() { m_mpc.m_param.prediction_horizon = node_->declare_parameter("mpc_prediction_horizon");