tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
de3c8b6ec9
nass-simscape/org/control_voice_coil.org

2301 lines
73 KiB
Org Mode
Raw Normal View History

Update html pages
2020-03-23 10:05:32 +01:00
#+TITLE: Control of the NASS with Voice coil actuators
Add org-setup-file that all org file links to
2020-04-17 10:25:44 +02:00
#+SETUPFILE: ./setup/org-setup-file.org
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
* Introduction :ignore:
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
* HAC-LAC + Cascade Control Topology
** Introduction :ignore:
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+name: fig:cascade_control_architecture
#+caption: Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the frame of the Legs
#+RESULTS:
[[file:figs/cascade_control_architecture.png]]
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** Matlab Init :noexport:ignore:
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab :tangle no
simulinkproject('../');
#+end_src
#+begin_src matlab
open('nass_model.slx')
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** Initialization
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
We initialize all the stages with the default parameters.
#+begin_src matlab
initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
#+end_src
The nano-hexapod is a voice coil based hexapod and the sample has a mass of 1kg.
#+begin_src matlab
initializeNanoHexapod('actuator', 'lorentz');
initializeSample('mass', 1);
#+end_src
We set the references that corresponds to a tomography experiment.
#+begin_src matlab
initializeReferences('Rz_type', 'rotating', 'Rz_period', 1);
#+end_src
#+begin_src matlab
initializeDisturbances();
#+end_src
#+begin_src matlab
initializeController('type', 'cascade-hac-lac');
#+end_src
#+begin_src matlab
initializeSimscapeConfiguration('gravity', true);
#+end_src
We log the signals.
#+begin_src matlab
initializeLoggingConfiguration('log', 'all');
#+end_src
#+begin_src matlab
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
Kp = tf(zeros(6));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
<<sec:lac_iff>>
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Identification
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
Let's first identify the plant for the IFF controller.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
%% Run the linearization
G_iff = linearize(mdl, io, 0);
G_iff.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Plant
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
The obtained plant for IFF is shown in Figure [[fig:cascade_vc_iff_plant]].
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$\\tau_{m,%i}/\\tau_%i$', i, i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'northeast');
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_iff(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_iff_plant
#+caption: IFF Plant ([[./figs/cascade_vc_iff_plant.png][png]], [[./figs/cascade_vc_iff_plant.pdf][pdf]])
[[file:figs/cascade_vc_iff_plant.png]]
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Root Locus
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
As seen in the root locus (Figure [[fig:cascade_vc_iff_root_locus]], no damping can be added to modes corresponding to the resonance of the micro-station.
However, critical damping can be achieve for the resonances of the nano-hexapod as shown in the zoomed part of the root (Figure [[fig:cascade_vc_iff_root_locus]], left part).
The maximum damping is obtained for a control gain of $\approx 70$.
#+begin_src matlab :exports none
gains = logspace(0, 4, 500);
figure;
subplot(1, 2, 1);
hold on;
plot(real(pole(G_iff)), imag(pole(G_iff)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_iff)), imag(tzero(G_iff)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_iff, -(gains(i)/s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*500]);
xlim([-2*pi*500,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
subplot(1, 2, 2);
hold on;
plot(real(pole(G_iff)), imag(pole(G_iff)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_iff)), imag(tzero(G_iff)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_iff, -(gains(i)/s)*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*8]);
xlim([-2*pi*8,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_root_locus.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_iff_root_locus
#+caption: Root Locus for the IFF control ([[./figs/cascade_vc_iff_root_locus.png][png]], [[./figs/cascade_vc_iff_root_locus.pdf][pdf]])
[[file:figs/cascade_vc_iff_root_locus.png]]
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Controller and Loop Gain
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
We create the $6 \times 6$ diagonal Integral Force Feedback controller.
The obtained loop gain is shown in Figure [[fig:cascade_vc_iff_loop_gain]].
#+begin_src matlab
Kiff = -70/s*eye(6);
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_iff_loop_gain
#+caption: Obtained Loop gain the IFF Control ([[./figs/cascade_vc_iff_loop_gain.png][png]], [[./figs/cascade_vc_iff_loop_gain.pdf][pdf]])
[[file:figs/cascade_vc_iff_loop_gain.png]]
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
<<sec:hac_joint_space>>
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Identification of the damped plant
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
Let's identify the dynamics from $\bm{\tau}^\prime$ to $d\bm{\mathcal{L}}$ as shown in Figure [[fig:cascade_control_architecture]].
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'output', [], 'Dnlm'); io_i = io_i + 1; % Leg Displacement
%% Run the linearization
Gl = linearize(mdl, io, 0);
Gl.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
Gl.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
#+end_src
There are some unstable poles in the Plant with very small imaginary parts.
These unstable poles are probably not physical, and they disappear when taking the minimum realization of the plant.
#+begin_src matlab
isstable(Gl)
Gl = minreal(Gl);
isstable(Gl)
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Obtained Plant
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
The obtained dynamics is shown in Figure [[fig:cascade_vc_hac_joint_plant]].
Few things can be said on the dynamics:
- the dynamics of the diagonal elements are almost all the same
- the system is well decoupled below the resonances of the nano-hexapod (1Hz)
- the dynamics of the diagonal elements are almost equivalent to a critically damped mass-spring-system with some spurious resonances above 50Hz corresponding to the resonances of the micro-station
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_vc_hac_joint_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_vc_hac_joint_plant
#+caption: Plant for the High Authority Control in the Joint Space ([[./figs/cascade_vc_hac_joint_plant.png][png]], [[./figs/cascade_vc_hac_joint_plant.pdf][pdf]])
[[file:figs/cascade_vc_hac_joint_plant.png]]
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Controller Design and Loop Gain
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
As the plant is well decoupled, a diagonal plant is designed.
#+begin_src matlab
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
wc = 2*pi*10; % Bandwidth Bandwidth [rad/s]
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
h = 2; % Lead parameter
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
Kl = (s + 2*pi*1)/s;
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));
#+end_src
#+begin_src matlab :exports none
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
freqs = logspace(-1, 3, 1000);
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gl*Kl, eye(6), -1))
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** Primary Controller in the task space - $\bm{K}_\mathcal{X}$
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
<<sec:primary_controller>>
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Identification of the linearized plant
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\mathcal{X}$.
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'], 1, 'input'); io_i = io_i + 1;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
%% Run the linearization
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
Gp = linearize(mdl, io, 0);
Gp.InputName = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
Gp.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
A minus sign is added because the minus sign is already included in the plant identification.
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
isstable(Gp)
Gp = -minreal(Gp);
isstable(Gp)
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+end_src
#+begin_src matlab
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
load('mat/stages.mat', 'nano_hexapod');
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gpx = Gp*inv(nano_hexapod.kinematics.J');
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
Gpx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gpl = nano_hexapod.kinematics.J*Gp;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
Gpl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Obtained Plant
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab :exports none
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
freqs = logspace(-1, 3, 1000);
labels = {'$\epsilon_x/r_{xn}$', '$\epsilon_y/r_{yn}$', '$\epsilon_z/r_{zn}$', '$\epsilon_{R_x}/r_{R_xn}$', '$\epsilon_{R_y}/r_{R_yn}$', '$\epsilon_{R_z}/r_{R_zn}$'};
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, abs(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, abs(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
end
set(gca,'ColorOrderIndex',1);
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, abs(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
end
set(gca,'ColorOrderIndex',1);
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:primary_plant_voice_coil_X
#+caption: Obtained Primary plant in the Task space ([[./figs/primary_plant_voice_coil_X.png][png]], [[./figs/primary_plant_voice_coil_X.pdf][pdf]])
[[file:figs/primary_plant_voice_coil_X.png]]
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
labels = {'$\epsilon_{\mathcal{L}_1}/\tau_{1}$', '$\epsilon_{\mathcal{L}_2}/\tau_{2}$', '$\epsilon_{\mathcal{L}_3}/\tau_{3}$', '$\epsilon_{\mathcal{L}_4}/\tau_{4}$', '$\epsilon_{\mathcal{L}_5}/\tau_{5}$', '$\epsilon_{\mathcal{L}_6}/\tau_{6}$'};
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, abs(squeeze(freqresp(Gpl(i, i), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, abs(squeeze(freqresp(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
end
set(gca,'ColorOrderIndex',1);
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, abs(squeeze(freqresp(Gpl(1, 1), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
end
set(gca,'ColorOrderIndex',1);
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(1, 1), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/primary_plant_voice_coil_L.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:primary_plant_voice_coil_L
#+caption: Obtained Primary plant in the frame of the legs ([[./figs/primary_plant_voice_coil_L.png][png]], [[./figs/primary_plant_voice_coil_L.pdf][pdf]])
[[file:figs/primary_plant_voice_coil_L.png]]
*** Controller Design
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab
wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
(1/h) * (1 + s/wc*h)/(1 + s/wc/h); % For Piezo
% Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (s + 2*pi*10)/s * (s + 2*pi*1)/s ; % For voice coil
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
% Normalization of the gain of have a loop gain of 1 at frequency wc
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
Kp = Kp.*diag(1./diag(abs(freqresp(Gpx*Kp, wc))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp(i,i), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
ylabel('Loop Gpain'); set(gca, 'XTickLabel',[]);
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp(i,i), freqs, 'Hz'))));
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/loop_gain_primary_voice_coil_X.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:loop_gain_primary_voice_coil_X
#+caption: Obtained Loop gain for the primary controller in the Task space ([[./figs/loop_gain_primary_voice_coil_X.png][png]], [[./figs/loop_gain_primary_voice_coil_X.pdf][pdf]])
[[file:figs/loop_gain_primary_voice_coil_X.png]]
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gpx*Kp, eye(6), -1))
#+end_src
And now we include the Jacobian inside the controller.
#+begin_src matlab
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Kp = inv(nano_hexapod.kinematics.J')*Kp;
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
#+end_src
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab :exports none :tangle no
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
isstable(feedback(-Gp*Kp, eye(6), +1))
#+end_src
** Simulation
Let's first save the 3 controllers for further analysis:
#+begin_src matlab
save('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff', 'Kl', 'Kp')
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+end_src
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
And we simulate the system.
#+begin_src matlab
sim('nass_model');
#+end_src
#+begin_src matlab
cascade_hac_lac_lorentz = simout;
save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz', '-append');
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** Results
*** Load the simulation results
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
load('./mat/cascade_hac_lac.mat', 'cascade_hac_lac', 'cascade_hac_lac_lorentz');
#+end_src
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Control effort
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab :exports none
load('mat/stages.mat', 'nano_hexapod');
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
F_pz = [nano_hexapod.kinematics.J'*cascade_hac_lac.u.Data']';
F_vc = [nano_hexapod.kinematics.J'*cascade_hac_lac_lorentz.u.Data']';
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
% F_pz = [-nano_hexapod.kinematics.J'*cascade_hac_lac.yn.Fnlm.Data']';
% F_vc = [-nano_hexapod.kinematics.J'*cascade_hac_lac_lorentz.yn.Fnlm.Data']';
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+end_src
#+begin_src matlab :exports none
labels = {'$\mathcal{F}_x$', '$\mathcal{F}_y$', '$\mathcal{F}_z$', '$\mathcal{M}_x$', '$\mathcal{M}_y$', '$\mathcal{M}_z$'};
figure;
ax1 = subplot(1, 2, 1);
hold on;
for i = 1:6
plot(cascade_hac_lac.u.Time, F_pz(:, i), 'DisplayName', labels{i})
end
hold off;
xlabel('Time [s]'); ylabel('Force/Torque Piezo [N, N$\cdot$m]');
legend('location', 'northeast');
ax2 = subplot(1, 2, 2);
hold on;
for i = 1:6
plot(cascade_hac_lac_lorentz.u.Time, F_vc(:, i), 'DisplayName', labels{i})
end
hold off;
xlabel('Time [s]'); ylabel('Force/Torque Lorentz [N, N$\cdot$m]');
legend('location', 'northeast');
#+end_src
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
#+begin_src matlab :exports none
labels = {'$\mathcal{F}_x$', '$\mathcal{F}_y$', '$\mathcal{F}_z$', '$\mathcal{M}_x$', '$\mathcal{M}_y$', '$\mathcal{M}_z$'};
figure;
hold on;
for i = 1:6
plot(cascade_hac_lac_lorentz.u.Time, F_vc(:, i), 'DisplayName', labels{i})
end
hold off;
xlabel('Time [s]'); ylabel('Force/Torque Lorentz [N, N$\cdot$m]');
xlim([0.5, inf]);
legend('location', 'northeast');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/actuator_force_torques_tomography_voice_coil.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:actuator_force_torques_tomography_voice_coil
#+caption: Actuator Action during a tomography experiment when using Voice Coil actuators ([[./figs/actuator_force_torques_tomography_voice_coil.png][png]], [[./figs/actuator_force_torques_tomography_voice_coil.pdf][pdf]])
[[file:figs/actuator_force_torques_tomography_voice_coil.png]]
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
*** Load the simulation results
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab
n_av = 4;
han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(:,1))/n_av));
#+end_src
#+begin_src matlab
t = cascade_hac_lac.Em.En.Time;
Ts = t(2)-t(1);
[pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
[pxx_ca, ~] = pwelch(cascade_hac_lac.Em.En.Data, han_win, [], [], 1/Ts);
[pxx_vc, ~] = pwelch(cascade_hac_lac_lorentz.Em.En.Data, han_win, [], [], 1/Ts);
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(pxx_ol(:, 1)))
plot(f, sqrt(pxx_vc(:, 1)))
hold off;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
ylabel('$\Gamma_{D_x}$, $\Gamma_{D_y}$, $\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(pxx_ol(:, 2)))
plot(f, sqrt(pxx_vc(:, 2)))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(pxx_ol(:, 3)))
plot(f, sqrt(pxx_vc(:, 3)))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(pxx_ol(:, 4)))
plot(f, sqrt(pxx_vc(:, 4)))
hold off;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
ylabel('$\Gamma_{R_x}$, $\Gamma_{R_y}$, $\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
xlabel('Frequency [Hz]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(pxx_ol(:, 5)))
plot(f, sqrt(pxx_vc(:, 5)))
hold off;
xlabel('Frequency [Hz]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(pxx_ol(:, 6)), 'DisplayName', '$\mu$-Station')
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(f, sqrt(pxx_vc(:, 6)), 'DisplayName', 'Voice-Coil')
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
hold off;
xlabel('Frequency [Hz]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_psd_pos_error.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:exp_tomography_voice_coil_psd_pos_error
#+caption: Power Spectral Density of the position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_psd_pos_error.png][png]], [[./figs/exp_tomography_voice_coil_psd_pos_error.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_psd_pos_error.png]]
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 1))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 1))))))
hold off;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
ylabel('CAS $D_x$, $D_y$, $D_z$ [$m$]');
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 2))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 2))))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 3))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 3))))))
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 4))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 4))))))
hold off;
xlabel('Frequency [Hz]');
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
ylabel('CAS $R_x$, $R_y$, $R_z$ [$rad$]');
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 5))))))
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 5))))))
hold off;
xlabel('Frequency [Hz]');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 6))))), 'DisplayName', 'Voice Coil')
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
hold off;
xlabel('Frequency [Hz]');
legend('location', 'southwest');
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([f(2), f(end)])
#+end_src
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_cap_pos_error.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:exp_tomography_voice_coil_cap_pos_error
#+caption: Cumulative Amplitude Spectrum of the position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_cap_pos_error.png][png]], [[./figs/exp_tomography_voice_coil_cap_pos_error.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_cap_pos_error.png]]
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 1))
hold off;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
ylabel('$D_x$, $D_y$, $D_z$ [m]');
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
ax2 = subplot(2, 3, 2);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 2))
hold off;
ax3 = subplot(2, 3, 3);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 3))
hold off;
ax4 = subplot(2, 3, 4);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 4))
hold off;
xlabel('Time [s]');
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
ylabel('$R_x$, $R_y$, $R_z$ [rad]');
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
ax5 = subplot(2, 3, 5);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 5))
hold off;
xlabel('Time [s]');
ax6 = subplot(2, 3, 6);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.Em.En.Data(:, 6), 'DisplayName', 'Voice Coil')
Update links from index and publish html files
2020-03-25 19:23:22 +01:00
hold off;
xlabel('Time [s]');
legend();
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/exp_tomography_voice_coil_time_domain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:exp_tomography_voice_coil_time_domain
#+caption: Position error during a tomography experiment when using Voice Coil based nano-hexapod ([[./figs/exp_tomography_voice_coil_time_domain.png][png]], [[./figs/exp_tomography_voice_coil_time_domain.pdf][pdf]])
[[file:figs/exp_tomography_voice_coil_time_domain.png]]
Add analysis about compliance
2020-03-31 11:11:01 +02:00
** Compliance of the nano-hexapod
*** Identification
Let's identify the Compliance of the NASS:
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Disturbances/Fd'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the sample
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
#+end_src
First in open-loop:
#+begin_src matlab
Kp = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_ol = linearize(mdl, io, 0);
Gc_ol.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_ol.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
Then with the IFF control.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff')
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_iff = linearize(mdl, io, 0);
Gc_iff.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_iff.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
With the HAC control added
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kl')
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_hac = linearize(mdl, io, 0);
Gc_hac.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_hac.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
Finally with the primary controller
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kp')
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_pri = linearize(mdl, io, 0);
Gc_pri.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_pri.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
*** Obtained Compliance
#+begin_src matlab :exports none
labels = {'$\epsilon_x/F_{xd}$', '$\epsilon_y/F_{yd}$', '$\epsilon_z/F_{zd}$', '$\epsilon_{R_x}/M_{xd}$', '$\epsilon_{R_y}/M_{yd}$', '$\epsilon_{R_z}/M_{zd}$'};
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_ol( 1, 1), freqs, 'Hz'))), 'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(Gc_iff(1, 1), freqs, 'Hz'))), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(Gc_hac(1, 1), freqs, 'Hz'))), 'DisplayName', 'HAC');
plot(freqs, abs(squeeze(freqresp(Gc_pri(1, 1), freqs, 'Hz'))), 'DisplayName', 'PRI');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [m/N]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
ax2 = subplot(1, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_ol( 4, 4), freqs, 'Hz'))), 'DisplayName', 'OL');
plot(freqs, abs(squeeze(freqresp(Gc_iff(4, 4), freqs, 'Hz'))), 'DisplayName', 'IFF');
plot(freqs, abs(squeeze(freqresp(Gc_hac(4, 4), freqs, 'Hz'))), 'DisplayName', 'HAC');
plot(freqs, abs(squeeze(freqresp(Gc_pri(4, 4), freqs, 'Hz'))), 'DisplayName', 'PRI');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [$\frac{rad}{Nm}$]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/compliance_evolution_vc_cascade_control.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:compliance_evolution_vc_cascade_control
#+caption: Evolution of the NASS compliance with each control loop added ([[./figs/compliance_evolution_vc_cascade_control.png][png]], [[./figs/compliance_evolution_vc_cascade_control.pdf][pdf]])
[[file:figs/compliance_evolution_vc_cascade_control.png]]
Compliance comparison VC and PZ
2020-03-31 11:15:18 +02:00
*** Comparison with Piezo
Let's initialize a nano-hexapod with piezoelectric actuators.
#+begin_src matlab
initializeNanoHexapod('actuator', 'piezo');
#+end_src
We don't use any controller.
#+begin_src matlab
Kp = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
#+begin_src matlab
%% Run the linearization
Gc_pz = linearize(mdl, io, 0);
Gc_pz.InputName = {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'};
Gc_pz.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(1, 2, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_pri(1, 1), freqs, 'Hz'))), 'DisplayName', 'PRI');
plot(freqs, abs(squeeze(freqresp(Gc_pz( 1, 1), freqs, 'Hz'))), 'DisplayName', 'PZ - OL');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [m/N]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
ax2 = subplot(1, 2, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_pri(4, 4), freqs, 'Hz'))), 'DisplayName', 'PRI');
plot(freqs, abs(squeeze(freqresp(Gc_pz( 4, 4), freqs, 'Hz'))), 'DisplayName', 'PZ - OL');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Compliance [$\frac{rad}{Nm}$]');
xlabel('Frequency [Hz]');
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/compliance_comp_pz_vc_cascade.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:compliance_comp_pz_vc_cascade
#+caption: Comparison of the compliance using the open-loop piezo-nano hexapod and the voice coil nano-hexapod with the cascade control topology ([[./figs/compliance_comp_pz_vc_cascade.png][png]], [[./figs/compliance_comp_pz_vc_cascade.pdf][pdf]])
[[file:figs/compliance_comp_pz_vc_cascade.png]]
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** Robustness to Payload Variability
*** Initialization
Let's change the payload mass, and see if the controller design for a payload mass of 1 still gives good performance.
#+begin_src matlab
initializeSample('mass', 50);
#+end_src
#+begin_src matlab
Kp = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
#+end_src
*** Low Authority Control
Let's first identify the transfer function for the Low Authority control.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Fnlm'); io_i = io_i + 1; % Force Sensors
%% Run the linearization
G_iff_m = linearize(mdl, io, 0);
G_iff_m.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_iff_m.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
#+end_src
The obtained dynamics is compared when using a payload of 1Kg in Figure [[fig:voice_coil_variability_mass_iff]].
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_iff_m(i, i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$\\tau_{m,%i}/\\tau_%i$', i, i));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_iff_m(i, i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_iff
#+caption: Dynamics of the LAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff.png][png]], [[./figs/voice_coil_variability_mass_iff.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_iff.png]]
A gain of 50 will here suffice to obtain critical damping of the nano-hexapod modes.
Let's load the IFF controller designed when the payload has a mass of 1Kg.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kiff')
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(i,i)*G_iff_m(i,i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_iff_loop_gain
#+caption: Loop gain for the IFF Control when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_iff_loop_gain.png][png]], [[./figs/voice_coil_variability_mass_iff_loop_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_iff_loop_gain.png]]
*** High Authority Control
We use the Integral Force Feedback developed with a mass of 1Kg and we identify the dynamics for the High Authority Controller in the case of the 50Kg payload
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'output', [], 'Dnlm'); io_i = io_i + 1; % Leg Displacement
%% Run the linearization
Gl_m = linearize(mdl, io, 0);
Gl_m.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
Gl_m.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
isstable(Gl_m)
Gl_m = minreal(Gl_m);
isstable(Gl_m)
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl_m(i, i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_hac_plant
#+caption: Dynamics of the HAC plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_plant.png][png]], [[./figs/voice_coil_variability_mass_hac_plant.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_hac_plant.png]]
We load the HAC controller design when the payload has a mass of 1Kg.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kl')
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))), '-');
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl_m(i, i)*Kl(i,i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_hac_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_hac_lool_gain
#+caption: Loop Gain of the HAC when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_hac_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_hac_lool_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_hac_lool_gain.png]]
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gl_m*Kl, eye(6), -1))
#+end_src
*** Primary Plant
We use the Low Authority Controller developed with a mass of 1Kg and we identify the dynamics for the Primary controller in the case of the 50Kg payload.
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/Cascade-HAC-LAC/Kp'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
Gp_m = linearize(mdl, io, 0);
Gp_m.InputName = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
Gp_m.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
A minus sign is added to cancel the minus sign already included in the identified plant.
#+begin_src matlab
isstable(Gp_m)
Gp_m = -minreal(Gp_m);
isstable(Gp_m)
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gpx_m = Gp_m*inv(nano_hexapod.kinematics.J');
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
Gpx_m.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gpl_m = nano_hexapod.kinematics.J*Gp_m;
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
Gpl_m.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src
#+begin_important
There are two zeros with positive real part for the plant in the y direction at about 100Hz.
This is problematic as it limits the bandwidth to be less than $\approx 50\ \text{Hz}$.
It is important here to physically understand why such "positive" zero appears.
If we make a "rigid" 50kg paylaod, the positive zero disappears.
#+end_important
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
labels = {'$\epsilon_x/r_{xn}$', '$\epsilon_y/r_{yn}$', '$\epsilon_z/r_{zn}$', '$\epsilon_{R_x}/r_{R_xn}$', '$\epsilon_{R_y}/r_{R_yn}$', '$\epsilon_{R_z}/r_{R_zn}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx_m(i, i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gpx_m(i, j), freqs, 'Hz'))), '--', 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gpx_m(1, 1), freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_primary_plant
#+caption: Dynamics of the Primary plant when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_plant.png][png]], [[./figs/voice_coil_variability_mass_primary_plant.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_primary_plant.png]]
We load the primary controller that was design when the payload has a mass of 1Kg.
We load the HAC controller design when the payload has a mass of 1Kg.
#+begin_src matlab
load('mat/hac_lac_cascade_vc_controllers.mat', 'Kp')
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Kp_x = nano_hexapod.kinematics.J'*Kp;
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
#+end_src
#+begin_src matlab
wc = 2*pi*50; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kp = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
(1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ...
(s + 2*pi*1)/s * ...
1/(1+s/2/wc); % For Piezo
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kp = Kp.*diag(1./diag(abs(freqresp(Gpx_m*Kp, wc))));
#+end_src
#+begin_src matlab :exports none
labels = {'$L_{x}$', '$L_{y}$', '$L_{z}$', '$L_{R_x}$', '$L_{R_y}$', '$L_{R_z}$'};
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), freqs, 'Hz'))), '--');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gpain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kp_x(i,i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
set(gca,'ColorOrderIndex',1);
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx_m(i, i)*Kp_x(i,i), freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'northwest')
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/voice_coil_variability_mass_primary_lool_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:voice_coil_variability_mass_primary_lool_gain
#+caption: Loop Gain of the Primary loop when using a 50Kg payload (dashed) and when using a 1Kg payload (solid) ([[./figs/voice_coil_variability_mass_primary_lool_gain.png][png]], [[./figs/voice_coil_variability_mass_primary_lool_gain.pdf][pdf]])
[[file:figs/voice_coil_variability_mass_primary_lool_gain.png]]
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gpx_m*Kp_x, eye(6), -1))
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gp_m*Kp, eye(6), -1))
#+end_src
*** Simulation
#+begin_src matlab
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
And we simulate the system.
#+begin_src matlab
sim('nass_model');
#+end_src
#+begin_src matlab
cascade_hac_lac_lorentz_high_mass = simout;
save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz_high_mass', '-append');
#+end_src
#+begin_src matlab
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 1))
hold off;
ylabel('$D_x$, $D_y$, $D_z$ [m]');
ax2 = subplot(2, 3, 2);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 2))
hold off;
ax3 = subplot(2, 3, 3);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 3))
hold off;
ax4 = subplot(2, 3, 4);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 4))
hold off;
xlabel('Time [s]');
ylabel('$R_x$, $R_y$, $R_z$ [rad]');
ax5 = subplot(2, 3, 5);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 5))
hold off;
xlabel('Time [s]');
ax6 = subplot(2, 3, 6);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
plot(cascade_hac_lac_lorentz_high_mass.Em.En.Time, cascade_hac_lac_lorentz_high_mass.Em.En.Data(:, 6), 'DisplayName', 'Voice Coil')
hold off;
xlabel('Time [s]');
legend();
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
* Other analysis
Add some analysis on Voice coil with high mass
2020-03-30 19:04:03 +02:00
** Robustness to Payload Variability
- [ ] For 3/masses (1kg, 10kg, 50kg), plot each of the 3 plants
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
** Direct HAC control in the task space - $\bm{K}_\mathcal{X}$
*** Introduction :ignore:
#+name: fig:control_architecture_hac_iff_pos_X
#+caption: Control Architecture containing an IFF controller and a Controller in the task space
[[file:figs/control_architecture_hac_iff_pos_X.png]]
*** Identification
#+begin_src matlab
initializeController('type', 'hac-iff');
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/HAC-IFF/Kx'], 1, 'input'); io_i = io_i + 1; % Control input
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
isstable(G)
G = -minreal(G);
isstable(G)
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gx = G*inv(nano_hexapod.kinematics.J');
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gl = nano_hexapod.kinematics.J*G;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
Gl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src
*** Obtained Plant in the Task Space
#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
labels = {'$\epsilon_x/F_{x}$', '$\epsilon_y/F_{y}$', '$\epsilon_z/F_{z}$', '$\epsilon_{R_x}/M_{x}$', '$\epsilon_{R_y}/M_{y}$', '$\epsilon_{R_z}/M_{z}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
*** Obtained Plant in the Joint Space
#+begin_src matlab :exports none
freqs = logspace(0, 4, 1000);
labels = {'$\epsilon_{\mathcal{L}_1}/\tau_{1}$', '$\epsilon_{\mathcal{L}_2}/\tau_{2}$', '$\epsilon_{\mathcal{L}_3}/\tau_{3}$', '$\epsilon_{\mathcal{L}_4}/\tau_{4}$', '$\epsilon_{\mathcal{L}_5}/\tau_{5}$', '$\epsilon_{\mathcal{L}_6}/\tau_{6}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
*** Controller Design in the Joint Space
#+begin_src matlab
wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(s + 2*pi*10)/s * ... % Pseudo Integrator
1/(1+s/2/pi/500); % Low pass Filter
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc))));
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gx*Kx, eye(6), -1))
#+end_src
#+begin_src matlab
wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
(s + 2*pi*1)/s * ... % Pseudo Integrator
(s + 2*pi*10)/s * ... % Pseudo Integrator
1/(1+s/2/pi/500); % Low pass Filter
% Normalization of the gain of have a loop gain of 1 at frequency wc
Kl = Kl.*diag(1./diag(abs(freqresp(Gl*Kl, wc))));
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i)*Kl(i,i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :exports none :tangle no
isstable(feedback(Gl*Kl, eye(6), -1))
#+end_src
** On the usefulness of the High Authority Control loop / Linearization loop
*** Introduction :ignore:
Let's see what happens is we omit the HAC loop and we directly try to control the damped plant using the measurement of the sample with respect to the granite $\bm{\mathcal{X}}$.
We can do that in two different ways:
- in the task space as shown in Figure [[fig:control_architecture_iff_X]]
- in the space of the legs as shown in Figure [[fig:control_architecture_iff_L]]
#+begin_src latex :file control_architecture_iff_X.pdf
\begin{tikzpicture}
% Blocs
\node[block={3.0cm}{3.0cm}] (P) {Plant};
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
\node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
\node[block, left= of addF] (J) {$\bm{J}^{-T}$};
\node[block, left= of J] (K) {$\bm{K}_\mathcal{X}$};
\node[addb={+}{}{}{}{-}, left= of K] (subr) {};
% Connections and labels
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
\draw[->] (Kiff.west) -| (addF.north);
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
\draw[->] (outputX) -- ++(1.6, 0);
\draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -2) -| (subr.south);
\draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}}$};
\draw[->] (K.east) -- (J.west) node[above left]{$\bm{\mathcal{F}}$};
\draw[->] (J.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
\end{tikzpicture}
#+end_src
#+name: fig:control_architecture_iff_X
#+caption: IFF control + primary controller in the task space
#+RESULTS:
[[file:figs/control_architecture_iff_X.png]]
#+begin_src latex :file control_architecture_iff_L.pdf
\begin{tikzpicture}
% Blocs
\node[block={3.0cm}{3.0cm}] (P) {Plant};
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
\coordinate[] (outputF) at ($(P.south east)!0.8!(P.north east)$);
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
\coordinate[] (outputL) at ($(P.south east)!0.2!(P.north east)$);
\node[block, above=0.4 of P] (Kiff) {$\bm{K}_\text{IFF}$};
\node[addb={+}{}{-}{}{}, left= of inputF] (addF) {};
\node[block, left= of addF] (K) {$\bm{K}_\mathcal{L}$};
\node[block, left= of K] (J) {$\bm{J}$};
\node[addb={+}{}{}{}{-}, left= of J] (subr) {};
% Connections and labels
\draw[->] (outputF) -- ++(1, 0) node[below left]{$\bm{\tau}_m$};
\draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east);
\draw[->] (Kiff.west) -| (addF.north);
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
\draw[->] (outputL) -- ++(1, 0) node[above left]{$d\bm{\mathcal{L}}$};
\draw[->] (outputX) -- ++(1.6, 0);
\draw[->] ($(outputX) + (1.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -2) -| (subr.south);
\draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
\draw[->] (subr.east) -- (J.west) node[above left]{$\bm{\epsilon}_{\mathcal{X}}$};
\draw[->] (J.east) -- (K.west) node[above left]{$\bm{\epsilon}_{\mathcal{L}}$};
\draw[->] (K.east) -- (addF.west) node[above left]{$\bm{\tau}^\prime$};
\end{tikzpicture}
#+end_src
#+name: fig:control_architecture_iff_L
#+caption: HAC-LAC control architecture in the frame of the legs
#+RESULTS:
[[file:figs/control_architecture_iff_L.png]]
*** Identification
#+begin_src matlab
initializeController('type', 'hac-iff');
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/HAC-IFF/Kx'], 1, 'input'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
isstable(G)
G = -minreal(G);
isstable(G)
#+end_src
*** Plant in the Task space
The obtained plant is shown in Figure
#+begin_src matlab
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gx = G*inv(nano_hexapod.kinematics.J');
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 4, 1000);
labels = {'$\epsilon_x/\mathcal{F}_{x}$', '$\epsilon_y/\mathcal{F}_{y}$', '$\epsilon_z/\mathcal{F}_{z}$', '$\epsilon_{R_x}/\mathcal{M}_{x}$', '$\epsilon_{R_y}/\mathcal{M}_{y}$', '$\epsilon_{R_z}/\mathcal{M}_{z}$'};
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i), freqs, 'Hz'))), 'DisplayName', labels{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
*** Plant in the Leg's space
#+begin_src matlab
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gl = nano_hexapod.kinematics.J*G;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
#+end_src
#+begin_src matlab :exports none
freqs = logspace(-1, 4, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_%i/\\tau_%i$', i, i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend();
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
** DVF instead of IFF?
*** Initialization and Identification
#+begin_src matlab
initializeController('type', 'hac-dvf');
Kdvf = tf(zeros(6));
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Displacement Sensors
%% Run the linearization
G_dvf = linearize(mdl, io, 0);
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G_dvf.OutputName = {'Dlm1', 'Dlm2', 'Dlm3', 'Dlm4', 'Dlm5', 'Dlm6'};
#+end_src
*** Obtained Plant
#+begin_src matlab :exports none
freqs = logspace(-1, 3, 1000);
figure;
ax1 = subplot(2, 2, 1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('Diagonal elements of the Plant');
ax2 = subplot(2, 2, 3);
hold on;
for i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(i, i), freqs, 'Hz'))), 'DisplayName', sprintf('$d\\mathcal{L}_{%i}/\\tau_%i$', i, i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'northeast');
ax3 = subplot(2, 2, 2);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, abs(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
title('Off-Diagonal elements of the Plant');
ax4 = subplot(2, 2, 4);
hold on;
for i = 1:5
for j = i+1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
set(gca,'ColorOrderIndex',1);
plot(freqs, 180/pi*angle(squeeze(freqresp(G_dvf(1, 1), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
linkaxes([ax1,ax2,ax3,ax4],'x');
#+end_src
#+begin_src matlab :exports none
gains = logspace(0, 4, 500);
figure;
subplot(1, 2, 1);
hold on;
plot(real(pole(G_dvf)), imag(pole(G_dvf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_dvf)), imag(tzero(G_dvf)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_dvf, (s*gains(i))*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*500]);
xlim([-2*pi*500,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
subplot(1, 2, 2);
hold on;
plot(real(pole(G_dvf)), imag(pole(G_dvf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G_dvf)), imag(tzero(G_dvf)), 'o');
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G_dvf, (s*gains(i))*eye(6)));
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0, 2*pi*8]);
xlim([-2*pi*8,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
#+end_src
*** Controller
#+begin_src matlab
Kdvf = -850*s/(1+s/2/pi/1000)*eye(6);
#+end_src
*** HAC Identification
#+begin_src matlab
%% Name of the Simulink File
mdl = 'nass_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller/HAC-DVF/Kx'], 1, 'input'); io_i = io_i + 1; % Control input
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
%% Run the linearization
G = linearize(mdl, io, 0);
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
#+begin_src matlab
isstable(G)
G = -minreal(G);
isstable(G)
#+end_src
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gx = G*inv(nano_hexapod.kinematics.J');
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Update files for new definition of hexapods
2020-05-05 11:38:52 +02:00
Gl = nano_hexapod.kinematics.J*G;
Add figures about tomography experiments with vc
2020-03-30 09:43:16 +02:00
Gl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
#+end_src
*** Conclusion
#+begin_important
DVF can be used instead of IFF.
#+end_important
Reference in New Issue Copy Permalink