diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..a06e566 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,3 @@ +*.pdf binary +*.svg binary +*.mat binary diff --git a/figs/control_architecture_hac_iff_L.pdf b/figs/control_architecture_hac_iff_L.pdf index abada49..417412e 100644 Binary files a/figs/control_architecture_hac_iff_L.pdf and b/figs/control_architecture_hac_iff_L.pdf differ diff --git a/figs/control_architecture_hac_iff_L.png b/figs/control_architecture_hac_iff_L.png index b1d3999..c2cb30e 100644 Binary files a/figs/control_architecture_hac_iff_L.png and b/figs/control_architecture_hac_iff_L.png differ diff --git a/figs/control_architecture_hac_iff_L.svg b/figs/control_architecture_hac_iff_L.svg index 35d1961..c6f614f 100644 Binary files a/figs/control_architecture_hac_iff_L.svg and b/figs/control_architecture_hac_iff_L.svg differ diff --git a/figs/control_architecture_hac_iff_struts.pdf b/figs/control_architecture_hac_iff_struts.pdf new file mode 100644 index 0000000..c2d0ea8 Binary files /dev/null and b/figs/control_architecture_hac_iff_struts.pdf differ diff --git a/figs/control_architecture_hac_iff_struts.png b/figs/control_architecture_hac_iff_struts.png new file mode 100644 index 0000000..0c98af7 Binary files /dev/null and b/figs/control_architecture_hac_iff_struts.png differ diff --git a/figs/control_architecture_hac_iff_struts.svg b/figs/control_architecture_hac_iff_struts.svg new file mode 100644 index 0000000..1e189ff Binary files /dev/null and b/figs/control_architecture_hac_iff_struts.svg differ diff --git a/figs/hac_iff_plates_exp_loop_gain_redesigned_K.pdf b/figs/hac_iff_plates_exp_loop_gain_redesigned_K.pdf new file mode 100644 index 0000000..14f535b Binary files /dev/null and b/figs/hac_iff_plates_exp_loop_gain_redesigned_K.pdf differ diff --git a/figs/hac_iff_plates_exp_loop_gain_redesigned_K.png b/figs/hac_iff_plates_exp_loop_gain_redesigned_K.png new file mode 100644 index 0000000..137b4d7 Binary files /dev/null and b/figs/hac_iff_plates_exp_loop_gain_redesigned_K.png differ diff --git a/figs/hac_iff_struts_enc_plates_plant_bode.pdf b/figs/hac_iff_struts_enc_plates_plant_bode.pdf index 9aa24ab..fb096e3 100644 Binary files a/figs/hac_iff_struts_enc_plates_plant_bode.pdf and b/figs/hac_iff_struts_enc_plates_plant_bode.pdf differ diff --git a/figs/hac_iff_struts_enc_plates_plant_bode.png b/figs/hac_iff_struts_enc_plates_plant_bode.png index 68d2116..5482961 100644 Binary files a/figs/hac_iff_struts_enc_plates_plant_bode.png and b/figs/hac_iff_struts_enc_plates_plant_bode.png differ diff --git a/figs/loop_gain_hac_iff_struts.pdf b/figs/loop_gain_hac_iff_struts.pdf index 1732683..b8d87e4 100644 Binary files a/figs/loop_gain_hac_iff_struts.pdf and b/figs/loop_gain_hac_iff_struts.pdf differ diff --git a/figs/loop_gain_hac_iff_struts.png b/figs/loop_gain_hac_iff_struts.png index 25cc1bc..32e3022 100644 Binary files a/figs/loop_gain_hac_iff_struts.png and b/figs/loop_gain_hac_iff_struts.png differ diff --git a/figs/nass_scans_first_test_exp.pdf b/figs/nass_scans_first_test_exp.pdf new file mode 100644 index 0000000..03b96da Binary files /dev/null and b/figs/nass_scans_first_test_exp.pdf differ diff --git a/figs/nass_scans_first_test_exp.png b/figs/nass_scans_first_test_exp.png new file mode 100644 index 0000000..a471a1c Binary files /dev/null and b/figs/nass_scans_first_test_exp.png differ diff --git a/figs/ref_track_nass_exp_hac_iff_struts.pdf b/figs/ref_track_nass_exp_hac_iff_struts.pdf index 24ff6d7..d9e35e9 100644 Binary files a/figs/ref_track_nass_exp_hac_iff_struts.pdf and b/figs/ref_track_nass_exp_hac_iff_struts.pdf differ diff --git a/figs/ref_track_nass_exp_hac_iff_struts.png b/figs/ref_track_nass_exp_hac_iff_struts.png index ae7cb76..36a88ce 100644 Binary files a/figs/ref_track_nass_exp_hac_iff_struts.png and b/figs/ref_track_nass_exp_hac_iff_struts.png differ diff --git a/figs/ref_track_nass_exp_hac_iff_struts_errors_angle.pdf b/figs/ref_track_nass_exp_hac_iff_struts_errors_angle.pdf new file mode 100644 index 0000000..0299d57 Binary files /dev/null and b/figs/ref_track_nass_exp_hac_iff_struts_errors_angle.pdf differ diff --git a/figs/ref_track_nass_exp_hac_iff_struts_errors_angle.png b/figs/ref_track_nass_exp_hac_iff_struts_errors_angle.png new file mode 100644 index 0000000..1b0b1f3 Binary files /dev/null and b/figs/ref_track_nass_exp_hac_iff_struts_errors_angle.png differ diff --git a/figs/ref_track_test_nass_3d.pdf b/figs/ref_track_test_nass_3d.pdf new file mode 100644 index 0000000..7d91883 Binary files /dev/null and b/figs/ref_track_test_nass_3d.pdf differ diff --git a/figs/ref_track_test_nass_3d.png b/figs/ref_track_test_nass_3d.png new file mode 100644 index 0000000..2a9b9f0 Binary files /dev/null and b/figs/ref_track_test_nass_3d.png differ diff --git a/figs/yz_scan_example_trajectory.pdf b/figs/yz_scan_example_trajectory.pdf new file mode 100644 index 0000000..e4aa04f Binary files /dev/null and b/figs/yz_scan_example_trajectory.pdf differ diff --git a/figs/yz_scan_example_trajectory.png b/figs/yz_scan_example_trajectory.png new file mode 100644 index 0000000..2c05e1d Binary files /dev/null and b/figs/yz_scan_example_trajectory.png differ diff --git a/figs/yz_scan_example_trajectory_struts.pdf b/figs/yz_scan_example_trajectory_struts.pdf new file mode 100644 index 0000000..e74fa13 Binary files /dev/null and b/figs/yz_scan_example_trajectory_struts.pdf differ diff --git a/figs/yz_scan_example_trajectory_struts.png b/figs/yz_scan_example_trajectory_struts.png new file mode 100644 index 0000000..0ceac54 Binary files /dev/null and b/figs/yz_scan_example_trajectory_struts.png differ diff --git a/figs/yz_scan_example_trajectory_yz_plane.pdf b/figs/yz_scan_example_trajectory_yz_plane.pdf new file mode 100644 index 0000000..702b1a1 Binary files /dev/null and b/figs/yz_scan_example_trajectory_yz_plane.pdf differ diff --git a/figs/yz_scan_example_trajectory_yz_plane.png b/figs/yz_scan_example_trajectory_yz_plane.png new file mode 100644 index 0000000..7bd4cff Binary files /dev/null and b/figs/yz_scan_example_trajectory_yz_plane.png differ diff --git a/figs/yz_scans_exp_results_first_K.pdf b/figs/yz_scans_exp_results_first_K.pdf new file mode 100644 index 0000000..dd8fc51 Binary files /dev/null and b/figs/yz_scans_exp_results_first_K.pdf differ diff --git a/figs/yz_scans_exp_results_first_K.png b/figs/yz_scans_exp_results_first_K.png new file mode 100644 index 0000000..027d0de Binary files /dev/null and b/figs/yz_scans_exp_results_first_K.png differ diff --git a/matlab/mat/Khac_iff_struts.mat b/matlab/mat/Khac_iff_struts.mat index a878320..1d04fd7 100644 Binary files a/matlab/mat/Khac_iff_struts.mat and b/matlab/mat/Khac_iff_struts.mat differ diff --git a/test-bench-nano-hexapod.html b/test-bench-nano-hexapod.html index bee3078..afadbf6 100644 --- a/test-bench-nano-hexapod.html +++ b/test-bench-nano-hexapod.html @@ -3,7 +3,7 @@ "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
- +generateXYZTrajectory
+
generateYZScanTrajectory
getTransformationMatrixAcc
+
getJacobianNanoHexapod
generateXYZTrajectory
-
-generateYZScanTrajectory
-
-getTransformationMatrixAcc
-
-getJacobianNanoHexapod
-
-This document is dedicated to the experimental study of the nano-hexapod shown in Figure 1. +This document is dedicated to the experimental study of the nano-hexapod shown in Figure 1.
-
Figure 1: Nano-Hexapod
-Here are the documentation of the equipment used for this test bench (lots of them are shwon in Figure 2): +Here are the documentation of the equipment used for this test bench (lots of them are shwon in Figure 2):
Figure 2: Nano-Hexapod and the control electronics
-In Figure 3 is shown a block diagram of the experimental setup. -When possible, the notations are consistent with this diagram and summarized in Table 1. +In Figure 3 is shown a block diagram of the experimental setup. +When possible, the notations are consistent with this diagram and summarized in Table 1.
-
Figure 3: Block diagram of the system with named signals
+
+In this section, we wish to estimated the effectiveness of the IFF strategy concerning the compliance. +
+ ++The top plate is excited vertically using the instrumented hammer two times: +
++The data is loaded. +
%% Load Identification Data -meas_acc = {}; - -for i = 1:6 - meas_acc(i) = {load(sprintf('mat/meas_acc_top_plat_strut_%i.mat', i), 't', 'Va', 'de', 'Am')}; -end --
%% Setup useful variables -% Sampling Time [s] -Ts = (meas_acc{1}.t(end) - (meas_acc{1}.t(1)))/(length(meas_acc{1}.t)-1); - -% Sampling Frequency [Hz] -Fs = 1/Ts; - -% Hannning Windows -win = hanning(ceil(1*Fs)); - -% And we get the frequency vector -[~, f] = tfestimate(meas_acc{1}.Va, meas_acc{1}.de, win, [], [], 1/Ts); +frf_ol = load('Measurement_Z_axis.mat'); % Open-Loop +frf_iff = load('Measurement_Z_axis_damped.mat'); % IFF
-The sensibility of the accelerometers are \(0.1 V/g \approx 0.01 V/(m/s^2)\). +The mean vertical motion of the top platform is computed by averaging all 5 accelerometers.
%% Compute the 6x6 transfer function matrix -G_acc = zeros(length(f), 6, 6); - -for i = 1:6 - G_acc(:,:,i) = tfestimate(meas_acc{i}.Va, 1/0.01*meas_acc{i}.Am, win, [], [], 1/Ts); -end --
Opm = [ 0.047, -0.112, 10e-3; - 0.047, -0.112, 10e-3; - -0.113, 0.011, 10e-3; - -0.113, 0.011, 10e-3; - 0.040, 0.113, 10e-3; - 0.040, 0.113, 10e-3]'; - -Osm = [-1, 0, 0; - 0, 0, 1; - 0, -1, 0; - 0, 0, 1; - -1, 0, 0; - 0, 0, 1]'; - --
Hbm = -15e-3; - -M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm); -J = getJacobianNanoHexapod(Hbm); --
G_acc_CoM = zeros(size(G_acc)); - -for i = 1:length(f) - G_acc_CoM(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J'); -end --
Hbm = -42.3e-3; - -M = getTransformationMatrixAcc(Opm-[0;0;Hbm], Osm); -J = getJacobianNanoHexapod(Hbm); --
G_acc_CoK = zeros(size(G_acc)); - -for i = 1:length(f) - G_acc_CoK(i, :, :) = inv(M)*squeeze(G_acc(i, :, :))*inv(J'); -end --
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'struts', ... - 'actuator_type', 'flexible', ... - 'MO_B', -42.3e-3); --
%% Input/Output definition -clear io; io_i = 1; -io(io_i) = linio([mdl, '/du'], 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 - -G = linearize(mdl, io, 0.0, options); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}; +%% Multiply by 10 (gain in m/s^2/V) and divide by 5 (number of accelerometers) +d_frf_ol = 10/5*(frf_ol.FFT1_H1_4_1_RMS_Y_Mod + frf_ol.FFT1_H1_7_1_RMS_Y_Mod + frf_ol.FFT1_H1_10_1_RMS_Y_Mod + frf_ol.FFT1_H1_13_1_RMS_Y_Mod + frf_ol.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_ol.FFT1_H1_16_1_RMS_X_Val).^2; +d_frf_iff = 10/5*(frf_iff.FFT1_H1_4_1_RMS_Y_Mod + frf_iff.FFT1_H1_7_1_RMS_Y_Mod + frf_iff.FFT1_H1_10_1_RMS_Y_Mod + frf_iff.FFT1_H1_13_1_RMS_Y_Mod + frf_iff.FFT1_H1_16_1_RMS_Y_Mod)./(2*pi*frf_iff.FFT1_H1_16_1_RMS_X_Val).^2;
-Then use the Jacobian matrices to obtain the “cartesian” centralized plant. +The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure 31.
-Gc = inv(n_hexapod.geometry.J)*... - G*... - inv(n_hexapod.geometry.J'); -+ +
+
+Figure 31: Measured vertical compliance with and without IFF
+From Figure 31, it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapod. +It also has the effect of (slightly) degrading the vertical compliance at low frequency. +
+ ++It also seems some damping can be added to the modes at around 205Hz which are flexible modes of the struts. +
+ ++Let’s now compare the measured vertical compliance with the vertical compliance as estimated from the Simscape model. +
+ ++The transfer function from a vertical external force to the absolute motion of the top platform is identified (with and without IFF) using the Simscape model. +The comparison is done in Figure 32. +Again, the model is quite accurate! +
+ ++
+Figure 32: Measured vertical compliance with and without IFF
+From the previous analysis, several conclusions can be drawn: +
++Therefore, in the following sections, the encoders will be fixed to the plates. +The goal is to be less sensitive to the flexible modes of the struts. +
+ +-In this section, the encoders are fixed to the plates rather than to the struts as shown in Figure 31. +In this section, the encoders are fixed to the plates rather than to the struts as shown in Figure 33.
--
Figure 31: Nano-Hexapod with encoders fixed to the struts
+Figure 33: Nano-Hexapod with encoders fixed to the struts
It is structured as follow:
+In this section, the dynamics of the nano-hexapod with the encoders fixed to the plates is identified. +
+ ++First, the measurement data are loaded in Section 2.1.1, then the transfer function matrix from the actuators to the encoders are estimated in Section 2.1.2. +Finally, the transfer function matrix from the actuators to the force sensors is estimated in Section 2.1.3.
+The actuators are excited one by one using a low pass filtered white noise. +For each excitation, the 6 force sensors and 6 encoders are measured and saved. +
%% Load Identification Data
meas_data_lf = {};
@@ -1739,34 +1804,16 @@ meas_data_lf = {};
%% Setup useful variables -% Sampling Time [s] -Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1); - -% Sampling Frequency [Hz] -Fs = 1/Ts; - -% Hannning Windows -win = hanning(ceil(1*Fs)); - -% And we get the frequency vector -[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts); --
-First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 32). +
++Let’s compute the coherence from the excitation voltage \(\bm{u}\) and the displacement \(d\bm{\mathcal{L}}_m\) as measured by the encoders. +
%% Coherence
coh_dvf = zeros(length(f), 6, 6);
@@ -1777,15 +1824,19 @@ coh_dvf = zeros(length(f), 6, 6);
+The obtained coherence shown in Figure 34 is quite good up to 400Hz. +
--
Figure 32: Obtained coherence for the DVF plant
+Figure 34: Obtained coherence for the DVF plant
-Then the 6x6 transfer function matrix is estimated (Figure 33). +Then the 6x6 transfer function matrix is estimated.
%% DVF Plant (transfer function from u to dLm)
@@ -1797,22 +1848,41 @@ G_dvf = zeros(length(f), 6, 6);
+The diagonal and off-diagonal terms of this transfer function matrix are shown in Figure 35. +
--
Figure 33: Measured FRF for the DVF plant
+Figure 35: Measured FRF for the DVF plant
++From Figure 35, we can draw few conclusions on the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) when the encoders are fixed to the plates: +
+-First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure 34). +
++Let’s now compute the coherence from the excitation voltage \(\bm{u}\) and the voltage \(\bm{\tau}_m\) generated by the Force senors. +
%% Coherence for the IFF plant
coh_iff = zeros(length(f), 6, 6);
@@ -1823,15 +1893,18 @@ coh_iff = zeros(length(f), 6, 6);
+The coherence is shown in Figure 36, and is very good for from 10Hz up to 2kHz. +
--
Figure 34: Obtained coherence for the IFF plant
+Figure 36: Obtained coherence for the IFF plant
-Then the 6x6 transfer function matrix is estimated (Figure 35). +Then the 6x6 transfer function matrix is estimated.
%% IFF Plant
@@ -1843,50 +1916,60 @@ G_iff = zeros(length(f), 6, 6);
+The bode plot of the diagonal and off-diagonal terms are shown in Figure 37. +
--
Figure 35: Measured FRF for the IFF plant
+Figure 37: Measured FRF for the IFF plant
++It is shown in Figure 38 that: +
++The identified dynamics is saved for further use. +
save('matlab/mat/identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf') +save('mat/identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf')
-In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model. +In this section, the measured dynamics done in Section 2.1 is compared with the dynamics estimated from the Simscape model.
%% Load data -load('identified_plants_enc_plates.mat', 'f', 'Ts', 'G_iff', 'G_dvf') --
+The nano-hexapod is initialized with the APA taken as flexible models. +
%% Initialize Nano-Hexapod n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... @@ -1895,7 +1978,15 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'< 'actuator_type', 'flexible');
+Then the transfer function from \(\bm{u}\) to \(\bm{\tau}_m\) is identified using the Simscape model. +
%% Identify the IFF Plant (transfer function from u to taum) clear io; io_i = 1; @@ -1906,41 +1997,44 @@ Giff = exp(-s*Ts)
+The identified dynamics is compared with the measured FRF: +
+-
Figure 36: IFF Plant for the first actuator input and all the force senosrs
+Figure 38: IFF Plant for the first actuator input and all the force senosrs
-
Figure 37: Diagonal elements of the IFF Plant
+Figure 39: Diagonal elements of the IFF Plant
-
Figure 38: Off diagonal elements of the IFF Plant
+Figure 40: Off diagonal elements of the IFF Plant
%% Initialization of the Nano-Hexapod -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', 'flexible'); --
+Now, the dynamics from the DAC voltage \(\bm{u}\) to the encoders \(d\bm{\mathcal{L}}_m\) is estimated using the Simscape model. +
%% Identify the DVF Plant (transfer function from u to dLm) clear io; io_i = 1; @@ -1951,83 +2045,93 @@ Gdvf = exp(-s*Ts)
+The identified dynamics is compared with the measured FRF: +
+-
Figure 39: DVF Plant for the first actuator input and all the encoders
+Figure 41: DVF Plant for the first actuator input and all the encoders
-
Figure 40: Diagonal elements of the DVF Plant
+Figure 42: Diagonal elements of the DVF Plant
-
Figure 41: Off diagonal elements of the DVF Plant
+Figure 43: Off diagonal elements of the DVF Plant
++The Simscape model is quite accurate for the transfer function matrices from \(\bm{u}\) to \(\bm{\tau}_m\) and from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) except at frequencies of the flexible modes of the top-plate. +The Simscape model can therefore be used to develop the control strategies. +
++In this section, the Integral Force Feedback (IFF) control strategy is applied to the nano-hexapod in order to add damping to the suspension modes.
-+The control architecture is shown in Figure 44: +
+-
Figure 42: Integral Force Feedback Strategy
+Figure 44: Integral Force Feedback Strategy
%% Initialize Nano-Hexapod -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof'); --
%% Identify the IFF Plant (transfer function from u to taum) -clear io; io_i = 1; -io(io_i) = linio([mdl, '/du'], 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 = exp(-s*Ts)*linearize(mdl, io, 0.0, options); --
load('Kiff.mat', 'Kiff') --
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain -clear io; io_i = 1; -io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder) --
+The nano-hexapod is initialized with flexible APA and the encoders fixed to the struts. +
%% Initialize the Simscape model in closed loop n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... @@ -2037,11 +2141,38 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'<
+The same controller as the one developed when the encoder were fixed to the struts is used. +
Gd_ol = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +%% Optimal IFF controller +load('Kiff.mat', 'Kiff')
+The transfer function from \(\bm{u}^\prime\) to \(d\bm{\mathcal{L}}_m\) is identified. +
+%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain +clear io; io_i = 1; +io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs +io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder) ++
+First in Open-Loop: +
+%% Transfer function from u to dL (open-loop) +Gd_ol = exp(-s*Ts)*linearize(mdl, io, 0.0, options); ++
+And then with the IFF controller: +
%% Initialize the Simscape model in closed loop n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... @@ -2049,11 +2180,17 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'< 'motion_sensor_type', 'plates', ... 'actuator_type', 'flexible', ... 'controller_type', 'iff'); + +%% Transfer function from u to dL (IFF) +Gd_iff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
+It is first verified that the system is stable: +
Gd_iff = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +isstable(Gd_iff)
+The diagonal and off-diagonal terms of the \(6 \times 6\) transfer function matrices identified are compared in Figure 45. +It is shown, as was the case when the encoders were fixed to the struts, that the IFF control strategy is very effective in damping the suspension modes of the nano-hexapod. +
--
Figure 43: Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)
+Figure 45: Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)
-Let’s now look at the \(6 \times 6\) damped plant with the optimal gain \(g = 400\). +The IFF control strategy is experimentally implemented. +The (damped) transfer function from \(\bm{u}^\prime\) to \(d\bm{\mathcal{L}}_m\) is experimentally identified.
-+The identification data are loaded: +
%% Load Identification Data meas_iff_plates = {}; @@ -2091,20 +2232,14 @@ meas_iff_plates = {}; end
+And the parameters used for the transfer function estimation are defined below. +
%% Setup useful variables -% Sampling Time [s] +% Sampling Time [s] Ts = (meas_iff_plates{1}.t(end) - (meas_iff_plates{1}.t(1)))/(length(meas_iff_plates{1}.t)-1); -% Sampling Frequency [Hz] -Fs = 1/Ts; - % Hannning Windows win = hanning(ceil(1*Fs)); @@ -2112,47 +2247,12 @@ win = hanning(ceil(1*Fs)); [~, f] = tfestimate(meas_iff_plates{1}.Va, meas_iff_plates{1}.de, win, [], [], 1/Ts);
+The estimation is performed using the tfestimate
command.
+
load('Kiff.mat', 'Kiff') --
%% Initialize the Simscape model in closed loop -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', 'flexible', ... - 'controller_type', 'iff'); --
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain -clear io; io_i = 1; -io(io_i) = linio([mdl, '/du'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/dL'], 1, 'openoutput'); io_i = io_i + 1; % Plate Displacement (encoder) --
Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options); --
%% IFF Plant +%% Estimation of the transfer function matrix from u to dL when IFF is applied G_enc_iff_opt = zeros(length(f), 6, 6); for i = 1:6 @@ -2161,96 +2261,174 @@ G_enc_iff_opt = zeros(length(f), 6, 6);
+
+The obtained diagonal and off-diagonal elements of the transfer function from \(\bm{u}^\prime\) to \(d\bm{\mathcal{L}}_m\) are shown in Figure 46 both without and with IFF.
-Figure 44: FRF from one actuator to all the encoders when the plant is damped using IFF
--
-Figure 45: Comparison of the diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)
--
-Figure 46: Comparison of the off-diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)
-load('identified_plants_enc_plates.mat', 'f', 'G_dvf'); --
-
Figure 47: Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)
+Figure 46: Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)
++As was predicted with the Simscape model, the IFF control strategy is very effective in damping the suspension modes of the nano-hexapod. +Little damping is also applied on the first flexible mode of the strut at 235Hz. +However, no damping is applied on other modes, such as the flexible modes of the top plate. +
++Let’s now compare the obtained damped plants obtained experimentally with the one extracted from Simscape: +
++
+Figure 47: FRF from one actuator to all the encoders when the plant is damped using IFF
++
+Figure 48: Comparison of the diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)
++
+Figure 49: Comparison of the off-diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)
++The experimentally identified plant is saved for further use. +
save('matlab/mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
-In a first approximation, the Jacobian matrix can be used instead of using the inverse kinematic equations. -
- - --
-Figure 48: HAC-LAC: IFF + Control in the frame of the legs
--Let’s start with the Simscape model and the damped plant. -
- --Apply HAC control and verify the system is stable. -
- --Then, try the control strategy on the real plant. -
load('Kiff.mat', 'Kiff') +save('mat/damped_plant_enc_plates.mat', 'f', 'Ts', 'G_enc_iff_opt')
+In this section, the dynamics of the nano-hexapod with the encoders fixed to the plates is studied. +
+ ++It has been found that: +
++In this section is studied the HAC-LAC architecture for the Nano-Hexapod. +More precisely: +
++The corresponding control architecture is shown in Figure 50 with: +
++
+Figure 50: HAC-LAC: IFF + Control in the frame of the legs
++In this section, the decentralized high authority controller \(\bm{K}_{\mathcal{L}}\) is first tuned using the Simscape model. +
++First initialized the nano-hexapod with a flexible APA model and with the IFF control strategy. +
%% Initialize the Simscape model in closed loop +%% Initialize the Simscape model n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... 'flex_top_type', '4dof', ... 'motion_sensor_type', 'plates', ... @@ -2259,6 +2437,18 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'<
+Then the controller is loaded +
+%% Load the decentralized IFF controller +load('Kiff.mat', 'Kiff') ++
+The inputs and outputs for the transfer function estimation are defined. +
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain clear io; io_i = 1; @@ -2267,57 +2457,58 @@ io(io_i) = linio([mdl, '/dL'], 1, 51. +--Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options); +%% Identified of the damped TF from u' to dL +Gd_iff_opt = exp(-s*Ts)*linearize(mdl, io, 0.0, options);--isstable(Gd_iff_opt) ---1 -- - - -+-
Figure 49: Transfer function from \(u\) to \(d\mathcal{L}_m\) with IFF (diagonal elements)
+Figure 51: Transfer functions from \(u\) to \(d\mathcal{L}_m\) with IFF (diagonal and off-diagonal elements)
-Let’s try to have 100Hz bandwidth: +Let’s first try to design a first decentralized controller with: +
++After some very basic and manual loop shaping, the following controller is developed:
%% Lead +%% Lead to increase phase margin a = 2; % Amount of phase lead / width of the phase lead / high frequency gain wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s] H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); -%% Low Pass filter +%% Low Pass filter to increase robustness H_lpf = 1/(1 + s/2/pi/200); -%% Notch +%% Notch at the top-plate resonance gm = 0.02; xi = 0.3; wn = 2*pi*700; H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); --
Khac_iff_struts = -(1/(2.87e-5)) * ... % Gain +%% Decentralized HAC +Khac_iff_struts = -(1/(2.87e-5)) * ... % Gain H_lead * ... % Lead H_notch * ... % Notch (2*pi*100/s) * ... % Integrator @@ -2325,33 +2516,78 @@ H_notch = (s^2 + 2
+This controller is saved for further use. +
save('matlab/mat/Khac_iff_struts.mat', 'Khac_iff_struts') +save('mat/Khac_iff_struts.mat', 'Khac_iff_struts')
+The Loop Gain is computed and shown in Figure 52. +
Lhac_iff_struts = Khac_iff_struts*Gd_iff_opt;
-
Figure 50: Diagonal and off-diagonal elements of the Loop gain for “HAC-IFF-Struts”
+Figure 52: Diagonal and off-diagonal elements of the Loop gain for “HAC-IFF-Struts”
+The HAC-IFF control strategy is implemented using Simscape. +
+%% Initialize the Simscape model in closed loop +n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... + 'flex_top_type', '4dof', ... + 'motion_sensor_type', 'plates', ... + 'actuator_type', 'flexible', ... + 'controller_type', 'hac-iff-struts'); ++
[ ]
Find a more clever way to do the multiplication+We identify the closed-loop system. +
+%% Identification
+Gd_iff_hac_opt = linearize(mdl, io, 0.0, options);
+
++And verify that it is indeed stable. +
+%% Verify the stability
+isstable(Gd_iff_hac_opt)
+
++1 ++
+Now, the loop gain is estimated from the measured FRF. +
L_frf = zeros(size(G_enc_iff_opt)); @@ -2361,131 +2597,146 @@ H_notch = (s^2 + 2
+The bode plot of the loop gain is shown in Figure 53. +
--
Figure 51: Diagonal and Off-diagonal elements of the Loop gain (experimental data)
+Figure 53: Diagonal and Off-diagonal elements of the Loop gain (experimental data)
+%% Initialize the Simscape model in closed loop -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', 'flexible', ... - 'controller_type', 'hac-iff-struts'); --
Gd_iff_hac_opt = linearize(mdl, io, 0.0, options); --
isstable(Gd_iff_hac_opt) --
-1 --
+In this section, several trajectories representing the wanted pose (position and orientation) of the top platform with respect to the bottom platform are defined. +
+ ++These trajectories will be used to test the HAC-LAC architecture. +
+ ++In order to transform the wanted pose to the wanted displacement of the 6 struts, the inverse kinematic is required. +As a first approximation, the Jacobian matrix can be used instead of using the full inverse kinematic equations. +
+ ++Therefore, the control architecture with the input trajectory \(\bm{r}_{\mathcal{X}_n}\) is shown in Figure 54. +
+ + ++
+Figure 54: HAC-LAC: IFF + Control in the frame of the legs
++In the following sections, several reference trajectories are defined: +
+ +load('Khac_iff_struts.mat', 'Khac_iff_struts') --
Rx_yz = generateYZScanTrajectory(... - 'y_tot', 4e-6, ... - 'z_tot', 8e-6, ... - 'n', 5, ... - 'Ts', 1e-3, ... - 'ti', 2, ... - 'tw', 0.5, ... - 'ty', 2, ... - 'tz', 1); --
figure; -hold on; -plot(Rx_yz(:,1), Rx_yz(:,3), ... - 'DisplayName', 'Y motion') -plot(Rx_yz(:,1), Rx_yz(:,4), ... - 'DisplayName', 'Z motion') -hold off; -xlabel('Time [s]'); -ylabel('Displacement [m]'); -legend('location', 'northeast'); --
figure; -plot(Rx_yz(:,3), Rx_yz(:,4)); -xlabel('y [m]'); ylabel('z [m]'); --
-Let’s use the Jacobian to estimate the wanted strut length as a function of time.
+A function generateYZScanTrajectory
has been developed (accessible here) in order to easily generate scans in the Y-Z plane.
+For instance, the following generated trajectory is represented in Figure 55. +
+%% Generate the Y-Z trajectory scan +Rx_yz = generateYZScanTrajectory(... + 'y_tot', 4e-6, ... % Length of Y scans [m] + 'z_tot', 8e-6, ... % Total Z distance [m] + 'n', 5, ... % Number of Y scans + 'Ts', 1e-3, ... % Sampling Time [s] + 'ti', 2, ... % Time to go to initial position [s] + 'tw', 0.5, ... % Waiting time between each points [s] + 'ty', 2, ... % Time for a scan in Y [s] + 'tz', 1); % Time for a scan in Z [s] ++
+
+Figure 55: Generated scan in the Y-Z plane
++The Y and Z positions as a function of time are shown in Figure 56. +
+ + ++
+Figure 56: Y and Z trajectories as a function of time
++Using the Jacobian matrix, it is possible to compute the wanted struts lengths as a function of time: +
+\begin{equation} + \bm{r}_{d\mathcal{L}} = \bm{J} \bm{r}_{\mathcal{X}_n} +\end{equation} +dL_ref = [n_hexapod.geometry.J*Rx_yz(:, 2:7)']';
figure; -hold on; -for i=1:6 - plot(Rx_yz(:,1), dL_ref(:, i)) -end -xlabel('Time [s]'); ylabel('Displacement [m]'); -+
+The reference signal for the strut length is shown in Figure 57. +
+ ++
+Figure 57: Trajectories for the 6 individual struts
+Before trying to follow this reference with the nano-hexapod, let’s try to do it using the Simscape model. +
+ ++The nano-hexapod is initialized with the APA modelled as 2DoF system (for the simulation to run quickly). +
%% Initialize the Simscape model in closed loop n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ... @@ -2496,92 +2747,70 @@ n_hexapod = initializeNanoHexapodFinal('flex_bot_type'<
set_param(mdl,'StopTime', num2str(Rx_yz(end,1))) -set_param(mdl,'SimulationCommand','start') --
+The reference path as well as the measured motion are compared in Figure 58. +
-out.X.Data = out.X.Data - out.X.Data(1,:); --
-
Figure 52: Simulated Y-Z motion
+Figure 58: Simulated Y-Z motion
+
+The motion errors are computed and shown in Figure 59. +It is clear that the hexapod is indeed tracking the reference path. +However, in this simulation, no disturbances are included nor sensor noises.
-Figure 53: Y and Z motion as a function of time as well as the reference signals
--
Figure 54: Positioning errors as a function of time
+Figure 59: Positioning errors as a function of time
+ +In this section, a reference path that “draws” the work “NASS” is developed. +
+ ++First, a series of points representing each letter are defined. +Between each letter, a negative Z motion is performed. +
ref_path = [ ... - 0, 0, 0; - 0, 0, 1; % N - 0, 4, 1; - 3, 0, 1; - 3, 4, 1; - 3, 4, 0; - 4, 0, 0; - 4, 0, 1; % A - 4, 3, 1; - 5, 4, 1; - 6, 4, 1; - 7, 3, 1; - 7, 2, 1; - 4, 2, 1; - 4, 3, 1; - 5, 4, 1; - 6, 4, 1; - 7, 3, 1; - 7, 0, 1; - 7, 0, 0; - 8, 0, 0; - 8, 0, 1; % S - 11, 0, 1; - 11, 2, 1; - 8, 2, 1; - 8, 4, 1; - 11, 4, 1; - 11, 4, 0; - 12, 0, 0; - 12, 0, 1; % S - 15, 0, 1; - 15, 2, 1; - 12, 2, 1; - 12, 4, 1; - 15, 4, 1; - 15, 4, 0; +%% List of points that draws "NASS" +ref_path = [ ... + 0, 0,0; % Initial Position + 0,0,1; 0,4,1; 3,0,1; 3,4,1; % N + 3,4,0; 4,0,0; % Transition + 4,0,1; 4,3,1; 5,4,1; 6,4,1; 7,3,1; 7,2,1; 4,2,1; 4,3,1; 5,4,1; 6,4,1; 7,3,1; 7,0,1; % A + 7,0,0; 8,0,0; % Transition + 8,0,1; 11,0,1; 11,2,1; 8,2,1; 8,4,1; 11,4,1; % S + 11,4,0; 12,0,0; % Transition + 12,0,1; 15,0,1; 15,2,1; 12,2,1; 12,4,1; 15,4,1; % S + 15,4,0; ]; -% Center the trajectory arround zero +%% Center the trajectory arround zero ref_path = ref_path - (max(ref_path) - min(ref_path))/2; -% Define the X-Y-Z cuboid dimensions containing the trajectory +%% Define the X-Y-Z cuboid dimensions containing the trajectory X_max = 10e-6; Y_max = 4e-6; Z_max = 2e-6; @@ -2590,199 +2819,217 @@ ref_path = ([X_max, Y_max, Z_max]./max(ref_path))<
+Then, using the generateXYZTrajectory
function, the \(6 \times 1\) trajectory signal is computed.
+
Rx_nass = generateXYZTrajectory('points', ref_path); +%% Generating the trajectory +Rx_nass = generateXYZTrajectory('points', ref_path);
figure; -plot(1e6*Rx_nass(Rx_nass(:,4)>0, 2), 1e6*Rx_nass(Rx_nass(:,4)>0, 3), 'k.') -xlabel('X [$\mu m$]'); -ylabel('Y [$\mu m$]'); -axis equal; -xlim(1e6*[min(Rx_nass(:,2)), max(Rx_nass(:,2))]); -ylim(1e6*[min(Rx_nass(:,3)), max(Rx_nass(:,3))]); --
+The trajectory in the X-Y plane is shown in Figure 60 (the transitions between the letters are removed). +
- --
Figure 55: Reference path corresponding to the “NASS” acronym
+Figure 60: Reference path corresponding to the “NASS” acronym
figure; -plot3(Rx_nass(:,2), Rx_nass(:,3), Rx_nass(:,4), 'k-'); -xlabel('x'); -ylabel('y'); -zlabel('z'); --
figure; -hold on; -plot(Rx_nass(:,1), Rx_nass(:,2)); -plot(Rx_nass(:,1), Rx_nass(:,3)); -plot(Rx_nass(:,1), Rx_nass(:,4)); -hold off; --
figure; -scatter(Rx_nass(:,2), Rx_nass(:,3), 1, Rx_nass(:,4), 'filled') -colormap winter --
%% Initialize the Simscape model in closed loop -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '2dof', ... - 'flex_top_type', '3dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof', ... - 'controller_type', 'hac-iff-struts'); --
set_param(mdl,'StopTime', num2str(Rx_nass(end,1))) -set_param(mdl,'SimulationCommand','start') --
out.X.Data = out.X.Data - out.X.Data(1,:); --
save('matlab/mat/reference_path.mat', 'Rx_yz', 'Rx_nass') --
-
-Figure 56: Feedforward control in the frame of the legs
--Let’s estimate the mean DC gain for the damped plant (diagonal elements:) -
--1.773e-05 -- -
-The feedforward controller is then taken as the inverse of this gain (the minus sign is there manually added as it is “removed” by the abs
function):
+It can also be better viewed in a 3D representation as in Figure 61.
+
+Figure 61: Reference path that draws “NASS” - 3D view
++Both the Integral Force Feedback controller (developed in Section 2.3) and the high authority controller working in the frame of the struts (developed in Section 3.1) are implemented experimentally. +
++The controller designed in Section 3.1 is implemented experimentally and some reference tracking tests are performed. +
+Kff_iff_L = -1/mean(diag(abs(squeeze(mean(G_enc_iff_opt(f>2 & f<4,:,:)))))); +%% Load the experimental data +load('hac_iff_struts_yz_scans.mat', 't', 'de')
-The open-loop gain (feedforward controller times the damped plant) is shown in Figure 57. +The position of the top-platform is estimated using the Jacobian matrix:
- - --
-Figure 57: Diagonal elements of the “open loop gain”
+%% Pose of the top platform from the encoder values +load('jacobian.mat', 'J'); +Xe = [inv(J)*de']'; +
-And save the feedforward controller for further use: +The reference path as well as the measured position are partially shown in the Y-Z plane in Figure 62.
-Kff_iff_L = zpk(Kff_iff_L)*eye(6);
-
-save('matlab/mat/feedforward_iff.mat', 'Kff_iff_L') --
load('reference_path.mat', 'Rx_yz'); --
+
-
Figure 58: Feedback/Feedforward control in the frame of the legs
-Figure 62: Measured position \(\bm{\mathcal{X}}_n\) and reference signal \(\bm{r}_{\mathcal{X}_n}\) in the Y-Z plane - Zoom on a change of direction
generateXYZTrajectory
- +It is clear from Figure 62 that the position of the nano-hexapod effectively tracks to reference signal. +However, oscillations with amplitudes as large as 50nm can be observe. +
+ ++It turns out that the frequency of these oscillations is 100Hz which is corresponding to the crossover frequency of the High Authority Control loop. +This clearly indicates poor stability margins. +In the next section, the controller is re-designed to improve the stability margins. +
+ ++The High Authority Controller is re-designed in order to improve the stability margins. +
+%% Lead +a = 5; % Amount of phase lead / width of the phase lead / high frequency gain +wc = 2*pi*110; % Frequency with the maximum phase lead [rad/s] + +H_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a))); + +%% Low Pass Filter +H_lpf = 1/(1 + s/2/pi/300); + +%% Notch +gm = 0.02; +xi = 0.5; +wn = 2*pi*700; + +H_notch = (s^2 + 2*gm*xi*wn*s + wn^2)/(s^2 + 2*xi*wn*s + wn^2); + +%% HAC Controller +Khac_iff_struts = -2.2e4 * ... % Gain + H_lead * ... % Lead + H_lpf * ... % Lead + H_notch * ... % Notch + (2*pi*100/s) * ... % Integrator + eye(6); % 6x6 Diagonal ++
+The bode plot of the new loop gain is shown in Figure 63. +
+ ++
+Figure 63: Loop Gain for the updated decentralized HAC controller
++This new controller is implemented experimentally and several tracking tests are performed. +
+%% Load Measurements +load('hac_iff_more_lead_nass_scan.mat', 't', 'de') ++
+The pose of the top platform is estimated from the encoder position using the Jacobian matrix. +
+%% Compute the pose of the top platform +load('jacobian.mat', 'J'); +Xe = [inv(J)*de']'; ++
+The measured motion as well as the trajectory are shown in Figure 64. +
+ ++
+Figure 64: Measured position \(\bm{\mathcal{X}}_n\) and reference signal \(\bm{r}_{\mathcal{X}_n}\) for the “NASS” trajectory
++The trajectory and measured motion are also shown in the X-Y plane in Figure 65. +
+ ++
+Figure 65: Reference path and measured motion in the X-Y plane
++The orientations errors as a function of time are shown in Figure 66. +
+ ++
+Figure 66: Orientation errors as a function of time during the “NASS” trajectory
++Using the updated High Authority Controller, the nano-hexapod can follow trajectories with high accuracy (the position errors are in the order of 50nm peak to peak, and the orientation errors 300nrad peak to peak). +
+ +generateXYZTrajectory
function [ref] = generateXYZTrajectory(args) % generateXYZTrajectory - @@ -2799,9 +3046,9 @@ And save the feedforward controller for further use:
arguments args.points double {mustBeNumeric} = zeros(2, 3) % [m] @@ -2817,9 +3064,9 @@ And save the feedforward controller for further use:
time_i = 0:args.Ts:args.ti; time_w = 0:args.Ts:args.tw; @@ -2829,9 +3076,9 @@ time_m = 0:args.Ts:a
% Go to initial position xyz = (args.points(1,:))'*(time_i/args.ti); @@ -2854,9 +3101,9 @@ xyz = [xyz, xyz(:,end) -
t = 0:args.Ts:args.Ts*(length(xyz) - 1);@@ -2873,17 +3120,17 @@ ref(:, 2:4) = xyz
generateYZScanTrajectory
generateYZScanTrajectory
function [ref] = generateYZScanTrajectory(args) % generateYZScanTrajectory - @@ -2900,9 +3147,9 @@ ref(:, 2:4) = xyz
arguments args.y_tot (1,1) double {mustBeNumeric} = 10e-6 % [m] @@ -2922,9 +3169,9 @@ ref(:, 2:4) = xyz
time_i = 0:args.Ts:args.ti; time_w = 0:args.Ts:args.tw; @@ -2935,9 +3182,9 @@ time_z = 0:args.Ts:a
% Go to initial position y = (time_i/args.ti)*(args.y_tot/2); @@ -2992,9 +3239,9 @@ z = [z, z(end) - z(end)*
t = 0:args.Ts:args.Ts*(length(y) - 1);@@ -3012,17 +3259,17 @@ ref(:, 4) = z;
getTransformationMatrixAcc
getTransformationMatrixAcc
function [M] = getTransformationMatrixAcc(Opm, Osm) % getTransformationMatrixAcc - @@ -3040,9 +3287,9 @@ ref(:, 4) = z;
Let’s try to estimate the x-y-z acceleration of any point of the solid body from the acceleration/angular acceleration of the solid body expressed in \(\{O\}\). For any point \(p_i\) of the solid body (corresponding to an accelerometer), we can write: @@ -3105,7 +3352,7 @@ a_i = \begin{bmatrix} And finally we can combine the 6 (line) vectors for the 6 accelerometers to write that in a matrix form. We obtain Eq. \eqref{eq:M_matrix}.
-The transformation from solid body acceleration \({}^O\vec{x}\) from sensor measured acceleration \(\vec{a}\) is:
@@ -3148,17 +3395,17 @@ Let’s define such matrix using matlab:getJacobianNanoHexapod
getJacobianNanoHexapod
function [J] = getJacobianNanoHexapod(Hbm) % getJacobianNanoHexapod - @@ -3175,9 +3422,9 @@ Let’s define such matrix using matlab:
Fa = [[-86.05, -74.78, 22.49], [ 86.05, -74.78, 22.49], @@ -3211,7 +3458,7 @@ J = [si', cross(Bb, si)'