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-10-13 14:51:15 +02:00
<!-- 2020 - 10 - 09 ven. 16:21 -->
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-10-13 14:51:15 +02:00
< li > < a href = "#org90d7008" > 1. Gravimeter - Simscape Model< / a >
2020-09-21 13:08:36 +02:00
< ul >
2020-10-13 14:51:15 +02:00
< li > < a href = "#org29b9308" > 1.1. Introduction< / a > < / li >
< li > < a href = "#orgd333b87" > 1.2. Simscape Model - Parameters< / a > < / li >
< li > < a href = "#org09b581d" > 1.3. System Identification - Without Gravity< / a > < / li >
< li > < a href = "#org4f091cc" > 1.4. System Identification - With Gravity< / a > < / li >
< li > < a href = "#org7c4effc" > 1.5. Analytical Model< / a >
2020-09-30 17:16:30 +02:00
< ul >
2020-10-13 14:51:15 +02:00
< li > < a href = "#org20ea2aa" > 1.5.1. Parameters< / a > < / li >
< li > < a href = "#org02cb447" > 1.5.2. Generation of the State Space Model< / a > < / li >
< li > < a href = "#org9417f40" > 1.5.3. Comparison with the Simscape Model< / a > < / li >
< li > < a href = "#org6c56e64" > 1.5.4. Analysis< / a > < / li >
< li > < a href = "#orgeb20c08" > 1.5.5. Control Section< / a > < / li >
< li > < a href = "#org931022f" > 1.5.6. Greshgorin radius< / a > < / li >
< li > < a href = "#org1d56ec4" > 1.5.7. Injecting ground motion in the system to have the output< / a > < / li >
2020-09-30 17:16:30 +02:00
< / ul >
< / li >
2020-09-21 13:08:36 +02:00
< / ul >
< / li >
2020-10-13 14:51:15 +02:00
< li > < a href = "#org36d6b85" > 2. Gravimeter - Functions< / a >
2020-09-21 13:08:36 +02:00
< ul >
2020-10-13 14:51:15 +02:00
< li > < a href = "#orgbb4529b" > 2.1. < code > align< / code > < / a > < / li >
< li > < a href = "#orge0ed8bf" > 2.2. < code > pzmap_testCL< / code > < / a > < / li >
2020-09-21 18:03:40 +02:00
< / ul >
< / li >
2020-10-13 14:51:15 +02:00
< li > < a href = "#org5afd29d" > 3. Stewart Platform - Simscape Model< / a >
2020-09-21 18:03:40 +02:00
< ul >
2020-10-13 14:51:15 +02:00
< li > < a href = "#orgff944f3" > 3.1. Jacobian< / a > < / li >
< li > < a href = "#org07ffe6c" > 3.2. Simscape Model< / a > < / li >
< li > < a href = "#org9aaf0d3" > 3.3. Identification of the plant< / a > < / li >
< li > < a href = "#orgb0b01e3" > 3.4. Obtained Dynamics< / a > < / li >
< li > < a href = "#org1de55ce" > 3.5. Real Approximation of \(G\) at the decoupling frequency< / a > < / li >
< li > < a href = "#org53d60e1" > 3.6. Verification of the decoupling using the “ Gershgorin Radii” < / a > < / li >
< li > < a href = "#org40c1d24" > 3.7. Decoupled Plant< / a > < / li >
< li > < a href = "#orgdfcd158" > 3.8. Diagonal Controller< / a > < / li >
< li > < a href = "#org25e3b35" > 3.9. Centralized Control< / a > < / li >
< li > < a href = "#org4d83793" > 3.10. SVD Control< / a > < / li >
< li > < a href = "#org7cece79" > 3.11. Results< / a > < / li >
< / ul >
< / li >
< li > < a href = "#org8b11aba" > 4. Stewart Platform - Analytical Model< / a >
< ul >
< li > < a href = "#org2a175f6" > 4.1. Characteristics< / a > < / li >
< li > < a href = "#org9efa4f4" > 4.2. Mass Matrix< / a > < / li >
< li > < a href = "#org97bc497" > 4.3. Jacobian Matrix< / a > < / li >
< li > < a href = "#org7c9679d" > 4.4. Stifnness matrix and Damping matrix< / a > < / li >
< li > < a href = "#org00e8691" > 4.5. State Space System< / a > < / li >
< li > < a href = "#org8a70996" > 4.6. Transmissibility< / a > < / li >
< li > < a href = "#org12c95c9" > 4.7. Real approximation of \(G(j\omega)\) at decoupling frequency< / a > < / li >
< li > < a href = "#orgc58b81c" > 4.8. Coupled and Decoupled Plant “ Gershgorin Radii” < / a > < / li >
< li > < a href = "#org2ba91f6" > 4.9. Decoupled Plant< / a > < / li >
< li > < a href = "#orgc73a283" > 4.10. Controller< / a > < / li >
< li > < a href = "#org9c82ee4" > 4.11. Closed Loop System< / a > < / li >
< li > < a href = "#org80cd406" > 4.12. Results< / a > < / li >
2020-09-21 13:08:36 +02:00
< / ul >
< / li >
< / ul >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org90d7008" class = "outline-2" >
< h2 id = "org90d7008" > < 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-10-13 14:51:15 +02:00
< div id = "outline-container-org29b9308" class = "outline-3" >
< h3 id = "org29b9308" > < span class = "section-number-3" > 1.1< / span > Introduction< / h3 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-3" id = "text-1-1" >
2020-10-05 18:06:49 +02:00
2020-10-13 14:51:15 +02:00
< div id = "org7df72f4" class = "figure" >
2020-10-05 18:06:49 +02:00
< p > < img src = "figs/gravimeter_model.png" alt = "gravimeter_model.png" / >
< / p >
< p > < span class = "figure-number" > Figure 1: < / span > Model of the gravimeter< / p >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-orgd333b87" class = "outline-3" >
< h3 id = "orgd333b87" > < span class = "section-number-3" > 1.2< / span > Simscape Model - Parameters< / h3 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-3" id = "text-1-2" >
2020-09-21 13:08:36 +02:00
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > open(< span class = "org-string" > 'gravimeter.slx'< / span > )
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
2020-09-30 17:16:30 +02:00
< p >
Parameters
< / p >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > l = 1.0; < span class = "org-comment" > % Length of the mass [m]< / span >
la = 0.5; < span class = "org-comment" > % Position of Act. [m]< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
h = 3.4; < span class = "org-comment" > % Height of the mass [m]< / span >
ha = 1.7; < span class = "org-comment" > % Position of Act. [m]< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
m = 400; < span class = "org-comment" > % Mass [kg]< / span >
I = 115; < span class = "org-comment" > % Inertia [kg m^2]< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
k = 15e3; < span class = "org-comment" > % Actuator Stiffness [N/m]< / span >
c = 0.03; < span class = "org-comment" > % Actuator Damping [N/(m/s)]< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
deq = 0.2; < span class = "org-comment" > % Length of the actuators [m]< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
g = 0; < span class = "org-comment" > % Gravity [m/s2]< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org09b581d" class = "outline-3" >
< h3 id = "org09b581d" > < span class = "section-number-3" > 1.3< / span > System Identification - Without Gravity< / h3 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-3" id = "text-1-3" >
2020-09-21 13:08:36 +02:00
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Name of the Simulink File< / span > < / span >
mdl = < span class = "org-string" > 'gravimeter'< / span > ;
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Input/Output definition< / span > < / span >
2020-09-21 13:08:36 +02:00
clear io; io_i = 1;
2020-10-05 18:06:49 +02:00
io(io_i) = linio([mdl, < span class = "org-string" > '/F1'< / span > ], 1, < span class = "org-string" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/F2'< / span > ], 1, < span class = "org-string" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/F3'< / span > ], 1, < span class = "org-string" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/Acc_side'< / span > ], 1, < span class = "org-string" > 'openoutput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/Acc_side'< / span > ], 2, < span class = "org-string" > 'openoutput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/Acc_top'< / span > ], 1, < span class = "org-string" > 'openoutput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/Acc_top'< / span > ], 2, < span class = "org-string" > 'openoutput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
2020-09-21 13:08:36 +02:00
G = linearize(mdl, io);
2020-10-05 18:06:49 +02:00
G.InputName = {< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > };
G.OutputName = {< span class = "org-string" > 'Ax1'< / span > , < span class = "org-string" > 'Az1'< / span > , < span class = "org-string" > 'Ax2'< / span > , < span class = "org-string" > 'Az2'< / span > };
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
2020-09-30 17:16:30 +02:00
< pre class = "example" >
pole(G)
ans =
2020-10-05 18:06:49 +02:00
-0.000473481142385795 + 21.7596190728632i
-0.000473481142385795 - 21.7596190728632i
-7.49842879459172e-05 + 8.6593576906982i
-7.49842879459172e-05 - 8.6593576906982i
-5.1538686792578e-06 + 2.27025295182756i
-5.1538686792578e-06 - 2.27025295182756i
2020-09-30 17:16:30 +02:00
< / pre >
2020-09-21 13:08:36 +02:00
< 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-10-13 14:51:15 +02:00
< div id = "orgdd275bb" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/open_loop_tf.png" alt = "open_loop_tf.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 2: < / span > Open Loop Transfer Function from 3 Actuators to 4 Accelerometers< / p >
2020-09-21 13:08:36 +02:00
< / div >
< / div >
< / div >
2020-09-30 17:16:30 +02:00
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org4f091cc" class = "outline-3" >
< h3 id = "org4f091cc" > < span class = "section-number-3" > 1.4< / span > System Identification - With Gravity< / h3 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-3" id = "text-1-4" >
2020-09-30 17:16:30 +02:00
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > g = 9.80665; < span class = "org-comment" > % Gravity [m/s2]< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Gg = linearize(mdl, io);
2020-10-05 18:06:49 +02:00
Gg.InputName = {< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > };
Gg.OutputName = {< span class = "org-string" > 'Ax1'< / span > , < span class = "org-string" > 'Az1'< / span > , < span class = "org-string" > 'Ax2'< / span > , < span class = "org-string" > 'Az2'< / span > };
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< p >
We can now see that the system is unstable due to gravity.
< / p >
< pre class = "example" >
2020-10-02 09:48:24 +02:00
pole(Gg)
2020-09-30 17:16:30 +02:00
ans =
2020-10-05 18:06:49 +02:00
-10.9848275341252 + 0i
10.9838836405201 + 0i
-7.49855379478109e-05 + 8.65962885770051i
-7.49855379478109e-05 - 8.65962885770051i
-6.68819548733559e-06 + 0.832960422243848i
-6.68819548733559e-06 - 0.832960422243848i
2020-09-30 17:16:30 +02:00
< / pre >
2020-10-13 14:51:15 +02:00
< div id = "org392bf82" class = "figure" >
2020-09-30 17:16:30 +02:00
< p > < img src = "figs/open_loop_tf_g.png" alt = "open_loop_tf_g.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 3: < / span > Open Loop Transfer Function from 3 Actuators to 4 Accelerometers with an without gravity< / p >
2020-09-30 17:16:30 +02:00
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org7c4effc" class = "outline-3" >
< h3 id = "org7c4effc" > < span class = "section-number-3" > 1.5< / span > Analytical Model< / h3 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-3" id = "text-1-5" >
2020-09-30 17:16:30 +02:00
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org20ea2aa" class = "outline-4" >
< h4 id = "org20ea2aa" > < span class = "section-number-4" > 1.5.1< / span > Parameters< / h4 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-4" id = "text-1-5-1" >
2020-09-30 17:16:30 +02:00
< p >
Bode options.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > P = bodeoptions;
2020-10-05 18:06:49 +02:00
P.FreqUnits = < span class = "org-string" > 'Hz'< / span > ;
P.MagUnits = < span class = "org-string" > 'abs'< / span > ;
P.MagScale = < span class = "org-string" > 'log'< / span > ;
P.Grid = < span class = "org-string" > 'on'< / span > ;
P.PhaseWrapping = < span class = "org-string" > 'on'< / span > ;
2020-09-30 17:16:30 +02:00
P.Title.FontSize = 14;
P.XLabel.FontSize = 14;
P.YLabel.FontSize = 14;
P.TickLabel.FontSize = 12;
2020-10-05 18:06:49 +02:00
P.Xlim = [1e< span class = "org-type" > -< / span > 1,1e2];
P.MagLowerLimMode = < span class = "org-string" > 'manual'< / span > ;
P.MagLowerLim= 1e< span class = "org-type" > -< / span > 3;
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< p >
Frequency vector.
< / p >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > w = 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > logspace(< span class = "org-type" > -< / span > 1,2,1000); < span class = "org-comment" > % [rad/s]< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org02cb447" class = "outline-4" >
< h4 id = "org02cb447" > < span class = "section-number-4" > 1.5.2< / span > Generation of the State Space Model< / h4 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-4" id = "text-1-5-2" >
2020-10-05 18:28:44 +02:00
< p >
Mass matrix
< / p >
2020-09-30 17:16:30 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > M = [m 0 0
0 m 0
0 0 I];
2020-10-05 18:28:44 +02:00
< / pre >
< / div >
2020-09-30 17:16:30 +02:00
2020-10-05 18:28:44 +02:00
< p >
Jacobian of the bottom sensor
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Js1 = [1 0 h< span class = "org-type" > /< / span > 2
2020-10-05 18:06:49 +02:00
0 1 < span class = "org-type" > -< / span > l< span class = "org-type" > /< / span > 2];
2020-10-05 18:28:44 +02:00
< / pre >
< / div >
< p >
Jacobian of the top sensor
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Js2 = [1 0 < span class = "org-type" > -< / span > h< span class = "org-type" > /< / span > 2
2020-10-05 18:06:49 +02:00
0 1 0];
2020-10-05 18:28:44 +02:00
< / pre >
< / div >
2020-10-05 18:06:49 +02:00
2020-10-05 18:28:44 +02:00
< p >
Jacobian of the actuators
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Ja = [1 0 ha < span class = "org-comment" > % Left horizontal actuator< / span >
2020-10-05 18:06:49 +02:00
0 1 < span class = "org-type" > -< / span > la < span class = "org-comment" > % Left vertical actuator< / span >
0 1 la]; < span class = "org-comment" > % Right vertical actuator< / span >
Jta = Ja< span class = "org-type" > '< / span > ;
2020-10-05 18:28:44 +02:00
< / pre >
< / div >
< p >
Stiffness and Damping matrices
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > K = k< span class = "org-type" > *< / span > Jta< span class = "org-type" > *< / span > Ja;
2020-10-05 18:06:49 +02:00
C = c< span class = "org-type" > *< / span > Jta< span class = "org-type" > *< / span > Ja;
2020-10-05 18:28:44 +02:00
< / pre >
< / div >
2020-09-30 17:16:30 +02:00
2020-10-05 18:28:44 +02:00
< p >
State Space Matrices
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > E = [1 0 0
2020-09-30 17:16:30 +02:00
0 1 0
2020-10-05 18:06:49 +02:00
0 0 1]; < span class = "org-comment" > %projecting ground motion in the directions of the legs< / span >
2020-09-30 17:16:30 +02:00
AA = [zeros(3) eye(3)
2020-10-05 18:06:49 +02:00
< span class = "org-type" > -< / span > M< span class = "org-type" > \< / span > K < span class = "org-type" > -< / span > M< span class = "org-type" > \< / span > C];
2020-09-30 17:16:30 +02:00
BB = [zeros(3,6)
2020-10-05 18:06:49 +02:00
M< span class = "org-type" > \< / span > Jta M< span class = "org-type" > \< / span > (k< span class = "org-type" > *< / span > Jta< span class = "org-type" > *< / span > E)];
2020-09-30 17:16:30 +02:00
CC = [[Js1;Js2] zeros(4,3);
zeros(2,6)
2020-10-05 18:06:49 +02:00
(Js1< span class = "org-type" > +< / span > Js2)< span class = "org-type" > ./< / span > 2 zeros(2,3)
(Js1< span class = "org-type" > -< / span > Js2)< span class = "org-type" > ./< / span > 2 zeros(2,3)
(Js1< span class = "org-type" > -< / span > Js2)< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > h) zeros(2,3)];
2020-09-30 17:16:30 +02:00
DD = [zeros(4,6)
2020-10-05 18:06:49 +02:00
zeros< span class = "org-type" > (2,3) eye(2,3)< / span >
zeros< span class = "org-type" > (6,6)];< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
2020-10-05 18:28:44 +02:00
< p >
State Space model:
< / p >
2020-09-30 17:16:30 +02:00
< ul class = "org-ul" >
< li > Input = three actuators and three ground motions< / li >
< li > Output = the bottom sensor; the top sensor; the ground motion; the half sum; the half difference; the rotation< / li >
< / ul >
2020-10-05 18:28:44 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > system_dec = ss(AA,BB,CC,DD);
< / pre >
< / div >
2020-09-30 17:16:30 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > size(system_dec)
< / pre >
< / div >
< pre class = "example" >
State-space model with 12 outputs, 6 inputs, and 6 states.
< / pre >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org9417f40" class = "outline-4" >
< h4 id = "org9417f40" > < span class = "section-number-4" > 1.5.3< / span > Comparison with the Simscape Model< / h4 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-4" id = "text-1-5-3" >
2020-09-30 17:16:30 +02:00
2020-10-13 14:51:15 +02:00
< div id = "orga6f165d" class = "figure" >
2020-09-30 17:16:30 +02:00
< p > < img src = "figs/gravimeter_analytical_system_open_loop_models.png" alt = "gravimeter_analytical_system_open_loop_models.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 4: < / span > Comparison of the analytical and the Simscape models< / p >
2020-09-30 17:16:30 +02:00
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org6c56e64" class = "outline-4" >
< h4 id = "org6c56e64" > < span class = "section-number-4" > 1.5.4< / span > Analysis< / h4 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-4" id = "text-1-5-4" >
2020-09-30 17:16:30 +02:00
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % bode(system_dec,P);< / span >
< span class = "org-comment" > % return< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% svd decomposition< / span > < / span >
< span class = "org-comment" > % system_dec_freq = freqresp(system_dec,w);< / span >
< span class = "org-comment" > % S = zeros(3,length(w));< / span >
< span class = "org-comment" > % for < / span > < span class = "org-variable-name" > < span class = "org-comment" > m< / span > < / span > < span class = "org-comment" > = < / span > < span class = "org-constant" > < span class = "org-comment" > 1:length(w)< / span > < / span >
< span class = "org-comment" > % S(:,m) = svd(system_dec_freq(1:4,1:3,m));< / span >
< span class = "org-comment" > % end< / span >
< span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % loglog(w./(2*pi), S);hold on;< / span >
< span class = "org-comment" > % % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:)));< / span >
< span class = "org-comment" > % xlabel('Frequency [Hz]');ylabel('Singular Value [-]');< / span >
< span class = "org-comment" > % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6');< / span >
< span class = "org-comment" > % ylim([1e-8 1e-2]);< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % %condition number< / span >
< span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % loglog(w./(2*pi), S(1,:)./S(3,:));hold on;< / span >
< span class = "org-comment" > % % loglog(w./(2*pi), abs(Val(1,:)),w./(2*pi), abs(Val(2,:)),w./(2*pi), abs(Val(3,:)));< / span >
< span class = "org-comment" > % xlabel('Frequency [Hz]');ylabel('Condition number [-]');< / span >
< span class = "org-comment" > % % legend('\sigma_1','\sigma_2','\sigma_3');%,'\sigma_4','\sigma_5','\sigma_6');< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % %performance indicator< / span >
< span class = "org-comment" > % system_dec_svd = freqresp(system_dec(1:4,1:3),2*pi*10);< / span >
< span class = "org-comment" > % [U,S,V] = svd(system_dec_svd);< / span >
< span class = "org-comment" > % H_svd_OL = -eye(3,4);%-[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*40,-2*pi*200,40/200) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);%< / span >
< span class = "org-comment" > % H_svd = pinv(V')*H_svd_OL*pinv(U);< / span >
< span class = "org-comment" > % % system_dec_control_svd_ = feedback(system_dec,g*pinv(V')*H*pinv(U));< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % OL_dec = g_svd*H_svd*system_dec(1:4,1:3);< / span >
< span class = "org-comment" > % OL_freq = freqresp(OL_dec,w); % OL = G*H< / span >
< span class = "org-comment" > % CL_system = feedback(eye(3),-g_svd*H_svd*system_dec(1:4,1:3));< / span >
< span class = "org-comment" > % CL_freq = freqresp(CL_system,w); % CL = (1+G*H)^-1< / span >
< span class = "org-comment" > % % CL_system_2 = feedback(system_dec,H);< / span >
< span class = "org-comment" > % % CL_freq_2 = freqresp(CL_system_2,w); % CL = G/(1+G*H)< / span >
< span class = "org-comment" > % for < / span > < span class = "org-variable-name" > < span class = "org-comment" > i< / span > < / span > < span class = "org-comment" > = < / span > < span class = "org-constant" > < span class = "org-comment" > 1:size(w,2)< / span > < / span >
< span class = "org-comment" > % < / span > < span class = "org-comment" > < span class = "org-constant" > OL(:,i)< / span > < / span > < span class = "org-comment" > = svd(OL_freq(:,:,i));< / span >
< span class = "org-comment" > % < / span > < span class = "org-comment" > < span class = "org-constant" > CL < / span > < / span > < span class = "org-comment" > (:,i) = svd(CL_freq(:,:,i));< / span >
< span class = "org-comment" > % %CL2 (:,i) = svd(CL_freq_2(:,:,i));< / span >
< span class = "org-comment" > % end< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % un = ones(1,length(w));< / span >
< span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % loglog(w./(2*pi),OL(3,:)+1,'k',w./(2*pi),OL(3,:)-1,'b',w./(2*pi),1./CL(1,:),'r--',w./(2*pi),un,'k:');hold on;%< / span >
< span class = "org-comment" > % % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:)));< / span >
< span class = "org-comment" > % % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:)));< / span >
< span class = "org-comment" > % xlabel('Frequency [Hz]');ylabel('Singular Value [-]');< / span >
< span class = "org-comment" > % legend('GH \sigma_{inf} +1 ','GH \sigma_{inf} -1','S 1/\sigma_{sup}');%,'\lambda_1','\lambda_2','\lambda_3');< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % loglog(w./(2*pi),OL(1,:)+1,'k',w./(2*pi),OL(1,:)-1,'b',w./(2*pi),1./CL(3,:),'r--',w./(2*pi),un,'k:');hold on;%< / span >
< span class = "org-comment" > % % loglog(w./(2*pi), 1./(CL(2,:)),w./(2*pi), 1./(CL(3,:)));< / span >
< span class = "org-comment" > % % semilogx(w./(2*pi), 1./(CL2(1,:)),w./(2*pi), 1./(CL2(2,:)),w./(2*pi), 1./(CL2(3,:)));< / span >
< span class = "org-comment" > % xlabel('Frequency [Hz]');ylabel('Singular Value [-]');< / span >
< span class = "org-comment" > % legend('GH \sigma_{sup} +1 ','GH \sigma_{sup} -1','S 1/\sigma_{inf}');%,'\lambda_1','\lambda_2','\lambda_3');< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-orgeb20c08" class = "outline-4" >
< h4 id = "orgeb20c08" > < span class = "section-number-4" > 1.5.5< / span > Control Section< / h4 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-4" id = "text-1-5-5" >
2020-09-30 17:16:30 +02:00
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > system_dec_10Hz = freqresp(system_dec,2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10);
2020-09-30 17:16:30 +02:00
system_dec_0Hz = freqresp(system_dec,0);
system_decReal_10Hz = pinv(align(system_dec_10Hz));
2020-10-05 18:06:49 +02:00
[Ureal,Sreal,Vreal] = svd(system_decReal_10Hz(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3));
normalizationMatrixReal = abs(pinv(Ureal)< span class = "org-type" > *< / span > system_dec_0Hz(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > *< / span > pinv(Vreal< span class = "org-type" > '< / span > ));
[U,S,V] = svd(system_dec_10Hz(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3));
normalizationMatrix = abs(pinv(U)< span class = "org-type" > *< / span > system_dec_0Hz(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > *< / span > pinv(V< span class = "org-type" > '< / span > ));
H_dec = ([zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 5,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 30,30< span class = "org-type" > /< / span > 5) 0 0 0
0 zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 4,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 20,20< span class = "org-type" > /< / span > 4) 0 0
0 0 0 zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10,10)]);
H_cen_OL = [zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10,10) 0 0; 0 zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10,10) 0;
0 0 zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 5,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 30,30< span class = "org-type" > /< / span > 5)];
H_cen = pinv(Jta)< span class = "org-type" > *< / span > H_cen_OL< span class = "org-type" > *< / span > pinv([Js1; Js2]);
< span class = "org-comment" > % H_svd_OL = -[1/normalizationMatrix(1,1) 0 0 0< / span >
< span class = "org-comment" > % 0 1/normalizationMatrix(2,2) 0 0< / span >
< span class = "org-comment" > % 0 0 1/normalizationMatrix(3,3) 0];< / span >
< span class = "org-comment" > % H_svd_OL_real = -[1/normalizationMatrixReal(1,1) 0 0 0< / span >
< span class = "org-comment" > % 0 1/normalizationMatrixReal(2,2) 0 0< / span >
< span class = "org-comment" > % 0 0 1/normalizationMatrixReal(3,3) 0];< / span >
H_svd_OL = < span class = "org-type" > -< / span > [1< span class = "org-type" > /< / span > normalizationMatrix(1,1)< span class = "org-type" > *< / span > zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 60,60< span class = "org-type" > /< / span > 10) 0 0 0
0 1< span class = "org-type" > /< / span > normalizationMatrix(2,2)< span class = "org-type" > *< / span > zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 5,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 30,30< span class = "org-type" > /< / span > 5) 0 0
0 0 1< span class = "org-type" > /< / span > normalizationMatrix(3,3)< span class = "org-type" > *< / span > zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 2,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10,10< span class = "org-type" > /< / span > 2) 0];
H_svd_OL_real = < span class = "org-type" > -< / span > [1< span class = "org-type" > /< / span > normalizationMatrixReal(1,1)< span class = "org-type" > *< / span > zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 60,60< span class = "org-type" > /< / span > 10) 0 0 0
0 1< span class = "org-type" > /< / span > normalizationMatrixReal(2,2)< span class = "org-type" > *< / span > zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 5,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 30,30< span class = "org-type" > /< / span > 5) 0 0
0 0 1< span class = "org-type" > /< / span > normalizationMatrixReal(3,3)< span class = "org-type" > *< / span > zpk(< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 2,< span class = "org-type" > -< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 10,10< span class = "org-type" > /< / span > 2) 0];
< span class = "org-comment" > % H_svd_OL_real = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 10*zpk(-2*pi*10,-2*pi*100,100/10) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];%-eye(3,4);< / span >
< span class = "org-comment" > % H_svd_OL = -[zpk(-2*pi*10,-2*pi*40,40/10) 0 0 0; 0 zpk(-2*pi*4,-2*pi*20,4/20) 0 0; 0 0 zpk(-2*pi*2,-2*pi*10,10/2) 0];% - eye(3,4);%< / span >
H_svd = pinv(V< span class = "org-type" > '< / span > )< span class = "org-type" > *< / span > H_svd_OL< span class = "org-type" > *< / span > pinv(U);
H_svd_real = pinv(Vreal< span class = "org-type" > '< / span > )< span class = "org-type" > *< / span > H_svd_OL_real< span class = "org-type" > *< / span > pinv(Ureal);
OL_dec = g< span class = "org-type" > *< / span > H_dec< span class = "org-type" > *< / span > system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3);
OL_cen = g< span class = "org-type" > *< / span > H_cen_OL< span class = "org-type" > *< / span > pinv([Js1; Js2])< span class = "org-type" > *< / span > system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > *< / span > pinv(Jta);
OL_svd = 100< span class = "org-type" > *< / span > H_svd_OL< span class = "org-type" > *< / span > pinv(U)< span class = "org-type" > *< / span > system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > *< / span > pinv(V< span class = "org-type" > '< / span > );
OL_svd_real = 100< span class = "org-type" > *< / span > H_svd_OL_real< span class = "org-type" > *< / span > pinv(Ureal)< span class = "org-type" > *< / span > system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > *< / span > pinv(Vreal< span class = "org-type" > '< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % bode(OL_dec,w,P);title('OL Decentralized');< / span >
< span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % bode(OL_cen,w,P);title('OL Centralized');< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
bode(g< span class = "org-type" > *< / span > system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3),w,P);
title(< span class = "org-string" > 'gain * Plant'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
2020-09-30 17:16:30 +02:00
bode(OL_svd,OL_svd_real,w,P);
2020-10-05 18:06:49 +02:00
title(< span class = "org-string" > 'OL SVD'< / span > );
legend(< span class = "org-string" > 'SVD of Complex plant'< / span > ,< span class = "org-string" > 'SVD of real approximation of the complex plant'< / span > )
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
bode(system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3),pinv(U)< span class = "org-type" > *< / span > system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > *< / span > pinv(V< span class = "org-type" > '< / span > ),P);
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > CL_dec = feedback(system_dec,g< span class = "org-type" > *< / span > H_dec,[1 2 3],[1 2 3 4]);
CL_cen = feedback(system_dec,g< span class = "org-type" > *< / span > H_cen,[1 2 3],[1 2 3 4]);
CL_svd = feedback(system_dec,100< span class = "org-type" > *< / span > H_svd,[1 2 3],[1 2 3 4]);
CL_svd_real = feedback(system_dec,100< span class = "org-type" > *< / span > H_svd_real,[1 2 3],[1 2 3 4]);
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > pzmap_testCL(system_dec,H_dec,g,[1 2 3],[1 2 3 4])
2020-10-05 18:06:49 +02:00
title(< span class = "org-string" > 'Decentralized control'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > pzmap_testCL(system_dec,H_cen,g,[1 2 3],[1 2 3 4])
2020-10-05 18:06:49 +02:00
title(< span class = "org-string" > 'Centralized control'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > pzmap_testCL(system_dec,H_svd,100,[1 2 3],[1 2 3 4])
2020-10-05 18:06:49 +02:00
title(< span class = "org-string" > 'SVD control'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > pzmap_testCL(system_dec,H_svd_real,100,[1 2 3],[1 2 3 4])
2020-10-05 18:06:49 +02:00
title(< span class = "org-string" > 'Real approximation SVD control'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > P.Ylim = [1e< span class = "org-type" > -< / span > 8 1e< span class = "org-type" > -< / span > 3];
< span class = "org-type" > figure< / span >
bodemag(system_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3),CL_dec(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3),CL_cen(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3),CL_svd(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3),CL_svd_real(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3),P);
title(< span class = "org-string" > 'Motion/actuator'< / span > )
legend(< span class = "org-string" > 'Control OFF'< / span > ,< span class = "org-string" > 'Decentralized control'< / span > ,< span class = "org-string" > 'Centralized control'< / span > ,< span class = "org-string" > 'SVD control'< / span > ,< span class = "org-string" > 'SVD control real appr.'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > P.Ylim = [1e< span class = "org-type" > -< / span > 5 1e1];
< span class = "org-type" > figure< / span >
bodemag(system_dec(1< span class = "org-type" > :< / span > 4,4< span class = "org-type" > :< / span > 6),CL_dec(1< span class = "org-type" > :< / span > 4,4< span class = "org-type" > :< / span > 6),CL_cen(1< span class = "org-type" > :< / span > 4,4< span class = "org-type" > :< / span > 6),CL_svd(1< span class = "org-type" > :< / span > 4,4< span class = "org-type" > :< / span > 6),CL_svd_real(1< span class = "org-type" > :< / span > 4,4< span class = "org-type" > :< / span > 6),P);
title(< span class = "org-string" > 'Transmissibility'< / span > );
legend(< span class = "org-string" > 'Control OFF'< / span > ,< span class = "org-string" > 'Decentralized control'< / span > ,< span class = "org-string" > 'Centralized control'< / span > ,< span class = "org-string" > 'SVD control'< / span > ,< span class = "org-string" > 'SVD control real appr.'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
bodemag(system_dec([7 9],4< span class = "org-type" > :< / span > 6),CL_dec([7 9],4< span class = "org-type" > :< / span > 6),CL_cen([7 9],4< span class = "org-type" > :< / span > 6),CL_svd([7 9],4< span class = "org-type" > :< / span > 6),CL_svd_real([7 9],4< span class = "org-type" > :< / span > 6),P);
title(< span class = "org-string" > 'Transmissibility from half sum and half difference in the X direction'< / span > );
legend(< span class = "org-string" > 'Control OFF'< / span > ,< span class = "org-string" > 'Decentralized control'< / span > ,< span class = "org-string" > 'Centralized control'< / span > ,< span class = "org-string" > 'SVD control'< / span > ,< span class = "org-string" > 'SVD control real appr.'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
bodemag(system_dec([8 10],4< span class = "org-type" > :< / span > 6),CL_dec([8 10],4< span class = "org-type" > :< / span > 6),CL_cen([8 10],4< span class = "org-type" > :< / span > 6),CL_svd([8 10],4< span class = "org-type" > :< / span > 6),CL_svd_real([8 10],4< span class = "org-type" > :< / span > 6),P);
title(< span class = "org-string" > 'Transmissibility from half sum and half difference in the Z direction'< / span > );
legend(< span class = "org-string" > 'Control OFF'< / span > ,< span class = "org-string" > 'Decentralized control'< / span > ,< span class = "org-string" > 'Centralized control'< / span > ,< span class = "org-string" > 'SVD control'< / span > ,< span class = "org-string" > 'SVD control real appr.'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org931022f" class = "outline-4" >
< h4 id = "org931022f" > < span class = "section-number-4" > 1.5.6< / span > Greshgorin radius< / h4 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-4" id = "text-1-5-6" >
2020-09-30 17:16:30 +02:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > system_dec_freq = freqresp(system_dec,w);
x1 = zeros(1,length(w));
z1 = zeros(1,length(w));
x2 = zeros(1,length(w));
S1 = zeros(1,length(w));
S2 = zeros(1,length(w));
S3 = zeros(1,length(w));
2020-10-05 18:06:49 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > t< / span > = < span class = "org-constant" > 1:length(w)< / span >
x1(t) = (abs(system_dec_freq(1,2,t))< span class = "org-type" > +< / span > abs(system_dec_freq(1,3,t)))< span class = "org-type" > /< / span > abs(system_dec_freq(1,1,t));
z1(t) = (abs(system_dec_freq(2,1,t))< span class = "org-type" > +< / span > abs(system_dec_freq(2,3,t)))< span class = "org-type" > /< / span > abs(system_dec_freq(2,2,t));
x2(t) = (abs(system_dec_freq(3,1,t))< span class = "org-type" > +< / span > abs(system_dec_freq(3,2,t)))< span class = "org-type" > /< / span > abs(system_dec_freq(3,3,t));
system_svd = pinv(Ureal)< span class = "org-type" > *< / span > system_dec_freq(1< span class = "org-type" > :< / span > 4,1< span class = "org-type" > :< / span > 3,t)< span class = "org-type" > *< / span > pinv(Vreal< span class = "org-type" > '< / span > );
S1(t) = (abs(system_svd(1,2))< span class = "org-type" > +< / span > abs(system_svd(1,3)))< span class = "org-type" > /< / span > abs(system_svd(1,1));
S2(t) = (abs(system_svd(2,1))< span class = "org-type" > +< / span > abs(system_svd(2,3)))< span class = "org-type" > /< / span > abs(system_svd(2,2));
S2(t) = (abs(system_svd(3,1))< span class = "org-type" > +< / span > abs(system_svd(3,2)))< span class = "org-type" > /< / span > abs(system_svd(3,3));
< span class = "org-keyword" > end< / span >
limit = 0.5< span class = "org-type" > *< / span > ones(1,length(w));
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
loglog(w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),x1,w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),z1,w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),x2,w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),limit,< span class = "org-string" > '--'< / span > );
legend(< span class = "org-string" > 'x_1'< / span > ,< span class = "org-string" > 'z_1'< / span > ,< span class = "org-string" > 'x_2'< / span > ,< span class = "org-string" > 'Limit'< / span > );
xlabel(< span class = "org-string" > 'Frequency [Hz]'< / span > );
ylabel(< span class = "org-string" > 'Greshgorin radius [-]'< / span > );
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
loglog(w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),S1,w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),S2,w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),S3,w< span class = "org-type" > ./< / span > (2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > ),limit,< span class = "org-string" > '--'< / span > );
legend(< span class = "org-string" > 'S1'< / span > ,< span class = "org-string" > 'S2'< / span > ,< span class = "org-string" > 'S3'< / span > ,< span class = "org-string" > 'Limit'< / span > );
xlabel(< span class = "org-string" > 'Frequency [Hz]'< / span > );
ylabel(< span class = "org-string" > 'Greshgorin radius [-]'< / span > );
< span class = "org-comment" > % set(gcf,'color','w')< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org1d56ec4" class = "outline-4" >
< h4 id = "org1d56ec4" > < span class = "section-number-4" > 1.5.7< / span > Injecting ground motion in the system to have the output< / h4 >
2020-10-05 18:06:49 +02:00
< div class = "outline-text-4" id = "text-1-5-7" >
2020-09-30 17:16:30 +02:00
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > Fr = logspace(< span class = "org-type" > -< / span > 2,3,1e3);
w=2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > Fr< span class = "org-type" > *< / span > 1< span class = "org-constant" > i< / span > ;
< span class = "org-comment" > %fit of the ground motion data in m/s^2/rtHz< / span >
2020-09-30 17:16:30 +02:00
Fr_ground_x = [0.07 0.1 0.15 0.3 0.7 0.8 0.9 1.2 5 10];
2020-10-05 18:06:49 +02:00
n_ground_x1 = [4e< span class = "org-type" > -< / span > 7 4e< span class = "org-type" > -< / span > 7 2e< span class = "org-type" > -< / span > 6 1e< span class = "org-type" > -< / span > 6 5e< span class = "org-type" > -< / span > 7 5e< span class = "org-type" > -< / span > 7 5e< span class = "org-type" > -< / span > 7 1e< span class = "org-type" > -< / span > 6 1e< span class = "org-type" > -< / span > 5 3.5e< span class = "org-type" > -< / span > 5];
2020-09-30 17:16:30 +02:00
Fr_ground_v = [0.07 0.08 0.1 0.11 0.12 0.15 0.25 0.6 0.8 1 1.2 1.6 2 6 10];
2020-10-05 18:06:49 +02:00
n_ground_v1 = [7e< span class = "org-type" > -< / span > 7 7e< span class = "org-type" > -< / span > 7 7e< span class = "org-type" > -< / span > 7 1e< span class = "org-type" > -< / span > 6 1.2e< span class = "org-type" > -< / span > 6 1.5e< span class = "org-type" > -< / span > 6 1e< span class = "org-type" > -< / span > 6 9e< span class = "org-type" > -< / span > 7 7e< span class = "org-type" > -< / span > 7 7e< span class = "org-type" > -< / span > 7 7e< span class = "org-type" > -< / span > 7 1e< span class = "org-type" > -< / span > 6 2e< span class = "org-type" > -< / span > 6 1e< span class = "org-type" > -< / span > 5 3e< span class = "org-type" > -< / span > 5];
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
n_ground_x = interp1(Fr_ground_x,n_ground_x1,Fr,< span class = "org-string" > 'linear'< / span > );
n_ground_v = interp1(Fr_ground_v,n_ground_v1,Fr,< span class = "org-string" > 'linear'< / span > );
< span class = "org-comment" > % figure< / span >
< span class = "org-comment" > % loglog(Fr,abs(n_ground_v),Fr_ground_v,n_ground_v1,'*');< / span >
< span class = "org-comment" > % xlabel('Frequency [Hz]');ylabel('ASD [m/s^2 /rtHz]');< / span >
< span class = "org-comment" > % return< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-comment" > %converting into PSD< / span >
n_ground_x = (n_ground_x)< span class = "org-type" > .^< / span > 2;
n_ground_v = (n_ground_v)< span class = "org-type" > .^< / span > 2;
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-comment" > %Injecting ground motion in the system and getting the outputs< / span >
2020-09-30 17:16:30 +02:00
system_dec_f = (freqresp(system_dec,abs(w)));
PHI = zeros(size(Fr,2),12,12);
2020-10-05 18:06:49 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > p< / span > = < span class = "org-constant" > 1:size(Fr,2)< / span >
2020-09-30 17:16:30 +02:00
Sw=zeros(6,6);
Iact = zeros(3,3);
2020-10-05 18:06:49 +02:00
Sw< span class = "org-type" > (4,4) < / span > = n_ground_x(p);
Sw< span class = "org-type" > (5,5) < / span > = n_ground_v(p);
Sw< span class = "org-type" > (6,6) < / span > = n_ground_v(p);
Sw< span class = "org-type" > (1:3,1:3) < / span > = Iact;
PHI(p,< span class = "org-type" > :< / span > ,< span class = "org-type" > :< / span > ) = (system_dec_f(< span class = "org-type" > :< / span > ,< span class = "org-type" > :< / span > ,p))< span class = "org-type" > *< / span > Sw(< span class = "org-type" > :< / span > ,< span class = "org-type" > :< / span > )< span class = "org-type" > *< / span > (system_dec_f(< span class = "org-type" > :< / span > ,< span class = "org-type" > :< / span > ,p))< span class = "org-type" > '< / span > ;
< span class = "org-keyword" > end< / span >
x1 = PHI(< span class = "org-type" > :< / span > ,1,1);
z1 = PHI(< span class = "org-type" > :< / span > ,2,2);
x2 = PHI(< span class = "org-type" > :< / span > ,3,3);
z2 = PHI(< span class = "org-type" > :< / span > ,4,4);
wx = PHI(< span class = "org-type" > :< / span > ,5,5);
wz = PHI(< span class = "org-type" > :< / span > ,6,6);
x12 = PHI(< span class = "org-type" > :< / span > ,1,3);
z12 = PHI(< span class = "org-type" > :< / span > ,2,4);
PHIwx = PHI(< span class = "org-type" > :< / span > ,1,5);
PHIwz = PHI(< span class = "org-type" > :< / span > ,2,6);
xsum = PHI(< span class = "org-type" > :< / span > ,7,7);
zsum = PHI(< span class = "org-type" > :< / span > ,8,8);
xdelta = PHI(< span class = "org-type" > :< / span > ,9,9);
zdelta = PHI(< span class = "org-type" > :< / span > ,10,10);
rot = PHI(< span class = "org-type" > :< / span > ,11,11);
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
< / div >
2020-09-21 13:08:36 +02:00
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org36d6b85" class = "outline-2" >
< h2 id = "org36d6b85" > < span class = "section-number-2" > 2< / span > Gravimeter - Functions< / h2 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-2" id = "text-2" >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-orgbb4529b" class = "outline-3" >
< h3 id = "orgbb4529b" > < span class = "section-number-3" > 2.1< / span > < code > align< / code > < / h3 >
2020-09-21 13:08:36 +02:00
< div class = "outline-text-3" id = "text-2-1" >
< p >
2020-10-13 14:51:15 +02:00
< a id = "orgf2b803a" > < / a >
2020-09-30 17:16:30 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "gravimeter/align.m" > here< / a > .
< / p >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > [A]< / span > = < span class = "org-function-name" > align< / span > (< span class = "org-variable-name" > V< / span > )
< span class = "org-comment" > %A!ALIGN(V) returns a constat matrix A which is the real alignment of the< / span >
< span class = "org-comment" > %INVERSE of the complex input matrix V< / span >
< span class = "org-comment" > %from Mohit slides< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-keyword" > if< / span > (nargin < span class = "org-type" > ==< / span > 0) < span class = "org-type" > ||< / span > (nargin < span class = "org-type" > > < / span > 1)
disp(< span class = "org-string" > 'usage: mat_inv_real = align(mat)'< / span > )
< span class = "org-keyword" > return< / span >
< span class = "org-keyword" > end< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
D = pinv(real(V< span class = "org-type" > '*< / span > V));
A = D< span class = "org-type" > *< / span > real(V< span class = "org-type" > '*< / span > diag(exp(1< span class = "org-constant" > i< / span > < span class = "org-type" > *< / span > angle(diag(V< span class = "org-type" > *< / span > D< span class = "org-type" > *< / span > V< span class = "org-type" > .'< / span > ))< span class = "org-type" > /< / span > 2)));
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-keyword" > end< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-orge0ed8bf" class = "outline-3" >
< h3 id = "orge0ed8bf" > < span class = "section-number-3" > 2.2< / span > < code > pzmap_testCL< / code > < / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-2-2" >
< p >
2020-10-13 14:51:15 +02:00
< a id = "orgf08bacf" > < / a >
2020-09-30 17:16:30 +02:00
< / p >
< p >
This Matlab function is accessible < a href = "gravimeter/pzmap_testCL.m" > here< / a > .
< / p >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-keyword" > function< / span > < span class = "org-variable-name" > []< / span > = < span class = "org-function-name" > pzmap_testCL< / span > (< span class = "org-variable-name" > system< / span > ,< span class = "org-variable-name" > H< / span > ,< span class = "org-variable-name" > gain< / span > ,< span class = "org-variable-name" > feedin< / span > ,< span class = "org-variable-name" > feedout< / span > )
< span class = "org-comment" > % evaluate and plot the pole-zero map for the closed loop system for< / span >
< span class = "org-comment" > % different values of the gain< / span >
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
[< span class = "org-type" > ~< / span > , n] = size(gain);
[m1, n1, < span class = "org-type" > ~< / span > ] = size(H);
[< span class = "org-type" > ~< / span > ,n2] = size(feedin);
2020-09-30 17:16:30 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-type" > figure< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / span > = < span class = "org-constant" > 1:n< / span >
< span class = "org-comment" > % if n1 == n2< / span >
system_CL = feedback(system,gain(< span class = "org-constant" > i< / span > )< span class = "org-type" > *< / span > H,feedin,feedout);
2020-09-30 17:16:30 +02:00
[P,Z] = pzmap(system_CL);
2020-10-05 18:06:49 +02:00
plot(real(P(< span class = "org-type" > :< / span > )),imag(P(< span class = "org-type" > :< / span > )),< span class = "org-string" > 'x'< / span > ,real(Z(< span class = "org-type" > :< / span > )),imag(Z(< span class = "org-type" > :< / span > )),< span class = "org-string" > 'o'< / span > );hold on
xlabel(< span class = "org-string" > 'Real axis (s^{-1})'< / span > );ylabel(< span class = "org-string" > 'Imaginary Axis (s^{-1})'< / span > );
< span class = "org-comment" > % clear P Z< / span >
< span class = "org-comment" > % else< / span >
< span class = "org-comment" > % system_CL = feedback(system,gain(i)*H(:,1+(i-1)*m1:m1+(i-1)*m1),feedin,feedout);< / span >
< span class = "org-comment" > %< / span >
< span class = "org-comment" > % [P,Z] = pzmap(system_CL);< / span >
< span class = "org-comment" > % plot(real(P(:)),imag(P(:)),'x',real(Z(:)),imag(Z(:)),'o');hold on< / span >
< span class = "org-comment" > % xlabel('Real axis (s^{-1})');ylabel('Imaginary Axis (s^{-1})');< / span >
< span class = "org-comment" > % clear P Z< / span >
< span class = "org-comment" > % end< / span >
< span class = "org-keyword" > end< / span >
str = {strcat(< span class = "org-string" > 'gain = '< / span > , num2str(gain(1)))}; < span class = "org-comment" > % at the end of first loop, z being loop output< / span >
str = [str , strcat(< span class = "org-string" > 'gain = '< / span > , num2str(gain(1)))]; < span class = "org-comment" > % after 2nd loop< / span >
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / span > = < span class = "org-constant" > 2:n< / span >
str = [str , strcat(< span class = "org-string" > 'gain = '< / span > , num2str(gain(< span class = "org-constant" > i< / span > )))]; < span class = "org-comment" > % after 2nd loop< / span >
str = [str , strcat(< span class = "org-string" > 'gain = '< / span > , num2str(gain(< span class = "org-constant" > i< / span > )))]; < span class = "org-comment" > % after 2nd loop< / span >
< span class = "org-keyword" > end< / span >
legend(str{< span class = "org-type" > :< / span > })
< span class = "org-keyword" > end< / span >
2020-09-30 17:16:30 +02:00
< / pre >
< / div >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org5afd29d" class = "outline-2" >
< h2 id = "org5afd29d" > < span class = "section-number-2" > 3< / span > Stewart Platform - Simscape Model< / h2 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-2" id = "text-3" >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-orgff944f3" class = "outline-3" >
< h3 id = "orgff944f3" > < span class = "section-number-3" > 3.1< / span > Jacobian< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-1" >
< p >
2020-09-21 13:08:36 +02:00
First, the position of the “ joints” (points of force application) are estimated and the Jacobian computed.
< / p >
< div class = "org-src-container" >
2020-10-13 14:51:15 +02:00
< pre class = "src src-matlab" > open(< span class = "org-string" > 'drone_platform_jacobian.slx'< / span > );
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-matlab-simulink-keyword" > sim< / span > (< span class = "org-string" > 'drone_platform_jacobian'< / span > );
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > Aa = [a1.Data(1,< span class = "org-type" > :< / span > );
a2.Data(1,< span class = "org-type" > :< / span > );
a3.Data(1,< span class = "org-type" > :< / span > );
a4.Data(1,< span class = "org-type" > :< / span > );
a5.Data(1,< span class = "org-type" > :< / span > );
a6.Data(1,< span class = "org-type" > :< / span > )]< span class = "org-type" > '< / span > ;
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
Ab = [b1.Data(1,< span class = "org-type" > :< / span > );
b2.Data(1,< span class = "org-type" > :< / span > );
b3.Data(1,< span class = "org-type" > :< / span > );
b4.Data(1,< span class = "org-type" > :< / span > );
b5.Data(1,< span class = "org-type" > :< / span > );
b6.Data(1,< span class = "org-type" > :< / span > )]< span class = "org-type" > '< / span > ;
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
As = (Ab < span class = "org-type" > -< / span > Aa)< span class = "org-type" > ./< / span > vecnorm(Ab < span class = "org-type" > -< / span > Aa);
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
l = vecnorm(Ab < span class = "org-type" > -< / span > Aa)< span class = "org-type" > '< / span > ;
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
J = [As< span class = "org-type" > '< / span > , cross(Ab, As)< span class = "org-type" > '< / span > ];
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
save(< span class = "org-string" > './jacobian.mat'< / span > , < span class = "org-string" > 'Aa'< / span > , < span class = "org-string" > 'Ab'< / span > , < span class = "org-string" > 'As'< / span > , < span class = "org-string" > 'l'< / span > , < span class = "org-string" > 'J'< / span > );
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org07ffe6c" class = "outline-3" >
< h3 id = "org07ffe6c" > < span class = "section-number-3" > 3.2< / span > Simscape Model< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-2" >
2020-09-21 13:08:36 +02:00
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > open(< span class = "org-string" > 'stewart_platform/drone_platform.slx'< / span > );
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
< p >
Definition of spring parameters
< / p >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > kx = 50; < span class = "org-comment" > % [N/m]< / span >
2020-09-21 13:08:36 +02:00
ky = 50;
kz = 50;
2020-10-05 18:06:49 +02:00
cx = 0.025; < span class = "org-comment" > % [Nm/rad]< / span >
2020-09-21 13:08:36 +02:00
cy = 0.025;
cz = 0.025;
< / pre >
< / div >
< p >
We load the Jacobian.
< / p >
< div class = "org-src-container" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > load(< span class = "org-string" > './jacobian.mat'< / span > , < span class = "org-string" > 'Aa'< / span > , < span class = "org-string" > 'Ab'< / span > , < span class = "org-string" > 'As'< / span > , < span class = "org-string" > 'l'< / span > , < span class = "org-string" > 'J'< / span > );
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-09-21 13:08:36 +02:00
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org9aaf0d3" class = "outline-3" >
< h3 id = "org9aaf0d3" > < span class = "section-number-3" > 3.3< / span > Identification of the plant< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Name of the Simulink File< / span > < / span >
mdl = < span class = "org-string" > 'drone_platform'< / span > ;
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Input/Output definition< / span > < / span >
2020-09-21 13:08:36 +02:00
clear io; io_i = 1;
2020-10-05 18:06:49 +02:00
io(io_i) = linio([mdl, < span class = "org-string" > '/Dw'< / span > ], 1, < span class = "org-string" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/u'< / span > ], 1, < span class = "org-string" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
io(io_i) = linio([mdl, < span class = "org-string" > '/Inertial Sensor'< / span > ], 1, < span class = "org-string" > 'openoutput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1;
2020-09-21 13:08:36 +02:00
G = linearize(mdl, io);
2020-10-05 18:06:49 +02:00
G.InputName = {< span class = "org-string" > 'Dwx'< / span > , < span class = "org-string" > 'Dwy'< / span > , < span class = "org-string" > 'Dwz'< / span > , < span class = "org-string" > 'Rwx'< / span > , < span class = "org-string" > 'Rwy'< / span > , < span class = "org-string" > 'Rwz'< / span > , ...
< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > };
G.OutputName = {< span class = "org-string" > 'Ax'< / span > , < span class = "org-string" > 'Ay'< / span > , < span class = "org-string" > 'Az'< / span > , < span class = "org-string" > 'Arx'< / span > , < span class = "org-string" > 'Ary'< / span > , < span class = "org-string" > 'Arz'< / span > };
2020-09-21 13:08:36 +02:00
< / 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > < span class = "org-comment" > % G = G*blkdiag(inv(J), eye(6));< / span >
< span class = "org-comment" > % G.InputName = {'Dw1', 'Dw2', 'Dw3', 'Dw4', 'Dw5', 'Dw6', ...< / span >
< span class = "org-comment" > % 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};< / span >
2020-09-21 18:03:40 +02:00
< / 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-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > Gx = G< span class = "org-type" > *< / span > blkdiag(eye(6), inv(J< span class = "org-type" > '< / span > ));
Gx.InputName = {< span class = "org-string" > 'Dwx'< / span > , < span class = "org-string" > 'Dwy'< / span > , < span class = "org-string" > 'Dwz'< / span > , < span class = "org-string" > 'Rwx'< / span > , < span class = "org-string" > 'Rwy'< / span > , < span class = "org-string" > 'Rwz'< / span > , ...
< span class = "org-string" > 'Fx'< / span > , < span class = "org-string" > 'Fy'< / span > , < span class = "org-string" > 'Fz'< / span > , < span class = "org-string" > 'Mx'< / span > , < span class = "org-string" > 'My'< / span > , < span class = "org-string" > 'Mz'< / span > };
2020-09-21 13:08:36 +02:00
2020-10-05 18:06:49 +02:00
Gl = J< span class = "org-type" > *< / span > G;
Gl.OutputName = {< span class = "org-string" > 'A1'< / span > , < span class = "org-string" > 'A2'< / span > , < span class = "org-string" > 'A3'< / span > , < span class = "org-string" > 'A4'< / span > , < span class = "org-string" > 'A5'< / span > , < span class = "org-string" > 'A6'< / span > };
2020-09-21 13:08:36 +02:00
< / pre >
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-09-21 13:08:36 +02:00
2020-10-13 14:51:15 +02:00
< div id = "outline-container-orgb0b01e3" class = "outline-3" >
< h3 id = "orgb0b01e3" > < span class = "section-number-3" > 3.4< / span > Obtained Dynamics< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-4" >
2020-09-21 13:08:36 +02:00
2020-10-13 14:51:15 +02:00
< div id = "org15e1aeb" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/stewart_platform_translations.png" alt = "stewart_platform_translations.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 5: < / span > Stewart Platform Plant from forces applied by the legs to the acceleration of the platform< / p >
2020-09-21 13:08:36 +02:00
< / div >
2020-10-13 14:51:15 +02:00
< div id = "org1a9b1c6" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/stewart_platform_rotations.png" alt = "stewart_platform_rotations.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 6: < / span > Stewart Platform Plant from torques applied by the legs to the angular acceleration of the platform< / p >
2020-09-21 13:08:36 +02:00
< / div >
2020-10-13 14:51:15 +02:00
< div id = "org2c0cea0" class = "figure" >
2020-09-21 13:08:36 +02:00
< p > < img src = "figs/stewart_platform_legs.png" alt = "stewart_platform_legs.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 7: < / span > Stewart Platform Plant from forces applied by the legs to displacement of the legs< / p >
2020-09-21 13:08:36 +02:00
< / div >
2020-09-21 18:03:40 +02:00
2020-10-13 14:51:15 +02:00
< div id = "org46a471a" class = "figure" >
2020-09-21 18:03:40 +02:00
< p > < img src = "figs/stewart_platform_transmissibility.png" alt = "stewart_platform_transmissibility.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 8: < / span > Transmissibility< / p >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org1de55ce" class = "outline-3" >
< h3 id = "org1de55ce" > < span class = "section-number-3" > 3.5< / span > Real Approximation of \(G\) at the decoupling frequency< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-5" >
2020-09-21 18:03:40 +02:00
< 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > wc = 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 20; < span class = "org-comment" > % Decoupling frequency [rad/s]< / span >
2020-09-21 18:03:40 +02:00
2020-10-05 18:06:49 +02:00
Gc = G({< span class = "org-string" > 'Ax'< / span > , < span class = "org-string" > 'Ay'< / span > , < span class = "org-string" > 'Az'< / span > , < span class = "org-string" > 'Arx'< / span > , < span class = "org-string" > 'Ary'< / span > , < span class = "org-string" > 'Arz'< / span > }, ...
{< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > }); < span class = "org-comment" > % Transfer function to find a real approximation< / span >
2020-09-21 18:54:41 +02:00
2020-10-05 18:06:49 +02:00
H1 = evalfr(Gc, < span class = "org-constant" > j< / span > < span class = "org-type" > *< / span > 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > D = pinv(real(H1< span class = "org-type" > '*< / span > H1));
H1 = inv(D< span class = "org-type" > *< / span > real(H1< span class = "org-type" > '*< / span > diag(exp(< span class = "org-constant" > j< / span > < span class = "org-type" > *< / span > angle(diag(H1< span class = "org-type" > *< / span > D< span class = "org-type" > *< / span > H1< span class = "org-type" > .'< / span > ))< span class = "org-type" > /< / span > 2))));
2020-09-21 18:03:40 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org53d60e1" class = "outline-3" >
< h3 id = "org53d60e1" > < span class = "section-number-3" > 3.6< / span > Verification of the decoupling using the “ Gershgorin Radii” < / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-6" >
2020-09-21 18:03:40 +02:00
< 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-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > freqs = logspace(< span class = "org-type" > -< / span > 2, 2, 1000); < span class = "org-comment" > % [Hz]< / span >
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));
2020-10-05 18:06:49 +02:00
H = abs(squeeze(freqresp(Gc, freqs, < span class = "org-string" > 'Hz'< / span > )));
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > out_i< / span > = < span class = "org-constant" > 1:size(Gc,2)< / span >
Gr_coupled(< span class = "org-type" > :< / span > , out_i) = squeeze((sum(H(out_i,< span class = "org-type" > :< / span > ,< span class = "org-type" > :< / span > )) < span class = "org-type" > -< / span > H(out_i,out_i,< span class = "org-type" > :< / span > ))< span class = "org-type" > ./< / span > H(out_i, out_i, < span class = "org-type" > :< / span > ));
< span class = "org-keyword" > end< / span >
2020-09-21 18:03:40 +02:00
< / 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > Gd = U< span class = "org-type" > '*< / span > Gc< span class = "org-type" > *< / span > 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-10-05 18:06:49 +02:00
H = abs(squeeze(freqresp(Gd, freqs, < span class = "org-string" > 'Hz'< / span > )));
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > out_i< / span > = < span class = "org-constant" > 1:size(Gd,2)< / span >
Gr_decoupled(< span class = "org-type" > :< / span > , out_i) = squeeze((sum(H(out_i,< span class = "org-type" > :< / span > ,< span class = "org-type" > :< / span > )) < span class = "org-type" > -< / span > H(out_i,out_i,< span class = "org-type" > :< / span > ))< span class = "org-type" > ./< / span > H(out_i, out_i, < span class = "org-type" > :< / span > ));
< span class = "org-keyword" > end< / span >
2020-09-21 18:03:40 +02:00
< / 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > Gj = Gc< span class = "org-type" > *< / span > inv(J< span class = "org-type" > '< / span > );
2020-09-21 18:54:41 +02:00
Gr_jacobian = zeros(length(freqs), size(Gj,2));
2020-10-05 18:06:49 +02:00
H = abs(squeeze(freqresp(Gj, freqs, < span class = "org-string" > 'Hz'< / span > )));
2020-09-21 18:54:41 +02:00
2020-10-05 18:06:49 +02:00
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > out_i< / span > = < span class = "org-constant" > 1:size(Gj,2)< / span >
Gr_jacobian(< span class = "org-type" > :< / span > , out_i) = squeeze((sum(H(out_i,< span class = "org-type" > :< / span > ,< span class = "org-type" > :< / span > )) < span class = "org-type" > -< / span > H(out_i,out_i,< span class = "org-type" > :< / span > ))< span class = "org-type" > ./< / span > H(out_i, out_i, < span class = "org-type" > :< / span > ));
< span class = "org-keyword" > end< / span >
2020-09-21 18:54:41 +02:00
< / pre >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "org6065705" class = "figure" >
2020-09-21 18:54:41 +02:00
< p > < img src = "figs/simscape_model_gershgorin_radii.png" alt = "simscape_model_gershgorin_radii.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 9: < / span > Gershgorin Radii of the Coupled and Decoupled plants< / p >
2020-09-21 18:54:41 +02:00
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org40c1d24" class = "outline-3" >
< h3 id = "org40c1d24" > < span class = "section-number-3" > 3.7< / span > Decoupled Plant< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-7" >
2020-09-21 18:03:40 +02:00
< 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
2020-10-13 14:51:15 +02:00
< div id = "orgbfa07c9" class = "figure" >
2020-09-21 18:54:41 +02:00
< p > < img src = "figs/simscape_model_decoupled_plant_svd.png" alt = "simscape_model_decoupled_plant_svd.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 10: < / span > Decoupled Plant using SVD< / p >
2020-09-21 18:54:41 +02:00
< / div >
2020-10-13 14:51:15 +02:00
< div id = "org28978a4" class = "figure" >
2020-09-21 18:54:41 +02:00
< p > < img src = "figs/simscape_model_decoupled_plant_jacobian.png" alt = "simscape_model_decoupled_plant_jacobian.png" / >
< / p >
2020-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 11: < / span > Decoupled Plant using the Jacobian< / p >
2020-09-21 18:54:41 +02:00
< / div >
2020-09-21 18:03:40 +02:00
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-orgdfcd158" class = "outline-3" >
< h3 id = "orgdfcd158" > < span class = "section-number-3" > 3.8< / span > Diagonal Controller< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-8" >
2020-09-21 18:03:40 +02:00
< 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > wc = 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 0.1; < span class = "org-comment" > % Crossover Frequency [rad/s]< / span >
C_g = 50; < span class = "org-comment" > % DC Gain< / span >
2020-09-21 18:03:40 +02:00
2020-10-05 18:06:49 +02:00
K = eye(6)< span class = "org-type" > *< / span > C_g< span class = "org-type" > /< / span > (s< span class = "org-type" > +< / span > wc);
2020-09-21 18:03:40 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org25e3b35" class = "outline-3" >
< h3 id = "org25e3b35" > < span class = "section-number-3" > 3.9< / span > Centralized Control< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-9" >
2020-09-21 18:03:40 +02:00
< 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > G_cen = feedback(G, inv(J< span class = "org-type" > '< / span > )< span class = "org-type" > *< / span > K, [7< span class = "org-type" > :< / span > 12], [1< span class = "org-type" > :< / span > 6]);
2020-09-21 18:03:40 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org4d83793" class = "outline-3" >
< h3 id = "org4d83793" > < span class = "section-number-3" > 3.10< / span > SVD Control< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-10" >
2020-09-21 18:03:40 +02:00
< 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" >
2020-10-05 18:06:49 +02:00
< pre class = "src src-matlab" > G_svd = feedback(G, pinv(V< span class = "org-type" > '< / span > )< span class = "org-type" > *< / span > K< span class = "org-type" > *< / span > pinv(U), [7< span class = "org-type" > :< / span > 12], [1< span class = "org-type" > :< / span > 6]);
2020-09-21 18:03:40 +02:00
< / pre >
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org7cece79" class = "outline-3" >
< h3 id = "org7cece79" > < span class = "section-number-3" > 3.11< / span > Results< / h3 >
2020-09-30 17:16:30 +02:00
< div class = "outline-text-3" id = "text-3-11" >
2020-09-21 18:03:40 +02:00
< 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 >
2020-10-13 14:51:15 +02:00
The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure < a href = "#org62fae46" > 14< / a > .
2020-09-21 18:03:40 +02:00
< / p >
2020-10-13 14:51:15 +02:00
< div id = "org62fae46" 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-10-05 18:06:49 +02:00
< p > < span class = "figure-number" > Figure 14: < / span > Obtained Transmissibility< / p >
2020-09-21 18:03:40 +02:00
< / div >
2020-09-21 13:08:36 +02:00
< / div >
< / div >
< / div >
2020-10-13 14:51:15 +02:00
< div id = "outline-container-org8b11aba" class = "outline-2" >
< h2 id = "org8b11aba" > < span class = "section-number-2" > 4< / span > Stewart Platform - Analytical Model< / h2 >
< div class = "outline-text-2" id = "text-4" >
< / div >
< div id = "outline-container-org2a175f6" class = "outline-3" >
< h3 id = "org2a175f6" > < span class = "section-number-3" > 4.1< / span > Characteristics< / h3 >
< div class = "outline-text-3" id = "text-4-1" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > L = 0.055;
Zc = 0;
m = 0.2;
k = 1e3;
c = 2< span class = "org-type" > *< / span > 0.1< span class = "org-type" > *< / span > sqrt(k< span class = "org-type" > *< / span > m);
Rx = 0.04;
Rz = 0.04;
Ix = m< span class = "org-type" > *< / span > Rx< span class = "org-type" > ^< / span > 2;
Iy = m< span class = "org-type" > *< / span > Rx< span class = "org-type" > ^< / span > 2;
Iz = m< span class = "org-type" > *< / span > Rz< span class = "org-type" > ^< / span > 2;
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org9efa4f4" class = "outline-3" >
< h3 id = "org9efa4f4" > < span class = "section-number-3" > 4.2< / span > Mass Matrix< / h3 >
< div class = "outline-text-3" id = "text-4-2" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > M = m< span class = "org-type" > *< / span > [1 0 0 0 Zc 0;
0 1 0 < span class = "org-type" > -< / span > Zc 0 0;
0 0 1 0 0 0;
0 < span class = "org-type" > -< / span > Zc 0 Rx< span class = "org-type" > ^< / span > 2< span class = "org-type" > +< / span > Zc< span class = "org-type" > ^< / span > 2 0 0;
Zc 0 0 0 Rx< span class = "org-type" > ^< / span > 2< span class = "org-type" > +< / span > Zc< span class = "org-type" > ^< / span > 2 0;
0 0 0 0 0 Rz< span class = "org-type" > ^< / span > 2];
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org97bc497" class = "outline-3" >
< h3 id = "org97bc497" > < span class = "section-number-3" > 4.3< / span > Jacobian Matrix< / h3 >
< div class = "outline-text-3" id = "text-4-3" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Bj=1< span class = "org-type" > /< / span > sqrt(6)< span class = "org-type" > *< / span > [ 1 1 < span class = "org-type" > -< / span > 2 1 1 < span class = "org-type" > -< / span > 2;
sqrt< span class = "org-type" > (3) -sqrt(3) 0 sqrt(3) -sqrt(3) 0;< / span >
sqrt< span class = "org-type" > (2) sqrt(2) sqrt(2) sqrt(2) sqrt(2) sqrt(2);< / span >
0 0 L L < span class = "org-type" > -< / span > L < span class = "org-type" > -< / span > L;
< span class = "org-type" > -< / span > L< span class = "org-type" > *< / span > 2< span class = "org-type" > /< / span > sqrt(3) < span class = "org-type" > -< / span > L< span class = "org-type" > *< / span > 2< span class = "org-type" > /< / span > sqrt(3) L< span class = "org-type" > /< / span > sqrt(3) L< span class = "org-type" > /< / span > sqrt(3) L< span class = "org-type" > /< / span > sqrt(3) L< span class = "org-type" > /< / span > sqrt(3);
L< span class = "org-type" > *< / span > sqrt(2) < span class = "org-type" > -< / span > L< span class = "org-type" > *< / span > sqrt(2) L< span class = "org-type" > *< / span > sqrt(2) < span class = "org-type" > -< / span > L< span class = "org-type" > *< / span > sqrt(2) L< span class = "org-type" > *< / span > sqrt(2) < span class = "org-type" > -< / span > L< span class = "org-type" > *< / span > sqrt(2)];
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org7c9679d" class = "outline-3" >
< h3 id = "org7c9679d" > < span class = "section-number-3" > 4.4< / span > Stifnness matrix and Damping matrix< / h3 >
< div class = "outline-text-3" id = "text-4-4" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > kv = k< span class = "org-type" > /< / span > 3; < span class = "org-comment" > % [N/m]< / span >
kh = 0.5< span class = "org-type" > *< / span > k< span class = "org-type" > /< / span > 3; < span class = "org-comment" > % [N/m]< / span >
K = diag([3< span class = "org-type" > *< / span > kh,3< span class = "org-type" > *< / span > kh,3< span class = "org-type" > *< / span > kv,3< span class = "org-type" > *< / span > kv< span class = "org-type" > *< / span > Rx< span class = "org-type" > ^< / span > 2< span class = "org-type" > /< / span > 2,3< span class = "org-type" > *< / span > kv< span class = "org-type" > *< / span > Rx< span class = "org-type" > ^< / span > 2< span class = "org-type" > /< / span > 2,3< span class = "org-type" > *< / span > kh< span class = "org-type" > *< / span > Rx< span class = "org-type" > ^< / span > 2]); < span class = "org-comment" > % Stiffness Matrix< / span >
C = c< span class = "org-type" > *< / span > K< span class = "org-type" > /< / span > 100000; < span class = "org-comment" > % Damping Matrix< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org00e8691" class = "outline-3" >
< h3 id = "org00e8691" > < span class = "section-number-3" > 4.5< / span > State Space System< / h3 >
< div class = "outline-text-3" id = "text-4-5" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > A = [zeros(6) eye(6); < span class = "org-type" > -< / span > M< span class = "org-type" > \< / span > K < span class = "org-type" > -< / span > M< span class = "org-type" > \< / span > C];
Bw = [zeros(6); < span class = "org-type" > -< / span > eye(6)];
Bu = [zeros(6); M< span class = "org-type" > \< / span > Bj];
Co = [< span class = "org-type" > -< / span > M< span class = "org-type" > \< / span > K < span class = "org-type" > -< / span > M< span class = "org-type" > \< / span > C];
D = [zeros(6) M< span class = "org-type" > \< / span > 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 = {< span class = "org-string" > 'x'< / span > ;< span class = "org-string" > 'y'< / span > ;< span class = "org-string" > 'z'< / span > ;< span class = "org-string" > 'theta_x'< / span > ;< span class = "org-string" > 'theta_y'< / span > ;< span class = "org-string" > 'theta_z'< / span > ;...
< span class = "org-string" > 'dx'< / span > ;< span class = "org-string" > 'dy'< / span > ;< span class = "org-string" > 'dz'< / span > ;< span class = "org-string" > 'dtheta_x'< / span > ;< span class = "org-string" > 'dtheta_y'< / span > ;< span class = "org-string" > 'dtheta_z'< / span > };
ST.InputName = {< span class = "org-string" > 'w1'< / span > ;< span class = "org-string" > 'w2'< / span > ;< span class = "org-string" > 'w3'< / span > ;< span class = "org-string" > 'w4'< / span > ;< span class = "org-string" > 'w5'< / span > ;< span class = "org-string" > 'w6'< / span > ;...
< span class = "org-string" > 'u1'< / span > ;< span class = "org-string" > 'u2'< / span > ;< span class = "org-string" > 'u3'< / span > ;< span class = "org-string" > 'u4'< / span > ;< span class = "org-string" > 'u5'< / span > ;< span class = "org-string" > 'u6'< / span > };
ST.OutputName = {< span class = "org-string" > 'ax'< / span > ;< span class = "org-string" > 'ay'< / span > ;< span class = "org-string" > 'az'< / span > ;< span class = "org-string" > 'atheta_x'< / span > ;< span class = "org-string" > 'atheta_y'< / span > ;< span class = "org-string" > 'atheta_z'< / span > };
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org8a70996" class = "outline-3" >
< h3 id = "org8a70996" > < span class = "section-number-3" > 4.6< / span > Transmissibility< / h3 >
< div class = "outline-text-3" id = "text-4-6" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > TR=ST< span class = "org-type" > *< / span > [eye(6); zeros(6)];
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
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 >
< div id = "org6ce913c" class = "figure" >
< p > < img src = "figs/stewart_platform_analytical_transmissibility.png" alt = "stewart_platform_analytical_transmissibility.png" / >
< / p >
< p > < span class = "figure-number" > Figure 15: < / span > Transmissibility< / p >
< / div >
< / div >
< / div >
< div id = "outline-container-org12c95c9" class = "outline-3" >
< h3 id = "org12c95c9" > < span class = "section-number-3" > 4.7< / span > Real approximation of \(G(j\omega)\) at decoupling frequency< / h3 >
< div class = "outline-text-3" id = "text-4-7" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > sys1 = ST< span class = "org-type" > *< / span > [zeros(6); eye(6)]; < span class = "org-comment" > % take only the forces inputs< / span >
dec_fr = 20;
H1 = evalfr(sys1,< span class = "org-constant" > j< / span > < span class = "org-type" > *< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > dec_fr);
H2 = H1;
D = pinv(real(H2< span class = "org-type" > '*< / span > H2));
H1 = inv(D< span class = "org-type" > *< / span > real(H2< span class = "org-type" > '*< / span > diag(exp(< span class = "org-constant" > j< / span > < span class = "org-type" > *< / span > angle(diag(H2< span class = "org-type" > *< / span > D< span class = "org-type" > *< / span > H2< span class = "org-type" > .'< / span > ))< span class = "org-type" > /< / span > 2)))) ;
[U,S,V] = svd(H1);
wf = logspace(< span class = "org-type" > -< / span > 1,2,1000);
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > i< / span > < / span > = < span class = "org-constant" > 1:length(wf)< / span >
H = abs(evalfr(sys1,< span class = "org-constant" > j< / span > < span class = "org-type" > *< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > wf(< span class = "org-constant" > i< / span > )));
H_dec = abs(evalfr(U< span class = "org-type" > '*< / span > sys1< span class = "org-type" > *< / span > V,< span class = "org-constant" > j< / span > < span class = "org-type" > *< / span > 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > wf(< span class = "org-constant" > i< / span > )));
< span class = "org-keyword" > for< / span > < span class = "org-variable-name" > < span class = "org-constant" > j< / span > < / span > = < span class = "org-constant" > 1:size(H,2)< / span >
g_r1(< span class = "org-constant" > i< / span > ,< span class = "org-constant" > j< / span > ) = (sum(H(< span class = "org-constant" > j< / span > ,< span class = "org-type" > :< / span > ))< span class = "org-type" > -< / span > H(< span class = "org-constant" > j< / span > ,< span class = "org-constant" > j< / span > ))< span class = "org-type" > /< / span > H(< span class = "org-constant" > j< / span > ,< span class = "org-constant" > j< / span > );
g_r2(< span class = "org-constant" > i< / span > ,< span class = "org-constant" > j< / span > ) = (sum(H_dec(< span class = "org-constant" > j< / span > ,< span class = "org-type" > :< / span > ))< span class = "org-type" > -< / span > H_dec(< span class = "org-constant" > j< / span > ,< span class = "org-constant" > j< / span > ))< span class = "org-type" > /< / span > H_dec(< span class = "org-constant" > j< / span > ,< span class = "org-constant" > j< / span > );
< span class = "org-comment" > % keyboard< / span >
< span class = "org-keyword" > end< / span >
g_lim(< span class = "org-constant" > i< / span > ) = 0.5;
< span class = "org-keyword" > end< / span >
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-orgc58b81c" class = "outline-3" >
< h3 id = "orgc58b81c" > < span class = "section-number-3" > 4.8< / span > Coupled and Decoupled Plant “ Gershgorin Radii” < / h3 >
< div class = "outline-text-3" id = "text-4-8" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span > ;
title(< span class = "org-string" > 'Coupled plant'< / span > )
loglog(wf,g_r1(< span class = "org-type" > :< / span > ,1),wf,g_r1(< span class = "org-type" > :< / span > ,2),wf,g_r1(< span class = "org-type" > :< / span > ,3),wf,g_r1(< span class = "org-type" > :< / span > ,4),wf,g_r1(< span class = "org-type" > :< / span > ,5),wf,g_r1(< span class = "org-type" > :< / span > ,6),wf,g_lim,< span class = "org-string" > '--'< / span > );
legend(< span class = "org-string" > '$a_x$'< / span > ,< span class = "org-string" > '$a_y$'< / span > ,< span class = "org-string" > '$a_z$'< / span > ,< span class = "org-string" > '$\theta_x$'< / span > ,< span class = "org-string" > '$\theta_y$'< / span > ,< span class = "org-string" > '$\theta_z$'< / span > ,< span class = "org-string" > 'Limit'< / span > );
xlabel(< span class = "org-string" > 'Frequency (Hz)'< / span > ); ylabel(< span class = "org-string" > 'Gershgorin Radii'< / span > )
< / pre >
< / div >
< div id = "org20fc2fd" class = "figure" >
< p > < img src = "figs/gershorin_raddii_coupled_analytical.png" alt = "gershorin_raddii_coupled_analytical.png" / >
< / p >
< p > < span class = "figure-number" > Figure 16: < / span > Gershorin Raddi for the coupled plant< / p >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span > ;
title(< span class = "org-string" > 'Decoupled plant (10 Hz)'< / span > )
loglog(wf,g_r2(< span class = "org-type" > :< / span > ,1),wf,g_r2(< span class = "org-type" > :< / span > ,2),wf,g_r2(< span class = "org-type" > :< / span > ,3),wf,g_r2(< span class = "org-type" > :< / span > ,4),wf,g_r2(< span class = "org-type" > :< / span > ,5),wf,g_r2(< span class = "org-type" > :< / span > ,6),wf,g_lim,< span class = "org-string" > '--'< / span > );
legend(< span class = "org-string" > '$S_1$'< / span > ,< span class = "org-string" > '$S_2$'< / span > ,< span class = "org-string" > '$S_3$'< / span > ,< span class = "org-string" > '$S_4$'< / span > ,< span class = "org-string" > '$S_5$'< / span > ,< span class = "org-string" > '$S_6$'< / span > ,< span class = "org-string" > 'Limit'< / span > );
xlabel(< span class = "org-string" > 'Frequency (Hz)'< / span > ); ylabel(< span class = "org-string" > 'Gershgorin Radii'< / span > )
< / pre >
< / div >
< div id = "org586d327" class = "figure" >
< p > < img src = "figs/gershorin_raddii_decoupled_analytical.png" alt = "gershorin_raddii_decoupled_analytical.png" / >
< / p >
< p > < span class = "figure-number" > Figure 17: < / span > Gershorin Raddi for the decoupled plant< / p >
< / div >
< / div >
< / div >
< div id = "outline-container-org2ba91f6" class = "outline-3" >
< h3 id = "org2ba91f6" > < span class = "section-number-3" > 4.9< / span > Decoupled Plant< / h3 >
< div class = "outline-text-3" id = "text-4-9" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span > ;
bodemag(U< span class = "org-type" > '*< / span > sys1< span class = "org-type" > *< / span > V,opts)
< / pre >
< / div >
< div id = "org5cd203f" class = "figure" >
< p > < img src = "figs/stewart_platform_analytical_decoupled_plant.png" alt = "stewart_platform_analytical_decoupled_plant.png" / >
< / p >
< p > < span class = "figure-number" > Figure 18: < / span > Decoupled Plant< / p >
< / div >
< / div >
< / div >
< div id = "outline-container-orgc73a283" class = "outline-3" >
< h3 id = "orgc73a283" > < span class = "section-number-3" > 4.10< / span > Controller< / h3 >
< div class = "outline-text-3" id = "text-4-10" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > fc = 2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > 0.1; < span class = "org-comment" > % Crossover Frequency [rad/s]< / span >
c_gain = 50; < span class = "org-comment" > %< / span >
cont = eye(6)< span class = "org-type" > *< / span > c_gain< span class = "org-type" > /< / span > (s< span class = "org-type" > +< / span > fc);
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org9c82ee4" class = "outline-3" >
< h3 id = "org9c82ee4" > < span class = "section-number-3" > 4.11< / span > Closed Loop System< / h3 >
< div class = "outline-text-3" id = "text-4-11" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > FEEDIN = [7< span class = "org-type" > :< / span > 12]; < span class = "org-comment" > % Input of controller< / span >
FEEDOUT = [1< span class = "org-type" > :< / span > 6]; < span class = "org-comment" > % Output of controller< / span >
< / pre >
< / div >
< p >
Centralized Control
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > STcen = feedback(ST, inv(Bj)< span class = "org-type" > *< / span > cont, FEEDIN, FEEDOUT);
TRcen = STcen< span class = "org-type" > *< / span > [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< span class = "org-type" > '< / span > )< span class = "org-type" > *< / span > cont< span class = "org-type" > *< / span > pinv(U), FEEDIN, FEEDOUT);
TRsvd = STsvd< span class = "org-type" > *< / span > [eye(6); zeros(6)];
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org80cd406" class = "outline-3" >
< h3 id = "org80cd406" > < span class = "section-number-3" > 4.12< / span > Results< / h3 >
< div class = "outline-text-3" id = "text-4-12" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span >
subplot(231)
bodemag(TR(1,1),TRcen(1,1),TRsvd(1,1),opts)
legend(< span class = "org-string" > 'OL'< / span > ,< span class = "org-string" > 'Centralized'< / span > ,< span class = "org-string" > 'SVD'< / span > )
subplot(232)
bodemag(TR(2,2),TRcen(2,2),TRsvd(2,2),opts)
legend(< span class = "org-string" > 'OL'< / span > ,< span class = "org-string" > 'Centralized'< / span > ,< span class = "org-string" > 'SVD'< / span > )
subplot(233)
bodemag(TR(3,3),TRcen(3,3),TRsvd(3,3),opts)
legend(< span class = "org-string" > 'OL'< / span > ,< span class = "org-string" > 'Centralized'< / span > ,< span class = "org-string" > 'SVD'< / span > )
subplot(234)
bodemag(TR(4,4),TRcen(4,4),TRsvd(4,4),opts)
legend(< span class = "org-string" > 'OL'< / span > ,< span class = "org-string" > 'Centralized'< / span > ,< span class = "org-string" > 'SVD'< / span > )
subplot(235)
bodemag(TR(5,5),TRcen(5,5),TRsvd(5,5),opts)
legend(< span class = "org-string" > 'OL'< / span > ,< span class = "org-string" > 'Centralized'< / span > ,< span class = "org-string" > 'SVD'< / span > )
subplot(236)
bodemag(TR(6,6),TRcen(6,6),TRsvd(6,6),opts)
legend(< span class = "org-string" > 'OL'< / span > ,< span class = "org-string" > 'Centralized'< / span > ,< span class = "org-string" > 'SVD'< / span > )
< / pre >
< / div >
< div id = "orgfadf6e5" class = "figure" >
< p > < img src = "figs/stewart_platform_analytical_svd_cen_comp.png" alt = "stewart_platform_analytical_svd_cen_comp.png" / >
< / p >
< p > < span class = "figure-number" > Figure 19: < / span > Comparison of the obtained transmissibility for the centralized control and the SVD control< / p >
< / div >
< / div >
< / div >
< / div >
2020-09-21 13:08:36 +02:00
< / div >
< div id = "postamble" class = "status" >
< p class = "author" > Author: Dehaeze Thomas< / p >
2020-10-13 14:51:15 +02:00
< p class = "date" > Created: 2020-10-09 ven. 16:21< / p >
2020-09-21 13:08:36 +02:00
< / div >
< / body >
< / html >
