249 lines
8.2 KiB
Matlab
249 lines
8.2 KiB
Matlab
%% Clear Workspace and Close figures
|
|
clear; close all; clc;
|
|
|
|
%% Intialize Laplace variable
|
|
s = zpk('s');
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./src/'); % Path for functions
|
|
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Initialize Frequency Vector
|
|
freqs = logspace(0, 3, 1000);
|
|
|
|
%% Compute Equation of motion
|
|
l = 1; h=2;
|
|
la = 0.5; % Horizontal position of actuators [m]
|
|
ha = 0.2; % Vertical of actuators [m]
|
|
|
|
m = 40; % Payload mass [kg]
|
|
I = 5; % Payload rotational inertia [kg m^2]
|
|
|
|
c = 2e2; % Actuator Damping [N/(m/s)]
|
|
k = 1e6; % Actuator Stiffness [N/m]
|
|
|
|
% Unit vectors of the actuators
|
|
s1 = [1;0];
|
|
s2 = [0;1];
|
|
s3 = [0;1];
|
|
|
|
% Stiffnesss and Damping matrices of the struts
|
|
Kr = diag([k,k,k]);
|
|
Cr = diag([c,c,c]);
|
|
|
|
% Location of the joints with respect to the center of mass
|
|
Mb1 = [-l/2;-ha];
|
|
Mb2 = [-la; -h/2];
|
|
Mb3 = [ la; -h/2];
|
|
|
|
% Jacobian matrix (Center of Mass)
|
|
J_CoM = [s1', Mb1(1)*s1(2)-Mb1(2)*s1(1);
|
|
s2', Mb2(1)*s2(2)-Mb2(2)*s2(1);
|
|
s3', Mb3(1)*s3(2)-Mb3(2)*s3(1)];
|
|
|
|
% Mass Matrix in frame {M}
|
|
M = diag([m,m,I]);
|
|
|
|
% Stiffness Matrix in frame {M}
|
|
K = J_CoM'*Kr*J_CoM;
|
|
|
|
% Damping Matrix in frame {M}
|
|
C = J_CoM'*Cr*J_CoM;
|
|
|
|
% Plant in the frame of the struts
|
|
G_L = J_CoM*inv(M*s^2 + C*s + K)*J_CoM';
|
|
|
|
figure;
|
|
tiledlayout(3, 3, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
for out_i = 1:3
|
|
for in_i = 1:3
|
|
nexttile;
|
|
plot(freqs, abs(squeeze(freqresp(G_L(out_i,in_i), freqs, 'Hz'))), 'k-', ...
|
|
'DisplayName', sprintf('$\\mathcal{L}_%i/\\tau_%i$', out_i, in_i));
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlim([freqs(1), freqs(end)]); ylim([2e-8, 4e-5]);
|
|
xticks([1e0, 1e1, 1e2])
|
|
yticks([1e-7, 1e-6, 1e-5])
|
|
leg = legend('location', 'northeast', 'FontSize', 8);
|
|
leg.ItemTokenSize(1) = 18;
|
|
|
|
if in_i == 1
|
|
ylabel('Mag. [m/N]')
|
|
else
|
|
set(gca, 'YTickLabel',[]);
|
|
end
|
|
|
|
if out_i == 3
|
|
xlabel('Frequency [Hz]')
|
|
else
|
|
set(gca, 'XTickLabel',[]);
|
|
end
|
|
end
|
|
end
|
|
|
|
%% Jacobian Decoupling - Center of Mass
|
|
G_CoM = pinv(J_CoM)*G_L*pinv(J_CoM');
|
|
G_CoM.InputName = {'Fx', 'Fy', 'Mz'};
|
|
G_CoM.OutputName = {'Dx', 'Dy', 'Rz'};
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 3), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$D_{x,\{M\}}/M_{z,\{M\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(3, 1), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$R_{z,\{M\}}/F_{x,\{M\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(1, 1), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$D_{x,\{M\}}/F_{x,\{M\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(2, 2), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$D_{y,\{M\}}/F_{y,\{M\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoM(3, 3), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$R_{z,\{M\}}/M_{z,\{M\}}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
|
ylim([1e-10, 1e-3]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8);
|
|
leg.ItemTokenSize(1) = 18;
|
|
|
|
%% Jacobian Decoupling - Center of Mass
|
|
% Location of the joints with respect to the center of stiffness
|
|
Mb1 = [-l/2; 0];
|
|
Mb2 = [-la; -h/2+ha];
|
|
Mb3 = [ la; -h/2+ha];
|
|
|
|
% Jacobian matrix (Center of Stiffness)
|
|
J_CoK = [s1', Mb1(1)*s1(2)-Mb1(2)*s1(1);
|
|
s2', Mb2(1)*s2(2)-Mb2(2)*s2(1);
|
|
s3', Mb3(1)*s3(2)-Mb3(2)*s3(1)];
|
|
|
|
G_CoK = pinv(J_CoK)*G_L*pinv(J_CoK');
|
|
G_CoK.InputName = {'Fx', 'Fy', 'Mz'};
|
|
G_CoK.OutputName = {'Dx', 'Dy', 'Rz'};
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 1), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$D_{x,\{K\}}/F_{x,\{K\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(2, 2), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$D_{y,\{K\}}/F_{y,\{K\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(3, 3), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$R_{z,\{K\}}/M_{z,\{K\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(1, 3), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$D_{x,\{K\}}/M_{z,\{K\}}$');
|
|
plot(freqs, abs(squeeze(freqresp(G_CoK(3, 1), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$R_{z,\{K\}}/F_{x,\{K\}}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Kagnitude');
|
|
ylim([1e-10, 1e-3]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 18;
|
|
|
|
%% Modal decoupling
|
|
% Compute the eigen vectors
|
|
[phi, wi] = eig(M\K);
|
|
% Sort the eigen vectors by increasing associated frequency
|
|
[~, i] = sort(diag(wi));
|
|
phi = phi(:, i);
|
|
|
|
% Plant in the modal space
|
|
Gm = inv(phi)*inv(J_CoM)*G_L*inv(J_CoM')*inv(phi');
|
|
|
|
%% Modal decoupled plant
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gm(1,1), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$\mathcal{X}_{m,1}/\tau_{m,1}$');
|
|
plot(freqs, abs(squeeze(freqresp(Gm(2,2), freqs, 'Hz'))), 'color', colors(2,:), 'DisplayName', '$\mathcal{X}_{m,2}/\tau_{m,2}$');
|
|
plot(freqs, abs(squeeze(freqresp(Gm(3,3), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$\mathcal{X}_{m,3}/\tau_{m,3}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
|
ylim([1e-8, 1e-4]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8);
|
|
leg.ItemTokenSize(1) = 18;
|
|
|
|
%% SVD Decoupling
|
|
wc = 2*pi*100; % Decoupling frequency [rad/s]
|
|
% System's response at the decoupling frequency
|
|
H1 = evalfr(G_L, j*wc);
|
|
|
|
% Real approximation of G(j.wc)
|
|
D = pinv(real(H1'*H1));
|
|
H1 = pinv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
|
|
|
|
[U,S,V] = svd(H1);
|
|
|
|
Gsvd = inv(U)*G_L*inv(V');
|
|
|
|
figure;
|
|
hold on;
|
|
for i_in = 1:3
|
|
for i_out = [i_in+1:3]
|
|
plot(freqs, abs(squeeze(freqresp(Gsvd(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(Gsvd(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$G_{SVD}(i,j)\ i \neq j$');
|
|
set(gca,'ColorOrderIndex',1)
|
|
for i_in_out = 1:3
|
|
plot(freqs, abs(squeeze(freqresp(Gsvd(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_{SVD}(%d,%d)$', i_in_out, i_in_out));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
|
ylim([1e-10, 2e-4]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8);
|
|
leg.ItemTokenSize(1) = 18;
|
|
|
|
%% Simscape model with relative motion sensor at alternative positions
|
|
mdl = 'detail_control_decoupling_test_model';
|
|
open(mdl)
|
|
|
|
deq = 0.2; % Length of the actuators [m]
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/F1'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/F2'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/F3'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Payload'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Payload'], 2, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Payload'], 3, 'openoutput'); io_i = io_i + 1;
|
|
|
|
G_L_alt = linearize(mdl, io);
|
|
G_L_alt.InputName = {'F1', 'F2', 'F3'};
|
|
G_L_alt.OutputName = {'d1', 'd2', 'd32'};
|
|
|
|
% SVD Decoupling with the new plant
|
|
wc = 2*pi*100; % Decoupling frequency [rad/s]
|
|
% System's response at the decoupling frequency
|
|
H1 = evalfr(G_L_alt, j*wc);
|
|
|
|
% Real approximation of G(j.wc)
|
|
D = pinv(real(H1'*H1));
|
|
H1 = pinv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
|
|
|
|
[U,S,V] = svd(H1);
|
|
|
|
Gsvd_alt = inv(U)*G_L_alt*inv(V');
|
|
|
|
%% Obtained plant after SVD decoupling - Relative motion sensors are not collocated with the actuators
|
|
figure;
|
|
hold on;
|
|
for i_in = 1:3
|
|
for i_out = [i_in+1:3]
|
|
plot(freqs, abs(squeeze(freqresp(Gsvd_alt(i_out, i_in), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(Gsvd_alt(1, 2), freqs, 'Hz'))), 'color', [0,0,0,0.2], ...
|
|
'DisplayName', '$G_{SVD}(i,j)\ i \neq j$');
|
|
set(gca,'ColorOrderIndex',1)
|
|
for i_in_out = 1:3
|
|
plot(freqs, abs(squeeze(freqresp(Gsvd_alt(i_in_out, i_in_out), freqs, 'Hz'))), 'DisplayName', sprintf('$G_{SVD}(%d,%d)$', i_in_out, i_in_out));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude');
|
|
ylim([5e-11, 7e-5]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8);
|
|
leg.ItemTokenSize(1) = 18;
|