PID-stabilised inverted pendulum
An inverted pendulum on a cart that a PID controller of its angle keeps upright.
- Name:
pid_inverted_pendulum - Field:
engineering/control - Formalism: ordinary differential equations
- References: Åström and Murray 2008
Factory
@register_model(
field='engineering/control',
tags=('control', 'feedback', 'pid', 'unstable equilibrium', 'ode'),
references=('Åström and Murray 2008',),
defaults={'until': 5.0, 'dt': 0.05, 'solver': RK4Solver(dt=0.001)},
observables=lambda system: [ObservableSpec('tilt', _tilt)],
)
def pid_inverted_pendulum(
theta: float = 0.1,
*,
kp: float = 40.0,
ki: float = 1.0,
kd: float = 8.0,
M: float = 1.0,
m: float = 0.1,
l: float = 0.5,
) -> System:
"""Return an inverted pendulum on a cart that a PID controller of its angle keeps upright.
The controller sets the force on the cart to `kp e + ki ∫ e dt - kd theta'` from the error `e = -theta` of the
angle, within the bounds of the force. Near upright the closed loop is stable when `kp > (M + m) g` and `kd > 0`,
and the angle returns to upright while the cart drifts.
Args:
theta:
The initial angle from upright.
kp:
The proportional gain.
ki:
The integral gain.
kd:
The derivative gain.
M:
The mass of the cart.
m:
The mass at the end of the rod.
l:
The length of the rod.
Returns:
system:
The model, with the angle `cart.theta`, the position `cart.x`, the force `cart.force`, and the observable
`tilt`.
Examples:
>>> from skcomplex.models.engineering.control import pid_inverted_pendulum
>>> pid_inverted_pendulum(kp=50.0).interactions[0].kp
50.0
"""
cart = CartPendulumElement('cart', theta=theta, M=M, m=m, l=l)
controller = PIDInteraction(
'pid',
plant='cart',
integrator='memory',
control='force',
measure='theta',
kp=kp,
ki=ki,
kd=kd,
limits=(-FORCE_LIMIT, FORCE_LIMIT),
)
return System(elements=[cart, IntegratorElement('memory')], interactions=[controller])
Components
class CartPendulumElement(Element):
"""Inverted pendulum on a cart that a bounded horizontal force pushes, after Åström and Murray.
The cart of mass `M` at position `x` carries a point mass `m` at the end of a rod of length `l`, at the angle
`theta` from upright, positive towards negative `x`. The equations of motion are
`(M + m) x'' - m l theta'' cos(theta) + m l theta'^2 sin(theta) = force` and
`l theta'' - x'' cos(theta) = g sin(theta)`, so upright is unstable and a positive force tilts the pendulum back
from a positive angle.
Args:
label:
The label of the cart.
visual:
How the element is drawn, or `None` for `default_visual_`.
x:
The initial position of the cart.
x_d1:
The initial velocity of the cart.
theta:
The initial angle from upright.
theta_d1:
The initial angular velocity.
force:
The horizontal force on the cart, the control input.
M:
The mass of the cart.
m:
The mass at the end of the rod.
l:
The length of the rod.
g:
The acceleration of gravity.
Examples:
>>> from skcomplex.models.engineering.control import CartPendulumElement
>>> CartPendulumElement('cart', theta=0.1).theta
0.1
"""
def __init__(
self,
label: str,
visual: VisualSpec | None = None,
*,
x: Annotated[float, StateSpec(order=2)] = 0.0,
x_d1: float = 0.0,
theta: Annotated[float, StateSpec(order=2)] = 0.0,
theta_d1: float = 0.0,
force: Annotated[float, ParameterSpec(adaptive=True, bounds=(-FORCE_LIMIT, FORCE_LIMIT))] = 0.0,
M: Annotated[float, ParameterSpec(domain=RealSet.open(0, oo))] = 1.0,
m: Annotated[float, ParameterSpec(domain=RealSet.open(0, oo))] = 0.1,
l: Annotated[float, ParameterSpec(domain=RealSet.open(0, oo))] = 0.5,
g: Annotated[float, ParameterSpec(domain=RealSet.open(0, oo))] = GRAVITY,
) -> None:
super().__init__(label, visual=visual)
self.x = x
self.x_d1 = x_d1
self.theta = theta
self.theta_d1 = theta_d1
self.force = force
self.M = M
self.m = m
self.l = l
self.g = g
def _get_laws(self, s: SimpleNamespace) -> list[BaseLaw]:
"""Return the equations of motion of the cart and the pendulum.
Args:
s:
The namespace of the symbols the laws can use.
Returns:
laws:
The laws of the component.
"""
x_acceleration, theta_acceleration = derivative(s.x, 2), derivative(s.theta, 2)
spin = derivative(s.theta, 1)
return [
EquationLaw(
(s.M + s.m) * x_acceleration
- s.m * s.l * theta_acceleration * cos(s.theta)
+ s.m * s.l * spin**2 * sin(s.theta)
== s.force,
),
EquationLaw(s.l * theta_acceleration - x_acceleration * cos(s.theta) == s.g * sin(s.theta)),
]
class IntegratorElement(Element):
"""Element that holds the integral of a signal, whose rate a controller sets.
Args:
label:
The label of the integrator.
visual:
How the element is drawn, or `None` for `default_visual_`.
value:
The initial value of the integral.
Examples:
>>> from skcomplex.models.engineering.control import IntegratorElement
>>> IntegratorElement('memory').value
0.0
"""
def __init__(
self,
label: str,
visual: VisualSpec | None = None,
*,
value: Annotated[float, StateSpec()] = 0.0,
) -> None:
super().__init__(label, visual=visual)
self.value = value
class PIDInteraction(Interaction):
"""Proportional-integral-derivative controller that sets a control input of a plant from the error of a measure.
The error is `e = setpoint - y` for the measured state `y`, and the controller sets the control input to
`u = kp e + ki I - kd y'`, clipped to the limits, with the derivative taken on the measure. The integral
`I' = e` of the error lives in an `IntegratorElement` bound to the role `integrator`. The control input is an
adaptive parameter of the plant, which the controller sets after every solver step, so it holds within a step like a
sampled controller. The derivative term reads the first derivative of the measure, so a measure with a derivative
term is a state of order 2.
Args:
label:
The label of the controller.
plant:
The path of the element that holds the measure and the control input.
integrator:
The path of the integrator of the error.
kp:
The proportional gain.
ki:
The integral gain.
kd:
The derivative gain.
setpoint:
The value the measure is kept at.
control:
The name of the adaptive parameter of the plant that the controller sets.
measure:
The name of the measured state of the plant.
limits:
The lower and upper limit of the control input, or `None` for no limits.
Examples:
>>> from skcomplex.models.engineering.control import PIDInteraction
>>> PIDInteraction('pid', plant='cart', integrator='memory', control='force', measure='theta', kp=40.0).kp
40.0
"""
def __init__(
self,
label: str,
*,
plant: Annotated[str, RoleSpec()],
integrator: Annotated[str, RoleSpec()],
kp: Annotated[float, ParameterSpec()] = 1.0,
ki: Annotated[float, ParameterSpec()] = 0.0,
kd: Annotated[float, ParameterSpec()] = 0.0,
setpoint: Annotated[float, ParameterSpec()] = 0.0,
control: str = 'u',
measure: str = 'y',
limits: tuple[float, float] | None = None,
) -> None:
super().__init__(label)
self.plant = plant
self.integrator = integrator
self.kp = kp
self.ki = ki
self.kd = kd
self.setpoint = setpoint
self.control = control
self.measure = measure
self.limits = limits
def _get_laws(self, s: SimpleNamespace) -> list[BaseLaw]:
"""Return the rate of the integral of the error and the control input it sets.
Args:
s:
The namespace of the symbols the laws can use.
Returns:
laws:
The laws of the component.
"""
measured = getattr(s.plant, self.measure)
error = s.setpoint - measured
output = s.kp * error + s.ki * s.integrator.value
if self.kd != 0:
output -= s.kd * derivative(measured, 1)
return [
RateLaw(s.integrator.value, error),
BoundaryLaw(getattr(s.plant, self.control), clip(output, *(self.limits or (None, None)))),
]
Equations
cart.x: diff(x_cart(t), t, t) == -(g_cart*sin(theta_cart(t)) - (l_cart*m_cart*sin(theta_cart(t))*diff(theta_cart(t), t)^2 - force_cart(t))*cos(theta_cart(t))/(M_cart + m_cart))*l_cart*m_cart*cos(theta_cart(t))/((l_cart*m_cart*cos(theta_cart(t))^2/(M_cart + m_cart) - l_cart)*(M_cart + m_cart)) - (l_cart*m_cart*sin(theta_cart(t))*diff(theta_cart(t), t)^2 - force_cart(t))/(M_cart + m_cart)
cart.theta: diff(theta_cart(t), t, t) == -(g_cart*sin(theta_cart(t)) - (l_cart*m_cart*sin(theta_cart(t))*diff(theta_cart(t), t)^2 - force_cart(t))*cos(theta_cart(t))/(M_cart + m_cart))/(l_cart*m_cart*cos(theta_cart(t))^2/(M_cart + m_cart) - l_cart)
cart.force: diff(force_cart(t), t) == 0
memory.value: diff(value_memory(t), t) == setpoint_pid - theta_cart(t)