tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
27f15fdf78
nass-simscape/org/control_cascade.org

887 lines
29 KiB
Org Mode
Raw Normal View History

Add analysis about Cascade Control
2020-03-23 09:44:05 +01:00
#+TITLE: Cascade Control applied on the Simscape Model
: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:
The control architecture we wish here to study is shown in Figure [[fig:cascade_control_architecture]].
#+begin_src latex :file cascade_control_architecture.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[addb={+}{}{}{}{-}, left= of K] (subr) {};
\node[block, align=center, left= of subr] (J) {Inverse\\Kinematics};
\node[block, left= of J] (Kx) {$\bm{K}_\mathcal{X}$};
\node[block, align=center, left= of Kx] (Ex) {Compute\\Pos. Error};
% Connections and labels
\draw[->] (outputF) -- ++(1.0, 0);
\draw[->] ($(outputF) + (0.6, 0)$)node[branch](taum){} node[below]{$\bm{\tau}_m$} |- (Kiff.east);
\draw[->] (Kiff.west) -| (addF.north);
\draw[->] (addF.east) -- (inputF) node[above left]{$\bm{\tau}$};
\draw[->] (outputL) -- ++(1.8, 0);
\draw[->] ($(outputL) + (1.4, 0)$)node[branch]{} node[above]{$d\bm{\mathcal{L}}$} -- ++(0, -1.2) node(Plinse){} -| (subr.south);
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$};
\draw[->] (K.east) -- (addF.west) node[above left=0 and 8pt]{$\bm{\tau}^\prime$};
\draw[->] (outputX) -- ++(2.6, 0);
\draw[->] ($(outputX) + (2.2, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -3.0) -| (Ex.south);
\draw[<-] (Ex.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-1, 0);
\draw[->] (Ex.east) -- (Kx.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
\draw[->] (Kx.east) -- (J.west) node[above left=0 and 6pt]{$\bm{r}_{\mathcal{X}_n}$};
\draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$};
\begin{scope}[on background layer]
\node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, opacity=0, inner sep=10pt] (Pdamped) {};
\node[fit={(Pdamped.north-|J.west) (Plinse)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Plin) {};
\node[anchor={north west}] at (Plin.north west){$P_\text{lin}$};
\node[fit={(P.south-|addF.west) (taum.east|-Kiff.north)}, fill=black!40!white, draw, dashed, inner sep=10pt] (Pdamped) {};
\node[anchor={north west}] at (Pdamped.north west){$P_\text{damped}$};
\end{scope}
\end{tikzpicture}
#+end_src
#+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]]
This cascade control is designed in three steps:
- In section [[sec:lac_iff]]: an active damping controller is designed.
This is based on the Integral Force Feedback and applied in a decentralized way
- In section [[sec:hac_joint_space]]: a decentralized tracking control is designed in the frame of the legs.
This controller is based on the displacement of each of the legs
- In section [[sec:primary_controller]]: a controller is designed in the task space in order to follow the wanted reference path corresponding to the sample position with respect to the granite
* 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 piezoelectric hexapod and the sample has a mass of 50kg.
#+begin_src matlab
initializeNanoHexapod('actuator', 'piezo');
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
Open Loop.
#+begin_src matlab
initializeController('type', 'cascade-hac-lac');
#+end_src
And we put some gravity.
#+begin_src matlab
initializeSimscapeConfiguration('gravity', true);
#+end_src
We log the signals.
#+begin_src matlab
initializeLoggingConfiguration('log', 'all');
#+end_src
#+begin_src matlab
Kx = 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
#+begin_src matlab :exports none
freqs = logspace(0, 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', 'northwest');
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_iff_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_iff_plant
#+caption: IFF Plant ([[./figs/cascade_iff_plant.png][png]], [[./figs/cascade_iff_plant.pdf][pdf]])
[[file:figs/cascade_iff_plant.png]]
** Root Locus
#+begin_src matlab :exports none
gains = logspace(0, 4, 500);
figure;
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
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_iff_root_locus.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_iff_root_locus
#+caption: Root Locus for the IFF control ([[./figs/cascade_iff_root_locus.png][png]], [[./figs/cascade_iff_root_locus.pdf][pdf]])
[[file:figs/cascade_iff_root_locus.png]]
The maximum damping is obtained for a control gain of $\approx 3000$.
** 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_iff_loop_gain]].
#+begin_src matlab
w0 = 2*pi*50;
Kiff = -3000/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_iff_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_iff_loop_gain
#+caption: Obtained Loop gain the IFF Control ([[./figs/cascade_iff_loop_gain.png][png]], [[./figs/cascade_iff_loop_gain.pdf][pdf]])
[[file:figs/cascade_iff_loop_gain.png]]
* High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
<<sec:hac_joint_space>>
** Identification of the damped plant
We now identify the transfer function from $\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 obtain plant is shown in Figure [[fig:cascade_hac_joint_plant]].
We can see that the plant is quite well decoupled.
#+begin_src matlab :exports none
freqs = logspace(0, 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_hac_joint_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_hac_joint_plant
#+caption: Plant for the High Authority Control in the Joint Space ([[./figs/cascade_hac_joint_plant.png][png]], [[./figs/cascade_hac_joint_plant.pdf][pdf]])
[[file:figs/cascade_hac_joint_plant.png]]
** Controller Design and Loop Gain
The controller consists of:
- A pure integrator
- A Second integrator up to half the wanted bandwidth
- A Lead around the cross-over frequency
- A low pass filter with a cut-off equal to two times the wanted bandwidth
#+begin_src matlab
wc = 2*pi*400; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
% Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/2));
Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/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(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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_hac_joint_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_hac_joint_loop_gain
#+caption: Loop Gain for the High Autority Control in the joint space ([[./figs/cascade_hac_joint_loop_gain.png][png]], [[./figs/cascade_hac_joint_loop_gain.pdf][pdf]])
[[file:figs/cascade_hac_joint_loop_gain.png]]
#+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/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
Gx = linearize(mdl, io, 0);
Gx.InputName = {'rL1', 'rL2', 'rL3', 'rL4', 'rL5', 'rL6'};
Gx.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
#+end_src
As before, we take the minimum realization.
#+begin_src matlab
isstable(Gx)
Gx = minreal(Gx);
isstable(Gx)
#+end_src
** Obtained Plant
#+begin_src matlab :exports none
freqs = logspace(0, 4, 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(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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_primary_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_primary_plant
#+caption: Plant for the Primary Controller ([[./figs/cascade_primary_plant.png][png]], [[./figs/cascade_primary_plant.pdf][pdf]])
[[file:figs/cascade_primary_plant.png]]
** Controller Design
#+begin_src matlab
wc = 2*pi*10; % Bandwidth Bandwidth [rad/s]
h = 2; % Lead parameter
Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * (s + 2*pi*5)/s * 1/(1+s/2/pi/20);
% Normalization of the gain of have a loop gain of 1 at frequency wc
Add tomography experiment for the cascade control
2020-03-23 09:54:16 +01:00
Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc))));
Add analysis about Cascade Control
2020-03-23 09:44:05 +01:00
#+end_src
#+begin_src matlab :exports none
freqs = logspace(0, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 1:6
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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/cascade_primary_loop_gain.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_primary_loop_gain
#+caption: Loop Gain for the primary controller (outer loop) ([[./figs/cascade_primary_loop_gain.png][png]], [[./figs/cascade_primary_loop_gain.pdf][pdf]])
[[file:figs/cascade_primary_loop_gain.png]]
* 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
Add tomography experiment for the cascade control
2020-03-23 09:54:16 +01:00
#+begin_src matlab
cascade_hac_lac = simout;
save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac');
#+end_src
* Results
#+begin_src matlab
load('./mat/experiment_tomography.mat', 'tomo_align_dist');
load('./mat/cascade_hac_lac.mat', 'cascade_hac_lac');
#+end_src
Add PSD and CAS analysis for cascade control
2020-03-23 14:45:26 +01:00
#+begin_src matlab
n_av = 4;
han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(:,1))/n_av));
#+end_src
#+begin_src matlab
t = cascade_hac_lac.Em.En.Time;
Ts = t(2)-t(1);
[pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1/Ts);
[pxx_ca, ~] = pwelch(cascade_hac_lac.Em.En.Data, han_win, [], [], 1/Ts);
#+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_ca(:, 1)))
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_x}$ [$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_ca(:, 2)))
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_y}$ [$m/\sqrt{Hz}$]');
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_ca(:, 3)))
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{D_z}$ [$m/\sqrt{Hz}$]');
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_ca(:, 4)))
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_x}$ [$rad/\sqrt{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_ca(:, 5)))
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_y}$ [$rad/\sqrt{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_ca(:, 6)), 'DisplayName', 'Cascade')
hold off;
xlabel('Frequency [Hz]');
ylabel('$\Gamma_{R_z}$ [$rad/\sqrt{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/cascade_hac_lac_tomography_psd.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_hac_lac_tomography_psd
#+caption: ASD of the position error ([[./figs/cascade_hac_lac_tomography_psd.png][png]], [[./figs/cascade_hac_lac_tomography_psd.pdf][pdf]])
[[file:figs/cascade_hac_lac_tomography_psd.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_ca(:, 1))))))
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_x$ [$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_ca(:, 2))))))
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_y$ [$m$]');
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_ca(:, 3))))))
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $D_z$ [$m$]');
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_ca(:, 4))))))
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_x$ [$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_ca(:, 5))))))
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_y$ [$rad$]');
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_ca(:, 6))))), 'DisplayName', 'Cascade')
hold off;
xlabel('Frequency [Hz]');
ylabel('CAS $R_z$ [$rad$]');
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/cascade_hac_lac_tomography_cas.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_hac_lac_tomography_cas
#+caption: Cumulative Amplitude Spectrum of the position error ([[./figs/cascade_hac_lac_tomography_cas.png][png]], [[./figs/cascade_hac_lac_tomography_cas.pdf][pdf]])
[[file:figs/cascade_hac_lac_tomography_cas.png]]
Add tomography experiment for the cascade control
2020-03-23 09:54:16 +01:00
#+begin_src matlab :exports none
figure;
ax1 = subplot(2, 3, 1);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 1))
plot(cascade_hac_lac.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 1))
hold off;
xlabel('Time [s]');
ylabel('Dx [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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 2))
hold off;
xlabel('Time [s]');
ylabel('Dy [m]');
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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 3))
hold off;
xlabel('Time [s]');
ylabel('Dz [m]');
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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 4))
hold off;
xlabel('Time [s]');
ylabel('Rx [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.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 5))
hold off;
xlabel('Time [s]');
ylabel('Ry [rad]');
ax6 = subplot(2, 3, 6);
hold on;
plot(tomo_align_dist.Em.En.Time, tomo_align_dist.Em.En.Data(:, 6), 'DisplayName', '$\mu$-Station')
Update one legend
2020-03-23 10:49:09 +01:00
plot(cascade_hac_lac.Em.En.Time, cascade_hac_lac.Em.En.Data(:, 6), 'DisplayName', 'Cascade')
Add tomography experiment for the cascade control
2020-03-23 09:54:16 +01:00
hold off;
xlabel('Time [s]');
ylabel('Rz [rad]');
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/cascade_hac_lac_tomography.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:cascade_hac_lac_tomography
#+caption: Results of the Tomography Experiment ([[./figs/cascade_hac_lac_tomography.png][png]], [[./figs/cascade_hac_lac_tomography.pdf][pdf]])
[[file:figs/cascade_hac_lac_tomography.png]]
Reference in New Issue Copy Permalink