1 Gravimeter - Simscape Model
+1 Gravimeter - Simscape Model
1.1 Simulink
+1.1 Simulink
open('gravimeter.slx')
@@ -122,7 +122,7 @@ State-space model with 4 outputs, 3 inputs, and 6 states.
-
+
Figure 1: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers
@@ -131,12 +131,12 @@ State-space model with 4 outputs, 3 inputs, and 6 states.
-
-2 Stewart Platform - Simscape Model
+
+2 Stewart Platform - Simscape Model
-
-2.1 Jacobian
+
+2.1 Jacobian
First, the position of the “joints” (points of force application) are estimated and the Jacobian computed.
@@ -178,8 +178,8 @@ save('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
-
-2.2 Simscape Model
+
+2.2 Simscape Model
open('stewart_platform/drone_platform.slx');
@@ -210,8 +210,8 @@ We load the Jacobian.
-
-2.3 Identification of the plant
+
+2.3 Identification of the plant
The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform.
@@ -268,32 +268,32 @@ Gl.OutputName = {'A1', 'A2', 'A3', 'A4', 'A5', 'A6'};
-
-2.4 Obtained Dynamics
+
+2.4 Obtained Dynamics
-
+
Figure 2: Stewart Platform Plant from forces applied by the legs to the acceleration of the platform
-
+
Figure 3: Stewart Platform Plant from torques applied by the legs to the angular acceleration of the platform
-
+
Figure 4: Stewart Platform Plant from forces applied by the legs to displacement of the legs
-
+
Figure 5: Transmissibility
@@ -301,8 +301,8 @@ Gl.OutputName = {'A1', 'A2', 'A3', 'A4', 'A5', 'A6'};
-
-2.5 Real Approximation of \(G\) at the decoupling frequency
+
+2.5 Real Approximation of \(G\) at the decoupling frequency
Let’s compute a real approximation of the complex matrix \(H_1\) which corresponds to the the transfer function \(G_c(j\omega_c)\) from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency \(\omega_c\).
@@ -328,8 +328,8 @@ H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
-
-2.6 Verification of the decoupling using the “Gershgorin Radii”
+
+2.6 Verification of the decoupling using the “Gershgorin Radii”
First, the Singular Value Decomposition of \(H_1\) is performed:
@@ -397,7 +397,7 @@ end
-
+
Figure 6: Gershgorin Radii of the Coupled and Decoupled plants
@@ -405,8 +405,8 @@ end
-
-2.7 Decoupled Plant
+
+2.7 Decoupled Plant
Let’s see the bode plot of the decoupled plant \(G_d(s)\).
@@ -414,14 +414,14 @@ Let’s see the bode plot of the decoupled plant \(G_d(s)\).
-
+
Figure 7: Decoupled Plant using SVD
-
+
Figure 8: Decoupled Plant using the Jacobian
@@ -429,8 +429,8 @@ Let’s see the bode plot of the decoupled plant \(G_d(s)\).
-
-2.8 Diagonal Controller
+
+2.8 Diagonal Controller
The controller \(K\) is a diagonal controller consisting a low pass filters with a crossover frequency \(\omega_c\) and a DC gain \(C_g\).
@@ -446,8 +446,8 @@ K = eye(6)*C_g/(s+wc);
-
-2.9 Centralized Control
+
+2.9 Centralized Control
The control diagram for the centralized control is shown below.
@@ -471,8 +471,8 @@ The Jacobian is used to convert forces in the cartesian frame to forces applied
-
-2.10 SVD Control
+
+2.10 SVD Control
The SVD control architecture is shown below.
@@ -495,8 +495,8 @@ SVD Control
-
-2.11 Results
+
+2.11 Results
Let’s first verify the stability of the closed-loop systems:
@@ -526,11 +526,11 @@ ans =
-The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure 11.
+The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure 11.
-
+
Figure 11: Obtained Transmissibility
@@ -539,12 +539,12 @@ The obtained transmissibility in Open-loop, for the centralized control as well
-
-3 Stewart Platform - Analytical Model
+
+3 Stewart Platform - Analytical Model
-
-3.1 Characteristics
+
+3.1 Characteristics
L = 0.055;
@@ -563,8 +563,8 @@ Iz = m*Rz^2;
-
-3.2 Mass Matrix
+
+3.2 Mass Matrix
M = m*[1 0 0 0 Zc 0;
@@ -578,8 +578,8 @@ Iz = m*Rz^2;
-
-3.3 Jacobian Matrix
+
+3.3 Jacobian Matrix
Bj=1/sqrt(6)*[ 1 1 -2 1 1 -2;
@@ -593,8 +593,8 @@ Iz = m*Rz^2;
-
-3.4 Stifnness matrix and Damping matrix
+
+3.4 Stifnness matrix and Damping matrix
kv = k/3; % [N/m]
@@ -608,8 +608,8 @@ C = c*K/100000; % Damping Matrix
-
-3.5 State Space System
+
+3.5 State Space System
A = [zeros(6) eye(6); -M\K -M\C];
@@ -638,8 +638,8 @@ ST.OutputName = {'ax';'ay';'az';'atheta_x';'atheta_y';'atheta_z'};
-
-3.6 Transmissibility
+
+3.6 Transmissibility
TR=ST*[eye(6); zeros(6)];
@@ -664,7 +664,7 @@ bodemag(TR(6,6),opts);
-
+
Figure 12: Transmissibility
@@ -672,8 +672,8 @@ bodemag(TR(6,6),opts);
-
-3.7 Real approximation of \(G(j\omega)\) at decoupling frequency
+
+3.7 Real approximation of \(G(j\omega)\) at decoupling frequency
sys1 = ST*[zeros(6); eye(6)]; % take only the forces inputs
@@ -701,8 +701,8 @@ end
-
-3.8 Coupled and Decoupled Plant “Gershgorin Radii”
+
+3.8 Coupled and Decoupled Plant “Gershgorin Radii”
figure;
@@ -714,7 +714,7 @@ xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
-
+
Figure 13: Gershorin Raddi for the coupled plant
@@ -730,7 +730,7 @@ xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
-
+
Figure 14: Gershorin Raddi for the decoupled plant
@@ -738,8 +738,8 @@ xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
-
-3.9 Decoupled Plant
+
+3.9 Decoupled Plant
figure;
@@ -748,7 +748,7 @@ bodemag(U'*sys1*V,opts)
-
+
Figure 15: Decoupled Plant
@@ -756,8 +756,8 @@ bodemag(U'*sys1*V,opts)
-
-3.10 Controller
+
+3.10 Controller
fc = 2*pi*0.1; % Crossover Frequency [rad/s]
@@ -769,8 +769,8 @@ cont = eye(6)*c_gain/(s+fc);
-
-3.11 Closed Loop System
+
+3.11 Closed Loop System
FEEDIN = [7:12]; % Input of controller
@@ -798,8 +798,8 @@ TRsvd = STsvd*[eye(6); zeros(6)];
-
-3.12 Results
+
+3.12 Results
figure
@@ -825,7 +825,7 @@ legend('OL','Centralized','SVD')
-
+
Figure 16: Comparison of the obtained transmissibility for the centralized control and the SVD control
@@ -836,7 +836,7 @@ legend('OL','Centralized','SVD')
-Created: 2020-09-21 lun. 18:53
+Created: 2020-09-21 lun. 18:57
diff --git a/index.org b/index.org
index dfae236..8bfd58a 100644
--- a/index.org
+++ b/index.org
@@ -573,12 +573,12 @@ Thanks to the Jacobian, we compute the transfer functions in the frame of the le
ax1 = subplot(2, 1, 1);
hold on;
- for i = 1:6
- plot(freqs, abs(squeeze(freqresp(Gl(sprintf('A%i', i), sprintf('F%i', i)), freqs, 'Hz'))));
+ for ch_i = 1:6
+ plot(freqs, abs(squeeze(freqresp(Gl(sprintf('A%i', ch_i), sprintf('F%i', ch_i)), freqs, 'Hz'))));
end
- for i = 1:5
- for j = i+1:6
- plot(freqs, abs(squeeze(freqresp(Gl(sprintf('A%i', i), sprintf('F%i', j)), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
+ for out_i = 1:5
+ for in_i = i+1:6
+ plot(freqs, abs(squeeze(freqresp(Gl(sprintf('A%i', out_i), sprintf('F%i', in_i)), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
hold off;
@@ -587,8 +587,8 @@ Thanks to the Jacobian, we compute the transfer functions in the frame of the le
ax2 = subplot(2, 1, 2);
hold on;
- for i = 1:6
- plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(sprintf('A%i', i), sprintf('F%i', i)), freqs, 'Hz'))));
+ for ch_i = 1:6
+ plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(sprintf('A%i', ch_i), sprintf('F%i', ch_i)), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
@@ -717,13 +717,13 @@ Gershgorin Radii for the decoupled plant using the Jacobian:
plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled');
plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD');
plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian');
- for i = 2:6
+ for in_i = 2:6
set(gca,'ColorOrderIndex',1)
- plot(freqs, Gr_coupled(:,i), 'HandleVisibility', 'off');
+ plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2)
- plot(freqs, Gr_decoupled(:,i), 'HandleVisibility', 'off');
+ plot(freqs, Gr_decoupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',3)
- plot(freqs, Gr_jacobian(:,i), 'HandleVisibility', 'off');
+ plot(freqs, Gr_jacobian(:,in_i), 'HandleVisibility', 'off');
end
plot(freqs, 0.5*ones(size(freqs)), 'k--', 'DisplayName', 'Limit')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');