tdehaeze/stewart-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
8173db25fb
stewart-simscape/org/control-vibration-isolation.org

1392 lines
49 KiB
Org Mode
Raw Blame History

#+TITLE: Stewart Platform - Vibration Isolation
:DRAWER:
#+STARTUP: overview
#+LANGUAGE: en
#+EMAIL: dehaeze.thomas@gmail.com
#+AUTHOR: Dehaeze Thomas
#+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/tikz/org/}{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 results
#+PROPERTY: header-args:latex+ :mkdirp yes
#+PROPERTY: header-args:latex+ :output-dir figs
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
:END:
* Introduction :ignore:
* HAC-LAC (Cascade) Control - Integral Control
** Introduction
In this section, we wish to study the use of the High Authority Control - Low Authority Control (HAC-LAC) architecture on the Stewart platform.
The control architectures are shown in Figures [[fig:control_arch_hac_iff]] and [[fig:control_arch_hac_dvf]].
First, the LAC loop is closed (the LAC control is described [[file:active-damping.org][here]]), and then the HAC controller is designed and the outer loop is closed.
#+begin_src latex :file control_arch_hac_iff.pdf
\begin{tikzpicture}
% Blocs
\node[block={2.0cm}{2.0cm}] (P) {};
\node[above] at (P.north) {Plant};
\node[block, below=0.7 of P] (Kiff) {$\bm{K}_\text{IFF}$};
\node[block, below=0.7 of Kiff] (Khac) {$\bm{K}_\text{HAC}$};
% Add
\node[addb, left=1 of P] (add) {};
\node[block, left=1 of add] (J) {$\bm{J}^{-T}$};
% Input and outputs coordinates
\coordinate[] (outputhac) at ($(P.south east)!0.75!(P.north east)$);
\coordinate[] (outputiff) at ($(P.south east)!0.25!(P.north east)$);
\draw[->] (outputiff) node[above right]{$\bm{\tau}_m$} -- ++(0.8, 0) |- (Kiff.east);
\draw[->] (outputhac) node[above right]{$\bm{\mathcal{X}}$} -- ++(1.6, 0) |- (Khac.east);
\draw[->] (Kiff.west) -| (add.south);
\draw[->] (J.east) -- (add.west);
\draw[<-] (J.west) node[above left]{$\bm{\mathcal{F}}$} -- ++(-0.8, 0) |- (Khac.west);
\draw[->] (add.east) -- (P.west) node[above left]{$\bm{\tau}$};
\end{tikzpicture}
#+end_src
#+name: fig:control_arch_hac_iff
#+caption: HAC-LAC architecture with IFF
#+RESULTS:
[[file:figs/control_arch_hac_iff.png]]
#+begin_src latex :file control_arch_hac_dvf.pdf
\begin{tikzpicture}
% Blocs
\node[block={2.0cm}{2.0cm}] (P) {};
\node[above] at (P.north) {Plant};
\node[block, below=0.7 of P] (Kdvf) {$\bm{K}_\text{DVF}$};
\node[block, below=0.7 of Kdvf] (Khac) {$\bm{K}_\text{HAC}$};
% Add
\node[addb, left=1 of P] (add) {};
\node[block, left=1 of add] (J) {$\bm{J}^{-T}$};
% Input and outputs coordinates
\coordinate[] (outputhac) at ($(P.south east)!0.75!(P.north east)$);
\coordinate[] (outputdvf) at ($(P.south east)!0.25!(P.north east)$);
\draw[->] (outputdvf) node[above right]{$\delta \bm{\mathcal{L}}_m$} -- ++(0.8, 0) |- (Kdvf.east);
\draw[->] (outputhac) node[above right]{$\bm{\mathcal{X}}$} -- ++(1.6, 0) |- (Khac.east);
\draw[->] (Kdvf.west) -| (add.south);
\draw[->] (J.east) -- (add.west);
\draw[<-] (J.west) node[above left]{$\bm{\mathcal{F}}$} -- ++(-0.8, 0) |- (Khac.west);
\draw[->] (add.east) -- (P.west) node[above left]{$\bm{\tau}$};
\end{tikzpicture}
#+end_src
#+name: fig:control_arch_hac_dvf
#+caption: HAC-LAC architecture with DVF
#+RESULTS:
[[file:figs/control_arch_hac_dvf.png]]
** 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
#+begin_src matlab
open('stewart_platform_model.slx')
#+end_src
** Initialization
We first initialize the Stewart platform.
#+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, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src
The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src
** Identification
*** Introduction :ignore:
We identify the transfer function from the actuator forces $\bm{\tau}$ to the absolute displacement of the mobile platform $\bm{\mathcal{X}}$ in three different cases:
- Open Loop plant
- Already damped plant using Integral Force Feedback
- Already damped plant using Direct velocity feedback
*** HAC - Without LAC
#+begin_src matlab
controller = initializeController('type', 'open-loop');
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_ol = linearize(mdl, io);
G_ol.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_ol.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
*** HAC - IFF
#+begin_src matlab
controller = initializeController('type', 'iff');
K_iff = -(1e4/s)*eye(6);
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_iff = linearize(mdl, io);
G_iff.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_iff.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
*** HAC - DVF
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
** Control Architecture
We use the Jacobian to express the actuator forces in the cartesian frame, and thus we obtain the transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$.
#+begin_src matlab
Gc_ol = minreal(G_ol)/stewart.kinematics.J';
Gc_ol.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gc_iff = minreal(G_iff)/stewart.kinematics.J';
Gc_iff.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src
We then design a controller based on the transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$, finally, we will pre-multiply the controller by $\bm{J}^{-T}$.
** 6x6 Plant Comparison
#+begin_src matlab :exports none
p_handle = zeros(6*6,1);
fig = figure;
for ix = 1:6
for iy = 1:6
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(Gc_ol(ix, iy), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(Gc_iff(ix, iy), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',3);
plot(freqs, abs(squeeze(freqresp(Gc_dvf(ix, iy), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
if ix < 6
xticklabels({});
end
if iy > 1
yticklabels({});
end
end
end
linkaxes(p_handle, 'xy')
xlim([freqs(1), freqs(end)]);
ylim([1e-9 1e-3]);
han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
xlabel(han, 'Frequency [Hz]');
ylabel(han, 'Plant');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_coupling_jacobian.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_coupling_jacobian
#+caption: Norm of the transfer functions from $\bm{\mathcal{F}}$ to $\bm{\mathcal{X}}$ ([[./figs/hac_lac_coupling_jacobian.png][png]], [[./figs/hac_lac_coupling_jacobian.pdf][pdf]])
[[file:figs/hac_lac_coupling_jacobian.png]]
** HAC - DVF
*** Plant
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_dvf('Rz', 'Mz'), 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(Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_dvf('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_plant_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_plant_dvf
#+caption: Diagonal elements of the plant for HAC control when DVF is previously applied ([[./figs/hac_lac_plant_dvf.png][png]], [[./figs/hac_lac_plant_dvf.pdf][pdf]])
[[file:figs/hac_lac_plant_dvf.png]]
*** Controller Design
We design a diagonal controller with equal bandwidth for the 6 terms.
The controller is a pure integrator with a small lead near the crossover.
#+begin_src matlab
wc = 2*pi*300; % Wanted Bandwidth [rad/s]
h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
Kd_dvf = diag(1./abs(diag(freqresp(1/s*Gc_dvf, wc)))) .* H_lead .* 1/s;
#+end_src
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Kd_dvf(1,1)*Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(2,2)*Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(3,3)*Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(4,4)*Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(5,5)*Gc_dvf('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_dvf(6,6)*Gc_dvf('Rz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(1,1)*Gc_dvf('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(2,2)*Gc_dvf('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(3,3)*Gc_dvf('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(4,4)*Gc_dvf('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(5,5)*Gc_dvf('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_dvf(6,6)*Gc_dvf('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_loop_gain_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_loop_gain_dvf
#+caption: Diagonal elements of the Loop Gain for the HAC control ([[./figs/hac_lac_loop_gain_dvf.png][png]], [[./figs/hac_lac_loop_gain_dvf.pdf][pdf]])
[[file:figs/hac_lac_loop_gain_dvf.png]]
Finally, we pre-multiply the diagonal controller by $\bm{J}^{-T}$ prior implementation.
#+begin_src matlab
K_hac_dvf = inv(stewart.kinematics.J')*Kd_dvf;
#+end_src
*** Obtained Performance
We identify the transmissibility and compliance of the system.
#+begin_src matlab
controller = initializeController('type', 'open-loop');
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
[C_ol, C_norm_ol, ~] = computeCompliance();
#+end_src
#+begin_src matlab
controller = initializeController('type', 'dvf');
[T_dvf, T_norm_dvf, ~] = computeTransmissibility();
[C_dvf, C_norm_dvf, ~] = computeCompliance();
#+end_src
#+begin_src matlab
controller = initializeController('type', 'hac-dvf');
[T_hac_dvf, T_norm_hac_dvf, ~] = computeTransmissibility();
[C_hac_dvf, C_norm_hac_dvf, ~] = computeCompliance();
#+end_src
#+begin_src matlab :exports none
figure;
subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_dvf)
plot(freqs, T_norm_hac_dvf)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');
subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_dvf, 'DisplayName', 'DVF')
plot(freqs, C_norm_hac_dvf, 'DisplayName', 'HAC-DVF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_T_dvf.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_C_T_dvf
#+caption: Obtained Compliance and Transmissibility ([[./figs/hac_lac_C_T_dvf.png][png]], [[./figs/hac_lac_C_T_dvf.pdf][pdf]])
[[file:figs/hac_lac_C_T_dvf.png]]
** HAC - IFF
*** Plant
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Gc_iff('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Gc_iff('Rz', 'Mz'), 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(Gc_iff('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Gc_iff('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_plant_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_plant_iff
#+caption: Diagonal elements of the plant for HAC control when IFF is previously applied ([[./figs/hac_lac_plant_iff.png][png]], [[./figs/hac_lac_plant_iff.pdf][pdf]])
[[file:figs/hac_lac_plant_iff.png]]
*** Controller Design
We design a diagonal controller with equal bandwidth for the 6 terms.
The controller is a pure integrator with a small lead near the crossover.
#+begin_src matlab
wc = 2*pi*300; % Wanted Bandwidth [rad/s]
h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
Kd_iff = diag(1./abs(diag(freqresp(1/s*Gc_iff, wc)))) .* H_lead .* 1/s;
#+end_src
#+begin_src matlab :exports none
freqs = logspace(1, 4, 1000);
figure;
ax1 = subplot(2, 1, 1);
hold on;
plot(freqs, abs(squeeze(freqresp(Kd_iff(1,1)*Gc_iff('Dx', 'Fx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(2,2)*Gc_iff('Dy', 'Fy'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(3,3)*Gc_iff('Dz', 'Fz'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(4,4)*Gc_iff('Rx', 'Mx'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(5,5)*Gc_iff('Ry', 'My'), freqs, 'Hz'))));
plot(freqs, abs(squeeze(freqresp(Kd_iff(6,6)*Gc_iff('Rz', 'Mz'), freqs, 'Hz'))));
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
ax2 = subplot(2, 1, 2);
hold on;
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(1,1)*Gc_iff('Dx', 'Fx'), freqs, 'Hz'))), 'DisplayName', 'Dx/Fx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(2,2)*Gc_iff('Dy', 'Fy'), freqs, 'Hz'))), 'DisplayName', 'Dy/Fy');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(3,3)*Gc_iff('Dz', 'Fz'), freqs, 'Hz'))), 'DisplayName', 'Dz/Fz');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(4,4)*Gc_iff('Rx', 'Mx'), freqs, 'Hz'))), 'DisplayName', 'Rx/Mx');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(5,5)*Gc_iff('Ry', 'My'), freqs, 'Hz'))), 'DisplayName', 'Ry/My');
plot(freqs, 180/pi*angle(squeeze(freqresp(Kd_iff(6,6)*Gc_iff('Rz', 'Mz'), freqs, 'Hz'))), 'DisplayName', 'Rz/Mz');
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');
legend('location', 'northeast');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_loop_gain_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_loop_gain_iff
#+caption: Diagonal elements of the Loop Gain for the HAC control ([[./figs/hac_lac_loop_gain_iff.png][png]], [[./figs/hac_lac_loop_gain_iff.pdf][pdf]])
[[file:figs/hac_lac_loop_gain_iff.png]]
Finally, we pre-multiply the diagonal controller by $\bm{J}^{-T}$ prior implementation.
#+begin_src matlab
K_hac_iff = inv(stewart.kinematics.J')*Kd_iff;
#+end_src
*** Obtained Performance
We identify the transmissibility and compliance of the system.
#+begin_src matlab
controller = initializeController('type', 'open-loop');
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
[C_ol, C_norm_ol, ~] = computeCompliance();
#+end_src
#+begin_src matlab
controller = initializeController('type', 'iff');
[T_iff, T_norm_iff, ~] = computeTransmissibility();
[C_iff, C_norm_iff, ~] = computeCompliance();
#+end_src
#+begin_src matlab
controller = initializeController('type', 'hac-iff');
[T_hac_iff, T_norm_hac_iff, ~] = computeTransmissibility();
[C_hac_iff, C_norm_hac_iff, ~] = computeCompliance();
#+end_src
#+begin_src matlab :exports none
figure;
subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_iff)
plot(freqs, T_norm_hac_iff)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');
subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_iff, 'DisplayName', 'IFF')
plot(freqs, C_norm_hac_iff, 'DisplayName', 'HAC-IFF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_T_iff.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_C_T_iff
#+caption: Obtained Compliance and Transmissibility ([[./figs/hac_lac_C_T_iff.png][png]], [[./figs/hac_lac_C_T_iff.pdf][pdf]])
[[file:figs/hac_lac_C_T_iff.png]]
** Comparison
#+begin_src matlab :exports none
p_handle = zeros(6*6,1);
fig = figure;
for ix = 1:6
for iy = 1:6
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(C_ol(ix, iy), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(C_hac_dvf(ix, iy), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',3);
plot(freqs, abs(squeeze(freqresp(C_hac_iff(ix, iy), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
if ix < 6
xticklabels({});
end
if iy > 1
yticklabels({});
end
end
end
linkaxes(p_handle, 'xy')
ylim([1e-10, 1e-3]);
xlim([freqs(1), freqs(end)]);
han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
xlabel(han, 'Frequency [Hz]');
ylabel(han, 'Compliance');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_full_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_C_full_comparison
#+caption: Comparison of the norm of the Compliance matrices for the HAC-LAC architecture ([[./figs/hac_lac_C_full_comparison.png][png]], [[./figs/hac_lac_C_full_comparison.pdf][pdf]])
[[file:figs/hac_lac_C_full_comparison.png]]
#+begin_src matlab :exports none
p_handle = zeros(6*6,1);
fig = figure;
for ix = 1:6
for iy = 1:6
p_handle((ix-1)*6 + iy) = subplot(6, 6, (ix-1)*6 + iy);
hold on;
set(gca,'ColorOrderIndex',1);
plot(freqs, abs(squeeze(freqresp(T_ol(ix, iy), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',2);
plot(freqs, abs(squeeze(freqresp(T_hac_dvf(ix, iy), freqs, 'Hz'))));
set(gca,'ColorOrderIndex',3);
plot(freqs, abs(squeeze(freqresp(T_hac_iff(ix, iy), freqs, 'Hz'))));
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
if ix < 6
xticklabels({});
end
if iy > 1
yticklabels({});
end
end
end
linkaxes(p_handle, 'xy')
ylim([1e-5, 10]);
xlim([freqs(1), freqs(end)]);
han = axes(fig, 'visible', 'off');
han.XLabel.Visible = 'on';
han.YLabel.Visible = 'on';
xlabel(han, 'Frequency [Hz]');
ylabel(han, 'Transmissibility');
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_T_full_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_T_full_comparison
#+caption: Comparison of the norm of the Transmissibility matrices for the HAC-LAC architecture ([[./figs/hac_lac_T_full_comparison.png][png]], [[./figs/hac_lac_T_full_comparison.pdf][pdf]])
[[file:figs/hac_lac_T_full_comparison.png]]
#+begin_src matlab :exports none
figure;
subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_hac_dvf)
plot(freqs, T_norm_hac_iff)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');
subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_hac_dvf, 'DisplayName', 'HAC-DVF')
plot(freqs, C_norm_hac_iff, 'DisplayName', 'HAC-IFF')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/hac_lac_C_T_comparison.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:hac_lac_C_T_comparison
#+caption: Comparison of the Frobenius norm of the Compliance and Transmissibility for the HAC-LAC architecture with both IFF and DVF ([[./figs/hac_lac_C_T_comparison.png][png]], [[./figs/hac_lac_C_T_comparison.pdf][pdf]])
[[file:figs/hac_lac_C_T_comparison.png]]
* MIMO Analysis
** Introduction :ignore:
Let's define the system as shown in figure [[fig:general_control_names]].
#+begin_src latex :file general_control_names.pdf
\begin{tikzpicture}
% Blocs
\node[block={2.0cm}{2.0cm}] (P) {$P$};
\node[block={1.5cm}{1.5cm}, below=0.7 of P] (K) {$K$};
% Input and outputs coordinates
\coordinate[] (inputw) at ($(P.south west)!0.75!(P.north west)$);
\coordinate[] (inputu) at ($(P.south west)!0.25!(P.north west)$);
\coordinate[] (outputz) at ($(P.south east)!0.75!(P.north east)$);
\coordinate[] (outputv) at ($(P.south east)!0.25!(P.north east)$);
% Connections and labels
\draw[<-] (inputw) node[above left, align=right]{(weighted)\\exogenous inputs\\$w$} -- ++(-1.5, 0);
\draw[<-] (inputu) -- ++(-0.8, 0) |- node[left, near start, align=right]{control signals\\$u$} (K.west);
\draw[->] (outputz) node[above right, align=left]{(weighted)\\exogenous outputs\\$z$} -- ++(1.5, 0);
\draw[->] (outputv) -- ++(0.8, 0) |- node[right, near start, align=left]{sensed output\\$v$} (K.east);
\end{tikzpicture}
#+end_src
#+name: fig:general_control_names
#+caption: General Control Architecture
#+RESULTS:
[[file:figs/general_control_names.png]]
#+name: tab:general_plant_signals
#+caption: Signals definition for the generalized plant
| | *Symbol* | *Meaning* |
|---------------------+-----------------------------+----------------------------------------|
| *Exogenous Inputs* | $\bm{\mathcal{X}}_w$ | Ground motion |
| | $\bm{\mathcal{F}}_d$ | External Forces applied to the Payload |
| | $\bm{r}$ | Reference signal for tracking |
|---------------------+-----------------------------+----------------------------------------|
| *Exogenous Outputs* | $\bm{\mathcal{X}}$ | Absolute Motion of the Payload |
| | $\bm{\tau}$ | Actuator Rate |
|---------------------+-----------------------------+----------------------------------------|
| *Sensed Outputs* | $\bm{\tau}_m$ | Force Sensors in each leg |
| | $\delta \bm{\mathcal{L}}_m$ | Measured displacement of each leg |
| | $\bm{\mathcal{X}}$ | Absolute Motion of the Payload |
|---------------------+-----------------------------+----------------------------------------|
| *Control Signals* | $\bm{\tau}$ | Actuator Inputs |
** 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
#+begin_src matlab
open('stewart_platform_model.slx')
#+end_src
** Initialization
We first initialize the Stewart platform.
#+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, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src
The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src
** Identification
*** HAC - Without LAC
#+begin_src matlab
controller = initializeController('type', 'open-loop');
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_ol = linearize(mdl, io);
G_ol.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_ol.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
*** HAC - DVF
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
*** Cartesian Frame
#+begin_src matlab
Gc_ol = minreal(G_ol)/stewart.kinematics.J';
Gc_ol.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src
** Singular Value Decomposition
#+begin_src matlab
freqs = logspace(1, 4, 1000);
U_ol = zeros(6,6,length(freqs));
S_ol = zeros(6,length(freqs));
V_ol = zeros(6,6,length(freqs));
U_dvf = zeros(6,6,length(freqs));
S_dvf = zeros(6,length(freqs));
V_dvf = zeros(6,6,length(freqs));
for i = 1:length(freqs)
[U,S,V] = svd(freqresp(Gc_ol, freqs(i), 'Hz'));
U_ol(:,:,i) = U;
S_ol(:,i) = diag(S);
V_ol(:,:,i) = V;
[U,S,V] = svd(freqresp(Gc_dvf, freqs(i), 'Hz'));
U_dvf(:,:,i) = U;
S_dvf(:,i) = diag(S);
V_dvf(:,:,i) = V;
end
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(1,2,1);
hold on;
plot(freqs, S_ol(1,:), '-');
plot(freqs, S_ol(2,:), '--');
plot(freqs, S_ol(3,:), '-.');
plot(freqs, S_ol(4,:), '--');
plot(freqs, S_ol(5,:), '-');
plot(freqs, S_ol(6,:), '-.');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
title('Undamped Plant');
ax2 = subplot(1,2,2);
hold on;
plot(freqs, S_dvf(1,:), '-' , 'DisplayName', '$\sigma_1$');
plot(freqs, S_dvf(2,:), '--', 'DisplayName', '$\sigma_2$');
plot(freqs, S_dvf(3,:), '-.', 'DisplayName', '$\sigma_3$');
plot(freqs, S_dvf(4,:), '-' , 'DisplayName', '$\sigma_4$');
plot(freqs, S_dvf(5,:), '--', 'DisplayName', '$\sigma_5$');
plot(freqs, S_dvf(6,:), '-.', 'DisplayName', '$\sigma_6$');
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
title('Damped Plant - DVF');
linkaxes([ax1, ax2], 'xy');
legend();
#+end_src
#+begin_src matlab :exports none
figure;
ax1 = subplot(1,2,1);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(V_ol(i,1,:))), '-' , 'DisplayName', Gc_ol.InputName{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
legend();
ax2 = subplot(1,2,2);
hold on;
for i = 1:6
plot(freqs, abs(squeeze(U_ol(i,1,:))), '-' , 'DisplayName', Gc_ol.OutputName{i});
end
hold off;
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
xlabel('Frequency [Hz]');
ylabel('Singular Values');
legend();
linkaxes([ax1,ax2], 'x');
#+end_src
* Diagonal Control based on the damped plant
** Introduction :ignore:
From cite:skogestad07_multiv_feedb_contr, a simple approach to multivariable control is the following two-step procedure:
1. *Design a pre-compensator* $W_1$, which counteracts the interactions in the plant and results in a new *shaped plant* $G_S(s) = G(s) W_1(s)$ which is *more diagonal and easier to control* than the original plant $G(s)$.
2. *Design a diagonal controller* $K_S(s)$ for the shaped plant using methods similar to those for SISO systems.
The overall controller is then:
\[ K(s) = W_1(s)K_s(s) \]
There are mainly three different cases:
1. *Dynamic decoupling*: $G_S(s)$ is diagonal at all frequencies. For that we can choose $W_1(s) = G^{-1}(s)$ and this is an inverse-based controller.
2. *Steady-state decoupling*: $G_S(0)$ is diagonal. This can be obtained by selecting $W_1(s) = G^{-1}(0)$.
3. *Approximate decoupling at frequency $\w_0$*: $G_S(j\w_0)$ is as diagonal as possible. Decoupling the system at $\w_0$ is a good choice because the effect on performance of reducing interaction is normally greatest at this frequency.
** Initialization
We first initialize the Stewart platform.
#+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, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src
The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src
** Identification
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
#+end_src
#+begin_src matlab
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
#+end_src
** Steady State Decoupling
*** Pre-Compensator Design
We choose $W_1 = G^{-1}(0)$.
#+begin_src matlab
W1 = inv(freqresp(G_dvf, 0));
#+end_src
The (static) decoupled plant is $G_s(s) = G(s) W_1$.
#+begin_src matlab
Gs = G_dvf*W1;
#+end_src
In the case of the Stewart platform, the pre-compensator for static decoupling is equal to $\mathcal{K} \bm{J}$:
\begin{align*}
W_1 &= \left( \frac{\bm{\mathcal{X}}}{\bm{\tau}}(s=0) \right)^{-1}\\
&= \left( \frac{\bm{\mathcal{X}}}{\bm{\tau}}(s=0) \bm{J}^T \right)^{-1}\\
&= \left( \bm{C} \bm{J}^T \right)^{-1}\\
&= \left( \bm{J}^{-1} \mathcal{K}^{-1} \right)^{-1}\\
&= \mathcal{K} \bm{J}
\end{align*}
The static decoupled plant is schematic shown in Figure [[fig:control_arch_static_decoupling]] and the bode plots of its diagonal elements are shown in Figure [[fig:static_decoupling_diagonal_plant]].
#+begin_src latex :file control_arch_static_decoupling.pdf
\begin{tikzpicture}
% Blocs
\node[block] (G) {$G(s)$};
\node[block, left=1 of G] (J) {$\mathcal{K}\bm{J}$};
\node[block, left=1 of J] (Ks) {$\bm{K}_s(s)$};
\draw[->] (Ks.east) -- (J.west);
\draw[->] (J.east) -- (G.west) node[above left]{$\bm{\tau}$};
\draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ++(0.8, -0.8) -| ($(Ks.west) + (-0.8, 0)$) -- (Ks.west);
\begin{scope}[on background layer]
\node[fit={(J.north west) (G.south east)}, inner sep=4pt, draw, dashed, fill=black!20!white, label={$G_s(s)$}] {};
\end{scope}
\end{tikzpicture}
#+end_src
#+name: fig:control_arch_static_decoupling
#+caption: Static Decoupling of the Stewart platform
#+RESULTS:
[[file:figs/control_arch_static_decoupling.png]]
#+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(Gs(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(Gs(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
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/static_decoupling_diagonal_plant.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:static_decoupling_diagonal_plant
#+caption: Bode plot of the diagonal elements of $G_s(s)$ ([[./figs/static_decoupling_diagonal_plant.png][png]], [[./figs/static_decoupling_diagonal_plant.pdf][pdf]])
[[file:figs/static_decoupling_diagonal_plant.png]]
*** Diagonal Control Design
We design a diagonal controller $K_s(s)$ that consist of a pure integrator and a lead around the crossover.
#+begin_src matlab
wc = 2*pi*300; % Wanted Bandwidth [rad/s]
h = 1.5;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
Ks_dvf = diag(1./abs(diag(freqresp(1/s*Gs, wc)))) .* H_lead .* 1/s;
#+end_src
The overall controller is then $K(s) = W_1 K_s(s)$ as shown in Figure [[fig:control_arch_static_decoupling_K]].
#+begin_src matlab
K_hac_dvf = W1 * Ks_dvf;
#+end_src
#+begin_src latex :file control_arch_static_decoupling_K.pdf
\begin{tikzpicture}
% Blocs
\node[block] (G) {$G(s)$};
\node[block, left=1 of G] (J) {$\mathcal{K}\bm{J}$};
\node[block, left=1 of J] (Ks) {$\bm{K}_s(s)$};
\draw[->] (Ks.east) -- (J.west);
\draw[->] (J.east) -- (G.west) node[above left]{$\bm{\tau}$};
\draw[->] (G.east) node[above right]{$\bm{\mathcal{X}}$} -| ++(0.8, -0.8) -| ($(Ks.west) + (-0.8, 0)$) -- (Ks.west);
\begin{scope}[on background layer]
\node[fit={(Ks.north west) (J.south east)}, inner sep=4pt, draw, dashed, fill=black!20!white, label={$K(s)$}] {};
\end{scope}
\end{tikzpicture}
#+end_src
#+name: fig:control_arch_static_decoupling_K
#+caption: Controller including the static decoupling matrix
#+RESULTS:
[[file:figs/control_arch_static_decoupling_K.png]]
*** Results
We identify the transmissibility and compliance of the Stewart platform under open-loop and closed-loop control.
#+begin_src matlab
controller = initializeController('type', 'open-loop');
[T_ol, T_norm_ol, freqs] = computeTransmissibility();
[C_ol, C_norm_ol, ~] = computeCompliance();
#+end_src
#+begin_src matlab
controller = initializeController('type', 'hac-dvf');
[T_hac_dvf, T_norm_hac_dvf, ~] = computeTransmissibility();
[C_hac_dvf, C_norm_hac_dvf, ~] = computeCompliance();
#+end_src
The results are shown in figure
#+begin_src matlab :exports none
figure;
subplot(1,2,1);
hold on;
plot(freqs, T_norm_ol)
plot(freqs, T_norm_hac_dvf)
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Transmissibility - Frobenius Norm');
subplot(1,2,2);
hold on;
plot(freqs, C_norm_ol, 'DisplayName', 'OL')
plot(freqs, C_norm_hac_dvf, 'DisplayName', 'HAC-DVF - Static decoupl.')
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
xlabel('Frequency [Hz]');
ylabel('Compliance - Frobenius Norm');
legend();
#+end_src
#+header: :tangle no :exports results :results none :noweb yes
#+begin_src matlab :var filepath="figs/static_decoupling_C_T_frobenius_norm.pdf" :var figsize="full-tall" :post pdf2svg(file=*this*, ext="png")
<<plt-matlab>>
#+end_src
#+name: fig:static_decoupling_C_T_frobenius_norm
#+caption: Frobenius norm of the Compliance and transmissibility matrices ([[./figs/static_decoupling_C_T_frobenius_norm.png][png]], [[./figs/static_decoupling_C_T_frobenius_norm.pdf][pdf]])
[[file:figs/static_decoupling_C_T_frobenius_norm.png]]
** TODO Decoupling at Crossover
- [ ] Find a method for real approximation of a complex matrix
* Time Domain Simulation
** 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
#+begin_src matlab
open('stewart_platform_model.slx')
#+end_src
** Initialization
We first initialize the Stewart platform.
#+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, 'type_F', 'universal', 'type_M', 'spherical');
stewart = initializeCylindricalPlatforms(stewart);
stewart = initializeCylindricalStruts(stewart);
stewart = computeJacobian(stewart);
stewart = initializeStewartPose(stewart);
stewart = initializeInertialSensor(stewart, 'type', 'none');
#+end_src
The rotation point of the ground is located at the origin of frame $\{A\}$.
#+begin_src matlab
ground = initializeGround('type', 'rigid', 'rot_point', stewart.platform_F.FO_A);
payload = initializePayload('type', 'none');
#+end_src
#+begin_src matlab
load('./mat/motion_error_ol.mat', 'Eg')
#+end_src
** HAC IFF
#+begin_src matlab
controller = initializeController('type', 'iff');
K_iff = -(1e4/s)*eye(6);
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_iff = linearize(mdl, io);
G_iff.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_iff.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
Gc_iff = minreal(G_iff)/stewart.kinematics.J';
Gc_iff.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src
#+begin_src matlab
wc = 2*pi*100; % Wanted Bandwidth [rad/s]
h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
Kd_iff = diag(1./abs(diag(freqresp(1/s*Gc_iff, wc)))) .* H_lead .* 1/s;
K_hac_iff = inv(stewart.kinematics.J')*Kd_iff;
#+end_src
#+begin_src matlab
controller = initializeController('type', 'hac-iff');
#+end_src
** HAC-DVF
#+begin_src matlab
controller = initializeController('type', 'dvf');
K_dvf = -1e4*s/(1+s/2/pi/5000)*eye(6);
%% Name of the Simulink File
mdl = 'stewart_platform_model';
%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Force Inputs [N]
io(io_i) = linio([mdl, '/Absolute Motion Sensor'], 1, 'openoutput'); io_i = io_i + 1; % Absolute Sensor [m, rad]
%% Run the linearization
G_dvf = linearize(mdl, io);
G_dvf.InputName = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G_dvf.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
Gc_dvf = minreal(G_dvf)/stewart.kinematics.J';
Gc_dvf.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
#+end_src
#+begin_src matlab
wc = 2*pi*100; % Wanted Bandwidth [rad/s]
h = 1.2;
H_lead = 1/h*(1 + s/(wc/h))/(1 + s/(wc*h));
Kd_dvf = diag(1./abs(diag(freqresp(1/s*Gc_dvf, wc)))) .* H_lead .* 1/s;
K_hac_dvf = inv(stewart.kinematics.J')*Kd_dvf;
#+end_src
#+begin_src matlab
controller = initializeController('type', 'hac-dvf');
#+end_src
** Results
#+begin_src matlab
figure;
subplot(1, 2, 1);
hold on;
plot(Eg.Time, Eg.Data(:, 1), 'DisplayName', 'X');
plot(Eg.Time, Eg.Data(:, 2), 'DisplayName', 'Y');
plot(Eg.Time, Eg.Data(:, 3), 'DisplayName', 'Z');
hold off;
xlabel('Time [s]');
ylabel('Position error [m]');
legend();
subplot(1, 2, 2);
hold on;
plot(simout.Xa.Time, simout.Xa.Data(:, 1));
plot(simout.Xa.Time, simout.Xa.Data(:, 2));
plot(simout.Xa.Time, simout.Xa.Data(:, 3));
hold off;
xlabel('Time [s]');
ylabel('Orientation error [rad]');
#+end_src
* Functions
** =initializeController=: Initialize the Controller
:PROPERTIES:
:header-args:matlab+: :tangle ../src/initializeController.m
:header-args:matlab+: :comments none :mkdirp yes :eval no
:END:
<<sec:initializeController>>
*** Function description
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
function [controller] = initializeController(args)
% initializeController - Initialize the Controller
%
% Syntax: [] = initializeController(args)
%
% Inputs:
% - args - Can have the following fields:
#+end_src
*** Optional Parameters
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
arguments
args.type char {mustBeMember(args.type, {'open-loop', 'iff', 'dvf', 'hac-iff', 'hac-dvf', 'ref-track-L', 'ref-track-X', 'ref-track-hac-dvf'})} = 'open-loop'
end
#+end_src
*** Structure initialization
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
controller = struct();
#+end_src
*** Add Type
:PROPERTIES:
:UNNUMBERED: t
:END:
#+begin_src matlab
switch args.type
case 'open-loop'
controller.type = 0;
case 'iff'
controller.type = 1;
case 'dvf'
controller.type = 2;
case 'hac-iff'
controller.type = 3;
case 'hac-dvf'
controller.type = 4;
case 'ref-track-L'
controller.type = 5;
case 'ref-track-X'
controller.type = 6;
case 'ref-track-hac-dvf'
controller.type = 7;
end
#+end_src
Reference in New Issue View Git Blame Copy Permalink