3772 lines
131 KiB
Org Mode
3772 lines
131 KiB
Org Mode
#+TITLE: Nano-Hexapod
|
|
#+SETUPFILE: ./setup/org-setup-file.org
|
|
|
|
#+begin_export html
|
|
<hr>
|
|
<p>This report is also available as a <a href="./nano_hexapod.pdf">pdf</a>.</p>
|
|
<hr>
|
|
#+end_export
|
|
|
|
* Introduction :ignore:
|
|
|
|
In this document, a Simscape model of the nano-hexapod is developed and studied.
|
|
|
|
It is structured as follows:
|
|
- Section [[sec:nano_hexapod]]: the simscape model of the nano-hexapod is presented. Few of its elements can be configured as wanted. The effect of the configuration on the obtained dynamics is studied.
|
|
- Section [[sec:integral_force_feedback]]: Direct Velocity Feedback is applied and the obtained damping is derived.
|
|
- Section [[sec:direct_velocity_feedback_struts]]: the encoders are fixed to the struts, and Integral Force Feedback is applied. The obtained damping is computed.
|
|
- Section [[sec:direct_velocity_feedback_plates]]: the same is done when the encoders are fixed on the plates
|
|
|
|
* Nano-Hexapod
|
|
<<sec:nano_hexapod>>
|
|
** Introduction :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
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
simulinkproject('../');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
addpath('nano_hexapod');
|
|
open('nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
addpath('matlab/nano_hexapod');
|
|
open('matlab/nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'nano_hexapod';
|
|
#+end_src
|
|
|
|
** Nano Hexapod - Configuration
|
|
<<sec:nano_hexapod_conf>>
|
|
*** Introduction :ignore:
|
|
The nano-hexapod can be initialized and configured using the =initializeNanoHexapodFinal= function ([[sec:initializeNanoHexapodFinal][link]]).
|
|
|
|
The following code would produce the model shown in Figure [[fig:nano_hexapod_simscape_encoder_struts]].
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'MO_B', 150e-3);
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_simscape_encoder_struts
|
|
#+caption: 3D view of the Sismcape model for the Nano-Hexapod
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/nano_hexapod_simscape_encoder_struts.png]]
|
|
|
|
Several elements on the nano-hexapod can be configured:
|
|
- The flexible joints (Section [[sec:conf_flexible_joint]])
|
|
- The amplified piezoelectric actuators (Section [[sec:conf_apa]])
|
|
- The encoders (Section [[sec:conf_encoders]])
|
|
- The Jacobian matrices (Section [[sec:conf_jacobian]])
|
|
|
|
*** Flexible Joints
|
|
<<sec:conf_flexible_joint>>
|
|
|
|
The model of the flexible joint is composed of 3 solid bodies as shown in Figure [[fig:simscape_model_flexible_joint]] which are connected by joints representing the flexibility of the joint.
|
|
|
|
We can represent:
|
|
- the bending flexibility $k_{R_x}$, $k_{R_y}$
|
|
- the torsional flexibility $k_{R_z}$
|
|
- the axial flexibility $k_z$
|
|
|
|
The configurations and the represented flexibilities are summarized in Table [[tab:flex_type_conf]].
|
|
|
|
#+name: tab:flex_type_conf
|
|
#+caption: Flexible joint's configuration and associated represented flexibility
|
|
#+attr_latex: :environment tabularx :width 0.6\linewidth :align lXXX
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| =flex_type= | Bending | Torsional | Axial |
|
|
|-------------+---------+-----------+-------|
|
|
| =2dof= | x | | |
|
|
| =3dof= | x | x | |
|
|
| =4dof= | x | x | x |
|
|
|
|
Of course, adding more DoF for the flexible joint will induce an addition of many states for the nano-hexapod simscape model.
|
|
|
|
#+name: fig:simscape_model_flexible_joint
|
|
#+caption: 3D view of the Sismcape model for the Flexible joint (4DoF configuration)
|
|
#+attr_latex: :width 0.8\linewidth
|
|
[[file:figs/simscape_model_flexible_joint.png]]
|
|
|
|
*** Amplified Piezoelectric Actuators
|
|
<<sec:conf_apa>>
|
|
|
|
The nano-hexapod's struts are containing one amplified piezoelectric actuator (APA300ML from Cedrat Technologies).
|
|
|
|
The APA can be modeled in different ways which can be configured with the =actuator_type= argument.
|
|
|
|
The simplest model is a 2-DoF system shown in Figure [[fig:2dof_apa_model]].
|
|
|
|
#+name: fig:2dof_apa_model
|
|
#+caption: Schematic of the 2DoF model for the Amplified Piezoelectric Actuator
|
|
[[file:figs/2dof_apa_model.png]]
|
|
|
|
Then, a more complex model based on a Finite Element Model can be used.
|
|
|
|
*** Encoders
|
|
<<sec:conf_encoders>>
|
|
|
|
The encoders can be either fixed directly on the struts (Figure [[fig:encoder_struts]]) or on the two plates (Figure [[fig:encoders_plates_with_apa]]).
|
|
|
|
This can be configured with the =motion_sensor_type= parameters which can be equal to ='struts'= or ='plates'=.
|
|
|
|
#+name: fig:encoder_struts
|
|
#+caption: 3D view of the Encoders fixed on the struts
|
|
#+attr_latex: :width 0.8\linewidth
|
|
[[file:figs/encoder_struts.png]]
|
|
|
|
#+name: fig:encoders_plates_with_apa
|
|
#+caption: 3D view of the Encoders fixed on the plates
|
|
#+attr_latex: :width 0.6\linewidth
|
|
[[file:figs/encoders_plates_with_apa.png]]
|
|
|
|
A complete view of the nano-hexapod with encoders fixed to the struts is shown in Figure [[fig:nano_hexapod_simscape_encoder_struts]] while it is shown in Figure [[fig:nano_hexapod_simscape_encoder_plates]] when the encoders are fixed to the plates.
|
|
|
|
#+name: fig:nano_hexapod_simscape_encoder_plates
|
|
#+caption: Nano-Hexapod with encoders fixed to the plates
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/nano_hexapod_simscape_encoder_plates.png]]
|
|
|
|
The encoder model is schematically represented in Figure [[fig:simscape_encoder_model]]:
|
|
- a frame {B}, fixed to the ruler is positioned on its top surface
|
|
- a frame {F}, rigidly fixed to the encoder is initially positioned such that its origin is aligned with the x axis of frame {B}
|
|
|
|
The output measurement is then the x displacement of the origin of the frame {F} expressed in frame {B}.
|
|
|
|
#+name: fig:simscape_encoder_model
|
|
#+caption: Schematic of the encoder model
|
|
[[file:figs/simscape_encoder_model.png]]
|
|
|
|
If the encoder is experiencing some tilt, it is then "converted" into a measured displacement as shown in Figure [[fig:simscape_encoder_model_disp]].
|
|
|
|
#+name: fig:simscape_encoder_model_disp
|
|
#+caption: Schematic of the encoder model
|
|
[[file:figs/simscape_encoder_model_disp.png]]
|
|
|
|
*** Jacobians
|
|
<<sec:conf_jacobian>>
|
|
|
|
While the Jacobian configuration will not change the physical system, it is still quite an important part of the model.
|
|
|
|
This configuration consists on defining the location of the frame {B} in which the Jacobian will be computed.
|
|
This Jacobian is then used to transform the actuator forces to forces/torques applied on the payload and expressed in frame {B}.
|
|
Same thing can be done for the measured encoder displacements.
|
|
|
|
** Effect of encoders on the decentralized plant
|
|
<<sec:effect_encoder_location>>
|
|
|
|
We here wish to compare the plant from actuators to the encoders when the encoders are either fixed on the struts or on the plates.
|
|
|
|
We initialize the identification parameters.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
#+end_src
|
|
|
|
Identify the plant when the encoders are on the struts:
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
|
|
Gs = linearize(mdl, io, 0.0, options);
|
|
Gs.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
And identify the plant when the encoders are fixed on the plates:
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof');
|
|
|
|
Gp = linearize(mdl, io, 0.0, options);
|
|
Gp.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gp.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
The obtained plants are compared in Figure [[fig:nano_hexapod_effect_encoder]].
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gs('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', 'Struts');
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gp('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', 'Plates');
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gp(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast');
|
|
ylim([1e-11, 1e-6]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-');
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gp(['D', num2str(i)], ['F', num2str(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_effect_encoder.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_effect_encoder
|
|
#+caption: Comparison of the plants from actuator to associated encoder when the encoders are either fixed to the struts or to the plates
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_effect_encoder.png]]
|
|
|
|
#+begin_important
|
|
The zeros at 280Hz should corresponds to resonances of the system when one of the APA is blocked.
|
|
It is linked to the axial stiffness of the flexible joints: increasing the axial stiffness of the joints will increase the frequency of the zeros.
|
|
This will be verified in
|
|
#+end_important
|
|
|
|
** Effect of APA flexibility
|
|
<<sec:effect_apa_flexibility>>
|
|
|
|
First identify the plant for APA represented by 2DoF system:
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'actuator_Ga', 2);
|
|
|
|
Gs = linearize(mdl, io, 0.0, options);
|
|
Gs.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
First identify the plant for APA represented by a flexible element:
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible');
|
|
|
|
Gf = linearize(mdl, io, 0.0, options);
|
|
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gf.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gs('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', '2DoF');
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gf('D1', 'F1'), freqs, 'Hz'))), '-', 'DisplayName', 'Flexible');
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gf(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'northeast');
|
|
ylim([1e-11, 1e-6]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gs(['D', num2str(i)], ['F', num2str(i)]), freqs, 'Hz'))), '-');
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf(['D', num2str(i)], ['F', num2str(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_effect_flexible_apa.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_effect_flexible_apa
|
|
#+caption: Comparison of the plants from actuator to associated strut encoder when the APA are modelled with a 2DoF system of with a flexible one
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_effect_flexible_apa.png]]
|
|
|
|
#+begin_question
|
|
The first resonance is strange when using the flexible APA model (Figure [[fig:nano_hexapod_effect_flexible_apa]]).
|
|
Moreover the system is unstable.
|
|
Otherwise, the 2DoF model matches quite well the flexible model considering its simplicity.
|
|
#+end_question
|
|
|
|
** Nano Hexapod - Number of DoF
|
|
<<sec:num_states>>
|
|
|
|
In this section, we wish to see how the configuration of each element changes the number of the states of the obtained system.
|
|
|
|
The most minimalist model is the following:
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
|
'flex_top_type', '2dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
G = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results
|
|
sprintf('There are %i states.', length(G.statename))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: There are 24 states.
|
|
|
|
These states are summarized on table [[tab:num_states_nano_hexapod]].
|
|
|
|
#+name: tab:num_states_nano_hexapod
|
|
#+caption: Number of states for the minimalist model
|
|
#+attr_latex: :environment tabularx :width 0.25\linewidth :align Xc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| Element | States |
|
|
|-----------+--------|
|
|
| Struts | 2*6 |
|
|
| Top Plate | 12 |
|
|
|-----------+--------|
|
|
| Total: | 24 |
|
|
#+TBLFM: @>$2=vsum(@I..@II)
|
|
|
|
If we add axial stiffness on the top joints, we should add 2 states for each struts.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '2dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
G = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results
|
|
sprintf('There are %i states.', length(G.statename))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: There are 36 states.
|
|
|
|
If we add torsional stiffness on the bottom joints, we should again add 2 states for each struts.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
G = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results
|
|
sprintf('There are %i states.', length(G.statename))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: There are 48 states.
|
|
|
|
Finally, if we add axial stiffness on the bottom joint, we should add 2 states for each struts.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
G = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results
|
|
sprintf('There are %i states.', length(G.statename))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: There are 60 states.
|
|
|
|
#+begin_important
|
|
Obtained number of states is very comprehensible.
|
|
Depending on the physical effects we want to model, we therefore know how many states are added when configuring the model.
|
|
#+end_important
|
|
|
|
** Direct Velocity Feedback Plant
|
|
<<sec:dvf_plant>>
|
|
|
|
The transfer function from actuator forces $\tau_i$ to the encoder measurements $\mathcal{L}_i$ is now identified both when the encoders are fixed to the struts.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative displacements [m]
|
|
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
|
|
Gdvf = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The DC gain from actuator to relative motion sensor should be equal to (for the 2dof APA):
|
|
\[ \frac{1}{k + k_a + kk_a/k_e} \]
|
|
|
|
Which is equal to:
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('DCgain = %.2e [m/N]', 1/(n_hexapod.actuator.k(1) + n_hexapod.actuator.ka(1) + n_hexapod.actuator.k(1)*n_hexapod.actuator.ka(1)/n_hexapod.actuator.ke(1)))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: DCgain = 1.87e-08 [m/N]
|
|
|
|
Let's verify that by looking at the DC gain of the $6 \times 6$ DVF plant in Table [[tab:dvf_dc_gain]].
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
dcgain(Gdvf)
|
|
#+end_src
|
|
|
|
#+name: tab:dvf_dc_gain
|
|
#+caption: DC gain of the DVF plant
|
|
#+attr_latex: :environment tabularx :width 0.8\linewidth :align cccccc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
#+RESULTS:
|
|
| 1.8617e-08 | -1.0408e-10 | 1.3034e-10 | 3.2559e-11 | -1.1188e-10 | 9.0385e-11 |
|
|
| -5.1839e-11 | 1.8593e-08 | -4.4868e-11 | 8.016e-11 | 4.3558e-11 | -1.1164e-10 |
|
|
| 5.1963e-12 | -6.9001e-12 | 1.8564e-08 | 3.0844e-11 | 4.0097e-11 | -3.4387e-11 |
|
|
| 1.9359e-11 | 1.7432e-10 | -5.0928e-11 | 1.855e-08 | 1.6406e-10 | 4.5757e-12 |
|
|
| -2.1185e-11 | 2.1724e-10 | 1.5333e-12 | -8.802e-11 | 1.8803e-08 | -4.6946e-11 |
|
|
| -1.1728e-11 | -5.7682e-11 | 1.6213e-10 | 2.1934e-12 | -1.6237e-10 | 1.8715e-08 |
|
|
|
|
And the bode plot of the DVF plant is shown in Figure [[fig:nano_hexapod_struts_2dof_dvf_plant]].
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_i$')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_struts_2dof_dvf_plant.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_struts_2dof_dvf_plant
|
|
#+caption: Bode plot of the transfer functions from actuator forces $\tau_i$ to relative motion sensors attached to the struts $\mathcal{L}_i$. Diagonal terms are shown in blue, and off-diagonal terms in black.
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_struts_2dof_dvf_plant.png]]
|
|
|
|
** Integral Force Feedback Plant
|
|
<<sec:iff_plant>>
|
|
|
|
The transfer function from actuators to force sensors is identified.
|
|
It corresponds to the plant for the Integral Force Feedback (IFF) control strategy.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
|
|
Giff = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The DC gain from actuator to the force sensor should be equal to (for the 2dof APA):
|
|
\[ \frac{k_e k_1}{k_1 k_e + k_a k_1 + k_e k_a} \]
|
|
|
|
Which is equal to:
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
sprintf('DCgain = %.2e [m/N]', n_hexapod.actuator.ke(1)*n_hexapod.actuator.k(1)/(n_hexapod.actuator.k(1)*n_hexapod.actuator.ke(1) + n_hexapod.actuator.ka(1)*n_hexapod.actuator.k(1) + n_hexapod.actuator.ke(1)*n_hexapod.actuator.ka(1)))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: DCgain = 6.56e-03 [m/N]
|
|
|
|
This is indeed close to what we see on DC gain of the $6 \times 6$ IFF plant in Table [[tab:iff_dc_gain]].
|
|
Also, the off diagonal terms are quite small compared to the diagonal terms.
|
|
#+begin_src matlab :exports results :results value table replace :tangle no
|
|
data2orgtable(dcgain(Giff), {}, {}, ' %.4f ');
|
|
#+end_src
|
|
|
|
#+name: tab:iff_dc_gain
|
|
#+caption: DC gain of the IFF plant
|
|
#+attr_latex: :environment tabularx :width 0.8\linewidth :align cccccc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
#+RESULTS:
|
|
| 0.0068 | 0.0002 | -0.0002 | -0.0 | 0.0002 | -0.0001 |
|
|
| 0.0003 | 0.0068 | 0.0001 | -0.0001 | -0.0001 | 0.0002 |
|
|
| 0.0001 | 0.0 | 0.0068 | -0.0001 | -0.0001 | 0.0001 |
|
|
| -0.0002 | -0.0003 | 0.0001 | 0.0068 | -0.0002 | -0.0 |
|
|
| -0.0001 | -0.0003 | -0.0 | 0.0001 | 0.0065 | 0.0 |
|
|
| 0.0001 | 0.0001 | -0.0002 | -0.0 | 0.0002 | 0.0066 |
|
|
|
|
The bode plot is shown in Figure [[fig:nano_hexapod_struts_2dof_iff_plant]].
|
|
#+begin_src matlab :exports none
|
|
freqs = 2*logspace(1, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, 20*abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$F_{m,i}/\tau_i$')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 20*abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$F_{m,i}/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_struts_2dof_iff_plant.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_struts_2dof_iff_plant
|
|
#+caption: Bode plot of the transfer functions from actuator forces $\tau_i$ to force sensors $F_{m,i}$. Diagonal terms are shown in blue, and off-diagonal terms in black.
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_struts_2dof_iff_plant.png]]
|
|
|
|
** Decentralized Plant - Cartesian coordinates
|
|
<<sec:decoupled_plant>>
|
|
*** Introduction :ignore:
|
|
Consider the plant shown in Figure [[fig:nano_hexapod_decentralized_schematic]] with:
|
|
- $\tau$ the 6 input forces (APA)
|
|
- $d\mathcal{L}$ the relative motion sensor outputs (encoders)
|
|
- $\mathcal{X}$ the motion of the top platform measured with "perfect" 6-dof sensor
|
|
- $J_a$ and $J_s$ the Jacobians for the actuators and sensors
|
|
|
|
#+begin_src latex :file nano_hexapod_decentralized_schematic.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{2.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at (P.west);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);
|
|
\coordinate[] (outputX) at ($(P.south east)!0.2!(P.north east)$);
|
|
|
|
\node[block, left= of inputF] (Ja) {$\bm{J}^{-T}_a$};
|
|
\node[block, right= of outputL] (Js) {$\bm{J}^{-1}_s$};
|
|
|
|
% Connections and labels
|
|
\draw[->] ($(Ja.west)+(-1,0)$) -- (Ja.west) node[above left]{$\bm{\mathcal{F}}$};
|
|
\draw[->] (Ja.east) -- (inputF) node[above left]{$\bm{\tau}$};
|
|
\draw[->] (outputL) -- (Js.west) node[above left]{$d\bm{\mathcal{L}}$};
|
|
\draw[->] (Js.east) -- ++(1, 0) node[above left]{$d\bm{\mathcal{X}}$};
|
|
\draw[->] (outputX.east) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_decentralized_schematic
|
|
#+caption: Plant in the cartesian Frame
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_decentralized_schematic.png]]
|
|
|
|
*** Verification of the Sensor Jacobian
|
|
The "perfect" sensor output $\mathcal{X}$ is used to verify that the sensor Jacobian is working correctly both when the encoders are fixed to the struts and to the plates.
|
|
|
|
Let's then identify the plant for both configuration, and compare the transfer functions from $\mathcal{F}$ to $d\mathcal{X}$ and to $\mathcal{X}$.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
#+end_src
|
|
|
|
Start when the encoders are fixed on the struts.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'MO_B', 150e-3);
|
|
Gs = linearize(mdl, io, 0.0, options);
|
|
Gs.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ...
|
|
'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
|
|
% Cartesian plant using the Jacobians
|
|
Gsc = inv(n_hexapod.geometry.Js)*Gs({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
|
|
% Cartesian plant using the perfect sensor
|
|
Gsp = -Gs({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
|
|
#+end_src
|
|
|
|
The diagonal elements of the plant are shown in Figure [[fig:nano_hexapod_comp_cartesian_plants_struts]].
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gsp(i,i), freqs, 'Hz'))), '--', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest');
|
|
ylim([1e-11, 1e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsp(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_comp_cartesian_plants_struts.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_comp_cartesian_plants_struts
|
|
#+caption: Bode plot of the diagonal elements of the decentralized (cartesian) plant when using the sensor Jacobian (solid) and when using "perfect" 6dof sensor (dashed). The encoders are fixed on the struts.
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_comp_cartesian_plants_struts.png]]
|
|
|
|
The same if performed when the encoders are fixed to the plates.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof', ...
|
|
'MO_B', 150e-3);
|
|
Gp = linearize(mdl, io, 0.0, options);
|
|
Gp.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gp.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ...
|
|
'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
|
|
% Cartesian plant using the Jacobians
|
|
Gpc = inv(n_hexapod.geometry.Js)*Gp({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
|
|
% Cartesian plant using the perfect sensor
|
|
Gpp = -Gp({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
|
|
#+end_src
|
|
|
|
The obtained bode plots are shown in Figure [[fig:nano_hexapod_comp_cartesian_plants_plates]].
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gpc(i,i), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gpp(i,i), freqs, 'Hz'))), '--', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest');
|
|
ylim([1e-11, 1e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpc(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpp(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_comp_cartesian_plants_plates.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_comp_cartesian_plants_plates
|
|
#+caption: Bode plot of the diagonal elements of the decentralized (cartesian) plant when using the sensor Jacobian (solid) and when using "perfect" 6dof sensor (dashed). The encoders are fixed on the plates.
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_comp_cartesian_plants_plates.png]]
|
|
|
|
#+begin_important
|
|
The Jacobian for the encoders is working properly both when the encoders are fixed to the plates or to the struts.
|
|
|
|
However, then the encoders are fixed to the struts, there is a mismatch between the estimated motion and the measured motion above 100Hz due to a complex conjugate zero.
|
|
#+end_important
|
|
|
|
*** Comparison of the decentralized plants
|
|
|
|
The decentralized plants are now compared whether the encoders are fixed on the struts or on the plates in Figure [[fig:nano_hexapod_cartesian_plant_encoder_comp]].
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gpc(i,i), freqs, 'Hz'))), '--', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest');
|
|
ylim([1e-11, 1e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
set(gca,'ColorOrderIndex',1);
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gpc(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_cartesian_plant_encoder_comp.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_cartesian_plant_encoder_comp
|
|
#+caption: Bode plot of the "cartesian" plant (transfer function from $\mathcal{F}$ to $d\mathcal{X}$) when the encoders are fixed on the struts (solid) and on the plates (dashed)
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_cartesian_plant_encoder_comp.png]]
|
|
|
|
#+begin_question
|
|
Is the complex conjugate zeros on Figure [[fig:nano_hexapod_cartesian_plant_encoder_comp]] could be due to the axial stiffness of the joints?
|
|
Let's find out by using a model with no axial flexibility on the joints and see if we still have this zero.
|
|
#+end_question
|
|
|
|
*** Perfect axial stiffness of the joints
|
|
Let's initialize the Nano-Hexapod with perfect joints (no axial stiffness):
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
|
'flex_top_type', '2dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'MO_B', 150e-3);
|
|
#+end_src
|
|
|
|
Identify its dynamics.
|
|
#+begin_src matlab
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ...
|
|
'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
#+end_src
|
|
|
|
And compute the transfer function in the cartesian frame.
|
|
#+begin_src matlab
|
|
G = inv(n_hexapod.geometry.Js)*G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
|
|
#+end_src
|
|
|
|
The obtained dynamics is shown in Figure [[fig:nano_hexapod_cartesian_plant_perfect_joints]].
|
|
It is shown that there are no zeros at high frequency.
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(G(i,i), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest');
|
|
ylim([1e-11, 1e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_cartesian_plant_perfect_joints.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_cartesian_plant_perfect_joints
|
|
#+caption: Bode plot of the "cartesian" plant (transfer function from $\mathcal{F}$ to $d\mathcal{X}$) when the encoders are fixed on the struts and with perfect joints (infinite axial stiffness)
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_cartesian_plant_perfect_joints.png]]
|
|
|
|
#+begin_important
|
|
The zeros appearing between 100Hz and 500Hz are due to the axial stiffness of the flexible joints.
|
|
They appear when the relative motion sensors are fixed to the struts.
|
|
They basically correspond to the resonance frequency where the APA are infinitively stiff.
|
|
#+end_important
|
|
|
|
*** Frequency of the zeros
|
|
Let's verify this last affirmation by generating a nano-hexapod with very stiff APA.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'actuator_k', 1e9*ones(6,1));
|
|
Gs = linearize(mdl, io, 0.0, options);
|
|
Gs.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Gs_p = imag(pole(Gs))/2/pi;
|
|
sort(Gs_p(Gs_p > 0))
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gsc(i,i), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southwest');
|
|
% ylim([1e-11, 1e-4]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsc(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
** Decentralized Plant - Decoupling at the Center of Stiffness
|
|
<<sec:decoupled_plant_cok>>
|
|
|
|
*** Center of Stiffness
|
|
<<sec:center_of_stiffness>>
|
|
|
|
Let's define some parameters that will be used for the computation of the stiffness matrix:
|
|
#+begin_src matlab
|
|
si = n_hexapod.geometry.si; % Orientation of struts
|
|
bi = n_hexapod.geometry.Fb; % Location of bi w.r.t. {F}
|
|
ki = ones(1,6); % Normalized strut stiffness
|
|
#+end_src
|
|
|
|
In order to find is the Center of Stiffness (CoK) exists, we have to verify is the following is diagonal:
|
|
#+begin_src matlab :results value replace :exports both
|
|
ki.*si*si'
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| 1.8977 | 2.4659e-17 | 5.1838e-19 |
|
|
| 2.4659e-17 | 1.8977 | -2.3143e-05 |
|
|
| 5.1838e-19 | -2.3143e-05 | 2.2046 |
|
|
|
|
And we can find the location of the CoK with respect to {F}:
|
|
#+begin_src matlab
|
|
OkX = (ki.*cross(bi, si)*si')/(ki.*si*si');
|
|
Ok = [OkX(3,2);OkX(1,3);OkX(2,1)]
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
ans = Ok
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
| -1.7444e-18 |
|
|
| 2.1511e-06 |
|
|
| 0.052707 |
|
|
|
|
The "center of stiffness" is therefore 52.7mm above the bottom platform {F} frame.
|
|
|
|
Let's initialize the hexapod with frame {A} and {B} at the CoK:
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'MO_B', Ok(3)-95e-3);
|
|
#+end_src
|
|
|
|
And the (normalized) stiffness matrix is computed as follows:
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod.geometry.J'*diag(ki)*n_hexapod.geometry.J
|
|
#+end_src
|
|
|
|
#+name: tab:stiffness_matrix_cok
|
|
#+caption: Normalized Stiffness Matrix - Center of Stiffness
|
|
#+attr_latex: :environment tabularx :width 0.85\linewidth :align cccccc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
#+RESULTS:
|
|
| 1.8977 | 0 | 0 | 0 | -2.0817e-17 | -1.5311e-06 |
|
|
| 0 | 1.8977 | -2.3143e-05 | 4.175e-06 | 0 | 0 |
|
|
| 0 | -2.3143e-05 | 2.2046 | 4.7422e-06 | 0 | 0 |
|
|
| 0 | 4.175e-06 | 4.7422e-06 | 0.012594 | 2.1684e-19 | -8.6736e-19 |
|
|
| -1.8521e-17 | 0 | 0 | 0 | 0.012594 | -9.3183e-08 |
|
|
| -1.5311e-06 | -6.9389e-18 | 2.7756e-17 | -8.6736e-19 | -9.3183e-08 | 0.043362 |
|
|
|
|
And we indeed obtain a diagonal stiffness matrix.
|
|
|
|
*** Obtained plant
|
|
Let's identify the transfer function from $\tau$ to $d\mathcal{L}$ and from $\tau$ to $\mathcal{X}$.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % 6DoF perfect measurement
|
|
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ...
|
|
'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
#+end_src
|
|
|
|
Then use the Jacobian matrices to obtain the "cartesian" centralized plant.
|
|
#+begin_src matlab
|
|
Gc = inv(n_hexapod.geometry.J)*...
|
|
G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*...
|
|
inv(n_hexapod.geometry.J');
|
|
#+end_src
|
|
|
|
The DC gain of the obtained plant is shown in Table [[tab:dc_gain_cartesian_cok_plan]].
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
dcgain(Gc)
|
|
#+end_src
|
|
|
|
#+name: tab:dc_gain_cartesian_cok_plan
|
|
#+caption: DC gain of the centralized plant at the center of stiffness
|
|
#+attr_latex: :environment tabularx :width 0.9\linewidth :align cccccc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
#+RESULTS:
|
|
| 9.8602e-09 | 7.8692e-11 | 1.4426e-11 | -2.663e-10 | 2.8e-10 | -4.7559e-11 |
|
|
| 8.457e-11 | 9.8788e-09 | -2.4002e-11 | -2.9502e-11 | -1.3262e-10 | -8.7346e-11 |
|
|
| -4.3244e-11 | 2.4075e-13 | 8.4775e-09 | 1.1442e-11 | -2.5809e-10 | 2.8796e-11 |
|
|
| -1.8326e-09 | -9.318e-10 | 6.8188e-10 | 1.4697e-06 | 5.5936e-09 | 8.7632e-10 |
|
|
| 4.6906e-10 | 1.5911e-09 | 1.6989e-10 | -5.223e-09 | 1.4729e-06 | -2.6059e-10 |
|
|
| -6.5754e-11 | -3.0408e-12 | 5.394e-11 | -1.0917e-10 | 6.9479e-10 | 4.2979e-07 |
|
|
|
|
As the rotations and translations have very different gains, we normalize each motion to one.
|
|
#+begin_src matlab
|
|
Gc = diag(1./diag(dcgain(Gc)))*Gc;
|
|
#+end_src
|
|
|
|
The diagonal and off-diagonal elements are shown in Figure [[fig:nano_hexapod_diagonal_plant_cok]], and we can see good decoupling at low frequency.
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
labels = {'$D_x/\mathcal{F}_x$', '$D_y/\mathcal{F}_y$', '$D_z/\mathcal{F}_z$', ...
|
|
'$R_x/\mathcal{M}_x$', '$R_y/\mathcal{M}_y$', '$R_z/\mathcal{M}_z$'};
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gc(i,i), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', labels{i});
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(Gc(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', 'Off-Diagonal');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gc(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
|
xlim([freqs(1), freqs(end)]); ylim([1e-8, 1e3])
|
|
legend('location', 'southwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_diagonal_plant_cok.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_diagonal_plant_cok
|
|
#+caption: Diagonal and off-diagonal elements of the (normalized) decentralized plant with the Jacobians estimated at the "center of stiffness"
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_diagonal_plant_cok.png]]
|
|
|
|
#+begin_important
|
|
The Jacobian matrices can be used to decoupled the plant at low frequency.
|
|
#+end_important
|
|
|
|
** Stiffness matrix
|
|
<<sec:stiffness_matrix>>
|
|
*** Introduction :ignore:
|
|
The stiffness matrix of the nano-hexapod describes its induced static displacement/rotation when a force/torque is applied on its top platform.
|
|
The location of the applied force/torque and the expressed displacement/rotation can be defined as wanted.
|
|
Such location (or frame) is then used for the computation of the Jacobian which in turns is used to compute the stiffness matrix.
|
|
|
|
*** Compute the theoretical stiffness of the nano-hexapod
|
|
Neglecting the torsional/bending stiffness of the joints, we have:
|
|
\[ K = J^t \mathcal{K} J \]
|
|
where $\mathcal{K}$ is a diagonal 6x6 matrix with the axial stiffness of the struts on the diagonal.
|
|
|
|
Let's note the axial stiffness of the APA:
|
|
\[ k_{\text{APA}} = k + \frac{k_e k_a}{k_e + k_a} \]
|
|
|
|
The axial stiffness of the struts $k_s$ is then:
|
|
\[ k_s = \frac{k_z k_{\text{APA}}}{k_z + 2 k_{\text{APA}}} \]
|
|
with $k_z$ the axial stiffness of the flexible joints.
|
|
|
|
Let's initialize the nano-hexapod.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
The total axial stiffness of the APA is:
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
kAPA = n_hexapod.actuator.k(1) + n_hexapod.actuator.ke(1)*n_hexapod.actuator.ka(1)/(n_hexapod.actuator.ke(1) + n_hexapod.actuator.ka(1));
|
|
sprintf('kAPA = %.3e [N/m]', kAPA);
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: kAPA = 1.799e+06 [N/m]
|
|
|
|
And the total axial stiffness of the struts is:
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
ks = n_hexapod.flex_top.kz(1)*kAPA/(n_hexapod.flex_top.kz(1) + 2*kAPA);
|
|
sprintf('ks = %.3e [N/m]', ks)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: ks = 1.737e+06 [N/m]
|
|
|
|
#+begin_important
|
|
We can see that the axial stiffness of the flexible joint as little impact on the total axial stiffness of the struts as it is much stiffer than the APA.
|
|
#+end_important
|
|
|
|
Let's now compute the stiffness matrix corresponding to an hexapod with perfect joints and the above computed axial strut stiffness:
|
|
#+begin_src matlab
|
|
Ks = n_hexapod.geometry.J'*(ks*eye(6))*n_hexapod.geometry.J;
|
|
#+end_src
|
|
|
|
And the compliance matrix can be computed as the inverse of the stiffness matrix.
|
|
#+begin_src matlab
|
|
C = inv(Ks);
|
|
#+end_src
|
|
|
|
The obtained compliance matrix is shown in Table [[tab:compliance_matrix_perfect_joints]].
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
ans = C
|
|
#+end_src
|
|
|
|
#+name: tab:compliance_matrix_perfect_joints
|
|
#+caption: Compliance Matrix - Perfect Joints
|
|
#+attr_latex: :environment tabularx :width 0.85\linewidth :align cccccc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
#+RESULTS:
|
|
| 1.9938e-06 | -2.3138e-22 | 3.3403e-23 | 1.0202e-21 | 8.7906e-06 | 2.9603e-11 |
|
|
| -3.1875e-23 | 1.9938e-06 | 2.2094e-11 | -8.7909e-06 | -1.6576e-22 | -3.5622e-28 |
|
|
| 6.6811e-23 | 2.2094e-11 | 2.6115e-07 | -9.8337e-11 | 3.4744e-22 | 7.4663e-28 |
|
|
| 1.4054e-22 | -8.7909e-06 | -9.8337e-11 | 4.5715e-05 | 7.3086e-22 | 1.5706e-27 |
|
|
| 8.7906e-06 | -1.0202e-21 | 1.7371e-22 | 4.498e-21 | 4.5714e-05 | 9.8237e-11 |
|
|
| 2.9603e-11 | -1.9261e-22 | -1.7611e-27 | 8.4925e-22 | 9.8237e-11 | 1.3277e-05 |
|
|
|
|
*** Comparison with Simscape Model
|
|
Let's now identify the compliance matrix using Simscape.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External forces [N, Nm/rad]
|
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Induced motion [m, rad]
|
|
|
|
G = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The DC gain of the identified plant is therefore the compliance matrix of the nano-hexapod.
|
|
It takes into account the bending and torsional stiffness of the flexible joints.
|
|
|
|
The obtained compliance matrix is shown in Table [[tab:compliance_matrix_simscape_joints]].
|
|
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
dcgain(G)
|
|
#+end_src
|
|
|
|
#+name: tab:compliance_matrix_simscape_joints
|
|
#+caption: Compliance Matrix - Estimated from Simscape
|
|
#+attr_latex: :environment tabularx :width 0.85\linewidth :align cccccc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
#+RESULTS:
|
|
| 1.9863e-06 | 2.4683e-08 | 2.2859e-09 | -6.4768e-08 | 8.7307e-06 | 3.2754e-10 |
|
|
| -3.693e-09 | 1.9663e-06 | -7.4503e-09 | -8.615e-06 | -6.7926e-08 | -2.0193e-08 |
|
|
| -3.5525e-09 | -8.8671e-10 | 2.6042e-07 | 4.7504e-09 | -9.9677e-09 | 1.7242e-10 |
|
|
| 2.1133e-08 | -8.6554e-06 | 3.2841e-08 | 4.4849e-05 | 3.0873e-07 | 8.1525e-08 |
|
|
| 8.7375e-06 | 9.9165e-08 | 8.135e-09 | -2.5208e-07 | 4.5331e-05 | 3.0602e-09 |
|
|
| 6.1611e-09 | 6.1733e-09 | 2.6778e-09 | -3.3188e-08 | 3.3887e-08 | 1.3212e-05 |
|
|
|
|
Let's compare the two obtained compliance matrices.
|
|
The results are shown in Table [[tab:compliance_matrices_compare]].
|
|
The diagonal terms are matching quite good, as well as the 1/4 and 2/5 of diagonal terms that are the most important ones.
|
|
|
|
All the other terms are zero in the ideal case (Table [[tab:compliance_matrix_perfect_joints]]).
|
|
There are not zero when considering the bending/torsional stiffness of the joints (Table [[tab:compliance_matrix_simscape_joints]]), however they are still small as compared to the diagonal terms.
|
|
#+begin_src matlab :results value replace :exports both :tangle no
|
|
C./dcgain(G)
|
|
#+end_src
|
|
|
|
#+name: tab:compliance_matrices_compare
|
|
#+caption: Compliance Matrices - Comparison
|
|
#+attr_latex: :environment tabularx :width 0.85\linewidth :align cccccc
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
#+RESULTS:
|
|
| 1.0037 | -9.3741e-15 | 1.4613e-14 | -1.5751e-14 | 1.0069 | 0.090379 |
|
|
| 8.6313e-15 | 1.014 | -0.0029656 | 1.0204 | 2.4404e-15 | 1.7641e-20 |
|
|
| -1.8807e-14 | -0.024917 | 1.0028 | -0.020701 | -3.4857e-14 | 4.3304e-18 |
|
|
| 6.6502e-15 | 1.0157 | -0.0029943 | 1.0193 | 2.3673e-15 | 1.9265e-20 |
|
|
| 1.0061 | -1.0288e-14 | 2.1354e-14 | -1.7844e-14 | 1.0085 | 0.032102 |
|
|
| 0.0048048 | -3.1201e-14 | -6.5769e-19 | -2.5589e-14 | 0.002899 | 1.005 |
|
|
|
|
* Active Damping using Integral Force Feedback
|
|
<<sec:integral_force_feedback>>
|
|
** Introduction :ignore:
|
|
In this section *Integral Force Feedback* (IFF) strategy is used to damp the nano-hexapod resonances.
|
|
|
|
It is structured as follows:
|
|
- Section [[sec:iff_identification]]: the IFF plant is identified
|
|
- Section [[sec:iff_controller]]: the optimal control gain is identified using the Root Locus plot
|
|
- Section [[sec:iff_effect_plant]]: the IFF is applied, and the effect on the damped plant is identified and compared with the un-damped one
|
|
- Section [[sec:iff_effect_compliance]]: the IFF is applied, and the effect on the compliance is identified
|
|
|
|
** 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 :eval no
|
|
addpath('nano_hexapod');
|
|
open('nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
addpath('matlab/nano_hexapod');
|
|
open('matlab/nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'nano_hexapod';
|
|
#+end_src
|
|
|
|
** Plant Identification
|
|
<<sec:iff_identification>>
|
|
|
|
The nano-hexapod is initialized as usual.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
The transfer function from actuator inputs to force sensors outputs is identified.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
Giff = linearize(mdl, io, 0.0, options);
|
|
Giff.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Giff.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
|
|
#+end_src
|
|
|
|
Its bode plot is shown in Figure
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$F_{m,i}/\tau_i$')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$F_{m,i}/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_iff_plant_bode_plot.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_iff_plant_bode_plot
|
|
#+caption: Integral Force Feedback plant
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_iff_plant_bode_plot.png]]
|
|
|
|
** Root Locus
|
|
<<sec:iff_controller>>
|
|
|
|
The controller is a diagonal (i.e. decentralized) controller with simple low pass filters (i.e. pseudo integrators) on the diagonal:
|
|
\begin{equation}
|
|
K_{\text{IFF}} = \frac{g}{s + \omega_c} \bm{I}_{6 \times 6}
|
|
\end{equation}
|
|
|
|
The value of $\omega_c$ has quite a large impact both on the attainable damping and on the compliance degradation at low frequency.
|
|
|
|
It is here chosen to have quite a large $\omega_c$ in order to not modify the plant at low frequency.
|
|
#+begin_src matlab
|
|
wc = 2*pi*20;
|
|
#+end_src
|
|
|
|
The obtained Root Locus is shown in Figure [[fig:nano_hexapod_iff_root_locus]].
|
|
The control gain chosen for future plots is shown by the red crosses.
|
|
#+begin_src matlab :exports none
|
|
gains = logspace(2, 6, 200);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
% Pure Integrator
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', '$g = 0$');
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
|
|
for g = gains
|
|
clpoles = pole(feedback(Giff, (g/(s + wc))*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
|
|
end
|
|
|
|
g = 1.5e4
|
|
clpoles = pole(feedback(Giff, (g/(s + wc))*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), 'rx', 'DisplayName', '$g = 1.5 \cdot 10^4$');
|
|
axis square;
|
|
xlim([-225, 5]); ylim([-10, 450]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_iff_root_locus
|
|
#+caption: Root locus for the decentralized IFF control strategy
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_iff_root_locus.png]]
|
|
|
|
#+begin_question
|
|
Are the zeros shown in the Root Locus (Figure [[fig:nano_hexapod_iff_root_locus]]) are the poles of the nano-hexapod when the active part of the strut ($k_e$ and $k_a$) is removed?
|
|
This will be checked in the next section.
|
|
#+end_question
|
|
|
|
The obtained controller is then:
|
|
#+begin_src matlab
|
|
Kiff = 1.5e4/(s + 2*pi*20)*eye(6); % IFF Controller
|
|
#+end_src
|
|
|
|
The corresponding loop gain of the diagonal terms are shown in Figure [[fig:nano_hexapod_iff_loop_gain]].
|
|
It is shown that the loop gain is quite large around resonances (which allows to add lots of damping) and less than one at low frequency thanks to the large value of $\omega_c$.
|
|
#+begin_src matlab :exports none
|
|
Piff = Giff*Kiff; % Loop Gain
|
|
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Piff(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Piff(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_iff_loop_gain.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_iff_loop_gain
|
|
#+caption: Loop gain of the diagonal terms $G(i,i) \cdot K_{\text{IFF}}(i,i)$
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_iff_loop_gain.png]]
|
|
|
|
** Frequency of the zeros of the IFF plant
|
|
In order to find the zeros of the IFF plant, the hexapod is initialized with quasi null actuator stiffness and the system is identified.
|
|
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'actuator_ke', ones(6,1)*1e-1, ...
|
|
'actuator_ka', ones(6,1)*1e-1);
|
|
|
|
Giff_z = linearize(mdl, io, 0.0, options);
|
|
Giff_z.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Giff_z.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
|
|
#+end_src
|
|
|
|
Then, we can look at the frequency of the poles.
|
|
#+begin_src matlab :results value replace :exports results :tangle no
|
|
G_z = pole(Giff_z);
|
|
ans = sort(imag(G_z(imag(G_z)>0)))
|
|
#+end_src
|
|
|
|
#+name: tab:iff_frequency_zero
|
|
#+caption: Frequency of the poles corresponding to the IFF zeros (Hz).
|
|
#+attr_latex: :environment tabularx :width 0.2\linewidth :align c
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| 50.750 |
|
|
| 51.483 |
|
|
| 117.94 |
|
|
| 157.79 |
|
|
| 197.21 |
|
|
| 198.47 |
|
|
|
|
And we indeed find the frequency of the zeros for the IFF plant.
|
|
|
|
** Effect of IFF on the plant
|
|
<<sec:iff_effect_plant>>
|
|
|
|
Let's now see how the IFF control strategy effectively damps the plant and how it affects the transfer functions from the actuator forces to the relative motion sensors (encoders).
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motions
|
|
#+end_src
|
|
|
|
First identify the plant in open-loop.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'none');
|
|
|
|
Gol = linearize(mdl, io, 0.0, options);
|
|
Gol.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gol.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
And then with the IFF controller.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'iff');
|
|
|
|
Giff = linearize(mdl, io, 0.0, options);
|
|
Giff.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Giff.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
The obtained plants are compared in Figure [[fig:nano_hexapod_effect_iff_plant]].
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gol(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_i$ - OL')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_i$ - IFF')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_effect_iff_plant.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_effect_iff_plant
|
|
#+caption: Bode plots of the transfer functions from actuator forces $\tau_i$ to relative motion sensors $\mathcal{L}_i$ with and without the IFF controller.
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_effect_iff_plant.png]]
|
|
|
|
#+begin_important
|
|
The Integral Force Feedback Strategy is very effective to damp the 6 suspension modes of the nano-hexapod.
|
|
#+end_important
|
|
|
|
** Effect of IFF on the compliance
|
|
<<sec:iff_effect_compliance>>
|
|
|
|
The IFF strategy has the well known drawback of degrading the compliance (transfer function from external forces/torques applied to the top platform to the motion of the top platform), especially at low frequency where the control gain is large.
|
|
Let's quantify that for the nano-hexapod.
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motions
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the compliance in Open Loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'none');
|
|
|
|
Gol = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the compliance with the IFF controller
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'iff');
|
|
|
|
Giff = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The obtained compliances are compared in Figure
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gol(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{X}/F_e$ - OL')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{X}/F_e$ - IFF')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N, rad/Nm]');
|
|
legend('location', 'southeast');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_iff_compare_compliance.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_iff_compare_compliance
|
|
#+caption: Comparison of the compliances in Open Loop and with Integral Force Feedback controller
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_iff_compare_compliance.png]]
|
|
|
|
#+begin_important
|
|
The use of IFF induces a degradation of the compliance.
|
|
This degradation is limited due to the use of a pseudo integrator (instead of a pure integrator).
|
|
Also, it should not be a major problem for the NASS, as no direct forces should be applied to the top platform.
|
|
#+end_important
|
|
|
|
* Active Damping using Direct Velocity Feedback - Encoders on the struts
|
|
<<sec:direct_velocity_feedback_struts>>
|
|
** Introduction :ignore:
|
|
In this section, the *Direct Velocity Feedback* (DVF) strategy is used to damp the nano-hexapod resonances.
|
|
|
|
It is structured as follows:
|
|
- Section [[sec:dvf_identification_struts]]: the DVF plant is identified
|
|
- Section [[sec:dvf_controller_struts]]: the optimal control gain is identified using the Root Locus plot
|
|
- Section [[sec:dvf_effect_plant_struts]]: the DVF is applied, and the effect on the damped plant is identified and compared with the un-damped one
|
|
- Section [[sec:dvf_effect_compliance_struts]]: the DVF is applied, and the effect on the compliance is identified
|
|
|
|
** 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 :eval no
|
|
addpath('nano_hexapod');
|
|
open('nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
addpath('matlab/nano_hexapod');
|
|
open('matlab/nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'nano_hexapod';
|
|
#+end_src
|
|
|
|
** Plant Identification
|
|
<<sec:dvf_identification_struts>>
|
|
|
|
The nano-hexapod is initialized as usual.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
The transfer function from actuator inputs to force sensors outputs is identified.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
|
|
Gdvf = linearize(mdl, io, 0.0, options);
|
|
Gdvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gdvf.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
Its bode plot is shown in Figure [[fig:nano_hexapod_dvf_plant_bode_plot_struts]].
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_i/\tau_i$')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\mathcal{L}_i/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_plant_bode_plot_struts.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_plant_bode_plot_struts
|
|
#+caption: Direct Velocity Feedback plant
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_plant_bode_plot_struts.png]]
|
|
|
|
** Root Locus
|
|
<<sec:dvf_controller_struts>>
|
|
|
|
The controller is a diagonal (i.e. decentralized) controller with simple high pass filters (i.e. pseudo derivators) on the diagonal:
|
|
\begin{equation}
|
|
K_{\text{DVF}} = g \frac{s}{s + \omega_d} \bm{I}_{6 \times 6}
|
|
\end{equation}
|
|
|
|
The value of $\omega_d$ sets the frequency above high the derivative action is stopped.
|
|
|
|
#+begin_src matlab
|
|
wd = 2*pi*150;
|
|
#+end_src
|
|
|
|
The obtained Root Locus is shown in Figure [[fig:nano_hexapod_dvf_root_locus_struts]].
|
|
The control gain chosen for future plots is shown by the red crosses.
|
|
#+begin_src matlab :exports none
|
|
gains = logspace(6, 11, 200);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
% Pure Integrator
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(pole(Gdvf)), imag(pole(Gdvf)), 'x', 'DisplayName', '$g = 0$');
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(tzero(Gdvf)), imag(tzero(Gdvf)), 'o', 'HandleVisibility', 'off');
|
|
for g = gains
|
|
clpoles = pole(feedback(Gdvf, (g*s/(s + wd))*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
|
|
end
|
|
|
|
g = 5e8;
|
|
clpoles = pole(feedback(Gdvf, (g*s/(s + wd))*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), 'rx', 'DisplayName', '$g = 5 \cdot 10^8$');
|
|
axis square;
|
|
xlim([-350, 5]); ylim([-10, 2500]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northwest');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_root_locus_struts.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_root_locus_struts
|
|
#+caption: Root locus for the decentralized DVF control strategy
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_root_locus_struts.png]]
|
|
|
|
|
|
The obtained controller is then:
|
|
#+begin_src matlab
|
|
Kdvf = 5e8*s/(s + wd)*eye(6); % DVF Controller
|
|
#+end_src
|
|
|
|
The corresponding loop gain of the diagonal terms are shown in Figure [[fig:nano_hexapod_dvf_loop_gain_struts]].
|
|
It is shown that the loop gain is quite large around resonances (which allows to add lots of damping) and less than one at low frequency thanks to the large value of $\omega_c$.
|
|
#+begin_src matlab :exports none
|
|
Pdvf = Gdvf*Kdvf; % Loop Gain
|
|
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Pdvf(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Pdvf(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_loop_gain_struts.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_loop_gain_struts
|
|
#+caption: Loop gain of the diagonal terms $G(i,i) \cdot K_{\text{DVF}}(i,i)$
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_loop_gain_struts.png]]
|
|
|
|
** Effect of DVF on the plant
|
|
<<sec:dvf_effect_plant_struts>>
|
|
|
|
Let's now see how the DVF control strategy effectively damps the plant and how it affects the transfer functions from the actuator forces to the relative motion sensors (encoders).
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'output'); io_i = io_i + 1; % Relative Motions
|
|
#+end_src
|
|
|
|
First identify the plant in open-loop.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'none');
|
|
|
|
Gol = linearize(mdl, io, 0.0, options);
|
|
Gol.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gol.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
And then with the DVF controller.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'dvf');
|
|
|
|
Gdvf = linearize(mdl, io, 0.0, options);
|
|
Gdvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gdvf.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
The obtained plants are compared in Figure [[fig:nano_hexapod_effect_dvf_plant_struts]].
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gol(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_i$ - OL')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_i$ - DVF')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_effect_dvf_plant_struts.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_effect_dvf_plant_struts
|
|
#+caption: Bode plots of the transfer functions from actuator forces $\tau_i$ to relative motion sensors $\mathcal{L}_i$ with and without the DVF controller.
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_effect_dvf_plant_struts.png]]
|
|
|
|
#+begin_important
|
|
The Direct Velocity Feedback Strategy is very effective to damp the 6 suspension modes of the nano-hexapod.
|
|
#+end_important
|
|
|
|
** Effect of DVF on the compliance
|
|
<<sec:dvf_effect_compliance_struts>>
|
|
|
|
The DVF strategy has the well known drawback of degrading the compliance (transfer function from external forces/torques applied to the top platform to the motion of the top platform), especially at low frequency where the control gain is large.
|
|
Let's quantify that for the nano-hexapod.
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motions
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the compliance in Open Loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'none');
|
|
|
|
Gol = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the compliance with the DVF controller
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'dvf');
|
|
|
|
Gdvf = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The obtained compliances are compared in Figure [[fig:nano_hexapod_dvf_compare_compliance_struts]].
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gol(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{X}/F_e$ - OL')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{X}/F_e$ - DVF')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N, rad/Nm]');
|
|
legend('location', 'southeast');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_compare_compliance_struts.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_compare_compliance_struts
|
|
#+caption: Comparison of the compliances in Open Loop and with Direct Velocity Feedback controller
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_compare_compliance_struts.png]]
|
|
|
|
* Active Damping using Direct Velocity Feedback - Encoders on the plates
|
|
<<sec:direct_velocity_feedback_plates>>
|
|
** Introduction :ignore:
|
|
In this section, the *Direct Velocity Feedback* (DVF) strategy is used to damp the nano-hexapod resonances.
|
|
|
|
It is structured as follows:
|
|
- Section [[sec:dvf_identification_plates]]: the DVF plant is identified
|
|
- Section [[sec:dvf_controller_plates]]: the optimal control gain is identified using the Root Locus plot
|
|
- Section [[sec:dvf_effect_plant_plates]]: the DVF is applied, and the effect on the damped plant is identified and compared with the un-damped one
|
|
- Section [[sec:dvf_effect_compliance_plates]]: the DVF is applied, and the effect on the compliance is identified
|
|
|
|
** 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 :eval no
|
|
addpath('nano_hexapod');
|
|
open('nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no
|
|
addpath('matlab/nano_hexapod');
|
|
open('matlab/nano_hexapod/nano_hexapod.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'nano_hexapod';
|
|
#+end_src
|
|
|
|
** Plant Identification
|
|
<<sec:dvf_identification_plates>>
|
|
|
|
The nano-hexapod is initialized as usual.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
The transfer function from actuator inputs to force sensors outputs is identified.
|
|
#+begin_src matlab
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
|
|
Gdvf = linearize(mdl, io, 0.0, options);
|
|
Gdvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gdvf.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
Its bode plot is shown in Figure [[fig:nano_hexapod_dvf_plant_bode_plot_plates]].
|
|
#+begin_src matlab :exports none
|
|
freqs = 5*logspace(0, 2, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_i/\tau_i$')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\mathcal{L}_i/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_plant_bode_plot_plates.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_plant_bode_plot_plates
|
|
#+caption: Direct Velocity Feedback plant
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_plant_bode_plot_plates.png]]
|
|
|
|
** Root Locus
|
|
<<sec:dvf_controller_plates>>
|
|
|
|
The controller is a diagonal (i.e. decentralized) controller with simple high pass filters (i.e. pseudo derivators) on the diagonal:
|
|
\begin{equation}
|
|
K_{\text{DVF}} = g \frac{s}{s + \omega_d} \bm{I}_{6 \times 6}
|
|
\end{equation}
|
|
|
|
The value of $\omega_d$ sets the frequency above high the derivative action is stopped.
|
|
|
|
#+begin_src matlab
|
|
wd = 2*pi*150;
|
|
#+end_src
|
|
|
|
The obtained Root Locus is shown in Figure [[fig:nano_hexapod_dvf_root_locus_plates]].
|
|
The control gain chosen for future plots is shown by the red crosses.
|
|
#+begin_src matlab :exports none
|
|
gains = logspace(6, 11, 200);
|
|
|
|
figure;
|
|
|
|
hold on;
|
|
% Pure Integrator
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(pole(Gdvf)), imag(pole(Gdvf)), 'x', 'DisplayName', '$g = 0$');
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(tzero(Gdvf)), imag(tzero(Gdvf)), 'o', 'HandleVisibility', 'off');
|
|
for g = gains
|
|
clpoles = pole(feedback(Gdvf, (g*s/(s + wd))*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
|
|
end
|
|
|
|
g = 2e8;
|
|
clpoles = pole(feedback(Gdvf, (g*s/(s + wd))*eye(6)));
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(real(clpoles), imag(clpoles), 'rx', 'DisplayName', '$g = 5 \cdot 10^8$');
|
|
axis square;
|
|
xlim([-600, 5]); ylim([-10, 3000]);
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
legend('location', 'northeast');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_root_locus_plates.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_root_locus_plates
|
|
#+caption: Root locus for the decentralized DVF control strategy
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_root_locus_plates.png]]
|
|
|
|
|
|
The obtained controller is then:
|
|
#+begin_src matlab
|
|
Kdvf = 2e8*s/(s + wd)*eye(6); % DVF Controller
|
|
#+end_src
|
|
|
|
The corresponding loop gain of the diagonal terms are shown in Figure [[fig:nano_hexapod_dvf_loop_gain_plates]].
|
|
It is shown that the loop gain is quite large around resonances (which allows to add lots of damping).
|
|
#+begin_src matlab :exports none
|
|
Pdvf = Gdvf*Kdvf; % Loop Gain
|
|
|
|
freqs = 5*logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Pdvf(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Pdvf(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_loop_gain_plates.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_loop_gain_plates
|
|
#+caption: Loop gain of the diagonal terms $G(i,i) \cdot K_{\text{DVF}}(i,i)$
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_loop_gain_plates.png]]
|
|
|
|
** Effect of DVF on the plant
|
|
<<sec:dvf_effect_plant_plates>>
|
|
|
|
Let's now see how the DVF control strategy effectively damps the plant and how it affects the transfer functions from the actuator forces to the relative motion sensors (encoders).
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'output'); io_i = io_i + 1; % Relative Motions
|
|
#+end_src
|
|
|
|
First identify the plant in open-loop.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'none');
|
|
|
|
Gol = linearize(mdl, io, 0.0, options);
|
|
Gol.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gol.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
And then with the DVF controller.
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'dvf');
|
|
|
|
Gdvf = linearize(mdl, io, 0.0, options);
|
|
Gdvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gdvf.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
The obtained plants are compared in Figure [[fig:nano_hexapod_effect_dvf_plant_plates]].
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gol(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_i$ - OL')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_i$ - DVF')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
% Off diagonal terms
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$\mathcal{L}_{i}/\tau_j \quad i \neq j$');
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
legend('location', 'southeast');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-');
|
|
end
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(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');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_effect_dvf_plant_plates.pdf', 'width', 'full', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_effect_dvf_plant_plates
|
|
#+caption: Bode plots of the transfer functions from actuator forces $\tau_i$ to relative motion sensors $\mathcal{L}_i$ with and without the DVF controller.
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_effect_dvf_plant_plates.png]]
|
|
|
|
#+begin_important
|
|
The Direct Velocity Feedback Strategy is very effective in damping the 6 suspension modes of the nano-hexapod.
|
|
#+end_important
|
|
|
|
** Effect of DVF on the compliance
|
|
<<sec:dvf_effect_compliance_plates>>
|
|
|
|
The DVF strategy has the well known drawback of degrading the compliance (transfer function from external forces/torques applied to the top platform to the motion of the top platform), especially at low frequency where the control gain is large.
|
|
Let's quantify that for the nano-hexapod.
|
|
#+begin_src matlab :exports none
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motions
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the compliance in Open Loop
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'none');
|
|
|
|
Gol = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Identification of the compliance with the DVF controller
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof', ...
|
|
'controller_type', 'dvf');
|
|
|
|
Gdvf = linearize(mdl, io, 0.0, options);
|
|
#+end_src
|
|
|
|
The obtained compliances are compared in Figure [[fig:nano_hexapod_dvf_compare_compliance_plates]].
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gol(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{X}/F_e$ - OL')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',1);
|
|
plot(freqs, abs(squeeze(freqresp(Gol(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
|
'DisplayName', '$\mathcal{X}/F_e$ - DVF')
|
|
for i = 1:6
|
|
set(gca,'ColorOrderIndex',2);
|
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N, rad/Nm]');
|
|
legend('location', 'southeast');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nano_hexapod_dvf_compare_compliance_plates.pdf', 'width', 'wide', 'height', 'tall');
|
|
#+end_src
|
|
|
|
#+name: fig:nano_hexapod_dvf_compare_compliance_plates
|
|
#+caption: Comparison of the compliances in Open Loop and with Direct Velocity Feedback controller
|
|
#+RESULTS:
|
|
[[file:figs/nano_hexapod_dvf_compare_compliance_plates.png]]
|
|
|
|
* To-order :noexport:
|
|
** Why Zero when encoder on the struts
|
|
#+begin_src matlab
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '4dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
#+end_src
|
|
|
|
The transfer function from actuator inputs to force sensors outputs is identified.
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'test_apa300ml';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/dLs'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
io(io_i) = linio([mdl, '/dLp'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
|
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'F'};
|
|
G.OutputName = {'dLs', 'dLp'};
|
|
|
|
bodeFig({G(1), G(2)}, logspace(1,4,1000))
|
|
#+end_src
|
|
|
|
The zero seems to be linked to the axial flexibility of the top joint.
|
|
|
|
In this mode, the APA does not experience any motion (hence the zero).
|
|
The resonance frequency then corresponds to the top mass on top of the axial stiffness of the two joints in series.
|
|
|
|
For the nano-hexapod, it corresponds to the resonance of the top mass when all (or *just one*?) of the APA is blocked.
|
|
|
|
#+begin_src matlab
|
|
sqrt((n_hexapod.flex_bot.kz(1)/2)/(55/3))/2/pi
|
|
#+end_src
|
|
|
|
** Verify why unstable strut
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
|
'flex_top_type', '2dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
|
'flex_top_type', '2dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 0
|
|
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', 'flexible');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 0
|
|
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', 'flexible');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 0
|
|
|
|
|
|
** Verify why unstable strut
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
|
'flex_top_type', '2dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
|
'flex_top_type', '2dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'plates', ...
|
|
'actuator_type', '2dof');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
#+begin_src matlab :results value replace
|
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
|
'flex_top_type', '3dof', ...
|
|
'motion_sensor_type', 'struts', ...
|
|
'actuator_type', '2dof');
|
|
isstable(linearize(mdl, io, 0.0, options))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1
|
|
|
|
|
|
** APA300ML structure
|
|
|
|
#+begin_src matlab
|
|
open('matlab/nano_hexapod/apa300ml.slx')
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml_structure';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
io(io_i) = linio([mdl, '/x'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'F', 'Fa'};
|
|
G.OutputName = {'D', 'x'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
dcgain(G('D', 'Fa'))/dcgain(G('x', 'Fa'))
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(G('x', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('x', 'Fa'), 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],'x');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(G('x', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('D', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('x', 'Fa'), 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],'x');
|
|
#+end_src
|
|
|
|
** Full APA300ML
|
|
*** 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
|
|
|
|
*** APA
|
|
#+begin_src matlab
|
|
K = readmatrix('APA300ML_full_mat_K.CSV');
|
|
M = readmatrix('APA300ML_full_mat_M.CSV');
|
|
[int_xyz, int_i, n_xyz, n_i, nodes] = extractNodes('APA300ML_full_out_nodes_3D.txt');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
open('matlab/nano_hexapod/apa300ml.slx')
|
|
#+end_src
|
|
|
|
*** APA Stiffness
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'Fe'};
|
|
G.OutputName = {'L'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace
|
|
1./dcgain(G)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 1796000.0
|
|
|
|
Same as documentation
|
|
|
|
*** Amplification Factor
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Ls'], 1, 'openoutput'); io_i = io_i + 1;
|
|
% io(io_i) = linio([mdl, '/Sensor Stack'], 1, 'openoutput'); io_i = io_i + 1;
|
|
% io(io_i) = linio([mdl, '/Actuator Stack'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'F'};
|
|
G.OutputName = {'L', 'Ls'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results value replace
|
|
abs(dcgain(G('L', 'F'))./dcgain(G('Ls', 'F')))
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: 5.1155
|
|
|
|
Same as estimated.
|
|
|
|
*** Plant
|
|
Identification
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
|
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
|
|
io(io_i) = linio([mdl, '/Sensor Stack'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'F'};
|
|
G.OutputName = {'L', 'dL'};
|
|
#+end_src
|
|
|
|
IFF Plant
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G('dL', 'F'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('dL', 'F'), 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],'x');
|
|
#+end_src
|
|
|
|
DVF Plant
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G('L', 'F'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G('L', 'F'), 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],'x');
|
|
#+end_src
|
|
|
|
*** Reduction from Full APA to 2DoF system
|
|
|
|
#+begin_src matlab
|
|
m = 1; % [kg]
|
|
#+end_src
|
|
|
|
Identification of the full APA model:
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'Fa', 'Fe'};
|
|
G.OutputName = {'Fm', 'dL'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results output replace :exports results :tangle no
|
|
size(G)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: size(G)
|
|
: State-space model with 2 outputs, 2 inputs, and 50 states.
|
|
|
|
#+begin_src matlab
|
|
k = 0.35e6;
|
|
c = 3e1;
|
|
ka = 43e6;
|
|
ke = 1.5e6;
|
|
|
|
Leq = 0.056; % [m]
|
|
#+end_src
|
|
|
|
Identification of the 2-DoF APA model:
|
|
#+begin_src matlab
|
|
open('matlab/nano_hexapod/apa300ml_2dof.slx')
|
|
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml_2dof';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
|
|
|
|
%% Run the linearization
|
|
Gb = linearize(mdl, io, 0.0, options);
|
|
Gb.InputName = {'Fa', 'Fe'};
|
|
Gb.OutputName = {'Fm', 'dL'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results output replace :exports results :tangle no
|
|
size(Gb)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: size(Gb)
|
|
: State-space model with 2 outputs, 2 inputs, and 4 states.
|
|
|
|
From external force to displacement
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fe'), 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],'x');
|
|
#+end_src
|
|
|
|
From external force to force sensor
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fe'), 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],'x');
|
|
#+end_src
|
|
|
|
From Actuator to displacement
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fa'), 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],'x');
|
|
#+end_src
|
|
|
|
From Actuator to Force Sensor
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fa'), 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],'x');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('To vertical motion [m]');
|
|
title('From Actuator $F_a$ [N]');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
title('From External $F_e$ [N]');
|
|
|
|
ax3 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('To Force Sensor [N]');
|
|
|
|
ax4 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
|
linkaxes([ax1,ax2],'y');
|
|
linkaxes([ax3,ax4],'y');
|
|
#+end_src
|
|
|
|
*** Reduction from Full APA to 2DoF system
|
|
|
|
#+begin_src matlab
|
|
Ms = [0.1, 1, 5, 10]; % [kg]
|
|
#+end_src
|
|
|
|
Identification of the full APA model:
|
|
#+begin_src matlab
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {'Fa', 'Fe'};
|
|
G.OutputName = {'Fm', 'dL'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results output replace :exports results :tangle no
|
|
size(G)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: size(G)
|
|
: State-space model with 2 outputs, 2 inputs, and 50 states.
|
|
|
|
#+begin_src matlab
|
|
k = 0.35e6;
|
|
c = 3e1;
|
|
ka = 43e6;
|
|
ke = 1.5e6;
|
|
|
|
Leq = 0.056; % [m]
|
|
#+end_src
|
|
|
|
Identification of the 2-DoF APA model:
|
|
#+begin_src matlab
|
|
open('matlab/nano_hexapod/apa300ml_2dof.slx')
|
|
|
|
%% Options for Linearized
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
|
|
%% Name of the Simulink File
|
|
mdl = 'apa300ml_2dof';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Fa'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force
|
|
io(io_i) = linio([mdl, '/Fe'], 1, 'openinput'); io_i = io_i + 1; % External Force
|
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor
|
|
io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Relative Motion Sensor
|
|
|
|
%% Run the linearization
|
|
Gb = linearize(mdl, io, 0.0, options);
|
|
Gb.InputName = {'Fa', 'Fe'};
|
|
Gb.OutputName = {'Fm', 'dL'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results output replace :exports results :tangle no
|
|
size(Gb)
|
|
#+end_src
|
|
|
|
#+RESULTS:
|
|
: size(Gb)
|
|
: State-space model with 2 outputs, 2 inputs, and 4 states.
|
|
|
|
From external force to displacement
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fe'), 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],'x');
|
|
#+end_src
|
|
|
|
From external force to force sensor
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fe'), 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],'x');
|
|
#+end_src
|
|
|
|
From Actuator to displacement
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('dL', 'Fa'), 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],'x');
|
|
#+end_src
|
|
|
|
From Actuator to Force Sensor
|
|
#+begin_src matlab :exports none
|
|
freqs = logspace(1, 4, 1000);
|
|
|
|
figure;
|
|
|
|
ax1 = subplot(2, 1, 1);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
|
|
ax2 = subplot(2, 1, 2);
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gb('Fm', 'Fa'), 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],'x');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');
|
|
|
|
ax1 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); ylabel('To vertical motion [m]');
|
|
title('From Actuator $F_a$ [N]');
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'dL', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('dL', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
title('From External $F_e$ [N]');
|
|
|
|
ax3 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fa'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fa'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('To Force Sensor [N]');
|
|
|
|
ax4 = nexttile;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G( 'Fm', 'Fe'), freqs, 'Hz'))));
|
|
plot(freqs, abs(squeeze(freqresp(Gb('Fm', 'Fe'), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
|
|
|
|
linkaxes([ax1,ax2,ax3,ax4],'x');
|
|
linkaxes([ax1,ax2],'y');
|
|
linkaxes([ax3,ax4],'y');
|
|
#+end_src
|
|
|
|
* Function - Initialize Nano Hexapod
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle ../src/initializeNanoHexapodFinal.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:initializeNanoHexapodFinal>>
|
|
|
|
** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [nano_hexapod] = initializeNanoHexapodFinal(args)
|
|
#+end_src
|
|
|
|
** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
%% Bottom Flexible Joints
|
|
args.flex_bot_type char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
|
|
args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
|
|
args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
|
|
args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
|
|
args.flex_bot_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
|
|
args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
|
|
args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
|
|
args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
|
|
args.flex_bot_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
|
|
|
|
%% Top Flexible Joints
|
|
args.flex_top_type char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
|
|
args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
|
|
args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
|
|
args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
|
|
args.flex_top_kz (6,1) double {mustBeNumeric} = ones(6,1)*7e7 % Axial Stiffness [N/m]
|
|
args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % X bending Damping [Nm/(rad/s)]
|
|
args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Y bending Damping [Nm/(rad/s)]
|
|
args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Torsionnal Damping [Nm/(rad/s)]
|
|
args.flex_top_cz (6,1) double {mustBeNumeric} = ones(6,1)*0.001 % Axial Damping [N/(m/s)]
|
|
|
|
%% Jacobian - Location of frame {A} and {B}
|
|
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
|
|
|
|
%% Relative Motion Sensor
|
|
args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
|
|
|
|
%% Actuators
|
|
args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
|
|
args.actuator_Ga (6,1) double {mustBeNumeric} = zeros(6,1) % Actuator gain [N/V]
|
|
args.actuator_Gs (6,1) double {mustBeNumeric} = zeros(6,1) % Sensor gain [V/m]
|
|
% For 2DoF
|
|
args.actuator_k (6,1) double {mustBeNumeric} = ones(6,1)*0.38e6 % [N/m]
|
|
args.actuator_ke (6,1) double {mustBeNumeric} = ones(6,1)*1.75e6 % [N/m]
|
|
args.actuator_ka (6,1) double {mustBeNumeric} = ones(6,1)*3e7 % [N/m]
|
|
args.actuator_c (6,1) double {mustBeNumeric} = ones(6,1)*3e1 % [N/(m/s)]
|
|
args.actuator_ce (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
|
|
args.actuator_ca (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
|
|
args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
|
|
% For Flexible Frame
|
|
args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
|
|
args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
|
|
% Misalignment
|
|
args.actuator_d_align (6,3) double {mustBeNumeric} = zeros(6,3) % [m]
|
|
|
|
args.actuator_xi (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
|
|
|
|
%% Controller
|
|
args.controller_type char {mustBeMember(args.controller_type,{'none', 'iff', 'dvf', 'hac-iff-struts'})} = 'none'
|
|
end
|
|
#+end_src
|
|
|
|
** Nano Hexapod Object
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod = struct();
|
|
#+end_src
|
|
|
|
** Flexible Joints - Bot
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod.flex_bot = struct();
|
|
|
|
switch args.flex_bot_type
|
|
case '2dof'
|
|
nano_hexapod.flex_bot.type = 1;
|
|
case '3dof'
|
|
nano_hexapod.flex_bot.type = 2;
|
|
case '4dof'
|
|
nano_hexapod.flex_bot.type = 3;
|
|
case 'flexible'
|
|
nano_hexapod.flex_bot.type = 4;
|
|
end
|
|
|
|
nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
|
|
nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
|
|
nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
|
|
nano_hexapod.flex_bot.kz = args.flex_bot_kz; % Axial stiffness [N/m]
|
|
|
|
nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
|
|
nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
|
|
nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
|
|
nano_hexapod.flex_bot.cz = args.flex_bot_cz; %[N/(m/s)]
|
|
#+end_src
|
|
|
|
** Flexible Joints - Top
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod.flex_top = struct();
|
|
|
|
switch args.flex_top_type
|
|
case '2dof'
|
|
nano_hexapod.flex_top.type = 1;
|
|
case '3dof'
|
|
nano_hexapod.flex_top.type = 2;
|
|
case '4dof'
|
|
nano_hexapod.flex_top.type = 3;
|
|
case 'flexible'
|
|
nano_hexapod.flex_top.type = 4;
|
|
end
|
|
|
|
nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
|
|
nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
|
|
nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
|
|
nano_hexapod.flex_top.kz = args.flex_top_kz; % Axial stiffness [N/m]
|
|
|
|
nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
|
|
nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
|
|
nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
|
|
nano_hexapod.flex_top.cz = args.flex_top_cz; %[N/(m/s)]
|
|
#+end_src
|
|
|
|
** Relative Motion Sensor
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod.motion_sensor = struct();
|
|
|
|
switch args.motion_sensor_type
|
|
case 'struts'
|
|
nano_hexapod.motion_sensor.type = 1;
|
|
case 'plates'
|
|
nano_hexapod.motion_sensor.type = 2;
|
|
end
|
|
#+end_src
|
|
|
|
** Amplified Piezoelectric Actuator
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod.actuator = struct();
|
|
|
|
switch args.actuator_type
|
|
case '2dof'
|
|
nano_hexapod.actuator.type = 1;
|
|
case 'flexible frame'
|
|
nano_hexapod.actuator.type = 2;
|
|
case 'flexible'
|
|
nano_hexapod.actuator.type = 3;
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Actuator gain [N/V]
|
|
if all(args.actuator_Ga == 0)
|
|
switch args.actuator_type
|
|
case '2dof'
|
|
nano_hexapod.actuator.Ga = ones(6,1)*(-30.0);
|
|
case 'flexible frame'
|
|
nano_hexapod.actuator.Ga = ones(6,1); % TODO
|
|
case 'flexible'
|
|
nano_hexapod.actuator.Ga = ones(6,1)*23.4;
|
|
end
|
|
else
|
|
nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
|
|
end
|
|
|
|
%% Sensor gain [V/m]
|
|
if all(args.actuator_Gs == 0)
|
|
switch args.actuator_type
|
|
case '2dof'
|
|
nano_hexapod.actuator.Gs = ones(6,1)*0.098;
|
|
case 'flexible frame'
|
|
nano_hexapod.actuator.Gs = ones(6,1); % TODO
|
|
case 'flexible'
|
|
nano_hexapod.actuator.Gs = ones(6,1)*(-4674824);
|
|
end
|
|
else
|
|
nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]
|
|
end
|
|
#+end_src
|
|
|
|
Mechanical characteristics:
|
|
#+begin_src matlab
|
|
switch args.actuator_type
|
|
case '2dof'
|
|
nano_hexapod.actuator.k = args.actuator_k; % [N/m]
|
|
nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
|
|
nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]
|
|
|
|
nano_hexapod.actuator.c = args.actuator_c; % [N/(m/s)]
|
|
nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
|
|
nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]
|
|
|
|
nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]
|
|
|
|
case 'flexible frame'
|
|
nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
|
|
nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
|
|
nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
|
|
|
|
nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
|
|
nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]
|
|
nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
|
|
|
|
case 'flexible'
|
|
nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
|
|
nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
|
|
nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
|
|
|
|
nano_hexapod.actuator.d_align = args.actuator_d_align; % Misalignment
|
|
nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio
|
|
|
|
end
|
|
|
|
#+end_src
|
|
|
|
** Geometry
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod.geometry = struct();
|
|
#+end_src
|
|
|
|
Center of joints $a_i$ with respect to {F}:
|
|
#+begin_src matlab
|
|
Fa = [[-86.05, -74.78, 22.49],
|
|
[ 86.05, -74.78, 22.49],
|
|
[ 107.79, -37.13, 22.49],
|
|
[ 21.74, 111.91, 22.49],
|
|
[-21.74, 111.91, 22.49],
|
|
[-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]
|
|
#+end_src
|
|
|
|
Center of joints $b_i$ with respect to {M}:
|
|
#+begin_src matlab
|
|
Mb = [[-28.47, -106.25, -22.50],
|
|
[ 28.47, -106.25, -22.50],
|
|
[ 106.25, 28.47, -22.50],
|
|
[ 77.78, 77.78, -22.50],
|
|
[-77.78, 77.78, -22.50],
|
|
[-106.25, 28.47, -22.50]]'*1e-3; % Bi w.r.t. {M} [m]
|
|
#+end_src
|
|
|
|
Now compute the positions $b_i$ with respect to {F}:
|
|
#+begin_src matlab
|
|
Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]
|
|
#+end_src
|
|
|
|
The unit vector representing the orientation of the struts can then be computed:
|
|
#+begin_src matlab
|
|
si = Fb - Fa;
|
|
si = si./vecnorm(si); % Normalize
|
|
#+end_src
|
|
|
|
Location of encoder measurement points when fixed on the plates:
|
|
#+begin_src matlab
|
|
Fc = [[-29.362, -105.765, 52.605]
|
|
[ 29.362, -105.765, 52.605]
|
|
[ 106.276, 27.454, 52.605]
|
|
[ 76.914, 78.31, 52.605]
|
|
[-76.914, 78.31, 52.605]
|
|
[-106.276, 27.454, 52.605]]'*1e-3; % Meas pos w.r.t. {F}
|
|
Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
nano_hexapod.geometry.Fa = Fa;
|
|
nano_hexapod.geometry.Fb = Fb;
|
|
nano_hexapod.geometry.Fc = Fc;
|
|
nano_hexapod.geometry.Mb = Mb;
|
|
nano_hexapod.geometry.Mc = Mc;
|
|
nano_hexapod.geometry.si = si;
|
|
nano_hexapod.geometry.MO_B = args.MO_B;
|
|
#+end_src
|
|
|
|
** Jacobian for Actuators
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
Bb = Mb - [0; 0; args.MO_B];
|
|
|
|
nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];
|
|
#+end_src
|
|
|
|
** Jacobian for Sensors
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
switch args.motion_sensor_type
|
|
case 'struts'
|
|
nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
|
|
case 'plates'
|
|
Bc = Mc - [0; 0; args.MO_B];
|
|
nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
|
|
end
|
|
#+end_src
|
|
|
|
** Controller
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
switch args.controller_type
|
|
case 'none'
|
|
nano_hexapod.controller.type = 1;
|
|
case 'iff'
|
|
nano_hexapod.controller.type = 2;
|
|
case 'dvf'
|
|
nano_hexapod.controller.type = 3;
|
|
case 'hac-iff-struts'
|
|
nano_hexapod.controller.type = 4;
|
|
end
|
|
#+end_src
|
|
|
|
** Save the Structure
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
if nargout == 0
|
|
save('./mat/stages.mat', 'nano_hexapod', '-append');
|
|
end
|
|
#+end_src
|