tdehaeze/svd-control
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Remove Gravity for the Stewart platform model

Browse Source
This commit is contained in:
Thomas Dehaeze 2020-10-13 15:01:42 +02:00
parent da9f3ed7ad
commit 96d036d936
9 changed files with 892 additions and 183 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
figs/centralized_control.pdf
View File

Binary file not shown.

BIN
figs/stewart_platform_translations.pdf
View File

Binary file not shown.

BIN
figs/stewart_platform_translations.png
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 112 KiB

After

Width:  |  Height:  |  Size: 112 KiB

370
index.html
View File

@ -3,7 +3,7 @@
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2020-10-09 ven. 16:21 --> <!-- 2020-10-13 mar. 14:53 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<title>SVD Control</title> <title>SVD Control</title>
<meta name="generator" content="Org mode" /> <meta name="generator" content="Org mode" />
@ -35,75 +35,75 @@
<h2>Table of Contents</h2> <h2>Table of Contents</h2>
<div id="text-table-of-contents"> <div id="text-table-of-contents">
<ul> <ul>
<li><a href="#org90d7008">1. Gravimeter - Simscape Model</a> <li><a href="#org6dd65c1">1. Gravimeter - Simscape Model</a>
<ul> <ul>
<li><a href="#org29b9308">1.1. Introduction</a></li> <li><a href="#org85dbe5c">1.1. Introduction</a></li>
<li><a href="#orgd333b87">1.2. Simscape Model - Parameters</a></li> <li><a href="#org0b31481">1.2. Simscape Model - Parameters</a></li>
<li><a href="#org09b581d">1.3. System Identification - Without Gravity</a></li> <li><a href="#org949338c">1.3. System Identification - Without Gravity</a></li>
<li><a href="#org4f091cc">1.4. System Identification - With Gravity</a></li> <li><a href="#org3e8d708">1.4. System Identification - With Gravity</a></li>
<li><a href="#org7c4effc">1.5. Analytical Model</a> <li><a href="#org8263a33">1.5. Analytical Model</a>
<ul> <ul>
<li><a href="#org20ea2aa">1.5.1. Parameters</a></li> <li><a href="#org5ce809b">1.5.1. Parameters</a></li>
<li><a href="#org02cb447">1.5.2. Generation of the State Space Model</a></li> <li><a href="#org485b7e0">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="#orgb77d12b">1.5.3. Comparison with the Simscape Model</a></li>
<li><a href="#org6c56e64">1.5.4. Analysis</a></li> <li><a href="#orgbede3a4">1.5.4. Analysis</a></li>
<li><a href="#orgeb20c08">1.5.5. Control Section</a></li> <li><a href="#org00d06a7">1.5.5. Control Section</a></li>
<li><a href="#org931022f">1.5.6. Greshgorin radius</a></li> <li><a href="#org8d48657">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> <li><a href="#org7348f99">1.5.7. Injecting ground motion in the system to have the output</a></li>
</ul> </ul>
</li> </li>
</ul> </ul>
</li> </li>
<li><a href="#org36d6b85">2. Gravimeter - Functions</a> <li><a href="#org534f1d2">2. Gravimeter - Functions</a>
<ul> <ul>
<li><a href="#orgbb4529b">2.1. <code>align</code></a></li> <li><a href="#org8fd3468">2.1. <code>align</code></a></li>
<li><a href="#orge0ed8bf">2.2. <code>pzmap_testCL</code></a></li> <li><a href="#org7fc9d1b">2.2. <code>pzmap_testCL</code></a></li>
</ul> </ul>
</li> </li>
<li><a href="#org5afd29d">3. Stewart Platform - Simscape Model</a> <li><a href="#orga726921">3. Stewart Platform - Simscape Model</a>
<ul> <ul>
<li><a href="#orgff944f3">3.1. Jacobian</a></li> <li><a href="#org0f4c378">3.1. Jacobian</a></li>
<li><a href="#org07ffe6c">3.2. Simscape Model</a></li> <li><a href="#org8e93915">3.2. Simscape Model</a></li>
<li><a href="#org9aaf0d3">3.3. Identification of the plant</a></li> <li><a href="#orga80ad9d">3.3. Identification of the plant</a></li>
<li><a href="#orgb0b01e3">3.4. Obtained Dynamics</a></li> <li><a href="#org820395d">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="#org531c180">3.5. Real Approximation of \(G\) at the decoupling frequency</a></li>
<li><a href="#org53d60e1">3.6. Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</a></li> <li><a href="#org04886ad">3.6. Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</a></li>
<li><a href="#org40c1d24">3.7. Decoupled Plant</a></li> <li><a href="#org96683a8">3.7. Decoupled Plant</a></li>
<li><a href="#orgdfcd158">3.8. Diagonal Controller</a></li> <li><a href="#orgde9fab8">3.8. Diagonal Controller</a></li>
<li><a href="#org25e3b35">3.9. Centralized Control</a></li> <li><a href="#org47bbca8">3.9. Centralized Control</a></li>
<li><a href="#org4d83793">3.10. SVD Control</a></li> <li><a href="#org2c1e3f7">3.10. SVD Control</a></li>
<li><a href="#org7cece79">3.11. Results</a></li> <li><a href="#orgd6985da">3.11. Results</a></li>
</ul> </ul>
</li> </li>
<li><a href="#org8b11aba">4. Stewart Platform - Analytical Model</a> <li><a href="#org99c6262">4. Stewart Platform - Analytical Model</a>
<ul> <ul>
<li><a href="#org2a175f6">4.1. Characteristics</a></li> <li><a href="#org6e044dd">4.1. Characteristics</a></li>
<li><a href="#org9efa4f4">4.2. Mass Matrix</a></li> <li><a href="#org20b7c2e">4.2. Mass Matrix</a></li>
<li><a href="#org97bc497">4.3. Jacobian Matrix</a></li> <li><a href="#org2f016df">4.3. Jacobian Matrix</a></li>
<li><a href="#org7c9679d">4.4. Stifnness matrix and Damping matrix</a></li> <li><a href="#org2c9ff6d">4.4. Stifnness and Damping matrices</a></li>
<li><a href="#org00e8691">4.5. State Space System</a></li> <li><a href="#orgffba0a8">4.5. State Space System</a></li>
<li><a href="#org8a70996">4.6. Transmissibility</a></li> <li><a href="#org42b1b07">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="#org38c8159">4.7. Real approximation of \(G(j\omega)\) at decoupling frequency</a></li>
<li><a href="#orgc58b81c">4.8. Coupled and Decoupled Plant &ldquo;Gershgorin Radii&rdquo;</a></li> <li><a href="#org477b3ce">4.8. Coupled and Decoupled Plant &ldquo;Gershgorin Radii&rdquo;</a></li>
<li><a href="#org2ba91f6">4.9. Decoupled Plant</a></li> <li><a href="#orgde4eec1">4.9. Decoupled Plant</a></li>
<li><a href="#orgc73a283">4.10. Controller</a></li> <li><a href="#org11b0182">4.10. Controller</a></li>
<li><a href="#org9c82ee4">4.11. Closed Loop System</a></li> <li><a href="#org5c893a8">4.11. Closed Loop System</a></li>
<li><a href="#org80cd406">4.12. Results</a></li> <li><a href="#orgb1c0711">4.12. Results</a></li>
</ul> </ul>
</li> </li>
</ul> </ul>
</div> </div>
</div> </div>
<div id="outline-container-org90d7008" class="outline-2"> <div id="outline-container-org6dd65c1" class="outline-2">
<h2 id="org90d7008"><span class="section-number-2">1</span> Gravimeter - Simscape Model</h2> <h2 id="org6dd65c1"><span class="section-number-2">1</span> Gravimeter - Simscape Model</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
</div> </div>
<div id="outline-container-org29b9308" class="outline-3"> <div id="outline-container-org85dbe5c" class="outline-3">
<h3 id="org29b9308"><span class="section-number-3">1.1</span> Introduction</h3> <h3 id="org85dbe5c"><span class="section-number-3">1.1</span> Introduction</h3>
<div class="outline-text-3" id="text-1-1"> <div class="outline-text-3" id="text-1-1">
<div id="org7df72f4" class="figure"> <div id="org02345c4" class="figure">
<p><img src="figs/gravimeter_model.png" alt="gravimeter_model.png" /> <p><img src="figs/gravimeter_model.png" alt="gravimeter_model.png" />
</p> </p>
<p><span class="figure-number">Figure 1: </span>Model of the gravimeter</p> <p><span class="figure-number">Figure 1: </span>Model of the gravimeter</p>
@ -111,8 +111,8 @@
</div> </div>
</div> </div>
<div id="outline-container-orgd333b87" class="outline-3"> <div id="outline-container-org0b31481" class="outline-3">
<h3 id="orgd333b87"><span class="section-number-3">1.2</span> Simscape Model - Parameters</h3> <h3 id="org0b31481"><span class="section-number-3">1.2</span> Simscape Model - Parameters</h3>
<div class="outline-text-3" id="text-1-2"> <div class="outline-text-3" id="text-1-2">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">open(<span class="org-string">'gravimeter.slx'</span>) <pre class="src src-matlab">open(<span class="org-string">'gravimeter.slx'</span>)
@ -143,8 +143,8 @@ g = 0; <span class="org-comment">% Gravity [m/s2]</span>
</div> </div>
</div> </div>
<div id="outline-container-org09b581d" class="outline-3"> <div id="outline-container-org949338c" class="outline-3">
<h3 id="org09b581d"><span class="section-number-3">1.3</span> System Identification - Without Gravity</h3> <h3 id="org949338c"><span class="section-number-3">1.3</span> System Identification - Without Gravity</h3>
<div class="outline-text-3" id="text-1-3"> <div class="outline-text-3" id="text-1-3">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span> <pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
@ -191,7 +191,7 @@ State-space model with 4 outputs, 3 inputs, and 6 states.
<div id="orgdd275bb" class="figure"> <div id="orga082635" class="figure">
<p><img src="figs/open_loop_tf.png" alt="open_loop_tf.png" /> <p><img src="figs/open_loop_tf.png" alt="open_loop_tf.png" />
</p> </p>
<p><span class="figure-number">Figure 2: </span>Open Loop Transfer Function from 3 Actuators to 4 Accelerometers</p> <p><span class="figure-number">Figure 2: </span>Open Loop Transfer Function from 3 Actuators to 4 Accelerometers</p>
@ -199,8 +199,8 @@ State-space model with 4 outputs, 3 inputs, and 6 states.
</div> </div>
</div> </div>
<div id="outline-container-org4f091cc" class="outline-3"> <div id="outline-container-org3e8d708" class="outline-3">
<h3 id="org4f091cc"><span class="section-number-3">1.4</span> System Identification - With Gravity</h3> <h3 id="org3e8d708"><span class="section-number-3">1.4</span> System Identification - With Gravity</h3>
<div class="outline-text-3" id="text-1-4"> <div class="outline-text-3" id="text-1-4">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">g = 9.80665; <span class="org-comment">% Gravity [m/s2]</span> <pre class="src src-matlab">g = 9.80665; <span class="org-comment">% Gravity [m/s2]</span>
@ -229,7 +229,7 @@ ans =
</pre> </pre>
<div id="org392bf82" class="figure"> <div id="org1a94741" class="figure">
<p><img src="figs/open_loop_tf_g.png" alt="open_loop_tf_g.png" /> <p><img src="figs/open_loop_tf_g.png" alt="open_loop_tf_g.png" />
</p> </p>
<p><span class="figure-number">Figure 3: </span>Open Loop Transfer Function from 3 Actuators to 4 Accelerometers with an without gravity</p> <p><span class="figure-number">Figure 3: </span>Open Loop Transfer Function from 3 Actuators to 4 Accelerometers with an without gravity</p>
@ -237,12 +237,12 @@ ans =
</div> </div>
</div> </div>
<div id="outline-container-org7c4effc" class="outline-3"> <div id="outline-container-org8263a33" class="outline-3">
<h3 id="org7c4effc"><span class="section-number-3">1.5</span> Analytical Model</h3> <h3 id="org8263a33"><span class="section-number-3">1.5</span> Analytical Model</h3>
<div class="outline-text-3" id="text-1-5"> <div class="outline-text-3" id="text-1-5">
</div> </div>
<div id="outline-container-org20ea2aa" class="outline-4"> <div id="outline-container-org5ce809b" class="outline-4">
<h4 id="org20ea2aa"><span class="section-number-4">1.5.1</span> Parameters</h4> <h4 id="org5ce809b"><span class="section-number-4">1.5.1</span> Parameters</h4>
<div class="outline-text-4" id="text-1-5-1"> <div class="outline-text-4" id="text-1-5-1">
<p> <p>
Bode options. Bode options.
@ -274,8 +274,8 @@ Frequency vector.
</div> </div>
</div> </div>
<div id="outline-container-org02cb447" class="outline-4"> <div id="outline-container-org485b7e0" class="outline-4">
<h4 id="org02cb447"><span class="section-number-4">1.5.2</span> Generation of the State Space Model</h4> <h4 id="org485b7e0"><span class="section-number-4">1.5.2</span> Generation of the State Space Model</h4>
<div class="outline-text-4" id="text-1-5-2"> <div class="outline-text-4" id="text-1-5-2">
<p> <p>
Mass matrix Mass matrix
@ -376,11 +376,11 @@ State-space model with 12 outputs, 6 inputs, and 6 states.
</div> </div>
</div> </div>
<div id="outline-container-org9417f40" class="outline-4"> <div id="outline-container-orgb77d12b" class="outline-4">
<h4 id="org9417f40"><span class="section-number-4">1.5.3</span> Comparison with the Simscape Model</h4> <h4 id="orgb77d12b"><span class="section-number-4">1.5.3</span> Comparison with the Simscape Model</h4>
<div class="outline-text-4" id="text-1-5-3"> <div class="outline-text-4" id="text-1-5-3">
<div id="orga6f165d" class="figure"> <div id="org7bbc6ef" class="figure">
<p><img src="figs/gravimeter_analytical_system_open_loop_models.png" alt="gravimeter_analytical_system_open_loop_models.png" /> <p><img src="figs/gravimeter_analytical_system_open_loop_models.png" alt="gravimeter_analytical_system_open_loop_models.png" />
</p> </p>
<p><span class="figure-number">Figure 4: </span>Comparison of the analytical and the Simscape models</p> <p><span class="figure-number">Figure 4: </span>Comparison of the analytical and the Simscape models</p>
@ -388,8 +388,8 @@ State-space model with 12 outputs, 6 inputs, and 6 states.
</div> </div>
</div> </div>
<div id="outline-container-org6c56e64" class="outline-4"> <div id="outline-container-orgbede3a4" class="outline-4">
<h4 id="org6c56e64"><span class="section-number-4">1.5.4</span> Analysis</h4> <h4 id="orgbede3a4"><span class="section-number-4">1.5.4</span> Analysis</h4>
<div class="outline-text-4" id="text-1-5-4"> <div class="outline-text-4" id="text-1-5-4">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-comment">% figure</span> <pre class="src src-matlab"><span class="org-comment">% figure</span>
@ -457,8 +457,8 @@ State-space model with 12 outputs, 6 inputs, and 6 states.
</div> </div>
</div> </div>
<div id="outline-container-orgeb20c08" class="outline-4"> <div id="outline-container-org00d06a7" class="outline-4">
<h4 id="orgeb20c08"><span class="section-number-4">1.5.5</span> Control Section</h4> <h4 id="org00d06a7"><span class="section-number-4">1.5.5</span> Control Section</h4>
<div class="outline-text-4" id="text-1-5-5"> <div class="outline-text-4" id="text-1-5-5">
<div class="org-src-container"> <div class="org-src-container">
<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); <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);
@ -598,8 +598,8 @@ legend(<span class="org-string">'Control OFF'</span>,<span class="org-string">'D
</div> </div>
</div> </div>
<div id="outline-container-org931022f" class="outline-4"> <div id="outline-container-org8d48657" class="outline-4">
<h4 id="org931022f"><span class="section-number-4">1.5.6</span> Greshgorin radius</h4> <h4 id="org8d48657"><span class="section-number-4">1.5.6</span> Greshgorin radius</h4>
<div class="outline-text-4" id="text-1-5-6"> <div class="outline-text-4" id="text-1-5-6">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">system_dec_freq = freqresp(system_dec,w); <pre class="src src-matlab">system_dec_freq = freqresp(system_dec,w);
@ -645,8 +645,8 @@ ylabel(<span class="org-string">'Greshgorin radius [-]'</span>);
</div> </div>
</div> </div>
<div id="outline-container-org1d56ec4" class="outline-4"> <div id="outline-container-org7348f99" 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> <h4 id="org7348f99"><span class="section-number-4">1.5.7</span> Injecting ground motion in the system to have the output</h4>
<div class="outline-text-4" id="text-1-5-7"> <div class="outline-text-4" id="text-1-5-7">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">Fr = logspace(<span class="org-type">-</span>2,3,1e3); <pre class="src src-matlab">Fr = logspace(<span class="org-type">-</span>2,3,1e3);
@ -702,15 +702,15 @@ rot = PHI(<span class="org-type">:</span>,11,11);
</div> </div>
</div> </div>
<div id="outline-container-org36d6b85" class="outline-2"> <div id="outline-container-org534f1d2" class="outline-2">
<h2 id="org36d6b85"><span class="section-number-2">2</span> Gravimeter - Functions</h2> <h2 id="org534f1d2"><span class="section-number-2">2</span> Gravimeter - Functions</h2>
<div class="outline-text-2" id="text-2"> <div class="outline-text-2" id="text-2">
</div> </div>
<div id="outline-container-orgbb4529b" class="outline-3"> <div id="outline-container-org8fd3468" class="outline-3">
<h3 id="orgbb4529b"><span class="section-number-3">2.1</span> <code>align</code></h3> <h3 id="org8fd3468"><span class="section-number-3">2.1</span> <code>align</code></h3>
<div class="outline-text-3" id="text-2-1"> <div class="outline-text-3" id="text-2-1">
<p> <p>
<a id="orgf2b803a"></a> <a id="org95a25f3"></a>
</p> </p>
<p> <p>
@ -739,11 +739,11 @@ This Matlab function is accessible <a href="gravimeter/align.m">here</a>.
</div> </div>
<div id="outline-container-orge0ed8bf" class="outline-3"> <div id="outline-container-org7fc9d1b" class="outline-3">
<h3 id="orge0ed8bf"><span class="section-number-3">2.2</span> <code>pzmap_testCL</code></h3> <h3 id="org7fc9d1b"><span class="section-number-3">2.2</span> <code>pzmap_testCL</code></h3>
<div class="outline-text-3" id="text-2-2"> <div class="outline-text-3" id="text-2-2">
<p> <p>
<a id="orgf08bacf"></a> <a id="orge776d7f"></a>
</p> </p>
<p> <p>
@ -792,12 +792,12 @@ This Matlab function is accessible <a href="gravimeter/pzmap_testCL.m">here</a>.
</div> </div>
</div> </div>
<div id="outline-container-org5afd29d" class="outline-2"> <div id="outline-container-orga726921" class="outline-2">
<h2 id="org5afd29d"><span class="section-number-2">3</span> Stewart Platform - Simscape Model</h2> <h2 id="orga726921"><span class="section-number-2">3</span> Stewart Platform - Simscape Model</h2>
<div class="outline-text-2" id="text-3"> <div class="outline-text-2" id="text-3">
</div> </div>
<div id="outline-container-orgff944f3" class="outline-3"> <div id="outline-container-org0f4c378" class="outline-3">
<h3 id="orgff944f3"><span class="section-number-3">3.1</span> Jacobian</h3> <h3 id="org0f4c378"><span class="section-number-3">3.1</span> Jacobian</h3>
<div class="outline-text-3" id="text-3-1"> <div class="outline-text-3" id="text-3-1">
<p> <p>
First, the position of the &ldquo;joints&rdquo; (points of force application) are estimated and the Jacobian computed. First, the position of the &ldquo;joints&rdquo; (points of force application) are estimated and the Jacobian computed.
@ -839,11 +839,11 @@ save(<span class="org-string">'./jacobian.mat'</span>, <span class="org-string">
</div> </div>
</div> </div>
<div id="outline-container-org07ffe6c" class="outline-3"> <div id="outline-container-org8e93915" class="outline-3">
<h3 id="org07ffe6c"><span class="section-number-3">3.2</span> Simscape Model</h3> <h3 id="org8e93915"><span class="section-number-3">3.2</span> Simscape Model</h3>
<div class="outline-text-3" id="text-3-2"> <div class="outline-text-3" id="text-3-2">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">open(<span class="org-string">'stewart_platform/drone_platform.slx'</span>); <pre class="src src-matlab">open(<span class="org-string">'drone_platform.slx'</span>);
</pre> </pre>
</div> </div>
@ -851,9 +851,9 @@ save(<span class="org-string">'./jacobian.mat'</span>, <span class="org-string">
Definition of spring parameters Definition of spring parameters
</p> </p>
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">kx = 50; <span class="org-comment">% [N/m]</span> <pre class="src src-matlab">kx = 0.5<span class="org-type">*</span>1e3<span class="org-type">/</span>3; <span class="org-comment">% [N/m]</span>
ky = 50; ky = 0.5<span class="org-type">*</span>1e3<span class="org-type">/</span>3;
kz = 50; kz = 1e3<span class="org-type">/</span>3;
cx = 0.025; <span class="org-comment">% [Nm/rad]</span> cx = 0.025; <span class="org-comment">% [Nm/rad]</span>
cy = 0.025; cy = 0.025;
@ -871,8 +871,8 @@ We load the Jacobian.
</div> </div>
</div> </div>
<div id="outline-container-org9aaf0d3" class="outline-3"> <div id="outline-container-orga80ad9d" class="outline-3">
<h3 id="org9aaf0d3"><span class="section-number-3">3.3</span> Identification of the plant</h3> <h3 id="orga80ad9d"><span class="section-number-3">3.3</span> Identification of the plant</h3>
<div class="outline-text-3" id="text-3-3"> <div class="outline-text-3" id="text-3-3">
<p> <p>
The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform. The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform.
@ -929,32 +929,32 @@ Gl.OutputName = {<span class="org-string">'A1'</span>, <span class="org-string"
</div> </div>
</div> </div>
<div id="outline-container-orgb0b01e3" class="outline-3"> <div id="outline-container-org820395d" class="outline-3">
<h3 id="orgb0b01e3"><span class="section-number-3">3.4</span> Obtained Dynamics</h3> <h3 id="org820395d"><span class="section-number-3">3.4</span> Obtained Dynamics</h3>
<div class="outline-text-3" id="text-3-4"> <div class="outline-text-3" id="text-3-4">
<div id="org15e1aeb" class="figure"> <div id="orgf45efb1" class="figure">
<p><img src="figs/stewart_platform_translations.png" alt="stewart_platform_translations.png" /> <p><img src="figs/stewart_platform_translations.png" alt="stewart_platform_translations.png" />
</p> </p>
<p><span class="figure-number">Figure 5: </span>Stewart Platform Plant from forces applied by the legs to the acceleration of the platform</p> <p><span class="figure-number">Figure 5: </span>Stewart Platform Plant from forces applied by the legs to the acceleration of the platform</p>
</div> </div>
<div id="org1a9b1c6" class="figure"> <div id="org7a9f376" class="figure">
<p><img src="figs/stewart_platform_rotations.png" alt="stewart_platform_rotations.png" /> <p><img src="figs/stewart_platform_rotations.png" alt="stewart_platform_rotations.png" />
</p> </p>
<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> <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>
</div> </div>
<div id="org2c0cea0" class="figure"> <div id="org01ccd4c" class="figure">
<p><img src="figs/stewart_platform_legs.png" alt="stewart_platform_legs.png" /> <p><img src="figs/stewart_platform_legs.png" alt="stewart_platform_legs.png" />
</p> </p>
<p><span class="figure-number">Figure 7: </span>Stewart Platform Plant from forces applied by the legs to displacement of the legs</p> <p><span class="figure-number">Figure 7: </span>Stewart Platform Plant from forces applied by the legs to displacement of the legs</p>
</div> </div>
<div id="org46a471a" class="figure"> <div id="org18ecae5" class="figure">
<p><img src="figs/stewart_platform_transmissibility.png" alt="stewart_platform_transmissibility.png" /> <p><img src="figs/stewart_platform_transmissibility.png" alt="stewart_platform_transmissibility.png" />
</p> </p>
<p><span class="figure-number">Figure 8: </span>Transmissibility</p> <p><span class="figure-number">Figure 8: </span>Transmissibility</p>
@ -962,8 +962,8 @@ Gl.OutputName = {<span class="org-string">'A1'</span>, <span class="org-string"
</div> </div>
</div> </div>
<div id="outline-container-org1de55ce" class="outline-3"> <div id="outline-container-org531c180" class="outline-3">
<h3 id="org1de55ce"><span class="section-number-3">3.5</span> Real Approximation of \(G\) at the decoupling frequency</h3> <h3 id="org531c180"><span class="section-number-3">3.5</span> Real Approximation of \(G\) at the decoupling frequency</h3>
<div class="outline-text-3" id="text-3-5"> <div class="outline-text-3" id="text-3-5">
<p> <p>
Let&rsquo;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\). Let&rsquo;s compute a real approximation of the complex matrix \(H_1\) which corresponds to the the transfer function \(G_c(j\omega_c)\) from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency \(\omega_c\).
@ -989,8 +989,8 @@ H1 = inv(D<span class="org-type">*</span>real(H1<span class="org-type">'*</span>
</div> </div>
</div> </div>
<div id="outline-container-org53d60e1" class="outline-3"> <div id="outline-container-org04886ad" class="outline-3">
<h3 id="org53d60e1"><span class="section-number-3">3.6</span> Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</h3> <h3 id="org04886ad"><span class="section-number-3">3.6</span> Verification of the decoupling using the &ldquo;Gershgorin Radii&rdquo;</h3>
<div class="outline-text-3" id="text-3-6"> <div class="outline-text-3" id="text-3-6">
<p> <p>
First, the Singular Value Decomposition of \(H_1\) is performed: First, the Singular Value Decomposition of \(H_1\) is performed:
@ -1058,7 +1058,7 @@ H = abs(squeeze(freqresp(Gj, freqs, <span class="org-string">'Hz'</span>)));
</div> </div>
<div id="org6065705" class="figure"> <div id="org98d0c86" class="figure">
<p><img src="figs/simscape_model_gershgorin_radii.png" alt="simscape_model_gershgorin_radii.png" /> <p><img src="figs/simscape_model_gershgorin_radii.png" alt="simscape_model_gershgorin_radii.png" />
</p> </p>
<p><span class="figure-number">Figure 9: </span>Gershgorin Radii of the Coupled and Decoupled plants</p> <p><span class="figure-number">Figure 9: </span>Gershgorin Radii of the Coupled and Decoupled plants</p>
@ -1066,8 +1066,8 @@ H = abs(squeeze(freqresp(Gj, freqs, <span class="org-string">'Hz'</span>)));
</div> </div>
</div> </div>
<div id="outline-container-org40c1d24" class="outline-3"> <div id="outline-container-org96683a8" class="outline-3">
<h3 id="org40c1d24"><span class="section-number-3">3.7</span> Decoupled Plant</h3> <h3 id="org96683a8"><span class="section-number-3">3.7</span> Decoupled Plant</h3>
<div class="outline-text-3" id="text-3-7"> <div class="outline-text-3" id="text-3-7">
<p> <p>
Let&rsquo;s see the bode plot of the decoupled plant \(G_d(s)\). Let&rsquo;s see the bode plot of the decoupled plant \(G_d(s)\).
@ -1075,14 +1075,14 @@ Let&rsquo;s see the bode plot of the decoupled plant \(G_d(s)\).
</p> </p>
<div id="orgbfa07c9" class="figure"> <div id="org2351e85" class="figure">
<p><img src="figs/simscape_model_decoupled_plant_svd.png" alt="simscape_model_decoupled_plant_svd.png" /> <p><img src="figs/simscape_model_decoupled_plant_svd.png" alt="simscape_model_decoupled_plant_svd.png" />
</p> </p>
<p><span class="figure-number">Figure 10: </span>Decoupled Plant using SVD</p> <p><span class="figure-number">Figure 10: </span>Decoupled Plant using SVD</p>
</div> </div>
<div id="org28978a4" class="figure"> <div id="org6699d5a" class="figure">
<p><img src="figs/simscape_model_decoupled_plant_jacobian.png" alt="simscape_model_decoupled_plant_jacobian.png" /> <p><img src="figs/simscape_model_decoupled_plant_jacobian.png" alt="simscape_model_decoupled_plant_jacobian.png" />
</p> </p>
<p><span class="figure-number">Figure 11: </span>Decoupled Plant using the Jacobian</p> <p><span class="figure-number">Figure 11: </span>Decoupled Plant using the Jacobian</p>
@ -1090,8 +1090,8 @@ Let&rsquo;s see the bode plot of the decoupled plant \(G_d(s)\).
</div> </div>
</div> </div>
<div id="outline-container-orgdfcd158" class="outline-3"> <div id="outline-container-orgde9fab8" class="outline-3">
<h3 id="orgdfcd158"><span class="section-number-3">3.8</span> Diagonal Controller</h3> <h3 id="orgde9fab8"><span class="section-number-3">3.8</span> Diagonal Controller</h3>
<div class="outline-text-3" id="text-3-8"> <div class="outline-text-3" id="text-3-8">
<p> <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\). The controller \(K\) is a diagonal controller consisting a low pass filters with a crossover frequency \(\omega_c\) and a DC gain \(C_g\).
@ -1107,8 +1107,8 @@ K = eye(6)<span class="org-type">*</span>C_g<span class="org-type">/</span>(s<sp
</div> </div>
</div> </div>
<div id="outline-container-org25e3b35" class="outline-3"> <div id="outline-container-org47bbca8" class="outline-3">
<h3 id="org25e3b35"><span class="section-number-3">3.9</span> Centralized Control</h3> <h3 id="org47bbca8"><span class="section-number-3">3.9</span> Centralized Control</h3>
<div class="outline-text-3" id="text-3-9"> <div class="outline-text-3" id="text-3-9">
<p> <p>
The control diagram for the centralized control is shown below. The control diagram for the centralized control is shown below.
@ -1132,8 +1132,8 @@ The Jacobian is used to convert forces in the cartesian frame to forces applied
</div> </div>
</div> </div>
<div id="outline-container-org4d83793" class="outline-3"> <div id="outline-container-org2c1e3f7" class="outline-3">
<h3 id="org4d83793"><span class="section-number-3">3.10</span> SVD Control</h3> <h3 id="org2c1e3f7"><span class="section-number-3">3.10</span> SVD Control</h3>
<div class="outline-text-3" id="text-3-10"> <div class="outline-text-3" id="text-3-10">
<p> <p>
The SVD control architecture is shown below. The SVD control architecture is shown below.
@ -1156,8 +1156,8 @@ SVD Control
</div> </div>
</div> </div>
<div id="outline-container-org7cece79" class="outline-3"> <div id="outline-container-orgd6985da" class="outline-3">
<h3 id="org7cece79"><span class="section-number-3">3.11</span> Results</h3> <h3 id="orgd6985da"><span class="section-number-3">3.11</span> Results</h3>
<div class="outline-text-3" id="text-3-11"> <div class="outline-text-3" id="text-3-11">
<p> <p>
Let&rsquo;s first verify the stability of the closed-loop systems: Let&rsquo;s first verify the stability of the closed-loop systems:
@ -1182,16 +1182,16 @@ ans =
<pre class="example"> <pre class="example">
ans = ans =
logical logical
1 0
</pre> </pre>
<p> <p>
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>. The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure <a href="#org0856618">14</a>.
</p> </p>
<div id="org62fae46" class="figure"> <div id="org0856618" class="figure">
<p><img src="figs/stewart_platform_simscape_cl_transmissibility.png" alt="stewart_platform_simscape_cl_transmissibility.png" /> <p><img src="figs/stewart_platform_simscape_cl_transmissibility.png" alt="stewart_platform_simscape_cl_transmissibility.png" />
</p> </p>
<p><span class="figure-number">Figure 14: </span>Obtained Transmissibility</p> <p><span class="figure-number">Figure 14: </span>Obtained Transmissibility</p>
@ -1200,83 +1200,85 @@ The obtained transmissibility in Open-loop, for the centralized control as well
</div> </div>
</div> </div>
<div id="outline-container-org8b11aba" class="outline-2"> <div id="outline-container-org99c6262" class="outline-2">
<h2 id="org8b11aba"><span class="section-number-2">4</span> Stewart Platform - Analytical Model</h2> <h2 id="org99c6262"><span class="section-number-2">4</span> Stewart Platform - Analytical Model</h2>
<div class="outline-text-2" id="text-4"> <div class="outline-text-2" id="text-4">
</div> </div>
<div id="outline-container-org2a175f6" class="outline-3"> <div id="outline-container-org6e044dd" class="outline-3">
<h3 id="org2a175f6"><span class="section-number-3">4.1</span> Characteristics</h3> <h3 id="org6e044dd"><span class="section-number-3">4.1</span> Characteristics</h3>
<div class="outline-text-3" id="text-4-1"> <div class="outline-text-3" id="text-4-1">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">L = 0.055; <pre class="src src-matlab">L = 0.055; <span class="org-comment">% Leg length [m]</span>
Zc = 0; Zc = 0; <span class="org-comment">% ?</span>
m = 0.2; m = 0.2; <span class="org-comment">% Top platform mass [m]</span>
k = 1e3; k = 1e3; <span class="org-comment">% Total vertical stiffness [N/m]</span>
c = 2<span class="org-type">*</span>0.1<span class="org-type">*</span>sqrt(k<span class="org-type">*</span>m); c = 2<span class="org-type">*</span>0.1<span class="org-type">*</span>sqrt(k<span class="org-type">*</span>m); <span class="org-comment">% Damping ? [N/(m/s)]</span>
Rx = 0.04; Rx = 0.04; <span class="org-comment">% ?</span>
Rz = 0.04; Rz = 0.04; <span class="org-comment">% ?</span>
Ix = m<span class="org-type">*</span>Rx<span class="org-type">^</span>2; Ix = m<span class="org-type">*</span>Rx<span class="org-type">^</span>2; <span class="org-comment">% ?</span>
Iy = 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; <span class="org-comment">% ?</span>
Iz = m<span class="org-type">*</span>Rz<span class="org-type">^</span>2; Iz = m<span class="org-type">*</span>Rz<span class="org-type">^</span>2; <span class="org-comment">% ?</span>
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org9efa4f4" class="outline-3"> <div id="outline-container-org20b7c2e" class="outline-3">
<h3 id="org9efa4f4"><span class="section-number-3">4.2</span> Mass Matrix</h3> <h3 id="org20b7c2e"><span class="section-number-3">4.2</span> Mass Matrix</h3>
<div class="outline-text-3" id="text-4-2"> <div class="outline-text-3" id="text-4-2">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">M = m<span class="org-type">*</span>[1 0 0 0 Zc 0; <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 1 0 <span class="org-type">-</span>Zc 0 0;
0 0 1 0 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; 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; 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]; 0 0 0 0 0 Rz<span class="org-type">^</span>2];
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org97bc497" class="outline-3"> <div id="outline-container-org2f016df" class="outline-3">
<h3 id="org97bc497"><span class="section-number-3">4.3</span> Jacobian Matrix</h3> <h3 id="org2f016df"><span class="section-number-3">4.3</span> Jacobian Matrix</h3>
<div class="outline-text-3" id="text-4-3"> <div class="outline-text-3" id="text-4-3">
<div class="org-src-container"> <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; <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">(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> 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; 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); <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)]; 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> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org7c9679d" class="outline-3"> <div id="outline-container-org2c9ff6d" class="outline-3">
<h3 id="org7c9679d"><span class="section-number-3">4.4</span> Stifnness matrix and Damping matrix</h3> <h3 id="org2c9ff6d"><span class="section-number-3">4.4</span> Stifnness and Damping matrices</h3>
<div class="outline-text-3" id="text-4-4"> <div class="outline-text-3" id="text-4-4">
<div class="org-src-container"> <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> <pre class="src src-matlab">kv = k<span class="org-type">/</span>3; <span class="org-comment">% Vertical Stiffness of the springs [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> kh = 0.5<span class="org-type">*</span>k<span class="org-type">/</span>3; <span class="org-comment">% Horizontal Stiffness of the springs [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>
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> C = c<span class="org-type">*</span>K<span class="org-type">/</span>100000; <span class="org-comment">% Damping Matrix</span>
</pre> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org00e8691" class="outline-3"> <div id="outline-container-orgffba0a8" class="outline-3">
<h3 id="org00e8691"><span class="section-number-3">4.5</span> State Space System</h3> <h3 id="orgffba0a8"><span class="section-number-3">4.5</span> State Space System</h3>
<div class="outline-text-3" id="text-4-5"> <div class="outline-text-3" id="text-4-5">
<div class="org-src-container"> <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]; <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)]; Bw = [zeros(6); <span class="org-type">-</span>eye(6)];
Bu = [zeros(6); M<span class="org-type">\</span>Bj]; 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]; 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]; D = [zeros(6) M<span class="org-type">\</span>Bj];
ST = ss(A,[Bw Bu],Co,D); ST = ss(A,[Bw Bu],Co,D);
@ -1291,16 +1293,18 @@ ST = ss(A,[Bw Bu],Co,D);
<div class="org-src-container"> <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>;... <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>}; <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>;... 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>}; <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>}; 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> </pre>
</div> </div>
</div> </div>
</div> </div>
<div id="outline-container-org8a70996" class="outline-3"> <div id="outline-container-org42b1b07" class="outline-3">
<h3 id="org8a70996"><span class="section-number-3">4.6</span> Transmissibility</h3> <h3 id="org42b1b07"><span class="section-number-3">4.6</span> Transmissibility</h3>
<div class="outline-text-3" id="text-4-6"> <div class="outline-text-3" id="text-4-6">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">TR=ST<span class="org-type">*</span>[eye(6); zeros(6)]; <pre class="src src-matlab">TR=ST<span class="org-type">*</span>[eye(6); zeros(6)];
@ -1310,22 +1314,22 @@ ST.OutputName = {<span class="org-string">'ax'</span>;<span class="org-string">'
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span> <pre class="src src-matlab"><span class="org-type">figure</span>
subplot(231) subplot(231)
bodemag(TR(1,1),opts); bodemag(TR(1,1));
subplot(232) subplot(232)
bodemag(TR(2,2),opts); bodemag(TR(2,2));
subplot(233) subplot(233)
bodemag(TR(3,3),opts); bodemag(TR(3,3));
subplot(234) subplot(234)
bodemag(TR(4,4),opts); bodemag(TR(4,4));
subplot(235) subplot(235)
bodemag(TR(5,5),opts); bodemag(TR(5,5));
subplot(236) subplot(236)
bodemag(TR(6,6),opts); bodemag(TR(6,6));
</pre> </pre>
</div> </div>
<div id="org6ce913c" class="figure"> <div id="orgd9b6731" class="figure">
<p><img src="figs/stewart_platform_analytical_transmissibility.png" alt="stewart_platform_analytical_transmissibility.png" /> <p><img src="figs/stewart_platform_analytical_transmissibility.png" alt="stewart_platform_analytical_transmissibility.png" />
</p> </p>
<p><span class="figure-number">Figure 15: </span>Transmissibility</p> <p><span class="figure-number">Figure 15: </span>Transmissibility</p>
@ -1333,8 +1337,8 @@ bodemag(TR(6,6),opts);
</div> </div>
</div> </div>
<div id="outline-container-org12c95c9" class="outline-3"> <div id="outline-container-org38c8159" class="outline-3">
<h3 id="org12c95c9"><span class="section-number-3">4.7</span> Real approximation of \(G(j\omega)\) at decoupling frequency</h3> <h3 id="org38c8159"><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="outline-text-3" id="text-4-7">
<div class="org-src-container"> <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> <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>
@ -1362,8 +1366,8 @@ wf = logspace(<span class="org-type">-</span>1,2,1000);
</div> </div>
</div> </div>
<div id="outline-container-orgc58b81c" class="outline-3"> <div id="outline-container-org477b3ce" class="outline-3">
<h3 id="orgc58b81c"><span class="section-number-3">4.8</span> Coupled and Decoupled Plant &ldquo;Gershgorin Radii&rdquo;</h3> <h3 id="org477b3ce"><span class="section-number-3">4.8</span> Coupled and Decoupled Plant &ldquo;Gershgorin Radii&rdquo;</h3>
<div class="outline-text-3" id="text-4-8"> <div class="outline-text-3" id="text-4-8">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>; <pre class="src src-matlab"><span class="org-type">figure</span>;
@ -1375,7 +1379,7 @@ xlabel(<span class="org-string">'Frequency (Hz)'</span>); ylabel(<span class="or
</div> </div>
<div id="org20fc2fd" class="figure"> <div id="org1416731" class="figure">
<p><img src="figs/gershorin_raddii_coupled_analytical.png" alt="gershorin_raddii_coupled_analytical.png" /> <p><img src="figs/gershorin_raddii_coupled_analytical.png" alt="gershorin_raddii_coupled_analytical.png" />
</p> </p>
<p><span class="figure-number">Figure 16: </span>Gershorin Raddi for the coupled plant</p> <p><span class="figure-number">Figure 16: </span>Gershorin Raddi for the coupled plant</p>
@ -1391,7 +1395,7 @@ xlabel(<span class="org-string">'Frequency (Hz)'</span>); ylabel(<span class="or
</div> </div>
<div id="org586d327" class="figure"> <div id="orgdfe4880" class="figure">
<p><img src="figs/gershorin_raddii_decoupled_analytical.png" alt="gershorin_raddii_decoupled_analytical.png" /> <p><img src="figs/gershorin_raddii_decoupled_analytical.png" alt="gershorin_raddii_decoupled_analytical.png" />
</p> </p>
<p><span class="figure-number">Figure 17: </span>Gershorin Raddi for the decoupled plant</p> <p><span class="figure-number">Figure 17: </span>Gershorin Raddi for the decoupled plant</p>
@ -1399,8 +1403,8 @@ xlabel(<span class="org-string">'Frequency (Hz)'</span>); ylabel(<span class="or
</div> </div>
</div> </div>
<div id="outline-container-org2ba91f6" class="outline-3"> <div id="outline-container-orgde4eec1" class="outline-3">
<h3 id="org2ba91f6"><span class="section-number-3">4.9</span> Decoupled Plant</h3> <h3 id="orgde4eec1"><span class="section-number-3">4.9</span> Decoupled Plant</h3>
<div class="outline-text-3" id="text-4-9"> <div class="outline-text-3" id="text-4-9">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>; <pre class="src src-matlab"><span class="org-type">figure</span>;
@ -1409,7 +1413,7 @@ bodemag(U<span class="org-type">'*</span>sys1<span class="org-type">*</span>V,op
</div> </div>
<div id="org5cd203f" class="figure"> <div id="orge835f28" class="figure">
<p><img src="figs/stewart_platform_analytical_decoupled_plant.png" alt="stewart_platform_analytical_decoupled_plant.png" /> <p><img src="figs/stewart_platform_analytical_decoupled_plant.png" alt="stewart_platform_analytical_decoupled_plant.png" />
</p> </p>
<p><span class="figure-number">Figure 18: </span>Decoupled Plant</p> <p><span class="figure-number">Figure 18: </span>Decoupled Plant</p>
@ -1417,8 +1421,8 @@ bodemag(U<span class="org-type">'*</span>sys1<span class="org-type">*</span>V,op
</div> </div>
</div> </div>
<div id="outline-container-orgc73a283" class="outline-3"> <div id="outline-container-org11b0182" class="outline-3">
<h3 id="orgc73a283"><span class="section-number-3">4.10</span> Controller</h3> <h3 id="org11b0182"><span class="section-number-3">4.10</span> Controller</h3>
<div class="outline-text-3" id="text-4-10"> <div class="outline-text-3" id="text-4-10">
<div class="org-src-container"> <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> <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>
@ -1430,8 +1434,8 @@ cont = eye(6)<span class="org-type">*</span>c_gain<span class="org-type">/</span
</div> </div>
</div> </div>
<div id="outline-container-org9c82ee4" class="outline-3"> <div id="outline-container-org5c893a8" class="outline-3">
<h3 id="org9c82ee4"><span class="section-number-3">4.11</span> Closed Loop System</h3> <h3 id="org5c893a8"><span class="section-number-3">4.11</span> Closed Loop System</h3>
<div class="outline-text-3" id="text-4-11"> <div class="outline-text-3" id="text-4-11">
<div class="org-src-container"> <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> <pre class="src src-matlab">FEEDIN = [7<span class="org-type">:</span>12]; <span class="org-comment">% Input of controller</span>
@ -1459,8 +1463,8 @@ TRsvd = STsvd<span class="org-type">*</span>[eye(6); zeros(6)];
</div> </div>
</div> </div>
<div id="outline-container-org80cd406" class="outline-3"> <div id="outline-container-orgb1c0711" class="outline-3">
<h3 id="org80cd406"><span class="section-number-3">4.12</span> Results</h3> <h3 id="orgb1c0711"><span class="section-number-3">4.12</span> Results</h3>
<div class="outline-text-3" id="text-4-12"> <div class="outline-text-3" id="text-4-12">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span> <pre class="src src-matlab"><span class="org-type">figure</span>
@ -1486,7 +1490,7 @@ legend(<span class="org-string">'OL'</span>,<span class="org-string">'Centralize
</div> </div>
<div id="orgfadf6e5" class="figure"> <div id="orgb680082" class="figure">
<p><img src="figs/stewart_platform_analytical_svd_cen_comp.png" alt="stewart_platform_analytical_svd_cen_comp.png" /> <p><img src="figs/stewart_platform_analytical_svd_cen_comp.png" alt="stewart_platform_analytical_svd_cen_comp.png" />
</p> </p>
<p><span class="figure-number">Figure 19: </span>Comparison of the obtained transmissibility for the centralized control and the SVD control</p> <p><span class="figure-number">Figure 19: </span>Comparison of the obtained transmissibility for the centralized control and the SVD control</p>
@ -1497,7 +1501,7 @@ legend(<span class="org-string">'OL'</span>,<span class="org-string">'Centralize
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2020-10-09 ven. 16:21</p> <p class="date">Created: 2020-10-13 mar. 14:53</p>
</div> </div>
</body> </body>
</html> </html>

10
index.org
View File

@ -686,6 +686,9 @@ This Matlab function is accessible [[file:gravimeter/pzmap_testCL.m][here]].
#+end_src #+end_src
* Stewart Platform - Simscape Model * Stewart Platform - Simscape Model
:PROPERTIES:
:header-args:matlab+: :tangle stewart_platform/simscape_model.m
:END:
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
@ -700,6 +703,10 @@ This Matlab function is accessible [[file:gravimeter/pzmap_testCL.m][here]].
addpath('stewart_platform/STEP'); addpath('stewart_platform/STEP');
#+end_src #+end_src
#+begin_src matlab :eval no
addpath('STEP');
#+end_src
** Jacobian ** Jacobian
First, the position of the "joints" (points of force application) are estimated and the Jacobian computed. First, the position of the "joints" (points of force application) are estimated and the Jacobian computed.
#+begin_src matlab #+begin_src matlab
@ -1292,6 +1299,9 @@ The obtained transmissibility in Open-loop, for the centralized control as well
[[file:figs/stewart_platform_simscape_cl_transmissibility.png]] [[file:figs/stewart_platform_simscape_cl_transmissibility.png]]
* Stewart Platform - Analytical Model * Stewart Platform - Analytical Model
:PROPERTIES:
:header-args:matlab+: :tangle stewart_platform/analytical_model.m
:END:
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>

196
stewart_platform/analytical_model.m Normal file
View File

@ -0,0 +1,196 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
%% Bode plot options
opts = bodeoptions('cstprefs');
opts.FreqUnits = 'Hz';
opts.MagUnits = 'abs';
opts.MagScale = 'log';
opts.PhaseWrapping = 'on';
opts.xlim = [1 1000];
% Characteristics
L = 0.055; % Leg length [m]
Zc = 0; % ?
m = 0.2; % Top platform mass [m]
k = 1e3; % Total vertical stiffness [N/m]
c = 2*0.1*sqrt(k*m); % Damping ? [N/(m/s)]
Rx = 0.04; % ?
Rz = 0.04; % ?
Ix = m*Rx^2; % ?
Iy = m*Rx^2; % ?
Iz = m*Rz^2; % ?
% Mass Matrix
M = m*[1 0 0 0 Zc 0;
0 1 0 -Zc 0 0;
0 0 1 0 0 0;
0 -Zc 0 Rx^2+Zc^2 0 0;
Zc 0 0 0 Rx^2+Zc^2 0;
0 0 0 0 0 Rz^2];
% Jacobian Matrix
Bj=1/sqrt(6)*[ 1 1 -2 1 1 -2;
sqrt(3) -sqrt(3) 0 sqrt(3) -sqrt(3) 0;
sqrt(2) sqrt(2) sqrt(2) sqrt(2) sqrt(2) sqrt(2);
0 0 L L -L -L;
-L*2/sqrt(3) -L*2/sqrt(3) L/sqrt(3) L/sqrt(3) L/sqrt(3) L/sqrt(3);
L*sqrt(2) -L*sqrt(2) L*sqrt(2) -L*sqrt(2) L*sqrt(2) -L*sqrt(2)];
% Stifnness and Damping matrices
kv = k/3; % Vertical Stiffness of the springs [N/m]
kh = 0.5*k/3; % Horizontal Stiffness of the springs [N/m]
K = diag([3*kh, 3*kh, 3*kv, 3*kv*Rx^2/2, 3*kv*Rx^2/2, 3*kh*Rx^2]); % Stiffness Matrix
C = c*K/100000; % Damping Matrix
% State Space System
A = [ zeros(6) eye(6); ...
-M\K -M\C];
Bw = [zeros(6); -eye(6)];
Bu = [zeros(6); M\Bj];
Co = [-M\K -M\C];
D = [zeros(6) M\Bj];
ST = ss(A,[Bw Bu],Co,D);
% - OUT 1-6: 6 dof
% - IN 1-6 : ground displacement in the directions of the legs
% - IN 7-12: forces in the actuators.
ST.StateName = {'x';'y';'z';'theta_x';'theta_y';'theta_z';...
'dx';'dy';'dz';'dtheta_x';'dtheta_y';'dtheta_z'};
ST.InputName = {'w1';'w2';'w3';'w4';'w5';'w6';...
'u1';'u2';'u3';'u4';'u5';'u6'};
ST.OutputName = {'ax';'ay';'az';'atheta_x';'atheta_y';'atheta_z'};
% Transmissibility
TR=ST*[eye(6); zeros(6)];
figure
subplot(231)
bodemag(TR(1,1));
subplot(232)
bodemag(TR(2,2));
subplot(233)
bodemag(TR(3,3));
subplot(234)
bodemag(TR(4,4));
subplot(235)
bodemag(TR(5,5));
subplot(236)
bodemag(TR(6,6));
% Real approximation of $G(j\omega)$ at decoupling frequency
sys1 = ST*[zeros(6); eye(6)]; % take only the forces inputs
dec_fr = 20;
H1 = evalfr(sys1,j*2*pi*dec_fr);
H2 = H1;
D = pinv(real(H2'*H2));
H1 = inv(D*real(H2'*diag(exp(j*angle(diag(H2*D*H2.'))/2)))) ;
[U,S,V] = svd(H1);
wf = logspace(-1,2,1000);
for i = 1:length(wf)
H = abs(evalfr(sys1,j*2*pi*wf(i)));
H_dec = abs(evalfr(U'*sys1*V,j*2*pi*wf(i)));
for j = 1:size(H,2)
g_r1(i,j) = (sum(H(j,:))-H(j,j))/H(j,j);
g_r2(i,j) = (sum(H_dec(j,:))-H_dec(j,j))/H_dec(j,j);
% keyboard
end
g_lim(i) = 0.5;
end
% Coupled and Decoupled Plant "Gershgorin Radii"
figure;
title('Coupled plant')
loglog(wf,g_r1(:,1),wf,g_r1(:,2),wf,g_r1(:,3),wf,g_r1(:,4),wf,g_r1(:,5),wf,g_r1(:,6),wf,g_lim,'--');
legend('$a_x$','$a_y$','$a_z$','$\theta_x$','$\theta_y$','$\theta_z$','Limit');
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
% #+name: fig:gershorin_raddii_coupled_analytical
% #+caption: Gershorin Raddi for the coupled plant
% #+RESULTS:
% [[file:figs/gershorin_raddii_coupled_analytical.png]]
figure;
title('Decoupled plant (10 Hz)')
loglog(wf,g_r2(:,1),wf,g_r2(:,2),wf,g_r2(:,3),wf,g_r2(:,4),wf,g_r2(:,5),wf,g_r2(:,6),wf,g_lim,'--');
legend('$S_1$','$S_2$','$S_3$','$S_4$','$S_5$','$S_6$','Limit');
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
% Decoupled Plant
figure;
bodemag(U'*sys1*V,opts)
% Controller
fc = 2*pi*0.1; % Crossover Frequency [rad/s]
c_gain = 50; %
cont = eye(6)*c_gain/(s+fc);
% Closed Loop System
FEEDIN = [7:12]; % Input of controller
FEEDOUT = [1:6]; % Output of controller
% Centralized Control
STcen = feedback(ST, inv(Bj)*cont, FEEDIN, FEEDOUT);
TRcen = STcen*[eye(6); zeros(6)];
% SVD Control
STsvd = feedback(ST, pinv(V')*cont*pinv(U), FEEDIN, FEEDOUT);
TRsvd = STsvd*[eye(6); zeros(6)];
% Results
figure
subplot(231)
bodemag(TR(1,1),TRcen(1,1),TRsvd(1,1),opts)
legend('OL','Centralized','SVD')
subplot(232)
bodemag(TR(2,2),TRcen(2,2),TRsvd(2,2),opts)
legend('OL','Centralized','SVD')
subplot(233)
bodemag(TR(3,3),TRcen(3,3),TRsvd(3,3),opts)
legend('OL','Centralized','SVD')
subplot(234)
bodemag(TR(4,4),TRcen(4,4),TRsvd(4,4),opts)
legend('OL','Centralized','SVD')
subplot(235)
bodemag(TR(5,5),TRcen(5,5),TRsvd(5,5),opts)
legend('OL','Centralized','SVD')
subplot(236)
bodemag(TR(6,6),TRcen(6,6),TRsvd(6,6),opts)
legend('OL','Centralized','SVD')

BIN
stewart_platform/drone_platform.slx
View File

Binary file not shown.

BIN
stewart_platform/drone_platform_R2017b.slx Normal file
View File

Binary file not shown.

499
stewart_platform/simscape_model.m Normal file
View File

@ -0,0 +1,499 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
addpath('STEP');
% Jacobian
% First, the position of the "joints" (points of force application) are estimated and the Jacobian computed.
open('drone_platform_jacobian.slx');
sim('drone_platform_jacobian');
Aa = [a1.Data(1,:);
a2.Data(1,:);
a3.Data(1,:);
a4.Data(1,:);
a5.Data(1,:);
a6.Data(1,:)]';
Ab = [b1.Data(1,:);
b2.Data(1,:);
b3.Data(1,:);
b4.Data(1,:);
b5.Data(1,:);
b6.Data(1,:)]';
As = (Ab - Aa)./vecnorm(Ab - Aa);
l = vecnorm(Ab - Aa)';
J = [As' , cross(Ab, As)'];
save('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
% Simscape Model
open('drone_platform.slx');
% Definition of spring parameters
kx = 0.5*1e3/3; % [N/m]
ky = 0.5*1e3/3;
kz = 1e3/3;
cx = 0.025; % [Nm/rad]
cy = 0.025;
cz = 0.025;
% We load the Jacobian.
load('./jacobian.mat', 'Aa', 'Ab', 'As', 'l', 'J');
% Identification of the plant
% The dynamics is identified from forces applied by each legs to the measured acceleration of the top platform.
%% Name of the Simulink File
mdl = 'drone_platform';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Dw'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/u'], 1, 'openinput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Inertial Sensor'], 1, 'openoutput'); io_i = io_i + 1;
G = linearize(mdl, io);
G.InputName = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'};
% There are 24 states (6dof for the bottom platform + 6dof for the top platform).
size(G)
% #+RESULTS:
% : State-space model with 6 outputs, 12 inputs, and 24 states.
% G = G*blkdiag(inv(J), eye(6));
% G.InputName = {'Dw1', 'Dw2', 'Dw3', 'Dw4', 'Dw5', 'Dw6', ...
% 'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
% Thanks to the Jacobian, we compute the transfer functions in the frame of the legs and in an inertial frame.
Gx = G*blkdiag(eye(6), inv(J'));
Gx.InputName = {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz', ...
'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gl = J*G;
Gl.OutputName = {'A1', 'A2', 'A3', 'A4', 'A5', 'A6'};
% Obtained Dynamics
freqs = logspace(-1, 2, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))), 'DisplayName', '$A_x/F_x$');
plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))), 'DisplayName', '$A_y/F_y$');
plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))), 'DisplayName', '$A_z/F_z$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ax', 'Fx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ay', 'Fy'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Az', 'Fz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
% #+name: fig:stewart_platform_translations
% #+caption: Stewart Platform Plant from forces applied by the legs to the acceleration of the platform
% #+RESULTS:
% [[file:figs/stewart_platform_translations.png]]
freqs = logspace(-1, 2, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))), 'DisplayName', '$A_{R_x}/M_x$');
plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))), 'DisplayName', '$A_{R_y}/M_y$');
plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))), 'DisplayName', '$A_{R_z}/M_z$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [rad/(Nm)]'); set(gca, 'XTickLabel',[]);
legend('location', 'southeast');
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arx', 'Mx'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Ary', 'My'), freqs, 'Hz'))));
plot(freqs, 180/pi*angle(squeeze(freqresp(Gx('Arz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
% #+name: fig:stewart_platform_rotations
% #+caption: Stewart Platform Plant from torques applied by the legs to the angular acceleration of the platform
% #+RESULTS:
% [[file:figs/stewart_platform_rotations.png]]
freqs = logspace(-1, 2, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for out_i = 1:5
for in_i = i+1:6
plot(freqs, abs(squeeze(freqresp(Gl(sprintf('A%i', out_i), sprintf('F%i', in_i)), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
end
end
for ch_i = 1:6
plot(freqs, abs(squeeze(freqresp(Gl(sprintf('A%i', ch_i), sprintf('F%i', ch_i)), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for ch_i = 1:6
plot(freqs, 180/pi*angle(squeeze(freqresp(Gl(sprintf('A%i', ch_i), sprintf('F%i', ch_i)), freqs, 'Hz'))));
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-360:90:360]);
linkaxes([ax1,ax2],'x');
% #+name: fig:stewart_platform_legs
% #+caption: Stewart Platform Plant from forces applied by the legs to displacement of the legs
% #+RESULTS:
% [[file:figs/stewart_platform_legs.png]]
freqs = logspace(-1, 2, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
% plot(freqs, abs(squeeze(freqresp(Gx('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', '$D_x/D_{w,x}$');
% plot(freqs, abs(squeeze(freqresp(Gx('Ay', 'Dwy')/s^2, freqs, 'Hz'))), 'DisplayName', '$D_y/D_{w,y}$');
% plot(freqs, abs(squeeze(freqresp(Gx('Az', 'Dwz')/s^2, freqs, 'Hz'))), 'DisplayName', '$D_z/D_{w,z}$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(TR(1,1), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$');
plot(freqs, abs(squeeze(freqresp(TR(2,2), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$');
plot(freqs, abs(squeeze(freqresp(TR(3,3), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - Translations'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
ax2 = subplot(2, 1, 2);
hold on;
% plot(freqs, abs(squeeze(freqresp(Gx('Arx', 'Rwx')/s^2, freqs, 'Hz'))), 'DisplayName', '$R_x/R_{w,x}$');
% plot(freqs, abs(squeeze(freqresp(Gx('Ary', 'Rwy')/s^2, freqs, 'Hz'))), 'DisplayName', '$R_y/R_{w,y}$');
% plot(freqs, abs(squeeze(freqresp(Gx('Arz', 'Rwz')/s^2, freqs, 'Hz'))), 'DisplayName', '$R_z/R_{w,z}$');
set(gca,'ColorOrderIndex',1)
plot(freqs, abs(squeeze(freqresp(TR(4,4), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$');
plot(freqs, abs(squeeze(freqresp(TR(5,5), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$');
plot(freqs, abs(squeeze(freqresp(TR(6,6), freqs, 'Hz'))), '--', 'DisplayName', '$D_x/D_{w,x}$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - Rotations'); xlabel('Frequency [Hz]');
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
% Real Approximation of $G$ at the decoupling frequency
% Let's compute a real approximation of the complex matrix $H_1$ which corresponds to the the transfer function $G_c(j\omega_c)$ from forces applied by the actuators to the measured acceleration of the top platform evaluated at the frequency $\omega_c$.
wc = 2*pi*20; % Decoupling frequency [rad/s]
Gc = G({'Ax', 'Ay', 'Az', 'Arx', 'Ary', 'Arz'}, ...
{'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}); % Transfer function to find a real approximation
H1 = evalfr(Gc, j*wc);
% The real approximation is computed as follows:
D = pinv(real(H1'*H1));
H1 = inv(D*real(H1'*diag(exp(j*angle(diag(H1*D*H1.'))/2))));
% Verification of the decoupling using the "Gershgorin Radii"
% First, the Singular Value Decomposition of $H_1$ is performed:
% \[ H_1 = U \Sigma V^H \]
[U,S,V] = svd(H1);
% Then, the "Gershgorin Radii" is computed for the plant $G_c(s)$ and the "SVD Decoupled Plant" $G_d(s)$:
% \[ G_d(s) = U^T G_c(s) V \]
% This is computed over the following frequencies.
freqs = logspace(-2, 2, 1000); % [Hz]
% Gershgorin Radii for the coupled plant:
Gr_coupled = zeros(length(freqs), size(Gc,2));
H = abs(squeeze(freqresp(Gc, freqs, 'Hz')));
for out_i = 1:size(Gc,2)
Gr_coupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
% Gershgorin Radii for the decoupled plant using SVD:
Gd = U'*Gc*V;
Gr_decoupled = zeros(length(freqs), size(Gd,2));
H = abs(squeeze(freqresp(Gd, freqs, 'Hz')));
for out_i = 1:size(Gd,2)
Gr_decoupled(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
% Gershgorin Radii for the decoupled plant using the Jacobian:
Gj = Gc*inv(J');
Gr_jacobian = zeros(length(freqs), size(Gj,2));
H = abs(squeeze(freqresp(Gj, freqs, 'Hz')));
for out_i = 1:size(Gj,2)
Gr_jacobian(:, out_i) = squeeze((sum(H(out_i,:,:)) - H(out_i,out_i,:))./H(out_i, out_i, :));
end
figure;
hold on;
plot(freqs, Gr_coupled(:,1), 'DisplayName', 'Coupled');
plot(freqs, Gr_decoupled(:,1), 'DisplayName', 'SVD');
plot(freqs, Gr_jacobian(:,1), 'DisplayName', 'Jacobian');
for in_i = 2:6
set(gca,'ColorOrderIndex',1)
plot(freqs, Gr_coupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',2)
plot(freqs, Gr_decoupled(:,in_i), 'HandleVisibility', 'off');
set(gca,'ColorOrderIndex',3)
plot(freqs, Gr_jacobian(:,in_i), 'HandleVisibility', 'off');
end
plot(freqs, 0.5*ones(size(freqs)), 'k--', 'DisplayName', 'Limit')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
hold off;
xlabel('Frequency (Hz)'); ylabel('Gershgorin Radii')
legend('location', 'northeast');
% Decoupled Plant
% Let's see the bode plot of the decoupled plant $G_d(s)$.
% \[ G_d(s) = U^T G_c(s) V \]
freqs = logspace(-1, 2, 1000);
figure;
hold on;
for ch_i = 1:6
plot(freqs, abs(squeeze(freqresp(Gd(ch_i, ch_i), freqs, 'Hz'))), ...
'DisplayName', sprintf('$G(%i, %i)$', ch_i, ch_i));
end
for in_i = 1:5
for out_i = in_i+1:6
plot(freqs, abs(squeeze(freqresp(Gd(out_i, in_i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
legend('location', 'southeast');
% #+name: fig:simscape_model_decoupled_plant_svd
% #+caption: Decoupled Plant using SVD
% #+RESULTS:
% [[file:figs/simscape_model_decoupled_plant_svd.png]]
freqs = logspace(-1, 2, 1000);
figure;
hold on;
for ch_i = 1:6
plot(freqs, abs(squeeze(freqresp(Gj(ch_i, ch_i), freqs, 'Hz'))), ...
'DisplayName', sprintf('$G(%i, %i)$', ch_i, ch_i));
end
for in_i = 1:5
for out_i = in_i+1:6
plot(freqs, abs(squeeze(freqresp(Gj(out_i, in_i), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
'HandleVisibility', 'off');
end
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude'); xlabel('Frequency [Hz]');
legend('location', 'southeast');
% Diagonal Controller
% The controller $K$ is a diagonal controller consisting a low pass filters with a crossover frequency $\omega_c$ and a DC gain $C_g$.
wc = 2*pi*0.1; % Crossover Frequency [rad/s]
C_g = 50; % DC Gain
K = eye(6)*C_g/(s+wc);
% #+RESULTS:
% [[file:figs/centralized_control.png]]
G_cen = feedback(G, inv(J')*K, [7:12], [1:6]);
% #+RESULTS:
% [[file:figs/svd_control.png]]
% SVD Control
G_svd = feedback(G, pinv(V')*K*pinv(U), [7:12], [1:6]);
% Results
% Let's first verify the stability of the closed-loop systems:
isstable(G_cen)
% #+RESULTS:
% : ans =
% : logical
% : 1
isstable(G_svd)
% #+RESULTS:
% : ans =
% : logical
% : 0
% The obtained transmissibility in Open-loop, for the centralized control as well as for the SVD control are shown in Figure [[fig:stewart_platform_simscape_cl_transmissibility]].
freqs = logspace(-3, 3, 1000);
figure
ax1 = subplot(2, 3, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Open-Loop');
plot(freqs, abs(squeeze(freqresp(G_cen('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'Centralized');
plot(freqs, abs(squeeze(freqresp(G_svd('Ax', 'Dwx')/s^2, freqs, 'Hz'))), 'DisplayName', 'SVD');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - $D_x/D_{w,x}$'); xlabel('Frequency [Hz]');
legend('location', 'southwest');
ax2 = subplot(2, 3, 2);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Ay', 'Dwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Ay', 'Dwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Ay', 'Dwy')/s^2, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - $D_y/D_{w,y}$'); xlabel('Frequency [Hz]');
ax3 = subplot(2, 3, 3);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Az', 'Dwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Az', 'Dwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Az', 'Dwz')/s^2, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - $D_z/D_{w,z}$'); xlabel('Frequency [Hz]');
ax4 = subplot(2, 3, 4);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arx', 'Rwx')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arx', 'Rwx')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Arx', 'Rwx')/s^2, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - $R_x/R_{w,x}$'); xlabel('Frequency [Hz]');
ax5 = subplot(2, 3, 5);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Ary', 'Rwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Ary', 'Rwy')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Ary', 'Rwy')/s^2, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - $R_y/R_{w,y}$'); xlabel('Frequency [Hz]');
ax6 = subplot(2, 3, 6);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Arz', 'Rwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_cen('Arz', 'Rwz')/s^2, freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(G_svd('Arz', 'Rwz')/s^2, freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Transmissibility - $R_z/R_{w,z}$'); xlabel('Frequency [Hz]');
linkaxes([ax1,ax2,ax3,ax4,ax5,ax6],'x');
xlim([freqs(1), freqs(end)]);