1424 lines
68 KiB
HTML
1424 lines
68 KiB
HTML
<?xml version="1.0" encoding="utf-8"?>
|
|
<?xml version="1.0" encoding="utf-8"?>
|
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
|
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
|
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
|
<head>
|
|
<!-- 2020-03-16 lun. 11:21 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<meta name="viewport" content="width=device-width, initial-scale=1" />
|
|
<title>Stewart Platform - Vibration Isolation</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<style type="text/css">
|
|
<!--/*--><![CDATA[/*><!--*/
|
|
.title { text-align: center;
|
|
margin-bottom: .2em; }
|
|
.subtitle { text-align: center;
|
|
font-size: medium;
|
|
font-weight: bold;
|
|
margin-top:0; }
|
|
.todo { font-family: monospace; color: red; }
|
|
.done { font-family: monospace; color: green; }
|
|
.priority { font-family: monospace; color: orange; }
|
|
.tag { background-color: #eee; font-family: monospace;
|
|
padding: 2px; font-size: 80%; font-weight: normal; }
|
|
.timestamp { color: #bebebe; }
|
|
.timestamp-kwd { color: #5f9ea0; }
|
|
.org-right { margin-left: auto; margin-right: 0px; text-align: right; }
|
|
.org-left { margin-left: 0px; margin-right: auto; text-align: left; }
|
|
.org-center { margin-left: auto; margin-right: auto; text-align: center; }
|
|
.underline { text-decoration: underline; }
|
|
#postamble p, #preamble p { font-size: 90%; margin: .2em; }
|
|
p.verse { margin-left: 3%; }
|
|
pre {
|
|
border: 1px solid #ccc;
|
|
box-shadow: 3px 3px 3px #eee;
|
|
padding: 8pt;
|
|
font-family: monospace;
|
|
overflow: auto;
|
|
margin: 1.2em;
|
|
}
|
|
pre.src {
|
|
position: relative;
|
|
overflow: visible;
|
|
padding-top: 1.2em;
|
|
}
|
|
pre.src:before {
|
|
display: none;
|
|
position: absolute;
|
|
background-color: white;
|
|
top: -10px;
|
|
right: 10px;
|
|
padding: 3px;
|
|
border: 1px solid black;
|
|
}
|
|
pre.src:hover:before { display: inline;}
|
|
/* Languages per Org manual */
|
|
pre.src-asymptote:before { content: 'Asymptote'; }
|
|
pre.src-awk:before { content: 'Awk'; }
|
|
pre.src-C:before { content: 'C'; }
|
|
/* pre.src-C++ doesn't work in CSS */
|
|
pre.src-clojure:before { content: 'Clojure'; }
|
|
pre.src-css:before { content: 'CSS'; }
|
|
pre.src-D:before { content: 'D'; }
|
|
pre.src-ditaa:before { content: 'ditaa'; }
|
|
pre.src-dot:before { content: 'Graphviz'; }
|
|
pre.src-calc:before { content: 'Emacs Calc'; }
|
|
pre.src-emacs-lisp:before { content: 'Emacs Lisp'; }
|
|
pre.src-fortran:before { content: 'Fortran'; }
|
|
pre.src-gnuplot:before { content: 'gnuplot'; }
|
|
pre.src-haskell:before { content: 'Haskell'; }
|
|
pre.src-hledger:before { content: 'hledger'; }
|
|
pre.src-java:before { content: 'Java'; }
|
|
pre.src-js:before { content: 'Javascript'; }
|
|
pre.src-latex:before { content: 'LaTeX'; }
|
|
pre.src-ledger:before { content: 'Ledger'; }
|
|
pre.src-lisp:before { content: 'Lisp'; }
|
|
pre.src-lilypond:before { content: 'Lilypond'; }
|
|
pre.src-lua:before { content: 'Lua'; }
|
|
pre.src-matlab:before { content: 'MATLAB'; }
|
|
pre.src-mscgen:before { content: 'Mscgen'; }
|
|
pre.src-ocaml:before { content: 'Objective Caml'; }
|
|
pre.src-octave:before { content: 'Octave'; }
|
|
pre.src-org:before { content: 'Org mode'; }
|
|
pre.src-oz:before { content: 'OZ'; }
|
|
pre.src-plantuml:before { content: 'Plantuml'; }
|
|
pre.src-processing:before { content: 'Processing.js'; }
|
|
pre.src-python:before { content: 'Python'; }
|
|
pre.src-R:before { content: 'R'; }
|
|
pre.src-ruby:before { content: 'Ruby'; }
|
|
pre.src-sass:before { content: 'Sass'; }
|
|
pre.src-scheme:before { content: 'Scheme'; }
|
|
pre.src-screen:before { content: 'Gnu Screen'; }
|
|
pre.src-sed:before { content: 'Sed'; }
|
|
pre.src-sh:before { content: 'shell'; }
|
|
pre.src-sql:before { content: 'SQL'; }
|
|
pre.src-sqlite:before { content: 'SQLite'; }
|
|
/* additional languages in org.el's org-babel-load-languages alist */
|
|
pre.src-forth:before { content: 'Forth'; }
|
|
pre.src-io:before { content: 'IO'; }
|
|
pre.src-J:before { content: 'J'; }
|
|
pre.src-makefile:before { content: 'Makefile'; }
|
|
pre.src-maxima:before { content: 'Maxima'; }
|
|
pre.src-perl:before { content: 'Perl'; }
|
|
pre.src-picolisp:before { content: 'Pico Lisp'; }
|
|
pre.src-scala:before { content: 'Scala'; }
|
|
pre.src-shell:before { content: 'Shell Script'; }
|
|
pre.src-ebnf2ps:before { content: 'ebfn2ps'; }
|
|
/* additional language identifiers per "defun org-babel-execute"
|
|
in ob-*.el */
|
|
pre.src-cpp:before { content: 'C++'; }
|
|
pre.src-abc:before { content: 'ABC'; }
|
|
pre.src-coq:before { content: 'Coq'; }
|
|
pre.src-groovy:before { content: 'Groovy'; }
|
|
/* additional language identifiers from org-babel-shell-names in
|
|
ob-shell.el: ob-shell is the only babel language using a lambda to put
|
|
the execution function name together. */
|
|
pre.src-bash:before { content: 'bash'; }
|
|
pre.src-csh:before { content: 'csh'; }
|
|
pre.src-ash:before { content: 'ash'; }
|
|
pre.src-dash:before { content: 'dash'; }
|
|
pre.src-ksh:before { content: 'ksh'; }
|
|
pre.src-mksh:before { content: 'mksh'; }
|
|
pre.src-posh:before { content: 'posh'; }
|
|
/* Additional Emacs modes also supported by the LaTeX listings package */
|
|
pre.src-ada:before { content: 'Ada'; }
|
|
pre.src-asm:before { content: 'Assembler'; }
|
|
pre.src-caml:before { content: 'Caml'; }
|
|
pre.src-delphi:before { content: 'Delphi'; }
|
|
pre.src-html:before { content: 'HTML'; }
|
|
pre.src-idl:before { content: 'IDL'; }
|
|
pre.src-mercury:before { content: 'Mercury'; }
|
|
pre.src-metapost:before { content: 'MetaPost'; }
|
|
pre.src-modula-2:before { content: 'Modula-2'; }
|
|
pre.src-pascal:before { content: 'Pascal'; }
|
|
pre.src-ps:before { content: 'PostScript'; }
|
|
pre.src-prolog:before { content: 'Prolog'; }
|
|
pre.src-simula:before { content: 'Simula'; }
|
|
pre.src-tcl:before { content: 'tcl'; }
|
|
pre.src-tex:before { content: 'TeX'; }
|
|
pre.src-plain-tex:before { content: 'Plain TeX'; }
|
|
pre.src-verilog:before { content: 'Verilog'; }
|
|
pre.src-vhdl:before { content: 'VHDL'; }
|
|
pre.src-xml:before { content: 'XML'; }
|
|
pre.src-nxml:before { content: 'XML'; }
|
|
/* add a generic configuration mode; LaTeX export needs an additional
|
|
(add-to-list 'org-latex-listings-langs '(conf " ")) in .emacs */
|
|
pre.src-conf:before { content: 'Configuration File'; }
|
|
|
|
table { border-collapse:collapse; }
|
|
caption.t-above { caption-side: top; }
|
|
caption.t-bottom { caption-side: bottom; }
|
|
td, th { vertical-align:top; }
|
|
th.org-right { text-align: center; }
|
|
th.org-left { text-align: center; }
|
|
th.org-center { text-align: center; }
|
|
td.org-right { text-align: right; }
|
|
td.org-left { text-align: left; }
|
|
td.org-center { text-align: center; }
|
|
dt { font-weight: bold; }
|
|
.footpara { display: inline; }
|
|
.footdef { margin-bottom: 1em; }
|
|
.figure { padding: 1em; }
|
|
.figure p { text-align: center; }
|
|
.equation-container {
|
|
display: table;
|
|
text-align: center;
|
|
width: 100%;
|
|
}
|
|
.equation {
|
|
vertical-align: middle;
|
|
}
|
|
.equation-label {
|
|
display: table-cell;
|
|
text-align: right;
|
|
vertical-align: middle;
|
|
}
|
|
.inlinetask {
|
|
padding: 10px;
|
|
border: 2px solid gray;
|
|
margin: 10px;
|
|
background: #ffffcc;
|
|
}
|
|
#org-div-home-and-up
|
|
{ text-align: right; font-size: 70%; white-space: nowrap; }
|
|
textarea { overflow-x: auto; }
|
|
.linenr { font-size: smaller }
|
|
.code-highlighted { background-color: #ffff00; }
|
|
.org-info-js_info-navigation { border-style: none; }
|
|
#org-info-js_console-label
|
|
{ font-size: 10px; font-weight: bold; white-space: nowrap; }
|
|
.org-info-js_search-highlight
|
|
{ background-color: #ffff00; color: #000000; font-weight: bold; }
|
|
.org-svg { width: 90%; }
|
|
/*]]>*/-->
|
|
</style>
|
|
<link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
|
<link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
|
<script src="./js/jquery.min.js"></script>
|
|
<script src="./js/bootstrap.min.js"></script>
|
|
<script src="./js/jquery.stickytableheaders.min.js"></script>
|
|
<script src="./js/readtheorg.js"></script>
|
|
<script type="text/javascript">
|
|
// @license magnet:?xt=urn:btih:1f739d935676111cfff4b4693e3816e664797050&dn=gpl-3.0.txt GPL-v3-or-Later
|
|
<!--/*--><![CDATA[/*><!--*/
|
|
function CodeHighlightOn(elem, id)
|
|
{
|
|
var target = document.getElementById(id);
|
|
if(null != target) {
|
|
elem.cacheClassElem = elem.className;
|
|
elem.cacheClassTarget = target.className;
|
|
target.className = "code-highlighted";
|
|
elem.className = "code-highlighted";
|
|
}
|
|
}
|
|
function CodeHighlightOff(elem, id)
|
|
{
|
|
var target = document.getElementById(id);
|
|
if(elem.cacheClassElem)
|
|
elem.className = elem.cacheClassElem;
|
|
if(elem.cacheClassTarget)
|
|
target.className = elem.cacheClassTarget;
|
|
}
|
|
/*]]>*///-->
|
|
// @license-end
|
|
</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>
|
|
<body>
|
|
<div id="org-div-home-and-up">
|
|
<a accesskey="h" href="./index.html"> UP </a>
|
|
|
|
|
<a accesskey="H" href="./index.html"> HOME </a>
|
|
</div><div id="content">
|
|
<h1 class="title">Stewart Platform - Vibration Isolation</h1>
|
|
<div id="table-of-contents">
|
|
<h2>Table of Contents</h2>
|
|
<div id="text-table-of-contents">
|
|
<ul>
|
|
<li><a href="#orgf86b757">1. HAC-LAC (Cascade) Control - Integral Control</a>
|
|
<ul>
|
|
<li><a href="#org3a9f4d4">1.1. Introduction</a></li>
|
|
<li><a href="#org58e2ab0">1.2. Initialization</a></li>
|
|
<li><a href="#orgab56a44">1.3. Identification</a>
|
|
<ul>
|
|
<li><a href="#org2309d71">1.3.1. HAC - Without LAC</a></li>
|
|
<li><a href="#orgd3d2942">1.3.2. HAC - IFF</a></li>
|
|
<li><a href="#org492aabc">1.3.3. HAC - DVF</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org4d7a6d8">1.4. Control Architecture</a></li>
|
|
<li><a href="#org3e1b1b7">1.5. 6x6 Plant Comparison</a></li>
|
|
<li><a href="#org14108ef">1.6. HAC - DVF</a>
|
|
<ul>
|
|
<li><a href="#org71d45ac">1.6.1. Plant</a></li>
|
|
<li><a href="#org8236bd6">1.6.2. Controller Design</a></li>
|
|
<li><a href="#org7810516">1.6.3. Obtained Performance</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org55f17b6">1.7. HAC - IFF</a>
|
|
<ul>
|
|
<li><a href="#org87cf3a4">1.7.1. Plant</a></li>
|
|
<li><a href="#org6d26667">1.7.2. Controller Design</a></li>
|
|
<li><a href="#orgef0abff">1.7.3. Obtained Performance</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org81c1767">1.8. Comparison</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org6f94eba">2. MIMO Analysis</a>
|
|
<ul>
|
|
<li><a href="#org925bb20">2.1. Initialization</a></li>
|
|
<li><a href="#org57c87f0">2.2. Identification</a>
|
|
<ul>
|
|
<li><a href="#org661b495">2.2.1. HAC - Without LAC</a></li>
|
|
<li><a href="#orgdd8b824">2.2.2. HAC - DVF</a></li>
|
|
<li><a href="#orgf606814">2.2.3. Cartesian Frame</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org8349fa6">2.3. Singular Value Decomposition</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgc8479b7">3. Diagonal Control based on the damped plant</a>
|
|
<ul>
|
|
<li><a href="#org99665a2">3.1. Initialization</a></li>
|
|
<li><a href="#org42a5e98">3.2. Identification</a></li>
|
|
<li><a href="#orgae85e0d">3.3. Steady State Decoupling</a>
|
|
<ul>
|
|
<li><a href="#org1e2bbe7">3.3.1. Pre-Compensator Design</a></li>
|
|
<li><a href="#org077e6f6">3.3.2. Diagonal Control Design</a></li>
|
|
<li><a href="#orgf7c304f">3.3.3. Results</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgad35bf9">3.4. Decoupling at Crossover</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org846cef9">4. Time Domain Simulation</a>
|
|
<ul>
|
|
<li><a href="#org2a9e89f">4.1. Initialization</a></li>
|
|
<li><a href="#org8dbc004">4.2. HAC IFF</a></li>
|
|
<li><a href="#org7dc4716">4.3. HAC-DVF</a></li>
|
|
<li><a href="#org65730fb">4.4. Results</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org69ebad1">5. Functions</a>
|
|
<ul>
|
|
<li><a href="#orgc7bcc65">5.1. <code>initializeController</code>: Initialize the Controller</a>
|
|
<ul>
|
|
<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>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<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>
|
|
<div class="outline-text-2" id="text-1">
|
|
</div>
|
|
<div id="outline-container-org3a9f4d4" class="outline-3">
|
|
<h3 id="org3a9f4d4"><span class="section-number-3">1.1</span> Introduction</h3>
|
|
<div class="outline-text-3" id="text-1-1">
|
|
<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>
|
|
The control architectures are shown in Figures <a href="#org63523c7">1</a> and <a href="#org7ad8618">2</a>.
|
|
</p>
|
|
|
|
<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.
|
|
</p>
|
|
|
|
|
|
<div id="org63523c7" class="figure">
|
|
<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>
|
|
|
|
|
|
|
|
<div id="org7ad8618" class="figure">
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org58e2ab0" class="outline-3">
|
|
<h3 id="org58e2ab0"><span class="section-number-3">1.2</span> Initialization</h3>
|
|
<div class="outline-text-3" id="text-1-2">
|
|
<p>
|
|
We first initialize the Stewart platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, <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">'type_F'</span>, <span class="org-string">'universal'</span>, <span class="org-string">'type_M'</span>, <span class="org-string">'spherical'</span>);
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The rotation point of the ground is located at the origin of frame \(\{A\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'rot_point'</span>, stewart.platform_F.FO_A);
|
|
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgab56a44" class="outline-3">
|
|
<h3 id="orgab56a44"><span class="section-number-3">1.3</span> Identification</h3>
|
|
<div class="outline-text-3" id="text-1-3">
|
|
<p>
|
|
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:
|
|
</p>
|
|
<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>
|
|
|
|
<div id="outline-container-org2309d71" class="outline-4">
|
|
<h4 id="org2309d71"><span class="section-number-4">1.3.1</span> HAC - Without LAC</h4>
|
|
<div class="outline-text-4" id="text-1-3-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_ol = linearize(mdl, io);
|
|
G_ol.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_ol.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd3d2942" class="outline-4">
|
|
<h4 id="orgd3d2942"><span class="section-number-4">1.3.2</span> HAC - IFF</h4>
|
|
<div class="outline-text-4" id="text-1-3-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'iff'</span>);
|
|
K_iff = <span class="org-type">-</span>(1e4<span class="org-type">/</span>s)<span class="org-type">*</span>eye(6);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_iff = linearize(mdl, io);
|
|
G_iff.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_iff.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org492aabc" class="outline-4">
|
|
<h4 id="org492aabc"><span class="section-number-4">1.3.3</span> HAC - DVF</h4>
|
|
<div class="outline-text-4" id="text-1-3-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'dvf'</span>);
|
|
K_dvf = <span class="org-type">-</span>1e4<span class="org-type">*</span>s<span class="org-type">/</span>(1<span class="org-type">+</span>s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>5000)<span class="org-type">*</span>eye(6);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_dvf = linearize(mdl, io);
|
|
G_dvf.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_dvf.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org4d7a6d8" class="outline-3">
|
|
<h3 id="org4d7a6d8"><span class="section-number-3">1.4</span> Control Architecture</h3>
|
|
<div class="outline-text-3" id="text-1-4">
|
|
<p>
|
|
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">
|
|
<pre class="src src-matlab">Gc_ol = minreal(G_ol)<span class="org-type">/</span>stewart.kinematics.J<span class="org-type">'</span>;
|
|
Gc_ol.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
|
|
|
|
Gc_iff = minreal(G_iff)<span class="org-type">/</span>stewart.kinematics.J<span class="org-type">'</span>;
|
|
Gc_iff.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
|
|
|
|
Gc_dvf = minreal(G_dvf)<span class="org-type">/</span>stewart.kinematics.J<span class="org-type">'</span>;
|
|
Gc_dvf.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
|
|
</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}\).
|
|
</p>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org3e1b1b7" class="outline-3">
|
|
<h3 id="org3e1b1b7"><span class="section-number-3">1.5</span> 6x6 Plant Comparison</h3>
|
|
<div class="outline-text-3" id="text-1-5">
|
|
|
|
<div id="orgbee4071" class="figure">
|
|
<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>
|
|
|
|
<div id="outline-container-org14108ef" class="outline-3">
|
|
<h3 id="org14108ef"><span class="section-number-3">1.6</span> HAC - DVF</h3>
|
|
<div class="outline-text-3" id="text-1-6">
|
|
</div>
|
|
<div id="outline-container-org71d45ac" class="outline-4">
|
|
<h4 id="org71d45ac"><span class="section-number-4">1.6.1</span> Plant</h4>
|
|
<div class="outline-text-4" id="text-1-6-1">
|
|
|
|
<div id="org487a558" class="figure">
|
|
<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>
|
|
|
|
<div id="outline-container-org8236bd6" class="outline-4">
|
|
<h4 id="org8236bd6"><span class="section-number-4">1.6.2</span> Controller Design</h4>
|
|
<div class="outline-text-4" id="text-1-6-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">
|
|
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>300; <span class="org-comment">% Wanted Bandwidth [rad/s]</span>
|
|
|
|
h = 1.2;
|
|
H_lead = 1<span class="org-type">/</span>h<span class="org-type">*</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">/</span>h))<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">*</span>h));
|
|
|
|
Kd_dvf = diag(1<span class="org-type">./</span>abs(diag(freqresp(1<span class="org-type">/</span>s<span class="org-type">*</span>Gc_dvf, wc)))) <span class="org-type">.*</span> H_lead <span class="org-type">.*</span> 1<span class="org-type">/</span>s;
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org5d0e2e3" class="figure">
|
|
<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">
|
|
<pre class="src src-matlab">K_hac_dvf = inv(stewart.kinematics.J<span class="org-type">'</span>)<span class="org-type">*</span>Kd_dvf;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7810516" class="outline-4">
|
|
<h4 id="org7810516"><span class="section-number-4">1.6.3</span> Obtained Performance</h4>
|
|
<div class="outline-text-4" id="text-1-6-3">
|
|
<p>
|
|
We identify the transmissibility and compliance of the system.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
|
|
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
|
|
[C_ol, C_norm_ol, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'dvf'</span>);
|
|
[T_dvf, T_norm_dvf, <span class="org-type">~</span>] = computeTransmissibility();
|
|
[C_dvf, C_norm_dvf, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'hac-dvf'</span>);
|
|
[T_hac_dvf, T_norm_hac_dvf, <span class="org-type">~</span>] = computeTransmissibility();
|
|
[C_hac_dvf, C_norm_hac_dvf, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orga06a318" class="figure">
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org55f17b6" class="outline-3">
|
|
<h3 id="org55f17b6"><span class="section-number-3">1.7</span> HAC - IFF</h3>
|
|
<div class="outline-text-3" id="text-1-7">
|
|
</div>
|
|
<div id="outline-container-org87cf3a4" class="outline-4">
|
|
<h4 id="org87cf3a4"><span class="section-number-4">1.7.1</span> Plant</h4>
|
|
<div class="outline-text-4" id="text-1-7-1">
|
|
|
|
<div id="org0fc8dea" class="figure">
|
|
<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>
|
|
|
|
<div id="outline-container-org6d26667" class="outline-4">
|
|
<h4 id="org6d26667"><span class="section-number-4">1.7.2</span> Controller Design</h4>
|
|
<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">
|
|
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>300; <span class="org-comment">% Wanted Bandwidth [rad/s]</span>
|
|
|
|
h = 1.2;
|
|
H_lead = 1<span class="org-type">/</span>h<span class="org-type">*</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">/</span>h))<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">*</span>h));
|
|
|
|
Kd_iff = diag(1<span class="org-type">./</span>abs(diag(freqresp(1<span class="org-type">/</span>s<span class="org-type">*</span>Gc_iff, wc)))) <span class="org-type">.*</span> H_lead <span class="org-type">.*</span> 1<span class="org-type">/</span>s;
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org211b595" class="figure">
|
|
<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">
|
|
<pre class="src src-matlab">K_hac_iff = inv(stewart.kinematics.J<span class="org-type">'</span>)<span class="org-type">*</span>Kd_iff;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgef0abff" class="outline-4">
|
|
<h4 id="orgef0abff"><span class="section-number-4">1.7.3</span> Obtained Performance</h4>
|
|
<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">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
|
|
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
|
|
[C_ol, C_norm_ol, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'iff'</span>);
|
|
[T_iff, T_norm_iff, <span class="org-type">~</span>] = computeTransmissibility();
|
|
[C_iff, C_norm_iff, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'hac-iff'</span>);
|
|
[T_hac_iff, T_norm_hac_iff, <span class="org-type">~</span>] = computeTransmissibility();
|
|
[C_hac_iff, C_norm_hac_iff, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgc47bfe0" class="figure">
|
|
<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>
|
|
|
|
<div id="outline-container-org81c1767" class="outline-3">
|
|
<h3 id="org81c1767"><span class="section-number-3">1.8</span> Comparison</h3>
|
|
<div class="outline-text-3" id="text-1-8">
|
|
|
|
<div id="orgf042a3f" class="figure">
|
|
<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>
|
|
|
|
|
|
<div id="org5a9df7e" class="figure">
|
|
<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>
|
|
|
|
|
|
<div id="orgdc12a99" class="figure">
|
|
<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>
|
|
|
|
<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">
|
|
<p>
|
|
Let’s define the system as shown in figure <a href="#orgac8f77c">13</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgac8f77c" class="figure">
|
|
<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>
|
|
|
|
<table id="orgc2ee4b0" border="2" cellspacing="0" cellpadding="6" rules="groups" frame="hsides">
|
|
<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>
|
|
<th scope="col" class="org-left"> </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>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"> </td>
|
|
<td class="org-left">\(\bm{\mathcal{F}}_d\)</td>
|
|
<td class="org-left">External Forces applied to the Payload</td>
|
|
</tr>
|
|
|
|
<tr>
|
|
<td class="org-left"> </td>
|
|
<td class="org-left">\(\bm{r}\)</td>
|
|
<td class="org-left">Reference signal for tracking</td>
|
|
</tr>
|
|
</tbody>
|
|
<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"> </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"> </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"> </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>
|
|
|
|
<div id="outline-container-org925bb20" class="outline-3">
|
|
<h3 id="org925bb20"><span class="section-number-3">2.1</span> Initialization</h3>
|
|
<div class="outline-text-3" id="text-2-1">
|
|
<p>
|
|
We first initialize the Stewart platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, <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">'type_F'</span>, <span class="org-string">'universal'</span>, <span class="org-string">'type_M'</span>, <span class="org-string">'spherical'</span>);
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The rotation point of the ground is located at the origin of frame \(\{A\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'rot_point'</span>, stewart.platform_F.FO_A);
|
|
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org57c87f0" class="outline-3">
|
|
<h3 id="org57c87f0"><span class="section-number-3">2.2</span> Identification</h3>
|
|
<div class="outline-text-3" id="text-2-2">
|
|
</div>
|
|
<div id="outline-container-org661b495" class="outline-4">
|
|
<h4 id="org661b495"><span class="section-number-4">2.2.1</span> HAC - Without LAC</h4>
|
|
<div class="outline-text-4" id="text-2-2-1">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_ol = linearize(mdl, io);
|
|
G_ol.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_ol.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgdd8b824" class="outline-4">
|
|
<h4 id="orgdd8b824"><span class="section-number-4">2.2.2</span> HAC - DVF</h4>
|
|
<div class="outline-text-4" id="text-2-2-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'dvf'</span>);
|
|
K_dvf = <span class="org-type">-</span>1e4<span class="org-type">*</span>s<span class="org-type">/</span>(1<span class="org-type">+</span>s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>5000)<span class="org-type">*</span>eye(6);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_dvf = linearize(mdl, io);
|
|
G_dvf.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_dvf.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgf606814" class="outline-4">
|
|
<h4 id="orgf606814"><span class="section-number-4">2.2.3</span> Cartesian Frame</h4>
|
|
<div class="outline-text-4" id="text-2-2-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gc_ol = minreal(G_ol)<span class="org-type">/</span>stewart.kinematics.J<span class="org-type">'</span>;
|
|
Gc_ol.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
|
|
|
|
Gc_dvf = minreal(G_dvf)<span class="org-type">/</span>stewart.kinematics.J<span class="org-type">'</span>;
|
|
Gc_dvf.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8349fa6" class="outline-3">
|
|
<h3 id="org8349fa6"><span class="section-number-3">2.3</span> Singular Value Decomposition</h3>
|
|
<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));
|
|
|
|
<span class="org-keyword">for</span> <span class="org-variable-name"><span class="org-constant">i</span></span> = <span class="org-constant">1:length(freqs)</span>
|
|
[U,S,V] = svd(freqresp(Gc_ol, freqs(<span class="org-constant">i</span>), <span class="org-string">'Hz'</span>));
|
|
U_ol(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = U;
|
|
S_ol(<span class="org-type">:</span>,<span class="org-constant">i</span>) = diag(S);
|
|
V_ol(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = V;
|
|
|
|
[U,S,V] = svd(freqresp(Gc_dvf, freqs(<span class="org-constant">i</span>), <span class="org-string">'Hz'</span>));
|
|
U_dvf(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = U;
|
|
S_dvf(<span class="org-type">:</span>,<span class="org-constant">i</span>) = diag(S);
|
|
V_dvf(<span class="org-type">:</span>,<span class="org-type">:</span>,<span class="org-constant">i</span>) = V;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<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>
|
|
<div class="outline-text-2" id="text-3">
|
|
<p>
|
|
From <a class='org-ref-reference' href="#skogestad07_multiv_feedb_contr">skogestad07_multiv_feedb_contr</a>, a simple approach to multivariable control is the following two-step procedure:
|
|
</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>
|
|
|
|
<div id="outline-container-org99665a2" class="outline-3">
|
|
<h3 id="org99665a2"><span class="section-number-3">3.1</span> Initialization</h3>
|
|
<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();
|
|
stewart = initializeFramesPositions(stewart, <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">'type_F'</span>, <span class="org-string">'universal'</span>, <span class="org-string">'type_M'</span>, <span class="org-string">'spherical'</span>);
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The rotation point of the ground is located at the origin of frame \(\{A\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'rot_point'</span>, stewart.platform_F.FO_A);
|
|
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org42a5e98" class="outline-3">
|
|
<h3 id="org42a5e98"><span class="section-number-3">3.2</span> Identification</h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'dvf'</span>);
|
|
K_dvf = <span class="org-type">-</span>1e4<span class="org-type">*</span>s<span class="org-type">/</span>(1<span class="org-type">+</span>s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>5000)<span class="org-type">*</span>eye(6);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_dvf = linearize(mdl, io);
|
|
G_dvf.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_dvf.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgae85e0d" class="outline-3">
|
|
<h3 id="orgae85e0d"><span class="section-number-3">3.3</span> Steady State Decoupling</h3>
|
|
<div class="outline-text-3" id="text-3-3">
|
|
</div>
|
|
<div id="outline-container-org1e2bbe7" class="outline-4">
|
|
<h4 id="org1e2bbe7"><span class="section-number-4">3.3.1</span> Pre-Compensator Design</h4>
|
|
<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">
|
|
<pre class="src src-matlab">Gs = G_dvf<span class="org-type">*</span>W1;
|
|
</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>
|
|
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>.
|
|
</p>
|
|
|
|
|
|
<div id="org76617c6" class="figure">
|
|
<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>
|
|
|
|
|
|
<div id="org96093e0" class="figure">
|
|
<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>
|
|
|
|
<div id="outline-container-org077e6f6" class="outline-4">
|
|
<h4 id="org077e6f6"><span class="section-number-4">3.3.2</span> Diagonal Control Design</h4>
|
|
<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">
|
|
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>300; <span class="org-comment">% Wanted Bandwidth [rad/s]</span>
|
|
|
|
h = 1.5;
|
|
H_lead = 1<span class="org-type">/</span>h<span class="org-type">*</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">/</span>h))<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">*</span>h));
|
|
|
|
Ks_dvf = diag(1<span class="org-type">./</span>abs(diag(freqresp(1<span class="org-type">/</span>s<span class="org-type">*</span>Gs, wc)))) <span class="org-type">.*</span> H_lead <span class="org-type">.*</span> 1<span class="org-type">/</span>s;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The overall controller is then \(K(s) = W_1 K_s(s)\) as shown in Figure <a href="#org4d1ce48">16</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">K_hac_dvf = W1 <span class="org-type">*</span> Ks_dvf;
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org4d1ce48" class="figure">
|
|
<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>
|
|
|
|
<div id="outline-container-orgf7c304f" class="outline-4">
|
|
<h4 id="orgf7c304f"><span class="section-number-4">3.3.3</span> Results</h4>
|
|
<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">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
|
|
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
|
|
[C_ol, C_norm_ol, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'hac-dvf'</span>);
|
|
[T_hac_dvf, T_norm_hac_dvf, <span class="org-type">~</span>] = computeTransmissibility();
|
|
[C_hac_dvf, C_norm_hac_dvf, <span class="org-type">~</span>] = computeCompliance();
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The results are shown in figure
|
|
</p>
|
|
|
|
|
|
<div id="orge29798b" class="figure">
|
|
<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>
|
|
|
|
<div id="outline-container-orgad35bf9" class="outline-3">
|
|
<h3 id="orgad35bf9"><span class="section-number-3">3.4</span> Decoupling at Crossover</h3>
|
|
<div class="outline-text-3" id="text-3-4">
|
|
<ul class="org-ul">
|
|
<li class="off"><code>[ ]</code> Find a method for real approximation of a complex matrix</li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org846cef9" class="outline-2">
|
|
<h2 id="org846cef9"><span class="section-number-2">4</span> Time Domain Simulation</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
</div>
|
|
<div id="outline-container-org2a9e89f" class="outline-3">
|
|
<h3 id="org2a9e89f"><span class="section-number-3">4.1</span> Initialization</h3>
|
|
<div class="outline-text-3" id="text-4-1">
|
|
<p>
|
|
We first initialize the Stewart platform.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, <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">'type_F'</span>, <span class="org-string">'universal'</span>, <span class="org-string">'type_M'</span>, <span class="org-string">'spherical'</span>);
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeInertialSensor(stewart, <span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The rotation point of the ground is located at the origin of frame \(\{A\}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>, <span class="org-string">'rot_point'</span>, stewart.platform_F.FO_A);
|
|
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load(<span class="org-string">'./mat/motion_error_ol.mat'</span>, <span class="org-string">'Eg'</span>)
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8dbc004" class="outline-3">
|
|
<h3 id="org8dbc004"><span class="section-number-3">4.2</span> HAC IFF</h3>
|
|
<div class="outline-text-3" id="text-4-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'iff'</span>);
|
|
K_iff = <span class="org-type">-</span>(1e4<span class="org-type">/</span>s)<span class="org-type">*</span>eye(6);
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_iff = linearize(mdl, io);
|
|
G_iff.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_iff.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
|
|
Gc_iff = minreal(G_iff)<span class="org-type">/</span>stewart.kinematics.J<span class="org-type">'</span>;
|
|
Gc_iff.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>100; <span class="org-comment">% Wanted Bandwidth [rad/s]</span>
|
|
|
|
h = 1.2;
|
|
H_lead = 1<span class="org-type">/</span>h<span class="org-type">*</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">/</span>h))<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">*</span>h));
|
|
|
|
Kd_iff = diag(1<span class="org-type">./</span>abs(diag(freqresp(1<span class="org-type">/</span>s<span class="org-type">*</span>Gc_iff, wc)))) <span class="org-type">.*</span> H_lead <span class="org-type">.*</span> 1<span class="org-type">/</span>s;
|
|
K_hac_iff = inv(stewart.kinematics.J<span class="org-type">'</span>)<span class="org-type">*</span>Kd_iff;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'hac-iff'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7dc4716" class="outline-3">
|
|
<h3 id="org7dc4716"><span class="section-number-3">4.3</span> HAC-DVF</h3>
|
|
<div class="outline-text-3" id="text-4-3">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'dvf'</span>);
|
|
K_dvf = <span class="org-type">-</span>1e4<span class="org-type">*</span>s<span class="org-type">/</span>(1<span class="org-type">+</span>s<span class="org-type">/</span>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>5000)<span class="org-type">*</span>eye(6);
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Name of the Simulink File</span></span>
|
|
mdl = <span class="org-string">'stewart_platform_model'</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">'/Controller'</span>], 1, <span class="org-string">'input'</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">'/Absolute Motion Sensor'</span>], 1, <span class="org-string">'openoutput'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Absolute Sensor [m, rad]</span>
|
|
|
|
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
|
|
G_dvf = linearize(mdl, io);
|
|
G_dvf.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_dvf.OutputName = {<span class="org-string">'Dx'</span>, <span class="org-string">'Dy'</span>, <span class="org-string">'Dz'</span>, <span class="org-string">'Rx'</span>, <span class="org-string">'Ry'</span>, <span class="org-string">'Rz'</span>};
|
|
|
|
Gc_dvf = minreal(G_dvf)<span class="org-type">/</span>stewart.kinematics.J<span class="org-type">'</span>;
|
|
Gc_dvf.InputName = {<span class="org-string">'Fx'</span>, <span class="org-string">'Fy'</span>, <span class="org-string">'Fz'</span>, <span class="org-string">'Mx'</span>, <span class="org-string">'My'</span>, <span class="org-string">'Mz'</span>};
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">wc = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>100; <span class="org-comment">% Wanted Bandwidth [rad/s]</span>
|
|
|
|
h = 1.2;
|
|
H_lead = 1<span class="org-type">/</span>h<span class="org-type">*</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">/</span>h))<span class="org-type">/</span>(1 <span class="org-type">+</span> s<span class="org-type">/</span>(wc<span class="org-type">*</span>h));
|
|
|
|
Kd_dvf = diag(1<span class="org-type">./</span>abs(diag(freqresp(1<span class="org-type">/</span>s<span class="org-type">*</span>Gc_dvf, wc)))) <span class="org-type">.*</span> H_lead <span class="org-type">.*</span> 1<span class="org-type">/</span>s;
|
|
|
|
K_hac_dvf = inv(stewart.kinematics.J<span class="org-type">'</span>)<span class="org-type">*</span>Kd_dvf;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'hac-dvf'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org65730fb" class="outline-3">
|
|
<h3 id="org65730fb"><span class="section-number-3">4.4</span> Results</h3>
|
|
<div class="outline-text-3" id="text-4-4">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-type">figure</span>;
|
|
subplot(1, 2, 1);
|
|
hold on;
|
|
plot(Eg.Time, Eg.Data(<span class="org-type">:</span>, 1), <span class="org-string">'DisplayName'</span>, <span class="org-string">'X'</span>);
|
|
plot(Eg.Time, Eg.Data(<span class="org-type">:</span>, 2), <span class="org-string">'DisplayName'</span>, <span class="org-string">'Y'</span>);
|
|
plot(Eg.Time, Eg.Data(<span class="org-type">:</span>, 3), <span class="org-string">'DisplayName'</span>, <span class="org-string">'Z'</span>);
|
|
hold off;
|
|
xlabel(<span class="org-string">'Time [s]'</span>);
|
|
ylabel(<span class="org-string">'Position error [m]'</span>);
|
|
legend();
|
|
|
|
subplot(1, 2, 2);
|
|
hold on;
|
|
plot(simout.Xa.Time, simout.Xa.Data(<span class="org-type">:</span>, 1));
|
|
plot(simout.Xa.Time, simout.Xa.Data(<span class="org-type">:</span>, 2));
|
|
plot(simout.Xa.Time, simout.Xa.Data(<span class="org-type">:</span>, 3));
|
|
hold off;
|
|
xlabel(<span class="org-string">'Time [s]'</span>);
|
|
ylabel(<span class="org-string">'Orientation error [rad]'</span>);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org69ebad1" class="outline-2">
|
|
<h2 id="org69ebad1"><span class="section-number-2">5</span> Functions</h2>
|
|
<div class="outline-text-2" id="text-5">
|
|
</div>
|
|
<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>
|
|
<div class="outline-text-3" id="text-5-1">
|
|
<p>
|
|
<a id="org33a5401"></a>
|
|
</p>
|
|
</div>
|
|
|
|
<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">
|
|
<pre class="src src-matlab"><span class="org-keyword">function</span> <span class="org-variable-name">[controller]</span> = <span class="org-function-name">initializeController</span>(<span class="org-variable-name">args</span>)
|
|
<span class="org-comment">% initializeController - Initialize the Controller</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Syntax: [] = initializeController(args)</span>
|
|
<span class="org-comment">%</span>
|
|
<span class="org-comment">% Inputs:</span>
|
|
<span class="org-comment">% - args - Can have the following fields:</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<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
|
|
args.type char {mustBeMember(args.type, {<span class="org-string">'open-loop'</span>, <span class="org-string">'iff'</span>, <span class="org-string">'dvf'</span>, <span class="org-string">'hac-iff'</span>, <span class="org-string">'hac-dvf'</span>, <span class="org-string">'ref-track-L'</span>, <span class="org-string">'ref-track-X'</span>, <span class="org-string">'ref-track-hac-dvf'</span>})} = <span class="org-string">'open-loop'</span>
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<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>
|
|
|
|
<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">
|
|
<pre class="src src-matlab"><span class="org-keyword">switch</span> <span class="org-constant">args.type</span>
|
|
<span class="org-keyword">case</span> <span class="org-string">'open-loop'</span>
|
|
controller.type = 0;
|
|
<span class="org-keyword">case</span> <span class="org-string">'iff'</span>
|
|
controller.type = 1;
|
|
<span class="org-keyword">case</span> <span class="org-string">'dvf'</span>
|
|
controller.type = 2;
|
|
<span class="org-keyword">case</span> <span class="org-string">'hac-iff'</span>
|
|
controller.type = 3;
|
|
<span class="org-keyword">case</span> <span class="org-string">'hac-dvf'</span>
|
|
controller.type = 4;
|
|
<span class="org-keyword">case</span> <span class="org-string">'ref-track-L'</span>
|
|
controller.type = 5;
|
|
<span class="org-keyword">case</span> <span class="org-string">'ref-track-X'</span>
|
|
controller.type = 6;
|
|
<span class="org-keyword">case</span> <span class="org-string">'ref-track-hac-dvf'</span>
|
|
controller.type = 7;
|
|
<span class="org-keyword">end</span>
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2020-03-16 lun. 11:21</p>
|
|
</div>
|
|
</body>
|
|
</html>
|