2020-09-21 13:08:36 +02:00
<?xml version="1.0" encoding="utf-8"?>
< !DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"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-09-21 18:54:41 +02:00
<!-- 2020 - 09 - 21 lun. 18:53 -->
2020-09-21 13:08:36 +02:00
< meta http-equiv = "Content-Type" content = "text/html;charset=utf-8" / >
< title > SVD Control< / title >
< meta name = "generator" content = "Org mode" / >
< meta name = "author" content = "Dehaeze Thomas" / >
< link rel = "stylesheet" type = "text/css" href = "./css/htmlize.css" / >
< link rel = "stylesheet" type = "text/css" href = "./css/readtheorg.css" / >
< link rel = "stylesheet" type = "text/css" href = "./css/zenburn.css" / >
< script type = "text/javascript" src = "./js/jquery.min.js" > < / script >
< script type = "text/javascript" src = "./js/bootstrap.min.js" > < / script >
< script type = "text/javascript" src = "./js/jquery.stickytableheaders.min.js" > < / script >
< script type = "text/javascript" src = "./js/readtheorg.js" > < / script >
2020-09-21 18:03:40 +02:00
< script > M a t h J a x = {
tex: {
tags: 'ams',
macros: {bm: ["\\boldsymbol{#1}",1],}
}
};
< / script >
< script type = "text/javascript" src = "https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js" > < / script >
2020-09-21 13:08:36 +02:00
< / head >
< body >
< div id = "org-div-home-and-up" >
< a accesskey = "h" href = "../index.html" > UP < / a >
|
< a accesskey = "H" href = "../index.html" > HOME < / a >
< / div > < div id = "content" >
< h1 class = "title" > SVD Control< / h1 >
< div id = "table-of-contents" >
< h2 > Table of Contents< / h2 >
< div id = "text-table-of-contents" >
< ul >
2020-09-21 18:54:41 +02:00
< li > < a href = "#orgcdbafa9" > 1. Gravimeter - Simscape Model< / a >
2020-09-21 13:08:36 +02:00
< ul >
2020-09-21 18:54:41 +02:00
< li > < a href = "#orge0e53b8" > 1.1. Simulink< / a > < / li >
2020-09-21 13:08:36 +02:00
< / ul >
< / li >
2020-09-21 18:54:41 +02:00
< li > < a href = "#orgbbb84cc" > 2. Stewart Platform - Simscape Model< / a >
2020-09-21 13:08:36 +02:00
< ul >
2020-09-21 18:54:41 +02:00
< li > < a href = "#org22e9f4a" > 2.1. Jacobian< / a > < / li >
< li > < a href = "#org692c2cc" > 2.2. Simscape Model< / a > < / li >
< li > < a href = "#orga491806" > 2.3. Identification of the plant< / a > < / li >
< li > < a href = "#orgac4aba9" > 2.4. Obtained Dynamics< / a > < / li >
< li > < a href = "#org1e57236" > 2.5. Real Approximation of \(G\) at the decoupling frequency< / a > < / li >
< li > < a href = "#org0172b58" > 2.6. Verification of the decoupling using the “ Gershgorin Radii” < / a > < / li >
< li > < a href = "#org37cffae" > 2.7. Decoupled Plant< / a > < / li >
< li > < a href = "#org868aae1" > 2.8. Diagonal Controller< / a > < / li >
< li > < a href = "#org14f6c79" > 2.9. Centralized Control< / a > < / li >
< li > < a href = "#orgdfd243c" > 2.10. SVD Control< / a > < / li >
< li > < a href = "#orge500424" > 2.11. Results< / a > < / li >
2020-09-21 18:03:40 +02:00
< / ul >
< / li >
2020-09-21 18:54:41 +02:00
< li > < a href = "#org95fefe5" > 3. Stewart Platform - Analytical Model< / a >
2020-09-21 18:03:40 +02:00
< ul >
2020-09-21 18:54:41 +02:00
< li > < a href = "#org79b474c" > 3.1. Characteristics< / a > < / li >
< li > < a href = "#org8f55e38" > 3.2. Mass Matrix< / a > < / li >
< li > < a href = "#orgecf190a" > 3.3. Jacobian Matrix< / a > < / li >
< li > < a href = "#org7260a03" > 3.4. Stifnness matrix and Damping matrix< / a > < / li >
< li > < a href = "#orga9cf2e9" > 3.5. State Space System< / a > < / li >
< li > < a href = "#org41defbd" > 3.6. Transmissibility< / a > < / li >
< li > < a href = "#org5767eaf" > 3.7. Real approximation of \(G(j\omega)\) at decoupling frequency< / a > < / li >
< li > < a href = "#orgae9dbc4" > 3.8. Coupled and Decoupled Plant “ Gershgorin Radii” < / a > < / li >
< li > < a href = "#orgc43f18b" > 3.9. Decoupled Plant< / a > < / li >
< li > < a href = "#orgf75bbde" > 3.10. Controller< / a > < / li >
< li > < a href = "#org275519b" > 3.11. Closed Loop System< / a > < / li >
< li > < a href = "#org16451fc" > 3.12. Results< / a > < / li >
2020-09-21 13:08:36 +02:00
< / ul >
< / li >
< / ul >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgcdbafa9" class = "outline-2" >
< h2 id = "orgcdbafa9" > < span class = "section-number-2" > 1< / span > Gravimeter - Simscape Model< / h2 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-2" id = "text-1" >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orge0e53b8" class = "outline-3" >
< h3 id = "orge0e53b8" > < span class = "section-number-3" > 1.1< / span > Simulink< / h3 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-3" id = "text-1-1" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > open('gravimeter.slx')
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > %% Name of the Simulink File
mdl = 'gravimeter';
%% 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, '/Acc_side'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Acc_side'], 2, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Acc_top'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Acc_top'], 2, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io);
G.InputName = {'F1', 'F2', 'F3'};
G.OutputName = {'Ax1', 'Az1', 'Ax2', 'Az2'};
< / pre >
< / div >
< p >
The plant as 6 states as expected (2 translations + 1 rotation)
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > size(G)
< / pre >
< / div >
< pre class = "example" >
State-space model with 4 outputs, 3 inputs, and 6 states.
< / pre >
2020-09-21 18:54:41 +02:00
< div id = "org3d20c51" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/open_loop_tf.png" alt = "open_loop_tf.png" / >
< / p >
< p > < span class = "figure-number" > Figure 1: < / span > Open Loop Transfer Function from 3 Actuators to 4 Accelerometers< / p >
< / div >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgbbb84cc" class = "outline-2" >
< h2 id = "orgbbb84cc" > < span class = "section-number-2" > 2< / span > Stewart Platform - Simscape Model< / h2 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-2" id = "text-2" >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org22e9f4a" class = "outline-3" >
< h3 id = "org22e9f4a" > < span class = "section-number-3" > 2.1< / span > Jacobian< / h3 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-3" id = "text-2-1" >
< p >
First, the position of the “ joints” (points of force application) are estimated and the Jacobian computed.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > open('stewart_platform/drone_platform_jacobian.slx');
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > sim('drone_platform_jacobian');
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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');
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org692c2cc" class = "outline-3" >
< h3 id = "org692c2cc" > < span class = "section-number-3" > 2.2< / span > Simscape Model< / h3 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-3" id = "text-2-2" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > open('stewart_platform/drone_platform.slx');
< / pre >
< / div >
< p >
Definition of spring parameters
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > kx = 50; % [N/m]
ky = 50;
kz = 50;
cx = 0.025; % [Nm/rad]
cy = 0.025;
cz = 0.025;
< / pre >
< / div >
< p >
We load the Jacobian.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > load('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
< / pre >
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-09-21 13:08:36 +02:00
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orga491806" class = "outline-3" >
< h3 id = "orga491806" > < span class = "section-number-3" > 2.3< / span > Identification of the plant< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-3" >
2020-09-21 13:08:36 +02:00
< p >
The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > %% Name of the Simulink File
mdl = 'drone_platform';
%% Input/Output definition
clear io; io_i = 1;
2020-09-21 18:03:40 +02:00
io(io_i) = linio([mdl, '/Dw'], 1, 'openinput'); io_i = io_i + 1;
2020-09-21 13:08:36 +02:00
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);
2020-09-21 18:03:40 +02:00
G.InputName = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
2020-09-21 13:08:36 +02:00
G.OutputName = {'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'};
< / pre >
< / div >
2020-09-21 18:03:40 +02:00
< p >
There are 24 states (6dof for the bottom platform + 6dof for the top platform).
< / p >
2020-09-21 13:14:25 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > size(G)
< / pre >
< / div >
< pre class = "example" >
2020-09-21 18:03:40 +02:00
State-space model with 6 outputs, 12 inputs, and 24 states.
2020-09-21 13:14:25 +02:00
< / pre >
2020-09-21 18:03:40 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > % G = G*blkdiag(inv(J), eye(6));
% G.InputName = {'Dw1', 'Dw2', 'Dw3', 'Dw4', 'Dw5', 'Dw6', ...
% 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
< / pre >
< / div >
2020-09-21 13:08:36 +02:00
< p >
Thanks to the Jacobian, we compute the transfer functions in the frame of the legs and in an inertial frame.
< / p >
< div class = "org-src-container" >
2020-09-21 18:03:40 +02:00
< pre class = "src src-matlab" > Gx = G*blkdiag(eye(6), inv(J'));
Gx.InputName = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-09-21 13:08:36 +02:00
2020-09-21 18:03:40 +02:00
Gl = J*G;
2020-09-21 13:08:36 +02:00
Gl.OutputName = {'A1', 'A2', 'A3', 'A4', 'A5', 'A6'};
< / pre >
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-09-21 13:08:36 +02:00
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgac4aba9" class = "outline-3" >
< h3 id = "orgac4aba9" > < span class = "section-number-3" > 2.4< / span > Obtained Dynamics< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-4" >
2020-09-21 13:08:36 +02:00
2020-09-21 18:54:41 +02:00
< div id = "org24b69fe" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/stewart_platform_translations.png" alt = "stewart_platform_translations.png" / >
< / p >
< p > < span class = "figure-number" > Figure 2: < / span > Stewart Platform Plant from forces applied by the legs to the acceleration of the platform< / p >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "orgba8e960" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/stewart_platform_rotations.png" alt = "stewart_platform_rotations.png" / >
< / p >
< p > < span class = "figure-number" > Figure 3: < / span > Stewart Platform Plant from torques applied by the legs to the angular acceleration of the platform< / p >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "orga66f388" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/stewart_platform_legs.png" alt = "stewart_platform_legs.png" / >
< / p >
< p > < span class = "figure-number" > Figure 4: < / span > Stewart Platform Plant from forces applied by the legs to displacement of the legs< / p >
< / div >
2020-09-21 18:03:40 +02:00
2020-09-21 18:54:41 +02:00
< div id = "orga7254f2" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/stewart_platform_transmissibility.png" alt = "stewart_platform_transmissibility.png" / >
< / p >
< p > < span class = "figure-number" > Figure 5: < / span > Transmissibility< / p >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org1e57236" class = "outline-3" >
< h3 id = "org1e57236" > < span class = "section-number-3" > 2.5< / span > Real Approximation of \(G\) at the decoupling frequency< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-5" >
< p >
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\).
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > wc = 2*pi*20; % Decoupling frequency [rad/s]
2020-09-21 18:54:41 +02:00
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);
2020-09-21 18:03:40 +02:00
< / pre >
< / div >
< p >
The real approximation is computed as follows:
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > D = pinv(real(H1'*H1));
H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org0172b58" class = "outline-3" >
< h3 id = "org0172b58" > < span class = "section-number-3" > 2.6< / span > Verification of the decoupling using the “ Gershgorin Radii” < / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-6" >
< p >
First, the Singular Value Decomposition of \(H_1\) is performed:
\[ H_1 = U \Sigma V^H \]
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > [U,S,V] = svd(H1);
< / pre >
< / div >
< p >
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 \]
< / p >
< p >
2020-09-21 18:54:41 +02:00
This is computed over the following frequencies.
2020-09-21 18:03:40 +02:00
< / p >
< div class = "org-src-container" >
2020-09-21 18:54:41 +02:00
< pre class = "src src-matlab" > freqs = logspace(-2, 2, 1000); % [Hz]
2020-09-21 18:03:40 +02:00
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< p >
Gershgorin Radii for the coupled plant:
< / p >
2020-09-21 18:03:40 +02:00
< div class = "org-src-container" >
2020-09-21 18:54:41 +02:00
< pre class = "src src-matlab" > 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, :));
2020-09-21 18:03:40 +02:00
end
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< p >
Gershgorin Radii for the decoupled plant using SVD:
< / p >
2020-09-21 18:03:40 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > Gd = U'*Gc*V;
2020-09-21 18:54:41 +02:00
Gr_decoupled = zeros(length(freqs), size(Gd,2));
2020-09-21 18:03:40 +02:00
2020-09-21 18:54:41 +02:00
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, :));
2020-09-21 18:03:40 +02:00
end
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< p >
Gershgorin Radii for the decoupled plant using the Jacobian:
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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
< / pre >
< / div >
< div id = "orgca6454b" class = "figure" >
< p > < img src = "figs/simscape_model_gershgorin_radii.png" alt = "simscape_model_gershgorin_radii.png" / >
< / p >
< p > < span class = "figure-number" > Figure 6: < / span > Gershgorin Radii of the Coupled and Decoupled plants< / p >
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org37cffae" class = "outline-3" >
< h3 id = "org37cffae" > < span class = "section-number-3" > 2.7< / span > Decoupled Plant< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-7" >
< p >
Let’ s see the bode plot of the decoupled plant \(G_d(s)\).
\[ G_d(s) = U^T G_c(s) V \]
< / p >
2020-09-21 18:54:41 +02:00
< div id = "org5478477" 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 7: < / span > Decoupled Plant using SVD< / p >
< / div >
< div id = "org8fa8056" 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 8: < / span > Decoupled Plant using the Jacobian< / p >
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org868aae1" class = "outline-3" >
< h3 id = "org868aae1" > < span class = "section-number-3" > 2.8< / span > Diagonal Controller< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-8" >
< p >
The controller \(K\) is a diagonal controller consisting a low pass filters with a crossover frequency \(\omega_c\) and a DC gain \(C_g\).
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > wc = 2*pi*0.1; % Crossover Frequency [rad/s]
C_g = 50; % DC Gain
K = eye(6)*C_g/(s+wc);
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org14f6c79" class = "outline-3" >
< h3 id = "org14f6c79" > < span class = "section-number-3" > 2.9< / span > Centralized Control< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-9" >
< p >
The control diagram for the centralized control is shown below.
< / p >
< p >
The controller \(K_c\) is “ working” in an cartesian frame.
The Jacobian is used to convert forces in the cartesian frame to forces applied by the actuators.
< / p >
< div class = "figure" >
< p > < img src = "figs/centralized_control.png" alt = "centralized_control.png" / >
< / p >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > G_cen = feedback(G, inv(J')*K, [7:12], [1:6]);
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgdfd243c" class = "outline-3" >
< h3 id = "orgdfd243c" > < span class = "section-number-3" > 2.10< / span > SVD Control< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-10" >
< p >
The SVD control architecture is shown below.
The matrices \(U\) and \(V\) are used to decoupled the plant \(G\).
< / p >
< div class = "figure" >
< p > < img src = "figs/svd_control.png" alt = "svd_control.png" / >
< / p >
< / div >
< p >
SVD Control
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > G_svd = feedback(G, pinv(V')*K*pinv(U), [7:12], [1:6]);
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orge500424" class = "outline-3" >
< h3 id = "orge500424" > < span class = "section-number-3" > 2.11< / span > Results< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-2-11" >
< p >
2020-09-21 18:54:41 +02:00
Let’ s first verify the stability of the closed-loop systems:
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > isstable(G_cen)
< / pre >
< / div >
< pre class = "example" >
ans =
logical
1
< / pre >
< div class = "org-src-container" >
< pre class = "src src-matlab" > isstable(G_svd)
< / pre >
< / div >
< pre class = "example" >
ans =
logical
1
< / pre >
< p >
The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure < a href = "#orgcffbfc0" > 11< / a > .
2020-09-21 18:03:40 +02:00
< / p >
2020-09-21 18:54:41 +02:00
< div id = "orgcffbfc0" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/stewart_platform_simscape_cl_transmissibility.png" alt = "stewart_platform_simscape_cl_transmissibility.png" / >
< / p >
2020-09-21 18:54:41 +02:00
< p > < span class = "figure-number" > Figure 11: < / span > Obtained Transmissibility< / p >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org95fefe5" class = "outline-2" >
< h2 id = "org95fefe5" > < span class = "section-number-2" > 3< / span > Stewart Platform - Analytical Model< / h2 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-2" id = "text-3" >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org79b474c" class = "outline-3" >
< h3 id = "org79b474c" > < span class = "section-number-3" > 3.1< / span > Characteristics< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-1" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > L = 0.055;
Zc = 0;
m = 0.2;
k = 1e3;
c = 2*0.1*sqrt(k*m);
Rx = 0.04;
Rz = 0.04;
Ix = m*Rx^2;
Iy = m*Rx^2;
Iz = m*Rz^2;
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org8f55e38" class = "outline-3" >
< h3 id = "org8f55e38" > < span class = "section-number-3" > 3.2< / span > Mass Matrix< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-2" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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];
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgecf190a" class = "outline-3" >
< h3 id = "orgecf190a" > < span class = "section-number-3" > 3.3< / span > Jacobian Matrix< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-3" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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)];
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org7260a03" class = "outline-3" >
< h3 id = "org7260a03" > < span class = "section-number-3" > 3.4< / span > Stifnness matrix and Damping matrix< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-4" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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
C = c*K/100000; % Damping Matrix
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orga9cf2e9" class = "outline-3" >
< h3 id = "orga9cf2e9" > < span class = "section-number-3" > 3.5< / span > State Space System< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-5" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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);
< / pre >
< / div >
< ul class = "org-ul" >
< li > OUT 1-6: 6 dof< / li >
< li > IN 1-6 : ground displacement in the directions of the legs< / li >
< li > IN 7-12: forces in the actuators.< / li >
< / ul >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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'};
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org41defbd" class = "outline-3" >
< h3 id = "org41defbd" > < span class = "section-number-3" > 3.6< / span > Transmissibility< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-6" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > TR=ST*[eye(6); zeros(6)];
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > figure
subplot(231)
bodemag(TR(1,1),opts);
subplot(232)
bodemag(TR(2,2),opts);
subplot(233)
bodemag(TR(3,3),opts);
subplot(234)
bodemag(TR(4,4),opts);
subplot(235)
bodemag(TR(5,5),opts);
subplot(236)
bodemag(TR(6,6),opts);
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "orgf939ecb" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/stewart_platform_analytical_transmissibility.png" alt = "stewart_platform_analytical_transmissibility.png" / >
< / p >
2020-09-21 18:54:41 +02:00
< p > < span class = "figure-number" > Figure 12: < / span > Transmissibility< / p >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org5767eaf" class = "outline-3" >
< h3 id = "org5767eaf" > < span class = "section-number-3" > 3.7< / span > Real approximation of \(G(j\omega)\) at decoupling frequency< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-7" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgae9dbc4" class = "outline-3" >
< h3 id = "orgae9dbc4" > < span class = "section-number-3" > 3.8< / span > Coupled and Decoupled Plant “ Gershgorin Radii” < / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-8" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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')
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "org52a89d1" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/gershorin_raddii_coupled_analytical.png" alt = "gershorin_raddii_coupled_analytical.png" / >
< / p >
2020-09-21 18:54:41 +02:00
< p > < span class = "figure-number" > Figure 13: < / span > Gershorin Raddi for the coupled plant< / p >
2020-09-21 18:03:40 +02:00
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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')
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "org31edc7e" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/gershorin_raddii_decoupled_analytical.png" alt = "gershorin_raddii_decoupled_analytical.png" / >
< / p >
2020-09-21 18:54:41 +02:00
< p > < span class = "figure-number" > Figure 14: < / span > Gershorin Raddi for the decoupled plant< / p >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgc43f18b" class = "outline-3" >
< h3 id = "orgc43f18b" > < span class = "section-number-3" > 3.9< / span > Decoupled Plant< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-9" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > figure;
bodemag(U'*sys1*V,opts)
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "org160c886" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/stewart_platform_analytical_decoupled_plant.png" alt = "stewart_platform_analytical_decoupled_plant.png" / >
< / p >
2020-09-21 18:54:41 +02:00
< p > < span class = "figure-number" > Figure 15: < / span > Decoupled Plant< / p >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-orgf75bbde" class = "outline-3" >
< h3 id = "orgf75bbde" > < span class = "section-number-3" > 3.10< / span > Controller< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-10" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > fc = 2*pi*0.1; % Crossover Frequency [rad/s]
c_gain = 50; %
cont = eye(6)*c_gain/(s+fc);
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org275519b" class = "outline-3" >
< h3 id = "org275519b" > < span class = "section-number-3" > 3.11< / span > Closed Loop System< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-11" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > FEEDIN = [7:12]; % Input of controller
FEEDOUT = [1:6]; % Output of controller
< / pre >
< / div >
< p >
Centralized Control
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > STcen = feedback(ST, inv(Bj)*cont, FEEDIN, FEEDOUT);
TRcen = STcen*[eye(6); zeros(6)];
< / pre >
< / div >
< p >
SVD Control
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > STsvd = feedback(ST, pinv(V')*cont*pinv(U), FEEDIN, FEEDOUT);
TRsvd = STsvd*[eye(6); zeros(6)];
< / pre >
< / div >
< / div >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "outline-container-org16451fc" class = "outline-3" >
< h3 id = "org16451fc" > < span class = "section-number-3" > 3.12< / span > Results< / h3 >
2020-09-21 18:03:40 +02:00
< div class = "outline-text-3" id = "text-3-12" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > 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')
< / pre >
< / div >
2020-09-21 18:54:41 +02:00
< div id = "org0df88b9" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/stewart_platform_analytical_svd_cen_comp.png" alt = "stewart_platform_analytical_svd_cen_comp.png" / >
< / p >
2020-09-21 18:54:41 +02:00
< p > < span class = "figure-number" > Figure 16: < / span > Comparison of the obtained transmissibility for the centralized control and the SVD control< / p >
2020-09-21 18:03:40 +02:00
< / div >
2020-09-21 13:08:36 +02:00
< / div >
< / div >
< / div >
< / div >
< div id = "postamble" class = "status" >
< p class = "author" > Author: Dehaeze Thomas< / p >
2020-09-21 18:54:41 +02:00
< p class = "date" > Created: 2020-09-21 lun. 18:53< / p >
2020-09-21 13:08:36 +02:00
< / div >
< / body >
< / html >
