tdehaeze/svd-control
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
87a0d98e01
svd-control/index.org

2641 lines
91 KiB
Org Mode
Raw Normal View History

Initial Commit
2020-09-17 18:35:07 +02:00
#+TITLE: SVD Control
:DRAWER:
#+STARTUP: overview
#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas
#+HTML_LINK_HOME: ../index.html
Use online CSS and JS
2020-11-12 10:18:50 +01:00
#+HTML_LINK_UP: ../index.html
Initial Commit
2020-09-17 18:35:07 +02:00
Use online CSS and JS
2020-11-12 10:18:50 +01:00
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
Initial Commit
2020-09-17 18:35:07 +02:00
#+HTML_MATHJAX: align: center tagside: right font: TeX
#+PROPERTY: header-args:matlab :session *MATLAB*
#+PROPERTY: header-args:matlab+ :comments org
#+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :exports both
#+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :output-dir figs
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:shell :eval no-export
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+PROPERTY: header-args:latex+ :results file raw replace
#+PROPERTY: header-args:latex+ :buffer no
Initial Commit
2020-09-17 18:35:07 +02:00
#+PROPERTY: header-args:latex+ :eval no-export
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+PROPERTY: header-args:latex+ :exports results
Initial Commit
2020-09-17 18:35:07 +02:00
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
Initial Commit
2020-09-17 18:35:07 +02:00
:END:
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
* Gravimeter - Simscape Model
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
:PROPERTIES:
:header-args:matlab+: :tangle gravimeter/script.m
:END:
Update gravimeter parameters
2020-10-05 18:06:49 +02:00
** Introduction
#+name: fig:gravimeter_model
#+caption: Model of the gravimeter
[[file:figs/gravimeter_model.png]]
** Matlab Init :noexport:ignore:
Initial Commit
2020-09-17 18:35:07 +02:00
#+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
Use online CSS and JS
2020-11-12 10:18:50 +01:00
#+begin_src matlab :tangle no
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
addpath('gravimeter');
#+end_src
** Simscape Model - Parameters
Initial Commit
2020-09-17 18:35:07 +02:00
#+begin_src matlab
open('gravimeter.slx')
#+end_src
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Parameters
#+begin_src matlab
Update gravimeter parameters
2020-10-05 18:06:49 +02:00
l = 1.0; % Length of the mass [m]
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
h = 1.7; % Height of the mass [m]
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
la = l/2; % Position of Act. [m]
ha = h/2; % Position of Act. [m]
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
m = 400; % Mass [kg]
I = 115; % Inertia [kg m^2]
k = 15e3; % Actuator Stiffness [N/m]
c = 0.03; % Actuator Damping [N/(m/s)]
deq = 0.2; % Length of the actuators [m]
g = 0; % Gravity [m/s2]
#+end_src
** System Identification - Without Gravity
Initial Commit
2020-09-17 18:35:07 +02:00
#+begin_src matlab
%% Name of the Simulink File
mdl = 'gravimeter';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F1'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/F2'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/F3'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Acc_side'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Acc_side'], 2, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Acc_top'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Acc_top'], 2, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io);
Update gravimeter model
2020-09-21 13:08:27 +02:00
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'};
Initial Commit
2020-09-17 18:35:07 +02:00
#+end_src
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
The inputs and outputs of the plant are shown in Figure [[fig:gravimeter_plant_schematic]].
#+begin_src latex :file gravimeter_plant_schematic.pdf :tangle no :exports results
\begin{tikzpicture}
\node[block] (G) {$G$};
% Connections and labels
\draw[<-] (G.west) -- ++(-2.0, 0) node[above right]{$\bm{\tau} = \begin{bmatrix}\tau_1 \\ \tau_2 \\ \tau_2 \end{bmatrix}$};
\draw[->] (G.east) -- ++( 2.0, 0) node[above left]{$\bm{a} = \begin{bmatrix} a_{1x} \\ a_{1z} \\ a_{2x} \\ a_{2z} \end{bmatrix}$};
\end{tikzpicture}
#+end_src
#+name: fig:gravimeter_plant_schematic
#+caption: Schematic of the gravimeter plant
#+RESULTS:
[[file:figs/gravimeter_plant_schematic.png]]
\begin{equation}
\bm{a} = \begin{bmatrix} a_{1x} \\ a_{1z} \\ a_{2x} \\ a_{2z} \end{bmatrix}
\end{equation}
\begin{equation}
\bm{\tau} = \begin{bmatrix}\tau_1 \\ \tau_2 \\ \tau_2 \end{bmatrix}
\end{equation}
We can check the poles of the plant:
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab :results output replace :exports results
pole(G)
#+end_src
#+RESULTS:
#+begin_example
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
-0.000183495485977108 + 13.546056874877i
-0.000183495485977108 - 13.546056874877i
-7.49842878906757e-05 + 8.65934902322567i
-7.49842878906757e-05 - 8.65934902322567i
-1.33171230256362e-05 + 3.64924169037897i
-1.33171230256362e-05 - 3.64924169037897i
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+end_example
Update gravimeter model
2020-09-21 13:08:27 +02:00
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
The plant as 6 states as expected (2 translations + 1 rotation)
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+begin_src matlab :results output replace
size(G)
#+end_src
#+RESULTS:
: State-space model with 4 outputs, 3 inputs, and 6 states.
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
The bode plot of all elements of the plant are shown in Figure [[fig:open_loop_tf]].
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+begin_src matlab :exports none
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
freqs = logspace(-1, 2, 1000);
Initial Commit
2020-09-17 18:35:07 +02:00
figure;
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
tiledlayout(4, 3, 'TileSpacing', 'None', 'Padding', 'None');
for out_i = 1:4
for in_i = 1:3
nexttile;
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-');
Initial Commit
2020-09-17 18:35:07 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
xlim([1e-1, 2e1]); ylim([1e-4, 1e0]);
if in_i == 1
ylabel('Amplitude [m/N]')
else
set(gca, 'YTickLabel',[]);
end
if out_i == 4
xlabel('Frequency [Hz]')
else
set(gca, 'XTickLabel',[]);
end
Initial Commit
2020-09-17 18:35:07 +02:00
end
end
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/open_loop_tf.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:open_loop_tf
#+caption: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers
#+RESULTS:
[[file:figs/open_loop_tf.png]]
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Physical Decoupling using the Jacobian
<<sec:gravimeter_jacobian_decoupling>>
Consider the control architecture shown in Figure [[fig:gravimeter_decouple_jacobian]].
The Jacobian matrix $J_{\tau}$ is used to transform forces applied by the three actuators into forces/torques applied on the gravimeter at its center of mass.
The Jacobian matrix $J_{a}$ is used to compute the vertical acceleration, horizontal acceleration and rotational acceleration of the mass with respect to its center of mass.
We thus define a new plant as defined in Figure [[fig:gravimeter_decouple_jacobian]].
\[ G_x(s) = J_a G(s) J_{\tau}^{-T} \]
$G_x(s)$ correspond to the transfer function from forces and torques applied to the gravimeter at its center of mass to the absolute acceleration of the gravimeter's center of mass.
#+begin_src latex :file gravimeter_decouple_jacobian.pdf :tangle no :exports results
\begin{tikzpicture}
\node[block] (G) {$G$};
\node[block, left=0.6 of G] (Jt) {$J_{\tau}^{-T}$};
\node[block, right=0.6 of G] (Ja) {$J_{a}$};
% Connections and labels
\draw[<-] (Jt.west) -- ++(-1.1, 0) node[above right]{$\bm{\mathcal{F}}$};
\draw[->] (Jt.east) -- (G.west) node[above left]{$\bm{\tau}$};
\draw[->] (G.east) -- (Ja.west) node[above left]{$\bm{a}$};
\draw[->] (Ja.east) -- ++( 1.1, 0) node[above left]{$\bm{\mathcal{X}}$};
\begin{scope}[on background layer]
\node[fit={(Jt.south west) (Ja.north east)}, fill=black!10!white, draw, dashed, inner sep=14pt] (Gx) {};
\node[below right] at (Gx.north west) {$\bm{G}_x$};
\end{scope}
\end{tikzpicture}
#+end_src
#+name: fig:gravimeter_decouple_jacobian
#+caption: Decoupled plant $\bm{G}_x$ using the Jacobian matrix $J$
#+RESULTS:
[[file:figs/gravimeter_decouple_jacobian.png]]
The jacobian corresponding to the sensors and actuators are defined below.
#+begin_src matlab
Ja = [1 0 h/2
0 1 -l/2
1 0 -h/2
0 1 0];
Jt = [1 0 ha
0 1 -la
0 1 la];
#+end_src
#+begin_src matlab
Gx = pinv(Ja)*G*pinv(Jt');
Gx.InputName = {'Fx', 'Fz', 'My'};
Gx.OutputName = {'Dx', 'Dz', 'Ry'};
#+end_src
The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravimeter_jacobian_plant]].
#+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure;
% Magnitude
hold on;
for i_in = 1:3
for i_out = [1:i_in-1, i_in+1:3]
plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'DisplayName', '$G_x(i,j)\ i \neq j$');
set(gca,'ColorOrderIndex',1)
for i_in_out = 1:3
plot(freqs, abs(squeeze(freqresp(Gx(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
legend('location', 'southeast');
ylim([1e-8, 1e0]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/gravimeter_jacobian_plant.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:gravimeter_jacobian_plant
#+caption: Diagonal and off-diagonal elements of $G_x$
#+RESULTS:
[[file:figs/gravimeter_jacobian_plant.png]]
** Real Approximation of $G$ at the decoupling frequency
<<sec:gravimeter_real_approx>>
Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G_u(j\omega_c)$ from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency $\omega_c$.
#+begin_src matlab
wc = 2*pi*10; % Decoupling frequency [rad/s]
H1 = evalfr(G, j*wc);
#+end_src
The real approximation is computed as follows:
#+begin_src matlab
D = pinv(real(H1'*H1));
H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
#+end_src
#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(H1, {}, {}, ' %.2g ');
#+end_src
#+caption: Real approximate of $G$ at the decoupling frequency $\omega_c$
#+RESULTS:
| 0.0026 | -3.7e-05 | 3.7e-05 |
| 1.9e-10 | 0.0025 | 0.0025 |
| -0.0078 | 0.0045 | -0.0045 |
** SVD Decoupling
<<sec:gravimeter_svd_decoupling>>
First, the Singular Value Decomposition of $H_1$ is performed:
\[ H_1 = U \Sigma V^H \]
#+begin_src matlab
[U,~,V] = svd(H1);
#+end_src
The obtained matrices $U$ and $V$ are used to decouple the system as shown in Figure [[fig:gravimeter_decouple_svd]].
#+begin_src latex :file gravimeter_decouple_svd.pdf :tangle no :exports results
\begin{tikzpicture}
\node[block] (G) {$G_u$};
\node[block, left=0.6 of G.west] (V) {$V^{-T}$};
\node[block, right=0.6 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]{$\tau$};
\draw[->] (G.east) -- (U.west) node[above left]{$a$};
\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:gravimeter_decouple_svd
#+caption: Decoupled plant $\bm{G}_{SVD}$ using the Singular Value Decomposition
#+RESULTS:
[[file:figs/gravimeter_decouple_svd.png]]
The decoupled plant is then:
\[ G_{SVD}(s) = U^{-1} G_u(s) V^{-H} \]
#+begin_src matlab
Gsvd = inv(U)*G*inv(V');
#+end_src
The diagonal and off-diagonal elements of the "SVD" plant are shown in Figure [[fig:gravimeter_svd_plant]].
#+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure;
% Magnitude
hold on;
for i_in = 1:3
for i_out = [1:i_in-1, i_in+1:3]
plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'DisplayName', '$G_x(i,j)\ i \neq j$');
set(gca,'ColorOrderIndex',1)
for i_in_out = 1:3
plot(freqs, abs(squeeze(freqresp(Gsvd(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_x(%d,%d)$', i_in_out, i_in_out));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude');
legend('location', 'southeast', 'FontSize', 8);
ylim([1e-8, 1e0]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/gravimeter_svd_plant.pdf', 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:gravimeter_svd_plant
#+caption: Diagonal and off-diagonal elements of $G_{svd}$
#+RESULTS:
[[file:figs/gravimeter_svd_plant.png]]
** TODO Verification of the decoupling using the "Gershgorin Radii"
<<sec:comp_decoupling>>
The "Gershgorin Radii" is computed for the coupled plant $G(s)$, for the "Jacobian plant" $G_x(s)$ and the "SVD Decoupled Plant" $G_{SVD}(s)$:
The "Gershgorin Radii" of a matrix $S$ is defined by:
\[ \zeta_i(j\omega) = \frac{\sum\limits_{j\neq i}|S_{ij}(j\omega)|}{|S_{ii}(j\omega)|} \]
This is computed over the following frequencies.
#+begin_src matlab
freqs = logspace(-2, 2, 1000); % [Hz]
#+end_src
#+begin_src matlab :exports none
% Gershgorin Radii for the coupled plant:
Gr_coupled = zeros(length(freqs), size(Gu,2));
H = abs(squeeze(freqresp(Gu, freqs, 'Hz')));
for out_i = 1:size(Gu,2)
Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
% Gershgorin Radii for the decoupled plant using SVD:
Gr_decoupled = zeros(length(freqs), size(Gsvd,2));
H = abs(squeeze(freqresp(Gsvd, freqs, 'Hz')));
for out_i = 1:size(Gsvd,2)
Gr_decoupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
% Gershgorin Radii for the decoupled plant using the Jacobian:
Gr_jacobian = zeros(length(freqs), size(Gx,2));
H = abs(squeeze(freqresp(Gx, freqs, 'Hz')));
for out_i = 1:size(Gx,2)
Gr_jacobian(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
#+end_src
#+begin_src matlab :exports results
figure;
hold on;
plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled');
plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD');
plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian');
for in_i = 2:6
set(gca,'ColorOrderIndex',1)
plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2)
plot(freqs, Gr_decoupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',3)
plot(freqs, Gr_jacobian(:,in_i), 'HandleVisibility', 'off');
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
legend('location', 'northwest');
ylim([1e-3, 1e3]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/simscape_model_gershgorin_radii.pdf', 'eps', true, 'width', 'wide', 'height', 'normal');
#+end_src
#+name: fig:simscape_model_gershgorin_radii
#+caption: Gershgorin Radii of the Coupled and Decoupled plants
#+RESULTS:
[[file:figs/simscape_model_gershgorin_radii.png]]
** TODO Obtained Decoupled Plants
<<sec:gravimeter_decoupled_plant>>
The bode plot of the diagonal and off-diagonal elements of $G_{SVD}$ are shown in Figure [[fig:simscape_model_decoupled_plant_svd]].
#+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile([2, 1]);
hold on;
for i_in = 1:6
for i_out = [1:i_in-1, i_in+1:6]
plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
'DisplayName', '$G_{SVD}(i,j),\ i \neq j$');
set(gca,'ColorOrderIndex',1)
for ch_i = 1:6
plot(freqs, abs(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))), ...
'DisplayName', sprintf('$G_{SVD}(%i,%i)$', ch_i, ch_i));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
legend('location', 'northwest');
ylim([1e-1, 1e5])
% Phase
ax2 = nexttile;
hold on;
for ch_i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180:90:360]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/simscape_model_decoupled_plant_svd.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:simscape_model_decoupled_plant_svd
#+caption: Decoupled Plant using SVD
#+RESULTS:
[[file:figs/simscape_model_decoupled_plant_svd.png]]
Similarly, the bode plots of the diagonal elements and off-diagonal elements of the decoupled plant $G_x(s)$ using the Jacobian are shown in Figure [[fig:simscape_model_decoupled_plant_jacobian]].
#+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile([2, 1]);
hold on;
for i_in = 1:6
for i_out = [1:i_in-1, i_in+1:6]
plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off');
end
end
plot(freqs, abs(squeeze(freqresp(Gx(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
'DisplayName', '$G_x(i,j),\ i \neq j$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))), 'DisplayName', '$G_x(1,1) = A_x/F_x$');
plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))), 'DisplayName', '$G_x(2,2) = A_y/F_y$');
plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))), 'DisplayName', '$G_x(3,3) = A_z/F_z$');
plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))), 'DisplayName', '$G_x(4,4) = A_{R_x}/M_x$');
plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))), 'DisplayName', '$G_x(5,5) = A_{R_y}/M_y$');
plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))), 'DisplayName', '$G_x(6,6) = A_{R_z}/M_z$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
legend('location', 'northwest');
ylim([1e-2, 2e6])
% Phase
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([0, 180]);
yticks([0:45:360]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/simscape_model_decoupled_plant_jacobian.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:simscape_model_decoupled_plant_jacobian
#+caption: Gravimeter Platform Plant from forces (resp. torques) applied by the legs to the acceleration (resp. angular acceleration) of the platform as well as all the coupling terms between the two (non-diagonal terms of the transfer function matrix)
#+RESULTS:
[[file:figs/simscape_model_decoupled_plant_jacobian.png]]
** TODO Diagonal Controller
<<sec:gravimeter_diagonal_control>>
The control diagram for the centralized control is shown in Figure [[fig:centralized_control]].
The controller $K_c$ is "working" in an cartesian frame.
The Jacobian is used to convert forces in the cartesian frame to forces applied by the actuators.
#+begin_src latex :file centralized_control.pdf :tangle no :exports results
\begin{tikzpicture}
\node[block={2cm}{1.5cm}] (G) {$\begin{bmatrix}G_d\\G_u\end{bmatrix}$};
\node[above] at (G.north) {$\bm{G}$};
\node[block, below right=0.6 and -0.5 of G] (K) {$K_c$};
\node[block, below left= 0.6 and -0.5 of G] (J) {$J^{-T}$};
% Inputs of the controllers
\coordinate[] (inputd) at ($(G.south west)!0.75!(G.north west)$);
\coordinate[] (inputu) at ($(G.south west)!0.25!(G.north west)$);
% Connections and labels
\draw[<-] (inputd) -- ++(-0.8, 0) node[above right]{$D_w$};
\draw[->] (G.east) -- ++(2.0, 0) node[above left]{$a$};
\draw[->] ($(G.east)+(1.4, 0)$)node[branch]{} |- (K.east);
\draw[->] (K.west) -- (J.east) node[above right]{$\mathcal{F}$};
\draw[->] (J.west) -- ++(-0.6, 0) |- (inputu) node[above left]{$\tau$};
\end{tikzpicture}
#+end_src
#+name: fig:centralized_control
#+caption: Control Diagram for the Centralized control
#+RESULTS:
[[file:figs/centralized_control.png]]
The SVD control architecture is shown in Figure [[fig:svd_control]].
The matrices $U$ and $V$ are used to decoupled the plant $G$.
#+begin_src latex :file svd_control.pdf :tangle no :exports results
\begin{tikzpicture}
\node[block={2cm}{1.5cm}] (G) {$\begin{bmatrix}G_d\\G_u\end{bmatrix}$};
\node[above] at (G.north) {$\bm{G}$};
\node[block, below right=0.6 and 0 of G] (U) {$U^{-1}$};
\node[block, below=0.6 of G] (K) {$K_{\text{SVD}}$};
\node[block, below left= 0.6 and 0 of G] (V) {$V^{-T}$};
% Inputs of the controllers
\coordinate[] (inputd) at ($(G.south west)!0.75!(G.north west)$);
\coordinate[] (inputu) at ($(G.south west)!0.25!(G.north west)$);
% Connections and labels
\draw[<-] (inputd) -- ++(-0.8, 0) node[above right]{$D_w$};
\draw[->] (G.east) -- ++(2.5, 0) node[above left]{$a$};
\draw[->] ($(G.east)+(2.0, 0)$) node[branch]{} |- (U.east);
\draw[->] (U.west) -- (K.east);
\draw[->] (K.west) -- (V.east);
\draw[->] (V.west) -- ++(-0.6, 0) |- (inputu) node[above left]{$\tau$};
\end{tikzpicture}
#+end_src
#+name: fig:svd_control
#+caption: Control Diagram for the SVD control
#+RESULTS:
[[file:figs/svd_control.png]]
We choose the controller to be a low pass filter:
\[ K_c(s) = \frac{G_0}{1 + \frac{s}{\omega_0}} \]
$G_0$ is tuned such that the crossover frequency corresponding to the diagonal terms of the loop gain is equal to $\omega_c$
#+begin_src matlab
wc = 2*pi*80; % Crossover Frequency [rad/s]
w0 = 2*pi*0.1; % Controller Pole [rad/s]
#+end_src
#+begin_src matlab
K_cen = diag(1./diag(abs(evalfr(Gx, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0);
L_cen = K_cen*Gx;
G_cen = feedback(G, pinv(J')*K_cen, [7:12], [1:6]);
#+end_src
#+begin_src matlab
K_svd = diag(1./diag(abs(evalfr(Gsvd, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0);
L_svd = K_svd*Gsvd;
G_svd = feedback(G, inv(V')*K_svd*inv(U), [7:12], [1:6]);
#+end_src
The obtained diagonal elements of the loop gains are shown in Figure [[fig:gravimeter_comp_loop_gain_diagonal]].
#+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(L_svd(1, 1), freqs, 'Hz'))), 'DisplayName', '$L_{SVD}(i,i)$');
for i_in_out = 2:6
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(L_cen(1, 1), freqs, 'Hz'))), ...
'DisplayName', '$L_{J}(i,i)$');
for i_in_out = 2:6
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
legend('location', 'northwest');
ylim([5e-2, 2e3])
% Phase
ax2 = nexttile;
hold on;
for i_in_out = 1:6
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',2)
for i_in_out = 1:6
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*angle(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180:90:360]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/gravimeter_comp_loop_gain_diagonal.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:gravimeter_comp_loop_gain_diagonal
#+caption: Comparison of the diagonal elements of the loop gains for the SVD control architecture and the Jacobian one
#+RESULTS:
[[file:figs/gravimeter_comp_loop_gain_diagonal.png]]
** TODO Closed-Loop system Performances
<<sec:gravimeter_closed_loop_results>>
Let's first verify the stability of the closed-loop systems:
#+begin_src matlab :results output replace text
isstable(G_cen)
#+end_src
#+RESULTS:
: ans =
: logical
: 1
#+begin_src matlab :results output replace text
isstable(G_svd)
#+end_src
#+RESULTS:
: ans =
: logical
: 1
The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure [[fig:gravimeter_platform_simscape_cl_transmissibility]].
#+begin_src matlab :exports results
freqs = logspace(-2, 2, 1000);
figure;
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
plot(freqs, abs(squeeze(freqresp(G_cen('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
plot(freqs, abs(squeeze(freqresp(G_svd('Ax', 'Dwx')/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G( 'Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_cen('Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_svd('Ay', 'Dwy')/s^2, freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$D_x/D_{w,x}$, $D_y/D_{w, y}$'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest');
ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Az', 'Dwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Az', 'Dwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Az', 'Dwz')/s^2, freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$D_z/D_{w,z}$'); set(gca, 'XTickLabel',[]);
ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arx', 'Rwx')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arx', 'Rwx')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Arx', 'Rwx')/s^2, freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G( 'Ary', 'Rwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Ary', 'Rwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Ary', 'Rwy')/s^2, freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$R_x/R_{w,x}$, $R_y/R_{w,y}$'); xlabel('Frequency [Hz]');
ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arz', 'Rwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arz', 'Rwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Arz', 'Rwz')/s^2, freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$R_z/R_{w,z}$'); xlabel('Frequency [Hz]');
linkaxes([ax1,ax2,ax3,ax4],'xy');
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 1e2]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/gravimeter_platform_simscape_cl_transmissibility.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:gravimeter_platform_simscape_cl_transmissibility
#+caption: Obtained Transmissibility
#+RESULTS:
[[file:figs/gravimeter_platform_simscape_cl_transmissibility.png]]
* Gravimeter - Analytical Model :noexport:
** System Identification - With Gravity :noexport:
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab
g = 9.80665; % Gravity [m/s2]
#+end_src
Initial Commit
2020-09-17 18:35:07 +02:00
#+begin_src matlab
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Gg = linearize(mdl, io);
Gg.InputName = {'F1', 'F2', 'F3'};
Gg.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'};
#+end_src
We can now see that the system is unstable due to gravity.
#+begin_src matlab :results output replace :exports results
pole(Gg)
#+end_src
#+RESULTS:
#+begin_example
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
-7.49865861504606e-05 + 8.65948534948982i
-7.49865861504606e-05 - 8.65948534948982i
-4.76450798645977 + 0i
4.7642612321107 + 0i
-7.34348883628024e-05 + 4.29133825321225i
-7.34348883628024e-05 - 4.29133825321225i
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+end_example
#+begin_src matlab :exports none
freqs = logspace(-2, 2, 1000);
figure;
for in_i = 1:3
for out_i = 1:4
subplot(4, 3, 3*(out_i-1)+in_i);
hold on;
plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-');
plot(freqs, abs(squeeze(freqresp(Gg(out_i,in_i), freqs, 'Hz'))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
end
end
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/open_loop_tf_g.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:open_loop_tf_g
#+caption: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers with an without gravity
#+RESULTS:
[[file:figs/open_loop_tf_g.png]]
Initial Commit
2020-09-17 18:35:07 +02:00
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Parameters
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Bode options.
#+begin_src matlab
Initial Commit
2020-09-17 18:35:07 +02:00
P = bodeoptions;
P.FreqUnits = 'Hz';
P.MagUnits = 'abs';
P.MagScale = 'log';
P.Grid = 'on';
P.PhaseWrapping = 'on';
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
P.Title.FontSize = 14;
P.XLabel.FontSize = 14;
P.YLabel.FontSize = 14;
P.TickLabel.FontSize = 12;
Initial Commit
2020-09-17 18:35:07 +02:00
P.Xlim = [1e-1,1e2];
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
P.MagLowerLimMode = 'manual';
P.MagLowerLim= 1e-3;
#+end_src
Initial Commit
2020-09-17 18:35:07 +02:00
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Frequency vector.
#+begin_src matlab
w = 2*pi*logspace(-1,2,1000); % [rad/s]
#+end_src
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Generation of the State Space Model
Minor modifications
2020-10-05 18:28:44 +02:00
Mass matrix
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab
M = [m 0 0
0 m 0
0 0 I];
Minor modifications
2020-10-05 18:28:44 +02:00
#+end_src
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Minor modifications
2020-10-05 18:28:44 +02:00
Jacobian of the bottom sensor
#+begin_src matlab
Update gravimeter parameters
2020-10-05 18:06:49 +02:00
Js1 = [1 0 h/2
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
0 1 -l/2];
Minor modifications
2020-10-05 18:28:44 +02:00
#+end_src
Jacobian of the top sensor
#+begin_src matlab
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Js2 = [1 0 -h/2
Update gravimeter parameters
2020-10-05 18:06:49 +02:00
0 1 0];
Minor modifications
2020-10-05 18:28:44 +02:00
#+end_src
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Minor modifications
2020-10-05 18:28:44 +02:00
Jacobian of the actuators
#+begin_src matlab
Update gravimeter parameters
2020-10-05 18:06:49 +02:00
Ja = [1 0 ha % Left horizontal actuator
0 1 -la % Left vertical actuator
0 1 la]; % Right vertical actuator
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Jta = Ja';
Minor modifications
2020-10-05 18:28:44 +02:00
#+end_src
Stiffness and Damping matrices
#+begin_src matlab
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
K = k*Jta*Ja;
Update analytical parameters
2020-09-30 17:21:18 +02:00
C = c*Jta*Ja;
Minor modifications
2020-10-05 18:28:44 +02:00
#+end_src
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
Minor modifications
2020-10-05 18:28:44 +02:00
State Space Matrices
#+begin_src matlab
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
E = [1 0 0
0 1 0
0 0 1]; %projecting ground motion in the directions of the legs
AA = [zeros(3) eye(3)
-M\K -M\C];
BB = [zeros(3,6)
M\Jta M\(k*Jta*E)];
CC = [[Js1;Js2] zeros(4,3);
zeros(2,6)
(Js1+Js2)./2 zeros(2,3)
(Js1-Js2)./2 zeros(2,3)
(Js1-Js2)./(2*h) zeros(2,3)];
DD = [zeros(4,6)
zeros(2,3) eye(2,3)
zeros(6,6)];
#+end_src
Minor modifications
2020-10-05 18:28:44 +02:00
State Space model:
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
- Input = three actuators and three ground motions
- Output = the bottom sensor; the top sensor; the ground motion; the half sum; the half difference; the rotation
Minor modifications
2020-10-05 18:28:44 +02:00
#+begin_src matlab
system_dec = ss(AA,BB,CC,DD);
#+end_src
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab :results output replace
size(system_dec)
#+end_src
#+RESULTS:
: State-space model with 12 outputs, 6 inputs, and 6 states.
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Comparison with the Simscape Model
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab :exports none
freqs = logspace(-2, 2, 1000);
figure;
for in_i = 1:3
for out_i = 1:4
subplot(4, 3, 3*(out_i-1)+in_i);
hold on;
plot(freqs, abs(squeeze(freqresp(G(out_i,in_i), freqs, 'Hz'))), '-');
plot(freqs, abs(squeeze(freqresp(system_dec(out_i,in_i)*s^2, freqs, 'Hz'))), '-');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
end
end
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/gravimeter_analytical_system_open_loop_models.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:gravimeter_analytical_system_open_loop_models
#+caption: Comparison of the analytical and the Simscape models
#+RESULTS:
[[file:figs/gravimeter_analytical_system_open_loop_models.png]]
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Analysis
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab
Initial Commit
2020-09-17 18:35:07 +02:00
% figure
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
% bode(system_dec,P);
% return
#+end_src
Initial Commit
2020-09-17 18:35:07 +02:00
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab
%% svd decomposition
% system_dec_freq = freqresp(system_dec,w);
% S = zeros(3,length(w));
% for m = 1:length(w)
% S(:,m) = svd(system_dec_freq(1:4,1:3,m));
% end
Initial Commit
2020-09-17 18:35:07 +02:00
% figure
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
% loglog(w./(2*pi), S);hold on;
% % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:)));
% xlabel('Frequency [Hz]');ylabel('Singular Value [-]');
% legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6');
% ylim([1e-8 1e-2]);
Initial Commit
2020-09-17 18:35:07 +02:00
%
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
% %condition number
Initial Commit
2020-09-17 18:35:07 +02:00
% figure
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
% loglog(w./(2*pi), S(1,:)./S(3,:));hold on;
% % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:)));
% xlabel('Frequency [Hz]');ylabel('Condition number [-]');
% % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6');
%
% %performance indicator
% system_dec_svd = freqresp(system_dec(1:4,1:3),2*pi*10);
% [U,S,V] = svd(system_dec_svd);
% H_svd_OL = -eye(3,4);%-[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*40,-2*pi*200,40/200) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);%
% H_svd = pinv(V')*H_svd_OL*pinv(U);
% % system_dec_control_svd_ = feedback(system_dec,g*pinv(V')*H*pinv(U));
%
% OL_dec = g_svd*H_svd*system_dec(1:4,1:3);
% OL_freq = freqresp(OL_dec,w); % OL = G*H
% CL_system = feedback(eye(3),-g_svd*H_svd*system_dec(1:4,1:3));
% CL_freq = freqresp(CL_system,w); % CL = (1+G*H)^-1
% % CL_system_2 = feedback(system_dec,H);
% % CL_freq_2 = freqresp(CL_system_2,w); % CL = G/(1+G*H)
% for i = 1:size(w,2)
% OL(:,i) = svd(OL_freq(:,:,i));
% CL (:,i) = svd(CL_freq(:,:,i));
% %CL2 (:,i) = svd(CL_freq_2(:,:,i));
% end
%
% un = ones(1,length(w));
% figure
% loglog(w./(2*pi),OL(3,:)+1,'k',w./(2*pi),OL(3,:)-1,'b',w./(2*pi),1./CL(1,:),'r--',w./(2*pi),un,'k:');hold on;%
% % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:)));
% % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:)));
% xlabel('Frequency [Hz]');ylabel('Singular Value [-]');
% legend('GH \sigma_{inf} +1 ','GH \sigma_{inf} -1','S 1/\sigma_{sup}');%,'\lambda_1','\lambda_2','\lambda_3');
%
% figure
% loglog(w./(2*pi),OL(1,:)+1,'k',w./(2*pi),OL(1,:)-1,'b',w./(2*pi),1./CL(3,:),'r--',w./(2*pi),un,'k:');hold on;%
% % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:)));
% % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:)));
% xlabel('Frequency [Hz]');ylabel('Singular Value [-]');
% legend('GH \sigma_{sup} +1 ','GH \sigma_{sup} -1','S 1/\sigma_{inf}');%,'\lambda_1','\lambda_2','\lambda_3');
#+end_src
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Control Section
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab
system_dec_10Hz = freqresp(system_dec,2*pi*10);
system_dec_0Hz = freqresp(system_dec,0);
system_decReal_10Hz = pinv(align(system_dec_10Hz));
[Ureal,Sreal,Vreal] = svd(system_decReal_10Hz(1:4,1:3));
normalizationMatrixReal = abs(pinv(Ureal)*system_dec_0Hz(1:4,1:3)*pinv(Vreal'));
[U,S,V] = svd(system_dec_10Hz(1:4,1:3));
normalizationMatrix = abs(pinv(U)*system_dec_0Hz(1:4,1:3)*pinv(V'));
H_dec = ([zpk(-2*pi*5,-2*pi*30,30/5) 0 0 0
0 zpk(-2*pi*4,-2*pi*20,20/4) 0 0
0 0 0 zpk(-2*pi,-2*pi*10,10)]);
H_cen_OL = [zpk(-2*pi,-2*pi*10,10) 0 0; 0 zpk(-2*pi,-2*pi*10,10) 0;
0 0 zpk(-2*pi*5,-2*pi*30,30/5)];
H_cen = pinv(Jta)*H_cen_OL*pinv([Js1; Js2]);
% H_svd_OL = -[1/normalizationMatrix(1,1) 0 0 0
% 0 1/normalizationMatrix(2,2) 0 0
% 0 0 1/normalizationMatrix(3,3) 0];
% H_svd_OL_real = -[1/normalizationMatrixReal(1,1) 0 0 0
% 0 1/normalizationMatrixReal(2,2) 0 0
% 0 0 1/normalizationMatrixReal(3,3) 0];
H_svd_OL = -[1/normalizationMatrix(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0
0 1/normalizationMatrix(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0
0 0 1/normalizationMatrix(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0];
H_svd_OL_real = -[1/normalizationMatrixReal(1,1)*zpk(-2*pi*10,-2*pi*60,60/10) 0 0 0
0 1/normalizationMatrixReal(2,2)*zpk(-2*pi*5,-2*pi*30,30/5) 0 0
0 0 1/normalizationMatrixReal(3,3)*zpk(-2*pi*2,-2*pi*10,10/2) 0];
% H_svd_OL_real = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*10,-2*pi*100,100/10) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];%-eye(3,4);
% H_svd_OL = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 zpk(-2*pi*4,-2*pi*20,4/20) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);%
H_svd = pinv(V')*H_svd_OL*pinv(U);
H_svd_real = pinv(Vreal')*H_svd_OL_real*pinv(Ureal);
OL_dec = g*H_dec*system_dec(1:4,1:3);
OL_cen = g*H_cen_OL*pinv([Js1; Js2])*system_dec(1:4,1:3)*pinv(Jta);
OL_svd = 100*H_svd_OL*pinv(U)*system_dec(1:4,1:3)*pinv(V');
OL_svd_real = 100*H_svd_OL_real*pinv(Ureal)*system_dec(1:4,1:3)*pinv(Vreal');
#+end_src
#+begin_src matlab
% figure
% bode(OL_dec,w,P);title('OL Decentralized');
% figure
% bode(OL_cen,w,P);title('OL Centralized');
#+end_src
#+begin_src matlab
figure
bode(g*system_dec(1:4,1:3),w,P);
title('gain * Plant');
#+end_src
#+begin_src matlab
figure
bode(OL_svd,OL_svd_real,w,P);
title('OL SVD');
legend('SVD of Complex plant','SVD of real approximation of the complex plant')
#+end_src
#+begin_src matlab
figure
bode(system_dec(1:4,1:3),pinv(U)*system_dec(1:4,1:3)*pinv(V'),P);
#+end_src
#+begin_src matlab
CL_dec = feedback(system_dec,g*H_dec,[1 2 3],[1 2 3 4]);
CL_cen = feedback(system_dec,g*H_cen,[1 2 3],[1 2 3 4]);
CL_svd = feedback(system_dec,100*H_svd,[1 2 3],[1 2 3 4]);
CL_svd_real = feedback(system_dec,100*H_svd_real,[1 2 3],[1 2 3 4]);
#+end_src
#+begin_src matlab
pzmap_testCL(system_dec,H_dec,g,[1 2 3],[1 2 3 4])
title('Decentralized control');
#+end_src
#+begin_src matlab
pzmap_testCL(system_dec,H_cen,g,[1 2 3],[1 2 3 4])
title('Centralized control');
#+end_src
#+begin_src matlab
pzmap_testCL(system_dec,H_svd,100,[1 2 3],[1 2 3 4])
title('SVD control');
#+end_src
#+begin_src matlab
pzmap_testCL(system_dec,H_svd_real,100,[1 2 3],[1 2 3 4])
title('Real approximation SVD control');
#+end_src
#+begin_src matlab
P.Ylim = [1e-8 1e-3];
figure
bodemag(system_dec(1:4,1:3),CL_dec(1:4,1:3),CL_cen(1:4,1:3),CL_svd(1:4,1:3),CL_svd_real(1:4,1:3),P);
title('Motion/actuator')
legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src
#+begin_src matlab
P.Ylim = [1e-5 1e1];
Initial Commit
2020-09-17 18:35:07 +02:00
figure
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
bodemag(system_dec(1:4,4:6),CL_dec(1:4,4:6),CL_cen(1:4,4:6),CL_svd(1:4,4:6),CL_svd_real(1:4,4:6),P);
title('Transmissibility');
legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src
#+begin_src matlab
figure
bodemag(system_dec([7 9],4:6),CL_dec([7 9],4:6),CL_cen([7 9],4:6),CL_svd([7 9],4:6),CL_svd_real([7 9],4:6),P);
title('Transmissibility from half sum and half difference in the X direction');
legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src
#+begin_src matlab
figure
bodemag(system_dec([8 10],4:6),CL_dec([8 10],4:6),CL_cen([8 10],4:6),CL_svd([8 10],4:6),CL_svd_real([8 10],4:6),P);
title('Transmissibility from half sum and half difference in the Z direction');
legend('Control OFF','Decentralized control','Centralized control','SVD control','SVD control real appr.');
#+end_src
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Greshgorin radius
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab
system_dec_freq = freqresp(system_dec,w);
x1 = zeros(1,length(w));
z1 = zeros(1,length(w));
x2 = zeros(1,length(w));
S1 = zeros(1,length(w));
S2 = zeros(1,length(w));
S3 = zeros(1,length(w));
for t = 1:length(w)
x1(t) = (abs(system_dec_freq(1,2,t))+abs(system_dec_freq(1,3,t)))/abs(system_dec_freq(1,1,t));
z1(t) = (abs(system_dec_freq(2,1,t))+abs(system_dec_freq(2,3,t)))/abs(system_dec_freq(2,2,t));
x2(t) = (abs(system_dec_freq(3,1,t))+abs(system_dec_freq(3,2,t)))/abs(system_dec_freq(3,3,t));
system_svd = pinv(Ureal)*system_dec_freq(1:4,1:3,t)*pinv(Vreal');
S1(t) = (abs(system_svd(1,2))+abs(system_svd(1,3)))/abs(system_svd(1,1));
S2(t) = (abs(system_svd(2,1))+abs(system_svd(2,3)))/abs(system_svd(2,2));
S2(t) = (abs(system_svd(3,1))+abs(system_svd(3,2)))/abs(system_svd(3,3));
end
limit = 0.5*ones(1,length(w));
#+end_src
#+begin_src matlab
figure
loglog(w./(2*pi),x1,w./(2*pi),z1,w./(2*pi),x2,w./(2*pi),limit,'--');
legend('x_1','z_1','x_2','Limit');
xlabel('Frequency [Hz]');
ylabel('Greshgorin radius [-]');
#+end_src
#+begin_src matlab
Initial Commit
2020-09-17 18:35:07 +02:00
figure
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
loglog(w./(2*pi),S1,w./(2*pi),S2,w./(2*pi),S3,w./(2*pi),limit,'--');
legend('S1','S2','S3','Limit');
xlabel('Frequency [Hz]');
ylabel('Greshgorin radius [-]');
% set(gcf,'color','w')
#+end_src
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
** Injecting ground motion in the system to have the output
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
#+begin_src matlab
Fr = logspace(-2,3,1e3);
w=2*pi*Fr*1i;
%fit of the ground motion data in m/s^2/rtHz
Fr_ground_x = [0.07 0.1 0.15 0.3 0.7 0.8 0.9 1.2 5 10];
n_ground_x1 = [4e-7 4e-7 2e-6 1e-6 5e-7 5e-7 5e-7 1e-6 1e-5 3.5e-5];
Fr_ground_v = [0.07 0.08 0.1 0.11 0.12 0.15 0.25 0.6 0.8 1 1.2 1.6 2 6 10];
n_ground_v1 = [7e-7 7e-7 7e-7 1e-6 1.2e-6 1.5e-6 1e-6 9e-7 7e-7 7e-7 7e-7 1e-6 2e-6 1e-5 3e-5];
n_ground_x = interp1(Fr_ground_x,n_ground_x1,Fr,'linear');
n_ground_v = interp1(Fr_ground_v,n_ground_v1,Fr,'linear');
% figure
% loglog(Fr,abs(n_ground_v),Fr_ground_v,n_ground_v1,'*');
% xlabel('Frequency [Hz]');ylabel('ASD [m/s^2 /rtHz]');
% return
%converting into PSD
n_ground_x = (n_ground_x).^2;
n_ground_v = (n_ground_v).^2;
%Injecting ground motion in the system and getting the outputs
system_dec_f = (freqresp(system_dec,abs(w)));
PHI = zeros(size(Fr,2),12,12);
for p = 1:size(Fr,2)
Sw=zeros(6,6);
Iact = zeros(3,3);
Sw(4,4) = n_ground_x(p);
Sw(5,5) = n_ground_v(p);
Sw(6,6) = n_ground_v(p);
Sw(1:3,1:3) = Iact;
PHI(p,:,:) = (system_dec_f(:,:,p))*Sw(:,:)*(system_dec_f(:,:,p))';
end
x1 = PHI(:,1,1);
z1 = PHI(:,2,2);
x2 = PHI(:,3,3);
z2 = PHI(:,4,4);
wx = PHI(:,5,5);
wz = PHI(:,6,6);
x12 = PHI(:,1,3);
z12 = PHI(:,2,4);
PHIwx = PHI(:,1,5);
PHIwz = PHI(:,2,6);
xsum = PHI(:,7,7);
zsum = PHI(:,8,8);
xdelta = PHI(:,9,9);
zdelta = PHI(:,10,10);
rot = PHI(:,11,11);
#+end_src
Analysis of the Gravimeter (Jacobian + SVD)
2020-11-16 14:58:26 +01:00
* Gravimeter - Functions :noexport:
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
:PROPERTIES:
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
** =align=
:PROPERTIES:
:header-args:matlab+: :tangle gravimeter/align.m
:END:
<<sec:align>>
This Matlab function is accessible [[file:gravimeter/align.m][here]].
#+begin_src matlab
function [A] = align(V)
%A!ALIGN(V) returns a constat matrix A which is the real alignment of the
%INVERSE of the complex input matrix V
%from Mohit slides
if (nargin ==0) || (nargin > 1)
disp('usage: mat_inv_real = align(mat)')
return
Initial Commit
2020-09-17 18:35:07 +02:00
end
Add two open-loop plant figures
2020-09-30 17:16:30 +02:00
D = pinv(real(V'*V));
A = D*real(V'*diag(exp(1i * angle(diag(V*D*V.'))/2)));
end
#+end_src
** =pzmap_testCL=
:PROPERTIES:
:header-args:matlab+: :tangle gravimeter/pzmap_testCL.m
:END:
<<sec:pzmap_testCL>>
This Matlab function is accessible [[file:gravimeter/pzmap_testCL.m][here]].
#+begin_src matlab
function [] = pzmap_testCL(system,H,gain,feedin,feedout)
% evaluate and plot the pole-zero map for the closed loop system for
% different values of the gain
[~, n] = size(gain);
[m1, n1, ~] = size(H);
[~,n2] = size(feedin);
figure
for i = 1:n
% if n1 == n2
system_CL = feedback(system,gain(i)*H,feedin,feedout);
[P,Z] = pzmap(system_CL);
plot(real(P(:)),imag(P(:)),'x',real(Z(:)),imag(Z(:)),'o');hold on
xlabel('Real axis (s^{-1})');ylabel('Imaginary Axis (s^{-1})');
% clear P Z
% else
% system_CL = feedback(system,gain(i)*H(:,1+(i-1)*m1:m1+(i-1)*m1),feedin,feedout);
%
% [P,Z] = pzmap(system_CL);
% plot(real(P(:)),imag(P(:)),'x',real(Z(:)),imag(Z(:)),'o');hold on
% xlabel('Real axis (s^{-1})');ylabel('Imaginary Axis (s^{-1})');
% clear P Z
% end
end
str = {strcat('gain = ' , num2str(gain(1)))}; % at the end of first loop, z being loop output
str = [str , strcat('gain = ' , num2str(gain(1)))]; % after 2nd loop
for i = 2:n
str = [str , strcat('gain = ' , num2str(gain(i)))]; % after 2nd loop
str = [str , strcat('gain = ' , num2str(gain(i)))]; % after 2nd loop
end
legend(str{:})
end
Initial Commit
2020-09-17 18:35:07 +02:00
#+end_src
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
* Stewart Platform - Simscape Model
Remove Gravity for the Stewart platform model
2020-10-13 15:01:42 +02:00
:PROPERTIES:
:header-args:matlab+: :tangle stewart_platform/simscape_model.m
:END:
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
** Introduction :ignore:
In this analysis, we wish to applied SVD control to the Stewart Platform shown in Figure [[fig:SP_assembly]].
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
Some notes about the system:
- 6 voice coils actuators are used to control the motion of the top platform.
- the motion of the top platform is measured with a 6-axis inertial unit (3 acceleration + 3 angular accelerations)
- the control objective is to isolate the top platform from vibrations coming from the bottom platform
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+name: fig:SP_assembly
#+caption: Stewart Platform CAD View
[[file:figs/SP_assembly.png]]
The analysis of the SVD control applied to the Stewart platform is performed in the following sections:
- Section [[sec:stewart_simscape]]: The parameters of the Simscape model of the Stewart platform are defined
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
- Section [[sec:stewart_identification]]: The plant is identified from the Simscape model and the system coupling is shown
- Section [[sec:stewart_jacobian_decoupling]]: The plant is first decoupled using the Jacobian
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
- Section [[sec:stewart_real_approx]]: A real approximation of the plant is computed for further decoupling using the Singular Value Decomposition (SVD)
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
- Section [[sec:stewart_svd_decoupling]]: The decoupling is performed thanks to the SVD
- Section [[sec:comp_decoupling]]: The effectiveness of the decoupling with the Jacobian and SVD are compared using the Gershorin Radii
- Section [[sec:stewart_decoupled_plant]]: The dynamics of the decoupled plants are shown
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
- Section [[sec:stewart_diagonal_control]]: A diagonal controller is defined to control the decoupled plant
- Section [[sec:stewart_closed_loop_results]]: Finally, the closed loop system properties are studied
Better Matlab notations
2020-11-09 14:37:04 +01:00
** Matlab Init :noexport:ignore:
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+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
Tangle scripts
2020-10-13 14:51:15 +02:00
#+begin_src matlab :tangle no
addpath('stewart_platform');
addpath('stewart_platform/STEP');
#+end_src
Remove Gravity for the Stewart platform model
2020-10-13 15:01:42 +02:00
#+begin_src matlab :eval no
addpath('STEP');
#+end_src
Add figure about RGA
2020-11-23 18:01:13 +01:00
#+begin_src matlab
freqs = logspace(-1, 2, 1000);
#+end_src
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
** Jacobian :noexport:
Update gravimeter model
2020-09-21 13:08:27 +02:00
First, the position of the "joints" (points of force application) are estimated and the Jacobian computed.
Remove jacobian computation section
2020-11-06 15:06:25 +01:00
#+begin_src matlab :tangle no
Tangle scripts
2020-10-13 14:51:15 +02:00
open('drone_platform_jacobian.slx');
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+end_src
Remove jacobian computation section
2020-11-06 15:06:25 +01:00
#+begin_src matlab :tangle no
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
sim('drone_platform_jacobian');
#+end_src
Remove jacobian computation section
2020-11-06 15:06:25 +01:00
#+begin_src matlab :tangle no
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
Aa = [a1.Data(1,:);
a2.Data(1,:);
a3.Data(1,:);
a4.Data(1,:);
a5.Data(1,:);
a6.Data(1,:)]';
Ab = [b1.Data(1,:);
b2.Data(1,:);
b3.Data(1,:);
b4.Data(1,:);
b5.Data(1,:);
b6.Data(1,:)]';
As = (Ab - Aa)./vecnorm(Ab - Aa);
l = vecnorm(Ab - Aa)';
J = [As' , cross(Ab, As)'];
Add figure about RGA
2020-11-23 18:01:13 +01:00
save('stewart_platform/jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+end_src
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
** Simscape Model - Parameters
<<sec:stewart_simscape>>
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+begin_src matlab
Tangle scripts
2020-10-13 14:51:15 +02:00
open('drone_platform.slx');
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+end_src
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
Definition of spring parameters:
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+begin_src matlab
Tangle scripts
2020-10-13 14:51:15 +02:00
kx = 0.5*1e3/3; % [N/m]
ky = 0.5*1e3/3;
kz = 1e3/3;
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
Update gravimeter model
2020-09-21 13:08:27 +02:00
cx = 0.025; % [Nm/rad]
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
cy = 0.025;
cz = 0.025;
#+end_src
Add figure about RGA
2020-11-23 18:01:13 +01:00
We suppose the sensor is perfectly positioned.
#+begin_src matlab
sens_pos_error = zeros(3,1);
#+end_src
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
Gravity:
Updated parameters with mohit
2020-10-22 16:07:10 +02:00
#+begin_src matlab
Update stewart platform after discus. with mohit
2020-10-22 18:04:02 +02:00
g = 0;
Updated parameters with mohit
2020-10-22 16:07:10 +02:00
#+end_src
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
We load the Jacobian (previously computed from the geometry):
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+begin_src matlab
Add figure about RGA
2020-11-23 18:01:13 +01:00
load('jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+end_src
Add controller to Simscape
2020-11-06 15:01:55 +01:00
We initialize other parameters:
#+begin_src matlab
U = eye(6);
V = eye(6);
Kc = tf(zeros(6));
#+end_src
Add screenshots of the Simscape Model
2020-11-06 17:02:58 +01:00
#+name: fig:stewart_simscape
#+caption: General view of the Simscape Model
[[file:figs/stewart_simscape.png]]
#+name: fig:stewart_platform_details
#+caption: Simscape model of the Stewart platform
[[file:figs/stewart_platform_details.png]]
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
** Identification of the plant
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
<<sec:stewart_identification>>
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
The plant shown in Figure [[fig:stewart_platform_plant]] is identified from the Simscape model.
The inputs are:
- $D_w$ translation and rotation of the bottom platform (with respect to the center of mass of the top platform)
- $\tau$ the 6 forces applied by the voice coils
The outputs are the 6 accelerations measured by the inertial unit.
#+begin_src latex :file stewart_platform_plant.pdf :tangle no :exports results
\begin{tikzpicture}
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[block={2cm}{1.5cm}] (G) {$\begin{bmatrix}G_d\\G_u\end{bmatrix}$};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
\node[above] at (G.north) {$\bm{G}$};
% Inputs of the controllers
\coordinate[] (inputd) at ($(G.south west)!0.75!(G.north west)$);
\coordinate[] (inputu) at ($(G.south west)!0.25!(G.north west)$);
% Connections and labels
\draw[<-] (inputd) -- ++(-0.8, 0) node[above right]{$D_w$};
\draw[<-] (inputu) -- ++(-0.8, 0) node[above right]{$\tau$};
\draw[->] (G.east) -- ++(0.8, 0) node[above left]{$a$};
\end{tikzpicture}
#+end_src
#+name: fig:stewart_platform_plant
Better Matlab notations
2020-11-09 14:37:04 +01:00
#+caption: Considered plant $\bm{G} = \begin{bmatrix}G_d\\G_u\end{bmatrix}$. $D_w$ is the translation/rotation of the support, $\tau$ the actuator forces, $a$ the acceleration/angular acceleration of the top platform
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+RESULTS:
[[file:figs/stewart_platform_plant.png]]
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+begin_src matlab
%% Name of the Simulink File
mdl = 'drone_platform';
%% Input/Output definition
clear io; io_i = 1;
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
io(io_i) = linio([mdl, '/Dw'], 1, 'openinput'); io_i = io_i + 1; % Ground Motion
io(io_i) = linio([mdl, '/V-T'], 1, 'openinput'); io_i = io_i + 1; % Actuator Forces
io(io_i) = linio([mdl, '/Inertial Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Top platform acceleration
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
G = linearize(mdl, io);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
G.InputName = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
G.OutputName = {'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'};
Better Matlab notations
2020-11-09 14:37:04 +01:00
% Plant
Gu = G(:, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'});
% Disturbance dynamics
Gd = G(:, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'});
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+end_src
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
There are 24 states (6dof for the bottom platform + 6dof for the top platform).
Add stewart platform model size
2020-09-21 13:14:25 +02:00
#+begin_src matlab :results output replace
size(G)
#+end_src
#+RESULTS:
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
: State-space model with 6 outputs, 12 inputs, and 24 states.
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
The elements of the transfer matrix $\bm{G}$ corresponding to the transfer function from actuator forces $\tau$ to the measured acceleration $a$ are shown in Figure [[fig:stewart_platform_coupled_plant]].
Add stewart platform model size
2020-09-21 13:14:25 +02:00
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
One can easily see that the system is strongly coupled.
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+begin_src matlab :exports none
figure;
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
% Magnitude
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
hold on;
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
for i_in = 1:6
for i_out = [1:i_in-1, i_in+1:6]
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, abs(squeeze(freqresp(Gu(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
'HandleVisibility', 'off');
end
end
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, abs(squeeze(freqresp(Gu(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'DisplayName', '$G_u(i,j)\ i \neq j$');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
set(gca,'ColorOrderIndex',1)
for i_in_out = 1:6
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, abs(squeeze(freqresp(Gu(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_u(%d,%d)$', i_in_out, i_in_out));
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
end
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
xlabel('Frequency [Hz]'); ylabel('Magnitude');
ylim([1e-2, 1e5]);
legend('location', 'northwest');
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
#+end_src
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
exportFig('figs/stewart_platform_coupled_plant.pdf', 'eps', true, 'width', 'wide', 'height', 'normal');
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+end_src
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+name: fig:stewart_platform_coupled_plant
Better Matlab notations
2020-11-09 14:37:04 +01:00
#+caption: Magnitude of all 36 elements of the transfer function matrix $G_u$
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+RESULTS:
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
[[file:figs/stewart_platform_coupled_plant.png]]
Update gravimeter model
2020-09-21 13:08:27 +02:00
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
** Physical Decoupling using the Jacobian
<<sec:stewart_jacobian_decoupling>>
Consider the control architecture shown in Figure [[fig:plant_decouple_jacobian]].
The Jacobian matrix is used to transform forces/torques applied on the top platform to the equivalent forces applied by each actuator.
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
Add figure about RGA
2020-11-23 18:01:13 +01:00
The Jacobian matrix is computed from the geometry of the platform (position and orientation of the actuators).
#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(J, {}, {}, ' %.3f ');
#+end_src
#+caption: Computed Jacobian Matrix
#+RESULTS:
| 0.811 | 0.0 | 0.584 | -0.018 | -0.008 | 0.025 |
| -0.406 | -0.703 | 0.584 | -0.016 | -0.012 | -0.025 |
| -0.406 | 0.703 | 0.584 | 0.016 | -0.012 | 0.025 |
| 0.811 | 0.0 | 0.584 | 0.018 | -0.008 | -0.025 |
| -0.406 | -0.703 | 0.584 | 0.002 | 0.019 | 0.025 |
| -0.406 | 0.703 | 0.584 | -0.002 | 0.019 | -0.025 |
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+begin_src latex :file plant_decouple_jacobian.pdf :tangle no :exports results
\begin{tikzpicture}
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[block] (G) {$G_u$};
\node[block, left=0.6 of G] (J) {$J^{-T}$};
Rework Stewart Platform => Working correctly
2020-09-21 10:47:34 +02:00
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
% Connections and labels
Better Matlab notations
2020-11-09 14:37:04 +01:00
\draw[<-] (J.west) -- ++(-1.0, 0) node[above right]{$\mathcal{F}$};
\draw[->] (J.east) -- (G.west) node[above left]{$\tau$};
\draw[->] (G.east) -- ++( 1.0, 0) node[above left]{$a$};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
\begin{scope}[on background layer]
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[fit={(J.south west) (G.north east)}, fill=black!10!white, draw, dashed, inner sep=14pt] (Gx) {};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
\node[below right] at (Gx.north west) {$\bm{G}_x$};
\end{scope}
\end{tikzpicture}
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+end_src
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+name: fig:plant_decouple_jacobian
#+caption: Decoupled plant $\bm{G}_x$ using the Jacobian matrix $J$
Update gravimeter model
2020-09-21 13:08:27 +02:00
#+RESULTS:
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
[[file:figs/plant_decouple_jacobian.png]]
We define a new plant:
\[ G_x(s) = G(s) J^{-T} \]
$G_x(s)$ correspond to the transfer function from forces and torques applied to the top platform to the absolute acceleration of the top platform.
#+begin_src matlab
Better Matlab notations
2020-11-09 14:37:04 +01:00
Gx = Gu*inv(J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+end_src
Update gravimeter model
2020-09-21 13:08:27 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
** Real Approximation of $G$ at the decoupling frequency
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
<<sec:stewart_real_approx>>
Better Matlab notations
2020-11-09 14:37:04 +01:00
Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G_u(j\omega_c)$ from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency $\omega_c$.
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+begin_src matlab
Update stewart platform after discus. with mohit
2020-10-22 18:04:02 +02:00
wc = 2*pi*30; % Decoupling frequency [rad/s]
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Better Matlab notations
2020-11-09 14:37:04 +01:00
H1 = evalfr(Gu, j*wc);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
The real approximation is computed as follows:
#+begin_src matlab
D = pinv(real(H1'*H1));
H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
#+end_src
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(H1, {}, {}, ' %.1f ');
#+end_src
#+caption: Real approximate of $G$ at the decoupling frequency $\omega_c$
#+RESULTS:
| 4.4 | -2.1 | -2.1 | 4.4 | -2.4 | -2.4 |
| -0.2 | -3.9 | 3.9 | 0.2 | -3.8 | 3.8 |
| 3.4 | 3.4 | 3.4 | 3.4 | 3.4 | 3.4 |
| -367.1 | -323.8 | 323.8 | 367.1 | 43.3 | -43.3 |
| -162.0 | -237.0 | -237.0 | -162.0 | 398.9 | 398.9 |
| 220.6 | -220.6 | 220.6 | -220.6 | 220.6 | -220.6 |
Better Matlab notations
2020-11-09 14:37:04 +01:00
Note that the plant $G_u$ at $\omega_c$ is already an almost real matrix.
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
This can be seen on the Bode plots where the phase is close to 1.
Better Matlab notations
2020-11-09 14:37:04 +01:00
This can be verified below where only the real value of $G_u(\omega_c)$ is shown
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+begin_src matlab :exports results :results value table replace :tangle no
Better Matlab notations
2020-11-09 14:37:04 +01:00
data2orgtable(real(evalfr(Gu, j*wc)), {}, {}, ' %.1f ');
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+end_src
Add figure about RGA
2020-11-23 18:01:13 +01:00
#+caption: Real part of $G$ at the decoupling frequency $\omega_c$
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+RESULTS:
| 4.4 | -2.1 | -2.1 | 4.4 | -2.4 | -2.4 |
| -0.2 | -3.9 | 3.9 | 0.2 | -3.8 | 3.8 |
| 3.4 | 3.4 | 3.4 | 3.4 | 3.4 | 3.4 |
| -367.1 | -323.8 | 323.8 | 367.1 | 43.3 | -43.3 |
| -162.0 | -237.0 | -237.0 | -162.0 | 398.9 | 398.9 |
| 220.6 | -220.6 | 220.6 | -220.6 | 220.6 | -220.6 |
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
** SVD Decoupling
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
<<sec:stewart_svd_decoupling>>
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
First, the Singular Value Decomposition of $H_1$ is performed:
\[ H_1 = U \Sigma V^H \]
#+begin_src matlab
Better Matlab notations
2020-11-09 14:37:04 +01:00
[U,~,V] = svd(H1);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
Add figure about RGA
2020-11-23 18:01:13 +01:00
#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(U, {}, {}, ' %.1g ');
#+end_src
#+caption: Obtained matrix $U$
#+RESULTS:
| -0.005 | 7e-06 | 6e-11 | -3e-06 | -1 | 0.1 |
| -7e-06 | -0.005 | -9e-09 | -5e-09 | -0.1 | -1 |
| 4e-08 | -2e-10 | -6e-11 | -1 | 3e-06 | -3e-07 |
| -0.002 | -1 | -5e-06 | 2e-10 | 0.0006 | 0.005 |
| 1 | -0.002 | -1e-08 | 2e-08 | -0.005 | 0.0006 |
| -4e-09 | 5e-06 | -1 | 6e-11 | -2e-09 | -1e-08 |
#+begin_src matlab :exports results :results value table replace :tangle no
data2orgtable(V, {}, {}, ' %.1g ');
#+end_src
#+caption: Obtained matrix $V$
#+RESULTS:
| -0.2 | 0.5 | -0.4 | -0.4 | -0.6 | -0.2 |
| -0.3 | 0.5 | 0.4 | -0.4 | 0.5 | 0.3 |
| -0.3 | -0.5 | -0.4 | -0.4 | 0.4 | -0.4 |
| -0.2 | -0.5 | 0.4 | -0.4 | -0.5 | 0.3 |
| 0.6 | -0.06 | -0.4 | -0.4 | 0.1 | 0.6 |
| 0.6 | 0.06 | 0.4 | -0.4 | -0.006 | -0.6 |
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
The obtained matrices $U$ and $V$ are used to decouple the system as shown in Figure [[fig:plant_decouple_svd]].
#+begin_src latex :file plant_decouple_svd.pdf :tangle no :exports results
\begin{tikzpicture}
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[block] (G) {$G_u$};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[block, left=0.6 of G.west] (V) {$V^{-T}$};
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
\node[block, right=0.6 of G.east] (U) {$U^{-1}$};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
% Connections and labels
Better Matlab notations
2020-11-09 14:37:04 +01:00
\draw[<-] (V.west) -- ++(-1.0, 0) node[above right]{$u$};
\draw[->] (V.east) -- (G.west) node[above left]{$\tau$};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
\draw[->] (G.east) -- (U.west) node[above left]{$a$};
Better Matlab notations
2020-11-09 14:37:04 +01:00
\draw[->] (U.east) -- ++( 1.0, 0) node[above left]{$y$};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
\begin{scope}[on background layer]
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[fit={(V.south west) (G.north-|U.east)}, fill=black!10!white, draw, dashed, inner sep=14pt] (Gsvd) {};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
\node[below right] at (Gsvd.north west) {$\bm{G}_{SVD}$};
\end{scope}
\end{tikzpicture}
#+end_src
#+name: fig:plant_decouple_svd
#+caption: Decoupled plant $\bm{G}_{SVD}$ using the Singular Value Decomposition
#+RESULTS:
[[file:figs/plant_decouple_svd.png]]
The decoupled plant is then:
Better Matlab notations
2020-11-09 14:37:04 +01:00
\[ G_{SVD}(s) = U^{-1} G_u(s) V^{-H} \]
#+begin_src matlab
Gsvd = inv(U)*Gu*inv(V');
#+end_src
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
** Verification of the decoupling using the "Gershgorin Radii"
<<sec:comp_decoupling>>
The "Gershgorin Radii" is computed for the coupled plant $G(s)$, for the "Jacobian plant" $G_x(s)$ and the "SVD Decoupled Plant" $G_{SVD}(s)$:
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Better Matlab notations
2020-11-09 14:37:04 +01:00
The "Gershgorin Radii" of a matrix $S$ is defined by:
\[ \zeta_i(j\omega) = \frac{\sum\limits_{j\neq i}|S_{ij}(j\omega)|}{|S_{ii}(j\omega)|} \]
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
This is computed over the following frequencies.
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+begin_src matlab :exports none
% Gershgorin Radii for the coupled plant:
Better Matlab notations
2020-11-09 14:37:04 +01:00
Gr_coupled = zeros(length(freqs), size(Gu,2));
H = abs(squeeze(freqresp(Gu, freqs, 'Hz')));
for out_i = 1:size(Gu,2)
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
end
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
% Gershgorin Radii for the decoupled plant using SVD:
Better Matlab notations
2020-11-09 14:37:04 +01:00
Gr_decoupled = zeros(length(freqs), size(Gsvd,2));
H = abs(squeeze(freqresp(Gsvd, freqs, 'Hz')));
for out_i = 1:size(Gsvd,2)
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
Gr_decoupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
% Gershgorin Radii for the decoupled plant using the Jacobian:
Better Matlab notations
2020-11-09 14:37:04 +01:00
Gr_jacobian = zeros(length(freqs), size(Gx,2));
H = abs(squeeze(freqresp(Gx, freqs, 'Hz')));
for out_i = 1:size(Gx,2)
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
Gr_jacobian(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
end
#+end_src
#+begin_src matlab :exports results
figure;
hold on;
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled');
plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD');
plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian');
Change some for loops
2020-09-21 18:57:37 +02:00
for in_i = 2:6
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
set(gca,'ColorOrderIndex',1)
Change some for loops
2020-09-21 18:57:37 +02:00
plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
set(gca,'ColorOrderIndex',2)
Change some for loops
2020-09-21 18:57:37 +02:00
plot(freqs, Gr_decoupled(:,in_i), 'HandleVisibility', 'off');
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
set(gca,'ColorOrderIndex',3)
Change some for loops
2020-09-21 18:57:37 +02:00
plot(freqs, Gr_jacobian(:,in_i), 'HandleVisibility', 'off');
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
end
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
legend('location', 'northwest');
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
ylim([1e-3, 1e3]);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
exportFig('figs/simscape_model_gershgorin_radii.pdf', 'eps', true, 'width', 'wide', 'height', 'normal');
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
#+end_src
#+name: fig:simscape_model_gershgorin_radii
#+caption: Gershgorin Radii of the Coupled and Decoupled plants
#+RESULTS:
[[file:figs/simscape_model_gershgorin_radii.png]]
Add figure about RGA
2020-11-23 18:01:13 +01:00
** Verification of the decoupling using the "Relative Gain Array"
The relative gain array (RGA) is defined as:
\begin{equation}
\Lambda\big(G(s)\big) = G(s) \times \big( G(s)^{-1} \big)^T
\end{equation}
where $\times$ denotes an element by element multiplication and $G(s)$ is an $n \times n$ square transfer matrix.
The obtained RGA elements are shown in Figure [[fig:simscape_model_rga]].
#+begin_src matlab :exports none
% Relative Gain Array for the coupled plant:
RGA_coupled = zeros(length(freqs), size(Gu,1), size(Gu,2));
Gu_inv = inv(Gu);
for f_i = 1:length(freqs)
RGA_coupled(f_i, :, :) = abs(evalfr(Gu, j*2*pi*freqs(f_i)).*evalfr(Gu_inv, j*2*pi*freqs(f_i))');
end
% Relative Gain Array for the decoupled plant using SVD:
RGA_svd = zeros(length(freqs), size(Gsvd,1), size(Gsvd,2));
Gsvd_inv = inv(Gsvd);
for f_i = 1:length(freqs)
RGA_svd(f_i, :, :) = abs(evalfr(Gsvd, j*2*pi*freqs(f_i)).*evalfr(Gsvd_inv, j*2*pi*freqs(f_i))');
end
% Relative Gain Array for the decoupled plant using the Jacobian:
RGA_x = zeros(length(freqs), size(Gx,1), size(Gx,2));
Gx_inv = inv(Gx);
for f_i = 1:length(freqs)
RGA_x(f_i, :, :) = abs(evalfr(Gx, j*2*pi*freqs(f_i)).*evalfr(Gx_inv, j*2*pi*freqs(f_i))');
end
#+end_src
#+begin_src matlab :exports none
figure;
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile;
hold on;
for i_in = 1:6
for i_out = [1:i_in-1, i_in+1:6]
plot(freqs, RGA_svd(:, i_out, i_in), '--', 'color', [0 0 0 0.2], ...
'HandleVisibility', 'off');
end
end
plot(freqs, RGA_svd(:, 1, 2), '--', 'color', [0 0 0 0.2], ...
'DisplayName', '$RGA_{SVD}(i,j),\ i \neq j$');
plot(freqs, RGA_svd(:, 1, 1), 'k-', ...
'DisplayName', '$RGA_{SVD}(i,i)$');
for ch_i = 1:6
plot(freqs, RGA_svd(:, ch_i, ch_i), 'k-', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); xlabel('Frequency [Hz]');
legend('location', 'southwest');
ax2 = nexttile;
hold on;
for i_in = 1:6
for i_out = [1:i_in-1, i_in+1:6]
plot(freqs, RGA_x(:, i_out, i_in), '--', 'color', [0 0 0 0.2], ...
'HandleVisibility', 'off');
end
end
plot(freqs, RGA_x(:, 1, 2), '--', 'color', [0 0 0 0.2], ...
'DisplayName', '$RGA_{X}(i,j),\ i \neq j$');
plot(freqs, RGA_x(:, 1, 1), 'k-', ...
'DisplayName', '$RGA_{X}(i,i)$');
for ch_i = 1:6
plot(freqs, RGA_x(:, ch_i, ch_i), 'k-', ...
'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]);
legend('location', 'southwest');
linkaxes([ax1,ax2],'y');
ylim([1e-5, 1e1]);
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/simscape_model_rga.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:simscape_model_rga
#+caption: Obtained norm of RGA elements for the SVD decoupled plant and the Jacobian decoupled plant
#+RESULTS:
[[file:figs/simscape_model_rga.png]]
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
** Obtained Decoupled Plants
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
<<sec:stewart_decoupled_plant>>
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
The bode plot of the diagonal and off-diagonal elements of $G_{SVD}$ are shown in Figure [[fig:simscape_model_decoupled_plant_svd]].
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+begin_src matlab :exports none
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
figure;
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile([2, 1]);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold on;
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
for i_in = 1:6
for i_out = [1:i_in-1, i_in+1:6]
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
'HandleVisibility', 'off');
end
end
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
'DisplayName', '$G_{SVD}(i,j),\ i \neq j$');
set(gca,'ColorOrderIndex',1)
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
for ch_i = 1:6
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, abs(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))), ...
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
'DisplayName', sprintf('$G_{SVD}(%i,%i)$', ch_i, ch_i));
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
end
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
legend('location', 'northwest');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
ylim([1e-1, 1e5])
% Phase
ax2 = nexttile;
hold on;
for ch_i = 1:6
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))));
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180:90:360]);
linkaxes([ax1,ax2],'x');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
exportFig('figs/simscape_model_decoupled_plant_svd.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
#+end_src
#+name: fig:simscape_model_decoupled_plant_svd
#+caption: Decoupled Plant using SVD
#+RESULTS:
[[file:figs/simscape_model_decoupled_plant_svd.png]]
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
Similarly, the bode plots of the diagonal elements and off-diagonal elements of the decoupled plant $G_x(s)$ using the Jacobian are shown in Figure [[fig:simscape_model_decoupled_plant_jacobian]].
#+begin_src matlab :exports none
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
figure;
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile([2, 1]);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold on;
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
for i_in = 1:6
for i_out = [1:i_in-1, i_in+1:6]
Better Matlab notations
2020-11-09 14:37:04 +01:00
plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
'HandleVisibility', 'off');
end
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
end
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
plot(freqs, abs(squeeze(freqresp(Gx(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
'DisplayName', '$G_x(i,j),\ i \neq j$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))), 'DisplayName', '$G_x(1,1) = A_x/F_x$');
plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))), 'DisplayName', '$G_x(2,2) = A_y/F_y$');
plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))), 'DisplayName', '$G_x(3,3) = A_z/F_z$');
plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))), 'DisplayName', '$G_x(4,4) = A_{R_x}/M_x$');
plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))), 'DisplayName', '$G_x(5,5) = A_{R_y}/M_y$');
plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))), 'DisplayName', '$G_x(6,6) = A_{R_z}/M_z$');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
legend('location', 'northwest');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
ylim([1e-2, 2e6])
% Phase
ax2 = nexttile;
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([0, 180]);
yticks([0:45:360]);
linkaxes([ax1,ax2],'x');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
#+begin_src matlab :tangle no :exports results :results file replace
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
exportFig('figs/simscape_model_decoupled_plant_jacobian.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
#+end_src
#+name: fig:simscape_model_decoupled_plant_jacobian
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
#+caption: Stewart Platform Plant from forces (resp. torques) applied by the legs to the acceleration (resp. angular acceleration) of the platform as well as all the coupling terms between the two (non-diagonal terms of the transfer function matrix)
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
#+RESULTS:
[[file:figs/simscape_model_decoupled_plant_jacobian.png]]
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
** Diagonal Controller
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
<<sec:stewart_diagonal_control>>
The control diagram for the centralized control is shown in Figure [[fig:centralized_control]].
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
The controller $K_c$ is "working" in an cartesian frame.
The Jacobian is used to convert forces in the cartesian frame to forces applied by the actuators.
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+begin_src latex :file centralized_control.pdf :tangle no :exports results
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
\begin{tikzpicture}
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[block={2cm}{1.5cm}] (G) {$\begin{bmatrix}G_d\\G_u\end{bmatrix}$};
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
\node[above] at (G.north) {$\bm{G}$};
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
\node[block, below right=0.6 and -0.5 of G] (K) {$K_c$};
\node[block, below left= 0.6 and -0.5 of G] (J) {$J^{-T}$};
% Inputs of the controllers
\coordinate[] (inputd) at ($(G.south west)!0.75!(G.north west)$);
\coordinate[] (inputu) at ($(G.south west)!0.25!(G.north west)$);
% Connections and labels
\draw[<-] (inputd) -- ++(-0.8, 0) node[above right]{$D_w$};
\draw[->] (G.east) -- ++(2.0, 0) node[above left]{$a$};
\draw[->] ($(G.east)+(1.4, 0)$)node[branch]{} |- (K.east);
\draw[->] (K.west) -- (J.east) node[above right]{$\mathcal{F}$};
\draw[->] (J.west) -- ++(-0.6, 0) |- (inputu) node[above left]{$\tau$};
\end{tikzpicture}
#+end_src
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+name: fig:centralized_control
#+caption: Control Diagram for the Centralized control
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+RESULTS:
[[file:figs/centralized_control.png]]
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
The SVD control architecture is shown in Figure [[fig:svd_control]].
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
The matrices $U$ and $V$ are used to decoupled the plant $G$.
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+begin_src latex :file svd_control.pdf :tangle no :exports results
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
\begin{tikzpicture}
Better Matlab notations
2020-11-09 14:37:04 +01:00
\node[block={2cm}{1.5cm}] (G) {$\begin{bmatrix}G_d\\G_u\end{bmatrix}$};
\node[above] at (G.north) {$\bm{G}$};
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
\node[block, below right=0.6 and 0 of G] (U) {$U^{-1}$};
\node[block, below=0.6 of G] (K) {$K_{\text{SVD}}$};
\node[block, below left= 0.6 and 0 of G] (V) {$V^{-T}$};
% Inputs of the controllers
\coordinate[] (inputd) at ($(G.south west)!0.75!(G.north west)$);
\coordinate[] (inputu) at ($(G.south west)!0.25!(G.north west)$);
% Connections and labels
\draw[<-] (inputd) -- ++(-0.8, 0) node[above right]{$D_w$};
\draw[->] (G.east) -- ++(2.5, 0) node[above left]{$a$};
\draw[->] ($(G.east)+(2.0, 0)$) node[branch]{} |- (U.east);
\draw[->] (U.west) -- (K.east);
\draw[->] (K.west) -- (V.east);
\draw[->] (V.west) -- ++(-0.6, 0) |- (inputu) node[above left]{$\tau$};
\end{tikzpicture}
#+end_src
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
#+name: fig:svd_control
#+caption: Control Diagram for the SVD control
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+RESULTS:
[[file:figs/svd_control.png]]
Change the controller tuning for Stewart platform
2020-11-06 18:00:30 +01:00
We choose the controller to be a low pass filter:
\[ K_c(s) = \frac{G_0}{1 + \frac{s}{\omega_0}} \]
$G_0$ is tuned such that the crossover frequency corresponding to the diagonal terms of the loop gain is equal to $\omega_c$
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+begin_src matlab
Better Matlab notations
2020-11-09 14:37:04 +01:00
wc = 2*pi*80; % Crossover Frequency [rad/s]
w0 = 2*pi*0.1; % Controller Pole [rad/s]
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
Change the controller tuning for Stewart platform
2020-11-06 18:00:30 +01:00
#+begin_src matlab
Better Matlab notations
2020-11-09 14:37:04 +01:00
K_cen = diag(1./diag(abs(evalfr(Gx, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0);
L_cen = K_cen*Gx;
Change the controller tuning for Stewart platform
2020-11-06 18:00:30 +01:00
G_cen = feedback(G, pinv(J')*K_cen, [7:12], [1:6]);
#+end_src
Add screenshots of the Simscape Model
2020-11-06 17:02:58 +01:00
#+begin_src matlab
Better Matlab notations
2020-11-09 14:37:04 +01:00
K_svd = diag(1./diag(abs(evalfr(Gsvd, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0);
L_svd = K_svd*Gsvd;
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
G_svd = feedback(G, inv(V')*K_svd*inv(U), [7:12], [1:6]);
Add screenshots of the Simscape Model
2020-11-06 17:02:58 +01:00
#+end_src
Change the controller tuning for Stewart platform
2020-11-06 18:00:30 +01:00
The obtained diagonal elements of the loop gains are shown in Figure [[fig:stewart_comp_loop_gain_diagonal]].
#+begin_src matlab :exports none
figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude
ax1 = nexttile([2, 1]);
hold on;
plot(freqs, abs(squeeze(freqresp(L_svd(1, 1), freqs, 'Hz'))), 'DisplayName', '$L_{SVD}(i,i)$');
for i_in_out = 2:6
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
end
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(L_cen(1, 1), freqs, 'Hz'))), ...
'DisplayName', '$L_{J}(i,i)$');
for i_in_out = 2:6
set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
legend('location', 'northwest');
ylim([5e-2, 2e3])
% Phase
ax2 = nexttile;
hold on;
for i_in_out = 1:6
set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',2)
for i_in_out = 1:6
set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*angle(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180:90:360]);
linkaxes([ax1,ax2],'x');
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/stewart_comp_loop_gain_diagonal.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:stewart_comp_loop_gain_diagonal
#+caption: Comparison of the diagonal elements of the loop gains for the SVD control architecture and the Jacobian one
#+RESULTS:
[[file:figs/stewart_comp_loop_gain_diagonal.png]]
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
** Closed-Loop system Performances
<<sec:stewart_closed_loop_results>>
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
Let's first verify the stability of the closed-loop systems:
#+begin_src matlab :results output replace text
isstable(G_cen)
#+end_src
#+RESULTS:
: ans =
: logical
: 1
#+begin_src matlab :results output replace text
isstable(G_svd)
#+end_src
#+RESULTS:
: ans =
: logical
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
: 1
Correct wrong computation of Gershgorin radii
2020-09-21 18:54:41 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure [[fig:stewart_platform_simscape_cl_transmissibility]].
#+begin_src matlab :exports results
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
figure;
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
ax1 = nexttile;
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
plot(freqs, abs(squeeze(freqresp(G_cen('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
plot(freqs, abs(squeeze(freqresp(G_svd('Ax', 'Dwx')/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G( 'Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_cen('Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off');
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
plot(freqs, abs(squeeze(freqresp(G_svd('Ay', 'Dwy')/s^2, freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
ylabel('$D_x/D_{w,x}$, $D_y/D_{w, y}$'); set(gca, 'XTickLabel',[]);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
legend('location', 'southwest');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
ax2 = nexttile;
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Az', 'Dwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Az', 'Dwz')/s^2, freqs, 'Hz'))));
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
plot(freqs, abs(squeeze(freqresp(G_svd('Az', 'Dwz')/s^2, freqs, 'Hz'))), '--');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
ylabel('$D_z/D_{w,z}$'); set(gca, 'XTickLabel',[]);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
ax3 = nexttile;
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arx', 'Rwx')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arx', 'Rwx')/s^2, freqs, 'Hz'))));
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
plot(freqs, abs(squeeze(freqresp(G_svd('Arx', 'Rwx')/s^2, freqs, 'Hz'))), '--');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G( 'Ary', 'Rwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Ary', 'Rwy')/s^2, freqs, 'Hz'))));
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
plot(freqs, abs(squeeze(freqresp(G_svd('Ary', 'Rwy')/s^2, freqs, 'Hz'))), '--');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
ylabel('$R_x/R_{w,x}$, $R_y/R_{w,y}$'); xlabel('Frequency [Hz]');
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
ax4 = nexttile;
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arz', 'Rwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arz', 'Rwz')/s^2, freqs, 'Hz'))));
Update wrong formula for the closed-loop system
2020-11-09 10:54:46 +01:00
plot(freqs, abs(squeeze(freqresp(G_svd('Arz', 'Rwz')/s^2, freqs, 'Hz'))), '--');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
ylabel('$R_z/R_{w,z}$'); xlabel('Frequency [Hz]');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Rework some figures + add some control diagrams
2020-11-06 16:59:03 +01:00
linkaxes([ax1,ax2,ax3,ax4],'xy');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
xlim([freqs(1), freqs(end)]);
Change the controller tuning for Stewart platform
2020-11-06 18:00:30 +01:00
ylim([1e-3, 1e2]);
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
Update Stewart Platform figures
2020-11-06 12:22:37 +01:00
exportFig('figs/stewart_platform_simscape_cl_transmissibility.pdf', 'eps', true, 'width', 'wide', 'height', 'tall');
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
#+name: fig:stewart_platform_simscape_cl_transmissibility
#+caption: Obtained Transmissibility
#+RESULTS:
[[file:figs/stewart_platform_simscape_cl_transmissibility.png]]
Add figure about RGA
2020-11-23 18:01:13 +01:00
** Small error on the sensor location :no_export:
Let's now consider a small position error of the sensor:
#+begin_src matlab
sens_pos_error = [105 5 -1]*1e-3; % [m]
#+end_src
The system is identified again:
#+begin_src matlab :exports none
%% Name of the Simulink File
mdl = 'drone_platform';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Dw'], 1, 'openinput'); io_i = io_i + 1; % Ground Motion
io(io_i) = linio([mdl, '/V-T'], 1, 'openinput'); io_i = io_i + 1; % Actuator Forces
io(io_i) = linio([mdl, '/Inertial Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Top platform acceleration
G = linearize(mdl, io);
G.InputName = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'};
% Plant
Gu = G(:, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'});
% Disturbance dynamics
Gd = G(:, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'});
#+end_src
#+begin_src matlab
Gx = Gu*inv(J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src
#+begin_src matlab
Gsvd = inv(U)*Gu*inv(V');
#+end_src
#+begin_src matlab :exports none
% Gershgorin Radii for the coupled plant:
Gr_coupled = zeros(length(freqs), size(Gu,2));
H = abs(squeeze(freqresp(Gu, freqs, 'Hz')));
for out_i = 1:size(Gu,2)
Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
% Gershgorin Radii for the decoupled plant using SVD:
Gr_decoupled = zeros(length(freqs), size(Gsvd,2));
H = abs(squeeze(freqresp(Gsvd, freqs, 'Hz')));
for out_i = 1:size(Gsvd,2)
Gr_decoupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
% Gershgorin Radii for the decoupled plant using the Jacobian:
Gr_jacobian = zeros(length(freqs), size(Gx,2));
H = abs(squeeze(freqresp(Gx, freqs, 'Hz')));
for out_i = 1:size(Gx,2)
Gr_jacobian(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
#+end_src
#+begin_src matlab :exports results
figure;
hold on;
plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled');
plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD');
plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian');
for in_i = 2:6
set(gca,'ColorOrderIndex',1)
plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2)
plot(freqs, Gr_decoupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',3)
plot(freqs, Gr_jacobian(:,in_i), 'HandleVisibility', 'off');
end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
legend('location', 'northwest');
ylim([1e-3, 1e3]);
#+end_src
#+begin_src matlab
L_cen = K_cen*Gx;
G_cen = feedback(G, pinv(J')*K_cen, [7:12], [1:6]);
#+end_src
#+begin_src matlab
L_svd = K_svd*Gsvd;
G_svd = feedback(G, inv(V')*K_svd*inv(U), [7:12], [1:6]);
#+end_src
#+begin_src matlab :results output replace text
isstable(G_cen)
#+end_src
#+begin_src matlab :results output replace text
isstable(G_svd)
#+end_src
#+begin_src matlab :exports results
figure;
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
plot(freqs, abs(squeeze(freqresp(G_cen('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
plot(freqs, abs(squeeze(freqresp(G_svd('Ax', 'Dwx')/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G( 'Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_cen('Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'HandleVisibility', 'off');
plot(freqs, abs(squeeze(freqresp(G_svd('Ay', 'Dwy')/s^2, freqs, 'Hz'))), '--', 'HandleVisibility', 'off');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$D_x/D_{w,x}$, $D_y/D_{w, y}$'); set(gca, 'XTickLabel',[]);
legend('location', 'southwest');
ax2 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Az', 'Dwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Az', 'Dwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Az', 'Dwz')/s^2, freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$D_z/D_{w,z}$'); set(gca, 'XTickLabel',[]);
ax3 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arx', 'Rwx')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arx', 'Rwx')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Arx', 'Rwx')/s^2, freqs, 'Hz'))), '--');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(G( 'Ary', 'Rwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Ary', 'Rwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Ary', 'Rwy')/s^2, freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$R_x/R_{w,x}$, $R_y/R_{w,y}$'); xlabel('Frequency [Hz]');
ax4 = nexttile;
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arz', 'Rwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arz', 'Rwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Arz', 'Rwz')/s^2, freqs, 'Hz'))), '--');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$R_z/R_{w,z}$'); xlabel('Frequency [Hz]');
linkaxes([ax1,ax2,ax3,ax4],'xy');
xlim([freqs(1), freqs(end)]);
ylim([1e-3, 1e2]);
#+end_src
Add picture of platform and sections summary
2020-11-06 11:59:09 +01:00
* Stewart Platform - Analytical Model :noexport:
Remove Gravity for the Stewart platform model
2020-10-13 15:01:42 +02:00
:PROPERTIES:
:header-args:matlab+: :tangle stewart_platform/analytical_model.m
:END:
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
%% Bode plot options
opts = bodeoptions('cstprefs');
opts.FreqUnits = 'Hz';
opts.MagUnits = 'abs';
opts.MagScale = 'log';
opts.PhaseWrapping = 'on';
opts.xlim = [1 1000];
#+end_src
** Characteristics
#+begin_src matlab
Tangle scripts
2020-10-13 14:51:15 +02:00
L = 0.055; % Leg length [m]
Zc = 0; % ?
m = 0.2; % Top platform mass [m]
k = 1e3; % Total vertical stiffness [N/m]
c = 2*0.1*sqrt(k*m); % Damping ? [N/(m/s)]
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Tangle scripts
2020-10-13 14:51:15 +02:00
Rx = 0.04; % ?
Rz = 0.04; % ?
Ix = m*Rx^2; % ?
Iy = m*Rx^2; % ?
Iz = m*Rz^2; % ?
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
** Mass Matrix
#+begin_src matlab
Tangle scripts
2020-10-13 14:51:15 +02:00
M = m*[1 0 0 0 Zc 0;
0 1 0 -Zc 0 0;
0 0 1 0 0 0;
0 -Zc 0 Rx^2+Zc^2 0 0;
Zc 0 0 0 Rx^2+Zc^2 0;
0 0 0 0 0 Rz^2];
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
** Jacobian Matrix
#+begin_src matlab
Tangle scripts
2020-10-13 14:51:15 +02:00
Bj=1/sqrt(6)*[ 1 1 -2 1 1 -2;
sqrt(3) -sqrt(3) 0 sqrt(3) -sqrt(3) 0;
sqrt(2) sqrt(2) sqrt(2) sqrt(2) sqrt(2) sqrt(2);
0 0 L L -L -L;
-L*2/sqrt(3) -L*2/sqrt(3) L/sqrt(3) L/sqrt(3) L/sqrt(3) L/sqrt(3);
L*sqrt(2) -L*sqrt(2) L*sqrt(2) -L*sqrt(2) L*sqrt(2) -L*sqrt(2)];
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
Tangle scripts
2020-10-13 14:51:15 +02:00
** Stifnness and Damping matrices
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+begin_src matlab
Tangle scripts
2020-10-13 14:51:15 +02:00
kv = k/3; % Vertical Stiffness of the springs [N/m]
kh = 0.5*k/3; % Horizontal Stiffness of the springs [N/m]
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Tangle scripts
2020-10-13 14:51:15 +02:00
K = diag([3*kh, 3*kh, 3*kv, 3*kv*Rx^2/2, 3*kv*Rx^2/2, 3*kh*Rx^2]); % Stiffness Matrix
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
C = c*K/100000; % Damping Matrix
#+end_src
** State Space System
#+begin_src matlab
Tangle scripts
2020-10-13 14:51:15 +02:00
A = [ zeros(6) eye(6); ...
-M\K -M\C];
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Bw = [zeros(6); -eye(6)];
Bu = [zeros(6); M\Bj];
Tangle scripts
2020-10-13 14:51:15 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
Co = [-M\K -M\C];
Tangle scripts
2020-10-13 14:51:15 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
D = [zeros(6) M\Bj];
ST = ss(A,[Bw Bu],Co,D);
#+end_src
- OUT 1-6: 6 dof
- IN 1-6 : ground displacement in the directions of the legs
- IN 7-12: forces in the actuators.
#+begin_src matlab
ST.StateName = {'x';'y';'z';'theta_x';'theta_y';'theta_z';...
'dx';'dy';'dz';'dtheta_x';'dtheta_y';'dtheta_z'};
Tangle scripts
2020-10-13 14:51:15 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
ST.InputName = {'w1';'w2';'w3';'w4';'w5';'w6';...
'u1';'u2';'u3';'u4';'u5';'u6'};
Tangle scripts
2020-10-13 14:51:15 +02:00
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
ST.OutputName = {'ax';'ay';'az';'atheta_x';'atheta_y';'atheta_z'};
#+end_src
** Transmissibility
#+begin_src matlab
TR=ST*[eye(6); zeros(6)];
#+end_src
#+begin_src matlab
figure
subplot(231)
Tangle scripts
2020-10-13 14:51:15 +02:00
bodemag(TR(1,1));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
subplot(232)
Tangle scripts
2020-10-13 14:51:15 +02:00
bodemag(TR(2,2));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
subplot(233)
Tangle scripts
2020-10-13 14:51:15 +02:00
bodemag(TR(3,3));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
subplot(234)
Tangle scripts
2020-10-13 14:51:15 +02:00
bodemag(TR(4,4));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
subplot(235)
Tangle scripts
2020-10-13 14:51:15 +02:00
bodemag(TR(5,5));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
subplot(236)
Tangle scripts
2020-10-13 14:51:15 +02:00
bodemag(TR(6,6));
Analysis + Control of Simscape Model
2020-09-21 18:03:40 +02:00
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/stewart_platform_analytical_transmissibility.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:stewart_platform_analytical_transmissibility
#+caption: Transmissibility
#+RESULTS:
[[file:figs/stewart_platform_analytical_transmissibility.png]]
** Real approximation of $G(j\omega)$ at decoupling frequency
#+begin_src matlab
sys1 = ST*[zeros(6); eye(6)]; % take only the forces inputs
dec_fr = 20;
H1 = evalfr(sys1,j*2*pi*dec_fr);
H2 = H1;
D = pinv(real(H2'*H2));
H1 = inv(D*real(H2'*diag(exp(j*angle(diag(H2*D*H2.'))/2)))) ;
[U,S,V] = svd(H1);
wf = logspace(-1,2,1000);
for i = 1:length(wf)
H = abs(evalfr(sys1,j*2*pi*wf(i)));
H_dec = abs(evalfr(U'*sys1*V,j*2*pi*wf(i)));
for j = 1:size(H,2)
g_r1(i,j) = (sum(H(j,:))-H(j,j))/H(j,j);
g_r2(i,j) = (sum(H_dec(j,:))-H_dec(j,j))/H_dec(j,j);
% keyboard
end
g_lim(i) = 0.5;
end
#+end_src
** Coupled and Decoupled Plant "Gershgorin Radii"
#+begin_src matlab
figure;
title('Coupled plant')
loglog(wf,g_r1(:,1),wf,g_r1(:,2),wf,g_r1(:,3),wf,g_r1(:,4),wf,g_r1(:,5),wf,g_r1(:,6),wf,g_lim,'--');
legend('$a_x$','$a_y$','$a_z$','$\theta_x$','$\theta_y$','$\theta_z$','Limit');
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/gershorin_raddii_coupled_analytical.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:gershorin_raddii_coupled_analytical
#+caption: Gershorin Raddi for the coupled plant
#+RESULTS:
[[file:figs/gershorin_raddii_coupled_analytical.png]]
#+begin_src matlab
figure;
title('Decoupled plant (10 Hz)')
loglog(wf,g_r2(:,1),wf,g_r2(:,2),wf,g_r2(:,3),wf,g_r2(:,4),wf,g_r2(:,5),wf,g_r2(:,6),wf,g_lim,'--');
legend('$S_1$','$S_2$','$S_3$','$S_4$','$S_5$','$S_6$','Limit');
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/gershorin_raddii_decoupled_analytical.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:gershorin_raddii_decoupled_analytical
#+caption: Gershorin Raddi for the decoupled plant
#+RESULTS:
[[file:figs/gershorin_raddii_decoupled_analytical.png]]
** Decoupled Plant
#+begin_src matlab
figure;
bodemag(U'*sys1*V,opts)
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/stewart_platform_analytical_decoupled_plant.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:stewart_platform_analytical_decoupled_plant
#+caption: Decoupled Plant
#+RESULTS:
[[file:figs/stewart_platform_analytical_decoupled_plant.png]]
** Controller
#+begin_src matlab
fc = 2*pi*0.1; % Crossover Frequency [rad/s]
c_gain = 50; %
cont = eye(6)*c_gain/(s+fc);
#+end_src
** Closed Loop System
#+begin_src matlab
FEEDIN = [7:12]; % Input of controller
FEEDOUT = [1:6]; % Output of controller
#+end_src
Centralized Control
#+begin_src matlab
STcen = feedback(ST, inv(Bj)*cont, FEEDIN, FEEDOUT);
TRcen = STcen*[eye(6); zeros(6)];
#+end_src
SVD Control
#+begin_src matlab
STsvd = feedback(ST, pinv(V')*cont*pinv(U), FEEDIN, FEEDOUT);
TRsvd = STsvd*[eye(6); zeros(6)];
#+end_src
** Results
#+begin_src matlab
figure
subplot(231)
bodemag(TR(1,1),TRcen(1,1),TRsvd(1,1),opts)
legend('OL','Centralized','SVD')
subplot(232)
bodemag(TR(2,2),TRcen(2,2),TRsvd(2,2),opts)
legend('OL','Centralized','SVD')
subplot(233)
bodemag(TR(3,3),TRcen(3,3),TRsvd(3,3),opts)
legend('OL','Centralized','SVD')
subplot(234)
bodemag(TR(4,4),TRcen(4,4),TRsvd(4,4),opts)
legend('OL','Centralized','SVD')
subplot(235)
bodemag(TR(5,5),TRcen(5,5),TRsvd(5,5),opts)
legend('OL','Centralized','SVD')
subplot(236)
bodemag(TR(6,6),TRcen(6,6),TRsvd(6,6),opts)
legend('OL','Centralized','SVD')
#+end_src
#+begin_src matlab :tangle no :exports results :results file replace
exportFig('figs/stewart_platform_analytical_svd_cen_comp.pdf', 'width', 'full', 'height', 'full');
#+end_src
#+name: fig:stewart_platform_analytical_svd_cen_comp
#+caption: Comparison of the obtained transmissibility for the centralized control and the SVD control
#+RESULTS:
[[file:figs/stewart_platform_analytical_svd_cen_comp.png]]
Reference in New Issue Copy Permalink