stewart-simscape/docs/control-vibration-isolation.html

1217 lines
41 KiB
HTML
Raw Normal View History

2020-01-22 16:31:44 +01:00
<?xml version="1.0" encoding="utf-8"?>
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
<head>
2020-08-05 13:28:14 +02:00
<!-- 2020-08-05 mer. 13:27 -->
2020-01-22 16:31:44 +01:00
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
2020-02-28 17:35:44 +01:00
<title>Stewart Platform - Vibration Isolation</title>
2020-01-22 16:31:44 +01:00
<meta name="generator" content="Org mode" />
<meta name="author" content="Dehaeze Thomas" />
<link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
<link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
<script src="./js/jquery.min.js"></script>
<script src="./js/bootstrap.min.js"></script>
<script src="./js/jquery.stickytableheaders.min.js"></script>
<script src="./js/readtheorg.js"></script>
2020-08-05 13:28:14 +02:00
<script>MathJax = {
tex: {
tags: 'ams',
macros: {bm: ["\\boldsymbol{#1}",1],}
}
2020-02-11 15:27:39 +01:00
};
</script>
2020-08-05 13:28:14 +02:00
<script type="text/javascript" src="https://cdn.jsdelivr.net/npm/mathjax@3/es5/tex-mml-chtml.js"></script>
2020-01-22 16:31:44 +01:00
</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">
2020-02-28 17:35:44 +01:00
<h1 class="title">Stewart Platform - Vibration Isolation</h1>
2020-01-22 16:31:44 +01:00
<div id="table-of-contents">
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
2020-03-13 10:35:21 +01:00
<li><a href="#orgf86b757">1. HAC-LAC (Cascade) Control - Integral Control</a>
2020-01-22 16:31:44 +01:00
<ul>
2020-03-13 10:35:21 +01:00
<li><a href="#org3a9f4d4">1.1. Introduction</a></li>
2020-08-05 13:28:14 +02:00
<li><a href="#org72e0133">1.2. Initialization</a></li>
<li><a href="#orgb6e0288">1.3. Identification</a>
2020-02-28 17:35:44 +01:00
<ul>
2020-08-05 13:28:14 +02:00
<li><a href="#orgd37eab6">1.3.1. HAC - Without LAC</a></li>
<li><a href="#org451f819">1.3.2. HAC - IFF</a></li>
<li><a href="#org36fc937">1.3.3. HAC - DVF</a></li>
2020-02-28 17:35:44 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#org4d7a6d8">1.4. Control Architecture</a></li>
<li><a href="#org3e1b1b7">1.5. 6x6 Plant Comparison</a></li>
2020-08-05 13:28:14 +02:00
<li><a href="#orgd88e4bb">1.6. HAC - DVF</a>
2020-02-28 17:35:44 +01:00
<ul>
2020-08-05 13:28:14 +02:00
<li><a href="#org948a0ef">1.6.1. Plant</a></li>
<li><a href="#org2c7f7fe">1.6.2. Controller Design</a></li>
<li><a href="#orgab6b6ba">1.6.3. Obtained Performance</a></li>
2020-02-28 17:35:44 +01:00
</ul>
</li>
2020-08-05 13:28:14 +02:00
<li><a href="#org72402cd">1.7. HAC - IFF</a>
2020-02-28 17:35:44 +01:00
<ul>
2020-08-05 13:28:14 +02:00
<li><a href="#org6228267">1.7.1. Plant</a></li>
<li><a href="#org46e2ce2">1.7.2. Controller Design</a></li>
<li><a href="#org3e940d0">1.7.3. Obtained Performance</a></li>
2020-01-22 16:31:44 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#org81c1767">1.8. Comparison</a></li>
2020-02-28 17:35:44 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#org6f94eba">2. MIMO Analysis</a>
<ul>
2020-08-05 13:28:14 +02:00
<li><a href="#org389b32d">2.1. Initialization</a></li>
<li><a href="#orgab59403">2.2. Identification</a>
2020-02-28 17:35:44 +01:00
<ul>
2020-08-05 13:28:14 +02:00
<li><a href="#org6a97945">2.2.1. HAC - Without LAC</a></li>
<li><a href="#org2c0fb88">2.2.2. HAC - DVF</a></li>
2020-03-13 10:35:21 +01:00
<li><a href="#orgf606814">2.2.3. Cartesian Frame</a></li>
2020-02-28 17:35:44 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#org8349fa6">2.3. Singular Value Decomposition</a></li>
2020-02-28 17:35:44 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#orgc8479b7">3. Diagonal Control based on the damped plant</a>
2020-02-28 17:35:44 +01:00
<ul>
2020-08-05 13:28:14 +02:00
<li><a href="#org0bf14d6">3.1. Initialization</a></li>
<li><a href="#org1540d5b">3.2. Identification</a></li>
2020-03-13 10:35:21 +01:00
<li><a href="#orgae85e0d">3.3. Steady State Decoupling</a>
2020-02-28 17:35:44 +01:00
<ul>
2020-03-13 10:35:21 +01:00
<li><a href="#org1e2bbe7">3.3.1. Pre-Compensator Design</a></li>
<li><a href="#org077e6f6">3.3.2. Diagonal Control Design</a></li>
2020-08-05 13:28:14 +02:00
<li><a href="#org2ef5df1">3.3.3. Results</a></li>
2020-02-28 17:35:44 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#orgad35bf9">3.4. Decoupling at Crossover</a></li>
2020-02-28 17:35:44 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#org846cef9">4. Time Domain Simulation</a>
2020-02-28 17:35:44 +01:00
<ul>
2020-08-05 13:28:14 +02:00
<li><a href="#org323700d">4.1. Initialization</a></li>
2020-03-13 10:35:21 +01:00
<li><a href="#org8dbc004">4.2. HAC IFF</a></li>
<li><a href="#org7dc4716">4.3. HAC-DVF</a></li>
2020-08-05 13:28:14 +02:00
<li><a href="#org1230d9e">4.4. Results</a></li>
2020-03-11 19:00:22 +01:00
</ul>
</li>
2020-03-13 10:35:21 +01:00
<li><a href="#org69ebad1">5. Functions</a>
2020-03-11 19:00:22 +01:00
<ul>
2020-03-13 10:35:21 +01:00
<li><a href="#orgc7bcc65">5.1. <code>initializeController</code>: Initialize the Controller</a>
<ul>
2020-03-13 10:35:21 +01:00
<li><a href="#orgf672f64">Function description</a></li>
<li><a href="#org941466e">Optional Parameters</a></li>
<li><a href="#org65d3a7d">Structure initialization</a></li>
<li><a href="#org32be93f">Add Type</a></li>
</ul>
</li>
</ul>
</li>
2020-01-22 16:31:44 +01:00
</ul>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-orgf86b757" class="outline-2">
<h2 id="orgf86b757"><span class="section-number-2">1</span> HAC-LAC (Cascade) Control - Integral Control</h2>
2020-02-11 15:50:52 +01:00
<div class="outline-text-2" id="text-1">
2020-01-22 16:31:44 +01:00
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org3a9f4d4" class="outline-3">
<h3 id="org3a9f4d4"><span class="section-number-3">1.1</span> Introduction</h3>
2020-02-11 15:50:52 +01:00
<div class="outline-text-3" id="text-1-1">
2020-02-28 17:35:44 +01:00
<p>
In this section, we wish to study the use of the High Authority Control - Low Authority Control (HAC-LAC) architecture on the Stewart platform.
</p>
<p>
2020-03-13 10:35:21 +01:00
The control architectures are shown in Figures <a href="#org63523c7">1</a> and <a href="#org7ad8618">2</a>.
2020-02-28 17:35:44 +01:00
</p>
2020-01-22 16:31:44 +01:00
2020-02-28 17:35:44 +01:00
<p>
First, the LAC loop is closed (the LAC control is described <a href="active-damping.html">here</a>), and then the HAC controller is designed and the outer loop is closed.
2020-01-22 16:31:44 +01:00
</p>
2020-02-28 17:35:44 +01:00
2020-03-13 10:35:21 +01:00
<div id="org63523c7" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/control_arch_hac_iff.png" alt="control_arch_hac_iff.png" />
</p>
<p><span class="figure-number">Figure 1: </span>HAC-LAC architecture with IFF</p>
</div>
2020-03-13 10:35:21 +01:00
<div id="org7ad8618" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/control_arch_hac_dvf.png" alt="control_arch_hac_dvf.png" />
</p>
<p><span class="figure-number">Figure 2: </span>HAC-LAC architecture with DVF</p>
2020-01-22 16:31:44 +01:00
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org72e0133" class="outline-3">
<h3 id="org72e0133"><span class="section-number-3">1.2</span> Initialization</h3>
2020-02-11 15:50:52 +01:00
<div class="outline-text-3" id="text-1-2">
2020-02-28 17:35:44 +01:00
<p>
We first initialize the Stewart platform.
</p>
2020-01-22 16:31:44 +01:00
<div class="org-src-container">
2020-02-11 15:27:39 +01:00
<pre class="src src-matlab">stewart = initializeStewartPlatform();
2020-08-05 13:28:14 +02:00
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
2020-01-22 16:31:44 +01:00
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
2020-02-11 15:27:39 +01:00
stewart = initializeStrutDynamics(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
2020-02-11 15:27:39 +01:00
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
2020-01-22 16:31:44 +01:00
stewart = computeJacobian(stewart);
2020-02-11 15:27:39 +01:00
stewart = initializeStewartPose(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeInertialSensor(stewart, 'type', 'none');
2020-02-13 14:51:49 +01:00
</pre>
</div>
2020-02-28 17:35:44 +01:00
<p>
The rotation point of the ground is located at the origin of frame \(\{A\}\).
</p>
2020-02-13 14:51:49 +01:00
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
2020-01-22 16:31:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-orgb6e0288" class="outline-3">
<h3 id="orgb6e0288"><span class="section-number-3">1.3</span> Identification</h3>
2020-02-11 15:50:52 +01:00
<div class="outline-text-3" id="text-1-3">
2020-01-22 16:31:44 +01:00
<p>
2020-02-28 17:35:44 +01:00
We identify the transfer function from the actuator forces \(\bm{\tau}\) to the absolute displacement of the mobile platform \(\bm{\mathcal{X}}\) in three different cases:
2020-01-22 16:31:44 +01:00
</p>
2020-02-28 17:35:44 +01:00
<ul class="org-ul">
<li>Open Loop plant</li>
<li>Already damped plant using Integral Force Feedback</li>
<li>Already damped plant using Direct velocity feedback</li>
</ul>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-orgd37eab6" class="outline-4">
<h4 id="orgd37eab6"><span class="section-number-4">1.3.1</span> HAC - Without LAC</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-3-1">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'open-loop');
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-02-28 17:35:44 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-02-28 17:35:44 +01:00
G_ol = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_ol.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_ol.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org451f819" class="outline-4">
<h4 id="org451f819"><span class="section-number-4">1.3.2</span> HAC - IFF</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-3-2">
2020-01-22 16:31:44 +01:00
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'iff');
K_iff = -(1e4/s)*eye(6);
2020-02-28 17:35:44 +01:00
</pre>
</div>
2020-01-22 16:31:44 +01:00
2020-02-28 17:35:44 +01:00
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-01-22 16:31:44 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-01-22 16:31:44 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-01-22 16:31:44 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-02-28 17:35:44 +01:00
G_iff = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_iff.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_iff.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-01-22 16:31:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org36fc937" class="outline-4">
<h4 id="org36fc937"><span class="section-number-4">1.3.3</span> HAC - DVF</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-3-3">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-02-28 17:35:44 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-02-28 17:35:44 +01:00
G_dvf = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org4d7a6d8" class="outline-3">
<h3 id="org4d7a6d8"><span class="section-number-3">1.4</span> Control Architecture</h3>
2020-02-11 15:50:52 +01:00
<div class="outline-text-3" id="text-1-4">
2020-01-22 16:31:44 +01:00
<p>
2020-02-28 17:35:44 +01:00
We use the Jacobian to express the actuator forces in the cartesian frame, and thus we obtain the transfer functions from \(\bm{\mathcal{F}}\) to \(\bm{\mathcal{X}}\).
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">Gc_ol = minreal(G_ol)/stewart.kinematics.J';
Gc_ol.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
Gc_iff = minreal(G_iff)/stewart.kinematics.J';
Gc_iff.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-02-28 17:35:44 +01:00
</pre>
</div>
<p>
We then design a controller based on the transfer functions from \(\bm{\mathcal{F}}\) to \(\bm{\mathcal{X}}\), finally, we will pre-multiply the controller by \(\bm{J}^{-T}\).
2020-01-22 16:31:44 +01:00
</p>
</div>
</div>
2020-02-28 17:35:44 +01:00
2020-03-13 10:35:21 +01:00
<div id="outline-container-org3e1b1b7" class="outline-3">
<h3 id="org3e1b1b7"><span class="section-number-3">1.5</span> 6x6 Plant Comparison</h3>
2020-02-11 15:50:52 +01:00
<div class="outline-text-3" id="text-1-5">
2020-02-28 17:35:44 +01:00
2020-03-13 10:35:21 +01:00
<div id="orgbee4071" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_coupling_jacobian.png" alt="hac_lac_coupling_jacobian.png" />
</p>
<p><span class="figure-number">Figure 3: </span>Norm of the transfer functions from \(\bm{\mathcal{F}}\) to \(\bm{\mathcal{X}}\) (<a href="./figs/hac_lac_coupling_jacobian.png">png</a>, <a href="./figs/hac_lac_coupling_jacobian.pdf">pdf</a>)</p>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-orgd88e4bb" class="outline-3">
<h3 id="orgd88e4bb"><span class="section-number-3">1.6</span> HAC - DVF</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-1-6">
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org948a0ef" class="outline-4">
<h4 id="org948a0ef"><span class="section-number-4">1.6.1</span> Plant</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-6-1">
2020-03-13 10:35:21 +01:00
<div id="org487a558" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_plant_dvf.png" alt="hac_lac_plant_dvf.png" />
</p>
<p><span class="figure-number">Figure 4: </span>Diagonal elements of the plant for HAC control when DVF is previously applied (<a href="./figs/hac_lac_plant_dvf.png">png</a>, <a href="./figs/hac_lac_plant_dvf.pdf">pdf</a>)</p>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org2c7f7fe" class="outline-4">
<h4 id="org2c7f7fe"><span class="section-number-4">1.6.2</span> Controller Design</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-6-2">
2020-01-22 16:31:44 +01:00
<p>
2020-02-28 17:35:44 +01:00
We design a diagonal controller with equal bandwidth for the 6 terms.
The controller is a pure integrator with a small lead near the crossover.
2020-01-22 16:31:44 +01:00
</p>
2020-02-28 17:35:44 +01:00
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">wc = 2*pi*300; % Wanted Bandwidth [rad/s]
2020-02-28 17:35:44 +01:00
h = 1.2;
2020-08-05 13:28:14 +02:00
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
Kd_dvf = diag(1./abs(diag(freqresp(1/s*Gc_dvf, wc)))) .* H_lead .* 1/s;
2020-02-28 17:35:44 +01:00
</pre>
</div>
2020-03-13 10:35:21 +01:00
<div id="org5d0e2e3" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_loop_gain_dvf.png" alt="hac_lac_loop_gain_dvf.png" />
</p>
<p><span class="figure-number">Figure 5: </span>Diagonal elements of the Loop Gain for the HAC control (<a href="./figs/hac_lac_loop_gain_dvf.png">png</a>, <a href="./figs/hac_lac_loop_gain_dvf.pdf">pdf</a>)</p>
</div>
<p>
Finally, we pre-multiply the diagonal controller by \(\bm{J}^{-T}\) prior implementation.
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">K_hac_dvf = inv(stewart.kinematics.J')*Kd_dvf;
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-orgab6b6ba" class="outline-4">
<h4 id="orgab6b6ba"><span class="section-number-4">1.6.3</span> Obtained Performance</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-6-3">
2020-01-22 16:31:44 +01:00
<p>
2020-02-28 17:35:44 +01:00
We identify the transmissibility and compliance of the system.
2020-01-22 16:31:44 +01:00
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'open-loop');
2020-02-28 17:35:44 +01:00
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
2020-08-05 13:28:14 +02:00
[C_ol, C_norm_ol, ~] = computeCompliance();
2020-02-28 17:35:44 +01:00
</pre>
</div>
2020-01-22 16:31:44 +01:00
2020-02-28 17:35:44 +01:00
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'dvf');
[T_dvf, T_norm_dvf, ~] = computeTransmissibility();
[C_dvf, C_norm_dvf, ~] = computeCompliance();
2020-02-28 17:35:44 +01:00
</pre>
</div>
2020-01-22 16:31:44 +01:00
2020-02-28 17:35:44 +01:00
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'hac-dvf');
[T_hac_dvf, T_norm_hac_dvf, ~] = computeTransmissibility();
[C_hac_dvf, C_norm_hac_dvf, ~] = computeCompliance();
2020-01-22 16:31:44 +01:00
</pre>
</div>
2020-02-28 17:35:44 +01:00
2020-03-13 10:35:21 +01:00
<div id="orga06a318" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_C_T_dvf.png" alt="hac_lac_C_T_dvf.png" />
</p>
<p><span class="figure-number">Figure 6: </span>Obtained Compliance and Transmissibility (<a href="./figs/hac_lac_C_T_dvf.png">png</a>, <a href="./figs/hac_lac_C_T_dvf.pdf">pdf</a>)</p>
</div>
2020-01-22 16:31:44 +01:00
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org72402cd" class="outline-3">
<h3 id="org72402cd"><span class="section-number-3">1.7</span> HAC - IFF</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-1-7">
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org6228267" class="outline-4">
<h4 id="org6228267"><span class="section-number-4">1.7.1</span> Plant</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-7-1">
2020-03-13 10:35:21 +01:00
<div id="org0fc8dea" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_plant_iff.png" alt="hac_lac_plant_iff.png" />
</p>
<p><span class="figure-number">Figure 7: </span>Diagonal elements of the plant for HAC control when IFF is previously applied (<a href="./figs/hac_lac_plant_iff.png">png</a>, <a href="./figs/hac_lac_plant_iff.pdf">pdf</a>)</p>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org46e2ce2" class="outline-4">
<h4 id="org46e2ce2"><span class="section-number-4">1.7.2</span> Controller Design</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-7-2">
<p>
We design a diagonal controller with equal bandwidth for the 6 terms.
The controller is a pure integrator with a small lead near the crossover.
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">wc = 2*pi*300; % Wanted Bandwidth [rad/s]
2020-02-28 17:35:44 +01:00
h = 1.2;
2020-08-05 13:28:14 +02:00
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
Kd_iff = diag(1./abs(diag(freqresp(1/s*Gc_iff, wc)))) .* H_lead .* 1/s;
2020-02-28 17:35:44 +01:00
</pre>
</div>
2020-03-13 10:35:21 +01:00
<div id="org211b595" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_loop_gain_iff.png" alt="hac_lac_loop_gain_iff.png" />
</p>
<p><span class="figure-number">Figure 8: </span>Diagonal elements of the Loop Gain for the HAC control (<a href="./figs/hac_lac_loop_gain_iff.png">png</a>, <a href="./figs/hac_lac_loop_gain_iff.pdf">pdf</a>)</p>
</div>
<p>
Finally, we pre-multiply the diagonal controller by \(\bm{J}^{-T}\) prior implementation.
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">K_hac_iff = inv(stewart.kinematics.J')*Kd_iff;
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org3e940d0" class="outline-4">
<h4 id="org3e940d0"><span class="section-number-4">1.7.3</span> Obtained Performance</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-1-7-3">
<p>
We identify the transmissibility and compliance of the system.
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'open-loop');
2020-02-28 17:35:44 +01:00
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
2020-08-05 13:28:14 +02:00
[C_ol, C_norm_ol, ~] = computeCompliance();
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'iff');
[T_iff, T_norm_iff, ~] = computeTransmissibility();
[C_iff, C_norm_iff, ~] = computeCompliance();
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'hac-iff');
[T_hac_iff, T_norm_hac_iff, ~] = computeTransmissibility();
[C_hac_iff, C_norm_hac_iff, ~] = computeCompliance();
2020-02-28 17:35:44 +01:00
</pre>
</div>
2020-03-13 10:35:21 +01:00
<div id="orgc47bfe0" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_C_T_iff.png" alt="hac_lac_C_T_iff.png" />
</p>
<p><span class="figure-number">Figure 9: </span>Obtained Compliance and Transmissibility (<a href="./figs/hac_lac_C_T_iff.png">png</a>, <a href="./figs/hac_lac_C_T_iff.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org81c1767" class="outline-3">
<h3 id="org81c1767"><span class="section-number-3">1.8</span> Comparison</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-1-8">
2020-03-13 10:35:21 +01:00
<div id="orgf042a3f" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_C_full_comparison.png" alt="hac_lac_C_full_comparison.png" />
</p>
<p><span class="figure-number">Figure 10: </span>Comparison of the norm of the Compliance matrices for the HAC-LAC architecture (<a href="./figs/hac_lac_C_full_comparison.png">png</a>, <a href="./figs/hac_lac_C_full_comparison.pdf">pdf</a>)</p>
</div>
2020-03-13 10:35:21 +01:00
<div id="org5a9df7e" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_T_full_comparison.png" alt="hac_lac_T_full_comparison.png" />
</p>
<p><span class="figure-number">Figure 11: </span>Comparison of the norm of the Transmissibility matrices for the HAC-LAC architecture (<a href="./figs/hac_lac_T_full_comparison.png">png</a>, <a href="./figs/hac_lac_T_full_comparison.pdf">pdf</a>)</p>
</div>
2020-03-13 10:35:21 +01:00
<div id="orgdc12a99" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/hac_lac_C_T_comparison.png" alt="hac_lac_C_T_comparison.png" />
</p>
<p><span class="figure-number">Figure 12: </span>Comparison of the Frobenius norm of the Compliance and Transmissibility for the HAC-LAC architecture with both IFF and DVF (<a href="./figs/hac_lac_C_T_comparison.png">png</a>, <a href="./figs/hac_lac_C_T_comparison.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org6f94eba" class="outline-2">
<h2 id="org6f94eba"><span class="section-number-2">2</span> MIMO Analysis</h2>
<div class="outline-text-2" id="text-2">
2020-02-28 17:35:44 +01:00
<p>
2020-03-13 10:35:21 +01:00
Let&rsquo;s define the system as shown in figure <a href="#orgac8f77c">13</a>.
2020-02-28 17:35:44 +01:00
</p>
2020-03-13 10:35:21 +01:00
<div id="orgac8f77c" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/general_control_names.png" alt="general_control_names.png" />
</p>
<p><span class="figure-number">Figure 13: </span>General Control Architecture</p>
</div>
2020-02-28 17:35:44 +01:00
2020-03-13 10:35:21 +01:00
<table id="orgc2ee4b0" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
2020-02-28 17:35:44 +01:00
<caption class="t-above"><span class="table-number">Table 1:</span> Signals definition for the generalized plant</caption>
<colgroup>
<col class="org-left" />
<col class="org-left" />
<col class="org-left" />
</colgroup>
<thead>
<tr>
2020-03-11 19:00:22 +01:00
<th scope="col" class="org-left">&#xa0;</th>
<th scope="col" class="org-left"><b>Symbol</b></th>
<th scope="col" class="org-left"><b>Meaning</b></th>
</tr>
</thead>
<tbody>
<tr>
<td class="org-left"><b>Exogenous Inputs</b></td>
<td class="org-left">\(\bm{\mathcal{X}}_w\)</td>
<td class="org-left">Ground motion</td>
2020-02-28 17:35:44 +01:00
</tr>
<tr>
2020-03-11 19:00:22 +01:00
<td class="org-left">&#xa0;</td>
<td class="org-left">\(\bm{\mathcal{F}}_d\)</td>
<td class="org-left">External Forces applied to the Payload</td>
2020-02-28 17:35:44 +01:00
</tr>
<tr>
2020-03-11 19:00:22 +01:00
<td class="org-left">&#xa0;</td>
<td class="org-left">\(\bm{r}\)</td>
<td class="org-left">Reference signal for tracking</td>
2020-02-28 17:35:44 +01:00
</tr>
2020-03-11 19:00:22 +01:00
</tbody>
2020-02-28 17:35:44 +01:00
<tbody>
<tr>
<td class="org-left"><b>Exogenous Outputs</b></td>
<td class="org-left">\(\bm{\mathcal{X}}\)</td>
<td class="org-left">Absolute Motion of the Payload</td>
</tr>
<tr>
<td class="org-left">&#xa0;</td>
<td class="org-left">\(\bm{\tau}\)</td>
<td class="org-left">Actuator Rate</td>
</tr>
</tbody>
<tbody>
<tr>
<td class="org-left"><b>Sensed Outputs</b></td>
<td class="org-left">\(\bm{\tau}_m\)</td>
<td class="org-left">Force Sensors in each leg</td>
</tr>
<tr>
<td class="org-left">&#xa0;</td>
<td class="org-left">\(\delta \bm{\mathcal{L}}_m\)</td>
<td class="org-left">Measured displacement of each leg</td>
</tr>
<tr>
<td class="org-left">&#xa0;</td>
<td class="org-left">\(\bm{\mathcal{X}}\)</td>
<td class="org-left">Absolute Motion of the Payload</td>
</tr>
</tbody>
<tbody>
<tr>
<td class="org-left"><b>Control Signals</b></td>
<td class="org-left">\(\bm{\tau}\)</td>
<td class="org-left">Actuator Inputs</td>
</tr>
</tbody>
</table>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org389b32d" class="outline-3">
<h3 id="org389b32d"><span class="section-number-3">2.1</span> Initialization</h3>
<div class="outline-text-3" id="text-2-1">
<p>
2020-02-28 17:35:44 +01:00
We first initialize the Stewart platform.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeStewartPlatform();
2020-08-05 13:28:14 +02:00
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
2020-02-28 17:35:44 +01:00
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
2020-02-28 17:35:44 +01:00
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeInertialSensor(stewart, 'type', 'none');
2020-02-28 17:35:44 +01:00
</pre>
</div>
<p>
The rotation point of the ground is located at the origin of frame \(\{A\}\).
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-orgab59403" class="outline-3">
<h3 id="orgab59403"><span class="section-number-3">2.2</span> Identification</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-2-2">
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org6a97945" class="outline-4">
<h4 id="org6a97945"><span class="section-number-4">2.2.1</span> HAC - Without LAC</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-2-2-1">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'open-loop');
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-02-28 17:35:44 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-02-28 17:35:44 +01:00
G_ol = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_ol.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_ol.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org2c0fb88" class="outline-4">
<h4 id="org2c0fb88"><span class="section-number-4">2.2.2</span> HAC - DVF</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-2-2-2">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-02-28 17:35:44 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-02-28 17:35:44 +01:00
G_dvf = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-orgf606814" class="outline-4">
<h4 id="orgf606814"><span class="section-number-4">2.2.3</span> Cartesian Frame</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-2-2-3">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">Gc_ol = minreal(G_ol)/stewart.kinematics.J';
Gc_ol.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org8349fa6" class="outline-3">
<h3 id="org8349fa6"><span class="section-number-3">2.3</span> Singular Value Decomposition</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-2-3">
<div class="org-src-container">
<pre class="src src-matlab">freqs = logspace(1, 4, 1000);
U_ol = zeros(6,6,length(freqs));
S_ol = zeros(6,length(freqs));
V_ol = zeros(6,6,length(freqs));
U_dvf = zeros(6,6,length(freqs));
S_dvf = zeros(6,length(freqs));
V_dvf = zeros(6,6,length(freqs));
2020-08-05 13:28:14 +02:00
for i = 1:length(freqs)
[U,S,V] = svd(freqresp(Gc_ol, freqs(i), 'Hz'));
U_ol(:,:,i) = U;
S_ol(:,i) = diag(S);
V_ol(:,:,i) = V;
[U,S,V] = svd(freqresp(Gc_dvf, freqs(i), 'Hz'));
U_dvf(:,:,i) = U;
S_dvf(:,i) = diag(S);
V_dvf(:,:,i) = V;
end
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-orgc8479b7" class="outline-2">
<h2 id="orgc8479b7"><span class="section-number-2">3</span> Diagonal Control based on the damped plant</h2>
2020-02-28 17:35:44 +01:00
<div class="outline-text-2" id="text-3">
<p>
2020-08-05 13:28:14 +02:00
From (<a href="#citeproc_bib_item_1">Skogestad and Postlethwaite 2007</a>), a simple approach to multivariable control is the following two-step procedure:
2020-02-28 17:35:44 +01:00
</p>
<ol class="org-ol">
<li><b>Design a pre-compensator</b> \(W_1\), which counteracts the interactions in the plant and results in a new <b>shaped plant</b> \(G_S(s) = G(s) W_1(s)\) which is <b>more diagonal and easier to control</b> than the original plant \(G(s)\).</li>
<li><b>Design a diagonal controller</b> \(K_S(s)\) for the shaped plant using methods similar to those for SISO systems.</li>
</ol>
<p>
The overall controller is then:
\[ K(s) = W_1(s)K_s(s) \]
</p>
<p>
There are mainly three different cases:
</p>
<ol class="org-ol">
<li><b>Dynamic decoupling</b>: \(G_S(s)\) is diagonal at all frequencies. For that we can choose \(W_1(s) = G^{-1}(s)\) and this is an inverse-based controller.</li>
<li><b>Steady-state decoupling</b>: \(G_S(0)\) is diagonal. This can be obtained by selecting \(W_1(s) = G^{-1}(0)\).</li>
<li><b>Approximate decoupling at frequency \(\w_0\)</b>: \(G_S(j\w_0)\) is as diagonal as possible. Decoupling the system at \(\w_0\) is a good choice because the effect on performance of reducing interaction is normally greatest at this frequency.</li>
</ol>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org0bf14d6" class="outline-3">
<h3 id="org0bf14d6"><span class="section-number-3">3.1</span> Initialization</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-3-1">
<p>
We first initialize the Stewart platform.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeStewartPlatform();
2020-08-05 13:28:14 +02:00
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
2020-02-28 17:35:44 +01:00
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
2020-02-28 17:35:44 +01:00
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeInertialSensor(stewart, 'type', 'none');
2020-02-28 17:35:44 +01:00
</pre>
</div>
<p>
The rotation point of the ground is located at the origin of frame \(\{A\}\).
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org1540d5b" class="outline-3">
<h3 id="org1540d5b"><span class="section-number-3">3.2</span> Identification</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-3-2">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-02-28 17:35:44 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-02-28 17:35:44 +01:00
G_dvf = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-02-28 17:35:44 +01:00
</pre>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-orgae85e0d" class="outline-3">
<h3 id="orgae85e0d"><span class="section-number-3">3.3</span> Steady State Decoupling</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-3-3">
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org1e2bbe7" class="outline-4">
<h4 id="org1e2bbe7"><span class="section-number-4">3.3.1</span> Pre-Compensator Design</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-3-3-1">
<p>
We choose \(W_1 = G^{-1}(0)\).
</p>
<div class="org-src-container">
<pre class="src src-matlab">W1 = inv(freqresp(G_dvf, 0));
</pre>
</div>
<p>
The (static) decoupled plant is \(G_s(s) = G(s) W_1\).
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">Gs = G_dvf*W1;
2020-02-28 17:35:44 +01:00
</pre>
</div>
<p>
In the case of the Stewart platform, the pre-compensator for static decoupling is equal to \(\mathcal{K} \bm{J}\):
</p>
\begin{align*}
W_1 &= \left( \frac{\bm{\mathcal{X}}}{\bm{\tau}}(s=0) \right)^{-1}\\
&= \left( \frac{\bm{\mathcal{X}}}{\bm{\tau}}(s=0) \bm{J}^T \right)^{-1}\\
&= \left( \bm{C} \bm{J}^T \right)^{-1}\\
&= \left( \bm{J}^{-1} \mathcal{K}^{-1} \right)^{-1}\\
&= \mathcal{K} \bm{J}
\end{align*}
<p>
2020-03-13 10:35:21 +01:00
The static decoupled plant is schematic shown in Figure <a href="#org76617c6">14</a> and the bode plots of its diagonal elements are shown in Figure <a href="#org96093e0">15</a>.
2020-02-28 17:35:44 +01:00
</p>
2020-03-13 10:35:21 +01:00
<div id="org76617c6" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/control_arch_static_decoupling.png" alt="control_arch_static_decoupling.png" />
</p>
<p><span class="figure-number">Figure 14: </span>Static Decoupling of the Stewart platform</p>
</div>
2020-03-13 10:35:21 +01:00
<div id="org96093e0" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/static_decoupling_diagonal_plant.png" alt="static_decoupling_diagonal_plant.png" />
</p>
<p><span class="figure-number">Figure 15: </span>Bode plot of the diagonal elements of \(G_s(s)\) (<a href="./figs/static_decoupling_diagonal_plant.png">png</a>, <a href="./figs/static_decoupling_diagonal_plant.pdf">pdf</a>)</p>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org077e6f6" class="outline-4">
<h4 id="org077e6f6"><span class="section-number-4">3.3.2</span> Diagonal Control Design</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-3-3-2">
<p>
We design a diagonal controller \(K_s(s)\) that consist of a pure integrator and a lead around the crossover.
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">wc = 2*pi*300; % Wanted Bandwidth [rad/s]
2020-02-28 17:35:44 +01:00
h = 1.5;
2020-08-05 13:28:14 +02:00
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
2020-02-28 17:35:44 +01:00
2020-08-05 13:28:14 +02:00
Ks_dvf = diag(1./abs(diag(freqresp(1/s*Gs, wc)))) .* H_lead .* 1/s;
2020-02-28 17:35:44 +01:00
</pre>
</div>
<p>
2020-03-13 10:35:21 +01:00
The overall controller is then \(K(s) = W_1 K_s(s)\) as shown in Figure <a href="#org4d1ce48">16</a>.
2020-02-28 17:35:44 +01:00
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">K_hac_dvf = W1 * Ks_dvf;
2020-02-28 17:35:44 +01:00
</pre>
</div>
2020-03-13 10:35:21 +01:00
<div id="org4d1ce48" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/control_arch_static_decoupling_K.png" alt="control_arch_static_decoupling_K.png" />
</p>
<p><span class="figure-number">Figure 16: </span>Controller including the static decoupling matrix</p>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org2ef5df1" class="outline-4">
<h4 id="org2ef5df1"><span class="section-number-4">3.3.3</span> Results</h4>
2020-02-28 17:35:44 +01:00
<div class="outline-text-4" id="text-3-3-3">
<p>
We identify the transmissibility and compliance of the Stewart platform under open-loop and closed-loop control.
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'open-loop');
2020-02-28 17:35:44 +01:00
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
2020-08-05 13:28:14 +02:00
[C_ol, C_norm_ol, ~] = computeCompliance();
2020-02-28 17:35:44 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'hac-dvf');
[T_hac_dvf, T_norm_hac_dvf, ~] = computeTransmissibility();
[C_hac_dvf, C_norm_hac_dvf, ~] = computeCompliance();
2020-02-28 17:35:44 +01:00
</pre>
</div>
<p>
The results are shown in figure
</p>
2020-03-13 10:35:21 +01:00
<div id="orge29798b" class="figure">
2020-02-28 17:35:44 +01:00
<p><img src="figs/static_decoupling_C_T_frobenius_norm.png" alt="static_decoupling_C_T_frobenius_norm.png" />
</p>
<p><span class="figure-number">Figure 17: </span>Frobenius norm of the Compliance and transmissibility matrices (<a href="./figs/static_decoupling_C_T_frobenius_norm.png">png</a>, <a href="./figs/static_decoupling_C_T_frobenius_norm.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-orgad35bf9" class="outline-3">
<h3 id="orgad35bf9"><span class="section-number-3">3.4</span> Decoupling at Crossover</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-3-4">
<ul class="org-ul">
<li class="off"><code>[&#xa0;]</code> Find a method for real approximation of a complex matrix</li>
</ul>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org846cef9" class="outline-2">
<h2 id="org846cef9"><span class="section-number-2">4</span> Time Domain Simulation</h2>
2020-02-28 17:35:44 +01:00
<div class="outline-text-2" id="text-4">
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org323700d" class="outline-3">
<h3 id="org323700d"><span class="section-number-3">4.1</span> Initialization</h3>
2020-02-28 17:35:44 +01:00
<div class="outline-text-3" id="text-4-1">
<p>
2020-03-11 19:00:22 +01:00
We first initialize the Stewart platform.
</p>
<div class="org-src-container">
<pre class="src src-matlab">stewart = initializeStewartPlatform();
2020-08-05 13:28:14 +02:00
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
2020-03-11 19:00:22 +01:00
stewart = generateGeneralConfiguration(stewart);
stewart = computeJointsPose(stewart);
stewart = initializeStrutDynamics(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeJointDynamics(stewart, 'type_F', 'universal', 'type_M', 'spherical');
2020-03-11 19:00:22 +01:00
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
2020-08-05 13:28:14 +02:00
stewart = initializeInertialSensor(stewart, 'type', 'none');
2020-03-11 19:00:22 +01:00
</pre>
</div>
<p>
The rotation point of the ground is located at the origin of frame \(\{A\}\).
</p>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
2020-03-11 19:00:22 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">load('./mat/motion_error_ol.mat', 'Eg')
2020-03-11 19:00:22 +01:00
</pre>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org8dbc004" class="outline-3">
<h3 id="org8dbc004"><span class="section-number-3">4.2</span> HAC IFF</h3>
2020-03-11 19:00:22 +01:00
<div class="outline-text-3" id="text-4-2">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'iff');
K_iff = -(1e4/s)*eye(6);
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-03-11 19:00:22 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-03-11 19:00:22 +01:00
G_iff = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_iff.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_iff.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
Gc_iff = minreal(G_iff)/stewart.kinematics.J';
Gc_iff.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-03-11 19:00:22 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">wc = 2*pi*100; % Wanted Bandwidth [rad/s]
2020-03-11 19:00:22 +01:00
h = 1.2;
2020-08-05 13:28:14 +02:00
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
Kd_iff = diag(1./abs(diag(freqresp(1/s*Gc_iff, wc)))) .* H_lead .* 1/s;
K_hac_iff = inv(stewart.kinematics.J')*Kd_iff;
2020-03-11 19:00:22 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'hac-iff');
2020-03-11 19:00:22 +01:00
</pre>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org7dc4716" class="outline-3">
<h3 id="org7dc4716"><span class="section-number-3">4.3</span> HAC-DVF</h3>
2020-03-11 19:00:22 +01:00
<div class="outline-text-3" id="text-4-3">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
%% Name of the Simulink File
mdl = 'stewart_platform_model';
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
%% Input/Output definition
2020-03-11 19:00:22 +01:00
clear io; io_i = 1;
2020-08-05 13:28:14 +02:00
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
%% Run the linearization
2020-03-11 19:00:22 +01:00
G_dvf = linearize(mdl, io);
2020-08-05 13:28:14 +02:00
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
2020-03-11 19:00:22 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">wc = 2*pi*100; % Wanted Bandwidth [rad/s]
2020-03-11 19:00:22 +01:00
h = 1.2;
2020-08-05 13:28:14 +02:00
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
Kd_dvf = diag(1./abs(diag(freqresp(1/s*Gc_dvf, wc)))) .* H_lead .* 1/s;
2020-03-11 19:00:22 +01:00
2020-08-05 13:28:14 +02:00
K_hac_dvf = inv(stewart.kinematics.J')*Kd_dvf;
2020-03-11 19:00:22 +01:00
</pre>
</div>
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">controller = initializeController('type', 'hac-dvf');
2020-03-11 19:00:22 +01:00
</pre>
</div>
</div>
</div>
2020-08-05 13:28:14 +02:00
<div id="outline-container-org1230d9e" class="outline-3">
<h3 id="org1230d9e"><span class="section-number-3">4.4</span> Results</h3>
2020-03-11 19:00:22 +01:00
<div class="outline-text-3" id="text-4-4">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">figure;
2020-03-11 19:00:22 +01:00
subplot(1, 2, 1);
hold on;
2020-08-05 13:28:14 +02:00
plot(Eg.Time, Eg.Data(:, 1), 'DisplayName', 'X');
plot(Eg.Time, Eg.Data(:, 2), 'DisplayName', 'Y');
plot(Eg.Time, Eg.Data(:, 3), 'DisplayName', 'Z');
2020-03-11 19:00:22 +01:00
hold off;
2020-08-05 13:28:14 +02:00
xlabel('Time [s]');
ylabel('Position error [m]');
2020-03-11 19:00:22 +01:00
legend();
subplot(1, 2, 2);
hold on;
2020-08-05 13:28:14 +02:00
plot(simout.Xa.Time, simout.Xa.Data(:, 1));
plot(simout.Xa.Time, simout.Xa.Data(:, 2));
plot(simout.Xa.Time, simout.Xa.Data(:, 3));
2020-03-11 19:00:22 +01:00
hold off;
2020-08-05 13:28:14 +02:00
xlabel('Time [s]');
ylabel('Orientation error [rad]');
2020-03-11 19:00:22 +01:00
</pre>
</div>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org69ebad1" class="outline-2">
<h2 id="org69ebad1"><span class="section-number-2">5</span> Functions</h2>
2020-03-11 19:00:22 +01:00
<div class="outline-text-2" id="text-5">
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-orgc7bcc65" class="outline-3">
<h3 id="orgc7bcc65"><span class="section-number-3">5.1</span> <code>initializeController</code>: Initialize the Controller</h3>
2020-03-11 19:00:22 +01:00
<div class="outline-text-3" id="text-5-1">
<p>
2020-03-13 10:35:21 +01:00
<a id="org33a5401"></a>
</p>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-orgf672f64" class="outline-4">
<h4 id="orgf672f64">Function description</h4>
<div class="outline-text-4" id="text-orgf672f64">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">function [controller] = initializeController(args)
% initializeController - Initialize the Controller
%
% Syntax: [] = initializeController(args)
%
% Inputs:
% - args - Can have the following fields:
</pre>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org941466e" class="outline-4">
<h4 id="org941466e">Optional Parameters</h4>
<div class="outline-text-4" id="text-org941466e">
<div class="org-src-container">
<pre class="src src-matlab">arguments
2020-08-05 13:28:14 +02:00
args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X', 'ref-track-hac-dvf'})} = 'open-loop'
end
</pre>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org65d3a7d" class="outline-4">
<h4 id="org65d3a7d">Structure initialization</h4>
<div class="outline-text-4" id="text-org65d3a7d">
<div class="org-src-container">
<pre class="src src-matlab">controller = struct();
</pre>
</div>
</div>
</div>
2020-03-13 10:35:21 +01:00
<div id="outline-container-org32be93f" class="outline-4">
<h4 id="org32be93f">Add Type</h4>
<div class="outline-text-4" id="text-org32be93f">
<div class="org-src-container">
2020-08-05 13:28:14 +02:00
<pre class="src src-matlab">switch args.type
case 'open-loop'
controller.type = 0;
2020-08-05 13:28:14 +02:00
case 'iff'
controller.type = 1;
2020-08-05 13:28:14 +02:00
case 'dvf'
controller.type = 2;
2020-08-05 13:28:14 +02:00
case 'hac-iff'
2020-02-28 17:35:44 +01:00
controller.type = 3;
2020-08-05 13:28:14 +02:00
case 'hac-dvf'
2020-02-28 17:35:44 +01:00
controller.type = 4;
2020-08-05 13:28:14 +02:00
case 'ref-track-L'
2020-03-11 19:00:22 +01:00
controller.type = 5;
2020-08-05 13:28:14 +02:00
case 'ref-track-X'
2020-03-11 19:00:22 +01:00
controller.type = 6;
2020-08-05 13:28:14 +02:00
case 'ref-track-hac-dvf'
2020-03-12 18:06:56 +01:00
controller.type = 7;
2020-08-05 13:28:14 +02:00
end
</pre>
</div>
2020-08-05 13:28:14 +02:00
<style>.csl-entry{text-indent: -1.5em; margin-left: 1.5em;}</style><h2 class='citeproc-org-bib-h2'>Bibliography</h2>
<div class="csl-bib-body">
<div class="csl-entry"><a name="citeproc_bib_item_1"></a>Skogestad, Sigurd, and Ian Postlethwaite. 2007. <i>Multivariable Feedback Control: Analysis and Design</i>. John Wiley.</div>
</div>
</div>
</div>
</div>
</div>
2020-01-22 16:31:44 +01:00
</div>
<div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p>
2020-08-05 13:28:14 +02:00
<p class="date">Created: 2020-08-05 mer. 13:27</p>
2020-01-22 16:31:44 +01:00
</div>
</body>
</html>