1366 lines
54 KiB
HTML
1366 lines
54 KiB
HTML
<?xml version="1.0" encoding="utf-8"?>
|
|
<?xml version="1.0" encoding="utf-8"?>
|
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
|
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
|
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
|
<head>
|
|
<!-- 2020-03-11 mer. 19: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>
|
|
<li><a href="#orgd7b25e5">1. Decentralized Control Architecture using Strut Length</a>
|
|
<ul>
|
|
<li><a href="#org49467e8">1.1. Control Schematic</a></li>
|
|
<li><a href="#org67db718">1.2. Initialize the Stewart platform</a></li>
|
|
<li><a href="#org641cba6">1.3. Identification of the plant</a></li>
|
|
<li><a href="#orgd9d7b44">1.4. Plant Analysis</a></li>
|
|
<li><a href="#orgfaf80fa">1.5. Controller Design</a></li>
|
|
<li><a href="#org8c10905">1.6. Simulation</a></li>
|
|
<li><a href="#org974b430">1.7. Results</a></li>
|
|
<li><a href="#org2c54a80">1.8. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orga519721">2. Centralized Control Architecture using Pose Measurement</a>
|
|
<ul>
|
|
<li><a href="#org54419d3">2.1. Control Schematic</a></li>
|
|
<li><a href="#orgfbc962f">2.2. Initialize the Stewart platform</a></li>
|
|
<li><a href="#org5b50e6c">2.3. Identification of the plant</a></li>
|
|
<li><a href="#org2223469">2.4. Diagonal Control - Leg’s Frame</a>
|
|
<ul>
|
|
<li><a href="#org8ff61f7">2.4.1. Control Architecture</a></li>
|
|
<li><a href="#org7d457dc">2.4.2. Plant Analysis</a></li>
|
|
<li><a href="#orge101512">2.4.3. Controller Design</a></li>
|
|
<li><a href="#orgb422b25">2.4.4. Simulation</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org26a8857">2.5. Diagonal Control - Cartesian Frame</a>
|
|
<ul>
|
|
<li><a href="#orgcad4c0f">2.5.1. Control Architecture</a></li>
|
|
<li><a href="#orga0f64cf">2.5.2. Plant Analysis</a></li>
|
|
<li><a href="#orgc665ea1">2.5.3. Controller Design</a></li>
|
|
<li><a href="#org30afd72">2.5.4. Simulation</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgad7bc54">2.6. Diagonal Control - Steady State Decoupling</a>
|
|
<ul>
|
|
<li><a href="#org224a0bb">2.6.1. Control Architecture</a></li>
|
|
<li><a href="#org127af6e">2.6.2. Plant Analysis</a></li>
|
|
<li><a href="#org64fe247">2.6.3. Controller Design</a></li>
|
|
</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>
|
|
<li><a href="#orge3e2e02">2.8. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org4b8c360">3. Hybrid Control Architecture - HAC-LAC with relative DVF</a>
|
|
<ul>
|
|
<li><a href="#orgf22ae1f">3.1. Control Schematic</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org445f7a9">4. Position Error computation</a></li>
|
|
</ul>
|
|
</div>
|
|
</div>
|
|
|
|
<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>
|
|
|
|
<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>
|
|
|
|
<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>
|
|
<div class="outline-text-2" id="text-1">
|
|
<p>
|
|
<a id="orgea7df6c"></a>
|
|
</p>
|
|
</div>
|
|
<div id="outline-container-org49467e8" class="outline-3">
|
|
<h3 id="org49467e8"><span class="section-number-3">1.1</span> Control Schematic</h3>
|
|
<div class="outline-text-3" id="text-1-1">
|
|
<p>
|
|
The control architecture is shown in Figure <a href="#org4f704a1">1</a>.
|
|
</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>
|
|
|
|
<p>
|
|
Then, a diagonal (decentralized) controller \(\bm{K}_\mathcal{L}\) is used such that each leg lengths stays close to its required length.
|
|
</p>
|
|
|
|
|
|
<div id="org4f704a1" class="figure">
|
|
<p><img src="figs/control_measure_rotating_2dof.png" alt="control_measure_rotating_2dof.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 1: </span>Decentralized control for reference tracking</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org67db718" class="outline-3">
|
|
<h3 id="org67db718"><span class="section-number-3">1.2</span> Initialize the Stewart platform</h3>
|
|
<div class="outline-text-3" id="text-1-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, <span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
|
|
stewart = generateGeneralConfiguration(stewart);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart);
|
|
stewart = initializeJointDynamics(stewart, <span class="org-string">'type_F'</span>, <span class="org-string">'universal_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);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>);
|
|
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">disturbances = initializeDisturbances();
|
|
references = initializeReferences(stewart);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org641cba6" class="outline-3">
|
|
<h3 id="org641cba6"><span class="section-number-3">1.3</span> Identification of the plant</h3>
|
|
<div class="outline-text-3" id="text-1-3">
|
|
<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>
|
|
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>
|
|
|
|
<div id="outline-container-orgd9d7b44" class="outline-3">
|
|
<h3 id="orgd9d7b44"><span class="section-number-3">1.4</span> Plant Analysis</h3>
|
|
<div class="outline-text-3" id="text-1-4">
|
|
<p>
|
|
The diagonal terms of the plant is shown in Figure <a href="#org8c82316">2</a>.
|
|
</p>
|
|
|
|
<p>
|
|
All the diagonal terms are equal.
|
|
</p>
|
|
|
|
|
|
<div id="org8c82316" class="figure">
|
|
<p><img src="figs/plant_decentralized_diagonal.png" alt="plant_decentralized_diagonal.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 2: </span>Diagonal Elements of the Plant (<a href="./figs/plant_decentralized_diagonal.png">png</a>, <a href="./figs/plant_decentralized_diagonal.pdf">pdf</a>)</p>
|
|
</div>
|
|
|
|
<p>
|
|
The off-diagonal terms are shown in Figure <a href="#org2ba5482">3</a>.
|
|
</p>
|
|
|
|
<p>
|
|
We see that the plant is decoupled at low frequency which indicate that decentralized control may be a good idea.
|
|
</p>
|
|
|
|
|
|
<div id="org2ba5482" class="figure">
|
|
<p><img src="figs/plant_decentralized_off_diagonal.png" alt="plant_decentralized_off_diagonal.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 3: </span>Diagonal Elements of the Plant (<a href="./figs/plant_decentralized_off_diagonal.png">png</a>, <a href="./figs/plant_decentralized_off_diagonal.pdf">pdf</a>)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfaf80fa" class="outline-3">
|
|
<h3 id="orgfaf80fa"><span class="section-number-3">1.5</span> Controller Design</h3>
|
|
<div class="outline-text-3" id="text-1-5">
|
|
<p>
|
|
The controller consists of:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>A pure integrator</li>
|
|
<li>A lead around the crossover frequency to increase the phase margin</li>
|
|
<li>A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin</li>
|
|
</ul>
|
|
|
|
<p>
|
|
The obtained loop gains corresponding to the diagonal elements are shown in Figure <a href="#org08e591a">4</a>.
|
|
</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" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 4: </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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8c10905" class="outline-3">
|
|
<h3 id="org8c10905"><span class="section-number-3">1.6</span> Simulation</h3>
|
|
<div class="outline-text-3" id="text-1-6">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">t = linspace(0, 10, 1000);
|
|
|
|
r = zeros(6, length(t));
|
|
|
|
r(1, <span class="org-type">:</span>) = 5e<span class="org-type">-</span>3<span class="org-type">*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
|
|
|
|
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-L'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'stewart_platform_model'</span>)
|
|
simout_D = simout;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<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">
|
|
|
|
<div id="org1ac9124" class="figure">
|
|
<p><img src="figs/decentralized_control_Ex.png" alt="decentralized_control_Ex.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 5: </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>
|
|
</div>
|
|
|
|
|
|
<div id="org10eb8ae" class="figure">
|
|
<p><img src="figs/decentralized_control_El.png" alt="decentralized_control_El.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 6: </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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2c54a80" class="outline-3">
|
|
<h3 id="org2c54a80"><span class="section-number-3">1.8</span> Conclusion</h3>
|
|
<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>
|
|
<div id="outline-container-org54419d3" class="outline-3">
|
|
<h3 id="org54419d3"><span class="section-number-3">2.1</span> Control Schematic</h3>
|
|
<div class="outline-text-3" id="text-2-1">
|
|
<p>
|
|
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">7</a>).
|
|
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>
|
|
<li>payload pose \(\bm{\mathcal{X}) = \begin{bmatrix} x & y & z & R_x & R_y & R_z \end{bmatrix}\)</li>
|
|
</ul>
|
|
|
|
|
|
<div id="org97ec686" class="figure">
|
|
<p><img src="figs/centralized_reference_tracking.png" alt="centralized_reference_tracking.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 7: </span>Centralized Controller</p>
|
|
</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>
|
|
Note here that the subtraction shown in Figure <a href="#org97ec686">7</a> is not a real subtraction.
|
|
It is indeed a more complex computation explained in section <a href="#org5f61540">4</a>.
|
|
</p>
|
|
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfbc962f" class="outline-3">
|
|
<h3 id="orgfbc962f"><span class="section-number-3">2.2</span> Initialize the Stewart platform</h3>
|
|
<div class="outline-text-3" id="text-2-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, <span class="org-string">'H'</span>, 90e<span class="org-type">-</span>3, <span class="org-string">'MO_B'</span>, 45e<span class="org-type">-</span>3);
|
|
stewart = generateGeneralConfiguration(stewart);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart);
|
|
stewart = initializeJointDynamics(stewart, <span class="org-string">'type_F'</span>, <span class="org-string">'universal_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);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">ground = initializeGround(<span class="org-string">'type'</span>, <span class="org-string">'rigid'</span>);
|
|
payload = initializePayload(<span class="org-string">'type'</span>, <span class="org-string">'none'</span>);
|
|
controller = initializeController(<span class="org-string">'type'</span>, <span class="org-string">'open-loop'</span>);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">disturbances = initializeDisturbances();
|
|
references = initializeReferences(stewart);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5b50e6c" class="outline-3">
|
|
<h3 id="org5b50e6c"><span class="section-number-3">2.3</span> Identification of the plant</h3>
|
|
<div class="outline-text-3" id="text-2-3">
|
|
<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">'/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>
|
|
<div id="outline-container-org8ff61f7" class="outline-4">
|
|
<h4 id="org8ff61f7"><span class="section-number-4">2.4.1</span> Control Architecture</h4>
|
|
<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.
|
|
The final implemented controller is \(\bm{K} = \bm{K}_\mathcal{L} \cdot \bm{J}\) as shown in Figure <a href="#orgb1f5ad2">8</a>
|
|
</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>
|
|
<p><span class="figure-number">Figure 8: </span>Controller in the frame of the legs</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7d457dc" class="outline-4">
|
|
<h4 id="org7d457dc"><span class="section-number-4">2.4.2</span> Plant Analysis</h4>
|
|
<div class="outline-text-4" id="text-2-4-2">
|
|
<p>
|
|
We now multiply the plant by the Jacobian matrix as shown in Figure <a href="#orgb1f5ad2">8</a> to obtain a more diagonal plant.
|
|
</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>
|
|
|
|
|
|
<div id="org6658ce5" class="figure">
|
|
<p><img src="figs/plant_centralized_diagonal_L.png" alt="plant_centralized_diagonal_L.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 9: </span>Diagonal Elements of the plant \(\bm{J} \bm{G}\) (<a href="./figs/plant_centralized_diagonal_L.png">png</a>, <a href="./figs/plant_centralized_diagonal_L.pdf">pdf</a>)</p>
|
|
</div>
|
|
|
|
<p>
|
|
All the diagonal elements are identical.
|
|
This will simplify the design of the controller as all the elements of the diagonal controller can be made identical.
|
|
</p>
|
|
|
|
<p>
|
|
The off-diagonal terms of the controller are shown in Figure <a href="#orgba050e4">10</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgba050e4" class="figure">
|
|
<p><img src="figs/plant_centralized_off_diagonal_L.png" alt="plant_centralized_off_diagonal_L.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 10: </span>Off Diagonal Elements of the plant \(\bm{J} \bm{G}\) (<a href="./figs/plant_centralized_off_diagonal_L.png">png</a>, <a href="./figs/plant_centralized_off_diagonal_L.pdf">pdf</a>)</p>
|
|
</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>
|
|
|
|
<div id="outline-container-orge101512" class="outline-4">
|
|
<h4 id="orge101512"><span class="section-number-4">2.4.3</span> Controller Design</h4>
|
|
<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 lead around the crossover frequency to increase the phase margin</li>
|
|
<li>A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin</li>
|
|
</ul>
|
|
|
|
<p>
|
|
The obtained loop gains corresponding to the diagonal elements are shown in Figure <a href="#orga803083">11</a>.
|
|
</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>
|
|
<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>
|
|
</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>
|
|
|
|
<div id="outline-container-orgb422b25" class="outline-4">
|
|
<h4 id="orgb422b25"><span class="section-number-4">2.4.4</span> Simulation</h4>
|
|
<div class="outline-text-4" id="text-2-4-4">
|
|
<p>
|
|
We specify the reference path to follow.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">t = linspace(0, 10, 1000);
|
|
|
|
r = zeros(6, length(t));
|
|
|
|
r(1, <span class="org-type">:</span>) = 5e<span class="org-type">-</span>3<span class="org-type">*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
|
|
|
|
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">
|
|
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'stewart_platform_model'</span>)
|
|
simout_L = simout;
|
|
</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>
|
|
<div id="outline-container-orgcad4c0f" class="outline-4">
|
|
<h4 id="orgcad4c0f"><span class="section-number-4">2.5.1</span> Control Architecture</h4>
|
|
<div class="outline-text-4" id="text-2-5-1">
|
|
<p>
|
|
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>.
|
|
</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>
|
|
<p><span class="figure-number">Figure 12: </span>Controller in the cartesian frame</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga0f64cf" class="outline-4">
|
|
<h4 id="orga0f64cf"><span class="section-number-4">2.5.2</span> Plant Analysis</h4>
|
|
<div class="outline-text-4" id="text-2-5-2">
|
|
<p>
|
|
We now multiply the plant by the Jacobian matrix as shown in Figure <a href="#org6b158db">12</a> to obtain a more diagonal plant.
|
|
</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>
|
|
|
|
|
|
<div id="org2b61181" class="figure">
|
|
<p><img src="figs/plant_centralized_diagonal_X.png" alt="plant_centralized_diagonal_X.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 13: </span>Diagonal Elements of the plant \(\bm{G} \bm{J}^{-T}\) (<a href="./figs/plant_centralized_diagonal_X.png">png</a>, <a href="./figs/plant_centralized_diagonal_X.pdf">pdf</a>)</p>
|
|
</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>
|
|
|
|
|
|
<div id="org1ff5b9c" class="figure">
|
|
<p><img src="figs/plant_centralized_off_diagonal_X.png" alt="plant_centralized_off_diagonal_X.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 14: </span>Off Diagonal Elements of the plant \(\bm{G} \bm{J}^{-T}\) (<a href="./figs/plant_centralized_off_diagonal_X.png">png</a>, <a href="./figs/plant_centralized_off_diagonal_X.pdf">pdf</a>)</p>
|
|
</div>
|
|
|
|
<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>
|
|
|
|
<div id="outline-container-orgc665ea1" class="outline-4">
|
|
<h4 id="orgc665ea1"><span class="section-number-4">2.5.3</span> Controller Design</h4>
|
|
<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 lead around the crossover frequency to increase the phase margin</li>
|
|
<li>A low pass filter with a cut-off frequency 3 times the crossover to increase the gain margin</li>
|
|
</ul>
|
|
|
|
<p>
|
|
The obtained loop gains corresponding to the diagonal elements are shown in Figure <a href="#org9051c86">15</a>.
|
|
</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>
|
|
<p><span class="figure-number">Figure 15: </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>
|
|
</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>
|
|
|
|
<div id="outline-container-org30afd72" class="outline-4">
|
|
<h4 id="org30afd72"><span class="section-number-4">2.5.4</span> Simulation</h4>
|
|
<div class="outline-text-4" id="text-2-5-4">
|
|
<p>
|
|
We specify the reference path to follow.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">t = linspace(0, 10, 1000);
|
|
|
|
r = zeros(6, length(t));
|
|
|
|
r(1, <span class="org-type">:</span>) = 5e<span class="org-type">-</span>3<span class="org-type">*</span>sin(2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>t);
|
|
|
|
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">
|
|
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'stewart_platform_model'</span>)
|
|
simout_X = simout;
|
|
</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>
|
|
<div id="outline-container-org224a0bb" class="outline-4">
|
|
<h4 id="org224a0bb"><span class="section-number-4">2.6.1</span> Control Architecture</h4>
|
|
<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>
|
|
The control architecture is shown in Figure <a href="#orgb226e44">16</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgb226e44" class="figure">
|
|
<p><img src="figs/centralized_reference_tracking_S.png" alt="centralized_reference_tracking_S.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 16: </span>Static Decoupling of the Plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org127af6e" class="outline-4">
|
|
<h4 id="org127af6e"><span class="section-number-4">2.6.2</span> Plant Analysis</h4>
|
|
<div class="outline-text-4" id="text-2-6-2">
|
|
<p>
|
|
The plant is pre-multiplied by \(\bm{G}^{-1}(\omega = 0)\).
|
|
The diagonal elements of the shaped plant are shown in Figure <a href="#orgc15aa83">17</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">G0 = G<span class="org-type">*</span>inv(freqresp(G, 0));
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orgc15aa83" class="figure">
|
|
<p><img src="figs/plant_centralized_diagonal_SD.png" alt="plant_centralized_diagonal_SD.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 17: </span>Diagonal Elements of the plant \(\bm{G} \bm{G}^{-1}(\omega = 0)\) (<a href="./figs/plant_centralized_diagonal_SD.png">png</a>, <a href="./figs/plant_centralized_diagonal_SD.pdf">pdf</a>)</p>
|
|
</div>
|
|
|
|
|
|
<div id="orga6b8b41" class="figure">
|
|
<p><img src="figs/plant_centralized_off_diagonal_SD.png" alt="plant_centralized_off_diagonal_SD.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 18: </span>Off Diagonal Elements of the plant \(\bm{G} \bm{J}^{-T}\) (<a href="./figs/plant_centralized_off_diagonal_SD.png">png</a>, <a href="./figs/plant_centralized_off_diagonal_SD.pdf">pdf</a>)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org64fe247" class="outline-4">
|
|
<h4 id="org64fe247"><span class="section-number-4">2.6.3</span> Controller Design</h4>
|
|
<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>
|
|
<p><span class="figure-number">Figure 19: </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>
|
|
</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>
|
|
The position error \(\bm{\epsilon}_\mathcal{X}\) for both centralized architecture are shown in Figure <a href="#org9fa8c8a">20</a>.
|
|
The corresponding leg’s length errors \(\bm{\epsilon}_\mathcal{L}\) are shown in Figure <a href="#orgb139e02">21</a>.
|
|
</p>
|
|
|
|
<p>
|
|
Based on Figure <a href="#org9fa8c8a">20</a>, we can see that:
|
|
</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>
|
|
<p><span class="figure-number">Figure 20: </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>
|
|
</div>
|
|
|
|
|
|
<div id="orgb139e02" class="figure">
|
|
<p><img src="figs/centralized_control_comp_El.png" alt="centralized_control_comp_El.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 21: </span>Comparison of the leg’s length error \(\bm{\epsilon}_\mathcal{L}\) for both centralized controllers (<a href="./figs/centralized_control_comp_El.png">png</a>, <a href="./figs/centralized_control_comp_El.pdf">pdf</a>)</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge3e2e02" class="outline-3">
|
|
<h3 id="orge3e2e02"><span class="section-number-3">2.8</span> Conclusion</h3>
|
|
<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>
|
|
<div id="outline-container-orgf22ae1f" class="outline-3">
|
|
<h3 id="orgf22ae1f"><span class="section-number-3">3.1</span> Control Schematic</h3>
|
|
<div class="outline-text-3" id="text-3-1">
|
|
|
|
<div id="org3a1b1db" class="figure">
|
|
<p><img src="figs/hybrid_reference_tracking.png" alt="hybrid_reference_tracking.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 22: </span>Centralized Controller</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
|
|
|
|
<div id="outline-container-org445f7a9" class="outline-2">
|
|
<h2 id="org445f7a9"><span class="section-number-2">4</span> Position Error computation</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
<p>
|
|
<a id="org5f61540"></a>
|
|
Let’s denote:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\(\{W\}\) the initial fixed frame (base in which the measurement is done)</li>
|
|
<li>\(\{R\}\) the reference frame corresponding to the wanted pose of the sample</li>
|
|
<li>\(\{M\}\) the frame corresponding to the measured pose of the sample</li>
|
|
</ul>
|
|
|
|
<p>
|
|
We have then computed:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<li>\({}^W\bm{T}_R\) which corresponds to the wanted pose of the sample with respect to the granite</li>
|
|
<li>\({}^W\bm{T}_M\) which corresponds to the measured pose of the sample with respect to the granite</li>
|
|
</ul>
|
|
|
|
<p>
|
|
Reference Position with respect to fixed frame {W}: \({}^WT_R\)
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Dx = 0;
|
|
Dy = 0;
|
|
Dz = 0.1;
|
|
Rx = <span class="org-constant">pi</span>;
|
|
Ry = 0;
|
|
Rz = 0;
|
|
|
|
WTr = zeros(4,4);
|
|
|
|
R = [cos(Rz) <span class="org-type">-</span>sin(Rz) 0;
|
|
sin(Rz) cos(Rz) 0;
|
|
0 0 1] <span class="org-type">*</span> ...
|
|
[cos(Ry) 0 sin(Ry);
|
|
0 1 0;
|
|
<span class="org-type">-</span>sin(Ry) 0 cos(Ry)] <span class="org-type">*</span> ...
|
|
[1 0 0;
|
|
0 cos(Rx) <span class="org-type">-</span>sin(Rx);
|
|
0 sin(Rx) cos(Rx)];
|
|
|
|
|
|
WTr(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = R;
|
|
WTr(1<span class="org-type">:</span>4, 4) = [Dx ; Dy ; Dz; 1];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Measured Position with respect to fixed frame {W}: \({}^WT_M\)
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Dx = 0;
|
|
Dy = 0;
|
|
Dz = 0;
|
|
Rx = <span class="org-constant">pi</span>;
|
|
Ry = 0;
|
|
Rz = 0;
|
|
|
|
WTm = zeros(4,4);
|
|
|
|
R = [cos(Rz) <span class="org-type">-</span>sin(Rz) 0;
|
|
sin(Rz) cos(Rz) 0;
|
|
0 0 1] <span class="org-type">*</span> ...
|
|
[cos(Ry) 0 sin(Ry);
|
|
0 1 0;
|
|
<span class="org-type">-</span>sin(Ry) 0 cos(Ry)] <span class="org-type">*</span> ...
|
|
[1 0 0;
|
|
0 cos(Rx) <span class="org-type">-</span>sin(Rx);
|
|
0 sin(Rx) cos(Rx)];
|
|
|
|
|
|
WTm(1<span class="org-type">:</span>3, 1<span class="org-type">:</span>3) = R;
|
|
WTm(1<span class="org-type">:</span>4, 4) = [Dx ; Dy ; Dz; 1];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We would like to compute \({}^M\bm{T}_R\) which corresponds to the wanted pose of the sample expressed in a frame attached to the top platform of the nano-hexapod (frame \(\{M\}\)).
|
|
</p>
|
|
|
|
<p>
|
|
We have:
|
|
</p>
|
|
\begin{align}
|
|
{}^M\bm{T}_R &= {}^M\bm{T}_W \cdot {}^W\bm{T}_R \\
|
|
&= {}^W{\bm{T}_M}^{-1} \cdot {}^W\bm{T}_R
|
|
\end{align}
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab"><span class="org-comment">% Error with respect to the top platform</span>
|
|
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;
|
|
|
|
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));
|
|
|
|
[Edx, Edy, Edz, Erx, Ery, Erz]
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2020-03-11 mer. 19:00</p>
|
|
</div>
|
|
</body>
|
|
</html>
|