#########################################################################################
##
## CONTROL BLOCKS
## (blocks/ctrl.py)
##
## Milan Rother 2025
##
#########################################################################################
# IMPORTS ===============================================================================
import numpy as np
from ._block import Block
from ..optim.operator import DynamicOperator
# SISO BLOCKS ===========================================================================
[docs]
class PID(Block):
"""Proportional-Integral-Differntiation (PID) controller.
The transfer function is defined as
.. math::
H_\\mathrm{diff}(s) = K_p + K_i \\frac{1}{s} + K_d \\frac{s}{1 + s / f_\\mathrm{max}}
where the differentiation is approximated by a high pass filter that holds
for signals up to a frequency of approximately `f_max`.
Note
----
Depending on `f_max`, the resulting system might become stiff or ill conditioned!
As a practical choice set `f_max` to 3x the highest expected signal frequency.
Since this block uses an approximation of real differentiation, the approximation will
not hold if there are high frequency components present in the signal. For example if
you have discontinuities such as steps or square waves.
Example
-------
The block is initialized like this:
.. code-block:: python
#cutoff at 1kHz
pid = PID(Kp=2, Ki=0.5, Kd=0.1, f_max=1e3)
Parameters
----------
Kp : float
poroportional controller coefficient
Ki : float
integral controller coefficient
Kd : float
differentiator controller coefficient
f_max : float
highest expected signal frequency
Attributes
----------
op_dyn : DynamicOperator
internal dynamic operator for ODE component
op_alg : DynamicOperator
internal algebraic operator
"""
#max number of ports
_n_in_max = 1
_n_out_max = 1
#maps for input and output port labels
_port_map_in = {"in": 0}
_port_map_out = {"out": 0}
def __init__(self, Kp=0, Ki=0, Kd=0, f_max=100):
super().__init__()
#pid controller coefficients
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
#maximum frequency for differentiator approximation
self.f_max = f_max
def _g_pid(x, u, t):
x1, x2 = x
yp = self.Kp * u
yi = self.Ki * x2
yd = self.Kd * self.f_max * (u - x1)
return yp + yi + yd
def _jac_x_g_pid(x, u, t):
return np.array([-self.Kd * self.f_max, self.Ki])
def _jac_u_g_pid(x, u, t):
return self.Kd * self.f_max + self.Kp
def _f_pid(x, u, t):
x1, x2 = x
dx1, dx2 = self.f_max * (u - x1), u
return np.array([dx1, dx2])
#internal operators
self.op_dyn = DynamicOperator(
func=_f_pid,
)
self.op_alg = DynamicOperator(
func=_g_pid,
jac_x=_jac_x_g_pid,
jac_u=_jac_u_g_pid,
)
def __len__(self):
return 1 if self._active and (self.Kp or self.Kd) else 0
[docs]
def set_solver(self, Solver, parent, **solver_args):
"""set the internal numerical integrator
Parameters
----------
Solver : Solver
numerical integration solver class
parent : None | Solver
numerical solver instance
solver_args : dict
parameters for solver initialization
"""
#initialize the numerical integration engine with kernel
if not self.engine: self.engine = Solver(np.zeros(2), parent, **solver_args)
#change solver if already initialized
else: self.engine = Solver.cast(self.engine, parent, **solver_args)
[docs]
def update(self, t):
"""update system equation fixed point loop, with convergence control
Parameters
----------
t : float
evaluation time
"""
x, u = self.engine.get(), self.inputs[0]
y = self.op_alg(x, u, t)
self.outputs.update_from_array(y)
[docs]
def solve(self, t, dt):
"""advance solution of implicit update equation
Parameters
----------
t : float
evaluation time
dt : float
integration timestep
Returns
-------
error : float
solver residual norm
"""
x, u = self.engine.get(), self.inputs[0]
f, J = self.op_dyn(x, u, t), self.op_dyn.jac_x(x, u, t)
return self.engine.solve(f, J, dt)
[docs]
def step(self, t, dt):
"""compute update step with integration engine
Parameters
----------
t : float
evaluation time
dt : float
integration timestep
Returns
-------
success : bool
step was successful
error : float
local truncation error from adaptive integrators
scale : float
timestep rescale from adaptive integrators
"""
x, u = self.engine.get(), self.inputs[0]
f = self.op_dyn(x, u, t)
return self.engine.step(f, dt)
[docs]
class AntiWindupPID(PID):
"""Proportional-Integral-Differntiation (PID) controller with tracking
anti-windup mechanism (back-calculation).
Anti-windup mechanisms are needed when the magnitude of the control signal
from the PID controller is limited by some real world saturation. In these cases,
the integrator will continue to acumulate the control error and "wind itself up".
Once the setpoint is reached, this can result in significant overshoots. This
implementation adds a conditional feedback term to the internal integrator that
"unwinds" it when the PID output crosses some limits. This is pretty much a
deadzone feedback element for the integrator.
Mathematically, this block implements the following set of ODEs
.. math::
\\begin{eqnarray}
\\dot{x}_1 =& f_\\mathrm{max} (u - x_1) \\\\
\\dot{x}_2 =& u - w \\\\
\\end{eqnarray}
with the anti-windup feedback (depending on the pid output)
.. math::
w = K_s (y - \\min(\\max(y, y_\\mathrm{min}), y_\\mathrm{max}))
and the output itself
.. math::
y = K_p u - K_d f_\\mathrm{max} x_1 + K_i x_2
Note
----
Depending on `f_max`, the resulting system might become stiff or ill conditioned!
As a practical choice set `f_max` to 3x the highest expected signal frequency.
Since this block uses an approximation of real differentiation, the approximation will
not hold if there are high frequency components present in the signal. For example if
you have discontinuities such as steps or squere waves.
Example
-------
The block is initialized like this:
.. code-block:: python
#cutoff at 1kHz, windup limits at [-5, 5]
pid = AntiWindupPID(Kp=2, Ki=0.5, Kd=0.1, f_max=1e3, limits=[-5, 5])
Parameters
----------
Kp : float
poroportional controller coefficient
Ki : float
integral controller coefficient
Kd : float
differentiator controller coefficient
f_max : float
highest expected signal frequency
Ks : float
feedback term for back calculation for anti-windup control of integrator
limits : array_like[float]
lower and upper limit for PID output that triggers anti-windup of integrator
Attributes
----------
op_dyn : DynamicOperator
internal dynamic operator for ODE component
op_alg : DynamicOperator
internal algebraic operator
"""
def __init__(self, Kp=0, Ki=0, Kd=0, f_max=100, Ks=10, limits=[-10, 10]):
super().__init__(Kp, Ki, Kd, f_max)
#anti-windup control
self.Ks = Ks
self.limits = limits
def _g_pid(x, u, t):
x1, x2 = x
yp = self.Kp * u
yi = self.Ki * x2
yd = self.Kd * self.f_max * (u - x1)
return yp + yi + yd
def _jac_x_g_pid(x, u, t):
return np.array([-self.Kd * self.f_max, self.Ki])
def _jac_u_g_pid(x, u, t):
return self.Kd * self.f_max + self.Kp
def _f_pid(x, u, t):
x1, x2 = x
#differentiator state
dx1 = self.f_max * (u - x1)
#integrator state with windup control
y = _g_pid(x, u, t) #pid output
w = self.Ks * (y - np.clip(y, *self.limits)) #anti-windup feedback
dx2 = u - w
return np.array([dx1, dx2])
#internal operators
self.op_dyn = DynamicOperator(
func=_f_pid,
)
self.op_alg = DynamicOperator(
func=_g_pid,
jac_x=_jac_x_g_pid,
jac_u=_jac_u_g_pid,
)