Org and Docs folders
This commit is contained in:
731
org/active-damping.org
Normal file
731
org/active-damping.org
Normal file
@@ -0,0 +1,731 @@
|
||||
#+TITLE: Stewart Platform - Decentralized Active Damping
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The following decentralized active damping techniques are briefly studied:
|
||||
- Inertial Control (proportional feedback of the absolute velocity): Section [[sec:active_damping_inertial]]
|
||||
- Integral Force Feedback: Section [[sec:active_damping_iff]]
|
||||
- Direct feedback of the relative velocity of each strut: Section [[sec:active_damping_dvf]]
|
||||
|
||||
* Inertial Control
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/active_damping_inertial.m
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:END:
|
||||
<<sec:active_damping_inertial>>
|
||||
|
||||
** Introduction :ignore:
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('simulink/stewart_active_damping.slx')
|
||||
#+end_src
|
||||
|
||||
** Identification of the Dynamics
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'disable', true);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_active_damping';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1; % Absolute velocity of each leg [m/s]
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
|
||||
#+end_src
|
||||
|
||||
The transfer function from actuator forces to force sensors is shown in Figure [[fig:inertial_plant_coupling]].
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 2:6
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
plot(freqs, abs(squeeze(freqresp(G(['Vm', num2str(i)], 'F1'), freqs, 'Hz'))));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, abs(squeeze(freqresp(G('Vm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 2:6
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Vm', num2str(i)], 'F1'), freqs, 'Hz'))));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Vm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/inertial_plant_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:inertial_plant_coupling
|
||||
#+caption: Transfer function from the Actuator force $F_{i}$ to the absolute velocity of the same leg $v_{m,i}$ and to the absolute velocity of the other legs $v_{m,j}$ with $i \neq j$ in grey ([[./figs/inertial_plant_coupling.png][png]], [[./figs/inertial_plant_coupling.pdf][pdf]])
|
||||
[[file:figs/inertial_plant_coupling.png]]
|
||||
|
||||
** Effect of the Flexible Joint stiffness on the Dynamics
|
||||
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
|
||||
#+begin_src matlab
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
Gf = linearize(mdl, io, options);
|
||||
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
Gf.OutputName = {'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6'};
|
||||
#+end_src
|
||||
|
||||
The new dynamics from force actuator to force sensor is shown in Figure [[fig:inertial_plant_flexible_joint_decentralized]].
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [$\frac{m/s}{N}$]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Vm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend('location', 'southwest')
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/inertial_plant_flexible_joint_decentralized.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:inertial_plant_flexible_joint_decentralized
|
||||
#+caption: Transfer function from the Actuator force $F_{i}$ to the absolute velocity sensor $v_{m,i}$ ([[./figs/inertial_plant_flexible_joint_decentralized.png][png]], [[./figs/inertial_plant_flexible_joint_decentralized.pdf][pdf]])
|
||||
[[file:figs/inertial_plant_flexible_joint_decentralized.png]]
|
||||
|
||||
** Obtained Damping
|
||||
The control is a performed in a decentralized manner.
|
||||
The $6 \times 6$ control is a diagonal matrix with pure proportional action on the diagonal:
|
||||
\[ K(s) = g
|
||||
\begin{bmatrix}
|
||||
1 & & 0 \\
|
||||
& \ddots & \\
|
||||
0 & & 1
|
||||
\end{bmatrix} \]
|
||||
|
||||
The root locus is shown in figure [[fig:root_locus_inertial_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_inertial_rot_stiffness]].
|
||||
#+begin_src matlab :exports none
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(G)), imag(pole(G)), 'x');
|
||||
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(real(tzero(G)), imag(tzero(G)), 'o');
|
||||
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
|
||||
for i = 1:length(gains)
|
||||
cl_poles = pole(feedback(G, gains(i)*eye(6)));
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(real(cl_poles), imag(cl_poles), '.');
|
||||
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
plot(real(cl_poles), imag(cl_poles), '.');
|
||||
end
|
||||
ylim([0,2000]);
|
||||
xlim([-2000,0]);
|
||||
xlabel('Real Part')
|
||||
ylabel('Imaginary Part')
|
||||
axis square
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/root_locus_inertial_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:root_locus_inertial_rot_stiffness
|
||||
#+caption: Root Locus plot with Decentralized Inertial Control when considering the stiffness of flexible joints ([[./figs/root_locus_inertial_rot_stiffness.png][png]], [[./figs/root_locus_inertial_rot_stiffness.pdf][pdf]])
|
||||
[[file:figs/root_locus_inertial_rot_stiffness.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(gains)
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
cl_poles = pole(feedback(G, gains(i)*eye(6)));
|
||||
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
|
||||
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
cl_poles = pole(feedback(Gf, gains(i)*eye(6)));
|
||||
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
|
||||
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
|
||||
end
|
||||
xlabel('Control Gain');
|
||||
ylabel('Damping of the Poles');
|
||||
set(gca, 'XScale', 'log');
|
||||
ylim([0,pi/2]);
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/pole_damping_gain_inertial_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:pole_damping_gain_inertial_rot_stiffness
|
||||
#+caption: Damping of the poles with respect to the gain of the Decentralized Inertial Control when considering the stiffness of flexible joints ([[./figs/pole_damping_gain_inertial_rot_stiffness.png][png]], [[./figs/pole_damping_gain_inertial_rot_stiffness.pdf][pdf]])
|
||||
[[file:figs/pole_damping_gain_inertial_rot_stiffness.png]]
|
||||
|
||||
** Conclusion
|
||||
#+begin_important
|
||||
Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors.
|
||||
#+end_important
|
||||
|
||||
* Integral Force Feedback
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/active_damping_iff.m
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:END:
|
||||
<<sec:active_damping_iff>>
|
||||
|
||||
** Introduction :ignore:
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('simulink/stewart_active_damping.slx')
|
||||
#+end_src
|
||||
|
||||
** Identification of the Dynamics with perfect Joints
|
||||
We first initialize the Stewart platform without joint stiffness.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeAmplifiedStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'disable', true);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
And we identify the dynamics from force actuators to force sensors.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_active_damping';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Fm'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensor Outputs [N]
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
|
||||
#+end_src
|
||||
|
||||
The transfer function from actuator forces to force sensors is shown in Figure [[fig:iff_plant_coupling]].
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 2:6
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
plot(freqs, abs(squeeze(freqresp(G(['Fm', num2str(i)], 'F1'), freqs, 'Hz'))));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, abs(squeeze(freqresp(G('Fm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 2:6
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Fm', num2str(i)], 'F1'), freqs, 'Hz'))));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Fm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend([p1, p2], {'$F_{m,i}/F_i$', '$F_{m,j}/F_i$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/iff_plant_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:iff_plant_coupling
|
||||
#+caption: Transfer function from the Actuator force $F_{i}$ to the Force sensor of the same leg $F_{m,i}$ and to the force sensor of the other legs $F_{m,j}$ with $i \neq j$ in grey ([[./figs/iff_plant_coupling.png][png]], [[./figs/iff_plant_coupling.pdf][pdf]])
|
||||
[[file:figs/iff_plant_coupling.png]]
|
||||
|
||||
** Effect of the Flexible Joint stiffness on the Dynamics
|
||||
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
|
||||
#+begin_src matlab
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
Gf = linearize(mdl, io, options);
|
||||
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
Gf.OutputName = {'Fm1', 'Fm2', 'Fm3', 'Fm4', 'Fm5', 'Fm6'};
|
||||
#+end_src
|
||||
|
||||
The new dynamics from force actuator to force sensor is shown in Figure [[fig:iff_plant_flexible_joint_decentralized]].
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Fm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend('location', 'southwest')
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/iff_plant_flexible_joint_decentralized.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:iff_plant_flexible_joint_decentralized
|
||||
#+caption: Transfer function from the Actuator force $F_{i}$ to the force sensor $F_{m,i}$ ([[./figs/iff_plant_flexible_joint_decentralized.png][png]], [[./figs/iff_plant_flexible_joint_decentralized.pdf][pdf]])
|
||||
[[file:figs/iff_plant_flexible_joint_decentralized.png]]
|
||||
|
||||
** Obtained Damping
|
||||
The control is a performed in a decentralized manner.
|
||||
The $6 \times 6$ control is a diagonal matrix with pure integration action on the diagonal:
|
||||
\[ K(s) = g
|
||||
\begin{bmatrix}
|
||||
\frac{1}{s} & & 0 \\
|
||||
& \ddots & \\
|
||||
0 & & \frac{1}{s}
|
||||
\end{bmatrix} \]
|
||||
|
||||
The root locus is shown in figure [[fig:root_locus_iff_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_iff_rot_stiffness]].
|
||||
#+begin_src matlab :exports none
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(G)), imag(pole(G)), 'x');
|
||||
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(real(tzero(G)), imag(tzero(G)), 'o');
|
||||
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
|
||||
for i = 1:length(gains)
|
||||
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(real(cl_poles), imag(cl_poles), '.');
|
||||
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
plot(real(cl_poles), imag(cl_poles), '.');
|
||||
end
|
||||
ylim([0,inf]);
|
||||
xlim([-3000,0]);
|
||||
xlabel('Real Part')
|
||||
ylabel('Imaginary Part')
|
||||
axis square
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/root_locus_iff_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:root_locus_iff_rot_stiffness
|
||||
#+caption: Root Locus plot with Decentralized Integral Force Feedback when considering the stiffness of flexible joints ([[./figs/root_locus_iff_rot_stiffness.png][png]], [[./figs/root_locus_iff_rot_stiffness.pdf][pdf]])
|
||||
[[file:figs/root_locus_iff_rot_stiffness.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(gains)
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
cl_poles = pole(feedback(G, (gains(i)/s)*eye(6)));
|
||||
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
|
||||
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
cl_poles = pole(feedback(Gf, (gains(i)/s)*eye(6)));
|
||||
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
|
||||
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
|
||||
end
|
||||
xlabel('Control Gain');
|
||||
ylabel('Damping of the Poles');
|
||||
set(gca, 'XScale', 'log');
|
||||
ylim([0,pi/2]);
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/pole_damping_gain_iff_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:pole_damping_gain_iff_rot_stiffness
|
||||
#+caption: Damping of the poles with respect to the gain of the Decentralized Integral Force Feedback when considering the stiffness of flexible joints ([[./figs/pole_damping_gain_iff_rot_stiffness.png][png]], [[./figs/pole_damping_gain_iff_rot_stiffness.pdf][pdf]])
|
||||
[[file:figs/pole_damping_gain_iff_rot_stiffness.png]]
|
||||
|
||||
** Conclusion
|
||||
#+begin_important
|
||||
The joint stiffness has a huge impact on the attainable active damping performance when using force sensors.
|
||||
Thus, if Integral Force Feedback is to be used in a Stewart platform with flexible joints, the rotational stiffness of the joints should be minimized.
|
||||
#+end_important
|
||||
|
||||
* Direct Velocity Feedback
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/active_damping_dvf.m
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:END:
|
||||
<<sec:active_damping_dvf>>
|
||||
|
||||
** Introduction :ignore:
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
open('simulink/stewart_active_damping.slx')
|
||||
#+end_src
|
||||
|
||||
** Identification of the Dynamics with perfect Joints
|
||||
We first initialize the Stewart platform without joint stiffness.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart, 'disable', true);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
And we identify the dynamics from force actuators to force sensors.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_active_damping';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1; % Actuator Force Inputs [N]
|
||||
io(io_i) = linio([mdl, '/Dm'], 1, 'openoutput'); io_i = io_i + 1; % Relative Displacement Outputs [N]
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
|
||||
#+end_src
|
||||
|
||||
The transfer function from actuator forces to relative motion sensors is shown in Figure [[fig:dvf_plant_coupling]].
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 2:6
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
plot(freqs, abs(squeeze(freqresp(G(['Dm', num2str(i)], 'F1'), freqs, 'Hz'))));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, abs(squeeze(freqresp(G('Dm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 2:6
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
p2 = plot(freqs, 180/pi*angle(squeeze(freqresp(G(['Dm', num2str(i)], 'F1'), freqs, 'Hz'))));
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
p1 = plot(freqs, 180/pi*angle(squeeze(freqresp(G('Dm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend([p1, p2], {'$D_{m,i}/F_i$', '$D_{m,j}/F_i$'})
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/dvf_plant_coupling.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:dvf_plant_coupling
|
||||
#+caption: Transfer function from the Actuator force $F_{i}$ to the Relative Motion Sensor $D_{m,j}$ with $i \neq j$ ([[./figs/dvf_plant_coupling.png][png]], [[./figs/dvf_plant_coupling.pdf][pdf]])
|
||||
[[file:figs/dvf_plant_coupling.png]]
|
||||
|
||||
|
||||
** Effect of the Flexible Joint stiffness on the Dynamics
|
||||
We add some stiffness and damping in the flexible joints and we re-identify the dynamics.
|
||||
#+begin_src matlab
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
Gf = linearize(mdl, io, options);
|
||||
Gf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
Gf.OutputName = {'Dm1', 'Dm2', 'Dm3', 'Dm4', 'Dm5', 'Dm6'};
|
||||
#+end_src
|
||||
|
||||
The new dynamics from force actuator to relative motion sensor is shown in Figure [[fig:dvf_plant_flexible_joint_decentralized]].
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G( 'Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Perfect Joints');
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Gf('Dm1', 'F1'), freqs, 'Hz'))), 'DisplayName', 'Flexible Joints');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
legend('location', 'northeast');
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/dvf_plant_flexible_joint_decentralized.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:dvf_plant_flexible_joint_decentralized
|
||||
#+caption: Transfer function from the Actuator force $F_{i}$ to the relative displacement sensor $D_{m,i}$ ([[./figs/dvf_plant_flexible_joint_decentralized.png][png]], [[./figs/dvf_plant_flexible_joint_decentralized.pdf][pdf]])
|
||||
[[file:figs/dvf_plant_flexible_joint_decentralized.png]]
|
||||
|
||||
** Obtained Damping
|
||||
The control is a performed in a decentralized manner.
|
||||
The $6 \times 6$ control is a diagonal matrix with pure derivative action on the diagonal:
|
||||
\[ K(s) = g
|
||||
\begin{bmatrix}
|
||||
s & & \\
|
||||
& \ddots & \\
|
||||
& & s
|
||||
\end{bmatrix} \]
|
||||
|
||||
The root locus is shown in figure [[fig:root_locus_dvf_rot_stiffness]] and the obtained pole damping function of the control gain is shown in figure [[fig:pole_damping_gain_dvf_rot_stiffness]].
|
||||
#+begin_src matlab :exports none
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
plot(real(pole(G)), imag(pole(G)), 'x');
|
||||
plot(real(pole(Gf)), imag(pole(Gf)), 'x');
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(real(tzero(G)), imag(tzero(G)), 'o');
|
||||
plot(real(tzero(Gf)), imag(tzero(Gf)), 'o');
|
||||
for i = 1:length(gains)
|
||||
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(real(cl_poles), imag(cl_poles), '.');
|
||||
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
plot(real(cl_poles), imag(cl_poles), '.');
|
||||
end
|
||||
ylim([0,inf]);
|
||||
xlim([-3000,0]);
|
||||
xlabel('Real Part')
|
||||
ylabel('Imaginary Part')
|
||||
axis square
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/root_locus_dvf_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:root_locus_dvf_rot_stiffness
|
||||
#+caption: Root Locus plot with Direct Velocity Feedback when considering the Stiffness of flexible joints ([[./figs/root_locus_dvf_rot_stiffness.png][png]], [[./figs/root_locus_dvf_rot_stiffness.pdf][pdf]])
|
||||
[[file:figs/root_locus_dvf_rot_stiffness.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
gains = logspace(0, 5, 1000);
|
||||
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:length(gains)
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
cl_poles = pole(feedback(G, (gains(i)*s)*eye(6)));
|
||||
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
|
||||
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
|
||||
set(gca,'ColorOrderIndex',2);
|
||||
cl_poles = pole(feedback(Gf, (gains(i)*s)*eye(6)));
|
||||
poles_damp = phase(cl_poles(imag(cl_poles)>0)) - pi/2;
|
||||
plot(gains(i)*ones(size(poles_damp)), poles_damp, '.');
|
||||
end
|
||||
xlabel('Control Gain');
|
||||
ylabel('Damping of the Poles');
|
||||
set(gca, 'XScale', 'log');
|
||||
ylim([0,pi/2]);
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/pole_damping_gain_dvf_rot_stiffness.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:pole_damping_gain_dvf_rot_stiffness
|
||||
#+caption: Damping of the poles with respect to the gain of the Direct Velocity Feedback when considering the Stiffness of flexible joints ([[./figs/pole_damping_gain_dvf_rot_stiffness.png][png]], [[./figs/pole_damping_gain_dvf_rot_stiffness.pdf][pdf]])
|
||||
[[file:figs/pole_damping_gain_dvf_rot_stiffness.png]]
|
||||
|
||||
** Conclusion
|
||||
#+begin_important
|
||||
Joint stiffness does increase the resonance frequencies of the system but does not change the attainable damping when using relative motion sensors.
|
||||
#+end_important
|
||||
248
org/control-study.org
Normal file
248
org/control-study.org
Normal file
@@ -0,0 +1,248 @@
|
||||
#+TITLE: Stewart Platform - Control Study
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results file raw replace
|
||||
#+PROPERTY: header-args:latex+ :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
||||
:END:
|
||||
|
||||
* First Control Architecture
|
||||
** Matlab Init :noexport:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Control Schematic
|
||||
#+begin_src latex :file control_measure_rotating_2dof.pdf
|
||||
\begin{tikzpicture}
|
||||
% Blocs
|
||||
\node[block] (J) at (0, 0) {$J$};
|
||||
\node[addb={+}{}{}{}{-}, right=1 of J] (subr) {};
|
||||
\node[block, right=0.8 of subr] (K) {$K_{L}$};
|
||||
\node[block, right=1 of K] (G) {$G_{L}$};
|
||||
|
||||
% Connections and labels
|
||||
\draw[<-] (J.west)node[above left]{$\bm{r}_{n}$} -- ++(-1, 0);
|
||||
\draw[->] (J.east) -- (subr.west) node[above left]{$\bm{r}_{L}$};
|
||||
\draw[->] (subr.east) -- (K.west) node[above left]{$\bm{\epsilon}_{L}$};
|
||||
\draw[->] (K.east) -- (G.west) node[above left]{$\bm{\tau}$};
|
||||
\draw[->] (G.east) node[above right]{$\bm{L}$} -| ($(G.east)+(1, -1)$) -| (subr.south);
|
||||
\end{tikzpicture}
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
[[file:figs/control_measure_rotating_2dof.png]]
|
||||
|
||||
** Initialize the Stewart platform
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
** Identification of the plant
|
||||
Let's identify the transfer function from $\bm{\tau}}$ to $\bm{L}$.
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_control';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
||||
#+end_src
|
||||
|
||||
** Plant Analysis
|
||||
Diagonal terms
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G(i, i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, i), freqs, 'Hz'))));
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
Compare to off-diagonal terms
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, abs(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, abs(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(G(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
** Controller Design
|
||||
One integrator should be present in the controller.
|
||||
|
||||
A lead is added around the crossover frequency which is set to be around 500Hz.
|
||||
|
||||
#+begin_src matlab
|
||||
% wint = 2*pi*100; % Integrate until [rad]
|
||||
% wlead = 2*pi*500; % Location of the lead [rad]
|
||||
% hlead = 2; % Lead strengh
|
||||
|
||||
% Kl = 1e6 * ... % Gain
|
||||
% (s + wint)/(s) * ... % Integrator until 100Hz
|
||||
% (1 + s/(wlead/hlead)/(1 + s/(wlead*hlead))); % Lead
|
||||
|
||||
wc = 2*pi*100;
|
||||
Kl = 1/abs(freqresp(G(1,1), wc)) * wc/s * 1/(1 + s/(3*wc));
|
||||
Kl = Kl * eye(6);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 3, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
freqs = logspace(1, 4, 1000);
|
||||
|
||||
figure;
|
||||
|
||||
ax1 = subplot(2, 1, 1);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, abs(squeeze(freqresp(Kl(i,i)*G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, abs(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
||||
|
||||
ax2 = subplot(2, 1, 2);
|
||||
hold on;
|
||||
for i = 1:5
|
||||
for j = i+1:6
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(i, i)*G(i, j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2]);
|
||||
end
|
||||
end
|
||||
set(gca,'ColorOrderIndex',1);
|
||||
plot(freqs, 180/pi*angle(squeeze(freqresp(Kl(1,1)*G(1, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
||||
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
||||
ylim([-180, 180]);
|
||||
yticks([-180, -90, 0, 90, 180]);
|
||||
|
||||
linkaxes([ax1,ax2],'x');
|
||||
#+end_src
|
||||
479
org/cubic-configuration.org
Normal file
479
org/cubic-configuration.org
Normal file
@@ -0,0 +1,479 @@
|
||||
#+TITLE: Cubic configuration for the Stewart Platform
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :tangle matlab/cubic_configuration.m
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The discovery of the Cubic configuration is done in cite:geng94_six_degree_of_freed_activ.
|
||||
Further analysis is conducted in
|
||||
|
||||
The specificity of the Cubic configuration is that each actuator is orthogonal with the others.
|
||||
|
||||
The cubic (or orthogonal) configuration of the Stewart platform is now widely used (cite:preumont07_six_axis_singl_stage_activ,jafari03_orthog_gough_stewar_platf_microm).
|
||||
|
||||
According to cite:preumont07_six_axis_singl_stage_activ, the cubic configuration provides a uniform stiffness in all directions and *minimizes the crosscoupling* from actuator to sensor of different legs (being orthogonal to each other).
|
||||
|
||||
To generate and study the Cubic configuration, =generateCubicConfiguration= is used (description in section [[sec:generateCubicConfiguration]]).
|
||||
The goal is to study the benefits of using a cubic configuration:
|
||||
- Equal stiffness in all the degrees of freedom?
|
||||
- No coupling between the actuators?
|
||||
- Is the center of the cube an important point?
|
||||
|
||||
* Configuration Analysis - Stiffness Matrix
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Cubic Stewart platform centered with the cube center - Jacobian estimated at the cube center
|
||||
We create a cubic Stewart platform (figure [[fig:3d-cubic-stewart-aligned]]) in such a way that the center of the cube (black dot) is located at the center of the Stewart platform (blue dot).
|
||||
The Jacobian matrix is estimated at the location of the center of the cube.
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', -50e-3);
|
||||
stewart = generateCubicConfiguration(stewart, 'Hc', 100e-3, 'FOc', 50e-3, 'FHa', 0, 'MHb', 0);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', ones(6,1));
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 175e-3, 'Mpr', 150e-3);
|
||||
#+end_src
|
||||
|
||||
#+name: fig:3d-cubic-stewart-aligned
|
||||
#+caption: Centered cubic configuration
|
||||
[[file:./figs/3d-cubic-stewart-aligned.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
displayArchitecture(stewart, 'labels', false);
|
||||
scatter3(0, 0, 50e-3, 200, 'kh');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/cubic_conf_centered_J_center.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:cubic_conf_centered_J_center
|
||||
#+caption: Cubic Stewart platform centered with the cube center - Jacobian estimated at the cube center ([[./figs/cubic_conf_centered_J_center.png][png]], [[./figs/cubic_conf_centered_J_center.pdf][pdf]])
|
||||
[[file:figs/cubic_conf_centered_J_center.png]]
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||
data2orgtable(stewart.K, {}, {}, ' %.2g ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 2 | 0 | -2.5e-16 | 0 | 2.1e-17 | 0 |
|
||||
| 0 | 2 | 0 | -7.8e-19 | 0 | 0 |
|
||||
| -2.5e-16 | 0 | 2 | -2.4e-18 | -1.4e-17 | 0 |
|
||||
| 0 | -7.8e-19 | -2.4e-18 | 0.015 | -4.3e-19 | 1.7e-18 |
|
||||
| 1.8e-17 | 0 | -1.1e-17 | 0 | 0.015 | 0 |
|
||||
| 6.6e-18 | -3.3e-18 | 0 | 1.7e-18 | 0 | 0.06 |
|
||||
|
||||
** Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center
|
||||
We create a cubic Stewart platform with center of the cube located at the center of the Stewart platform (figure [[fig:3d-cubic-stewart-aligned]]).
|
||||
The Jacobian matrix is not estimated at the location of the center of the cube.
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', 0);
|
||||
stewart = generateCubicConfiguration(stewart, 'Hc', 100e-3, 'FOc', 50e-3, 'FHa', 0, 'MHb', 0);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', ones(6,1));
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 175e-3, 'Mpr', 150e-3);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
displayArchitecture(stewart, 'labels', false);
|
||||
scatter3(0, 0, 50e-3, 200, 'kh');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/cubic_conf_centered_J_not_center.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:cubic_conf_centered_J_not_center
|
||||
#+caption: Cubic Stewart platform centered with the cube center - Jacobian not estimated at the cube center ([[./figs/cubic_conf_centered_J_not_center.png][png]], [[./figs/cubic_conf_centered_J_not_center.pdf][pdf]])
|
||||
[[file:figs/cubic_conf_centered_J_not_center.png]]
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||
data2orgtable(stewart.K, {}, {}, ' %.2g ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 2 | 0 | -2.5e-16 | 1.4e-17 | -0.1 | 0 |
|
||||
| 0 | 2 | 0 | 0.1 | 0 | 0 |
|
||||
| -2.5e-16 | 0 | 2 | 3.4e-18 | -1.4e-17 | 0 |
|
||||
| 1.4e-17 | 0.1 | 3.4e-18 | 0.02 | 1.1e-20 | 3.4e-18 |
|
||||
| -0.1 | 0 | -1.4e-17 | 1.4e-19 | 0.02 | -1.7e-18 |
|
||||
| 6.6e-18 | -3.3e-18 | 0 | 3.6e-18 | -1.7e-18 | 0.06 |
|
||||
|
||||
** Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center
|
||||
Here, the "center" of the Stewart platform is not at the cube center (figure [[fig:3d-cubic-stewart-misaligned]]).
|
||||
The Jacobian is estimated at the cube center.
|
||||
|
||||
#+name: fig:3d-cubic-stewart-misaligned
|
||||
#+caption: Not centered cubic configuration
|
||||
[[file:./figs/3d-cubic-stewart-misaligned.png]]
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 80e-3, 'MO_B', -40e-3);
|
||||
stewart = generateCubicConfiguration(stewart, 'Hc', 100e-3, 'FOc', 50e-3, 'FHa', 0, 'MHb', 0);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', ones(6,1));
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 175e-3, 'Mpr', 150e-3);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
displayArchitecture(stewart, 'labels', false);
|
||||
scatter3(0, 0, 50e-3, 200, 'kh');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/cubic_conf_not_centered_J_center.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:cubic_conf_not_centered_J_center
|
||||
#+caption: Cubic Stewart platform not centered with the cube center - Jacobian estimated at the cube center ([[./figs/cubic_conf_not_centered_J_center.png][png]], [[./figs/cubic_conf_not_centered_J_center.pdf][pdf]])
|
||||
[[file:figs/cubic_conf_not_centered_J_center.png]]
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||
data2orgtable(stewart.K, {}, {}, ' %.2g ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 2 | 0 | -1.7e-16 | 0 | 0.02 | 0 |
|
||||
| 0 | 2 | 0 | -0.02 | 0 | 2.8e-17 |
|
||||
| -1.7e-16 | 0 | 2 | 1.2e-19 | -1.4e-17 | 1.4e-17 |
|
||||
| 0 | -0.02 | 1.2e-19 | 0.015 | -4.3e-19 | 1.7e-18 |
|
||||
| 0.02 | 0 | -7.3e-18 | -3.3e-21 | 0.015 | 0 |
|
||||
| 6.6e-18 | 2.5e-17 | 0 | 2e-18 | 0 | 0.06 |
|
||||
|
||||
We obtain $k_x = k_y = k_z$ and $k_{\theta_x} = k_{\theta_y}$, but the Stiffness matrix is not diagonal.
|
||||
|
||||
** Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center
|
||||
Here, the "center" of the Stewart platform is not at the cube center.
|
||||
The Jacobian is estimated at the center of the Stewart platform.
|
||||
|
||||
The center of the cube is at $z = 110$.
|
||||
The Stewart platform is from $z = H_0 = 75$ to $z = H_0 + H_{tot} = 175$.
|
||||
The center height of the Stewart platform is then at $z = \frac{175-75}{2} = 50$.
|
||||
The center of the cube from the top platform is at $z = 110 - 175 = -65$.
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 80e-3, 'MO_B', -40e-3);
|
||||
stewart = generateCubicConfiguration(stewart, 'Hc', 100e-3, 'FOc', 50e-3, 'FHa', 0, 'MHb', 0);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', ones(6,1));
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart, 'Fpr', 175e-3, 'Mpr', 150e-3);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
displayArchitecture(stewart, 'labels', false);
|
||||
scatter3(0, 0, 50e-3, 200, 'kh');
|
||||
#+end_src
|
||||
|
||||
#+header: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/cubic_conf_not_centered_J_stewart_center.pdf" :var figsize="wide-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+name: fig:cubic_conf_not_centered_J_stewart_center
|
||||
#+caption: Cubic Stewart platform not centered with the cube center - Jacobian estimated at the Stewart platform center ([[./figs/cubic_conf_not_centered_J_stewart_center.png][png]], [[./figs/cubic_conf_not_centered_J_stewart_center.pdf][pdf]])
|
||||
[[file:figs/cubic_conf_not_centered_J_stewart_center.png]]
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||
data2orgtable(stewart.K, {}, {}, ' %.2g ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 2 | 0 | -1.7e-16 | 0 | 0.02 | 0 |
|
||||
| 0 | 2 | 0 | -0.02 | 0 | 2.8e-17 |
|
||||
| -1.7e-16 | 0 | 2 | 1.2e-19 | -1.4e-17 | 1.4e-17 |
|
||||
| 0 | -0.02 | 1.2e-19 | 0.015 | -4.3e-19 | 1.7e-18 |
|
||||
| 0.02 | 0 | -7.3e-18 | -3.3e-21 | 0.015 | 0 |
|
||||
| 6.6e-18 | 2.5e-17 | 0 | 2e-18 | 0 | 0.06 |
|
||||
|
||||
** Conclusion
|
||||
#+begin_important
|
||||
- The cubic configuration permits to have $k_x = k_y = k_z$ and $k_{\theta_x} = k_{\theta_y}$
|
||||
- The stiffness matrix $K$ is diagonal for the cubic configuration if the Jacobian is estimated at the cube center.
|
||||
#+end_important
|
||||
|
||||
** Having Cube's center above the top platform
|
||||
Let's say we want to have a decouple dynamics above the top platform.
|
||||
Thus, we want the cube's center to be located above the top center.
|
||||
This is possible, to do so:
|
||||
- The position of the center of the cube should be positioned at A
|
||||
- The Height of the "useful" part of the cube should be at least equal to two times the distance from F to A.
|
||||
It is possible to have small cube, but then to configuration is a little bit strange.
|
||||
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 100e-3, 'MO_B', 50e-3);
|
||||
FOc = stewart.H + stewart.MO_B(3);
|
||||
Hc = 2*(stewart.H + stewart.MO_B(3));
|
||||
stewart = generateCubicConfiguration(stewart, 'Hc', Hc, 'FOc', FOc, 'FHa', 10e-3, 'MHb', 10e-3);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', ones(6,1));
|
||||
stewart = initializeJointDynamics(stewart, 'disable', true);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
displayArchitecture(stewart, 'labels', false);
|
||||
scatter3(0, 0, 50e-3, 200, 'kh');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||
data2orgtable(stewart.K, {}, {}, ' %.2g ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 2 | 0 | -3.2e-16 | 0 | 3.1e-16 | 0 |
|
||||
| 0 | 2 | 0 | -1.2e-16 | 0 | 0 |
|
||||
| -3.2e-16 | 0 | 2 | 5e-18 | -5.6e-17 | 0 |
|
||||
| 0 | -1.2e-16 | 5e-18 | 0.14 | 3.5e-18 | 1.4e-17 |
|
||||
| 3e-16 | 0 | -5.4e-17 | 2.1e-19 | 0.14 | -6.9e-18 |
|
||||
| 7.4e-19 | -2.6e-17 | 0 | 1.3e-17 | -6.9e-18 | 0.54 |
|
||||
|
||||
We obtain $k_x = k_y = k_z$ and $k_{\theta_x} = k_{\theta_y}$, but the Stiffness matrix is not diagonal.
|
||||
|
||||
* TODO Cubic size analysis :noexport:
|
||||
We here study the effect of the size of the cube used for the Stewart configuration.
|
||||
|
||||
We fix the height of the Stewart platform, the center of the cube is at the center of the Stewart platform.
|
||||
|
||||
We only vary the size of the cube.
|
||||
|
||||
#+begin_src matlab :results silent
|
||||
H_cubes = 250:20:350;
|
||||
stewarts = {zeros(length(H_cubes), 1)};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results silent
|
||||
for i = 1:length(H_cubes)
|
||||
H_cube = H_cubes(i);
|
||||
H_tot = 100;
|
||||
H = 80;
|
||||
|
||||
opts = struct(...
|
||||
'H_tot', H_tot, ... % Total height of the Hexapod [mm]
|
||||
'L', H_cube/sqrt(3), ... % Size of the Cube [mm]
|
||||
'H', H, ... % Height between base joints and platform joints [mm]
|
||||
'H0', H_cube/2-H/2 ... % Height between the corner of the cube and the plane containing the base joints [mm]
|
||||
);
|
||||
stewart = initializeCubicConfiguration(opts);
|
||||
|
||||
opts = struct(...
|
||||
'Jd_pos', [0, 0, H_cube/2-opts.H0-opts.H_tot], ... % Position of the Jacobian for displacement estimation from the top of the mobile platform [mm]
|
||||
'Jf_pos', [0, 0, H_cube/2-opts.H0-opts.H_tot] ... % Position of the Jacobian for force location from the top of the mobile platform [mm]
|
||||
);
|
||||
stewart = computeGeometricalProperties(stewart, opts);
|
||||
stewarts(i) = {stewart};
|
||||
end
|
||||
#+end_src
|
||||
|
||||
The Stiffness matrix is computed for all generated Stewart platforms.
|
||||
#+begin_src matlab :results none :exports code
|
||||
Ks = zeros(6, 6, length(H_cube));
|
||||
for i = 1:length(H_cubes)
|
||||
Ks(:, :, i) = stewarts{i}.Jd'*stewarts{i}.Jd;
|
||||
end
|
||||
#+end_src
|
||||
|
||||
The only elements of $K$ that vary are $k_{\theta_x} = k_{\theta_y}$ and $k_{\theta_z}$.
|
||||
|
||||
Finally, we plot $k_{\theta_x} = k_{\theta_y}$ and $k_{\theta_z}$
|
||||
#+begin_src matlab :results none :exports code
|
||||
figure;
|
||||
hold on;
|
||||
plot(H_cubes, squeeze(Ks(4, 4, :)), 'DisplayName', '$k_{\theta_x}$');
|
||||
plot(H_cubes, squeeze(Ks(6, 6, :)), 'DisplayName', '$k_{\theta_z}$');
|
||||
hold off;
|
||||
legend('location', 'northwest');
|
||||
xlabel('Cube Size [mm]'); ylabel('Rotational stiffnes [normalized]');
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:stiffness_cube_size
|
||||
#+HEADER: :tangle no :exports results :results raw :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/stiffness_cube_size.pdf" :var figsize="normal-normal" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:stiffness_cube_size
|
||||
#+CAPTION: $k_{\theta_x} = k_{\theta_y}$ and $k_{\theta_z}$ function of the size of the cube
|
||||
#+RESULTS: fig:stiffness_cube_size
|
||||
[[file:figs/stiffness_cube_size.png]]
|
||||
|
||||
|
||||
We observe that $k_{\theta_x} = k_{\theta_y}$ and $k_{\theta_z}$ increase linearly with the cube size.
|
||||
|
||||
#+begin_important
|
||||
In order to maximize the rotational stiffness of the Stewart platform, the size of the cube should be the highest possible.
|
||||
In that case, the legs will the further separated. Size of the cube is then limited by allowed space.
|
||||
#+end_important
|
||||
|
||||
* Functions
|
||||
<<sec:functions>>
|
||||
|
||||
** =generateCubicConfiguration=: Generate a Cubic Configuration
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle src/generateCubicConfiguration.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:generateCubicConfiguration>>
|
||||
|
||||
This Matlab function is accessible [[file:src/generateCubicConfiguration.m][here]].
|
||||
|
||||
*** Function description
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [stewart] = generateCubicConfiguration(stewart, args)
|
||||
% generateCubicConfiguration - Generate a Cubic Configuration
|
||||
%
|
||||
% Syntax: [stewart] = generateCubicConfiguration(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - geometry.H [1x1] - Total height of the platform [m]
|
||||
% - args - Can have the following fields:
|
||||
% - Hc [1x1] - Height of the "useful" part of the cube [m]
|
||||
% - FOc [1x1] - Height of the center of the cube with respect to {F} [m]
|
||||
% - FHa [1x1] - Height of the plane joining the points ai with respect to the frame {F} [m]
|
||||
% - MHb [1x1] - Height of the plane joining the points bi with respect to the frame {M} [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - updated Stewart structure with the added fields:
|
||||
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
||||
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
||||
#+end_src
|
||||
|
||||
*** Documentation
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+name: fig:cubic-configuration-definition
|
||||
#+caption: Cubic Configuration
|
||||
[[file:figs/cubic-configuration-definition.png]]
|
||||
|
||||
*** Optional Parameters
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.Hc (1,1) double {mustBeNumeric, mustBePositive} = 60e-3
|
||||
args.FOc (1,1) double {mustBeNumeric} = 50e-3
|
||||
args.FHa (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||||
args.MHb (1,1) double {mustBeNumeric, mustBeNonnegative} = 15e-3
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.geometry, 'H'), 'stewart.geometry should have attribute H')
|
||||
H = stewart.geometry.H;
|
||||
#+end_src
|
||||
|
||||
*** Position of the Cube
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
We define the useful points of the cube with respect to the Cube's center.
|
||||
${}^{C}C$ are the 6 vertices of the cubes expressed in a frame {C} which is
|
||||
located at the center of the cube and aligned with {F} and {M}.
|
||||
|
||||
#+begin_src matlab
|
||||
sx = [ 2; -1; -1];
|
||||
sy = [ 0; 1; -1];
|
||||
sz = [ 1; 1; 1];
|
||||
|
||||
R = [sx, sy, sz]./vecnorm([sx, sy, sz]);
|
||||
|
||||
L = args.Hc*sqrt(3);
|
||||
|
||||
Cc = R'*[[0;0;L],[L;0;L],[L;0;0],[L;L;0],[0;L;0],[0;L;L]] - [0;0;1.5*args.Hc];
|
||||
|
||||
CCf = [Cc(:,1), Cc(:,3), Cc(:,3), Cc(:,5), Cc(:,5), Cc(:,1)]; % CCf(:,i) corresponds to the bottom cube's vertice corresponding to the i'th leg
|
||||
CCm = [Cc(:,2), Cc(:,2), Cc(:,4), Cc(:,4), Cc(:,6), Cc(:,6)]; % CCm(:,i) corresponds to the top cube's vertice corresponding to the i'th leg
|
||||
#+end_src
|
||||
|
||||
*** Compute the pose
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
We can compute the vector of each leg ${}^{C}\hat{\bm{s}}_{i}$ (unit vector from ${}^{C}C_{f}$ to ${}^{C}C_{m}$).
|
||||
#+begin_src matlab
|
||||
CSi = (CCm - CCf)./vecnorm(CCm - CCf);
|
||||
#+end_src
|
||||
|
||||
We now which to compute the position of the joints $a_{i}$ and $b_{i}$.
|
||||
#+begin_src matlab
|
||||
Fa = CCf + [0; 0; args.FOc] + ((args.FHa-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
Mb = CCf + [0; 0; args.FOc-H] + ((H-args.MHb-(args.FOc-args.Hc/2))./CSi(3,:)).*CSi;
|
||||
#+end_src
|
||||
|
||||
*** Populate the =stewart= structure
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
stewart.platform_F.Fa = Fa;
|
||||
stewart.platform_M.Mb = Mb;
|
||||
#+end_src
|
||||
|
||||
* Bibliography :ignore:
|
||||
bibliographystyle:unsrtnat
|
||||
bibliography:ref.bib
|
||||
311
org/dynamics-study.org
Normal file
311
org/dynamics-study.org
Normal file
@@ -0,0 +1,311 @@
|
||||
#+TITLE: Stewart Platform - Dynamics Study
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Some tests
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Simscape Model
|
||||
#+begin_src matlab
|
||||
open('stewart_platform_dynamics.slx')
|
||||
#+end_src
|
||||
|
||||
** test
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_dynamics';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_dynamics';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/J-T'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
G_cart = minreal(G*inv(stewart.J'));
|
||||
G_cart.InputName = {'Fnx', 'Fny', 'Fnz', 'Mnx', 'Mny', 'Mnz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
figure; bode(G, G_cart)
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_dynamics';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
Gd = linearize(mdl, io, options);
|
||||
Gd.InputName = {'Fex', 'Fey', 'Fez', 'Mex', 'Mey', 'Mez'};
|
||||
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = logspace(0, 3, 1000);
|
||||
|
||||
figure;
|
||||
bode(Gd, G)
|
||||
#+end_src
|
||||
|
||||
** Compare external forces and forces applied by the actuators
|
||||
Initialization of the Stewart platform.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_dynamics';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
Estimation of the transfer function from $\mathcal{\bm{F}}_{d}$ to $\mathcal{\bm{X}}$:
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_dynamics';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
Gd = linearize(mdl, io, options);
|
||||
Gd.InputName = {'Fex', 'Fey', 'Fez', 'Mex', 'Mey', 'Mez'};
|
||||
Gd.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
Comparison of the two transfer function matrices.
|
||||
#+begin_src matlab
|
||||
freqs = logspace(0, 4, 1000);
|
||||
|
||||
figure;
|
||||
bode(Gd, G, freqs)
|
||||
#+end_src
|
||||
|
||||
#+begin_important
|
||||
Seems quite similar.
|
||||
#+end_important
|
||||
|
||||
** Comparison of the static transfer function and the Compliance matrix
|
||||
Initialization of the Stewart platform.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
Estimation of the transfer function from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$:
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_dynamics';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/F'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
G.OutputName = {'Edx', 'Edy', 'Edz', 'Erx', 'Ery', 'Erz'};
|
||||
#+end_src
|
||||
|
||||
Let's first look at the low frequency transfer function matrix from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$.
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||
data2orgtable(real(freqresp(G, 0.1)), {}, {}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 2.0e-06 | -9.1e-19 | -5.3e-12 | 7.3e-18 | 1.7e-05 | 1.3e-18 |
|
||||
| -1.7e-18 | 2.0e-06 | 8.6e-19 | -1.7e-05 | -1.5e-17 | 6.7e-12 |
|
||||
| 3.6e-13 | 3.2e-19 | 5.0e-07 | -2.5e-18 | 8.1e-12 | -1.5e-19 |
|
||||
| 1.0e-17 | -1.7e-05 | -5.0e-18 | 1.9e-04 | 9.1e-17 | -3.5e-11 |
|
||||
| 1.7e-05 | -6.9e-19 | -5.3e-11 | 6.9e-18 | 1.9e-04 | 4.8e-18 |
|
||||
| -3.5e-18 | -4.5e-12 | 1.5e-18 | 7.1e-11 | -3.4e-17 | 4.6e-05 |
|
||||
|
||||
And now at the Compliance matrix.
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no
|
||||
data2orgtable(stewart.C, {}, {}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| 2.0e-06 | 2.9e-22 | 2.8e-22 | -3.2e-21 | 1.7e-05 | 1.5e-37 |
|
||||
| -2.1e-22 | 2.0e-06 | -1.8e-23 | -1.7e-05 | -2.3e-21 | 1.1e-22 |
|
||||
| 3.1e-22 | -1.6e-23 | 5.0e-07 | 1.7e-22 | 2.2e-21 | -8.1e-39 |
|
||||
| 2.1e-21 | -1.7e-05 | 2.0e-22 | 1.9e-04 | 2.3e-20 | -8.7e-21 |
|
||||
| 1.7e-05 | 2.5e-21 | 2.0e-21 | -2.8e-20 | 1.9e-04 | 1.3e-36 |
|
||||
| 3.7e-23 | 3.1e-22 | -6.0e-39 | -1.0e-20 | 3.1e-22 | 4.6e-05 |
|
||||
|
||||
#+begin_important
|
||||
The low frequency transfer function matrix from $\mathcal{\bm{F}}$ to $\mathcal{\bm{X}}$ corresponds to the compliance matrix of the Stewart platform.
|
||||
#+end_important
|
||||
|
||||
** Transfer function from forces applied in the legs to the displacement of the legs
|
||||
Initialization of the Stewart platform.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
Estimation of the transfer function from $\bm{\tau}$ to $\bm{L}$:
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_dynamics';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/J-T'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/L'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
|
||||
G.OutputName = {'L1', 'L2', 'L3', 'L4', 'L5', 'L6'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
freqs = logspace(1, 3, 1000);
|
||||
figure; bode(G, 2*pi*freqs)
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
bodeFig({G(1,1), G(1,2)}, freqs, struct('phase', true));
|
||||
#+end_src
|
||||
589
org/identification.org
Normal file
589
org/identification.org
Normal file
@@ -0,0 +1,589 @@
|
||||
#+TITLE: Identification of the Stewart Platform using Simscape
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :tangle matlab/identification.m
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
We would like to extract a state space model of the Stewart Platform from the Simscape model.
|
||||
|
||||
The inputs are:
|
||||
| Symbol | Meaning |
|
||||
|------------------------+--------------------------------------------------|
|
||||
| $\bm{\mathcal{F}}_{d}$ | External forces applied in {B} |
|
||||
| $\bm{\tau}$ | Joint forces |
|
||||
| $\bm{\mathcal{F}}$ | Cartesian forces applied by the Joints |
|
||||
| $\bm{D}_{w}$ | Fixed Based translation and rotations around {A} |
|
||||
|
||||
The outputs are:
|
||||
| Symbol | Meaning |
|
||||
|--------------------+---------------------------------------------------------------------------|
|
||||
| $\bm{\mathcal{X}}$ | Relative Motion of {B} with respect to {A} |
|
||||
| $\bm{\mathcal{L}}$ | Joint Displacement |
|
||||
| $\bm{F}_{m}$ | Force Sensors in each strut |
|
||||
| $\bm{v}_{m}$ | Inertial Sensors located at $b_i$ measuring in the direction of the strut |
|
||||
|
||||
|
||||
#+begin_quote
|
||||
An important difference from basic Simulink models is that the states in a physical network are not independent in general, because some states have dependencies on other states through constraints.
|
||||
#+end_quote
|
||||
|
||||
|
||||
|
||||
* Identification
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Simscape Model
|
||||
|
||||
** Initialize the Stewart Platform
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_identification';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Fext'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Vm'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Taum'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Lm'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6', ...
|
||||
'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
||||
|
||||
G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', ...
|
||||
'Vm1', 'Vm2', 'Vm3', 'Vm4', 'Vm5', 'Vm6', ...
|
||||
'taum1', 'taum2', 'taum3', 'taum4', 'taum5', 'taum6', ...
|
||||
'Lm1', 'Lm2', 'Lm3', 'Lm4', 'Lm5', 'Lm6'};
|
||||
#+end_src
|
||||
|
||||
* States as the motion of the mobile platform
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Initialize the Stewart Platform
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_identification_simple';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Xdot'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io);
|
||||
% G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
|
||||
% G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
|
||||
#+end_src
|
||||
|
||||
Let's check the size of =G=:
|
||||
#+begin_src matlab :results replace output
|
||||
size(G)
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
: size(G)
|
||||
: State-space model with 12 outputs, 6 inputs, and 18 states.
|
||||
: 'org_babel_eoe'
|
||||
: ans =
|
||||
: 'org_babel_eoe'
|
||||
|
||||
We expect to have only 12 states (corresponding to the 6dof of the mobile platform).
|
||||
#+begin_src matlab :results replace output
|
||||
Gm = minreal(G);
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
: Gm = minreal(G);
|
||||
: 6 states removed.
|
||||
|
||||
And indeed, we obtain 12 states.
|
||||
|
||||
** Coordinate transformation
|
||||
We can perform the following transformation using the =ss2ss= command.
|
||||
#+begin_src matlab
|
||||
Gt = ss2ss(Gm, Gm.C);
|
||||
#+end_src
|
||||
|
||||
Then, the =C= matrix of =Gt= is the unity matrix which means that the states of the state space model are equal to the measurements $\bm{Y}$.
|
||||
|
||||
The measurements are the 6 displacement and 6 velocities of mobile platform with respect to $\{B\}$.
|
||||
|
||||
We could perform the transformation by hand:
|
||||
#+begin_src matlab
|
||||
At = Gm.C*Gm.A*pinv(Gm.C);
|
||||
|
||||
Bt = Gm.C*Gm.B;
|
||||
|
||||
Ct = eye(12);
|
||||
Dt = zeros(12, 6);
|
||||
|
||||
Gt = ss(At, Bt, Ct, Dt);
|
||||
#+end_src
|
||||
|
||||
** Analysis
|
||||
#+begin_src matlab
|
||||
[V,D] = eig(Gt.A);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
ws = imag(diag(D))/2/pi;
|
||||
[ws,I] = sort(ws)
|
||||
|
||||
xi = 100*real(diag(D))./imag(diag(D));
|
||||
xi = xi(I);
|
||||
|
||||
data2orgtable([[1:length(ws(ws>0))]', ws(ws>0), xi(xi>0)], {}, {'Mode Number', 'Resonance Frequency [Hz]', 'Damping Ratio [%]'}, ' %.1f ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| Mode Number | Resonance Frequency [Hz] | Damping Ratio [%] |
|
||||
|-------------+--------------------------+-------------------|
|
||||
| 1.0 | 174.5 | 0.9 |
|
||||
| 2.0 | 174.5 | 0.7 |
|
||||
| 3.0 | 202.1 | 0.7 |
|
||||
| 4.0 | 237.3 | 0.6 |
|
||||
| 5.0 | 237.3 | 0.5 |
|
||||
| 6.0 | 283.8 | 0.5 |
|
||||
|
||||
** Visualizing the modes
|
||||
To visualize the i'th mode, we may excite the system using the inputs $U_i$ such that $B U_i$ is co-linear to $\xi_i$ (the mode we want to excite).
|
||||
|
||||
\[ U(t) = e^{\alpha t} ( ) \]
|
||||
|
||||
Let's first sort the modes and just take the modes corresponding to a eigenvalue with a positive imaginary part.
|
||||
#+begin_src matlab
|
||||
ws = imag(diag(D));
|
||||
[ws,I] = sort(ws)
|
||||
ws = ws(7:end); I = I(7:end);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
for i = 1:length(ws)
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
i_mode = I(i); % the argument is the i'th mode
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
lambda_i = D(i_mode, i_mode);
|
||||
xi_i = V(:,i_mode);
|
||||
|
||||
a_i = real(lambda_i);
|
||||
b_i = imag(lambda_i);
|
||||
#+end_src
|
||||
|
||||
Let do 10 periods of the mode.
|
||||
#+begin_src matlab
|
||||
t = linspace(0, 10/(imag(lambda_i)/2/pi), 1000);
|
||||
U_i = pinv(Gt.B) * real(xi_i * lambda_i * (cos(b_i * t) + 1i*sin(b_i * t)));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
U = timeseries(U_i, t);
|
||||
#+end_src
|
||||
|
||||
Simulation:
|
||||
#+begin_src matlab
|
||||
load('mat/conf_simscape.mat');
|
||||
set_param(conf_simscape, 'StopTime', num2str(t(end)));
|
||||
sim(mdl);
|
||||
#+end_src
|
||||
|
||||
Save the movie of the mode shape.
|
||||
#+begin_src matlab
|
||||
smwritevideo(mdl, sprintf('figs/mode%i', i), ...
|
||||
'PlaybackSpeedRatio', 1/(b_i/2/pi), ...
|
||||
'FrameRate', 30, ...
|
||||
'FrameSize', [800, 400]);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+name: fig:mode1
|
||||
#+caption: Identified mode - 1
|
||||
[[file:figs/mode1.gif]]
|
||||
|
||||
#+name: fig:mode3
|
||||
#+caption: Identified mode - 3
|
||||
[[file:figs/mode3.gif]]
|
||||
|
||||
#+name: fig:mode5
|
||||
#+caption: Identified mode - 5
|
||||
[[file:figs/mode5.gif]]
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_identification';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Lm'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
% G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
|
||||
% G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
size(G)
|
||||
#+end_src
|
||||
|
||||
** Change of states
|
||||
#+begin_src matlab
|
||||
At = G.C*G.A*pinv(G.C);
|
||||
|
||||
Bt = G.C*G.B;
|
||||
|
||||
Ct = eye(12);
|
||||
Dt = zeros(12, 6);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
Gt = ss(At, Bt, Ct, Dt);
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
size(Gt)
|
||||
#+end_src
|
||||
|
||||
* Simple Model without any sensor
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Simscape Model
|
||||
#+begin_src matlab
|
||||
open 'stewart_identification_simple.slx'
|
||||
#+end_src
|
||||
|
||||
|
||||
** Initialize the Stewart Platform
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
#+end_src
|
||||
|
||||
** Identification
|
||||
#+begin_src matlab
|
||||
stateorder = {...
|
||||
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_1_1_1',...
|
||||
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_2_1_1',...
|
||||
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_3_1_1',...
|
||||
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_4_1_1',...
|
||||
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_5_1_1',...
|
||||
'stewart_platform_identification_simple/Solver Configuration/EVAL_KEY/INPUT_6_1_1',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Rz.q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Rz.q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Rz.q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Rz.q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Rz.q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Rz.q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Pz.p',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Pz.p',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Pz.p',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Pz.p',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Pz.p',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Pz.p',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Rz.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Rz.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Rz.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Rz.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Rz.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Rz.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.cylindrical_joint.Pz.v',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.cylindrical_joint.Pz.v',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.cylindrical_joint.Pz.v',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.cylindrical_joint.Pz.v',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.cylindrical_joint.Pz.v',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.cylindrical_joint.Pz.v',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_F.S.Q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.spherical_joint_F.S.Q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.spherical_joint_F.S.Q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.spherical_joint_F.S.Q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.spherical_joint_F.S.Q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.spherical_joint_F.S.Q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_2.Subsystem.spherical_joint_F.S.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_3.Subsystem.spherical_joint_F.S.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_4.Subsystem.spherical_joint_F.S.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_5.Subsystem.spherical_joint_F.S.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_6.Subsystem.spherical_joint_F.S.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_F.S.w',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_M.S.Q',...
|
||||
'stewart_platform_identification_simple.Stewart_Platform.Strut_1.Subsystem.spherical_joint_M.S.w'};
|
||||
#+end_src
|
||||
|
||||
|
||||
#+begin_src matlab
|
||||
%% Options for Linearized
|
||||
options = linearizeOptions;
|
||||
options.SampleTime = 0;
|
||||
|
||||
%% Name of the Simulink File
|
||||
mdl = 'stewart_platform_identification_simple';
|
||||
|
||||
%% Input/Output definition
|
||||
clear io; io_i = 1;
|
||||
io(io_i) = linio([mdl, '/tau'], 1, 'openinput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/X'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
io(io_i) = linio([mdl, '/Xdot'], 1, 'openoutput'); io_i = io_i + 1;
|
||||
|
||||
%% Run the linearization
|
||||
G = linearize(mdl, io, options);
|
||||
G.InputName = {'tau1', 'tau2', 'tau3', 'tau4', 'tau5', 'tau6'};
|
||||
|
||||
G.OutputName = {'Xdx', 'Xdy', 'Xdz', 'Xrx', 'Xry', 'Xrz', 'Vdx', 'Vdy', 'Vdz', 'Vrx', 'Vry', 'Vrz'};
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
size(G)
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
G.StateName
|
||||
#+end_src
|
||||
|
||||
* Cartesian Plot
|
||||
From a force applied in the Cartesian frame to a displacement in the Cartesian frame.
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_cart(1, 1), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_cart(2, 1), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_cart(3, 1), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
bode(G.G_cart, freqs);
|
||||
#+end_src
|
||||
|
||||
* From a force to force sensor
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_forc(1, 1), freqs, 'Hz'))), 'k-', 'DisplayName', '$F_{m_i}/F_{i}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
|
||||
legend('location', 'southeast');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_forc(1, 1), freqs, 'Hz'))), 'k-', 'DisplayName', '$F_{m_i}/F_{i}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_forc(2, 1), freqs, 'Hz'))), 'k--', 'DisplayName', '$F_{m_j}/F_{i}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_forc(3, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_forc(4, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_forc(5, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_forc(6, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [N/N]');
|
||||
legend('location', 'southeast');
|
||||
#+end_src
|
||||
|
||||
* From a force applied in the leg to the displacement of the leg
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_legs(1, 1), freqs, 'Hz'))), 'k-', 'DisplayName', '$D_{i}/F_{i}$');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_legs(1, 1), freqs, 'Hz'))), 'k-', 'DisplayName', '$D_{i}/F_{i}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_legs(2, 1), freqs, 'Hz'))), 'k--', 'DisplayName', '$D_{j}/F_{i}$');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_legs(3, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_legs(4, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_legs(5, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_legs(6, 1), freqs, 'Hz'))), 'k--', 'HandleVisibility', 'off');
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
||||
legend('location', 'northeast');
|
||||
#+end_src
|
||||
|
||||
* Transmissibility
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(1, 1), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(2, 2), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(3, 3), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/m]');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(4, 4), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(5, 5), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(6, 6), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [$\frac{rad/s}{rad/s}$]');
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(1, 1), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(1, 2), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_tran(1, 3), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/m]');
|
||||
#+end_src
|
||||
|
||||
* Compliance
|
||||
From a force applied in the Cartesian frame to a relative displacement of the mobile platform with respect to the base.
|
||||
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_comp(1, 1), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_comp(2, 2), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_comp(3, 3), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
||||
#+end_src
|
||||
|
||||
* Inertial
|
||||
From a force applied on the Cartesian frame to the absolute displacement of the mobile platform.
|
||||
|
||||
#+begin_src matlab :results none
|
||||
figure;
|
||||
hold on;
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_iner(1, 1), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_iner(2, 2), freqs, 'Hz'))));
|
||||
plot(freqs, abs(squeeze(freqresp(G.G_iner(3, 3), freqs, 'Hz'))));
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Frequency [Hz]'); ylabel('Amplitude [m/N]');
|
||||
#+end_src
|
||||
|
||||
82
org/index.org
Normal file
82
org/index.org
Normal file
@@ -0,0 +1,82 @@
|
||||
#+TITLE: Stewart Platforms
|
||||
:DRAWER:
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The goal of this project is to provide a Matlab/Simscape Toolbox to study Stewart platforms.
|
||||
|
||||
The project is divided into several section listed below.
|
||||
|
||||
* Simulink Project ([[file:simulink-project.org][link]])
|
||||
The project is managed with a *Simulink Project*.
|
||||
Such project is briefly presented [[file:simulink-project.org][here]].
|
||||
|
||||
* Stewart Platform Architecture Definition ([[file:stewart-architecture.org][link]])
|
||||
The way the Stewart Platform is defined [[file:stewart-architecture.org][here]].
|
||||
|
||||
All the geometrical parameters are defined including:
|
||||
- Definition of the location of the frames
|
||||
- Location/orientation of the limbs
|
||||
- Size/inertia of the platforms and the limbs
|
||||
|
||||
Other parameters are also defined such as:
|
||||
- Stiffness and damping of the struts
|
||||
- Rest position of the Stewart platform
|
||||
|
||||
* Simscape Model of the Stewart Platform ([[file:simscape-model.org][link]])
|
||||
The Stewart Platform is then modeled using [[https://www.mathworks.com/products/simscape.html][Simscape]].
|
||||
|
||||
The way to model is build and works is explained [[file:simscape-model.org][here]].
|
||||
|
||||
* Kinematic Analysis ([[file:kinematic-study.org][link]])
|
||||
From the defined geometry of the Stewart platform, we can perform static analysis such as:
|
||||
- *Jacobian Analysis* that links the velocity of each limb to the velocity of the mobile platform
|
||||
- *Static Forces Analysis* that links the forces applied by each limb to the resulting force/torque applied to the mobile platform
|
||||
|
||||
From the strut stiffness, we can also perform a *Stiffness Analysis* that consists of determining the Stiffness matrix and Compliance matrix of the Stewart platform from the geometry.
|
||||
|
||||
All these analysis are described [[file:kinematic-study.org][here]].
|
||||
|
||||
* Identification of the Stewart Dynamics ([[file:identification.org][link]])
|
||||
The Dynamics of the Stewart platform can be identified using the Simscape model.
|
||||
|
||||
It is possible to:
|
||||
- Determine the dynamics from the actuators to the various sensors included in the Stewart platform
|
||||
- Extract State Space models for further analysis / control synthesis
|
||||
- Extract the Resonant Frequencies, Modal Damping, and associated Mode Shapes
|
||||
|
||||
The code that is used for identification is explained [[file:identification.org][here]].
|
||||
|
||||
* Active Damping ([[file:active-damping.org][link]])
|
||||
The use of different sensors are compared for active damping:
|
||||
- Inertial Sensor in each strut
|
||||
- Inertial Sensor fixed to the mobile platform
|
||||
- Force Sensor in each strut
|
||||
- Relative Motion Sensor in each strut
|
||||
|
||||
The result of the analysis is accessible [[file:active-damping.org][here]].
|
||||
|
||||
* Motion Control of the Stewart Platform ([[file:control-study.org][link]])
|
||||
Some control architecture for motion control of the Stewart platform are applied on the Simscape model and compared in [[file:control-study.org][this]] document.
|
||||
|
||||
* Cubic Configuration ([[file:cubic-configuration.org][link]])
|
||||
The cubic configuration is a special class of Stewart platform that has interesting properties.
|
||||
|
||||
These properties are studied in [[file:cubic-configuration.org][this]] document.
|
||||
|
||||
850
org/kinematic-study.org
Normal file
850
org/kinematic-study.org
Normal file
@@ -0,0 +1,850 @@
|
||||
#+TITLE: Kinematic Study of the Stewart Platform
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
The kinematic analysis of a parallel manipulator is well described in cite:taghirad13_paral:
|
||||
#+begin_quote
|
||||
Kinematic analysis refers to the study of the geometry of motion of a robot, without considering the forces an torques that cause the motion.
|
||||
In this analysis, the relation between the geometrical parameters of the manipulator with the final motion of the moving platform is derived and analyzed.
|
||||
#+end_quote
|
||||
|
||||
The current document is divided in the following sections:
|
||||
- Section [[sec:jacobian_analysis]]: The Jacobian matrix is derived from the geometry of the Stewart platform. Then it is shown that the Jacobian can link velocities and forces present in the system, and thus this matrix can be very useful for both analysis and control of the Stewart platform.
|
||||
- Section [[sec:stiffness_analysis]]: The stiffness and compliance matrices are derived from the Jacobian matrix and the stiffness of each strut.
|
||||
- Section [[sec:forward_inverse_kinematics]]: The Forward and Inverse kinematic problems are presented.
|
||||
- Section [[sec:required_actuator_stroke]]: The Inverse kinematic solution is used to estimate required actuator stroke from the wanted mobility of the Stewart platform.
|
||||
|
||||
* Jacobian Analysis
|
||||
<<sec:jacobian_analysis>>
|
||||
** Introduction :ignore:
|
||||
|
||||
From cite:taghirad13_paral:
|
||||
#+begin_quote
|
||||
The Jacobian matrix not only reveals the *relation between the joint variable velocities of a parallel manipulator to the moving platform linear and angular velocities*, it also constructs the transformation needed to find the *actuator forces from the forces and moments acting on the moving platform*.
|
||||
#+end_quote
|
||||
|
||||
** Jacobian Computation
|
||||
If we note:
|
||||
- ${}^A\hat{\bm{s}}_i$ the unit vector representing the direction of the i'th strut and expressed in frame $\{A\}$
|
||||
- ${}^A\bm{b}_i$ the position vector of the i'th joint fixed to the mobile platform and expressed in frame $\{A\}$
|
||||
|
||||
Then, we can compute the Jacobian with the following equation (the superscript $A$ is ignored):
|
||||
\begin{equation*}
|
||||
\bm{J} = \begin{bmatrix}
|
||||
{\hat{\bm{s}}_1}^T & (\bm{b}_1 \times \hat{\bm{s}}_1)^T \\
|
||||
{\hat{\bm{s}}_2}^T & (\bm{b}_2 \times \hat{\bm{s}}_2)^T \\
|
||||
{\hat{\bm{s}}_3}^T & (\bm{b}_3 \times \hat{\bm{s}}_3)^T \\
|
||||
{\hat{\bm{s}}_4}^T & (\bm{b}_4 \times \hat{\bm{s}}_4)^T \\
|
||||
{\hat{\bm{s}}_5}^T & (\bm{b}_5 \times \hat{\bm{s}}_5)^T \\
|
||||
{\hat{\bm{s}}_6}^T & (\bm{b}_6 \times \hat{\bm{s}}_6)^T
|
||||
\end{bmatrix}
|
||||
\end{equation*}
|
||||
|
||||
The Jacobian matrix $\bm{J}$ can be computed using the =computeJacobian= function (accessible [[sec:computeJacobian][here]]).
|
||||
For instance:
|
||||
#+begin_src matlab :eval no
|
||||
stewart = computeJacobian(stewart);
|
||||
#+end_src
|
||||
This will add three new matrix to the =stewart= structure:
|
||||
- =J= the Jacobian matrix
|
||||
- =K= the stiffness matrix
|
||||
- =C= the compliance matrix
|
||||
|
||||
** Jacobian - Velocity loop closure
|
||||
The Jacobian matrix links the input joint rate $\dot{\bm{\mathcal{L}}} = [ \dot{l}_1, \dot{l}_2, \dot{l}_3, \dot{l}_4, \dot{l}_5, \dot{l}_6 ]^T$ of each strut to the output twist vector of the mobile platform is denoted by $\dot{\bm{X}} = [^A\bm{v}_p, {}^A\bm{\omega}]^T$:
|
||||
\begin{equation*}
|
||||
\dot{\bm{\mathcal{L}}} = \bm{J} \dot{\bm{\mathcal{X}}}
|
||||
\end{equation*}
|
||||
|
||||
The input joint rate $\dot{\bm{\mathcal{L}}}$ can be measured by taking the derivative of the relative motion sensor in each strut.
|
||||
The output twist vector can be measured with a "Transform Sensor" block measuring the relative velocity and relative angular velocity of frame $\{B\}$ with respect to frame $\{A\}$.
|
||||
|
||||
If the Jacobian matrix is inversible, we can also compute $\dot{\bm{\mathcal{X}}}$ from $\dot{\bm{\mathcal{L}}}$.
|
||||
\begin{equation*}
|
||||
\dot{\bm{\mathcal{X}}} = \bm{J}^{-1} \dot{\bm{\mathcal{L}}}
|
||||
\end{equation*}
|
||||
|
||||
The Jacobian matrix can also be used to approximate forward and inverse kinematics for small displacements.
|
||||
This is explained in section [[sec:approximate_forward_inverse_kinematics]].
|
||||
|
||||
** Jacobian - Static Force Transformation
|
||||
If we note:
|
||||
- $\bm{\tau} = [\tau_1, \tau_2, \cdots, \tau_6]^T$: vector of actuator forces applied in each strut
|
||||
- $\bm{\mathcal{F}} = [\bm{f}, \bm{n}]^T$: external force/torque action on the mobile platform at $\bm{O}_B$
|
||||
|
||||
We find that the transpose of the Jacobian matrix links the two by the following equation:
|
||||
\begin{equation*}
|
||||
\bm{\mathcal{F}} = \bm{J}^T \bm{\tau}
|
||||
\end{equation*}
|
||||
|
||||
If the Jacobian matrix is inversible, we also have the following relation:
|
||||
\begin{equation*}
|
||||
\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}
|
||||
\end{equation*}
|
||||
|
||||
* Stiffness Analysis
|
||||
<<sec:stiffness_analysis>>
|
||||
** Introduction :ignore:
|
||||
Here, we focus on the deflections of the manipulator moving platform that are the result of the external applied wrench to the mobile platform.
|
||||
The amount of these deflections are a function of the applied wrench as well as the manipulator *structural stiffness*.
|
||||
|
||||
** Computation of the Stiffness and Compliance Matrix
|
||||
As explain in [[file:stewart-architecture.org][this]] document, each Actuator is modeled by 3 elements in parallel:
|
||||
- A spring with a stiffness $k_{i}$
|
||||
- A dashpot with a damping $c_{i}$
|
||||
|
||||
The stiffness of the actuator $k_i$ links the applied actuator force $\delta \tau_i$ and the corresponding small deflection $\delta l_i$:
|
||||
\begin{equation*}
|
||||
\tau_i = k_i \delta l_i, \quad i = 1,\ \dots,\ 6
|
||||
\end{equation*}
|
||||
If we combine these 6 relations:
|
||||
\begin{equation*}
|
||||
\bm{\tau} = \mathcal{K} \delta \bm{\mathcal{L}} \quad \mathcal{K} = \text{diag}\left[ k_1,\ \dots,\ k_6 \right]
|
||||
\end{equation*}
|
||||
|
||||
Substituting $\bm{\tau} = \bm{J}^{-T} \bm{\mathcal{F}}$ and $\delta \bm{\mathcal{L}} = \bm{J} \cdot \delta \bm{\mathcal{X}}$ gives
|
||||
\begin{equation*}
|
||||
\bm{\mathcal{F}} = \bm{J}^T \mathcal{K} \bm{J} \cdot \delta \bm{\mathcal{X}}
|
||||
\end{equation*}
|
||||
And then we identify the stiffness matrix $\bm{K}$:
|
||||
\begin{equation*}
|
||||
\bm{K} = \bm{J}^T \mathcal{K} \bm{J}
|
||||
\end{equation*}
|
||||
|
||||
If the stiffness matrix $\bm{K}$ is inversible, the *compliance matrix* of the manipulator is defined as
|
||||
\begin{equation*}
|
||||
\bm{C} = \bm{K}^{-1} = (\bm{J}^T \mathcal{K} \bm{J})^{-1}
|
||||
\end{equation*}
|
||||
|
||||
The compliance matrix of a manipulator shows the mapping of the moving platform wrench applied at $\bm{O}_B$ to its small deflection by
|
||||
\begin{equation*}
|
||||
\delta \bm{\mathcal{X}} = \bm{C} \cdot \bm{\mathcal{F}}
|
||||
\end{equation*}
|
||||
|
||||
The stiffness and compliance matrices are computed using the =computeJacobian= function (accessible [[sec:computeJacobian][here]]).
|
||||
|
||||
* Forward and Inverse Kinematics
|
||||
<<sec:forward_inverse_kinematics>>
|
||||
** Inverse Kinematics
|
||||
<<sec:inverse_kinematics>>
|
||||
|
||||
#+begin_quote
|
||||
For *inverse kinematic analysis*, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables $\bm{\mathcal{L}} = \left[ l_1, l_2, l_3, l_4, l_5, l_6 \right]^T$.
|
||||
#+end_quote
|
||||
|
||||
This problem can be easily solved using the loop closures.
|
||||
|
||||
The obtain joint variables are:
|
||||
\begin{equation*}
|
||||
\begin{aligned}
|
||||
l_i = &\Big[ {}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + \dots\\
|
||||
&2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i \Big]^{1/2}
|
||||
\end{aligned}
|
||||
\end{equation*}
|
||||
|
||||
If the position and orientation of the platform lie in the feasible workspace, the solution is unique.
|
||||
Otherwise, the solution gives complex numbers.
|
||||
|
||||
This inverse kinematic solution can be obtained using the function =inverseKinematics= (described [[sec:inverseKinematics][here]]).
|
||||
|
||||
** Forward Kinematics
|
||||
<<sec:forward_kinematics>>
|
||||
|
||||
#+begin_quote
|
||||
In *forward kinematic analysis*, it is assumed that the vector of limb lengths $\bm{L}$ is given and the problem is to find the position ${}^A\bm{P}$ and the orientation ${}^A\bm{R}_B$.
|
||||
#+end_quote
|
||||
|
||||
This is a difficult problem that requires to solve nonlinear equations.
|
||||
|
||||
In a next section, an approximate solution of the forward kinematics problem is proposed for small displacements.
|
||||
|
||||
** Approximate solution of the Forward and Inverse Kinematic problem for small displacement using the Jacobian matrix
|
||||
<<sec:approximate_forward_inverse_kinematics>>
|
||||
|
||||
For small displacements mobile platform displacement $\delta \bm{\mathcal{X}} = [\delta x, \delta y, \delta z, \delta \theta_x, \delta \theta_y, \delta \theta_z ]^T$ around $\bm{\mathcal{X}}_0$, the associated joint displacement can be computed using the Jacobian (approximate solution of the inverse kinematic problem):
|
||||
\begin{equation*}
|
||||
\delta\bm{\mathcal{L}} = \bm{J} \delta\bm{\mathcal{X}}
|
||||
\end{equation*}
|
||||
|
||||
Similarly, for small joint displacements $\delta\bm{\mathcal{L}} = [ \delta l_1,\ \dots,\ \delta l_6 ]^T$ around $\bm{\mathcal{L}}_0$, it is possible to find the induced small displacement of the mobile platform (approximate solution of the forward kinematic problem):
|
||||
\begin{equation*}
|
||||
\delta\bm{\mathcal{X}} = \bm{J}^{-1} \delta\bm{\mathcal{L}}
|
||||
\end{equation*}
|
||||
|
||||
These two relations solve the forward and inverse kinematic problems for small displacement in a *approximate* way.
|
||||
As the inverse kinematic can be easily solved exactly this is not much useful, however, as the forward kinematic problem is difficult to solve, this approximation can be very useful for small displacements.
|
||||
|
||||
The function =forwardKinematicsApprox= (described [[sec:forwardKinematicsApprox][here]]) can be used to solve the forward kinematic problem using the Jacobian matrix.
|
||||
|
||||
** Estimation of the range validity of the approximate inverse kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/approximate_inverse_kinematics_validity.m
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:END:
|
||||
<<sec:approximate_inverse_kinematics_validity>>
|
||||
|
||||
*** Introduction :ignore:
|
||||
As we know how to exactly solve the Inverse kinematic problem, we can compare the exact solution with the approximate solution using the Jacobian matrix.
|
||||
For small displacements, the approximate solution is expected to work well.
|
||||
We would like here to determine up to what displacement this approximation can be considered as correct.
|
||||
|
||||
Then, we can determine the range for which the approximate inverse kinematic is valid.
|
||||
This will also gives us the range for which the approximate forward kinematic is valid.
|
||||
|
||||
*** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
*** Stewart architecture definition
|
||||
We first define some general Stewart architecture.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart);
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
#+end_src
|
||||
|
||||
*** Comparison for "pure" translations
|
||||
Let's first compare the perfect and approximate solution of the inverse for pure $x$ translations.
|
||||
|
||||
We compute the approximate and exact required strut stroke to have the wanted mobile platform $x$ displacement.
|
||||
The estimate required strut stroke for both the approximate and exact solutions are shown in Figure [[fig:inverse_kinematics_approx_validity_x_translation]].
|
||||
The relative strut length displacement is shown in Figure [[fig:inverse_kinematics_approx_validity_x_translation_relative]].
|
||||
#+begin_src matlab
|
||||
Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m]
|
||||
|
||||
Ls_approx = zeros(6, length(Xrs));
|
||||
Ls_exact = zeros(6, length(Xrs));
|
||||
|
||||
for i = 1:length(Xrs)
|
||||
Xr = Xrs(i);
|
||||
L_approx(:, i) = stewart.J*[Xr; 0; 0; 0; 0; 0;];
|
||||
[~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xr; 0; 0]);
|
||||
end
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(Xrs, abs(L_approx(i, :)));
|
||||
set(gca,'ColorOrderIndex',i);
|
||||
plot(Xrs, abs(L_exact(i, :)), '--');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Wanted $x$ displacement [m]');
|
||||
ylabel('Estimated required stroke');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/inverse_kinematics_approx_validity_x_translation.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:inverse_kinematics_approx_validity_x_translation
|
||||
#+CAPTION: Comparison of the Approximate solution and True solution for the Inverse kinematic problem ([[./figs/inverse_kinematics_approx_validity_x_translation.png][png]], [[./figs/inverse_kinematics_approx_validity_x_translation.pdf][pdf]])
|
||||
[[file:figs/inverse_kinematics_approx_validity_x_translation.png]]
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
hold on;
|
||||
for i = 1:6
|
||||
plot(Xrs, abs(L_approx(i, :) - L_exact(i, :))./abs(L_approx(i, :) + L_exact(i, :)), 'k-');
|
||||
end
|
||||
hold off;
|
||||
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
||||
xlabel('Wanted $x$ displacement [m]');
|
||||
ylabel('Relative Stroke Error');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/inverse_kinematics_approx_validity_x_translation_relative.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:inverse_kinematics_approx_validity_x_translation_relative
|
||||
#+CAPTION: Relative length error by using the Approximate solution of the Inverse kinematic problem ([[./figs/inverse_kinematics_approx_validity_x_translation_relative.png][png]], [[./figs/inverse_kinematics_approx_validity_x_translation_relative.pdf][pdf]])
|
||||
[[file:figs/inverse_kinematics_approx_validity_x_translation_relative.png]]
|
||||
|
||||
*** Conclusion
|
||||
For small wanted displacements (up to $\approx 1\%$ of the size of the Hexapod), the approximate inverse kinematic solution using the Jacobian matrix is quite correct.
|
||||
|
||||
* Estimated required actuator stroke from specified platform mobility
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/required_stroke_from_mobility.m
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:END:
|
||||
<<sec:required_actuator_stroke>>
|
||||
** Introduction :ignore:
|
||||
Let's say one want to design a Stewart platform with some specified mobility (position and orientation).
|
||||
One may want to determine the required actuator stroke required to obtain the specified mobility.
|
||||
This is what is analyzed in this section.
|
||||
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Stewart architecture definition
|
||||
Let's first define the Stewart platform architecture that we want to study.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
#+end_src
|
||||
|
||||
** Wanted translations and rotations
|
||||
Let's now define the wanted extreme translations and rotations.
|
||||
#+begin_src matlab
|
||||
Tx_max = 50e-6; % Translation [m]
|
||||
Ty_max = 50e-6; % Translation [m]
|
||||
Tz_max = 50e-6; % Translation [m]
|
||||
Rx_max = 30e-6; % Rotation [rad]
|
||||
Ry_max = 30e-6; % Rotation [rad]
|
||||
Rz_max = 0; % Rotation [rad]
|
||||
#+end_src
|
||||
|
||||
** Needed stroke for "pure" rotations or translations
|
||||
As a first estimation, we estimate the needed actuator stroke for "pure" rotations and translation.
|
||||
We do that using either the Inverse Kinematic solution or the Jacobian matrix as an approximation.
|
||||
|
||||
#+begin_src matlab
|
||||
LTx = stewart.J*[Tx_max 0 0 0 0 0]';
|
||||
LTy = stewart.J*[0 Ty_max 0 0 0 0]';
|
||||
LTz = stewart.J*[0 0 Tz_max 0 0 0]';
|
||||
LRx = stewart.J*[0 0 0 Rx_max 0 0]';
|
||||
LRy = stewart.J*[0 0 0 0 Ry_max 0]';
|
||||
LRz = stewart.J*[0 0 0 0 0 Rz_max]';
|
||||
#+end_src
|
||||
|
||||
The obtain required stroke is:
|
||||
#+begin_src matlab :results value replace :exports results
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', min(min([LTx,LTy,LTz,LRx,LRy])), max(max([LTx,LTy,LTz,LRx,LRy])), 1e6*(max(max([LTx,LTy,LTz,LRx,LRy]))-min(min([LTx,LTy,LTz,LRx,LRy]))))
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
: From -3.8e-05[m] to 3.8e-05[m]: Total stroke = 76.1[um]
|
||||
|
||||
This is surely a low estimation of the required stroke.
|
||||
|
||||
** Needed stroke for "combined" rotations or translations
|
||||
We know would like to have a more precise estimation.
|
||||
|
||||
To do so, we may estimate the required actuator stroke for all possible combination of translation and rotation.
|
||||
|
||||
Let's first generate all the possible combination of maximum translation and rotations.
|
||||
#+begin_src matlab
|
||||
Ps = [2*(dec2bin(0:5^2-1,5)-'0')-1, zeros(5^2, 1)].*[Tx_max Ty_max Tz_max Rx_max Ry_max Rz_max];
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable(Ps, {}, {'*Tx [m]*', '*Ty [m]*', '*Tz [m]*', '*Rx [rad]*', '*Ry [rad]*', '*Rz [rad]*'}, ' %.1e ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| *Tx [m]* | *Ty [m]* | *Tz [m]* | *Rx [rad]* | *Ry [rad]* | *Rz [rad]* |
|
||||
|----------+----------+----------+------------+------------+------------|
|
||||
| -5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | -5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | -5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | -5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | 5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | 5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | 5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| -5.0e-05 | 5.0e-05 | 5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | -5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | -5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | 5.0e-05 | -3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | -5.0e-05 | 5.0e-05 | 3.0e-05 | 3.0e-05 | 0.0e+00 |
|
||||
| 5.0e-05 | 5.0e-05 | -5.0e-05 | -3.0e-05 | -3.0e-05 | 0.0e+00 |
|
||||
|
||||
For all possible combination, we compute the required actuator stroke using the inverse kinematic solution.
|
||||
#+begin_src matlab
|
||||
L_min = 0;
|
||||
L_max = 0;
|
||||
|
||||
for i = 1:size(Ps,1)
|
||||
Rx = [1 0 0;
|
||||
0 cos(Ps(i, 4)) -sin(Ps(i, 4));
|
||||
0 sin(Ps(i, 4)) cos(Ps(i, 4))];
|
||||
|
||||
Ry = [ cos(Ps(i, 5)) 0 sin(Ps(i, 5));
|
||||
0 1 0;
|
||||
-sin(Ps(i, 5)) 0 cos(Ps(i, 5))];
|
||||
|
||||
Rz = [cos(Ps(i, 6)) -sin(Ps(i, 6)) 0;
|
||||
sin(Ps(i, 6)) cos(Ps(i, 6)) 0;
|
||||
0 0 1];
|
||||
|
||||
ARB = Rz*Ry*Rx;
|
||||
[~, Ls] = inverseKinematics(stewart, 'AP', Ps(i, 1:3)', 'ARB', ARB);
|
||||
|
||||
if min(Ls) < L_min
|
||||
L_min = min(Ls)
|
||||
end
|
||||
if max(Ls) > L_max
|
||||
L_max = max(Ls)
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
We obtain the required actuator stroke:
|
||||
#+begin_src matlab :results value replace :exports results
|
||||
ans = sprintf('From %.2g[m] to %.2g[m]: Total stroke = %.1f[um]', L_min, L_max, 1e6*(L_max-L_min))
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
: From -8.9e-05[m] to 8.9e-05[m]: Total stroke = 177.2[um]
|
||||
|
||||
This is probably a much realistic estimation of the required actuator stroke.
|
||||
|
||||
* Estimated platform mobility from specified actuator stroke
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/mobility_from_actuator_stroke.m
|
||||
:header-args:matlab+: :comments org :mkdirp yes
|
||||
:END:
|
||||
<<sec:obtained_mobility_from_stroke>>
|
||||
** Introduction :ignore:
|
||||
Here, from some value of the actuator stroke, we would like to estimate the mobility of the Stewart platform.
|
||||
|
||||
As explained in section [[sec:forward_inverse_kinematics]], the forward kinematic problem of the Stewart platform is quite difficult to solve.
|
||||
However, for small displacements, we can use the Jacobian as an approximate solution.
|
||||
|
||||
** Matlab Init :noexport:ignore:
|
||||
#+begin_src matlab :tangle no :exports none :results silent :noweb yes :var current_dir=(file-name-directory buffer-file-name)
|
||||
<<matlab-dir>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :exports none :results silent :noweb yes
|
||||
<<matlab-init>>
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab :results none :exports none
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
** Stewart architecture definition
|
||||
Let's first define the Stewart platform architecture that we want to study.
|
||||
#+begin_src matlab
|
||||
stewart = initializeStewartPlatform();
|
||||
stewart = initializeFramesPositions(stewart, 'H', 90e-3, 'MO_B', 45e-3);
|
||||
stewart = generateGeneralConfiguration(stewart);
|
||||
stewart = computeJointsPose(stewart);
|
||||
stewart = initializeStewartPose(stewart);
|
||||
stewart = initializeCylindricalPlatforms(stewart);
|
||||
stewart = initializeCylindricalStruts(stewart);
|
||||
stewart = initializeStrutDynamics(stewart, 'Ki', 1e6*ones(6,1), 'Ci', 1e2*ones(6,1));
|
||||
stewart = initializeJointDynamics(stewart);
|
||||
stewart = computeJacobian(stewart);
|
||||
#+end_src
|
||||
|
||||
Let's now define the actuator stroke.
|
||||
#+begin_src matlab
|
||||
L_min = -50e-6; % [m]
|
||||
L_max = 50e-6; % [m]
|
||||
#+end_src
|
||||
|
||||
** Pure translations
|
||||
Let's first estimate the mobility in translation when the orientation of the Stewart platform stays the same.
|
||||
|
||||
As shown previously, for such small stroke, we can use the approximate Forward Dynamics solution using the Jacobian matrix:
|
||||
\begin{equation*}
|
||||
\delta\bm{\mathcal{L}} = \bm{J} \delta\bm{\mathcal{X}}
|
||||
\end{equation*}
|
||||
|
||||
To obtain the mobility "volume" attainable by the Stewart platform when it's orientation is set to zero, we use the spherical coordinate $(r, \theta, \phi)$.
|
||||
|
||||
For each possible value of $(\theta, \phi)$, we compute the maximum radius $r$ attainable with the constraint that the stroke of each actuator should be between =L_min= and =L_max=.
|
||||
#+begin_src matlab
|
||||
thetas = linspace(0, pi, 50);
|
||||
phis = linspace(0, 2*pi, 50);
|
||||
rs = zeros(length(thetas), length(phis));
|
||||
|
||||
for i = 1:length(thetas)
|
||||
for j = 1:length(phis)
|
||||
Tx = sin(thetas(i))*cos(phis(j));
|
||||
Ty = sin(thetas(i))*sin(phis(j));
|
||||
Tz = cos(thetas(i));
|
||||
|
||||
dL = stewart.J*[Tx; Ty; Tz; 0; 0; 0;]; % dL required for 1m displacement in theta/phi direction
|
||||
|
||||
rs(i, j) = max([dL(dL<0)*L_min; dL(dL>0)*L_max]);
|
||||
end
|
||||
end
|
||||
#+end_src
|
||||
|
||||
|
||||
Now that we have found the corresponding radius $r$, we plot the obtained mobility.
|
||||
We can also approximate the mobility by a sphere with a radius equal to the minimum obtained value of $r$, this is however a pessimistic estimation of the mobility.
|
||||
|
||||
#+begin_src matlab :exports results :results value table replace :tangle no :post addhdr(*this*)
|
||||
data2orgtable([1e6*L_min, 1e6*L_max, 1e6*(min(min(rs)))], {}, {'=L_min= [$\mu m$]', '=L_max= [$\mu m$]', '=R= [$\mu m$]'}, ' %.1f ');
|
||||
#+end_src
|
||||
|
||||
#+RESULTS:
|
||||
| =L_min= [$\mu m$] | =L_max= [$\mu m$] | =R= [$\mu m$] |
|
||||
|-------------------+-------------------+---------------|
|
||||
| -50.0 | 50.0 | 31.5 |
|
||||
|
||||
#+begin_src matlab :exports none
|
||||
figure;
|
||||
plot3(reshape(rs.*(sin(thetas)'*cos(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(sin(thetas)'*sin(phis)), [1, length(thetas)*length(phis)]), ...
|
||||
reshape(rs.*(cos(thetas)'*ones(1, length(phis))), [1, length(thetas)*length(phis)]))
|
||||
xlabel('X Translation [m]');
|
||||
ylabel('Y Translation [m]');
|
||||
zlabel('Z Translation [m]');
|
||||
#+end_src
|
||||
|
||||
#+HEADER: :tangle no :exports results :results none :noweb yes
|
||||
#+begin_src matlab :var filepath="figs/mobility_translations_null_rotation.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
|
||||
<<plt-matlab>>
|
||||
#+end_src
|
||||
|
||||
#+NAME: fig:mobility_translations_null_rotation
|
||||
#+CAPTION: Obtain mobility of the Stewart platform for zero rotations ([[./figs/mobility_translations_null_rotation.png][png]], [[./figs/mobility_translations_null_rotation.pdf][pdf]])
|
||||
[[file:figs/mobility_translations_null_rotation.png]]
|
||||
|
||||
*** TODO Do that by slice :noexport:
|
||||
using this function https://fr.mathworks.com/help/matlab/ref/contour3.html
|
||||
|
||||
* Functions
|
||||
<<sec:functions>>
|
||||
** =computeJacobian=: Compute the Jacobian Matrix
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle src/computeJacobian.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:computeJacobian>>
|
||||
|
||||
This Matlab function is accessible [[file:src/computeJacobian.m][here]].
|
||||
|
||||
*** Function description
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [stewart] = computeJacobian(stewart)
|
||||
% computeJacobian -
|
||||
%
|
||||
% Syntax: [stewart] = computeJacobian(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - With at least the following fields:
|
||||
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
||||
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
||||
% - actuators.K [6x1] - Total stiffness of the actuators
|
||||
%
|
||||
% Outputs:
|
||||
% - stewart - With the 3 added field:
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - kinematics.K [6x6] - The Stiffness Matrix
|
||||
% - kinematics.C [6x6] - The Compliance Matrix
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
||||
As = stewart.geometry.As;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
||||
Ab = stewart.geometry.Ab;
|
||||
|
||||
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
||||
Ki = stewart.actuators.K;
|
||||
#+end_src
|
||||
|
||||
|
||||
*** Compute Jacobian Matrix
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
J = [As' , cross(Ab, As)'];
|
||||
#+end_src
|
||||
|
||||
*** Compute Stiffness Matrix
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
K = J'*diag(Ki)*J;
|
||||
#+end_src
|
||||
|
||||
*** Compute Compliance Matrix
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
C = inv(K);
|
||||
#+end_src
|
||||
|
||||
*** Populate the =stewart= structure
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
stewart.kinematics.J = J;
|
||||
stewart.kinematics.K = K;
|
||||
stewart.kinematics.C = C;
|
||||
#+end_src
|
||||
|
||||
|
||||
** =inverseKinematics=: Compute Inverse Kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle src/inverseKinematics.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:inverseKinematics>>
|
||||
|
||||
This Matlab function is accessible [[file:src/inverseKinematics.m][here]].
|
||||
|
||||
*** Theory
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
|
||||
|
||||
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
|
||||
\begin{align*}
|
||||
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
||||
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
||||
\end{align*}
|
||||
|
||||
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
|
||||
\begin{equation}
|
||||
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
||||
\end{equation}
|
||||
|
||||
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
||||
\begin{equation}
|
||||
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
||||
\end{equation}
|
||||
|
||||
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
||||
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
||||
|
||||
*** Function description
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [Li, dLi] = inverseKinematics(stewart, args)
|
||||
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
||||
%
|
||||
% Syntax: [stewart] = inverseKinematics(stewart)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
||||
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
||||
% - geometry.l [6x1] - Length of each strut
|
||||
% - args - Can have the following fields:
|
||||
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
||||
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
||||
%
|
||||
% Outputs:
|
||||
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
||||
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
||||
Aa = stewart.geometry.Aa;
|
||||
|
||||
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
||||
Bb = stewart.geometry.Bb;
|
||||
|
||||
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
||||
l = stewart.geometry.l;
|
||||
#+end_src
|
||||
|
||||
|
||||
*** Compute
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
||||
#+end_src
|
||||
|
||||
#+begin_src matlab
|
||||
dLi = Li-l;
|
||||
#+end_src
|
||||
|
||||
** =forwardKinematicsApprox=: Compute the Approximate Forward Kinematics
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle src/forwardKinematicsApprox.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:forwardKinematicsApprox>>
|
||||
|
||||
This Matlab function is accessible [[file:src/forwardKinematicsApprox.m][here]].
|
||||
|
||||
*** Function description
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
function [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
% forwardKinematicsApprox - Computed the approximate pose of {B} with respect to {A} from the length of each strut and using
|
||||
% the Jacobian Matrix
|
||||
%
|
||||
% Syntax: [P, R] = forwardKinematicsApprox(stewart, args)
|
||||
%
|
||||
% Inputs:
|
||||
% - stewart - A structure with the following fields
|
||||
% - kinematics.J [6x6] - The Jacobian Matrix
|
||||
% - args - Can have the following fields:
|
||||
% - dL [6x1] - Displacement of each strut [m]
|
||||
%
|
||||
% Outputs:
|
||||
% - P [3x1] - The estimated position of {B} with respect to {A}
|
||||
% - R [3x3] - The estimated rotation matrix that gives the orientation of {B} with respect to {A}
|
||||
#+end_src
|
||||
|
||||
*** Optional Parameters
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
arguments
|
||||
stewart
|
||||
args.dL (6,1) double {mustBeNumeric} = zeros(6,1)
|
||||
end
|
||||
#+end_src
|
||||
|
||||
*** Check the =stewart= structure elements
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
#+begin_src matlab
|
||||
assert(isfield(stewart.kinematics, 'J'), 'stewart.kinematics should have attribute J')
|
||||
J = stewart.kinematics.J;
|
||||
#+end_src
|
||||
|
||||
*** Computation
|
||||
:PROPERTIES:
|
||||
:UNNUMBERED: t
|
||||
:END:
|
||||
From a small displacement of each strut $d\bm{\mathcal{L}}$, we can compute the
|
||||
position and orientation of {B} with respect to {A} using the following formula:
|
||||
\[ d \bm{\mathcal{X}} = \bm{J}^{-1} d\bm{\mathcal{L}} \]
|
||||
#+begin_src matlab
|
||||
X = J\args.dL;
|
||||
#+end_src
|
||||
|
||||
The position vector corresponds to the first 3 elements.
|
||||
#+begin_src matlab
|
||||
P = X(1:3);
|
||||
#+end_src
|
||||
|
||||
The next 3 elements are the orientation of {B} with respect to {A} expressed
|
||||
using the screw axis.
|
||||
#+begin_src matlab
|
||||
theta = norm(X(4:6));
|
||||
s = X(4:6)/theta;
|
||||
#+end_src
|
||||
|
||||
We then compute the corresponding rotation matrix.
|
||||
#+begin_src matlab
|
||||
R = [s(1)^2*(1-cos(theta)) + cos(theta) , s(1)*s(2)*(1-cos(theta)) - s(3)*sin(theta), s(1)*s(3)*(1-cos(theta)) + s(2)*sin(theta);
|
||||
s(2)*s(1)*(1-cos(theta)) + s(3)*sin(theta), s(2)^2*(1-cos(theta)) + cos(theta), s(2)*s(3)*(1-cos(theta)) - s(1)*sin(theta);
|
||||
s(3)*s(1)*(1-cos(theta)) - s(2)*sin(theta), s(3)*s(2)*(1-cos(theta)) + s(1)*sin(theta), s(3)^2*(1-cos(theta)) + cos(theta)];
|
||||
#+end_src
|
||||
|
||||
* Bibliography :ignore:
|
||||
bibliographystyle:unsrt
|
||||
bibliography:ref.bib
|
||||
247
org/simscape-model.org
Normal file
247
org/simscape-model.org
Normal file
@@ -0,0 +1,247 @@
|
||||
#+TITLE: Stewart Platform - Simscape Model
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Introduction :ignore:
|
||||
In this document is explained how the Simscape model of the Stewart Platform is implemented.
|
||||
|
||||
It is divided in the following sections:
|
||||
- section [[sec:simscape_parameters]]: is explained how the parameters of the Stewart platform are set for the Simscape model
|
||||
- section [[sec:simulink_configuration]]: the Simulink configuration (solver, simulation time, ...) is shared among all the Simulink files. It is explain how this is done.
|
||||
- section [[sec:subsystem_reference]]: All the elements (platforms, struts, sensors, ...) are saved in separate files and imported in Simulink files using "subsystem referenced".
|
||||
- section [[sec:fixed_mobile_platforms]]: The simscape model for the fixed base and mobile platform are described in this section.
|
||||
- section [[sec:struts]]: The simscape model for the Stewart platform struts is described in this section.
|
||||
|
||||
* Parameters used for the Simscape Model
|
||||
<<sec:simscape_parameters>>
|
||||
The Simscape Model of the Stewart Platform is working with the =stewart= structure generated using the functions described [[file:stewart-architecture.org][here]].
|
||||
|
||||
All the geometry and inertia of the mechanical elements are defined in the =stewart= structure.
|
||||
|
||||
By updating the =stewart= structure in the workspace, the Simscape model will be automatically updated.
|
||||
|
||||
Thus, nothing should be changed by hand inside the Simscape model.
|
||||
|
||||
The main advantage to have all the parameters defined in one structure (and not hard-coded in some simulink blocs) it that we can easily change the Stewart architecture/parameters in a Matlab script to perform some parametric study for instance.
|
||||
|
||||
|
||||
* Simulation Configuration - Configuration reference
|
||||
<<sec:simulink_configuration>>
|
||||
As multiple simulink files will be used for simulation and tests, it is very useful to determine good simulation configuration that will be *shared* among all the simulink files.
|
||||
|
||||
This is done using something called "*Configuration Reference*" ([[https://fr.mathworks.com/help/simulink/ug/more-about-configuration-references.html][documentation]]).
|
||||
|
||||
Basically, the configuration is stored in a mat file =conf_simscape.mat= and then loaded in the workspace for it to be accessible to all the simulink models.
|
||||
It is automatically loaded when the Simulink project is open. It can be loaded manually with the command:
|
||||
#+begin_src matlab :eval no
|
||||
load('mat/conf_simscape.mat');
|
||||
#+end_src
|
||||
|
||||
It is however possible to modify specific parameters just for one simulation using the =set_param= command:
|
||||
#+begin_src matlab :eval no
|
||||
set_param(conf_simscape, 'StopTime', 1);
|
||||
#+end_src
|
||||
|
||||
* Subsystem Reference
|
||||
<<sec:subsystem_reference>>
|
||||
Several Stewart platform models are used, for instance one is use to study the dynamics while the other is used to apply active damping techniques.
|
||||
|
||||
However, all the Simscape models share some subsystems using the *Subsystem Reference* Simulink block ([[https://fr.mathworks.com/help/simulink/ug/referenced-subsystem-1.html][documentation]]).
|
||||
|
||||
These shared subsystems are:
|
||||
- =Fixed_Based.slx= - Fixed base of the Stewart Platform
|
||||
- =Mobile_Platform.slx= - Mobile platform of the Stewart Platform
|
||||
- =stewart_strut.slx= - One strut containing two spherical/universal joints, the actuator as well as the included sensors. A parameter =i= is initialized to determine what it the "number" of the strut.
|
||||
|
||||
These subsystems are referenced from another subsystem called =Stewart_Platform.slx= shown in figure [[fig:simscape_stewart_platform]], that basically connect them correctly.
|
||||
This subsystem is then referenced in other simulink models for various purposes (control, analysis, simulation, ...).
|
||||
|
||||
#+name: fig:simscape_stewart_platform
|
||||
#+caption: Simscape Subsystem of the Stewart platform. Encapsulate the Subsystems corresponding to the fixed base, mobile platform and all the struts.
|
||||
[[file:figs/simscape_stewart_platform.png]]
|
||||
|
||||
* Subsystem - Fixed base and Mobile Platform
|
||||
<<sec:fixed_mobile_platforms>>
|
||||
Both the fixed base and the mobile platform simscape models share many similarities.
|
||||
|
||||
Their are both composed of:
|
||||
- a solid body representing the platform
|
||||
- 6 rigid transform blocks to go from the frame $\{F\}$ (resp. $\{M\}$) to the location of the joints.
|
||||
These rigid transform are using ${}^F\bm{a}_i$ (resp. ${}^M\bm{b}_i$) for the position of the joint and ${}^F\bm{R}_{a_i}$ (resp. ${}^M\bm{R}_{b_i}$) for the orientation of the joint.
|
||||
|
||||
As always, the parameters that define the geometry are taken from the =stewart= structure.
|
||||
|
||||
#+name: fig:simscape_fixed_base
|
||||
#+caption: Simscape Model of the Fixed base
|
||||
#+attr_html: :width 1000px
|
||||
[[file:figs/simscape_fixed_base.png]]
|
||||
|
||||
#+name: fig:simscape_mobile_platform
|
||||
#+caption: Simscape Model of the Mobile platform
|
||||
#+attr_html: :width 800px
|
||||
[[file:figs/simscape_mobile_platform.png]]
|
||||
|
||||
* Subsystem - Struts
|
||||
<<sec:struts>>
|
||||
** Strut Configuration
|
||||
For the Stewart platform, the 6 struts are identical.
|
||||
Thus, all the struts used in the Stewart platform are referring to the same subsystem called =stewart_strut.slx= and shown in Figure [[fig:simscape_strut]].
|
||||
|
||||
This strut as the following structure:
|
||||
- *Universal Joint** connected on the Fixed base
|
||||
- *Prismatic Joint** for the actuator
|
||||
- *Spherical Joint** connected on the Mobile platform
|
||||
|
||||
This configuration is called *UPS*.
|
||||
|
||||
The other common configuration *SPS* has the disadvantage of having additional passive degrees-of-freedom corresponding to the rotation of the strut around its main axis.
|
||||
This is why the *UPS* configuration is used, but other configuration can be easily implemented.
|
||||
|
||||
#+name: fig:simscape_strut
|
||||
#+caption: Simscape model of the Stewart platform's strut
|
||||
#+attr_html: :width 800px
|
||||
[[file:figs/simscape_strut.png]]
|
||||
|
||||
Several sensors are included in the strut that may or may not be used for control:
|
||||
- Relative Displacement sensor: gives the relative displacement of the strut.
|
||||
- Force sensor: measure the total force applied by the force actuator, the stiffness and damping forces in the direction of the strut.
|
||||
- Inertial sensor: measure the absolute motion (velocity) of the top part of the strut in the direction of the strut.
|
||||
|
||||
There is two main types of inertial sensor that can be used to measure the absolute motion of the top part of the strut in the direction of the strut:
|
||||
- a geophone that measures the absolute velocity above some frequency
|
||||
- an accelerometer that measures the absolute acceleration below some frequency
|
||||
|
||||
Both inertial sensors are described bellow.
|
||||
|
||||
* Other Elements
|
||||
** Z-Axis Geophone
|
||||
*** Working Principle
|
||||
From the schematic of the Z-axis geophone shown in Figure [[fig:z_axis_geophone]], we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
|
||||
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
||||
with:
|
||||
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
||||
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
||||
|
||||
#+name: fig:z_axis_geophone
|
||||
#+caption: Schematic of a Z-Axis geophone
|
||||
[[file:figs/inertial_sensor.png]]
|
||||
|
||||
We see that at frequencies above $\omega_0$:
|
||||
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
|
||||
|
||||
And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
|
||||
|
||||
We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.
|
||||
|
||||
*** Initialization function
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ./src/initializeZAxisGeophone.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeZAxisGeophone>>
|
||||
|
||||
This Matlab function is accessible [[file:../src/initializeZAxisGeophone.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function [geophone] = initializeZAxisGeophone(args)
|
||||
arguments
|
||||
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
|
||||
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 1 % [Hz]
|
||||
end
|
||||
|
||||
%%
|
||||
geophone.m = args.mass;
|
||||
|
||||
%% The Stiffness is set to have the damping resonance frequency
|
||||
geophone.k = geophone.m * (2*pi*args.freq)^2;
|
||||
|
||||
%% We set the damping value to have critical damping
|
||||
geophone.c = 2*sqrt(geophone.m * geophone.k);
|
||||
|
||||
%% Save
|
||||
save('./mat/geophone_z_axis.mat', 'geophone');
|
||||
end
|
||||
#+end_src
|
||||
|
||||
** Z-Axis Accelerometer
|
||||
*** Working Principle
|
||||
From the schematic of the Z-axis accelerometer shown in Figure [[fig:z_axis_accelerometer]], we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
|
||||
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
||||
with:
|
||||
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
||||
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
||||
|
||||
#+name: fig:z_axis_accelerometer
|
||||
#+caption: Schematic of a Z-Axis geophone
|
||||
[[file:figs/inertial_sensor.png]]
|
||||
|
||||
We see that at frequencies below $\omega_0$:
|
||||
\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
|
||||
|
||||
And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
|
||||
|
||||
Note that there is trade-off between:
|
||||
- the highest measurable acceleration $\omega_0$
|
||||
- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$
|
||||
|
||||
*** Initialization function
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle ./src/initializeZAxisAccelerometer.m
|
||||
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
||||
:END:
|
||||
<<sec:initializeZAxisAccelerometer>>
|
||||
|
||||
This Matlab function is accessible [[file:../src/initializeZAxisAccelerometer.m][here]].
|
||||
|
||||
#+begin_src matlab
|
||||
function [accelerometer] = initializeZAxisAccelerometer(args)
|
||||
arguments
|
||||
args.mass (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % [kg]
|
||||
args.freq (1,1) double {mustBeNumeric, mustBePositive} = 5e3 % [Hz]
|
||||
end
|
||||
|
||||
%%
|
||||
accelerometer.m = args.mass;
|
||||
|
||||
%% The Stiffness is set to have the damping resonance frequency
|
||||
accelerometer.k = accelerometer.m * (2*pi*args.freq)^2;
|
||||
|
||||
%% We set the damping value to have critical damping
|
||||
accelerometer.c = 2*sqrt(accelerometer.m * accelerometer.k);
|
||||
|
||||
%% Gain correction of the accelerometer to have a unity gain until the resonance
|
||||
accelerometer.gain = -accelerometer.k/accelerometer.m;
|
||||
|
||||
%% Save
|
||||
save('./mat/accelerometer_z_axis.mat', 'accelerometer');
|
||||
end
|
||||
#+end_src
|
||||
|
||||
88
org/simulink-project.org
Normal file
88
org/simulink-project.org
Normal file
@@ -0,0 +1,88 @@
|
||||
#+TITLE: Simulink Project for the Stewart Simscape folder
|
||||
:DRAWER:
|
||||
#+STARTUP: overview
|
||||
|
||||
#+LANGUAGE: en
|
||||
#+EMAIL: dehaeze.thomas@gmail.com
|
||||
#+AUTHOR: Dehaeze Thomas
|
||||
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+OPTIONS: toc:nil
|
||||
#+OPTIONS: html-postamble:nil
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/zenburn.css"/>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script type="text/javascript" src="./js/readtheorg.js"></script>
|
||||
|
||||
#+HTML_MATHJAX: align: center tagside: right font: TeX
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
|
||||
#+PROPERTY: header-args:shell :eval no-export
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
:END:
|
||||
|
||||
A Simulink Project is used for the study of Stewart platforms using Simscape.
|
||||
|
||||
From the [[https://mathworks.com/products/simulink/projects.html][Simulink project]] mathworks page:
|
||||
#+begin_quote
|
||||
Simulink® and Simulink Projects provide a collaborative, scalable environment that enables teams to manage their files and data in one place.
|
||||
|
||||
With Simulink Projects, you can:
|
||||
- *Collaborate*: Enforce companywide standards such as company tools, libraries, and standard startup and shutdown scripts. Share your work with rich sharing options including MATLAB® toolboxes, email, and archives.
|
||||
- *Automate*: Set up your project environment correctly every time by automating steps such as loading the data, managing the path, and opening the models.
|
||||
- *Integrate with source control*: Enable easy integration with source control and configuration management tools.
|
||||
#+end_quote
|
||||
|
||||
The project can be opened using the =simulinkproject= function:
|
||||
|
||||
#+begin_src matlab
|
||||
simulinkproject('./');
|
||||
#+end_src
|
||||
|
||||
When the project opens, a startup script is ran.
|
||||
The startup script is defined below and is exported to the =project_startup.m= script.
|
||||
#+begin_src matlab :eval no :tangle ./src/project_startup.m
|
||||
project = simulinkproject;
|
||||
projectRoot = project.RootFolder;
|
||||
|
||||
myCacheFolder = fullfile(projectRoot, '.SimulinkCache');
|
||||
myCodeFolder = fullfile(projectRoot, '.SimulinkCode');
|
||||
|
||||
Simulink.fileGenControl('set',...
|
||||
'CacheFolder', myCacheFolder,...
|
||||
'CodeGenFolder', myCodeFolder,...
|
||||
'createDir', true);
|
||||
|
||||
%% Load the Simscape Configuration
|
||||
load('mat/conf_simscape.mat');
|
||||
#+end_src
|
||||
|
||||
When the project closes, it runs the =project_shutdown.m= script defined below.
|
||||
#+begin_src matlab :eval no :tangle ./src/project_shutdown.m
|
||||
Simulink.fileGenControl('reset');
|
||||
#+end_src
|
||||
|
||||
The project also permits to automatically add defined folder to the path when the project is opened.
|
||||
58
org/static-analysis.org
Normal file
58
org/static-analysis.org
Normal file
@@ -0,0 +1,58 @@
|
||||
#+TITLE: Stewart Platform - Static Analysis
|
||||
:DRAWER:
|
||||
#+HTML_LINK_HOME: ./index.html
|
||||
#+HTML_LINK_UP: ./index.html
|
||||
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/htmlize.css"/>
|
||||
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="./css/readtheorg.css"/>
|
||||
#+HTML_HEAD: <script src="./js/jquery.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/bootstrap.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/jquery.stickytableheaders.min.js"></script>
|
||||
#+HTML_HEAD: <script src="./js/readtheorg.js"></script>
|
||||
|
||||
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/thesis/latex/}{config.tex}")
|
||||
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
||||
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
||||
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
||||
#+PROPERTY: header-args:latex+ :results raw replace :buffer no
|
||||
#+PROPERTY: header-args:latex+ :eval no-export
|
||||
#+PROPERTY: header-args:latex+ :exports both
|
||||
#+PROPERTY: header-args:latex+ :mkdirp yes
|
||||
#+PROPERTY: header-args:latex+ :output-dir figs
|
||||
|
||||
#+PROPERTY: header-args:matlab :session *MATLAB*
|
||||
#+PROPERTY: header-args:matlab+ :comments org
|
||||
#+PROPERTY: header-args:matlab+ :exports both
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+PROPERTY: header-args:matlab+ :output-dir figs
|
||||
:END:
|
||||
|
||||
* Coupling
|
||||
What causes the coupling from $F_i$ to $X_i$ ?
|
||||
|
||||
#+begin_src latex :file coupling.pdf :post pdf2svg(file=*this*, ext="png") :exports both
|
||||
\begin{tikzpicture}
|
||||
\node[block] (Jt) at (0, 0) {$J^{-T}$};
|
||||
\node[block, right= of Jt] (G) {$G$};
|
||||
\node[block, right= of G] (J) {$J^{-1}$};
|
||||
|
||||
\draw[->] ($(Jt.west)+(-0.8, 0)$) -- (Jt.west) node[above left]{$F_i$};
|
||||
\draw[->] (Jt.east) -- (G.west) node[above left]{$\tau_i$};
|
||||
\draw[->] (G.east) -- (J.west) node[above left]{$q_i$};
|
||||
\draw[->] (J.east) -- ++(0.8, 0) node[above left]{$X_i$};
|
||||
\end{tikzpicture}
|
||||
#+end_src
|
||||
|
||||
#+name: fig:block_diag_coupling
|
||||
#+caption: Block diagram to control an hexapod
|
||||
#+RESULTS:
|
||||
[[file:figs/coupling.png]]
|
||||
|
||||
There is no coupling from $F_i$ to $X_j$ if $J^{-1} G J^{-T}$ is diagonal.
|
||||
|
||||
If $G$ is diagonal (cubic configuration), then $J^{-1} G J^{-T} = G J^{-1} J^{-T} = G (J^{T} J)^{-1} = G K^{-1}$
|
||||
|
||||
Thus, the system is uncoupled if $G$ and $K$ are diagonal.
|
||||
1695
org/stewart-architecture.org
Normal file
1695
org/stewart-architecture.org
Normal file
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user