3867 lines
127 KiB
Org Mode
3867 lines
127 KiB
Org Mode
|
|
** Analysis backup of HAC - Decoupling analysis
|
|
<<sec:test_nhexa_decentralized_hac_iff>>
|
|
|
|
*** 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:
|
|
<<sec:test_nhexa_hac_iff_struts_ref_track>>
|
|
**** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
**** Y-Z Scans
|
|
<<sec:test_nhexa_yz_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
|
|
<<sec:test_nhexa_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
|
|
<<sec:test_nhexa_nass_scans>>
|
|
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:
|
|
<<sec:test_nhexa_hac_iff_struts_controller>>
|
|
**** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-simscape>>
|
|
<<m-init-other>>
|
|
#+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
|
|
<<sec:test_nhexa_hac_iff_no_payload_tuning>>
|
|
|
|
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
|
|
<<sec:test_nhexa_hac_iff_no_payload_stability>>
|
|
|
|
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:
|
|
<<sec:test_nhexa_interaction_analysis>>
|
|
**** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+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)
|
|
<<sec:test_nhexa_interaction_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
|
|
<<sec:test_nhexa_interaction_static>>
|
|
|
|
#+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
|
|
<<sec:test_nhexa_interaction_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
|
|
<<sec:test_nhexa_interaction_svd>>
|
|
|
|
#+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
|
|
<<sec:test_nhexa_interaction_dynamic>>
|
|
|
|
#+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
|
|
<<sec:test_nhexa_interaction_jacobian_cok>>
|
|
|
|
#+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
|
|
<<sec:test_nhexa_interaction_jacobian_com>>
|
|
|
|
#+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
|
|
<<sec:test_nhexa_interaction_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
|
|
<<sec:test_nhexa_interaction_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:
|
|
<<sec:test_nhexa_robust_hac_design>>
|
|
**** 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)
|
|
<<matlab-dir>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results silent :noweb yes
|
|
<<matlab-init>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+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
|
|
<<sec:test_nhexa_compliance_effect_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
|
|
<<sec:test_nhexa_compliance_effect_iff_comp_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
|