[Python] Implementing Phase-Locked Loop (PLL) filter for equations of motion in Python

I've been playing around with learning about PPL filters and I found in interesting application in the field of mechanical engineering, where this is fits perfectly. A short description: I have a body with a single degree of freedom (DOF) (x-direction) and attached to that body is a rotating device, which (in the end) should dampen the movement of the body. Combined (linear and rotating body) the 1DOF model becomes: (m+m_c)ddx(t)+cdx(t)+kx(t) = m_cr*(dphi(t)^2*cos(phi(t))+ddphi(t)*sin(phi(t)))+f_e(t) , where m is mass of the body, m_c the mass of the rotating device, ddx(t) the second derivative (acceleration), c the damping coefficient, dx(t) the first derivative (velocity), k the stiffness, x(t) the displacement, r the radius, ddphi(t) the second derivative (rotational acceleration), dphi(t) the first derivate (rotational velocity), phi(t) the angle, and f_e(t) is a forcing function causing the body to move. Note that the terms before f_e(t) is what should dampen the movement.

Once solved with the initial conditions (only for x and dx I assume, as phi and its derivatives should come from the PPL filter), a target position is calculated to give the desired position for the PLL filter as: phi_t(t) = arctan2(x(t),dx(t)/omega_n)+pi

Here comes the PLL filter into the mix. The design of the filter is specifically for this type of problem so I have it well defined.

1. An error is calculated (limited to be between -pi and pi): e_t = phi(t)-phi_t_f
2. This enters a controller in laplace: C_phi(s) = omega_f^2/(s+2*omega_f) with the
output being the control velocity c_phi(t)
3. Calculate the target angular velocity: dphi_t,f(t) = omega_n + c_phi(t)
4. Integrate resuling in the filtered target angular position phi_t_f (used in
the error calculation)


So the loop should then be the output (target angular position) and its derivates (velocity and acceleration), which then act as input in the next load step when solving the 1DOF model for x, dx, and ddx. This should then run for a specified amount of time.

I haven't had any luck implementing the correct Laplace space solutions, so I had to do some short-cuts to implement it. However, I'm pretty sure it is incorrect as setting the amplitude function F0 = 0 still results in motion, i.e. my damper excites the body.

To make a long story short, I was hoping you guys could help me implement the correct Laplace solution, so any suggestions, ideas etc. are welcome.

Thanks a lot!

Here's my current attempt.

import numpy as np
import matplotlib.pyplot as plt
from scipy.integrate import solve_ivp

# Define system parameters
m = 272939 # Modal mass (kg)
delta = 0.01 # Logarithmic decrement (%)
f_t = 0.128 # frequency (Hz)
k = (f_t*2*np.pi)**2*m # Stiffness (N/m)
c = 2*f_t*2*np.pi*delta/2/np.pi*m # Damping coefficient (N*s/m)
F0 = 100 # Amplitude of external force (N)
m_c = 50 # Mass of rotating body (kg)
r = 0.6 # Radius (m)
J = 52 # Moment of inertia (kg*m^2)
t = 10 # Simulation time

# Natural frequency and period
omega_n = np.sqrt(k / (m + m_c)) # Natural frequency (rad/s)
#T_n = 2 * np.pi / omega_n # Natural period (s)

# Sampling time
omega_f = omega_n # Define omega_f as omega_n for now
T_s = 1 / (100 * ((2 * np.pi) / max(omega_n, omega_f)))

# Define the equations of motion for SDOF with damper and PLL filter
# x is displacement, dx the velocity, phi_t_f the filtered target angular position (TAP)
def sdof_trd(t, y):
x, dx, phi_t_f, c_phi = y # Unpack state variables
F_e = F0 * np.sin(omega_f * t) # Sine wave excitation force

# Compute TAP phi_t(t)
phi_t = np.arctan2(x, dx / omega_n) + np.pi

# Compute error e_t and limit to [-pi, pi]
e_t = np.mod(phi_t - phi_t_f + np.pi, 2 * np.pi) - np.pi

# Compute control velocity c_phi_dot
c_phi_dot = omega_f**2 * e_t - 2 * omega_f * c_phi # Check this for errors!

# Compute filtered derivative dphi_t_f (filtered velocity)
dphi_t_f = omega_n + c_phi # Check this for errors!

# Compute second derivative ddphi_t_f
ddphi_t_f = c_phi_dot # Since d/dt (omega_n + c_phi) = d(c_phi)/dt # Check this for errors!

# Compute damper force
F_T = m_c * r * ((dphi_t_f)**2 * np.cos(phi_t_f) + ddphi_t_f * np.sin(phi_t_f))

# Equation of Motion for the
ddx = (-c * dx - k * x + F_T + F_e) / (m + m_c)

return [dx, ddx, dphi_t_f, c_phi_dot]

# Time span
T = np.arange(0, t, T_s)

# Initial conditions
x_0, dx_0 = 0, 0
phi_t_f_0, c_phi_0 = 0, 0
init_conditions_trd = [x_0, dx_0, phi_t_f_0, c_phi_0]

# Solve ODEs
sol_trd = solve_ivp(sdof_trd, [0,t], init_conditions_trd, t_eval=T)
x_vals_trd = sol_trd.y[0] # Displacement
dx_vals_trd = sol_trd.y[1] # Velocity
dphi_t_f_vals = sol_trd.y[2] # Filtered derivative dphi_t_f, i.e. filtered velocity
c_phi_vals = sol_trd.y[3] # Controller velocity output

ddphi_t_f_vals = np.gradient(dphi_t_f_vals, T_s) # Compute second derivative numerically
ddx_vals_trd = np.gradient(dx_vals_trd, T_s) # Compute acceleration numerically

# Compute TAP phi_t(t)
phi_t_vals = np.arctan2(x_vals_trd, dx_vals_trd / omega_n) + np.pi

# Compute control torque
M_t_vals = J * ddphi_t_f_vals - 0.5 * ddx_vals_trd * m_c * r * np.sin(phi_t_vals)

# Plots
plt.figure(figsize=(10, 5))
plt.plot(T, x_vals_trd, label='Displacement $x_t [m]$', color='black', linestyle='-')
plt.xlabel(r'$t$ $$')
plt.title('Displacement')
plt.legend()
plt.grid()
plt.show()

plt.figure(figsize=(10, 5))
plt.plot(T, phi_t_vals, label='TAP $\phi_t [rad]$', color='blue', linestyle='-')
plt.xlabel(r'$t$ $$')
plt.ylabel(r'$[rad/s]$')
plt.title('TAP Over Time')
plt.legend()
plt.grid()
plt.show()

# Plot control effort
plt.figure(figsize=(10, 5))
plt.plot(T, c_phi_vals, label='Control Effort $c_\phi$', color='green')
plt.xlabel(r'$t$ $$')
plt.ylabel(r'$c_\phi$ $[rad/s]$')
plt.title('Control Effort Over Time')
plt.legend()
plt.grid()
plt.show()

# Plot error e_t
plt.figure(figsize=(10, 5))
plt.plot(T, np.mod(phi_t_vals - dphi_t_f_vals + np.pi, 2 * np.pi) - np.pi, label='Error $e_t$', color='orange')
plt.xlabel(r'$t$ $$')
plt.ylabel(r'$e_t$ $[rad]$')
plt.title('Error $e_t$')
plt.legend()
plt.grid()
plt.show()

# Plot deviation
plt.figure(figsize=(10, 5))
plt.plot(T, c_phi_vals/omega_n, label='$c_\phi(t)$ / $\omega_n$', color='red')
plt.xlabel(r'$t$ $$')
plt.ylabel(r'$c_\phi(t)$ / $\omega_n [-]$')
plt.title('Deviation')
plt.legend()
plt.grid()
plt.show()

# Plot control torque
plt.figure(figsize=(10, 5))
plt.plot(T, M_t_vals, label='Control Torque $M_t$', color='teal')
plt.xlabel(r'$t$ $$')
plt.ylabel(r'$M_t$ [Nm]')
plt.title('Control Torque Over Time')
plt.legend()
plt.grid()
plt.show()

Continue reading...