3218 lines
156 KiB
HTML
3218 lines
156 KiB
HTML
<?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>
|
|
<!-- 2021-06-30 mer. 22:41 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<title>Nano-Hexapod - Test Bench</title>
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<meta name="generator" content="Org Mode" />
|
|
<link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
|
|
<script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
<script>
|
|
MathJax = {
|
|
svg: {
|
|
scale: 1,
|
|
fontCache: "global"
|
|
},
|
|
tex: {
|
|
tags: "ams",
|
|
multlineWidth: "%MULTLINEWIDTH",
|
|
tagSide: "right",
|
|
macros: {bm: ["\\boldsymbol{#1}",1],},
|
|
tagIndent: ".8em"
|
|
}
|
|
};
|
|
</script>
|
|
<script id="MathJax-script" async
|
|
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-svg.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">Nano-Hexapod - Test Bench</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#org850a381">1. Encoders fixed to the Struts</a>
|
|
<ul>
|
|
<li><a href="#org9fdd0ef">1.1. Introduction</a></li>
|
|
<li><a href="#org3af1b8c">1.2. Identification of the dynamics</a>
|
|
<ul>
|
|
<li><a href="#org668e8ab">1.2.1. Load Measurement Data</a></li>
|
|
<li><a href="#orgdd451e1">1.2.2. Spectral Analysis - Setup</a></li>
|
|
<li><a href="#org502869a">1.2.3. DVF Plant</a></li>
|
|
<li><a href="#org294fac1">1.2.4. IFF Plant</a></li>
|
|
<li><a href="#org27bc005">1.2.5. Save Identified Plants</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org16dfab3">1.3. Comparison with the Simscape Model</a>
|
|
<ul>
|
|
<li><a href="#orgbec3680">1.3.1. Load measured FRF</a></li>
|
|
<li><a href="#org416f14a">1.3.2. Dynamics from Actuator to Force Sensors</a></li>
|
|
<li><a href="#org7740929">1.3.3. Dynamics from Actuator to Encoder</a></li>
|
|
<li><a href="#org07b9456">1.3.4. Effect of a change in bending damping of the joints</a></li>
|
|
<li><a href="#org74cdaef">1.3.5. Effect of a change in damping factor of the APA</a></li>
|
|
<li><a href="#org2265b20">1.3.6. Effect of a change in stiffness damping coef of the APA</a></li>
|
|
<li><a href="#org81f3c3c">1.3.7. Effect of a change in mass damping coef of the APA</a></li>
|
|
<li><a href="#org35c44c1">1.3.8. Using Flexible model</a></li>
|
|
<li><a href="#org90aa71a">1.3.9. Flexible model + encoders fixed to the plates</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org3cf2cf1">1.4. Integral Force Feedback</a>
|
|
<ul>
|
|
<li><a href="#orgfef1ee8">1.4.1. Identification of the IFF Plant</a></li>
|
|
<li><a href="#orgabb73e5">1.4.2. Root Locus and Decentralized Loop gain</a></li>
|
|
<li><a href="#orgbdac543">1.4.3. Multiple Gains - Simulation</a></li>
|
|
<li><a href="#org3bd1e0b">1.4.4. Experimental Results - Gains</a>
|
|
<ul>
|
|
<li><a href="#org69457dc">1.4.4.1. Load Data</a></li>
|
|
<li><a href="#orge77daca">1.4.4.2. Spectral Analysis - Setup</a></li>
|
|
<li><a href="#orga18329e">1.4.4.3. DVF Plant</a></li>
|
|
<li><a href="#orgd7b3950">1.4.4.4. Experimental Results - Comparison of the un-damped and fully damped system</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org37f477e">1.4.5. Experimental Results - Damped Plant with Optimal gain</a>
|
|
<ul>
|
|
<li><a href="#org2716b9a">1.4.5.1. Load Data</a></li>
|
|
<li><a href="#org8de917f">1.4.5.2. Spectral Analysis - Setup</a></li>
|
|
<li><a href="#orgfa9bce2">1.4.5.3. DVF Plant</a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org4284ac6">1.5. Modal Analysis</a>
|
|
<ul>
|
|
<li><a href="#org2d0c26f">1.5.1. Effectiveness of the IFF Strategy - Compliance</a></li>
|
|
<li><a href="#org3ff9878">1.5.2. Comparison with the Simscape Model</a></li>
|
|
<li><a href="#orgd41f51c">1.5.3. Obtained Mode Shapes</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org2527b87">1.6. Accelerometers fixed on the top platform</a>
|
|
<ul>
|
|
<li><a href="#org1fd49c3">1.6.1. Experimental Identification</a></li>
|
|
<li><a href="#org4f8d7c7">1.6.2. Location and orientation of accelerometers</a></li>
|
|
<li><a href="#org86ce650">1.6.3. COM</a></li>
|
|
<li><a href="#org4309cac">1.6.4. COK</a></li>
|
|
<li><a href="#org2577983">1.6.5. Comp with the Simscape Model</a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org712faa3">2. Encoders fixed to the plates</a>
|
|
<ul>
|
|
<li><a href="#orgd3ae77d">2.1. Identification of the dynamics</a>
|
|
<ul>
|
|
<li><a href="#org7a53f0c">2.1.1. Load Measurement Data</a></li>
|
|
<li><a href="#orgd3052ca">2.1.2. Spectral Analysis - Setup</a></li>
|
|
<li><a href="#orga653b6d">2.1.3. DVF Plant</a></li>
|
|
<li><a href="#orgfe3a401">2.1.4. IFF Plant</a></li>
|
|
<li><a href="#orge8c6a42">2.1.5. Save Identified Plants</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgdfb05f4">2.2. Comparison with the Simscape Model</a>
|
|
<ul>
|
|
<li><a href="#org37605d2">2.2.1. Load measured FRF</a></li>
|
|
<li><a href="#org793238f">2.2.2. Dynamics from Actuator to Force Sensors</a></li>
|
|
<li><a href="#org9c52c3e">2.2.3. Dynamics from Actuator to Encoder</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org3616410">2.3. Integral Force Feedback</a>
|
|
<ul>
|
|
<li><a href="#org8cbdfcb">2.3.1. Identification of the IFF Plant</a></li>
|
|
<li><a href="#orgab3fa5e">2.3.2. Effect of IFF on the plant - Simscape Model</a></li>
|
|
<li><a href="#org1fbe8b3">2.3.3. Experimental Results - Damped Plant with Optimal gain</a>
|
|
<ul>
|
|
<li><a href="#org456a248">2.3.3.1. Load Data</a></li>
|
|
<li><a href="#org54fe798">2.3.3.2. Spectral Analysis - Setup</a></li>
|
|
<li><a href="#orgf69ea08">2.3.3.3. Simscape Model</a></li>
|
|
<li><a href="#orgf21ad91">2.3.3.4. DVF Plant</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org9090d9d">2.3.4. Effect of IFF on the plant - FRF</a></li>
|
|
<li><a href="#org5a28e2e">2.3.5. Save Damped Plant</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org14e43ac">2.4. HAC Control - Frame of the struts</a>
|
|
<ul>
|
|
<li><a href="#org991fc81">2.4.1. Simscape Model</a></li>
|
|
<li><a href="#org457939e">2.4.2. HAC Controller</a></li>
|
|
<li><a href="#org98d910a">2.4.3. Experimental Loop Gain</a></li>
|
|
<li><a href="#org317850e">2.4.4. Verification of the Stability using the Simscape model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org247dba8">2.5. Reference Tracking</a>
|
|
<ul>
|
|
<li><a href="#orgd862f99">2.5.1. Load</a></li>
|
|
<li><a href="#org2a92c68">2.5.2. Y-Z Scans</a>
|
|
<ul>
|
|
<li><a href="#org3c5f01d">2.5.2.1. Generate the Scan</a></li>
|
|
<li><a href="#org213a892">2.5.2.2. Reference Signal for the Strut lengths</a></li>
|
|
<li><a href="#orge04e29b">2.5.2.3. Time domain simulation with 2DoF model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org995b778">2.5.3. “NASS” reference path</a>
|
|
<ul>
|
|
<li><a href="#org065d872">2.5.3.1. Generate Path</a></li>
|
|
<li><a href="#org57675af">2.5.3.2. Simscape Simulations</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org75465ae">2.5.4. Save Reference paths</a></li>
|
|
<li><a href="#orge43ac25">2.5.5. Experimental Results</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org71bb087">2.6. Feedforward (Open-Loop) Control</a>
|
|
<ul>
|
|
<li><a href="#org3ea802c">2.6.1. Introduction</a></li>
|
|
<li><a href="#orgc7d508e">2.6.2. Simple Feedforward Controller</a></li>
|
|
<li><a href="#org4c2c4ae">2.6.3. Test with Simscape Model</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgd127949">2.7. Feedback/Feedforward control in the frame of the struts</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org762064f">3. Functions</a>
|
|
<ul>
|
|
<li><a href="#org9c82eb4">3.1. <code>generateXYZTrajectory</code></a>
|
|
<ul>
|
|
<li><a href="#org6177d10">Function description</a></li>
|
|
<li><a href="#orgbf6b8b1">Optional Parameters</a></li>
|
|
<li><a href="#orgb818291">Initialize Time Vectors</a></li>
|
|
<li><a href="#org361222a">XYZ Trajectory</a></li>
|
|
<li><a href="#org84542e6">Reference Signal</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org4433a1b">3.2. <code>generateYZScanTrajectory</code></a>
|
|
<ul>
|
|
<li><a href="#org0479bfd">Function description</a></li>
|
|
<li><a href="#org7be834b">Optional Parameters</a></li>
|
|
<li><a href="#org81214f7">Initialize Time Vectors</a></li>
|
|
<li><a href="#org5ab49af">Y and Z vectors</a></li>
|
|
<li><a href="#org796db82">Reference Signal</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org01056ff">3.3. <code>getTransformationMatrixAcc</code></a>
|
|
<ul>
|
|
<li><a href="#org37f755d">Function description</a></li>
|
|
<li><a href="#orgf2435ad">Transformation matrix from motion of the solid body to accelerometer measurements</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgab18063">3.4. <code>getJacobianNanoHexapod</code></a>
|
|
<ul>
|
|
<li><a href="#org7e86fe2">Function description</a></li>
|
|
<li><a href="#orgb9e790e">Transformation matrix from motion of the solid body to accelerometer measurements</a></li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
<hr>
|
|
<p>This report is also available as a <a href="./test-bench-nano-hexapod.pdf">pdf</a>.</p>
|
|
<hr>
|
|
|
|
<p>
|
|
This document is dedicated to the experimental study of the nano-hexapod shown in Figure <a href="#org1278ac9">1</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org1278ac9" class="figure">
|
|
<p><img src="figs/IMG_20210608_152917.jpg" alt="IMG_20210608_152917.jpg" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 1: </span>Nano-Hexapod</p>
|
|
</div>
|
|
|
|
<div class="note" id="org19f78fa">
|
|
<p>
|
|
Here are the documentation of the equipment used for this test bench (lots of them are shwon in Figure <a href="#orga71ba57">2</a>):
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Voltage Amplifier: PiezoDrive <a href="doc/PD200-V7-R1.pdf">PD200</a></li>
|
|
<li>Amplified Piezoelectric Actuator: Cedrat <a href="doc/APA300ML.pdf">APA300ML</a></li>
|
|
<li>DAC/ADC: Speedgoat <a href="doc/IO131-OEM-Datasheet.pdf">IO313</a></li>
|
|
<li>Encoder: Renishaw <a href="doc/L-9517-9678-05-A_Data_sheet_VIONiC_series_en.pdf">Vionic</a> and used <a href="doc/L-9517-9862-01-C_Data_sheet_RKLC_EN.pdf">Ruler</a></li>
|
|
<li>Interferometers: Attocube</li>
|
|
</ul>
|
|
|
|
</div>
|
|
|
|
|
|
<div id="orga71ba57" class="figure">
|
|
<p><img src="figs/IMG_20210608_154722.jpg" alt="IMG_20210608_154722.jpg" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 2: </span>Nano-Hexapod and the control electronics</p>
|
|
</div>
|
|
|
|
<p>
|
|
In Figure <a href="#org5326b3a">3</a> is shown a block diagram of the experimental setup.
|
|
When possible, the notations are consistent with this diagram and summarized in Table <a href="#org0036e4f">1</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org5326b3a" 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="org0036e4f" 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>
|
|
|
|
<p>
|
|
This document is divided in the following sections:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Section <a href="#org261275f">1</a>: the encoders are fixed to the struts</li>
|
|
<li>Section <a href="#orgcf084fc">2</a>: the encoders are fixed to the plates</li>
|
|
</ul>
|
|
|
|
<div id="outline-container-org850a381" class="outline-2">
|
|
<h2 id="org850a381"><span class="section-number-2">1</span> Encoders fixed to the Struts</h2>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="org261275f"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org9fdd0ef" class="outline-3">
|
|
<h3 id="org9fdd0ef"><span class="section-number-3">1.1</span> Introduction</h3>
|
|
<div class="outline-text-3" id="text-1-1">
|
|
<p>
|
|
In this section, the encoders are fixed to the struts.
|
|
</p>
|
|
|
|
<p>
|
|
It is divided in the following sections:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Section <a href="#org0c70bc8">1.2</a>: the transfer function matrix from the actuators to the force sensors and to the encoders is experimentally identified.</li>
|
|
<li>Section <a href="#orgb7175f9">1.3</a>: the obtained FRF matrix is compared with the dynamics of the simscape model</li>
|
|
<li>Section <a href="#orgffc7d0d">1.4</a>: decentralized Integral Force Feedback (IFF) is applied and its performances are evaluated.</li>
|
|
<li>Section <a href="#org10a48cf">1.5</a>: a modal analysis of the nano-hexapod is performed</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3af1b8c" class="outline-3">
|
|
<h3 id="org3af1b8c"><span class="section-number-3">1.2</span> Identification of the dynamics</h3>
|
|
<div class="outline-text-3" id="text-1-2">
|
|
<p>
|
|
<a id="org0c70bc8"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org668e8ab" class="outline-4">
|
|
<h4 id="org668e8ab"><span class="section-number-4">1.2.1</span> Load Measurement Data</h4>
|
|
<div class="outline-text-4" id="text-1-2-1">
|
|
<div class="org-src-container">
|
|
<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>
|
|
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_hf(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/frf_data_exc_strut_%i_noise_hf.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>)};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdd451e1" class="outline-4">
|
|
<h4 id="orgdd451e1"><span class="section-number-4">1.2.2</span> Spectral Analysis - Setup</h4>
|
|
<div class="outline-text-4" id="text-1-2-2">
|
|
<div class="org-src-container">
|
|
<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);
|
|
|
|
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
|
Fs = 1<span class="org-type">/</span>Ts;
|
|
|
|
<span class="org-comment">% Hannning Windows</span>
|
|
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
|
|
|
<span class="org-comment">% And we get the frequency vector</span>
|
|
[<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);
|
|
|
|
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>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org502869a" class="outline-4">
|
|
<h4 id="org502869a"><span class="section-number-4">1.2.3</span> DVF Plant</h4>
|
|
<div class="outline-text-4" id="text-1-2-3">
|
|
<p>
|
|
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#orgca37e8c">4</a>).
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Coherence</span></span>
|
|
coh_dvf = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
coh_dvf_lf = 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 = 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(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [coh_dvf_lf(i_lf, <span class="org-type">:</span>); coh_dvf_hf(i_hf, <span class="org-type">:</span>)];
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgca37e8c" class="figure">
|
|
<p><img src="figs/enc_struts_dvf_coh.png" alt="enc_struts_dvf_coh.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 4: </span>Obtained coherence for the DVF plant</p>
|
|
</div>
|
|
|
|
<p>
|
|
Then the 6x6 transfer function matrix is estimated (Figure <a href="#org4233c6b">5</a>).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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 = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
G_dvf_lf = tfestimate(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);
|
|
G_dvf_hf = tfestimate(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);
|
|
G_dvf(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [G_dvf_lf(i_lf, <span class="org-type">:</span>); G_dvf_hf(i_hf, <span class="org-type">:</span>)];
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org4233c6b" class="figure">
|
|
<p><img src="figs/enc_struts_dvf_frf.png" alt="enc_struts_dvf_frf.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 5: </span>Measured FRF for the DVF plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org294fac1" class="outline-4">
|
|
<h4 id="org294fac1"><span class="section-number-4">1.2.4</span> IFF Plant</h4>
|
|
<div class="outline-text-4" id="text-1-2-4">
|
|
<p>
|
|
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#org50b0b59">6</a>).
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Coherence for the IFF plant</span></span>
|
|
coh_iff = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
coh_iff_lf = mscohere(meas_data_lf{<span class="org-constant">i</span>}.Va, meas_data_lf{<span class="org-constant">i</span>}.Vs, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
coh_iff_hf = mscohere(meas_data_hf{<span class="org-constant">i</span>}.Va, meas_data_hf{<span class="org-constant">i</span>}.Vs, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
coh_iff(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [coh_iff_lf(i_lf, <span class="org-type">:</span>); coh_iff_hf(i_hf, <span class="org-type">:</span>)];
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org50b0b59" class="figure">
|
|
<p><img src="figs/enc_struts_iff_coh.png" alt="enc_struts_iff_coh.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 6: </span>Obtained coherence for the IFF plant</p>
|
|
</div>
|
|
|
|
<p>
|
|
Then the 6x6 transfer function matrix is estimated (Figure <a href="#orga852dc8">7</a>).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% IFF Plant</span></span>
|
|
G_iff = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
G_iff_lf = tfestimate(meas_data_lf{<span class="org-constant">i</span>}.Va, meas_data_lf{<span class="org-constant">i</span>}.Vs, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
G_iff_hf = tfestimate(meas_data_hf{<span class="org-constant">i</span>}.Va, meas_data_hf{<span class="org-constant">i</span>}.Vs, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
G_iff(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = [G_iff_lf(i_lf, <span class="org-type">:</span>); G_iff_hf(i_hf, <span class="org-type">:</span>)];
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orga852dc8" class="figure">
|
|
<p><img src="figs/enc_struts_iff_frf.png" alt="enc_struts_iff_frf.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 7: </span>Measured FRF for the IFF plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org27bc005" class="outline-4">
|
|
<h4 id="org27bc005"><span class="section-number-4">1.2.5</span> Save Identified Plants</h4>
|
|
<div class="outline-text-4" id="text-1-2-5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">save(<span class="org-string">'matlab/mat/identified_plants_enc_struts.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'Ts'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'G_dvf'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org16dfab3" class="outline-3">
|
|
<h3 id="org16dfab3"><span class="section-number-3">1.3</span> Comparison with the Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-1-3">
|
|
<p>
|
|
<a id="orgb7175f9"></a>
|
|
</p>
|
|
<p>
|
|
In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model.
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-orgbec3680" class="outline-4">
|
|
<h4 id="orgbec3680"><span class="section-number-4">1.3.1</span> Load measured FRF</h4>
|
|
<div class="outline-text-4" id="text-1-3-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Load data</span></span>
|
|
load(<span class="org-string">'identified_plants_enc_struts.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'Ts'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'G_dvf'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org416f14a" class="outline-4">
|
|
<h4 id="org416f14a"><span class="section-number-4">1.3.2</span> Dynamics from Actuator to Force Sensors</h4>
|
|
<div class="outline-text-4" id="text-1-3-2">
|
|
<div class="org-src-container">
|
|
<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>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dum'</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>
|
|
</div>
|
|
|
|
|
|
<div id="org7fc37a7" class="figure">
|
|
<p><img src="figs/enc_struts_iff_comp_simscape.png" alt="enc_struts_iff_comp_simscape.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 8: </span>Diagonal elements of the IFF Plant</p>
|
|
</div>
|
|
|
|
|
|
<div id="orga701417" 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 id="outline-container-org7740929" class="outline-4">
|
|
<h4 id="org7740929"><span class="section-number-4">1.3.3</span> Dynamics from Actuator to Encoder</h4>
|
|
<div class="outline-text-4" id="text-1-3-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Initialization of the 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">'flexible'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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>
|
|
</div>
|
|
|
|
|
|
<div id="org82e0a0f" class="figure">
|
|
<p><img src="figs/enc_struts_dvf_comp_simscape.png" alt="enc_struts_dvf_comp_simscape.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 10: </span>Diagonal elements of the DVF Plant</p>
|
|
</div>
|
|
|
|
|
|
<div id="org11c75ab" 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 id="outline-container-org07b9456" class="outline-4">
|
|
<h4 id="org07b9456"><span class="section-number-4">1.3.4</span> Effect of a change in bending damping of the joints</h4>
|
|
<div class="outline-text-4" id="text-1-3-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Tested bending dampings [Nm/(rad/s)]</span></span>
|
|
cRs = [1e<span class="org-type">-</span>3, 5e<span class="org-type">-</span>3, 1e<span class="org-type">-</span>2, 5e<span class="org-type">-</span>2, 1e<span class="org-type">-</span>1];
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Then the identification is performed for all the values of the bending damping.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Idenfity the transfer function from actuator to encoder for all bending dampins</span></span>
|
|
Gs = {zeros(length(cRs), 1)};
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(cRs)</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">'flexible'</span>, ...
|
|
<span class="org-string">'flex_bot_cRx'</span>, cRs(<span class="org-constant">i</span>), ...
|
|
<span class="org-string">'flex_bot_cRy'</span>, cRs(<span class="org-constant">i</span>), ...
|
|
<span class="org-string">'flex_top_cRx'</span>, cRs(<span class="org-constant">i</span>), ...
|
|
<span class="org-string">'flex_top_cRy'</span>, cRs(<span class="org-constant">i</span>));
|
|
|
|
G = exp(<span class="org-type">-</span>s<span class="org-type">*</span>Ts)<span class="org-type">*</span>linearize(mdl, io, 0.0, options);
|
|
G.InputName = {<span class="org-string">'Va1'</span>, <span class="org-string">'Va2'</span>, <span class="org-string">'Va3'</span>, <span class="org-string">'Va4'</span>, <span class="org-string">'Va5'</span>, <span class="org-string">'Va6'</span>};
|
|
G.OutputName = {<span class="org-string">'dL1'</span>, <span class="org-string">'dL2'</span>, <span class="org-string">'dL3'</span>, <span class="org-string">'dL4'</span>, <span class="org-string">'dL5'</span>, <span class="org-string">'dL6'</span>};
|
|
|
|
Gs(<span class="org-constant">i</span>) = {G};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<ul class="org-ul">
|
|
<li>Could be nice</li>
|
|
<li>Actual damping is very small</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org74cdaef" class="outline-4">
|
|
<h4 id="org74cdaef"><span class="section-number-4">1.3.5</span> Effect of a change in damping factor of the APA</h4>
|
|
<div class="outline-text-4" id="text-1-3-5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Tested bending dampings [Nm/(rad/s)]</span></span>
|
|
xis = [1e<span class="org-type">-</span>3, 5e<span class="org-type">-</span>3, 1e<span class="org-type">-</span>2, 5e<span class="org-type">-</span>2, 1e<span class="org-type">-</span>1];
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Idenfity the transfer function from actuator to encoder for all bending dampins</span></span>
|
|
Gs = {zeros(length(xis), 1)};
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(xis)</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">'flexible'</span>, ...
|
|
<span class="org-string">'actuator_xi'</span>, xis(<span class="org-constant">i</span>));
|
|
|
|
G = exp(<span class="org-type">-</span>s<span class="org-type">*</span>Ts)<span class="org-type">*</span>linearize(mdl, io, 0.0, options);
|
|
G.InputName = {<span class="org-string">'Va1'</span>, <span class="org-string">'Va2'</span>, <span class="org-string">'Va3'</span>, <span class="org-string">'Va4'</span>, <span class="org-string">'Va5'</span>, <span class="org-string">'Va6'</span>};
|
|
G.OutputName = {<span class="org-string">'dL1'</span>, <span class="org-string">'dL2'</span>, <span class="org-string">'dL3'</span>, <span class="org-string">'dL4'</span>, <span class="org-string">'dL5'</span>, <span class="org-string">'dL6'</span>};
|
|
|
|
Gs(<span class="org-constant">i</span>) = {G};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org9c60432" class="figure">
|
|
<p><img src="figs/bode_Va_dL_effect_xi_damp.png" alt="bode_Va_dL_effect_xi_damp.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 12: </span>Effect of the APA damping factor \(\xi\) on the dynamics from \(u\) to \(d\mathcal{L}\)</p>
|
|
</div>
|
|
|
|
<div class="important" id="org9cb3638">
|
|
<p>
|
|
Damping factor \(\xi\) has a large impact on the damping of the “spurious resonances” at 200Hz and 300Hz.
|
|
</p>
|
|
|
|
</div>
|
|
|
|
<div class="question" id="org3692ed5">
|
|
<p>
|
|
Why is the damping factor does not change the damping of the first peak?
|
|
</p>
|
|
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2265b20" class="outline-4">
|
|
<h4 id="org2265b20"><span class="section-number-4">1.3.6</span> Effect of a change in stiffness damping coef of the APA</h4>
|
|
<div class="outline-text-4" id="text-1-3-6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">m_coef = 1e1;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Tested bending dampings [Nm/(rad/s)]</span></span>
|
|
k_coefs = [1e<span class="org-type">-</span>6, 5e<span class="org-type">-</span>6, 1e<span class="org-type">-</span>5, 5e<span class="org-type">-</span>5, 1e<span class="org-type">-</span>4];
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Idenfity the transfer function from actuator to encoder for all bending dampins</span></span>
|
|
Gs = {zeros(length(k_coefs), 1)};
|
|
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">'flexible'</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(k_coefs)</span>
|
|
k_coef = k_coefs(<span class="org-constant">i</span>);
|
|
|
|
G = exp(<span class="org-type">-</span>s<span class="org-type">*</span>Ts)<span class="org-type">*</span>linearize(mdl, io, 0.0, options);
|
|
G.InputName = {<span class="org-string">'Va1'</span>, <span class="org-string">'Va2'</span>, <span class="org-string">'Va3'</span>, <span class="org-string">'Va4'</span>, <span class="org-string">'Va5'</span>, <span class="org-string">'Va6'</span>};
|
|
G.OutputName = {<span class="org-string">'dL1'</span>, <span class="org-string">'dL2'</span>, <span class="org-string">'dL3'</span>, <span class="org-string">'dL4'</span>, <span class="org-string">'dL5'</span>, <span class="org-string">'dL6'</span>};
|
|
|
|
Gs(<span class="org-constant">i</span>) = {G};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org0191bb1" class="figure">
|
|
<p><img src="figs/bode_Va_dL_effect_k_coef.png" alt="bode_Va_dL_effect_k_coef.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 13: </span>Effect of a change of the damping “stiffness coeficient” on the transfer function from \(u\) to \(d\mathcal{L}\)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org81f3c3c" class="outline-4">
|
|
<h4 id="org81f3c3c"><span class="section-number-4">1.3.7</span> Effect of a change in mass damping coef of the APA</h4>
|
|
<div class="outline-text-4" id="text-1-3-7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">k_coef = 1e<span class="org-type">-</span>6;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Tested bending dampings [Nm/(rad/s)]</span></span>
|
|
m_coefs = [1e1, 5e1, 1e2, 5e2, 1e3];
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Idenfity the transfer function from actuator to encoder for all bending dampins</span></span>
|
|
Gs = {zeros(length(m_coefs), 1)};
|
|
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">'flexible'</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(m_coefs)</span>
|
|
m_coef = m_coefs(<span class="org-constant">i</span>);
|
|
|
|
G = exp(<span class="org-type">-</span>s<span class="org-type">*</span>Ts)<span class="org-type">*</span>linearize(mdl, io, 0.0, options);
|
|
G.InputName = {<span class="org-string">'Va1'</span>, <span class="org-string">'Va2'</span>, <span class="org-string">'Va3'</span>, <span class="org-string">'Va4'</span>, <span class="org-string">'Va5'</span>, <span class="org-string">'Va6'</span>};
|
|
G.OutputName = {<span class="org-string">'dL1'</span>, <span class="org-string">'dL2'</span>, <span class="org-string">'dL3'</span>, <span class="org-string">'dL4'</span>, <span class="org-string">'dL5'</span>, <span class="org-string">'dL6'</span>};
|
|
|
|
Gs(<span class="org-constant">i</span>) = {G};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgeff421b" class="figure">
|
|
<p><img src="figs/bode_Va_dL_effect_m_coef.png" alt="bode_Va_dL_effect_m_coef.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 14: </span>Effect of a change of the damping “mass coeficient” on the transfer function from \(u\) to \(d\mathcal{L}\)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org35c44c1" class="outline-4">
|
|
<h4 id="org35c44c1"><span class="section-number-4">1.3.8</span> Using Flexible model</h4>
|
|
<div class="outline-text-4" id="text-1-3-8">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">d_aligns = [[<span class="org-type">-</span>0.05, <span class="org-type">-</span>0.3, 0];
|
|
[ 0, 0.5, 0];
|
|
[<span class="org-type">-</span>0.1, <span class="org-type">-</span>0.3, 0];
|
|
[ 0, 0.3, 0];
|
|
[<span class="org-type">-</span>0.05, 0.05, 0];
|
|
[0, 0, 0]]<span class="org-type">*</span>1e<span class="org-type">-</span>3;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">d_aligns = zeros(6,3);
|
|
<span class="org-comment">% d_aligns(1,:) = [-0.05, -0.3, 0]*1e-3;</span>
|
|
d_aligns<span class="org-type">(2,:) </span>= [ 0, 0.3, 0]<span class="org-type">*</span>1e<span class="org-type">-</span>3;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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">'flexible'</span>, ...
|
|
<span class="org-string">'actuator_d_align'</span>, d_aligns);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="question" id="org45fc7d0">
|
|
<p>
|
|
Why do we have smaller resonances when using flexible APA?
|
|
On the test bench we have the same resonance as the 2DoF model.
|
|
Could it be due to the compliance in other dof of the flexible model?
|
|
</p>
|
|
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dum'</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>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org90aa71a" class="outline-4">
|
|
<h4 id="org90aa71a"><span class="section-number-4">1.3.9</span> Flexible model + encoders fixed to the plates</h4>
|
|
<div class="outline-text-4" id="text-1-3-9">
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">% Force Sensors</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">d_aligns = [[<span class="org-type">-</span>0.05, <span class="org-type">-</span>0.3, 0];
|
|
[ 0, 0.5, 0];
|
|
[<span class="org-type">-</span>0.1, <span class="org-type">-</span>0.3, 0];
|
|
[ 0, 0.3, 0];
|
|
[<span class="org-type">-</span>0.05, 0.05, 0];
|
|
[0, 0, 0]]<span class="org-type">*</span>1e<span class="org-type">-</span>3;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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">'flexible'</span>, ...
|
|
<span class="org-string">'actuator_d_align'</span>, d_aligns);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gdvf_struts = 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>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</span>, ...
|
|
<span class="org-string">'actuator_d_align'</span>, d_aligns);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gdvf_plates = 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>
|
|
</div>
|
|
|
|
|
|
<div id="org7c0b25c" class="figure">
|
|
<p><img src="figs/dvf_plant_comp_struts_plates.png" alt="dvf_plant_comp_struts_plates.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 15: </span>Comparison of the dynamics from \(V_a\) to \(d_L\) when the encoders are fixed to the struts (blue) and to the plates (red). APA are modeled as a flexible element.</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3cf2cf1" class="outline-3">
|
|
<h3 id="org3cf2cf1"><span class="section-number-3">1.4</span> Integral Force Feedback</h3>
|
|
<div class="outline-text-3" id="text-1-4">
|
|
<p>
|
|
<a id="orgffc7d0d"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-orgfef1ee8" class="outline-4">
|
|
<h4 id="orgfef1ee8"><span class="section-number-4">1.4.1</span> Identification of the IFF Plant</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">%% 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>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dum'</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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgabb73e5" class="outline-4">
|
|
<h4 id="orgabb73e5"><span class="section-number-4">1.4.2</span> Root Locus and Decentralized Loop gain</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">%% IFF Controller</span></span>
|
|
Kiff_g1 = <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>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="org345f502" 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 16: </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 class="org-src-container">
|
|
<pre class="src src-matlab">save(<span class="org-string">'matlab/mat/Kiff.mat'</span>, <span class="org-string">'Kiff'</span>)
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orga311644" 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 17: </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-orgbdac543" class="outline-4">
|
|
<h4 id="orgbdac543"><span class="section-number-4">1.4.3</span> Multiple Gains - Simulation</h4>
|
|
<div class="outline-text-4" id="text-1-4-3">
|
|
<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];
|
|
</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">'/du'</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="orga7ed5d5" 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 18: </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-org3bd1e0b" class="outline-4">
|
|
<h4 id="org3bd1e0b"><span class="section-number-4">1.4.4</span> Experimental Results - Gains</h4>
|
|
<div class="outline-text-4" id="text-1-4-4">
|
|
<p>
|
|
Let’s look at the damping introduced by IFF as a function of the IFF gain and compare that with the results obtained using the Simscape model.
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org69457dc" class="outline-5">
|
|
<h5 id="org69457dc"><span class="section-number-5">1.4.4.1</span> Load Data</h5>
|
|
<div class="outline-text-5" id="text-1-4-4-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Load Identification Data</span></span>
|
|
meas_iff_gains = {};
|
|
|
|
<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>
|
|
meas_iff_gains(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/iff_strut_1_noise_g_%i.mat'</span>, iff_gains(<span class="org-constant">i</span>)), <span class="org-string">'t'</span>, <span class="org-string">'Vexc'</span>, <span class="org-string">'Vs'</span>, <span class="org-string">'de'</span>, <span class="org-string">'u'</span>)};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge77daca" class="outline-5">
|
|
<h5 id="orge77daca"><span class="section-number-5">1.4.4.2</span> Spectral Analysis - Setup</h5>
|
|
<div class="outline-text-5" id="text-1-4-4-2">
|
|
<div class="org-src-container">
|
|
<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_iff_gains{1}.t(end) <span class="org-type">-</span> (meas_iff_gains{1}.t(1)))<span class="org-type">/</span>(length(meas_iff_gains{1}.t)<span class="org-type">-</span>1);
|
|
|
|
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
|
Fs = 1<span class="org-type">/</span>Ts;
|
|
|
|
<span class="org-comment">% Hannning Windows</span>
|
|
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
|
|
|
<span class="org-comment">% And we get the frequency vector</span>
|
|
[<span class="org-type">~</span>, f] = tfestimate(meas_iff_gains{1}.Vexc, meas_iff_gains{1}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga18329e" class="outline-5">
|
|
<h5 id="orga18329e"><span class="section-number-5">1.4.4.3</span> DVF Plant</h5>
|
|
<div class="outline-text-5" id="text-1-4-4-3">
|
|
<div class="org-src-container">
|
|
<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_iff_gains = {};
|
|
|
|
<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>
|
|
G_iff_gains{<span class="org-constant">i</span>} = tfestimate(meas_iff_gains{<span class="org-constant">i</span>}.Vexc, meas_iff_gains{<span class="org-constant">i</span>}.de(<span class="org-type">:</span>,1), win, [], [], 1<span class="org-type">/</span>Ts);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org9939225" class="figure">
|
|
<p><img src="figs/comp_iff_gains_dvf_plant.png" alt="comp_iff_gains_dvf_plant.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 19: </span>Transfer function from \(u\) to \(d\mathcal{L}_m\) for multiple values of the IFF gain</p>
|
|
</div>
|
|
|
|
|
|
<div id="org22bf508" class="figure">
|
|
<p><img src="figs/comp_iff_gains_dvf_plant_zoom.png" alt="comp_iff_gains_dvf_plant_zoom.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 20: </span>Transfer function from \(u\) to \(d\mathcal{L}_m\) for multiple values of the IFF gain (Zoom)</p>
|
|
</div>
|
|
|
|
<div class="important" id="org08ce868">
|
|
<p>
|
|
The IFF control strategy is very effective for the damping of the suspension modes.
|
|
It however does not damp the modes at 200Hz, 300Hz and 400Hz (flexible modes of the APA).
|
|
This is very logical.
|
|
</p>
|
|
|
|
<p>
|
|
Also, the experimental results and the models obtained from the Simscape model are in agreement.
|
|
</p>
|
|
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd7b3950" class="outline-5">
|
|
<h5 id="orgd7b3950"><span class="section-number-5">1.4.4.4</span> Experimental Results - Comparison of the un-damped and fully damped system</h5>
|
|
<div class="outline-text-5" id="text-1-4-4-4">
|
|
|
|
<div id="org5e4268d" class="figure">
|
|
<p><img src="figs/comp_undamped_opt_iff_gain_diagonal.png" alt="comp_undamped_opt_iff_gain_diagonal.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 21: </span>Comparison of the diagonal elements of the tranfer function from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) without active damping and with optimal IFF gain</p>
|
|
</div>
|
|
|
|
<div class="question" id="org63597e0">
|
|
<p>
|
|
A series of modes at around 205Hz are also damped.
|
|
</p>
|
|
|
|
<p>
|
|
Are these damped modes at 205Hz additional “suspension” modes or flexible modes of the struts?
|
|
</p>
|
|
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org37f477e" class="outline-4">
|
|
<h4 id="org37f477e"><span class="section-number-4">1.4.5</span> Experimental Results - Damped Plant with Optimal gain</h4>
|
|
<div class="outline-text-4" id="text-1-4-5">
|
|
<p>
|
|
Let’s now look at the \(6 \times 6\) damped plant with the optimal gain \(g = 400\).
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org2716b9a" class="outline-5">
|
|
<h5 id="org2716b9a"><span class="section-number-5">1.4.5.1</span> Load Data</h5>
|
|
<div class="outline-text-5" id="text-1-4-5-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Load Identification Data</span></span>
|
|
meas_iff_struts = {};
|
|
|
|
<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_iff_struts(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/iff_strut_%i_noise_g_400.mat'</span>, <span class="org-constant">i</span>), <span class="org-string">'t'</span>, <span class="org-string">'Vexc'</span>, <span class="org-string">'Vs'</span>, <span class="org-string">'de'</span>, <span class="org-string">'u'</span>)};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8de917f" class="outline-5">
|
|
<h5 id="org8de917f"><span class="section-number-5">1.4.5.2</span> Spectral Analysis - Setup</h5>
|
|
<div class="outline-text-5" id="text-1-4-5-2">
|
|
<div class="org-src-container">
|
|
<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_iff_struts{1}.t(end) <span class="org-type">-</span> (meas_iff_struts{1}.t(1)))<span class="org-type">/</span>(length(meas_iff_struts{1}.t)<span class="org-type">-</span>1);
|
|
|
|
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
|
Fs = 1<span class="org-type">/</span>Ts;
|
|
|
|
<span class="org-comment">% Hannning Windows</span>
|
|
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
|
|
|
<span class="org-comment">% And we get the frequency vector</span>
|
|
[<span class="org-type">~</span>, f] = tfestimate(meas_iff_struts{1}.Vexc, meas_iff_struts{1}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfa9bce2" class="outline-5">
|
|
<h5 id="orgfa9bce2"><span class="section-number-5">1.4.5.3</span> DVF Plant</h5>
|
|
<div class="outline-text-5" id="text-1-4-5-3">
|
|
<div class="org-src-container">
|
|
<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_iff_opt = {};
|
|
|
|
<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>
|
|
G_iff_opt{<span class="org-constant">i</span>} = tfestimate(meas_iff_struts{<span class="org-constant">i</span>}.Vexc, meas_iff_struts{<span class="org-constant">i</span>}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgcc09028" class="figure">
|
|
<p><img src="figs/damped_iff_plant_comp_diagonal.png" alt="damped_iff_plant_comp_diagonal.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 22: </span>Comparison of the diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgbd8c52a" class="figure">
|
|
<p><img src="figs/damped_iff_plant_comp_off_diagonal.png" alt="damped_iff_plant_comp_off_diagonal.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 23: </span>Comparison of the off-diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)</p>
|
|
</div>
|
|
|
|
<div class="important" id="orgcff805f">
|
|
<p>
|
|
With the IFF control strategy applied and the optimal gain used, the suspension modes are very well damped.
|
|
Remains the undamped flexible modes of the APA (200Hz, 300Hz, 400Hz), and the modes of the plates (700Hz).
|
|
</p>
|
|
|
|
<p>
|
|
The Simscape model and the experimental results are in very good agreement.
|
|
</p>
|
|
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4284ac6" class="outline-3">
|
|
<h3 id="org4284ac6"><span class="section-number-3">1.5</span> Modal Analysis</h3>
|
|
<div class="outline-text-3" id="text-1-5">
|
|
<p>
|
|
<a id="org10a48cf"></a>
|
|
</p>
|
|
<p>
|
|
Several 3-axis accelerometers are fixed on the top platform of the nano-hexapod as shown in Figure <a href="#orgd9eb98c">26</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orge611ee2" class="figure">
|
|
<p><img src="figs/accelerometers_nano_hexapod.jpg" alt="accelerometers_nano_hexapod.jpg" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 24: </span>Location of the accelerometers on top of the nano-hexapod</p>
|
|
</div>
|
|
|
|
<p>
|
|
The top platform is then excited using an instrumented hammer as shown in Figure <a href="#org9e78cd0">25</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org9e78cd0" class="figure">
|
|
<p><img src="figs/hammer_excitation_compliance_meas.jpg" alt="hammer_excitation_compliance_meas.jpg" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 25: </span>Example of an excitation using an instrumented hammer</p>
|
|
</div>
|
|
</div>
|
|
<div id="outline-container-org2d0c26f" class="outline-4">
|
|
<h4 id="org2d0c26f"><span class="section-number-4">1.5.1</span> Effectiveness of the IFF Strategy - Compliance</h4>
|
|
<div class="outline-text-4" id="text-1-5-1">
|
|
<p>
|
|
In this section, we wish to estimated the effectiveness of the IFF strategy concerning the compliance.
|
|
</p>
|
|
|
|
<p>
|
|
The top plate is excited vertically using the instrumented hammer two times:
|
|
</p>
|
|
<ol class="org-ol">
|
|
<li>no control loop is used</li>
|
|
<li>decentralized IFF is used</li>
|
|
</ol>
|
|
|
|
<p>
|
|
The data is loaded.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">frf_ol = load(<span class="org-string">'Measurement_Z_axis.mat'</span>); <span class="org-comment">% Open-Loop</span>
|
|
frf_iff = load(<span class="org-string">'Measurement_Z_axis_damped.mat'</span>); <span class="org-comment">% IFF</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The mean vertical motion of the top platform is computed by averaging all 5 accelerometers.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Multiply by 10 (gain in m/s^2/V) and divide by 5 (number of accelerometers)</span></span>
|
|
d_frf_ol = 10<span class="org-type">/</span>5<span class="org-type">*</span>(frf_ol.FFT1_H1_4_1_RMS_Y_Mod <span class="org-type">+</span> frf_ol.FFT1_H1_7_1_RMS_Y_Mod <span class="org-type">+</span> frf_ol.FFT1_H1_10_1_RMS_Y_Mod <span class="org-type">+</span> frf_ol.FFT1_H1_13_1_RMS_Y_Mod <span class="org-type">+</span> frf_ol.FFT1_H1_16_1_RMS_Y_Mod)<span class="org-type">./</span>(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>frf_ol.FFT1_H1_16_1_RMS_X_Val)<span class="org-type">.^</span>2;
|
|
d_frf_iff = 10<span class="org-type">/</span>5<span class="org-type">*</span>(frf_iff.FFT1_H1_4_1_RMS_Y_Mod <span class="org-type">+</span> frf_iff.FFT1_H1_7_1_RMS_Y_Mod <span class="org-type">+</span> frf_iff.FFT1_H1_10_1_RMS_Y_Mod <span class="org-type">+</span> frf_iff.FFT1_H1_13_1_RMS_Y_Mod <span class="org-type">+</span> frf_iff.FFT1_H1_16_1_RMS_Y_Mod)<span class="org-type">./</span>(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>frf_iff.FFT1_H1_16_1_RMS_X_Val)<span class="org-type">.^</span>2;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The vertical compliance (magnitude of the transfer function from a vertical force applied on the top plate to the vertical motion of the top plate) is shown in Figure <a href="#orgd9eb98c">26</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgd9eb98c" class="figure">
|
|
<p><img src="figs/compliance_vertical_comp_iff.png" alt="compliance_vertical_comp_iff.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 26: </span>Measured vertical compliance with and without IFF</p>
|
|
</div>
|
|
|
|
<div class="important" id="orgbd002a6">
|
|
<p>
|
|
From Figure <a href="#orgd9eb98c">26</a>, it is clear that the IFF control strategy is very effective in damping the suspensions modes of the nano-hexapode.
|
|
It also has the effect of degrading (slightly) the vertical compliance at low frequency.
|
|
</p>
|
|
|
|
<p>
|
|
It also seems some damping can be added to the modes at around 205Hz which are flexible modes of the struts.
|
|
</p>
|
|
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3ff9878" class="outline-4">
|
|
<h4 id="org3ff9878"><span class="section-number-4">1.5.2</span> Comparison with the Simscape Model</h4>
|
|
<div class="outline-text-4" id="text-1-5-2">
|
|
<p>
|
|
Let’s now compare the measured vertical compliance with the vertical compliance as estimated from the Simscape model.
|
|
</p>
|
|
|
|
<p>
|
|
The transfer function from a vertical external force to the absolute motion of the top platform is identified (with and without IFF) using the Simscape model.
|
|
The comparison is done in Figure <a href="#orgfb4e6d0">27</a>.
|
|
Again, the model is quite accurate!
|
|
</p>
|
|
|
|
<div id="orgfb4e6d0" class="figure">
|
|
<p><img src="figs/compliance_vertical_comp_model_iff.png" alt="compliance_vertical_comp_model_iff.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 27: </span>Measured vertical compliance with and without IFF</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd41f51c" class="outline-4">
|
|
<h4 id="orgd41f51c"><span class="section-number-4">1.5.3</span> Obtained Mode Shapes</h4>
|
|
<div class="outline-text-4" id="text-1-5-3">
|
|
<p>
|
|
Then, several excitation are performed using the instrumented Hammer and the mode shapes are extracted.
|
|
</p>
|
|
|
|
<p>
|
|
We can observe the mode shapes of the first 6 modes that are the suspension modes (the plate is behaving as a solid body) in Figure <a href="#orgdee89e2">28</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgdee89e2" class="figure">
|
|
<p><img src="figs/mode_shapes_annotated.gif" alt="mode_shapes_annotated.gif" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 28: </span>Measured mode shapes for the first six modes</p>
|
|
</div>
|
|
|
|
<p>
|
|
Then, there is a mode at 692Hz which corresponds to a flexible mode of the top plate (Figure <a href="#orgb2ad4e3">29</a>).
|
|
</p>
|
|
|
|
|
|
<div id="orgb2ad4e3" class="figure">
|
|
<p><img src="figs/ModeShapeFlex1_crop.gif" alt="ModeShapeFlex1_crop.gif" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 29: </span>First flexible mode at 692Hz</p>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained modes are summarized in Table <a href="#org9394fd0">2</a>.
|
|
</p>
|
|
|
|
<table id="org9394fd0" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<caption class="t-above"><span class="table-number">Table 2:</span> Description of the identified modes</caption>
|
|
|
|
<colgroup>
|
|
<col class="org-right" />
|
|
|
|
<col class="org-right" />
|
|
|
|
<col class="org-left" />
|
|
</colgroup>
|
|
<thead>
|
|
<tr>
|
|
<th scope="col" class="org-right">Mode</th>
|
|
<th scope="col" class="org-right">Freq. [Hz]</th>
|
|
<th scope="col" class="org-left">Description</th>
|
|
</tr>
|
|
</thead>
|
|
<tbody>
|
|
<tr>
|
|
<td class="org-right">1</td>
|
|
<td class="org-right">105</td>
|
|
<td class="org-left">Suspension Mode: Y-translation</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-right">2</td>
|
|
<td class="org-right">107</td>
|
|
<td class="org-left">Suspension Mode: X-translation</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-right">3</td>
|
|
<td class="org-right">131</td>
|
|
<td class="org-left">Suspension Mode: Z-translation</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-right">4</td>
|
|
<td class="org-right">161</td>
|
|
<td class="org-left">Suspension Mode: Y-tilt</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-right">5</td>
|
|
<td class="org-right">162</td>
|
|
<td class="org-left">Suspension Mode: X-tilt</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-right">6</td>
|
|
<td class="org-right">180</td>
|
|
<td class="org-left">Suspension Mode: Z-rotation</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-right">7</td>
|
|
<td class="org-right">692</td>
|
|
<td class="org-left">(flexible) Membrane mode of the top platform</td>
|
|
</tr>
|
|
</tbody>
|
|
</table>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2527b87" class="outline-3">
|
|
<h3 id="org2527b87"><span class="section-number-3">1.6</span> Accelerometers fixed on the top platform</h3>
|
|
<div class="outline-text-3" id="text-1-6">
|
|
|
|
<div id="orgda28621" class="figure">
|
|
<p><img src="figs/acc_top_plat_top_view.jpg" alt="acc_top_plat_top_view.jpg" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 30: </span>Accelerometers fixed on the top platform</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1fd49c3" class="outline-4">
|
|
<h4 id="org1fd49c3"><span class="section-number-4">1.6.1</span> Experimental Identification</h4>
|
|
<div class="outline-text-4" id="text-1-6-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Load Identification Data</span></span>
|
|
meas_acc = {};
|
|
|
|
<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_acc(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/meas_acc_top_plat_strut_%i.mat'</span>, <span class="org-constant">i</span>), <span class="org-string">'t'</span>, <span class="org-string">'Va'</span>, <span class="org-string">'de'</span>, <span class="org-string">'Am'</span>)};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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_acc{1}.t(end) <span class="org-type">-</span> (meas_acc{1}.t(1)))<span class="org-type">/</span>(length(meas_acc{1}.t)<span class="org-type">-</span>1);
|
|
|
|
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
|
Fs = 1<span class="org-type">/</span>Ts;
|
|
|
|
<span class="org-comment">% Hannning Windows</span>
|
|
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
|
|
|
<span class="org-comment">% And we get the frequency vector</span>
|
|
[<span class="org-type">~</span>, f] = tfestimate(meas_acc{1}.Va, meas_acc{1}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The sensibility of the accelerometers are \(0.1 V/g \approx 0.01 V/(m/s^2)\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Compute the 6x6 transfer function matrix</span></span>
|
|
G_acc = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
G_acc(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = tfestimate(meas_acc{<span class="org-constant">i</span>}.Va, 1<span class="org-type">/</span>0.01<span class="org-type">*</span>meas_acc{<span class="org-constant">i</span>}.Am, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4f8d7c7" class="outline-4">
|
|
<h4 id="org4f8d7c7"><span class="section-number-4">1.6.2</span> Location and orientation of accelerometers</h4>
|
|
<div class="outline-text-4" id="text-1-6-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Opm = [ 0.047, <span class="org-type">-</span>0.112, 10e<span class="org-type">-</span>3;
|
|
0.047, <span class="org-type">-</span>0.112, 10e<span class="org-type">-</span>3;
|
|
<span class="org-type">-</span>0.113, 0.011, 10e<span class="org-type">-</span>3;
|
|
<span class="org-type">-</span>0.113, 0.011, 10e<span class="org-type">-</span>3;
|
|
0.040, 0.113, 10e<span class="org-type">-</span>3;
|
|
0.040, 0.113, 10e<span class="org-type">-</span>3]<span class="org-type">'</span>;
|
|
|
|
Osm = [<span class="org-type">-</span>1, 0, 0;
|
|
0, 0, 1;
|
|
0, <span class="org-type">-</span>1, 0;
|
|
0, 0, 1;
|
|
<span class="org-type">-</span>1, 0, 0;
|
|
0, 0, 1]<span class="org-type">'</span>;
|
|
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org86ce650" class="outline-4">
|
|
<h4 id="org86ce650"><span class="section-number-4">1.6.3</span> COM</h4>
|
|
<div class="outline-text-4" id="text-1-6-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Hbm = <span class="org-type">-</span>15e<span class="org-type">-</span>3;
|
|
|
|
M = getTransformationMatrixAcc(Opm<span class="org-type">-</span>[0;0;Hbm], Osm);
|
|
J = getJacobianNanoHexapod(<span class="org-variable-name">Hbm</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">G_acc_CoM = zeros(size(G_acc));
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(f)</span>
|
|
G_acc_CoM(<span class="org-constant">i</span>, <span class="org-type">:</span>, <span class="org-type">:</span>) = inv(M)<span class="org-type">*</span>squeeze(G_acc(<span class="org-constant">i</span>, <span class="org-type">:</span>, <span class="org-type">:</span>))<span class="org-type">*</span>inv(J<span class="org-type">'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4309cac" class="outline-4">
|
|
<h4 id="org4309cac"><span class="section-number-4">1.6.4</span> COK</h4>
|
|
<div class="outline-text-4" id="text-1-6-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Hbm = <span class="org-type">-</span>42.3e<span class="org-type">-</span>3;
|
|
|
|
M = getTransformationMatrixAcc(Opm<span class="org-type">-</span>[0;0;Hbm], Osm);
|
|
J = getJacobianNanoHexapod(<span class="org-variable-name">Hbm</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">G_acc_CoK = zeros(size(G_acc));
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(f)</span>
|
|
G_acc_CoK(<span class="org-constant">i</span>, <span class="org-type">:</span>, <span class="org-type">:</span>) = inv(M)<span class="org-type">*</span>squeeze(G_acc(<span class="org-constant">i</span>, <span class="org-type">:</span>, <span class="org-type">:</span>))<span class="org-type">*</span>inv(J<span class="org-type">'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2577983" class="outline-4">
|
|
<h4 id="org2577983"><span class="section-number-4">1.6.5</span> Comp with the Simscape Model</h4>
|
|
<div class="outline-text-4" id="text-1-6-5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">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">'flexible'</span>, ...
|
|
<span class="org-string">'MO_B'</span>, <span class="org-type">-</span>42.3e<span class="org-type">-</span>3);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">% Relative Motion Outputs</span>
|
|
|
|
G = linearize(mdl, io, 0.0, options);
|
|
G.InputName = {<span class="org-string">'F1'</span>, <span class="org-string">'F2'</span>, <span class="org-string">'F3'</span>, <span class="org-string">'F4'</span>, <span class="org-string">'F5'</span>, <span class="org-string">'F6'</span>};
|
|
G.OutputName = {<span class="org-string">'D1'</span>, <span class="org-string">'D2'</span>, <span class="org-string">'D3'</span>, <span class="org-string">'D4'</span>, <span class="org-string">'D5'</span>, <span class="org-string">'D6'</span>};
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Then use the Jacobian matrices to obtain the “cartesian” centralized plant.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gc = inv(n_hexapod.geometry.J)<span class="org-type">*</span>...
|
|
G<span class="org-type">*</span>...
|
|
inv(n_hexapod.geometry.J<span class="org-type">'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org712faa3" class="outline-2">
|
|
<h2 id="org712faa3"><span class="section-number-2">2</span> Encoders fixed to the plates</h2>
|
|
<div class="outline-text-2" id="text-2">
|
|
<p>
|
|
<a id="orgcf084fc"></a>
|
|
</p>
|
|
<p>
|
|
In this section, the encoders are fixed to the plates rather than to the struts as shown in Figure <a href="#orgb246d13">31</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgb246d13" class="figure">
|
|
<p><img src="figs/IMG_20210625_083801.jpg" alt="IMG_20210625_083801.jpg" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 31: </span>Nano-Hexapod with encoders fixed to the struts</p>
|
|
</div>
|
|
|
|
<p>
|
|
It is structured as follow:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>Section <a href="#orgf841264">2.1</a>: The dynamics of the nano-hexapod is identified</li>
|
|
<li>Section <a href="#org867bae7">2.2</a>: The identified dynamics is compared with the Simscape model</li>
|
|
<li>Section <a href="#org65fc5ba">2.3</a>: The Integral Force Feedback (IFF) control strategy is applied and the dynamics of the damped nano-hexapod is identified and compare with the Simscape model</li>
|
|
<li>Section <a href="#org69c97ad">2.4</a>: The High Authority Control (HAC) in the frame of the struts is developed</li>
|
|
<li>Section <a href="#org7f2d506">2.5</a>: Some reference tracking tests are performed in order to experimentally validate the HAC-LAC control strategy.</li>
|
|
</ul>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd3ae77d" class="outline-3">
|
|
<h3 id="orgd3ae77d"><span class="section-number-3">2.1</span> Identification of the dynamics</h3>
|
|
<div class="outline-text-3" id="text-2-1">
|
|
<p>
|
|
<a id="orgf841264"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org7a53f0c" class="outline-4">
|
|
<h4 id="org7a53f0c"><span class="section-number-4">2.1.1</span> Load Measurement Data</h4>
|
|
<div class="outline-text-4" id="text-2-1-1">
|
|
<div class="org-src-container">
|
|
<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>
|
|
meas_data_lf(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/frf_exc_strut_%i_enc_plates_noise.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>)};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd3052ca" class="outline-4">
|
|
<h4 id="orgd3052ca"><span class="section-number-4">2.1.2</span> Spectral Analysis - Setup</h4>
|
|
<div class="outline-text-4" id="text-2-1-2">
|
|
<div class="org-src-container">
|
|
<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);
|
|
|
|
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
|
Fs = 1<span class="org-type">/</span>Ts;
|
|
|
|
<span class="org-comment">% Hannning Windows</span>
|
|
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
|
|
|
<span class="org-comment">% And we get the frequency vector</span>
|
|
[<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>
|
|
</div>
|
|
|
|
<div id="outline-container-orga653b6d" class="outline-4">
|
|
<h4 id="orga653b6d"><span class="section-number-4">2.1.3</span> DVF Plant</h4>
|
|
<div class="outline-text-4" id="text-2-1-3">
|
|
<p>
|
|
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#org00babe0">32</a>).
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Coherence</span></span>
|
|
coh_dvf = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
coh_dvf(<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);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org00babe0" class="figure">
|
|
<p><img src="figs/enc_plates_dvf_coh.png" alt="enc_plates_dvf_coh.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 32: </span>Obtained coherence for the DVF plant</p>
|
|
</div>
|
|
|
|
<p>
|
|
Then the 6x6 transfer function matrix is estimated (Figure <a href="#org7f5e25c">33</a>).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<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 = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
G_dvf(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = tfestimate(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);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org7f5e25c" class="figure">
|
|
<p><img src="figs/enc_plates_dvf_frf.png" alt="enc_plates_dvf_frf.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 33: </span>Measured FRF for the DVF plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfe3a401" class="outline-4">
|
|
<h4 id="orgfe3a401"><span class="section-number-4">2.1.4</span> IFF Plant</h4>
|
|
<div class="outline-text-4" id="text-2-1-4">
|
|
<p>
|
|
First, let’s compute the coherence from the excitation voltage and the displacement as measured by the encoders (Figure <a href="#org45ac66f">34</a>).
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Coherence for the IFF plant</span></span>
|
|
coh_iff = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
coh_iff(<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>}.Vs, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org45ac66f" class="figure">
|
|
<p><img src="figs/enc_plates_iff_coh.png" alt="enc_plates_iff_coh.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 34: </span>Obtained coherence for the IFF plant</p>
|
|
</div>
|
|
|
|
<p>
|
|
Then the 6x6 transfer function matrix is estimated (Figure <a href="#org1630892">35</a>).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% IFF Plant</span></span>
|
|
G_iff = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
G_iff(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = tfestimate(meas_data_lf{<span class="org-constant">i</span>}.Va, meas_data_lf{<span class="org-constant">i</span>}.Vs, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org1630892" class="figure">
|
|
<p><img src="figs/enc_plates_iff_frf.png" alt="enc_plates_iff_frf.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 35: </span>Measured FRF for the IFF plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge8c6a42" class="outline-4">
|
|
<h4 id="orge8c6a42"><span class="section-number-4">2.1.5</span> Save Identified Plants</h4>
|
|
<div class="outline-text-4" id="text-2-1-5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">save(<span class="org-string">'matlab/mat/identified_plants_enc_plates.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'Ts'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'G_dvf'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdfb05f4" class="outline-3">
|
|
<h3 id="orgdfb05f4"><span class="section-number-3">2.2</span> Comparison with the Simscape Model</h3>
|
|
<div class="outline-text-3" id="text-2-2">
|
|
<p>
|
|
<a id="org867bae7"></a>
|
|
</p>
|
|
<p>
|
|
In this section, the measured dynamics is compared with the dynamics estimated from the Simscape model.
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org37605d2" class="outline-4">
|
|
<h4 id="org37605d2"><span class="section-number-4">2.2.1</span> Load measured FRF</h4>
|
|
<div class="outline-text-4" id="text-2-2-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Load data</span></span>
|
|
load(<span class="org-string">'identified_plants_enc_plates.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'Ts'</span>, <span class="org-string">'G_iff'</span>, <span class="org-string">'G_dvf'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org793238f" class="outline-4">
|
|
<h4 id="org793238f"><span class="section-number-4">2.2.2</span> Dynamics from Actuator to Force Sensors</h4>
|
|
<div class="outline-text-4" id="text-2-2-2">
|
|
<div class="org-src-container">
|
|
<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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dum'</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>
|
|
</div>
|
|
|
|
|
|
<div id="org2e32848" class="figure">
|
|
<p><img src="figs/enc_plates_iff_comp_simscape_all.png" alt="enc_plates_iff_comp_simscape_all.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 36: </span>IFF Plant for the first actuator input and all the force senosrs</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgc93c28d" class="figure">
|
|
<p><img src="figs/enc_plates_iff_comp_simscape.png" alt="enc_plates_iff_comp_simscape.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 37: </span>Diagonal elements of the IFF Plant</p>
|
|
</div>
|
|
|
|
|
|
<div id="orga1ca50f" class="figure">
|
|
<p><img src="figs/enc_plates_iff_comp_offdiag_simscape.png" alt="enc_plates_iff_comp_offdiag_simscape.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 38: </span>Off diagonal elements of the IFF Plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9c52c3e" class="outline-4">
|
|
<h4 id="org9c52c3e"><span class="section-number-4">2.2.3</span> Dynamics from Actuator to Encoder</h4>
|
|
<div class="outline-text-4" id="text-2-2-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Initialization of the 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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dL'</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>
|
|
</div>
|
|
|
|
|
|
<div id="org5781868" class="figure">
|
|
<p><img src="figs/enc_plates_dvf_comp_simscape_all.png" alt="enc_plates_dvf_comp_simscape_all.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 39: </span>DVF Plant for the first actuator input and all the encoders</p>
|
|
</div>
|
|
|
|
|
|
<div id="org71185f6" class="figure">
|
|
<p><img src="figs/enc_plates_dvf_comp_simscape.png" alt="enc_plates_dvf_comp_simscape.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 40: </span>Diagonal elements of the DVF Plant</p>
|
|
</div>
|
|
|
|
|
|
<div id="org9b58e6a" class="figure">
|
|
<p><img src="figs/enc_plates_dvf_comp_offdiag_simscape.png" alt="enc_plates_dvf_comp_offdiag_simscape.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 41: </span>Off diagonal elements of the DVF Plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3616410" class="outline-3">
|
|
<h3 id="org3616410"><span class="section-number-3">2.3</span> Integral Force Feedback</h3>
|
|
<div class="outline-text-3" id="text-2-3">
|
|
<p>
|
|
<a id="org65fc5ba"></a>
|
|
</p>
|
|
|
|
<div id="org96b6cf3" class="figure">
|
|
<p><img src="figs/control_architecture_iff.png" alt="control_architecture_iff.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 42: </span>Integral Force Feedback Strategy</p>
|
|
</div>
|
|
</div>
|
|
<div id="outline-container-org8cbdfcb" class="outline-4">
|
|
<h4 id="org8cbdfcb"><span class="section-number-4">2.3.1</span> Identification of the IFF Plant</h4>
|
|
<div class="outline-text-4" id="text-2-3-1">
|
|
<div class="org-src-container">
|
|
<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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'2dof'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgab3fa5e" class="outline-4">
|
|
<h4 id="orgab3fa5e"><span class="section-number-4">2.3.2</span> Effect of IFF on the plant - Simscape Model</h4>
|
|
<div class="outline-text-4" id="text-2-3-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load(<span class="org-string">'Kiff.mat'</span>, <span class="org-string">'Kiff'</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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dL'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Plate Displacement (encoder)</span>
|
|
</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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gd_ol = 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>
|
|
</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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</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">Gd_iff = 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>
|
|
</div>
|
|
|
|
<pre class="example">
|
|
1
|
|
</pre>
|
|
|
|
|
|
|
|
<div id="org6eb320c" class="figure">
|
|
<p><img src="figs/enc_plates_iff_gains_effect_dvf_plant.png" alt="enc_plates_iff_gains_effect_dvf_plant.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 43: </span>Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org1fbe8b3" class="outline-4">
|
|
<h4 id="org1fbe8b3"><span class="section-number-4">2.3.3</span> Experimental Results - Damped Plant with Optimal gain</h4>
|
|
<div class="outline-text-4" id="text-2-3-3">
|
|
<p>
|
|
Let’s now look at the \(6 \times 6\) damped plant with the optimal gain \(g = 400\).
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org456a248" class="outline-5">
|
|
<h5 id="org456a248"><span class="section-number-5">2.3.3.1</span> Load Data</h5>
|
|
<div class="outline-text-5" id="text-2-3-3-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Load Identification Data</span></span>
|
|
meas_iff_plates = {};
|
|
|
|
<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_iff_plates(<span class="org-constant">i</span>) = {load(sprintf(<span class="org-string">'mat/frf_exc_iff_strut_%i_enc_plates_noise.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>, <span class="org-string">'u'</span>)};
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org54fe798" class="outline-5">
|
|
<h5 id="org54fe798"><span class="section-number-5">2.3.3.2</span> Spectral Analysis - Setup</h5>
|
|
<div class="outline-text-5" id="text-2-3-3-2">
|
|
<div class="org-src-container">
|
|
<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_iff_plates{1}.t(end) <span class="org-type">-</span> (meas_iff_plates{1}.t(1)))<span class="org-type">/</span>(length(meas_iff_plates{1}.t)<span class="org-type">-</span>1);
|
|
|
|
<span class="org-comment">% Sampling Frequency [Hz]</span>
|
|
Fs = 1<span class="org-type">/</span>Ts;
|
|
|
|
<span class="org-comment">% Hannning Windows</span>
|
|
win = hanning(ceil(1<span class="org-type">*</span>Fs));
|
|
|
|
<span class="org-comment">% And we get the frequency vector</span>
|
|
[<span class="org-type">~</span>, f] = tfestimate(meas_iff_plates{1}.Va, meas_iff_plates{1}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf69ea08" class="outline-5">
|
|
<h5 id="orgf69ea08"><span class="section-number-5">2.3.3.3</span> Simscape Model</h5>
|
|
<div class="outline-text-5" id="text-2-3-3-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load(<span class="org-string">'Kiff.mat'</span>, <span class="org-string">'Kiff'</span>)
|
|
</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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dL'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Plate Displacement (encoder)</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gd_iff_opt = 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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf21ad91" class="outline-5">
|
|
<h5 id="orgf21ad91"><span class="section-number-5">2.3.3.4</span> DVF Plant</h5>
|
|
<div class="outline-text-5" id="text-2-3-3-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% IFF Plant</span></span>
|
|
G_enc_iff_opt = zeros(length(f), 6, 6);
|
|
|
|
<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>
|
|
G_enc_iff_opt(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = tfestimate(meas_iff_plates{<span class="org-constant">i</span>}.Va, meas_iff_plates{<span class="org-constant">i</span>}.de, win, [], [], 1<span class="org-type">/</span>Ts);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org1dd1189" class="figure">
|
|
<p><img src="figs/enc_plates_opt_iff_comp_simscape_all.png" alt="enc_plates_opt_iff_comp_simscape_all.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 44: </span>FRF from one actuator to all the encoders when the plant is damped using IFF</p>
|
|
</div>
|
|
|
|
|
|
<div id="org579ee95" class="figure">
|
|
<p><img src="figs/damped_iff_plates_plant_comp_diagonal.png" alt="damped_iff_plates_plant_comp_diagonal.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 45: </span>Comparison of the diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)</p>
|
|
</div>
|
|
|
|
|
|
<div id="org1ba23df" class="figure">
|
|
<p><img src="figs/damped_iff_plates_plant_comp_off_diagonal.png" alt="damped_iff_plates_plant_comp_off_diagonal.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 46: </span>Comparison of the off-diagonal elements of the transfer functions from \(\bm{u}\) to \(d\bm{\mathcal{L}}_m\) with active damping (IFF) applied with an optimal gain \(g = 400\)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9090d9d" class="outline-4">
|
|
<h4 id="org9090d9d"><span class="section-number-4">2.3.4</span> Effect of IFF on the plant - FRF</h4>
|
|
<div class="outline-text-4" id="text-2-3-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load(<span class="org-string">'identified_plants_enc_plates.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'G_dvf'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org1fa27f9" class="figure">
|
|
<p><img src="figs/enc_plant_plates_effect_iff.png" alt="enc_plant_plates_effect_iff.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 47: </span>Effect of the IFF control strategy on the transfer function from \(\bm{\tau}\) to \(d\bm{\mathcal{L}}_m\)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5a28e2e" class="outline-4">
|
|
<h4 id="org5a28e2e"><span class="section-number-4">2.3.5</span> Save Damped Plant</h4>
|
|
<div class="outline-text-4" id="text-2-3-5">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">save(<span class="org-string">'matlab/mat/damped_plant_enc_plates.mat'</span>, <span class="org-string">'f'</span>, <span class="org-string">'Ts'</span>, <span class="org-string">'G_enc_iff_opt'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org14e43ac" class="outline-3">
|
|
<h3 id="org14e43ac"><span class="section-number-3">2.4</span> HAC Control - Frame of the struts</h3>
|
|
<div class="outline-text-3" id="text-2-4">
|
|
<p>
|
|
<a id="org69c97ad"></a>
|
|
</p>
|
|
<p>
|
|
In a first approximation, the Jacobian matrix can be used instead of using the inverse kinematic equations.
|
|
</p>
|
|
|
|
|
|
<div id="org600fbb5" class="figure">
|
|
<p><img src="figs/control_architecture_hac_iff_L.png" alt="control_architecture_hac_iff_L.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 48: </span>HAC-LAC: IFF + Control in the frame of the legs</p>
|
|
</div>
|
|
</div>
|
|
<div id="outline-container-org991fc81" class="outline-4">
|
|
<h4 id="org991fc81"><span class="section-number-4">2.4.1</span> Simscape Model</h4>
|
|
<div class="outline-text-4" id="text-2-4-1">
|
|
<p>
|
|
Let’s start with the Simscape model and the damped plant.
|
|
</p>
|
|
|
|
<p>
|
|
Apply HAC control and verify the system is stable.
|
|
</p>
|
|
|
|
<p>
|
|
Then, try the control strategy on the real plant.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load(<span class="org-string">'Kiff.mat'</span>, <span class="org-string">'Kiff'</span>)
|
|
</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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</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>
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, <span class="org-string">'/du'</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">'/dL'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Plate Displacement (encoder)</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gd_iff_opt = 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>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">isstable(Gd_iff_opt)
|
|
</pre>
|
|
</div>
|
|
|
|
<pre class="example">
|
|
1
|
|
</pre>
|
|
|
|
|
|
|
|
<div id="orgc147a1b" class="figure">
|
|
<p><img src="figs/hac_iff_struts_enc_plates_plant_bode.png" alt="hac_iff_struts_enc_plates_plant_bode.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 49: </span>Transfer function from \(u\) to \(d\mathcal{L}_m\) with IFF (diagonal elements)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org457939e" class="outline-4">
|
|
<h4 id="org457939e"><span class="section-number-4">2.4.2</span> HAC Controller</h4>
|
|
<div class="outline-text-4" id="text-2-4-2">
|
|
<p>
|
|
Let’s try to have 100Hz bandwidth:
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Lead</span></span>
|
|
a = 2; <span class="org-comment">% Amount of phase lead / width of the phase lead / high frequency gain</span>
|
|
wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>100; <span class="org-comment">% Frequency with the maximum phase lead [rad/s]</span>
|
|
|
|
H_lead = (1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">/</span>sqrt(a)))<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">*</span>sqrt(a)));
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Low Pass filter</span></span>
|
|
H_lpf = 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>200);
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Notch</span></span>
|
|
gm = 0.02;
|
|
xi = 0.3;
|
|
wn = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>700;
|
|
|
|
H_notch = (s<span class="org-type">^</span>2 <span class="org-type">+</span> 2<span class="org-type">*</span>gm<span class="org-type">*</span>xi<span class="org-type">*</span>wn<span class="org-type">*</span>s <span class="org-type">+</span> wn<span class="org-type">^</span>2)<span class="org-type">/</span>(s<span class="org-type">^</span>2 <span class="org-type">+</span> 2<span class="org-type">*</span>xi<span class="org-type">*</span>wn<span class="org-type">*</span>s <span class="org-type">+</span> wn<span class="org-type">^</span>2);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Khac_iff_struts = <span class="org-type">-</span>(1<span class="org-type">/</span>(2.87e<span class="org-type">-</span>5)) <span class="org-type">*</span> ...<span class="org-comment"> % Gain</span>
|
|
H_lead <span class="org-type">*</span> ...<span class="org-comment"> % Lead</span>
|
|
H_notch <span class="org-type">*</span> ...<span class="org-comment"> % Notch</span>
|
|
(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>100<span class="org-type">/</span>s) <span class="org-type">*</span> ...<span class="org-comment"> % Integrator</span>
|
|
eye(6); <span class="org-comment">% 6x6 Diagonal</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">save(<span class="org-string">'matlab/mat/Khac_iff_struts.mat'</span>, <span class="org-string">'Khac_iff_struts'</span>)
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Lhac_iff_struts = Khac_iff_struts<span class="org-type">*</span>Gd_iff_opt;
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org152d5ee" class="figure">
|
|
<p><img src="figs/loop_gain_hac_iff_struts.png" alt="loop_gain_hac_iff_struts.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 50: </span>Diagonal and off-diagonal elements of the Loop gain for “HAC-IFF-Struts”</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org98d910a" class="outline-4">
|
|
<h4 id="org98d910a"><span class="section-number-4">2.4.3</span> Experimental Loop Gain</h4>
|
|
<div class="outline-text-4" id="text-2-4-3">
|
|
<ul class="org-ul">
|
|
<li class="off"><code>[ ]</code> Find a more clever way to do the multiplication</li>
|
|
</ul>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">L_frf = zeros(size(G_enc_iff_opt));
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:size(G_enc_iff_opt, 1)</span>
|
|
L_frf(<span class="org-constant">i</span>, <span class="org-type">:</span>, <span class="org-type">:</span>) = squeeze(G_enc_iff_opt(<span class="org-constant">i</span>,<span class="org-type">:</span>,<span class="org-type">:</span>))<span class="org-type">*</span>freqresp(Khac_iff_struts, f(<span class="org-constant">i</span>), <span class="org-string">'Hz'</span>);
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org5d41dbc" class="figure">
|
|
<p><img src="figs/hac_iff_plates_exp_loop_gain_diag.png" alt="hac_iff_plates_exp_loop_gain_diag.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 51: </span>Diagonal and Off-diagonal elements of the Loop gain (experimental data)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org317850e" class="outline-4">
|
|
<h4 id="org317850e"><span class="section-number-4">2.4.4</span> Verification of the Stability using the Simscape model</h4>
|
|
<div class="outline-text-4" id="text-2-4-4">
|
|
<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">'plates'</span>, ...
|
|
<span class="org-string">'actuator_type'</span>, <span class="org-string">'flexible'</span>, ...
|
|
<span class="org-string">'controller_type'</span>, <span class="org-string">'hac-iff-struts'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gd_iff_hac_opt = linearize(mdl, io, 0.0, options);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">isstable(Gd_iff_hac_opt)
|
|
</pre>
|
|
</div>
|
|
|
|
<pre class="example">
|
|
1
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org247dba8" class="outline-3">
|
|
<h3 id="org247dba8"><span class="section-number-3">2.5</span> Reference Tracking</h3>
|
|
<div class="outline-text-3" id="text-2-5">
|
|
<p>
|
|
<a id="org7f2d506"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-orgd862f99" class="outline-4">
|
|
<h4 id="orgd862f99"><span class="section-number-4">2.5.1</span> Load</h4>
|
|
<div class="outline-text-4" id="text-2-5-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load(<span class="org-string">'Khac_iff_struts.mat'</span>, <span class="org-string">'Khac_iff_struts'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2a92c68" class="outline-4">
|
|
<h4 id="org2a92c68"><span class="section-number-4">2.5.2</span> Y-Z Scans</h4>
|
|
<div class="outline-text-4" id="text-2-5-2">
|
|
</div>
|
|
<div id="outline-container-org3c5f01d" class="outline-5">
|
|
<h5 id="org3c5f01d"><span class="section-number-5">2.5.2.1</span> Generate the Scan</h5>
|
|
<div class="outline-text-5" id="text-2-5-2-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Rx_yz = generateYZScanTrajectory(...
|
|
<span class="org-string">'y_tot'</span>, 4e<span class="org-type">-</span>6, ...
|
|
<span class="org-string">'z_tot'</span>, 8e<span class="org-type">-</span>6, ...
|
|
<span class="org-string">'n'</span>, 5, ...
|
|
<span class="org-string">'Ts'</span>, 1e<span class="org-type">-</span>3, ...
|
|
<span class="org-string">'ti'</span>, 2, ...
|
|
<span class="org-string">'tw'</span>, 0.5, ...
|
|
<span class="org-string">'ty'</span>, 2, ...
|
|
<span class="org-string">'tz'</span>, 1);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
hold on;
|
|
plot(Rx_yz(<span class="org-type">:</span>,1), Rx_yz(<span class="org-type">:</span>,3), ...
|
|
<span class="org-string">'DisplayName'</span>, <span class="org-string">'Y motion'</span>)
|
|
plot(Rx_yz(<span class="org-type">:</span>,1), Rx_yz(<span class="org-type">:</span>,4), ...
|
|
<span class="org-string">'DisplayName'</span>, <span class="org-string">'Z motion'</span>)
|
|
hold off;
|
|
xlabel(<span class="org-string">'Time [s]'</span>);
|
|
ylabel(<span class="org-string">'Displacement [m]'</span>);
|
|
legend(<span class="org-string">'location'</span>, <span class="org-string">'northeast'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
plot(Rx_yz(<span class="org-type">:</span>,3), Rx_yz(<span class="org-type">:</span>,4));
|
|
xlabel(<span class="org-string">'y [m]'</span>); ylabel(<span class="org-string">'z [m]'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org213a892" class="outline-5">
|
|
<h5 id="org213a892"><span class="section-number-5">2.5.2.2</span> Reference Signal for the Strut lengths</h5>
|
|
<div class="outline-text-5" id="text-2-5-2-2">
|
|
<p>
|
|
Let’s use the Jacobian to estimate the wanted strut length as a function of time.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">dL_ref = [n_hexapod.geometry.J<span class="org-type">*</span>Rx_yz(<span class="org-type">:</span>, 2<span class="org-type">:</span>7)<span class="org-type">'</span>]<span class="org-type">'</span>;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
hold on;
|
|
<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>
|
|
plot(Rx_yz(<span class="org-type">:</span>,1), dL_ref(<span class="org-type">:</span>, <span class="org-constant">i</span>))
|
|
<span class="org-keyword">end</span>
|
|
xlabel(<span class="org-string">'Time [s]'</span>); ylabel(<span class="org-string">'Displacement [m]'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge04e29b" class="outline-5">
|
|
<h5 id="orge04e29b"><span class="section-number-5">2.5.2.3</span> Time domain simulation with 2DoF model</h5>
|
|
<div class="outline-text-5" id="text-2-5-2-3">
|
|
<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">'2dof'</span>, ...
|
|
<span class="org-string">'flex_top_type'</span>, <span class="org-string">'3dof'</span>, ...
|
|
<span class="org-string">'motion_sensor_type'</span>, <span class="org-string">'plates'</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">'hac-iff-struts'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">mdl</span>,<span class="org-string">'StopTime'</span>, num2str(Rx_yz(<span class="org-variable-name">end</span>,1)))
|
|
<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">mdl</span>,<span class="org-string">'SimulationCommand'</span>,<span class="org-string">'start'</span>)
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">out.X.Data = out.X.Data <span class="org-type">-</span> out.X.Data(1,<span class="org-type">:</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org7b6723e" class="figure">
|
|
<p><img src="figs/ref_track_hac_iff_struts_yz_plane.png" alt="ref_track_hac_iff_struts_yz_plane.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 52: </span>Simulated Y-Z motion</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgd3216d3" class="figure">
|
|
<p><img src="figs/ref_track_hac_iff_struts_yz_time.png" alt="ref_track_hac_iff_struts_yz_time.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 53: </span>Y and Z motion as a function of time as well as the reference signals</p>
|
|
</div>
|
|
|
|
|
|
<div id="orgdf9d1bd" class="figure">
|
|
<p><img src="figs/ref_track_hac_iff_struts_pos_error.png" alt="ref_track_hac_iff_struts_pos_error.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 54: </span>Positioning errors as a function of time</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org995b778" class="outline-4">
|
|
<h4 id="org995b778"><span class="section-number-4">2.5.3</span> “NASS” reference path</h4>
|
|
<div class="outline-text-4" id="text-2-5-3">
|
|
</div>
|
|
<div id="outline-container-org065d872" class="outline-5">
|
|
<h5 id="org065d872"><span class="section-number-5">2.5.3.1</span> Generate Path</h5>
|
|
<div class="outline-text-5" id="text-2-5-3-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ref_path = [ ...
|
|
0, 0, 0;
|
|
0, 0, 1; <span class="org-comment">% N</span>
|
|
0, 4, 1;
|
|
3, 0, 1;
|
|
3, 4, 1;
|
|
3, 4, 0;
|
|
4, 0, 0;
|
|
4, 0, 1; <span class="org-comment">% A</span>
|
|
4, 3, 1;
|
|
5, 4, 1;
|
|
6, 4, 1;
|
|
7, 3, 1;
|
|
7, 2, 1;
|
|
4, 2, 1;
|
|
4, 3, 1;
|
|
5, 4, 1;
|
|
6, 4, 1;
|
|
7, 3, 1;
|
|
7, 0, 1;
|
|
7, 0, 0;
|
|
8, 0, 0;
|
|
8, 0, 1; <span class="org-comment">% S</span>
|
|
11, 0, 1;
|
|
11, 2, 1;
|
|
8, 2, 1;
|
|
8, 4, 1;
|
|
11, 4, 1;
|
|
11, 4, 0;
|
|
12, 0, 0;
|
|
12, 0, 1; <span class="org-comment">% S</span>
|
|
15, 0, 1;
|
|
15, 2, 1;
|
|
12, 2, 1;
|
|
12, 4, 1;
|
|
15, 4, 1;
|
|
15, 4, 0;
|
|
];
|
|
|
|
<span class="org-comment">% Center the trajectory arround zero</span>
|
|
ref_path = ref_path <span class="org-type">-</span> (max(ref_path) <span class="org-type">-</span> min(ref_path))<span class="org-type">/</span>2;
|
|
|
|
<span class="org-comment">% Define the X-Y-Z cuboid dimensions containing the trajectory</span>
|
|
X_max = 10e<span class="org-type">-</span>6;
|
|
Y_max = 4e<span class="org-type">-</span>6;
|
|
Z_max = 2e<span class="org-type">-</span>6;
|
|
|
|
ref_path = ([X_max, Y_max, Z_max]<span class="org-type">./</span>max(ref_path))<span class="org-type">.*</span>ref_path; <span class="org-comment">% [m]</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Rx_nass = generateXYZTrajectory(<span class="org-string">'points'</span>, ref_path);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
plot(1e6<span class="org-type">*</span>Rx_nass(Rx_nass(<span class="org-type">:</span>,4)<span class="org-type">></span>0, 2), 1e6<span class="org-type">*</span>Rx_nass(Rx_nass(<span class="org-type">:</span>,4)<span class="org-type">></span>0, 3), <span class="org-string">'k.'</span>)
|
|
xlabel(<span class="org-string">'X [$\mu m$]'</span>);
|
|
ylabel(<span class="org-string">'Y [$\mu m$]'</span>);
|
|
<span class="org-type">axis</span> equal;
|
|
xlim(1e6<span class="org-type">*</span>[min(Rx_nass(<span class="org-type">:</span>,2)), max(Rx_nass(<span class="org-type">:</span>,2))]);
|
|
ylim(1e6<span class="org-type">*</span>[min(Rx_nass(<span class="org-type">:</span>,3)), max(Rx_nass(<span class="org-type">:</span>,3))]);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org73c947b" class="figure">
|
|
<p><img src="figs/ref_track_test_nass.png" alt="ref_track_test_nass.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 55: </span>Reference path corresponding to the “NASS” acronym</p>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
plot3(Rx_nass(<span class="org-type">:</span>,2), Rx_nass(<span class="org-type">:</span>,3), Rx_nass(<span class="org-type">:</span>,4), <span class="org-string">'k-'</span>);
|
|
xlabel(<span class="org-string">'x'</span>);
|
|
ylabel(<span class="org-string">'y'</span>);
|
|
zlabel(<span class="org-string">'z'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
hold on;
|
|
plot(Rx_nass(<span class="org-type">:</span>,1), Rx_nass(<span class="org-type">:</span>,2));
|
|
plot(Rx_nass(<span class="org-type">:</span>,1), Rx_nass(<span class="org-type">:</span>,3));
|
|
plot(Rx_nass(<span class="org-type">:</span>,1), Rx_nass(<span class="org-type">:</span>,4));
|
|
hold off;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
scatter(Rx_nass(<span class="org-type">:</span>,2), Rx_nass(<span class="org-type">:</span>,3), 1, Rx_nass(<span class="org-type">:</span>,4), <span class="org-string">'filled'</span>)
|
|
colormap winter
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="outline-container-org57675af" class="outline-5">
|
|
<h5 id="org57675af"><span class="section-number-5">2.5.3.2</span> Simscape Simulations</h5>
|
|
<div class="outline-text-5" id="text-2-5-3-2">
|
|
<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">'2dof'</span>, ...
|
|
<span class="org-string">'flex_top_type'</span>, <span class="org-string">'3dof'</span>, ...
|
|
<span class="org-string">'motion_sensor_type'</span>, <span class="org-string">'plates'</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">'hac-iff-struts'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">mdl</span>,<span class="org-string">'StopTime'</span>, num2str(Rx_nass(<span class="org-variable-name">end</span>,1)))
|
|
<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">mdl</span>,<span class="org-string">'SimulationCommand'</span>,<span class="org-string">'start'</span>)
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">out.X.Data = out.X.Data <span class="org-type">-</span> out.X.Data(1,<span class="org-type">:</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org75465ae" class="outline-4">
|
|
<h4 id="org75465ae"><span class="section-number-4">2.5.4</span> Save Reference paths</h4>
|
|
<div class="outline-text-4" id="text-2-5-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">save(<span class="org-string">'matlab/mat/reference_path.mat'</span>, <span class="org-string">'Rx_yz'</span>, <span class="org-string">'Rx_nass'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge43ac25" class="outline-4">
|
|
<h4 id="orge43ac25"><span class="section-number-4">2.5.5</span> Experimental Results</h4>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org71bb087" class="outline-3">
|
|
<h3 id="org71bb087"><span class="section-number-3">2.6</span> Feedforward (Open-Loop) Control</h3>
|
|
<div class="outline-text-3" id="text-2-6">
|
|
</div>
|
|
<div id="outline-container-org3ea802c" class="outline-4">
|
|
<h4 id="org3ea802c"><span class="section-number-4">2.6.1</span> Introduction</h4>
|
|
<div class="outline-text-4" id="text-2-6-1">
|
|
|
|
<div id="org4a1bdc9" class="figure">
|
|
<p><img src="figs/control_architecture_iff_feedforward.png" alt="control_architecture_iff_feedforward.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 56: </span>Feedforward control in the frame of the legs</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgc7d508e" class="outline-4">
|
|
<h4 id="orgc7d508e"><span class="section-number-4">2.6.2</span> Simple Feedforward Controller</h4>
|
|
<div class="outline-text-4" id="text-2-6-2">
|
|
<p>
|
|
Let’s estimate the mean DC gain for the damped plant (diagonal elements:)
|
|
</p>
|
|
<pre class="example">
|
|
1.773e-05
|
|
</pre>
|
|
|
|
|
|
<p>
|
|
The feedforward controller is then taken as the inverse of this gain (the minus sign is there manually added as it is “removed” by the <code>abs</code> function):
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Kff_iff_L = <span class="org-type">-</span>1<span class="org-type">/</span>mean(diag(abs(squeeze(mean(G_enc_iff_opt(f<span class="org-type">></span>2 <span class="org-type">&</span> f<span class="org-type"><</span>4,<span class="org-type">:</span>,<span class="org-type">:</span>))))));
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The open-loop gain (feedforward controller times the damped plant) is shown in Figure <a href="#org2a759b4">57</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org2a759b4" class="figure">
|
|
<p><img src="figs/open_loop_gain_feedforward_iff_struts.png" alt="open_loop_gain_feedforward_iff_struts.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 57: </span>Diagonal elements of the “open loop gain”</p>
|
|
</div>
|
|
|
|
<p>
|
|
And save the feedforward controller for further use:
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Kff_iff_L = zpk(Kff_iff_L)<span class="org-type">*</span>eye(6);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">save(<span class="org-string">'matlab/mat/feedforward_iff.mat'</span>, <span class="org-string">'Kff_iff_L'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4c2c4ae" class="outline-4">
|
|
<h4 id="org4c2c4ae"><span class="section-number-4">2.6.3</span> Test with Simscape Model</h4>
|
|
<div class="outline-text-4" id="text-2-6-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load(<span class="org-string">'reference_path.mat'</span>, <span class="org-string">'Rx_yz'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd127949" class="outline-3">
|
|
<h3 id="orgd127949"><span class="section-number-3">2.7</span> Feedback/Feedforward control in the frame of the struts</h3>
|
|
<div class="outline-text-3" id="text-2-7">
|
|
|
|
<div id="org962f0fa" class="figure">
|
|
<p><img src="figs/control_architecture_hac_iff_L_feedforward.png" alt="control_architecture_hac_iff_L_feedforward.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 58: </span>Feedback/Feedforward control in the frame of the legs</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-org762064f" class="outline-2">
|
|
<h2 id="org762064f"><span class="section-number-2">3</span> Functions</h2>
|
|
<div class="outline-text-2" id="text-3">
|
|
</div>
|
|
<div id="outline-container-org9c82eb4" class="outline-3">
|
|
<h3 id="org9c82eb4"><span class="section-number-3">3.1</span> <code>generateXYZTrajectory</code></h3>
|
|
<div class="outline-text-3" id="text-3-1">
|
|
<p>
|
|
<a id="org4e35c09"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org6177d10" class="outline-4">
|
|
<h4 id="org6177d10">Function description</h4>
|
|
<div class="outline-text-4" id="text-org6177d10">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[ref]</span> = <span class="org-function-name">generateXYZTrajectory</span>(<span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% generateXYZTrajectory -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [ref] = generateXYZTrajectory(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - ref - Reference Signal</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgbf6b8b1" class="outline-4">
|
|
<h4 id="orgbf6b8b1">Optional Parameters</h4>
|
|
<div class="outline-text-4" id="text-orgbf6b8b1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.points double {mustBeNumeric} = zeros(2, 3) <span class="org-comment">% [m]</span>
|
|
|
|
<span class="org-variable-name">args</span>.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 <span class="org-comment">% Time to go to first point and after last point [s]</span>
|
|
<span class="org-variable-name">args</span>.tw (1,1) double {mustBeNumeric, mustBePositive} = 0.5 <span class="org-comment">% Time wait between each point [s]</span>
|
|
<span class="org-variable-name">args</span>.tm (1,1) double {mustBeNumeric, mustBePositive} = 1 <span class="org-comment">% Motion time between points [s]</span>
|
|
|
|
<span class="org-variable-name">args</span>.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e<span class="org-type">-</span>3 <span class="org-comment">% Sampling Time [s]</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb818291" class="outline-4">
|
|
<h4 id="orgb818291">Initialize Time Vectors</h4>
|
|
<div class="outline-text-4" id="text-orgb818291">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">time_i = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.ti;
|
|
time_w = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.tw;
|
|
time_m = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.tm;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org361222a" class="outline-4">
|
|
<h4 id="org361222a">XYZ Trajectory</h4>
|
|
<div class="outline-text-4" id="text-org361222a">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-comment">% Go to initial position</span>
|
|
xyz = (args.points(1,<span class="org-type">:</span>))<span class="org-type">'*</span>(time_i<span class="org-type">/</span>args.ti);
|
|
|
|
<span class="org-comment">% Wait</span>
|
|
xyz = [xyz, xyz(<span class="org-type">:</span>,end)<span class="org-type">.*</span>ones(size(time_w))];
|
|
|
|
<span class="org-comment">% Scans</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">2:size(args.points, 1)</span>
|
|
<span class="org-comment">% Go to next point</span>
|
|
xyz = [xyz, xyz(<span class="org-type">:</span>,end) <span class="org-type">+</span> (args.points(<span class="org-constant">i</span>,<span class="org-type">:</span>)<span class="org-type">'</span> <span class="org-type">-</span> xyz(<span class="org-type">:</span>,end))<span class="org-type">*</span>(time_m<span class="org-type">/</span>args.tm)];
|
|
<span class="org-comment">% Wait a litle bit</span>
|
|
xyz = [xyz, xyz(<span class="org-type">:</span>,end)<span class="org-type">.*</span>ones(size(time_w))];
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-comment">% End motion</span>
|
|
xyz = [xyz, xyz(<span class="org-type">:</span>,end) <span class="org-type">-</span> xyz(<span class="org-type">:</span>,end)<span class="org-type">*</span>(time_i<span class="org-type">/</span>args.ti)];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org84542e6" class="outline-4">
|
|
<h4 id="org84542e6">Reference Signal</h4>
|
|
<div class="outline-text-4" id="text-org84542e6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">t = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.Ts<span class="org-type">*</span>(length(xyz) <span class="org-type">-</span> 1);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ref = zeros(length(xyz), 7);
|
|
|
|
ref(<span class="org-type">:</span>, 1) = t;
|
|
ref(<span class="org-type">:</span>, 2<span class="org-type">:</span>4) = xyz<span class="org-type">'</span>;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4433a1b" class="outline-3">
|
|
<h3 id="org4433a1b"><span class="section-number-3">3.2</span> <code>generateYZScanTrajectory</code></h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
<p>
|
|
<a id="org5b64c20"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org0479bfd" class="outline-4">
|
|
<h4 id="org0479bfd">Function description</h4>
|
|
<div class="outline-text-4" id="text-org0479bfd">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[ref]</span> = <span class="org-function-name">generateYZScanTrajectory</span>(<span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% generateYZScanTrajectory -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [ref] = generateYZScanTrajectory(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - ref - Reference Signal</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7be834b" class="outline-4">
|
|
<h4 id="org7be834b">Optional Parameters</h4>
|
|
<div class="outline-text-4" id="text-org7be834b">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">arguments</span>
|
|
<span class="org-variable-name">args</span>.y_tot (1,1) double {mustBeNumeric} = 10e<span class="org-type">-</span>6 <span class="org-comment">% [m]</span>
|
|
<span class="org-variable-name">args</span>.z_tot (1,1) double {mustBeNumeric} = 10e<span class="org-type">-</span>6 <span class="org-comment">% [m]</span>
|
|
|
|
<span class="org-variable-name">args</span>.n (1,1) double {mustBeInteger, mustBePositive} = 10 <span class="org-comment">% [-]</span>
|
|
|
|
<span class="org-variable-name">args</span>.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e<span class="org-type">-</span>4 <span class="org-comment">% [s]</span>
|
|
|
|
<span class="org-variable-name">args</span>.ti (1,1) double {mustBeNumeric, mustBePositive} = 1 <span class="org-comment">% [s]</span>
|
|
<span class="org-variable-name">args</span>.tw (1,1) double {mustBeNumeric, mustBePositive} = 1 <span class="org-comment">% [s]</span>
|
|
<span class="org-variable-name">args</span>.ty (1,1) double {mustBeNumeric, mustBePositive} = 1 <span class="org-comment">% [s]</span>
|
|
<span class="org-variable-name">args</span>.tz (1,1) double {mustBeNumeric, mustBePositive} = 1 <span class="org-comment">% [s]</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org81214f7" class="outline-4">
|
|
<h4 id="org81214f7">Initialize Time Vectors</h4>
|
|
<div class="outline-text-4" id="text-org81214f7">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">time_i = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.ti;
|
|
time_w = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.tw;
|
|
time_y = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.ty;
|
|
time_z = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.tz;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5ab49af" class="outline-4">
|
|
<h4 id="org5ab49af">Y and Z vectors</h4>
|
|
<div class="outline-text-4" id="text-org5ab49af">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-comment">% Go to initial position</span>
|
|
y = (time_i<span class="org-type">/</span>args.ti)<span class="org-type">*</span>(args.y_tot<span class="org-type">/</span>2);
|
|
|
|
<span class="org-comment">% Wait</span>
|
|
y = [y, y(end)<span class="org-type">*</span>ones(size(time_w))];
|
|
|
|
<span class="org-comment">% Scans</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:args.n</span>
|
|
<span class="org-keyword">if</span> mod(<span class="org-constant">i</span>,2) <span class="org-type">==</span> 0
|
|
y = [y, <span class="org-type">-</span>(args.y_tot<span class="org-type">/</span>2) <span class="org-type">+</span> (time_y<span class="org-type">/</span>args.ty)<span class="org-type">*</span>args.y_tot];
|
|
<span class="org-keyword">else</span>
|
|
y = [y, (args.y_tot<span class="org-type">/</span>2) <span class="org-type">-</span> (time_y<span class="org-type">/</span>args.ty)<span class="org-type">*</span>args.y_tot];
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-keyword">if</span> <span class="org-constant">i</span> <span class="org-type"><</span> args.n
|
|
y = [y, y(end)<span class="org-type">*</span>ones(size(time_z))];
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-comment">% Wait a litle bit</span>
|
|
y = [y, y(end)<span class="org-type">*</span>ones(size(time_w))];
|
|
|
|
<span class="org-comment">% End motion</span>
|
|
y = [y, y(end) <span class="org-type">-</span> y(end)<span class="org-type">*</span>time_i<span class="org-type">/</span>args.ti];
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-comment">% Go to initial position</span>
|
|
z = (time_i<span class="org-type">/</span>args.ti)<span class="org-type">*</span>(args.z_tot<span class="org-type">/</span>2);
|
|
|
|
<span class="org-comment">% Wait</span>
|
|
z = [z, z(end)<span class="org-type">*</span>ones(size(time_w))];
|
|
|
|
<span class="org-comment">% Scans</span>
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:args.n</span>
|
|
z = [z, z(end)<span class="org-type">*</span>ones(size(time_y))];
|
|
|
|
<span class="org-keyword">if</span> <span class="org-constant">i</span> <span class="org-type"><</span> args.n
|
|
z = [z, z(end) <span class="org-type">-</span> (time_z<span class="org-type">/</span>args.tz)<span class="org-type">*</span>args.z_tot<span class="org-type">/</span>(args.n<span class="org-type">-</span>1)];
|
|
<span class="org-keyword">end</span>
|
|
<span class="org-keyword">end</span>
|
|
|
|
<span class="org-comment">% Wait a litle bit</span>
|
|
z = [z, z(end)<span class="org-type">*</span>ones(size(time_w))];
|
|
|
|
<span class="org-comment">% End motion</span>
|
|
z = [z, z(end) <span class="org-type">-</span> z(end)<span class="org-type">*</span>time_i<span class="org-type">/</span>args.ti];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org796db82" class="outline-4">
|
|
<h4 id="org796db82">Reference Signal</h4>
|
|
<div class="outline-text-4" id="text-org796db82">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">t = 0<span class="org-type">:</span>args.Ts<span class="org-type">:</span>args.Ts<span class="org-type">*</span>(length(y) <span class="org-type">-</span> 1);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ref = zeros(length(y), 7);
|
|
|
|
ref(<span class="org-type">:</span>, 1) = t;
|
|
ref(<span class="org-type">:</span>, 3) = y;
|
|
ref(<span class="org-type">:</span>, 4) = z;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org01056ff" class="outline-3">
|
|
<h3 id="org01056ff"><span class="section-number-3">3.3</span> <code>getTransformationMatrixAcc</code></h3>
|
|
<div class="outline-text-3" id="text-3-3">
|
|
<p>
|
|
<a id="org39415ee"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org37f755d" class="outline-4">
|
|
<h4 id="org37f755d">Function description</h4>
|
|
<div class="outline-text-4" id="text-org37f755d">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[M]</span> = <span class="org-function-name">getTransformationMatrixAcc</span>(<span class="org-variable-name">Opm</span>, <span class="org-variable-name">Osm</span>)
|
|
<span class="org-comment">% getTransformationMatrixAcc -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [M] = getTransformationMatrixAcc(Opm, Osm)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - Opm - Nx3 (N = number of accelerometer measurements) X,Y,Z position of accelerometers</span>
|
|
<span class="org-comment">% - Opm - Nx3 (N = number of accelerometer measurements) Unit vectors representing the accelerometer orientation</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - M - Transformation Matrix</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf2435ad" class="outline-4">
|
|
<h4 id="orgf2435ad">Transformation matrix from motion of the solid body to accelerometer measurements</h4>
|
|
<div class="outline-text-4" id="text-orgf2435ad">
|
|
<p>
|
|
Let’s try to estimate the x-y-z acceleration of any point of the solid body from the acceleration/angular acceleration of the solid body expressed in \(\{O\}\).
|
|
For any point \(p_i\) of the solid body (corresponding to an accelerometer), we can write:
|
|
</p>
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
a_{i,x} \\ a_{i,y} \\ a_{i,z}
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
|
|
\end{bmatrix} + p_i \times \begin{bmatrix}
|
|
\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
<p>
|
|
We can write the cross product as a matrix product using the skew-symmetric transformation:
|
|
</p>
|
|
\begin{equation}
|
|
\begin{bmatrix}
|
|
a_{i,x} \\ a_{i,y} \\ a_{i,z}
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
|
|
\end{bmatrix} + \underbrace{\begin{bmatrix}
|
|
0 & p_{i,z} & -p_{i,y} \\
|
|
-p_{i,z} & 0 & p_{i,x} \\
|
|
p_{i,y} & -p_{i,x} & 0
|
|
\end{bmatrix}}_{P_{i,[\times]}} \cdot \begin{bmatrix}
|
|
\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
<p>
|
|
If we now want to know the (scalar) acceleration \(a_i\) of the point \(p_i\) in the direction of the accelerometer direction \(\hat{s}_i\), we can just project the 3d acceleration on \(\hat{s}_i\):
|
|
</p>
|
|
\begin{equation}
|
|
a_i = \hat{s}_i^T \cdot \begin{bmatrix}
|
|
a_{i,x} \\ a_{i,y} \\ a_{i,z}
|
|
\end{bmatrix} = \hat{s}_i^T \cdot \begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
|
|
\end{bmatrix} + \left( \hat{s}_i^T \cdot P_{i,[\times]} \right) \cdot \begin{bmatrix}
|
|
\dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
<p>
|
|
Which is equivalent as a simple vector multiplication:
|
|
</p>
|
|
\begin{equation}
|
|
a_i = \begin{bmatrix}
|
|
\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
|
|
\end{bmatrix}
|
|
\begin{bmatrix}
|
|
\dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \\ \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
|
|
\end{bmatrix} = \begin{bmatrix}
|
|
\hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
|
|
\end{bmatrix} {}^O\vec{x}
|
|
\end{equation}
|
|
|
|
<p>
|
|
And finally we can combine the 6 (line) vectors for the 6 accelerometers to write that in a matrix form.
|
|
We obtain Eq. \eqref{eq:M_matrix}.
|
|
</p>
|
|
<div class="important" id="org340212d">
|
|
<p>
|
|
The transformation from solid body acceleration \({}^O\vec{x}\) from sensor measured acceleration \(\vec{a}\) is:
|
|
</p>
|
|
\begin{equation} \label{eq:M_matrix}
|
|
\vec{a} = \underbrace{\begin{bmatrix}
|
|
\hat{s}_1^T & \hat{s}_1^T \cdot P_{1,[\times]} \\
|
|
\vdots & \vdots \\
|
|
\hat{s}_6^T & \hat{s}_6^T \cdot P_{6,[\times]}
|
|
\end{bmatrix}}_{M} {}^O\vec{x}
|
|
\end{equation}
|
|
|
|
<p>
|
|
with \(\hat{s}_i\) the unit vector representing the measured direction of the i’th accelerometer expressed in frame \(\{O\}\) and \(P_{i,[\times]}\) the skew-symmetric matrix representing the cross product of the position of the i’th accelerometer expressed in frame \(\{O\}\).
|
|
</p>
|
|
|
|
</div>
|
|
|
|
<p>
|
|
Let’s define such matrix using matlab:
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">M = zeros(length(Opm), 6);
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(Opm)</span>
|
|
Ri = [0, Opm(3,<span class="org-constant">i</span>), <span class="org-type">-</span>Opm(2,<span class="org-constant">i</span>);
|
|
<span class="org-type">-</span>Opm(3,<span class="org-constant">i</span>), 0, Opm(1,<span class="org-constant">i</span>);
|
|
Opm(2,<span class="org-constant">i</span>), <span class="org-type">-</span>Opm(1,<span class="org-constant">i</span>), 0];
|
|
M(<span class="org-constant">i</span>, 1<span class="org-type">:</span>3) = Osm(<span class="org-type">:</span>,<span class="org-constant">i</span>)<span class="org-type">'</span>;
|
|
M(<span class="org-constant">i</span>, 4<span class="org-type">:</span>6) = Osm(<span class="org-type">:</span>,<span class="org-constant">i</span>)<span class="org-type">'*</span>Ri;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
<div id="outline-container-orgab18063" class="outline-3">
|
|
<h3 id="orgab18063"><span class="section-number-3">3.4</span> <code>getJacobianNanoHexapod</code></h3>
|
|
<div class="outline-text-3" id="text-3-4">
|
|
<p>
|
|
<a id="orgd78b26a"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<div id="outline-container-org7e86fe2" class="outline-4">
|
|
<h4 id="org7e86fe2">Function description</h4>
|
|
<div class="outline-text-4" id="text-org7e86fe2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[J]</span> = <span class="org-function-name">getJacobianNanoHexapod</span>(<span class="org-variable-name">Hbm</span>)
|
|
<span class="org-comment">% getJacobianNanoHexapod -</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [J] = getJacobianNanoHexapod(Hbm)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - Hbm - Height of {B} w.r.t. {M} [m]</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Outputs:</span>
|
|
<span class="org-comment">% - J - Jacobian Matrix</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb9e790e" class="outline-4">
|
|
<h4 id="orgb9e790e">Transformation matrix from motion of the solid body to accelerometer measurements</h4>
|
|
<div class="outline-text-4" id="text-orgb9e790e">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Fa = [[<span class="org-type">-</span>86.05, <span class="org-type">-</span>74.78, 22.49],
|
|
[ 86.05, <span class="org-type">-</span>74.78, 22.49],
|
|
[ 107.79, <span class="org-type">-</span>37.13, 22.49],
|
|
[ 21.74, 111.91, 22.49],
|
|
[<span class="org-type">-</span>21.74, 111.91, 22.49],
|
|
[<span class="org-type">-</span>107.79, <span class="org-type">-</span>37.13, 22.49]]<span class="org-type">'*</span>1e<span class="org-type">-</span>3; <span class="org-comment">% Ai w.r.t. {F} [m]</span>
|
|
|
|
Mb = [[<span class="org-type">-</span>28.47, <span class="org-type">-</span>106.25, <span class="org-type">-</span>22.50],
|
|
[ 28.47, <span class="org-type">-</span>106.25, <span class="org-type">-</span>22.50],
|
|
[ 106.25, 28.47, <span class="org-type">-</span>22.50],
|
|
[ 77.78, 77.78, <span class="org-type">-</span>22.50],
|
|
[<span class="org-type">-</span>77.78, 77.78, <span class="org-type">-</span>22.50],
|
|
[<span class="org-type">-</span>106.25, 28.47, <span class="org-type">-</span>22.50]]<span class="org-type">'*</span>1e<span class="org-type">-</span>3; <span class="org-comment">% Bi w.r.t. {M} [m]</span>
|
|
|
|
H = 95e<span class="org-type">-</span>3; <span class="org-comment">% Stewart platform height [m]</span>
|
|
Fb = Mb <span class="org-type">+</span> [0; 0; H]; <span class="org-comment">% Bi w.r.t. {F} [m]</span>
|
|
|
|
si = Fb <span class="org-type">-</span> Fa;
|
|
si = si<span class="org-type">./</span>vecnorm(si); <span class="org-comment">% Normalize</span>
|
|
|
|
Bb = Mb <span class="org-type">-</span> [0; 0; Hbm];
|
|
|
|
J = [si<span class="org-type">'</span>, cross(Bb, si)<span class="org-type">'</span>];
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2021-06-30 mer. 22:41</p>
|
|
</div>
|
|
</body>
|
|
</html>
|