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-05 18:28:44 +02:00
<!-- 2020 - 10 - 05 lun. 18:28 -->
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-05 18:28:44 +02:00
< li > < a href = "#org67dc64e" > 1. Gravimeter - Simscape Model< / a >
2020-09-21 13:08:36 +02:00
< ul >
2020-10-05 18:28:44 +02:00
< li > < a href = "#orgbc83858" > 1.1. Introduction< / a > < / li >
< li > < a href = "#org6a10d93" > 1.2. Simscape Model - Parameters< / a > < / li >
< li > < a href = "#org0efee8e" > 1.3. System Identification - Without Gravity< / a > < / li >
< li > < a href = "#org98fd3fd" > 1.4. System Identification - With Gravity< / a > < / li >
< li > < a href = "#org6400b2e" > 1.5. Analytical Model< / a >
2020-09-30 17:16:30 +02:00
< ul >
2020-10-05 18:28:44 +02:00
< li > < a href = "#orgd401b7a" > 1.5.1. Parameters< / a > < / li >
< li > < a href = "#orgdc4cf04" > 1.5.2. Generation of the State Space Model< / a > < / li >
< li > < a href = "#org2f36845" > 1.5.3. Comparison with the Simscape Model< / a > < / li >
< li > < a href = "#org028f15a" > 1.5.4. Analysis< / a > < / li >
< li > < a href = "#orgaf39b24" > 1.5.5. Control Section< / a > < / li >
< li > < a href = "#orga450746" > 1.5.6. Greshgorin radius< / a > < / li >
< li > < a href = "#orgd41a3f6" > 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-05 18:28:44 +02:00
< li > < a href = "#org866fa85" > 2. Gravimeter - Functions< / a >
2020-09-21 13:08:36 +02:00
< ul >
2020-10-05 18:28:44 +02:00
< li > < a href = "#orgcf775e2" > 2.1. < code > align< / code > < / a > < / li >
< li > < a href = "#org78f2c7e" > 2.2. < code > pzmap_testCL< / code > < / a > < / li >
2020-09-21 18:03:40 +02:00
< / ul >
< / li >
2020-10-05 18:28:44 +02:00
< li > < a href = "#orgd5b9491" > 3. Stewart Platform - Simscape Model< / a >
2020-09-21 18:03:40 +02:00
< ul >
2020-10-05 18:28:44 +02:00
< li > < a href = "#org6d58a07" > 3.1. Jacobian< / a > < / li >
< li > < a href = "#org4f58a34" > 3.2. Simscape Model< / a > < / li >
< li > < a href = "#org51c99d1" > 3.3. Identification of the plant< / a > < / li >
< li > < a href = "#org84418dd" > 3.4. Obtained Dynamics< / a > < / li >
< li > < a href = "#org315ca7e" > 3.5. Real Approximation of \(G\) at the decoupling frequency< / a > < / li >
< li > < a href = "#org91c0ed9" > 3.6. Verification of the decoupling using the “ Gershgorin Radii” < / a > < / li >
< li > < a href = "#org0bd0b38" > 3.7. Decoupled Plant< / a > < / li >
< li > < a href = "#org4b22e32" > 3.8. Diagonal Controller< / a > < / li >
< li > < a href = "#orgac4cf9b" > 3.9. Centralized Control< / a > < / li >
< li > < a href = "#org4ae317c" > 3.10. SVD Control< / a > < / li >
< li > < a href = "#orgabc897d" > 3.11. Results< / a > < / li >
2020-09-21 13:08:36 +02:00
< / ul >
< / li >
< / ul >
< / div >
< / div >
2020-10-05 18:28:44 +02:00
< div id = "outline-container-org67dc64e" class = "outline-2" >
< h2 id = "org67dc64e" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgbc83858" class = "outline-3" >
< h3 id = "orgbc83858" > < 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-05 18:28:44 +02:00
< div id = "orge6f0a72" 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-05 18:28:44 +02:00
< div id = "outline-container-org6a10d93" class = "outline-3" >
< h3 id = "org6a10d93" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org0efee8e" class = "outline-3" >
< h3 id = "org0efee8e" > < 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-05 18:28:44 +02:00
< div id = "orgf223fb8" 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-05 18:28:44 +02:00
< div id = "outline-container-org98fd3fd" class = "outline-3" >
< h3 id = "org98fd3fd" > < 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-05 18:28:44 +02:00
< div id = "org4d66bba" 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-05 18:28:44 +02:00
< div id = "outline-container-org6400b2e" class = "outline-3" >
< h3 id = "org6400b2e" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgd401b7a" class = "outline-4" >
< h4 id = "orgd401b7a" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgdc4cf04" class = "outline-4" >
< h4 id = "orgdc4cf04" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org2f36845" class = "outline-4" >
< h4 id = "org2f36845" > < 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-05 18:28:44 +02:00
< div id = "orgd96c232" 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-05 18:28:44 +02:00
< div id = "outline-container-org028f15a" class = "outline-4" >
< h4 id = "org028f15a" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgaf39b24" class = "outline-4" >
< h4 id = "orgaf39b24" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orga450746" class = "outline-4" >
< h4 id = "orga450746" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgd41a3f6" class = "outline-4" >
< h4 id = "orgd41a3f6" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org866fa85" class = "outline-2" >
< h2 id = "org866fa85" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgcf775e2" class = "outline-3" >
< h3 id = "orgcf775e2" > < 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-05 18:28:44 +02:00
< a id = "orgbb32c31" > < / 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-05 18:28:44 +02:00
< div id = "outline-container-org78f2c7e" class = "outline-3" >
< h3 id = "org78f2c7e" > < 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-05 18:28:44 +02:00
< a id = "org655412c" > < / 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-05 18:28:44 +02:00
< div id = "outline-container-orgd5b9491" class = "outline-2" >
< h2 id = "orgd5b9491" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org6d58a07" class = "outline-3" >
< h3 id = "org6d58a07" > < 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-05 18:06:49 +02:00
< pre class = "src src-matlab" > open(< span class = "org-string" > 'stewart_platform/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-05 18:28:44 +02:00
< div id = "outline-container-org4f58a34" class = "outline-3" >
< h3 id = "org4f58a34" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org51c99d1" class = "outline-3" >
< h3 id = "org51c99d1" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org84418dd" class = "outline-3" >
< h3 id = "org84418dd" > < 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-05 18:28:44 +02:00
< div id = "org77aab4b" 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-05 18:28:44 +02:00
< div id = "org9222b17" 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-05 18:28:44 +02:00
< div id = "org9d77253" 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-05 18:28:44 +02:00
< div id = "org4cce08b" 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-05 18:28:44 +02:00
< div id = "outline-container-org315ca7e" class = "outline-3" >
< h3 id = "org315ca7e" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org91c0ed9" class = "outline-3" >
< h3 id = "org91c0ed9" > < 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-05 18:28:44 +02:00
< div id = "orgda863a3" 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-05 18:28:44 +02:00
< div id = "outline-container-org0bd0b38" class = "outline-3" >
< h3 id = "org0bd0b38" > < 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-05 18:28:44 +02:00
< div id = "org6ba4690" 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-05 18:28:44 +02:00
< div id = "org5342ca6" 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-05 18:28:44 +02:00
< div id = "outline-container-org4b22e32" class = "outline-3" >
< h3 id = "org4b22e32" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgac4cf9b" class = "outline-3" >
< h3 id = "orgac4cf9b" > < 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-05 18:28:44 +02:00
< div id = "outline-container-org4ae317c" class = "outline-3" >
< h3 id = "org4ae317c" > < 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-05 18:28:44 +02:00
< div id = "outline-container-orgabc897d" class = "outline-3" >
< h3 id = "orgabc897d" > < 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-05 18:28:44 +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 = "#org92a495c" > 14< / a > .
2020-09-21 18:03:40 +02:00
< / p >
2020-10-05 18:28:44 +02:00
< div id = "org92a495c" 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 >
< / div >
< div id = "postamble" class = "status" >
< p class = "author" > Author: Dehaeze Thomas< / p >
2020-10-05 18:28:44 +02:00
< p class = "date" > Created: 2020-10-05 lun. 18:28< / p >
2020-09-21 13:08:36 +02:00
< / div >
< / body >
< / html >
