Tangle matlab scripts
This commit is contained in:
parent
72676fbe54
commit
029b712260
119
matlab/meas_transformation.m
Normal file
119
matlab/meas_transformation.m
Normal 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
238
matlab/simscape_model.m
Normal 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)})
|
||||
@ -25,6 +25,7 @@
|
||||
#+PROPERTY: header-args:matlab+ :results none
|
||||
#+PROPERTY: header-args:matlab+ :eval no-export
|
||||
#+PROPERTY: header-args:matlab+ :noweb yes
|
||||
#+PROPERTY: header-args:matlab+ :tangle no
|
||||
#+PROPERTY: header-args:matlab+ :mkdirp yes
|
||||
#+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]] |
|
||||
|
||||
* Compute the 6DoF solid body motion from several inertial sensors
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/meas_transformation.m
|
||||
:END:
|
||||
<<sec:meas_transformation>>
|
||||
** Introduction :ignore:
|
||||
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 |
|
||||
|
||||
* Simscape Model
|
||||
:PROPERTIES:
|
||||
:header-args:matlab+: :tangle matlab/simscape_model.m
|
||||
:END:
|
||||
<<sec:simscape_model>>
|
||||
** Introduction :ignore:
|
||||
In this section, the Simscape model of the vibration table is described.
|
||||
@ -406,6 +413,12 @@ addpath('matlab/')
|
||||
#+end_src
|
||||
|
||||
#+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')
|
||||
#+end_src
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user