275 lines
10 KiB
Org Mode
275 lines
10 KiB
Org Mode
#+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
|
|
:END:
|
|
|
|
* 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
|
|
|
|
** Script
|
|
#+begin_src matlab :results none :exports none
|
|
open stewart
|
|
#+end_src
|
|
|
|
The hexapod structure and Sample structure are initialized.
|
|
#+begin_src matlab :results none
|
|
stewart = initializeGeneralConfiguration();
|
|
stewart = computeGeometricalProperties(stewart);
|
|
stewart = initializeMechanicalElements(stewart);
|
|
save('./mat/stewart.mat', 'stewart');
|
|
|
|
initializeSample();
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results none
|
|
G = identifyPlant();
|
|
#+end_src
|
|
|
|
#+begin_src matlab :results none
|
|
freqs = logspace(2, 4, 1000);
|
|
#+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
|
|
|
|
* identifyPlant
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :exports code
|
|
:HEADER-ARGS:matlab+: :comments yes
|
|
:HEADER-ARGS:matlab+: :eval no
|
|
:HEADER-ARGS:matlab+: :tangle src/identifyPlant.m
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
function [sys] = identifyPlant(opts_param)
|
|
#+end_src
|
|
|
|
We use this code block to pass optional parameters.
|
|
#+begin_src matlab
|
|
%% Default values for opts
|
|
opts = struct();
|
|
|
|
%% Populate opts with input parameters
|
|
if exist('opts_param','var')
|
|
for opt = fieldnames(opts_param)'
|
|
opts.(opt{1}) = opts_param.(opt{1});
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
We defined the options for the =linearize= command.
|
|
Here, we just identify the system at time $t = 0$.
|
|
#+begin_src matlab
|
|
options = linearizeOptions;
|
|
options.SampleTime = 0;
|
|
#+end_src
|
|
|
|
We define the name of the Simulink File used to identification.
|
|
#+begin_src matlab
|
|
mdl = 'stewart';
|
|
#+end_src
|
|
|
|
Then we defined the input/output of the transfer function we want to identify.
|
|
#+begin_src matlab
|
|
%% Inputs
|
|
io(1) = linio([mdl, '/F'], 1, 'input'); % Cartesian forces
|
|
io(2) = linio([mdl, '/Fl'], 1, 'input'); % Leg forces
|
|
io(3) = linio([mdl, '/Fd'], 1, 'input'); % Direct forces
|
|
io(4) = linio([mdl, '/Dw'], 1, 'input'); % Base motion
|
|
|
|
%% Outputs
|
|
io(5) = linio([mdl, '/Dm'], 1, 'output'); % Relative Motion
|
|
io(6) = linio([mdl, '/Dlm'], 1, 'output'); % Displacement of each leg
|
|
io(7) = linio([mdl, '/Flm'], 1, 'output'); % Force sensor in each leg
|
|
io(8) = linio([mdl, '/Xm'], 1, 'output'); % Absolute motion of platform
|
|
#+end_src
|
|
|
|
The linearization is run.
|
|
#+begin_src matlab
|
|
G = linearize(mdl, io, 0);
|
|
#+end_src
|
|
|
|
We defined all the Input/Output names of the identified transfer function.
|
|
#+begin_src matlab
|
|
G.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz', ...
|
|
'F1', 'F2', 'F3', 'F4', 'F5', 'F6', ...
|
|
'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz', ...
|
|
'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'};
|
|
G.OutputName = {'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm', ...
|
|
'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m', ...
|
|
'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m', ...
|
|
'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'};
|
|
#+end_src
|
|
|
|
We split the transfer function into sub transfer functions and we compute their minimum realization.
|
|
#+begin_src matlab
|
|
sys.G_cart = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'}));
|
|
sys.G_forc = minreal(G({'F1m', 'F2m', 'F3m', 'F4m', 'F5m', 'F6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
|
|
sys.G_legs = minreal(G({'D1m', 'D2m', 'D3m', 'D4m', 'D5m', 'D6m'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'}));
|
|
sys.G_tran = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Dwx', 'Dwy', 'Dwz', 'Rwx', 'Rwy', 'Rwz'}));
|
|
sys.G_comp = minreal(G({'Dxm', 'Dym', 'Dzm', 'Rxm', 'Rym', 'Rzm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
|
|
sys.G_iner = minreal(G({'Dxtm', 'Dytm', 'Dztm', 'Rxtm', 'Rytm', 'Rztm'}, {'Fdx', 'Fdy', 'Fdz', 'Mdx', 'Mdy', 'Mdz'}));
|
|
% sys.G_all = minreal(G);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
end
|
|
#+end_src
|