tdehaeze/dehaeze20_activ_dampin_rotat_platf_integ_force_feedb
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Add tangled matlab scripts

Browse Source
This commit is contained in:
Thomas Dehaeze
2020-06-23 18:27:33 +02:00
parent 915cd53093
commit 249e7872d0
11 changed files with 1609 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
matlab/figs-inkscape
View File

@@ -1 +1 @@
../inkscape/figs
../inkscape

BIN
matlab/figs/mod_iff_damping_wi.pdf
View File

Binary file not shown.

BIN
matlab/figs/root_locus_wi_modified_iff.pdf
View File

Binary file not shown.

BIN
matlab/figs/root_locus_wi_modified_iff.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 71 KiB

After

Width:  |  Height:  |  Size: 70 KiB

50
matlab/index.org
View File

@@ -14,7 +14,6 @@
#+HTML_HEAD: <script src="../js/readtheorg.js"></script>
#+PROPERTY: header-args:matlab :session *MATLAB*
#+PROPERTY: header-args:matlab+ :tangle matlab/comp_filters_design.m
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :results none
@@ -35,6 +34,9 @@
- Section [[sec:notations]]
* System Description and Analysis
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s1_system_description.m
:END:
<<sec:system_description>>
** Introduction :ignore:
@@ -47,7 +49,7 @@
<<matlab-init>>
#+end_src
#+begin_src matlab
#+begin_src matlab :tangle no
addpath('./matlab/');
addpath('./src/');
#+end_src
@@ -75,13 +77,11 @@ Based on the Figure [[fig:rotating_xy_platform]], the equations of motions are:
Where $\bm{G}_d$ is a $2 \times 2$ transfer function matrix.
\begin{equation}
\begin{bmatrix} d_u \\ d_v \end{bmatrix} =
\frac{1}{k} \frac{1}{G_{dp}}
\bm{G}_d = \frac{1}{k} \frac{1}{G_{dp}}
\begin{bmatrix}
G_{dz} & G_{dc} \\
-G_{dc} & G_{dz}
\end{bmatrix}
\begin{bmatrix} F_u \\ F_v \end{bmatrix}
\end{equation}
With:
\begin{align}
@@ -391,6 +391,9 @@ They are compared in Figure [[fig:plant_compare_rotating_speed]].
#+end_src
* Problem with pure Integral Force Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s2_iff_pure_int.m
:END:
<<sec:iff_pure_int>>
** Introduction :ignore:
@@ -406,7 +409,7 @@ They are compared in Figure [[fig:plant_compare_rotating_speed]].
<<matlab-init>>
#+end_src
#+begin_src matlab
#+begin_src matlab :tangle no
addpath('./matlab/');
addpath('./src/');
#+end_src
@@ -714,6 +717,9 @@ It is shown that for non-null rotating speed, one pole is bound to the right-hal
#+end_src
* Integral Force Feedback with an High Pass Filter
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s3_iff_hpf.m
:END:
<<sec:iff_pseudo_int>>
** Introduction :ignore:
@@ -728,7 +734,7 @@ It is shown that for non-null rotating speed, one pole is bound to the right-hal
<<matlab-init>>
#+end_src
#+begin_src matlab
#+begin_src matlab :tangle no
addpath('./matlab/');
addpath('./src/');
#+end_src
@@ -983,7 +989,7 @@ In order to visualize the effect of $\omega_i$ on the attainable damping, the Ro
for wi_i = 1:length(wis)
set(gca,'ColorOrderIndex',wi_i);
wi = wis(wi_i);
L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\Omega_i = %.2f \\omega_0$', wi./w0));
L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0));
for g = gains
clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
set(gca,'ColorOrderIndex',wi_i);
@@ -1030,8 +1036,8 @@ In order to visualize the effect of $\omega_i$ on the attainable damping, the Ro
#+RESULTS:
[[file:figs/root_locus_wi_modified_iff.png]]
#+begin_src matlab :exports none
gains = logspace(-2, 4, 100);
#+begin_src matlab :exports none :tangle no
gains = logspace(-2, 4, 500);
poles_iff_hpf = rootLocusPolesSorted(Giff, 1/(s + wi)*eye(2), gains, 'd_max', 1e-4);
@@ -1043,7 +1049,7 @@ In order to visualize the effect of $\omega_i$ on the attainable damping, the Ro
wi = wis(wi_i);
set(gca,'ColorOrderIndex',wi_i);
L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\Omega_i = %.2f \\omega_0$', wi./w0));
L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0));
poles = rootLocusPolesSorted(Giff, 1/(s + wi)*eye(2), gains, 'd_max', 1e-4);
for p_i = 1:size(poles, 2)
@@ -1159,10 +1165,13 @@ To find the optimum, the gain that maximize the simultaneous damping of the mode
[[file:figs/mod_iff_damping_wi.png]]
#+begin_src matlab :tangle no :exports none :results none
exportFig('figs-inkscape/root_locus_wi_modified_iff.pdf', 'width', 'wide', 'height', 'normal', 'png', false, 'pdf', false, 'svg', true);
exportFig('figs-inkscape/mod_iff_damping_wi.pdf', 'width', 'wide', 'height', 'normal', 'png', false, 'pdf', false, 'svg', true);
#+end_src
* IFF with a stiffness in parallel with the force sensor
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s4_iff_kp.m
:END:
<<sec:iff_parallel_stiffness>>
** Introduction :ignore:
@@ -1175,7 +1184,7 @@ To find the optimum, the gain that maximize the simultaneous damping of the mode
<<matlab-init>>
#+end_src
#+begin_src matlab
#+begin_src matlab :tangle no
addpath('./matlab/');
addpath('./src/');
#+end_src
@@ -1873,6 +1882,10 @@ Let's take $k_p = 5 m \Omega^2$ and find the optimal IFF control gain $g$ such t
#+end_src
* Direct Velocity Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s5_dvf.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:dvf>>
** Introduction :ignore:
@@ -1885,7 +1898,7 @@ Let's take $k_p = 5 m \Omega^2$ and find the optimal IFF control gain $g$ such t
<<matlab-init>>
#+end_src
#+begin_src matlab
#+begin_src matlab :tangle no
addpath('./matlab/');
addpath('./src/');
#+end_src
@@ -2097,7 +2110,7 @@ It is shown that for rotating speed $\Omega < \omega_0$, the closed loop system
#+RESULTS:
[[file:figs/root_locus_dvf.png]]
#+begin_src matlab :exports none
#+begin_src matlab :exports none :tangle no
gains = logspace(-2, 1, 1000);
figure;
@@ -2137,6 +2150,9 @@ It is shown that for rotating speed $\Omega < \omega_0$, the closed loop system
#+end_src
* Comparison
:PROPERTIES:
:header-args:matlab+: :tangle matlab/s6_act_damp_comparison.m
:END:
<<sec:comparison>>
** Introduction :ignore:
@@ -2150,7 +2166,7 @@ It is shown that for rotating speed $\Omega < \omega_0$, the closed loop system
<<matlab-init>>
#+end_src
#+begin_src matlab
#+begin_src matlab :tangle no
addpath('./matlab/');
addpath('./src/');
#+end_src
@@ -2658,7 +2674,7 @@ The obtained damping ratio and control are shown below.
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(Ciff(1,1), freqs))), ...
'DisplayName', 'IFF + LPF')
'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(Ciff_kp(1,1), freqs))), ...
'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(Cdvf(1,1), freqs))), ...

219
matlab/matlab/s1_system_description.m Normal file
View File

@@ -0,0 +1,219 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Numerical Values
% Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s]
% Campbell Diagram
% The Campbell Diagram displays the evolution of the real and imaginary parts of the system as a function of the rotating speed.
% It is shown in Figure [[fig:campbell_diagram]], and one can see that the system becomes unstable for $\Omega > \omega_0$ (the real part of one of the poles becomes positive).
Ws = linspace(0, 2, 51); % Vector of considered rotation speed [rad/s]
p_ws = zeros(4, length(Ws));
for W_i = 1:length(Ws)
W = Ws(W_i);
pole_G = pole(1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2));
[~, i_sort] = sort(imag(pole_G));
p_ws(:, W_i) = pole_G(i_sort);
end
clear pole_G;
figure;
ax1 = subplot(1,2,1);
hold on;
for p_i = 1:size(p_ws, 1)
plot(Ws, real(p_ws(p_i, :)), 'k-')
end
plot(Ws, zeros(size(Ws)), 'k--')
hold off;
xlabel('Rotation Frequency [rad/s]'); ylabel('Real Part');
ax2 = subplot(1,2,2);
hold on;
for p_i = 1:size(p_ws, 1)
plot(Ws, imag(p_ws(p_i, :)), 'k-')
plot(Ws, -imag(p_ws(p_i, :)), 'k-')
end
hold off;
xlabel('Rotation Frequency [rad/s]'); ylabel('Imaginary Part');
% Simscape Model
% Define the rotating speed for the Simscape Model.
W = 0.1; % Rotation Speed [rad/s]
Kiff = tf(zeros(2));
Kdvf = tf(zeros(2));
kp = 0; % Parallel Stiffness [N/m]
cp = 0; % Parallel Damping [N/(m/s)]
open('rotating_frame.slx');
% The transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is identified from the Simscape model.
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/G'], 3, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io, 0);
%% Input/Output definition
G.InputName = {'Fu', 'Fv'};
G.OutputName = {'du', 'dv'};
% The same transfer function from $[F_u, F_v]$ to $[d_u, d_v]$ is written down from the analytical model.
Gth = (1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
-2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];
% Both transfer functions are compared in Figure [[fig:plant_simscape_analytical]] and are found to perfectly match.
freqs = logspace(-1, 1, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G(1,1), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Gth(1,1), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
title('$d_u/F_u$, $d_v/F_v$');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,1), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,1), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G(1,2), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Gth(1,2), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
title('$d_u/F_v$, $d_v/F_u$');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G(1,2), freqs))), '-', ...
'DisplayName', 'Simscape')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gth(1,2), freqs))), '--', ...
'DisplayName', 'Analytical')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
legend('location', 'southwest');
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y');
% Effect of the rotation speed
% The transfer functions from $[F_u, F_v]$ to $[d_u, d_v]$ are identified for the following rotating speeds.
Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s]
Gs = {zeros(2, 2, length(Ws))};
for W_i = 1:length(Ws)
W = Ws(W_i);
Gs(:, :, W_i) = {(1/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
-2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)]};
end
% They are compared in Figure [[fig:plant_compare_rotating_speed]].
freqs = logspace(-2, 1, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for W_i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{W_i}(1,1), freqs))), ...
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
legend('location', 'southwest');
title('$d_u/F_u$, $d_v/F_v$');
ax3 = subplot(2, 2, 3);
hold on;
for W_i = 1:length(Ws)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(1,1), freqs))))
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
ax2 = subplot(2, 2, 2);
hold on;
for W_i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gs{W_i}(2,1), freqs))))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [m/N]');
title('$d_u/F_v$, $d_v/F_u$');
ax4 = subplot(2, 2, 4);
hold on;
for W_i = 1:length(Ws)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs{W_i}(1,1), freqs))))
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y');

193
matlab/matlab/s2_iff_pure_int.m Normal file
View File

@@ -0,0 +1,193 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Plant Parameters
% Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m]
cp = 0; % [N/(m/s)]
% Simscape Model
% The rotation speed is set to $\Omega = 0.1 \omega_0$.
W = 0.1*w0; % [rad/s]
Kiff = tf(zeros(2));
Kdvf = tf(zeros(2));
open('rotating_frame.slx');
% And the transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is identified using the Simscape model.
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1;
Giff = linearize(mdl, io, 0);
%% Input/Output definition
Giff.InputName = {'Fu', 'Fv'};
Giff.OutputName = {'fu', 'fv'};
% Comparison of the Analytical Model and the Simscape Model
% The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model.
Giff_th = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
% The two are compared in Figure [[fig:plant_iff_comp_simscape_analytical]] and found to perfectly match.
freqs = logspace(-1, 1, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_u$, $f_v/F_v$');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_v$, $f_v/F_u$');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-', ...
'DisplayName', 'Simscape')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--', ...
'DisplayName', 'Analytical')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y');
% Effect of the rotation speed
% The transfer functions from $[F_u, F_v]$ to $[f_u, f_v]$ are identified for the following rotating speeds.
Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s]
Gsiff = {zeros(2, 2, length(Ws))};
for W_i = 1:length(Ws)
W = Ws(W_i);
Gsiff(:, :, W_i) = {1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2]};
end
% The obtained transfer functions are shown in Figure [[fig:plant_iff_compare_rotating_speed]].
freqs = logspace(-2, 1, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for W_i = 1:length(Ws)
plot(freqs, abs(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))), ...
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0))
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
legend('location', 'southeast');
ax2 = subplot(2, 1, 2);
hold on;
for W_i = 1:length(Ws)
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsiff{W_i}(1,1), freqs))))
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% Decentralized Integral Force Feedback
% The decentralized IFF controller consists of pure integrators:
% \begin{equation}
% \bm{K}_{\text{IFF}}(s) = \frac{g}{s} \begin{bmatrix}
% 1 & 0 \\
% 0 & 1
% \end{bmatrix}
% \end{equation}
% The Root Locus (evolution of the poles of the closed loop system in the complex plane as a function of $g$) is shown in Figure [[fig:root_locus_pure_iff]].
% It is shown that for non-null rotating speed, one pole is bound to the right-half plane, and thus the closed loop system is unstable.
figure;
gains = logspace(-2, 4, 100);
hold on;
for W_i = 1:length(Ws)
set(gca,'ColorOrderIndex',W_i);
plot(real(pole(Gsiff{W_i})), imag(pole(Gsiff{W_i})), 'x', ...
'DisplayName', sprintf('$\\Omega = %.1f \\omega_0 $', Ws(W_i)/w0));
set(gca,'ColorOrderIndex',W_i);
plot(real(tzero(Gsiff{W_i})), imag(tzero(Gsiff{W_i})), 'o', ...
'HandleVisibility', 'off');
for g = gains
set(gca,'ColorOrderIndex',W_i);
cl_poles = pole(feedback(Gsiff{W_i}, g/s*eye(2)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
end
hold off;
axis square;
xlim([-2, 0.5]); ylim([0, 2.5]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');

251
matlab/matlab/s3_iff_hpf.m Normal file
View File

@@ -0,0 +1,251 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Plant Parameters
% Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m]
cp = 0; % [N/(m/s)]
% Modified Integral Force Feedback Controller
% Let's modify the initial Integral Force Feedback Controller ; instead of using pure integrators, pseudo integrators (i.e. low pass filters) are used:
% \begin{equation}
% K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix}
% 1 & 0 \\
% 0 & 1
% \end{bmatrix}
% \end{equation}
% where $\omega_i$ characterize down to which frequency the signal is integrated.
% Let's arbitrary choose the following control parameters:
g = 2;
wi = 0.1*w0;
Kiff = (g/(wi+s))*eye(2);
% And the following rotating speed.
W = 0.1*w0;
Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
% The obtained Loop Gain is shown in Figure [[fig:loop_gain_modified_iff]].
freqs = logspace(-2, 1, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1)*(g/s), freqs))))
plot(freqs, abs(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Loop Gain');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*(g/s), freqs))), ...
'DisplayName', 'IFF')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1)*Kiff(1,1), freqs))), ...
'DisplayName', 'IFF + HPF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
legend('location', 'southwest');
hold off;
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% Root Locus
% As shown in the Root Locus plot (Figure [[fig:root_locus_modified_iff]]), for some value of the gain, the system remains stable.
figure;
gains = logspace(-2, 4, 100);
ax1 = subplot(1, 2, 1);
hold on;
% Pure Integrator
set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(Giff, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',1);
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
end
% Modified IFF
set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', 'IFF + HPF');
set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
for g = gains
clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
set(gca,'ColorOrderIndex',2);
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
end
hold off;
axis square;
xlim([-2, 0.5]); ylim([-1.25, 1.25]);
legend('location', 'northwest');
xlabel('Real Part'); ylabel('Imaginary Part');
ax2 = subplot(1, 2, 2);
hold on;
% Pure Integrator
set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o');
for g = gains
clpoles = pole(feedback(Giff, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',1);
plot(real(clpoles), imag(clpoles), '.');
end
% Modified IFF
set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff)), imag(pole(Giff)), 'x');
set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o');
for g = gains
clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
set(gca,'ColorOrderIndex',2);
plot(real(clpoles), imag(clpoles), '.');
end
hold off;
axis square;
xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
xlabel('Real Part'); ylabel('Imaginary Part');
% What is the optimal $\omega_i$ and $g$?
% In order to visualize the effect of $\omega_i$ on the attainable damping, the Root Locus is displayed in Figure [[fig:root_locus_wi_modified_iff]] for the following $\omega_i$:
wis = [0.01, 0.1, 0.5, 1]*w0; % [rad/s]
figure;
gains = logspace(-2, 4, 100);
ax1 = subplot(1, 2, 1);
hold on;
for wi_i = 1:length(wis)
set(gca,'ColorOrderIndex',wi_i);
wi = wis(wi_i);
L(wi_i) = plot(nan, nan, '.', 'DisplayName', sprintf('$\\omega_i = %.2f \\omega_0$', wi./w0));
for g = gains
clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
set(gca,'ColorOrderIndex',wi_i);
plot(real(clpoles), imag(clpoles), '.');
end
end
plot(real(pole(Giff)), imag(pole(Giff)), 'kx');
plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko');
hold off;
axis square;
xlim([-2.3, 0.1]); ylim([-1.2, 1.2]);
xticks([-2:1:2]); yticks([-2:1:2]);
legend(L, 'location', 'northwest');
xlabel('Real Part'); ylabel('Imaginary Part');
clear L
ax2 = subplot(1, 2, 2);
hold on;
for wi_i = 1:length(wis)
set(gca,'ColorOrderIndex', wi_i);
wi = wis(wi_i);
for g = gains
clpoles = pole(feedback(Giff, (g/(wi+s))*eye(2)));
set(gca,'ColorOrderIndex', wi_i);
plot(real(clpoles), imag(clpoles), '.');
end
end
plot(real(pole(Giff)), imag(pole(Giff)), 'kx');
plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko');
hold off;
axis square;
xlim([-0.2, 0.1]); ylim([-0.15, 0.15]);
xticks([-0.2:0.1:0.1]); yticks([-0.2:0.1:0.2]);
xlabel('Real Part'); ylabel('Imaginary Part');
% For the controller
% \begin{equation}
% K_{\text{IFF}}(s) = g\frac{1}{\omega_i + s} \begin{bmatrix}
% 1 & 0 \\
% 0 & 1
% \end{bmatrix}
% \end{equation}
% The gain at which the system becomes unstable is
% \begin{equation}
% g_\text{max} = \omega_i \left( \frac{{\omega_0}^2}{\Omega^2} - 1 \right) \label{eq:iff_gmax}
% \end{equation}
% While it seems that small $\omega_i$ do allow more damping to be added to the system (Figure [[fig:root_locus_wi_modified_iff]]), the control gains may be limited to small values due to eqref:eq:iff_gmax thus reducing the attainable damping.
% There must be an optimum for $\omega_i$.
% To find the optimum, the gain that maximize the simultaneous damping of the mode is identified for a wide range of $\omega_i$ (Figure [[fig:mod_iff_damping_wi]]).
wis = logspace(-2, 1, 31)*w0; % [rad/s]
opt_zeta = zeros(1, length(wis)); % Optimal simultaneous damping
opt_gain = zeros(1, length(wis)); % Corresponding optimal gain
for wi_i = 1:length(wis)
wi = wis(wi_i);
gains = linspace(0, (w0^2/W^2 - 1)*wi, 100);
for g = gains
Kiff = (g/(wi+s))*eye(2);
[w, zeta] = damp(minreal(feedback(Giff, Kiff)));
if min(zeta) > opt_zeta(wi_i) && all(zeta > 0)
opt_zeta(wi_i) = min(zeta);
opt_gain(wi_i) = g;
end
end
end
figure;
yyaxis left
plot(wis, opt_zeta, '-o', 'DisplayName', '$\xi_{cl}$');
set(gca, 'YScale', 'lin');
ylim([0,1]);
ylabel('Attainable Damping Ratio $\xi$');
yyaxis right
hold on;
plot(wis, opt_gain, '-x', 'DisplayName', '$g_{opt}$');
plot(wis, wis*((w0/W)^2 - 1), '--', 'DisplayName', '$g_{max}$');
set(gca, 'YScale', 'lin');
ylim([0,10]);
ylabel('Controller gain $g$');
xlabel('$\omega_i/\omega_0$');
set(gca, 'XScale', 'log');
legend('location', 'northeast');

389
matlab/matlab/s4_iff_kp.m Normal file
View File

@@ -0,0 +1,389 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Plant Parameters
% Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m]
cp = 0; % [N/(m/s)]
% Comparison of the Analytical Model and the Simscape Model
% The same transfer function from $[F_u, F_v]$ to $[f_u, f_v]$ is written down from the analytical model.
W = 0.1*w0; % [rad/s]
kp = 1.5*m*W^2;
cp = 0;
Kiff = tf(zeros(2));
Kdvf = tf(zeros(2));
open('rotating_frame.slx');
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/G'], 2, 'openoutput'); io_i = io_i + 1;
Giff = linearize(mdl, io, 0);
%% Input/Output definition
Giff.InputName = {'Fu', 'Fv'};
Giff.OutputName = {'fu', 'fv'};
w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m));
Giff_th = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];
Giff_th.InputName = {'Fu', 'Fv'};
Giff_th.OutputName = {'fu', 'fv'};
freqs = logspace(-1, 1, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_u$, $f_v/F_v$');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,1), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,2), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Giff_th(1,2), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
title('$f_u/F_v$, $f_v/F_u$');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,2), freqs))), '-', ...
'DisplayName', 'Simscape')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_th(1,2), freqs))), '--', ...
'DisplayName', 'Analytical')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
legend('location', 'northeast');
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y');
% Effect of the parallel stiffness on the IFF plant
% The rotation speed is set to $\Omega = 0.1 \omega_0$.
W = 0.1*w0; % [rad/s]
% And the IFF plant (transfer function from $[F_u, F_v]$ to $[f_u, f_v]$) is identified in three different cases:
% - without parallel stiffness
% - with a small parallel stiffness $k_p < m \Omega^2$
% - with a large parallel stiffness $k_p > m \Omega^2$
% The results are shown in Figure [[fig:plant_iff_kp]].
% One can see that for $k_p > m \Omega^2$, the systems shows alternating complex conjugate poles and zeros.
kp = 0;
cp = 0;
w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m));
Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
kp = 0.5*m*W^2;
cp = 0;
w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m));
Giff_s = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
kp = 1.5*m*W^2;
cp = 0;
w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m));
Giff_l = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2];
freqs = logspace(-2, 1, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs))), 'k-')
plot(freqs, abs(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--')
plot(freqs, abs(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [N/N]');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(1,1), freqs))), 'k-', ...
'DisplayName', '$k_p = 0$')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_s(1,1), freqs))), 'k--', ...
'DisplayName', '$k_p < m\Omega^2$')
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff_l(1,1), freqs))), 'k:', ...
'DisplayName', '$k_p > m\Omega^2$')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
legend('location', 'southwest');
linkaxes([ax1,ax2],'x');
xlim([freqs(1), freqs(end)]);
% IFF when adding a spring in parallel
% In Figure [[fig:root_locus_iff_kp]] is displayed the Root Locus in the three considered cases with
% \begin{equation}
% K_{\text{IFF}} = \frac{g}{s} \begin{bmatrix}
% 1 & 0 \\
% 0 & 1
% \end{bmatrix}
% \end{equation}
% One can see that for $k_p > m \Omega^2$, the root locus stays in the left half of the complex plane and thus the control system is unconditionally stable.
% Thus, decentralized IFF controller with pure integrators can be used if:
% \begin{equation}
% k_{p} > m \Omega^2
% \end{equation}
figure;
gains = logspace(-2, 2, 100);
subplot(1,2,1);
hold on;
set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', ...
'DisplayName', '$k_p = 0$');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ...
'HandleVisibility', 'off');
for g = gains
cl_poles = pole(feedback(Giff, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff_s)), imag(pole(Giff_s)), 'x', ...
'DisplayName', '$k_p < m\Omega^2$');
set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff_s)), imag(tzero(Giff_s)), 'o', ...
'HandleVisibility', 'off');
for g = gains
cl_poles = pole(feedback(Giff_s, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',3);
plot(real(pole(Giff_l)), imag(pole(Giff_l)), 'x', ...
'DisplayName', '$k_p > m\Omega^2$');
set(gca,'ColorOrderIndex',3);
plot(real(tzero(Giff_l)), imag(tzero(Giff_l)), 'o', ...
'HandleVisibility', 'off');
for g = gains
set(gca,'ColorOrderIndex',3);
cl_poles = pole(feedback(Giff_l, (g/s)*eye(2)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
hold off;
axis square;
xlim([-1, 0.2]); ylim([0, 1.2]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
subplot(1,2,2);
hold on;
set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o');
for g = gains
cl_poles = pole(feedback(Giff, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
end
set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff_s)), imag(pole(Giff_s)), 'x');
set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff_s)), imag(tzero(Giff_s)), 'o');
for g = gains
cl_poles = pole(feedback(Giff_s, (g/s)*eye(2)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
end
set(gca,'ColorOrderIndex',3);
plot(real(pole(Giff_l)), imag(pole(Giff_l)), 'x');
set(gca,'ColorOrderIndex',3);
plot(real(tzero(Giff_l)), imag(tzero(Giff_l)), 'o');
for g = gains
set(gca,'ColorOrderIndex',3);
cl_poles = pole(feedback(Giff_l, (g/s)*eye(2)));
plot(real(cl_poles), imag(cl_poles), '.');
end
hold off;
axis square;
xlim([-0.04, 0.06]); ylim([0, 0.1]);
xlabel('Real Part'); ylabel('Imaginary Part');
% Effect of $k_p$ on the attainable damping
% However, having large values of $k_p$ may:
% - decrease the actuator force authority
% - decrease the attainable damping
% To study the second point, Root Locus plots for the following values of $k_p$ are shown in Figure [[fig:root_locus_iff_kps]].
kps = [2, 20, 40]*m*W^2;
% It is shown that large values of $k_p$ decreases the attainable damping.
figure;
gains = logspace(-2, 4, 500);
hold on;
for kp_i = 1:length(kps)
kp = kps(kp_i);
w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m));
Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];
set(gca,'ColorOrderIndex',kp_i);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', ...
'DisplayName', sprintf('$k_p = %.1f m \\Omega^2$', kp/(m*W^2)));
set(gca,'ColorOrderIndex',kp_i);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ...
'HandleVisibility', 'off');
for g = gains
Kiffa = (g/s)*eye(2);
cl_poles = pole(feedback(Giff, Kiffa));
set(gca,'ColorOrderIndex',kp_i);
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
end
hold off;
axis square;
xlim([-1.2, 0.2]); ylim([0, 1.4]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
% Optimal Gain
% Let's take $k_p = 5 m \Omega^2$ and find the optimal IFF control gain $g$ such that maximum damping are added to the poles of the closed loop system.
kp = 5*m*W^2;
cp = 0.01;
w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m));
Giff = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];
opt_zeta = 0;
opt_gain = 0;
gains = logspace(-2, 4, 1000);
for g = gains
Kiff = (g/s)*eye(2);
[w, zeta] = damp(minreal(feedback(Giff, Kiff)));
if min(zeta) > opt_zeta && all(zeta > 0)
opt_zeta = min(zeta);
opt_gain = min(g);
end
end
figure;
gains = logspace(-2, 4, 1000);
hold on;
plot(real(pole(Giff)), imag(pole(Giff)), 'kx');
plot(real(tzero(Giff)), imag(tzero(Giff)), 'ko');
for g = gains
clpoles = pole(minreal(feedback(Giff, (g/s)*eye(2))));
plot(real(clpoles), imag(clpoles), 'k.');
end
% Optimal Gain
clpoles = pole(minreal(feedback(Giff, (opt_gain/s)*eye(2))));
set(gca,'ColorOrderIndex',1);
plot(real(clpoles), imag(clpoles), 'x');
for clpole = clpoles'
set(gca,'ColorOrderIndex',1);
plot([0, real(clpole)], [0, imag(clpole)], '-', 'LineWidth', 1);
end
hold off;
axis square;
xlim([-1.2, 0.2]); ylim([0, 1.4]);
xlabel('Real Part'); ylabel('Imaginary Part');

159
matlab/matlab/s5_dvf.m Normal file
View File

@@ -0,0 +1,159 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Plant Parameters
% Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m]
cp = 0; % [N/(m/s)]
% Comparison of the Analytical Model and the Simscape Model
% The rotating speed is set to $\Omega = 0.1 \omega_0$.
W = 0.1*w0;
Kiff = tf(zeros(2));
Kdvf = tf(zeros(2));
open('rotating_frame.slx');
% And the transfer function from $[F_u, F_v]$ to $[v_u, v_v]$ is identified using the Simscape model.
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/K'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/G'], 1, 'openoutput'); io_i = io_i + 1;
Gdvf = linearize(mdl, io, 0);
%% Input/Output definition
Gdvf.InputName = {'Fu', 'Fv'};
Gdvf.OutputName = {'Vu', 'Vv'};
% The same transfer function from $[F_u, F_v]$ to $[v_u, v_v]$ is written down from the analytical model.
Gdvf_th = (s/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
-2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];
Gdvf_th.InputName = {'Fu', 'Fv'};
Gdvf_th.OutputName = {'vu', 'vv'};
% The two are compared in Figure [[fig:plant_iff_comp_simscape_analytical]] and found to perfectly match.
freqs = logspace(-1, 1, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Gdvf_th(1,1), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [$\frac{m/s}{N}$]');
title('$v_u/F_u$, $v_v/F_v$');
ax3 = subplot(2, 2, 3);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(1,1), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_th(1,1), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
ax2 = subplot(2, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Gdvf(1,2), freqs))), '-')
plot(freqs, abs(squeeze(freqresp(Gdvf_th(1,2), freqs))), '--')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
set(gca, 'XTickLabel',[]); ylabel('Magnitude [$\frac{m/s}{N}$]');
title('$v_u/F_v$, $v_v/F_u$');
ax4 = subplot(2, 2, 4);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(1,2), freqs))), '-')
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf_th(1,2), freqs))), '--')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [rad/s]'); ylabel('Phase [deg]');
yticks(-180:90:180);
ylim([-180 180]);
hold off;
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([freqs(1), freqs(end)]);
linkaxes([ax1,ax2],'y');
% Root Locus
% The Decentralized Direct Velocity Feedback controller consist of a pure gain on the diagonal:
% \begin{equation}
% K_{\text{DVF}}(s) = g \begin{bmatrix}
% 1 & 0 \\
% 0 & 1
% \end{bmatrix}
% \end{equation}
% The corresponding Root Locus plots for the following rotating speeds are shown in Figure [[fig:root_locus_dvf]].
Ws = [0, 0.2, 0.7, 1.1]*w0; % Rotating Speeds [rad/s]
% It is shown that for rotating speed $\Omega < \omega_0$, the closed loop system is unconditionally stable and arbitrary damping can be added to the poles.
gains = logspace(-2, 1, 100);
figure;
hold on;
for W_i = 1:length(Ws)
W = Ws(W_i);
Gdvf = (s/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
-2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];
set(gca,'ColorOrderIndex',W_i);
plot(real(pole(Gdvf)), imag(pole(Gdvf)), 'x', ...
'DisplayName', sprintf('$\\Omega = %.2f \\omega_0 $', W/w0));
set(gca,'ColorOrderIndex',W_i);
plot(real(tzero(Gdvf)), imag(tzero(Gdvf)), 'o', ...
'HandleVisibility', 'off');
for g = gains
set(gca,'ColorOrderIndex',W_i);
cl_poles = pole(feedback(Gdvf, g*eye(2)));
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
end
hold off;
axis square;
xlim([-2, 0.5]); ylim([0, 2.5]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');

364
matlab/matlab/s6_act_damp_comparison.m Normal file
View File

@@ -0,0 +1,364 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Plant Parameters
% Let's define initial values for the model.
k = 1; % Actuator Stiffness [N/m]
c = 0.05; % Actuator Damping [N/(m/s)]
m = 1; % Payload mass [kg]
xi = c/(2*sqrt(k*m));
w0 = sqrt(k/m); % [rad/s]
kp = 0; % [N/m]
cp = 0; % [N/(m/s)]
% The rotating speed is set to $\Omega = 0.1 \omega_0$.
W = 0.1*w0;
% Root Locus
% IFF with High Pass Filter
wi = 0.1*w0; % [rad/s]
Giff = 1/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)) + (2*W*s/(w0^2))^2, - (2*xi*s/w0 + 1)*2*W*s/(w0^2) ; ...
(2*xi*s/w0 + 1)*2*W*s/(w0^2), (s^2/w0^2 - W^2/w0^2)*((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))+ (2*W*s/(w0^2))^2];
% IFF With parallel Stiffness
kp = 5*m*W^2;
k = k - kp;
w0p = sqrt((k + kp)/m);
xip = c/(2*sqrt((k+kp)*m));
Giff_kp = 1/( (s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2)^2 + (2*(s/w0p)*(W/w0p))^2 ) * [ ...
(s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2, -(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p));
(2*xip*s/w0p + k/(k + kp))*(2*(s/w0p)*(W/w0p)), (s^2/w0p^2 + kp/(k + kp) - W^2/w0p^2)*(s^2/w0p^2 + 2*xip*s/w0p + 1 - W^2/w0p^2) + (2*(s/w0p)*(W/w0p))^2 ];
k = k + kp;
% DVF
Gdvf = (s/k)/(((s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2))^2 + (2*W*s/(w0^2))^2) * ...
[(s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2), 2*W*s/(w0^2) ; ...
-2*W*s/(w0^2), (s^2)/(w0^2) + 2*xi*s/w0 + 1 - (W^2)/(w0^2)];
figure;
gains = logspace(-2, 2, 100);
hold on;
set(gca,'ColorOrderIndex',1);
plot(real(pole(Giff)), imag(pole(Giff)), 'x', ...
'DisplayName', 'IFF + LFP');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', ...
'HandleVisibility', 'off');
for g = gains
Kiff = (g/(wi + s))*eye(2);
cl_poles = pole(feedback(Giff, Kiff));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',2);
plot(real(pole(Giff_kp)), imag(pole(Giff_kp)), 'x', ...
'DisplayName', 'IFF + $k_p$');
set(gca,'ColorOrderIndex',2);
plot(real(tzero(Giff_kp)), imag(tzero(Giff_kp)), 'o', ...
'HandleVisibility', 'off');
for g = gains
Kiffa = (g/s)*eye(2);
cl_poles = pole(feedback(Giff_kp, Kiffa));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',3);
plot(real(pole(Gdvf)), imag(pole(Gdvf)), 'x', ...
'DisplayName', 'DVF');
set(gca,'ColorOrderIndex',3);
plot(real(tzero(Gdvf)), imag(tzero(Gdvf)), 'o', ...
'HandleVisibility', 'off');
for g = gains
Kdvf = g*eye(2);
cl_poles = pole(feedback(Gdvf, Kdvf));
set(gca,'ColorOrderIndex',3);
plot(real(cl_poles), imag(cl_poles), '.', ...
'HandleVisibility', 'off');
end
hold off;
axis square;
xlim([-1.2, 0.05]); ylim([0, 1.25]);
xlabel('Real Part'); ylabel('Imaginary Part');
legend('location', 'northwest');
% Controllers - Optimal Gains
% In order to compare to three considered Active Damping techniques, gains that yield maximum damping of all the modes are computed for each case.
%% IFF with pseudo integrators
gains = linspace(0, (w0^2/W^2 - 1)*wi, 100);
opt_zeta_iff = 0;
opt_gain_iff = 0;
for g = gains
Kiff = (g/(wi+s))*eye(2);
[w, zeta] = damp(minreal(feedback(Giff, Kiff)));
if min(zeta) > opt_zeta_iff && all(zeta > 0)
opt_zeta_iff = min(zeta);
opt_gain_iff = g;
end
end
%% IFF with Parallel Stiffness
gains = logspace(-2, 4, 100);
opt_zeta_kp = 0;
opt_gain_kp = 0;
for g = gains
Kiff = g/s*eye(2);
[w, zeta] = damp(minreal(feedback(Giff_kp, Kiff)));
if min(zeta) > opt_zeta_kp && all(zeta > 0)
opt_zeta_kp = min(zeta);
opt_gain_kp = g;
end
end
%% Direct Velocity Feedback
gains = logspace(0, 2, 100);
opt_zeta_dvf = 0;
opt_gain_dvf = 0;
for g = gains
Kdvf = g*eye(2);
[w, zeta] = damp(minreal(feedback(Gdvf, Kdvf)));
if min(zeta) > opt_zeta_dvf && all(zeta > 0) && min(zeta) < 0.85
opt_zeta_dvf = min(zeta);
opt_gain_dvf = g;
end
end
% Transmissibility
% <<sec:comp_transmissibilty>>
open('rotating_frame.slx');
% Open Loop :ignore:
Kdvf = tf(zeros(2));
Kiff = tf(zeros(2));
kp = 0;
cp = 0;
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1;
Tol = linearize(mdl, io, 0);
%% Input/Output definition
Tol.InputName = {'Dwx', 'Dwy'};
Tol.OutputName = {'Dx', 'Dy'};
% Pseudo Integrator IFF :ignore:
kp = 0;
cp = 0;
Kdvf = tf(zeros(2));
Kiff = opt_gain_iff/(wi + s)*tf(eye(2));
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1;
Tiff = linearize(mdl, io, 0);
%% Input/Output definition
Tiff.InputName = {'Dwx', 'Dwy'};
Tiff.OutputName = {'Dx', 'Dy'};
% IFF With parallel Stiffness :ignore:
kp = 5*m*W^2;
cp = 0.01;
Kiff = opt_gain_kp/s*tf(eye(2));
Kdvf = tf(zeros(2));
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1;
Tiff_kp = linearize(mdl, io, 0);
%% Input/Output definition
Tiff_kp.InputName = {'Dwx', 'Dwy'};
Tiff_kp.OutputName = {'Dx', 'Dy'};
% DVF :ignore:
kp = 0;
cp = 0;
Kiff = tf(zeros(2));
Kdvf = opt_gain_kp*tf(eye(2));
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/dw'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1;
Tdvf = linearize(mdl, io, 0);
%% Input/Output definition
Tdvf.InputName = {'Dwx', 'Dwy'};
Tdvf.OutputName = {'Dx', 'Dy'};
% Transmissibility :ignore:
freqs = logspace(-2, 1, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(Tiff(1,1), freqs))), ...
'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(Tiff_kp(1,1), freqs))), ...
'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(Tdvf(1,1), freqs))), ...
'DisplayName', 'DVF')
plot(freqs, abs(squeeze(freqresp(Tol(1,1), freqs))), 'k-', ...
'DisplayName', 'Open-Loop')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [rad/s]'); ylabel('Transmissibility [m/m]');
legend('location', 'southwest');
% Open Loop :ignore:
Kdvf = tf(zeros(2));
Kiff = tf(zeros(2));
kp = 0;
cp = 0;
%% Name of the Simulink File
mdl = 'rotating_frame';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/fd'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Meas'], 1, 'output'); io_i = io_i + 1;
Col = linearize(mdl, io, 0);
%% Input/Output definition
Col.InputName = {'Fdx', 'Fdy'};
Col.OutputName = {'Dx', 'Dy'};
% Pseudo Integrator IFF :ignore:
kp = 0;
cp = 0;
Kdvf = tf(zeros(2));
Kiff = opt_gain_iff/(wi + s)*tf(eye(2));
Ciff = linearize(mdl, io, 0);
%% Input/Output definition
Ciff.InputName = {'Fdx', 'Fdy'};
Ciff.OutputName = {'Dx', 'Dy'};
% IFF With parallel Stiffness :ignore:
kp = 5*m*W^2;
cp = 0.01;
Kiff = opt_gain_kp/s*tf(eye(2));
Kdvf = tf(zeros(2));
Ciff_kp = linearize(mdl, io, 0);
%% Input/Output definition
Ciff_kp.InputName = {'Fdx', 'Fdy'};
Ciff_kp.OutputName = {'Dx', 'Dy'};
% DVF :ignore:
kp = 0;
cp = 0;
Kiff = tf(zeros(2));
Kdvf = opt_gain_kp*tf(eye(2));
Cdvf = linearize(mdl, io, 0);
%% Input/Output definition
Cdvf.InputName = {'Fdx', 'Fdy'};
Cdvf.OutputName = {'Dx', 'Dy'};
% Compliance :ignore:
freqs = logspace(-2, 1, 1000);
figure;
hold on;
plot(freqs, abs(squeeze(freqresp(Ciff(1,1), freqs))), ...
'DisplayName', 'IFF + HPF')
plot(freqs, abs(squeeze(freqresp(Ciff_kp(1,1), freqs))), ...
'DisplayName', 'IFF + $k_p$')
plot(freqs, abs(squeeze(freqresp(Cdvf(1,1), freqs))), ...
'DisplayName', 'DVF')
plot(freqs, abs(squeeze(freqresp(Col(1,1), freqs))), 'k-', ...
'DisplayName', 'Open-Loop')
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [rad/s]'); ylabel('Compliance [m/N]');
legend('location', 'southwest');