Bearing Control¶
%config InlineBackend.figure_format = 'svg'
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
import control as c
Plant Transfer Function Derivation¶
The total force acting on the rotor consists of a control force and displacement force:
\begin{align} F = F_c + k_{\delta}x \end{align}Using Newtons second law:
\begin{align} m\ddot{x}-k_{\delta}x=F_c \end{align}and we get the following transfer function relating the control force the to position:
\begin{align} \frac{X(s)}{F_c(s)} = \frac{1}{ms^2 - k_{\delta}} = \frac{1/m}{s^2 - k_{\delta}/m} \end{align}We can write this as a difference of squares as:
\begin{align} \frac{1/m}{s^2 - \omega_x^2} = \frac{1/m}{(s+\omega_x)(s-\omega_x)} \end{align}where $\omega_x = \sqrt{\frac{k_{\delta}}{m}}$
finally, we can write this in the canonical form for two poles as follows:
\begin{align} \frac{X(s)}{F_c(s)} = \frac{\frac{-1}{m\omega_x^2}}{(1 + \frac{s}{\omega_x})(1 - \frac{s}{\omega_x})} \end{align}Effective Mass¶
The simple model above does not capture all of the actual dynamics of this actual setup but is used for simplicity. In the model, the mass must be known, however, for our system which essentially acts as a pendulum the control forces are not acting on the entire mass of the rotor. Because of this, we must determine an effective mass.
The diagram above shows the setup with a coordinate system located at the pivot point of the rotor (point $O_1$). Inertia and mass properties are listed as well as the lever arm length that control forces act through. We can calculate the effective mass of the system by determining point mass that results in the same acceleration of point $O_2$ as is created in system above. If the point $O_1$ is a fixed point, the acceleration of point $O_2$ is given as $a = r \alpha$, and thus we get the following:
\begin{align} F = ma \rightarrow \alpha r = \frac{F}{m} \end{align}However,
\begin{align} T = F r = I \alpha \end{align}and thus,
\begin{align} \frac{F r^2}{I} = \frac{F}{m} \end{align}Solving, we get that the effective mass of the system is:
\begin{align} m = \frac{I}{r^2} \end{align}where $I$ is the moment of interia about the "allowable" point $O_1$ and $r$ is the length of the lever arm
Calculating the mass:
m_total = 3.4341 #kg
I = 0.2586612 #kg-m^2
r = 0.3069 #m
m = I/(r**2)
m
m/m_total
We can see that the effective mass is around 80% of the total mass of the rod. This method of calculating the mass took into account the distribution of the mass. Had we just taken the effective mass to be half of the total mass our dynamic model would be off by a lot.
Define System Properties¶
k_delta_x_mm = 129.945 #N/mm
k_delta_y_mm = 129.856 #N/mm
k_delta_x = k_delta_x_mm*1000
k_delta_y = k_delta_y_mm*1000
We can look at the unstable pole location:
wx = np.sqrt(k_delta_x/m)
fx = wx/(2*np.pi)
print(f'Unstable Pole Location: {fx:.2f} Hz')
Plant Transfer Function¶
First we create our unstable plant transfer function
Gp = c.TransferFunction([1],[m, 0, -k_delta_x])
Gp
We can take a look at the plants bode plot
mag, phase, omega = c.bode_plot(Gp, dB = True, Hz = True)
fig = plt.gcf()
fig.set_size_inches(12,8)
Inputs¶
Fc = 150 #Hz (bandwidth of position control)
phi_pm_deg = 60 #degrees (phase margin)
#convert to proper units
Wc = 2*np.pi*Fc
phi_pm = np.deg2rad(phi_pm_deg)
Kernel of Loop Gain¶
The kernel of the loop gain consists of the current regulator and plant
We will us PI controller for our current regulator as follows. The transfer function relating input voltage to output current over an RL load is as follows:
\begin{align} \frac{I(s)}{V(s)} = \frac{1}{Ls + R} = \frac{1}{L(s + \frac{R}{L})} \end{align}A PI controller can be written as follows:
\begin{align} G_c(s) = k_p\big(1 + \frac{k_i}{k_p s}\big) = \frac{k_p}{s}\big(s + \frac{k_i}{k_p}\big) \end{align}The open loop transfer function including the controller and plant are as follows:
\begin{align} G_{\textrm{OL}}(s) = \frac{k_p\big(s + \frac{k_i}{k_p}\big)}{Ls\big(s + \frac{R}{L}\big)} \end{align}We can use pole-zero cancelation to reduce the order of our system by setting,
\begin{align} \frac{k_i}{k_p} = \frac{R}{L} \end{align}the open loop transfer function then becomes:
\begin{align} G_{\textrm{OL}}(s) = \frac{k_p}{Ls} \end{align}which has the corresponding closed loop transfer function:
\begin{align} I_{\textrm{CL}}(s) = \frac{k_p}{Ls + k_p} = \frac{1}{\frac{L}{k_p}s + 1} = \frac{1}{\frac{s}{\omega_{ci}} + 1} \end{align}where $\omega_{ci} = k_p/L$
Thus we see that we can set our current regulator bandwidth by setting $k_p$ and then $k_i = \frac{R}{L} k_p$
Wci = 10*Wc #set current regulator bandwidth 10 times higher than position bandwidth
Icl = c.TransferFunction([1], [1/Wci, 1]) #current regulator
Tk = Icl*Gp
Tk
mag, phase, omega = c.bode_plot(Tk, dB = True, Hz = True)
fig = plt.gcf()
fig.set_size_inches(12,8)
Note that the phase should be starting at -180 degrees, not at 180 degrees. This is incorrect in the controls package source code. If you look at the source code, it calculates the angle of the transfer function $G(j\omega)$ simply as $\textrm{Arg}(G(j\omega))$ without having any regard to where the zeros and poles are.
Controller Design¶
A PID controller with a low pass filter on the derivative has the following form.
\begin{align} G_c(s) &= k_p + k_i\frac{1}{s} + k_d \frac{s}{s/\omega_p + 1} \\ &= \frac{(k_p/\omega_p + k_d)s^2 + (k_p + k_i/\omega_p)s + k_i}{s(s/\omega_p + 1)} \\ &=k_{dc} \frac{(s/\omega_{z1} + 1)(s/\omega_{z2} + 1)}{s(s/\omega_p + 1)} \end{align}where,
\begin{align} k_p &= k_{dc} \bigg(\frac{\omega_{z1} + \omega_{z2}}{\omega_{z1}\omega_{z2}} - \frac{1}{\omega_p}\bigg) \\ k_i &= k_{dc} \\ k_d &= k_{dc} \bigg(\frac{1}{\omega_{z1} \omega_{z2}} - \frac{\omega_{z1} + \omega_{z2}}{\omega_{z1}\omega_{z2} \omega_p} + \frac{1}{\omega_p^2}\bigg) \end{align}We see that the controller can be written out as a DC gain with two zeros, a pole, and a pole at the origin which represents 4 design variables that we can manipulate.
We can systematically place these poles and zeros to achieve the phase margin and bandwidth that we desire. Note that this is two input variables but we have four parameters to determine so some engineering judgement must be taken into account. The arbitrary choices that we will be as follows:
\begin{align} \omega_{z1} &= \frac{\omega_c}{100} \\ \omega_p &= 4\omega_c \end{align}$\omega_{z1}$ is placed two decades below our controller bandwidth because the kernel of our loop gain starts out at -180 degrees and we need to gain phase. Zero's increase the phase of the system.
$\omega_p$ is placed 4 times higher than the bandwidth of our controller such that it can filter out noise but doesn't affect the overall system dynamics.
The second zero location is chosen to give the desired phase margin as:
\begin{align} \omega_{z2} = \frac{\omega_c}{\tan{\big(\phi_{\textrm{PM}} + \pi/2 + \arctan{\frac{\omega_{c}}{\omega_{ci}}} - \arctan{\frac{\omega_{c}}{\omega_{ci}}} + \arctan{\frac{\omega_{z1}}{\omega_{p}}} \big)}} \end{align}Finally, the DC gain is set such that the gain cross-over frequency occurs at the desired controller bandwidth:
\begin{align} k_{dc} = \frac{1}{\bigg|G_c(j\omega_c)\big|_{k_{dc} = 1}\bigg||I_{CL}(j\omega_c)|G_p(j\omega_c)|} \end{align}Wz1 = Wc/100
Wp = 4*Wc
Wz2 = Wc/np.tan(phi_pm + np.pi/2 + np.arctan(Wc/Wci) - np.arctan(Wc/Wz1) + np.arctan(Wc/Wp))
We can then create the controller transfer function as a combination of simple pole and zero transfer functions
z1 = c.TransferFunction([1/Wz1, 1], 1)
z2 = c.TransferFunction([1/Wz2, 1], 1)
p1 = c.TransferFunction([1], [1/Wp, 1])
p_origin = c.TransferFunction(1,[1,0])
Gc = z1*z2*p_origin*p1
Gc
Now we can compute the DC gain
M1 = np.abs(c.evalfr(Gc, 1j*Wc))
M2 = np.abs(c.evalfr(Icl, 1j*Wc))
M3 = np.abs(c.evalfr(Gp, 1j*Wc))
kdc = 1/(M1*M2*M3)
Re-define the controller with the DC gain
Gc = kdc*Gc
Gc
Create a function to compute the PID gains given the pole and zero locations
def PID_Gains(Wz1, Wz2, Wp, kdc):
kp = kdc*((Wz1 + Wz2)/(Wz1*Wz2) - 1/Wp) #N/m
ki = kdc #N/m-s
kd = kdc*(1/(Wz1*Wz2) - (Wz1 + Wz2)/(Wz1*Wz2*Wp) + 1/Wp**2)
return kp, ki, kd
We can observe what the bode plot of our controller looks like
mag, phase, omega = c.bode_plot(Gc, dB = True, Hz = True)
fig = plt.gcf()
fig.set_size_inches(12,8)
Finally we can obtain our total open loop transfer function by multiply our controller by the kernel of our loop gain.
T = Gc*Tk
mag, phase, omega = c.bode_plot(T, dB = True, Hz = True)#, margins = True
fig = plt.gcf()
fig.set_size_inches(12,8)
Note again that the 180 degrees should be -180 (and 90 degrees should be -270). We could manually plot our data instead of using the convenience plotting functiong built into the controls library but this isn't worth the effort. We can see that we get the cross over frequency and phase margin that we designed for.
Stability Check¶
We can also verify the stability of our system by looking at its closed loop poles.
Gcl = c.feedback(T, 1)
Gcl
poles = Gcl.pole()
for pole in poles:
print(pole)
We see that all of our closed loop poles are in the left half of the complex plane indicating that the system is indeed stable
print('Design Inputs:')
print(f"Phase Margin: {phi_pm_deg:.2f} deg")
print(f"Controller Bandwidth: {Wc/(2*np.pi):.2f} Hz")
kp, ki, kd = PID_Gains(Wz1, Wz2, Wp, kdc)
print('\nController Parameters:')
char = 35
print("Proportional Gain,".ljust(char) + f"Kp = {kp/1000:.3f} N/mm")
print("Integral Gain,".ljust(char) + f"Ki = {ki/1000:.3f} N/mm-s")
print("Derivative Gain,".ljust(char) + f"Kd = {kd/1000:.3f} N-s/mm")
print("Derivative Filter Bandwidth [Hz],".ljust(char) + f"fp = {Wp/(2*np.pi)} Hz")
print("Derivative Filter Bandwidth [rad/s],".ljust(char) + f"fp = {Wp} rad/s")