197 lines
4.7 KiB
Matlab
197 lines
4.7 KiB
Matlab
%% 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')
|