Clean simscape models
This commit is contained in:
parent
9b29c73fff
commit
a1581cb873
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
Before Width: | Height: | Size: 124 KiB After Width: | Height: | Size: 115 KiB |
2223
figs/gravimeter_rga.pdf
Normal file
2223
figs/gravimeter_rga.pdf
Normal file
File diff suppressed because it is too large
Load Diff
BIN
figs/gravimeter_rga.png
Normal file
BIN
figs/gravimeter_rga.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 164 KiB |
@ -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
|
||||
@ -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
|
||||
@ -4,13 +4,23 @@ clear; close all; clc;
|
||||
%% Intialize Laplace variable
|
||||
s = zpk('s');
|
||||
|
||||
% Simscape Model - Parameters
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
% Gravimeter Model - Parameters
|
||||
% <<sec:gravimeter_model>>
|
||||
|
||||
|
||||
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]
|
||||
h = 1.7; % Height of the mass [m]
|
||||
@ -22,13 +32,15 @@ m = 400; % Mass [kg]
|
||||
I = 115; % Inertia [kg m^2]
|
||||
|
||||
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]
|
||||
|
||||
g = 0; % Gravity [m/s2]
|
||||
|
||||
% System Identification - Without Gravity
|
||||
% System Identification
|
||||
% <<sec:gravimeter_identification>>
|
||||
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'gravimeter';
|
||||
@ -54,32 +66,21 @@ G.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'};
|
||||
% #+RESULTS:
|
||||
% [[file:figs/gravimeter_plant_schematic.png]]
|
||||
|
||||
% \begin{equation}
|
||||
% \bm{a} = \begin{bmatrix} a_{1x} \\ a_{1z} \\ a_{2x} \\ a_{2z} \end{bmatrix}
|
||||
% \end{equation}
|
||||
|
||||
% \begin{equation}
|
||||
% \bm{\tau} = \begin{bmatrix}\tau_1 \\ \tau_2 \\ \tau_2 \end{bmatrix}
|
||||
% \end{equation}
|
||||
|
||||
% We can check the poles of the plant:
|
||||
|
||||
|
||||
pole(G)
|
||||
|
||||
|
||||
|
||||
% #+RESULTS:
|
||||
% #+begin_example
|
||||
% -0.000183495485977108 + 13.546056874877i
|
||||
% -0.000183495485977108 - 13.546056874877i
|
||||
% -7.49842878906757e-05 + 8.65934902322567i
|
||||
% -7.49842878906757e-05 - 8.65934902322567i
|
||||
% -1.33171230256362e-05 + 3.64924169037897i
|
||||
% -1.33171230256362e-05 - 3.64924169037897i
|
||||
% #+end_example
|
||||
% | -0.12243+13.551i |
|
||||
% | -0.12243-13.551i |
|
||||
% | -0.05+8.6601i |
|
||||
% | -0.05-8.6601i |
|
||||
% | -0.0088785+3.6493i |
|
||||
% | -0.0088785-3.6493i |
|
||||
|
||||
% The plant as 6 states as expected (2 translations + 1 rotation)
|
||||
% As expected, the plant as 6 states (2 translations + 1 rotation)
|
||||
|
||||
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]].
|
||||
|
||||
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(4, 3, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
@ -124,7 +123,7 @@ end
|
||||
% #+RESULTS:
|
||||
% [[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
|
||||
0 1 -l/2
|
||||
@ -135,17 +134,25 @@ Jt = [1 0 ha
|
||||
0 1 -la
|
||||
0 1 la];
|
||||
|
||||
|
||||
|
||||
% And the plant $\bm{G}_x$ is computed:
|
||||
|
||||
Gx = pinv(Ja)*G*pinv(Jt');
|
||||
Gx.InputName = {'Fx', 'Fz', 'My'};
|
||||
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]].
|
||||
|
||||
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
% Magnitude
|
||||
@ -168,10 +175,12 @@ xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
legend('location', 'southeast');
|
||||
ylim([1e-8, 1e0]);
|
||||
|
||||
% Real Approximation of $G$ at the decoupling frequency
|
||||
% <<sec:gravimeter_real_approx>>
|
||||
% Decoupling using the SVD
|
||||
% <<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]
|
||||
|
||||
@ -182,16 +191,23 @@ H1 = evalfr(G, j*wc);
|
||||
% The real approximation is computed as follows:
|
||||
|
||||
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 \]
|
||||
|
||||
|
||||
[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]]
|
||||
|
||||
% 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');
|
||||
|
||||
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]].
|
||||
|
||||
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
% Magnitude
|
||||
@ -232,25 +257,22 @@ end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
||||
legend('location', 'southeast', 'FontSize', 8);
|
||||
legend('location', 'southwest', 'FontSize', 8);
|
||||
ylim([1e-8, 1e0]);
|
||||
|
||||
% TODO Verification of the decoupling using the "Gershgorin Radii"
|
||||
% <<sec:comp_decoupling>>
|
||||
% Verification of the decoupling using the "Gershgorin Radii"
|
||||
% <<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" of a matrix $S$ is defined by:
|
||||
% \[ \zeta_i(j\omega) = \frac{\sum\limits_{j\neq i}|S_{ij}(j\omega)|}{|S_{ii}(j\omega)|} \]
|
||||
|
||||
% This is computed over the following frequencies.
|
||||
|
||||
freqs = logspace(-2, 2, 1000); % [Hz]
|
||||
|
||||
% 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 = zeros(length(freqs), size(G,2));
|
||||
H = abs(squeeze(freqresp(G, freqs, 'Hz')));
|
||||
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, :));
|
||||
end
|
||||
|
||||
@ -273,7 +295,7 @@ 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
|
||||
for in_i = 2:3
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
@ -284,25 +306,99 @@ end
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
hold off;
|
||||
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
|
||||
legend('location', 'northwest');
|
||||
ylim([1e-3, 1e3]);
|
||||
legend('location', 'southwest');
|
||||
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>>
|
||||
|
||||
% 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;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i_in = 1:6
|
||||
for i_out = [1:i_in-1, i_in+1:6]
|
||||
for i_in = 1:3
|
||||
for i_out = [1:i_in-1, i_in+1:3]
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
@ -310,20 +406,20 @@ end
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
|
||||
'DisplayName', '$G_{SVD}(i,j),\ i \neq j$');
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
for ch_i = 1:6
|
||||
for ch_i = 1:3
|
||||
plot(freqs, abs(squeeze(freqresp(Gsvd(ch_i, ch_i), freqs, 'Hz'))), ...
|
||||
'DisplayName', sprintf('$G_{SVD}(%i,%i)$', ch_i, ch_i));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'northwest');
|
||||
ylim([1e-1, 1e5])
|
||||
legend('location', 'southwest');
|
||||
ylim([1e-8, 1e0])
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
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'))));
|
||||
end
|
||||
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
|
||||
% #+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;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
% Magnitude
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
for i_in = 1:6
|
||||
for i_out = [1:i_in-1, i_in+1:6]
|
||||
for i_in = 1:3
|
||||
for i_out = [1:i_in-1, i_in+1:3]
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
@ -361,41 +455,35 @@ end
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.5], ...
|
||||
'DisplayName', '$G_x(i,j),\ i \neq j$');
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))), 'DisplayName', '$G_x(1,1) = A_x/F_x$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))), 'DisplayName', '$G_x(2,2) = A_y/F_y$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))), 'DisplayName', '$G_x(3,3) = A_z/F_z$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))), 'DisplayName', '$G_x(4,4) = A_{R_x}/M_x$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))), 'DisplayName', '$G_x(5,5) = A_{R_y}/M_y$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))), 'DisplayName', '$G_x(6,6) = A_{R_z}/M_z$');
|
||||
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(2, 2), freqs, 'Hz'))), 'DisplayName', '$G_x(2,2) = A_y/F_y$');
|
||||
plot(freqs, abs(squeeze(freqresp(Gx(3, 3), freqs, 'Hz'))), 'DisplayName', '$G_x(3,3) = R_y/M_y$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Magnitude'); set(gca, 'XTickLabel',[]);
|
||||
legend('location', 'northwest');
|
||||
ylim([1e-2, 2e6])
|
||||
legend('location', 'southwest');
|
||||
ylim([1e-8, 1e0])
|
||||
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(1, 1), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(2, 2), freqs, 'Hz'))));
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx(3, 3), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([0, 180]);
|
||||
ylim([-180, 180]);
|
||||
yticks([0:45:360]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
|
||||
|
||||
% #+name: fig:svd_control
|
||||
% #+name: fig:svd_control_gravimeter
|
||||
% #+caption: Control Diagram for the SVD control
|
||||
% #+RESULTS:
|
||||
% [[file:figs/svd_control.png]]
|
||||
% [[file:figs/svd_control_gravimeter.png]]
|
||||
|
||||
|
||||
% 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$
|
||||
|
||||
|
||||
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]
|
||||
|
||||
K_cen = diag(1./diag(abs(evalfr(Gx, j*wc))))*(1/abs(evalfr(1/(1 + s/w0), j*wc)))/(1 + s/w0);
|
||||
L_cen = K_cen*Gx;
|
||||
G_cen = feedback(G, pinv(J')*K_cen, [7:12], [1:6]);
|
||||
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);
|
||||
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]].
|
||||
|
||||
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
@ -429,7 +516,7 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
ax1 = nexttile([2, 1]);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(L_svd(1, 1), freqs, 'Hz'))), 'DisplayName', '$L_{SVD}(i,i)$');
|
||||
for i_in_out = 2:6
|
||||
for i_in_out = 2:3
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, abs(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
|
||||
end
|
||||
@ -437,7 +524,7 @@ end
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(L_cen(1, 1), freqs, 'Hz'))), ...
|
||||
'DisplayName', '$L_{J}(i,i)$');
|
||||
for i_in_out = 2:6
|
||||
for i_in_out = 2:3
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, abs(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))), 'HandleVisibility', 'off');
|
||||
end
|
||||
@ -450,12 +537,12 @@ ylim([5e-2, 2e3])
|
||||
% Phase
|
||||
ax2 = nexttile;
|
||||
hold on;
|
||||
for i_in_out = 1:6
|
||||
for i_in_out = 1:3
|
||||
set(gca,'ColorOrderIndex',1)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(L_svd(i_in_out, i_in_out), freqs, 'Hz'))));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
for i_in_out = 1:6
|
||||
for i_in_out = 1:3
|
||||
set(gca,'ColorOrderIndex',2)
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(L_cen(i_in_out, i_in_out), freqs, 'Hz'))));
|
||||
end
|
||||
@ -467,7 +554,7 @@ yticks([-180:90:360]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
|
||||
% TODO Closed-Loop system Performances
|
||||
% Closed-Loop system Performances
|
||||
% <<sec:gravimeter_closed_loop_results>>
|
||||
|
||||
% Let's first verify the stability of the closed-loop systems:
|
||||
@ -497,53 +584,39 @@ isstable(G_svd)
|
||||
freqs = logspace(-2, 2, 1000);
|
||||
|
||||
figure;
|
||||
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
tiledlayout(1, 3, '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');
|
||||
plot(freqs, abs(squeeze(freqresp(G( 1,1)/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
|
||||
plot(freqs, abs(squeeze(freqresp(G_cen(1,1)/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
|
||||
plot(freqs, abs(squeeze(freqresp(G_svd(1,1)/s^2, freqs, 'Hz'))), '--', 'DisplayName', 'SVD');
|
||||
hold off;
|
||||
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');
|
||||
|
||||
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'))), '--');
|
||||
plot(freqs, abs(squeeze(freqresp(G( 2,2)/s^2, freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_cen(2,2)/s^2, freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_svd(2,2)/s^2, freqs, 'Hz'))), '--');
|
||||
hold off;
|
||||
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;
|
||||
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'))), '--');
|
||||
plot(freqs, abs(squeeze(freqresp(G( 3,3)/s^2, freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_cen(3,3)/s^2, freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G_svd(3,3)/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]');
|
||||
set(gca, 'YTickLabel',[]); xlabel('Frequency [Hz]');
|
||||
title('$R_y/R_{w,y}$');
|
||||
|
||||
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');
|
||||
linkaxes([ax1,ax2,ax3],'xy');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
ylim([1e-3, 1e2]);
|
||||
xlim([1e-2, 5e1]); ylim([1e-7, 1e-2]);
|
||||
|
||||
508
index.html
508
index.html
@ -3,9 +3,9 @@
|
||||
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
||||
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
||||
<head>
|
||||
<!-- 2020-11-25 mer. 09:16 -->
|
||||
<!-- 2020-11-25 mer. 09:32 -->
|
||||
<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="author" content="Dehaeze Thomas" />
|
||||
<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>
|
||||
</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">
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#org3a856a2">1. Gravimeter - Simscape Model</a>
|
||||
<li><a href="#org4bcea32">1. Gravimeter - Simscape Model</a>
|
||||
<ul>
|
||||
<li><a href="#org7cee37e">1.1. Introduction</a></li>
|
||||
<li><a href="#orgec45da7">1.2. Simscape Model - Parameters</a></li>
|
||||
<li><a href="#org30962d3">1.3. System Identification - Without Gravity</a></li>
|
||||
<li><a href="#org5b19440">1.4. Physical Decoupling using the Jacobian</a></li>
|
||||
<li><a href="#orgc8cb89d">1.5. SVD Decoupling</a></li>
|
||||
<li><a href="#orgd2f1c19">1.6. Verification of the decoupling using the “Gershgorin Radii”</a></li>
|
||||
<li><a href="#orgc761edd">1.7. Obtained Decoupled Plants</a></li>
|
||||
<li><a href="#org669229d">1.8. Diagonal Controller</a></li>
|
||||
<li><a href="#org582b3be">1.9. Closed-Loop system Performances</a></li>
|
||||
<li><a href="#org5f6e249">1.1. Introduction</a></li>
|
||||
<li><a href="#org01cc89e">1.2. Gravimeter Model - Parameters</a></li>
|
||||
<li><a href="#orgd1738d1">1.3. System Identification</a></li>
|
||||
<li><a href="#org35c239c">1.4. Decoupling using the Jacobian</a></li>
|
||||
<li><a href="#org6f938e2">1.5. Decoupling using the SVD</a></li>
|
||||
<li><a href="#orgfabe265">1.6. Verification of the decoupling using the “Gershgorin Radii”</a></li>
|
||||
<li><a href="#orgef088ef">1.7. Verification of the decoupling using the “Relative Gain Array”</a></li>
|
||||
<li><a href="#org09b55fe">1.8. Obtained Decoupled Plants</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>
|
||||
</li>
|
||||
<li><a href="#org71d7b34">2. Stewart Platform - Simscape Model</a>
|
||||
<li><a href="#orgce40d0e">2. Stewart Platform - Simscape Model</a>
|
||||
<ul>
|
||||
<li><a href="#orgf9cb366">2.1. Simscape Model - Parameters</a></li>
|
||||
<li><a href="#orga72c2be">2.2. Identification of the plant</a></li>
|
||||
<li><a href="#orga9ba8e1">2.3. Physical 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="#org87a2d2f">2.5. SVD Decoupling</a></li>
|
||||
<li><a href="#org4b81d86">2.6. Verification of the decoupling using the “Gershgorin Radii”</a></li>
|
||||
<li><a href="#org2845b5e">2.7. Verification of the decoupling using the “Relative Gain Array”</a></li>
|
||||
<li><a href="#orgdb7f2df">2.8. Obtained Decoupled Plants</a></li>
|
||||
<li><a href="#org0143a9d">2.9. Diagonal Controller</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   <span class="tag"><span class="no_export">no_export</span></span></a></li>
|
||||
<li><a href="#org63cce53">2.1. Simscape Model - Parameters</a></li>
|
||||
<li><a href="#org6cc3c6a">2.2. Identification of the plant</a></li>
|
||||
<li><a href="#org6c203ed">2.3. Decoupling using the Jacobian</a></li>
|
||||
<li><a href="#orgc3c9daf">2.4. Decoupling using the SVD</a></li>
|
||||
<li><a href="#orga514e47">2.5. Verification of the decoupling using the “Gershgorin Radii”</a></li>
|
||||
<li><a href="#orgd5f8212">2.6. Verification of the decoupling using the “Relative Gain Array”</a></li>
|
||||
<li><a href="#org887521e">2.7. Obtained Decoupled Plants</a></li>
|
||||
<li><a href="#orga134c9c">2.8. Diagonal Controller</a></li>
|
||||
<li><a href="#org962b3f8">2.9. Closed-Loop system Performances</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org3a856a2" class="outline-2">
|
||||
<h2 id="org3a856a2"><span class="section-number-2">1</span> Gravimeter - Simscape Model</h2>
|
||||
<p>
|
||||
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>
|
||||
<div id="outline-container-org7cee37e" class="outline-3">
|
||||
<h3 id="org7cee37e"><span class="section-number-3">1.1</span> Introduction</h3>
|
||||
<p>
|
||||
<a id="orga85e8ac"></a>
|
||||
</p>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgec45da7" class="outline-3">
|
||||
<h3 id="orgec45da7"><span class="section-number-3">1.2</span> Simscape Model - Parameters</h3>
|
||||
<div id="outline-container-org5f6e249" class="outline-3">
|
||||
<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="org-src-container">
|
||||
<pre class="src src-matlab">open(<span class="org-string">'gravimeter.slx'</span>)
|
||||
</pre>
|
||||
</div>
|
||||
<p>
|
||||
<a id="org516b437"></a>
|
||||
</p>
|
||||
|
||||
<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>
|
||||
<p><span class="figure-number">Figure 1: </span>Model of the gravimeter</p>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Parameters
|
||||
The parameters used for the simulation are the following:
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<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 id="outline-container-org30962d3" class="outline-3">
|
||||
<h3 id="org30962d3"><span class="section-number-3">1.3</span> System Identification - Without Gravity</h3>
|
||||
<div id="outline-container-orgd1738d1" class="outline-3">
|
||||
<h3 id="orgd1738d1"><span class="section-number-3">1.3</span> System Identification</h3>
|
||||
<div class="outline-text-3" id="text-1-3">
|
||||
<p>
|
||||
<a id="org1c874d0"></a>
|
||||
</p>
|
||||
|
||||
<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>
|
||||
mdl = <span class="org-string">'gravimeter'</span>;
|
||||
@ -133,7 +172,7 @@ G.OutputName = {<span class="org-string">'Ax1'</span>, <span class="org-string">
|
||||
</div>
|
||||
|
||||
<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>
|
||||
@ -150,7 +189,7 @@ And 4 outputs (the two 2-DoF accelerometers):
|
||||
\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>
|
||||
<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>
|
||||
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>
|
||||
|
||||
|
||||
<div id="org64e7053" class="figure">
|
||||
<div id="org85271f8" class="figure">
|
||||
<p><img src="figs/open_loop_tf.png" alt="open_loop_tf.png" />
|
||||
</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 id="outline-container-org5b19440" class="outline-3">
|
||||
<h3 id="org5b19440"><span class="section-number-3">1.4</span> Physical Decoupling using the Jacobian</h3>
|
||||
<div id="outline-container-org35c239c" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
<a id="org5c9e033"></a>
|
||||
<a id="org3e3089f"></a>
|
||||
</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>
|
||||
@ -244,16 +283,16 @@ The Jacobian matrix \(J_{a}\) is used to compute the vertical acceleration, hori
|
||||
\end{equation}
|
||||
|
||||
<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} \]
|
||||
</p>
|
||||
|
||||
<p>
|
||||
\(\bm{G}_x(s)\) correspond to the $3 × 3$transfer function matrix from forces and torques applied to the gravimeter at its center of mass to the absolute acceleration of the gravimeter’s center of mass (Figure <a href="#org056bfbe">4</a>).
|
||||
\(\bm{G}_x(s)\) correspond to the $3 × 3$transfer function matrix from forces and torques applied to the gravimeter at its center of mass to the absolute acceleration of the gravimeter’s center of mass (Figure <a href="#org38ba9bc">4</a>).
|
||||
</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>
|
||||
<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>
|
||||
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>
|
||||
|
||||
|
||||
<div id="org71d3d59" class="figure">
|
||||
<div id="orgee1af52" class="figure">
|
||||
<p><img src="figs/gravimeter_jacobian_plant.png" alt="gravimeter_jacobian_plant.png" />
|
||||
</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 id="outline-container-orgc8cb89d" class="outline-3">
|
||||
<h3 id="orgc8cb89d"><span class="section-number-3">1.5</span> SVD Decoupling</h3>
|
||||
<div id="outline-container-org6f938e2" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
<a id="org6daebae"></a>
|
||||
<a id="orgdb7c950"></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>
|
||||
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\).
|
||||
</p>
|
||||
@ -383,11 +421,11 @@ Now, the Singular Value Decomposition of \(H_1\) is performed:
|
||||
</div>
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<div id="org05538bc" class="figure">
|
||||
<div id="org5b03e6b" class="figure">
|
||||
<p><img src="figs/gravimeter_decouple_svd.png" alt="gravimeter_decouple_svd.png" />
|
||||
</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>
|
||||
|
||||
<p>
|
||||
The diagonal and off-diagonal elements of the “SVD” plant are shown in Figure <a href="#orgcf0d284">7</a>.
|
||||
The diagonal and off-diagonal elements of the “SVD” plant are shown in Figure <a href="#org941635f">7</a>.
|
||||
</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>
|
||||
<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 “SVD” plant are shown
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgd2f1c19" class="outline-3">
|
||||
<h3 id="orgd2f1c19"><span class="section-number-3">1.6</span> Verification of the decoupling using the “Gershgorin Radii”</h3>
|
||||
<div id="outline-container-orgfabe265" class="outline-3">
|
||||
<h3 id="orgfabe265"><span class="section-number-3">1.6</span> Verification of the decoupling using the “Gershgorin Radii”</h3>
|
||||
<div class="outline-text-3" id="text-1-6">
|
||||
<p>
|
||||
<a id="org72fd0a9"></a>
|
||||
<a id="org3ecdcf9"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -454,7 +492,7 @@ This is computed over the following frequencies.
|
||||
</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>
|
||||
<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 id="outline-container-orgc761edd" class="outline-3">
|
||||
<h3 id="orgc761edd"><span class="section-number-3">1.7</span> Obtained Decoupled Plants</h3>
|
||||
<div id="outline-container-orgef088ef" class="outline-3">
|
||||
<h3 id="orgef088ef"><span class="section-number-3">1.7</span> Verification of the decoupling using the “Relative Gain Array”</h3>
|
||||
<div class="outline-text-3" id="text-1-7">
|
||||
<p>
|
||||
<a id="org871fe90"></a>
|
||||
<a id="org30ee3ad"></a>
|
||||
</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>
|
||||
|
||||
|
||||
<div id="org85343af" class="figure">
|
||||
<p><img src="figs/gravimeter_decoupled_plant_svd.png" alt="gravimeter_decoupled_plant_svd.png" />
|
||||
\begin{equation}
|
||||
\Lambda\big(G(s)\big) = G(s) \times \big( G(s)^{-1} \big)^T
|
||||
\end{equation}
|
||||
<p>
|
||||
where \(\times\) denotes an element by element multiplication and \(G(s)\) is an \(n \times n\) square transfer matrix.
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 9: </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="#org5d69920">10</a>.
|
||||
The obtained RGA elements are shown in Figure <a href="#orgee3b5aa">9</a>.
|
||||
</p>
|
||||
|
||||
|
||||
<div id="org5d69920" class="figure">
|
||||
<p><img src="figs/gravimeter_decoupled_plant_jacobian.png" alt="gravimeter_decoupled_plant_jacobian.png" />
|
||||
<div id="orgee3b5aa" class="figure">
|
||||
<p><img src="figs/gravimeter_rga.png" alt="gravimeter_rga.png" />
|
||||
</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 id="outline-container-org669229d" class="outline-3">
|
||||
<h3 id="org669229d"><span class="section-number-3">1.8</span> Diagonal Controller</h3>
|
||||
<div id="outline-container-org09b55fe" class="outline-3">
|
||||
<h3 id="org09b55fe"><span class="section-number-3">1.8</span> Obtained Decoupled Plants</h3>
|
||||
<div class="outline-text-3" id="text-1-8">
|
||||
<p>
|
||||
<a id="org14b50b3"></a>
|
||||
The control diagram for the centralized control is shown in Figure <a href="#orga8cccea">11</a>.
|
||||
<a id="org62590c4"></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>
|
||||
@ -507,22 +575,22 @@ The Jacobian is used to convert forces in the cartesian frame to forces applied
|
||||
</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>
|
||||
<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>
|
||||
|
||||
<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\).
|
||||
</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>
|
||||
<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>
|
||||
|
||||
|
||||
@ -557,23 +625,23 @@ G_svd = feedback(G, inv(V<span class="org-type">'</span>)<span class="org-type">
|
||||
</div>
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<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>
|
||||
<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 id="outline-container-org582b3be" class="outline-3">
|
||||
<h3 id="org582b3be"><span class="section-number-3">1.9</span> Closed-Loop system Performances</h3>
|
||||
<div class="outline-text-3" id="text-1-9">
|
||||
<div id="outline-container-org78d9a5b" class="outline-3">
|
||||
<h3 id="org78d9a5b"><span class="section-number-3">1.10</span> Closed-Loop system Performances</h3>
|
||||
<div class="outline-text-3" id="text-1-10">
|
||||
<p>
|
||||
<a id="orgfc06310"></a>
|
||||
<a id="org1a4c704"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -604,24 +672,27 @@ ans =
|
||||
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<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>
|
||||
<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 id="outline-container-org71d7b34" class="outline-2">
|
||||
<h2 id="org71d7b34"><span class="section-number-2">2</span> Stewart Platform - Simscape Model</h2>
|
||||
<div id="outline-container-orgce40d0e" class="outline-2">
|
||||
<h2 id="orgce40d0e"><span class="section-number-2">2</span> Stewart Platform - Simscape Model</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
<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>
|
||||
@ -634,32 +705,33 @@ Some notes about the system:
|
||||
</ul>
|
||||
|
||||
|
||||
<div id="org0b6cb48" class="figure">
|
||||
<div id="orge8ec29d" class="figure">
|
||||
<p><img src="figs/SP_assembly.png" alt="SP_assembly.png" />
|
||||
</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>
|
||||
|
||||
<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>
|
||||
<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="#orgd1816b3">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="#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="#orgaa48d31">2.5</a>: The decoupling is performed thanks to the SVD</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="#orgf6dad38">2.8</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="#org7c48c81">2.10</a>: Finally, the closed loop system properties are studied</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="#org2fcc666">2.2</a>: The plant is identified from the Simscape model and the system coupling is shown</li>
|
||||
<li>Section <a href="#org64578de">2.3</a>: The plant is first decoupled using the Jacobian</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="#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="#org577bb4e">2.6</a>:</li>
|
||||
<li>Section <a href="#orgd33facb">2.7</a>: The dynamics of the decoupled plants are shown</li>
|
||||
<li>Section <a href="#org4708058">2.8</a>: A diagonal controller is defined to control the decoupled plant</li>
|
||||
<li>Section <a href="#orgc0d06ab">2.9</a>: Finally, the closed loop system properties are studied</li>
|
||||
</ul>
|
||||
</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">
|
||||
<p>
|
||||
<a id="orgf1d35e2"></a>
|
||||
<a id="org00b1234"></a>
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">open(<span class="org-string">'drone_platform.slx'</span>);
|
||||
@ -715,30 +787,30 @@ Kc = tf(zeros(6));
|
||||
</div>
|
||||
|
||||
|
||||
<div id="orgb09e537" class="figure">
|
||||
<div id="orgf441445" class="figure">
|
||||
<p><img src="figs/stewart_simscape.png" alt="stewart_simscape.png" />
|
||||
</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 id="org4946596" class="figure">
|
||||
<div id="org11d6516" class="figure">
|
||||
<p><img src="figs/stewart_platform_details.png" alt="stewart_platform_details.png" />
|
||||
</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 id="outline-container-orga72c2be" class="outline-3">
|
||||
<h3 id="orga72c2be"><span class="section-number-3">2.2</span> Identification of the plant</h3>
|
||||
<div id="outline-container-org6cc3c6a" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
<a id="orgd1816b3"></a>
|
||||
<a id="org2fcc666"></a>
|
||||
</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>
|
||||
@ -754,10 +826,10 @@ The outputs are the 6 accelerations measured by the inertial unit.
|
||||
</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>
|
||||
<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 class="org-src-container">
|
||||
@ -796,7 +868,7 @@ State-space model with 6 outputs, 12 inputs, and 24 states.
|
||||
|
||||
|
||||
<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>
|
||||
@ -804,20 +876,20 @@ One can easily see that the system is strongly coupled.
|
||||
</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>
|
||||
<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 id="outline-container-orga9ba8e1" class="outline-3">
|
||||
<h3 id="orga9ba8e1"><span class="section-number-3">2.3</span> Physical Decoupling using the Jacobian</h3>
|
||||
<div id="outline-container-org6c203ed" class="outline-3">
|
||||
<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">
|
||||
<p>
|
||||
<a id="org7c9242b"></a>
|
||||
Consider the control architecture shown in Figure <a href="#org89c1f08">20</a>.
|
||||
<a id="org64578de"></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.
|
||||
</p>
|
||||
|
||||
@ -899,10 +971,10 @@ The Jacobian matrix is computed from the geometry of the platform (position and
|
||||
</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>
|
||||
<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>
|
||||
|
||||
<p>
|
||||
@ -922,11 +994,15 @@ Gx.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4b07807" class="outline-3">
|
||||
<h3 id="org4b07807"><span class="section-number-3">2.4</span> Real Approximation of \(G\) at the decoupling frequency</h3>
|
||||
<div id="outline-container-orgc3c9daf" class="outline-3">
|
||||
<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">
|
||||
<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>
|
||||
@ -1100,18 +1176,9 @@ This can be verified below where only the real value of \(G_u(\omega_c)\) is sho
|
||||
</tr>
|
||||
</tbody>
|
||||
</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>
|
||||
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 \]
|
||||
</p>
|
||||
|
||||
@ -1267,14 +1334,14 @@ First, the Singular Value Decomposition of \(H_1\) is performed:
|
||||
</table>
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<div id="orgad6c96b" class="figure">
|
||||
<div id="org6b45a64" class="figure">
|
||||
<p><img src="figs/plant_decouple_svd.png" alt="plant_decouple_svd.png" />
|
||||
</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>
|
||||
|
||||
<p>
|
||||
@ -1289,11 +1356,11 @@ The decoupled plant is then:
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4b81d86" class="outline-3">
|
||||
<h3 id="org4b81d86"><span class="section-number-3">2.6</span> Verification of the decoupling using the “Gershgorin Radii”</h3>
|
||||
<div class="outline-text-3" id="text-2-6">
|
||||
<div id="outline-container-orga514e47" class="outline-3">
|
||||
<h3 id="orga514e47"><span class="section-number-3">2.5</span> Verification of the decoupling using the “Gershgorin Radii”</h3>
|
||||
<div class="outline-text-3" id="text-2-5">
|
||||
<p>
|
||||
<a id="org9102f7f"></a>
|
||||
<a id="orgecdb237"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1309,17 +1376,21 @@ The “Gershgorin Radii” of a matrix \(S\) is defined by:
|
||||
This is computed over the following frequencies.
|
||||
</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>
|
||||
<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 id="outline-container-org2845b5e" class="outline-3">
|
||||
<h3 id="org2845b5e"><span class="section-number-3">2.7</span> Verification of the decoupling using the “Relative Gain Array”</h3>
|
||||
<div class="outline-text-3" id="text-2-7">
|
||||
<div id="outline-container-orgd5f8212" class="outline-3">
|
||||
<h3 id="orgd5f8212"><span class="section-number-3">2.6</span> Verification of the decoupling using the “Relative Gain Array”</h3>
|
||||
<div class="outline-text-3" id="text-2-6">
|
||||
<p>
|
||||
<a id="org577bb4e"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
The relative gain array (RGA) is defined as:
|
||||
</p>
|
||||
@ -1331,55 +1402,55 @@ where \(\times\) denotes an element by element multiplication and \(G(s)\) is an
|
||||
</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>
|
||||
|
||||
|
||||
<div id="orga76f805" class="figure">
|
||||
<div id="org3af8336" class="figure">
|
||||
<p><img src="figs/simscape_model_rga.png" alt="simscape_model_rga.png" />
|
||||
</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 id="outline-container-orgdb7f2df" class="outline-3">
|
||||
<h3 id="orgdb7f2df"><span class="section-number-3">2.8</span> Obtained Decoupled Plants</h3>
|
||||
<div class="outline-text-3" id="text-2-8">
|
||||
<div id="outline-container-org887521e" class="outline-3">
|
||||
<h3 id="org887521e"><span class="section-number-3">2.7</span> Obtained Decoupled Plants</h3>
|
||||
<div class="outline-text-3" id="text-2-7">
|
||||
<p>
|
||||
<a id="orgf6dad38"></a>
|
||||
<a id="orgd33facb"></a>
|
||||
</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>
|
||||
|
||||
|
||||
<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>
|
||||
<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>
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<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>
|
||||
<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 id="outline-container-org0143a9d" class="outline-3">
|
||||
<h3 id="org0143a9d"><span class="section-number-3">2.9</span> Diagonal Controller</h3>
|
||||
<div class="outline-text-3" id="text-2-9">
|
||||
<div id="outline-container-orga134c9c" class="outline-3">
|
||||
<h3 id="orga134c9c"><span class="section-number-3">2.8</span> Diagonal Controller</h3>
|
||||
<div class="outline-text-3" id="text-2-8">
|
||||
<p>
|
||||
<a id="orga082064"></a>
|
||||
The control diagram for the centralized control is shown in Figure <a href="#org43eaa56">26</a>.
|
||||
<a id="org4708058"></a>
|
||||
The control diagram for the centralized control is shown in Figure <a href="#org9680611">27</a>.
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1388,22 +1459,22 @@ The Jacobian is used to convert forces in the cartesian frame to forces applied
|
||||
</p>
|
||||
|
||||
|
||||
<div id="org43eaa56" class="figure">
|
||||
<div id="org9680611" class="figure">
|
||||
<p><img src="figs/centralized_control.png" alt="centralized_control.png" />
|
||||
</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>
|
||||
|
||||
<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\).
|
||||
</p>
|
||||
|
||||
|
||||
<div id="orgee73430" class="figure">
|
||||
<div id="org255f076" class="figure">
|
||||
<p><img src="figs/svd_control.png" alt="svd_control.png" />
|
||||
</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>
|
||||
|
||||
|
||||
@ -1437,23 +1508,23 @@ G_svd = feedback(G, inv(V<span class="org-type">'</span>)<span class="org-type">
|
||||
</div>
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<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>
|
||||
<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 id="outline-container-org7f0526e" class="outline-3">
|
||||
<h3 id="org7f0526e"><span class="section-number-3">2.10</span> Closed-Loop system Performances</h3>
|
||||
<div class="outline-text-3" id="text-2-10">
|
||||
<div id="outline-container-org962b3f8" class="outline-3">
|
||||
<h3 id="org962b3f8"><span class="section-number-3">2.9</span> Closed-Loop system Performances</h3>
|
||||
<div class="outline-text-3" id="text-2-9">
|
||||
<p>
|
||||
<a id="org7c48c81"></a>
|
||||
<a id="orgc0d06ab"></a>
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -1484,63 +1555,14 @@ ans =
|
||||
|
||||
|
||||
<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>
|
||||
|
||||
|
||||
<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>
|
||||
<p><span class="figure-number">Figure 29: </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   <span class="tag"><span class="no_export">no_export</span></span></h3>
|
||||
<div class="outline-text-3" id="text-2-11">
|
||||
<p>
|
||||
Let’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>
|
||||
<p><span class="figure-number">Figure 30: </span>Obtained Transmissibility</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
@ -1548,7 +1570,7 @@ G_svd = feedback(G, inv(V<span class="org-type">'</span>)<span class="org-type">
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<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>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
338
index.org
338
index.org
@ -1,4 +1,4 @@
|
||||
#+TITLE: SVD Control
|
||||
#+TITLE: Diagonal control using the SVD and the Jacobian Matrix
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
@ -38,12 +38,34 @@
|
||||
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
||||
: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
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle gravimeter/script.m
|
||||
:END:
|
||||
<<sec:gravimeter>>
|
||||
|
||||
** 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:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
@ -57,16 +79,24 @@
|
||||
addpath('gravimeter');
|
||||
#+end_src
|
||||
|
||||
** Simscape Model - Parameters
|
||||
#+begin_src matlab
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
#+end_src
|
||||
|
||||
** Gravimeter Model - Parameters
|
||||
<<sec:gravimeter_model>>
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
open('gravimeter.slx')
|
||||
#+end_src
|
||||
|
||||
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]]
|
||||
|
||||
Parameters
|
||||
The parameters used for the simulation are the following:
|
||||
#+begin_src matlab
|
||||
l = 1.0; % Length of the mass [m]
|
||||
h = 1.7; % Height of the mass [m]
|
||||
@ -85,7 +115,9 @@ Parameters
|
||||
g = 0; % Gravity [m/s2]
|
||||
#+end_src
|
||||
|
||||
** System Identification - Without Gravity
|
||||
** System Identification
|
||||
<<sec:gravimeter_identification>>
|
||||
|
||||
#+begin_src matlab
|
||||
%% Name of the Simulink File
|
||||
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]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
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:
|
||||
[[file:figs/open_loop_tf.png]]
|
||||
|
||||
** Physical Decoupling using the Jacobian
|
||||
** Decoupling using the Jacobian
|
||||
<<sec:gravimeter_jacobian_decoupling>>
|
||||
|
||||
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]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
% Magnitude
|
||||
@ -299,12 +327,11 @@ The diagonal and off-diagonal elements of $G_x$ are shown in Figure [[fig:gravim
|
||||
#+RESULTS:
|
||||
[[file:figs/gravimeter_jacobian_plant.png]]
|
||||
|
||||
** SVD Decoupling
|
||||
** Decoupling using the SVD
|
||||
<<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.
|
||||
|
||||
|
||||
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
|
||||
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]].
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
% 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]]
|
||||
|
||||
** 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" of a matrix $S$ is defined by:
|
||||
\[ \zeta_i(j\omega) = \frac{\sum\limits_{j\neq i}|S_{ij}(j\omega)|}{|S_{ii}(j\omega)|} \]
|
||||
|
||||
This is computed over the following frequencies.
|
||||
#+begin_src matlab
|
||||
freqs = logspace(-2, 2, 1000); % [Hz]
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
% Gershgorin Radii for the coupled plant:
|
||||
Gr_coupled = zeros(length(freqs), size(G,2));
|
||||
@ -473,7 +493,7 @@ This is computed over the following frequencies.
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
hold off;
|
||||
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
|
||||
legend('location', 'northwest');
|
||||
legend('location', 'southwest');
|
||||
ylim([1e-4, 1e2]);
|
||||
#+end_src
|
||||
|
||||
@ -486,14 +506,100 @@ This is computed over the following frequencies.
|
||||
#+RESULTS:
|
||||
[[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
|
||||
<<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]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
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]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
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]].
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 2, 1000);
|
||||
|
||||
figure;
|
||||
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');
|
||||
xlim([freqs(1), freqs(end)]);
|
||||
ylim([1e-7, 1e-2]);
|
||||
xlim([1e-2, 5e1]); ylim([1e-7, 1e-2]);
|
||||
#+end_src
|
||||
|
||||
#+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
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle stewart_platform/simscape_model.m
|
||||
:header-args:matlab+: :tangle stewart_platform/script.m
|
||||
:END:
|
||||
<<sec:stewart_platform>>
|
||||
|
||||
** Introduction :ignore:
|
||||
|
||||
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
|
||||
[[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_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_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
|
||||
- Section [[sec:comp_decoupling]]: The effectiveness of the decoupling with the Jacobian and SVD are compared using the Gershorin Radii
|
||||
- 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_gershorin_radii]]: 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_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
|
||||
@ -1047,7 +1151,7 @@ One can easily see that the system is strongly coupled.
|
||||
#+RESULTS:
|
||||
[[file:figs/stewart_platform_coupled_plant.png]]
|
||||
|
||||
** Physical Decoupling using the Jacobian
|
||||
** Decoupling using the Jacobian
|
||||
<<sec:stewart_jacobian_decoupling>>
|
||||
Consider the control architecture shown in Figure [[fig:plant_decouple_jacobian]].
|
||||
The Jacobian matrix is used to transform forces/torques applied on the top platform to the equivalent forces applied by each actuator.
|
||||
@ -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'};
|
||||
#+end_src
|
||||
|
||||
** Real Approximation of $G$ at the decoupling frequency
|
||||
<<sec:stewart_real_approx>>
|
||||
** Decoupling using the SVD
|
||||
<<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$.
|
||||
#+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 |
|
||||
| 220.6 | -220.6 | 220.6 | -220.6 | 220.6 | -220.6 |
|
||||
|
||||
** SVD Decoupling
|
||||
<<sec:stewart_svd_decoupling>>
|
||||
|
||||
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 \]
|
||||
|
||||
#+begin_src matlab
|
||||
@ -1217,7 +1320,7 @@ The decoupled plant is then:
|
||||
#+end_src
|
||||
|
||||
** 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)$:
|
||||
|
||||
@ -1279,6 +1382,8 @@ This is computed over the following frequencies.
|
||||
[[file:figs/simscape_model_gershgorin_radii.png]]
|
||||
|
||||
** Verification of the decoupling using the "Relative Gain Array"
|
||||
<<sec:stewart_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
|
||||
@ -1713,156 +1818,3 @@ The obtained transmissibility in Open-loop, for the centralized control as well
|
||||
#+RESULTS:
|
||||
[[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
|
||||
|
||||
|
||||
@ -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')
|
||||
Binary file not shown.
@ -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';
|
||||
|
||||
@ -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';
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@ -132,8 +132,10 @@ legend('location', 'northwest');
|
||||
Gx = Gu*inv(J');
|
||||
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
|
||||
% Real Approximation of $G$ at the decoupling frequency
|
||||
% <<sec:stewart_real_approx>>
|
||||
% Decoupling using the SVD
|
||||
% <<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$.
|
||||
|
||||
@ -148,10 +150,18 @@ H1 = evalfr(Gu, j*wc);
|
||||
D = pinv(real(H1'*H1));
|
||||
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 \]
|
||||
|
||||
|
||||
@ -171,7 +181,7 @@ H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
|
||||
Gsvd = inv(U)*Gu*inv(V');
|
||||
|
||||
% 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)$:
|
||||
|
||||
@ -221,6 +231,8 @@ legend('location', 'northwest');
|
||||
ylim([1e-3, 1e3]);
|
||||
|
||||
% Verification of the decoupling using the "Relative Gain Array"
|
||||
% <<sec:stewart_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
|
||||
@ -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]].
|
||||
|
||||
|
||||
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;
|
||||
tiledlayout(2, 2, 'TileSpacing', 'None', 'Padding', 'None');
|
||||
|
||||
Loading…
Reference in New Issue
Block a user