1517 lines
52 KiB
HTML
1517 lines
52 KiB
HTML
<?xml version="1.0" encoding="utf-8"?>
|
|
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Strict//EN"
|
|
"http://www.w3.org/TR/xhtml1/DTD/xhtml1-strict.dtd">
|
|
<html xmlns="http://www.w3.org/1999/xhtml" lang="en" xml:lang="en">
|
|
<head>
|
|
<!-- 2020-08-05 mer. 13:27 -->
|
|
<meta http-equiv="Content-Type" content="text/html;charset=utf-8" />
|
|
<title>Stewart Platform - Tracking Control</title>
|
|
<meta name="generator" content="Org mode" />
|
|
<meta name="author" content="Dehaeze Thomas" />
|
|
<link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
|
<link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
|
<script src="./js/jquery.min.js"></script>
|
|
<script src="./js/bootstrap.min.js"></script>
|
|
<script src="./js/jquery.stickytableheaders.min.js"></script>
|
|
<script src="./js/readtheorg.js"></script>
|
|
<script>MathJax = {
|
|
tex: {
|
|
tags: 'ams',
|
|
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="#orgf40962b">1.1. Control Schematic</a></li>
|
|
<li><a href="#orgb67ba8a">1.2. Initialize the Stewart platform</a></li>
|
|
<li><a href="#org869bd42">1.3. Identification of the plant</a></li>
|
|
<li><a href="#org297755f">1.4. Plant Analysis</a></li>
|
|
<li><a href="#org2d0848b">1.5. Controller Design</a></li>
|
|
<li><a href="#orgfa88e97">1.6. Simulation</a></li>
|
|
<li><a href="#org974b430">1.7. Results</a></li>
|
|
<li><a href="#orga4f734e">1.8. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orga519721">2. Centralized Control Architecture using Pose Measurement</a>
|
|
<ul>
|
|
<li><a href="#orgc6622fd">2.1. Control Schematic</a></li>
|
|
<li><a href="#orgb2ce884">2.2. Initialize the Stewart platform</a></li>
|
|
<li><a href="#orgd3f8474">2.3. Identification of the plant</a></li>
|
|
<li><a href="#org2223469">2.4. Diagonal Control - Leg’s Frame</a>
|
|
<ul>
|
|
<li><a href="#org4a4490e">2.4.1. Control Architecture</a></li>
|
|
<li><a href="#org8aeb07e">2.4.2. Plant Analysis</a></li>
|
|
<li><a href="#orgba1c283">2.4.3. Controller Design</a></li>
|
|
<li><a href="#org6e37e44">2.4.4. Simulation</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org26a8857">2.5. Diagonal Control - Cartesian Frame</a>
|
|
<ul>
|
|
<li><a href="#orgff2007b">2.5.1. Control Architecture</a></li>
|
|
<li><a href="#orgd009ff3">2.5.2. Plant Analysis</a></li>
|
|
<li><a href="#org025c468">2.5.3. Controller Design</a></li>
|
|
<li><a href="#org948e147">2.5.4. Simulation</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#orgad7bc54">2.6. Diagonal Control - Steady State Decoupling</a>
|
|
<ul>
|
|
<li><a href="#org068c66b">2.6.1. Control Architecture</a></li>
|
|
<li><a href="#org114dd11">2.6.2. Plant Analysis</a></li>
|
|
<li><a href="#orge08ccd6">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="#org06dbde5">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="#org6cef32b">3.1. Control Schematic</a></li>
|
|
<li><a href="#org7b3baad">3.2. Initialize the Stewart platform</a></li>
|
|
<li><a href="#org3274a98">3.3. First Control Loop - \(\bm{K}_\mathcal{L}\)</a>
|
|
<ul>
|
|
<li><a href="#org6886bdb">3.3.1. Identification</a></li>
|
|
<li><a href="#org0130d27">3.3.2. Obtained Plant</a></li>
|
|
<li><a href="#org7f5301a">3.3.3. Controller Design</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org8440c0b">3.4. Second Control Loop - \(\bm{K}_\mathcal{X}\)</a>
|
|
<ul>
|
|
<li><a href="#org8ca6b32">3.4.1. Identification</a></li>
|
|
<li><a href="#org5235b22">3.4.2. Obtained Plant</a></li>
|
|
<li><a href="#orge58f568">3.4.3. Controller Design</a></li>
|
|
</ul>
|
|
</li>
|
|
<li><a href="#org74d3dcd">3.5. Simulations</a></li>
|
|
<li><a href="#org9ce7b75">3.6. Conclusion</a></li>
|
|
</ul>
|
|
</li>
|
|
<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>
|
|
</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>
|
|
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>
|
|
|
|
<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>
|
|
|
|
<p>
|
|
The control configuration are compare in section <a href="#orgb3403cb">4</a>.
|
|
</p>
|
|
|
|
<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-orgf40962b" class="outline-3">
|
|
<h3 id="orgf40962b"><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="#org897886b">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="org897886b" class="figure">
|
|
<p><img src="figs/decentralized_reference_tracking_L.png" alt="decentralized_reference_tracking_L.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 1: </span>Decentralized control for reference tracking</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb67ba8a" class="outline-3">
|
|
<h3 id="orgb67ba8a"><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 Platform
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
|
stewart = generateGeneralConfiguration(stewart);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
|
|
|
% Ground and Payload
|
|
ground = initializeGround('type', 'rigid');
|
|
payload = initializePayload('type', 'none');
|
|
|
|
% Controller
|
|
controller = initializeController('type', 'open-loop');
|
|
|
|
% Disturbances and References
|
|
disturbances = initializeDisturbances();
|
|
references = initializeReferences(stewart);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org869bd42" class="outline-3">
|
|
<h3 id="org869bd42"><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{\mathcal{L}}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">%% Name of the Simulink File
|
|
mdl = 'stewart_platform_model';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
|
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io);
|
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org297755f" class="outline-3">
|
|
<h3 id="org297755f"><span class="section-number-3">1.4</span> Plant Analysis</h3>
|
|
<div class="outline-text-3" id="text-1-4">
|
|
<p>
|
|
The diagonal and off-diagonal terms of the plant are shown in Figure <a href="#org50fb6db">2</a>.
|
|
</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>
|
|
|
|
|
|
<div id="org50fb6db" class="figure">
|
|
<p><img src="figs/plant_decentralized_L.png" alt="plant_decentralized_L.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org2d0848b" class="outline-3">
|
|
<h3 id="org2d0848b"><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 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">3</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">wc = 2*pi*30;
|
|
Kl = diag(1./diag(abs(freqresp(G, wc)))) * wc/s * 1/(1 + s/3/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 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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgfa88e97" class="outline-3">
|
|
<h3 id="orgfa88e97"><span class="section-number-3">1.6</span> Simulation</h3>
|
|
<div class="outline-text-3" id="text-1-6">
|
|
<p>
|
|
Let’s define some reference path to follow.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">t = [0:1e-4:10];
|
|
|
|
r = zeros(6, length(t));
|
|
|
|
r(1, :) = 1e-3.*t.*sin(2*pi*t);
|
|
r(2, :) = 1e-3.*t.*cos(2*pi*t);
|
|
r(3, :) = 1e-3.*t;
|
|
|
|
references = initializeReferences(stewart, 't', t, 'r', r);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The reference path is shown in Figure <a href="#orga6384f7">4</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">figure;
|
|
plot3(squeeze(references.r.Data(1,1,:)), squeeze(references.r.Data(2,1,:)), squeeze(references.r.Data(3,1,:)));
|
|
xlabel('X [m]');
|
|
ylabel('Y [m]');
|
|
zlabel('Z [m]');
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orga6384f7" class="figure">
|
|
<p><img src="figs/tracking_control_reference_path.png" alt="tracking_control_reference_path.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController('type', 'ref-track-L');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">set_param('stewart_platform_model', 'StopTime', '10')
|
|
sim('stewart_platform_model');
|
|
simout_D = simout;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">save('./mat/control_tracking.mat', 'simout_D', '-append');
|
|
</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">
|
|
<p>
|
|
The reference path and the position of the mobile platform are shown in Figure <a href="#orge497f54">5</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">figure;
|
|
hold on;
|
|
plot3(simout_D.x.Xr.Data(:, 1), simout_D.x.Xr.Data(:, 2), simout_D.x.Xr.Data(:, 3), 'k-');
|
|
plot3(squeeze(references.r.Data(1,1,:)), squeeze(references.r.Data(2,1,:)), squeeze(references.r.Data(3,1,:)), '--');
|
|
hold off;
|
|
xlabel('X [m]'); ylabel('Y [m]'); zlabel('Z [m]');
|
|
view([1,1,1])
|
|
zlim([0, inf]);
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="orge497f54" class="figure">
|
|
<p><img src="figs/decentralized_control_3d_pos.png" alt="decentralized_control_3d_pos.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
|
|
|
|
<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 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>
|
|
</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 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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orga4f734e" class="outline-3">
|
|
<h3 id="orga4f734e"><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-orgc6622fd" class="outline-3">
|
|
<h3 id="orgc6622fd"><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">8</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 8: </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">8</a> is not a real subtraction.
|
|
It is indeed a more complex computation explained in section <a href="#org5f61540">5</a>.
|
|
</p>
|
|
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgb2ce884" class="outline-3">
|
|
<h3 id="orgb2ce884"><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 Platform
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
|
stewart = generateGeneralConfiguration(stewart);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
|
|
|
% Ground and Payload
|
|
ground = initializeGround('type', 'rigid');
|
|
payload = initializePayload('type', 'none');
|
|
|
|
% Controller
|
|
controller = initializeController('type', 'open-loop');
|
|
|
|
% Disturbances and References
|
|
disturbances = initializeDisturbances();
|
|
references = initializeReferences(stewart);
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orgd3f8474" class="outline-3">
|
|
<h3 id="orgd3f8474"><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{\mathcal{X}}\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">%% Name of the Simulink File
|
|
mdl = 'stewart_platform_model';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
|
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [m]
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io);
|
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
</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-org4a4490e" class="outline-4">
|
|
<h4 id="org4a4490e"><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">9</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 9: </span>Controller in the frame of the legs</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org8aeb07e" class="outline-4">
|
|
<h4 id="org8aeb07e"><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">9</a> to obtain a more diagonal plant.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Gl = stewart.kinematics.J*G;
|
|
Gl.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The bode plot of the plant is shown in Figure <a href="#org6c8d99f">10</a>.
|
|
We can see that 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>
|
|
|
|
|
|
<div id="org6c8d99f" class="figure">
|
|
<p><img src="figs/plant_centralized_L.png" alt="plant_centralized_L.png" />
|
|
</p>
|
|
<p><span class="figure-number">Figure 10: </span>Diagonal and off-diagonal elements of the plant \(\bm{J}\bm{G}\) (<a href="./figs/plant_centralized_L.png">png</a>, <a href="./figs/plant_centralized_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-orgba1c283" class="outline-4">
|
|
<h4 id="orgba1c283"><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 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*pi*30;
|
|
Kl = diag(1./diag(abs(freqresp(Gl, wc)))) * wc/s * 1/(1 + s/3/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*stewart.kinematics.J;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org6e37e44" class="outline-4">
|
|
<h4 id="org6e37e44"><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 = [0:1e-4:10];
|
|
|
|
r = zeros(6, length(t));
|
|
|
|
r(1, :) = 1e-3.*t.*sin(2*pi*t);
|
|
r(2, :) = 1e-3.*t.*cos(2*pi*t);
|
|
r(3, :) = 1e-3.*t;
|
|
|
|
references = initializeReferences(stewart, 't', t, 'r', r);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController('type', 'ref-track-X');
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We run the simulation and we save the results.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">set_param('stewart_platform_model', 'StopTime', '10')
|
|
sim('stewart_platform_model')
|
|
simout_L = simout;
|
|
save('./mat/control_tracking.mat', 'simout_L', '-append');
|
|
</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-orgff2007b" class="outline-4">
|
|
<h4 id="orgff2007b"><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-orgd009ff3" class="outline-4">
|
|
<h4 id="orgd009ff3"><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*inv(stewart.kinematics.J');
|
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org0173211" class="figure">
|
|
<p><img src="figs/plant_centralized_X.png" alt="plant_centralized_X.png" />
|
|
</p>
|
|
<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>
|
|
</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>
|
|
|
|
<div id="outline-container-org025c468" class="outline-4">
|
|
<h4 id="org025c468"><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 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">14</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">wc = 2*pi*30;
|
|
Kx = diag(1./diag(abs(freqresp(Gx, wc)))) * wc/s * 1/(1 + s/3/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 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>
|
|
</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')*Kx;
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org948e147" class="outline-4">
|
|
<h4 id="org948e147"><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 = [0:1e-4:10];
|
|
|
|
r = zeros(6, length(t));
|
|
|
|
r(1, :) = 1e-3.*t.*sin(2*pi*t);
|
|
r(2, :) = 1e-3.*t.*cos(2*pi*t);
|
|
r(3, :) = 1e-3.*t;
|
|
|
|
references = initializeReferences(stewart, 't', t, 'r', r);
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">controller = initializeController('type', 'ref-track-X');
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We run the simulation and we save the results.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">set_param('stewart_platform_model', 'StopTime', '10')
|
|
sim('stewart_platform_model')
|
|
simout_X = simout;
|
|
save('./mat/control_tracking.mat', 'simout_X', '-append');
|
|
</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-org068c66b" class="outline-4">
|
|
<h4 id="org068c66b"><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">15</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 15: </span>Static Decoupling of the Plant</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org114dd11" class="outline-4">
|
|
<h4 id="org114dd11"><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 and off-diagonal elements of the shaped plant are shown in Figure <a href="#org0b73eca">16</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">G0 = G*inv(freqresp(G, 0));
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org0b73eca" class="figure">
|
|
<p><img src="figs/plant_centralized_SD.png" alt="plant_centralized_SD.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge08ccd6" class="outline-4">
|
|
<h4 id="orge08ccd6"><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 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>
|
|
</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">18</a>.
|
|
</p>
|
|
|
|
<p>
|
|
Based on Figure <a href="#org9fa8c8a">18</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 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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org06dbde5" class="outline-3">
|
|
<h3 id="org06dbde5"><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-org6cef32b" class="outline-3">
|
|
<h3 id="org6cef32b"><span class="section-number-3">3.1</span> Control Schematic</h3>
|
|
<div class="outline-text-3" id="text-3-1">
|
|
<p>
|
|
Let’s consider the control schematic shown in Figure <a href="#org3a1b1db">19</a>.
|
|
</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>
|
|
|
|
|
|
<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 19: </span>Hybrid Control Architecture</p>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7b3baad" class="outline-3">
|
|
<h3 id="org7b3baad"><span class="section-number-3">3.2</span> Initialize the Stewart platform</h3>
|
|
<div class="outline-text-3" id="text-3-2">
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">% Stewart Platform
|
|
stewart = initializeStewartPlatform();
|
|
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
|
stewart = generateGeneralConfiguration(stewart);
|
|
stewart = computeJointsPose(stewart);
|
|
stewart = initializeStrutDynamics(stewart);
|
|
stewart = initializeJointDynamics(stewart, 'type_F', 'universal_p', 'type_M', 'spherical_p');
|
|
stewart = initializeCylindricalPlatforms(stewart);
|
|
stewart = initializeCylindricalStruts(stewart);
|
|
stewart = computeJacobian(stewart);
|
|
stewart = initializeStewartPose(stewart);
|
|
stewart = initializeInertialSensor(stewart, 'type', 'accelerometer', 'freq', 5e3);
|
|
|
|
% Ground and Payload
|
|
ground = initializeGround('type', 'rigid');
|
|
payload = initializePayload('type', 'none');
|
|
|
|
% Controller
|
|
controller = initializeController('type', 'open-loop');
|
|
|
|
% Disturbances and References
|
|
disturbances = initializeDisturbances();
|
|
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>
|
|
<div id="outline-container-org6886bdb" class="outline-4">
|
|
<h4 id="org6886bdb"><span class="section-number-4">3.3.1</span> Identification</h4>
|
|
<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">%% Name of the Simulink File
|
|
mdl = 'stewart_platform_model';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
|
io(io_i) = linio([mdl, '/Stewart Platform'], 1, 'openoutput', [], 'dLm'); io_i = io_i + 1; % Relative Displacement Outputs [m]
|
|
|
|
%% Run the linearization
|
|
Gl = linearize(mdl, io);
|
|
Gl.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
Gl.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org0130d27" class="outline-4">
|
|
<h4 id="org0130d27"><span class="section-number-4">3.3.2</span> Obtained Plant</h4>
|
|
<div class="outline-text-4" id="text-3-3-2">
|
|
<p>
|
|
The obtained plant is shown in Figure <a href="#orgf627577">20</a>.
|
|
</p>
|
|
|
|
|
|
<div id="orgf627577" class="figure">
|
|
<p><img src="figs/hybrid_control_Kl_plant.png" alt="hybrid_control_Kl_plant.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org7f5301a" class="outline-4">
|
|
<h4 id="org7f5301a"><span class="section-number-4">3.3.3</span> Controller Design</h4>
|
|
<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>
|
|
The obtain loop gain is shown in Figure <a href="#orgb74befe">21</a>.
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Kl = 1e4 * s / (1 + s/2/pi/1e4) * 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>
|
|
<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>
|
|
</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>
|
|
<div id="outline-container-org8ca6b32" class="outline-4">
|
|
<h4 id="org8ca6b32"><span class="section-number-4">3.4.1</span> Identification</h4>
|
|
<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('type', 'ref-track-hac-dvf');
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">%% Name of the Simulink File
|
|
mdl = 'stewart_platform_model';
|
|
|
|
%% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
|
io(io_i) = linio([mdl, '/Relative Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [m]
|
|
|
|
%% Run the linearization
|
|
G = linearize(mdl, io);
|
|
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
|
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org5235b22" class="outline-4">
|
|
<h4 id="org5235b22"><span class="section-number-4">3.4.2</span> Obtained Plant</h4>
|
|
<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*inv(stewart.kinematics.J');
|
|
Gx.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained plant is shown in Figure <a href="#org2517e3d">22</a>.
|
|
</p>
|
|
|
|
<div id="org2517e3d" class="figure">
|
|
<p><img src="figs/hybrid_control_Kx_plant.png" alt="hybrid_control_Kx_plant.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-orge58f568" class="outline-4">
|
|
<h4 id="orge58f568"><span class="section-number-4">3.4.3</span> Controller Design</h4>
|
|
<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>
|
|
<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>
|
|
</ul>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">wc = 2*pi*200; % Bandwidth Bandwidth [rad/s]
|
|
|
|
h = 3; % Lead parameter
|
|
|
|
Kx = (1/h) * (1 + s/wc*h)/(1 + s/wc/h) * wc/s * ((s/wc*2 + 1)/(s/wc*2)) * (1/(1 + s/wc/3));
|
|
|
|
% Normalization of the gain of have a loop gain of 1 at frequency wc
|
|
Kx = Kx.*diag(1./diag(abs(freqresp(Gx*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>
|
|
<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>
|
|
</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')*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">
|
|
<pre class="src src-matlab">t = [0:1e-4:10];
|
|
|
|
r = zeros(6, length(t));
|
|
|
|
r(1, :) = 1e-3.*t.*sin(2*pi*t);
|
|
r(2, :) = 1e-3.*t.*cos(2*pi*t);
|
|
r(3, :) = 1e-3.*t;
|
|
|
|
references = initializeReferences(stewart, 't', t, 'r', r);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We run the simulation and we save the results.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">set_param('stewart_platform_model', 'StopTime', '10')
|
|
sim('stewart_platform_model')
|
|
simout_H = simout;
|
|
save('./mat/control_tracking.mat', 'simout_H', '-append');
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
The obtained position error is shown in Figure <a href="#org19456cf">24</a>.
|
|
</p>
|
|
|
|
|
|
<div id="org19456cf" class="figure">
|
|
<p><img src="figs/hybrid_control_Ex.png" alt="hybrid_control_Ex.png" />
|
|
</p>
|
|
<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>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org9ce7b75" class="outline-3">
|
|
<h3 id="org9ce7b75"><span class="section-number-3">3.6</span> Conclusion</h3>
|
|
</div>
|
|
</div>
|
|
|
|
<div id="outline-container-org798d54f" class="outline-2">
|
|
<h2 id="org798d54f"><span class="section-number-2">4</span> Comparison of all the methods</h2>
|
|
<div class="outline-text-2" id="text-4">
|
|
<p>
|
|
<a id="orgb3403cb"></a>
|
|
</p>
|
|
|
|
<p>
|
|
Let’s load the simulation results and compare them in Figure <a href="#org6fa53fa">25</a>.
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">load('./mat/control_tracking.mat', 'simout_D', 'simout_L', 'simout_X', 'simout_H');
|
|
</pre>
|
|
</div>
|
|
|
|
|
|
<div id="org6fa53fa" class="figure">
|
|
<p><img src="figs/reference_tracking_performance_comparison.png" alt="reference_tracking_performance_comparison.png" />
|
|
</p>
|
|
<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>
|
|
</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>
|
|
<a id="org5f61540"></a>
|
|
</p>
|
|
|
|
<p>
|
|
Let’s note:
|
|
</p>
|
|
<ul class="org-ul">
|
|
<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>
|
|
</ul>
|
|
|
|
<p>
|
|
Reference Position with respect to fixed frame {W}: \({}^WT_R\)
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Dxr = 0;
|
|
Dyr = 0;
|
|
Dzr = 0.1;
|
|
Rxr = pi;
|
|
Ryr = 0;
|
|
Rzr = 0;
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
Measured Position with respect to fixed frame {W}: \({}^WT_M\)
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">Dxm = 0;
|
|
Dym = 0;
|
|
Dzm = 0;
|
|
Rxm = pi;
|
|
Rym = 0;
|
|
Rzm = 0;
|
|
</pre>
|
|
</div>
|
|
|
|
<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">%% Measured Pose
|
|
WTm = zeros(4,4);
|
|
|
|
WTm(1:3, 1:3) = [cos(Rzm) -sin(Rzm) 0;
|
|
sin(Rzm) cos(Rzm) 0;
|
|
0 0 1] * ...
|
|
[cos(Rym) 0 sin(Rym);
|
|
0 1 0;
|
|
-sin(Rym) 0 cos(Rym)] * ...
|
|
[1 0 0;
|
|
0 cos(Rxm) -sin(Rxm);
|
|
0 sin(Rxm) cos(Rxm)];
|
|
WTm(1:4, 4) = [Dxm ; Dym ; Dzm; 1];
|
|
</pre>
|
|
</div>
|
|
|
|
<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">%% Reference Pose
|
|
WTr = zeros(4,4);
|
|
|
|
WTr(1:3, 1:3) = [cos(Rzr) -sin(Rzr) 0;
|
|
sin(Rzr) cos(Rzr) 0;
|
|
0 0 1] * ...
|
|
[cos(Ryr) 0 sin(Ryr);
|
|
0 1 0;
|
|
-sin(Ryr) 0 cos(Ryr)] * ...
|
|
[1 0 0;
|
|
0 cos(Rxr) -sin(Rxr);
|
|
0 sin(Rxr) cos(Rxr)];
|
|
WTr(1:4, 4) = [Dxr ; Dyr ; Dzr; 1];
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
We can also compute \({}^WT_V\).
|
|
</p>
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">WTv = eye(4);
|
|
WTv(1:3, 4) = WTm(1:3, 4);
|
|
</pre>
|
|
</div>
|
|
|
|
<p>
|
|
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 \]
|
|
</p>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">%% Wanted pose expressed in a frame corresponding to the actual measured pose
|
|
MTr = [WTm(1:3,1:3)', -WTm(1:3,1:3)'*WTm(1:3,4) ; 0 0 0 1]*WTr;
|
|
</pre>
|
|
</div>
|
|
|
|
<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">%% Wanted pose expressed in a frame coincident with the actual position but with no rotation
|
|
VTr = [WTv(1:3,1:3)', -WTv(1:3,1:3)'*WTv(1:3,4) ; 0 0 0 1] * WTr;
|
|
</pre>
|
|
</div>
|
|
|
|
<div class="org-src-container">
|
|
<pre class="src src-matlab">%% Extract Translations and Rotations from the Homogeneous Matrix
|
|
T = MTr;
|
|
Edx = T(1, 4);
|
|
Edy = T(2, 4);
|
|
Edz = T(3, 4);
|
|
|
|
% The angles obtained are u-v-w Euler angles (rotations in the moving frame)
|
|
Ery = atan2( T(1, 3), sqrt(T(1, 1)^2 + T(1, 2)^2));
|
|
Erx = atan2(-T(2, 3)/cos(Ery), T(3, 3)/cos(Ery));
|
|
Erz = atan2(-T(1, 2)/cos(Ery), T(1, 1)/cos(Ery));
|
|
</pre>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
</div>
|
|
<div id="postamble" class="status">
|
|
<p class="author">Author: Dehaeze Thomas</p>
|
|
<p class="date">Created: 2020-08-05 mer. 13:27</p>
|
|
</div>
|
|
</body>
|
|
</html>
|