Add some analysis on Voice coil with high mass
This commit is contained in:
@@ -60,9 +60,9 @@ The control architecture we wish here to study is shown in Figure [[fig:cascade_
|
||||
\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, left= of J] (Kp) {$\bm{K}_\mathcal{X}$};
|
||||
|
||||
\node[block, align=center, left= of Kx] (Ex) {Compute\\Pos. Error};
|
||||
\node[block, align=center, left= of Kp] (Ex) {Compute\\Pos. Error};
|
||||
|
||||
% Connections and labels
|
||||
\draw[->] (outputF) -- ++(1.0, 0);
|
||||
@@ -79,8 +79,8 @@ The control architecture we wish here to study is shown in Figure [[fig:cascade_
|
||||
\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[->] (Ex.east) -- (Kp.west) node[above left]{$\bm{r}_{\mathcal{X}}$};
|
||||
\draw[->] (Kp.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]
|
||||
@@ -169,7 +169,7 @@ We log the signals.
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Kx = tf(zeros(6));
|
||||
Kp = tf(zeros(6));
|
||||
Kl = tf(zeros(6));
|
||||
Kiff = tf(zeros(6));
|
||||
#+end_src
|
||||
@@ -510,7 +510,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
|
||||
%% 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, '/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
|
||||
@@ -603,10 +603,10 @@ As before, we take the minimum realization.
|
||||
|
||||
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);
|
||||
Kp = (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
|
||||
Kx = Kx.*diag(1./diag(abs(freqresp(Gx*Kx, wc))));
|
||||
Kp = Kp.*diag(1./diag(abs(freqresp(Gx*Kp, wc))));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@@ -617,7 +617,7 @@ As before, we take the minimum realization.
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kx(i,i), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(i, i)*Kp(i,i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
@@ -626,7 +626,7 @@ As before, we take the minimum realization.
|
||||
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'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(i, i)*Kp(i,i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
|
||||
@@ -44,15 +44,15 @@
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The goal here is to study the use of a voice coil based nano-hexapod.
|
||||
That is to say a nano-hexapod with a very small stiffness.
|
||||
* 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:
|
||||
** 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
|
||||
@@ -69,7 +69,7 @@ That is to say a nano-hexapod with a very small stiffness.
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
* Initialization
|
||||
** Initialization
|
||||
We initialize all the stages with the default parameters.
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
@@ -111,14 +111,14 @@ We log the signals.
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Kx = tf(zeros(6));
|
||||
Kp = tf(zeros(6));
|
||||
Kl = tf(zeros(6));
|
||||
Kiff = tf(zeros(6));
|
||||
#+end_src
|
||||
|
||||
* Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
|
||||
** Low Authority Control - Integral Force Feedback $\bm{K}_\text{IFF}$
|
||||
<<sec:lac_iff>>
|
||||
** Identification
|
||||
*** Identification
|
||||
Let's first identify the plant for the IFF controller.
|
||||
#+begin_src matlab
|
||||
%% Name of the Simulink File
|
||||
@@ -135,7 +135,7 @@ Let's first identify the plant for the IFF controller.
|
||||
G_iff.OutputName = {'Fnlm1', 'Fnlm2', 'Fnlm3', 'Fnlm4', 'Fnlm5', 'Fnlm6'};
|
||||
#+end_src
|
||||
|
||||
** Plant
|
||||
*** Plant
|
||||
The obtained plant for IFF is shown in Figure [[fig:cascade_vc_iff_plant]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@@ -206,7 +206,7 @@ The obtained plant for IFF is shown in Figure [[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
|
||||
*** 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).
|
||||
@@ -259,7 +259,7 @@ The maximum damping is obtained for a control gain of $\approx 70$.
|
||||
#+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
|
||||
*** 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
|
||||
@@ -303,9 +303,9 @@ The obtained loop gain is shown in Figure [[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}$
|
||||
** High Authority Control in the joint space - $\bm{K}_\mathcal{L}$
|
||||
<<sec:hac_joint_space>>
|
||||
** Identification of the damped plant
|
||||
*** 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
|
||||
@@ -331,7 +331,7 @@ These unstable poles are probably not physical, and they disappear when taking t
|
||||
isstable(Gl)
|
||||
#+end_src
|
||||
|
||||
** Obtained Plant
|
||||
*** Obtained Plant
|
||||
The obtained dynamics is shown in Figure [[fig:cascade_vc_hac_joint_plant]].
|
||||
|
||||
Few things can be said on the dynamics:
|
||||
@@ -407,7 +407,7 @@ Few things can be said on the dynamics:
|
||||
#+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
|
||||
*** Controller Design and Loop Gain
|
||||
As the plant is well decoupled, a diagonal plant is designed.
|
||||
|
||||
#+begin_src matlab
|
||||
@@ -415,15 +415,7 @@ As the plant is well decoupled, a diagonal plant is designed.
|
||||
|
||||
h = 2; % Lead parameter
|
||||
|
||||
Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * ... % Lead
|
||||
(s + 2*pi*1)/s * ... % Weak Integrator
|
||||
(s + 2*pi*10)/s;
|
||||
|
||||
% 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 * ... % Weak Integrator
|
||||
% (s + 2*pi*10)/s * ... % Weak Integrator
|
||||
% 1/(1 + s/2/wc); % Low pass filter after crossover
|
||||
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))));
|
||||
@@ -461,9 +453,9 @@ As the plant is well decoupled, a diagonal plant is designed.
|
||||
isstable(feedback(Gl*Kl, eye(6), -1))
|
||||
#+end_src
|
||||
|
||||
* Primary Controller in the task space - $\bm{K}_\mathcal{X}$
|
||||
** Primary Controller in the task space - $\bm{K}_\mathcal{X}$
|
||||
<<sec:primary_controller>>
|
||||
** Identification of the linearized plant
|
||||
*** 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
|
||||
@@ -471,15 +463,16 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
|
||||
%% 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, '/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
|
||||
G = linearize(mdl, io, 0);
|
||||
G.InputName = {'rl1', 'rl2', 'rl3', 'rl4', 'rl5', 'rl6'};
|
||||
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||
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);
|
||||
@@ -495,7 +488,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
Gpl.OutputName = {'El1', 'El2', 'El3', 'El4', 'El5', 'El6'};
|
||||
#+end_src
|
||||
|
||||
** Obtained Plant
|
||||
*** Obtained Plant
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
@@ -557,6 +550,16 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
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);
|
||||
|
||||
@@ -618,21 +621,28 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
** Controller Design
|
||||
#+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
|
||||
|
||||
Kx = (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 * ...
|
||||
(s + 2*pi*10)/s * ...
|
||||
1/(1+s/2/wc); % For Piezo
|
||||
% Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * (s + 2*pi*10)/s * (s + 2*pi*1)/s ; % For voice coil
|
||||
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
|
||||
Kx = Kx.*diag(1./diag(abs(freqresp(Gpx*Kx, wc))));
|
||||
Kp = Kp.*diag(1./diag(abs(freqresp(Gpx*Kp, wc))));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
@@ -643,7 +653,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kx(i,i), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(Gpx(i, i)*Kp(i,i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
@@ -652,7 +662,7 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpx(i, i)*Kx(i,i), freqs, 'Hz'))));
|
||||
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');
|
||||
@@ -663,11 +673,35 @@ We know identify the dynamics between $\bm{r}_{\mathcal{X}_n}$ and $\bm{r}_\math
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :tangle no
|
||||
isstable(feedback(Gpx*Kx, eye(6), -1))
|
||||
#+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
|
||||
|
||||
* Simulation
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
set_param(conf_simulink, 'StopTime', '2');
|
||||
@@ -683,14 +717,14 @@ And we simulate the system.
|
||||
save('./mat/cascade_hac_lac.mat', 'cascade_hac_lac_lorentz', '-append');
|
||||
#+end_src
|
||||
|
||||
* Results
|
||||
** Load the simulation results
|
||||
** 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
|
||||
*** Control effort
|
||||
#+begin_src matlab :exports none
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
@@ -747,7 +781,7 @@ And we simulate the system.
|
||||
#+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
|
||||
*** Load the simulation results
|
||||
#+begin_src matlab
|
||||
n_av = 4;
|
||||
han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(:,1))/n_av));
|
||||
@@ -946,7 +980,538 @@ And we simulate the system.
|
||||
[[file:figs/exp_tomography_voice_coil_time_domain.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:
|
||||
|
||||
|
||||
Reference in New Issue
Block a user