From 99e3e09e9b71114645f2ca389c89e4c539a5e668 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 5 Dec 2023 15:50:16 +0300 Subject: [PATCH 1/2] feat(pid_longitudinal_controller): improve FF gain constant calculation Signed-off-by: Berkay Karaman --- .../src/pid_longitudinal_controller.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index aab2ecf8f081e..bfeeee95bd81c 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -1053,6 +1053,8 @@ PidLongitudinalController::StateAfterDelay PidLongitudinalController::predictedS double PidLongitudinalController::applyVelocityFeedback(const ControlData & control_data) { // NOTE: Acceleration command is always positive even if the ego drives backward. + const double nearest_target_vel = + control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps; const double vel_sign = (control_data.shift == Shift::Forward) ? 1.0 : (control_data.shift == Shift::Reverse ? -1.0 : 0.0); @@ -1087,7 +1089,9 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont constexpr double ff_scale_max = 2.0; // for safety constexpr double ff_scale_min = 0.5; // for safety const double ff_scale = std::clamp( - std::abs(current_vel) / std::max(std::abs(target_motion.vel), 0.1), ff_scale_min, ff_scale_max); + 1.0 + (abs(nearest_target_vel - current_vel) / std::max(abs(current_vel), 0.1)), ff_scale_min, + ff_scale_max); + const double ff_acc = control_data.interpolated_traj.points.at(control_data.target_idx).acceleration_mps2 * ff_scale; From 4bf0b18798120d026791e759a46305f059f52133 Mon Sep 17 00:00:00 2001 From: Berkay Karaman Date: Tue, 26 Sep 2023 20:16:38 +0300 Subject: [PATCH 2/2] fix(pid_longitudinal_controller): change pid structure with respect to design Signed-off-by: Berkay Karaman --- .../include/pid_longitudinal_controller/pid.hpp | 3 ++- control/pid_longitudinal_controller/src/pid.cpp | 6 +++--- .../src/pid_longitudinal_controller.cpp | 4 ++-- .../pid_longitudinal_controller/test/test_pid.cpp | 12 ++++++------ 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp index 8b981c0485ed9..2e989321e0c70 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp +++ b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid.hpp @@ -29,6 +29,7 @@ class PIDController /** * @brief calculate the output of this PID * @param [in] error previous error + * @param [in] error_filtered filtered error * @param [in] dt time step [s] * @param [in] is_integrated if true, will use the integral component for calculation * @param [out] pid_contributions values of the proportional, integral, and derivative components @@ -36,7 +37,7 @@ class PIDController * @throw std::runtime_error if gains or limits have not been set */ double calculate( - const double error, const double dt, const bool is_integrated, + const double error, const double error_filtered, const double dt, const bool is_integrated, std::vector & pid_contributions); /** * @brief set the coefficients for the P (proportional) I (integral) D (derivative) terms diff --git a/control/pid_longitudinal_controller/src/pid.cpp b/control/pid_longitudinal_controller/src/pid.cpp index 530b5cd15e754..4ca8ebbd35ef2 100644 --- a/control/pid_longitudinal_controller/src/pid.cpp +++ b/control/pid_longitudinal_controller/src/pid.cpp @@ -32,7 +32,7 @@ PIDController::PIDController() } double PIDController::calculate( - const double error, const double dt, const bool enable_integration, + const double error, const double filtered_error, const double dt, const bool enable_integration, std::vector & pid_contributions) { if (!m_is_gains_set || !m_is_limits_set) { @@ -55,12 +55,12 @@ double PIDController::calculate( error_differential = 0; m_is_first_time = false; } else { - error_differential = (error - m_prev_error) / dt; + error_differential = (filtered_error - m_prev_error) / dt; } double ret_d = p.kd * error_differential; ret_d = std::min(std::max(ret_d, p.min_ret_d), p.max_ret_d); - m_prev_error = error; + m_prev_error = filtered_error; pid_contributions.resize(3); pid_contributions.at(0) = ret_p; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index bfeeee95bd81c..edad56b460093 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -1078,8 +1078,8 @@ double PidLongitudinalController::applyVelocityFeedback(const ControlData & cont const double error_vel_filtered = m_lpf_vel_error->filter(diff_vel); std::vector pid_contributions(3); - const double pid_acc = - m_pid_vel.calculate(error_vel_filtered, control_data.dt, enable_integration, pid_contributions); + const double pid_acc = m_pid_vel.calculate( + diff_vel, error_vel_filtered, control_data.dt, enable_integration, pid_contributions); // Feedforward scaling: // This is for the coordinate conversion where feedforward is applied, from Time to Arclength. diff --git a/control/pid_longitudinal_controller/test/test_pid.cpp b/control/pid_longitudinal_controller/test/test_pid.cpp index 82d01e0028a9c..f0ceccd8499a3 100644 --- a/control/pid_longitudinal_controller/test/test_pid.cpp +++ b/control/pid_longitudinal_controller/test/test_pid.cpp @@ -29,14 +29,14 @@ TEST(TestPID, calculate_pid_output) PIDController pid; // Cannot calculate before initializing gains and limits - EXPECT_THROW(pid.calculate(0.0, dt, enable_integration, contributions), std::runtime_error); + EXPECT_THROW(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), std::runtime_error); pid.setGains(1.0, 1.0, 1.0); pid.setLimits(10.0, 0.0, 10.0, 0.0, 10.0, 0.0, 10.0, 0.0); double error = target - current; double prev_error = error; while (current != target) { - current = pid.calculate(error, dt, enable_integration, contributions); + current = pid.calculate(error, error, dt, enable_integration, contributions); EXPECT_EQ(contributions[0], error); EXPECT_EQ(contributions[1], 0.0); // integration is deactivated EXPECT_EQ(contributions[2], error - prev_error); @@ -50,18 +50,18 @@ TEST(TestPID, calculate_pid_output) enable_integration = true; // High errors to force each component to its upper limit - EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0); + EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0); for (double error = 100.0; error < 1000.0; error += 100.0) { - EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), 10.0); + EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), 10.0); EXPECT_EQ(contributions[0], 10.0); EXPECT_EQ(contributions[1], 10.0); // integration is activated EXPECT_EQ(contributions[2], 10.0); } // Low errors to force each component to its lower limit - EXPECT_EQ(pid.calculate(0.0, dt, enable_integration, contributions), 0.0); + EXPECT_EQ(pid.calculate(0.0, 0.0, dt, enable_integration, contributions), 0.0); for (double error = -100.0; error > -1000.0; error -= 100.0) { - EXPECT_EQ(pid.calculate(error, dt, enable_integration, contributions), -10.0); + EXPECT_EQ(pid.calculate(error, error, dt, enable_integration, contributions), -10.0); EXPECT_EQ(contributions[0], -10.0); EXPECT_EQ(contributions[1], -10.0); // integration is activated EXPECT_EQ(contributions[2], -10.0);