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). +
+ ++figs/rotating_xy_platform.pdf +
+ ++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} + +
+\begin{equation}
+\begin{bmatrix} F_{um} \\ F_{vm} \end{bmatrix} =
+\frac{1}{\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} - \frac{\Omega^2}{{\omega_0}^2}) (\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 & - (2 \xi \frac{s}{\omega_0} + 1) 2 \frac{\Omega}{\omega_0} \frac{s}{\omega_0} \\
+ (2 \xi \frac{s}{\omega_0} + 1) 2 \frac{\Omega}{\omega_0}\frac{s}{\omega_0} & (\frac{s^2}{{\omega_0}^2} - \frac{\Omega^2}{{\omega_0}^2}) (\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 \\
+\end{bmatrix}
+\begin{bmatrix} F_u \\ F_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: +
+ +
+\begin{equation}
+\begin{bmatrix} \dot{d}_u \\ \dot{d}_v \end{bmatrix} =
+\frac{s \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}
+
+
+
+
+
+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)]; + ++
+
+