diff --git a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml index f683d8b4f0736..6d9b93ab98870 100644 --- a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml +++ b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml @@ -61,6 +61,10 @@ emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 + # acceleration feedback + lpf_acc_error_gain: 0.98 + acc_feedback_gain: 0.0 + # acceleration limit max_acc: 3.0 min_acc: -5.0 diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp index e8834665de3ec..a0bceda625b21 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/debug_values.hpp @@ -58,6 +58,10 @@ class DebugValues STOP_DIST = 28, FF_SCALE = 29, ACC_CMD_FF = 30, + ERROR_ACC = 31, + ERROR_ACC_FILTERED = 32, + ACC_CMD_ACC_FB_APPLIED = 33, + SIZE // this is the number of enum elements }; diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp index 081e78e2de214..1ec43282ca4d3 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/lowpass_filter.hpp @@ -61,6 +61,8 @@ class LowpassFilter1d m_x = ret; return ret; } + + void setGain(const double g) { m_gain = g; } }; } // namespace autoware::motion::control::pid_longitudinal_controller #endif // AUTOWARE__PID_LONGITUDINAL_CONTROLLER__LOWPASS_FILTER_HPP_ diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 57999372dceed..1d4192d51d98d 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -196,6 +196,10 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro }; EmergencyStateParams m_emergency_state_params; + // acc feedback + double m_acc_feedback_gain; + std::shared_ptr m_lpf_acc_error{nullptr}; + // acceleration limit double m_max_acc; double m_min_acc; diff --git a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json index fbbd2dfc7a59f..ef1272582e52b 100644 --- a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json +++ b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json @@ -251,6 +251,16 @@ "description": "target jerk in an EMERGENCY state [m/s^3]", "default": "-3.0" }, + "lpf_acc_error_gain": { + "type": "number", + "description": "gain of low-pass filter for acceleration", + "default": "0.98" + }, + "acc_feedback_gain": { + "type": "number", + "description": "acceleration feedback gain", + "default": "0.0" + }, "max_acc": { "type": "number", "description": "max value of output acceleration [m/s^2]", diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 9190c065eca3c..96e2796a91d22 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -164,6 +164,13 @@ PidLongitudinalController::PidLongitudinalController( p.jerk = node.declare_parameter("emergency_jerk"); // [m/s^3] } + // parameters for acc feedback + { + const double lpf_acc_error_gain{node.declare_parameter("lpf_acc_error_gain")}; + m_lpf_acc_error = std::make_shared(0.0, lpf_acc_error_gain); + m_acc_feedback_gain = node.declare_parameter("acc_feedback_gain"); + } + // parameters for acceleration limit m_max_acc = node.declare_parameter("max_acc"); // [m/s^2] m_min_acc = node.declare_parameter("min_acc"); // [m/s^2] @@ -291,6 +298,10 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac update_param("kd", kd); m_pid_vel.setGains(kp, ki, kd); + double lpf_vel_error_gain{node_parameters_->get_parameter("lpf_vel_error_gain").as_double()}; + update_param("lpf_vel_error_gain", lpf_vel_error_gain); + m_lpf_vel_error->setGain(lpf_vel_error_gain); + double max_pid{node_parameters_->get_parameter("max_out").as_double()}; double min_pid{node_parameters_->get_parameter("min_out").as_double()}; double max_p{node_parameters_->get_parameter("max_p_effort").as_double()}; @@ -365,6 +376,12 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac update_param("emergency_jerk", p.jerk); } + // acceleration feedback + update_param("acc_feedback_gain", m_acc_feedback_gain); + double lpf_acc_error_gain{node_parameters_->get_parameter("lpf_acc_error_gain").as_double()}; + update_param("lpf_acc_error_gain", lpf_acc_error_gain); + m_lpf_acc_error->setGain(lpf_acc_error_gain); + // acceleration limit update_param("min_acc", m_min_acc); @@ -753,6 +770,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d m_pid_vel.reset(); m_lpf_vel_error->reset(0.0); + m_lpf_acc_error->reset(0.0); return changeState(ControlState::DRIVE); } @@ -844,8 +862,22 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( // store acceleration without slope compensation m_prev_raw_ctrl_cmd = raw_ctrl_cmd; + // calc acc feedback + const double acc_err = control_data.current_motion.acc - raw_ctrl_cmd.acc; + m_debug_values.setValues(DebugValues::TYPE::ERROR_ACC, acc_err); + m_lpf_acc_error->filter(acc_err); + m_debug_values.setValues(DebugValues::TYPE::ERROR_ACC_FILTERED, m_lpf_acc_error->getValue()); + + const double acc_cmd = raw_ctrl_cmd.acc - m_lpf_acc_error->getValue() * m_acc_feedback_gain; + m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_ACC_FB_APPLIED, acc_cmd); + RCLCPP_DEBUG( + logger_, + "[acc feedback]: raw_ctrl_cmd.acc: %1.3f, control_data.current_motion.acc: %1.3f, acc_cmd: " + "%1.3f", + raw_ctrl_cmd.acc, control_data.current_motion.acc, acc_cmd); + ctrl_cmd_as_pedal_pos.acc = - applySlopeCompensation(raw_ctrl_cmd.acc, control_data.slope_angle, control_data.shift); + applySlopeCompensation(acc_cmd, control_data.slope_angle, control_data.shift); m_debug_values.setValues(DebugValues::TYPE::ACC_CMD_SLOPE_APPLIED, ctrl_cmd_as_pedal_pos.acc); ctrl_cmd_as_pedal_pos.vel = raw_ctrl_cmd.vel; } diff --git a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml index ab127c2d47b97..e458edaf6c471 100644 --- a/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml +++ b/control/autoware_trajectory_follower_node/config/plot_juggler_trajectory_follower.xml @@ -1,130 +1,130 @@ - - + + - - + + - - + + - + - + - + - + - + - + - + - - + + - + - + - - + + - + - + - + - - + + - + - + - + - + - - + + - + - + - - + + @@ -133,100 +133,114 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + - - + + - + - + - + - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -234,17 +248,17 @@ - + - + - + @@ -254,13 +268,13 @@ - + - + @@ -272,11 +286,10 @@ - + - diff --git a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml index 5027c94afe7c1..68fb172b19d2a 100644 --- a/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml +++ b/control/autoware_trajectory_follower_node/param/longitudinal/pid.param.yaml @@ -60,6 +60,10 @@ emergency_acc: -5.0 # denotes acceleration emergency_jerk: -3.0 + # acceleration feedback + lpf_acc_error_gain: 0.98 + acc_feedback_gain: 0.0 + # acceleration limit max_acc: 3.0 min_acc: -5.0