Add study of active damping techniques

This commit is contained in:
Thomas Dehaeze 2020-02-06 15:39:35 +01:00
parent 053329875b
commit f06a119922
33 changed files with 1587 additions and 93 deletions

View File

@ -0,0 +1,2 @@
<?xml version='1.0' encoding='UTF-8'?>
<Info Ref="matlab" Type="Relative" />

View File

@ -1,14 +1,13 @@
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<?xml version="1.0" encoding="utf-8"?> <?xml version="1.0" encoding="utf-8"?>
<?xml version="1.0" encoding="utf-8"?>
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN" <!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd"> "http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en"> <html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head> <head>
<!-- 2020-01-22 mer. 16:31 --> <!-- 2020-02-06 jeu. 15:39 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" /> <meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" /> <meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Stewart Platform - Active Damping</title> <title>Stewart Platform - Decentralized Active Damping</title>
<meta name="generator" content="Org mode" /> <meta name="generator" content="Org mode" />
<meta name="author" content="Dehaeze Thomas" /> <meta name="author" content="Dehaeze Thomas" />
<style type="text/css"> <style type="text/css">
@ -247,6 +246,16 @@ for the JavaScript code in this tag.
} }
/*]]>*///--> /*]]>*///-->
</script> </script>
<script>
MathJax = {
tex: { macros: {
bm: ["\\boldsymbol{#1}",1],
}
}
};
</script>
<script type="text/javascript"
src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
</head> </head>
<body> <body>
<div id="org-div-home-and-up"> <div id="org-div-home-and-up">
@ -254,55 +263,72 @@ for the JavaScript code in this tag.
| |
<a accesskey="H" href="./index.html"> HOME </a> <a accesskey="H" href="./index.html"> HOME </a>
</div><div id="content"> </div><div id="content">
<h1 class="title">Stewart Platform - Active Damping</h1> <h1 class="title">Stewart Platform - Decentralized Active Damping</h1>
<div id="table-of-contents"> <div id="table-of-contents">
<h2>Table of Contents</h2> <h2>Table of Contents</h2>
<div id="text-table-of-contents"> <div id="text-table-of-contents">
<ul> <ul>
<li><a href="#orgbab9da7">1. Inertial Control</a> <li><a href="#orgfba33d4">1. Inertial Control</a>
<ul> <ul>
<li><a href="#orgd9e8d20">1.1. Simscape Model</a></li> <li><a href="#org0ea4bd4">1.1. Identification of the Dynamics</a></li>
<li><a href="#orge6285ea">1.2. Initialize the Stewart model</a></li> <li><a href="#org5a29480">1.2. Effect of the Flexible Joint stiffness on the Dynamics</a></li>
<li><a href="#org471f4bb">1.3. Identification of the Dynamics</a></li> <li><a href="#orga92be75">1.3. Obtained Damping</a></li>
<li><a href="#org30b2e11">1.4. Analysis</a></li> <li><a href="#orgb29f377">1.4. Conclusion</a></li>
<li><a href="#org7f07dca">1.5. Conclusion</a></li> </ul>
</li>
<li><a href="#org5fde56d">2. Integral Force Feedback</a>
<ul>
<li><a href="#org8823e64">2.1. Identification of the Dynamics with perfect Joints</a></li>
<li><a href="#org2aff899">2.2. Effect of the Flexible Joint stiffness on the Dynamics</a></li>
<li><a href="#org40dffdd">2.3. Obtained Damping</a></li>
<li><a href="#org2ae5aaf">2.4. Conclusion</a></li>
</ul>
</li>
<li><a href="#org9425768">3. Direct Velocity Feedback</a>
<ul>
<li><a href="#org61043ac">3.1. Identification of the Dynamics with perfect Joints</a></li>
<li><a href="#org8f71141">3.2. Effect of the Flexible Joint stiffness on the Dynamics</a></li>
<li><a href="#org87c6911">3.3. Obtained Damping</a></li>
<li><a href="#org516fed1">3.4. Conclusion</a></li>
</ul> </ul>
</li> </li>
</ul> </ul>
</div> </div>
</div> </div>
<div id="outline-container-orgbab9da7" class="outline-2"> <p>
<h2 id="orgbab9da7"><span class="section-number-2">1</span> Inertial Control</h2> The following decentralized active damping techniques are briefly studied:
</p>
<ul class="org-ul">
<li>Inertial Control (proportional feedback of the absolute velocity): Section <a href="#org3c68d9e">1</a></li>
<li>Integral Force Feedback: Section <a href="#org62cd19c">2</a></li>
<li>Direct feedback of the relative velocity of each strut: Section <a href="#org587277a">3</a></li>
</ul>
<div id="outline-container-orgfba33d4" class="outline-2">
<h2 id="orgfba33d4"><span class="section-number-2">1</span> Inertial Control</h2>
<div class="outline-text-2" id="text-1"> <div class="outline-text-2" id="text-1">
<p>
<a id="org3c68d9e"></a>
</p>
</div> </div>
<div id="outline-container-orgd9e8d20" class="outline-3">
<h3 id="orgd9e8d20"><span class="section-number-3">1.1</span> Simscape Model</h3> <div id="outline-container-org0ea4bd4" class="outline-3">
<h3 id="org0ea4bd4"><span class="section-number-3">1.1</span> Identification of the Dynamics</h3>
<div class="outline-text-3" id="text-1-1"> <div class="outline-text-3" id="text-1-1">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab">open(<span class="org-string">'simulink/stewart_active_damping.slx'</span>)
</pre>
</div>
</div>
</div>
<div id="outline-container-orge6285ea" class="outline-3">
<h3 id="orge6285ea"><span class="section-number-3">1.2</span> Initialize the Stewart model</h3>
<div class="outline-text-3" id="text-1-2">
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeFramesPositions(<span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3); <pre class="src src-matlab">stewart = initializeFramesPositions(<span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
stewart = generateCubicConfiguration(stewart, <span class="org-string">'Hc'</span>, 40e<span class="org-type">-</span>3, <span class="org-string">'FOc'</span>, 45e<span class="org-type">-</span>3, <span class="org-string">'FHa'</span>, 5e<span class="org-type">-</span>3, <span class="org-string">'MHb'</span>, 5e<span class="org-type">-</span>3); stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart); stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, <span class="org-string">'Ki'</span>, 1e6<span class="org-type">*</span>ones(6,1), <span class="org-string">'Ci'</span>, 1e2<span class="org-type">*</span>ones(6,1)); stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, <span class="org-string">'disable'</span>, <span class="org-constant">true</span>);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart); stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
</pre> </pre>
</div> </div>
</div>
</div>
<div id="outline-container-org471f4bb" class="outline-3">
<h3 id="org471f4bb"><span class="section-number-3">1.3</span> Identification of the Dynamics</h3>
<div class="outline-text-3" id="text-1-3">
<div class="org-src-container"> <div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Options for Linearized</span></span> <pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Options for Linearized</span></span>
options = linearizeOptions; options = linearizeOptions;
@ -313,59 +339,373 @@ mdl = <span class="org-string">'stewart_active_damping'</span>;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%% Input/Output definition</span></span>
clear io; io_i = 1; clear io; io_i = 1;
io(io_i) = linio([mdl, <span class="org-string">'/F'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; io(io_i) = linio([mdl, <span class="org-string">'/F'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Force Inputs [N]</span>
io(io_i) = linio([mdl, <span class="org-string">'/WVB'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; io(io_i) = linio([mdl, <span class="org-string">'/Vm'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute velocity of each leg [m/s]</span>
io(io_i) = linio([mdl, <span class="org-string">'/Dm'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span> <span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
G = linearize(mdl, io, options); G = linearize(mdl, io, 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.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">'Vx'</span>, <span class="org-string">'Vy'</span>, <span class="org-string">'Vz'</span>, <span class="org-string">'Wx'</span>, <span class="org-string">'Wy'</span>, <span class="org-string">'Wz'</span>, ... G.OutputName = {<span class="org-string">'Vm1'</span>, <span class="org-string">'Vm2'</span>, <span class="org-string">'Vm3'</span>, <span class="org-string">'Vm4'</span>, <span class="org-string">'Vm5'</span>, <span class="org-string">'Vm6'</span>};
<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>
</div>
</div>
<div id="outline-container-org30b2e11" class="outline-3">
<h3 id="org30b2e11"><span class="section-number-3">1.4</span> Analysis</h3>
<div class="outline-text-3" id="text-1-4">
<div class="org-src-container">
<pre class="src src-matlab">freqs = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>logspace(1, 4, 1000);
<span class="org-type">figure</span>;
bode(G({<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>}, {<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>}), freqs)
<span class="org-type">figure</span>;
bode(stewart.J<span class="org-type">*</span>G({<span class="org-string">'Vx'</span>, <span class="org-string">'Vy'</span>, <span class="org-string">'Vz'</span>, <span class="org-string">'Wx'</span>, <span class="org-string">'Wy'</span>, <span class="org-string">'Wz'</span>}, {<span class="org-string">'F1'</span>, <span class="org-string">'F2'</span>, <span class="org-string">'F3'</span>, <span class="org-string">'F4'</span>, <span class="org-string">'F5'</span>, <span class="org-string">'F6'</span>}), freqs)
</pre> </pre>
</div> </div>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-type">figure</span>;
bode(G({<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>}, {<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>}), stewart.J<span class="org-type">*</span>G({<span class="org-string">'Vx'</span>, <span class="org-string">'Vy'</span>, <span class="org-string">'Vz'</span>, <span class="org-string">'Wx'</span>, <span class="org-string">'Wy'</span>, <span class="org-string">'Wz'</span>}, {<span class="org-string">'F1'</span>, <span class="org-string">'F2'</span>, <span class="org-string">'F3'</span>, <span class="org-string">'F4'</span>, <span class="org-string">'F5'</span>, <span class="org-string">'F6'</span>}), freqs)
</pre>
</div>
</div>
</div>
<div id="outline-container-org7f07dca" class="outline-3">
<h3 id="org7f07dca"><span class="section-number-3">1.5</span> Conclusion</h3>
<div class="outline-text-3" id="text-1-5">
<p> <p>
It is similar to use: The transfer function from actuator forces to force sensors is shown in Figure <a href="#orgfc5367b">1</a>.
</p> </p>
<ul class="org-ul">
<li>one 6dof inertial sensor and the Jacobian the have the velocity of each lim</li> <div id="orgfc5367b" class="figure">
<li>6 1dof inertial sensor in each top part of the limbs</li> <p><img src="figs/inertial_plant_coupling.png" alt="inertial_plant_coupling.png" />
</ul> </p>
<p><span class="figure-number">Figure 1: </span>Transfer function from the Actuator force \(F_{i}\) to the absolute velocity of the same leg \(v_{m,i}\) and to the absolute velocity of the other legs \(v_{m,j}\) with \(i \neq j\) in grey (<a href="./figs/inertial_plant_coupling.png">png</a>, <a href="./figs/inertial_plant_coupling.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org5a29480" class="outline-3">
<h3 id="org5a29480"><span class="section-number-3">1.2</span> Effect of the Flexible Joint stiffness on the Dynamics</h3>
<div class="outline-text-3" id="text-1-2">
<p>
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.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>};
Gf.OutputName = {<span class="org-string">'Vm1'</span>, <span class="org-string">'Vm2'</span>, <span class="org-string">'Vm3'</span>, <span class="org-string">'Vm4'</span>, <span class="org-string">'Vm5'</span>, <span class="org-string">'Vm6'</span>};
</pre>
</div>
<p>
The new dynamics from force actuator to force sensor is shown in Figure <a href="#org2ee5d65">2</a>.
</p>
<div id="org2ee5d65" class="figure">
<p><img src="figs/inertial_plant_flexible_joint_decentralized.png" alt="inertial_plant_flexible_joint_decentralized.png" />
</p>
<p><span class="figure-number">Figure 2: </span>Transfer function from the Actuator force \(F_{i}\) to the absolute velocity sensor \(v_{m,i}\) (<a href="./figs/inertial_plant_flexible_joint_decentralized.png">png</a>, <a href="./figs/inertial_plant_flexible_joint_decentralized.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orga92be75" class="outline-3">
<h3 id="orga92be75"><span class="section-number-3">1.3</span> Obtained Damping</h3>
<div class="outline-text-3" id="text-1-3">
<p>
The control is a performed in a decentralized manner.
The \(6 \times 6\) control is a diagonal matrix with pure proportional action on the diagonal:
\[ K(s) = g
\begin{bmatrix}
1 & & 0 \\
& \ddots & \\
0 & & 1
\end{bmatrix} \]
</p>
<p>
The root locus is shown in figure <a href="#org78a599c">3</a> and the obtained pole damping function of the control gain is shown in figure <a href="#org0b6bb28">4</a>.
</p>
<div id="org78a599c" class="figure">
<p><img src="figs/root_locus_inertial_rot_stiffness.png" alt="root_locus_inertial_rot_stiffness.png" />
</p>
<p><span class="figure-number">Figure 3: </span>Root Locus plot with Decentralized Inertial Control when considering the stiffness of flexible joints (<a href="./figs/root_locus_inertial_rot_stiffness.png">png</a>, <a href="./figs/root_locus_inertial_rot_stiffness.pdf">pdf</a>)</p>
</div>
<div id="org0b6bb28" class="figure">
<p><img src="figs/pole_damping_gain_inertial_rot_stiffness.png" alt="pole_damping_gain_inertial_rot_stiffness.png" />
</p>
<p><span class="figure-number">Figure 4: </span>Damping of the poles with respect to the gain of the Decentralized Inertial Control when considering the stiffness of flexible joints (<a href="./figs/pole_damping_gain_inertial_rot_stiffness.png">png</a>, <a href="./figs/pole_damping_gain_inertial_rot_stiffness.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orgb29f377" class="outline-3">
<h3 id="orgb29f377"><span class="section-number-3">1.4</span> Conclusion</h3>
<div class="outline-text-3" id="text-1-4">
<div class="important">
<p>
Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors.
</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org5fde56d" class="outline-2">
<h2 id="org5fde56d"><span class="section-number-2">2</span> Integral Force Feedback</h2>
<div class="outline-text-2" id="text-2">
<p>
<a id="org62cd19c"></a>
</p>
</div>
<div id="outline-container-org8823e64" class="outline-3">
<h3 id="org8823e64"><span class="section-number-3">2.1</span> Identification of the Dynamics with perfect Joints</h3>
<div class="outline-text-3" id="text-2-1">
<p>
We first initialize the Stewart platform without joint stiffness.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeFramesPositions(<span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, <span class="org-string">'disable'</span>, <span class="org-constant">true</span>);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
</pre>
</div>
<p>
And we identify the dynamics from force actuators to force sensors.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Options for Linearized</span></span>
options = linearizeOptions;
options.SampleTime = 0;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'stewart_active_damping'</span>;
<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">'/F'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Force Inputs [N]</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 Sensor Outputs [N]</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
G = linearize(mdl, io, 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">'Fm1'</span>, <span class="org-string">'Fm2'</span>, <span class="org-string">'Fm3'</span>, <span class="org-string">'Fm4'</span>, <span class="org-string">'Fm5'</span>, <span class="org-string">'Fm6'</span>};
</pre>
</div>
<p>
The transfer function from actuator forces to force sensors is shown in Figure <a href="#orgae4e327">5</a>.
</p>
<div id="orgae4e327" class="figure">
<p><img src="figs/iff_plant_coupling.png" alt="iff_plant_coupling.png" />
</p>
<p><span class="figure-number">Figure 5: </span>Transfer function from the Actuator force \(F_{i}\) to the Force sensor of the same leg \(F_{m,i}\) and to the force sensor of the other legs \(F_{m,j}\) with \(i \neq j\) in grey (<a href="./figs/iff_plant_coupling.png">png</a>, <a href="./figs/iff_plant_coupling.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org2aff899" class="outline-3">
<h3 id="org2aff899"><span class="section-number-3">2.2</span> Effect of the Flexible Joint stiffness on the Dynamics</h3>
<div class="outline-text-3" id="text-2-2">
<p>
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.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>};
Gf.OutputName = {<span class="org-string">'Fm1'</span>, <span class="org-string">'Fm2'</span>, <span class="org-string">'Fm3'</span>, <span class="org-string">'Fm4'</span>, <span class="org-string">'Fm5'</span>, <span class="org-string">'Fm6'</span>};
</pre>
</div>
<p>
The new dynamics from force actuator to force sensor is shown in Figure <a href="#orgd21a8a8">6</a>.
</p>
<div id="orgd21a8a8" class="figure">
<p><img src="figs/iff_plant_flexible_joint_decentralized.png" alt="iff_plant_flexible_joint_decentralized.png" />
</p>
<p><span class="figure-number">Figure 6: </span>Transfer function from the Actuator force \(F_{i}\) to the force sensor \(F_{m,i}\) (<a href="./figs/iff_plant_flexible_joint_decentralized.png">png</a>, <a href="./figs/iff_plant_flexible_joint_decentralized.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org40dffdd" class="outline-3">
<h3 id="org40dffdd"><span class="section-number-3">2.3</span> Obtained Damping</h3>
<div class="outline-text-3" id="text-2-3">
<p>
The control is a performed in a decentralized manner.
The \(6 \times 6\) control is a diagonal matrix with pure integration action on the diagonal:
\[ K(s) = g
\begin{bmatrix}
\frac{1}{s} & & 0 \\
& \ddots & \\
0 & & \frac{1}{s}
\end{bmatrix} \]
</p>
<p>
The root locus is shown in figure <a href="#org2cdbf69">7</a> and the obtained pole damping function of the control gain is shown in figure <a href="#orge344229">8</a>.
</p>
<div id="org2cdbf69" class="figure">
<p><img src="figs/root_locus_iff_rot_stiffness.png" alt="root_locus_iff_rot_stiffness.png" />
</p>
<p><span class="figure-number">Figure 7: </span>Root Locus plot with Decentralized Integral Force Feedback when considering the stiffness of flexible joints (<a href="./figs/root_locus_iff_rot_stiffness.png">png</a>, <a href="./figs/root_locus_iff_rot_stiffness.pdf">pdf</a>)</p>
</div>
<div id="orge344229" class="figure">
<p><img src="figs/pole_damping_gain_iff_rot_stiffness.png" alt="pole_damping_gain_iff_rot_stiffness.png" />
</p>
<p><span class="figure-number">Figure 8: </span>Damping of the poles with respect to the gain of the Decentralized Integral Force Feedback when considering the stiffness of flexible joints (<a href="./figs/pole_damping_gain_iff_rot_stiffness.png">png</a>, <a href="./figs/pole_damping_gain_iff_rot_stiffness.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org2ae5aaf" class="outline-3">
<h3 id="org2ae5aaf"><span class="section-number-3">2.4</span> Conclusion</h3>
<div class="outline-text-3" id="text-2-4">
<div class="important">
<p>
The joint stiffness has a huge impact on the attainable active damping performance when using force sensors.
Thus, if Integral Force Feedback is to be used in a Stewart platform with flexible joints, the rotational stiffness of the joints should be minimized.
</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org9425768" class="outline-2">
<h2 id="org9425768"><span class="section-number-2">3</span> Direct Velocity Feedback</h2>
<div class="outline-text-2" id="text-3">
<p>
<a id="org587277a"></a>
</p>
</div>
<div id="outline-container-org61043ac" class="outline-3">
<h3 id="org61043ac"><span class="section-number-3">3.1</span> Identification of the Dynamics with perfect Joints</h3>
<div class="outline-text-3" id="text-3-1">
<p>
We first initialize the Stewart platform without joint stiffness.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeFramesPositions(<span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, <span class="org-string">'disable'</span>, <span class="org-constant">true</span>);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
</pre>
</div>
<p>
And we identify the dynamics from force actuators to force sensors.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Options for Linearized</span></span>
options = linearizeOptions;
options.SampleTime = 0;
<span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
mdl = <span class="org-string">'stewart_active_damping'</span>;
<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">'/F'</span>], 1, <span class="org-string">'openinput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Actuator Force Inputs [N]</span>
io(io_i) = linio([mdl, <span class="org-string">'/Dm'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Relative Displacement Outputs [N]</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
G = linearize(mdl, io, 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">'Dm1'</span>, <span class="org-string">'Dm2'</span>, <span class="org-string">'Dm3'</span>, <span class="org-string">'Dm4'</span>, <span class="org-string">'Dm5'</span>, <span class="org-string">'Dm6'</span>};
</pre>
</div>
<p>
The transfer function from actuator forces to relative motion sensors is shown in Figure <a href="#orgd8d51db">9</a>.
</p>
<div id="orgd8d51db" class="figure">
<p><img src="figs/dvf_plant_coupling.png" alt="dvf_plant_coupling.png" />
</p>
<p><span class="figure-number">Figure 9: </span>Transfer function from the Actuator force \(F_{i}\) to the Relative Motion Sensor \(D_{m,j}\) with \(i \neq j\) (<a href="./figs/dvf_plant_coupling.png">png</a>, <a href="./figs/dvf_plant_coupling.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org8f71141" class="outline-3">
<h3 id="org8f71141"><span class="section-number-3">3.2</span> Effect of the Flexible Joint stiffness on the Dynamics</h3>
<div class="outline-text-3" id="text-3-2">
<p>
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.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>};
Gf.OutputName = {<span class="org-string">'Dm1'</span>, <span class="org-string">'Dm2'</span>, <span class="org-string">'Dm3'</span>, <span class="org-string">'Dm4'</span>, <span class="org-string">'Dm5'</span>, <span class="org-string">'Dm6'</span>};
</pre>
</div>
<p>
The new dynamics from force actuator to relative motion sensor is shown in Figure <a href="#orgb18f950">10</a>.
</p>
<div id="orgb18f950" class="figure">
<p><img src="figs/dvf_plant_flexible_joint_decentralized.png" alt="dvf_plant_flexible_joint_decentralized.png" />
</p>
<p><span class="figure-number">Figure 10: </span>Transfer function from the Actuator force \(F_{i}\) to the relative displacement sensor \(D_{m,i}\) (<a href="./figs/dvf_plant_flexible_joint_decentralized.png">png</a>, <a href="./figs/dvf_plant_flexible_joint_decentralized.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org87c6911" class="outline-3">
<h3 id="org87c6911"><span class="section-number-3">3.3</span> Obtained Damping</h3>
<div class="outline-text-3" id="text-3-3">
<p>
The control is a performed in a decentralized manner.
The \(6 \times 6\) control is a diagonal matrix with pure derivative action on the diagonal:
\[ K(s) = g
\begin{bmatrix}
s & & \\
& \ddots & \\
& & s
\end{bmatrix} \]
</p>
<p>
The root locus is shown in figure <a href="#org5cb31c8">11</a> and the obtained pole damping function of the control gain is shown in figure <a href="#org4618492">12</a>.
</p>
<div id="org5cb31c8" class="figure">
<p><img src="figs/root_locus_dvf_rot_stiffness.png" alt="root_locus_dvf_rot_stiffness.png" />
</p>
<p><span class="figure-number">Figure 11: </span>Root Locus plot with Direct Velocity Feedback when considering the Stiffness of flexible joints (<a href="./figs/root_locus_dvf_rot_stiffness.png">png</a>, <a href="./figs/root_locus_dvf_rot_stiffness.pdf">pdf</a>)</p>
</div>
<div id="org4618492" class="figure">
<p><img src="figs/pole_damping_gain_dvf_rot_stiffness.png" alt="pole_damping_gain_dvf_rot_stiffness.png" />
</p>
<p><span class="figure-number">Figure 12: </span>Damping of the poles with respect to the gain of the Direct Velocity Feedback when considering the Stiffness of flexible joints (<a href="./figs/pole_damping_gain_dvf_rot_stiffness.png">png</a>, <a href="./figs/pole_damping_gain_dvf_rot_stiffness.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org516fed1" class="outline-3">
<h3 id="org516fed1"><span class="section-number-3">3.4</span> Conclusion</h3>
<div class="outline-text-3" id="text-3-4">
<div class="important">
<p>
Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors.
</p>
</div>
</div> </div>
</div> </div>
</div> </div>
</div> </div>
<div id="postamble" class="status"> <div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p> <p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2020-01-22 mer. 16:31</p> <p class="date">Created: 2020-02-06 jeu. 15:39</p>
</div> </div>
</body> </body>
</html> </html>

View File

@ -1,4 +1,4 @@
#+TITLE: Stewart Platform - Active Damping #+TITLE: Stewart Platform - Decentralized Active Damping
:DRAWER: :DRAWER:
#+HTML_LINK_HOME: ./index.html #+HTML_LINK_HOME: ./index.html
#+HTML_LINK_UP: ./index.html #+HTML_LINK_UP: ./index.html
@ -20,7 +20,21 @@
#+PROPERTY: header-args:matlab+ :output-dir figs #+PROPERTY: header-args:matlab+ :output-dir figs
:END: :END:
* Introduction :ignore:
The following decentralized active damping techniques are briefly studied:
- Inertial Control (proportional feedback of the absolute velocity): Section [[sec:active_damping_inertial]]
- Integral Force Feedback: Section [[sec:active_damping_iff]]
- Direct feedback of the relative velocity of each strut: Section [[sec:active_damping_dvf]]
* Inertial Control * Inertial Control
:PROPERTIES:
:header-args:matlab+: :tangle matlab/active_damping_inertial.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:active_damping_inertial>>
** Introduction :ignore:
** Matlab Init :noexport:ignore: ** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name) #+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>> <<matlab-dir>>
@ -34,24 +48,23 @@
simulinkproject('./'); simulinkproject('./');
#+end_src #+end_src
** Simscape Model
#+begin_src matlab #+begin_src matlab
open('simulink/stewart_active_damping.slx') open('simulink/stewart_active_damping.slx')
#+end_src #+end_src
** Initialize the Stewart model ** Identification of the Dynamics
#+begin_src matlab #+begin_src matlab
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3); stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = generateCubicConfiguration(stewart, 'Hc', 40e-3, 'FOc', 45e-3, 'FHa', 5e-3, 'MHb', 5e-3); stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart); stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1)); stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeCylindricalPlatforms(stewart); stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart); stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart); stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart); stewart = initializeStewartPose(stewart);
#+end_src #+end_src
** Identification of the Dynamics
#+begin_src matlab #+begin_src matlab
%% Options for Linearized %% Options for Linearized
options = linearizeOptions; options = linearizeOptions;
@ -62,34 +75,644 @@
%% Input/Output definition %% Input/Output definition
clear io; io_i = 1; clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/WVB'], 1, 'openoutput'); io_i = io_i + 1; io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1; % Absolute velocity of each leg [m/s]
io(io_i) = linio([mdl, '/Dm'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization %% Run the linearization
G = linearize(mdl, io, options); G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}; G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Vx', 'Vy', 'Vz', 'Wx', 'Wy', 'Wz', ... G.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
#+end_src #+end_src
** Analysis The transfer function from actuator forces to force sensors is shown in Figure [[fig:inertial_plant_coupling]].
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(G(['Vm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G('Vm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Vm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Vm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/inertial_plant_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:inertial_plant_coupling
#+caption: Transfer function from the Actuator force $F_{i}$ to the absolute velocity of the same leg $v_{m,i}$ and to the absolute velocity of the other legs $v_{m,j}$ with $i \neq j$ in grey ([[./figs/inertial_plant_coupling.png][png]], [[./figs/inertial_plant_coupling.pdf][pdf]])
[[file:figs/inertial_plant_coupling.png]]
** Effect of the Flexible Joint stiffness on the Dynamics
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
#+begin_src matlab #+begin_src matlab
freqs = 2*pi*logspace(1, 4, 1000); stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
figure; Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
bode(G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), freqs) Gf.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
figure;
bode(stewart.J*G({'Vx', 'Vy', 'Vz', 'Wx', 'Wy', 'Wz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), freqs)
#+end_src #+end_src
#+begin_src matlab The new dynamics from force actuator to force sensor is shown in Figure [[fig:inertial_plant_flexible_joint_decentralized]].
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure; figure;
bode(G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), stewart.J*G({'Vx', 'Vy', 'Vz', 'Wx', 'Wy', 'Wz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}), freqs)
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest')
linkaxes([ax1,ax2],'x');
#+end_src #+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/inertial_plant_flexible_joint_decentralized.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:inertial_plant_flexible_joint_decentralized
#+caption: Transfer function from the Actuator force $F_{i}$ to the absolute velocity sensor $v_{m,i}$ ([[./figs/inertial_plant_flexible_joint_decentralized.png][png]], [[./figs/inertial_plant_flexible_joint_decentralized.pdf][pdf]])
[[file:figs/inertial_plant_flexible_joint_decentralized.png]]
** Obtained Damping
The control is a performed in a decentralized manner.
The $6 \times 6$ control is a diagonal matrix with pure proportional action on the diagonal:
\[ K(s) = g
\begin{bmatrix}
1 & & 0 \\
& \ddots & \\
0 & & 1
\end{bmatrix} \]
The root locus is shown in figure [[fig:root_locus_inertial_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_inertial_rot_stiffness]].
#+begin_src matlab :exports none
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, gains(i)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,2000]);
xlim([-2000,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/root_locus_inertial_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:root_locus_inertial_rot_stiffness
#+caption: Root Locus plot with Decentralized Inertial Control when considering the stiffness of flexible joints ([[./figs/root_locus_inertial_rot_stiffness.png][png]], [[./figs/root_locus_inertial_rot_stiffness.pdf][pdf]])
[[file:figs/root_locus_inertial_rot_stiffness.png]]
#+begin_src matlab :exports none
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, gains(i)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/pole_damping_gain_inertial_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:pole_damping_gain_inertial_rot_stiffness
#+caption: Damping of the poles with respect to the gain of the Decentralized Inertial Control when considering the stiffness of flexible joints ([[./figs/pole_damping_gain_inertial_rot_stiffness.png][png]], [[./figs/pole_damping_gain_inertial_rot_stiffness.pdf][pdf]])
[[file:figs/pole_damping_gain_inertial_rot_stiffness.png]]
** Conclusion ** Conclusion
It is similar to use: #+begin_important
- one 6dof inertial sensor and the Jacobian the have the velocity of each lim Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors.
- 6 1dof inertial sensor in each top part of the limbs #+end_important
* Integral Force Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/active_damping_iff.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:active_damping_iff>>
** Introduction :ignore:
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
simulinkproject('./');
#+end_src
#+begin_src matlab
open('simulink/stewart_active_damping.slx')
#+end_src
** Identification of the Dynamics with perfect Joints
We first initialize the Stewart platform without joint stiffness.
#+begin_src matlab
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
#+end_src
And we identify the dynamics from force actuators to force sensors.
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor Outputs [N]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
#+end_src
The transfer function from actuator forces to force sensors is shown in Figure [[fig:iff_plant_coupling]].
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(G(['Fm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G('Fm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Fm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/iff_plant_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:iff_plant_coupling
#+caption: Transfer function from the Actuator force $F_{i}$ to the Force sensor of the same leg $F_{m,i}$ and to the force sensor of the other legs $F_{m,j}$ with $i \neq j$ in grey ([[./figs/iff_plant_coupling.png][png]], [[./figs/iff_plant_coupling.pdf][pdf]])
[[file:figs/iff_plant_coupling.png]]
** Effect of the Flexible Joint stiffness on the Dynamics
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
#+begin_src matlab
stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
#+end_src
The new dynamics from force actuator to force sensor is shown in Figure [[fig:iff_plant_flexible_joint_decentralized]].
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest')
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/iff_plant_flexible_joint_decentralized.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:iff_plant_flexible_joint_decentralized
#+caption: Transfer function from the Actuator force $F_{i}$ to the force sensor $F_{m,i}$ ([[./figs/iff_plant_flexible_joint_decentralized.png][png]], [[./figs/iff_plant_flexible_joint_decentralized.pdf][pdf]])
[[file:figs/iff_plant_flexible_joint_decentralized.png]]
** Obtained Damping
The control is a performed in a decentralized manner.
The $6 \times 6$ control is a diagonal matrix with pure integration action on the diagonal:
\[ K(s) = g
\begin{bmatrix}
\frac{1}{s} & & 0 \\
& \ddots & \\
0 & & \frac{1}{s}
\end{bmatrix} \]
The root locus is shown in figure [[fig:root_locus_iff_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_iff_rot_stiffness]].
#+begin_src matlab :exports none
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,inf]);
xlim([-3000,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/root_locus_iff_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:root_locus_iff_rot_stiffness
#+caption: Root Locus plot with Decentralized Integral Force Feedback when considering the stiffness of flexible joints ([[./figs/root_locus_iff_rot_stiffness.png][png]], [[./figs/root_locus_iff_rot_stiffness.pdf][pdf]])
[[file:figs/root_locus_iff_rot_stiffness.png]]
#+begin_src matlab :exports none
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/pole_damping_gain_iff_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:pole_damping_gain_iff_rot_stiffness
#+caption: Damping of the poles with respect to the gain of the Decentralized Integral Force Feedback when considering the stiffness of flexible joints ([[./figs/pole_damping_gain_iff_rot_stiffness.png][png]], [[./figs/pole_damping_gain_iff_rot_stiffness.pdf][pdf]])
[[file:figs/pole_damping_gain_iff_rot_stiffness.png]]
** Conclusion
#+begin_important
The joint stiffness has a huge impact on the attainable active damping performance when using force sensors.
Thus, if Integral Force Feedback is to be used in a Stewart platform with flexible joints, the rotational stiffness of the joints should be minimized.
#+end_important
* Direct Velocity Feedback
:PROPERTIES:
:header-args:matlab+: :tangle matlab/active_damping_dvf.m
:header-args:matlab+: :comments org :mkdirp yes
:END:
<<sec:active_damping_dvf>>
** Introduction :ignore:
** Matlab Init :noexport:ignore:
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
<<matlab-dir>>
#+end_src
#+begin_src matlab :exports none :results silent :noweb yes
<<matlab-init>>
#+end_src
#+begin_src matlab
simulinkproject('./');
#+end_src
#+begin_src matlab
open('simulink/stewart_active_damping.slx')
#+end_src
** Identification of the Dynamics with perfect Joints
We first initialize the Stewart platform without joint stiffness.
#+begin_src matlab
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
#+end_src
And we identify the dynamics from force actuators to force sensors.
#+begin_src matlab
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Dm'], 1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [N]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
#+end_src
The transfer function from actuator forces to relative motion sensors is shown in Figure [[fig:dvf_plant_coupling]].
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(G(['Dm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G('Dm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Dm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$D_{m,i}/F_i$', '$D_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/dvf_plant_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:dvf_plant_coupling
#+caption: Transfer function from the Actuator force $F_{i}$ to the Relative Motion Sensor $D_{m,j}$ with $i \neq j$ ([[./figs/dvf_plant_coupling.png][png]], [[./figs/dvf_plant_coupling.pdf][pdf]])
[[file:figs/dvf_plant_coupling.png]]
** Effect of the Flexible Joint stiffness on the Dynamics
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
#+begin_src matlab
stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
#+end_src
The new dynamics from force actuator to relative motion sensor is shown in Figure [[fig:dvf_plant_flexible_joint_decentralized]].
#+begin_src matlab :exports none
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/dvf_plant_flexible_joint_decentralized.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:dvf_plant_flexible_joint_decentralized
#+caption: Transfer function from the Actuator force $F_{i}$ to the relative displacement sensor $D_{m,i}$ ([[./figs/dvf_plant_flexible_joint_decentralized.png][png]], [[./figs/dvf_plant_flexible_joint_decentralized.pdf][pdf]])
[[file:figs/dvf_plant_flexible_joint_decentralized.png]]
** Obtained Damping
The control is a performed in a decentralized manner.
The $6 \times 6$ control is a diagonal matrix with pure derivative action on the diagonal:
\[ K(s) = g
\begin{bmatrix}
s & & \\
& \ddots & \\
& & s
\end{bmatrix} \]
The root locus is shown in figure [[fig:root_locus_dvf_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_dvf_rot_stiffness]].
#+begin_src matlab :exports none
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,inf]);
xlim([-3000,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/root_locus_dvf_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:root_locus_dvf_rot_stiffness
#+caption: Root Locus plot with Direct Velocity Feedback when considering the Stiffness of flexible joints ([[./figs/root_locus_dvf_rot_stiffness.png][png]], [[./figs/root_locus_dvf_rot_stiffness.pdf][pdf]])
[[file:figs/root_locus_dvf_rot_stiffness.png]]
#+begin_src matlab :exports none
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/pole_damping_gain_dvf_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:pole_damping_gain_dvf_rot_stiffness
#+caption: Damping of the poles with respect to the gain of the Direct Velocity Feedback when considering the Stiffness of flexible joints ([[./figs/pole_damping_gain_dvf_rot_stiffness.png][png]], [[./figs/pole_damping_gain_dvf_rot_stiffness.pdf][pdf]])
[[file:figs/pole_damping_gain_dvf_rot_stiffness.png]]
** Conclusion
#+begin_important
Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors.
#+end_important

BIN
figs/dvf_plant_coupling.pdf Normal file

Binary file not shown.

BIN
figs/dvf_plant_coupling.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 138 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 102 KiB

BIN
figs/iff_plant_coupling.pdf Normal file

Binary file not shown.

BIN
figs/iff_plant_coupling.png Normal file

Binary file not shown.

After

Width:  |  Height:  |  Size: 137 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 112 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 100 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 89 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 112 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 118 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 143 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 58 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 69 KiB

Binary file not shown.

Binary file not shown.

After

Width:  |  Height:  |  Size: 71 KiB

178
matlab/active_damping_dvf.m Normal file
View File

@ -0,0 +1,178 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
open('simulink/stewart_active_damping.slx')
% Identification of the Dynamics with perfect Joints
% We first initialize the Stewart platform without joint stiffness.
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
% And we identify the dynamics from force actuators to force sensors.
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Dm'], 1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [N]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
% The transfer function from actuator forces to relative motion sensors is shown in Figure [[fig:dvf_plant_coupling]].
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(G(['Dm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G('Dm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Dm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$D_{m,i}/F_i$', '$D_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
% Effect of the Flexible Joint stiffness on the Dynamics
% We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
% The new dynamics from force actuator to relative motion sensor is shown in Figure [[fig:dvf_plant_flexible_joint_decentralized]].
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'northeast');
linkaxes([ax1,ax2],'x');
% Obtained Damping
% The control is a performed in a decentralized manner.
% The $6 \times 6$ control is a diagonal matrix with pure derivative action on the diagonal:
% \[ K(s) = g
% \begin{bmatrix}
% s & & \\
% & \ddots & \\
% & & s
% \end{bmatrix} \]
% The root locus is shown in figure [[fig:root_locus_dvf_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_dvf_rot_stiffness]].
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,inf]);
xlim([-3000,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
% #+name: fig:root_locus_dvf_rot_stiffness
% #+caption: Root Locus plot with Direct Velocity Feedback when considering the Stiffness of flexible joints ([[./figs/root_locus_dvf_rot_stiffness.png][png]], [[./figs/root_locus_dvf_rot_stiffness.pdf][pdf]])
% [[file:figs/root_locus_dvf_rot_stiffness.png]]
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);

178
matlab/active_damping_iff.m Normal file
View File

@ -0,0 +1,178 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
open('simulink/stewart_active_damping.slx')
% Identification of the Dynamics with perfect Joints
% We first initialize the Stewart platform without joint stiffness.
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
% And we identify the dynamics from force actuators to force sensors.
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor Outputs [N]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
% The transfer function from actuator forces to force sensors is shown in Figure [[fig:iff_plant_coupling]].
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(G(['Fm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G('Fm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Fm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
% Effect of the Flexible Joint stiffness on the Dynamics
% We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
% The new dynamics from force actuator to force sensor is shown in Figure [[fig:iff_plant_flexible_joint_decentralized]].
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest')
linkaxes([ax1,ax2],'x');
% Obtained Damping
% The control is a performed in a decentralized manner.
% The $6 \times 6$ control is a diagonal matrix with pure integration action on the diagonal:
% \[ K(s) = g
% \begin{bmatrix}
% \frac{1}{s} & & 0 \\
% & \ddots & \\
% 0 & & \frac{1}{s}
% \end{bmatrix} \]
% The root locus is shown in figure [[fig:root_locus_iff_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_iff_rot_stiffness]].
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,inf]);
xlim([-3000,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
% #+name: fig:root_locus_iff_rot_stiffness
% #+caption: Root Locus plot with Decentralized Integral Force Feedback when considering the stiffness of flexible joints ([[./figs/root_locus_iff_rot_stiffness.png][png]], [[./figs/root_locus_iff_rot_stiffness.pdf][pdf]])
% [[file:figs/root_locus_iff_rot_stiffness.png]]
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);

View File

@ -0,0 +1,173 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
simulinkproject('./');
open('simulink/stewart_active_damping.slx')
% Identification of the Dynamics
stewart = initializeFramesPositions('H', 90e-3, 'MO_B', 45e-3);
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
stewart = initializeJointDynamics(stewart, 'disable', true);
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'stewart_active_damping';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1; % Absolute velocity of each leg [m/s]
%% Run the linearization
G = linearize(mdl, io, options);
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
% The transfer function from actuator forces to force sensors is shown in Figure [[fig:inertial_plant_coupling]].
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(G(['Vm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(G('Vm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
for i = 2:6
set(gca,'ColorOrderIndex',2);
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Vm', num2str(i)], 'F1'), freqs, 'Hz'))));
end
set(gca,'ColorOrderIndex',1);
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Vm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
linkaxes([ax1,ax2],'x');
% Effect of the Flexible Joint stiffness on the Dynamics
% We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
stewart = initializeJointDynamics(stewart);
Gf = linearize(mdl, io, options);
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
% The new dynamics from force actuator to force sensor is shown in Figure [[fig:inertial_plant_flexible_joint_decentralized]].
freqs = logspace(1, 3, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
ylim([-180, 180]);
yticks([-180, -90, 0, 90, 180]);
legend('location', 'southwest')
linkaxes([ax1,ax2],'x');
% Obtained Damping
% The control is a performed in a decentralized manner.
% The $6 \times 6$ control is a diagonal matrix with pure proportional action on the diagonal:
% \[ K(s) = g
% \begin{bmatrix}
% 1 & & 0 \\
% & \ddots & \\
% 0 & & 1
% \end{bmatrix} \]
% The root locus is shown in figure [[fig:root_locus_inertial_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_inertial_rot_stiffness]].
gains = logspace(0, 5, 1000);
figure;
hold on;
plot(real(pole(G)), imag(pole(G)), 'x');
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
set(gca,'ColorOrderIndex',1);
plot(real(tzero(G)), imag(tzero(G)), 'o');
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
for i = 1:length(gains)
cl_poles = pole(feedback(G, gains(i)*eye(6)));
set(gca,'ColorOrderIndex',1);
plot(real(cl_poles), imag(cl_poles), '.');
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
set(gca,'ColorOrderIndex',2);
plot(real(cl_poles), imag(cl_poles), '.');
end
ylim([0,2000]);
xlim([-2000,0]);
xlabel('Real Part')
ylabel('Imaginary Part')
axis square
% #+name: fig:root_locus_inertial_rot_stiffness
% #+caption: Root Locus plot with Decentralized Inertial Control when considering the stiffness of flexible joints ([[./figs/root_locus_inertial_rot_stiffness.png][png]], [[./figs/root_locus_inertial_rot_stiffness.pdf][pdf]])
% [[file:figs/root_locus_inertial_rot_stiffness.png]]
gains = logspace(0, 5, 1000);
figure;
hold on;
for i = 1:length(gains)
set(gca,'ColorOrderIndex',1);
cl_poles = pole(feedback(G, gains(i)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
set(gca,'ColorOrderIndex',2);
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
end
xlabel('Control Gain');
ylabel('Damping of the Poles');
set(gca, 'XScale', 'log');
ylim([0,pi/2]);

Binary file not shown.