1 Gravimeter - Simscape Model
+
+
-
1 Gravimeter - Simscape Model
-
1.1 Introduction
+
+
1.1 Introduction
-
+
-
Figure 1: Model of the gravimeter
@@ -111,8 +111,8 @@
-
1.2 Simscape Model - Parameters
+
+
-
1.2 Simscape Model - Parameters
open('gravimeter.slx') @@ -143,8 +143,8 @@ g = 0; % Gravity [m/s2]
-
1.3 System Identification - Without Gravity
+
+
1.3 System Identification - Without Gravity
%% Name of the Simulink File @@ -191,7 +191,7 @@ State-space model with 4 outputs, 3 inputs, and 6 states. -+-
Figure 2: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers
@@ -199,8 +199,8 @@ State-space model with 4 outputs, 3 inputs, and 6 states.-1.4 System Identification - With Gravity
++1.4 System Identification - With Gravity
g = 9.80665; % Gravity [m/s2] @@ -229,7 +229,7 @@ ans =-+-
Figure 3: Open Loop Transfer Function from 3 Actuators to 4 Accelerometers with an without gravity
@@ -237,12 +237,12 @@ ans =-1.5 Analytical Model
++1.5 Analytical Model
--1.5.1 Parameters
++-1.5.1 Parameters
Bode options. @@ -274,8 +274,8 @@ Frequency vector.
-1.5.2 Generation of the State Space Model
++-1.5.2 Generation of the State Space Model
Mass matrix @@ -376,11 +376,11 @@ State-space model with 12 outputs, 6 inputs, and 6 states.
-1.5.3 Comparison with the Simscape Model
++1.5.3 Comparison with the Simscape Model
-+-
Figure 4: Comparison of the analytical and the Simscape models
@@ -388,8 +388,8 @@ State-space model with 12 outputs, 6 inputs, and 6 states.-1.5.4 Analysis
++1.5.4 Analysis
-% figure @@ -457,8 +457,8 @@ State-space model with 12 outputs, 6 inputs, and 6 states.-1.5.5 Control Section
++1.5.5 Control Section
-system_dec_10Hz = freqresp(system_dec,2*pi*10); @@ -598,8 +598,8 @@ legend('Control OFF','D-1.5.6 Greshgorin radius
++1.5.6 Greshgorin radius
-system_dec_freq = freqresp(system_dec,w); @@ -645,8 +645,8 @@ ylabel('Greshgorin radius [-]');-1.5.7 Injecting ground motion in the system to have the output
++1.5.7 Injecting ground motion in the system to have the output
-Fr = logspace(-2,3,1e3); @@ -702,15 +702,15 @@ rot = PHI(:,11,11);-2 Gravimeter - Functions
++2 Gravimeter - Functions
--2.1
+align+2.1
align-@@ -739,11 +739,11 @@ This Matlab function is accessible here.
-2.2
+pzmap_testCL+-2.2
pzmap_testCL@@ -792,12 +792,12 @@ This Matlab function is accessible here.
-3 Stewart Platform - Simscape Model
++3 Stewart Platform - Simscape Model
--3.1 Jacobian
++-3.1 Jacobian
First, the position of the “joints” (points of force application) are estimated and the Jacobian computed. @@ -839,11 +839,11 @@ save('./jacobian.mat',
-3.2 Simscape Model
++3.2 Simscape Model
--@@ -851,9 +851,9 @@ save('./jacobian.mat', Definition of spring parametersopen('stewart_platform/drone_platform.slx'); +open('drone_platform.slx');-kx = 50; % [N/m] -ky = 50; -kz = 50; +kx = 0.5*1e3/3; % [N/m] +ky = 0.5*1e3/3; +kz = 1e3/3; cx = 0.025; % [Nm/rad] cy = 0.025; @@ -871,8 +871,8 @@ We load the Jacobian.-3.3 Identification of the plant
++3.3 Identification of the plant
-The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform. @@ -929,32 +929,32 @@ Gl.OutputName = {'A1',
-3.4 Obtained Dynamics
++3.4 Obtained Dynamics
-+-
Figure 5: Stewart Platform Plant from forces applied by the legs to the acceleration of the platform
+-
Figure 6: Stewart Platform Plant from torques applied by the legs to the angular acceleration of the platform
+-
Figure 7: Stewart Platform Plant from forces applied by the legs to displacement of the legs
+-
Figure 8: Transmissibility
@@ -962,8 +962,8 @@ Gl.OutputName = {'A1',-3.5 Real Approximation of \(G\) at the decoupling frequency
++-3.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\). @@ -989,8 +989,8 @@ H1 = inv(D*real(H1'*
-3.6 Verification of the decoupling using the “Gershgorin Radii”
++3.6 Verification of the decoupling using the “Gershgorin Radii”
-First, the Singular Value Decomposition of \(H_1\) is performed: @@ -1058,7 +1058,7 @@ H = abs(squeeze(freqresp(Gj, freqs, 'Hz')));
+-
Figure 9: Gershgorin Radii of the Coupled and Decoupled plants
@@ -1066,8 +1066,8 @@ H = abs(squeeze(freqresp(Gj, freqs, 'Hz')));-3.7 Decoupled Plant
++3.7 Decoupled Plant
Let’s see the bode plot of the decoupled plant \(G_d(s)\). @@ -1075,14 +1075,14 @@ Let’s see the bode plot of the decoupled plant \(G_d(s)\).
-+-
Figure 10: Decoupled Plant using SVD
+-
Figure 11: Decoupled Plant using the Jacobian
@@ -1090,8 +1090,8 @@ Let’s see the bode plot of the decoupled plant \(G_d(s)\).-3.8 Diagonal Controller
++3.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\). @@ -1107,8 +1107,8 @@ K = eye(6)*C_g/(s
3.9 Centralized Control
++-3.9 Centralized Control
The control diagram for the centralized control is shown below. @@ -1132,8 +1132,8 @@ The Jacobian is used to convert forces in the cartesian frame to forces applied
-3.10 SVD Control
++-3.10 SVD Control
The SVD control architecture is shown below. @@ -1156,8 +1156,8 @@ SVD Control
-3.11 Results
++3.11 Results
Let’s first verify the stability of the closed-loop systems: @@ -1182,16 +1182,16 @@ ans =
ans = logical - 1 + 0-The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure 14. +The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure 14.
-+-
Figure 14: Obtained Transmissibility
@@ -1200,83 +1200,85 @@ The obtained transmissibility in Open-loop, for the centralized control as well-4 Stewart Platform - Analytical Model
++4 Stewart Platform - Analytical Model
--4.1 Characteristics
++-4.1 Characteristics
-L = 0.055; -Zc = 0; -m = 0.2; -k = 1e3; -c = 2*0.1*sqrt(k*m); +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; +Rx = 0.04; % ? +Rz = 0.04; % ? +Ix = m*Rx^2; % ? +Iy = m*Rx^2; % ? +Iz = m*Rz^2; % ?-4.2 Mass Matrix
++-4.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]; +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];-4.3 Jacobian Matrix
++-4.3 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)]; +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)];-4.4 Stifnness matrix and Damping matrix
++-4.4 Stifnness and Damping matrices
-kv = k/3; % [N/m] -kh = 0.5*k/3; % [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 +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-4.5 State Space System
++4.5 State Space System
--A = [zeros(6) eye(6); -M\K -M\C]; +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); @@ -1291,16 +1293,18 @@ ST = ss(A,[Bw Bu],Co,D);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'};-4.6 Transmissibility
++4.6 Transmissibility
TR=ST*[eye(6); zeros(6)]; @@ -1310,22 +1314,22 @@ ST.OutputName = {'ax';'-figure subplot(231) -bodemag(TR(1,1),opts); +bodemag(TR(1,1)); subplot(232) -bodemag(TR(2,2),opts); +bodemag(TR(2,2)); subplot(233) -bodemag(TR(3,3),opts); +bodemag(TR(3,3)); subplot(234) -bodemag(TR(4,4),opts); +bodemag(TR(4,4)); subplot(235) -bodemag(TR(5,5),opts); +bodemag(TR(5,5)); subplot(236) -bodemag(TR(6,6),opts); +bodemag(TR(6,6));+-
Figure 15: Transmissibility
@@ -1333,8 +1337,8 @@ bodemag(TR(6,6),opts);-4.7 Real approximation of \(G(j\omega)\) at decoupling frequency
++4.7 Real approximation of \(G(j\omega)\) at decoupling frequency
-sys1 = ST*[zeros(6); eye(6)]; % take only the forces inputs @@ -1362,8 +1366,8 @@ wf = logspace(-1,2,1000);-4.8 Coupled and Decoupled Plant “Gershgorin Radii”
++4.8 Coupled and Decoupled Plant “Gershgorin Radii”
figure; @@ -1375,7 +1379,7 @@ xlabel('Frequency (Hz)'); ylabel( +
Figure 16: Gershorin Raddi for the coupled plant
@@ -1391,7 +1395,7 @@ xlabel('Frequency (Hz)'); ylabel( +
Figure 17: Gershorin Raddi for the decoupled plant
@@ -1399,8 +1403,8 @@ xlabel('Frequency (Hz)'); ylabel( -4.9 Decoupled Plant
++4.9 Decoupled Plant
-figure; @@ -1409,7 +1413,7 @@ bodemag(U'*sys1*V,op+-
Figure 18: Decoupled Plant
@@ -1417,8 +1421,8 @@ bodemag(U'*sys1*V,op-4.10 Controller
++4.10 Controller
-fc = 2*pi*0.1; % Crossover Frequency [rad/s] @@ -1430,8 +1434,8 @@ cont = eye(6)*c_gain/-4.11 Closed Loop System
++4.11 Closed Loop System
-FEEDIN = [7:12]; % Input of controller @@ -1459,8 +1463,8 @@ TRsvd = STsvd*[eye(6); zeros(6)];-4.12 Results
++4.12 Results
-figure @@ -1486,7 +1490,7 @@ legend('OL','Centralize+
Figure 19: Comparison of the obtained transmissibility for the centralized control and the SVD control
@@ -1497,7 +1501,7 @@ legend('OL','Centralize-diff --git a/index.org b/index.org index 77d74c3..c3e4f3d 100644 --- a/index.org +++ b/index.org @@ -686,6 +686,9 @@ This Matlab function is accessible [[file:gravimeter/pzmap_testCL.m][here]]. #+end_src * Stewart Platform - Simscape Model +:PROPERTIES: +:header-args:matlab+: :tangle stewart_platform/simscape_model.m +:END: ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) <Created: 2020-10-09 ven. 16:21
+Created: 2020-10-13 mar. 14:53
> @@ -700,6 +703,10 @@ This Matlab function is accessible [[file:gravimeter/pzmap_testCL.m][here]]. addpath('stewart_platform/STEP'); #+end_src +#+begin_src matlab :eval no + addpath('STEP'); +#+end_src + ** Jacobian First, the position of the "joints" (points of force application) are estimated and the Jacobian computed. #+begin_src matlab @@ -1292,6 +1299,9 @@ The obtained transmissibility in Open-loop, for the centralized control as well [[file:figs/stewart_platform_simscape_cl_transmissibility.png]] * Stewart Platform - Analytical Model +:PROPERTIES: +:header-args:matlab+: :tangle stewart_platform/analytical_model.m +:END: ** Matlab Init :noexport:ignore: #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) < > diff --git a/stewart_platform/analytical_model.m b/stewart_platform/analytical_model.m new file mode 100644 index 0000000..e1139f5 --- /dev/null +++ b/stewart_platform/analytical_model.m @@ -0,0 +1,196 @@ +%% 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') diff --git a/stewart_platform/drone_platform.slx b/stewart_platform/drone_platform.slx index dda03fe..46e6303 100644 Binary files a/stewart_platform/drone_platform.slx and b/stewart_platform/drone_platform.slx differ diff --git a/stewart_platform/drone_platform_R2017b.slx b/stewart_platform/drone_platform_R2017b.slx new file mode 100644 index 0000000..7599cdd Binary files /dev/null and b/stewart_platform/drone_platform_R2017b.slx differ diff --git a/stewart_platform/simscape_model.m b/stewart_platform/simscape_model.m new file mode 100644 index 0000000..b3df6d5 --- /dev/null +++ b/stewart_platform/simscape_model.m @@ -0,0 +1,499 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +addpath('STEP'); + +% Jacobian +% First, the position of the "joints" (points of force application) are estimated and the Jacobian computed. + +open('drone_platform_jacobian.slx'); + +sim('drone_platform_jacobian'); + +Aa = [a1.Data(1,:); + a2.Data(1,:); + a3.Data(1,:); + a4.Data(1,:); + a5.Data(1,:); + a6.Data(1,:)]'; + +Ab = [b1.Data(1,:); + b2.Data(1,:); + b3.Data(1,:); + b4.Data(1,:); + b5.Data(1,:); + b6.Data(1,:)]'; + +As = (Ab - Aa)./vecnorm(Ab - Aa); + +l = vecnorm(Ab - Aa)'; + +J = [As' , cross(Ab, As)']; + +save('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J'); + +% Simscape Model + +open('drone_platform.slx'); + + + +% Definition of spring parameters + +kx = 0.5*1e3/3; % [N/m] +ky = 0.5*1e3/3; +kz = 1e3/3; + +cx = 0.025; % [Nm/rad] +cy = 0.025; +cz = 0.025; + + + +% We load the Jacobian. + +load('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J'); + +% Identification of the plant +% The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform. + +%% 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; +io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1; +io(io_i) = linio([mdl, '/Inertial Sensor'], 1, 'openoutput'); io_i = io_i + 1; + +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'}; + + + +% There are 24 states (6dof for the bottom platform + 6dof for the top platform). + +size(G) + + + +% #+RESULTS: +% : State-space model with 6 outputs, 12 inputs, and 24 states. + + +% G = G*blkdiag(inv(J), eye(6)); +% G.InputName = {'Dw1', 'Dw2', 'Dw3', 'Dw4', 'Dw5', 'Dw6', ... +% 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; + + + +% Thanks to the Jacobian, we compute the transfer functions in the frame of the legs and in an inertial frame. + +Gx = G*blkdiag(eye(6), inv(J')); +Gx.InputName = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ... + 'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}; + +Gl = J*G; +Gl.OutputName = {'A1', 'A2', 'A3', 'A4', 'A5', 'A6'}; + +% Obtained Dynamics + +freqs = logspace(-1, 2, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))), 'DisplayName', '$A_x/F_x$'); +plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))), 'DisplayName', '$A_y/F_y$'); +plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))), 'DisplayName', '$A_z/F_z$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast'); + +ax2 = subplot(2, 1, 2); +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')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-360:90:360]); + +linkaxes([ax1,ax2],'x'); + + + +% #+name: fig:stewart_platform_translations +% #+caption: Stewart Platform Plant from forces applied by the legs to the acceleration of the platform +% #+RESULTS: +% [[file:figs/stewart_platform_translations.png]] + + +freqs = logspace(-1, 2, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))), 'DisplayName', '$A_{R_x}/M_x$'); +plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))), 'DisplayName', '$A_{R_y}/M_y$'); +plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))), 'DisplayName', '$A_{R_z}/M_z$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude [rad/(Nm)]'); set(gca, 'XTickLabel',[]); +legend('location', 'southeast'); + +ax2 = subplot(2, 1, 2); +hold on; +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz')))); +plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-360:90:360]); + +linkaxes([ax1,ax2],'x'); + + + +% #+name: fig:stewart_platform_rotations +% #+caption: Stewart Platform Plant from torques applied by the legs to the angular acceleration of the platform +% #+RESULTS: +% [[file:figs/stewart_platform_rotations.png]] + + +freqs = logspace(-1, 2, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +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 +for ch_i = 1:6 + plot(freqs, abs(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', 'log'); +ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]); + +ax2 = subplot(2, 1, 2); +hold on; +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'); +ylabel('Phase [deg]'); xlabel('Frequency [Hz]'); +ylim([-180, 180]); +yticks([-360:90:360]); + +linkaxes([ax1,ax2],'x'); + + + +% #+name: fig:stewart_platform_legs +% #+caption: Stewart Platform Plant from forces applied by the legs to displacement of the legs +% #+RESULTS: +% [[file:figs/stewart_platform_legs.png]] + + +freqs = logspace(-1, 2, 1000); + +figure; + +ax1 = subplot(2, 1, 1); +hold on; +% plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', '$D_x/D_{w,x}$'); +% plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'DisplayName', '$D_y/D_{w,y}$'); +% plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Dwz')/s^2, freqs, 'Hz'))), 'DisplayName', '$D_z/D_{w,z}$'); +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(TR(1,1), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$'); +plot(freqs, abs(squeeze(freqresp(TR(2,2), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$'); +plot(freqs, abs(squeeze(freqresp(TR(3,3), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Transmissibility - Translations'); xlabel('Frequency [Hz]'); +legend('location', 'northeast'); + +ax2 = subplot(2, 1, 2); +hold on; +% plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Rwx')/s^2, freqs, 'Hz'))), 'DisplayName', '$R_x/R_{w,x}$'); +% plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'Rwy')/s^2, freqs, 'Hz'))), 'DisplayName', '$R_y/R_{w,y}$'); +% plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Rwz')/s^2, freqs, 'Hz'))), 'DisplayName', '$R_z/R_{w,z}$'); +set(gca,'ColorOrderIndex',1) +plot(freqs, abs(squeeze(freqresp(TR(4,4), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$'); +plot(freqs, abs(squeeze(freqresp(TR(5,5), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$'); +plot(freqs, abs(squeeze(freqresp(TR(6,6), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Transmissibility - Rotations'); xlabel('Frequency [Hz]'); +legend('location', 'northeast'); + +linkaxes([ax1,ax2],'x'); + +% 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$. + +wc = 2*pi*20; % Decoupling frequency [rad/s] + +Gc = G({'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'}, ... + {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}); % Transfer function to find a real approximation + +H1 = evalfr(Gc, 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)))); + +% Verification of the decoupling using the "Gershgorin Radii" +% First, the Singular Value Decomposition of $H_1$ is performed: +% \[ H_1 = U \Sigma V^H \] + + +[U,S,V] = svd(H1); + + + +% Then, the "Gershgorin Radii" is computed for the plant $G_c(s)$ and the "SVD Decoupled Plant" $G_d(s)$: +% \[ G_d(s) = U^T G_c(s) V \] + +% 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(Gc,2)); + +H = abs(squeeze(freqresp(Gc, freqs, 'Hz'))); +for out_i = 1:size(Gc,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: + +Gd = U'*Gc*V; +Gr_decoupled = zeros(length(freqs), size(Gd,2)); + +H = abs(squeeze(freqresp(Gd, freqs, 'Hz'))); +for out_i = 1:size(Gd,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: + +Gj = Gc*inv(J'); +Gr_jacobian = zeros(length(freqs), size(Gj,2)); + +H = abs(squeeze(freqresp(Gj, freqs, 'Hz'))); + +for out_i = 1:size(Gj,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 +plot(freqs, 0.5*ones(size(freqs)), 'k--', 'DisplayName', 'Limit') +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +hold off; +xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii') +legend('location', 'northeast'); + +% Decoupled Plant +% Let's see the bode plot of the decoupled plant $G_d(s)$. +% \[ G_d(s) = U^T G_c(s) V \] + + +freqs = logspace(-1, 2, 1000); + +figure; +hold on; +for ch_i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gd(ch_i, ch_i), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$G(%i, %i)$', ch_i, ch_i)); +end +for in_i = 1:5 + for out_i = in_i+1:6 + plot(freqs, abs(squeeze(freqresp(Gd(out_i, in_i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); xlabel('Frequency [Hz]'); +legend('location', 'southeast'); + + + +% #+name: fig:simscape_model_decoupled_plant_svd +% #+caption: Decoupled Plant using SVD +% #+RESULTS: +% [[file:figs/simscape_model_decoupled_plant_svd.png]] + + +freqs = logspace(-1, 2, 1000); + +figure; +hold on; +for ch_i = 1:6 + plot(freqs, abs(squeeze(freqresp(Gj(ch_i, ch_i), freqs, 'Hz'))), ... + 'DisplayName', sprintf('$G(%i, %i)$', ch_i, ch_i)); +end +for in_i = 1:5 + for out_i = in_i+1:6 + plot(freqs, abs(squeeze(freqresp(Gj(out_i, in_i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ... + 'HandleVisibility', 'off'); + end +end +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Amplitude'); xlabel('Frequency [Hz]'); +legend('location', 'southeast'); + +% 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$. + + +wc = 2*pi*0.1; % Crossover Frequency [rad/s] +C_g = 50; % DC Gain + +K = eye(6)*C_g/(s+wc); + + + +% #+RESULTS: +% [[file:figs/centralized_control.png]] + + +G_cen = feedback(G, inv(J')*K, [7:12], [1:6]); + + + +% #+RESULTS: +% [[file:figs/svd_control.png]] + +% SVD Control + +G_svd = feedback(G, pinv(V')*K*pinv(U), [7:12], [1:6]); + +% Results +% Let's first verify the stability of the closed-loop systems: + +isstable(G_cen) + + + +% #+RESULTS: +% : ans = +% : logical +% : 1 + + +isstable(G_svd) + + + +% #+RESULTS: +% : ans = +% : logical +% : 0 + +% 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]]. + + +freqs = logspace(-3, 3, 1000); + +figure + +ax1 = subplot(2, 3, 1); +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'); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Transmissibility - $D_x/D_{w,x}$'); xlabel('Frequency [Hz]'); +legend('location', 'southwest'); + +ax2 = subplot(2, 3, 2); +hold on; +plot(freqs, abs(squeeze(freqresp(G( 'Ay', 'Dwy')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_cen('Ay', 'Dwy')/s^2, freqs, 'Hz')))); +plot(freqs, abs(squeeze(freqresp(G_svd('Ay', 'Dwy')/s^2, freqs, 'Hz')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Transmissibility - $D_y/D_{w,y}$'); xlabel('Frequency [Hz]'); + +ax3 = subplot(2, 3, 3); +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('Transmissibility - $D_z/D_{w,z}$'); xlabel('Frequency [Hz]'); + +ax4 = subplot(2, 3, 4); +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')))); +hold off; +set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log'); +ylabel('Transmissibility - $R_x/R_{w,x}$'); xlabel('Frequency [Hz]'); + +ax5 = subplot(2, 3, 5); +hold on; +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('Transmissibility - $R_y/R_{w,y}$'); xlabel('Frequency [Hz]'); + +ax6 = subplot(2, 3, 6); +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('Transmissibility - $R_z/R_{w,z}$'); xlabel('Frequency [Hz]'); + +linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x'); +xlim([freqs(1), freqs(end)]);