2343 lines
75 KiB
Org Mode
2343 lines
75 KiB
Org Mode
#+TITLE: Control of the NASS with Voice coil actuators
|
|
:DRAWER:
|
|
#+STARTUP: overview
|
|
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_LINK_HOME: ./index.html
|
|
#+HTML_LINK_UP: ./index.html
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
|
|
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>
|
|
|
|
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :results none
|
|
#+PROPERTY: header-args:matlab+ :exports both
|
|
#+PROPERTY: header-args:matlab+ :eval no-export
|
|
#+PROPERTY: header-args:matlab+ :output-dir figs
|
|
#+PROPERTY: header-args:matlab+ :tangle no
|
|
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
|
|
|
#+PROPERTY: header-args:shell :eval no-export
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/org/}{config.tex}")
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
|
#+PROPERTY: header-args:latex+ :results file raw replace
|
|
#+PROPERTY: header-args:latex+ :buffer no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports results
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
|
:END:
|
|
|
|
* Introduction :ignore:
|
|
* HAC-LAC + Cascade Control Topology
|
|
** Introduction :ignore:
|
|
|
|
#+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]]
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+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
|
|
|
|
** Initialization
|
|
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
|
|
Kp = tf(zeros(6));
|
|
Kl = tf(zeros(6));
|
|
Kiff = tf(zeros(6));
|
|
#+end_src
|
|
|
|
** Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
|
|
<<sec:lac_iff>>
|
|
*** Identification
|
|
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
|
|
|
|
*** Plant
|
|
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]]
|
|
|
|
*** Root Locus
|
|
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]]
|
|
|
|
*** Controller and Loop Gain
|
|
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]]
|
|
|
|
** High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
|
|
<<sec:hac_joint_space>>
|
|
*** Identification of the damped plant
|
|
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
|
|
|
|
*** Obtained Plant
|
|
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]]
|
|
|
|
*** Controller Design and Loop Gain
|
|
As the plant is well decoupled, a diagonal plant is designed.
|
|
|
|
#+begin_src matlab
|
|
wc = 2*pi*10; % Bandwidth Bandwidth [rad/s]
|
|
|
|
h = 2; % Lead parameter
|
|
|
|
Kl = (s + 2*pi*1)/s;
|
|
|
|
% 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(-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
|
|
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
|
|
|
|
** Primary Controller in the task space - $\bm{K}_\mathcal{X}$
|
|
<<sec:primary_controller>>
|
|
*** Identification of the linearized plant
|
|
We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\mathcal{X}$.
|
|
#+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 = linearize(mdl, io, 0);
|
|
Gp.InputName = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
|
|
Gp.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
|
#+end_src
|
|
|
|
A minus sign is added because the minus sign is already included in the plant identification.
|
|
#+begin_src matlab
|
|
isstable(Gp)
|
|
Gp = -minreal(Gp);
|
|
isstable(Gp)
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
load('mat/stages.mat', 'nano_hexapod');
|
|
Gpx = Gp*inv(nano_hexapod.J');
|
|
Gpx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
|
|
Gpl = nano_hexapod.J*Gp;
|
|
Gpl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
|
|
#+end_src
|
|
|
|
*** Obtained Plant
|
|
#+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
|
|
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
|
|
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
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gpx(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/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]]
|
|
|
|
|
|
#+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(Gpl(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(Gpl(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(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gpl(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(Gpl(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
|
end
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpl(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/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
|
|
#+begin_src matlab
|
|
wc = 2*pi*200; % 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); % 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
|
|
|
|
% Normalization of the gain of have a loop gain of 1 at frequency wc
|
|
Kp = Kp.*diag(1./diag(abs(freqresp(Gpx*Kp, 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(Gpx(i, i)*Kp(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(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/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
|
|
Kp = inv(nano_hexapod.J')*Kp;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
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')
|
|
#+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
|
|
|
|
** Results
|
|
*** Load the simulation results
|
|
#+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
|
|
|
|
*** Control effort
|
|
#+begin_src matlab :exports none
|
|
load('mat/stages.mat', 'nano_hexapod');
|
|
|
|
F_pz = [nano_hexapod.J'*cascade_hac_lac.u.Data']';
|
|
F_vc = [nano_hexapod.J'*cascade_hac_lac_lorentz.u.Data']';
|
|
|
|
% F_pz = [-nano_hexapod.J'*cascade_hac_lac.yn.Fnlm.Data']';
|
|
% F_vc = [-nano_hexapod.J'*cascade_hac_lac_lorentz.yn.Fnlm.Data']';
|
|
#+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
|
|
|
|
#+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]]
|
|
|
|
*** Load the simulation results
|
|
#+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;
|
|
ylabel('$\Gamma_{D_x}$, $\Gamma_{D_y}$, $\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
|
|
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;
|
|
ylabel('$\Gamma_{R_x}$, $\Gamma_{R_y}$, $\Gamma_{R_z}$ [$rad/\sqrt{Hz}$]');
|
|
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')
|
|
plot(f, sqrt(pxx_vc(:, 6)), 'DisplayName', 'Voice-Coil')
|
|
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
|
|
|
|
#+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]]
|
|
|
|
#+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;
|
|
ylabel('CAS $D_x$, $D_y$, $D_z$ [$m$]');
|
|
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]');
|
|
ylabel('CAS $R_x$, $R_y$, $R_z$ [$rad$]');
|
|
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')
|
|
plot(f, sqrt(flip(-cumtrapz(flip(f), flip(pxx_vc(:, 6))))), 'DisplayName', 'Voice Coil')
|
|
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
|
|
|
|
#+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]]
|
|
|
|
#+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;
|
|
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.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]');
|
|
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.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')
|
|
plot(cascade_hac_lac_lorentz.Em.En.Time, cascade_hac_lac_lorentz.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
|
|
|
|
#+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]]
|
|
|
|
|
|
** 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]]
|
|
|
|
*** 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]]
|
|
|
|
|
|
** 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');
|
|
Gpx_m = Gp_m*inv(nano_hexapod.J');
|
|
Gpx_m.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
|
|
Gpl_m = nano_hexapod.J*Gp_m;
|
|
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')
|
|
Kp_x = nano_hexapod.J'*Kp;
|
|
#+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
|
|
|
|
* Other analysis
|
|
** Robustness to Payload Variability
|
|
- [ ] For 3/masses (1kg, 10kg, 50kg), plot each of the 3 plants
|
|
|
|
** 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');
|
|
Gx = G*inv(nano_hexapod.J');
|
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
|
|
Gl = nano_hexapod.J*G;
|
|
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
|
|
Gx = G*inv(nano_hexapod.J');
|
|
#+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
|
|
Gl = nano_hexapod.J*G;
|
|
#+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');
|
|
Gx = G*inv(nano_hexapod.J');
|
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
|
|
Gl = nano_hexapod.J*G;
|
|
Gl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
|
|
#+end_src
|
|
|
|
*** Conclusion
|
|
#+begin_important
|
|
DVF can be used instead of IFF.
|
|
#+end_important
|