From 029b7122607c6cb76896cc600a6659a17121fdf3 Mon Sep 17 00:00:00 2001 From: Thomas Dehaeze Date: Mon, 19 Apr 2021 12:15:41 +0200 Subject: [PATCH] Tangle matlab scripts --- matlab/meas_transformation.m | 119 ++++++++++++++++++ matlab/simscape_model.m | 238 +++++++++++++++++++++++++++++++++++ vibration-table.org | 13 ++ 3 files changed, 370 insertions(+) create mode 100644 matlab/meas_transformation.m create mode 100644 matlab/simscape_model.m diff --git a/matlab/meas_transformation.m b/matlab/meas_transformation.m new file mode 100644 index 0000000..89daff2 --- /dev/null +++ b/matlab/meas_transformation.m @@ -0,0 +1,119 @@ +%% Clear Workspace and Close figures +clear; close all; clc; + +%% Intialize Laplace variable +s = zpk('s'); + +% Define accelerometers positions/orientations +% <> +% 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 +% <> + +% 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 diff --git a/matlab/simscape_model.m b/matlab/simscape_model.m new file mode 100644 index 0000000..9eb54df --- /dev/null +++ b/matlab/simscape_model.m @@ -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 +% <> + +% 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) +% <> + +% 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) +% <> + +% 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)}) diff --git a/vibration-table.org b/vibration-table.org index 38e7481..c5ace3d 100644 --- a/vibration-table.org +++ b/vibration-table.org @@ -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: <> ** 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: <> ** 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