tdehaeze/vibration-table
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Tangle matlab scripts

Browse Source
This commit is contained in:
Thomas Dehaeze 2021-04-19 12:15:41 +02:00
parent 72676fbe54
commit 029b712260
3 changed files with 370 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
matlab/meas_transformation.m Normal file
View File

@ -0,0 +1,119 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Define accelerometers positions/orientations
% <<sec:accelerometer_pos>>
% Let's first define the position and orientation of all measured accelerations with respect to a defined frame $\{O\}$.
Opm = [-0.1875, -0.1875, -0.245;
-0.1875, -0.1875, -0.245;
0.1875, -0.1875, -0.245;
0.1875, -0.1875, -0.245;
0.1875, 0.1875, -0.245;
0.1875, 0.1875, -0.245]';
% #+name: tab:accelerometers_table_positions
% #+caption: Positions of the accelerometers fixed to the vibration table with respect to $\{O\}$
% #+attr_latex: :environment tabularx :width 0.55\linewidth :align Xcccccc
% #+attr_latex: :center t :booktabs t :float t
% #+RESULTS:
% | | $a_1$ | $a_2$ | $a_3$ | $a_4$ | $a_5$ | $a_6$ |
% |---+--------+--------+--------+--------+--------+--------|
% | x | -0.188 | -0.188 | 0.188 | 0.188 | 0.188 | 0.188 |
% | y | -0.188 | -0.188 | -0.188 | -0.188 | 0.188 | 0.188 |
% | z | -0.245 | -0.245 | -0.245 | -0.245 | -0.245 | -0.245 |
% We then define the direction of the measured accelerations (unit vectors):
Osm = [0, 1, 0;
0, 0, 1;
1, 0, 0;
0, 0, 1;
1, 0, 0;
0, 0, 1;]';
% Transformation matrix from motion of the solid body to accelerometer measurements
% <<sec:transformation_motion_to_acc>>
% Let's try to estimate the x-y-z acceleration of any point of the solid body from the acceleration/angular acceleration of the solid body expressed in $\{O\}$.
% For any point $p_i$ of the solid body (corresponding to an accelerometer), we can write:
% \begin{equation}
% \begin{bmatrix}
% a_{i,x} \\ a_{i,y} \\ a_{i,z}
% \end{bmatrix} = \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
% \end{bmatrix} + p_i \times \begin{bmatrix}
% \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix}
% \end{equation}
% We can write the cross product as a matrix product using the skew-symmetric transformation:
% \begin{equation}
% \begin{bmatrix}
% a_{i,x} \\ a_{i,y} \\ a_{i,z}
% \end{bmatrix} = \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
% \end{bmatrix} + \underbrace{\begin{bmatrix}
% 0 & p_{i,z} & -p_{i,y} \\
% -p_{i,z} & 0 & p_{i,x} \\
% p_{i,y} & -p_{i,x} & 0
% \end{bmatrix}}_{P_{i,[\times]}} \cdot \begin{bmatrix}
% \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix}
% \end{equation}
% If we now want to know the (scalar) acceleration $a_i$ of the point $p_i$ in the direction of the accelerometer direction $\hat{s}_i$, we can just project the 3d acceleration on $\hat{s}_i$:
% \begin{equation}
% a_i = \hat{s}_i^T \cdot \begin{bmatrix}
% a_{i,x} \\ a_{i,y} \\ a_{i,z}
% \end{bmatrix} = \hat{s}_i^T \cdot \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z
% \end{bmatrix} + \left( \hat{s}_i^T \cdot P_{i,[\times]} \right) \cdot \begin{bmatrix}
% \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix}
% \end{equation}
% Which is equivalent as a simple vector multiplication:
% \begin{equation}
% a_i = \begin{bmatrix}
% \hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
% \end{bmatrix}
% \begin{bmatrix}
% \dot{v}_x \\ \dot{v}_y \\ \dot{v}_z \\ \dot{\omega}_x \\ \dot{\omega}_y \\ \dot{\omega}_z
% \end{bmatrix} = \begin{bmatrix}
% \hat{s}_i^T & \hat{s}_i^T \cdot P_{i,[\times]}
% \end{bmatrix} {}^O\vec{x}
% \end{equation}
% And finally we can combine the 6 (line) vectors for the 6 accelerometers to write that in a matrix form.
% We obtain Eq. eqref:eq:M_matrix.
% #+begin_important
% The transformation from solid body acceleration ${}^O\vec{x}$ from sensor measured acceleration $\vec{a}$ is:
% \begin{equation} \label{eq:M_matrix}
% \vec{a} = \underbrace{\begin{bmatrix}
% \hat{s}_1^T & \hat{s}_1^T \cdot P_{1,[\times]} \\
% \vdots & \vdots \\
% \hat{s}_6^T & \hat{s}_6^T \cdot P_{6,[\times]}
% \end{bmatrix}}_{M} {}^O\vec{x}
% \end{equation}
% with $\hat{s}_i$ the unit vector representing the measured direction of the i'th accelerometer expressed in frame $\{O\}$ and $P_{i,[\times]}$ the skew-symmetric matrix representing the cross product of the position of the i'th accelerometer expressed in frame $\{O\}$.
% #+end_important
% Let's define such matrix using matlab:
M = zeros(length(Opm), 6);
for i = 1:length(Opm)
Ri = [0, Opm(3,i), -Opm(2,i);
-Opm(3,i), 0, Opm(1,i);
Opm(2,i), -Opm(1,i), 0];
M(i, 1:3) = Osm(:,i)';
M(i, 4:6) = Osm(:,i)'*Ri;
end

238
matlab/simscape_model.m Normal file
View File

@ -0,0 +1,238 @@
%% Clear Workspace and Close figures
clear; close all; clc;
%% Intialize Laplace variable
s = zpk('s');
% Run meas_transformation.m script in order to get the tranformation (jacobian) matrix
run('meas_transformation.m')
% Open the Simulink File
open('vibration_table.slx')
% Springs
% <<sec:simscape_springs>>
% The 4 springs supporting the suspended optical table are modelled with "bushing joints" having stiffness and damping in the x-y-z directions:
spring.kx = 1e4; % X- Stiffness [N/m]
spring.cx = 1e1; % X- Damping [N/(m/s)]
spring.ky = 1e4; % Y- Stiffness [N/m]
spring.cy = 1e1; % Y- Damping [N/(m/s)]
spring.kz = 1e4; % Z- Stiffness [N/m]
spring.cz = 1e1; % Z- Damping [N/(m/s)]
spring.z0 = 32e-3; % Equilibrium z-length [m]
% Inertial Shaker (IS20)
% <<sec:simscape_inertial_shaker>>
% The inertial shaker is defined as two solid bodies:
% - the "housing" that is fixed to the element that we want to excite
% - the "inertial mass" that is suspended inside the housing
% The inertial mass is guided inside the housing and an actuator (coil and magnet) can be used to apply a force between the inertial mass and the support.
% The "reacting" force on the support is then used as an excitation.
% #+name: tab:is20_characteristics
% #+caption: Summary of the IS20 datasheet
% #+attr_latex: :environment tabularx :width 0.4\linewidth :align lX
% #+attr_latex: :center t :booktabs t :float t
% | Characteristic | Value |
% |-----------------+------------|
% | Output Force | 20 N |
% | Frequency Range | 10-3000 Hz |
% | Moving Mass | 0.1 kg |
% | Total Mass | 0.3 kg |
% From the datasheet in Table [[tab:is20_characteristics]], we can estimate the parameters of the physical shaker.
% These parameters are defined below
shaker.w0 = 2*pi*10; % Resonance frequency of moving mass [rad/s]
shaker.m = 0.1; % Moving mass [m]
shaker.m_tot = 0.3; % Total mass [m]
shaker.k = shaker.m*shaker.w0^2; % Spring constant [N/m]
shaker.c = 0.2*sqrt(shaker.k*shaker.m); % Damping [N/(m/s)]
% 3D accelerometer (356B18)
% <<sec:simscape_accelerometers>>
% An accelerometer consists of 2 solids:
% - a "housing" rigidly fixed to the measured body
% - an "inertial mass" suspended inside the housing by springs and guided in the measured direction
% The relative motion between the housing and the inertial mass gives a measurement of the acceleration of the measured body (up to the suspension mode of the inertial mass).
% #+name: tab:356b18_characteristics
% #+caption: Summary of the 356B18 datasheet
% #+attr_latex: :environment tabularx :width 0.5\linewidth :align lX
% #+attr_latex: :center t :booktabs t :float t
% | Characteristic | Value |
% |---------------------+---------------------|
% | Sensitivity | 0.102 V/(m/s2) |
% | Frequency Range | 0.5 to 3000 Hz |
% | Resonance Frequency | > 20 kHz |
% | Resolution | 0.0005 m/s2 rms |
% | Weight | 0.025 kg |
% | Size | 20.3x26.1x20.3 [mm] |
% Here are defined the parameters for the triaxial accelerometer:
acc_3d.m = 0.005; % Inertial mass [kg]
acc_3d.m_tot = 0.025; % Total mass [m]
acc_3d.w0 = 2*pi*20e3; % Resonance frequency [rad/s]
acc_3d.kx = acc_3d.m*acc_3d.w0^2; % Spring constant [N/m]
acc_3d.ky = acc_3d.m*acc_3d.w0^2; % Spring constant [N/m]
acc_3d.kz = acc_3d.m*acc_3d.w0^2; % Spring constant [N/m]
acc_3d.cx = 1e2; % Damping [N/(m/s)]
acc_3d.cy = 1e2; % Damping [N/(m/s)]
acc_3d.cz = 1e2; % Damping [N/(m/s)]
% DC gain between support acceleration and inertial mass displacement is $-m/k$:
acc_3d.g_x = 1/(-acc_3d.m/acc_3d.kx); % [m/s^2/m]
acc_3d.g_y = 1/(-acc_3d.m/acc_3d.ky); % [m/s^2/m]
acc_3d.g_z = 1/(-acc_3d.m/acc_3d.kz); % [m/s^2/m]
% We also define the sensitivity in order to have the outputs in volts.
acc_3d.gV_x = 0.102; % [V/(m/s^2)]
acc_3d.gV_y = 0.102; % [V/(m/s^2)]
acc_3d.gV_z = 0.102; % [V/(m/s^2)]
% The problem with using such model for accelerometers is that this adds states to the identified models (2x3 states for each triaxial accelerometer).
% These states represents the dynamics of the suspended inertial mass.
% In the frequency band of interest (few Hz up to ~1 kHz), the dynamics of the inertial mass can be ignore (its resonance is way above 1kHz).
% Therefore, we might as well use idealized "transform sensors" blocks as they will give the same result up to ~20kHz while allowing to reduce the number of identified states.
% The accelerometer model can be chosen by setting the =type= property:
acc_3d.type = 2; % 1: inertial mass, 2: perfect
% Number of states
% Let's first use perfect 3d accelerometers:
acc_3d.type = 2; % 1: inertial mass, 2: perfect
% And identify the dynamics from the shaker force to the measured accelerations:
%% Name of the Simulink File
mdl = 'vibration_table';
%% 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, '/acc'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
Gp = linearize(mdl, io);
Gp.InputName = {'F'};
Gp.OutputName = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6'};
% #+RESULTS:
% : size(Gp)
% : State-space model with 6 outputs, 1 inputs, and 12 states.
% We indeed have the 12 states corresponding to the 6 DoF of the suspended optical table.
% Let's now consider the inertial masses for the triaxial accelerometers:
acc_3d.type = 1; % 1: inertial mass, 2: perfect
%% Name of the Simulink File
mdl = 'vibration_table';
%% 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, '/acc'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
Ga = linearize(mdl, io);
Ga.InputName = {'F'};
Ga.OutputName = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6'};
% Resonance frequencies and mode shapes
% Let's now identify the resonance frequency and mode shapes associated with the suspension modes of the optical table.
acc_3d.type = 2; % 1: inertial mass, 2: perfect
%% Name of the Simulink File
mdl = 'vibration_table';
%% 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, '/acc_O'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io);
G.InputName = {'F'};
G.OutputName = {'ax', 'ay', 'az', 'wx', 'wy', 'wz'};
% Compute the resonance frequencies
ws = eig(G.A);
ws = ws(imag(ws) > 0);
% And the associated response of the optical table
x_mod = zeros(6, 6);
for i = 1:length(ws)
xi = evalfr(G, ws(i));
x_mod(:,i) = xi./norm(xi);
end
% Verify transformation
%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;
%% Name of the Simulink File
mdl = 'vibration_table';
%% 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, '/acc'], 1, 'openoutput'); io_i = io_i + 1;
io(io_i) = linio([mdl, '/Absolute_Accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
%% Run the linearization
G = linearize(mdl, io, 0.0, options);
G.InputName = {'F'};
G.OutputName = {'a1', 'a2', 'a3', 'a4', 'a5', 'a6', ...
'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
G_acc = inv(M)*G(1:6, 1);
G_id = G(7:12, 1);
bodeFig({G_acc(1), G_id(1)})
bodeFig({G_acc(2), G_id(2)})
bodeFig({G_acc(3), G_id(3)})
bodeFig({G_acc(4), G_id(4)})
bodeFig({G_acc(5), G_id(5)})
bodeFig({G_acc(6), G_id(6)})

13
vibration-table.org
View File

@ -25,6 +25,7 @@
#+PROPERTY: header-args:matlab+ :results none #+PROPERTY: header-args:matlab+ :results none
#+PROPERTY: header-args:matlab+ :eval no-export #+PROPERTY: header-args:matlab+ :eval no-export
#+PROPERTY: header-args:matlab+ :noweb yes #+PROPERTY: header-args:matlab+ :noweb yes
#+PROPERTY: header-args:matlab+ :tangle no
#+PROPERTY: header-args:matlab+ :mkdirp yes #+PROPERTY: header-args:matlab+ :mkdirp yes
#+PROPERTY: header-args:matlab+ :output-dir figs #+PROPERTY: header-args:matlab+ :output-dir figs
@ -97,6 +98,9 @@ Here are the documentation of the equipment used for this vibration table:
| (4x) 3D accelerometer [[https://www.pcbpiezotronics.fr/produit/accelerometres/356b18/][PCB 356B18]] | | (4x) 3D accelerometer [[https://www.pcbpiezotronics.fr/produit/accelerometres/356b18/][PCB 356B18]] |
* Compute the 6DoF solid body motion from several inertial sensors * Compute the 6DoF solid body motion from several inertial sensors
:PROPERTIES:
:header-args:matlab+: :tangle matlab/meas_transformation.m
:END:
<<sec:meas_transformation>> <<sec:meas_transformation>>
** Introduction :ignore: ** Introduction :ignore:
Let's consider a solid body with several accelerometers attached to it (Figure [[fig:local_to_global_coordinates]]). Let's consider a solid body with several accelerometers attached to it (Figure [[fig:local_to_global_coordinates]]).
@ -383,6 +387,9 @@ data2orgtable(inv(M), {'$\dot{x}_x$', '$\dot{x}_y$', '$\dot{x}_z$', '$\dot{\omeg
| $\dot{\omega}_z$ | 0.0 | 0.0 | 2.7 | 0.0 | -2.7 | 0.0 | | $\dot{\omega}_z$ | 0.0 | 0.0 | 2.7 | 0.0 | -2.7 | 0.0 |
* Simscape Model * Simscape Model
:PROPERTIES:
:header-args:matlab+: :tangle matlab/simscape_model.m
:END:
<<sec:simscape_model>> <<sec:simscape_model>>
** Introduction :ignore: ** Introduction :ignore:
In this section, the Simscape model of the vibration table is described. In this section, the Simscape model of the vibration table is described.
@ -406,6 +413,12 @@ addpath('matlab/')
#+end_src #+end_src
#+begin_src matlab #+begin_src matlab
% Run meas_transformation.m script in order to get the tranformation (jacobian) matrix
run('meas_transformation.m')
#+end_src
#+begin_src matlab
% Open the Simulink File
open('vibration_table.slx') open('vibration_table.slx')
#+end_src #+end_src