diff --git a/backup.org b/backup.org deleted file mode 100644 index 0fd66c7..0000000 --- a/backup.org +++ /dev/null @@ -1,3866 +0,0 @@ - -** Analysis backup of HAC - Decoupling analysis -<> - -*** Introduction :ignore: - -In this section is studied the HAC-IFF architecture for the Nano-Hexapod. -More precisely: -- The LAC control is a decentralized integral force feedback as studied in Section ref:sec:test_nhexa_enc_plates_iff -- The HAC control is a decentralized controller working in the frame of the struts - -The corresponding control architecture is shown in Figure ref:fig:test_nhexa_control_architecture_hac_iff_struts with: -- $\bm{r}_{\mathcal{X}_n}$: the $6 \times 1$ reference signal in the cartesian frame -- $\bm{r}_{d\mathcal{L}}$: the $6 \times 1$ reference signal transformed in the frame of the struts thanks to the inverse kinematic -- $\bm{\epsilon}_{d\mathcal{L}}$: the $6 \times 1$ length error of the 6 struts -- $\bm{u}^\prime$: input of the damped plant -- $\bm{u}$: generated DAC voltages -- $\bm{\tau}_m$: measured force sensors -- $d\bm{\mathcal{L}}_m$: measured displacement of the struts by the encoders - -#+begin_src latex :file control_architecture_hac_iff_struts.pdf -\definecolor{instrumentation}{rgb}{0, 0.447, 0.741} -\definecolor{mechanics}{rgb}{0.8500, 0.325, 0.098} -\definecolor{control}{rgb}{0.4660, 0.6740, 0.1880} - -\begin{tikzpicture} - % Blocs - \node[block={3.0cm}{2.0cm}, fill=black!20!white] (P) {Plant}; - \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); - \coordinate[] (outputF) at ($(P.south east)!0.2!(P.north east)$); - \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$); - - \node[block, below=0.4 of P, fill=control!20!white] (Kiff) {$\bm{K}_\text{IFF}$}; - \node[block, left=0.8 of inputF, fill=instrumentation!20!white] (pd200) {\tiny PD200}; - \node[addb, left=0.8 of pd200, fill=control!20!white] (addF) {}; - \node[block, left=0.8 of addF, fill=control!20!white] (K) {$\bm{K}_\mathcal{L}$}; - \node[addb={+}{}{-}{}{}, left=0.8 of K, fill=control!20!white] (subr) {}; - \node[block, align=center, left= of subr, fill=control!20!white] (J) {\tiny Inverse\\\tiny Kinematics}; - - % Connections and labels - \draw[->] (outputF) -- ++(1.0, 0) node[above left]{$\bm{\tau}_m$}; - \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); - \draw[->] (Kiff.west) -| (addF.south); - \draw[->] (addF.east) -- (pd200.west) node[above left]{$\bm{u}$}; - \draw[->] (pd200.east) -- (inputF) node[above left]{$\bm{u}_a$}; - - \draw[->] (outputL) -- ++(1.0, 0) node[below left]{$d\bm{\mathcal{L}_m}$}; - \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, 1) -| (subr.north); - \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$}; - \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{u}^\prime$}; - - \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; - \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0); -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_control_architecture_hac_iff_struts -#+caption: HAC-LAC: IFF + Control in the frame of the legs -#+RESULTS: -[[file:figs/test_nhexa_control_architecture_hac_iff_struts.png]] - -This part is structured as follow: -- Section ref:sec:test_nhexa_hac_iff_struts_ref_track: some reference tracking tests are performed -- Section ref:sec:test_nhexa_hac_iff_struts_controller: the decentralized high authority controller is tuned using the Simscape model and is implemented and tested experimentally -- Section ref:sec:test_nhexa_interaction_analysis: an interaction analysis is performed, from which the best decoupling strategy can be determined -- Section ref:sec:test_nhexa_robust_hac_design: Robust High Authority Controller are designed - -*** Reference Tracking - Trajectories -:PROPERTIES: -:header-args:matlab+: :tangle matlab/scripts/reference_tracking_paths.m -:END: -<> -**** Introduction :ignore: -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 $\bm{J}$ 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 ref:fig:test_nhexa_control_architecture_hac_iff_L. - -#+begin_src latex :file control_architecture_hac_iff_struts_L.pdf -\definecolor{instrumentation}{rgb}{0, 0.447, 0.741} -\definecolor{mechanics}{rgb}{0.8500, 0.325, 0.098} -\definecolor{control}{rgb}{0.4660, 0.6740, 0.1880} - -\begin{tikzpicture} - % Blocs - \node[block={3.0cm}{2.0cm}, fill=black!20!white] (P) {Plant}; - \coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$); - \coordinate[] (outputF) at ($(P.south east)!0.2!(P.north east)$); - \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$); - - \node[block, below=0.4 of P, fill=control!20!white] (Kiff) {$\bm{K}_\text{IFF}$}; - \node[block, left=0.8 of inputF, fill=instrumentation!20!white] (pd200) {\tiny PD200}; - \node[addb, left=0.8 of pd200, fill=control!20!white] (addF) {}; - \node[block, left=0.8 of addF, fill=control!20!white] (K) {$\bm{K}_\mathcal{L}$}; - \node[addb={+}{}{-}{}{}, left=0.8 of K, fill=control!20!white] (subr) {}; - \node[block, align=center, left= of subr, fill=control!20!white] (J) {$\bm{J}$}; - - % Connections and labels - \draw[->] (outputF) -- ++(1.0, 0) node[above left]{$\bm{\tau}_m$}; - \draw[->] ($(outputF) + (0.6, 0)$)node[branch]{} |- (Kiff.east); - \draw[->] (Kiff.west) -| (addF.south); - \draw[->] (addF.east) -- (pd200.west) node[above left]{$\bm{u}$}; - \draw[->] (pd200.east) -- (inputF) node[above left]{$\bm{u}_a$}; - - \draw[->] (outputL) -- ++(1.0, 0) node[below left]{$d\bm{\mathcal{L}_m}$}; - \draw[->] ($(outputL) + (0.6, 0)$)node[branch]{} -- ++(0, 1) -| (subr.north); - \draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{d\mathcal{L}}$}; - \draw[->] (K.east) -- (addF.west) node[above left]{$\bm{u}^\prime$}; - - \draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{d\mathcal{L}}$}; - \draw[<-] (J.west)node[above left]{$\bm{r}_{\mathcal{X}_n}$} -- ++(-1, 0); -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_control_architecture_hac_iff_L -#+caption: HAC-LAC: IFF + Control in the frame of the legs -#+RESULTS: -[[file:figs/test_nhexa_control_architecture_hac_iff_struts_L.png]] - -In the following sections, several reference trajectories are defined: -- Section ref:sec:test_nhexa_yz_scans: simple scans in the Y-Z plane -- Section ref:sec:test_nhexa_tilt_scans: scans in tilt are performed -- Section ref:sec:test_nhexa_nass_scans: scans with X-Y-Z translations in order to draw the word "NASS" - -**** Matlab Init :noexport:ignore: -#+begin_src matlab -%% reference_tracking_paths.m -% Computation of several reference paths -#+end_src - -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) -<> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes -<> -#+end_src - -#+begin_src matlab :tangle no :noweb yes -<> -#+end_src - -#+begin_src matlab :eval no :noweb yes -<> -#+end_src - -#+begin_src matlab :noweb yes -<> -#+end_src - -**** Y-Z Scans -<> -A function =generateYZScanTrajectory= has been developed in order to easily generate scans in the Y-Z plane. - -For instance, the following generated trajectory is represented in Figure ref:fig:test_nhexa_yz_scan_example_trajectory_yz_plane. -#+begin_src matlab -%% Generate the Y-Z trajectory scan -Rx_yz = generateYZScanTrajectory(... - 'y_tot', 4e-6, ... % Length of Y scans [m] - 'z_tot', 4e-6, ... % Total Z distance [m] - 'n', 5, ... % Number of Y scans - 'Ts', 1e-3, ... % Sampling Time [s] - 'ti', 1, ... % Time to go to initial position [s] - 'tw', 0, ... % Waiting time between each points [s] - 'ty', 0.6, ... % Time for a scan in Y [s] - 'tz', 0.2); % Time for a scan in Z [s] -#+end_src - -#+begin_src matlab :exports none -%% Plot the trajectory in the Y-Z plane -figure; -plot(Rx_yz(:,3), Rx_yz(:,4)); -xlabel('y [m]'); ylabel('z [m]'); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/yz_scan_example_trajectory_yz_plane.pdf', 'width', 'normal', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_yz_scan_example_trajectory_yz_plane -#+caption: Generated scan in the Y-Z plane -#+RESULTS: -[[file:figs/test_nhexa_yz_scan_example_trajectory_yz_plane.png]] - -The Y and Z positions as a function of time are shown in Figure ref:fig:test_nhexa_yz_scan_example_trajectory. - -#+begin_src matlab :exports none -%% Plot the Y-Z trajectory as a function of time -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'); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/yz_scan_example_trajectory.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_yz_scan_example_trajectory -#+caption: Y and Z trajectories as a function of time -#+RESULTS: -[[file:figs/test_nhexa_yz_scan_example_trajectory.png]] - -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} - -#+begin_src matlab :exports none -load('jacobian.mat', 'J'); -#+end_src - -#+begin_src matlab -%% Compute the reference in the frame of the legs -dL_ref = [J*Rx_yz(:, 2:7)']'; -#+end_src - -The reference signal for the strut length is shown in Figure ref:fig:test_nhexa_yz_scan_example_trajectory_struts. -#+begin_src matlab :exports none -%% Plot the reference in the frame of the legs -figure; -hold on; -for i=1:6 - plot(Rx_yz(:,1), dL_ref(:, i), ... - 'DisplayName', sprintf('$r_{d\\mathcal{L}_%i}$', i)) -end -xlabel('Time [s]'); ylabel('Strut Motion [m]'); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); -yticks(1e-6*[-5:5]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/yz_scan_example_trajectory_struts.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_yz_scan_example_trajectory_struts -#+caption: Trajectories for the 6 individual struts -#+RESULTS: -[[file:figs/test_nhexa_yz_scan_example_trajectory_struts.png]] - -**** Tilt Scans -<> - -A function =generalSpiralAngleTrajectory= has been developed in order to easily generate $R_x,R_y$ tilt scans. - -For instance, the following generated trajectory is represented in Figure ref:fig:test_nhexa_tilt_scan_example_trajectory. -#+begin_src matlab -%% Generate the "tilt-spiral" trajectory scan -R_tilt = generateSpiralAngleTrajectory(... - 'R_tot', 20e-6, ... % Total Tilt [ad] - 'n_turn', 5, ... % Number of scans - 'Ts', 1e-3, ... % Sampling Time [s] - 't_turn', 1, ... % Turn time [s] - 't_end', 1); % End time to go back to zero [s] -#+end_src - -#+begin_src matlab :exports none -%% Plot the trajectory -figure; -plot(1e6*R_tilt(:,5), 1e6*R_tilt(:,6)); -xlabel('$R_x$ [$\mu$rad]'); ylabel('$R_y$ [$\mu$rad]'); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/tilt_scan_example_trajectory.pdf', 'width', 'normal', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_tilt_scan_example_trajectory -#+caption: Generated "spiral" scan -#+RESULTS: -[[file:figs/test_nhexa_tilt_scan_example_trajectory.png]] - -#+begin_src matlab :exports none -%% Compute the reference in the frame of the legs -load('jacobian.mat', 'J'); -dL_ref = [J*R_tilt(:, 2:7)']'; -#+end_src - -The reference signal for the strut length is shown in Figure ref:fig:test_nhexa_tilt_scan_example_trajectory_struts. -#+begin_src matlab :exports none -%% Plot the reference in the frame of the legs -figure; -hold on; -for i=1:6 - plot(R_tilt(:,1), dL_ref(:, i), ... - 'DisplayName', sprintf('$r_{d\\mathcal{L}_%i}$', i)) -end -xlabel('Time [s]'); ylabel('Strut Motion [m]'); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); -yticks(1e-6*[-5:5]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/tilt_scan_example_trajectory_struts.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_tilt_scan_example_trajectory_struts -#+caption: Trajectories for the 6 individual struts - Tilt scan -#+RESULTS: -[[file:figs/test_nhexa_tilt_scan_example_trajectory_struts.png]] - -**** "NASS" reference path -<> -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. -#+begin_src matlab -%% 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 -ref_path = ref_path - (max(ref_path) - min(ref_path))/2; - -%% Define the X-Y-Z cuboid dimensions containing the trajectory -X_max = 10e-6; -Y_max = 4e-6; -Z_max = 2e-6; - -ref_path = ([X_max, Y_max, Z_max]./max(ref_path)).*ref_path; % [m] -#+end_src - -Then, using the =generateXYZTrajectory= function, the $6 \times 1$ trajectory signal is computed. -#+begin_src matlab -%% Generating the trajectory -Rx_nass = generateXYZTrajectory('points', ref_path); -#+end_src - -The trajectory in the X-Y plane is shown in Figure ref:fig:test_nhexa_ref_track_test_nass (the transitions between the letters are removed). -#+begin_src matlab :exports none -%% "NASS" trajectory in the X-Y plane -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))]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/ref_track_test_nass.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_ref_track_test_nass -#+caption: Reference path corresponding to the "NASS" acronym -#+RESULTS: -[[file:figs/test_nhexa_ref_track_test_nass.png]] - -It can also be better viewed in a 3D representation as in Figure ref:fig:test_nhexa_ref_track_test_nass_3d. - -#+begin_src matlab :exports none -figure; -plot3(1e6*Rx_nass(:,2), 1e6*Rx_nass(:,3), 1e6*Rx_nass(:,4), 'k-'); -xlabel('x [$\mu m$]'); ylabel('y [$\mu m$]'); zlabel('z [$\mu m$]'); -view(-13, 41) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/ref_track_test_nass_3d.pdf', 'width', 'normal', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_ref_track_test_nass_3d -#+caption: Reference path that draws "NASS" - 3D view -#+RESULTS: -[[file:figs/test_nhexa_ref_track_test_nass_3d.png]] - -*** First Basic High Authority Controller -:PROPERTIES: -:header-args:matlab+: :tangle matlab/scripts/hac_lac_first_try.m -:END: -<> -**** Introduction :ignore: -In this section, a simple decentralized high authority controller $\bm{K}_{\mathcal{L}}$ is developed to work without any payload. - -The diagonal controller is tuned using classical Loop Shaping in Section ref:sec:test_nhexa_hac_iff_no_payload_tuning. -The stability is verified in Section ref:sec:test_nhexa_hac_iff_no_payload_stability using the Simscape model. - -**** Matlab Init :noexport:ignore: -#+begin_src matlab -%% hac_lac_first_try.m -% Development and analysis of a first basic High Authority Controller -#+end_src - -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) -<> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes -<> -#+end_src - -#+begin_src matlab :tangle no :noweb yes -<> -#+end_src - -#+begin_src matlab :eval no :noweb yes -<> -#+end_src - -#+begin_src matlab :noweb yes -<> -<> -#+end_src - -#+begin_src matlab -%% Load the identified FRF and Simscape model -frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); -sim_iff = load('sim_iff_vib_table_m.mat', 'G_de'); -#+end_src - -**** HAC Controller -<> - -Let's first try to design a first decentralized controller with: -- a bandwidth of 100Hz -- sufficient phase margin -- simple and understandable components - -After some very basic and manual loop shaping, A diagonal controller is developed. -Each diagonal terms are identical and are composed of: -- A lead around 100Hz -- A first order low pass filter starting at 200Hz to add some robustness to high frequency modes -- A notch at 700Hz to cancel the flexible modes of the top plate -- A pure integrator - -#+begin_src matlab -%% 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 to increase robustness -H_lpf = 1/(1 + s/2/pi/200); - -%% 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); - -%% Decentralized HAC -Khac_iff_struts = -(1/(2.87e-5)) * ... % Gain - H_lead * ... % Lead - H_notch * ... % Notch - (2*pi*100/s) * ... % Integrator - eye(6); % 6x6 Diagonal -#+end_src - -This controller is saved for further use. -#+begin_src matlab :exports none :tangle no -save('matlab/data_sim/Khac_iff_struts.mat', 'Khac_iff_struts') -#+end_src - -#+begin_src matlab :eval no -save('data_sim/Khac_iff_struts.mat', 'Khac_iff_struts') -#+end_src - -The experimental loop gain is computed and shown in Figure ref:fig:test_nhexa_loop_gain_hac_iff_struts. -#+begin_src matlab -L_hac_iff_struts = pagemtimes(permute(frf_iff.G_de{1}, [2 3 1]), squeeze(freqresp(Khac_iff_struts, frf_iff.f, 'Hz'))); -#+end_src - -#+begin_src matlab :exports none -%% Bode plot of the Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - - -ax1 = nexttile([2,1]); -hold on; -% Diagonal Elements Model -plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(1,1,:))), 'color', colors(1,:), ... - 'DisplayName', 'Diagonal'); -for i = 2:6 - plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(i,i,:))), 'color', colors(1,:), ... - 'HandleVisibility', 'off'); -end -plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(1,2,:))), 'color', [colors(2,:), 0.2], ... - 'DisplayName', 'Off-Diag'); -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(squeeze(L_hac_iff_struts(i,j,:))), 'color', [colors(2,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e2]); -legend('location', 'northeast'); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(squeeze(L_hac_iff_struts(i,i,:))), 'color', colors(1,:)); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([2, 2e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/loop_gain_hac_iff_struts.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_loop_gain_hac_iff_struts -#+caption: Diagonal and off-diagonal elements of the Loop gain for "HAC-IFF-Struts" -#+RESULTS: -[[file:figs/test_nhexa_loop_gain_hac_iff_struts.png]] - -**** Verification of the Stability using the Simscape model -<> - -The HAC-IFF control strategy is implemented using Simscape. -#+begin_src matlab -%% 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'); -#+end_src - -#+begin_src matlab :exports none -support.type = 1; % On top of vibration table -payload.type = 3; % Payload / 1 "mass layer" - -load('Kiff_opt.mat', 'Kiff'); -#+end_src - -#+begin_src matlab -%% Identify the (damped) transfer function from u to dLm -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) -#+end_src - -We identify the closed-loop system. -#+begin_src matlab -%% Identification -Gd_iff_hac_opt = linearize(mdl, io, 0.0, options); -#+end_src - -And verify that it is indeed stable. -#+begin_src matlab :results value replace :exports both -%% Verify the stability -isstable(Gd_iff_hac_opt) -#+end_src - -#+RESULTS: -: 1 - -**** Experimental Validation -Both the Integral Force Feedback controller (developed in Section ref:sec:test_nhexa_enc_plates_iff) and the high authority controller working in the frame of the struts (developed in Section ref:sec:test_nhexa_hac_iff_struts_controller) are implemented experimentally. - -Two reference tracking experiments are performed to evaluate the stability and performances of the implemented control. - -#+begin_src matlab -%% Load the experimental data -load('hac_iff_struts_yz_scans.mat', 't', 'de') -#+end_src - -#+begin_src matlab :exports none -%% Reset initial time -t = t - t(1); -#+end_src - -The position of the top-platform is estimated using the Jacobian matrix: -#+begin_src matlab -%% Pose of the top platform from the encoder values -load('jacobian.mat', 'J'); -Xe = [inv(J)*de']'; -#+end_src - -#+begin_src matlab -%% 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', 1, ... % Time to go to initial position [s] - 'tw', 0, ... % Waiting time between each points [s] - 'ty', 0.6, ... % Time for a scan in Y [s] - 'tz', 0.2); % Time for a scan in Z [s] -#+end_src - -The reference path as well as the measured position are partially shown in the Y-Z plane in Figure ref:fig:test_nhexa_yz_scans_exp_results_first_K. -#+begin_src matlab :exports none -%% Position and reference signal in the Y-Z plane -figure; -tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile; -hold on; -plot(1e6*Xe(t>2,2), 1e6*Xe(t>2,3)); -plot(1e6*Rx_yz(:,3), 1e6*Rx_yz(:,4), '--'); -hold off; -xlabel('Y [$\mu m$]'); ylabel('Z [$\mu m$]'); -xlim([-2.05, 2.05]); ylim([-4.1, 4.1]); -axis equal; - -ax2 = nexttile([1,2]); -hold on; -plot(1e6*Xe(:,2), 1e6*Xe(:,3), ... - 'DisplayName', '$\mathcal{X}_n$'); -plot(1e6*Rx_yz(:,3), 1e6*Rx_yz(:,4), '--', ... - 'DisplayName', '$r_{\mathcal{X}_n}$'); -hold off; -legend('location', 'northwest'); -xlabel('Y [$\mu m$]'); ylabel('Z [$\mu m$]'); -axis equal; -xlim([1.6, 2.1]); ylim([-4.1, -3.6]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/yz_scans_exp_results_first_K.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_yz_scans_exp_results_first_K -#+caption: 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 -#+RESULTS: -[[file:figs/test_nhexa_yz_scans_exp_results_first_K.png]] - -#+begin_important -It is clear from Figure ref:fig:test_nhexa_yz_scans_exp_results_first_K 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. -#+end_important - -**** Controller with increased stability margins -The High Authority Controller is re-designed in order to improve the stability margins. -#+begin_src matlab -%% 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 -#+end_src - -#+begin_src matlab :exports none -%% Load the FRF of the transfer function from u to dL with IFF -frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); -#+end_src - -#+begin_src matlab :exports none -%% Compute the Loop Gain -L_frf = pagemtimes(permute(frf_iff.G_de{1}, [2 3 1]), squeeze(freqresp(Khac_iff_struts, frf_iff.f, 'Hz'))); -#+end_src - -The bode plot of the new loop gain is shown in Figure ref:fig:test_nhexa_hac_iff_plates_exp_loop_gain_redesigned_K. -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm -freqs = 2*logspace(1, 3, 1000); - -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -% Diagonal Elements FRF -plot(frf_iff.f, abs(squeeze(L_frf(1,1,:))), 'color', colors(1,:), ... - 'DisplayName', 'Diagonal'); -for i = 2:6 - plot(frf_iff.f, abs(squeeze(L_frf(i,i,:))), 'color', colors(1,:), ... - 'HandleVisibility', 'off'); -end -plot(frf_iff.f, abs(squeeze(L_frf(1,2,:))), 'color', [colors(2,:), 0.2], ... - 'DisplayName', 'Off-Diag'); -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(squeeze(L_frf(i,j,:))), 'color', [colors(2,:), 0.2], ... - 'HandleVisibility', 'off'); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain [-]'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e2]); -legend('location', 'northeast'); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(squeeze(L_frf(i,i,:))), 'color', colors(1,:)); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([1, 2e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/hac_iff_plates_exp_loop_gain_redesigned_K.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_hac_iff_plates_exp_loop_gain_redesigned_K -#+caption: Loop Gain for the updated decentralized HAC controller -#+RESULTS: -[[file:figs/test_nhexa_hac_iff_plates_exp_loop_gain_redesigned_K.png]] - -This new controller is implemented experimentally and several tracking tests are performed. -#+begin_src matlab -%% Load Measurements -load('hac_iff_more_lead_nass_scan.mat', 't', 'de') -#+end_src - -#+begin_src matlab :exports none -%% Reset Time -t = t - t(1); -#+end_src - -The pose of the top platform is estimated from the encoder position using the Jacobian matrix. -#+begin_src matlab -%% Compute the pose of the top platform -load('jacobian.mat', 'J'); -Xe = [inv(J)*de']'; -#+end_src - -#+begin_src matlab :exports none -%% Load the reference path -load('reference_path.mat', 'Rx_nass') -#+end_src - -The measured motion as well as the trajectory are shown in Figure ref:fig:test_nhexa_nass_scans_first_test_exp. -#+begin_src matlab :exports none -%% Plot the X-Y-Z "NASS" trajectory -figure; -hold on; -plot3(Xe(1:100:end,1), Xe(1:100:end,2), Xe(1:100:end,3)) -plot3(Rx_nass(1:100:end,2), Rx_nass(1:100:end,3), Rx_nass(1:100:end,4)) -hold off; -xlabel('x [$\mu m$]'); ylabel('y [$\mu m$]'); zlabel('z [$\mu m$]'); -view(-13, 41) -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/nass_scans_first_test_exp.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_nass_scans_first_test_exp -#+caption: Measured position $\bm{\mathcal{X}}_n$ and reference signal $\bm{r}_{\mathcal{X}_n}$ for the "NASS" trajectory -#+RESULTS: -[[file:figs/test_nhexa_nass_scans_first_test_exp.png]] - -The trajectory and measured motion are also shown in the X-Y plane in Figure ref:fig:test_nhexa_ref_track_nass_exp_hac_iff_struts. -#+begin_src matlab :exports none -%% Estimate when the hexpod is on top position and drawing the letters -i_top = Xe(:,3) > 1.9e-6; -i_rx = Rx_nass(:,4) > 0; -#+end_src - -#+begin_src matlab :exports none -%% Plot the reference as well as the measurement in the X-Y plane -figure; -tiledlayout(1, 3, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([1,2]); -hold on; -scatter(1e6*Xe(i_top,1), 1e6*Xe(i_top,2),'.'); -plot(1e6*Rx_nass(i_rx,2), 1e6*Rx_nass(i_rx,3), '--'); -hold off; -xlabel('X [$\mu m$]'); ylabel('Y [$\mu m$]'); -axis equal; -xlim([-10.5, 10.5]); ylim([-4.5, 4.5]); - -ax2 = nexttile; -hold on; -scatter(1e6*Xe(i_top,1), 1e6*Xe(i_top,2),'.'); -plot(1e6*Rx_nass(i_rx,2), 1e6*Rx_nass(i_rx,3), '--'); -hold off; -xlabel('X [$\mu m$]'); ylabel('Y [$\mu m$]'); -axis equal; -xlim([4.5, 4.7]); ylim([-0.15, 0.05]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/ref_track_nass_exp_hac_iff_struts.pdf', 'width', 'full', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_ref_track_nass_exp_hac_iff_struts -#+caption: Reference path and measured motion in the X-Y plane -#+RESULTS: -[[file:figs/test_nhexa_ref_track_nass_exp_hac_iff_struts.png]] - -The orientation errors during all the scans are shown in Figure ref:fig:test_nhexa_nass_ref_rx_ry. -#+begin_src matlab :exports none -%% Orientation Errors -figure; -hold on; -plot(t(t>20&t<20.1), 1e6*Xe(t>20&t<20.1,4), '-', 'DisplayName', '$\epsilon_{\theta_x}$'); -plot(t(t>20&t<20.1), 1e6*Xe(t>20&t<20.1,5), '-', 'DisplayName', '$\epsilon_{\theta_y}$'); -plot(t(t>20&t<20.1), 1e6*Xe(t>20&t<20.1,6), '-', 'DisplayName', '$\epsilon_{\theta_z}$'); -hold off; -xlabel('Time [s]'); ylabel('Orientation Error [$\mu$ rad]'); -legend('location', 'northeast'); -#+end_src - -#+begin_src matlab :exports none -%% Orientation Errors -figure; -hold on; -plot(1e9*Xe(100000:100:end,4), 1e9*Xe(100000:100:end,5), '.'); -th = 0:pi/50:2*pi; -xunit = 90 * cos(th); -yunit = 90 * sin(th); -plot(xunit, yunit, '--'); -hold off; -xlabel('$R_x$ [nrad]'); ylabel('$R_y$ [nrad]'); -xlim([-100, 100]); -ylim([-100, 100]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/nass_ref_rx_ry.pdf', 'width', 500, 'height', 500); -#+end_src - -#+name: fig:test_nhexa_nass_ref_rx_ry -#+caption: Orientation errors during the scan -#+RESULTS: -[[file:figs/test_nhexa_nass_ref_rx_ry.png]] - -#+begin_important -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). -#+end_important - -*** Interaction Analysis and Decoupling -:PROPERTIES: -:header-args:matlab+: :tangle matlab/scripts/interaction_analysis_enc_plates.m -:END: -<> -**** Introduction :ignore: - -In this section, the interaction in the identified plant is estimated using the Relative Gain Array (RGA) [[cite:skogestad07_multiv_feedb_contr][Chap. 3.4]]. - -Then, several decoupling strategies are compared for the nano-hexapod. - -The RGA Matrix is defined as follow: -\begin{equation} - \text{RGA}(G(f)) = G(f) \times (G(f)^{-1})^T -\end{equation} - -Then, the RGA number is defined: -\begin{equation} -\text{RGA-num}(f) = \| \text{I - RGA(G(f))} \|_{\text{sum}} -\end{equation} - - -In this section, the plant with 2 added mass is studied. - -**** Matlab Init :noexport:ignore: -#+begin_src matlab -%% interaction_analysis_enc_plates.m -% Interaction analysis of several decoupling strategies -#+end_src - -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) -<> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes -<> -#+end_src - -#+begin_src matlab :tangle no :noweb yes -<> -#+end_src - -#+begin_src matlab :eval no :noweb yes -<> -#+end_src - -#+begin_src matlab :noweb yes -<> -#+end_src - -#+begin_src matlab -%% Load the identified FRF and Simscape model -frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); -sim_iff = load('sim_iff_vib_table_m.mat', 'G_de'); -#+end_src - -**** Parameters -#+begin_src matlab -wc = 100; % Wanted crossover frequency [Hz] -[~, i_wc] = min(abs(frf_iff.f - wc)); % Indice corresponding to wc -#+end_src - -#+begin_src matlab -%% Plant to be decoupled -frf_coupled = frf_iff.G_de{2}; -G_coupled = sim_iff.G_de{2}; -#+end_src - -**** No Decoupling (Decentralized) -<> - -#+begin_src latex :file decoupling_arch_decentralized.pdf -\begin{tikzpicture} - \node[block] (G) {$\bm{G}$}; - - % Connections and labels - \draw[<-] (G.west) -- ++(-1.8, 0) node[above right]{$\bm{\tau}$}; - \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; - - \begin{scope}[on background layer] - \node[fit={(G.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gdec) {}; - \node[below right] at (Gdec.north west) {$\bm{G}_{\text{dec}}$}; - \end{scope} -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_decoupling_arch_decentralized -#+caption: Block diagram representing the plant. -#+RESULTS: -[[file:figs/test_nhexa_decoupling_arch_decentralized.png]] - -#+begin_src matlab :exports none -%% Decentralized Plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(frf_coupled(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -for i = 1:6 - plot(frf_iff.f, abs(frf_coupled(:,i,i)), ... - 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); -end -plot(frf_iff.f, abs(frf_coupled(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-9, 1e-4]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 3); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(frf_coupled(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_decentralized_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_decentralized_plant -#+caption: Bode Plot of the decentralized plant (diagonal and off-diagonal terms) -#+RESULTS: -[[file:figs/test_nhexa_interaction_decentralized_plant.png]] - -#+begin_src matlab :exports none -%% Decentralized RGA -RGA_dec = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - RGA_dec(i,:,:) = squeeze(frf_coupled(i,:,:)).*inv(squeeze(frf_coupled(i,:,:))).'; -end - -RGA_dec_sum = zeros(length(frf_iff), 1); -for i = 1:length(frf_iff.f) - RGA_dec_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_dec(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% RGA for Decentralized plant -figure; -plot(frf_iff.f, RGA_dec_sum, 'k-'); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_rga_decentralized.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_interaction_rga_decentralized -#+caption: RGA number for the decentralized plant -#+RESULTS: -[[file:figs/test_nhexa_interaction_rga_decentralized.png]] - -**** Static Decoupling -<> - -#+begin_src latex :file decoupling_arch_static.pdf -\begin{tikzpicture} - \node[block] (G) {$\bm{G}$}; - \node[block, left=0.8 of G] (Ginv) {$\bm{\hat{G}}(j0)^{-1}$}; - - % Connections and labels - \draw[<-] (Ginv.west) -- ++(-1.8, 0) node[above right]{$\bm{u}$}; - \draw[->] (Ginv.east) -- (G.west) node[above left]{$\bm{\tau}$}; - \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; - - \begin{scope}[on background layer] - \node[fit={(Ginv.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; - \node[below right] at (Gx.north west) {$\bm{G}_{\text{static}}$}; - \end{scope} -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_decoupling_arch_static -#+caption: Decoupling using the inverse of the DC gain of the plant -#+RESULTS: -[[file:figs/test_nhexa_decoupling_arch_static.png]] - -The DC gain is evaluated from the model as be have bad low frequency identification. - -#+begin_src matlab :exports none -%% Compute the inverse of the DC gain -G_model = G_coupled; -G_model.outputdelay = 0; % necessary for further inversion -dc_inv = inv(dcgain(G_model)); - -%% Compute the inversed plant -G_de_sta = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - G_de_sta(i,:,:) = squeeze(frf_coupled(i,:,:))*dc_inv; -end -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no -data2orgtable(dc_inv, {}, {}, ' %.1f '); -#+end_src - -#+RESULTS: -| -62011.5 | 3910.6 | 4299.3 | 660.7 | -4016.5 | -4373.6 | -| 3914.4 | -61991.2 | -4356.8 | -4019.2 | 640.2 | 4281.6 | -| -4020.0 | -4370.5 | -62004.5 | 3914.6 | 4295.8 | 653.8 | -| 660.9 | 4292.4 | 3903.3 | -62012.2 | -4366.5 | -4008.9 | -| 4302.8 | 655.6 | -4025.8 | -4377.8 | -62006.0 | 3919.7 | -| -4377.9 | -4013.2 | 668.6 | 4303.7 | 3906.8 | -62019.3 | - -#+begin_src matlab :exports none -%% Bode plot of the static decoupled plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_sta(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -for i = 1:6 - plot(frf_iff.f, abs(G_de_sta(:,i,i)), ... - 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); -end -plot(frf_iff.f, abs(G_de_sta(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e1]); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_sta(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_static_dec_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_static_dec_plant -#+caption: Bode Plot of the static decoupled plant -#+RESULTS: -[[file:figs/test_nhexa_interaction_static_dec_plant.png]] - -#+begin_src matlab :exports none -%% Compute RGA Matrix -RGA_sta = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - RGA_sta(i,:,:) = squeeze(G_de_sta(i,:,:)).*inv(squeeze(G_de_sta(i,:,:))).'; -end - -%% Compute RGA-number -RGA_sta_sum = zeros(length(frf_iff), 1); -for i = 1:size(RGA_sta, 1) - RGA_sta_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_sta(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Plot the RGA-number for statically decoupled plant -figure; -plot(frf_iff.f, RGA_sta_sum, 'k-'); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_rga_static_dec.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_interaction_rga_static_dec -#+caption: RGA number for the statically decoupled plant -#+RESULTS: -[[file:figs/test_nhexa_interaction_rga_static_dec.png]] - -**** Decoupling at the Crossover -<> - -#+begin_src latex :file decoupling_arch_crossover.pdf -\begin{tikzpicture} - \node[block] (G) {$\bm{G}$}; - \node[block, left=0.8 of G] (Ginv) {$\bm{\hat{G}}(j\omega_c)^{-1}$}; - - % Connections and labels - \draw[<-] (Ginv.west) -- ++(-1.8, 0) node[above right]{$\bm{u}$}; - \draw[->] (Ginv.east) -- (G.west) node[above left]{$\bm{\tau}$}; - \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; - - \begin{scope}[on background layer] - \node[fit={(Ginv.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; - \node[below right] at (Gx.north west) {$\bm{G}_{\omega_c}$}; - \end{scope} -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_decoupling_arch_crossover -#+caption: Decoupling using the inverse of a dynamical model $\bm{\hat{G}}$ of the plant dynamics $\bm{G}$ -#+RESULTS: -[[file:figs/test_nhexa_decoupling_arch_crossover.png]] - -#+begin_src matlab :exports none -%% Take complex matrix corresponding to the plant at 100Hz -V = squeeze(frf_coupled(i_wc,:,:)); - -%% Real approximation of inv(G(100Hz)) -D = pinv(real(V'*V)); -H1 = D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2))); - -%% Compute the decoupled plant -G_de_wc = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - G_de_wc(i,:,:) = squeeze(frf_coupled(i,:,:))*H1; -end -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no -data2orgtable(H1, {}, {}, ' %.1f '); -#+end_src - -#+RESULTS: -| 67229.8 | 3769.3 | -13704.6 | -23084.8 | -6318.2 | 23378.7 | -| 3486.2 | 67708.9 | 23220.0 | -6314.5 | -22699.8 | -14060.6 | -| -5731.7 | 22471.7 | 66701.4 | 3070.2 | -13205.6 | -21944.6 | -| -23305.5 | -14542.6 | 2743.2 | 70097.6 | 24846.8 | -5295.0 | -| -14882.9 | -22957.8 | -5344.4 | 25786.2 | 70484.6 | 2979.9 | -| 24353.3 | -5195.2 | -22449.0 | -14459.2 | 2203.6 | 69484.2 | - -#+begin_src matlab :exports none -%% Bode plot of the plant decoupled at the crossover -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_wc(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -for i = 1:6 - plot(frf_iff.f, abs(G_de_wc(:,i,i)), ... - 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); -end -plot(frf_iff.f, abs(G_de_wc(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude $d_L/V_a$ [m/V]'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e1]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_wc(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_wc_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_wc_plant -#+caption: Bode Plot of the plant decoupled at the crossover -#+RESULTS: -[[file:figs/test_nhexa_interaction_wc_plant.png]] - -#+begin_src matlab -%% Compute RGA Matrix -RGA_wc = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - RGA_wc(i,:,:) = squeeze(G_de_wc(i,:,:)).*inv(squeeze(G_de_wc(i,:,:))).'; -end - -%% Compute RGA-number -RGA_wc_sum = zeros(size(RGA_wc, 1), 1); -for i = 1:size(RGA_wc, 1) - RGA_wc_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_wc(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Plot the RGA-Number for the plant decoupled at crossover -figure; -plot(frf_iff.f, RGA_wc_sum, 'k-'); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_rga_wc.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_interaction_rga_wc -#+caption: RGA number for the plant decoupled at the crossover -#+RESULTS: -[[file:figs/test_nhexa_interaction_rga_wc.png]] - -**** SVD Decoupling -<> - -#+begin_src latex :file decoupling_arch_svd.pdf -\begin{tikzpicture} - \node[block] (G) {$\bm{G}$}; - - \node[block, left=0.8 of G.west] (V) {$V^{-T}$}; - \node[block, right=0.8 of G.east] (U) {$U^{-1}$}; - - % Connections and labels - \draw[<-] (V.west) -- ++(-1.0, 0) node[above right]{$u$}; - \draw[->] (V.east) -- (G.west) node[above left]{$\bm{\tau}$}; - \draw[->] (G.east) -- (U.west) node[above left]{$d\bm{\mathcal{L}}$}; - \draw[->] (U.east) -- ++( 1.0, 0) node[above left]{$y$}; - - \begin{scope}[on background layer] - \node[fit={(V.south west) (G.north-|U.east)}, fill=black!10!white, draw, dashed, inner sep=14pt] (Gsvd) {}; - \node[below right] at (Gsvd.north west) {$\bm{G}_{SVD}$}; - \end{scope} -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_decoupling_arch_svd -#+caption: Decoupling using the Singular Value Decomposition -#+RESULTS: -[[file:figs/test_nhexa_decoupling_arch_svd.png]] - -#+begin_src matlab :exports none -%% Take complex matrix corresponding to the plant at 100Hz -V = squeeze(frf_coupled(i_wc,:,:)); - -%% Real approximation of G(100Hz) -D = pinv(real(V'*V)); -H1 = pinv(D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2)))); - -%% Singular Value Decomposition -[U,S,V] = svd(H1); - -%% Compute the decoupled plant using SVD -G_de_svd = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - G_de_svd(i,:,:) = inv(U)*squeeze(frf_coupled(i,:,:))*inv(V'); -end -#+end_src - -#+begin_src matlab :exports none -%% Bode Plot of the SVD decoupled plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_svd(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -for i = 1:6 - plot(frf_iff.f, abs(G_de_svd(:,i,i)), ... - 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); -end -plot(frf_iff.f, abs(G_de_svd(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-9, 1e-4]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 3); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_svd(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_svd_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_svd_plant -#+caption: Bode Plot of the plant decoupled using the Singular Value Decomposition -#+RESULTS: -[[file:figs/test_nhexa_interaction_svd_plant.png]] - -#+begin_src matlab -%% Compute the RGA matrix for the SVD decoupled plant -RGA_svd = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - RGA_svd(i,:,:) = squeeze(G_de_svd(i,:,:)).*inv(squeeze(G_de_svd(i,:,:))).'; -end - -%% Compute the RGA-number -RGA_svd_sum = zeros(size(RGA_svd, 1), 1); -for i = 1:length(frf_iff.f) - RGA_svd_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_svd(i,:,:))))); -end -#+end_src - -#+begin_src matlab -%% RGA Number for the SVD decoupled plant -figure; -plot(frf_iff.f, RGA_svd_sum, 'k-'); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_rga_svd.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_interaction_rga_svd -#+caption: RGA number for the plant decoupled using the SVD -#+RESULTS: -[[file:figs/test_nhexa_interaction_rga_svd.png]] - -**** Dynamic decoupling -<> - -#+begin_src latex :file decoupling_arch_dynamic.pdf -\begin{tikzpicture} - \node[block] (G) {$\bm{G}$}; - \node[block, left=0.8 of G] (Ginv) {$\bm{\hat{G}}^{-1}$}; - - % Connections and labels - \draw[<-] (Ginv.west) -- ++(-1.8, 0) node[above right]{$\bm{u}$}; - \draw[->] (Ginv.east) -- (G.west) node[above left]{$\bm{\tau}$}; - \draw[->] (G.east) -- ++( 1.8, 0) node[above left]{$d\bm{\mathcal{L}}$}; - - \begin{scope}[on background layer] - \node[fit={(Ginv.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; - \node[below right] at (Gx.north west) {$\bm{G}_{\text{inv}}$}; - \end{scope} -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_decoupling_arch_dynamic -#+caption: Decoupling using the inverse of a dynamical model $\bm{\hat{G}}$ of the plant dynamics $\bm{G}$ -#+RESULTS: -[[file:figs/test_nhexa_decoupling_arch_dynamic.png]] - -#+begin_src matlab :exports none -%% Compute the plant inverse from the model -G_model = G_coupled; -G_model.outputdelay = 0; % necessary for further inversion -G_inv = inv(G_model); - -%% Compute the decoupled plant -G_de_inv = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - G_de_inv(i,:,:) = squeeze(frf_coupled(i,:,:))*squeeze(evalfr(G_inv, 1j*2*pi*frf_iff.f(i))); -end -#+end_src - -#+begin_src matlab :exports none -%% Bode plot of the decoupled plant by full inversion -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_inv(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -for i = 1:6 - plot(frf_iff.f, abs(G_de_inv(:,i,i)), ... - 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); -end -plot(frf_iff.f, abs(G_de_inv(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e1]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_inv(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_dynamic_dec_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_dynamic_dec_plant -#+caption: Bode Plot of the dynamically decoupled plant -#+RESULTS: -[[file:figs/test_nhexa_interaction_dynamic_dec_plant.png]] - -#+begin_src matlab :exports none -%% Compute the RGA matrix for the inverse based decoupled plant -RGA_inv = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - RGA_inv(i,:,:) = squeeze(G_de_inv(i,:,:)).*inv(squeeze(G_de_inv(i,:,:))).'; -end - -%% Compute the RGA-number -RGA_inv_sum = zeros(size(RGA_inv, 1), 1); -for i = 1:size(RGA_inv, 1) - RGA_inv_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_inv(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% RGA Number for the decoupled plant using full inversion -figure; -plot(frf_iff.f, RGA_inv_sum, 'k-'); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_rga_dynamic_dec.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_interaction_rga_dynamic_dec -#+caption: RGA number for the dynamically decoupled plant -#+RESULTS: -[[file:figs/test_nhexa_interaction_rga_dynamic_dec.png]] - -**** Jacobian Decoupling - Center of Stiffness -<> - -#+begin_src latex :file decoupling_arch_jacobian_cok.pdf -\begin{tikzpicture} - \node[block] (G) {$\bm{G}$}; - \node[block, left=0.8 of G] (Jt) {$J_{s,\{K\}}^{-T}$}; - \node[block, right=0.8 of G] (Ja) {$J_{a,\{K\}}^{-1}$}; - - % Connections and labels - \draw[<-] (Jt.west) -- ++(-1.8, 0) node[above right]{$\bm{\mathcal{F}}_{\{K\}}$}; - \draw[->] (Jt.east) -- (G.west) node[above left]{$\bm{\tau}$}; - \draw[->] (G.east) -- (Ja.west) node[above left]{$d\bm{\mathcal{L}}$}; - \draw[->] (Ja.east) -- ++( 1.8, 0) node[above left]{$\bm{\mathcal{X}}_{\{K\}}$}; - - \begin{scope}[on background layer] - \node[fit={(Jt.south west) (Ja.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; - \node[below right] at (Gx.north west) {$\bm{G}_{\{K\}}$}; - \end{scope} -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_decoupling_arch_jacobian_cok -#+caption: Decoupling using Jacobian matrices evaluated at the Center of Stiffness -#+RESULTS: -[[file:figs/test_nhexa_decoupling_arch_jacobian_cok.png]] - -#+begin_src matlab :exports none -%% Initialize the Nano-Hexapod -n_hexapod = initializeNanoHexapodFinal('MO_B', -42e-3, ... - 'motion_sensor_type', 'plates'); - -%% Get the Jacobians -J_cok = n_hexapod.geometry.J; -Js_cok = n_hexapod.geometry.Js; - -%% Decouple plant using Jacobian (CoM) -G_de_J_cok = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - G_de_J_cok(i,:,:) = inv(Js_cok)*squeeze(frf_coupled(i,:,:))*inv(J_cok'); -end -#+end_src - -The obtained plant is shown in Figure ref:fig:test_nhexa_interaction_J_cok_plant_not_normalized. -We can see that the stiffness in the $x$, $y$ and $z$ directions are equal, which is due to the cubic architecture of the Stewart platform. - -#+begin_src matlab :exports none -%% Bode Plot of the SVD decoupled plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_J_cok(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -plot(frf_iff.f, abs(G_de_J_cok(:,1,1)), ... - 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); -plot(frf_iff.f, abs(G_de_J_cok(:,2,2)), ... - 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); -plot(frf_iff.f, abs(G_de_J_cok(:,3,3)), ... - 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); -plot(frf_iff.f, abs(G_de_J_cok(:,4,4)), ... - 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); -plot(frf_iff.f, abs(G_de_J_cok(:,5,5)), ... - 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); -plot(frf_iff.f, abs(G_de_J_cok(:,6,6)), ... - 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); -plot(frf_iff.f, abs(G_de_J_cok(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-8, 2e-2]); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 3); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_J_cok(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_J_cok_plant_not_normalized.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_J_cok_plant_not_normalized -#+caption: Bode Plot of the plant decoupled using the Jacobian evaluated at the "center of stiffness" -#+RESULTS: -[[file:figs/test_nhexa_interaction_J_cok_plant_not_normalized.png]] - -Because the plant in translation and rotation has very different gains, we choose to normalize the plant inputs such that the gain of the diagonal term is equal to $1$ at 100Hz. - -The results is shown in Figure ref:fig:test_nhexa_interaction_J_cok_plant. -#+begin_src matlab :exports none -%% Normalize the plant input -[~, i_100] = min(abs(frf_iff.f - 100)); -input_normalize = diag(1./diag(abs(squeeze(G_de_J_cok(i_100,:,:))))); - -for i = 1:length(frf_iff.f) - G_de_J_cok(i,:,:) = squeeze(G_de_J_cok(i,:,:))*input_normalize; -end -#+end_src - -#+begin_src matlab :exports none -%% Bode Plot of the SVD decoupled plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_J_cok(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -plot(frf_iff.f, abs(G_de_J_cok(:,1,1)), ... - 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); -plot(frf_iff.f, abs(G_de_J_cok(:,2,2)), ... - 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); -plot(frf_iff.f, abs(G_de_J_cok(:,3,3)), ... - 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); -plot(frf_iff.f, abs(G_de_J_cok(:,4,4)), ... - 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); -plot(frf_iff.f, abs(G_de_J_cok(:,5,5)), ... - 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); -plot(frf_iff.f, abs(G_de_J_cok(:,6,6)), ... - 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); -plot(frf_iff.f, abs(G_de_J_cok(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-4, 1e1]); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_J_cok(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_J_cok_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_J_cok_plant -#+caption: Bode Plot of the plant decoupled using the Jacobian evaluated at the "center of stiffness" -#+RESULTS: -[[file:figs/test_nhexa_interaction_J_cok_plant.png]] - -#+begin_src matlab :exports none -%% Compute RGA Matrix -RGA_cok = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - RGA_cok(i,:,:) = squeeze(G_de_J_cok(i,:,:)).*inv(squeeze(G_de_J_cok(i,:,:))).'; -end - -%% Compute RGA-number -RGA_cok_sum = zeros(length(frf_iff.f), 1); -for i = 1:length(frf_iff.f) - RGA_cok_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_cok(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Plot the RGA-Number for the Jacobian (CoK) decoupled plant -figure; -plot(frf_iff.f, RGA_cok_sum, 'k-'); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_rga_J_cok.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_interaction_rga_J_cok -#+caption: RGA number for the plant decoupled using the Jacobian evaluted at the Center of Stiffness -#+RESULTS: -[[file:figs/test_nhexa_interaction_rga_J_cok.png]] - -**** Jacobian Decoupling - Center of Mass -<> - -#+begin_src latex :file decoupling_arch_jacobian_com.pdf -\begin{tikzpicture} - \node[block] (G) {$\bm{G}$}; - \node[block, left=0.8 of G] (Jt) {$J_{s,\{M\}}^{-T}$}; - \node[block, right=0.8 of G] (Ja) {$J_{a,\{M\}}^{-1}$}; - - % Connections and labels - \draw[<-] (Jt.west) -- ++(-1.8, 0) node[above right]{$\bm{\mathcal{F}}_{\{M\}}$}; - \draw[->] (Jt.east) -- (G.west) node[above left]{$\bm{\tau}$}; - \draw[->] (G.east) -- (Ja.west) node[above left]{$d\bm{\mathcal{L}}$}; - \draw[->] (Ja.east) -- ++( 1.8, 0) node[above left]{$\bm{\mathcal{X}}_{\{M\}}$}; - - \begin{scope}[on background layer] - \node[fit={(Jt.south west) (Ja.north east)}, fill=black!10!white, draw, dashed, inner sep=16pt] (Gx) {}; - \node[below right] at (Gx.north west) {$\bm{G}_{\{M\}}$}; - \end{scope} -\end{tikzpicture} -#+end_src - -#+name: fig:test_nhexa_decoupling_arch_jacobian_com -#+caption: Decoupling using Jacobian matrices evaluated at the Center of Mass -#+RESULTS: -[[file:figs/test_nhexa_decoupling_arch_jacobian_com.png]] - -#+begin_src matlab :exports none -%% Initialize the Nano-Hexapod -n_hexapod = initializeNanoHexapodFinal('MO_B', 25e-3, ... - 'motion_sensor_type', 'plates'); - -%% Get the Jacobians -J_com = n_hexapod.geometry.J; -Js_com = n_hexapod.geometry.Js; - -%% Decouple plant using Jacobian (CoM) -G_de_J_com = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - G_de_J_com(i,:,:) = inv(Js_com)*squeeze(frf_coupled(i,:,:))*inv(J_com'); -end - -%% Normalize the plant input -[~, i_100] = min(abs(frf_iff.f - 100)); -input_normalize = diag(1./diag(abs(squeeze(G_de_J_com(i_100,:,:))))); - -for i = 1:length(frf_iff.f) - G_de_J_com(i,:,:) = squeeze(G_de_J_com(i,:,:))*input_normalize; -end -#+end_src - -#+begin_src matlab :exports none -%% Bode Plot of the SVD decoupled plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_J_com(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -plot(frf_iff.f, abs(G_de_J_com(:,1,1)), ... - 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); -plot(frf_iff.f, abs(G_de_J_com(:,2,2)), ... - 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); -plot(frf_iff.f, abs(G_de_J_com(:,3,3)), ... - 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); -plot(frf_iff.f, abs(G_de_J_com(:,4,4)), ... - 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); -plot(frf_iff.f, abs(G_de_J_com(:,5,5)), ... - 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); -plot(frf_iff.f, abs(G_de_J_com(:,6,6)), ... - 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); -plot(frf_iff.f, abs(G_de_J_com(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e1]); -legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_J_com(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_J_com_plant.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_J_com_plant -#+caption: Bode Plot of the plant decoupled using the Jacobian evaluated at the Center of Mass -#+RESULTS: -[[file:figs/test_nhexa_interaction_J_com_plant.png]] - -#+begin_src matlab :exports none -%% Compute RGA Matrix -RGA_com = zeros(size(frf_coupled)); -for i = 1:length(frf_iff.f) - RGA_com(i,:,:) = squeeze(G_de_J_com(i,:,:)).*inv(squeeze(G_de_J_com(i,:,:))).'; -end - -%% Compute RGA-number -RGA_com_sum = zeros(size(RGA_com, 1), 1); -for i = 1:size(RGA_com, 1) - RGA_com_sum(i) = sum(sum(abs(eye(6) - squeeze(RGA_com(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Plot the RGA-Number for the Jacobian (CoM) decoupled plant -figure; -plot(frf_iff.f, RGA_com_sum, 'k-'); -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_rga_J_com.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_interaction_rga_J_com -#+caption: RGA number for the plant decoupled using the Jacobian evaluted at the Center of Mass -#+RESULTS: -[[file:figs/test_nhexa_interaction_rga_J_com.png]] - -**** Decoupling Comparison -<> - -Let's now compare all of the decoupling methods (Figure ref:fig:test_nhexa_interaction_compare_rga_numbers). - -#+begin_important -From Figure ref:fig:test_nhexa_interaction_compare_rga_numbers, the following remarks are made: -- *Decentralized plant*: well decoupled below suspension modes -- *Static inversion*: similar to the decentralized plant as the decentralized plant has already a good decoupling at low frequency -- *Crossover inversion*: the decoupling is improved around the crossover frequency as compared to the decentralized plant. However, the decoupling is increased at lower frequency. -- *SVD decoupling*: Very good decoupling up to 235Hz. Especially between 100Hz and 200Hz. -- *Dynamic Inversion*: the plant is very well decoupled at frequencies where the model is accurate (below 235Hz where flexible modes are not modelled). -- *Jacobian - Stiffness*: good decoupling at low frequency. The decoupling increases at the frequency of the suspension modes, but is acceptable up to the strut flexible modes (235Hz). -- *Jacobian - Mass*: bad decoupling at low frequency. Better decoupling above the frequency of the suspension modes, and acceptable decoupling up to the strut flexible modes (235Hz). -#+end_important - -#+begin_src matlab :exports none -%% Comparison of the RGA-Numbers -figure; -hold on; -plot(frf_iff.f, RGA_dec_sum, 'DisplayName', 'Decentralized'); -plot(frf_iff.f, RGA_sta_sum, 'DisplayName', 'Static inv.'); -plot(frf_iff.f, RGA_wc_sum, 'DisplayName', 'Crossover inv.'); -plot(frf_iff.f, RGA_svd_sum, 'DisplayName', 'SVD'); -plot(frf_iff.f, RGA_inv_sum, 'DisplayName', 'Dynamic inv.'); -plot(frf_iff.f, RGA_cok_sum, 'DisplayName', 'Jacobian - CoK'); -plot(frf_iff.f, RGA_com_sum, 'DisplayName', 'Jacobian - CoM'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_compare_rga_numbers.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_compare_rga_numbers -#+caption: Comparison of the obtained RGA-numbers for all the decoupling methods -#+RESULTS: -[[file:figs/test_nhexa_interaction_compare_rga_numbers.png]] - -**** Decoupling Robustness -<> - -Let's now see how the decoupling is changing when changing the payload's mass. -#+begin_src matlab -frf_new = frf_iff.G_de{3}; -#+end_src - -#+begin_src matlab :exports none -%% Decentralized RGA -RGA_dec_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - RGA_dec_b(i,:,:) = squeeze(frf_new(i,:,:)).*inv(squeeze(frf_new(i,:,:))).'; -end - -RGA_dec_sum_b = zeros(length(frf_iff), 1); -for i = 1:length(frf_iff.f) - RGA_dec_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_dec_b(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Static Decoupling -G_de_sta_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - G_de_sta_b(i,:,:) = squeeze(frf_new(i,:,:))*dc_inv; -end - -RGA_sta_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - RGA_sta_b(i,:,:) = squeeze(G_de_sta_b(i,:,:)).*inv(squeeze(G_de_sta_b(i,:,:))).'; -end - -RGA_sta_sum_b = zeros(size(RGA_sta_b, 1), 1); -for i = 1:size(RGA_sta_b, 1) - RGA_sta_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_sta_b(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Crossover Decoupling -V = squeeze(frf_coupled(i_wc,:,:)); -D = pinv(real(V'*V)); -H1 = D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2))); - -G_de_wc_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - G_de_wc_b(i,:,:) = squeeze(frf_new(i,:,:))*H1; -end - -RGA_wc_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - RGA_wc_b(i,:,:) = squeeze(G_de_wc_b(i,:,:)).*inv(squeeze(G_de_wc_b(i,:,:))).'; -end - -RGA_wc_sum_b = zeros(size(RGA_wc_b, 1), 1); -for i = 1:size(RGA_wc_b, 1) - RGA_wc_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_wc_b(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% SVD -V = squeeze(frf_coupled(i_wc,:,:)); -D = pinv(real(V'*V)); -H1 = pinv(D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2)))); -[U,S,V] = svd(H1); - -G_de_svd_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - G_de_svd_b(i,:,:) = inv(U)*squeeze(frf_new(i,:,:))*inv(V'); -end - -RGA_svd_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - RGA_svd_b(i,:,:) = squeeze(G_de_svd_b(i,:,:)).*inv(squeeze(G_de_svd_b(i,:,:))).'; -end - -RGA_svd_sum_b = zeros(size(RGA_svd_b, 1), 1); -for i = 1:size(RGA_svd, 1) - RGA_svd_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_svd_b(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Dynamic Decoupling -G_model = G_coupled; -G_model.outputdelay = 0; % necessary for further inversion -G_inv = inv(G_model); - -G_de_inv_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - G_de_inv_b(i,:,:) = squeeze(frf_new(i,:,:))*squeeze(evalfr(G_inv, 1j*2*pi*frf_iff.f(i))); -end - -RGA_inv_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - RGA_inv_b(i,:,:) = squeeze(G_de_inv_b(i,:,:)).*inv(squeeze(G_de_inv_b(i,:,:))).'; -end - -RGA_inv_sum_b = zeros(size(RGA_inv_b, 1), 1); -for i = 1:size(RGA_inv_b, 1) - RGA_inv_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_inv_b(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Jacobian (CoK) -G_de_J_cok_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - G_de_J_cok_b(i,:,:) = inv(Js_cok)*squeeze(frf_new(i,:,:))*inv(J_cok'); -end - -RGA_cok_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - RGA_cok_b(i,:,:) = squeeze(G_de_J_cok_b(i,:,:)).*inv(squeeze(G_de_J_cok_b(i,:,:))).'; -end - -RGA_cok_sum_b = zeros(size(RGA_cok_b, 1), 1); -for i = 1:size(RGA_cok_b, 1) - RGA_cok_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_cok_b(i,:,:))))); -end -#+end_src - -#+begin_src matlab :exports none -%% Jacobian (CoM) -G_de_J_com_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - G_de_J_com_b(i,:,:) = inv(Js_com)*squeeze(frf_new(i,:,:))*inv(J_com'); -end - -RGA_com_b = zeros(size(frf_new)); -for i = 1:length(frf_iff.f) - RGA_com_b(i,:,:) = squeeze(G_de_J_com_b(i,:,:)).*inv(squeeze(G_de_J_com_b(i,:,:))).'; -end - -RGA_com_sum_b = zeros(size(RGA_com_b, 1), 1); -for i = 1:size(RGA_com_b, 1) - RGA_com_sum_b(i) = sum(sum(abs(eye(6) - squeeze(RGA_com_b(i,:,:))))); -end -#+end_src - -The obtained RGA-numbers are shown in Figure ref:fig:test_nhexa_interaction_compare_rga_numbers_rob. - -#+begin_important -From Figure ref:fig:test_nhexa_interaction_compare_rga_numbers_rob: -- The decoupling using the Jacobian evaluated at the "center of stiffness" seems to give the most robust results. -#+end_important - -#+begin_src matlab :exports none -%% Robustness of the Decoupling method -figure; -hold on; -plot(frf_iff.f, RGA_dec_sum, '-', 'DisplayName', 'Decentralized'); -plot(frf_iff.f, RGA_sta_sum, '-', 'DisplayName', 'Static inv.'); -plot(frf_iff.f, RGA_wc_sum, '-', 'DisplayName', 'Crossover inv.'); -plot(frf_iff.f, RGA_svd_sum, '-', 'DisplayName', 'SVD'); -plot(frf_iff.f, RGA_inv_sum, '-', 'DisplayName', 'Dynamic inv.'); -plot(frf_iff.f, RGA_cok_sum, '-', 'DisplayName', 'Jacobian - CoK'); -plot(frf_iff.f, RGA_com_sum, '-', 'DisplayName', 'Jacobian - CoM'); -set(gca,'ColorOrderIndex',1) -plot(frf_iff.f, RGA_dec_sum_b, '--', 'HandleVisibility', 'off'); -plot(frf_iff.f, RGA_sta_sum_b, '--', 'HandleVisibility', 'off'); -plot(frf_iff.f, RGA_wc_sum_b, '--', 'HandleVisibility', 'off'); -plot(frf_iff.f, RGA_svd_sum_b, '--', 'HandleVisibility', 'off'); -plot(frf_iff.f, RGA_inv_sum_b, '--', 'HandleVisibility', 'off'); -plot(frf_iff.f, RGA_cok_sum_b, '--', 'HandleVisibility', 'off'); -plot(frf_iff.f, RGA_com_sum_b, '--', 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 2); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/interaction_compare_rga_numbers_rob.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_interaction_compare_rga_numbers_rob -#+caption: Change of the RGA-number with a change of the payload. Indication of the robustness of the inversion method. -#+RESULTS: -[[file:figs/test_nhexa_interaction_compare_rga_numbers_rob.png]] - -**** Conclusion - -#+begin_important -Several decoupling methods can be used: -- SVD -- Inverse -- Jacobian (CoK) -#+end_important - -#+name: tab:interaction_analysis_conclusion -#+caption: Summary of the interaction analysis and different decoupling strategies -#+attr_latex: :environment tabularx :width \linewidth :align lccc -#+attr_latex: :center t :booktabs t -| *Method* | *RGA* | *Diag Plant* | *Robustness* | -|----------------+-------+--------------+--------------| -| Decentralized | -- | Equal | ++ | -| Static dec. | -- | Equal | ++ | -| Crossover dec. | - | Equal | 0 | -| SVD | ++ | Diff | + | -| Dynamic dec. | ++ | Unity, equal | - | -| Jacobian - CoK | + | Diff | ++ | -| Jacobian - CoM | 0 | Diff | + | - -*** Robust High Authority Controller -:PROPERTIES: -:header-args:matlab+: :tangle matlab/scripts/hac_lac_enc_plates_suspended_table.m -:END: -<> -**** Introduction :ignore: -In this section we wish to develop a robust High Authority Controller (HAC) that is working for all payloads. - -cite:indri20_mechat_robot - -**** Matlab Init :noexport:ignore: -#+begin_src matlab -%% hac_lac_enc_plates_suspended_table.m -% Development and analysis of a robust High Authority Controller -#+end_src - -#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) -<> -#+end_src - -#+begin_src matlab :exports none :results silent :noweb yes -<> -#+end_src - -#+begin_src matlab :tangle no :noweb yes -<> -#+end_src - -#+begin_src matlab :eval no :noweb yes -<> -#+end_src - -#+begin_src matlab :noweb yes -<> -#+end_src - -#+begin_src matlab -%% Load the identified FRF and Simscape model -frf_iff = load('frf_iff_vib_table_m.mat', 'f', 'Ts', 'G_de'); -sim_iff = load('sim_iff_vib_table_m.mat', 'G_de'); -#+end_src - -**** Using Jacobian evaluated at the center of stiffness -***** Decoupled Plant -#+begin_src matlab -G_nom = frf_iff.G_de{2}; % Nominal Plant -#+end_src - -#+begin_src matlab :exports none -%% Initialize the Nano-Hexapod -n_hexapod = initializeNanoHexapodFinal('MO_B', -42e-3, ... - 'motion_sensor_type', 'plates'); - -%% Get the Jacobians -J_cok = n_hexapod.geometry.J; -Js_cok = n_hexapod.geometry.Js; - -%% Decouple plant using Jacobian (CoM) -G_de_J_cok = zeros(size(G_nom)); -for i = 1:length(frf_iff.f) - G_de_J_cok(i,:,:) = inv(Js_cok)*squeeze(G_nom(i,:,:))*inv(J_cok'); -end - -%% Normalize the plant input -[~, i_100] = min(abs(frf_iff.f - 10)); -input_normalize = diag(1./diag(abs(squeeze(G_de_J_cok(i_100,:,:))))); - -for i = 1:length(frf_iff.f) - G_de_J_cok(i,:,:) = squeeze(G_de_J_cok(i,:,:))*input_normalize; -end -#+end_src - -#+begin_src matlab :exports none -%% Bode Plot of the decoupled plant -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_J_cok(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1) -plot(frf_iff.f, abs(G_de_J_cok(:,1,1)), ... - 'DisplayName', '$D_x/\tilde{\mathcal{F}}_x$'); -plot(frf_iff.f, abs(G_de_J_cok(:,2,2)), ... - 'DisplayName', '$D_y/\tilde{\mathcal{F}}_y$'); -plot(frf_iff.f, abs(G_de_J_cok(:,3,3)), ... - 'DisplayName', '$D_z/\tilde{\mathcal{F}}_z$'); -plot(frf_iff.f, abs(G_de_J_cok(:,4,4)), ... - 'DisplayName', '$R_x/\tilde{\mathcal{M}}_x$'); -plot(frf_iff.f, abs(G_de_J_cok(:,5,5)), ... - 'DisplayName', '$R_y/\tilde{\mathcal{M}}_y$'); -plot(frf_iff.f, abs(G_de_J_cok(:,6,6)), ... - 'DisplayName', '$R_z/\tilde{\mathcal{M}}_z$'); -plot(frf_iff.f, abs(G_de_J_cok(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e1]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_J_cok(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/bode_plot_hac_iff_plant_jacobian_cok.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_bode_plot_hac_iff_plant_jacobian_cok -#+caption: Bode plot of the decoupled plant using the Jacobian evaluated at the Center of Stiffness -#+RESULTS: -[[file:figs/test_nhexa_bode_plot_hac_iff_plant_jacobian_cok.png]] - -***** SISO Controller Design -As the diagonal elements of the plant are not equal, several SISO controllers are designed and then combined to form a diagonal controller. -All the diagonal terms of the controller consists of: -- A double integrator to have high gain at low frequency -- A lead around the crossover frequency to increase stability margins -- Two second order low pass filters above the crossover frequency to increase the robustness to high frequency modes - -#+begin_src matlab :exports none -%% Controller Ry,Rz - -% Wanted crossover frequency -wc_Rxy = 2*pi*80; - -% Lead -a = 8.0; % Amount of phase lead / width of the phase lead / high frequency gain -wc = wc_Rxy; % Frequency with the maximum phase lead [rad/s] -Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); - -% Integrator -w0_int = wc_Rxy/2; % [rad/s] -xi_int = 0.3; - -Kd_int = (1 + 2*xi_int/w0_int*s + s^2/w0_int^2)/(s^2/w0_int^2); - -% Low Pass Filter (High frequency robustness) -w0_lpf = wc_Rxy*2; % Cut-off frequency [rad/s] -xi_lpf = 0.6; % Damping Ratio - -Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); - -w0_lpf_b = wc_Rxy*4; % Cut-off frequency [rad/s] -xi_lpf_b = 0.7; % Damping Ratio - -Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); - -% Unity Gain frequency -[~, i_80] = min(abs(frf_iff.f - wc_Rxy/2/pi)); - -% Combination of all the elements -Kd_Rxy = ... - -1/abs(G_de_J_cok(i_80,4,4)) * ... - Kd_lead/abs(evalfr(Kd_lead, 1j*wc_Rxy)) * ... % Lead (gain of 1 at wc) - Kd_int /abs(evalfr(Kd_int, 1j*wc_Rxy)) * ... - Kd_lpf_b/abs(evalfr(Kd_lpf_b, 1j*wc_Rxy)) * ... - Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc_Rxy)); % Low Pass Filter -#+end_src - -#+begin_src matlab :exports none -%% Controller Dx,Dy,Rz - -% Wanted crossover frequency -wc_Dxy = 2*pi*100; - -% Lead -a = 8.0; % Amount of phase lead / width of the phase lead / high frequency gain -wc = wc_Dxy; % Frequency with the maximum phase lead [rad/s] -Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); - -% Integrator -w0_int = wc_Dxy/2; % [rad/s] -xi_int = 0.3; - -Kd_int = (1 + 2*xi_int/w0_int*s + s^2/w0_int^2)/(s^2/w0_int^2); - -% Low Pass Filter (High frequency robustness) -w0_lpf = wc_Dxy*2; % Cut-off frequency [rad/s] -xi_lpf = 0.6; % Damping Ratio - -Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); - -w0_lpf_b = wc_Dxy*4; % Cut-off frequency [rad/s] -xi_lpf_b = 0.7; % Damping Ratio - -Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); - -% Unity Gain frequency -[~, i_100] = min(abs(frf_iff.f - wc_Dxy/2/pi)); - -% Combination of all the elements -Kd_Dyx_Rz = ... - -1/abs(G_de_J_cok(i_100,1,1)) * ... - Kd_int /abs(evalfr(Kd_int, 1j*wc_Dxy)) * ... % Integrator - Kd_lead/abs(evalfr(Kd_lead, 1j*wc_Dxy)) * ... % Lead (gain of 1 at wc) - Kd_lpf_b/abs(evalfr(Kd_lpf_b, 1j*wc_Dxy)) * ... % Lead (gain of 1 at wc) - Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc_Dxy)); % Low Pass Filter -#+end_src - -#+begin_src matlab :exports none -%% Controller Dz - -% Wanted crossover frequency -wc_Dz = 2*pi*100; - -% Lead -a = 8.0; % Amount of phase lead / width of the phase lead / high frequency gain -wc = wc_Dz; % Frequency with the maximum phase lead [rad/s] -Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); - -% Integrator -w0_int = wc_Dz/2; % [rad/s] -xi_int = 0.3; - -Kd_int = (1 + 2*xi_int/w0_int*s + s^2/w0_int^2)/(s^2/w0_int^2); - -% Low Pass Filter (High frequency robustness) -w0_lpf = wc_Dz*2; % Cut-off frequency [rad/s] -xi_lpf = 0.6; % Damping Ratio - -Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); - -w0_lpf_b = wc_Dz*4; % Cut-off frequency [rad/s] -xi_lpf_b = 0.7; % Damping Ratio - -Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); - -% Unity Gain frequency -[~, i_100] = min(abs(frf_iff.f - wc_Dz/2/pi)); - -% Combination of all the elements -Kd_Dz = ... - -1/abs(G_de_J_cok(i_100,3,3)) * ... - Kd_int /abs(evalfr(Kd_int, 1j*wc_Dz)) * ... % Integrator - Kd_lead/abs(evalfr(Kd_lead, 1j*wc_Dz)) * ... % Lead (gain of 1 at wc) - Kd_lpf_b/abs(evalfr(Kd_lpf_b, 1j*wc_Dz)) * ... % Lead (gain of 1 at wc) - Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc_Dz)); % Low Pass Filter -#+end_src - -#+begin_src matlab :exports none -%% Diagonal Controller -Kd_diag = blkdiag(Kd_Dyx_Rz, Kd_Dyx_Rz, Kd_Dz, Kd_Rxy, Kd_Rxy, Kd_Dyx_Rz); -#+end_src - -***** Obtained Loop Gain -#+begin_src matlab :exports none -%% Experimental Loop Gain -Lmimo = permute(pagemtimes(permute(G_de_J_cok, [2,3,1]), squeeze(freqresp(Kd_diag, frf_iff.f, 'Hz'))), [3,1,2]); -#+end_src - -#+begin_src matlab :exports none -%% Bode plot of the experimental Loop Gain -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(frf_iff.f, abs(Lmimo(:,i,i)), '-'); -end -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(squeeze(Lmimo(:,i,j))), 'color', [0,0,0,0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e+3]); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(frf_iff.f, 180/pi*angle(Lmimo(:,i,i)), '-'); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:45:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([1, 2e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/bode_plot_hac_iff_loop_gain_jacobian_cok.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_bode_plot_hac_iff_loop_gain_jacobian_cok -#+caption: Bode plot of the Loop Gain when using the Jacobian evaluated at the Center of Stiffness to decouple the system -#+RESULTS: -[[file:figs/test_nhexa_bode_plot_hac_iff_loop_gain_jacobian_cok.png]] - -#+begin_src matlab -%% Controller to be implemented -Kd = inv(J_cok')*input_normalize*ss(Kd_diag)*inv(Js_cok); -#+end_src - -***** Verification of the Stability -Now the stability of the feedback loop is verified using the generalized Nyquist criteria. - -#+begin_src matlab :exports none -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(3, 6, length(frf_iff.f)); - -for i_mass = 1:3 - % Loop gain - Lmimo = pagemtimes(permute(frf_iff.G_de{i_mass}, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))); - for i_f = 2:length(frf_iff.f) - Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); - end -end -#+end_src - -#+begin_src matlab :exports none -%% Plot of the eigenvalues of L in the complex plane -figure; -hold on; -for i_mass = 2:3 - plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'DisplayName', sprintf('%i masses', i_mass)); - plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - for i = 1:6 - plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - end -end -plot(-1, 0, 'kx', 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -legend('location', 'southeast'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/loci_hac_iff_loop_gain_jacobian_cok.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_loci_hac_iff_loop_gain_jacobian_cok -#+caption: Loci of $L(j\omega)$ in the complex plane. -#+RESULTS: -[[file:figs/test_nhexa_loci_hac_iff_loop_gain_jacobian_cok.png]] - -***** Save for further analysis -#+begin_src matlab :exports none :tangle no -save('matlab/data_sim/Khac_iff_struts_jacobian_cok.mat', 'Kd') -#+end_src - -#+begin_src matlab :eval no -save('data_sim/Khac_iff_struts_jacobian_cok.mat', 'Kd') -#+end_src - -***** Sensitivity transfer function from the model -#+begin_src matlab :exports none -%% Open Simulink Model -mdl = 'nano_hexapod_simscape'; - -options = linearizeOptions; -options.SampleTime = 0; - -open(mdl) - -Rx = zeros(1, 7); - -colors = colororder; -#+end_src - -#+begin_src matlab :exports none -%% Initialize the Simscape model in closed loop -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof', ... - 'controller_type', 'hac-iff-struts'); - -support.type = 1; % On top of vibration table -payload.type = 2; % Payload -#+end_src - -#+begin_src matlab :exports none -%% Load controllers -load('Kiff_opt.mat', 'Kiff'); -Kiff = c2d(Kiff, Ts, 'Tustin'); -load('Khac_iff_struts_jacobian_cok.mat', 'Kd') -Khac_iff_struts = c2d(Kd, Ts, 'Tustin'); -#+end_src - -#+begin_src matlab :exports none -%% Identify the (damped) transfer function from u to dLm -clear io; io_i = 1; -io(io_i) = linio([mdl, '/Rx'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/dL'], 1, 'output'); io_i = io_i + 1; % Plate Displacement (encoder) -#+end_src - -#+begin_src matlab :exports none -%% Identification of the dynamics -Gcl = linearize(mdl, io, 0.0, options); -#+end_src - -#+begin_src matlab :exports none -%% Computation of the sensitivity transfer function -S = eye(6) - inv(n_hexapod.geometry.J)*Gcl; -#+end_src - -The results are shown in Figure ref:fig:test_nhexa_sensitivity_hac_jacobian_cok_3m_comp_model. - -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm -freqs = logspace(0, 3, 1000); - -figure; -hold on; -for i =1:6 - set(gca,'ColorOrderIndex',i); - plot(freqs, abs(squeeze(freqresp(S(i,i), freqs, 'Hz'))), '--', ... - 'DisplayName', sprintf('$S_{%s}$ - Model', labels{i})); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Sensitivity [-]'); -ylim([1e-4, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([1, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/sensitivity_hac_jacobian_cok_3m_comp_model.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_sensitivity_hac_jacobian_cok_3m_comp_model -#+caption: Estimated sensitivity transfer functions for the HAC controller using the Jacobian estimated at the Center of Stiffness -#+RESULTS: -[[file:figs/test_nhexa_sensitivity_hac_jacobian_cok_3m_comp_model.png]] - -**** Using Singular Value Decomposition -***** Decoupled Plant -#+begin_src matlab -G_nom = frf_iff.G_de{2}; % Nominal Plant -#+end_src - -#+begin_src matlab :exports none -%% Take complex matrix corresponding to the plant at 100Hz -wc = 100; % Wanted crossover frequency [Hz] -[~, i_wc] = min(abs(frf_iff.f - wc)); % Indice corresponding to wc - -V = squeeze(G_nom(i_wc,:,:)); - -%% Real approximation of G(100Hz) -D = pinv(real(V'*V)); -H1 = pinv(D*real(V'*diag(exp(1j*angle(diag(V*D*V.'))/2)))); - -%% Singular Value Decomposition -[U,S,V] = svd(H1); - -%% Compute the decoupled plant using SVD -G_de_svd = zeros(size(G_nom)); -for i = 1:length(frf_iff.f) - G_de_svd(i,:,:) = inv(U)*squeeze(G_nom(i,:,:))*inv(V'); -end -#+end_src - -#+begin_src matlab :exports none -%% Bode plot of the decoupled plant using SVD -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(G_de_svd(:,i,j)), 'color', [0,0,0,0.2], ... - 'HandleVisibility', 'off'); - end -end -set(gca,'ColorOrderIndex',1); -for i = 1:6 - plot(frf_iff.f, abs(G_de_svd(:,i,i)), ... - 'DisplayName', sprintf('$y_%i/u_%i$', i, i)); -end -plot(frf_iff.f, abs(G_de_svd(:,1,2)), 'color', [0,0,0,0.2], ... - 'DisplayName', 'Coupling'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-9, 1e-4]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2); - -ax2 = nexttile; -hold on; -for i =1:6 - plot(frf_iff.f, 180/pi*angle(G_de_svd(:,i,i))); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:90:360); -ylim([-180, 180]); - -linkaxes([ax1,ax2],'x'); -xlim([10, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/bode_plot_hac_iff_plant_svd.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_bode_plot_hac_iff_plant_svd -#+caption: Bode plot of the decoupled plant using the SVD -#+RESULTS: -[[file:figs/test_nhexa_bode_plot_hac_iff_plant_svd.png]] - -***** Controller Design -#+begin_src matlab :exports none -%% Lead -a = 6.0; % Amount of phase lead / width of the phase lead / high frequency gain -wc = 2*pi*100; % Frequency with the maximum phase lead [rad/s] -Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); - -%% Integrator -Kd_int = ((2*pi*50 + s)/(2*pi*0.1 + s))^2; - -%% Low Pass Filter (High frequency robustness) -w0_lpf = 2*pi*200; % Cut-off frequency [rad/s] -xi_lpf = 0.3; % Damping Ratio - -Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); - -%% Normalize Gain -Kd_norm = diag(1./abs(diag(squeeze(G_de_svd(i_wc,:,:))))); - -%% Diagonal Control -Kd_diag = ... - Kd_norm * ... % Normalize gain at 100Hz - Kd_int /abs(evalfr(Kd_int, 1j*2*pi*100)) * ... % Integrator - Kd_lead/abs(evalfr(Kd_lead, 1j*2*pi*100)) * ... % Lead (gain of 1 at wc) - Kd_lpf /abs(evalfr(Kd_lpf, 1j*2*pi*100)); % Low Pass Filter -#+end_src - -#+begin_src matlab :exports none -%% MIMO Controller -Kd = -inv(V') * ... % Output decoupling - ss(Kd_diag) * ... - inv(U); % Input decoupling -#+end_src - -***** Loop Gain -#+begin_src matlab :exports none -%% Experimental Loop Gain -Lmimo = permute(pagemtimes(permute(G_nom, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))), [3,1,2]); -#+end_src - -#+begin_src matlab :exports none -%% Loop gain when using SVD -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(frf_iff.f, abs(Lmimo(:,i,i)), '-'); -end -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(squeeze(Lmimo(:,i,j))), 'color', [0,0,0,0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e+3]); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(frf_iff.f, 180/pi*angle(Lmimo(:,i,i)), '-'); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:30:360); -ylim([-180, 0]); - -linkaxes([ax1,ax2],'x'); -xlim([1, 2e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/bode_plot_hac_iff_loop_gain_svd.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_bode_plot_hac_iff_loop_gain_svd -#+caption: Bode plot of Loop Gain when using the SVD -#+RESULTS: -[[file:figs/test_nhexa_bode_plot_hac_iff_loop_gain_svd.png]] - -***** Stability Verification -#+begin_src matlab -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(3, 6, length(frf_iff.f)); - -for i = 1:3 - Lmimo = pagemtimes(permute(frf_iff.G_de{i}, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))); - for i_f = 2:length(frf_iff.f) - Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); - end -end -#+end_src - -#+begin_src matlab :exports none -%% Plot of the eigenvalues of L in the complex plane -figure; -hold on; -for i_mass = 2:3 - plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'DisplayName', sprintf('%i masses', i_mass)); - plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - for i = 1:6 - plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - end -end -plot(-1, 0, 'kx', 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -legend('location', 'southeast'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/loci_hac_iff_loop_gain_svd.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_loci_hac_iff_loop_gain_svd -#+caption: Locis of $L(j\omega)$ in the complex plane. -#+RESULTS: -[[file:figs/test_nhexa_loci_hac_iff_loop_gain_svd.png]] - -***** Save for further analysis -#+begin_src matlab :exports none :tangle no -save('matlab/data_sim/Khac_iff_struts_svd.mat', 'Kd') -#+end_src - -#+begin_src matlab :eval no -save('data_sim/Khac_iff_struts_svd.mat', 'Kd') -#+end_src - -***** Measured Sensitivity Transfer Function -The sensitivity transfer function is estimated by adding a reference signal $R_x$ consisting of a low pass filtered white noise, and measuring the position error $E_x$ at the same time. - -The transfer function from $R_x$ to $E_x$ is the sensitivity transfer function. - -In order to identify the sensitivity transfer function for all directions, six reference signals are used, one for each direction. - -#+begin_src matlab :exports none -%% Tested directions -labels = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; -#+end_src - -#+begin_src matlab :exports none -%% Load Identification Data -meas_hac_svd_3m = {}; - -for i = 1:6 - meas_hac_svd_3m(i) = {load(sprintf('T_S_meas_%s_3m_hac_svd_iff.mat', labels{i}), 't', 'Va', 'Vs', 'de', 'Rx')}; -end -#+end_src - -#+begin_src matlab :exports none -%% Setup useful variables -% Sampling Time [s] -Ts = (meas_hac_svd_3m{1}.t(end) - (meas_hac_svd_3m{1}.t(1)))/(length(meas_hac_svd_3m{1}.t)-1); - -% Sampling Frequency [Hz] -Fs = 1/Ts; - -% Hannning Windows -win = hanning(ceil(5*Fs)); - -% And we get the frequency vector -[~, f] = tfestimate(meas_hac_svd_3m{1}.Va, meas_hac_svd_3m{1}.de, win, Noverlap, Nfft, 1/Ts); -#+end_src - -#+begin_src matlab :exports none -%% Load Jacobian matrix -load('jacobian.mat', 'J'); - -%% Compute position error -for i = 1:6 - meas_hac_svd_3m{i}.Xm = [inv(J)*meas_hac_svd_3m{i}.de']'; - meas_hac_svd_3m{i}.Ex = meas_hac_svd_3m{i}.Rx - meas_hac_svd_3m{i}.Xm; -end -#+end_src - -An example is shown in Figure ref:fig:test_nhexa_ref_track_hac_svd_3m where both the reference signal and the measured position are shown for translations in the $x$ direction. - -#+begin_src matlab :exports none -figure; -hold on; -plot(meas_hac_svd_3m{1}.t, meas_hac_svd_3m{1}.Xm(:,1), 'DisplayName', 'Pos.') -plot(meas_hac_svd_3m{1}.t, meas_hac_svd_3m{1}.Rx(:,1), 'DisplayName', 'Ref.') -hold off; -xlabel('Time [s]'); ylabel('Dx motion [m]'); -xlim([20, 22]); -legend('location', 'northeast'); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/ref_track_hac_svd_3m.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_ref_track_hac_svd_3m -#+caption: Reference position and measured position -#+RESULTS: -[[file:figs/test_nhexa_ref_track_hac_svd_3m.png]] - -#+begin_src matlab :exports none -%% Transfer function estimate of S -S_hac_svd_3m = zeros(length(f), 6, 6); - -for i = 1:6 - S_hac_svd_3m(:,:,i) = tfestimate(meas_hac_svd_3m{i}.Rx, meas_hac_svd_3m{i}.Ex, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -The sensitivity transfer functions estimated for all directions are shown in Figure ref:fig:test_nhexa_sensitivity_hac_svd_3m. - -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm -figure; -hold on; -for i =1:6 - plot(f, abs(S_hac_svd_3m(:,i,i)), ... - 'DisplayName', sprintf('$S_{%s}$', labels{i})); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Sensitivity [-]'); -ylim([1e-4, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([0.5, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/sensitivity_hac_svd_3m.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_sensitivity_hac_svd_3m -#+caption: Measured diagonal elements of the sensitivity transfer function matrix. -#+RESULTS: -[[file:figs/test_nhexa_sensitivity_hac_svd_3m.png]] - -#+begin_important -From Figure ref:fig:test_nhexa_sensitivity_hac_svd_3m: -- The sensitivity transfer functions are similar for all directions -- The disturbance attenuation at 1Hz is almost a factor 1000 as wanted -- The sensitivity transfer functions for $R_x$ and $R_y$ have high peak values which indicate poor stability margins. -#+end_important - -***** Sensitivity transfer function from the model -The sensitivity transfer function is now estimated using the model and compared with the one measured. - -#+begin_src matlab :exports none -%% Open Simulink Model -mdl = 'nano_hexapod_simscape'; - -options = linearizeOptions; -options.SampleTime = 0; - -open(mdl) - -Rx = zeros(1, 7); - -colors = colororder; -#+end_src - -#+begin_src matlab :exports none -%% Initialize the Simscape model in closed loop -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'plates', ... - 'actuator_type', '2dof', ... - 'controller_type', 'hac-iff-struts'); - -support.type = 1; % On top of vibration table -payload.type = 2; % Payload -#+end_src - -#+begin_src matlab :exports none -%% Load controllers -load('Kiff_opt.mat', 'Kiff'); -Kiff = c2d(Kiff, Ts, 'Tustin'); -load('Khac_iff_struts_svd.mat', 'Kd') -Khac_iff_struts = c2d(Kd, Ts, 'Tustin'); -#+end_src - -#+begin_src matlab :exports none -%% Identify the (damped) transfer function from u to dLm -clear io; io_i = 1; -io(io_i) = linio([mdl, '/Rx'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs -io(io_i) = linio([mdl, '/dL'], 1, 'output'); io_i = io_i + 1; % Plate Displacement (encoder) -#+end_src - -#+begin_src matlab :exports none -%% Identification of the dynamics -Gcl = linearize(mdl, io, 0.0, options); -#+end_src - -#+begin_src matlab :exports none -%% Computation of the sensitivity transfer function -S = eye(6) - inv(n_hexapod.geometry.J)*Gcl; -#+end_src - -The results are shown in Figure ref:fig:test_nhexa_sensitivity_hac_svd_3m_comp_model. -The model is quite effective in estimating the sensitivity transfer functions except around 60Hz were there is a peak for the measurement. - -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm -freqs = logspace(0,3,1000); - -figure; -hold on; -for i =1:6 - set(gca,'ColorOrderIndex',i); - plot(f, abs(S_hac_svd_3m(:,i,i)), ... - 'DisplayName', sprintf('$S_{%s}$', labels{i})); - set(gca,'ColorOrderIndex',i); - plot(freqs, abs(squeeze(freqresp(S(i,i), freqs, 'Hz'))), '--', ... - 'DisplayName', sprintf('$S_{%s}$ - Model', labels{i})); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Sensitivity [-]'); -ylim([1e-4, 1e1]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3); -xlim([0.5, 1e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/sensitivity_hac_svd_3m_comp_model.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_sensitivity_hac_svd_3m_comp_model -#+caption: Comparison of the measured sensitivity transfer functions with the model -#+RESULTS: -[[file:figs/test_nhexa_sensitivity_hac_svd_3m_comp_model.png]] - -**** Using (diagonal) Dynamical Inverse :noexport: -***** Decoupled Plant -#+begin_src matlab -G_nom = frf_iff.G_de{2}; % Nominal Plant -G_model = sim_iff.G_de{2}; % Model of the Plant -#+end_src - -#+begin_src matlab :exports none -%% Simplified model of the diagonal term -balred_opts = balredOptions('FreqIntervals', 2*pi*[0, 1000], 'StateElimMethod', 'Truncate'); - -G_red = balred(G_model(1,1), 8, balred_opts); -G_red.outputdelay = 0; % necessary for further inversion -#+end_src - -#+begin_src matlab -%% Inverse -G_inv = inv(G_red); -[G_z, G_p, G_g] = zpkdata(G_inv); -p_uns = real(G_p{1}) > 0; -G_p{1}(p_uns) = -G_p{1}(p_uns); -G_inv_stable = zpk(G_z, G_p, G_g); -#+end_src - -#+begin_src matlab :exports none -%% "Uncertainty" of inversed plant -freqs = logspace(0,3,1000); - -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i_mass = i_masses - for i = 1 - plot(freqs, abs(squeeze(freqresp(G_inv_stable*sim_iff.G_de{i_mass+1}(i,i), freqs, 'Hz'))), '-', 'color', colors(i_mass+1, :), ... - 'DisplayName', sprintf('$d\\mathcal{L}_i/u^\\prime_i$ - %i', i_mass)); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Amplitude'); set(gca, 'XTickLabel',[]); -ylim([1e-1, 1e1]); -legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 4); - -ax2 = nexttile; -hold on; -for i_mass = i_masses - for i = 1 - plot(freqs, 180/pi*angle(squeeze(freqresp(G_inv_stable*sim_iff.G_de{i_mass+1}(1,1), freqs, 'Hz'))), '-', 'color', colors(i_mass+1, :)); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:15:360); -ylim([-45, 45]); - -linkaxes([ax1,ax2],'x'); -xlim([freqs(1), freqs(end)]); -#+end_src - -***** Controller Design -#+begin_src matlab :exports none -% Wanted crossover frequency -wc = 2*pi*80; -[~, i_wc] = min(abs(frf_iff.f - wc/2/pi)); - -%% Lead -a = 20.0; % Amount of phase lead / width of the phase lead / high frequency gain -Kd_lead = (1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)))/sqrt(a); - -%% Integrator -Kd_int = ((wc)/(2*pi*0.2 + s))^2; - -%% Low Pass Filter (High frequency robustness) -w0_lpf = 2*wc; % Cut-off frequency [rad/s] -xi_lpf = 0.3; % Damping Ratio - -Kd_lpf = 1/(1 + 2*xi_lpf/w0_lpf*s + s^2/w0_lpf^2); - -w0_lpf_b = wc*4; % Cut-off frequency [rad/s] -xi_lpf_b = 0.7; % Damping Ratio - -Kd_lpf_b = 1/(1 + 2*xi_lpf_b/w0_lpf_b*s + s^2/w0_lpf_b^2); - -%% Normalize Gain -Kd_norm = diag(1./abs(diag(squeeze(G_de_svd(i_wc,:,:))))); - -%% Diagonal Control -Kd_diag = ... - G_inv_stable * ... % Normalize gain at 100Hz - Kd_int /abs(evalfr(Kd_int, 1j*wc)) * ... % Integrator - Kd_lead/abs(evalfr(Kd_lead, 1j*wc)) * ... % Lead (gain of 1 at wc) - Kd_lpf /abs(evalfr(Kd_lpf, 1j*wc)); % Low Pass Filter -#+end_src - -#+begin_src matlab :exports none -Kd = ss(Kd_diag)*eye(6); -#+end_src - -***** Loop Gain -#+begin_src matlab :exports none -%% Experimental Loop Gain -Lmimo = permute(pagemtimes(permute(G_nom, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))), [3,1,2]); -#+end_src - -#+begin_src matlab :exports none -%% Loop gain when using SVD -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(frf_iff.f, abs(Lmimo(:,i,i)), '-'); -end -for i = 1:5 - for j = i+1:6 - plot(frf_iff.f, abs(squeeze(Lmimo(:,i,j))), 'color', [0,0,0,0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -ylabel('Loop Gain'); set(gca, 'XTickLabel',[]); -ylim([1e-3, 1e+3]); - -ax2 = nexttile; -hold on; -for i = 1:6 - plot(frf_iff.f, 180/pi*angle(Lmimo(:,i,i)), '-'); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); -xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); -hold off; -yticks(-360:30:360); -ylim([-180, 0]); - -linkaxes([ax1,ax2],'x'); -xlim([1, 2e3]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/bode_plot_hac_iff_loop_gain_diag_inverse.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_bode_plot_hac_iff_loop_gain_diag_inverse -#+caption: Bode plot of Loop Gain when using the Diagonal inversion -#+RESULTS: -[[file:figs/test_nhexa_bode_plot_hac_iff_loop_gain_diag_inverse.png]] - -***** Stability Verification -MIMO Nyquist with eigenvalues -#+begin_src matlab -%% Compute the Eigenvalues of the loop gain -Ldet = zeros(3, 6, length(frf_iff.f)); - -for i = 1:3 - Lmimo = pagemtimes(permute(frf_iff.G_de{i}, [2,3,1]),squeeze(freqresp(Kd, frf_iff.f, 'Hz'))); - for i_f = 2:length(frf_iff.f) - Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); - end -end -#+end_src - -#+begin_src matlab :exports none -%% Plot of the eigenvalues of L in the complex plane -figure; -hold on; -for i_mass = 2:3 - plot(real(squeeze(Ldet(i_mass, 1,:))), imag(squeeze(Ldet(i_mass, 1,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'DisplayName', sprintf('%i masses', i_mass)); - plot(real(squeeze(Ldet(i_mass, 1,:))), -imag(squeeze(Ldet(i_mass, 1,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - for i = 1:6 - plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - plot(real(squeeze(Ldet(i_mass, i,:))), -imag(squeeze(Ldet(i_mass, i,:))), ... - '.', 'color', colors(i_mass+1, :), ... - 'HandleVisibility', 'off'); - end -end -plot(-1, 0, 'kx', 'HandleVisibility', 'off'); -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -legend('location', 'southeast'); -xlim([-3, 1]); ylim([-2, 2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/loci_hac_iff_loop_gain_diag_inverse.pdf', 'width', 'wide', 'height', 'tall'); -#+end_src - -#+name: fig:test_nhexa_loci_hac_iff_loop_gain_diag_inverse -#+caption: Locis of $L(j\omega)$ in the complex plane. -#+RESULTS: -[[file:figs/test_nhexa_loci_hac_iff_loop_gain_diag_inverse.png]] - -#+begin_important -Even though the loop gain seems to be fine, the closed-loop system is unstable. -This might be due to the fact that there is large interaction in the plant. -We could look at the RGA-number to verify that. -#+end_important - -***** Save for further use -#+begin_src matlab :exports none :tangle no -save('matlab/data_sim/Khac_iff_struts_diag_inverse.mat', 'Kd') -#+end_src - -#+begin_src matlab :eval no -save('data_sim/Khac_iff_struts_diag_inverse.mat', 'Kd') -#+end_src - -**** Closed Loop Stability (Model) :noexport: -Verify stability using Simscape model -#+begin_src matlab -%% 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'); -#+end_src - -#+begin_src matlab -%% IFF Controller -Kiff = -g_opt*Kiff_g1*eye(6); -Khac_iff_struts = Kd*eye(6); -#+end_src - -#+begin_src matlab -%% 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) -#+end_src - -#+begin_src matlab -GG_cl = {}; - -for i = i_masses - payload.type = i; - GG_cl(i+1) = {exp(-s*Ts)*linearize(mdl, io, 0.0, options)}; -end -#+end_src - -#+begin_src matlab -for i = i_masses - isstable(GG_cl{i+1}) -end -#+end_src - -MIMO Nyquist -#+begin_src matlab -Kdm = Kd*eye(6); - -Ldet = zeros(3, length(fb(i_lim))); - -for i = 1:3 - Lmimo = pagemtimes(permute(G_damp_m{i}(i_lim,:,:), [2,3,1]),squeeze(freqresp(Kdm, fb(i_lim), 'Hz'))); - Ldet(i,:) = arrayfun(@(t) det(eye(6) + squeeze(Lmimo(:,:,t))), 1:size(Lmimo,3)); -end -#+end_src - -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm -figure; -hold on; -for i_mass = 3 - for i = 1 - plot(real(Ldet(i_mass,:)), imag(Ldet(i_mass,:)), ... - '-', 'color', colors(i_mass+1, :)); - end -end -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -xlim([-10, 1]); ylim([-4, 4]); -#+end_src - -MIMO Nyquist with eigenvalues -#+begin_src matlab -Kdm = Kd*eye(6); - -Ldet = zeros(3, 6, length(fb(i_lim))); - -for i = 1:3 - Lmimo = pagemtimes(permute(G_damp_m{i}(i_lim,:,:), [2,3,1]),squeeze(freqresp(Kdm, fb(i_lim), 'Hz'))); - for i_f = 1:length(fb(i_lim)) - Ldet(i,:, i_f) = eig(squeeze(Lmimo(:,:,i_f))); - end -end -#+end_src - -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm -figure; -hold on; -for i_mass = 1 - for i = 1:6 - plot(real(squeeze(Ldet(i_mass, i,:))), imag(squeeze(Ldet(i_mass, i,:))), ... - '-', 'color', colors(i_mass+1, :)); - end -end -hold off; -set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin'); -xlabel('Real'); ylabel('Imag'); -xlim([-10, 1]); ylim([-4, 2]); -#+end_src -** Other Backups -*** Nano-Hexapod Compliance - Effect of IFF -<> - -In this section, we wish to estimate the effectiveness of the IFF strategy regarding the compliance. - -The top plate is excited vertically using the instrumented hammer two times: -1. no control loop is used -2. decentralized IFF is used - -The data are loaded. -#+begin_src matlab -frf_ol = load('Measurement_Z_axis.mat'); % Open-Loop -frf_iff = load('Measurement_Z_axis_damped.mat'); % IFF -#+end_src - -The mean vertical motion of the top platform is computed by averaging all 5 vertical accelerometers. -#+begin_src matlab -%% 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; -#+end_src - -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 ref:fig:test_nhexa_compliance_vertical_comp_iff. -#+begin_src matlab :exports none -%% Comparison of the vertical compliances (OL and IFF) -figure; -hold on; -plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, 'DisplayName', 'OL'); -plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, 'DisplayName', 'IFF'); -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]'); -xlim([20, 2e3]); ylim([2e-9, 2e-5]); -legend('location', 'northeast'); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/compliance_vertical_comp_iff.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_compliance_vertical_comp_iff -#+caption: Measured vertical compliance with and without IFF -#+RESULTS: -[[file:figs/test_nhexa_compliance_vertical_comp_iff.png]] - -#+begin_important -From Figure ref:fig:test_nhexa_compliance_vertical_comp_iff, 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. -#+end_important - -*** Comparison with the Simscape Model -<> - -Let's initialize the Simscape model such that it corresponds to the experiment. -#+begin_src matlab -%% Nano-Hexapod is fixed on a rigid granite -support.type = 0; - -%% No Payload on top of the Nano-Hexapod -payload.type = 0; - -%% Initialize Nano-Hexapod in Open Loop -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'struts', ... - 'actuator_type', '2dof'); - -#+end_src - -And let's 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. -#+begin_src matlab :exports none -%% Identify the IFF Plant (transfer function from u to taum) -clear io; io_i = 1; -io(io_i) = linio([mdl, '/Fz_ext'], 1, 'openinput'); io_i = io_i + 1; % External - Vertical force -io(io_i) = linio([mdl, '/Z_top_plat'], 1, 'openoutput'); io_i = io_i + 1; % Absolute vertical motion of top platform -#+end_src - -#+begin_src matlab :exports none -%% Perform the identifications -G_compl_z_ol = linearize(mdl, io, 0.0, options); -#+end_src - -#+begin_src matlab :exports none -%% Initialize Nano-Hexapod with IFF -Kiff = 400*(1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz) - (s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain - (1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances - eye(6); % Diagonal 6x6 controller - -%% Initialize the Nano-Hexapod with IFF -n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ... - 'flex_top_type', '4dof', ... - 'motion_sensor_type', 'struts', ... - 'actuator_type', '2dof', ... - 'controller_type', 'iff'); - -%% Perform the identification -G_compl_z_iff = linearize(mdl, io, 0.0, options); -#+end_src - -The comparison is done in Figure ref:fig:test_nhexa_compliance_vertical_comp_model_iff. -Again, the model is quite accurate in predicting the (closed-loop) behavior of the system. - -#+begin_src matlab :exports none -%% Comparison of the measured compliance and the one obtained from the model -freqs = 2*logspace(1,3,1000); - -figure; -hold on; -plot(frf_ol.FFT1_H1_16_1_RMS_X_Val, d_frf_ol, '-', 'DisplayName', 'OL - Meas.'); -plot(frf_iff.FFT1_H1_16_1_RMS_X_Val, d_frf_iff, '-', 'DisplayName', 'IFF - Meas.'); -set(gca,'ColorOrderIndex',1) -plot(freqs, abs(squeeze(freqresp(G_compl_z_ol, freqs, 'Hz'))), '--', 'DisplayName', 'OL - Model') -plot(freqs, abs(squeeze(freqresp(G_compl_z_iff, freqs, 'Hz'))), '--', 'DisplayName', 'IFF - Model') -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Vertical Compliance [$m/N$]'); -xlim([20, 2e3]); ylim([2e-9, 2e-5]); -legend('location', 'northeast', 'FontSize', 8); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/compliance_vertical_comp_model_iff.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_compliance_vertical_comp_model_iff -#+caption: Measured vertical compliance with and without IFF -#+RESULTS: -[[file:figs/test_nhexa_compliance_vertical_comp_model_iff.png]] - -*** Computation of the transmissibility from accelerometer data -**** Introduction :ignore: - -The goal is to compute the $6 \times 6$ transfer function matrix corresponding to the transmissibility of the Nano-Hexapod. - -To do so, several accelerometers are located both on the vibration table and on the top of the nano-hexapod. - -The vibration table is then excited using a Shaker and all the accelerometers signals are recorded. - -Using transformation (jacobian) matrices, it is then possible to compute both the motion of the top and bottom platform of the nano-hexapod. - -Finally, it is possible to compute the $6 \times 6$ transmissibility matrix. - -Such procedure is explained in cite:marneffe04_stewar_platf_activ_vibrat_isolat. - -**** Jacobian matrices - -How to compute the Jacobian matrices is explained in Section ref:sec:meas_transformation. - -#+begin_src matlab -%% Bottom Accelerometers -Opb = [-0.1875, -0.1875, -0.245; - -0.1875, -0.1875, -0.245; - 0.1875, -0.1875, -0.245; - 0.1875, -0.1875, -0.245; - 0.1875, 0.1875, -0.245; - 0.1875, 0.1875, -0.245]'; - -Osb = [0, 1, 0; - 0, 0, 1; - 1, 0, 0; - 0, 0, 1; - 1, 0, 0; - 0, 0, 1;]'; - -Jb = zeros(length(Opb), 6); - -for i = 1:length(Opb) - Ri = [0, Opb(3,i), -Opb(2,i); - -Opb(3,i), 0, Opb(1,i); - Opb(2,i), -Opb(1,i), 0]; - Jb(i, 1:3) = Osb(:,i)'; - Jb(i, 4:6) = Osb(:,i)'*Ri; -end - -Jbinv = inv(Jb); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable(Jbinv, {'$\dot{x}_x$', '$\dot{x}_y$', '$\dot{x}_z$', '$\dot{\omega}_x$', '$\dot{\omega}_y$', '$\dot{\omega}_z$'}, {'$a_1$', '$a_2$', '$a_3$', '$a_4$', '$a_5$', '$a_6$'}, ' %.1f '); -#+end_src - -#+RESULTS: -| | $a_1$ | $a_2$ | $a_3$ | $a_4$ | $a_5$ | $a_6$ | -|------------------+-------+-------+-------+-------+-------+-------| -| $\dot{x}_x$ | 0.0 | 0.7 | 0.5 | -0.7 | 0.5 | 0.0 | -| $\dot{x}_y$ | 1.0 | 0.0 | 0.5 | 0.7 | -0.5 | -0.7 | -| $\dot{x}_z$ | 0.0 | 0.5 | 0.0 | 0.0 | 0.0 | 0.5 | -| $\dot{\omega}_x$ | 0.0 | 0.0 | 0.0 | -2.7 | 0.0 | 2.7 | -| $\dot{\omega}_y$ | 0.0 | 2.7 | 0.0 | -2.7 | 0.0 | 0.0 | -| $\dot{\omega}_z$ | 0.0 | 0.0 | 2.7 | 0.0 | -2.7 | 0.0 | - -#+begin_src matlab -%% Top Accelerometers -Opt = [-0.1, 0, -0.150; - -0.1, 0, -0.150; - 0.05, 0.075, -0.150; - 0.05, 0.075, -0.150; - 0.05, -0.075, -0.150; - 0.05, -0.075, -0.150]'; - -Ost = [0, 1, 0; - 0, 0, 1; - 1, 0, 0; - 0, 0, 1; - 1, 0, 0; - 0, 0, 1;]'; - -Jt = zeros(length(Opt), 6); - -for i = 1:length(Opt) - Ri = [0, Opt(3,i), -Opt(2,i); - -Opt(3,i), 0, Opt(1,i); - Opt(2,i), -Opt(1,i), 0]; - Jt(i, 1:3) = Ost(:,i)'; - Jt(i, 4:6) = Ost(:,i)'*Ri; -end - -Jtinv = inv(Jt); -#+end_src - -#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*) -data2orgtable(Jtinv, {'$\dot{x}_x$', '$\dot{x}_y$', '$\dot{x}_z$', '$\dot{\omega}_x$', '$\dot{\omega}_y$', '$\dot{\omega}_z$'}, {'$b_1$', '$b_2$', '$b_3$', '$b_4$', '$b_5$', '$b_6$'}, ' %.1f '); -#+end_src - -#+RESULTS: -| | $b_1$ | $b_2$ | $b_3$ | $b_4$ | $b_5$ | $b_6$ | -|------------------+-------+-------+-------+-------+-------+-------| -| $\dot{x}_x$ | 0.0 | 1.0 | 0.5 | -0.5 | 0.5 | -0.5 | -| $\dot{x}_y$ | 1.0 | 0.0 | -0.7 | -1.0 | 0.7 | 1.0 | -| $\dot{x}_z$ | 0.0 | 0.3 | 0.0 | 0.3 | 0.0 | 0.3 | -| $\dot{\omega}_x$ | 0.0 | 0.0 | 0.0 | 6.7 | 0.0 | -6.7 | -| $\dot{\omega}_y$ | 0.0 | 6.7 | 0.0 | -3.3 | 0.0 | -3.3 | -| $\dot{\omega}_z$ | 0.0 | 0.0 | -6.7 | 0.0 | 6.7 | 0.0 | - -**** Using =linearize= function - -#+begin_src matlab -acc_3d.type = 2; % 1: inertial mass, 2: perfect - -%% Name of the Simulink File -mdl = 'vibration_table'; - -%% Input/Output definition -clear io; io_i = 1; -io(io_i) = linio([mdl, '/F_shaker'], 1, 'openinput'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/acc'], 1, 'openoutput'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/acc_top'], 1, 'openoutput'); io_i = io_i + 1; - -%% Run the linearization -G = linearize(mdl, io); -G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; -G.OutputName = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6', ... - 'b1', 'b2', 'b3', 'b4', 'b5', 'b6'}; -#+end_src - -#+begin_src matlab -Gb = Jbinv*G({'a1', 'a2', 'a3', 'a4', 'a5', 'a6'}, :); -Gt = Jtinv*G({'b1', 'b2', 'b3', 'b4', 'b5', 'b6'}, :); -#+end_src - -#+begin_src matlab -T = inv(Gb)*Gt; -T = minreal(T); -T = prescale(T, {2*pi*0.1, 2*pi*1e3}); -#+end_src - -#+begin_src matlab :exports none -freqs = logspace(0, 3, 1000); - -figure; -hold on; -for i = 1:6 - plot(freqs, abs(squeeze(freqresp(T(i, i), freqs, 'Hz')))); -end -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(T(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Transmissibility'); -ylim([1e-4, 1e2]); -xlim([freqs(1), freqs(end)]); -#+end_src - -*** Comparison with "true" transmissibility - -#+begin_src matlab -%% Name of the Simulink File -mdl = 'test_transmissibility'; - -%% Input/Output definition -clear io; io_i = 1; -io(io_i) = linio([mdl, '/d'], 1, 'openinput'); io_i = io_i + 1; -io(io_i) = linio([mdl, '/acc'], 1, 'openoutput'); io_i = io_i + 1; - -%% Run the linearization -G = linearize(mdl, io); -G.InputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}; -G.OutputName = {'Ax', 'Ay', 'Az', 'Bx', 'By', 'Bz'}; -#+end_src - -#+begin_src matlab -Tp = G/s^2; -#+end_src - -#+begin_src matlab :exports none -freqs = logspace(0, 3, 1000); - -figure; -hold on; -for i = 1:6 - plot(freqs, abs(squeeze(freqresp(Tp(i, i), freqs, 'Hz')))); -end -for i = 1:5 - for j = i+1:6 - plot(freqs, abs(squeeze(freqresp(Tp(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]); - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Transmissibility'); -ylim([1e-4, 1e2]); -xlim([freqs(1), freqs(end)]); -#+end_src - -*** Rigidification of the added payloads -- [ ] figure - -#+begin_src matlab -%% Load Identification Data -meas_added_mass = {}; - -for i_strut = 1:6 - meas_added_mass(i_strut) = {load(sprintf('frf_data_exc_strut_%i_spindle_1m_solid.mat', i_strut), 't', 'Va', 'Vs', 'de')}; -end -#+end_src - -Finally the $6 \times 6$ transfer function matrices from $\bm{u}$ to $d\bm{\mathcal{L}}_m$ and from $\bm{u}$ to $\bm{\tau}_m$ are identified: -#+begin_src matlab -%% DVF Plant (transfer function from u to dLm) - -G_de = zeros(length(f), 6, 6); -for i_strut = 1:6 - G_de(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.de, win, Noverlap, Nfft, 1/Ts); -end - -%% IFF Plant (transfer function from u to taum) -G_Vs = zeros(length(f), 6, 6); -for i_strut = 1:6 - G_Vs(:,:,i_strut) = tfestimate(meas_added_mass{i_strut}.Va, meas_added_mass{i_strut}.Vs, win, Noverlap, Nfft, 1/Ts); -end -#+end_src - -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm - Several payloads -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -% Diagonal terms -for i = 1:6 - plot(frf_ol.f, abs(frf_ol.G_de{1}(:,i, i)), 'color', colors(1,:)); - plot(f, abs(G_de(:,i, i)), 'color', colors(2,:)); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); -ylim([1e-8, 1e-3]); -xlim([20, 2e3]); -#+end_src - -#+begin_src matlab :exports none -%% Bode plot for the transfer function from u to dLm -figure; -tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None'); - -ax1 = nexttile([2,1]); -hold on; -for i = 1:6 - plot(frf_ol.f, abs(frf_ol.G_de(:,i, i)), 'color', colors(1,:)); - plot(f, abs(G_de(:,i, i)), 'color', colors(2,:)); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('Amplitude [m/V]'); -ylim([1e-8, 1e-3]); -xlim([10, 1e3]); -#+end_src - - -*** Table with signals - -#+name: tab:list_signals -#+caption: List of signals -#+attr_latex: :environment tabularx :width \linewidth :align Xllll -#+attr_latex: :center t :booktabs t -| | *Unit* | *Matlab* | *Vector* | *Elements* | -|------------------------------------+-----------+-----------+-----------------------+----------------------| -| Control Input (wanted DAC voltage) | =[V]= | =u= | $\bm{u}$ | $u_i$ | -| DAC Output Voltage | =[V]= | =u= | $\tilde{\bm{u}}$ | $\tilde{u}_i$ | -| PD200 Output Voltage | =[V]= | =ua= | $\bm{u}_a$ | $u_{a,i}$ | -| Actuator applied force | =[N]= | =tau= | $\bm{\tau}$ | $\tau_i$ | -|------------------------------------+-----------+-----------+-----------------------+----------------------| -| Strut motion | =[m]= | =dL= | $d\bm{\mathcal{L}}$ | $d\mathcal{L}_i$ | -| Encoder measured displacement | =[m]= | =dLm= | $d\bm{\mathcal{L}}_m$ | $d\mathcal{L}_{m,i}$ | -|------------------------------------+-----------+-----------+-----------------------+----------------------| -| Force Sensor strain | =[m]= | =epsilon= | $\bm{\epsilon}$ | $\epsilon_i$ | -| Force Sensor Generated Voltage | =[V]= | =taum= | $\tilde{\bm{\tau}}_m$ | $\tilde{\tau}_{m,i}$ | -| Measured Generated Voltage | =[V]= | =taum= | $\bm{\tau}_m$ | $\tau_{m,i}$ | -|------------------------------------+-----------+-----------+-----------------------+----------------------| -| Motion of the top platform | =[m,rad]= | =dX= | $d\bm{\mathcal{X}}$ | $d\mathcal{X}_i$ | -| Metrology measured displacement | =[m,rad]= | =dXm= | $d\bm{\mathcal{X}}_m$ | $d\mathcal{X}_{m,i}$ | - -*** RGA - - -The RGA-number, which is a measure of the interaction in the system, is computed for the transfer function matrix from $\mathbf{u}$ to $d\mathbf{\mathcal{L}}_m$ for all the payloads. -The obtained numbers are compared in Figure ref:fig:test_nhexa_rga_num_ol_masses. - -#+begin_src matlab :exports none -%% Decentralized RGA - Undamped Plant -RGA_num = zeros(length(frf_ol.f), 4); -for i_mass = [0:3] - for i = 1:length(frf_ol.f) - RGA_num(i, i_mass+1) = sum(sum(abs(eye(6) - squeeze(frf_ol.G_de{i_mass+1}(i,:,:)).*inv(squeeze(frf_ol.G_de{i_mass+1}(i,:,:))).'))); - end -end -#+end_src - -#+begin_src matlab :exports none -%% RGA for Decentralized plant -figure; -hold on; -for i_mass = [0:3] - plot(frf_ol.f, RGA_num(:,i_mass+1), '-', 'color', colors(i_mass+1,:), ... - 'DisplayName', sprintf('RGA-num - %i mass', i_mass)); -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2); -#+end_src - -#+begin_src matlab :exports none -%% Decentralized RGA - Undamped Plant -RGA = zeros(length(frf_ol.f), 6, 6); -for i = 1:length(frf_ol.f) -RGA(i, :, :) = squeeze(frf_ol.G_Vs{1}(i,:,:)).*inv(squeeze(frf_ol.G_Vs{1}(i,:,:))).'; -end -#+end_src - -#+begin_src matlab :exports none -%% RGA -figure; -hold on; -for i = 1:6 - plot(frf_ol.f, abs(RGA(:,i,i)), 'k-') -end -for i = 1:5 - for j = i+1:6 - plot(frf_ol.f, abs(RGA(:,i,j)), 'r-') - end -end -hold off; -set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); -xlabel('Frequency [Hz]'); ylabel('RGA Number'); -xlim([10, 1e3]); ylim([1e-2, 1e2]); -#+end_src - -#+begin_src matlab :tangle no :exports results :results file replace -exportFig('figs/rga_num_ol_masses.pdf', 'width', 'wide', 'height', 'normal'); -#+end_src - -#+name: fig:test_nhexa_rga_num_ol_masses -#+caption: RGA-number for the open-loop transfer function from $\mathbf{u}$ to $d\mathbf{\mathcal{L}}_m$ -#+RESULTS: -[[file:figs/test_nhexa_rga_num_ol_masses.png]] - -#+begin_important -From Figure ref:fig:test_nhexa_rga_num_ol_masses, it is clear that the coupling is quite large starting from the first suspension mode of the nano-hexapod. -Therefore, is the payload's mass is increase, the coupling in the system start to become unacceptably large at lower frequencies. -#+end_important