diff --git a/control/pid_longitudinal_controller/README.md b/control/pid_longitudinal_controller/README.md index 9d25322c793e0..a110cb94d1d91 100644 --- a/control/pid_longitudinal_controller/README.md +++ b/control/pid_longitudinal_controller/README.md @@ -90,8 +90,13 @@ This PID logic has a maximum value for the output of each term. This is to preve - Large integral terms may cause unintended behavior by users. - Unintended noise may cause the output of the derivative term to be very large. -Also, the integral term is not accumulated when the vehicle is stopped. This is to prevent unintended accumulation of the integral term in cases such as Autoware assumes that the vehicle is engaged, but an external system has locked the vehicle to start. -On the other hand, if the vehicle gets stuck in a depression in the road surface when starting, the vehicle will not start forever, which is currently being addressed by developers. +Note: by default, the integral term in the control system is not accumulated when the vehicle is stationary. This precautionary measure aims to prevent unintended accumulation of the integral term in scenarios where Autoware assumes the vehicle is engaged, but an external system has immobilized the vehicle to initiate startup procedures. + +However, certain situations may arise, such as when the vehicle encounters a depression in the road surface during startup or if the slope compensation is inaccurately estimated (lower than necessary), leading to a failure to initiate motion. To address these scenarios, it is possible to activate error integration even when the vehicle is at rest by setting the `enable_integration_at_low_speed` parameter to true. + +When `enable_integration_at_low_speed` is set to true, the PID controller will initiate integration of the acceleration error after a specified duration defined by the `time_threshold_before_pid_integration` parameter has elapsed without the vehicle surpassing a minimum velocity set by the `current_vel_threshold_pid_integration` parameter. + +The presence of the `time_threshold_before_pid_integration` parameter is important for practical PID tuning. Integrating the error when the vehicle is stationary or at low speed can complicate PID tuning. This parameter effectively introduces a delay before the integral part becomes active, preventing it from kicking in immediately. This delay allows for more controlled and effective tuning of the PID controller. At present, PID control is implemented from the viewpoint of trade-off between development/maintenance cost and performance. This may be replaced by a higher performance controller (adaptive control or robust control) in future development. @@ -207,7 +212,9 @@ AutonomouStuff Lexus RX 450h for under 40 km/h driving. | max_d_effort | double | max value of acceleration with d gain | 0.0 | | min_d_effort | double | min value of acceleration with d gain | 0.0 | | lpf_vel_error_gain | double | gain of low-pass filter for velocity error | 0.9 | -| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | 0.5 | +| enable_integration_at_low_speed | bool | Whether to enable integration of acceleration errors when the vehicle speed is lower than `current_vel_threshold_pid_integration` or not. | +| current_vel_threshold_pid_integration | double | Velocity error is integrated for I-term only when the absolute value of current velocity is larger than this parameter. [m/s] | +| time_threshold_before_pid_integration | double | How much time without the vehicle moving must past to enable PID error integration. [s] | 5.0 | | brake_keeping_acc | double | If `enable_brake_keeping_before_stop` is true, a certain acceleration is kept during DRIVE state before the ego stops [m/s^2] See [Brake keeping](#brake-keeping). | 0.2 | ### STOPPING Parameter (smooth stop) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 921cd3995f6a9..b4ac9e1506eb7 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -42,6 +42,7 @@ #include #include +#include #include #include #include @@ -97,6 +98,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // vehicle info double m_wheel_base; + bool m_prev_vehicle_is_under_control{false}; + std::shared_ptr m_under_control_starting_time{nullptr}; // control state enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY }; @@ -145,7 +148,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // drive PIDController m_pid_vel; std::shared_ptr m_lpf_vel_error{nullptr}; + bool m_enable_integration_at_low_speed; double m_current_vel_threshold_pid_integrate; + double m_time_threshold_before_pid_integrate; bool m_enable_brake_keeping_before_stop; double m_brake_keeping_acc; @@ -384,6 +389,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro void updateDebugVelAcc( const Motion & ctrl_cmd, const geometry_msgs::msg::Pose & current_pose, const ControlData & control_data); + + double getTimeUnderControl(); }; } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml index eb2ef443c4576..f1ddea7ebad88 100644 --- a/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml +++ b/control/pid_longitudinal_controller/param/longitudinal_controller_defaults.param.yaml @@ -32,7 +32,9 @@ max_d_effort: 0.0 min_d_effort: 0.0 lpf_vel_error_gain: 0.9 + enable_integration_at_low_speed: false current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 enable_brake_keeping_before_stop: false brake_keeping_acc: -0.2 diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index d13e628f2e1d4..12f0eece1e477 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -100,9 +100,14 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) const double lpf_vel_error_gain{node.declare_parameter("lpf_vel_error_gain")}; m_lpf_vel_error = std::make_shared(0.0, lpf_vel_error_gain); + m_enable_integration_at_low_speed = + node.declare_parameter("enable_integration_at_low_speed"); m_current_vel_threshold_pid_integrate = node.declare_parameter("current_vel_threshold_pid_integration"); // [m/s] + m_time_threshold_before_pid_integrate = + node.declare_parameter("time_threshold_before_pid_integration"); // [s] + m_enable_brake_keeping_before_stop = node.declare_parameter("enable_brake_keeping_before_stop"); // [-] m_brake_keeping_acc = node.declare_parameter("brake_keeping_acc"); // [m/s^2] @@ -284,6 +289,7 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d); update_param("current_vel_threshold_pid_integration", m_current_vel_threshold_pid_integrate); + update_param("time_threshold_before_pid_integration", m_time_threshold_before_pid_integrate); } // stopping state @@ -556,6 +562,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + if (is_under_control != m_prev_vehicle_is_under_control) { + m_prev_vehicle_is_under_control = is_under_control; + m_under_control_starting_time = + is_under_control ? std::make_shared(clock_->now()) : nullptr; + } // transit state // in DRIVE state if (m_control_state == ControlState::DRIVE) { @@ -959,8 +970,15 @@ double PidLongitudinalController::applyVelocityFeedback( const double diff_vel = (target_motion.vel - current_vel) * vel_sign; const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled && m_current_operation_mode.mode == OperationModeState::AUTONOMOUS; + + const bool vehicle_is_moving = std::abs(current_vel) > m_current_vel_threshold_pid_integrate; + const double time_under_control = getTimeUnderControl(); + const bool vehicle_is_stuck = + !vehicle_is_moving && time_under_control > m_time_threshold_before_pid_integrate; + const bool enable_integration = - (std::abs(current_vel) > m_current_vel_threshold_pid_integrate) && is_under_control; + (vehicle_is_moving || (m_enable_integration_at_low_speed && vehicle_is_stuck)) && + is_under_control; const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); @@ -1059,4 +1077,10 @@ void PidLongitudinalController::checkControlState( stat.summary(level, msg); } +double PidLongitudinalController::getTimeUnderControl() +{ + if (!m_under_control_starting_time) return 0.0; + return (clock_->now() - *m_under_control_starting_time).seconds(); +} + } // namespace autoware::motion::control::pid_longitudinal_controller diff --git a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml index bc3213081d86e..c39088753fe64 100644 --- a/control/trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -32,7 +32,9 @@ max_d_effort: 0.0 min_d_effort: 0.0 lpf_vel_error_gain: 0.9 + enable_integration_at_low_speed: false current_vel_threshold_pid_integration: 0.5 + time_threshold_before_pid_integration: 2.0 enable_brake_keeping_before_stop: false brake_keeping_acc: -0.2