nass-simscape/docs/control_cascade.html

733 lines
31 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-25 mer. 19:23 -->
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1" />
<title>Cascade Control applied on the Simscape Model</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"/>
<link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
<script type="text/javascript" src="./js/jquery.min.js"></script>
<script type="text/javascript" src="./js/bootstrap.min.js"></script>
<script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
<script type="text/javascript" src="./js/readtheorg.js"></script>
<script type="text/javascript">
/*
@licstart The following is the entire license notice for the
JavaScript code in this tag.
Copyright (C) 2012-2020 Free Software Foundation, Inc.
The JavaScript code in this tag is free software: you can
redistribute it and/or modify it under the terms of the GNU
General Public License (GNU GPL) as published by the Free Software
Foundation, either version 3 of the License, or (at your option)
any later version. The code is distributed WITHOUT ANY WARRANTY;
without even the implied warranty of MERCHANTABILITY or FITNESS
FOR A PARTICULAR PURPOSE. See the GNU GPL for more details.
As additional permission under GNU GPL version 3 section 7, you
may distribute non-source (e.g., minimized or compacted) forms of
that code without the copy of the GNU GPL normally required by
section 4, provided you include this license notice and a URL
through which recipients can access the Corresponding Source.
@licend The above is the entire license notice
for the JavaScript code in this tag.
*/
<!--/*--><![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;
}
/*]]>*///-->
</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">Cascade Control applied on the Simscape Model</h1>
<div id="table-of-contents">
<h2>Table of Contents</h2>
<div id="text-table-of-contents">
<ul>
<li><a href="#org143e6e7">1. Initialization</a></li>
<li><a href="#orga3bd04c">2. Low Authority Control - Integral Force Feedback \(\bm{K}_\text{IFF}\)</a>
<ul>
<li><a href="#org702c612">2.1. Identification</a></li>
<li><a href="#org1eda739">2.2. Plant</a></li>
<li><a href="#orgf33a40f">2.3. Root Locus</a></li>
<li><a href="#orgd895b21">2.4. Controller and Loop Gain</a></li>
</ul>
</li>
<li><a href="#org101bfbc">3. High Authority Control in the joint space - \(\bm{K}_\mathcal{L}\)</a>
<ul>
<li><a href="#org259240d">3.1. Identification of the damped plant</a></li>
<li><a href="#org878cff7">3.2. Obtained Plant</a></li>
<li><a href="#orgdea5e17">3.3. Controller Design and Loop Gain</a></li>
</ul>
</li>
<li><a href="#org58f9f32">4. Primary Controller in the task space - \(\bm{K}_\mathcal{X}\)</a>
<ul>
<li><a href="#org82ca884">4.1. Identification of the linearized plant</a></li>
<li><a href="#orgb574395">4.2. Obtained Plant</a></li>
<li><a href="#org839cdb3">4.3. Controller Design</a></li>
</ul>
</li>
<li><a href="#org3300911">5. Simulation</a></li>
<li><a href="#org2943496">6. Results</a></li>
</ul>
</div>
</div>
<p>
The control architecture we wish here to study is shown in Figure <a href="#org19741db">1</a>.
</p>
<div id="org19741db" class="figure">
<p><img src="figs/cascade_control_architecture.png" alt="cascade_control_architecture.png" />
</p>
<p><span class="figure-number">Figure 1: </span>Cascaded Control consisting of (from inner to outer loop): IFF, Linearization Loop, Tracking Control in the frame of the Legs</p>
</div>
<p>
This cascade control is designed in three steps:
</p>
<ul class="org-ul">
<li>In section <a href="#orga20c211">2</a>: an active damping controller is designed.
This is based on the Integral Force Feedback and applied in a decentralized way</li>
<li>In section <a href="#org9367d49">3</a>: a decentralized tracking control is designed in the frame of the legs.
This controller is based on the displacement of each of the legs</li>
<li>In section <a href="#orgdd883a0">4</a>: a controller is designed in the task space in order to follow the wanted reference path corresponding to the sample position with respect to the granite</li>
</ul>
<div id="outline-container-org143e6e7" class="outline-2">
<h2 id="org143e6e7"><span class="section-number-2">1</span> Initialization</h2>
<div class="outline-text-2" id="text-1">
<p>
We initialize all the stages with the default parameters.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeGround();
initializeGranite();
initializeTy();
initializeRy();
initializeRz();
initializeMicroHexapod();
initializeAxisc();
initializeMirror();
</pre>
</div>
<p>
The nano-hexapod is a piezoelectric hexapod and the sample has a mass of 50kg.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeNanoHexapod(<span class="org-string">'actuator'</span>, <span class="org-string">'piezo'</span>);
initializeSample(<span class="org-string">'mass'</span>, 1);
</pre>
</div>
<p>
We set the references that corresponds to a tomography experiment.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeReferences(<span class="org-string">'Rz_type'</span>, <span class="org-string">'rotating'</span>, <span class="org-string">'Rz_period'</span>, 1);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">initializeDisturbances();
</pre>
</div>
<p>
Open Loop.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeController(<span class="org-string">'type'</span>, <span class="org-string">'cascade-hac-lac'</span>);
</pre>
</div>
<p>
And we put some gravity.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeSimscapeConfiguration(<span class="org-string">'gravity'</span>, <span class="org-constant">true</span>);
</pre>
</div>
<p>
We log the signals.
</p>
<div class="org-src-container">
<pre class="src src-matlab">initializeLoggingConfiguration(<span class="org-string">'log'</span>, <span class="org-string">'all'</span>);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">Kx = tf(zeros(6));
Kl = tf(zeros(6));
Kiff = tf(zeros(6));
</pre>
</div>
</div>
</div>
<div id="outline-container-orga3bd04c" class="outline-2">
<h2 id="orga3bd04c"><span class="section-number-2">2</span> Low Authority Control - Integral Force Feedback \(\bm{K}_\text{IFF}\)</h2>
<div class="outline-text-2" id="text-2">
<p>
<a id="orga20c211"></a>
</p>
</div>
<div id="outline-container-org702c612" class="outline-3">
<h3 id="org702c612"><span class="section-number-3">2.1</span> Identification</h3>
<div class="outline-text-3" id="text-2-1">
<p>
Let&rsquo;s first identify the plant for the IFF controller.
</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">'nass_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 Inputs</span>
io(io_i) = linio([mdl, <span class="org-string">'/Micro-Station'</span>], 3, <span class="org-string">'openoutput'</span>, [], <span class="org-string">'Fnlm'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Force Sensors</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
G_iff = linearize(mdl, io, 0);
G_iff.InputName = {<span class="org-string">'Fnl1'</span>, <span class="org-string">'Fnl2'</span>, <span class="org-string">'Fnl3'</span>, <span class="org-string">'Fnl4'</span>, <span class="org-string">'Fnl5'</span>, <span class="org-string">'Fnl6'</span>};
G_iff.OutputName = {<span class="org-string">'Fnlm1'</span>, <span class="org-string">'Fnlm2'</span>, <span class="org-string">'Fnlm3'</span>, <span class="org-string">'Fnlm4'</span>, <span class="org-string">'Fnlm5'</span>, <span class="org-string">'Fnlm6'</span>};
</pre>
</div>
</div>
</div>
<div id="outline-container-org1eda739" class="outline-3">
<h3 id="org1eda739"><span class="section-number-3">2.2</span> Plant</h3>
<div class="outline-text-3" id="text-2-2">
<div id="org366ff26" class="figure">
<p><img src="figs/cascade_iff_plant.png" alt="cascade_iff_plant.png" />
</p>
<p><span class="figure-number">Figure 2: </span>IFF Plant (<a href="./figs/cascade_iff_plant.png">png</a>, <a href="./figs/cascade_iff_plant.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orgf33a40f" class="outline-3">
<h3 id="orgf33a40f"><span class="section-number-3">2.3</span> Root Locus</h3>
<div class="outline-text-3" id="text-2-3">
<div id="orgdf7a93e" class="figure">
<p><img src="figs/cascade_iff_root_locus.png" alt="cascade_iff_root_locus.png" />
</p>
<p><span class="figure-number">Figure 3: </span>Root Locus for the IFF control (<a href="./figs/cascade_iff_root_locus.png">png</a>, <a href="./figs/cascade_iff_root_locus.pdf">pdf</a>)</p>
</div>
<p>
The maximum damping is obtained for a control gain of \(\approx 3000\).
</p>
</div>
</div>
<div id="outline-container-orgd895b21" class="outline-3">
<h3 id="orgd895b21"><span class="section-number-3">2.4</span> Controller and Loop Gain</h3>
<div class="outline-text-3" id="text-2-4">
<p>
We create the \(6 \times 6\) diagonal Integral Force Feedback controller.
The obtained loop gain is shown in Figure <a href="#org7aaed92">4</a>.
</p>
<div class="org-src-container">
<pre class="src src-matlab">w0 = 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>50;
Kiff = <span class="org-type">-</span>3000<span class="org-type">/</span>s<span class="org-type">*</span>eye(6);
</pre>
</div>
<div id="org7aaed92" class="figure">
<p><img src="figs/cascade_iff_loop_gain.png" alt="cascade_iff_loop_gain.png" />
</p>
<p><span class="figure-number">Figure 4: </span>Obtained Loop gain the IFF Control (<a href="./figs/cascade_iff_loop_gain.png">png</a>, <a href="./figs/cascade_iff_loop_gain.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org101bfbc" class="outline-2">
<h2 id="org101bfbc"><span class="section-number-2">3</span> High Authority Control in the joint space - \(\bm{K}_\mathcal{L}\)</h2>
<div class="outline-text-2" id="text-3">
<p>
<a id="org9367d49"></a>
</p>
</div>
<div id="outline-container-org259240d" class="outline-3">
<h3 id="org259240d"><span class="section-number-3">3.1</span> Identification of the damped plant</h3>
<div class="outline-text-3" id="text-3-1">
<p>
We now identify the transfer function from \(\tau^\prime\) to \(d\bm{\mathcal{L}}\) as shown in Figure <a href="#org19741db">1</a>.
</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">'nass_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 Inputs</span>
io(io_i) = linio([mdl, <span class="org-string">'/Micro-Station'</span>], 3, <span class="org-string">'output'</span>, [], <span class="org-string">'Dnlm'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Leg Displacement</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
Gl = linearize(mdl, io, 0);
Gl.InputName = {<span class="org-string">'Fnl1'</span>, <span class="org-string">'Fnl2'</span>, <span class="org-string">'Fnl3'</span>, <span class="org-string">'Fnl4'</span>, <span class="org-string">'Fnl5'</span>, <span class="org-string">'Fnl6'</span>};
Gl.OutputName = {<span class="org-string">'Dnlm1'</span>, <span class="org-string">'Dnlm2'</span>, <span class="org-string">'Dnlm3'</span>, <span class="org-string">'Dnlm4'</span>, <span class="org-string">'Dnlm5'</span>, <span class="org-string">'Dnlm6'</span>};
</pre>
</div>
<p>
There are some unstable poles in the Plant with very small imaginary parts.
These unstable poles are probably not physical, and they disappear when taking the minimum realization of the plant.
</p>
<div class="org-src-container">
<pre class="src src-matlab">isstable(Gl)
Gl = minreal(Gl);
isstable(Gl)
</pre>
</div>
</div>
</div>
<div id="outline-container-org878cff7" class="outline-3">
<h3 id="org878cff7"><span class="section-number-3">3.2</span> Obtained Plant</h3>
<div class="outline-text-3" id="text-3-2">
<p>
The obtain plant is shown in Figure <a href="#org455eb07">5</a>.
</p>
<p>
We can see that the plant is quite well decoupled.
</p>
<div id="org455eb07" class="figure">
<p><img src="figs/cascade_hac_joint_plant.png" alt="cascade_hac_joint_plant.png" />
</p>
<p><span class="figure-number">Figure 5: </span>Plant for the High Authority Control in the Joint Space (<a href="./figs/cascade_hac_joint_plant.png">png</a>, <a href="./figs/cascade_hac_joint_plant.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-orgdea5e17" class="outline-3">
<h3 id="orgdea5e17"><span class="section-number-3">3.3</span> Controller Design and Loop Gain</h3>
<div class="outline-text-3" id="text-3-3">
<p>
The controller consists of:
</p>
<ul class="org-ul">
<li>A pure integrator</li>
<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 two times the wanted bandwidth</li>
</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>400; <span class="org-comment">% Bandwidth Bandwidth [rad/s]</span>
h = 2; <span class="org-comment">% Lead parameter</span>
<span class="org-comment">% Kl = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/2));</span>
Kl = (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> (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-comment">% Normalization of the gain of have a loop gain of 1 at frequency wc</span>
Kl = Kl<span class="org-type">.*</span>diag(1<span class="org-type">./</span>diag(abs(freqresp(Gl<span class="org-type">*</span>Kl, wc))));
</pre>
</div>
<div id="org3dd0142" class="figure">
<p><img src="figs/cascade_hac_joint_loop_gain.png" alt="cascade_hac_joint_loop_gain.png" />
</p>
<p><span class="figure-number">Figure 6: </span>Loop Gain for the High Autority Control in the joint space (<a href="./figs/cascade_hac_joint_loop_gain.png">png</a>, <a href="./figs/cascade_hac_joint_loop_gain.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org58f9f32" class="outline-2">
<h2 id="org58f9f32"><span class="section-number-2">4</span> Primary Controller in the task space - \(\bm{K}_\mathcal{X}\)</h2>
<div class="outline-text-2" id="text-4">
<p>
<a id="orgdd883a0"></a>
</p>
</div>
<div id="outline-container-org82ca884" class="outline-3">
<h3 id="org82ca884"><span class="section-number-3">4.1</span> Identification of the linearized plant</h3>
<div class="outline-text-3" id="text-4-1">
<p>
We know identify the dynamics between \(\bm{r}_{\mathcal{X}_n}\) and \(\bm{r}_\mathcal{X}\).
</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">'nass_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/Cascade-HAC-LAC/Kx'</span>], 1, <span class="org-string">'input'</span>); io_i = io_i <span class="org-type">+</span> 1;
io(io_i) = linio([mdl, <span class="org-string">'/Tracking Error'</span>], 1, <span class="org-string">'output'</span>, [], <span class="org-string">'En'</span>); io_i = io_i <span class="org-type">+</span> 1; <span class="org-comment">% Position Errror</span>
<span class="org-matlab-cellbreak"><span class="org-comment">%% Run the linearization</span></span>
Gx = linearize(mdl, io, 0);
Gx.InputName = {<span class="org-string">'rL1'</span>, <span class="org-string">'rL2'</span>, <span class="org-string">'rL3'</span>, <span class="org-string">'rL4'</span>, <span class="org-string">'rL5'</span>, <span class="org-string">'rL6'</span>};
Gx.OutputName = {<span class="org-string">'Ex'</span>, <span class="org-string">'Ey'</span>, <span class="org-string">'Ez'</span>, <span class="org-string">'Erx'</span>, <span class="org-string">'Ery'</span>, <span class="org-string">'Erz'</span>};
</pre>
</div>
<p>
As before, we take the minimum realization.
</p>
<div class="org-src-container">
<pre class="src src-matlab">isstable(Gx)
Gx = minreal(Gx);
isstable(Gx)
</pre>
</div>
</div>
</div>
<div id="outline-container-orgb574395" class="outline-3">
<h3 id="orgb574395"><span class="section-number-3">4.2</span> Obtained Plant</h3>
<div class="outline-text-3" id="text-4-2">
<div id="orge364e46" class="figure">
<p><img src="figs/cascade_primary_plant.png" alt="cascade_primary_plant.png" />
</p>
<p><span class="figure-number">Figure 7: </span>Plant for the Primary Controller (<a href="./figs/cascade_primary_plant.png">png</a>, <a href="./figs/cascade_primary_plant.pdf">pdf</a>)</p>
</div>
</div>
</div>
<div id="outline-container-org839cdb3" class="outline-3">
<h3 id="org839cdb3"><span class="section-number-3">4.3</span> Controller Design</h3>
<div class="outline-text-3" id="text-4-3">
<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>10; <span class="org-comment">% Bandwidth Bandwidth [rad/s]</span>
h = 2; <span class="org-comment">% Lead parameter</span>
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> 2<span class="org-type">*</span><span class="org-constant">pi</span><span class="org-type">*</span>5)<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>2<span class="org-type">/</span><span class="org-constant">pi</span><span class="org-type">/</span>20);
<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="orgb9d3971" class="figure">
<p><img src="figs/cascade_primary_loop_gain.png" alt="cascade_primary_loop_gain.png" />
</p>
<p><span class="figure-number">Figure 8: </span>Loop Gain for the primary controller (outer loop) (<a href="./figs/cascade_primary_loop_gain.png">png</a>, <a href="./figs/cascade_primary_loop_gain.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
<div id="outline-container-org3300911" class="outline-2">
<h2 id="org3300911"><span class="section-number-2">5</span> Simulation</h2>
<div class="outline-text-2" id="text-5">
<div class="org-src-container">
<pre class="src src-matlab">load(<span class="org-string">'mat/conf_simulink.mat'</span>);
<span class="org-matlab-simulink-keyword">set_param</span>(<span class="org-variable-name">conf_simulink</span>, <span class="org-string">'StopTime'</span>, <span class="org-string">'2'</span>);
</pre>
</div>
<p>
And we simulate the system.
</p>
<div class="org-src-container">
<pre class="src src-matlab"><span class="org-matlab-simulink-keyword">sim</span>(<span class="org-string">'nass_model'</span>);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">cascade_hac_lac = simout;
save(<span class="org-string">'./mat/cascade_hac_lac.mat'</span>, <span class="org-string">'cascade_hac_lac'</span>);
</pre>
</div>
</div>
</div>
<div id="outline-container-org2943496" class="outline-2">
<h2 id="org2943496"><span class="section-number-2">6</span> Results</h2>
<div class="outline-text-2" id="text-6">
<div class="org-src-container">
<pre class="src src-matlab">load(<span class="org-string">'./mat/experiment_tomography.mat'</span>, <span class="org-string">'tomo_align_dist'</span>);
load(<span class="org-string">'./mat/cascade_hac_lac.mat'</span>, <span class="org-string">'cascade_hac_lac'</span>);
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">n_av = 4;
han_win = hanning(ceil(length(cascade_hac_lac.Em.En.Data(<span class="org-type">:</span>,1))<span class="org-type">/</span>n_av));
</pre>
</div>
<div class="org-src-container">
<pre class="src src-matlab">t = cascade_hac_lac.Em.En.Time;
Ts = t(2)<span class="org-type">-</span>t(1);
[pxx_ol, f] = pwelch(tomo_align_dist.Em.En.Data, han_win, [], [], 1<span class="org-type">/</span>Ts);
[pxx_ca, <span class="org-type">~</span>] = pwelch(cascade_hac_lac.Em.En.Data, han_win, [], [], 1<span class="org-type">/</span>Ts);
</pre>
</div>
<div id="org11aa945" class="figure">
<p><img src="figs/cascade_hac_lac_tomography_psd.png" alt="cascade_hac_lac_tomography_psd.png" />
</p>
<p><span class="figure-number">Figure 9: </span>ASD of the position error (<a href="./figs/cascade_hac_lac_tomography_psd.png">png</a>, <a href="./figs/cascade_hac_lac_tomography_psd.pdf">pdf</a>)</p>
</div>
<div id="org845b982" class="figure">
<p><img src="figs/cascade_hac_lac_tomography_cas.png" alt="cascade_hac_lac_tomography_cas.png" />
</p>
<p><span class="figure-number">Figure 10: </span>Cumulative Amplitude Spectrum of the position error (<a href="./figs/cascade_hac_lac_tomography_cas.png">png</a>, <a href="./figs/cascade_hac_lac_tomography_cas.pdf">pdf</a>)</p>
</div>
<div id="org0151895" class="figure">
<p><img src="figs/cascade_hac_lac_tomography.png" alt="cascade_hac_lac_tomography.png" />
</p>
<p><span class="figure-number">Figure 11: </span>Results of the Tomography Experiment (<a href="./figs/cascade_hac_lac_tomography.png">png</a>, <a href="./figs/cascade_hac_lac_tomography.pdf">pdf</a>)</p>
</div>
</div>
</div>
</div>
<div id="postamble" class="status">
<p class="author">Author: Dehaeze Thomas</p>
<p class="date">Created: 2020-03-25 mer. 19:23</p>
</div>
</body>
</html>