From bc45970b685c3cfdb33672897a3eec090b28ee71 Mon Sep 17 00:00:00 2001 From: Carlos Rosales Date: Fri, 23 Jan 2015 17:07:31 +0100 Subject: [PATCH] Improvement in integral contribution implementation. Resolve #32. --- include/control_toolbox/pid.h | 4 +- src/pid.cpp | 74 ++++++++++++++++++++++------------- test/pid_tests.cpp | 19 ++++----- 3 files changed, 56 insertions(+), 41 deletions(-) diff --git a/include/control_toolbox/pid.h b/include/control_toolbox/pid.h index 30cadf8f..4a2cef89 100644 --- a/include/control_toolbox/pid.h +++ b/include/control_toolbox/pid.h @@ -370,8 +370,8 @@ class Pid double p_error_last_; /**< _Save position state for derivative state calculation. */ double p_error_; /**< Position error. */ - double d_error_; /**< Derivative error. */ - double i_term_; /**< Integral term. */ + double i_error_; /**< Integral of position error. */ + double d_error_; /**< Derivative of position error. */ double cmd_; /**< Command to send. */ // Dynamics reconfigure diff --git a/src/pid.cpp b/src/pid.cpp index 98255f8e..3d6d9bdc 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -175,8 +175,8 @@ void Pid::reset() { p_error_last_ = 0.0; p_error_ = 0.0; + i_error_ = 0.0; d_error_ = 0.0; - i_term_ = 0.0; cmd_ = 0.0; } @@ -269,7 +269,7 @@ double Pid::computeCommand(double error, ros::Duration dt) // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); - double p_term, d_term; + double p_term, d_term, i_term; p_error_ = error; // this is error = target - state if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error)) @@ -278,11 +278,14 @@ double Pid::computeCommand(double error, ros::Duration dt) // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; - //Calculate integral contribution to command - i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; + // Calculate the integral of the position error + i_error_ += dt.toSec() * p_error_; + + // Calculate integral contribution to command + i_term = gains.i_gain_ * i_error_; - // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); + // Limit i_term so that the limit is meaningful in the output + i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) ); // Calculate the derivative error if (dt.toSec() > 0.0) @@ -292,7 +295,9 @@ double Pid::computeCommand(double error, ros::Duration dt) } // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; - cmd_ = p_term + i_term_ + d_term; + + // Compute the command + cmd_ = p_term + i_term + d_term; return cmd_; } @@ -302,7 +307,7 @@ double Pid::updatePid(double error, ros::Duration dt) // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); - double p_term, d_term; + double p_term, d_term, i_term; p_error_ = error; //this is pError = pState-pTarget if (dt == ros::Duration(0.0) || std::isnan(error) || std::isinf(error)) @@ -311,11 +316,14 @@ double Pid::updatePid(double error, ros::Duration dt) // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; - //Calculate integral contribution to command - i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; + // Calculate the integral of the position error + i_error_ += dt.toSec() * p_error_; + + // Calculate integral contribution to command + i_term = gains.i_gain_ * i_error_; - // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); + // Limit i_term so that the limit is meaningful in the output + i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) ); // Calculate the derivative error if (dt.toSec() > 0.0) @@ -325,7 +333,9 @@ double Pid::updatePid(double error, ros::Duration dt) } // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; - cmd_ = - p_term - i_term_ - d_term; + + // Compute the command + cmd_ = - p_term - i_term - d_term; return cmd_; } @@ -335,7 +345,7 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt) // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); - double p_term, d_term; + double p_term, d_term, i_term; p_error_ = error; // this is error = target - state d_error_ = error_dot; @@ -346,15 +356,20 @@ double Pid::computeCommand(double error, double error_dot, ros::Duration dt) // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; - //Calculate integral contribution to command - i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; + // Calculate the integral of the position error + i_error_ += dt.toSec() * p_error_; + + // Calculate integral contribution to command + i_term = gains.i_gain_ * i_error_; - // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); + // Limit i_term so that the limit is meaningful in the output + i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) ); // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; - cmd_ = p_term + i_term_ + d_term; + + // Compute the command + cmd_ = - p_term - i_term - d_term; return cmd_; } @@ -364,7 +379,7 @@ double Pid::updatePid(double error, double error_dot, ros::Duration dt) // Get the gain parameters from the realtime buffer Gains gains = *gains_buffer_.readFromRT(); - double p_term, d_term; + double p_term, d_term, i_term; p_error_ = error; //this is pError = pState-pTarget d_error_ = error_dot; @@ -375,15 +390,20 @@ double Pid::updatePid(double error, double error_dot, ros::Duration dt) // Calculate proportional contribution to command p_term = gains.p_gain_ * p_error_; - //Calculate integral contribution to command - i_term_ = i_term_ + gains.i_gain_ * dt.toSec() * p_error_; + // Calculate the integral of the position error + i_error_ += dt.toSec() * p_error_; + + // Calculate integral contribution to command + i_term = gains.i_gain_ * i_error_; - // Limit i_term_ so that the limit is meaningful in the output - i_term_ = std::max( gains.i_min_, std::min( i_term_, gains.i_max_) ); + // Limit i_term so that the limit is meaningful in the output + i_term = std::max( gains.i_min_, std::min( i_term, gains.i_max_) ); // Calculate derivative contribution to command d_term = gains.d_gain_ * d_error_; - cmd_ = - p_term - i_term_ - d_term; + + // Compute the command + cmd_ = - p_term - i_term - d_term; return cmd_; } @@ -404,7 +424,7 @@ void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de) Gains gains = *gains_buffer_.readFromRT(); *pe = p_error_; - *ie = gains.i_gain_ ? i_term_/gains.i_gain_ : 0.0; + *ie = i_error_; *de = d_error_; } @@ -420,8 +440,8 @@ void Pid::printValues() << " I_Min: " << gains.i_min_ << "\n" << " P_Error_Last: " << p_error_last_ << "\n" << " P_Error: " << p_error_ << "\n" + << " I_Error: " << i_error_ << "\n" << " D_Error: " << d_error_ << "\n" - << " I_Term: " << i_term_ << "\n" << " Command: " << cmd_ ); diff --git a/test/pid_tests.cpp b/test/pid_tests.cpp index 2dec4a9d..86be85f3 100644 --- a/test/pid_tests.cpp +++ b/test/pid_tests.cpp @@ -32,27 +32,23 @@ TEST(ParameterTest, zeroITermBadIBoundsTest) TEST(ParameterTest, integrationWindupTest) { - RecordProperty("description","This test succeeds if the integral error is prevented from winding up when the integral gain is non-zero."); + RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is non-zero."); - Pid pid(0.0, 1.0, 0.0, 1.0, -1.0); + Pid pid(0.0, 2.0, 0.0, 1.0, -1.0); double cmd = 0.0; double pe,ie,de; cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); - pid.getCurrentPIDErrors(&pe,&ie,&de); - EXPECT_EQ(-1.0, ie); EXPECT_EQ(-1.0, cmd); cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); - pid.getCurrentPIDErrors(&pe,&ie,&de); - EXPECT_EQ(-1.0, ie); EXPECT_EQ(-1.0, cmd); } TEST(ParameterTest, integrationWindupZeroGainTest) { - RecordProperty("description","This test succeeds if the integral error is prevented from winding up when the integral gain is zero. If the integral error is allowed to wind up while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled."); + RecordProperty("description","This test succeeds if the integral contribution is prevented from winding up when the integral gain is zero. If the integral contribution is allowed to wind up while it is disabled, it can cause sudden jumps to the minimum or maximum bound in control command when re-enabled."); double i_gain = 0.0; double i_min = -1.0; @@ -64,14 +60,13 @@ TEST(ParameterTest, integrationWindupZeroGainTest) cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); pid.getCurrentPIDErrors(&pe,&ie,&de); - EXPECT_LE(i_min, ie); - EXPECT_LE(ie, i_max); + EXPECT_LE(i_min, cmd); + EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); cmd = pid.computeCommand(-1.0, ros::Duration(1.0)); - pid.getCurrentPIDErrors(&pe,&ie,&de); - EXPECT_LE(i_min, ie); - EXPECT_LE(ie, i_max); + EXPECT_LE(i_min, cmd); + EXPECT_LE(cmd, i_max); EXPECT_EQ(0.0, cmd); }