IFF analysis with Simscape model
Before Width: | Height: | Size: 125 KiB After Width: | Height: | Size: 147 KiB |
Before Width: | Height: | Size: 156 KiB After Width: | Height: | Size: 160 KiB |
Before Width: | Height: | Size: 115 KiB After Width: | Height: | Size: 128 KiB |
Before Width: | Height: | Size: 126 KiB After Width: | Height: | Size: 130 KiB |
BIN
figs/enc_struts_iff_gains_effect_dvf_plant.pdf
Normal file
BIN
figs/enc_struts_iff_gains_effect_dvf_plant.png
Normal file
After Width: | Height: | Size: 150 KiB |
BIN
figs/enc_struts_iff_opt_loop_gain.pdf
Normal file
BIN
figs/enc_struts_iff_opt_loop_gain.png
Normal file
After Width: | Height: | Size: 140 KiB |
BIN
figs/enc_struts_iff_root_locus.pdf
Normal file
BIN
figs/enc_struts_iff_root_locus.png
Normal file
After Width: | Height: | Size: 40 KiB |
@ -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>
|
||||||
<!-- 2021-06-08 mar. 22:38 -->
|
<!-- 2021-06-09 mer. 18:13 -->
|
||||||
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
||||||
<title>Nano-Hexapod - Test Bench</title>
|
<title>Nano-Hexapod - Test Bench</title>
|
||||||
<meta name="author" content="Dehaeze Thomas" />
|
<meta name="author" content="Dehaeze Thomas" />
|
||||||
@ -39,21 +39,33 @@
|
|||||||
<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="#orgd7e7f5e">1. Encoders fixed to the Struts</a>
|
<li><a href="#orgd5a3ff2">1. Encoders fixed to the Struts</a>
|
||||||
<ul>
|
<ul>
|
||||||
<li><a href="#org00dcf35">1.1. Introduction</a></li>
|
<li><a href="#orgaaf36d1">1.1. Introduction</a></li>
|
||||||
<li><a href="#orgb763144">1.2. Load Data</a></li>
|
<li><a href="#org4eac0e4">1.2. Identification of the dynamics</a>
|
||||||
<li><a href="#orgb853f20">1.3. Spectral Analysis - Setup</a></li>
|
|
||||||
<li><a href="#orge1489cc">1.4. DVF Plant</a></li>
|
|
||||||
<li><a href="#org0c1cf8a">1.5. IFF Plant</a></li>
|
|
||||||
<li><a href="#orgc6ecc36">1.6. Jacobian</a>
|
|
||||||
<ul>
|
<ul>
|
||||||
<li><a href="#org1c3941e">1.6.1. DVF Plant</a></li>
|
<li><a href="#orge7631cb">1.2.1. Load Data</a></li>
|
||||||
<li><a href="#org31caf05">1.6.2. IFF Plant</a></li>
|
<li><a href="#org3d8f0db">1.2.2. Spectral Analysis - Setup</a></li>
|
||||||
|
<li><a href="#orgfe475e0">1.2.3. DVF Plant</a></li>
|
||||||
|
<li><a href="#org9c55cb0">1.2.4. IFF Plant</a></li>
|
||||||
|
</ul>
|
||||||
|
</li>
|
||||||
|
<li><a href="#orgb32a800">1.3. Comparison with the Simscape Model</a>
|
||||||
|
<ul>
|
||||||
|
<li><a href="#org49d6b51">1.3.1. Dynamics from Actuator to Force Sensors</a></li>
|
||||||
|
<li><a href="#org68f8e6c">1.3.2. Dynamics from Actuator to Encoder</a></li>
|
||||||
|
</ul>
|
||||||
|
</li>
|
||||||
|
<li><a href="#orge6221eb">1.4. Integral Force Feedback</a>
|
||||||
|
<ul>
|
||||||
|
<li><a href="#org1ccd985">1.4.1. Root Locus and Decentralized Loop gain</a></li>
|
||||||
|
<li><a href="#orgd6bc33c">1.4.2. Multiple Gains - Simulation</a></li>
|
||||||
|
<li><a href="#orgcbdb9eb">1.4.3. Experimental Results</a></li>
|
||||||
</ul>
|
</ul>
|
||||||
</li>
|
</li>
|
||||||
</ul>
|
</ul>
|
||||||
</li>
|
</li>
|
||||||
|
<li><a href="#org16300e1">2. Encoders fixed to the plates</a></li>
|
||||||
</ul>
|
</ul>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
@ -61,7 +73,11 @@
|
|||||||
<p>This report is also available as a <a href="./test-bench-nano-hexapod.pdf">pdf</a>.</p>
|
<p>This report is also available as a <a href="./test-bench-nano-hexapod.pdf">pdf</a>.</p>
|
||||||
<hr>
|
<hr>
|
||||||
|
|
||||||
<div class="note" id="orgf7b18a3">
|
<p>
|
||||||
|
In this document, the dynamics of the nano-hexapod shown in Figure <a href="#orgcaac3cd">1</a> is identified.
|
||||||
|
</p>
|
||||||
|
|
||||||
|
<div class="note" id="org64d5e50">
|
||||||
<p>
|
<p>
|
||||||
Here are the documentation of the equipment used for this test bench:
|
Here are the documentation of the equipment used for this test bench:
|
||||||
</p>
|
</p>
|
||||||
@ -76,25 +92,149 @@ Here are the documentation of the equipment used for this test bench:
|
|||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="org2a6b667" class="figure">
|
<div id="orgcaac3cd" class="figure">
|
||||||
<p><img src="figs/IMG_20210608_152917.jpg" alt="IMG_20210608_152917.jpg" />
|
<p><img src="figs/IMG_20210608_152917.jpg" alt="IMG_20210608_152917.jpg" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 1: </span>Nano-Hexapod</p>
|
<p><span class="figure-number">Figure 1: </span>Nano-Hexapod</p>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="org7b600dc" class="figure">
|
<div id="org6004b44" class="figure">
|
||||||
<p><img src="figs/IMG_20210608_154722.jpg" alt="IMG_20210608_154722.jpg" />
|
<p><img src="figs/IMG_20210608_154722.jpg" alt="IMG_20210608_154722.jpg" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 2: </span>Nano-Hexapod and the control electronics</p>
|
<p><span class="figure-number">Figure 2: </span>Nano-Hexapod and the control electronics</p>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<div id="outline-container-orgd7e7f5e" class="outline-2">
|
|
||||||
<h2 id="orgd7e7f5e"><span class="section-number-2">1</span> Encoders fixed to the Struts</h2>
|
<div id="orgc32dab5" class="figure">
|
||||||
|
<p><img src="figs/nano_hexapod_signals.png" alt="nano_hexapod_signals.png" />
|
||||||
|
</p>
|
||||||
|
<p><span class="figure-number">Figure 3: </span>Block diagram of the system with named signals</p>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<table id="orgcb52f65" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
||||||
|
<caption class="t-above"><span class="table-number">Table 1:</span> List of signals</caption>
|
||||||
|
|
||||||
|
<colgroup>
|
||||||
|
<col class="org-left" />
|
||||||
|
|
||||||
|
<col class="org-left" />
|
||||||
|
|
||||||
|
<col class="org-left" />
|
||||||
|
|
||||||
|
<col class="org-left" />
|
||||||
|
|
||||||
|
<col class="org-left" />
|
||||||
|
</colgroup>
|
||||||
|
<thead>
|
||||||
|
<tr>
|
||||||
|
<th scope="col" class="org-left"> </th>
|
||||||
|
<th scope="col" class="org-left"><b>Unit</b></th>
|
||||||
|
<th scope="col" class="org-left"><b>Matlab</b></th>
|
||||||
|
<th scope="col" class="org-left"><b>Vector</b></th>
|
||||||
|
<th scope="col" class="org-left"><b>Elements</b></th>
|
||||||
|
</tr>
|
||||||
|
</thead>
|
||||||
|
<tbody>
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Control Input (wanted DAC voltage)</td>
|
||||||
|
<td class="org-left"><code>[V]</code></td>
|
||||||
|
<td class="org-left"><code>u</code></td>
|
||||||
|
<td class="org-left">\(\bm{u}\)</td>
|
||||||
|
<td class="org-left">\(u_i\)</td>
|
||||||
|
</tr>
|
||||||
|
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">DAC Output Voltage</td>
|
||||||
|
<td class="org-left"><code>[V]</code></td>
|
||||||
|
<td class="org-left"><code>u</code></td>
|
||||||
|
<td class="org-left">\(\tilde{\bm{u}}\)</td>
|
||||||
|
<td class="org-left">\(\tilde{u}_i\)</td>
|
||||||
|
</tr>
|
||||||
|
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">PD200 Output Voltage</td>
|
||||||
|
<td class="org-left"><code>[V]</code></td>
|
||||||
|
<td class="org-left"><code>ua</code></td>
|
||||||
|
<td class="org-left">\(\bm{u}_a\)</td>
|
||||||
|
<td class="org-left">\(u_{a,i}\)</td>
|
||||||
|
</tr>
|
||||||
|
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Actuator applied force</td>
|
||||||
|
<td class="org-left"><code>[N]</code></td>
|
||||||
|
<td class="org-left"><code>tau</code></td>
|
||||||
|
<td class="org-left">\(\bm{\tau}\)</td>
|
||||||
|
<td class="org-left">\(\tau_i\)</td>
|
||||||
|
</tr>
|
||||||
|
</tbody>
|
||||||
|
<tbody>
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Strut motion</td>
|
||||||
|
<td class="org-left"><code>[m]</code></td>
|
||||||
|
<td class="org-left"><code>dL</code></td>
|
||||||
|
<td class="org-left">\(d\bm{\mathcal{L}}\)</td>
|
||||||
|
<td class="org-left">\(d\mathcal{L}_i\)</td>
|
||||||
|
</tr>
|
||||||
|
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Encoder measured displacement</td>
|
||||||
|
<td class="org-left"><code>[m]</code></td>
|
||||||
|
<td class="org-left"><code>dLm</code></td>
|
||||||
|
<td class="org-left">\(d\bm{\mathcal{L}}_m\)</td>
|
||||||
|
<td class="org-left">\(d\mathcal{L}_{m,i}\)</td>
|
||||||
|
</tr>
|
||||||
|
</tbody>
|
||||||
|
<tbody>
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Force Sensor strain</td>
|
||||||
|
<td class="org-left"><code>[m]</code></td>
|
||||||
|
<td class="org-left"><code>epsilon</code></td>
|
||||||
|
<td class="org-left">\(\bm{\epsilon}\)</td>
|
||||||
|
<td class="org-left">\(\epsilon_i\)</td>
|
||||||
|
</tr>
|
||||||
|
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Force Sensor Generated Voltage</td>
|
||||||
|
<td class="org-left"><code>[V]</code></td>
|
||||||
|
<td class="org-left"><code>taum</code></td>
|
||||||
|
<td class="org-left">\(\tilde{\bm{\tau}}_m\)</td>
|
||||||
|
<td class="org-left">\(\tilde{\tau}_{m,i}\)</td>
|
||||||
|
</tr>
|
||||||
|
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Measured Generated Voltage</td>
|
||||||
|
<td class="org-left"><code>[V]</code></td>
|
||||||
|
<td class="org-left"><code>taum</code></td>
|
||||||
|
<td class="org-left">\(\bm{\tau}_m\)</td>
|
||||||
|
<td class="org-left">\(\tau_{m,i}\)</td>
|
||||||
|
</tr>
|
||||||
|
</tbody>
|
||||||
|
<tbody>
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Motion of the top platform</td>
|
||||||
|
<td class="org-left"><code>[m,rad]</code></td>
|
||||||
|
<td class="org-left"><code>dX</code></td>
|
||||||
|
<td class="org-left">\(d\bm{\mathcal{X}}\)</td>
|
||||||
|
<td class="org-left">\(d\mathcal{X}_i\)</td>
|
||||||
|
</tr>
|
||||||
|
|
||||||
|
<tr>
|
||||||
|
<td class="org-left">Metrology measured displacement</td>
|
||||||
|
<td class="org-left"><code>[m,rad]</code></td>
|
||||||
|
<td class="org-left"><code>dXm</code></td>
|
||||||
|
<td class="org-left">\(d\bm{\mathcal{X}}_m\)</td>
|
||||||
|
<td class="org-left">\(d\mathcal{X}_{m,i}\)</td>
|
||||||
|
</tr>
|
||||||
|
</tbody>
|
||||||
|
</table>
|
||||||
|
|
||||||
|
<div id="outline-container-orgd5a3ff2" class="outline-2">
|
||||||
|
<h2 id="orgd5a3ff2"><span class="section-number-2">1</span> Encoders fixed to the Struts</h2>
|
||||||
<div class="outline-text-2" id="text-1">
|
<div class="outline-text-2" id="text-1">
|
||||||
</div>
|
</div>
|
||||||
<div id="outline-container-org00dcf35" class="outline-3">
|
<div id="outline-container-orgaaf36d1" class="outline-3">
|
||||||
<h3 id="org00dcf35"><span class="section-number-3">1.1</span> Introduction</h3>
|
<h3 id="orgaaf36d1"><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">
|
||||||
<p>
|
<p>
|
||||||
In this section, the encoders are fixed to the struts.
|
In this section, the encoders are fixed to the struts.
|
||||||
@ -102,11 +242,16 @@ In this section, the encoders are fixed to the struts.
|
|||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<div id="outline-container-orgb763144" class="outline-3">
|
<div id="outline-container-org4eac0e4" class="outline-3">
|
||||||
<h3 id="orgb763144"><span class="section-number-3">1.2</span> Load Data</h3>
|
<h3 id="org4eac0e4"><span class="section-number-3">1.2</span> Identification of the dynamics</h3>
|
||||||
<div class="outline-text-3" id="text-1-2">
|
<div class="outline-text-3" id="text-1-2">
|
||||||
|
</div>
|
||||||
|
<div id="outline-container-orge7631cb" class="outline-4">
|
||||||
|
<h4 id="orge7631cb"><span class="section-number-4">1.2.1</span> Load Data</h4>
|
||||||
|
<div class="outline-text-4" id="text-1-2-1">
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab">meas_data_lf = {};
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Load Identification Data</span></span>
|
||||||
|
meas_data_lf = {};
|
||||||
|
|
||||||
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:6</span>
|
||||||
meas_data_lf(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/frf_data_exc_strut_%i_noise_lf.mat'</span>, <span class="org-constant">i</span>), <span class="org-string">'t'</span>, <span class="org-string">'Va'</span>, <span class="org-string">'Vs'</span>, <span class="org-string">'de'</span>)};
|
meas_data_lf(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/frf_data_exc_strut_%i_noise_lf.mat'</span>, <span class="org-constant">i</span>), <span class="org-string">'t'</span>, <span class="org-string">'Va'</span>, <span class="org-string">'Vs'</span>, <span class="org-string">'de'</span>)};
|
||||||
@ -117,11 +262,12 @@ In this section, the encoders are fixed to the struts.
|
|||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<div id="outline-container-orgb853f20" class="outline-3">
|
<div id="outline-container-org3d8f0db" class="outline-4">
|
||||||
<h3 id="orgb853f20"><span class="section-number-3">1.3</span> Spectral Analysis - Setup</h3>
|
<h4 id="org3d8f0db"><span class="section-number-4">1.2.2</span> Spectral Analysis - Setup</h4>
|
||||||
<div class="outline-text-3" id="text-1-3">
|
<div class="outline-text-4" id="text-1-2-2">
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab"><span class="org-comment">% Sampling Time [s]</span>
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Setup useful variables</span></span>
|
||||||
|
<span class="org-comment">% Sampling Time [s]</span>
|
||||||
Ts = (meas_data_lf{1}.t(end) <span class="org-type">-</span> (meas_data_lf{1}.t(1)))<span class="org-type">/</span>(length(meas_data_lf{1}.t)<span class="org-type">-</span>1);
|
Ts = (meas_data_lf{1}.t(end) <span class="org-type">-</span> (meas_data_lf{1}.t(1)))<span class="org-type">/</span>(length(meas_data_lf{1}.t)<span class="org-type">-</span>1);
|
||||||
|
|
||||||
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
||||||
@ -129,30 +275,22 @@ Fs = 1<span class="org-type">/</span>Ts;
|
|||||||
|
|
||||||
<span class="org-comment">% Hannning Windows</span>
|
<span class="org-comment">% Hannning Windows</span>
|
||||||
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
||||||
</pre>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<p>
|
<span class="org-comment">% And we get the frequency vector</span>
|
||||||
And we get the frequency vector.
|
[<span class="org-type">~</span>, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
||||||
</p>
|
|
||||||
<div class="org-src-container">
|
|
||||||
<pre class="src src-matlab">[<span class="org-type">~</span>, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
||||||
</pre>
|
|
||||||
</div>
|
|
||||||
|
|
||||||
<div class="org-src-container">
|
i_lf = f <span class="org-type"><</span> 250; <span class="org-comment">% Points for low frequency excitation</span>
|
||||||
<pre class="src src-matlab">i_lf = f <span class="org-type"><</span> 250; <span class="org-comment">% Points for low frequency excitation</span>
|
|
||||||
i_hf = f <span class="org-type">></span> 250; <span class="org-comment">% Points for high frequency excitation</span>
|
i_hf = f <span class="org-type">></span> 250; <span class="org-comment">% Points for high frequency excitation</span>
|
||||||
</pre>
|
</pre>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<div id="outline-container-orge1489cc" class="outline-3">
|
<div id="outline-container-orgfe475e0" class="outline-4">
|
||||||
<h3 id="orge1489cc"><span class="section-number-3">1.4</span> DVF Plant</h3>
|
<h4 id="orgfe475e0"><span class="section-number-4">1.2.3</span> DVF Plant</h4>
|
||||||
<div class="outline-text-3" id="text-1-4">
|
<div class="outline-text-4" id="text-1-2-3">
|
||||||
<p>
|
<p>
|
||||||
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#orgdf189a0">3</a>).
|
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#org7027095">4</a>).
|
||||||
</p>
|
</p>
|
||||||
|
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
@ -164,22 +302,21 @@ coh_dvf_hf = zeros(length(f), 6, 6);
|
|||||||
coh_dvf_lf(<span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span>) = mscohere(meas_data_lf{<span class="org-constant">i</span>}.Va, meas_data_lf{<span class="org-constant">i</span>}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
coh_dvf_lf(<span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span>) = mscohere(meas_data_lf{<span class="org-constant">i</span>}.Va, meas_data_lf{<span class="org-constant">i</span>}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
||||||
coh_dvf_hf(<span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span>) = mscohere(meas_data_hf{<span class="org-constant">i</span>}.Va, meas_data_hf{<span class="org-constant">i</span>}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
coh_dvf_hf(<span class="org-type">:</span>, <span class="org-type">:</span>, <span class="org-constant">i</span>) = mscohere(meas_data_hf{<span class="org-constant">i</span>}.Va, meas_data_hf{<span class="org-constant">i</span>}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
||||||
<span class="org-keyword">end</span>
|
<span class="org-keyword">end</span>
|
||||||
|
|
||||||
</pre>
|
</pre>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="orgdf189a0" class="figure">
|
<div id="org7027095" class="figure">
|
||||||
<p><img src="figs/enc_struts_dvf_coh.png" alt="enc_struts_dvf_coh.png" />
|
<p><img src="figs/enc_struts_dvf_coh.png" alt="enc_struts_dvf_coh.png" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 3: </span>Obtained coherence for the DVF plant</p>
|
<p><span class="figure-number">Figure 4: </span>Obtained coherence for the DVF plant</p>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<p>
|
<p>
|
||||||
Then the 6x6 transfer function matrix is estimated (Figure <a href="#orgce0ab32">4</a>).
|
Then the 6x6 transfer function matrix is estimated (Figure <a href="#orgeda62ff">5</a>).
|
||||||
</p>
|
</p>
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% DVF Plant</span></span>
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% DVF Plant (transfer function from u to dLm)</span></span>
|
||||||
G_dvf_lf = zeros(length(f), 6, 6);
|
G_dvf_lf = zeros(length(f), 6, 6);
|
||||||
G_dvf_hf = zeros(length(f), 6, 6);
|
G_dvf_hf = zeros(length(f), 6, 6);
|
||||||
|
|
||||||
@ -191,24 +328,24 @@ G_dvf_hf = zeros(length(f), 6, 6);
|
|||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="orgce0ab32" class="figure">
|
<div id="orgeda62ff" class="figure">
|
||||||
<p><img src="figs/enc_struts_dvf_frf.png" alt="enc_struts_dvf_frf.png" />
|
<p><img src="figs/enc_struts_dvf_frf.png" alt="enc_struts_dvf_frf.png" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 4: </span>Measured FRF for the DVF plant</p>
|
<p><span class="figure-number">Figure 5: </span>Measured FRF for the DVF plant</p>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="outline-container-org0c1cf8a" class="outline-3">
|
<div id="outline-container-org9c55cb0" class="outline-4">
|
||||||
<h3 id="org0c1cf8a"><span class="section-number-3">1.5</span> IFF Plant</h3>
|
<h4 id="org9c55cb0"><span class="section-number-4">1.2.4</span> IFF Plant</h4>
|
||||||
<div class="outline-text-3" id="text-1-5">
|
<div class="outline-text-4" id="text-1-2-4">
|
||||||
<p>
|
<p>
|
||||||
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#org1ba438b">5</a>).
|
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#orga958a00">6</a>).
|
||||||
</p>
|
</p>
|
||||||
|
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Coherence</span></span>
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Coherence for the IFF plant</span></span>
|
||||||
coh_iff_lf = zeros(length(f), 6, 6);
|
coh_iff_lf = zeros(length(f), 6, 6);
|
||||||
coh_iff_hf = zeros(length(f), 6, 6);
|
coh_iff_hf = zeros(length(f), 6, 6);
|
||||||
|
|
||||||
@ -221,14 +358,14 @@ coh_iff_hf = zeros(length(f), 6, 6);
|
|||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="org1ba438b" class="figure">
|
<div id="orga958a00" class="figure">
|
||||||
<p><img src="figs/enc_struts_iff_coh.png" alt="enc_struts_iff_coh.png" />
|
<p><img src="figs/enc_struts_iff_coh.png" alt="enc_struts_iff_coh.png" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 5: </span>Obtained coherence for the IFF plant</p>
|
<p><span class="figure-number">Figure 6: </span>Obtained coherence for the IFF plant</p>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<p>
|
<p>
|
||||||
Then the 6x6 transfer function matrix is estimated (Figure <a href="#orge2cbf29">6</a>).
|
Then the 6x6 transfer function matrix is estimated (Figure <a href="#orgaa3ad1c">7</a>).
|
||||||
</p>
|
</p>
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% IFF Plant</span></span>
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% IFF Plant</span></span>
|
||||||
@ -243,96 +380,199 @@ G_iff_hf = zeros(length(f), 6, 6);
|
|||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="orge2cbf29" class="figure">
|
<div id="orgaa3ad1c" class="figure">
|
||||||
<p><img src="figs/enc_struts_iff_frf.png" alt="enc_struts_iff_frf.png" />
|
<p><img src="figs/enc_struts_iff_frf.png" alt="enc_struts_iff_frf.png" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 6: </span>Measured FRF for the IFF plant</p>
|
<p><span class="figure-number">Figure 7: </span>Measured FRF for the IFF plant</p>
|
||||||
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<div id="outline-container-orgc6ecc36" class="outline-3">
|
<div id="outline-container-orgb32a800" class="outline-3">
|
||||||
<h3 id="orgc6ecc36"><span class="section-number-3">1.6</span> Jacobian</h3>
|
<h3 id="orgb32a800"><span class="section-number-3">1.3</span> Comparison with the Simscape Model</h3>
|
||||||
<div class="outline-text-3" id="text-1-6">
|
<div class="outline-text-3" id="text-1-3">
|
||||||
<p>
|
<p>
|
||||||
The Jacobian is used to transform the excitation force in the cartesian frame as well as the displacements.
|
In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model.
|
||||||
</p>
|
</p>
|
||||||
|
|
||||||
<p>
|
|
||||||
Consider the plant shown in Figure <a href="#org573cce0">7</a> with:
|
|
||||||
</p>
|
|
||||||
<ul class="org-ul">
|
|
||||||
<li>\(\tau\) the 6 input voltages (going to the PD200 amplifier and then to the APA)</li>
|
|
||||||
<li>\(d\mathcal{L}\) the relative motion sensor outputs (encoders)</li>
|
|
||||||
<li>\(\bm{\tau}_m\) the generated voltage of the force sensor stacks</li>
|
|
||||||
<li>\(J_a\) and \(J_s\) the Jacobians for the actuators and sensors</li>
|
|
||||||
</ul>
|
|
||||||
|
|
||||||
|
|
||||||
<div id="org573cce0" class="figure">
|
|
||||||
<p><img src="figs/schematic_jacobian_in_out.png" alt="schematic_jacobian_in_out.png" />
|
|
||||||
</p>
|
|
||||||
<p><span class="figure-number">Figure 7: </span>Plant in the cartesian Frame</p>
|
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<p>
|
<div id="outline-container-org49d6b51" class="outline-4">
|
||||||
First, we load the Jacobian matrix (same for the actuators and sensors).
|
<h4 id="org49d6b51"><span class="section-number-4">1.3.1</span> Dynamics from Actuator to Force Sensors</h4>
|
||||||
</p>
|
<div class="outline-text-4" id="text-1-3-1">
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab">load(<span class="org-string">'jacobian.mat'</span>, <span class="org-string">'J'</span>);
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Initialize Nano-Hexapod</span></span>
|
||||||
|
n_hexapod = initializeNanoHexapodFinal(<span class="org-string">'flex_bot_type'</span>, <span class="org-string">'4dof'</span>, ...
|
||||||
|
<span class="org-string">'flex_top_type'</span>, <span class="org-string">'4dof'</span>, ...
|
||||||
|
<span class="org-string">'motion_sensor_type'</span>, <span class="org-string">'struts'</span>, ...
|
||||||
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'2dof'</span>);
|
||||||
</pre>
|
</pre>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
|
||||||
|
|
||||||
<div id="outline-container-org1c3941e" class="outline-4">
|
|
||||||
<h4 id="org1c3941e"><span class="section-number-4">1.6.1</span> DVF Plant</h4>
|
|
||||||
<div class="outline-text-4" id="text-1-6-1">
|
|
||||||
<p>
|
|
||||||
The transfer function from \(\bm{\mathcal{F}}\) to \(d\bm{\mathcal{X}}\) is computed and shown in Figure <a href="#org92f038e">8</a>.
|
|
||||||
</p>
|
|
||||||
|
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab">G_dvf_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_lf, [2 3 1]), inv(J<span class="org-type">'</span>))), [3 1 2]);
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Identify the IFF Plant (transfer function from u to taum)</span></span>
|
||||||
G_dvf_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_dvf_hf, [2 3 1]), inv(J<span class="org-type">'</span>))), [3 1 2]);
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, <span class="org-string">'/F'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Inputs</span>
|
||||||
|
io(io_i) = linio([mdl, <span class="org-string">'/Fm'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Force Sensors</span>
|
||||||
|
|
||||||
|
Giff = exp(<span class="org-type">-</span>s<span class="org-type">*</span>Ts)<span class="org-type">*</span>linearize(mdl, io, 0.0, options);
|
||||||
</pre>
|
</pre>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="org92f038e" class="figure">
|
<div id="orgb002d1f" class="figure">
|
||||||
<p><img src="figs/enc_struts_dvf_cart_frf.png" alt="enc_struts_dvf_cart_frf.png" />
|
<p><img src="figs/enc_struts_iff_comp_simscape.png" alt="enc_struts_iff_comp_simscape.png" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 8: </span>Measured FRF for the DVF plant in the cartesian frame</p>
|
<p><span class="figure-number">Figure 8: </span>Diagonal elements of the IFF Plant</p>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
<div id="orgef9afdd" class="figure">
|
||||||
|
<p><img src="figs/enc_struts_iff_comp_offdiag_simscape.png" alt="enc_struts_iff_comp_offdiag_simscape.png" />
|
||||||
|
</p>
|
||||||
|
<p><span class="figure-number">Figure 9: </span>Off diagonal elements of the IFF Plant</p>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
<div id="outline-container-org31caf05" class="outline-4">
|
<div id="outline-container-org68f8e6c" class="outline-4">
|
||||||
<h4 id="org31caf05"><span class="section-number-4">1.6.2</span> IFF Plant</h4>
|
<h4 id="org68f8e6c"><span class="section-number-4">1.3.2</span> Dynamics from Actuator to Encoder</h4>
|
||||||
<div class="outline-text-4" id="text-1-6-2">
|
<div class="outline-text-4" id="text-1-3-2">
|
||||||
<p>
|
<div class="org-src-container">
|
||||||
The transfer function from \(\bm{\mathcal{F}}\) to \(\bm{\mathcal{F}}_m\) is computed and shown in Figure <a href="#orge1b3404">9</a>.
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Initialization of the Nano-Hexapod</span></span>
|
||||||
</p>
|
n_hexapod = initializeNanoHexapodFinal(<span class="org-string">'flex_bot_type'</span>, <span class="org-string">'4dof'</span>, ...
|
||||||
|
<span class="org-string">'flex_top_type'</span>, <span class="org-string">'4dof'</span>, ...
|
||||||
|
<span class="org-string">'motion_sensor_type'</span>, <span class="org-string">'struts'</span>, ...
|
||||||
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'2dof'</span>);
|
||||||
|
</pre>
|
||||||
|
</div>
|
||||||
|
|
||||||
<div class="org-src-container">
|
<div class="org-src-container">
|
||||||
<pre class="src src-matlab">G_iff_J_lf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_lf, [2 3 1]), inv(J<span class="org-type">'</span>))), [3 1 2]);
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Identify the DVF Plant (transfer function from u to dLm)</span></span>
|
||||||
G_iff_J_hf = permute(pagemtimes(inv(J), pagemtimes(permute(G_iff_hf, [2 3 1]), inv(J<span class="org-type">'</span>))), [3 1 2]);
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, <span class="org-string">'/F'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Inputs</span>
|
||||||
|
io(io_i) = linio([mdl, <span class="org-string">'/D'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Encoders</span>
|
||||||
|
|
||||||
|
Gdvf = exp(<span class="org-type">-</span>s<span class="org-type">*</span>Ts)<span class="org-type">*</span>linearize(mdl, io, 0.0, options);
|
||||||
</pre>
|
</pre>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
|
||||||
<div id="orge1b3404" class="figure">
|
<div id="org8001ef8" class="figure">
|
||||||
<p><img src="figs/enc_struts_iff_cart_frf.png" alt="enc_struts_iff_cart_frf.png" />
|
<p><img src="figs/enc_struts_dvf_comp_simscape.png" alt="enc_struts_dvf_comp_simscape.png" />
|
||||||
</p>
|
</p>
|
||||||
<p><span class="figure-number">Figure 9: </span>Measured FRF for the IFF plant in the cartesian frame</p>
|
<p><span class="figure-number">Figure 10: </span>Diagonal elements of the DVF Plant</p>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
<div id="org8a8dc6a" class="figure">
|
||||||
|
<p><img src="figs/enc_struts_dvf_comp_offdiag_simscape.png" alt="enc_struts_dvf_comp_offdiag_simscape.png" />
|
||||||
|
</p>
|
||||||
|
<p><span class="figure-number">Figure 11: </span>Off diagonal elements of the DVF Plant</p>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
</div>
|
</div>
|
||||||
|
|
||||||
|
<div id="outline-container-orge6221eb" class="outline-3">
|
||||||
|
<h3 id="orge6221eb"><span class="section-number-3">1.4</span> Integral Force Feedback</h3>
|
||||||
|
<div class="outline-text-3" id="text-1-4">
|
||||||
|
</div>
|
||||||
|
<div id="outline-container-org1ccd985" class="outline-4">
|
||||||
|
<h4 id="org1ccd985"><span class="section-number-4">1.4.1</span> Root Locus and Decentralized Loop gain</h4>
|
||||||
|
<div class="outline-text-4" id="text-1-4-1">
|
||||||
|
<div class="org-src-container">
|
||||||
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% IFF Controller</span></span>
|
||||||
|
Kiff_g1 = (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>40))<span class="org-type">*</span>...<span class="org-comment"> % Low pass filter (provides integral action above 40Hz)</span>
|
||||||
|
(s<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>30))<span class="org-type">*</span>...<span class="org-comment"> % High pass filter to limit low frequency gain</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>...<span class="org-comment"> % Low pass filter to be more robust to high frequency resonances</span>
|
||||||
|
eye(6); <span class="org-comment">% Diagonal 6x6 controller</span>
|
||||||
|
</pre>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
<div id="org9d7fb85" class="figure">
|
||||||
|
<p><img src="figs/enc_struts_iff_root_locus.png" alt="enc_struts_iff_root_locus.png" />
|
||||||
|
</p>
|
||||||
|
<p><span class="figure-number">Figure 12: </span>Root Locus for the IFF control strategy</p>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<p>
|
||||||
|
Then the “optimal” IFF controller is:
|
||||||
|
</p>
|
||||||
|
<div class="org-src-container">
|
||||||
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% IFF controller with Optimal gain</span></span>
|
||||||
|
Kiff = g<span class="org-type">*</span>Kiff_g1;
|
||||||
|
</pre>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
<div id="org879ceab" class="figure">
|
||||||
|
<p><img src="figs/enc_struts_iff_opt_loop_gain.png" alt="enc_struts_iff_opt_loop_gain.png" />
|
||||||
|
</p>
|
||||||
|
<p><span class="figure-number">Figure 13: </span>Bode plot of the “decentralized loop gain” \(G_\text{iff}(i,i) \times K_\text{iff}(i,i)\)</p>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div id="outline-container-orgd6bc33c" class="outline-4">
|
||||||
|
<h4 id="orgd6bc33c"><span class="section-number-4">1.4.2</span> Multiple Gains - Simulation</h4>
|
||||||
|
<div class="outline-text-4" id="text-1-4-2">
|
||||||
|
<div class="org-src-container">
|
||||||
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Tested IFF gains</span></span>
|
||||||
|
iff_gains = [4, 10, 20, 40, 100, 200, 400, 1000];
|
||||||
|
</pre>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="org-src-container">
|
||||||
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Initialize the Simscape model in closed loop</span></span>
|
||||||
|
n_hexapod = initializeNanoHexapodFinal(<span class="org-string">'flex_bot_type'</span>, <span class="org-string">'4dof'</span>, ...
|
||||||
|
<span class="org-string">'flex_top_type'</span>, <span class="org-string">'4dof'</span>, ...
|
||||||
|
<span class="org-string">'motion_sensor_type'</span>, <span class="org-string">'struts'</span>, ...
|
||||||
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'2dof'</span>, ...
|
||||||
|
<span class="org-string">'controller_type'</span>, <span class="org-string">'iff'</span>);
|
||||||
|
</pre>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div class="org-src-container">
|
||||||
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain</span></span>
|
||||||
|
Gd_iff = {zeros(1, length(iff_gains))};
|
||||||
|
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, <span class="org-string">'/F'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Inputs</span>
|
||||||
|
io(io_i) = linio([mdl, <span class="org-string">'/D'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Strut Displacement (encoder)</span>
|
||||||
|
|
||||||
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(iff_gains)</span>
|
||||||
|
Kiff = iff_gains(<span class="org-constant">i</span>)<span class="org-type">*</span>Kiff_g1<span class="org-type">*</span>eye(6); <span class="org-comment">% IFF Controller</span>
|
||||||
|
Gd_iff(<span class="org-constant">i</span>) = {exp(<span class="org-type">-</span>s<span class="org-type">*</span>Ts)<span class="org-type">*</span>linearize(mdl, io, 0.0, options)};
|
||||||
|
|
||||||
|
isstable(Gd_iff{<span class="org-constant">i</span>})
|
||||||
|
<span class="org-keyword">end</span>
|
||||||
|
</pre>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
|
||||||
|
<div id="orgb5b5f55" class="figure">
|
||||||
|
<p><img src="figs/enc_struts_iff_gains_effect_dvf_plant.png" alt="enc_struts_iff_gains_effect_dvf_plant.png" />
|
||||||
|
</p>
|
||||||
|
<p><span class="figure-number">Figure 14: </span>Effect of the IFF gain \(g\) on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)</p>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div id="outline-container-orgcbdb9eb" class="outline-4">
|
||||||
|
<h4 id="orgcbdb9eb"><span class="section-number-4">1.4.3</span> Experimental Results</h4>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
</div>
|
||||||
|
|
||||||
|
<div id="outline-container-org16300e1" class="outline-2">
|
||||||
|
<h2 id="org16300e1"><span class="section-number-2">2</span> Encoders fixed to the plates</h2>
|
||||||
</div>
|
</div>
|
||||||
</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: 2021-06-08 mar. 22:38</p>
|
<p class="date">Created: 2021-06-09 mer. 18:13</p>
|
||||||
</div>
|
</div>
|
||||||
</body>
|
</body>
|
||||||
</html>
|
</html>
|
||||||
|
@ -46,7 +46,7 @@
|
|||||||
<hr>
|
<hr>
|
||||||
#+end_export
|
#+end_export
|
||||||
|
|
||||||
\clearpage
|
#+latex: \clearpage
|
||||||
|
|
||||||
* Introduction :ignore:
|
* Introduction :ignore:
|
||||||
In this document, the dynamics of the nano-hexapod shown in Figure [[fig:picture_bench_granite_nano_hexapod]] is identified.
|
In this document, the dynamics of the nano-hexapod shown in Figure [[fig:picture_bench_granite_nano_hexapod]] is identified.
|
||||||
@ -136,7 +136,6 @@ Here are the documentation of the equipment used for this test bench:
|
|||||||
| Motion of the top platform | =[m,rad]= | =dX= | $d\bm{\mathcal{X}}$ | $d\mathcal{X}_i$ |
|
| Motion of the top platform | =[m,rad]= | =dX= | $d\bm{\mathcal{X}}$ | $d\mathcal{X}_i$ |
|
||||||
| Metrology measured displacement | =[m,rad]= | =dXm= | $d\bm{\mathcal{X}}_m$ | $d\mathcal{X}_{m,i}$ |
|
| Metrology measured displacement | =[m,rad]= | =dXm= | $d\bm{\mathcal{X}}_m$ | $d\mathcal{X}_{m,i}$ |
|
||||||
|
|
||||||
*
|
|
||||||
* Encoders fixed to the Struts
|
* Encoders fixed to the Struts
|
||||||
** Introduction
|
** Introduction
|
||||||
In this section, the encoders are fixed to the struts.
|
In this section, the encoders are fixed to the struts.
|
||||||
@ -164,6 +163,7 @@ addpath('./src/');
|
|||||||
** Identification of the dynamics
|
** Identification of the dynamics
|
||||||
*** Load Data
|
*** Load Data
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
|
%% Load Identification Data
|
||||||
meas_data_lf = {};
|
meas_data_lf = {};
|
||||||
|
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
@ -174,6 +174,7 @@ end
|
|||||||
|
|
||||||
*** Spectral Analysis - Setup
|
*** Spectral Analysis - Setup
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
|
%% Setup useful variables
|
||||||
% Sampling Time [s]
|
% Sampling Time [s]
|
||||||
Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1);
|
Ts = (meas_data_lf{1}.t(end) - (meas_data_lf{1}.t(1)))/(length(meas_data_lf{1}.t)-1);
|
||||||
|
|
||||||
@ -182,14 +183,10 @@ Fs = 1/Ts;
|
|||||||
|
|
||||||
% Hannning Windows
|
% Hannning Windows
|
||||||
win = hanning(ceil(1*Fs));
|
win = hanning(ceil(1*Fs));
|
||||||
#+end_src
|
|
||||||
|
|
||||||
And we get the frequency vector.
|
% And we get the frequency vector
|
||||||
#+begin_src matlab
|
|
||||||
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
|
[~, f] = tfestimate(meas_data_lf{1}.Va, meas_data_lf{1}.de, win, [], [], 1/Ts);
|
||||||
#+end_src
|
|
||||||
|
|
||||||
#+begin_src matlab
|
|
||||||
i_lf = f < 250; % Points for low frequency excitation
|
i_lf = f < 250; % Points for low frequency excitation
|
||||||
i_hf = f > 250; % Points for high frequency excitation
|
i_hf = f > 250; % Points for high frequency excitation
|
||||||
#+end_src
|
#+end_src
|
||||||
@ -206,10 +203,10 @@ for i = 1:6
|
|||||||
coh_dvf_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
coh_dvf_lf(:, :, i) = mscohere(meas_data_lf{i}.Va, meas_data_lf{i}.de, win, [], [], 1/Ts);
|
||||||
coh_dvf_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
|
coh_dvf_hf(:, :, i) = mscohere(meas_data_hf{i}.Va, meas_data_hf{i}.de, win, [], [], 1/Ts);
|
||||||
end
|
end
|
||||||
|
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Coherence for the transfer function from u to dLm
|
||||||
figure;
|
figure;
|
||||||
hold on;
|
hold on;
|
||||||
for i = 1:5
|
for i = 1:5
|
||||||
@ -248,7 +245,7 @@ exportFig('figs/enc_struts_dvf_coh.pdf', 'width', 'wide', 'height', 'normal');
|
|||||||
|
|
||||||
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_dvf_frf]]).
|
Then the 6x6 transfer function matrix is estimated (Figure [[fig:enc_struts_dvf_frf]]).
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
%% DVF Plant
|
%% DVF Plant (transfer function from u to dLm)
|
||||||
G_dvf_lf = zeros(length(f), 6, 6);
|
G_dvf_lf = zeros(length(f), 6, 6);
|
||||||
G_dvf_hf = zeros(length(f), 6, 6);
|
G_dvf_hf = zeros(length(f), 6, 6);
|
||||||
|
|
||||||
@ -259,6 +256,7 @@ end
|
|||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Bode plot for the transfer function from u to dLm
|
||||||
figure;
|
figure;
|
||||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||||
|
|
||||||
@ -320,7 +318,7 @@ exportFig('figs/enc_struts_dvf_frf.pdf', 'width', 'wide', 'height', 'tall');
|
|||||||
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_struts_iff_coh]]).
|
First, let's compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure [[fig:enc_struts_iff_coh]]).
|
||||||
|
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
%% Coherence
|
%% Coherence for the IFF plant
|
||||||
coh_iff_lf = zeros(length(f), 6, 6);
|
coh_iff_lf = zeros(length(f), 6, 6);
|
||||||
coh_iff_hf = zeros(length(f), 6, 6);
|
coh_iff_hf = zeros(length(f), 6, 6);
|
||||||
|
|
||||||
@ -332,6 +330,7 @@ end
|
|||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Coherence of the IFF Plant (transfer function from u to taum)
|
||||||
figure;
|
figure;
|
||||||
hold on;
|
hold on;
|
||||||
for i = 1:5
|
for i = 1:5
|
||||||
@ -381,6 +380,7 @@ end
|
|||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Bode plot of the IFF Plant (transfer function from u to taum)
|
||||||
figure;
|
figure;
|
||||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||||
|
|
||||||
@ -619,6 +619,7 @@ In this section, the measured dynamics is compared with the dynamics estimated f
|
|||||||
|
|
||||||
*** Initialize :noexport:
|
*** Initialize :noexport:
|
||||||
#+begin_src matlab :tangle no
|
#+begin_src matlab :tangle no
|
||||||
|
%% Add all useful folders to the path
|
||||||
addpath('matlab/')
|
addpath('matlab/')
|
||||||
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
addpath('matlab/nass-simscape/matlab/nano_hexapod/')
|
||||||
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
addpath('matlab/nass-simscape/STEPS/nano_hexapod/')
|
||||||
@ -628,6 +629,7 @@ addpath('matlab/nass-simscape/mat/')
|
|||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :eval no
|
#+begin_src matlab :eval no
|
||||||
|
%% Add all useful folders to the path
|
||||||
addpath('nass-simscape/matlab/nano_hexapod/')
|
addpath('nass-simscape/matlab/nano_hexapod/')
|
||||||
addpath('nass-simscape/STEPS/nano_hexapod/')
|
addpath('nass-simscape/STEPS/nano_hexapod/')
|
||||||
addpath('nass-simscape/STEPS/png/')
|
addpath('nass-simscape/STEPS/png/')
|
||||||
@ -636,6 +638,7 @@ addpath('nass-simscape/mat/')
|
|||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
|
%% Open Simulink Model
|
||||||
mdl = 'nano_hexapod_simscape';
|
mdl = 'nano_hexapod_simscape';
|
||||||
|
|
||||||
options = linearizeOptions;
|
options = linearizeOptions;
|
||||||
@ -646,22 +649,24 @@ open(mdl)
|
|||||||
|
|
||||||
*** Dynamics from Actuator to Force Sensors
|
*** Dynamics from Actuator to Force Sensors
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
%% Initialize Nano-Hexapod
|
||||||
'flex_top_type', '2dof', ...
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
||||||
|
'flex_top_type', '4dof', ...
|
||||||
'motion_sensor_type', 'struts', ...
|
'motion_sensor_type', 'struts', ...
|
||||||
'actuator_type', '2dof');
|
'actuator_type', '2dof');
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
%% Input/Output definition
|
%% Identify the IFF Plant (transfer function from u to taum)
|
||||||
clear io; io_i = 1;
|
clear io; io_i = 1;
|
||||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||||
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors
|
||||||
|
|
||||||
Giff = 20*exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
Giff = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data
|
||||||
freqs = 2*logspace(1, 3, 1000);
|
freqs = 2*logspace(1, 3, 1000);
|
||||||
|
|
||||||
figure;
|
figure;
|
||||||
@ -669,21 +674,22 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|||||||
|
|
||||||
ax1 = nexttile([2,1]);
|
ax1 = nexttile([2,1]);
|
||||||
hold on;
|
hold on;
|
||||||
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
plot(f(i_lf), abs(G_iff_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ...
|
||||||
'DisplayName', '$\tau_{m,i}/u_i$ - Model')
|
|
||||||
plot(f(i_lf), abs(G_iff_lf(i_lf,1, 1)), ...
|
|
||||||
'DisplayName', '$\tau_{m,i}/u_i$ - FRF')
|
'DisplayName', '$\tau_{m,i}/u_i$ - FRF')
|
||||||
for i = 2:6
|
for i = 2:6
|
||||||
set(gca,'ColorOrderIndex',1);
|
set(gca,'ColorOrderIndex',2)
|
||||||
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ...
|
||||||
|
'HandleVisibility', 'off');
|
||||||
|
set(gca,'ColorOrderIndex',2)
|
||||||
|
plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ...
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Giff(1,1), freqs, 'Hz'))), '-', ...
|
||||||
|
'DisplayName', '$\tau_{m,i}/u_i$ - Model')
|
||||||
for i = 2:6
|
for i = 2:6
|
||||||
set(gca,'ColorOrderIndex',2)
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), ...
|
plot(freqs, abs(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-', ...
|
||||||
'HandleVisibility', 'off');
|
|
||||||
set(gca,'ColorOrderIndex',2)
|
|
||||||
plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), ...
|
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
hold off;
|
hold off;
|
||||||
@ -694,14 +700,12 @@ legend('location', 'southeast');
|
|||||||
ax2 = nexttile;
|
ax2 = nexttile;
|
||||||
hold on;
|
hold on;
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
set(gca,'ColorOrderIndex',1);
|
plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]);
|
||||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-');
|
plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2]);
|
||||||
end
|
end
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
set(gca,'ColorOrderIndex',2)
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)));
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Giff(i,i), freqs, 'Hz'))), '-');
|
||||||
set(gca,'ColorOrderIndex',2)
|
|
||||||
plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i)));
|
|
||||||
end
|
end
|
||||||
hold off;
|
hold off;
|
||||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
@ -723,31 +727,29 @@ exportFig('figs/enc_struts_iff_comp_simscape.pdf', 'width', 'wide', 'height', 't
|
|||||||
[[file:figs/enc_struts_iff_comp_simscape.png]]
|
[[file:figs/enc_struts_iff_comp_simscape.png]]
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Bode plot of the identified IFF Plant (Simscape) and measured FRF data (off-diagonal elements)
|
||||||
freqs = 2*logspace(1, 3, 1000);
|
freqs = 2*logspace(1, 3, 1000);
|
||||||
|
|
||||||
figure;
|
figure;
|
||||||
hold on;
|
hold on;
|
||||||
% Off diagonal terms
|
% Off diagonal terms
|
||||||
set(gca,'ColorOrderIndex',1);
|
plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), 'color', [0,0,0,0.2], ...
|
||||||
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), ...
|
'DisplayName', '$\tau_{m,i}/u_j$ - FRF')
|
||||||
'DisplayName', '$\tau_{m,i}/u_j$ - Model')
|
|
||||||
for i = 1:5
|
for i = 1:5
|
||||||
for j = i+1:6
|
for j = i+1:6
|
||||||
set(gca,'ColorOrderIndex',1);
|
plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), 'color', [0,0,0,0.2], ...
|
||||||
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), ...
|
'HandleVisibility', 'off');
|
||||||
|
plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), 'color', [0,0,0,0.2], ...
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
set(gca,'ColorOrderIndex',2);
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), ...
|
plot(freqs, abs(squeeze(freqresp(Giff(1, 2), freqs, 'Hz'))), ...
|
||||||
'DisplayName', '$\tau_{m,i}/u_j$ - FRF')
|
'DisplayName', '$\tau_{m,i}/u_j$ - Model')
|
||||||
for i = 1:5
|
for i = 1:5
|
||||||
for j = i+1:6
|
for j = i+1:6
|
||||||
set(gca,'ColorOrderIndex',2);
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), ...
|
plot(freqs, abs(squeeze(freqresp(Giff(i, j), freqs, 'Hz'))), ...
|
||||||
'HandleVisibility', 'off');
|
|
||||||
set(gca,'ColorOrderIndex',2);
|
|
||||||
plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), ...
|
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
@ -769,22 +771,24 @@ exportFig('figs/enc_struts_iff_comp_offdiag_simscape.pdf', 'width', 'wide', 'hei
|
|||||||
|
|
||||||
*** Dynamics from Actuator to Encoder
|
*** Dynamics from Actuator to Encoder
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
|
%% Initialization of the Nano-Hexapod
|
||||||
'flex_top_type', '2dof', ...
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
||||||
|
'flex_top_type', '4dof', ...
|
||||||
'motion_sensor_type', 'struts', ...
|
'motion_sensor_type', 'struts', ...
|
||||||
'actuator_type', '2dof');
|
'actuator_type', '2dof');
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab
|
#+begin_src matlab
|
||||||
%% Input/Output definition
|
%% Identify the DVF Plant (transfer function from u to dLm)
|
||||||
clear io; io_i = 1;
|
clear io; io_i = 1;
|
||||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||||
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Encoders
|
||||||
|
|
||||||
Gdvf = 20*exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
Gdvf = exp(-s*Ts)*linearize(mdl, io, 0.0, options);
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Diagonal elements of the DVF plant
|
||||||
freqs = 2*logspace(1, 3, 1000);
|
freqs = 2*logspace(1, 3, 1000);
|
||||||
|
|
||||||
figure;
|
figure;
|
||||||
@ -792,40 +796,39 @@ tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
|||||||
|
|
||||||
ax1 = nexttile([2,1]);
|
ax1 = nexttile([2,1]);
|
||||||
hold on;
|
hold on;
|
||||||
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
plot(f(i_lf), abs(G_dvf_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ...
|
||||||
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
|
||||||
plot(f(i_lf), abs(G_dvf_lf(i_lf,1, 1)), ...
|
|
||||||
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - FRF')
|
||||||
for i = 2:6
|
for i = 2:6
|
||||||
set(gca,'ColorOrderIndex',1);
|
set(gca,'ColorOrderIndex',2)
|
||||||
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ...
|
||||||
|
'HandleVisibility', 'off');
|
||||||
|
set(gca,'ColorOrderIndex',2)
|
||||||
|
plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ...
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1,1), freqs, 'Hz'))), '-', ...
|
||||||
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_i$ - Model')
|
||||||
for i = 2:6
|
for i = 2:6
|
||||||
set(gca,'ColorOrderIndex',2)
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), abs(G_dvf_lf(i_lf,i, i)), ...
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-', ...
|
||||||
'HandleVisibility', 'off');
|
|
||||||
set(gca,'ColorOrderIndex',2)
|
|
||||||
plot(f(i_hf), abs(G_dvf_hf(i_hf,i, i)), ...
|
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
hold off;
|
hold off;
|
||||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||||||
legend('location', 'northeast');
|
|
||||||
ylim([1e-8, 1e-3]);
|
ylim([1e-8, 1e-3]);
|
||||||
|
legend('location', 'northeast');
|
||||||
|
|
||||||
ax2 = nexttile;
|
ax2 = nexttile;
|
||||||
hold on;
|
hold on;
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
set(gca,'ColorOrderIndex',1);
|
plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)), 'color', [0,0,0,0.2]);
|
||||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-');
|
plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i)), 'color', [0,0,0,0.2]);
|
||||||
end
|
end
|
||||||
for i = 1:6
|
for i = 1:6
|
||||||
set(gca,'ColorOrderIndex',2)
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), 180/pi*angle(G_dvf_lf(i_lf,i, i)));
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gdvf(i,i), freqs, 'Hz'))), '-');
|
||||||
set(gca,'ColorOrderIndex',2)
|
|
||||||
plot(f(i_hf), 180/pi*angle(G_dvf_hf(i_hf,i, i)));
|
|
||||||
end
|
end
|
||||||
hold off;
|
hold off;
|
||||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
@ -847,31 +850,29 @@ exportFig('figs/enc_struts_dvf_comp_simscape.pdf', 'width', 'wide', 'height', 't
|
|||||||
[[file:figs/enc_struts_dvf_comp_simscape.png]]
|
[[file:figs/enc_struts_dvf_comp_simscape.png]]
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Off-diagonal elements of the DVF plant
|
||||||
freqs = 2*logspace(1, 3, 1000);
|
freqs = 2*logspace(1, 3, 1000);
|
||||||
|
|
||||||
figure;
|
figure;
|
||||||
hold on;
|
hold on;
|
||||||
% Off diagonal terms
|
% Off diagonal terms
|
||||||
set(gca,'ColorOrderIndex',1);
|
plot(f(i_lf), abs(G_dvf_lf(i_lf, 1, 2)), 'color', [0,0,0,0.2], ...
|
||||||
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), ...
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
||||||
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
|
||||||
for i = 1:5
|
for i = 1:5
|
||||||
for j = i+1:6
|
for j = i+1:6
|
||||||
set(gca,'ColorOrderIndex',1);
|
plot(f(i_lf), abs(G_dvf_lf(i_lf, i, j)), 'color', [0,0,0,0.2], ...
|
||||||
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), ...
|
'HandleVisibility', 'off');
|
||||||
|
plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), 'color', [0,0,0,0.2], ...
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
set(gca,'ColorOrderIndex',2);
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), abs(G_dvf_lf(i_lf, 1, 2)), ...
|
plot(freqs, abs(squeeze(freqresp(Gdvf(1, 2), freqs, 'Hz'))), ...
|
||||||
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - FRF')
|
'DisplayName', '$d\mathcal{L}_{m,i}/u_j$ - Model')
|
||||||
for i = 1:5
|
for i = 1:5
|
||||||
for j = i+1:6
|
for j = i+1:6
|
||||||
set(gca,'ColorOrderIndex',2);
|
set(gca,'ColorOrderIndex',2);
|
||||||
plot(f(i_lf), abs(G_dvf_lf(i_lf, i, j)), ...
|
plot(freqs, abs(squeeze(freqresp(Gdvf(i, j), freqs, 'Hz'))), ...
|
||||||
'HandleVisibility', 'off');
|
|
||||||
set(gca,'ColorOrderIndex',2);
|
|
||||||
plot(f(i_hf), abs(G_dvf_hf(i_hf, i, j)), ...
|
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
end
|
end
|
||||||
@ -891,58 +892,196 @@ exportFig('figs/enc_struts_dvf_comp_offdiag_simscape.pdf', 'width', 'wide', 'hei
|
|||||||
#+RESULTS:
|
#+RESULTS:
|
||||||
[[file:figs/enc_struts_dvf_comp_offdiag_simscape.png]]
|
[[file:figs/enc_struts_dvf_comp_offdiag_simscape.png]]
|
||||||
|
|
||||||
** TODO Integral Force Feedback
|
** Integral Force Feedback
|
||||||
*** Plant
|
*** Root Locus and Decentralized Loop gain
|
||||||
|
#+begin_src matlab
|
||||||
|
%% IFF Controller
|
||||||
|
Kiff_g1 = (1/(s + 2*pi*40))*... % Low pass filter (provides integral action above 40Hz)
|
||||||
|
(s/(s + 2*pi*30))*... % High pass filter to limit low frequency gain
|
||||||
|
(1/(1 + s/2/pi/500))*... % Low pass filter to be more robust to high frequency resonances
|
||||||
|
eye(6); % Diagonal 6x6 controller
|
||||||
|
#+end_src
|
||||||
|
|
||||||
#+begin_src matlab :exports none
|
#+begin_src matlab :exports none
|
||||||
|
%% Root Locus for IFF
|
||||||
|
gains = logspace(1, 4, 100);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
|
||||||
|
hold on;
|
||||||
|
% Pure Integrator
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(real(pole(Giff)), imag(pole(Giff)), 'x', 'DisplayName', '$g = 0$');
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(real(tzero(Giff)), imag(tzero(Giff)), 'o', 'HandleVisibility', 'off');
|
||||||
|
|
||||||
|
for g = gains
|
||||||
|
clpoles = pole(feedback(Giff, g*Kiff_g1*eye(6)));
|
||||||
|
set(gca,'ColorOrderIndex',1);
|
||||||
|
plot(real(clpoles), imag(clpoles), '.', 'HandleVisibility', 'off');
|
||||||
|
end
|
||||||
|
|
||||||
|
g = 4e2;
|
||||||
|
clpoles = pole(feedback(Giff, g*Kiff_g1*eye(6)));
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(real(clpoles), imag(clpoles), 'x', 'DisplayName', sprintf('$g=%.0f$', g));
|
||||||
|
hold off;
|
||||||
|
axis square;
|
||||||
|
xlim([-1250, 0]); ylim([0, 1250]);
|
||||||
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
||||||
|
legend('location', 'northwest');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/enc_struts_iff_root_locus.pdf', 'width', 'wide', 'height', 'tall');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:enc_struts_iff_root_locus
|
||||||
|
#+caption: Root Locus for the IFF control strategy
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/enc_struts_iff_root_locus.png]]
|
||||||
|
|
||||||
|
Then the "optimal" IFF controller is:
|
||||||
|
#+begin_src matlab
|
||||||
|
%% IFF controller with Optimal gain
|
||||||
|
Kiff = g*Kiff_g1;
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Bode plot of the "decentralized loop gain"
|
||||||
|
freqs = 2*logspace(1, 3, 1000);
|
||||||
|
|
||||||
figure;
|
figure;
|
||||||
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||||
|
|
||||||
ax1 = nexttile([2,1]);
|
ax1 = nexttile([2,1]);
|
||||||
hold on;
|
hold on;
|
||||||
for i = 1:5
|
plot(f(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,1, 1)), 'color', [0,0,0,0.2], ...
|
||||||
for j = i+1:6
|
'DisplayName', '$\tau_{m,i}/u_i \cdot K_{iff}$ - FRF')
|
||||||
plot(f(i_lf), abs(G_iff_lf(i_lf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
for i = 2:6
|
||||||
'HandleVisibility', 'off');
|
set(gca,'ColorOrderIndex',2)
|
||||||
plot(f(i_hf), abs(G_iff_hf(i_hf, i, j)), 'color', [0, 0, 0, 0.2], ...
|
plot(f(i_lf), abs(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2], ...
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
set(gca,'ColorOrderIndex',2)
|
||||||
end
|
plot(f(i_hf), abs(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2], ...
|
||||||
for i =1:6
|
'HandleVisibility', 'off');
|
||||||
set(gca,'ColorOrderIndex',i)
|
end
|
||||||
plot(f(i_lf), abs(G_iff_lf(i_lf,i, i)), ...
|
set(gca,'ColorOrderIndex',2);
|
||||||
'DisplayName', sprintf('$G_{iff}(%i,%i)$', i, i));
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*Giff(1,1), freqs, 'Hz'))), '-', ...
|
||||||
set(gca,'ColorOrderIndex',i)
|
'DisplayName', '$\tau_{m,i}/u_i \cdot K_{iff}$ - Model')
|
||||||
plot(f(i_hf), abs(G_iff_hf(i_hf,i, i)), ...
|
for i = 2:6
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Kiff(1,1)*Giff(i,i), freqs, 'Hz'))), '-', ...
|
||||||
'HandleVisibility', 'off');
|
'HandleVisibility', 'off');
|
||||||
end
|
end
|
||||||
plot(f(i_lf), abs(G_iff_lf(i_lf, 1, 2)), 'color', [0, 0, 0, 0.2], ...
|
|
||||||
'DisplayName', '$G_{iff}(i,j)$');
|
|
||||||
hold off;
|
hold off;
|
||||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
ylabel('Amplitude $V_s/V_a$ [V/V]'); set(gca, 'XTickLabel',[]);
|
ylabel('Amplitude [V/V]'); set(gca, 'XTickLabel',[]);
|
||||||
legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 3);
|
legend('location', 'northeast');
|
||||||
ylim([1e-3, 1e2]);
|
|
||||||
|
|
||||||
ax2 = nexttile;
|
ax2 = nexttile;
|
||||||
hold on;
|
hold on;
|
||||||
for i =1:6
|
for i = 1:6
|
||||||
set(gca,'ColorOrderIndex',i)
|
plot(f(i_lf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_lf), 'Hz')).*G_iff_lf(i_lf,i, i)), 'color', [0,0,0,0.2]);
|
||||||
plot(f(i_lf), 180/pi*angle(G_iff_lf(i_lf,i, i)));
|
plot(f(i_hf), 180/pi*angle(squeeze(freqresp(Kiff(1,1), f(i_hf), 'Hz')).*G_iff_hf(i_hf,i, i)), 'color', [0,0,0,0.2]);
|
||||||
set(gca,'ColorOrderIndex',i)
|
end
|
||||||
plot(f(i_hf), 180/pi*angle(G_iff_hf(i_hf,i, i)));
|
for i = 1:6
|
||||||
|
set(gca,'ColorOrderIndex',2);
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Kiff(1,1)*Giff(i,i), freqs, 'Hz'))), '-');
|
||||||
end
|
end
|
||||||
hold off;
|
hold off;
|
||||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
hold off;
|
ylim([-180, 180]);
|
||||||
yticks(-360:90:360);
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
linkaxes([ax1,ax2],'x');
|
linkaxes([ax1,ax2],'x');
|
||||||
xlim([20, 2e3]);
|
xlim([freqs(1), freqs(end)]);
|
||||||
#+end_src
|
#+end_src
|
||||||
|
|
||||||
*** Root Locus
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/enc_struts_iff_opt_loop_gain.pdf', 'width', 'wide', 'height', 'tall');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
*** Gains
|
#+name: fig:enc_struts_iff_opt_loop_gain
|
||||||
|
#+caption: Bode plot of the "decentralized loop gain" $G_\text{iff}(i,i) \times K_\text{iff}(i,i)$
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/enc_struts_iff_opt_loop_gain.png]]
|
||||||
|
|
||||||
|
*** Multiple Gains - Simulation
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Tested IFF gains
|
||||||
|
iff_gains = [4, 10, 20, 40, 100, 200, 400, 1000];
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Initialize the Simscape model in closed loop
|
||||||
|
n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
|
||||||
|
'flex_top_type', '4dof', ...
|
||||||
|
'motion_sensor_type', 'struts', ...
|
||||||
|
'actuator_type', '2dof', ...
|
||||||
|
'controller_type', 'iff');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab
|
||||||
|
%% Identify the (damped) transfer function from u to dLm for different values of the IFF gain
|
||||||
|
Gd_iff = {zeros(1, length(iff_gains))};
|
||||||
|
|
||||||
|
clear io; io_i = 1;
|
||||||
|
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs
|
||||||
|
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Strut Displacement (encoder)
|
||||||
|
|
||||||
|
for i = 1:length(iff_gains)
|
||||||
|
Kiff = iff_gains(i)*Kiff_g1*eye(6); % IFF Controller
|
||||||
|
Gd_iff(i) = {exp(-s*Ts)*linearize(mdl, io, 0.0, options)};
|
||||||
|
|
||||||
|
isstable(Gd_iff{i})
|
||||||
|
end
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :exports none
|
||||||
|
%% Bode plot of the transfer function from u to dLm for tested values of the IFF gain
|
||||||
|
freqs = 2*logspace(1, 3, 1000);
|
||||||
|
|
||||||
|
figure;
|
||||||
|
tiledlayout(3, 1, 'TileSpacing', 'None', 'Padding', 'None');
|
||||||
|
|
||||||
|
ax1 = nexttile([2,1]);
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(iff_gains)
|
||||||
|
plot(freqs, abs(squeeze(freqresp(Gd_iff{i}(1,1), freqs, 'Hz'))), '-', ...
|
||||||
|
'DisplayName', sprintf('$g = %.0f$', iff_gains(i)));
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||||
|
ylabel('Amplitude [m/V]'); set(gca, 'XTickLabel',[]);
|
||||||
|
legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 2);
|
||||||
|
|
||||||
|
ax2 = nexttile;
|
||||||
|
hold on;
|
||||||
|
for i = 1:length(iff_gains)
|
||||||
|
plot(freqs, 180/pi*angle(squeeze(freqresp(Gd_iff{i}(1,1), freqs, 'Hz'))), '-');
|
||||||
|
end
|
||||||
|
hold off;
|
||||||
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||||
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||||
|
ylim([-180, 180]);
|
||||||
|
yticks([-180, -90, 0, 90, 180]);
|
||||||
|
|
||||||
|
linkaxes([ax1,ax2],'x');
|
||||||
|
xlim([freqs(1), freqs(end)]);
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+begin_src matlab :tangle no :exports results :results file replace
|
||||||
|
exportFig('figs/enc_struts_iff_gains_effect_dvf_plant.pdf', 'width', 'wide', 'height', 'tall');
|
||||||
|
#+end_src
|
||||||
|
|
||||||
|
#+name: fig:enc_struts_iff_gains_effect_dvf_plant
|
||||||
|
#+caption: Effect of the IFF gain $g$ on the transfer function from $\bm{\tau}$ to $d\bm{\mathcal{L}}_m$
|
||||||
|
#+RESULTS:
|
||||||
|
[[file:figs/enc_struts_iff_gains_effect_dvf_plant.png]]
|
||||||
|
|
||||||
*** Experimental Results
|
*** Experimental Results
|
||||||
|
|
||||||
|
* Encoders fixed to the plates
|
||||||
|
** Introduction :ignore:
|
||||||
|