Matlab Computation
Table of Contents
Notations:
- \(k\): Actuator’s Stiffness [N/m]
- \(c\): Actuator’s Damping [N/(m/s)]
- \(m\): Payload’s mass [kg]
- \(\omega_0 = \sqrt{\frac{k}{m}}\): Resonance of the (non-rotating) mass-spring system [rad/s]
- \(\Omega = \dot{\theta}\): rotation speed [rad/s]
Mathematical Notation | Matlab | Unit | |
---|---|---|---|
Actuator Stiffness | \(k\) | k |
N/m |
Actuator Damping | \(c\) | c |
N/(m/s) |
1 System Description and Analysis
1.1 System description
The system consists of one 2 degree of freedom translation stage on top of a spindle (figure 1).
The control inputs are the forces applied by the actuators of the translation stage (\(F_u\) and \(F_v\)). As the translation stage is rotating around the Z axis due to the spindle, the forces are applied along \(u\) and \(v\).
The measurement is either the \(x-y\) displacement of the object located on top of the translation stage or the \(u-v\) displacement of the sample with respect to a fixed reference frame.
1.2 Equations
Based on the Figure 1.
\begin{equation} \begin{bmatrix} d_u \\ d_v \end{bmatrix} = \frac{\frac{1}{k}}{\left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right)^2 + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2} \begin{bmatrix} \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} & 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \\ -2 \frac{\Omega}{\omega_0}\frac{s}{\omega_0} & \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \\ \end{bmatrix} \begin{bmatrix} F_u \\ F_v \end{bmatrix} \end{equation}1.3 Numerical Values
k = 2; m = 1; c = 0.05; xi = c/(2*sqrt(k*m)); w0 = sqrt(k/m);
1.4 Campbell Diagram
Compute the poles
wrs = linspace(0, 2, 51); % [rad/s] polesvc = zeros(4, length(wrs)); for i = 1:length(wrs) wr = wrs(i); polei = pole(1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wrs(i)^2)/(w0^2))^2 + (2*wrs(i)*s/(w0^2))^2)); [~, i_sort] = sort(imag(polei)); polesvc(:, i) = polei(i_sort); end
1.5 Simscape Model
open('rotating_frame.slx');
k = 2; m = 1; c = 0.05; xi = c/(2*sqrt(k*m)); w0 = sqrt(k/m); wr = 0.1;
%% Name of the Simulink File mdl = 'rotating_frame'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fu'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Fv'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Tuv'], 1, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Tuv'], 2, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io, 0); %% Input/Output definition G.InputName = {'Fu', 'Fv'}; G.OutputName = {'du', 'dv'};
1.6 Comparison with the model
Ga = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2))^2 + (2*wr*s/(w0^2))^2) * ... [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2), 2*wr*s/(w0^2) ; ... -2*wr*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2)];
figure; bode(G, 'k-', Ga, 'r--')
2 Integral Force Feedback
2.1 Numerical Values
k = 1; m = 1; xi = 0.01; c = 2*xi*sqrt(k*m); w0 = sqrt(k/m); wr = 0.1*w0;
2.2 Equations
The sensed forces are equal to:
\begin{equation} \begin{bmatrix} F_{um} \\ F_{vm} \end{bmatrix} = \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} F_u \\ F_v \end{bmatrix} - (c s + k) \begin{bmatrix} d_u \\ d_v \end{bmatrix} \end{equation}Giffa = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2))^2 + (2*wr*s/(w0^2))^2) * ... [(s^2/w0^2 - wr^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2)) + (2*wr*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*wr*s/(w0^2) ; ... (2*xi*s/w0 + 1)*2*wr*s/(w0^2), (s^2/w0^2 - wr^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2))+ (2*wr*s/(w0^2))^2];
2.3 Poles and Zeros
syms wr w0 xi positive assumealso(w0 > wr) syms x
z = (x^2/w0^2 - wr^2/w0^2)*((x^2)/(w0^2) + 1 - (wr^2)/(w0^2)) + (2*wr*x/(w0^2))^2 == 0 p = ((x^2)/(w0^2) + 1 - (wr^2)/(w0^2))^2 + (2*wr*x/(w0^2))^2 == 0
solve(p, x)
solve(z, x)
The zeros are the roots of:
\begin{equation} \left( \frac{s^2}{{\omega_0}^2} - \frac{\Omega^2}{{\omega_0}^2} \right) \left( \frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1 - \frac{{\Omega}^2}{{\omega_0}^2} \right) + \left( 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \right)^2 = 0 \end{equation}Poles (without damping)
\begin{equation} \left(\begin{array}{c} -w_{0}\,1{}\mathrm{i}-\mathrm{wr}\,1{}\mathrm{i}\\ -w_{0}\,1{}\mathrm{i}+\mathrm{wr}\,1{}\mathrm{i}\\ w_{0}\,1{}\mathrm{i}-\mathrm{wr}\,1{}\mathrm{i}\\ w_{0}\,1{}\mathrm{i}+\mathrm{wr}\,1{}\mathrm{i} \end{array}\right) \end{equation}Zeros (without damping)
\begin{equation} \left(\begin{array}{c} -\sqrt{-\frac{w_{0}\,\sqrt{{w_{0}}^2+8\,{\mathrm{wr}}^2}}{2}-\frac{{w_{0}}^2}{2}-{\mathrm{wr}}^2}\\ -\sqrt{\frac{w_{0}\,\sqrt{{w_{0}}^2+8\,{\mathrm{wr}}^2}}{2}-\frac{{w_{0}}^2}{2}-{\mathrm{wr}}^2}\\ \sqrt{-\frac{w_{0}\,\sqrt{{w_{0}}^2+8\,{\mathrm{wr}}^2}}{2}-\frac{{w_{0}}^2}{2}-{\mathrm{wr}}^2}\\ \sqrt{\frac{w_{0}\,\sqrt{{w_{0}}^2+8\,{\mathrm{wr}}^2}}{2}-\frac{{w_{0}}^2}{2}-{\mathrm{wr}}^2} \end{array}\right) \end{equation}2.4 Simscape Model
%% Name of the Simulink File mdl = 'rotating_frame'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fu'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Fv'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Tuv'], 5, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Tuv'], 6, 'openoutput'); io_i = io_i + 1;
Giff = linearize(mdl, io, 0); %% Input/Output definition Giff.InputName = {'Fu', 'Fv'}; Giff.OutputName = {'Fmu', 'Fmv'};
figure; bode(Giff, Giffa)
2.5 IFF Plant
2.6 Loop Gain
Let’s take \(\Omega = \frac{\omega_0}{10}\).
ws = 0.1*w0; G_iff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (ws^2)/(w0^2))^2 + (2*ws*s/(w0^2))^2) * ... [(s^2/w0^2 - ws^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (ws^2)/(w0^2)) + (2*ws*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*ws*s/(w0^2) ; ... (2*xi*s/w0 + 1)*2*ws*s/(w0^2), (s^2/w0^2 - ws^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (ws^2)/(w0^2))+ (2*ws*s/(w0^2))^2];
2.7 Root Locus
2.8 Modified IFF
2.8.1 Control Law
Let’s take the integral feedback controller as a low pass filter (pseudo integrator):
\begin{equation} K_{\text{IFF}} = \begin{bmatrix} g\frac{\omega_i}{\omega_i + s} & 0 \\ 0 & g\frac{\omega_i}{\omega_i + s} \end{bmatrix} \end{equation}xi = 0.005; w0 = 1; ws = 0.1*w0; G_iff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (ws^2)/(w0^2))^2 + (2*ws*s/(w0^2))^2) * ... [(s^2/w0^2 - ws^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (ws^2)/(w0^2)) + (2*ws*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*ws*s/(w0^2) ; ... (2*xi*s/w0 + 1)*2*ws*s/(w0^2), (s^2/w0^2 - ws^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (ws^2)/(w0^2))+ (2*ws*s/(w0^2))^2];
g = 100; wi = ws; K_iff = (g/(1+s/wi))*eye(2);
2.8.2 Loop Gain
2.8.3 Root Locus
2.8.4 Optimal Gain
The DC gain for Giff is (for \(\Omega < \omega_0\)):
\begin{equation} G_{\text{IFF}}(\omega = 0) = \frac{1}{1 - \frac{{\omega_0}^2}{\Omega^2}} \begin{bmatrix} 1 & 0 \\ 0 & 1 \end{bmatrix} \end{equation}The maximum gain where is system is still stable is
\begin{equation} g_\text{max} = \frac{{\omega_0}^2}{\Omega^2} - 1 \end{equation}Let’s find the gain that maximize the simultaneous damping of the two modes.
K_opt = (opt_gain/(1+s/wi))*eye(2); G_cl = feedback(G_iff, K_opt);
3 Direct Velocity Feedback
3.1 Equations
The sensed relative velocity are equal to:
3.2 Numerical Values
k = 1; m = 1; c = 0.05; xi = c/(2*sqrt(k*m)); w0 = sqrt(k/m);
Ga = (s/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2))^2 + (2*wr*s/(w0^2))^2) * ... [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2), 2*wr*s/(w0^2) ; ... -2*wr*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2)];
3.3 Simscape Model
%% Name of the Simulink File mdl = 'rotating_frame'; %% Input/Output definition clear io; io_i = 1; io(io_i) = linio([mdl, '/Fu'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Fv'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Tuv'], 3, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Tuv'], 4, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io, 0); %% Input/Output definition G.InputName = {'Fu', 'Fv'}; G.OutputName = {'Vu', 'Vv'};
3.4 DVF Plant
figure; bode(G, Ga)
3.5 Loop Gain
Let’s take \(\Omega = \frac{\omega_0}{10}\).
ws = 0.1*w0; G_dvf = (s/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2))^2 + (2*wr*s/(w0^2))^2) * ... [(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2), 2*wr*s/(w0^2) ; ... -2*wr*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (wr^2)/(w0^2)];