Add analysis about virtual mass addition
This commit is contained in:
parent
f868ad68f0
commit
6522211e01
251
docs/control_virtual_mass.html
Normal file
251
docs/control_virtual_mass.html
Normal file
@ -0,0 +1,251 @@
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<?xml version="1.0" encoding="utf-8"?>
|
||||
<?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-04-17 ven. 14:10 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<title>Decentralize control to add virtual mass</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"/>
|
||||
<script src="./js/jquery.min.js"></script>
|
||||
<script src="./js/bootstrap.min.js"></script>
|
||||
<script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
<script src="./js/readtheorg.js"></script>
|
||||
<script>MathJax = {
|
||||
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>
|
||||
</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">Decentralize control to add virtual mass</h1>
|
||||
<div id="table-of-contents">
|
||||
<h2>Table of Contents</h2>
|
||||
<div id="text-table-of-contents">
|
||||
<ul>
|
||||
<li><a href="#org982b263">1. Initialization</a></li>
|
||||
<li><a href="#org35a3822">2. Identification</a>
|
||||
<ul>
|
||||
<li><a href="#org33f35d2">2.1. Identification of the transfer function from \(\tau\) to \(d\mathcal{L}\)</a></li>
|
||||
<li><a href="#org6663ed2">2.2. Identification of the Primary plant without virtual add of mass</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgd6fc719">3. Adding Virtual Mass in the Leg’s Space</a>
|
||||
<ul>
|
||||
<li><a href="#orgc37faa7">3.1. Plant</a></li>
|
||||
<li><a href="#org4ae3263">3.2. Controller Design</a></li>
|
||||
<li><a href="#orgb270293">3.3. Identification of the Primary Plant</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#orgc9131d0">4. Adding Virtual Mass in the Task Space</a>
|
||||
<ul>
|
||||
<li><a href="#org9ed2d4c">4.1. Plant</a></li>
|
||||
<li><a href="#org4f03a34">4.2. Controller Design</a></li>
|
||||
<li><a href="#org2fe0ce0">4.3. Identification of the Primary Plant</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org982b263" class="outline-2">
|
||||
<h2 id="org982b263"><span class="section-number-2">1</span> Initialization</h2>
|
||||
<div class="outline-text-2" id="text-1">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
|
||||
initializeSimscapeConfiguration();
|
||||
initializeDisturbances(<span class="org-string">'enable'</span>, <span class="org-constant">false</span>);
|
||||
initializeLoggingConfiguration(<span class="org-string">'log'</span>, <span class="org-string">'none'</span>);
|
||||
|
||||
initializeController(<span class="org-string">'type'</span>, <span class="org-string">'hac-dvf'</span>);
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We set the stiffness of the payload fixation:
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">Kp = 1e8; <span class="org-comment">% [N/m]</span>
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org35a3822" class="outline-2">
|
||||
<h2 id="org35a3822"><span class="section-number-2">2</span> Identification</h2>
|
||||
<div class="outline-text-2" id="text-2">
|
||||
</div>
|
||||
<div id="outline-container-org33f35d2" class="outline-3">
|
||||
<h3 id="org33f35d2"><span class="section-number-3">2.1</span> Identification of the transfer function from \(\tau\) to \(d\mathcal{L}\)</h3>
|
||||
<div class="outline-text-3" id="text-2-1">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">K = tf(zeros(6));
|
||||
Kdvf = tf(zeros(6));
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
We identify the system for the following payload masses:
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">Ms = [1, 10, 50];
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
The nano-hexapod has the following leg’s stiffness and damping.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">initializeNanoHexapod(<span class="org-string">'k'</span>, 1e5, <span class="org-string">'c'</span>, 2e2);
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org6663ed2" class="outline-3">
|
||||
<h3 id="org6663ed2"><span class="section-number-3">2.2</span> Identification of the Primary plant without virtual add of mass</h3>
|
||||
</div>
|
||||
</div>
|
||||
<div id="outline-container-orgd6fc719" class="outline-2">
|
||||
<h2 id="orgd6fc719"><span class="section-number-2">3</span> Adding Virtual Mass in the Leg’s Space</h2>
|
||||
<div class="outline-text-2" id="text-3">
|
||||
</div>
|
||||
<div id="outline-container-orgc37faa7" class="outline-3">
|
||||
<h3 id="orgc37faa7"><span class="section-number-3">3.1</span> Plant</h3>
|
||||
<div class="outline-text-3" id="text-3-1">
|
||||
|
||||
<div id="org98e7ba8" class="figure">
|
||||
<p><img src="figs/virtual_mass_plant_L.png" alt="virtual_mass_plant_L.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 1: </span>Transfer function from \(\tau_i\) to \(d\mathcal{L}_i\) for three payload masses</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4ae3263" class="outline-3">
|
||||
<h3 id="org4ae3263"><span class="section-number-3">3.2</span> Controller Design</h3>
|
||||
<div class="outline-text-3" id="text-3-2">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">Kdvf = 10<span class="org-type">*</span>s<span class="org-type">^</span>2<span class="org-type">/</span>(1<span class="org-type">+</span>s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>500)<span class="org-type">^</span>2<span class="org-type">*</span>eye(6);
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="orgccb3b9e" class="figure">
|
||||
<p><img src="figs/virtual_mass_loop_gain_L.png" alt="virtual_mass_loop_gain_L.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 2: </span>Loop Gain for the addition of virtual mass in the leg’s space</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgb270293" class="outline-3">
|
||||
<h3 id="orgb270293"><span class="section-number-3">3.3</span> Identification of the Primary Plant</h3>
|
||||
<div class="outline-text-3" id="text-3-3">
|
||||
|
||||
<div id="orgd49505e" class="figure">
|
||||
<p><img src="figs/virtual_mass_L_primary_plant_X.png" alt="virtual_mass_L_primary_plant_X.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 3: </span>Comparison of the transfer function from \(\mathcal{F}_{x,y,z}\) to \(\mathcal{X}_{x,y,z}\) with and without the virtual addition of mass in the leg’s space</p>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="org2281744" class="figure">
|
||||
<p><img src="figs/virtual_mass_L_primary_plant_L.png" alt="virtual_mass_L_primary_plant_L.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 4: </span>Comparison of the transfer function from \(\tau_i\) to \(\mathcal{L}_{i}\) with and without the virtual addition of mass in the leg’s space</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-orgc9131d0" class="outline-2">
|
||||
<h2 id="orgc9131d0"><span class="section-number-2">4</span> Adding Virtual Mass in the Task Space</h2>
|
||||
<div class="outline-text-2" id="text-4">
|
||||
</div>
|
||||
<div id="outline-container-org9ed2d4c" class="outline-3">
|
||||
<h3 id="org9ed2d4c"><span class="section-number-3">4.1</span> Plant</h3>
|
||||
<div class="outline-text-3" id="text-4-1">
|
||||
<p>
|
||||
Let’s look at the transfer function from \(\bm{\mathcal{F}}\) to \(d\bm{\mathcal{X}}\):
|
||||
\[ \frac{d\bm{\mathcal{L}}}{\bm{\mathcal{F}}} = \bm{J}^{-1} \frac{d\bm{\mathcal{L}}}{\bm{\tau}} \bm{J}^{-T} \]
|
||||
</p>
|
||||
|
||||
|
||||
<div id="org6488b4c" class="figure">
|
||||
<p><img src="figs/virtual_mass_plant_X.png" alt="virtual_mass_plant_X.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 5: </span>Dynamics from \(\mathcal{F}_{x,y,z}\) to \(\mathcal{X}_{x,y,z}\) used for virtual mass addition in the task space</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org4f03a34" class="outline-3">
|
||||
<h3 id="org4f03a34"><span class="section-number-3">4.2</span> Controller Design</h3>
|
||||
<div class="outline-text-3" id="text-4-2">
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">KmX = (s<span class="org-type">^</span>2<span class="org-type">*</span>1<span class="org-type">/</span>(1<span class="org-type">+</span>s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>500)<span class="org-type">^</span>2<span class="org-type">*</span>diag([1 1 50 0 0 0]));
|
||||
</pre>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="orgf411330" class="figure">
|
||||
<p><img src="figs/virtual_mass_loop_gain_X.png" alt="virtual_mass_loop_gain_X.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 6: </span>Loop gain for virtual mass addition in the task space</p>
|
||||
</div>
|
||||
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">Kdvf = inv(nano_hexapod.J<span class="org-type">'</span>)<span class="org-type">*</span>KmX<span class="org-type">*</span>inv(nano_hexapod.J);
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org2fe0ce0" class="outline-3">
|
||||
<h3 id="org2fe0ce0"><span class="section-number-3">4.3</span> Identification of the Primary Plant</h3>
|
||||
<div class="outline-text-3" id="text-4-3">
|
||||
|
||||
<div id="orge1df87b" class="figure">
|
||||
<p><img src="figs/virtual_mass_X_primary_plant_X.png" alt="virtual_mass_X_primary_plant_X.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 7: </span>Comparison of the transfer function from \(\mathcal{F}_{x,y,z}\) to \(\mathcal{X}_{x,y,z}\) with and without the virtual addition of mass in the task space</p>
|
||||
</div>
|
||||
|
||||
|
||||
<div id="org647b748" class="figure">
|
||||
<p><img src="figs/virtual_mass_X_primary_plant_L.png" alt="virtual_mass_X_primary_plant_L.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 8: </span>Comparison of the transfer function from \(\tau_i\) to \(\mathcal{L}_{i}\) with and without the virtual addition of mass in the task space</p>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2020-04-17 ven. 14:10</p>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
BIN
docs/figs/virtual_mass_L_primary_plant_L.pdf
Normal file
BIN
docs/figs/virtual_mass_L_primary_plant_L.pdf
Normal file
Binary file not shown.
BIN
docs/figs/virtual_mass_L_primary_plant_L.png
Normal file
BIN
docs/figs/virtual_mass_L_primary_plant_L.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 168 KiB |
BIN
docs/figs/virtual_mass_L_primary_plant_X.pdf
Normal file
BIN
docs/figs/virtual_mass_L_primary_plant_X.pdf
Normal file
Binary file not shown.
BIN
docs/figs/virtual_mass_L_primary_plant_X.png
Normal file
BIN
docs/figs/virtual_mass_L_primary_plant_X.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 257 KiB |
BIN
docs/figs/virtual_mass_X_primary_plant_L.pdf
Normal file
BIN
docs/figs/virtual_mass_X_primary_plant_L.pdf
Normal file
Binary file not shown.
BIN
docs/figs/virtual_mass_X_primary_plant_L.png
Normal file
BIN
docs/figs/virtual_mass_X_primary_plant_L.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 168 KiB |
BIN
docs/figs/virtual_mass_X_primary_plant_X.pdf
Normal file
BIN
docs/figs/virtual_mass_X_primary_plant_X.pdf
Normal file
Binary file not shown.
BIN
docs/figs/virtual_mass_X_primary_plant_X.png
Normal file
BIN
docs/figs/virtual_mass_X_primary_plant_X.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 255 KiB |
BIN
docs/figs/virtual_mass_loop_gain_L.pdf
Normal file
BIN
docs/figs/virtual_mass_loop_gain_L.pdf
Normal file
Binary file not shown.
BIN
docs/figs/virtual_mass_loop_gain_L.png
Normal file
BIN
docs/figs/virtual_mass_loop_gain_L.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 164 KiB |
2911
docs/figs/virtual_mass_loop_gain_X.pdf
Normal file
2911
docs/figs/virtual_mass_loop_gain_X.pdf
Normal file
File diff suppressed because it is too large
Load Diff
BIN
docs/figs/virtual_mass_loop_gain_X.png
Normal file
BIN
docs/figs/virtual_mass_loop_gain_X.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 220 KiB |
1533
docs/figs/virtual_mass_plant_L.pdf
Normal file
1533
docs/figs/virtual_mass_plant_L.pdf
Normal file
File diff suppressed because it is too large
Load Diff
BIN
docs/figs/virtual_mass_plant_L.png
Normal file
BIN
docs/figs/virtual_mass_plant_L.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 150 KiB |
3255
docs/figs/virtual_mass_plant_X.pdf
Normal file
3255
docs/figs/virtual_mass_plant_X.pdf
Normal file
File diff suppressed because it is too large
Load Diff
BIN
docs/figs/virtual_mass_plant_X.png
Normal file
BIN
docs/figs/virtual_mass_plant_X.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 248 KiB |
@ -4,7 +4,7 @@
|
||||
"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-04-17 ven. 10:25 -->
|
||||
<!-- 2020-04-17 ven. 14:10 -->
|
||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||
<title>Control of the NASS with optimal stiffness</title>
|
||||
<meta name="generator" content="Org mode" />
|
||||
@ -42,7 +42,7 @@
|
||||
<li><a href="#orgfef1a3f">1.3. Controller Design</a></li>
|
||||
<li><a href="#org3c73014">1.4. Effect of the Low Authority Control on the Primary Plant</a></li>
|
||||
<li><a href="#orgee5dbee">1.5. Effect of the Low Authority Control on the Sensibility to Disturbances</a></li>
|
||||
<li><a href="#org27f255e">1.6. Conclusion</a></li>
|
||||
<li><a href="#org882e1ac">1.6. Conclusion</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org81dc0a8">2. Primary Control in the leg space</a>
|
||||
@ -52,7 +52,7 @@
|
||||
<li><a href="#org16d192f">2.3. Sensibility to Disturbances and Noise Budget</a></li>
|
||||
<li><a href="#org84f68cc">2.4. Simulations</a></li>
|
||||
<li><a href="#orgbeadec8">2.5. Results</a></li>
|
||||
<li><a href="#org87fa1ac">2.6. Conclusion</a></li>
|
||||
<li><a href="#orgd61852c">2.6. Conclusion</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org9bd2bf8">3. Primary Control in the task space</a>
|
||||
@ -64,7 +64,7 @@
|
||||
</ul>
|
||||
</li>
|
||||
<li><a href="#org57e2cfd">3.3. Simulation</a></li>
|
||||
<li><a href="#org882e1ac">3.4. Conclusion</a></li>
|
||||
<li><a href="#org8c0882d">3.4. Conclusion</a></li>
|
||||
</ul>
|
||||
</li>
|
||||
</ul>
|
||||
@ -207,6 +207,12 @@ Then, we compute the transfer function from forces applied by the actuators \(\b
|
||||
The obtained dynamics is shown in Figure <a href="#org45c1265">5</a>.
|
||||
</p>
|
||||
|
||||
<div class="important">
|
||||
<p>
|
||||
A zero with a positive real part is introduced in the transfer function from \(\mathcal{F}_y\) to \(\mathcal{X}_y\) after Decentralized Direct Velocity Feedback is applied.
|
||||
</p>
|
||||
|
||||
</div>
|
||||
|
||||
<p>
|
||||
And we compute the transfer function from actuator forces \(\bm{\tau}\) to position error of each leg \(\bm{\epsilon}_\mathcal{L}\):
|
||||
@ -237,9 +243,15 @@ The coupling does not change a lot with DVF.
|
||||
|
||||
<p>
|
||||
The coupling in the space of the legs \(\bm{G}_\mathcal{L}\) are shown in Figure <a href="#orgc43d759">8</a>.
|
||||
The magnitude of the coupling around the resonance of the nano-hexapod (where the coupling is the highest) is considerably reduced when DVF is applied.
|
||||
</p>
|
||||
|
||||
<div class="important">
|
||||
<p>
|
||||
The magnitude of the coupling between \(\tau_i\) and \(d\mathcal{L}_j\) (Figure <a href="#orgc43d759">8</a>) around the resonance of the nano-hexapod (where the coupling is the highest) is considerably reduced when DVF is applied.
|
||||
</p>
|
||||
|
||||
</div>
|
||||
|
||||
|
||||
<div id="orgbb4e497" class="figure">
|
||||
<p><img src="figs/opt_stiff_primary_plant_damped_coupling_X.png" alt="opt_stiff_primary_plant_damped_coupling_X.png" />
|
||||
@ -286,12 +298,18 @@ The norm of these transfer functions are shown in Figure <a href="#org199898b">9
|
||||
<p><img src="figs/opt_stiff_sensibility_dist_dvf.png" alt="opt_stiff_sensibility_dist_dvf.png" />
|
||||
</p>
|
||||
<p><span class="figure-number">Figure 9: </span>Norm of the transfer function from vertical disturbances to vertical position error with (dashed) and without (solid) Direct Velocity Feedback applied</p>
|
||||
</div>
|
||||
<div class="important">
|
||||
<p>
|
||||
Decentralized Direct Velocity Feedback is shown to increase the effect of stages vibrations at high frequency and to reduce the effect of ground motion and direct forces at low frequency.
|
||||
</p>
|
||||
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org27f255e" class="outline-3">
|
||||
<h3 id="org27f255e"><span class="section-number-3">1.6</span> Conclusion</h3>
|
||||
<div id="outline-container-org882e1ac" class="outline-3">
|
||||
<h3 id="org882e1ac"><span class="section-number-3">1.6</span> Conclusion</h3>
|
||||
<div class="outline-text-3" id="text-1-6">
|
||||
<div class="important">
|
||||
<p>
|
||||
@ -332,7 +350,7 @@ The controller for decentralized direct velocity feedback is the one designed in
|
||||
<h3 id="org1e7a412"><span class="section-number-3">2.1</span> Plant in the leg space</h3>
|
||||
<div class="outline-text-3" id="text-2-1">
|
||||
<p>
|
||||
We now loop at the transfer function matrix from \(\bm{\tau}^\prime\) to \(\bm{\epsilon}_{\mathcal{X}_n}\) for the design of \(\bm{K}_\mathcal{L}\).
|
||||
We now look at the transfer function matrix from \(\bm{\tau}^\prime\) to \(\bm{\epsilon}_{\mathcal{X}_n}\) for the design of \(\bm{K}_\mathcal{L}\).
|
||||
</p>
|
||||
|
||||
<p>
|
||||
@ -400,17 +418,17 @@ Kl = 2e7 <span class="org-type">*</span> eye(6) <span class="org-type">*</span>
|
||||
<p><span class="figure-number">Figure 12: </span>Loop gain for the primary plant</p>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Finally, we include the Jacobian in the control and we ignore the measurement of the vertical rotation as for the real system.
|
||||
</p>
|
||||
<div class="org-src-container">
|
||||
<pre class="src src-matlab">load(<span class="org-string">'mat/stages.mat'</span>, <span class="org-string">'nano_hexapod'</span>);
|
||||
K = Kl<span class="org-type">*</span>nano_hexapod.J<span class="org-type">*</span>diag([1, 1, 1, 1, 1, 0]);
|
||||
</pre>
|
||||
</div>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<p>
|
||||
Check the MIMO stability
|
||||
</p>
|
||||
</div>
|
||||
</div>
|
||||
<div id="outline-container-org16d192f" class="outline-3">
|
||||
<h3 id="org16d192f"><span class="section-number-3">2.3</span> Sensibility to Disturbances and Noise Budget</h3>
|
||||
<div class="outline-text-3" id="text-2-3">
|
||||
@ -519,8 +537,8 @@ Finally, the time domain position error signals are shown in Figure <a href="#or
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<div id="outline-container-org87fa1ac" class="outline-3">
|
||||
<h3 id="org87fa1ac"><span class="section-number-3">2.6</span> Conclusion</h3>
|
||||
<div id="outline-container-orgd61852c" class="outline-3">
|
||||
<h3 id="orgd61852c"><span class="section-number-3">2.6</span> Conclusion</h3>
|
||||
<div class="outline-text-3" id="text-2-6">
|
||||
<div class="important">
|
||||
<p>
|
||||
@ -614,8 +632,8 @@ Kx<span class="org-type">(6,6) </span>= 5e4 <span class="org-type">*</span> ...
|
||||
<div id="outline-container-org57e2cfd" class="outline-3">
|
||||
<h3 id="org57e2cfd"><span class="section-number-3">3.3</span> Simulation</h3>
|
||||
</div>
|
||||
<div id="outline-container-org882e1ac" class="outline-3">
|
||||
<h3 id="org882e1ac"><span class="section-number-3">3.4</span> Conclusion</h3>
|
||||
<div id="outline-container-org8c0882d" class="outline-3">
|
||||
<h3 id="org8c0882d"><span class="section-number-3">3.4</span> Conclusion</h3>
|
||||
<div class="outline-text-3" id="text-3-4">
|
||||
<div class="important">
|
||||
<p>
|
||||
@ -629,7 +647,7 @@ Kx<span class="org-type">(6,6) </span>= 5e4 <span class="org-type">*</span> ...
|
||||
</div>
|
||||
<div id="postamble" class="status">
|
||||
<p class="author">Author: Dehaeze Thomas</p>
|
||||
<p class="date">Created: 2020-04-17 ven. 10:25</p>
|
||||
<p class="date">Created: 2020-04-17 ven. 14:10</p>
|
||||
</div>
|
||||
</body>
|
||||
</html>
|
||||
|
||||
BIN
mat/conf_log.mat
BIN
mat/conf_log.mat
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
707
org/control_virtual_mass.org
Normal file
707
org/control_virtual_mass.org
Normal file
@ -0,0 +1,707 @@
|
||||
#+TITLE: Decentralize control to add virtual mass
|
||||
#+SETUPFILE: ./setup/org-setup-file.org
|
||||
|
||||
* Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no
|
||||
simulinkproject('../');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simulink.mat');
|
||||
open('nass_model.slx')
|
||||
#+end_src
|
||||
|
||||
* Initialization
|
||||
#+begin_src matlab
|
||||
initializeGround();
|
||||
initializeGranite();
|
||||
initializeTy();
|
||||
initializeRy();
|
||||
initializeRz();
|
||||
initializeMicroHexapod();
|
||||
initializeAxisc();
|
||||
initializeMirror();
|
||||
|
||||
initializeSimscapeConfiguration();
|
||||
initializeDisturbances('enable', false);
|
||||
initializeLoggingConfiguration('log', 'none');
|
||||
|
||||
initializeController('type', 'hac-dvf');
|
||||
#+end_src
|
||||
|
||||
We set the stiffness of the payload fixation:
|
||||
#+begin_src matlab
|
||||
Kp = 1e8; % [N/m]
|
||||
#+end_src
|
||||
|
||||
* Identification
|
||||
** Identification of the transfer function from $\tau$ to $d\mathcal{L}$
|
||||
#+begin_src matlab
|
||||
K = tf(zeros(6));
|
||||
Kdvf = tf(zeros(6));
|
||||
#+end_src
|
||||
|
||||
We identify the system for the following payload masses:
|
||||
#+begin_src matlab
|
||||
Ms = [1, 10, 50];
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
Gm = {zeros(length(Ms), 1)};
|
||||
#+end_src
|
||||
|
||||
The nano-hexapod has the following leg's stiffness and damping.
|
||||
#+begin_src matlab
|
||||
initializeNanoHexapod('k', 1e5, 'c', 2e2);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
%% Name of the Simulink File
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Micro-Station'], 3, 'openoutput', [], 'Dnlm'); io_i = io_i + 1; % Force Sensors
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
|
||||
|
||||
%% Run the linearization
|
||||
G_dvf = linearize(mdl, io);
|
||||
G_dvf.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G_dvf.OutputName = {'Dnlm1', 'Dnlm2', 'Dnlm3', 'Dnlm4', 'Dnlm5', 'Dnlm6'};
|
||||
Gm(i) = {G_dvf};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Identification of the Primary plant without virtual add of mass
|
||||
#+begin_src matlab :exports none
|
||||
G_x = {zeros(length(Ms), 1)};
|
||||
G_l = {zeros(length(Ms), 1)};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||
|
||||
Gx = -G*inv(nano_hexapod.J');
|
||||
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
G_x(i) = {Gx};
|
||||
|
||||
Gl = -nano_hexapod.J*G;
|
||||
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||
G_l(i) = {Gl};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
* Adding Virtual Mass in the Leg's Space
|
||||
** Plant
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
plot(freqs, abs(squeeze(freqresp(Gm{i}(1, 1), 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 i = 1:length(Ms)
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm{i}(1, 1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_plant_L.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_plant_L
|
||||
#+caption: Transfer function from $\tau_i$ to $d\mathcal{L}_i$ for three payload masses
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_plant_L.png]]
|
||||
|
||||
** Controller Design
|
||||
#+begin_src matlab
|
||||
Kdvf = 10*s^2/(1+s/2/pi/500)^2*eye(6);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
isstable(feedback(Gm{i}*Kdvf, eye(6), -1))
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 4, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
plot(freqs, abs(squeeze(freqresp(Gm{i}(1, 1)*Kdvf(1,1), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(Gm{i}(1, 1)*Kdvf(1,1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_loop_gain_L.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_loop_gain_L
|
||||
#+caption: Loop Gain for the addition of virtual mass in the leg's space
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_loop_gain_L.png]]
|
||||
|
||||
** Identification of the Primary Plant
|
||||
#+begin_src matlab :exports none
|
||||
%% Name of the Simulink File
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
|
||||
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
GmL_x = {zeros(length(Ms), 1)};
|
||||
GmL_l = {zeros(length(Ms), 1)};
|
||||
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||
|
||||
Gx = -G*inv(nano_hexapod.J');
|
||||
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
GmL_x(i) = {Gx};
|
||||
|
||||
Gl = -nano_hexapod.J*G;
|
||||
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||
GmL_l(i) = {Gl};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 5000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmL_x{i}(1, 1), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmL_x{i}(2, 2), freqs, 'Hz'))), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
|
||||
|
||||
ax2 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmL_x{i}(3, 3), freqs, 'Hz'))), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('$\mathcal{X}_z/\mathcal{F}_z$')
|
||||
|
||||
ax3 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_x{i}(1, 1), freqs, 'Hz')))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_x{i}(2, 2), freqs, 'Hz')))), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_x{i}(3, 3), freqs, 'Hz')))), '--', ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_L_primary_plant_X.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_L_primary_plant_X
|
||||
#+caption: Comparison of the transfer function from $\mathcal{F}_{x,y,z}$ to $\mathcal{X}_{x,y,z}$ with and without the virtual addition of mass in the leg's space
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_L_primary_plant_X.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 5000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmL_l{i}(1, 1), 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 i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmL_l{i}(1, 1), freqs, 'Hz')))), '--', ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_L_primary_plant_L.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_L_primary_plant_L
|
||||
#+caption: Comparison of the transfer function from $\tau_i$ to $\mathcal{L}_{i}$ with and without the virtual addition of mass in the leg's space
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_L_primary_plant_L.png]]
|
||||
|
||||
* Adding Virtual Mass in the Task Space
|
||||
** Plant
|
||||
Let's look at the transfer function from $\bm{\mathcal{F}}$ to $d\bm{\mathcal{X}}$:
|
||||
\[ \frac{d\bm{\mathcal{L}}}{\bm{\mathcal{F}}} = \bm{J}^{-1} \frac{d\bm{\mathcal{L}}}{\bm{\tau}} \bm{J}^{-T} \]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
GmX = {zeros(length(Ms), 1)};
|
||||
for i = 1:length(Ms)
|
||||
GmX(i) = {inv(nano_hexapod.J) * Gm{i} * inv(nano_hexapod.J')};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmX{i}(1, 1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmX{i}(2, 2), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX{i}(1, 1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX{i}(2, 2), freqs, 'Hz')))), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax1 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmX{i}(3, 3), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX{i}(3, 3), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_plant_X.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_plant_X
|
||||
#+caption: Dynamics from $\mathcal{F}_{x,y,z}$ to $\mathcal{X}_{x,y,z}$ used for virtual mass addition in the task space
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_plant_X.png]]
|
||||
|
||||
** Controller Design
|
||||
#+begin_src matlab
|
||||
KmX = (s^2*1/(1+s/2/pi/500)^2*diag([1 1 50 0 0 0]));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
isstable(feedback(GmX{i}*KmX, eye(6), -1))
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(-1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
LmX = GmX{i}*KmX;
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(LmX(1, 1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(LmX(2, 2), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
LmX = GmX{i}*KmX;
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(LmX(1, 1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(LmX(2, 2), freqs, 'Hz')))), ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
ax1 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
LmX = GmX{i}*KmX;
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(LmX(3, 3), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
LmX = GmX{i}*KmX;
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(LmX(3, 3), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f$ [kg]', Ms(i)));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_loop_gain_X.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_loop_gain_X
|
||||
#+caption: Loop gain for virtual mass addition in the task space
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_loop_gain_X.png]]
|
||||
|
||||
#+begin_src matlab
|
||||
Kdvf = inv(nano_hexapod.J')*KmX*inv(nano_hexapod.J);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
isstable(feedback(Gm{i}*Kdvf, eye(6), -1))
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Identification of the Primary Plant
|
||||
#+begin_src matlab :exports none
|
||||
%% Name of the Simulink File
|
||||
mdl = 'nass_model';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs
|
||||
io(io_i) = linio([mdl, '/Tracking Error'], 1, 'output', [], 'En'); io_i = io_i + 1; % Position Errror
|
||||
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
|
||||
GmX_x = {zeros(length(Ms), 1)};
|
||||
GmX_l = {zeros(length(Ms), 1)};
|
||||
|
||||
for i = 1:length(Ms)
|
||||
initializeSample('mass', Ms(i), 'freq', sqrt(Kp/Ms(i))/2/pi*ones(6,1));
|
||||
initializeReferences('Rz_type', 'rotating-not-filtered', 'Rz_period', Ms(i));
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
G.InputName = {'Fnl1', 'Fnl2', 'Fnl3', 'Fnl4', 'Fnl5', 'Fnl6'};
|
||||
G.OutputName = {'Ex', 'Ey', 'Ez', 'Erx', 'Ery', 'Erz'};
|
||||
|
||||
Gx = -G*inv(nano_hexapod.J');
|
||||
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
GmX_x(i) = {Gx};
|
||||
|
||||
Gl = -nano_hexapod.J*G;
|
||||
Gl.OutputName = {'E1', 'E2', 'E3', 'E4', 'E5', 'E6'};
|
||||
GmX_l(i) = {Gl};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 5000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 2, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmX_x{i}(1, 1), freqs, 'Hz'))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmX_x{i}(2, 2), freqs, 'Hz'))), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('$\mathcal{X}_x/\mathcal{F}_x$, $\mathcal{X}_y/\mathcal{F}_y$')
|
||||
|
||||
ax2 = subplot(2, 2, 2);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmX_x{i}(3, 3), freqs, 'Hz'))), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
title('$\mathcal{X}_z/\mathcal{F}_z$')
|
||||
|
||||
ax3 = subplot(2, 2, 3);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(1, 1), freqs, 'Hz')))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(2, 2), freqs, 'Hz')))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_x{i}(1, 1), freqs, 'Hz')))), '--');
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_x{i}(2, 2), freqs, 'Hz')))), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
|
||||
ax4 = subplot(2, 2, 4);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_x{i}(3, 3), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_x{i}(3, 3), freqs, 'Hz')))), '--', ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2,ax3,ax4],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_X_primary_plant_X.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_X_primary_plant_X
|
||||
#+caption: Comparison of the transfer function from $\mathcal{F}_{x,y,z}$ to $\mathcal{X}_{x,y,z}$ with and without the virtual addition of mass in the task space
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_X_primary_plant_X.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 5000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz'))));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, abs(squeeze(freqresp(GmX_l{i}(1, 1), 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 i = 1:length(Ms)
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(G_l{i}(1, 1), freqs, 'Hz')))), ...
|
||||
'DisplayName', sprintf('$m_p = %.0f [kg]$', Ms(i)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(freqs, 180/pi*unwrap(angle(squeeze(freqresp(GmX_l{i}(1, 1), freqs, 'Hz')))), '--', ...
|
||||
'HandleVisibility', 'off');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-270, 90]);
|
||||
yticks([-360:90:360]);
|
||||
legend('location', 'southwest');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :tangle no :exports results :results file replace
|
||||
exportFig('figs/virtual_mass_X_primary_plant_L.pdf', 'width', 'full', 'height', 'full')
|
||||
#+end_src
|
||||
|
||||
#+name: fig:virtual_mass_X_primary_plant_L
|
||||
#+caption: Comparison of the transfer function from $\tau_i$ to $\mathcal{L}_{i}$ with and without the virtual addition of mass in the task space
|
||||
#+RESULTS:
|
||||
[[file:figs/virtual_mass_X_primary_plant_L.png]]
|
||||
@ -233,6 +233,9 @@ Then, we compute the transfer function from forces applied by the actuators $\bm
|
||||
\[ \bm{G}_\mathcal{X}(s) = \frac{\bm{\epsilon}_{\mathcal{X}_n}}{\bm{\mathcal{F}}} = \bm{G}(s) \bm{J}^{-T} \]
|
||||
The obtained dynamics is shown in Figure [[fig:opt_stiff_primary_plant_damped_X]].
|
||||
|
||||
#+begin_important
|
||||
A zero with a positive real part is introduced in the transfer function from $\mathcal{F}_y$ to $\mathcal{X}_y$ after Decentralized Direct Velocity Feedback is applied.
|
||||
#+end_important
|
||||
|
||||
And we compute the transfer function from actuator forces $\bm{\tau}$ to position error of each leg $\bm{\epsilon}_\mathcal{L}$:
|
||||
\[ \bm{G}_\mathcal{L} = \frac{\bm{\epsilon}_\mathcal{L}}{\bm{\tau}} = \bm{J} \bm{G}(s) \]
|
||||
@ -449,7 +452,10 @@ The coupling does not change a lot with DVF.
|
||||
|
||||
|
||||
The coupling in the space of the legs $\bm{G}_\mathcal{L}$ are shown in Figure [[fig:opt_stiff_primary_plant_damped_coupling_L]].
|
||||
The magnitude of the coupling around the resonance of the nano-hexapod (where the coupling is the highest) is considerably reduced when DVF is applied.
|
||||
|
||||
#+begin_important
|
||||
The magnitude of the coupling between $\tau_i$ and $d\mathcal{L}_j$ (Figure [[fig:opt_stiff_primary_plant_damped_coupling_L]]) around the resonance of the nano-hexapod (where the coupling is the highest) is considerably reduced when DVF is applied.
|
||||
#+end_important
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(0, 3, 1000);
|
||||
@ -653,6 +659,11 @@ exportFig('figs/opt_stiff_sensibility_dist_dvf.pdf', 'width', 'full', 'height',
|
||||
#+RESULTS:
|
||||
[[file:figs/opt_stiff_sensibility_dist_dvf.png]]
|
||||
|
||||
*** Conclusion :ignore:
|
||||
#+begin_important
|
||||
Decentralized Direct Velocity Feedback is shown to increase the effect of stages vibrations at high frequency and to reduce the effect of ground motion and direct forces at low frequency.
|
||||
#+end_important
|
||||
|
||||
** Conclusion
|
||||
#+begin_important
|
||||
|
||||
@ -672,7 +683,7 @@ In this section we implement the control architecture shown in Figure [[fig:cont
|
||||
The controller for decentralized direct velocity feedback is the one designed in Section [[sec:lac_dvf]].
|
||||
|
||||
** Plant in the leg space
|
||||
We now loop at the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the design of $\bm{K}_\mathcal{L}$.
|
||||
We now look at the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the design of $\bm{K}_\mathcal{L}$.
|
||||
|
||||
The diagonal elements of the transfer function matrix from $\bm{\tau}^\prime$ to $\bm{\epsilon}_{\mathcal{X}_n}$ for the three considered masses are shown in Figure [[fig:opt_stiff_primary_plant_L]].
|
||||
|
||||
@ -793,7 +804,7 @@ The loop gain is shown in Figure [[fig:opt_stiff_primary_loop_gain_L]].
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
@ -821,12 +832,12 @@ exportFig('figs/opt_stiff_primary_loop_gain_L.pdf', 'width', 'full', 'height', '
|
||||
#+RESULTS:
|
||||
[[file:figs/opt_stiff_primary_loop_gain_L.png]]
|
||||
|
||||
Finally, we include the Jacobian in the control and we ignore the measurement of the vertical rotation as for the real system.
|
||||
#+begin_src matlab
|
||||
load('mat/stages.mat', 'nano_hexapod');
|
||||
K = Kl*nano_hexapod.J*diag([1, 1, 1, 1, 1, 0]);
|
||||
#+end_src
|
||||
|
||||
Check the MIMO stability
|
||||
#+begin_src matlab :exports none
|
||||
for i = 1:length(Ms)
|
||||
isstable(feedback(nano_hexapod.J\Gm_l{i}*K, eye(6), -1))
|
||||
|
||||
Loading…
Reference in New Issue
Block a user