tdehaeze/svd-control
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Clean simscape models

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-11-25 09:40:17 +01:00
parent 9b29c73fff
commit a1581cb873
17 changed files with 2851 additions and 1261 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
figs/gravimeter_gershgorin_radii.eps
View File

Binary file not shown.

BIN
figs/gravimeter_gershgorin_radii.pdf
View File

Binary file not shown.

BIN
figs/gravimeter_gershgorin_radii.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 124 KiB

After

Width:  |  Height:  |  Size: 115 KiB

2223
figs/gravimeter_rga.pdf Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
figs/gravimeter_rga.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 164 KiB

15
gravimeter/align.m
View File

@ -1,15 +0,0 @@
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
end
D = pinv(real(V'*V));
A = D*real(V'*diag(exp(1i * angle(diag(V*D*V.'))/2)));
end

34
gravimeter/pzmap_testCL.m
View File

@ -1,34 +0,0 @@
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

327
gravimeter/script.m
View File

@ -4,13 +4,23 @@ clear; close all; clc;
%% Intialize Laplace variable %% Intialize Laplace variable
s = zpk('s'); s = zpk('s');
% Simscape Model - Parameters freqs = logspace(-1, 2, 1000);
% Gravimeter Model - Parameters
% <<sec:gravimeter_model>>
open('gravimeter.slx') open('gravimeter.slx')
% Parameters % The model of the gravimeter is schematically shown in Figure [[fig:gravimeter_model]].
% #+name: fig:gravimeter_model
% #+caption: Model of the gravimeter
% [[file:figs/gravimeter_model.png]]
% The parameters used for the simulation are the following:
l = 1.0; % Length of the mass [m] l = 1.0; % Length of the mass [m]
h = 1.7; % Height of the mass [m] h = 1.7; % Height of the mass [m]
@ -22,13 +32,15 @@ m = 400; % Mass [kg]
I = 115; % Inertia [kg m^2] I = 115; % Inertia [kg m^2]
k = 15e3; % Actuator Stiffness [N/m] k = 15e3; % Actuator Stiffness [N/m]
c = 0.03; % Actuator Damping [N/(m/s)] c = 2e1; % Actuator Damping [N/(m/s)]
deq = 0.2; % Length of the actuators [m] deq = 0.2; % Length of the actuators [m]
g = 0; % Gravity [m/s2] g = 0; % Gravity [m/s2]
% System Identification - Without Gravity % System Identification
% <<sec:gravimeter_identification>>
%% Name of the Simulink File %% Name of the Simulink File
mdl = 'gravimeter'; mdl = 'gravimeter';
@ -54,32 +66,21 @@ G.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'};
% #+RESULTS: % #+RESULTS:
% [[file:figs/gravimeter_plant_schematic.png]] % [[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: % We can check the poles of the plant:
pole(G) pole(G)
% #+RESULTS: % #+RESULTS:
% #+begin_example % | -0.12243+13.551i |
% -0.000183495485977108 + 13.546056874877i % | -0.12243-13.551i |
% -0.000183495485977108 - 13.546056874877i % | -0.05+8.6601i |
% -7.49842878906757e-05 + 8.65934902322567i % | -0.05-8.6601i |
% -7.49842878906757e-05 - 8.65934902322567i % | -0.0088785+3.6493i |
% -1.33171230256362e-05 + 3.64924169037897i % | -0.0088785-3.6493i |
% -1.33171230256362e-05 - 3.64924169037897i
% #+end_example
% The plant as 6 states as expected (2 translations + 1 rotation) % As expected, the plant as 6 states (2 translations + 1 rotation)
size(G) size(G)
@ -91,8 +92,6 @@ size(G)
% The bode plot of all elements of the plant are shown in Figure [[fig:open_loop_tf]]. % The bode plot of all elements of the plant are shown in Figure [[fig:open_loop_tf]].
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(4, 3, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(4, 3, 'TileSpacing', 'None', 'Padding', 'None');
@ -124,7 +123,7 @@ end
% #+RESULTS: % #+RESULTS:
% [[file:figs/gravimeter_decouple_jacobian.png]] % [[file:figs/gravimeter_decouple_jacobian.png]]
% The jacobian corresponding to the sensors and actuators are defined below. % The Jacobian corresponding to the sensors and actuators are defined below:
Ja = [1 0 h/2 Ja = [1 0 h/2
0 1 -l/2 0 1 -l/2
@ -135,17 +134,25 @@ Jt = [1 0 ha
0 1 -la 0 1 -la
0 1 la]; 0 1 la];
% And the plant $\bm{G}_x$ is computed:
Gx = pinv(Ja)*G*pinv(Jt'); Gx = pinv(Ja)*G*pinv(Jt');
Gx.InputName = {'Fx', 'Fz', 'My'}; Gx.InputName = {'Fx', 'Fz', 'My'};
Gx.OutputName = {'Dx', 'Dz', 'Ry'}; Gx.OutputName = {'Dx', 'Dz', 'Ry'};
size(Gx)
% #+RESULTS:
% : size(Gx)
% : State-space model with 3 outputs, 3 inputs, and 6 states.
% The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravimeter_jacobian_plant]]. % The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravimeter_jacobian_plant]].
freqs = logspace(-1, 2, 1000);
figure; figure;
% Magnitude % Magnitude
@ -168,10 +175,12 @@ xlabel('Frequency [Hz]'); ylabel('Magnitude');
legend('location', 'southeast'); legend('location', 'southeast');
ylim([1e-8, 1e0]); ylim([1e-8, 1e0]);
% Real Approximation of $G$ at the decoupling frequency % Decoupling using the SVD
% <<sec:gravimeter_real_approx>> % <<sec:gravimeter_svd_decoupling>>
% 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$. % In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required.
% Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G(j\omega_c)$ from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency $\omega_c$.
wc = 2*pi*10; % Decoupling frequency [rad/s] wc = 2*pi*10; % Decoupling frequency [rad/s]
@ -182,16 +191,23 @@ H1 = evalfr(G, j*wc);
% The real approximation is computed as follows: % The real approximation is computed as follows:
D = pinv(real(H1'*H1)); D = pinv(real(H1'*H1));
H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2)))); H1 = pinv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
% SVD Decoupling
% <<sec:gravimeter_svd_decoupling>>
% First, the Singular Value Decomposition of $H_1$ is performed:
% #+caption: Real approximate of $G$ at the decoupling frequency $\omega_c$
% #+RESULTS:
% | 0.0092 | -0.0039 | 0.0039 |
% | -0.0039 | 0.0048 | 0.00028 |
% | -0.004 | 0.0038 | -0.0038 |
% | 8.4e-09 | 0.0025 | 0.0025 |
% Now, the Singular Value Decomposition of $H_1$ is performed:
% \[ H_1 = U \Sigma V^H \] % \[ H_1 = U \Sigma V^H \]
[U,~,V] = svd(H1); [U,S,V] = svd(H1);
@ -201,18 +217,27 @@ H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
% [[file:figs/gravimeter_decouple_svd.png]] % [[file:figs/gravimeter_decouple_svd.png]]
% The decoupled plant is then: % The decoupled plant is then:
% \[ G_{SVD}(s) = U^{-1} G_u(s) V^{-H} \] % \[ \bm{G}_{SVD}(s) = U^{-1} \bm{G}(s) V^{-H} \]
Gsvd = inv(U)*G*inv(V'); Gsvd = inv(U)*G*inv(V');
size(Gsvd)
% #+RESULTS:
% : size(Gsvd)
% : State-space model with 4 outputs, 3 inputs, and 6 states.
% The 4th output (corresponding to the null singular value) is discarded, and we only keep the $3 \times 3$ plant:
Gsvd = Gsvd(1:3, 1:3);
% The diagonal and off-diagonal elements of the "SVD" plant are shown in Figure [[fig:gravimeter_svd_plant]]. % The diagonal and off-diagonal elements of the "SVD" plant are shown in Figure [[fig:gravimeter_svd_plant]].
freqs = logspace(-1, 2, 1000);
figure; figure;
% Magnitude % Magnitude
@ -232,25 +257,22 @@ end
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]'); ylabel('Magnitude'); xlabel('Frequency [Hz]'); ylabel('Magnitude');
legend('location', 'southeast', 'FontSize', 8); legend('location', 'southwest', 'FontSize', 8);
ylim([1e-8, 1e0]); ylim([1e-8, 1e0]);
% TODO Verification of the decoupling using the "Gershgorin Radii" % Verification of the decoupling using the "Gershgorin Radii"
% <<sec:comp_decoupling>> % <<sec:gravimeter_gershgorin_radii>>
% 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" 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: % 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)|} \] % \[ \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.
freqs = logspace(-2, 2, 1000); % [Hz]
% Gershgorin Radii for the coupled plant: % Gershgorin Radii for the coupled plant:
Gr_coupled = zeros(length(freqs), size(Gu,2)); Gr_coupled = zeros(length(freqs), size(G,2));
H = abs(squeeze(freqresp(Gu, freqs, 'Hz'))); H = abs(squeeze(freqresp(G, freqs, 'Hz')));
for out_i = 1:size(Gu,2) for out_i = 1:size(G,2)
Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :)); Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end end
@ -273,7 +295,7 @@ hold on;
plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled'); plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled');
plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD'); plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD');
plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian'); plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian');
for in_i = 2:6 for in_i = 2:3
set(gca,'ColorOrderIndex',1) set(gca,'ColorOrderIndex',1)
plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off'); plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2) set(gca,'ColorOrderIndex',2)
@ -284,25 +306,99 @@ end
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off; hold off;
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii') xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
legend('location', 'northwest'); legend('location', 'southwest');
ylim([1e-3, 1e3]); ylim([1e-4, 1e2]);
% TODO Obtained Decoupled Plants % Verification of the decoupling using the "Relative Gain Array"
% <<sec:gravimeter_rga>>
% 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:gravimeter_rga]].
% 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
figure;
tiledlayout(1, 2, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile;
hold on;
for i_in = 1:3
for i_out = [1:i_in-1, i_in+1:3]
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:3
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:3
for i_out = [1:i_in-1, i_in+1:3]
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:3
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]);
% Obtained Decoupled Plants
% <<sec:gravimeter_decoupled_plant>> % <<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]]. % The bode plot of the diagonal and off-diagonal elements of $G_{SVD}$ are shown in Figure [[fig:gravimeter_decoupled_plant_svd]].
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
for i_in = 1:6 for i_in = 1:3
for i_out = [1:i_in-1, i_in+1:6] 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], ... plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
@ -310,20 +406,20 @@ end
plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ... plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
'DisplayName', '$G_{SVD}(i,j),\ i \neq j$'); 'DisplayName', '$G_{SVD}(i,j),\ i \neq j$');
set(gca,'ColorOrderIndex',1) set(gca,'ColorOrderIndex',1)
for ch_i = 1:6 for ch_i = 1:3
plot(freqs, abs(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))), ... plot(freqs, abs(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))), ...
'DisplayName', sprintf('$G_{SVD}(%i,%i)$', ch_i, ch_i)); 'DisplayName', sprintf('$G_{SVD}(%i,%i)$', ch_i, ch_i));
end end
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]); ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
legend('location', 'northwest'); legend('location', 'southwest');
ylim([1e-1, 1e5]) ylim([1e-8, 1e0])
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
for ch_i = 1:6 for ch_i = 1:3
plot(freqs, 180/pi*angle(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))));
end end
hold off; hold off;
@ -336,24 +432,22 @@ linkaxes([ax1,ax2],'x');
% #+name: fig:simscape_model_decoupled_plant_svd % #+name: fig:gravimeter_decoupled_plant_svd
% #+caption: Decoupled Plant using SVD % #+caption: Decoupled Plant using SVD
% #+RESULTS: % #+RESULTS:
% [[file:figs/simscape_model_decoupled_plant_svd.png]] % [[file:figs/gravimeter_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]]. % 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:gravimeter_decoupled_plant_jacobian]].
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
% Magnitude % Magnitude
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
for i_in = 1:6 for i_in = 1:3
for i_out = [1:i_in-1, i_in+1:6] 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], ... plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
'HandleVisibility', 'off'); 'HandleVisibility', 'off');
end end
@ -361,41 +455,35 @@ end
plot(freqs, abs(squeeze(freqresp(Gx(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ... plot(freqs, abs(squeeze(freqresp(Gx(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
'DisplayName', '$G_x(i,j),\ i \neq j$'); 'DisplayName', '$G_x(i,j),\ i \neq j$');
set(gca,'ColorOrderIndex',1) 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(1, 1), 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(2, 2), 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(3, 3), freqs, 'Hz'))), 'DisplayName', '$G_x(3,3) = R_y/M_y$');
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; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Magnitude'); set(gca, 'XTickLabel',[]); ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
legend('location', 'northwest'); legend('location', 'southwest');
ylim([1e-2, 2e6]) ylim([1e-8, 1e0])
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(2, 2), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(3, 3), 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; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([0, 180]); ylim([-180, 180]);
yticks([0:45:360]); yticks([0:45:360]);
linkaxes([ax1,ax2],'x'); linkaxes([ax1,ax2],'x');
% #+name: fig:svd_control % #+name: fig:svd_control_gravimeter
% #+caption: Control Diagram for the SVD control % #+caption: Control Diagram for the SVD control
% #+RESULTS: % #+RESULTS:
% [[file:figs/svd_control.png]] % [[file:figs/svd_control_gravimeter.png]]
% We choose the controller to be a low pass filter: % We choose the controller to be a low pass filter:
@ -404,24 +492,23 @@ linkaxes([ax1,ax2],'x');
% $G_0$ is tuned such that the crossover frequency corresponding to the diagonal terms of the loop gain is equal to $\omega_c$ % $G_0$ is tuned such that the crossover frequency corresponding to the diagonal terms of the loop gain is equal to $\omega_c$
wc = 2*pi*80; % Crossover Frequency [rad/s] wc = 2*pi*10; % Crossover Frequency [rad/s]
w0 = 2*pi*0.1; % Controller Pole [rad/s] w0 = 2*pi*0.1; % Controller Pole [rad/s]
K_cen = diag(1./diag(abs(evalfr(Gx, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0); 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; L_cen = K_cen*Gx;
G_cen = feedback(G, pinv(J')*K_cen, [7:12], [1:6]); G_cen = feedback(G, pinv(Jt')*K_cen*pinv(Ja));
K_svd = diag(1./diag(abs(evalfr(Gsvd, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0); 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; L_svd = K_svd*Gsvd;
G_svd = feedback(G, inv(V')*K_svd*inv(U), [7:12], [1:6]); U_inv = inv(U);
G_svd = feedback(G, inv(V')*K_svd*U_inv(1:3, :));
% The obtained diagonal elements of the loop gains are shown in Figure [[fig:gravimeter_comp_loop_gain_diagonal]]. % The obtained diagonal elements of the loop gains are shown in Figure [[fig:gravimeter_comp_loop_gain_diagonal]].
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
@ -429,7 +516,7 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile([2, 1]); ax1 = nexttile([2, 1]);
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(L_svd(1, 1), freqs, 'Hz'))), 'DisplayName', '$L_{SVD}(i,i)$'); plot(freqs, abs(squeeze(freqresp(L_svd(1, 1), freqs, 'Hz'))), 'DisplayName', '$L_{SVD}(i,i)$');
for i_in_out = 2:6 for i_in_out = 2:3
set(gca,'ColorOrderIndex',1) set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off'); plot(freqs, abs(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
end end
@ -437,7 +524,7 @@ end
set(gca,'ColorOrderIndex',2) set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(L_cen(1, 1), freqs, 'Hz'))), ... plot(freqs, abs(squeeze(freqresp(L_cen(1, 1), freqs, 'Hz'))), ...
'DisplayName', '$L_{J}(i,i)$'); 'DisplayName', '$L_{J}(i,i)$');
for i_in_out = 2:6 for i_in_out = 2:3
set(gca,'ColorOrderIndex',2) set(gca,'ColorOrderIndex',2)
plot(freqs, abs(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off'); plot(freqs, abs(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
end end
@ -450,12 +537,12 @@ ylim([5e-2, 2e3])
% Phase % Phase
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
for i_in_out = 1:6 for i_in_out = 1:3
set(gca,'ColorOrderIndex',1) set(gca,'ColorOrderIndex',1)
plot(freqs, 180/pi*angle(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))));
end end
set(gca,'ColorOrderIndex',2) set(gca,'ColorOrderIndex',2)
for i_in_out = 1:6 for i_in_out = 1:3
set(gca,'ColorOrderIndex',2) set(gca,'ColorOrderIndex',2)
plot(freqs, 180/pi*angle(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz')))); plot(freqs, 180/pi*angle(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))));
end end
@ -467,7 +554,7 @@ yticks([-180:90:360]);
linkaxes([ax1,ax2],'x'); linkaxes([ax1,ax2],'x');
% TODO Closed-Loop system Performances % Closed-Loop system Performances
% <<sec:gravimeter_closed_loop_results>> % <<sec:gravimeter_closed_loop_results>>
% Let's first verify the stability of the closed-loop systems: % Let's first verify the stability of the closed-loop systems:
@ -497,53 +584,39 @@ isstable(G_svd)
freqs = logspace(-2, 2, 1000); freqs = logspace(-2, 2, 1000);
figure; figure;
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(1, 3, 'TileSpacing', 'None', 'Padding', 'None');
ax1 = nexttile; ax1 = nexttile;
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop'); plot(freqs, abs(squeeze(freqresp(G( 1,1)/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_cen(1,1)/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
plot(freqs, abs(squeeze(freqresp(G_svd('Ax', 'Dwx')/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD'); plot(freqs, abs(squeeze(freqresp(G_svd(1,1)/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; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$D_x/D_{w,x}$, $D_y/D_{w, y}$'); set(gca, 'XTickLabel',[]); ylabel('Transmissibility'); xlabel('Frequency [Hz]');
title('$D_x/D_{w,x}$');
legend('location', 'southwest'); legend('location', 'southwest');
ax2 = nexttile; ax2 = nexttile;
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Az', 'Dwz')/s^2, freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(G( 2,2)/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Az', 'Dwz')/s^2, freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(G_cen(2,2)/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Az', 'Dwz')/s^2, freqs, 'Hz'))), '--'); plot(freqs, abs(squeeze(freqresp(G_svd(2,2)/s^2, freqs, 'Hz'))), '--');
hold off; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$D_z/D_{w,z}$'); set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]); xlabel('Frequency [Hz]');
title('$D_z/D_{w,z}$');
ax3 = nexttile; ax3 = nexttile;
hold on; hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arx', 'Rwx')/s^2, freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(G( 3,3)/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arx', 'Rwx')/s^2, freqs, 'Hz')))); plot(freqs, abs(squeeze(freqresp(G_cen(3,3)/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Arx', 'Rwx')/s^2, freqs, 'Hz'))), '--'); plot(freqs, abs(squeeze(freqresp(G_svd(3,3)/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; hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('$R_x/R_{w,x}$, $R_y/R_{w,y}$'); xlabel('Frequency [Hz]'); set(gca, 'YTickLabel',[]); xlabel('Frequency [Hz]');
title('$R_y/R_{w,y}$');
ax4 = nexttile; linkaxes([ax1,ax2,ax3],'xy');
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)]); xlim([freqs(1), freqs(end)]);
ylim([1e-3, 1e2]); xlim([1e-2, 5e1]); ylim([1e-7, 1e-2]);

508
index.html
View File

@ -3,9 +3,9 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2020-11-25 mer. 09:16 --> <!-- 2020-11-25 mer. 09:32 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<title>SVD Control</title> <title>Diagonal control using the SVD and the Jacobian Matrix</title>
<meta name="generator" content="Org mode" /> <meta name="generator" content="Org mode" />
<meta name="author" content="Dehaeze Thomas" /> <meta name="author" content="Dehaeze Thomas" />
<link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/> <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
@ -25,68 +25,103 @@
| |
<a accesskey="H" href="../index.html"> HOME </a> <a accesskey="H" href="../index.html"> HOME </a>
</div><div id="content"> </div><div id="content">
<h1 class="title">SVD Control</h1> <h1 class="title">Diagonal control using the SVD and the Jacobian Matrix</h1>
<div id="table-of-contents"> <div id="table-of-contents">
<h2>Table of Contents</h2> <h2>Table of Contents</h2>
<div id="text-table-of-contents"> <div id="text-table-of-contents">
<ul> <ul>
<li><a href="#org3a856a2">1. Gravimeter - Simscape Model</a> <li><a href="#org4bcea32">1. Gravimeter - Simscape Model</a>
<ul> <ul>
<li><a href="#org7cee37e">1.1. Introduction</a></li> <li><a href="#org5f6e249">1.1. Introduction</a></li>
<li><a href="#orgec45da7">1.2. Simscape Model - Parameters</a></li> <li><a href="#org01cc89e">1.2. Gravimeter Model - Parameters</a></li>
<li><a href="#org30962d3">1.3. System Identification - Without Gravity</a></li> <li><a href="#orgd1738d1">1.3. System Identification</a></li>
<li><a href="#org5b19440">1.4. Physical Decoupling using the Jacobian</a></li> <li><a href="#org35c239c">1.4. Decoupling using the Jacobian</a></li>
<li><a href="#orgc8cb89d">1.5. SVD Decoupling</a></li> <li><a href="#org6f938e2">1.5. Decoupling using the SVD</a></li>
<li><a href="#orgd2f1c19">1.6. Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</a></li> <li><a href="#orgfabe265">1.6. Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</a></li>
<li><a href="#orgc761edd">1.7. Obtained Decoupled Plants</a></li> <li><a href="#orgef088ef">1.7. Verification of the decoupling using the &ldquo;Relative Gain Array&rdquo;</a></li>
<li><a href="#org669229d">1.8. Diagonal Controller</a></li> <li><a href="#org09b55fe">1.8. Obtained Decoupled Plants</a></li>
<li><a href="#org582b3be">1.9. Closed-Loop system Performances</a></li> <li><a href="#orgfa6fc3b">1.9. Diagonal Controller</a></li>
<li><a href="#org78d9a5b">1.10. Closed-Loop system Performances</a></li>
</ul> </ul>
</li> </li>
<li><a href="#org71d7b34">2. Stewart Platform - Simscape Model</a> <li><a href="#orgce40d0e">2. Stewart Platform - Simscape Model</a>
<ul> <ul>
<li><a href="#orgf9cb366">2.1. Simscape Model - Parameters</a></li> <li><a href="#org63cce53">2.1. Simscape Model - Parameters</a></li>
<li><a href="#orga72c2be">2.2. Identification of the plant</a></li> <li><a href="#org6cc3c6a">2.2. Identification of the plant</a></li>
<li><a href="#orga9ba8e1">2.3. Physical Decoupling using the Jacobian</a></li> <li><a href="#org6c203ed">2.3. Decoupling using the Jacobian</a></li>
<li><a href="#org4b07807">2.4. Real Approximation of \(G\) at the decoupling frequency</a></li> <li><a href="#orgc3c9daf">2.4. Decoupling using the SVD</a></li>
<li><a href="#org87a2d2f">2.5. SVD Decoupling</a></li> <li><a href="#orga514e47">2.5. Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</a></li>
<li><a href="#org4b81d86">2.6. Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</a></li> <li><a href="#orgd5f8212">2.6. Verification of the decoupling using the &ldquo;Relative Gain Array&rdquo;</a></li>
<li><a href="#org2845b5e">2.7. Verification of the decoupling using the &ldquo;Relative Gain Array&rdquo;</a></li> <li><a href="#org887521e">2.7. Obtained Decoupled Plants</a></li>
<li><a href="#orgdb7f2df">2.8. Obtained Decoupled Plants</a></li> <li><a href="#orga134c9c">2.8. Diagonal Controller</a></li>
<li><a href="#org0143a9d">2.9. Diagonal Controller</a></li> <li><a href="#org962b3f8">2.9. Closed-Loop system Performances</a></li>
<li><a href="#org7f0526e">2.10. Closed-Loop system Performances</a></li>
<li><a href="#org456839a">2.11. Small error on the sensor location&#xa0;&#xa0;&#xa0;<span class="tag"><span class="no_export">no_export</span></span></a></li>
</ul> </ul>
</li> </li>
</ul> </ul>
</div> </div>
</div> </div>
<div id="outline-container-org3a856a2" class="outline-2"> <p>
<h2 id="org3a856a2"><span class="section-number-2">1</span> Gravimeter - Simscape Model</h2> In this document, the use of the Jacobian matrix and the Singular Value Decomposition to render a physical plant diagonal dominant is studied.
Then, a diagonal controller is used.
</p>
<p>
These two methods are tested on two plants:
</p>
<ul class="org-ul">
<li>In Section <a href="#orga85e8ac">1</a> on a 3-DoF gravimeter</li>
<li>In Section <a href="#orgc4fc4b3">2</a> on a 6-DoF Stewart platform</li>
</ul>
<div id="outline-container-org4bcea32" class="outline-2">
<h2 id="org4bcea32"><span class="section-number-2">1</span> Gravimeter - Simscape Model</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
</div> <p>
<div id="outline-container-org7cee37e" class="outline-3"> <a id="orga85e8ac"></a>
<h3 id="org7cee37e"><span class="section-number-3">1.1</span> Introduction</h3> </p>
</div> </div>
<div id="outline-container-orgec45da7" class="outline-3"> <div id="outline-container-org5f6e249" class="outline-3">
<h3 id="orgec45da7"><span class="section-number-3">1.2</span> Simscape Model - Parameters</h3> <h3 id="org5f6e249"><span class="section-number-3">1.1</span> Introduction</h3>
<div class="outline-text-3" id="text-1-1">
<p>
In this part, diagonal control using both the SVD and the Jacobian matrices are applied on a gravimeter model:
</p>
<ul class="org-ul">
<li>Section <a href="#org516b437">1.2</a>: the model is described and its parameters are defined.</li>
<li>Section <a href="#org1c874d0">1.3</a>: the plant dynamics from the actuators to the sensors is computed from a Simscape model.</li>
<li>Section <a href="#org3e3089f">1.4</a>: the plant is decoupled using the Jacobian matrices.</li>
<li>Section <a href="#orgdb7c950">1.5</a>: the Singular Value Decomposition is performed on a real approximation of the plant transfer matrix and further use to decouple the system.</li>
<li>Section <a href="#org3ecdcf9">1.6</a>: the effectiveness of the decoupling is computed using the Gershorin radii</li>
<li>Section <a href="#org30ee3ad">1.7</a>: the effectiveness of the decoupling is computed using the Relative Gain Array</li>
<li>Section <a href="#org62590c4">1.8</a>: the obtained decoupled plants are compared</li>
<li>Section <a href="#orgf495bba">1.9</a>: the diagonal controller is developed</li>
<li>Section <a href="#org1a4c704">1.10</a>: the obtained closed-loop performances for the two methods are compared</li>
</ul>
</div>
</div>
<div id="outline-container-org01cc89e" class="outline-3">
<h3 id="org01cc89e"><span class="section-number-3">1.2</span> Gravimeter Model - Parameters</h3>
<div class="outline-text-3" id="text-1-2"> <div class="outline-text-3" id="text-1-2">
<div class="org-src-container"> <p>
<pre class="src src-matlab">open(<span class="org-string">'gravimeter.slx'</span>) <a id="org516b437"></a>
</pre> </p>
</div>
<p>
The model of the gravimeter is schematically shown in Figure <a href="#orgb4b224e">1</a>.
</p>
<div id="org57b14c6" class="figure"> <div id="orgb4b224e" class="figure">
<p><img src="figs/gravimeter_model.png" alt="gravimeter_model.png" /> <p><img src="figs/gravimeter_model.png" alt="gravimeter_model.png" />
</p> </p>
<p><span class="figure-number">Figure 1: </span>Model of the gravimeter</p> <p><span class="figure-number">Figure 1: </span>Model of the gravimeter</p>
</div> </div>
<p> <p>
Parameters The parameters used for the simulation are the following:
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">l = 1.0; <span class="org-comment">% Length of the mass [m]</span> <pre class="src src-matlab">l = 1.0; <span class="org-comment">% Length of the mass [m]</span>
@ -109,9 +144,13 @@ g = 0; <span class="org-comment">% Gravity [m/s2]</span>
</div> </div>
</div> </div>
<div id="outline-container-org30962d3" class="outline-3"> <div id="outline-container-orgd1738d1" class="outline-3">
<h3 id="org30962d3"><span class="section-number-3">1.3</span> System Identification - Without Gravity</h3> <h3 id="orgd1738d1"><span class="section-number-3">1.3</span> System Identification</h3>
<div class="outline-text-3" id="text-1-3"> <div class="outline-text-3" id="text-1-3">
<p>
<a id="org1c874d0"></a>
</p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span> <pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'gravimeter'</span>; mdl = <span class="org-string">'gravimeter'</span>;
@ -133,7 +172,7 @@ G.OutputName = {<span class="org-string">'Ax1'</span>, <span class="org-string">
</div> </div>
<p> <p>
The inputs and outputs of the plant are shown in Figure <a href="#orgb8af3e7">2</a>. The inputs and outputs of the plant are shown in Figure <a href="#orgc747e56">2</a>.
</p> </p>
<p> <p>
@ -150,7 +189,7 @@ And 4 outputs (the two 2-DoF accelerometers):
\end{equation} \end{equation}
<div id="orgb8af3e7" class="figure"> <div id="orgc747e56" class="figure">
<p><img src="figs/gravimeter_plant_schematic.png" alt="gravimeter_plant_schematic.png" /> <p><img src="figs/gravimeter_plant_schematic.png" alt="gravimeter_plant_schematic.png" />
</p> </p>
<p><span class="figure-number">Figure 2: </span>Schematic of the gravimeter plant</p> <p><span class="figure-number">Figure 2: </span>Schematic of the gravimeter plant</p>
@ -206,11 +245,11 @@ State-space model with 4 outputs, 3 inputs, and 6 states.
<p> <p>
The bode plot of all elements of the plant are shown in Figure <a href="#org64e7053">3</a>. The bode plot of all elements of the plant are shown in Figure <a href="#org85271f8">3</a>.
</p> </p>
<div id="org64e7053" class="figure"> <div id="org85271f8" class="figure">
<p><img src="figs/open_loop_tf.png" alt="open_loop_tf.png" /> <p><img src="figs/open_loop_tf.png" alt="open_loop_tf.png" />
</p> </p>
<p><span class="figure-number">Figure 3: </span>Open Loop Transfer Function from 3 Actuators to 4 Accelerometers</p> <p><span class="figure-number">Figure 3: </span>Open Loop Transfer Function from 3 Actuators to 4 Accelerometers</p>
@ -218,15 +257,15 @@ The bode plot of all elements of the plant are shown in Figure <a href="#org64e7
</div> </div>
</div> </div>
<div id="outline-container-org5b19440" class="outline-3"> <div id="outline-container-org35c239c" class="outline-3">
<h3 id="org5b19440"><span class="section-number-3">1.4</span> Physical Decoupling using the Jacobian</h3> <h3 id="org35c239c"><span class="section-number-3">1.4</span> Decoupling using the Jacobian</h3>
<div class="outline-text-3" id="text-1-4"> <div class="outline-text-3" id="text-1-4">
<p> <p>
<a id="org5c9e033"></a> <a id="org3e3089f"></a>
</p> </p>
<p> <p>
Consider the control architecture shown in Figure <a href="#org056bfbe">4</a>. Consider the control architecture shown in Figure <a href="#org38ba9bc">4</a>.
</p> </p>
<p> <p>
@ -244,16 +283,16 @@ The Jacobian matrix \(J_{a}\) is used to compute the vertical acceleration, hori
\end{equation} \end{equation}
<p> <p>
We thus define a new plant as defined in Figure <a href="#org056bfbe">4</a>. We thus define a new plant as defined in Figure <a href="#org38ba9bc">4</a>.
\[ \bm{G}_x(s) = J_a^{-1} \bm{G}(s) J_{\tau}^{-T} \] \[ \bm{G}_x(s) = J_a^{-1} \bm{G}(s) J_{\tau}^{-T} \]
</p> </p>
<p> <p>
\(\bm{G}_x(s)\) correspond to the $3 &times; 3$transfer function matrix from forces and torques applied to the gravimeter at its center of mass to the absolute acceleration of the gravimeter&rsquo;s center of mass (Figure <a href="#org056bfbe">4</a>). \(\bm{G}_x(s)\) correspond to the $3 &times; 3$transfer function matrix from forces and torques applied to the gravimeter at its center of mass to the absolute acceleration of the gravimeter&rsquo;s center of mass (Figure <a href="#org38ba9bc">4</a>).
</p> </p>
<div id="org056bfbe" class="figure"> <div id="org38ba9bc" class="figure">
<p><img src="figs/gravimeter_decouple_jacobian.png" alt="gravimeter_decouple_jacobian.png" /> <p><img src="figs/gravimeter_decouple_jacobian.png" alt="gravimeter_decouple_jacobian.png" />
</p> </p>
<p><span class="figure-number">Figure 4: </span>Decoupled plant \(\bm{G}_x\) using the Jacobian matrix \(J\)</p> <p><span class="figure-number">Figure 4: </span>Decoupled plant \(\bm{G}_x\) using the Jacobian matrix \(J\)</p>
@ -291,11 +330,11 @@ State-space model with 3 outputs, 3 inputs, and 6 states.
<p> <p>
The diagonal and off-diagonal elements of \(G_x\) are shown in Figure <a href="#org71d3d59">5</a>. The diagonal and off-diagonal elements of \(G_x\) are shown in Figure <a href="#orgee1af52">5</a>.
</p> </p>
<div id="org71d3d59" class="figure"> <div id="orgee1af52" class="figure">
<p><img src="figs/gravimeter_jacobian_plant.png" alt="gravimeter_jacobian_plant.png" /> <p><img src="figs/gravimeter_jacobian_plant.png" alt="gravimeter_jacobian_plant.png" />
</p> </p>
<p><span class="figure-number">Figure 5: </span>Diagonal and off-diagonal elements of \(G_x\)</p> <p><span class="figure-number">Figure 5: </span>Diagonal and off-diagonal elements of \(G_x\)</p>
@ -303,18 +342,17 @@ The diagonal and off-diagonal elements of \(G_x\) are shown in Figure <a href="#
</div> </div>
</div> </div>
<div id="outline-container-orgc8cb89d" class="outline-3"> <div id="outline-container-org6f938e2" class="outline-3">
<h3 id="orgc8cb89d"><span class="section-number-3">1.5</span> SVD Decoupling</h3> <h3 id="org6f938e2"><span class="section-number-3">1.5</span> Decoupling using the SVD</h3>
<div class="outline-text-3" id="text-1-5"> <div class="outline-text-3" id="text-1-5">
<p> <p>
<a id="org6daebae"></a> <a id="orgdb7c950"></a>
</p> </p>
<p> <p>
In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required. In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required.
</p> </p>
<p> <p>
Let&rsquo;s compute a real approximation of the complex matrix \(H_1\) which corresponds to the the transfer function \(G(j\omega_c)\) from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency \(\omega_c\). Let&rsquo;s compute a real approximation of the complex matrix \(H_1\) which corresponds to the the transfer function \(G(j\omega_c)\) from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency \(\omega_c\).
</p> </p>
@ -383,11 +421,11 @@ Now, the Singular Value Decomposition of \(H_1\) is performed:
</div> </div>
<p> <p>
The obtained matrices \(U\) and \(V\) are used to decouple the system as shown in Figure <a href="#org05538bc">6</a>. The obtained matrices \(U\) and \(V\) are used to decouple the system as shown in Figure <a href="#org5b03e6b">6</a>.
</p> </p>
<div id="org05538bc" class="figure"> <div id="org5b03e6b" class="figure">
<p><img src="figs/gravimeter_decouple_svd.png" alt="gravimeter_decouple_svd.png" /> <p><img src="figs/gravimeter_decouple_svd.png" alt="gravimeter_decouple_svd.png" />
</p> </p>
<p><span class="figure-number">Figure 6: </span>Decoupled plant \(\bm{G}_{SVD}\) using the Singular Value Decomposition</p> <p><span class="figure-number">Figure 6: </span>Decoupled plant \(\bm{G}_{SVD}\) using the Singular Value Decomposition</p>
@ -418,10 +456,10 @@ The 4th output (corresponding to the null singular value) is discarded, and we o
</div> </div>
<p> <p>
The diagonal and off-diagonal elements of the &ldquo;SVD&rdquo; plant are shown in Figure <a href="#orgcf0d284">7</a>. The diagonal and off-diagonal elements of the &ldquo;SVD&rdquo; plant are shown in Figure <a href="#org941635f">7</a>.
</p> </p>
<div id="orgcf0d284" class="figure"> <div id="org941635f" class="figure">
<p><img src="figs/gravimeter_svd_plant.png" alt="gravimeter_svd_plant.png" /> <p><img src="figs/gravimeter_svd_plant.png" alt="gravimeter_svd_plant.png" />
</p> </p>
<p><span class="figure-number">Figure 7: </span>Diagonal and off-diagonal elements of \(G_{svd}\)</p> <p><span class="figure-number">Figure 7: </span>Diagonal and off-diagonal elements of \(G_{svd}\)</p>
@ -429,11 +467,11 @@ The diagonal and off-diagonal elements of the &ldquo;SVD&rdquo; plant are shown
</div> </div>
</div> </div>
<div id="outline-container-orgd2f1c19" class="outline-3"> <div id="outline-container-orgfabe265" class="outline-3">
<h3 id="orgd2f1c19"><span class="section-number-3">1.6</span> Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</h3> <h3 id="orgfabe265"><span class="section-number-3">1.6</span> Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</h3>
<div class="outline-text-3" id="text-1-6"> <div class="outline-text-3" id="text-1-6">
<p> <p>
<a id="org72fd0a9"></a> <a id="org3ecdcf9"></a>
</p> </p>
<p> <p>
@ -454,7 +492,7 @@ This is computed over the following frequencies.
</div> </div>
<div id="orgdc7adbb" class="figure"> <div id="orgb00f2d8" class="figure">
<p><img src="figs/gravimeter_gershgorin_radii.png" alt="gravimeter_gershgorin_radii.png" /> <p><img src="figs/gravimeter_gershgorin_radii.png" alt="gravimeter_gershgorin_radii.png" />
</p> </p>
<p><span class="figure-number">Figure 8: </span>Gershgorin Radii of the Coupled and Decoupled plants</p> <p><span class="figure-number">Figure 8: </span>Gershgorin Radii of the Coupled and Decoupled plants</p>
@ -462,43 +500,73 @@ This is computed over the following frequencies.
</div> </div>
</div> </div>
<div id="outline-container-orgc761edd" class="outline-3"> <div id="outline-container-orgef088ef" class="outline-3">
<h3 id="orgc761edd"><span class="section-number-3">1.7</span> Obtained Decoupled Plants</h3> <h3 id="orgef088ef"><span class="section-number-3">1.7</span> Verification of the decoupling using the &ldquo;Relative Gain Array&rdquo;</h3>
<div class="outline-text-3" id="text-1-7"> <div class="outline-text-3" id="text-1-7">
<p> <p>
<a id="org871fe90"></a> <a id="org30ee3ad"></a>
</p> </p>
<p> <p>
The bode plot of the diagonal and off-diagonal elements of \(G_{SVD}\) are shown in Figure <a href="#org85343af">9</a>. The relative gain array (RGA) is defined as:
</p> </p>
\begin{equation}
\Lambda\big(G(s)\big) = G(s) \times \big( G(s)^{-1} \big)^T
<div id="org85343af" class="figure"> \end{equation}
<p><img src="figs/gravimeter_decoupled_plant_svd.png" alt="gravimeter_decoupled_plant_svd.png" /> <p>
where \(\times\) denotes an element by element multiplication and \(G(s)\) is an \(n \times n\) square transfer matrix.
</p> </p>
<p><span class="figure-number">Figure 9: </span>Decoupled Plant using SVD</p>
</div>
<p> <p>
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 <a href="#org5d69920">10</a>. The obtained RGA elements are shown in Figure <a href="#orgee3b5aa">9</a>.
</p> </p>
<div id="org5d69920" class="figure"> <div id="orgee3b5aa" class="figure">
<p><img src="figs/gravimeter_decoupled_plant_jacobian.png" alt="gravimeter_decoupled_plant_jacobian.png" /> <p><img src="figs/gravimeter_rga.png" alt="gravimeter_rga.png" />
</p> </p>
<p><span class="figure-number">Figure 10: </span>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)</p> <p><span class="figure-number">Figure 9: </span>Obtained norm of RGA elements for the SVD decoupled plant and the Jacobian decoupled plant</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org669229d" class="outline-3"> <div id="outline-container-org09b55fe" class="outline-3">
<h3 id="org669229d"><span class="section-number-3">1.8</span> Diagonal Controller</h3> <h3 id="org09b55fe"><span class="section-number-3">1.8</span> Obtained Decoupled Plants</h3>
<div class="outline-text-3" id="text-1-8"> <div class="outline-text-3" id="text-1-8">
<p> <p>
<a id="org14b50b3"></a> <a id="org62590c4"></a>
The control diagram for the centralized control is shown in Figure <a href="#orga8cccea">11</a>. </p>
<p>
The bode plot of the diagonal and off-diagonal elements of \(G_{SVD}\) are shown in Figure <a href="#org2e521bd">10</a>.
</p>
<div id="org2e521bd" class="figure">
<p><img src="figs/gravimeter_decoupled_plant_svd.png" alt="gravimeter_decoupled_plant_svd.png" />
</p>
<p><span class="figure-number">Figure 10: </span>Decoupled Plant using SVD</p>
</div>
<p>
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 <a href="#org3605219">11</a>.
</p>
<div id="org3605219" class="figure">
<p><img src="figs/gravimeter_decoupled_plant_jacobian.png" alt="gravimeter_decoupled_plant_jacobian.png" />
</p>
<p><span class="figure-number">Figure 11: </span>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)</p>
</div>
</div>
</div>
<div id="outline-container-orgfa6fc3b" class="outline-3">
<h3 id="orgfa6fc3b"><span class="section-number-3">1.9</span> Diagonal Controller</h3>
<div class="outline-text-3" id="text-1-9">
<p>
<a id="orgf495bba"></a>
The control diagram for the centralized control is shown in Figure <a href="#orgc82daae">12</a>.
</p> </p>
<p> <p>
@ -507,22 +575,22 @@ The Jacobian is used to convert forces in the cartesian frame to forces applied
</p> </p>
<div id="orga8cccea" class="figure"> <div id="orgc82daae" class="figure">
<p><img src="figs/centralized_control_gravimeter.png" alt="centralized_control_gravimeter.png" /> <p><img src="figs/centralized_control_gravimeter.png" alt="centralized_control_gravimeter.png" />
</p> </p>
<p><span class="figure-number">Figure 11: </span>Control Diagram for the Centralized control</p> <p><span class="figure-number">Figure 12: </span>Control Diagram for the Centralized control</p>
</div> </div>
<p> <p>
The SVD control architecture is shown in Figure <a href="#orgd9e0c5f">12</a>. The SVD control architecture is shown in Figure <a href="#org70d4619">13</a>.
The matrices \(U\) and \(V\) are used to decoupled the plant \(G\). The matrices \(U\) and \(V\) are used to decoupled the plant \(G\).
</p> </p>
<div id="orgd9e0c5f" class="figure"> <div id="org70d4619" class="figure">
<p><img src="figs/svd_control_gravimeter.png" alt="svd_control_gravimeter.png" /> <p><img src="figs/svd_control_gravimeter.png" alt="svd_control_gravimeter.png" />
</p> </p>
<p><span class="figure-number">Figure 12: </span>Control Diagram for the SVD control</p> <p><span class="figure-number">Figure 13: </span>Control Diagram for the SVD control</p>
</div> </div>
@ -557,23 +625,23 @@ G_svd = feedback(G, inv(V<span class="org-type">'</span>)<span class="org-type">
</div> </div>
<p> <p>
The obtained diagonal elements of the loop gains are shown in Figure <a href="#org7417f1d">13</a>. The obtained diagonal elements of the loop gains are shown in Figure <a href="#orgded6cd6">14</a>.
</p> </p>
<div id="org7417f1d" class="figure"> <div id="orgded6cd6" class="figure">
<p><img src="figs/gravimeter_comp_loop_gain_diagonal.png" alt="gravimeter_comp_loop_gain_diagonal.png" /> <p><img src="figs/gravimeter_comp_loop_gain_diagonal.png" alt="gravimeter_comp_loop_gain_diagonal.png" />
</p> </p>
<p><span class="figure-number">Figure 13: </span>Comparison of the diagonal elements of the loop gains for the SVD control architecture and the Jacobian one</p> <p><span class="figure-number">Figure 14: </span>Comparison of the diagonal elements of the loop gains for the SVD control architecture and the Jacobian one</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org582b3be" class="outline-3"> <div id="outline-container-org78d9a5b" class="outline-3">
<h3 id="org582b3be"><span class="section-number-3">1.9</span> Closed-Loop system Performances</h3> <h3 id="org78d9a5b"><span class="section-number-3">1.10</span> Closed-Loop system Performances</h3>
<div class="outline-text-3" id="text-1-9"> <div class="outline-text-3" id="text-1-10">
<p> <p>
<a id="orgfc06310"></a> <a id="org1a4c704"></a>
</p> </p>
<p> <p>
@ -604,24 +672,27 @@ ans =
<p> <p>
The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure <a href="#org614b48b">14</a>. The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure <a href="#org9726280">15</a>.
</p> </p>
<div id="org614b48b" class="figure"> <div id="org9726280" class="figure">
<p><img src="figs/gravimeter_platform_simscape_cl_transmissibility.png" alt="gravimeter_platform_simscape_cl_transmissibility.png" /> <p><img src="figs/gravimeter_platform_simscape_cl_transmissibility.png" alt="gravimeter_platform_simscape_cl_transmissibility.png" />
</p> </p>
<p><span class="figure-number">Figure 14: </span>Obtained Transmissibility</p> <p><span class="figure-number">Figure 15: </span>Obtained Transmissibility</p>
</div> </div>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org71d7b34" class="outline-2"> <div id="outline-container-orgce40d0e" class="outline-2">
<h2 id="org71d7b34"><span class="section-number-2">2</span> Stewart Platform - Simscape Model</h2> <h2 id="orgce40d0e"><span class="section-number-2">2</span> Stewart Platform - Simscape Model</h2>
<div class="outline-text-2" id="text-2"> <div class="outline-text-2" id="text-2">
<p> <p>
In this analysis, we wish to applied SVD control to the Stewart Platform shown in Figure <a href="#org0b6cb48">15</a>. <a id="orgc4fc4b3"></a>
</p>
<p>
In this analysis, we wish to applied SVD control to the Stewart Platform shown in Figure <a href="#orge8ec29d">16</a>.
</p> </p>
<p> <p>
@ -634,32 +705,33 @@ Some notes about the system:
</ul> </ul>
<div id="org0b6cb48" class="figure"> <div id="orge8ec29d" class="figure">
<p><img src="figs/SP_assembly.png" alt="SP_assembly.png" /> <p><img src="figs/SP_assembly.png" alt="SP_assembly.png" />
</p> </p>
<p><span class="figure-number">Figure 15: </span>Stewart Platform CAD View</p> <p><span class="figure-number">Figure 16: </span>Stewart Platform CAD View</p>
</div> </div>
<p> <p>
The analysis of the SVD control applied to the Stewart platform is performed in the following sections: The analysis of the SVD/Jacobian control applied to the Stewart platform is performed in the following sections:
</p> </p>
<ul class="org-ul"> <ul class="org-ul">
<li>Section <a href="#orgf1d35e2">2.1</a>: The parameters of the Simscape model of the Stewart platform are defined</li> <li>Section <a href="#org00b1234">2.1</a>: The parameters of the Simscape model of the Stewart platform are defined</li>
<li>Section <a href="#orgd1816b3">2.2</a>: The plant is identified from the Simscape model and the system coupling is shown</li> <li>Section <a href="#org2fcc666">2.2</a>: The plant is identified from the Simscape model and the system coupling is shown</li>
<li>Section <a href="#org7c9242b">2.3</a>: The plant is first decoupled using the Jacobian</li> <li>Section <a href="#org64578de">2.3</a>: The plant is first decoupled using the Jacobian</li>
<li>Section <a href="#orgd1b3ee0">2.4</a>: A real approximation of the plant is computed for further decoupling using the Singular Value Decomposition (SVD)</li> <li>Section <a href="#org8e0706f">2.4</a>: The decoupling is performed thanks to the SVD. To do so a real approximation of the plant is computed.</li>
<li>Section <a href="#orgaa48d31">2.5</a>: The decoupling is performed thanks to the SVD</li> <li>Section <a href="#orgecdb237">2.5</a>: The effectiveness of the decoupling with the Jacobian and SVD are compared using the Gershorin Radii</li>
<li>Section <a href="#org72fd0a9">1.6</a>: The effectiveness of the decoupling with the Jacobian and SVD are compared using the Gershorin Radii</li> <li>Section <a href="#org577bb4e">2.6</a>:</li>
<li>Section <a href="#orgf6dad38">2.8</a>: The dynamics of the decoupled plants are shown</li> <li>Section <a href="#orgd33facb">2.7</a>: The dynamics of the decoupled plants are shown</li>
<li>Section <a href="#orga082064">2.9</a>: A diagonal controller is defined to control the decoupled plant</li> <li>Section <a href="#org4708058">2.8</a>: A diagonal controller is defined to control the decoupled plant</li>
<li>Section <a href="#org7c48c81">2.10</a>: Finally, the closed loop system properties are studied</li> <li>Section <a href="#orgc0d06ab">2.9</a>: Finally, the closed loop system properties are studied</li>
</ul> </ul>
</div> </div>
<div id="outline-container-orgf9cb366" class="outline-3">
<h3 id="orgf9cb366"><span class="section-number-3">2.1</span> Simscape Model - Parameters</h3> <div id="outline-container-org63cce53" class="outline-3">
<h3 id="org63cce53"><span class="section-number-3">2.1</span> Simscape Model - Parameters</h3>
<div class="outline-text-3" id="text-2-1"> <div class="outline-text-3" id="text-2-1">
<p> <p>
<a id="orgf1d35e2"></a> <a id="org00b1234"></a>
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">open(<span class="org-string">'drone_platform.slx'</span>); <pre class="src src-matlab">open(<span class="org-string">'drone_platform.slx'</span>);
@ -715,30 +787,30 @@ Kc = tf(zeros(6));
</div> </div>
<div id="orgb09e537" class="figure"> <div id="orgf441445" class="figure">
<p><img src="figs/stewart_simscape.png" alt="stewart_simscape.png" /> <p><img src="figs/stewart_simscape.png" alt="stewart_simscape.png" />
</p> </p>
<p><span class="figure-number">Figure 16: </span>General view of the Simscape Model</p> <p><span class="figure-number">Figure 17: </span>General view of the Simscape Model</p>
</div> </div>
<div id="org4946596" class="figure"> <div id="org11d6516" class="figure">
<p><img src="figs/stewart_platform_details.png" alt="stewart_platform_details.png" /> <p><img src="figs/stewart_platform_details.png" alt="stewart_platform_details.png" />
</p> </p>
<p><span class="figure-number">Figure 17: </span>Simscape model of the Stewart platform</p> <p><span class="figure-number">Figure 18: </span>Simscape model of the Stewart platform</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orga72c2be" class="outline-3"> <div id="outline-container-org6cc3c6a" class="outline-3">
<h3 id="orga72c2be"><span class="section-number-3">2.2</span> Identification of the plant</h3> <h3 id="org6cc3c6a"><span class="section-number-3">2.2</span> Identification of the plant</h3>
<div class="outline-text-3" id="text-2-2"> <div class="outline-text-3" id="text-2-2">
<p> <p>
<a id="orgd1816b3"></a> <a id="org2fcc666"></a>
</p> </p>
<p> <p>
The plant shown in Figure <a href="#orge94f83d">18</a> is identified from the Simscape model. The plant shown in Figure <a href="#org8b5b7dd">19</a> is identified from the Simscape model.
</p> </p>
<p> <p>
@ -754,10 +826,10 @@ The outputs are the 6 accelerations measured by the inertial unit.
</p> </p>
<div id="orge94f83d" class="figure"> <div id="org8b5b7dd" class="figure">
<p><img src="figs/stewart_platform_plant.png" alt="stewart_platform_plant.png" /> <p><img src="figs/stewart_platform_plant.png" alt="stewart_platform_plant.png" />
</p> </p>
<p><span class="figure-number">Figure 18: </span>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</p> <p><span class="figure-number">Figure 19: </span>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</p>
</div> </div>
<div class="org-src-container"> <div class="org-src-container">
@ -796,7 +868,7 @@ State-space model with 6 outputs, 12 inputs, and 24 states.
<p> <p>
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 <a href="#orgaa55beb">19</a>. 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 <a href="#org248144c">20</a>.
</p> </p>
<p> <p>
@ -804,20 +876,20 @@ One can easily see that the system is strongly coupled.
</p> </p>
<div id="orgaa55beb" class="figure"> <div id="org248144c" class="figure">
<p><img src="figs/stewart_platform_coupled_plant.png" alt="stewart_platform_coupled_plant.png" /> <p><img src="figs/stewart_platform_coupled_plant.png" alt="stewart_platform_coupled_plant.png" />
</p> </p>
<p><span class="figure-number">Figure 19: </span>Magnitude of all 36 elements of the transfer function matrix \(G_u\)</p> <p><span class="figure-number">Figure 20: </span>Magnitude of all 36 elements of the transfer function matrix \(G_u\)</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orga9ba8e1" class="outline-3"> <div id="outline-container-org6c203ed" class="outline-3">
<h3 id="orga9ba8e1"><span class="section-number-3">2.3</span> Physical Decoupling using the Jacobian</h3> <h3 id="org6c203ed"><span class="section-number-3">2.3</span> Decoupling using the Jacobian</h3>
<div class="outline-text-3" id="text-2-3"> <div class="outline-text-3" id="text-2-3">
<p> <p>
<a id="org7c9242b"></a> <a id="org64578de"></a>
Consider the control architecture shown in Figure <a href="#org89c1f08">20</a>. Consider the control architecture shown in Figure <a href="#org9a6d62d">21</a>.
The Jacobian matrix is used to transform forces/torques applied on the top platform to the equivalent forces applied by each actuator. The Jacobian matrix is used to transform forces/torques applied on the top platform to the equivalent forces applied by each actuator.
</p> </p>
@ -899,10 +971,10 @@ The Jacobian matrix is computed from the geometry of the platform (position and
</table> </table>
<div id="org89c1f08" class="figure"> <div id="org9a6d62d" class="figure">
<p><img src="figs/plant_decouple_jacobian.png" alt="plant_decouple_jacobian.png" /> <p><img src="figs/plant_decouple_jacobian.png" alt="plant_decouple_jacobian.png" />
</p> </p>
<p><span class="figure-number">Figure 20: </span>Decoupled plant \(\bm{G}_x\) using the Jacobian matrix \(J\)</p> <p><span class="figure-number">Figure 21: </span>Decoupled plant \(\bm{G}_x\) using the Jacobian matrix \(J\)</p>
</div> </div>
<p> <p>
@ -922,11 +994,15 @@ Gx.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">
</div> </div>
</div> </div>
<div id="outline-container-org4b07807" class="outline-3"> <div id="outline-container-orgc3c9daf" class="outline-3">
<h3 id="org4b07807"><span class="section-number-3">2.4</span> Real Approximation of \(G\) at the decoupling frequency</h3> <h3 id="orgc3c9daf"><span class="section-number-3">2.4</span> Decoupling using the SVD</h3>
<div class="outline-text-3" id="text-2-4"> <div class="outline-text-3" id="text-2-4">
<p> <p>
<a id="orgd1b3ee0"></a> <a id="org8e0706f"></a>
</p>
<p>
In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required.
</p> </p>
<p> <p>
@ -1100,18 +1176,9 @@ This can be verified below where only the real value of \(G_u(\omega_c)\) is sho
</tr> </tr>
</tbody> </tbody>
</table> </table>
</div>
</div>
<div id="outline-container-org87a2d2f" class="outline-3">
<h3 id="org87a2d2f"><span class="section-number-3">2.5</span> SVD Decoupling</h3>
<div class="outline-text-3" id="text-2-5">
<p>
<a id="orgaa48d31"></a>
</p>
<p> <p>
First, the Singular Value Decomposition of \(H_1\) is performed: Now, the Singular Value Decomposition of \(H_1\) is performed:
\[ H_1 = U \Sigma V^H \] \[ H_1 = U \Sigma V^H \]
</p> </p>
@ -1267,14 +1334,14 @@ First, the Singular Value Decomposition of \(H_1\) is performed:
</table> </table>
<p> <p>
The obtained matrices \(U\) and \(V\) are used to decouple the system as shown in Figure <a href="#orgad6c96b">21</a>. The obtained matrices \(U\) and \(V\) are used to decouple the system as shown in Figure <a href="#org6b45a64">22</a>.
</p> </p>
<div id="orgad6c96b" class="figure"> <div id="org6b45a64" class="figure">
<p><img src="figs/plant_decouple_svd.png" alt="plant_decouple_svd.png" /> <p><img src="figs/plant_decouple_svd.png" alt="plant_decouple_svd.png" />
</p> </p>
<p><span class="figure-number">Figure 21: </span>Decoupled plant \(\bm{G}_{SVD}\) using the Singular Value Decomposition</p> <p><span class="figure-number">Figure 22: </span>Decoupled plant \(\bm{G}_{SVD}\) using the Singular Value Decomposition</p>
</div> </div>
<p> <p>
@ -1289,11 +1356,11 @@ The decoupled plant is then:
</div> </div>
</div> </div>
<div id="outline-container-org4b81d86" class="outline-3"> <div id="outline-container-orga514e47" class="outline-3">
<h3 id="org4b81d86"><span class="section-number-3">2.6</span> Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</h3> <h3 id="orga514e47"><span class="section-number-3">2.5</span> Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</h3>
<div class="outline-text-3" id="text-2-6"> <div class="outline-text-3" id="text-2-5">
<p> <p>
<a id="org9102f7f"></a> <a id="orgecdb237"></a>
</p> </p>
<p> <p>
@ -1309,17 +1376,21 @@ The &ldquo;Gershgorin Radii&rdquo; of a matrix \(S\) is defined by:
This is computed over the following frequencies. This is computed over the following frequencies.
</p> </p>
<div id="org1169672" class="figure"> <div id="org78b193b" class="figure">
<p><img src="figs/simscape_model_gershgorin_radii.png" alt="simscape_model_gershgorin_radii.png" /> <p><img src="figs/simscape_model_gershgorin_radii.png" alt="simscape_model_gershgorin_radii.png" />
</p> </p>
<p><span class="figure-number">Figure 22: </span>Gershgorin Radii of the Coupled and Decoupled plants</p> <p><span class="figure-number">Figure 23: </span>Gershgorin Radii of the Coupled and Decoupled plants</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org2845b5e" class="outline-3"> <div id="outline-container-orgd5f8212" class="outline-3">
<h3 id="org2845b5e"><span class="section-number-3">2.7</span> Verification of the decoupling using the &ldquo;Relative Gain Array&rdquo;</h3> <h3 id="orgd5f8212"><span class="section-number-3">2.6</span> Verification of the decoupling using the &ldquo;Relative Gain Array&rdquo;</h3>
<div class="outline-text-3" id="text-2-7"> <div class="outline-text-3" id="text-2-6">
<p>
<a id="org577bb4e"></a>
</p>
<p> <p>
The relative gain array (RGA) is defined as: The relative gain array (RGA) is defined as:
</p> </p>
@ -1331,55 +1402,55 @@ where \(\times\) denotes an element by element multiplication and \(G(s)\) is an
</p> </p>
<p> <p>
The obtained RGA elements are shown in Figure <a href="#orga76f805">23</a>. The obtained RGA elements are shown in Figure <a href="#org3af8336">24</a>.
</p> </p>
<div id="orga76f805" class="figure"> <div id="org3af8336" class="figure">
<p><img src="figs/simscape_model_rga.png" alt="simscape_model_rga.png" /> <p><img src="figs/simscape_model_rga.png" alt="simscape_model_rga.png" />
</p> </p>
<p><span class="figure-number">Figure 23: </span>Obtained norm of RGA elements for the SVD decoupled plant and the Jacobian decoupled plant</p> <p><span class="figure-number">Figure 24: </span>Obtained norm of RGA elements for the SVD decoupled plant and the Jacobian decoupled plant</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-orgdb7f2df" class="outline-3"> <div id="outline-container-org887521e" class="outline-3">
<h3 id="orgdb7f2df"><span class="section-number-3">2.8</span> Obtained Decoupled Plants</h3> <h3 id="org887521e"><span class="section-number-3">2.7</span> Obtained Decoupled Plants</h3>
<div class="outline-text-3" id="text-2-8"> <div class="outline-text-3" id="text-2-7">
<p> <p>
<a id="orgf6dad38"></a> <a id="orgd33facb"></a>
</p> </p>
<p> <p>
The bode plot of the diagonal and off-diagonal elements of \(G_{SVD}\) are shown in Figure <a href="#org1707d8c">24</a>. The bode plot of the diagonal and off-diagonal elements of \(G_{SVD}\) are shown in Figure <a href="#org013de60">25</a>.
</p> </p>
<div id="org1707d8c" class="figure"> <div id="org013de60" class="figure">
<p><img src="figs/simscape_model_decoupled_plant_svd.png" alt="simscape_model_decoupled_plant_svd.png" /> <p><img src="figs/simscape_model_decoupled_plant_svd.png" alt="simscape_model_decoupled_plant_svd.png" />
</p> </p>
<p><span class="figure-number">Figure 24: </span>Decoupled Plant using SVD</p> <p><span class="figure-number">Figure 25: </span>Decoupled Plant using SVD</p>
</div> </div>
<p> <p>
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 <a href="#orga1bf560">25</a>. 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 <a href="#orgb13c9d7">26</a>.
</p> </p>
<div id="orga1bf560" class="figure"> <div id="orgb13c9d7" class="figure">
<p><img src="figs/simscape_model_decoupled_plant_jacobian.png" alt="simscape_model_decoupled_plant_jacobian.png" /> <p><img src="figs/simscape_model_decoupled_plant_jacobian.png" alt="simscape_model_decoupled_plant_jacobian.png" />
</p> </p>
<p><span class="figure-number">Figure 25: </span>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)</p> <p><span class="figure-number">Figure 26: </span>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)</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org0143a9d" class="outline-3"> <div id="outline-container-orga134c9c" class="outline-3">
<h3 id="org0143a9d"><span class="section-number-3">2.9</span> Diagonal Controller</h3> <h3 id="orga134c9c"><span class="section-number-3">2.8</span> Diagonal Controller</h3>
<div class="outline-text-3" id="text-2-9"> <div class="outline-text-3" id="text-2-8">
<p> <p>
<a id="orga082064"></a> <a id="org4708058"></a>
The control diagram for the centralized control is shown in Figure <a href="#org43eaa56">26</a>. The control diagram for the centralized control is shown in Figure <a href="#org9680611">27</a>.
</p> </p>
<p> <p>
@ -1388,22 +1459,22 @@ The Jacobian is used to convert forces in the cartesian frame to forces applied
</p> </p>
<div id="org43eaa56" class="figure"> <div id="org9680611" class="figure">
<p><img src="figs/centralized_control.png" alt="centralized_control.png" /> <p><img src="figs/centralized_control.png" alt="centralized_control.png" />
</p> </p>
<p><span class="figure-number">Figure 26: </span>Control Diagram for the Centralized control</p> <p><span class="figure-number">Figure 27: </span>Control Diagram for the Centralized control</p>
</div> </div>
<p> <p>
The SVD control architecture is shown in Figure <a href="#orgee73430">27</a>. The SVD control architecture is shown in Figure <a href="#org255f076">28</a>.
The matrices \(U\) and \(V\) are used to decoupled the plant \(G\). The matrices \(U\) and \(V\) are used to decoupled the plant \(G\).
</p> </p>
<div id="orgee73430" class="figure"> <div id="org255f076" class="figure">
<p><img src="figs/svd_control.png" alt="svd_control.png" /> <p><img src="figs/svd_control.png" alt="svd_control.png" />
</p> </p>
<p><span class="figure-number">Figure 27: </span>Control Diagram for the SVD control</p> <p><span class="figure-number">Figure 28: </span>Control Diagram for the SVD control</p>
</div> </div>
@ -1437,23 +1508,23 @@ G_svd = feedback(G, inv(V<span class="org-type">'</span>)<span class="org-type">
</div> </div>
<p> <p>
The obtained diagonal elements of the loop gains are shown in Figure <a href="#orgb699a1c">28</a>. The obtained diagonal elements of the loop gains are shown in Figure <a href="#orgf733562">29</a>.
</p> </p>
<div id="orgb699a1c" class="figure"> <div id="orgf733562" class="figure">
<p><img src="figs/stewart_comp_loop_gain_diagonal.png" alt="stewart_comp_loop_gain_diagonal.png" /> <p><img src="figs/stewart_comp_loop_gain_diagonal.png" alt="stewart_comp_loop_gain_diagonal.png" />
</p> </p>
<p><span class="figure-number">Figure 28: </span>Comparison of the diagonal elements of the loop gains for the SVD control architecture and the Jacobian one</p> <p><span class="figure-number">Figure 29: </span>Comparison of the diagonal elements of the loop gains for the SVD control architecture and the Jacobian one</p>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org7f0526e" class="outline-3"> <div id="outline-container-org962b3f8" class="outline-3">
<h3 id="org7f0526e"><span class="section-number-3">2.10</span> Closed-Loop system Performances</h3> <h3 id="org962b3f8"><span class="section-number-3">2.9</span> Closed-Loop system Performances</h3>
<div class="outline-text-3" id="text-2-10"> <div class="outline-text-3" id="text-2-9">
<p> <p>
<a id="org7c48c81"></a> <a id="orgc0d06ab"></a>
</p> </p>
<p> <p>
@ -1484,63 +1555,14 @@ ans =
<p> <p>
The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure <a href="#orga97d4c0">29</a>. The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure <a href="#orgc3d8e7f">30</a>.
</p> </p>
<div id="orga97d4c0" class="figure"> <div id="orgc3d8e7f" class="figure">
<p><img src="figs/stewart_platform_simscape_cl_transmissibility.png" alt="stewart_platform_simscape_cl_transmissibility.png" /> <p><img src="figs/stewart_platform_simscape_cl_transmissibility.png" alt="stewart_platform_simscape_cl_transmissibility.png" />
</p> </p>
<p><span class="figure-number">Figure 29: </span>Obtained Transmissibility</p> <p><span class="figure-number">Figure 30: </span>Obtained Transmissibility</p>
</div>
</div>
</div>
<div id="outline-container-org456839a" class="outline-3">
<h3 id="org456839a"><span class="section-number-3">2.11</span> Small error on the sensor location&#xa0;&#xa0;&#xa0;<span class="tag"><span class="no_export">no_export</span></span></h3>
<div class="outline-text-3" id="text-2-11">
<p>
Let&rsquo;s now consider a small position error of the sensor:
</p>
<div class="org-src-container">
<pre class="src src-matlab">sens_pos_error = [105 5 <span class="org-type">-</span>1]<span class="org-type">*</span>1e<span class="org-type">-</span>3; <span class="org-comment">% [m]</span>
</pre>
</div>
<p>
The system is identified again:
</p>
<div class="org-src-container">
<pre class="src src-matlab">Gx = Gu<span class="org-type">*</span>inv(J<span class="org-type">'</span>);
Gx.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">Gsvd = inv(U)<span class="org-type">*</span>Gu<span class="org-type">*</span>inv(V<span class="org-type">'</span>);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">L_cen = K_cen<span class="org-type">*</span>Gx;
G_cen = feedback(G, pinv(J<span class="org-type">'</span>)<span class="org-type">*</span>K_cen, [7<span class="org-type">:</span>12], [1<span class="org-type">:</span>6]);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">L_svd = K_svd<span class="org-type">*</span>Gsvd;
G_svd = feedback(G, inv(V<span class="org-type">'</span>)<span class="org-type">*</span>K_svd<span class="org-type">*</span>inv(U), [7<span class="org-type">:</span>12], [1<span class="org-type">:</span>6]);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">isstable(G_cen)
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">isstable(G_svd)
</pre>
</div> </div>
</div> </div>
</div> </div>
@ -1548,7 +1570,7 @@ G_svd = feedback(G, inv(V<span class="org-type">'</span>)<span class="org-type">
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2020-11-25 mer. 09:16</p> <p class="date">Created: 2020-11-25 mer. 09:32</p>
</div> </div>
</body> </body>
</html> </html>

338
index.org
View File

@ -1,4 +1,4 @@
#+TITLE: SVD Control #+TITLE: Diagonal control using the SVD and the Jacobian Matrix
:DRAWER: :DRAWER:
#+STARTUP: overview #+STARTUP: overview
@ -38,12 +38,34 @@
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png") #+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END: :END:
* Introduction :ignore:
In this document, the use of the Jacobian matrix and the Singular Value Decomposition to render a physical plant diagonal dominant is studied.
Then, a diagonal controller is used.
These two methods are tested on two plants:
- In Section [[sec:gravimeter]] on a 3-DoF gravimeter
- In Section [[sec:stewart_platform]] on a 6-DoF Stewart platform
* Gravimeter - Simscape Model * Gravimeter - Simscape Model
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle gravimeter/script.m :header-args:matlab+: :tangle gravimeter/script.m
:END: :END:
<<sec:gravimeter>>
** Introduction ** Introduction
In this part, diagonal control using both the SVD and the Jacobian matrices are applied on a gravimeter model:
- Section [[sec:gravimeter_model]]: the model is described and its parameters are defined.
- Section [[sec:gravimeter_identification]]: the plant dynamics from the actuators to the sensors is computed from a Simscape model.
- Section [[sec:gravimeter_jacobian_decoupling]]: the plant is decoupled using the Jacobian matrices.
- Section [[sec:gravimeter_svd_decoupling]]: the Singular Value Decomposition is performed on a real approximation of the plant transfer matrix and further use to decouple the system.
- Section [[sec:gravimeter_gershgorin_radii]]: the effectiveness of the decoupling is computed using the Gershorin radii
- Section [[sec:gravimeter_rga]]: the effectiveness of the decoupling is computed using the Relative Gain Array
- Section [[sec:gravimeter_decoupled_plant]]: the obtained decoupled plants are compared
- Section [[sec:gravimeter_diagonal_control]]: the diagonal controller is developed
- Section [[sec:gravimeter_closed_loop_results]]: the obtained closed-loop performances for the two methods are compared
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
@ -57,16 +79,24 @@
addpath('gravimeter'); addpath('gravimeter');
#+end_src #+end_src
** Simscape Model - Parameters
#+begin_src matlab #+begin_src matlab
freqs = logspace(-1, 2, 1000);
#+end_src
** Gravimeter Model - Parameters
<<sec:gravimeter_model>>
#+begin_src matlab :exports none
open('gravimeter.slx') open('gravimeter.slx')
#+end_src #+end_src
The model of the gravimeter is schematically shown in Figure [[fig:gravimeter_model]].
#+name: fig:gravimeter_model #+name: fig:gravimeter_model
#+caption: Model of the gravimeter #+caption: Model of the gravimeter
[[file:figs/gravimeter_model.png]] [[file:figs/gravimeter_model.png]]
Parameters The parameters used for the simulation are the following:
#+begin_src matlab #+begin_src matlab
l = 1.0; % Length of the mass [m] l = 1.0; % Length of the mass [m]
h = 1.7; % Height of the mass [m] h = 1.7; % Height of the mass [m]
@ -85,7 +115,9 @@ Parameters
g = 0; % Gravity [m/s2] g = 0; % Gravity [m/s2]
#+end_src #+end_src
** System Identification - Without Gravity ** System Identification
<<sec:gravimeter_identification>>
#+begin_src matlab #+begin_src matlab
%% Name of the Simulink File %% Name of the Simulink File
mdl = 'gravimeter'; mdl = 'gravimeter';
@ -155,8 +187,6 @@ As expected, the plant as 6 states (2 translations + 1 rotation)
The bode plot of all elements of the plant are shown in Figure [[fig:open_loop_tf]]. The bode plot of all elements of the plant are shown in Figure [[fig:open_loop_tf]].
#+begin_src matlab :exports none #+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(4, 3, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(4, 3, 'TileSpacing', 'None', 'Padding', 'None');
@ -191,7 +221,7 @@ The bode plot of all elements of the plant are shown in Figure [[fig:open_loop_t
#+RESULTS: #+RESULTS:
[[file:figs/open_loop_tf.png]] [[file:figs/open_loop_tf.png]]
** Physical Decoupling using the Jacobian ** Decoupling using the Jacobian
<<sec:gravimeter_jacobian_decoupling>> <<sec:gravimeter_jacobian_decoupling>>
Consider the control architecture shown in Figure [[fig:gravimeter_decouple_jacobian]]. Consider the control architecture shown in Figure [[fig:gravimeter_decouple_jacobian]].
@ -265,8 +295,6 @@ And the plant $\bm{G}_x$ is computed:
The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravimeter_jacobian_plant]]. The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravimeter_jacobian_plant]].
#+begin_src matlab :exports none #+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure; figure;
% Magnitude % Magnitude
@ -299,12 +327,11 @@ The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravim
#+RESULTS: #+RESULTS:
[[file:figs/gravimeter_jacobian_plant.png]] [[file:figs/gravimeter_jacobian_plant.png]]
** SVD Decoupling ** Decoupling using the SVD
<<sec:gravimeter_svd_decoupling>> <<sec:gravimeter_svd_decoupling>>
In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required. In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required.
Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G(j\omega_c)$ from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency $\omega_c$. Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G(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 #+begin_src matlab
wc = 2*pi*10; % Decoupling frequency [rad/s] wc = 2*pi*10; % Decoupling frequency [rad/s]
@ -386,8 +413,6 @@ The 4th output (corresponding to the null singular value) is discarded, and we o
The diagonal and off-diagonal elements of the "SVD" plant are shown in Figure [[fig:gravimeter_svd_plant]]. The diagonal and off-diagonal elements of the "SVD" plant are shown in Figure [[fig:gravimeter_svd_plant]].
#+begin_src matlab :exports none #+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure; figure;
% Magnitude % Magnitude
@ -421,18 +446,13 @@ The diagonal and off-diagonal elements of the "SVD" plant are shown in Figure [[
[[file:figs/gravimeter_svd_plant.png]] [[file:figs/gravimeter_svd_plant.png]]
** Verification of the decoupling using the "Gershgorin Radii" ** Verification of the decoupling using the "Gershgorin Radii"
<<sec:comp_decoupling>> <<sec:gravimeter_gershgorin_radii>>
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" 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: 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)|} \] \[ \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 #+begin_src matlab :exports none
% Gershgorin Radii for the coupled plant: % Gershgorin Radii for the coupled plant:
Gr_coupled = zeros(length(freqs), size(G,2)); Gr_coupled = zeros(length(freqs), size(G,2));
@ -473,7 +493,7 @@ This is computed over the following frequencies.
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off; hold off;
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii') xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
legend('location', 'northwest'); legend('location', 'southwest');
ylim([1e-4, 1e2]); ylim([1e-4, 1e2]);
#+end_src #+end_src
@ -486,14 +506,100 @@ This is computed over the following frequencies.
#+RESULTS: #+RESULTS:
[[file:figs/gravimeter_gershgorin_radii.png]] [[file:figs/gravimeter_gershgorin_radii.png]]
** Verification of the decoupling using the "Relative Gain Array"
<<sec:gravimeter_rga>>
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:gravimeter_rga]].
#+begin_src matlab :exports none
% 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:3
for i_out = [1:i_in-1, i_in+1:3]
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:3
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:3
for i_out = [1:i_in-1, i_in+1:3]
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:3
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/gravimeter_rga.pdf', 'width', 'wide', 'height', 'tall');
#+end_src
#+name: fig:gravimeter_rga
#+caption: Obtained norm of RGA elements for the SVD decoupled plant and the Jacobian decoupled plant
#+RESULTS:
[[file:figs/gravimeter_rga.png]]
** Obtained Decoupled Plants ** Obtained Decoupled Plants
<<sec:gravimeter_decoupled_plant>> <<sec:gravimeter_decoupled_plant>>
The bode plot of the diagonal and off-diagonal elements of $G_{SVD}$ are shown in Figure [[fig:gravimeter_decoupled_plant_svd]]. The bode plot of the diagonal and off-diagonal elements of $G_{SVD}$ are shown in Figure [[fig:gravimeter_decoupled_plant_svd]].
#+begin_src matlab :exports none #+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
@ -546,8 +652,6 @@ The bode plot of the diagonal and off-diagonal elements of $G_{SVD}$ are shown i
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:gravimeter_decoupled_plant_jacobian]]. 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:gravimeter_decoupled_plant_jacobian]].
#+begin_src matlab :exports none #+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
@ -686,8 +790,6 @@ $G_0$ is tuned such that the crossover frequency corresponding to the diagonal t
The obtained diagonal elements of the loop gains are shown in Figure [[fig:gravimeter_comp_loop_gain_diagonal]]. The obtained diagonal elements of the loop gains are shown in Figure [[fig:gravimeter_comp_loop_gain_diagonal]].
#+begin_src matlab :exports none #+begin_src matlab :exports none
freqs = logspace(-1, 2, 1000);
figure; figure;
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
@ -806,7 +908,7 @@ The obtained transmissibility in Open-loop, for the centralized control as well
linkaxes([ax1,ax2,ax3],'xy'); linkaxes([ax1,ax2,ax3],'xy');
xlim([freqs(1), freqs(end)]); xlim([freqs(1), freqs(end)]);
ylim([1e-7, 1e-2]); xlim([1e-2, 5e1]); ylim([1e-7, 1e-2]);
#+end_src #+end_src
#+begin_src matlab :tangle no :exports results :results file replace #+begin_src matlab :tangle no :exports results :results file replace
@ -820,8 +922,10 @@ The obtained transmissibility in Open-loop, for the centralized control as well
* Stewart Platform - Simscape Model * Stewart Platform - Simscape Model
:PROPERTIES: :PROPERTIES:
:header-args:matlab+: :tangle stewart_platform/simscape_model.m :header-args:matlab+: :tangle stewart_platform/script.m
:END: :END:
<<sec:stewart_platform>>
** Introduction :ignore: ** Introduction :ignore:
In this analysis, we wish to applied SVD control to the Stewart Platform shown in Figure [[fig:SP_assembly]]. In this analysis, we wish to applied SVD control to the Stewart Platform shown in Figure [[fig:SP_assembly]].
@ -835,13 +939,13 @@ Some notes about the system:
#+caption: Stewart Platform CAD View #+caption: Stewart Platform CAD View
[[file:figs/SP_assembly.png]] [[file:figs/SP_assembly.png]]
The analysis of the SVD control applied to the Stewart platform is performed in the following sections: The analysis of the SVD/Jacobian 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 - Section [[sec:stewart_simscape]]: The parameters of the Simscape model of the Stewart platform are defined
- Section [[sec:stewart_identification]]: The plant is identified from the Simscape model and the system coupling is shown - 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 - Section [[sec:stewart_jacobian_decoupling]]: The plant is first decoupled using the Jacobian
- Section [[sec:stewart_real_approx]]: A real approximation of the plant is computed for further decoupling using the Singular Value Decomposition (SVD) - Section [[sec:stewart_svd_decoupling]]: The decoupling is performed thanks to the SVD. To do so a real approximation of the plant is computed.
- Section [[sec:stewart_svd_decoupling]]: The decoupling is performed thanks to the SVD - Section [[sec:stewart_gershorin_radii]]: The effectiveness of the decoupling with the Jacobian and SVD are compared using the Gershorin Radii
- Section [[sec:comp_decoupling]]: The effectiveness of the decoupling with the Jacobian and SVD are compared using the Gershorin Radii - Section [[sec:stewart_rga]]:
- Section [[sec:stewart_decoupled_plant]]: The dynamics of the decoupled plants are shown - Section [[sec:stewart_decoupled_plant]]: The dynamics of the decoupled plants are shown
- Section [[sec:stewart_diagonal_control]]: A diagonal controller is defined to control the decoupled plant - 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 - Section [[sec:stewart_closed_loop_results]]: Finally, the closed loop system properties are studied
@ -1047,7 +1151,7 @@ One can easily see that the system is strongly coupled.
#+RESULTS: #+RESULTS:
[[file:figs/stewart_platform_coupled_plant.png]] [[file:figs/stewart_platform_coupled_plant.png]]
** Physical Decoupling using the Jacobian ** Decoupling using the Jacobian
<<sec:stewart_jacobian_decoupling>> <<sec:stewart_jacobian_decoupling>>
Consider the control architecture shown in Figure [[fig:plant_decouple_jacobian]]. 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. The Jacobian matrix is used to transform forces/torques applied on the top platform to the equivalent forces applied by each actuator.
@ -1099,8 +1203,10 @@ $G_x(s)$ correspond to the transfer function from forces and torques applied to
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src #+end_src
** Real Approximation of $G$ at the decoupling frequency ** Decoupling using the SVD
<<sec:stewart_real_approx>> <<sec:stewart_svd_decoupling>>
In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required.
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$. 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 #+begin_src matlab
@ -1146,10 +1252,7 @@ This can be verified below where only the real value of $G_u(\omega_c)$ is shown
| -162.0 | -237.0 | -237.0 | -162.0 | 398.9 | 398.9 | | -162.0 | -237.0 | -237.0 | -162.0 | 398.9 | 398.9 |
| 220.6 | -220.6 | 220.6 | -220.6 | 220.6 | -220.6 | | 220.6 | -220.6 | 220.6 | -220.6 | 220.6 | -220.6 |
** SVD Decoupling Now, the Singular Value Decomposition of $H_1$ is performed:
<<sec:stewart_svd_decoupling>>
First, the Singular Value Decomposition of $H_1$ is performed:
\[ H_1 = U \Sigma V^H \] \[ H_1 = U \Sigma V^H \]
#+begin_src matlab #+begin_src matlab
@ -1217,7 +1320,7 @@ The decoupled plant is then:
#+end_src #+end_src
** Verification of the decoupling using the "Gershgorin Radii" ** Verification of the decoupling using the "Gershgorin Radii"
<<sec:comp_decoupling>> <<sec:stewart_gershorin_radii>>
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" is computed for the coupled plant $G(s)$, for the "Jacobian plant" $G_x(s)$ and the "SVD Decoupled Plant" $G_{SVD}(s)$:
@ -1279,6 +1382,8 @@ This is computed over the following frequencies.
[[file:figs/simscape_model_gershgorin_radii.png]] [[file:figs/simscape_model_gershgorin_radii.png]]
** Verification of the decoupling using the "Relative Gain Array" ** Verification of the decoupling using the "Relative Gain Array"
<<sec:stewart_rga>>
The relative gain array (RGA) is defined as: The relative gain array (RGA) is defined as:
\begin{equation} \begin{equation}
\Lambda\big(G(s)\big) = G(s) \times \big( G(s)^{-1} \big)^T \Lambda\big(G(s)\big) = G(s) \times \big( G(s)^{-1} \big)^T
@ -1713,156 +1818,3 @@ The obtained transmissibility in Open-loop, for the centralized control as well
#+RESULTS: #+RESULTS:
[[file:figs/stewart_platform_simscape_cl_transmissibility.png]] [[file:figs/stewart_platform_simscape_cl_transmissibility.png]]
** 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

196
stewart_platform/analytical_model.m
View File

@ -1,196 +0,0 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Bode plot options
opts = bodeoptions('cstprefs');
opts.FreqUnits = 'Hz';
opts.MagUnits = 'abs';
opts.MagScale = 'log';
opts.PhaseWrapping = 'on';
opts.xlim = [1 1000];
% Characteristics
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)]
Rx = 0.04; % ?
Rz = 0.04; % ?
Ix = m*Rx^2; % ?
Iy = m*Rx^2; % ?
Iz = m*Rz^2; % ?
% Mass Matrix
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];
% Jacobian Matrix
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)];
% Stifnness and Damping matrices
kv = k/3; % Vertical Stiffness of the springs [N/m]
kh = 0.5*k/3; % Horizontal Stiffness of the springs [N/m]
K = diag([3*kh, 3*kh, 3*kv, 3*kv*Rx^2/2, 3*kv*Rx^2/2, 3*kh*Rx^2]); % Stiffness Matrix
C = c*K/100000; % Damping Matrix
% State Space System
A = [ zeros(6) eye(6); ...
-M\K -M\C];
Bw = [zeros(6); -eye(6)];
Bu = [zeros(6); M\Bj];
Co = [-M\K -M\C];
D = [zeros(6) M\Bj];
ST = ss(A,[Bw Bu],Co,D);
% - OUT 1-6: 6 dof
% - IN 1-6 : ground displacement in the directions of the legs
% - IN 7-12: forces in the actuators.
ST.StateName = {'x';'y';'z';'theta_x';'theta_y';'theta_z';...
'dx';'dy';'dz';'dtheta_x';'dtheta_y';'dtheta_z'};
ST.InputName = {'w1';'w2';'w3';'w4';'w5';'w6';...
'u1';'u2';'u3';'u4';'u5';'u6'};
ST.OutputName = {'ax';'ay';'az';'atheta_x';'atheta_y';'atheta_z'};
% Transmissibility
TR=ST*[eye(6); zeros(6)];
figure
subplot(231)
bodemag(TR(1,1));
subplot(232)
bodemag(TR(2,2));
subplot(233)
bodemag(TR(3,3));
subplot(234)
bodemag(TR(4,4));
subplot(235)
bodemag(TR(5,5));
subplot(236)
bodemag(TR(6,6));
% Real approximation of $G(j\omega)$ at decoupling frequency
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
% Coupled and Decoupled Plant "Gershgorin Radii"
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')
% #+name: fig:gershorin_raddii_coupled_analytical
% #+caption: Gershorin Raddi for the coupled plant
% #+RESULTS:
% [[file:figs/gershorin_raddii_coupled_analytical.png]]
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')
% Decoupled Plant
figure;
bodemag(U'*sys1*V,opts)
% Controller
fc = 2*pi*0.1; % Crossover Frequency [rad/s]
c_gain = 50; %
cont = eye(6)*c_gain/(s+fc);
% Closed Loop System
FEEDIN = [7:12]; % Input of controller
FEEDOUT = [1:6]; % Output of controller
% Centralized Control
STcen = feedback(ST, inv(Bj)*cont, FEEDIN, FEEDOUT);
TRcen = STcen*[eye(6); zeros(6)];
% SVD Control
STsvd = feedback(ST, pinv(V')*cont*pinv(U), FEEDIN, FEEDOUT);
TRsvd = STsvd*[eye(6); zeros(6)];
% Results
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')

BIN
stewart_platform/drone_platform.slx
View File

Binary file not shown.

156
stewart_platform/drone_platform_DataFile.m
View File

@ -1,156 +0,0 @@
% Simscape(TM) Multibody(TM) version: 5.1
% This is a model data file derived from a Simscape Multibody Import XML file using the smimport function.
% The data in this file sets the block parameter values in an imported Simscape Multibody model.
% For more information on this file, see the smimport function help page in the Simscape Multibody documentation.
% You can modify numerical values, but avoid any other changes to this file.
% Do not add code to this file. Do not edit the physical units shown in comments.
%%%VariableName:smiData
%============= RigidTransform =============%
%Initialize the RigidTransform structure array by filling in null values.
smiData.RigidTransform(12).translation = [0.0 0.0 0.0];
smiData.RigidTransform(12).angle = 0.0;
smiData.RigidTransform(12).axis = [0.0 0.0 0.0];
smiData.RigidTransform(12).ID = '';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(1).translation = [17.500000000021 30.310889132419 5.1574606296500001]; % mm
smiData.RigidTransform(1).angle = 0.093902078374528131; % rad
smiData.RigidTransform(1).axis = [0 0 1];
smiData.RigidTransform(1).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Spring cylinder.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(2).translation = [0 0 0]; % mm
smiData.RigidTransform(2).angle = 0; % rad
smiData.RigidTransform(2).axis = [0 0 0];
smiData.RigidTransform(2).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Upper_Stewart_Platform_V8.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(3).translation = [17.500000000021 -30.310889132492001 5.1574606296500001]; % mm
smiData.RigidTransform(3).angle = 0.093902078374528131; % rad
smiData.RigidTransform(3).axis = [0 0 1];
smiData.RigidTransform(3).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Spring cylinder.CATPart-3]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(4).translation = [9.6240451994409995 -30.811470883506999 19.383957937758002]; % mm
smiData.RigidTransform(4).angle = 3.1415926535897931; % rad
smiData.RigidTransform(4).axis = [0.88807383397699313 6.3170900042374556e-16 -0.45970084338121897];
smiData.RigidTransform(4).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(5).translation = [21.871493913361999 -23.740403071639001 19.383957937758002]; % mm
smiData.RigidTransform(5).angle = 2.6777446800421498; % rad
smiData.RigidTransform(5).axis = [-0.79025275363749503 -0.45625264004045041 -0.40906492617245471];
smiData.RigidTransform(5).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-2]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(6).translation = [21.871493913361 23.740403071566 19.383957937758002]; % mm
smiData.RigidTransform(6).angle = 2.3226757410894345; % rad
smiData.RigidTransform(6).axis = [0.4840501729431676 0.83839949295006799 -0.25056280708588496];
smiData.RigidTransform(6).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-3]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(7).translation = [9.6240451994409995 30.811470883434001 19.383957937758002]; % mm
smiData.RigidTransform(7).angle = 2.1862760354647519; % rad
smiData.RigidTransform(7).axis = [5.6189562004407762e-16 -1 -1.3924588923326856e-16];
smiData.RigidTransform(7).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-4]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(8).translation = [-31.495539112739003 7.0710678118320001 19.383957937758002]; % mm
smiData.RigidTransform(8).angle = 2.3226757410894345; % rad
smiData.RigidTransform(8).axis = [-0.48405017294316754 0.83839949295006766 0.25056280708588635];
smiData.RigidTransform(8).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-5]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(9).translation = [-31.495539112739003 -7.0710678119050003 19.383957937758002]; % mm
smiData.RigidTransform(9).angle = 2.6777446800421507; % rad
smiData.RigidTransform(9).axis = [0.79025275363749525 -0.45625264004045002 0.40906492617245482];
smiData.RigidTransform(9).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-6]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(10).translation = [0 0 0]; % mm
smiData.RigidTransform(10).angle = 0; % rad
smiData.RigidTransform(10).axis = [0 0 0];
smiData.RigidTransform(10).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Bottom_Stewart_Platform_V8.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(11).translation = [-34.999999999978996 -3.7000000000000001e-11 5.1574606296500001]; % mm
smiData.RigidTransform(11).angle = 0.093902078374528131; % rad
smiData.RigidTransform(11).axis = [0 0 1];
smiData.RigidTransform(11).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Spring cylinder.CATPart-2]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(12).translation = [0 0 0]; % mm
smiData.RigidTransform(12).angle = 0; % rad
smiData.RigidTransform(12).axis = [0 0 0];
smiData.RigidTransform(12).ID = 'RootGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1]';
%============= Solid =============%
%Center of Mass (CoM) %Moments of Inertia (MoI) %Product of Inertia (PoI)
%Initialize the Solid structure array by filling in null values.
smiData.Solid(4).mass = 0.0;
smiData.Solid(4).CoM = [0.0 0.0 0.0];
smiData.Solid(4).MoI = [0.0 0.0 0.0];
smiData.Solid(4).PoI = [0.0 0.0 0.0];
smiData.Solid(4).color = [0.0 0.0 0.0];
smiData.Solid(4).opacity = 0.0;
smiData.Solid(4).ID = '';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(1).mass = 0.002953024625510399; % kg
smiData.Solid(1).CoM = [0.00019528918640542437 -0.0030390869707000701 3.4103846069334764]; % mm
smiData.Solid(1).MoI = [0.094245235034989605 0.095111221695610218 0.060564976663331278]; % kg*mm^2
smiData.Solid(1).PoI = [9.2822238167834426e-07 0.0015998098386190715 -2.9934446424201115e-05]; % kg*mm^2
smiData.Solid(1).color = [0.82352941176470584 0.82352941176470584 1];
smiData.Solid(1).opacity = 1;
smiData.Solid(1).ID = 'Spring cylinder.CATPart*:*Default';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(2).mass = 0.200; % kg
smiData.Solid(2).CoM = [0 0 22.029956321592298]; % mm
smiData.Solid(2).MoI = [10.958807269683136 10.958651478878741 21.418645161274547]; % kg*mm^2
smiData.Solid(2).PoI = [-2.7272640778847972e-06 2.0116845742236026e-06 2.0925824216820959e-06]; % kg*mm^2
smiData.Solid(2).color = [0 1 1];
smiData.Solid(2).opacity = 1;
smiData.Solid(2).ID = 'Upper_Stewart_Platform_V8.CATPart*:*Default';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(3).mass = 0.00072851613140055473; % kg
smiData.Solid(3).CoM = [1.6672752107505068e-07 0.049264195897497115 6.8426699166844651]; % mm
smiData.Solid(3).MoI = [0.023642069482559629 0.023409631588225184 0.0076150552208997047]; % kg*mm^2
smiData.Solid(3).PoI = [0.00019174011014260877 5.6635742238393906e-10 -6.4605353421422721e-10]; % kg*mm^2
smiData.Solid(3).color = [0.82352941176470584 0.82352941176470584 1];
smiData.Solid(3).opacity = 1;
smiData.Solid(3).ID = 'Voice-coil.CATPart*:*Default';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(4).mass = 0.036808422349281396; % kg
smiData.Solid(4).CoM = [1.5151381609408909e-08 -2.5842738425576439e-08 3.8488764942888207]; % mm
smiData.Solid(4).MoI = [16.573140175725058 16.573140156158694 32.475141872846649]; % kg*mm^2
smiData.Solid(4).PoI = [4.9976421207227284e-09 -2.8421200098863714e-09 -1.3395036264003659e-08]; % kg*mm^2
smiData.Solid(4).color = [1 1 0];
smiData.Solid(4).opacity = 1;
smiData.Solid(4).ID = 'Bottom_Stewart_Platform_V8.CATPart*:*Default';

156
stewart_platform/drone_platform_DataFile1.m
View File

@ -1,156 +0,0 @@
% Simscape(TM) Multibody(TM) version: 5.1
% This is a model data file derived from a Simscape Multibody Import XML file using the smimport function.
% The data in this file sets the block parameter values in an imported Simscape Multibody model.
% For more information on this file, see the smimport function help page in the Simscape Multibody documentation.
% You can modify numerical values, but avoid any other changes to this file.
% Do not add code to this file. Do not edit the physical units shown in comments.
%%%VariableName:smiData
%============= RigidTransform =============%
%Initialize the RigidTransform structure array by filling in null values.
smiData.RigidTransform(12).translation = [0.0 0.0 0.0];
smiData.RigidTransform(12).angle = 0.0;
smiData.RigidTransform(12).axis = [0.0 0.0 0.0];
smiData.RigidTransform(12).ID = '';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(1).translation = [17.500000000021 30.310889132419 5.1574606296500001]; % mm
smiData.RigidTransform(1).angle = 0.093902078374528131; % rad
smiData.RigidTransform(1).axis = [0 0 1];
smiData.RigidTransform(1).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Spring cylinder.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(2).translation = [0 0 0]; % mm
smiData.RigidTransform(2).angle = 0; % rad
smiData.RigidTransform(2).axis = [0 0 0];
smiData.RigidTransform(2).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Upper_Stewart_Platform_V8.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(3).translation = [17.500000000021 -30.310889132492001 5.1574606296500001]; % mm
smiData.RigidTransform(3).angle = 0.093902078374528131; % rad
smiData.RigidTransform(3).axis = [0 0 1];
smiData.RigidTransform(3).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Spring cylinder.CATPart-3]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(4).translation = [9.6240451994409995 -30.811470883506999 19.383957937758002]; % mm
smiData.RigidTransform(4).angle = 3.1415926535897931; % rad
smiData.RigidTransform(4).axis = [0.88807383397699313 6.3170900042374556e-16 -0.45970084338121897];
smiData.RigidTransform(4).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(5).translation = [21.871493913361999 -23.740403071639001 19.383957937758002]; % mm
smiData.RigidTransform(5).angle = 2.6777446800421498; % rad
smiData.RigidTransform(5).axis = [-0.79025275363749503 -0.45625264004045041 -0.40906492617245471];
smiData.RigidTransform(5).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-2]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(6).translation = [21.871493913361 23.740403071566 19.383957937758002]; % mm
smiData.RigidTransform(6).angle = 2.3226757410894345; % rad
smiData.RigidTransform(6).axis = [0.4840501729431676 0.83839949295006799 -0.25056280708588496];
smiData.RigidTransform(6).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-3]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(7).translation = [9.6240451994409995 30.811470883434001 19.383957937758002]; % mm
smiData.RigidTransform(7).angle = 2.1862760354647519; % rad
smiData.RigidTransform(7).axis = [5.6189562004407762e-16 -1 -1.3924588923326856e-16];
smiData.RigidTransform(7).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-4]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(8).translation = [-31.495539112739003 7.0710678118320001 19.383957937758002]; % mm
smiData.RigidTransform(8).angle = 2.3226757410894345; % rad
smiData.RigidTransform(8).axis = [-0.48405017294316754 0.83839949295006766 0.25056280708588635];
smiData.RigidTransform(8).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-5]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(9).translation = [-31.495539112739003 -7.0710678119050003 19.383957937758002]; % mm
smiData.RigidTransform(9).angle = 2.6777446800421507; % rad
smiData.RigidTransform(9).axis = [0.79025275363749525 -0.45625264004045002 0.40906492617245482];
smiData.RigidTransform(9).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Voice-coil.CATPart-6]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(10).translation = [0 0 0]; % mm
smiData.RigidTransform(10).angle = 0; % rad
smiData.RigidTransform(10).axis = [0 0 0];
smiData.RigidTransform(10).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Bottom_Stewart_Platform_V8.CATPart-1]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(11).translation = [-34.999999999978996 -3.7000000000000001e-11 5.1574606296500001]; % mm
smiData.RigidTransform(11).angle = 0.093902078374528131; % rad
smiData.RigidTransform(11).axis = [0 0 1];
smiData.RigidTransform(11).ID = 'AssemblyGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1:Spring cylinder.CATPart-2]';
%Translation Method - Cartesian
%Rotation Method - Arbitrary Axis
smiData.RigidTransform(12).translation = [0 0 0]; % mm
smiData.RigidTransform(12).angle = 0; % rad
smiData.RigidTransform(12).axis = [0 0 0];
smiData.RigidTransform(12).ID = 'RootGround[Drone_Stewart_Platform_V8 voice-coil.CATProduct-1]';
%============= Solid =============%
%Center of Mass (CoM) %Moments of Inertia (MoI) %Product of Inertia (PoI)
%Initialize the Solid structure array by filling in null values.
smiData.Solid(4).mass = 0.0;
smiData.Solid(4).CoM = [0.0 0.0 0.0];
smiData.Solid(4).MoI = [0.0 0.0 0.0];
smiData.Solid(4).PoI = [0.0 0.0 0.0];
smiData.Solid(4).color = [0.0 0.0 0.0];
smiData.Solid(4).opacity = 0.0;
smiData.Solid(4).ID = '';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(1).mass = 0.002953024625510399; % kg
smiData.Solid(1).CoM = [0.00019528918640542437 -0.0030390869707000701 3.4103846069334764]; % mm
smiData.Solid(1).MoI = [0.094245235034989605 0.095111221695610218 0.060564976663331278]; % kg*mm^2
smiData.Solid(1).PoI = [9.2822238167834426e-07 0.0015998098386190715 -2.9934446424201115e-05]; % kg*mm^2
smiData.Solid(1).color = [0.82352941176470584 0.82352941176470584 1];
smiData.Solid(1).opacity = 1;
smiData.Solid(1).ID = 'Spring cylinder.CATPart*:*Default';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(2).mass = 0.032807859410420866; % kg
smiData.Solid(2).CoM = [4.3204111866222267e-05 6.4929928363256894e-06 22.029956321592298]; % mm
smiData.Solid(2).MoI = [10.958807269683136 10.958651478878741 21.418645161274547]; % kg*mm^2
smiData.Solid(2).PoI = [-2.7272640778847972e-06 2.0116845742236026e-06 2.0925824216820959e-06]; % kg*mm^2
smiData.Solid(2).color = [0 1 1];
smiData.Solid(2).opacity = 1;
smiData.Solid(2).ID = 'Upper_Stewart_Platform_V8.CATPart*:*Default';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(3).mass = 0.00072851613140055473; % kg
smiData.Solid(3).CoM = [1.6672752107505068e-07 0.049264195897497115 6.8426699166844651]; % mm
smiData.Solid(3).MoI = [0.023642069482559629 0.023409631588225184 0.0076150552208997047]; % kg*mm^2
smiData.Solid(3).PoI = [0.00019174011014260877 5.6635742238393906e-10 -6.4605353421422721e-10]; % kg*mm^2
smiData.Solid(3).color = [0.82352941176470584 0.82352941176470584 1];
smiData.Solid(3).opacity = 1;
smiData.Solid(3).ID = 'Voice-coil.CATPart*:*Default';
%Inertia Type - Custom
%Visual Properties - Simple
smiData.Solid(4).mass = 0.036808422349281396; % kg
smiData.Solid(4).CoM = [1.5151381609408909e-08 -2.5842738425576439e-08 3.8488764942888207]; % mm
smiData.Solid(4).MoI = [16.573140175725058 16.573140156158694 32.475141872846649]; % kg*mm^2
smiData.Solid(4).PoI = [4.9976421207227284e-09 -2.8421200098863714e-09 -1.3395036264003659e-08]; % kg*mm^2
smiData.Solid(4).color = [1 1 0];
smiData.Solid(4).opacity = 1;
smiData.Solid(4).ID = 'Bottom_Stewart_Platform_V8.CATPart*:*Default';

BIN
stewart_platform/drone_platform_R2017b.slx
View File

Binary file not shown.

BIN
stewart_platform/drone_platform_jacobian.slx
View File

Binary file not shown.

159
stewart_platform/simscape_model.m → stewart_platform/script.m
View File

@ -132,8 +132,10 @@ legend('location', 'northwest');
Gx = Gu*inv(J'); Gx = Gu*inv(J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
% Real Approximation of $G$ at the decoupling frequency % Decoupling using the SVD
% <<sec:stewart_real_approx>> % <<sec:stewart_svd_decoupling>>
% In order to decouple the plant using the SVD, first a real approximation of the plant transfer function matrix as the crossover frequency is required.
% 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$. % 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$.
@ -148,10 +150,18 @@ H1 = evalfr(Gu, j*wc);
D = pinv(real(H1'*H1)); D = pinv(real(H1'*H1));
H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2)))); H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
% SVD Decoupling
% <<sec:stewart_svd_decoupling>>
% First, the Singular Value Decomposition of $H_1$ is performed:
% #+caption: Real part 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 |
% Now, the Singular Value Decomposition of $H_1$ is performed:
% \[ H_1 = U \Sigma V^H \] % \[ H_1 = U \Sigma V^H \]
@ -171,7 +181,7 @@ H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
Gsvd = inv(U)*Gu*inv(V'); Gsvd = inv(U)*Gu*inv(V');
% Verification of the decoupling using the "Gershgorin Radii" % Verification of the decoupling using the "Gershgorin Radii"
% <<sec:comp_decoupling>> % <<sec:stewart_gershorin_radii>>
% 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" is computed for the coupled plant $G(s)$, for the "Jacobian plant" $G_x(s)$ and the "SVD Decoupled Plant" $G_{SVD}(s)$:
@ -221,6 +231,8 @@ legend('location', 'northwest');
ylim([1e-3, 1e3]); ylim([1e-3, 1e3]);
% Verification of the decoupling using the "Relative Gain Array" % Verification of the decoupling using the "Relative Gain Array"
% <<sec:stewart_rga>>
% The relative gain array (RGA) is defined as: % The relative gain array (RGA) is defined as:
% \begin{equation} % \begin{equation}
% \Lambda\big(G(s)\big) = G(s) \times \big( G(s)^{-1} \big)^T % \Lambda\big(G(s)\big) = G(s) \times \big( G(s)^{-1} \big)^T
@ -502,141 +514,6 @@ isstable(G_svd)
% 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]]. % 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]].
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]);
% Small error on the sensor location :no_export:
% Let's now consider a small position error of the sensor:
sens_pos_error = [105 5 -1]*1e-3; % [m]
% The system is identified again:
%% 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'});
Gx = Gu*inv(J');
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gsvd = inv(U)*Gu*inv(V');
% 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
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]);
L_cen = K_cen*Gx;
G_cen = feedback(G, pinv(J')*K_cen, [7:12], [1:6]);
L_svd = K_svd*Gsvd;
G_svd = feedback(G, inv(V')*K_svd*inv(U), [7:12], [1:6]);
isstable(G_cen)
isstable(G_svd)
figure; figure;
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None'); tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');