Add simulation of HAC-DVF with opt-stiffness

This commit is contained in:
Thomas Dehaeze 2020-04-15 18:15:02 +02:00
parent 3f7c7de1ef
commit a2d2d14ca1
30 changed files with 8406 additions and 4828 deletions

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 16 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 8.1 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 16 KiB

File diff suppressed because it is too large Load Diff

Binary file not shown.

After

Width:  |  Height:  |  Size: 153 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 115 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 173 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 125 KiB

After

Width:  |  Height:  |  Size: 124 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 129 KiB

After

Width:  |  Height:  |  Size: 129 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 129 KiB

After

Width:  |  Height:  |  Size: 127 KiB

File diff suppressed because it is too large Load Diff

Binary file not shown.

Before

Width:  |  Height:  |  Size: 260 KiB

After

Width:  |  Height:  |  Size: 261 KiB

Binary file not shown.

Before

Width:  |  Height:  |  Size: 122 KiB

After

Width:  |  Height:  |  Size: 117 KiB

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

Binary file not shown.

View File

@ -202,7 +202,7 @@ And we save the obtained data.
In this section, we also perform a tomography experiment with the sample's center of mass aligned with the rotation axis.
However this time, we include perturbations such as ground motion and stage vibrations.
** TODO Simulation Setup
** Simulation Setup
We now activate the disturbances.
#+begin_src matlab
initializeDisturbances(...

View File

@ -678,6 +678,11 @@ exportFig('figs/opt_stiff_sensibility_dist_dvf.pdf', 'width', 'full', 'height',
#+RESULTS:
[[file:figs/opt_stiff_sensibility_dist_dvf.png]]
** Conclusion
#+begin_important
#+end_important
* Primary Control in the leg space
<<sec:primary_control_L>>
** Introduction :ignore:
@ -691,7 +696,7 @@ In this section we implement the control architecture shown in Figure [[fig:cont
The controller for decentralized direct velocity feedback is the one designed in Section [[sec:lac_dvf]].
** Plant in the task space
** Plant in the leg space
We now loop at the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the design of $\bm{K}_\mathcal{L}$.
The diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the three considered masses are shown in Figure [[fig:opt_stiff_primary_plant_L]].
@ -742,6 +747,37 @@ exportFig('figs/opt_stiff_primary_plant_L.pdf', 'width', 'full', 'height', 'full
#+RESULTS:
[[file:figs/opt_stiff_primary_plant_L.png]]
#+begin_src matlab :exports none
c1 = [ 0 0.4470 0.7410 0.2]; % Blue
c2 = [0.8500 0.3250 0.0980 0.2]; % Orange
c3 = [0.9290 0.6940 0.1250 0.2]; % Yellow
c4 = [0.4940 0.1840 0.5560 0.2]; % Purple
c5 = [0.4660 0.6740 0.1880 0.2]; % Green
c6 = [0.3010 0.7450 0.9330 0.2]; % Light Blue
c7 = [0.6350 0.0780 0.1840 0.2]; % Red
colors = [c1; c2; c3; c4; c5; c6; c7];
freqs = logspace(0, 3, 1000);
figure;
hold on;
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(freqs, abs(squeeze(freqresp(Gm_l{i}(1, 1), freqs, 'Hz'))), '-');
for j = 1:5
for k = j+1:6
plot(freqs, abs(squeeze(freqresp(Gm_l{i}(j, k), freqs, 'Hz'))), '--', 'color', colors(i, :));
end
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
ylim([1e-9, inf]);
#+end_src
** Control in the leg space
We design a diagonal controller with all the same diagonal elements.
@ -759,19 +795,14 @@ The design controller is as follows:
The loop gain is shown in Figure [[fig:opt_stiff_primary_loop_gain_L]].
#+begin_src matlab
h = 2.5;
h = 2.0;
Kl = 2e7 * eye(6) * ...
1/h*(s/(2*pi*100/h) + 1)/(s/(2*pi*100*h) + 1) * ...
1/h*(s/(2*pi*200/h) + 1)/(s/(2*pi*200*h) + 1) * ...
(s/2/pi/10 + 1)/(s/2/pi/10) * ...
1/(1 + s/2/pi/300);
#+end_src
#+begin_src matlab :exports none
for i = 1:length(Ms)
isstable(feedback(Gm_l{i}(1,1)*Kl(1,1), 1, -1))
end
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
@ -817,7 +848,14 @@ exportFig('figs/opt_stiff_primary_loop_gain_L.pdf', 'width', 'full', 'height', '
#+begin_src matlab
load('mat/stages.mat', 'nano_hexapod');
K = Kl*nano_hexapod.J;
K = Kl*nano_hexapod.J*diag([1, 1, 1, 1, 1, 0]);
#+end_src
Check the MIMO stability
#+begin_src matlab :exports none
for i = 1:length(Ms)
isstable(feedback(nano_hexapod.J\Gm_l{i}*K, eye(6), -1))
end
#+end_src
** Sensibility to Disturbances and Noise Budget
@ -1014,18 +1052,21 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
[[file:figs/opt_stiff_primary_control_L_cas_tot.png]]
** Simulations
Let's now simulate a tomography experiment.
To do so, we include all disturbances except vibrations of the translation stage.
#+begin_src matlab
initializeDisturbances('Fty_x', false, 'Fty_z', false);
initializeSimscapeConfiguration('gravity', false);
initializeLoggingConfiguration('log', 'all');
#+end_src
#+begin_src matlab
#+begin_src matlab :exports none
load('mat/conf_simulink.mat');
set_param(conf_simulink, 'StopTime', '2');
#+end_src
#+begin_src matlab
And we run the simulation for all three payload Masses.
#+begin_src matlab :exports none
hac_dvf_L = {zeros(length(Ms)), 1};
for i = 1:length(Ms)
@ -1037,21 +1078,27 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
end
#+end_src
#+begin_src matlab
#+begin_src matlab :exports none
save('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
#+end_src
** Results
#+begin_src matlab
#+begin_src matlab :exports none
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
load('./mat/tomo_exp_hac_dvf.mat', 'hac_dvf_L');
#+end_src
#+begin_src matlab
Let's now see how this controller performs.
First, we compute the Power Spectral Density of the sample's position error and we compare it with the open loop case in Figure [[fig:opt_stiff_hac_dvf_L_psd_disp_error]].
Similarly, the Cumulative Amplitude Spectrum is shown in Figure [[fig:opt_stiff_hac_dvf_L_cas_disp_error]].
Finally, the time domain position error signals are shown in Figure [[fig:opt_stiff_hac_dvf_L_pos_error]].
#+begin_src matlab :exports none
n_av = 4;
han_win = hanning(ceil(length(simout.Em.En.Data(:,1))/n_av));
#+end_src
#+begin_src matlab
t = simout.Em.En.Time;
Ts = t(2)-t(1);
@ -1068,8 +1115,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(f, sqrt(pxx_ol(:, 1)))
plot(f, sqrt(pxx_ol(:, 1)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 1, i)))
end
hold off;
@ -1079,8 +1127,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax2 = subplot(2, 3, 2);
hold on;
plot(f, sqrt(pxx_ol(:, 2)))
plot(f, sqrt(pxx_ol(:, 2)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 2, i)))
end
hold off;
@ -1090,8 +1139,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax3 = subplot(2, 3, 3);
hold on;
plot(f, sqrt(pxx_ol(:, 3)))
plot(f, sqrt(pxx_ol(:, 3)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 3, i)))
end
hold off;
@ -1101,8 +1151,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax4 = subplot(2, 3, 4);
hold on;
plot(f, sqrt(pxx_ol(:, 4)))
plot(f, sqrt(pxx_ol(:, 4)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 4, i)))
end
hold off;
@ -1112,8 +1163,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax5 = subplot(2, 3, 5);
hold on;
plot(f, sqrt(pxx_ol(:, 5)))
plot(f, sqrt(pxx_ol(:, 5)), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 5, i)))
end
hold off;
@ -1123,8 +1175,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(pxx_ol(:, 6)), 'DisplayName', '$\mu$-Station')
plot(f, sqrt(pxx_ol(:, 6)), 'k-', 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(pxx_dvf_L(:, 6, i)), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)))
end
@ -1138,12 +1191,22 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_psd_disp_error.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:opt_stiff_hac_dvf_L_psd_disp_error
#+caption: Amplitude Spectral Density of the position error in Open Loop and with the HAC-LAC controller
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_psd_disp_error.png]]
#+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_ol(:, 1))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 1, i))))));
end
hold off;
@ -1154,8 +1217,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
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_ol(:, 2))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 2, i))))));
end
hold off;
@ -1166,8 +1230,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
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_ol(:, 3))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 3, i))))));
end
hold off;
@ -1178,8 +1243,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
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_ol(:, 4))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 4, i))))));
end
hold off;
@ -1190,8 +1256,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
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_ol(:, 5))))), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 5, i))))));
end
hold off;
@ -1202,8 +1269,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax6 = subplot(2, 3, 6);
hold on;
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'DisplayName', '$\mu$-Station')
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_ol(:, 6))))), 'k-', 'DisplayName', '$\mu$-Station')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_dvf_L(:, 6, i))))), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
end
@ -1218,12 +1286,22 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
xlim([f(2), f(end)])
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_cas_disp_error.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:opt_stiff_hac_dvf_L_cas_disp_error
#+caption: Cumulative Amplitude Spectrum of the position error in Open Loop and with the HAC-LAC controller
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_cas_disp_error.png]]
#+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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 1));
end
hold off;
@ -1232,8 +1310,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax2 = subplot(2, 3, 2);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2))
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 2), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 2));
end
hold off;
@ -1242,8 +1321,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax3 = subplot(2, 3, 3);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3))
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 3), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 3));
end
hold off;
@ -1252,8 +1332,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax4 = subplot(2, 3, 4);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4))
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 4), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 4));
end
hold off;
@ -1262,8 +1343,9 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
ax5 = subplot(2, 3, 5);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5))
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 5), 'k-')
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 5));
end
hold off;
@ -1272,9 +1354,10 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
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(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'k-', ...
'DisplayName', '$\mu$-Station');
for i = 1:length(Ms)
set(gca,'ColorOrderIndex',i);
plot(hac_dvf_L{i}.Em.En.Time, hac_dvf_L{i}.Em.En.Data(:, 6), ...
'DisplayName', sprintf('HAC-DVF $m = %.0f kg$', Ms(i)));
end
@ -1286,9 +1369,31 @@ exportFig('figs/opt_stiff_primary_control_L_cas_tot.pdf', 'width', 'full', 'heig
linkaxes([ax1,ax2,ax3,ax4],'x');
xlim([0.5, inf]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/opt_stiff_hac_dvf_L_pos_error.pdf', 'width', 'full', 'height', 'full')
#+end_src
#+name: fig:opt_stiff_hac_dvf_L_pos_error
#+caption: Position Error of the sample during a tomography experiment when no control is applied and with the HAC-DVF control architecture
#+RESULTS:
[[file:figs/opt_stiff_hac_dvf_L_pos_error.png]]
** Conclusion
#+begin_important
#+end_important
* Primary Control in the task space
<<sec:primary_control_X>>
** Introduction :ignore:
In this section, the control architecture shown in Figure [[fig:control_architecture_hac_dvf_pos_X]] is applied and consists of:
- an inner Low Authority Control loop consisting of a decentralized direct velocity control controller
- an outer loop with the primary controller $\bm{K}_\mathcal{X}$ designed in the task space
#+name: fig:control_architecture_hac_dvf_pos_X
#+caption: HAC-LAC architecture
[[file:figs/control_architecture_hac_dvf_pos_X.png]]
** Plant in the task space
Let's look $\bm{G}_\mathcal{X}(s)$.
@ -1572,3 +1677,7 @@ Let's look $\bm{G}_\mathcal{X}(s)$.
#+end_src
** Simulation
** Conclusion
#+begin_important
#+end_important