diff --git a/control_toolbox/include/control_toolbox/pid.hpp b/control_toolbox/include/control_toolbox/pid.hpp index ab3eb1b5..5fc19498 100644 --- a/control_toolbox/include/control_toolbox/pid.hpp +++ b/control_toolbox/include/control_toolbox/pid.hpp @@ -202,69 +202,78 @@ inline bool is_zero(T value, T tolerance = std::numeric_limits::epsilon()) } /***************************************************/ -/*! \class Pid - \brief A basic pid class. - - This class implements a generic structure that - can be used to create a wide range of pid - controllers. It can function independently or - be subclassed to provide more specific controls - based on a particular control loop. - - This class also allows for retention of integral - term on reset. This is useful for control loops - that are enabled/disabled with a constant steady-state - external disturbance. Once the integrator cancels - out the external disturbance, disabling/resetting/ - re-enabling closed-loop control does not require - the integrator to wind up again. - - In particular, this class implements the standard - pid equation: - - \f$command = p_{term} + i_{term} + d_{term} \f$ - - where:
- - - given:
- - - \param p Proportional gain - - \param d Derivative gain - - \param i Integral gain - - \param i_clamp Minimum and maximum bounds for the integral windup, the clamp is applied to the \f$i_{term}\f$ - - \param u_clamp Minimum and maximum bounds for the controller output. The clamp is applied to the \f$command\f$. - - \section Usage - - To use the Pid class, you should first call some version of init() - (in non-realtime) and then call updatePid() at every update step. - For example: - - \verbatim +/*! + \class Pid + \brief Generic Proportional–Integral–Derivative (PID) controller. + + \details + The PID (Proportional–Integral–Derivative) controller is a widely used feedback + controller. This class implements a generic structure that can be used to create + a wide range of PID controllers. It can function independently or be subclassed + to provide more specific control loops. Integral retention on reset is supported, + which prevents re-winding the integrator after temporary disabling in presence + of constant disturbances. + + \section pid_equation PID Equation + The standard PID equation is: + \f[ + command = p\_term + i\_term + d\_term + \f] + where: + - \f$ p\_term = p\_gain \times error \f$ + - \f$ i\_term \mathrel{+}= i\_gain \times error \times dt \f$ + - \f$ d\_term = d\_gain \times d\_error \f$ + and: + - \f$ error = desired\_state - measured\_state \f$ + - \f$ d\_error = (error - error\_{last}) / dt \f$ + + \section parameters Parameters + \param p Proportional gain. Reacts to current error. + \param i Integral gain. Accumulates past error to eliminate steady-state error. + \param d Derivative gain. Predicts future error to reduce overshoot and settling time. + \param u\_min Minimum bound for the controller output. + \param u\_max Maximum bound for the controller output. + \param tracking\_time\_constant Tracking time constant for BACK_CALCULATION anti-windup. + If zero, a default is chosen based on gains: + - \f$ \sqrt{d\_gain / i\_gain} \f$ if \c d\_gain ≠ 0 + - \f$ p\_gain / i\_gain \f$ otherwise. + \param antiwindup\_strat Anti-windup strategy: + - NONE: no anti-windup (integral always accumulates). + - BACK_CALCULATION: adjusts \c i\_term based on difference between saturated + and unsaturated outputs using \c tracking\_time\_constant. + - CONDITIONAL_INTEGRATION: only integrates when output is not saturated + or error drives it away from saturation. + + \section antiwindup Anti-Windup Strategies + Without anti-windup, clamping causes integral windup, leading to overshoot and sluggish + recovery. This class provides two strategies: + + - **BACK_CALCULATION** + \f[ + i\_term \mathrel{+}= dt \times \Bigl(i\_gain \times error + \frac{1}{trk\_tc}\,(command_{sat} - command)\Bigr) + \f] + Prevents excessive accumulation by correcting \c i\_term toward the saturation limit. + + - **CONDITIONAL_INTEGRATION** + Integrates only if + \f[ + (command - command_{sat} = 0)\quad\lor\quad(error \times command \le 0) + \f] + Freezes integration when saturated and error drives further saturation. + + \section usage Usage Example + Initialize and compute at each control step: + \code{.cpp} control_toolbox::Pid pid; - pid.initialize(6.0, 1.0, 2.0, 0.3, -0.3); - double position_desired = 0.5; - ... - rclcpp::Time last_time = get_clock()->now(); - while (true) { - rclcpp::Time time = get_clock()->now(); - double effort = pid.compute_command(position_desired - currentPosition(), time - last_time); - last_time = time; + pid.initialize(6.0, 1.0, 2.0, -5.0, 5.0, + 2.0, control_toolbox::AntiwindupStrategy::BACK_CALCULATION); + rclcpp::Time last = get_clock()->now(); + while (running) { + rclcpp::Time now = get_clock()->now(); + double effort = pid.compute_command(setpoint - current(), now - last); + last = now; } - \endverbatim + \endcode */ /***************************************************/ diff --git a/doc/control_toolbox.md b/doc/control_toolbox.md index 72c774ec..8a596f54 100644 --- a/doc/control_toolbox.md +++ b/doc/control_toolbox.md @@ -1,3 +1,82 @@ # Base classes -Tbd. + +## PID + + +## PID Controller + +The PID (Proportional-Integral-Derivative) controller is a widely used feedback controller. This class implements a generic structure that can be used to create a wide range of PID controllers. It can function independently or be subclassed to provide more specific controls based on a particular control loop. Integral retention on reset is supported, which prevents re-winding the integrator after temporary disabling in presence of constant disturbances. + +### PID Equation + +The standard PID equation is given by: + +command = pterm + iterm + dterm + +where: +* pterm = pgain * error +* iterm = iterm + igain * error * dt +* dterm = dgain * derror + +and: +* error = desired_state - measured_state +* derror = (error - errorlast) / dt + +### Parameters + +* `p` (Proportional gain): This gain determines the reaction to the current error. A larger proportional gain results in a larger change in the controller output for a given change in the error. +* `i` (Integral gain): This gain determines the reaction based on the sum of recent errors. The integral term accounts for past values of the error and integrates them over time to produce the `i_term`. This helps in eliminating steady-state errors. +* `d` (Derivative gain): This gain determines the reaction based on the rate at which the error has been changing. The derivative term predicts future errors based on the rate of change of the current error. This helps in reducing overshoot, settling time, and other transient performance variables. +* `u_clamp` (Minimum and maximum bounds for the controller output): These bounds are applied to the final command output of the controller, ensuring the output stays within acceptable physical limits. +* `tracking_time_constant` (Tracking time constant): This parameter is specific to the 'back_calculation' anti-windup strategy. If set to 0.0 when this strategy is selected, a recommended default value will be applied. +* `antiwindup_strat` (Anti-windup strategy): This parameter selects how the integrator is prevented from winding up when the controller output saturates. Available options are: + * `NONE`: no anti-windup technique; the integral term accumulates without correction. + * `BACK_CALCULATION`: adjusts the integral term based on the difference between the unsaturated and saturated outputs using the tracking time constant `tracking_time_constant`. Faster correction for smaller `tracking_time_constant`. + * `CONDITIONAL_INTEGRATION`: only updates the integral term when the controller is not in saturation or when the error drives the output away from saturation, freezing integration otherwise. + +### Anti-Windup Strategies + +Anti-windup functionality is crucial for PID controllers, especially when the control output is subject to saturation (clamping). Without anti-windup, the integral term can accumulate excessively when the controller output is saturated, leading to large overshoots and sluggish response once the error changes direction. The `control_toolbox::Pid` class offers two anti-windup strategies: + +* **`BACK_CALCULATION`**: This strategy adjusts the integral term based on the difference between the saturated and unsaturated controller output. When the controller output `command` exceeds the output limits (`u_max` or `u_min`), the integral term `i_term` is adjusted by subtracting a value proportional to the difference between the saturated output `command_sat` and the unsaturated output `command`. This prevents the integral term from accumulating beyond what is necessary to maintain the output at its saturation limit. The `tracking_time_constant` parameter is used to tune the speed of this adjustment. A smaller value results in faster anti-windup action. + + The update rule for the integral term with back-calculation is: + + iterm += dt * (igain * error + (1 / trktc) * (commandsat - command)) + + If `trk_tc`, i.e., `tracking_time_constant` parameter, is set to 0.0, a default value is calculated based on the proportional and derivative gains: + * If `d_gain` is not zero: trktc = √(dgain / igain) + * If `d_gain` is zero: trktc = pgain / igain + +* **`CONDITIONAL_INTEGRATION`**: In this strategy, the integral term is only updated when the controller is not in saturation or when the error has a sign that would lead the controller out of saturation. Specifically, the integral term is frozen (not updated) if the controller output is saturated and the error has the same sign as the saturated output. This prevents further accumulation of the integral term in the direction of saturation. + + The integral term is updated only if the following condition is met: + + (command - commandsat = 0) ∨ (error * command ≤ 0) + + This means the integral term `i_term` is updated as `i_term += dt * i_gain * error` only when the controller is not saturated, or when it is saturated but the error is driving the output away from the saturation limit. + +### Usage Example + +To use the `Pid` class, you should first call some version of `initialize()` and then call `compute_command()` at every update step. For example: + +```cpp +control_toolbox::Pid pid; +pid.initialize(6.0, 1.0, 2.0, 5, -5,2,control_toolbox::AntiwindupStrategy::BACK_CALCULATION); +double position_desired = 0.5; +... +rclcpp::Time last_time = get_clock()->now(); +while (true) { + rclcpp::Time time = get_clock()->now(); + double effort = pid.compute_command(position_desired - currentPosition(), time - last_time); + last_time = time; +} +``` + +### References + +1. Visioli, A. _Practical PID Control_. London: Springer-Verlag London Limited, 2006. 476 p. +2. Vrancic, D., Horowitz, R., & Hagiwara, T. “Antiwindup, Bumpless, and Conditioned Transfer Techniques for PID Controllers.” _IEEE Control Systems Magazine_, vol. 16, no. 4, 1996, pp. 48–57. +3. Bohn, C.; Atherton, D. “An analysis package comparing PID anti-windup strategies.” _IEEE Control Systems Magazine_, 1995, pp. 34–40. +4. Åström, K.; Hägglund, T. _PID Controllers: Theory, Design and Tuning_. Research Triangle Park, USA: ISA Press / Springer-Verlag London Limited, 1995. 343 p. diff --git a/doc/migration.rst b/doc/migration.rst index 6e281f80..3d5ab630 100644 --- a/doc/migration.rst +++ b/doc/migration.rst @@ -3,3 +3,7 @@ Migration Guides: Jazzy to Kilted ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This list summarizes important changes between Jazzy (previous) and Kilted (current) releases, where changes to user code might be necessary. + +Pid/PidRos +*********************************************************** +* The parameters :paramref:`antiwindup`, :paramref:`i_clamp_max`, and :paramref:`i_clamp_min` have been removed. The anti-windup behavior is now configured via the :paramref:`AntiWindupStrategy` enum. (`#298 `_).