2020-02-28 17:35:44 +01:00
<?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 11:23:15 +01:00
<!-- 2020 - 03 - 16 lun. 11:22 -->
2020-02-28 17:35:44 +01:00
< meta http-equiv = "Content-Type" content = "text/html;charset=utf-8" / >
< meta name = "viewport" content = "width=device-width, initial-scale=1" / >
< title > Stewart Platform - Tracking Control< / 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 - Tracking Control< / h1 >
< div id = "table-of-contents" >
< h2 > Table of Contents< / h2 >
< div id = "text-table-of-contents" >
< ul >
2020-03-11 19:00:22 +01:00
< li > < a href = "#orgd7b25e5" > 1. Decentralized Control Architecture using Strut Length< / a >
2020-02-28 17:35:44 +01:00
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#orgaf4f125" > 1.1. Control Schematic< / a > < / li >
< li > < a href = "#org5efa5dc" > 1.2. Initialize the Stewart platform< / a > < / li >
< li > < a href = "#orgf2a4e09" > 1.3. Identification of the plant< / a > < / li >
< li > < a href = "#org346704a" > 1.4. Plant Analysis< / a > < / li >
< li > < a href = "#org303d728" > 1.5. Controller Design< / a > < / li >
< li > < a href = "#orgac9e2fb" > 1.6. Simulation< / a > < / li >
2020-03-11 19:00:22 +01:00
< li > < a href = "#org974b430" > 1.7. Results< / a > < / li >
2020-03-16 11:23:15 +01:00
< li > < a href = "#org8f3d960" > 1.8. Conclusion< / a > < / li >
2020-02-28 17:35:44 +01:00
< / ul >
< / li >
2020-03-11 19:00:22 +01:00
< li > < a href = "#orga519721" > 2. Centralized Control Architecture using Pose Measurement< / a >
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#org373826d" > 2.1. Control Schematic< / a > < / li >
< li > < a href = "#orgdb540d4" > 2.2. Initialize the Stewart platform< / a > < / li >
< li > < a href = "#org7e2bbea" > 2.3. Identification of the plant< / a > < / li >
2020-03-11 19:00:22 +01:00
< li > < a href = "#org2223469" > 2.4. Diagonal Control - Leg’ s Frame< / a >
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#org42dc407" > 2.4.1. Control Architecture< / a > < / li >
< li > < a href = "#org33774e9" > 2.4.2. Plant Analysis< / a > < / li >
< li > < a href = "#orgc7ddab1" > 2.4.3. Controller Design< / a > < / li >
< li > < a href = "#org3021cf3" > 2.4.4. Simulation< / a > < / li >
2020-03-11 19:00:22 +01:00
< / ul >
< / li >
< li > < a href = "#org26a8857" > 2.5. Diagonal Control - Cartesian Frame< / a >
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#org185190b" > 2.5.1. Control Architecture< / a > < / li >
< li > < a href = "#orgac4e8f3" > 2.5.2. Plant Analysis< / a > < / li >
< li > < a href = "#org87c1a48" > 2.5.3. Controller Design< / a > < / li >
< li > < a href = "#org73875ca" > 2.5.4. Simulation< / a > < / li >
2020-03-11 19:00:22 +01:00
< / ul >
< / li >
< li > < a href = "#orgad7bc54" > 2.6. Diagonal Control - Steady State Decoupling< / a >
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#orgdeee29c" > 2.6.1. Control Architecture< / a > < / li >
< li > < a href = "#org4c98210" > 2.6.2. Plant Analysis< / a > < / li >
< li > < a href = "#orgbf66d4c" > 2.6.3. Controller Design< / a > < / li >
2020-03-11 19:00:22 +01:00
< / ul >
< / li >
< li > < a href = "#orga2eadeb" > 2.7. Comparison< / a >
< ul >
< li > < a href = "#org09ae901" > 2.7.1. Obtained MIMO Controllers< / a > < / li >
< li > < a href = "#org23ae479" > 2.7.2. Simulation Results< / a > < / li >
< / ul >
< / li >
2020-03-16 11:23:15 +01:00
< li > < a href = "#orgd2764a2" > 2.8. Conclusion< / a > < / li >
2020-03-11 19:00:22 +01:00
< / ul >
< / li >
< li > < a href = "#org4b8c360" > 3. Hybrid Control Architecture - HAC-LAC with relative DVF< / a >
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#org539565c" > 3.1. Control Schematic< / a > < / li >
< li > < a href = "#orga9bdd4e" > 3.2. Initialize the Stewart platform< / a > < / li >
2020-03-12 18:06:56 +01:00
< li > < a href = "#org3274a98" > 3.3. First Control Loop - \(\bm{K}_\mathcal{L}\)< / a >
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#orgd69b37c" > 3.3.1. Identification< / a > < / li >
< li > < a href = "#org2a634da" > 3.3.2. Obtained Plant< / a > < / li >
< li > < a href = "#org23695fa" > 3.3.3. Controller Design< / a > < / li >
2020-03-12 18:06:56 +01:00
< / ul >
< / li >
< li > < a href = "#org8440c0b" > 3.4. Second Control Loop - \(\bm{K}_\mathcal{X}\)< / a >
< ul >
2020-03-16 11:23:15 +01:00
< li > < a href = "#orgeec6c35" > 3.4.1. Identification< / a > < / li >
< li > < a href = "#org57836ee" > 3.4.2. Obtained Plant< / a > < / li >
< li > < a href = "#orgcebb0d5" > 3.4.3. Controller Design< / a > < / li >
2020-03-12 18:06:56 +01:00
< / ul >
< / li >
< li > < a href = "#org74d3dcd" > 3.5. Simulations< / a > < / li >
2020-03-16 11:23:15 +01:00
< li > < a href = "#org771bea0" > 3.6. Conclusion< / a > < / li >
2020-03-11 19:00:22 +01:00
< / ul >
< / li >
2020-03-13 13:12:29 +01:00
< li > < a href = "#org798d54f" > 4. Comparison of all the methods< / a > < / li >
< li > < a href = "#orgd0502ff" > 5. Compute the pose error of the Stewart Platform< / a > < / li >
2020-02-28 17:35:44 +01:00
< / ul >
< / div >
< / div >
2020-03-11 19:00:22 +01:00
< p >
Let’ s suppose the control objective is to position \(\bm{\mathcal{X}}\) of the mobile platform of the Stewart platform such that it is following some reference position \(\bm{r}_\mathcal{X}\).
< / p >
2020-03-13 13:12:29 +01:00
< p >
In order to compute the pose error \(\bm{\epsilon}_\mathcal{X}\) from the actual pose \(\bm{\mathcal{X}}\) and the reference position \(\bm{r}_\mathcal{X}\), some computation is required and explained in section < a href = "#org5f61540" > 5< / a > .
< / p >
2020-03-11 19:00:22 +01:00
< p >
Depending of the measured quantity, different control architectures can be used:
< / p >
< ul class = "org-ul" >
< li > If the struts length \(\bm{\mathcal{L}}\) is measured, a decentralized control architecture can be used (Section < a href = "#orgea7df6c" > 1< / a > )< / li >
< li > If the pose of the mobile platform \(\bm{\mathcal{X}}\) is directly measured, a centralized control architecture can be used (Section < a href = "#org48604d1" > 2< / a > )< / li >
< li > If both \(\bm{\mathcal{L}}\) and \(\bm{\mathcal{X}}\) are measured, we can use an hybrid control architecture (Section < a href = "#org14e3e5f" > 3< / a > )< / li >
< / ul >
2020-03-13 13:12:29 +01:00
< p >
The control configuration are compare in section < a href = "#orgb3403cb" > 4< / a > .
< / p >
2020-03-11 19:00:22 +01:00
< div id = "outline-container-orgd7b25e5" class = "outline-2" >
< h2 id = "orgd7b25e5" > < span class = "section-number-2" > 1< / span > Decentralized Control Architecture using Strut Length< / h2 >
2020-02-28 17:35:44 +01:00
< div class = "outline-text-2" id = "text-1" >
2020-03-11 19:00:22 +01:00
< p >
< a id = "orgea7df6c" > < / a >
< / p >
2020-02-28 17:35:44 +01:00
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgaf4f125" class = "outline-3" >
< h3 id = "orgaf4f125" > < span class = "section-number-3" > 1.1< / span > Control Schematic< / h3 >
2020-02-28 17:35:44 +01:00
< div class = "outline-text-3" id = "text-1-1" >
2020-03-11 19:00:22 +01:00
< p >
2020-03-16 11:23:15 +01:00
The control architecture is shown in Figure < a href = "#org897886b" > 1< / a > .
2020-03-11 19:00:22 +01:00
< / p >
< p >
The required leg length \(\bm{r}_\mathcal{L}\) is computed from the reference path \(\bm{r}_\mathcal{X}\) using the inverse kinematics.
< / p >
2020-02-28 17:35:44 +01:00
2020-03-11 19:00:22 +01:00
< p >
Then, a diagonal (decentralized) controller \(\bm{K}_\mathcal{L}\) is used such that each leg lengths stays close to its required length.
< / p >
2020-03-16 11:23:15 +01:00
< div id = "org897886b" class = "figure" >
< p > < img src = "figs/decentralized_reference_tracking_L.png" alt = "decentralized_reference_tracking_L.png" / >
2020-02-28 17:35:44 +01:00
< / p >
2020-03-11 19:00:22 +01:00
< p > < span class = "figure-number" > Figure 1: < / span > Decentralized control for reference tracking< / p >
2020-02-28 17:35:44 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org5efa5dc" class = "outline-3" >
< h3 id = "org5efa5dc" > < span class = "section-number-3" > 1.2< / span > Initialize the Stewart platform< / h3 >
2020-02-28 17:35:44 +01:00
< div class = "outline-text-3" id = "text-1-2" >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > < span class = "org-comment" > % Stewart Platform< / span >
stewart = initializeStewartPlatform();
2020-02-28 17:35:44 +01:00
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_p'< / span > , < span class = "org-string" > 'type_M'< / span > , < span class = "org-string" > 'spherical_p'< / 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" > 'accelerometer'< / span > , < span class = "org-string" > 'freq'< / span > , 5e3);
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Ground and Payload< / span >
ground = initializeGround(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'rigid'< / span > );
2020-02-28 17:35:44 +01:00
payload = initializePayload(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'none'< / span > );
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Controller< / span >
2020-03-11 19:00:22 +01:00
controller = initializeController(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'open-loop'< / span > );
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Disturbances and References< / span >
disturbances = initializeDisturbances();
2020-03-11 19:00:22 +01:00
references = initializeReferences(stewart);
2020-02-28 17:35:44 +01:00
< / pre >
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgf2a4e09" class = "outline-3" >
< h3 id = "orgf2a4e09" > < span class = "section-number-3" > 1.3< / span > Identification of the plant< / h3 >
2020-02-28 17:35:44 +01:00
< div class = "outline-text-3" id = "text-1-3" >
< p >
2020-03-12 18:06:56 +01:00
Let’ s identify the transfer function from \(\bm{\tau}\) to \(\bm{\mathcal{L}}\).
2020-02-28 17:35:44 +01:00
< / p >
< 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" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1; < span class = "org-comment" > % Actuator Force Inputs [N]< / span >
io(io_i) = linio([mdl, < span class = "org-string" > '/Stewart Platform'< / span > ], 1, < span class = "org-string" > 'openoutput'< / span > , [], < span class = "org-string" > 'dLm'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1; < span class = "org-comment" > % Relative Displacement Outputs [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the linearization< / span > < / span >
G = linearize(mdl, io);
G.InputName = {< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > };
G.OutputName = {< span class = "org-string" > 'L1'< / span > , < span class = "org-string" > 'L2'< / span > , < span class = "org-string" > 'L3'< / span > , < span class = "org-string" > 'L4'< / span > , < span class = "org-string" > 'L5'< / span > , < span class = "org-string" > 'L6'< / span > };
< / pre >
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org346704a" class = "outline-3" >
< h3 id = "org346704a" > < span class = "section-number-3" > 1.4< / span > Plant Analysis< / h3 >
2020-02-28 17:35:44 +01:00
< div class = "outline-text-3" id = "text-1-4" >
< p >
2020-03-16 11:23:15 +01:00
The diagonal and off-diagonal terms of the plant are shown in Figure < a href = "#org50fb6db" > 2< / a > .
2020-03-11 19:00:22 +01:00
< / p >
< p >
All the diagonal terms are equal.
We see that the plant is decoupled at low frequency which indicate that decentralized control may be a good idea.
< / p >
2020-03-16 11:23:15 +01:00
< div id = "org50fb6db" class = "figure" >
< p > < img src = "figs/plant_decentralized_L.png" alt = "plant_decentralized_L.png" / >
2020-02-28 17:35:44 +01:00
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 2: < / span > Obtain Diagonal and off diagonal dynamics (< a href = "./figs/plant_decentralized_L.png" > png< / a > , < a href = "./figs/plant_decentralized_L.pdf" > pdf< / a > )< / p >
2020-02-28 17:35:44 +01:00
< / div >
< / div >
2020-03-11 19:00:22 +01:00
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org303d728" class = "outline-3" >
< h3 id = "org303d728" > < span class = "section-number-3" > 1.5< / span > Controller Design< / h3 >
2020-02-28 17:35:44 +01:00
< div class = "outline-text-3" id = "text-1-5" >
< p >
2020-03-11 19:00:22 +01:00
The controller consists of:
2020-02-28 17:35:44 +01:00
< / p >
2020-03-11 19:00:22 +01:00
< ul class = "org-ul" >
< li > A pure integrator< / li >
< li > A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin< / li >
< / ul >
2020-02-28 17:35:44 +01:00
< p >
2020-03-16 11:23:15 +01:00
The obtained loop gains corresponding to the diagonal elements are shown in Figure < a href = "#org08e591a" > 3< / a > .
2020-03-11 19:00:22 +01:00
< / 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 > 30;
Kl = diag(1< span class = "org-type" > ./< / span > diag(abs(freqresp(G, wc)))) < span class = "org-type" > *< / span > wc< span class = "org-type" > /< / span > s < span class = "org-type" > *< / span > 1< span class = "org-type" > /< / span > (1 < span class = "org-type" > +< / span > s< span class = "org-type" > /< / span > 3< span class = "org-type" > /< / span > wc);
< / pre >
< / div >
< div id = "org08e591a" class = "figure" >
< p > < img src = "figs/loop_gain_decentralized_L.png" alt = "loop_gain_decentralized_L.png" / >
2020-02-28 17:35:44 +01:00
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 3: < / span > Loop Gain of the diagonal elements (< a href = "./figs/loop_gain_decentralized_L.png" > png< / a > , < a href = "./figs/loop_gain_decentralized_L.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
2020-02-28 17:35:44 +01:00
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgac9e2fb" class = "outline-3" >
< h3 id = "orgac9e2fb" > < span class = "section-number-3" > 1.6< / span > Simulation< / h3 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-3" id = "text-1-6" >
2020-03-13 13:12:29 +01:00
< p >
Let’ s define some reference path to follow.
< / p >
2020-02-28 17:35:44 +01:00
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > t = [0< span class = "org-type" > :< / span > 1e< span class = "org-type" > -< / span > 4< span class = "org-type" > :< / span > 10];
2020-03-11 19:00:22 +01:00
r = zeros(6, length(t));
2020-03-13 13:12:29 +01:00
r(1, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > sin(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(2, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > cos(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(3, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t;
2020-03-11 19:00:22 +01:00
references = initializeReferences(stewart, < span class = "org-string" > 't'< / span > , t, < span class = "org-string" > 'r'< / span > , r);
< / pre >
< / div >
2020-02-28 17:35:44 +01:00
2020-03-13 13:12:29 +01:00
< p >
2020-03-16 11:23:15 +01:00
The reference path is shown in Figure < a href = "#orga6384f7" > 4< / a > .
2020-03-13 13:12:29 +01:00
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span > ;
plot3(squeeze(references.r.Data(1,1,< span class = "org-type" > :< / span > )), squeeze(references.r.Data(2,1,< span class = "org-type" > :< / span > )), squeeze(references.r.Data(3,1,< span class = "org-type" > :< / span > )));
xlabel(< span class = "org-string" > 'X [m]'< / span > );
ylabel(< span class = "org-string" > 'Y [m]'< / span > );
zlabel(< span class = "org-string" > 'Z [m]'< / span > );
< / pre >
< / div >
< div id = "orga6384f7" class = "figure" >
< p > < img src = "figs/tracking_control_reference_path.png" alt = "tracking_control_reference_path.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 4: < / span > Reference path used for Tracking control (< a href = "./figs/tracking_control_reference_path.png" > png< / a > , < a href = "./figs/tracking_control_reference_path.pdf" > pdf< / a > )< / p >
2020-03-13 13:12:29 +01:00
< / div >
2020-03-11 19:00:22 +01:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > controller = initializeController(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'ref-track-L'< / span > );
< / pre >
< / div >
2020-02-28 17:35:44 +01:00
2020-03-11 19:00:22 +01:00
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > < span class = "org-matlab-simulink-keyword" > set_param< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > , < span class = "org-string" > 'StopTime'< / span > , < span class = "org-string" > '10'< / span > )
< span class = "org-matlab-simulink-keyword" > sim< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > );
2020-03-11 19:00:22 +01:00
simout_D = simout;
2020-02-28 17:35:44 +01:00
< / pre >
< / div >
2020-03-13 13:12:29 +01:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > save(< span class = "org-string" > './mat/control_tracking.mat'< / span > , < span class = "org-string" > 'simout_D'< / span > , < span class = "org-string" > '-append'< / span > );
< / pre >
< / div >
2020-02-28 17:35:44 +01:00
< / div >
< / div >
2020-03-11 19:00:22 +01:00
< div id = "outline-container-org974b430" class = "outline-3" >
< h3 id = "org974b430" > < span class = "section-number-3" > 1.7< / span > Results< / h3 >
< div class = "outline-text-3" id = "text-1-7" >
2020-03-13 13:12:29 +01:00
< p >
2020-03-16 11:23:15 +01:00
The reference path and the position of the mobile platform are shown in Figure < a href = "#orge497f54" > 5< / a > .
2020-03-13 13:12:29 +01:00
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-type" > figure< / span > ;
hold on;
plot3(simout_D.x.Xr.Data(< span class = "org-type" > :< / span > , 1), simout_D.x.Xr.Data(< span class = "org-type" > :< / span > , 2), simout_D.x.Xr.Data(< span class = "org-type" > :< / span > , 3), < span class = "org-string" > 'k-'< / span > );
plot3(squeeze(references.r.Data(1,1,< span class = "org-type" > :< / span > )), squeeze(references.r.Data(2,1,< span class = "org-type" > :< / span > )), squeeze(references.r.Data(3,1,< span class = "org-type" > :< / span > )), < span class = "org-string" > '--'< / span > );
hold off;
xlabel(< span class = "org-string" > 'X [m]'< / span > ); ylabel(< span class = "org-string" > 'Y [m]'< / span > ); zlabel(< span class = "org-string" > 'Z [m]'< / span > );
view([1,1,1])
zlim([0, < span class = "org-constant" > inf< / span > ]);
< / pre >
< / div >
< div id = "orge497f54" class = "figure" >
< p > < img src = "figs/decentralized_control_3d_pos.png" alt = "decentralized_control_3d_pos.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 5: < / span > Reference path and Obtained Position (< a href = "./figs/decentralized_control_3d_pos.png" > png< / a > , < a href = "./figs/decentralized_control_3d_pos.pdf" > pdf< / a > )< / p >
2020-03-13 13:12:29 +01:00
< / div >
2020-03-11 19:00:22 +01:00
< div id = "org1ac9124" class = "figure" >
< p > < img src = "figs/decentralized_control_Ex.png" alt = "decentralized_control_Ex.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 6: < / span > Position error \(\bm{\epsilon}_\mathcal{X}\) (< a href = "./figs/decentralized_control_Ex.png" > png< / a > , < a href = "./figs/decentralized_control_Ex.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< div id = "org10eb8ae" class = "figure" >
< p > < img src = "figs/decentralized_control_El.png" alt = "decentralized_control_El.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 7: < / span > Position error \(\bm{\epsilon}_\mathcal{L}\) (< a href = "./figs/decentralized_control_El.png" > png< / a > , < a href = "./figs/decentralized_control_El.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org8f3d960" class = "outline-3" >
< h3 id = "org8f3d960" > < span class = "section-number-3" > 1.8< / span > Conclusion< / h3 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-3" id = "text-1-8" >
< p >
Such control architecture is easy to implement and give good results.
The inverse kinematics is easy to compute.
< / p >
< p >
However, as \(\mathcal{X}\) is not directly measured, it is possible that important positioning errors are due to finite stiffness of the joints and other imperfections.
< / p >
< / div >
< / div >
< / div >
< div id = "outline-container-orga519721" class = "outline-2" >
< h2 id = "orga519721" > < span class = "section-number-2" > 2< / span > Centralized Control Architecture using Pose Measurement< / h2 >
< div class = "outline-text-2" id = "text-2" >
< p >
< a id = "org48604d1" > < / a >
< / p >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org373826d" class = "outline-3" >
< h3 id = "org373826d" > < span class = "section-number-3" > 2.1< / span > Control Schematic< / h3 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-3" id = "text-2-1" >
< p >
2020-03-16 11:23:15 +01:00
The centralized controller takes the position error \(\bm{\epsilon}_\mathcal{X}\) as an inputs and generate actuator forces \(\bm{\tau}\) (see Figure < a href = "#org97ec686" > 8< / a > ).
2020-03-11 19:00:22 +01:00
The signals are:
< / p >
< ul class = "org-ul" >
< li > reference path \(\bm{r}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}\)< / li >
< li > tracking error \(\bm{\epsilon}_\mathcal{X} = \begin{bmatrix} \epsilon_x & \epsilon_y & \epsilon_z & \epsilon_{R_x} & \epsilon_{R_y} & \epsilon_{R_z} \end{bmatrix}\)< / li >
< li > actuator forces \(\bm{\tau} = \begin{bmatrix} \tau_1 & \tau_2 & \tau_3 & \tau_4 & \tau_5 & \tau_6 \end{bmatrix}\)< / li >
2020-03-11 19:01:50 +01:00
< li > payload pose \(\bm{\mathcal{X}} = \begin{bmatrix} x & y & z & R_x & R_y & R_z \end{bmatrix}\)< / li >
2020-03-11 19:00:22 +01:00
< / ul >
< div id = "org97ec686" class = "figure" >
< p > < img src = "figs/centralized_reference_tracking.png" alt = "centralized_reference_tracking.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 8: < / span > Centralized Controller< / p >
2020-03-11 19:00:22 +01:00
< / div >
< p >
Instead of designing a full MIMO controller \(K\), we first try to make the plant more diagonal by pre- or post-multiplying some constant matrix, then we design a diagonal controller.
< / p >
< p >
We can think of two ways to make the plant more diagonal that are described in sections < a href = "#org31fd942" > 2.4< / a > and < a href = "#orgfd201c3" > 2.5< / a > .
< / p >
< div class = "important" >
< p >
2020-03-16 11:23:15 +01:00
Note here that the subtraction shown in Figure < a href = "#org97ec686" > 8< / a > is not a real subtraction.
2020-03-13 13:12:29 +01:00
It is indeed a more complex computation explained in section < a href = "#org5f61540" > 5< / a > .
2020-03-11 19:00:22 +01:00
< / p >
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgdb540d4" class = "outline-3" >
< h3 id = "orgdb540d4" > < span class = "section-number-3" > 2.2< / span > Initialize the Stewart platform< / h3 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-3" id = "text-2-2" >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > < span class = "org-comment" > % Stewart Platform< / span >
stewart = initializeStewartPlatform();
2020-03-11 19:00:22 +01:00
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_p'< / span > , < span class = "org-string" > 'type_M'< / span > , < span class = "org-string" > 'spherical_p'< / 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" > 'accelerometer'< / span > , < span class = "org-string" > 'freq'< / span > , 5e3);
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Ground and Payload< / span >
ground = initializeGround(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'rigid'< / span > );
2020-03-11 19:00:22 +01:00
payload = initializePayload(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'none'< / span > );
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Controller< / span >
2020-03-11 19:00:22 +01:00
controller = initializeController(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'open-loop'< / span > );
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Disturbances and References< / span >
disturbances = initializeDisturbances();
2020-03-11 19:00:22 +01:00
references = initializeReferences(stewart);
< / pre >
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org7e2bbea" class = "outline-3" >
< h3 id = "org7e2bbea" > < span class = "section-number-3" > 2.3< / span > Identification of the plant< / h3 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-3" id = "text-2-3" >
< p >
2020-03-12 18:06:56 +01:00
Let’ s identify the transfer function from \(\bm{\tau}\) to \(\bm{\mathcal{X}}\).
2020-03-11 19:00:22 +01:00
< / p >
< 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" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1; < span class = "org-comment" > % Actuator Force Inputs [N]< / span >
io(io_i) = linio([mdl, < span class = "org-string" > '/Relative Motion Sensor'< / span > ], 1, < span class = "org-string" > 'openoutput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1; < span class = "org-comment" > % Relative Displacement Outputs [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the linearization< / span > < / span >
G = linearize(mdl, io);
G.InputName = {< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > };
G.OutputName = {< span class = "org-string" > '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-org2223469" class = "outline-3" >
< h3 id = "org2223469" > < span class = "section-number-3" > 2.4< / span > Diagonal Control - Leg’ s Frame< / h3 >
< div class = "outline-text-3" id = "text-2-4" >
< p >
< a id = "org31fd942" > < / a >
< / p >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org42dc407" class = "outline-4" >
< h4 id = "org42dc407" > < span class = "section-number-4" > 2.4.1< / span > Control Architecture< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-4-1" >
< p >
The pose error \(\bm{\epsilon}_\mathcal{X}\) is first converted in the frame of the leg by using the Jacobian matrix.
Then a diagonal controller \(\bm{K}_\mathcal{L}\) is designed.
2020-03-16 11:23:15 +01:00
The final implemented controller is \(\bm{K} = \bm{K}_\mathcal{L} \cdot \bm{J}\) as shown in Figure < a href = "#orgb1f5ad2" > 9< / a >
2020-03-11 19:00:22 +01:00
< / p >
< p >
Note here that the transformation from the pose error \(\bm{\epsilon}_\mathcal{X}\) to the leg’ s length errors by using the Jacobian matrix is only valid for small errors.
< / p >
< div id = "orgb1f5ad2" class = "figure" >
< p > < img src = "figs/centralized_reference_tracking_L.png" alt = "centralized_reference_tracking_L.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 9: < / span > Controller in the frame of the legs< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org33774e9" class = "outline-4" >
< h4 id = "org33774e9" > < span class = "section-number-4" > 2.4.2< / span > Plant Analysis< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-4-2" >
< p >
2020-03-16 11:23:15 +01:00
We now multiply the plant by the Jacobian matrix as shown in Figure < a href = "#orgb1f5ad2" > 9< / a > to obtain a more diagonal plant.
2020-03-11 19:00:22 +01:00
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Gl = stewart.kinematics.J< span class = "org-type" > *< / span > G;
Gl.OutputName = {< span class = "org-string" > 'D1'< / span > , < span class = "org-string" > 'D2'< / span > , < span class = "org-string" > 'D3'< / span > , < span class = "org-string" > 'D4'< / span > , < span class = "org-string" > 'D5'< / span > , < span class = "org-string" > 'D6'< / span > };
< / pre >
< / div >
< p >
2020-03-16 11:23:15 +01:00
The bode plot of the plant is shown in Figure < a href = "#org6c8d99f" > 10< / a > .
We can see that the diagonal elements are identical.
2020-03-11 19:00:22 +01:00
This will simplify the design of the controller as all the elements of the diagonal controller can be made identical.
< / p >
2020-03-16 11:23:15 +01:00
< div id = "org6c8d99f" class = "figure" >
< p > < img src = "figs/plant_centralized_L.png" alt = "plant_centralized_L.png" / >
2020-03-11 19:00:22 +01:00
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 10: < / span > Diagonal and off-diagonal elements of the plant \(\bm{K}\bm{G}\) (< a href = "./figs/plant_centralized_L.png" > png< / a > , < a href = "./figs/plant_centralized_L.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< p >
We can see that this < b > totally decouples the system at low frequency< / b > .
< / p >
< p >
This was expected since:
\[ \bm{G}(\omega = 0) = \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) = \bm{J}^{-1} \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \]
< / p >
< p >
Thus \(J \cdot G(\omega = 0) = J \cdot \frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)\) is a diagonal matrix containing the inverse of the joint’ s stiffness.
< / p >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgc7ddab1" class = "outline-4" >
< h4 id = "orgc7ddab1" > < span class = "section-number-4" > 2.4.3< / span > Controller Design< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-4-3" >
< p >
The controller consists of:
< / p >
< ul class = "org-ul" >
< li > A pure integrator< / li >
< li > A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin< / li >
< / ul >
< p >
2020-03-16 11:23:15 +01:00
The obtained loop gains corresponding to the diagonal elements are shown in Figure < a href = "#orga803083" > 11< / a > .
2020-03-11 19:00:22 +01:00
< / 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 > 30;
Kl = diag(1< span class = "org-type" > ./< / span > diag(abs(freqresp(Gl, wc)))) < span class = "org-type" > *< / span > wc< span class = "org-type" > /< / span > s < span class = "org-type" > *< / span > 1< span class = "org-type" > /< / span > (1 < span class = "org-type" > +< / span > s< span class = "org-type" > /< / span > 3< span class = "org-type" > /< / span > wc);
< / pre >
< / div >
< div id = "orga803083" class = "figure" >
< p > < img src = "figs/loop_gain_centralized_L.png" alt = "loop_gain_centralized_L.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 11: < / span > Loop Gain of the diagonal elements (< a href = "./figs/loop_gain_centralized_L.png" > png< / a > , < a href = "./figs/loop_gain_centralized_L.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< p >
The controller \(\bm{K} = \bm{K}_\mathcal{L} \bm{J}\) is computed.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > K = Kl< span class = "org-type" > *< / span > stewart.kinematics.J;
< / pre >
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org3021cf3" class = "outline-4" >
< h4 id = "org3021cf3" > < span class = "section-number-4" > 2.4.4< / span > Simulation< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-4-4" >
< p >
We specify the reference path to follow.
< / p >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > t = [0< span class = "org-type" > :< / span > 1e< span class = "org-type" > -< / span > 4< span class = "org-type" > :< / span > 10];
2020-03-11 19:00:22 +01:00
r = zeros(6, length(t));
2020-03-13 13:12:29 +01:00
r(1, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > sin(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(2, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > cos(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(3, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t;
2020-03-11 19:00:22 +01:00
references = initializeReferences(stewart, < span class = "org-string" > 't'< / span > , t, < span class = "org-string" > 'r'< / span > , r);
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > controller = initializeController(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'ref-track-X'< / span > );
< / pre >
< / div >
< p >
We run the simulation and we save the results.
< / p >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > < span class = "org-matlab-simulink-keyword" > set_param< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > , < span class = "org-string" > 'StopTime'< / span > , < span class = "org-string" > '10'< / span > )
< span class = "org-matlab-simulink-keyword" > sim< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > )
2020-03-11 19:00:22 +01:00
simout_L = simout;
2020-03-13 13:12:29 +01:00
save(< span class = "org-string" > './mat/control_tracking.mat'< / span > , < span class = "org-string" > 'simout_L'< / span > , < span class = "org-string" > '-append'< / span > );
2020-03-11 19:00:22 +01:00
< / pre >
< / div >
< / div >
< / div >
< / div >
< div id = "outline-container-org26a8857" class = "outline-3" >
< h3 id = "org26a8857" > < span class = "section-number-3" > 2.5< / span > Diagonal Control - Cartesian Frame< / h3 >
< div class = "outline-text-3" id = "text-2-5" >
< p >
< a id = "orgfd201c3" > < / a >
< / p >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org185190b" class = "outline-4" >
< h4 id = "org185190b" > < span class = "section-number-4" > 2.5.1< / span > Control Architecture< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-5-1" >
< p >
2020-03-16 11:23:15 +01:00
A diagonal controller \(\bm{K}_\mathcal{X}\) take the pose error \(\bm{\epsilon}_\mathcal{X}\) and generate cartesian forces \(\bm{\mathcal{F}}\) that are then converted to actuators forces using the Jacobian as shown in Figure e < a href = "#org6b158db" > 12< / a > .
2020-03-11 19:00:22 +01:00
< / p >
< p >
The final implemented controller is \(\bm{K} = \bm{J}^{-T} \cdot \bm{K}_\mathcal{X}\).
< / p >
< div id = "org6b158db" class = "figure" >
< p > < img src = "figs/centralized_reference_tracking_X.png" alt = "centralized_reference_tracking_X.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 12: < / span > Controller in the cartesian frame< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgac4e8f3" class = "outline-4" >
< h4 id = "orgac4e8f3" > < span class = "section-number-4" > 2.5.2< / span > Plant Analysis< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-5-2" >
< p >
2020-03-16 11:23:15 +01:00
We now multiply the plant by the Jacobian matrix as shown in Figure < a href = "#org6b158db" > 12< / a > to obtain a more diagonal plant.
2020-03-11 19:00:22 +01:00
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Gx = G< span class = "org-type" > *< / span > inv(stewart.kinematics.J< span class = "org-type" > '< / span > );
Gx.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 >
2020-03-16 11:23:15 +01:00
< div id = "org0173211" class = "figure" >
< p > < img src = "figs/plant_centralized_X.png" alt = "plant_centralized_X.png" / >
2020-03-11 19:00:22 +01:00
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 13: < / span > Diagonal and off-diagonal elements of the plant \(\bm{G} \bm{J}^{-T}\) (< a href = "./figs/plant_centralized_X.png" > png< / a > , < a href = "./figs/plant_centralized_X.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< p >
The diagonal terms are not the same.
The resonances of the system are “ decoupled” .
For instance, the vertical resonance of the system is only present on the diagonal term corresponding to \(D_z/\mathcal{F}_z\).
< / p >
< p >
Here the system is almost decoupled at all frequencies except for the transfer functions \(\frac{R_y}{\mathcal{F}_x}\) and \(\frac{R_x}{\mathcal{F}_y}\).
< / p >
< p >
This is due to the fact that the compliance matrix of the Stewart platform is not diagonal.
< / p >
< table border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< colgroup >
< col class = "org-right" / >
< col class = "org-right" / >
< col class = "org-right" / >
< col class = "org-right" / >
< col class = "org-right" / >
< col class = "org-right" / >
< / colgroup >
< tbody >
< tr >
< td class = "org-right" > 4.75e-08< / td >
< td class = "org-right" > -1.9751e-24< / td >
< td class = "org-right" > 7.3536e-25< / td >
< td class = "org-right" > 5.915e-23< / td >
< td class = "org-right" > 3.2093e-07< / td >
< td class = "org-right" > 5.8696e-24< / td >
< / tr >
< tr >
< td class = "org-right" > -7.1302e-25< / td >
< td class = "org-right" > 4.75e-08< / td >
< td class = "org-right" > 2.8866e-25< / td >
< td class = "org-right" > -3.2093e-07< / td >
< td class = "org-right" > -5.38e-24< / td >
< td class = "org-right" > -3.2725e-23< / td >
< / tr >
< tr >
< td class = "org-right" > 7.9012e-26< / td >
< td class = "org-right" > -6.3991e-25< / td >
< td class = "org-right" > 2.099e-08< / td >
< td class = "org-right" > 1.9073e-23< / td >
< td class = "org-right" > 5.3384e-25< / td >
< td class = "org-right" > -6.4867e-40< / td >
< / tr >
< tr >
< td class = "org-right" > 1.3724e-23< / td >
< td class = "org-right" > -3.2093e-07< / td >
< td class = "org-right" > 1.2799e-23< / td >
< td class = "org-right" > 5.1863e-06< / td >
< td class = "org-right" > 4.9412e-22< / td >
< td class = "org-right" > -3.8269e-24< / td >
< / tr >
< tr >
< td class = "org-right" > 3.2093e-07< / td >
< td class = "org-right" > 7.6013e-24< / td >
< td class = "org-right" > 1.2239e-23< / td >
< td class = "org-right" > 6.8886e-22< / td >
< td class = "org-right" > 5.1863e-06< / td >
< td class = "org-right" > -2.6025e-22< / td >
< / tr >
< tr >
< td class = "org-right" > 7.337e-24< / td >
< td class = "org-right" > -3.2395e-23< / td >
< td class = "org-right" > -1.571e-39< / td >
< td class = "org-right" > 9.927e-23< / td >
< td class = "org-right" > -3.2531e-22< / td >
< td class = "org-right" > 1.7073e-06< / td >
< / tr >
< / tbody >
< / table >
< p >
One way to have this compliance matrix diagonal (and thus having a decoupled plant at DC) is to use a < b > cubic architecture< / b > with the center of the cube’ s coincident with frame \(\{A\}\).
< / p >
< p >
This control architecture can also give a dynamically decoupled plant if the Center of mass of the payload is also coincident with frame \(\{A\}\).
< / p >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org87c1a48" class = "outline-4" >
< h4 id = "org87c1a48" > < span class = "section-number-4" > 2.5.3< / span > Controller Design< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-5-3" >
< p >
The controller consists of:
< / p >
< ul class = "org-ul" >
< li > A pure integrator< / li >
< li > A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin< / li >
< / ul >
< p >
2020-03-16 11:23:15 +01:00
The obtained loop gains corresponding to the diagonal elements are shown in Figure < a href = "#org9051c86" > 14< / a > .
2020-03-11 19:00:22 +01:00
< / 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 > 30;
Kx = diag(1< span class = "org-type" > ./< / span > diag(abs(freqresp(Gx, wc)))) < span class = "org-type" > *< / span > wc< span class = "org-type" > /< / span > s < span class = "org-type" > *< / span > 1< span class = "org-type" > /< / span > (1 < span class = "org-type" > +< / span > s< span class = "org-type" > /< / span > 3< span class = "org-type" > /< / span > wc);
< / pre >
< / div >
< div id = "org9051c86" class = "figure" >
< p > < img src = "figs/loop_gain_centralized_X.png" alt = "loop_gain_centralized_X.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 14: < / span > Loop Gain of the diagonal elements (< a href = "./figs/loop_gain_centralized_X.png" > png< / a > , < a href = "./figs/loop_gain_centralized_X.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< p >
The controller \(\bm{K} = \bm{J}^{-T} \bm{K}_\mathcal{X}\) is computed.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > K = inv(stewart.kinematics.J< span class = "org-type" > '< / span > )< span class = "org-type" > *< / span > Kx;
< / pre >
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org73875ca" class = "outline-4" >
< h4 id = "org73875ca" > < span class = "section-number-4" > 2.5.4< / span > Simulation< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-5-4" >
< p >
We specify the reference path to follow.
< / p >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > t = [0< span class = "org-type" > :< / span > 1e< span class = "org-type" > -< / span > 4< span class = "org-type" > :< / span > 10];
2020-03-11 19:00:22 +01:00
r = zeros(6, length(t));
2020-03-13 13:12:29 +01:00
r(1, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > sin(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(2, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > cos(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(3, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t;
2020-03-11 19:00:22 +01:00
references = initializeReferences(stewart, < span class = "org-string" > 't'< / span > , t, < span class = "org-string" > 'r'< / span > , r);
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > controller = initializeController(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'ref-track-X'< / span > );
< / pre >
< / div >
< p >
We run the simulation and we save the results.
< / p >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > < span class = "org-matlab-simulink-keyword" > set_param< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > , < span class = "org-string" > 'StopTime'< / span > , < span class = "org-string" > '10'< / span > )
< span class = "org-matlab-simulink-keyword" > sim< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > )
2020-03-11 19:00:22 +01:00
simout_X = simout;
2020-03-13 13:12:29 +01:00
save(< span class = "org-string" > './mat/control_tracking.mat'< / span > , < span class = "org-string" > 'simout_X'< / span > , < span class = "org-string" > '-append'< / span > );
2020-03-11 19:00:22 +01:00
< / pre >
< / div >
< / div >
< / div >
< / div >
< div id = "outline-container-orgad7bc54" class = "outline-3" >
< h3 id = "orgad7bc54" > < span class = "section-number-3" > 2.6< / span > Diagonal Control - Steady State Decoupling< / h3 >
< div class = "outline-text-3" id = "text-2-6" >
< p >
< a id = "org789ba4a" > < / a >
< / p >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgdeee29c" class = "outline-4" >
< h4 id = "orgdeee29c" > < span class = "section-number-4" > 2.6.1< / span > Control Architecture< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-6-1" >
< p >
The plant \(\bm{G}\) is pre-multiply by \(\bm{G}^{-1}(\omega = 0)\) such that the “ shaped plant” \(\bm{G}_0 = \bm{G} \bm{G}^{-1}(\omega = 0)\) is diagonal at low frequency.
< / p >
< p >
Then a diagonal controller \(\bm{K}_0\) is designed.
< / p >
< p >
2020-03-16 11:23:15 +01:00
The control architecture is shown in Figure < a href = "#orgb226e44" > 15< / a > .
2020-03-11 19:00:22 +01:00
< / p >
< div id = "orgb226e44" class = "figure" >
< p > < img src = "figs/centralized_reference_tracking_S.png" alt = "centralized_reference_tracking_S.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 15: < / span > Static Decoupling of the Plant< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org4c98210" class = "outline-4" >
< h4 id = "org4c98210" > < span class = "section-number-4" > 2.6.2< / span > Plant Analysis< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-6-2" >
< p >
The plant is pre-multiplied by \(\bm{G}^{-1}(\omega = 0)\).
2020-03-16 11:23:15 +01:00
The diagonal and off-diagonal elements of the shaped plant are shown in Figure < a href = "#org0b73eca" > 16< / a > .
2020-03-11 19:00:22 +01:00
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > G0 = G< span class = "org-type" > *< / span > inv(freqresp(G, 0));
< / pre >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "org0b73eca" class = "figure" >
< p > < img src = "figs/plant_centralized_SD.png" alt = "plant_centralized_SD.png" / >
2020-03-11 19:00:22 +01:00
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 16: < / span > Diagonal and off-diagonal elements of the plant \(\bm{G} \bm{G}^{-1}(\omega = 0)\) (< a href = "./figs/plant_centralized_SD.png" > png< / a > , < a href = "./figs/plant_centralized_SD.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgbf66d4c" class = "outline-4" >
< h4 id = "orgbf66d4c" > < span class = "section-number-4" > 2.6.3< / span > Controller Design< / h4 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-4" id = "text-2-6-3" >
< p >
We have that:
\[ \bm{G}^{-1}(\omega = 0) = \left(\frac{\delta\bm{\mathcal{X}}}{\delta\bm{\tau}}(\omega = 0)\right)^{-1} = \left( \bm{J}^{-1} \frac{\delta\bm{\mathcal{L}}}{\delta\bm{\tau}}(\omega = 0) \right)^{-1} = \text{diag}(\mathcal{K}_1^{-1} \ \dots \ \mathcal{K}_6^{-1}) \bm{J} \]
< / p >
< p >
And because:
< / p >
< ul class = "org-ul" >
< li > all the leg stiffness are equal< / li >
< li > the controller equal to a \(\bm{K}_0(s) = k(s) \bm{I}_6\)< / li >
< / ul >
< p >
We have that \(\bm{K}_0(s)\) commutes with \(\bm{G}^{-1}(\omega = 0)\) and thus the overall controller \(\bm{K}\) is the same as the one obtain in section < a href = "#org31fd942" > 2.4< / a > .
< / p >
< / div >
< / div >
< / div >
< div id = "outline-container-orga2eadeb" class = "outline-3" >
< h3 id = "orga2eadeb" > < span class = "section-number-3" > 2.7< / span > Comparison< / h3 >
< div class = "outline-text-3" id = "text-2-7" >
< / div >
< div id = "outline-container-org09ae901" class = "outline-4" >
< h4 id = "org09ae901" > < span class = "section-number-4" > 2.7.1< / span > Obtained MIMO Controllers< / h4 >
< div class = "outline-text-4" id = "text-2-7-1" >
< div id = "orgf4c7f15" class = "figure" >
< p > < img src = "figs/centralized_control_comp_K.png" alt = "centralized_control_comp_K.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 17: < / span > Comparison of the MIMO controller \(\bm{K}\) for both centralized architectures (< a href = "./figs/centralized_control_comp_K.png" > png< / a > , < a href = "./figs/centralized_control_comp_K.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
< div id = "outline-container-org23ae479" class = "outline-4" >
< h4 id = "org23ae479" > < span class = "section-number-4" > 2.7.2< / span > Simulation Results< / h4 >
< div class = "outline-text-4" id = "text-2-7-2" >
< p >
2020-03-16 11:23:15 +01:00
The position error \(\bm{\epsilon}_\mathcal{X}\) for both centralized architecture are shown in Figure < a href = "#org9fa8c8a" > 18< / a > .
2020-03-11 19:00:22 +01:00
< / p >
< p >
2020-03-16 11:23:15 +01:00
Based on Figure < a href = "#org9fa8c8a" > 18< / a > , we can see that:
2020-03-11 19:00:22 +01:00
< / p >
< ul class = "org-ul" >
< li > There is some tracking error \(\epsilon_x\)< / li >
< li > The errors \(\epsilon_y\), \(\epsilon_{R_x}\) and \(\epsilon_{R_z}\) are quite negligible< / li >
< li > There is some error in the vertical position \(\epsilon_z\).
The frequency of the error \(\epsilon_z\) is twice the frequency of the reference path \(r_x\).< / li >
< li > There is some error \(\epsilon_{R_y}\).
This error is much lower when using the diagonal control in the frame of the leg instead of the cartesian frame.< / li >
< / ul >
< div id = "org9fa8c8a" class = "figure" >
< p > < img src = "figs/centralized_control_comp_Ex.png" alt = "centralized_control_comp_Ex.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 18: < / span > Comparison of the position error \(\bm{\epsilon}_\mathcal{X}\) for both centralized controllers (< a href = "./figs/centralized_control_comp_Ex.png" > png< / a > , < a href = "./figs/centralized_control_comp_Ex.pdf" > pdf< / a > )< / p >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgd2764a2" class = "outline-3" >
< h3 id = "orgd2764a2" > < span class = "section-number-3" > 2.8< / span > Conclusion< / h3 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-3" id = "text-2-8" >
< p >
Both control architecture gives similar results even tough the control in the Leg’ s frame gives slightly better results.
< / p >
< p >
The main differences between the control architectures used in sections < a href = "#org31fd942" > 2.4< / a > and < a href = "#orgfd201c3" > 2.5< / a > are summarized in Table < a href = "#orgb1c0d5b" > 1< / a > .
< / p >
< table id = "orgb1c0d5b" border = "2" cellspacing = "0" cellpadding = "6" rules = "groups" frame = "hsides" >
< caption class = "t-above" > < span class = "table-number" > Table 1:< / span > Comparison of the two centralized control architectures< / caption >
< colgroup >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< col class = "org-left" / >
< / colgroup >
< thead >
< tr >
< th scope = "col" class = "org-left" >   < / th >
< th scope = "col" class = "org-left" > < b > Leg’ s Frame< / b > < / th >
< th scope = "col" class = "org-left" > < b > Cartesian Frame< / b > < / th >
< th scope = "col" class = "org-left" > < b > Static Decoupling< / b > < / th >
< / tr >
< / thead >
< tbody >
< tr >
< td class = "org-left" > < b > Plant Meaning< / b > < / td >
< td class = "org-left" > \(\delta\mathcal{L}_i/\tau_i\)< / td >
< td class = "org-left" > \(\delta\mathcal{X}_i/\mathcal{F}_i\)< / td >
< td class = "org-left" > No physical meaning< / td >
< / tr >
< tr >
< td class = "org-left" > < b > Obtained Decoupling< / b > < / td >
< td class = "org-left" > Decoupled at DC< / td >
< td class = "org-left" > Dynamical decoupling except few terms< / td >
< td class = "org-left" > Decoupled at DC< / td >
< / tr >
< tr >
< td class = "org-left" > < b > Diagonal Elements< / b > < / td >
< td class = "org-left" > Identical with all the Resonances< / td >
< td class = "org-left" > Different, resonances are cancel out< / td >
< td class = "org-left" > No Alternating poles and zeros< / td >
< / tr >
< tr >
< td class = "org-left" > < b > Mechanical Architecture< / b > < / td >
< td class = "org-left" > Architecture Independent< / td >
< td class = "org-left" > Better with Cubic Architecture< / td >
< td class = "org-left" >   < / td >
< / tr >
< tr >
< td class = "org-left" > < b > Robustness to Uncertainty< / b > < / td >
< td class = "org-left" > Good (only depends on \(J\))< / td >
< td class = "org-left" > Good (only depends on \(J\))< / td >
< td class = "org-left" > Bad (depends on the mass)< / td >
< / tr >
< / tbody >
< / table >
< p >
These decoupling methods only uses the Jacobian matrix which only depends on the Stewart platform geometry.
Thus, this method should be quite robust against parameter variation (e.g. the payload mass).
< / p >
< / div >
< / div >
< / div >
< div id = "outline-container-org4b8c360" class = "outline-2" >
< h2 id = "org4b8c360" > < span class = "section-number-2" > 3< / span > Hybrid Control Architecture - HAC-LAC with relative DVF< / h2 >
< div class = "outline-text-2" id = "text-3" >
< p >
< a id = "org14e3e5f" > < / a >
< / p >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org539565c" class = "outline-3" >
< h3 id = "org539565c" > < span class = "section-number-3" > 3.1< / span > Control Schematic< / h3 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-3" id = "text-3-1" >
2020-03-12 18:06:56 +01:00
< p >
2020-03-16 11:23:15 +01:00
Let’ s consider the control schematic shown in Figure < a href = "#org3a1b1db" > 19< / a > .
2020-03-12 18:06:56 +01:00
< / p >
< p >
The first loop containing \(\bm{K}_\mathcal{L}\) is a Decentralized Direct (Relative) Velocity Feedback.
< / p >
< p >
A reference \(\bm{r}_\mathcal{L}\) is computed using the inverse kinematics and corresponds to the wanted motion of each leg.
The actual length of each leg \(\bm{\mathcal{L}}\) is subtracted and then passed trough the controller \(\bm{K}_\mathcal{L}\).
< / p >
< p >
The controller is a diagonal controller with pure derivative action on the diagonal.
< / p >
< p >
The effect of this loop is:
< / p >
< ul class = "org-ul" >
< li > it adds damping to the system (the force applied in each actuator is proportional to the relative velocity of the strut)< / li >
< li > it however does not go “ against” the reference path \(\bm{r}_\mathcal{X}\) thanks to the use of the inverse kinematics< / li >
< / ul >
< p >
Then, the second loop containing \(\bm{K}_\mathcal{X}\) is designed based on the already damped plant (represented by the gray area).
This second loop is responsible for the reference tracking.
< / p >
2020-03-11 19:00:22 +01:00
< div id = "org3a1b1db" class = "figure" >
< p > < img src = "figs/hybrid_reference_tracking.png" alt = "hybrid_reference_tracking.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 19: < / span > Hybrid Control Architecture< / p >
2020-03-12 18:06:56 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orga9bdd4e" class = "outline-3" >
< h3 id = "orga9bdd4e" > < span class = "section-number-3" > 3.2< / span > Initialize the Stewart platform< / h3 >
2020-03-12 18:06:56 +01:00
< div class = "outline-text-3" id = "text-3-2" >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > < span class = "org-comment" > % Stewart Platform< / span >
stewart = initializeStewartPlatform();
2020-03-12 18:06:56 +01:00
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_p'< / span > , < span class = "org-string" > 'type_M'< / span > , < span class = "org-string" > 'spherical_p'< / 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" > 'accelerometer'< / span > , < span class = "org-string" > 'freq'< / span > , 5e3);
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Ground and Payload< / span >
ground = initializeGround(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'rigid'< / span > );
2020-03-12 18:06:56 +01:00
payload = initializePayload(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'none'< / span > );
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Controller< / span >
2020-03-12 18:06:56 +01:00
controller = initializeController(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'open-loop'< / span > );
2020-03-13 13:12:29 +01:00
< span class = "org-comment" > % Disturbances and References< / span >
disturbances = initializeDisturbances();
2020-03-12 18:06:56 +01:00
references = initializeReferences(stewart);
< / pre >
< / div >
< / div >
< / div >
< div id = "outline-container-org3274a98" class = "outline-3" >
< h3 id = "org3274a98" > < span class = "section-number-3" > 3.3< / span > First Control Loop - \(\bm{K}_\mathcal{L}\)< / h3 >
< div class = "outline-text-3" id = "text-3-3" >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgd69b37c" class = "outline-4" >
< h4 id = "orgd69b37c" > < span class = "section-number-4" > 3.3.1< / span > Identification< / h4 >
2020-03-12 18:06:56 +01:00
< div class = "outline-text-4" id = "text-3-3-1" >
< p >
Let’ s identify the transfer function from \(\bm{\tau}\) to \(\bm{L}\).
< / p >
< 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" > 'openinput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1; < span class = "org-comment" > % Actuator Force Inputs [N]< / span >
io(io_i) = linio([mdl, < span class = "org-string" > '/Stewart Platform'< / span > ], 1, < span class = "org-string" > 'openoutput'< / span > , [], < span class = "org-string" > 'dLm'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1; < span class = "org-comment" > % Relative Displacement Outputs [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the linearization< / span > < / span >
Gl = linearize(mdl, io);
Gl.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 > };
Gl.OutputName = {< span class = "org-string" > 'L1'< / span > , < span class = "org-string" > 'L2'< / span > , < span class = "org-string" > 'L3'< / span > , < span class = "org-string" > 'L4'< / span > , < span class = "org-string" > 'L5'< / span > , < span class = "org-string" > 'L6'< / span > };
< / pre >
2020-03-11 19:00:22 +01:00
< / div >
< / div >
< / div >
2020-03-12 18:06:56 +01:00
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org2a634da" class = "outline-4" >
< h4 id = "org2a634da" > < span class = "section-number-4" > 3.3.2< / span > Obtained Plant< / h4 >
2020-03-12 18:06:56 +01:00
< div class = "outline-text-4" id = "text-3-3-2" >
< p >
2020-03-16 11:23:15 +01:00
The obtained plant is shown in Figure < a href = "#orgf627577" > 20< / a > .
2020-03-12 18:06:56 +01:00
< / p >
2020-03-11 19:00:22 +01:00
2020-03-16 11:23:15 +01:00
< div id = "orgf627577" class = "figure" >
< p > < img src = "figs/hybrid_control_Kl_plant.png" alt = "hybrid_control_Kl_plant.png" / >
2020-03-12 18:06:56 +01:00
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 20: < / span > Diagonal and off-diagonal elements of the plant for the design of \(\bm{K}_\mathcal{L}\) (< a href = "./figs/hybrid_control_Kl_plant.png" > png< / a > , < a href = "./figs/hybrid_control_Kl_plant.pdf" > pdf< / a > )< / p >
2020-03-12 18:06:56 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org23695fa" class = "outline-4" >
< h4 id = "org23695fa" > < span class = "section-number-4" > 3.3.3< / span > Controller Design< / h4 >
2020-03-12 18:06:56 +01:00
< div class = "outline-text-4" id = "text-3-3-3" >
< p >
We apply a decentralized (diagonal) direct velocity feedback.
Thus, we apply a pure derivative action.
In order to make the controller realizable, we add a low pass filter at high frequency.
The gain of the controller is chosen such that the resonances are critically damped.
< / p >
< p >
2020-03-16 11:23:15 +01:00
The obtain loop gain is shown in Figure < a href = "#orgb74befe" > 21< / a > .
2020-03-12 18:06:56 +01:00
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Kl = 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 > 1e4) < span class = "org-type" > *< / span > eye(6);
< / pre >
< / div >
< div id = "orgb74befe" class = "figure" >
< p > < img src = "figs/hybrid_control_Kl_loop_gain.png" alt = "hybrid_control_Kl_loop_gain.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 21: < / span > Obtain Loop Gain for the DVF control loop (< a href = "./figs/hybrid_control_Kl_loop_gain.png" > png< / a > , < a href = "./figs/hybrid_control_Kl_loop_gain.pdf" > pdf< / a > )< / p >
2020-03-12 18:06:56 +01:00
< / div >
< / div >
< / div >
< / div >
< div id = "outline-container-org8440c0b" class = "outline-3" >
< h3 id = "org8440c0b" > < span class = "section-number-3" > 3.4< / span > Second Control Loop - \(\bm{K}_\mathcal{X}\)< / h3 >
< div class = "outline-text-3" id = "text-3-4" >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgeec6c35" class = "outline-4" >
< h4 id = "orgeec6c35" > < span class = "section-number-4" > 3.4.1< / span > Identification< / h4 >
2020-03-12 18:06:56 +01:00
< div class = "outline-text-4" id = "text-3-4-1" >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Kx = tf(zeros(6));
controller = initializeController(< span class = "org-string" > 'type'< / span > , < span class = "org-string" > 'ref-track-hac-dvf'< / 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" > '/Relative Motion Sensor'< / span > ], 1, < span class = "org-string" > 'openoutput'< / span > ); io_i = io_i < span class = "org-type" > +< / span > 1; < span class = "org-comment" > % Relative Displacement Outputs [m]< / span >
< span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Run the linearization< / span > < / span >
G = linearize(mdl, io);
G.InputName = {< span class = "org-string" > 'F1'< / span > , < span class = "org-string" > 'F2'< / span > , < span class = "org-string" > 'F3'< / span > , < span class = "org-string" > 'F4'< / span > , < span class = "org-string" > 'F5'< / span > , < span class = "org-string" > 'F6'< / span > };
G.OutputName = {< span class = "org-string" > '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 >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org57836ee" class = "outline-4" >
< h4 id = "org57836ee" > < span class = "section-number-4" > 3.4.2< / span > Obtained Plant< / h4 >
2020-03-12 18:06:56 +01:00
< div class = "outline-text-4" id = "text-3-4-2" >
< p >
We use the Jacobian matrix to apply forces in the cartesian frame.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Gx = G< span class = "org-type" > *< / span > inv(stewart.kinematics.J< span class = "org-type" > '< / span > );
Gx.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 >
2020-03-16 11:23:15 +01:00
The obtained plant is shown in Figure < a href = "#org2517e3d" > 22< / a > .
2020-03-12 18:06:56 +01:00
< / p >
< div id = "org2517e3d" class = "figure" >
< p > < img src = "figs/hybrid_control_Kx_plant.png" alt = "hybrid_control_Kx_plant.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 22: < / span > Diagonal and Off-diagonal elements of the plant for the design of \(\bm{K}_\mathcal{L}\) (< a href = "./figs/hybrid_control_Kx_plant.png" > png< / a > , < a href = "./figs/hybrid_control_Kx_plant.pdf" > pdf< / a > )< / p >
2020-03-12 18:06:56 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-orgcebb0d5" class = "outline-4" >
< h4 id = "orgcebb0d5" > < span class = "section-number-4" > 3.4.3< / span > Controller Design< / h4 >
2020-03-12 18:06:56 +01:00
< div class = "outline-text-4" id = "text-3-4-3" >
< p >
The controller consists of:
< / p >
< ul class = "org-ul" >
< li > A pure integrator< / li >
2020-03-13 13:23:16 +01:00
< li > A Second integrator up to half the wanted bandwidth< / li >
< li > A Lead around the cross-over frequency< / li >
< li > A low pass filter with a cut-off equal to three times the wanted bandwidth< / li >
2020-03-12 18:06:56 +01:00
< / ul >
< 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 > 200; < span class = "org-comment" > % Bandwidth Bandwidth [rad/s]< / span >
h = 3; < span class = "org-comment" > % Lead parameter< / span >
2020-03-13 13:23:16 +01:00
Kx = (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) < span class = "org-type" > *< / span > wc< span class = "org-type" > /< / span > s < span class = "org-type" > *< / span > ((s< span class = "org-type" > /< / span > wc< span class = "org-type" > *< / span > 2 < span class = "org-type" > +< / span > 1)< span class = "org-type" > /< / span > (s< span class = "org-type" > /< / span > wc< span class = "org-type" > *< / span > 2)) < span class = "org-type" > *< / span > (1< span class = "org-type" > /< / span > (1 < span class = "org-type" > +< / span > s< span class = "org-type" > /< / span > wc< span class = "org-type" > /< / span > 3));
2020-03-12 18:06:56 +01:00
< span class = "org-comment" > % Normalization of the gain of have a loop gain of 1 at frequency wc< / span >
Kx = Kx< span class = "org-type" > .*< / span > diag(1< span class = "org-type" > ./< / span > diag(abs(freqresp(Gx< span class = "org-type" > *< / span > Kx, wc))));
< / pre >
< / div >
< div id = "org30ad867" class = "figure" >
< p > < img src = "figs/hybrid_control_Kx_loop_gain.png" alt = "hybrid_control_Kx_loop_gain.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 23: < / span > Obtained Loop Gain for the controller \(\bm{K}_\mathcal{X}\) (< a href = "./figs/hybrid_control_Kx_loop_gain.png" > png< / a > , < a href = "./figs/hybrid_control_Kx_loop_gain.pdf" > pdf< / a > )< / p >
2020-03-12 18:06:56 +01:00
< / div >
< p >
Then we include the Jacobian in the controller matrix.
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > Kx = inv(stewart.kinematics.J< span class = "org-type" > '< / span > )< span class = "org-type" > *< / span > Kx;
< / pre >
< / div >
< / div >
< / div >
< / div >
< div id = "outline-container-org74d3dcd" class = "outline-3" >
< h3 id = "org74d3dcd" > < span class = "section-number-3" > 3.5< / span > Simulations< / h3 >
< div class = "outline-text-3" id = "text-3-5" >
< p >
We specify the reference path to follow.
< / p >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > t = [0< span class = "org-type" > :< / span > 1e< span class = "org-type" > -< / span > 4< span class = "org-type" > :< / span > 10];
2020-03-12 18:06:56 +01:00
r = zeros(6, length(t));
2020-03-13 13:12:29 +01:00
r(1, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > sin(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(2, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t< span class = "org-type" > .*< / span > cos(2< span class = "org-type" > *< / span > < span class = "org-constant" > pi< / span > < span class = "org-type" > *< / span > t);
r(3, < span class = "org-type" > :< / span > ) = 1e< span class = "org-type" > -< / span > 3< span class = "org-type" > .*< / span > t;
2020-03-12 18:06:56 +01:00
references = initializeReferences(stewart, < span class = "org-string" > 't'< / span > , t, < span class = "org-string" > 'r'< / span > , r);
< / pre >
< / div >
< p >
We run the simulation and we save the results.
< / p >
< div class = "org-src-container" >
2020-03-13 13:12:29 +01:00
< pre class = "src src-matlab" > < span class = "org-matlab-simulink-keyword" > set_param< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > , < span class = "org-string" > 'StopTime'< / span > , < span class = "org-string" > '10'< / span > )
< span class = "org-matlab-simulink-keyword" > sim< / span > (< span class = "org-string" > 'stewart_platform_model'< / span > )
2020-03-12 18:06:56 +01:00
simout_H = simout;
2020-03-13 13:12:29 +01:00
save(< span class = "org-string" > './mat/control_tracking.mat'< / span > , < span class = "org-string" > 'simout_H'< / span > , < span class = "org-string" > '-append'< / span > );
2020-03-12 18:06:56 +01:00
< / pre >
< / div >
< p >
2020-03-16 11:23:15 +01:00
The obtained position error is shown in Figure < a href = "#org19456cf" > 24< / a > .
2020-03-12 18:06:56 +01:00
< / p >
< div id = "org19456cf" class = "figure" >
< p > < img src = "figs/hybrid_control_Ex.png" alt = "hybrid_control_Ex.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 24: < / span > Obtained position error \(\bm{\epsilon}_\mathcal{X}\) (< a href = "./figs/hybrid_control_Ex.png" > png< / a > , < a href = "./figs/hybrid_control_Ex.pdf" > pdf< / a > )< / p >
2020-03-12 18:06:56 +01:00
< / div >
< / div >
< / div >
2020-03-16 11:23:15 +01:00
< div id = "outline-container-org771bea0" class = "outline-3" >
< h3 id = "org771bea0" > < span class = "section-number-3" > 3.6< / span > Conclusion< / h3 >
2020-03-12 18:06:56 +01:00
< / div >
< / div >
2020-03-11 19:00:22 +01:00
2020-03-13 13:12:29 +01:00
< div id = "outline-container-org798d54f" class = "outline-2" >
< h2 id = "org798d54f" > < span class = "section-number-2" > 4< / span > Comparison of all the methods< / h2 >
2020-03-11 19:00:22 +01:00
< div class = "outline-text-2" id = "text-4" >
< p >
2020-03-13 13:12:29 +01:00
< a id = "orgb3403cb" > < / a >
< / p >
< p >
2020-03-16 11:23:15 +01:00
Let’ s load the simulation results and compare them in Figure < a href = "#org6fa53fa" > 25< / a > .
2020-03-13 13:12:29 +01:00
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > load(< span class = "org-string" > './mat/control_tracking.mat'< / span > , < span class = "org-string" > 'simout_D'< / span > , < span class = "org-string" > 'simout_L'< / span > , < span class = "org-string" > 'simout_X'< / span > , < span class = "org-string" > 'simout_H'< / span > );
< / pre >
< / div >
< div id = "org6fa53fa" class = "figure" >
< p > < img src = "figs/reference_tracking_performance_comparison.png" alt = "reference_tracking_performance_comparison.png" / >
< / p >
2020-03-16 11:23:15 +01:00
< p > < span class = "figure-number" > Figure 25: < / span > Comparison of the position errors for all the Control architecture used (< a href = "./figs/reference_tracking_performance_comparison.png" > png< / a > , < a href = "./figs/reference_tracking_performance_comparison.pdf" > pdf< / a > )< / p >
2020-03-13 13:12:29 +01:00
< / div >
< / div >
< / div >
< div id = "outline-container-orgd0502ff" class = "outline-2" >
< h2 id = "orgd0502ff" > < span class = "section-number-2" > 5< / span > Compute the pose error of the Stewart Platform< / h2 >
< div class = "outline-text-2" id = "text-5" >
< p >
2020-03-11 19:00:22 +01:00
< a id = "org5f61540" > < / a >
< / p >
< p >
2020-03-12 18:06:56 +01:00
Let’ s note:
2020-03-11 19:00:22 +01:00
< / p >
< ul class = "org-ul" >
2020-03-12 18:06:56 +01:00
< li > \(\{W\}\) the fixed measurement frame (corresponding to the metrology frame / the frame where the wanted displacement are expressed).
The center of the frame if \(O_W\)< / li >
< li > \(\{M\}\) is the frame fixed to the measured elements.
\(O_M\) is the point where the pose of the element is measured< / li >
< li > \(\{R\}\) is a virtual frame corresponding to the wanted pose of the element.
\(O_R\) is the origin of this frame where the we want to position the point \(O_M\) of the element.< / li >
< li > \(\{V\}\) is a frame which its axes are aligned with \(\{W\}\) and its origin \(O_V\) is coincident with the \(O_M\)< / li >
2020-03-11 19:00:22 +01:00
< / ul >
< p >
Reference Position with respect to fixed frame {W}: \({}^WT_R\)
< / p >
< div class = "org-src-container" >
2020-03-12 18:06:56 +01:00
< pre class = "src src-matlab" > Dxr = 0;
Dyr = 0;
Dzr = 0.1;
Rxr = < span class = "org-constant" > pi< / span > ;
Ryr = 0;
Rzr = 0;
2020-03-11 19:00:22 +01:00
< / pre >
< / div >
< p >
Measured Position with respect to fixed frame {W}: \({}^WT_M\)
< / p >
< div class = "org-src-container" >
2020-03-12 18:06:56 +01:00
< pre class = "src src-matlab" > Dxm = 0;
Dym = 0;
Dzm = 0;
Rxm = < span class = "org-constant" > pi< / span > ;
Rym = 0;
Rzm = 0;
< / pre >
< / div >
2020-03-11 19:00:22 +01:00
2020-03-12 18:06:56 +01:00
< p >
We measure the position and orientation (pose) of the element represented by the frame \(\{M\}\) with respect to frame \(\{W\}\).
Thus we can compute the Homogeneous transformation matrix \({}^WT_M\).
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Measured Pose< / span > < / span >
2020-03-11 19:00:22 +01:00
WTm = zeros(4,4);
2020-03-12 18:06:56 +01:00
WTm(1< span class = "org-type" > :< / span > 3, 1< span class = "org-type" > :< / span > 3) = [cos(Rzm) < span class = "org-type" > -< / span > sin(Rzm) 0;
sin(Rzm) cos(Rzm) 0;
2020-03-11 19:00:22 +01:00
0 0 1] < span class = "org-type" > *< / span > ...
2020-03-12 18:06:56 +01:00
[cos(Rym) 0 sin(Rym);
2020-03-11 19:00:22 +01:00
0 1 0;
2020-03-12 18:06:56 +01:00
< span class = "org-type" > -< / span > sin(Rym) 0 cos(Rym)] < span class = "org-type" > *< / span > ...
2020-03-11 19:00:22 +01:00
[1 0 0;
2020-03-12 18:06:56 +01:00
0 cos(Rxm) < span class = "org-type" > -< / span > sin(Rxm);
0 sin(Rxm) cos(Rxm)];
WTm(1< span class = "org-type" > :< / span > 4, 4) = [Dxm ; Dym ; Dzm; 1];
< / pre >
< / div >
2020-03-11 19:00:22 +01:00
2020-03-12 18:06:56 +01:00
< p >
We can also compute the Homogeneous transformation matrix \({}^WT_R\) corresponding to the transformation required to go from fixed frame \(\{W\}\) to the wanted frame \(\{R\}\).
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Reference Pose< / span > < / span >
WTr = zeros(4,4);
2020-03-11 19:00:22 +01:00
2020-03-12 18:06:56 +01:00
WTr(1< span class = "org-type" > :< / span > 3, 1< span class = "org-type" > :< / span > 3) = [cos(Rzr) < span class = "org-type" > -< / span > sin(Rzr) 0;
sin(Rzr) cos(Rzr) 0;
0 0 1] < span class = "org-type" > *< / span > ...
[cos(Ryr) 0 sin(Ryr);
0 1 0;
< span class = "org-type" > -< / span > sin(Ryr) 0 cos(Ryr)] < span class = "org-type" > *< / span > ...
[1 0 0;
0 cos(Rxr) < span class = "org-type" > -< / span > sin(Rxr);
0 sin(Rxr) cos(Rxr)];
WTr(1< span class = "org-type" > :< / span > 4, 4) = [Dxr ; Dyr ; Dzr; 1];
2020-03-11 19:00:22 +01:00
< / pre >
< / div >
< p >
2020-03-12 18:06:56 +01:00
We can also compute \({}^WT_V\).
2020-03-11 19:00:22 +01:00
< / p >
2020-03-12 18:06:56 +01:00
< div class = "org-src-container" >
< pre class = "src src-matlab" > WTv = eye(4);
WTv(1< span class = "org-type" > :< / span > 3, 4) = WTm(1< span class = "org-type" > :< / span > 3, 4);
< / pre >
< / div >
2020-03-11 19:00:22 +01:00
< p >
2020-03-12 18:06:56 +01:00
Now we want to express \({}^MT_R\) which corresponds to the transformation required to go to wanted position expressed in the frame of the measured element.
This homogeneous transformation can be computed from the previously computed matrices:
\[ {}^MT_R = ({{}^WT_M}^{-1}) {}^WT_R \]
2020-03-11 19:00:22 +01:00
< / p >
< div class = "org-src-container" >
2020-03-12 18:06:56 +01:00
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Wanted pose expressed in a frame corresponding to the actual measured pose< / span > < / span >
2020-03-11 19:00:22 +01:00
MTr = [WTm(1< span class = "org-type" > :< / span > 3,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > '< / span > , < span class = "org-type" > -< / span > WTm(1< span class = "org-type" > :< / span > 3,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > '*< / span > WTm(1< span class = "org-type" > :< / span > 3,4) ; 0 0 0 1]< span class = "org-type" > *< / span > WTr;
2020-03-12 18:06:56 +01:00
< / pre >
< / div >
2020-03-11 19:00:22 +01:00
2020-03-12 18:06:56 +01:00
< p >
Now we want to express \({}^VT_R\):
\[ {}^VT_R = ({{}^WT_V}^{-1}) {}^WT_R \]
< / p >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Wanted pose expressed in a frame coincident with the actual position but with no rotation< / span > < / span >
VTr = [WTv(1< span class = "org-type" > :< / span > 3,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > '< / span > , < span class = "org-type" > -< / span > WTv(1< span class = "org-type" > :< / span > 3,1< span class = "org-type" > :< / span > 3)< span class = "org-type" > '*< / span > WTv(1< span class = "org-type" > :< / span > 3,4) ; 0 0 0 1] < span class = "org-type" > *< / span > WTr;
< / pre >
< / div >
< div class = "org-src-container" >
< pre class = "src src-matlab" > < span class = "org-matlab-cellbreak" > < span class = "org-comment" > %% Extract Translations and Rotations from the Homogeneous Matrix< / span > < / span >
2020-03-11 19:00:22 +01:00
T = MTr;
Edx = T(1, 4);
Edy = T(2, 4);
Edz = T(3, 4);
< span class = "org-comment" > % The angles obtained are u-v-w Euler angles (rotations in the moving frame)< / span >
Ery = atan2( T(1, 3), sqrt(T(1, 1)< span class = "org-type" > ^< / span > 2 < span class = "org-type" > +< / span > T(1, 2)< span class = "org-type" > ^< / span > 2));
Erx = atan2(< span class = "org-type" > -< / span > T(2, 3)< span class = "org-type" > /< / span > cos(Ery), T(3, 3)< span class = "org-type" > /< / span > cos(Ery));
Erz = atan2(< span class = "org-type" > -< / span > T(1, 2)< span class = "org-type" > /< / span > cos(Ery), T(1, 1)< span class = "org-type" > /< / span > cos(Ery));
< / pre >
< / div >
< / div >
2020-02-28 17:35:44 +01:00
< / div >
< / div >
< div id = "postamble" class = "status" >
< p class = "author" > Author: Dehaeze Thomas< / p >
2020-03-16 11:23:15 +01:00
< p class = "date" > Created: 2020-03-16 lun. 11:22< / p >
2020-02-28 17:35:44 +01:00
< / div >
< / body >
< / html >
