120 lines
3.7 KiB
Python
120 lines
3.7 KiB
Python
"""This module implements the Linear Quadratic Regulator with integral part and anti-windup."""
|
|
|
|
# Mathematics
|
|
from numpy import clip
|
|
from numpy import hstack
|
|
from numpy import ndarray
|
|
from numpy import vstack
|
|
from numpy import zeros
|
|
|
|
# LQR control
|
|
from .lqr import Lqr
|
|
|
|
|
|
class AntiWindupLqr(Lqr):
|
|
|
|
"""The anti-windup LQR controller, including an integration state for zero stationary error.
|
|
|
|
This controller resembles the LQR with added clipping on the control signal to a user-set
|
|
maximum value. Furthermore, the integral of the error over time is pruned (anti windup).
|
|
|
|
Examples:
|
|
First, import some helper functions from numpy.
|
|
|
|
>>> from numpy import array
|
|
>>> from numpy import eye
|
|
>>> from numpy import vstack
|
|
|
|
We then setup the Lqr controller with some control constants.
|
|
|
|
>>> controller = AntiWindupLqr(
|
|
... array([[0, 1], [0, 0]]),
|
|
... array([0, 1])[:, None],
|
|
... array([1, 0])[None, :],
|
|
... eye(3),
|
|
... array([[1.0]]),
|
|
... array([1.0]),
|
|
... 0.5,
|
|
... )
|
|
|
|
We then specify an initial and desired state.
|
|
|
|
>>> initial = vstack([1.0, 0.0])
|
|
>>> desired = vstack([0.0])
|
|
|
|
Finally, we retrieve a control signal from the Lqr based on the values we just set.
|
|
|
|
>>> signal = controller.control(desired, initial)
|
|
|
|
Args:
|
|
A: System matrix (continous time) ``(n, n)``
|
|
B: Input matrix ``(n, 1)``
|
|
C: Output matrix ``(1, n)``
|
|
Q: State cost matrix (pos. semi definite, symmetric) ``(n+1, n+1)``
|
|
R: Control cost matrix (pos. definite, symmetric) ``(1, n)``
|
|
max_control: Limit of control signal
|
|
dt: Time between measurements
|
|
keep_trace: Whether to store a trace of control signals, states, etc.
|
|
"""
|
|
|
|
# In this context, we reproduce a common PID notation
|
|
# pylint: disable=invalid-name, too-many-arguments
|
|
|
|
def __init__(
|
|
self,
|
|
A: ndarray,
|
|
B: ndarray,
|
|
C: ndarray,
|
|
Q: ndarray,
|
|
R: ndarray,
|
|
max_control: ndarray,
|
|
dt: float,
|
|
keep_trace: bool = False,
|
|
) -> None: # noqa: E741
|
|
# Controller specification for augmented state
|
|
n = A.shape[0] + 1
|
|
A_i = zeros((n, n))
|
|
A_i[1:, 1:] = A
|
|
A_i[0, 1:] = -C
|
|
B_i = vstack([zeros((1, 1)), B])
|
|
C_i = hstack([zeros((1, 1)), C])
|
|
|
|
# Setup internal LQR controller and own attributes
|
|
super().__init__(A_i, B_i, C_i, Q, R, dt, keep_trace, calculate_feed_forward=False)
|
|
self.V *= (self.C * self.K).sum()
|
|
self.max_control = max_control
|
|
self.summed_error = 0.0
|
|
|
|
def control(self, desired: ndarray, state: ndarray) -> ndarray:
|
|
"""Compute the control signal based on LQR controller.
|
|
|
|
Args:
|
|
desired: The desired output
|
|
state: The current state
|
|
|
|
Returns:
|
|
The control signal
|
|
"""
|
|
|
|
# Prepend summed error to state vector
|
|
state_i = vstack([self.summed_error, state])
|
|
|
|
# Compute errors
|
|
error = desired - self.C @ state_i
|
|
self.summed_error += self.dt * error
|
|
|
|
# Get the basic PID control signal and clip to specified boundary
|
|
lqr_signal = super().control(desired, state_i)
|
|
control_signal: ndarray = clip(lqr_signal, -abs(self.max_control), abs(self.max_control))
|
|
|
|
# Prune integral part, i.e. apply anti wind up
|
|
self.summed_error += (lqr_signal - control_signal) / self.K[0, 0]
|
|
|
|
return control_signal
|
|
|
|
def reset(self) -> None:
|
|
"""Resets the filter's memory, i.e. set the error integral to zero and empty the process trace."""
|
|
|
|
super().reset()
|
|
self.summed_error = 0.0
|