tdehaeze/nass-simscape
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
a3cf7fc537
nass-simscape/org/nano_hexapod.org

70 KiB
Raw Blame History

Nano-Hexapod

Introduction   ignore

Nano-Hexapod

Introduction   ignore

Nano Hexapod - Configuration

<<sec:nano_hexapod_conf>>

The nano-hexapod can be initialized and configured using the initializeNanoHexapodFinal function (link).

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof', ...
                                       'MO_B', 150e-3);

We initialize the identification parameters.

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'nano_hexapod';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs

Effect of encoders on the decentralized plant

<<sec:effect_encoder_location>>

We here wish to compare the plant from actuators to the encoders when the encoders are either fixed on the struts or on the plates.

Identify the plant when the encoders are on the struts:

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');

Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};

And identify the plant when the encoders are fixed on the plates:

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'plates', ...
                                       'actuator_type', '2dof');

Gp = linearize(mdl, io, 0.0, options);
Gp.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gp.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};

The obtained plants are compared in Figure fig:nano_hexapod_effect_encoder.

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_effect_encoder.png

Comparison of the plants from actuator to associated encoder when the encoders are either fixed to the struts or to the plates

Why do we have zeros at 400Hz and 800Hz when the encoders are fixed on the struts?

Effect of APA flexibility

<<sec:effect_apa_flexibility>>

First identify the plant for APA represented by 2DoF system:

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof', ...
                                       'actuator_Ga', 2);

Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};

First identify the plant for APA represented by a flexible element:

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', 'flexible');

Gf = linearize(mdl, io, 0.0, options);
Gf.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gf.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_effect_flexible_apa.png

Comparison of the plants from actuator to associated strut encoder when the APA are modelled with a 2DoF system of with a flexible one

The first resonance is strange when using the flexible APA model (Figure fig:nano_hexapod_effect_flexible_apa). Otherwise, the 2DoF model matches quite well the flexible model considering its simplicity.

Nano Hexapod - Number of DoF

<<sec:num_states>>

In this section, we wish to see how the configuration of each element changes the number of the states of the obtained system.

The most minimalist model is the following:

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '3dof', ...
                                       'flex_top_type', '2dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');
G = linearize(mdl, io, 0.0, options);
There are 24 states.

These states are summarized on table tab:num_states_nano_hexapod.

Element States
Struts 2*6
Top Plate 12
Total: 24

If we add axial stiffness on the top joints, we should add 2 states for each struts.

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '2dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');
G = linearize(mdl, io, 0.0, options);
There are 36 states.

If we add torsional stiffness on the bottom joints, we should again add 2 states for each struts.

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');
G = linearize(mdl, io, 0.0, options);
There are 48 states.

Finally, if we add axial stiffness on the bottom joint, we should add 2 states for each struts.

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '4dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');
G = linearize(mdl, io, 0.0, options);
There are 60 states.

Obtained number of states is very comprehensible. Depending on the physical effects we want to model, we therefore know how many states are added when configuring the model.

Direct Velocity Feedback Plant

<<sec:dvf_plant>>

The transfer function from actuator forces $\tau_i$ to the encoder measurements $\mathcal{L}_i$ is now identified both when the encoders are fixed to the struts.

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'nano_hexapod';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'], 1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/D'], 1, 'openoutput'); io_i = io_i + 1; % Force Sensors

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');

Gdvf = linearize(mdl, io, 0.0, options);

The DC gain from actuator to relative motion sensor should be equal to (for the 2dof APA): \[ \frac{1}{k + k_a + kk_a/k_e} \]

Which is equal to:

DCgain = 1.87e-08 [m/N]

Let's verify that by looking at the DC gain of the $6 \times 6$ DVF plant in Table tab:dvf_dc_gain.

1.8617e-08 -1.0408e-10 1.3034e-10 3.2559e-11 -1.1188e-10 9.0385e-11
-5.1839e-11 1.8593e-08 -4.4868e-11 8.016e-11 4.3558e-11 -1.1164e-10
5.1963e-12 -6.9001e-12 1.8564e-08 3.0844e-11 4.0097e-11 -3.4387e-11
1.9359e-11 1.7432e-10 -5.0928e-11 1.855e-08 1.6406e-10 4.5757e-12
-2.1185e-11 2.1724e-10 1.5333e-12 -8.802e-11 1.8803e-08 -4.6946e-11
-1.1728e-11 -5.7682e-11 1.6213e-10 2.1934e-12 -1.6237e-10 1.8715e-08

And the bode plot of the DVF plant is shown in Figure fig:nano_hexapod_struts_2dof_dvf_plant.

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_struts_2dof_dvf_plant.png

Bode plot of the transfer functions from actuator forces $\tau_i$ to relative motion sensors attached to the struts $\mathcal{L}_i$. Diagonal terms are shown in blue, and off-diagonal terms in black.

Integral Force Feedback Plant

<<sec:iff_plant>>

The transfer function from actuators to force sensors is identified.

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'nano_hexapod';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');   io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/Fm'],  1, 'openoutput'); io_i = io_i + 1; % Force Sensors

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');

Giff = linearize(mdl, io, 0.0, options);

This is corresponding to the dynamics for the Integral Force Feedback (IFF) control strategy.

The bode plot is shown in Figure fig:nano_hexapod_struts_2dof_iff_plant.

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_struts_2dof_iff_plant.png

Bode plot of the transfer functions from actuator forces $\tau_i$ to force sensors $F_{m,i}$. Diagonal terms are shown in blue, and off-diagonal terms in black.

Decentralized Plant - Cartesian coordinates

<<sec:decoupled_plant>>

Introduction   ignore

Consider the plant shown in Figure fig:nano_hexapod_decentralized_schematic with:

  • $\tau$ the 6 input forces (APA)
  • $d\mathcal{L}$ the relative motion sensor outputs (encoders)
  • $\mathcal{X}$ the motion of the top platform measured with "perfect" 6-dof sensor
  • $J_a$ and $J_s$ the Jacobians for the actuators and sensors
\begin{tikzpicture}
  % Blocs
  \node[block={2.0cm}{2.0cm}] (P) {Plant};
  \coordinate[] (inputF)  at (P.west);
  \coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);
  \coordinate[] (outputX) at ($(P.south east)!0.2!(P.north east)$);

  \node[block, left= of inputF]   (Ja) {$\bm{J}^{-T}_a$};
  \node[block, right= of outputL] (Js) {$\bm{J}^{-1}_s$};

  % Connections and labels
  \draw[->] ($(Ja.west)+(-1,0)$) -- (Ja.west) node[above left]{$\bm{\mathcal{F}}$};
  \draw[->] (Ja.east) -- (inputF)  node[above left]{$\bm{\tau}$};
  \draw[->] (outputL) -- (Js.west) node[above left]{$d\bm{\mathcal{L}}$};
  \draw[->] (Js.east) -- ++(1, 0) node[above left]{$d\bm{\mathcal{X}}$};
  \draw[->] (outputX.east) -- ++(1, 0) node[above left]{$\bm{\mathcal{X}}$};
\end{tikzpicture}

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_decentralized_schematic.png

Plant in the cartesian Frame

Verification of the Sensor Jacobian

The "perfect" sensor output $\mathcal{X}$ is used to verify that the sensor Jacobian is working correctly both when the encoders are fixed to the struts and to the plates.

Let's then identify the plant for both configuration, and compare the transfer functions from $\mathcal{F}$ to $d\mathcal{X}$ and to $\mathcal{X}$.

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'nano_hexapod';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
io(io_i) = linio([mdl, '/X'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs

Start when the encoders are fixed on the struts.

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof', ...
                                       'MO_B', 150e-3);
Gs = linearize(mdl, io, 0.0, options);
Gs.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gs.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ...
                 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

% Cartesian plant using the Jacobians
Gsc = inv(n_hexapod.geometry.Js)*Gs({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
% Cartesian plant using the perfect sensor
Gsp = -Gs({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';

The diagonal elements of the plant are shown in Figure fig:nano_hexapod_comp_cartesian_plants_struts.

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_comp_cartesian_plants_struts.png

Bode plot of the diagonal elements of the decentralized (cartesian) plant when using the sensor Jacobian (solid) and when using "perfect" 6dof sensor (dashed). The encoders are fixed on the struts.

The same if performed when the encoders are fixed to the plates.

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'plates', ...
                                       'actuator_type', '2dof', ...
                                       'MO_B', 150e-3);
Gp = linearize(mdl, io, 0.0, options);
Gp.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
Gp.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ...
                 'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

% Cartesian plant using the Jacobians
Gpc = inv(n_hexapod.geometry.Js)*Gp({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
% Cartesian plant using the perfect sensor
Gpp = -Gp({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';

The obtained bode plots are shown in Figure fig:nano_hexapod_comp_cartesian_plants_plates.

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_comp_cartesian_plants_plates.png

Bode plot of the diagonal elements of the decentralized (cartesian) plant when using the sensor Jacobian (solid) and when using "perfect" 6dof sensor (dashed). The encoders are fixed on the plates.

The Jacobian for the encoders is working properly both when the encoders are fixed to the plates or to the struts.

However, then the encoders are fixed to the struts, there is a mismatch between the estimated motion and the measured motion above 100Hz due to a complex conjugate zero.

Comparison of the decentralized plants

The decentralized plants are now compared whether the encoders are fixed on the struts or on the plates in Figure fig:nano_hexapod_cartesian_plant_encoder_comp.

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_cartesian_plant_encoder_comp.png

Bode plot of the "cartesian" plant (transfer function from $\mathcal{F}$ to $d\mathcal{X}$) when the encoders are fixed on the struts (solid) and on the plates (dashed)

Decentralized Plant - Decoupling at the Center of Stiffness

<<sec:decoupled_plant_cok>>

Center of Stiffness

<<sec:center_of_stiffness>>

Let's define some parameters:

si = n_hexapod.geometry.si; % Orientation of struts
bi = n_hexapod.geometry.Fb; % Location of bi w.r.t. {F}
ki = ones(1,6); % Normalized strut stiffness

In order to find is the Center of Stiffness (CoK) exists, we have to verify is the following is diagonal:

ki.*si*si'
1.8977 2.4659e-17 5.1838e-19
2.4659e-17 1.8977 -2.3143e-05
5.1838e-19 -2.3143e-05 2.2046

And we can find the location of the CoK with respect to {F}:

OkX = (ki.*cross(bi, si)*si')/(ki.*si*si');
Ok = [OkX(3,2);OkX(1,3);OkX(2,1)]
-1.7444e-18
2.1511e-06
0.052707

The center of the cube is therefore 52.7mm above the bottom platform {F} frame.

Let's initialize the hexapod with frame {A} and {B} at the CoK:

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '3dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof', ...
                                       'MO_B', Ok(3)-95e-3);

And the (normalized) stiffness matrix is computed as follows:

n_hexapod.geometry.J'*diag(ki)*n_hexapod.geometry.J
1.8977 0 0 0 -2.0817e-17 -1.5311e-06
0 1.8977 -2.3143e-05 4.175e-06 0 0
0 -2.3143e-05 2.2046 4.7422e-06 0 0
0 4.175e-06 4.7422e-06 0.012594 2.1684e-19 -8.6736e-19
-1.8521e-17 0 0 0 0.012594 -9.3183e-08
-1.5311e-06 -6.9389e-18 2.7756e-17 -8.6736e-19 -9.3183e-08 0.043362

And we indeed obtain a diagonal stiffness matrix.

Obtained plant 

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'nano_hexapod';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/F'],  1, 'openinput');  io_i = io_i + 1; % Actuator Inputs
io(io_i) = linio([mdl, '/D'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
io(io_i) = linio([mdl, '/X'],  1, 'openoutput'); io_i = io_i + 1; % Relative Motion Outputs
G = linearize(mdl, io, 0.0, options);
G.InputName  = {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'};
G.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6', ...
                'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};

% % Cartesian plant using the Jacobians
% Gsc = inv(n_hexapod.geometry.Js)*Gs({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
% % Cartesian plant using the perfect sensor
% Gsp = -Gs({'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J)';
Gc = inv(n_hexapod.geometry.J)*G({'D1', 'D2', 'D3', 'D4', 'D5', 'D6'}, {'F1', 'F2', 'F3', 'F4', 'F5', 'F6'})*inv(n_hexapod.geometry.J');
9.8602e-09 7.8692e-11 1.4426e-11 -2.663e-10 2.8e-10 -4.7559e-11
8.457e-11 9.8788e-09 -2.4002e-11 -2.9502e-11 -1.3262e-10 -8.7346e-11
-4.3244e-11 2.4075e-13 8.4775e-09 1.1442e-11 -2.5809e-10 2.8796e-11
-1.8326e-09 -9.318e-10 6.8188e-10 1.4697e-06 5.5936e-09 8.7632e-10
4.6906e-10 1.5911e-09 1.6989e-10 -5.223e-09 1.4729e-06 -2.6059e-10
-6.5754e-11 -3.0408e-12 5.394e-11 -1.0917e-10 6.9479e-10 4.2979e-07

As the rotations and translations have very different gains, we normalize each motion to one.

Gc = diag(1./diag(dcgain(Gc)))*Gc;

The diagonal and off-diagonal elements are shown in Figure fig:nano_hexapod_diagonal_plant_cok, and we can see good decoupling at low frequency.

/tdehaeze/nass-simscape/media/commit/a3cf7fc5373b97958b5eb5348f0343a85dad8390/org/figs/nano_hexapod_diagonal_plant_cok.png

Diagonal and off-diagonal elements of the (normalized) decentralized plant with the Jacobians estimated at the "center of stiffness"

The Jacobian matrices can be used to decoupled the plant at low frequency.

Stiffness matrix

Introduction   ignore

The stiffness matrix of the nano-hexapod describes its induced static displacement/rotation when a force/torque is applied on its top platform. The location of the applied force/torque and the expressed displacement/rotation can be defined as wanted. Such location (or frame) is then used for the computation of the Jacobian which in turns is used to compute the stiffness matrix.

Compute the theoretical stiffness of the nano-hexapod

Neglecting stiffness of the joints, we have: \[ K = J^t \mathcal{K} J \] where $\mathcal{K}$ is a diagonal 6x6 matrix with axial stiffness of the struts on the diagonal.

Let's note the axial stiffness of the APA: \[ k_{\text{APA}} = k + \frac{k_e k_a}{k_e + k_a} \]

Them axial stiffness of the struts $k_s$: \[ k_s = \frac{k_z k_{\text{APA}}}{k_z + 2 k_{\text{APA}}} \] with $k_z$ the axial stiffness of the flexible joints.

n_hexapod = initializeNanoHexapodFinal('flex_bot_type', '4dof', ...
                                       'flex_top_type', '4dof', ...
                                       'motion_sensor_type', 'struts', ...
                                       'actuator_type', '2dof');
k  = n_hexapod.actuator.k(1);
ke = n_hexapod.actuator.ke(1);
ka = n_hexapod.actuator.ka(1);
kz = n_hexapod.flex_top.kz(1);
kAPA = k + ke*ka/(ke + ka);
kAPA = 1.799e+06 [N/m]

ks = kz*kAPA/(kz + 2*kAPA);
ks = 1.737e+06 [N/m]

We can see that the axial stiffness of the flexible joint as little impact on the total axial stiffness of the struts.

Let's now compute the stiffness matrix corresponding to an hexapod with perfect joints and the above computed axial stiffness:

Ks = n_hexapod.geometry.J'*(ks*eye(6))*n_hexapod.geometry.J;

And the compliance matrix can be computed as the inverse of the stiffness matrix:

C = inv(Ks);
1.9938e-06 -2.3138e-22 3.3403e-23 1.0202e-21 8.7906e-06 2.9603e-11
-3.1875e-23 1.9938e-06 2.2094e-11 -8.7909e-06 -1.6576e-22 -3.5622e-28
6.6811e-23 2.2094e-11 2.6115e-07 -9.8337e-11 3.4744e-22 7.4663e-28
1.4054e-22 -8.7909e-06 -9.8337e-11 4.5715e-05 7.3086e-22 1.5706e-27
8.7906e-06 -1.0202e-21 1.7371e-22 4.498e-21 4.5714e-05 9.8237e-11
2.9603e-11 -1.9261e-22 -1.7611e-27 8.4925e-22 9.8237e-11 1.3277e-05

Comparison with Simscape Model 

%% Options for Linearized
options = linearizeOptions;
options.SampleTime = 0;

%% Name of the Simulink File
mdl = 'nano_hexapod';

%% Input/Output definition
clear io; io_i = 1;
io(io_i) = linio([mdl, '/Fe'],  1, 'openinput');  io_i = io_i + 1;
io(io_i) = linio([mdl, '/X'],   1, 'openoutput'); io_i = io_i + 1;

G = linearize(mdl, io, 0.0, options);
1.9863e-06 2.4683e-08 2.2859e-09 -6.4768e-08 8.7307e-06 3.2754e-10
-3.693e-09 1.9663e-06 -7.4503e-09 -8.615e-06 -6.7926e-08 -2.0193e-08
-3.5525e-09 -8.8671e-10 2.6042e-07 4.7504e-09 -9.9677e-09 1.7242e-10
2.1133e-08 -8.6554e-06 3.2841e-08 4.4849e-05 3.0873e-07 8.1525e-08
8.7375e-06 9.9165e-08 8.135e-09 -2.5208e-07 4.5331e-05 3.0602e-09
6.1611e-09 6.1733e-09 2.6778e-09 -3.3188e-08 3.3887e-08 1.3212e-05

Function - Initialize Nano Hexapod

<<sec:initializeNanoHexapodFinal>>

Function description 

function [nano_hexapod] = initializeNanoHexapodFinal(args)

Optional Parameters 

arguments
    %% Bottom Flexible Joints
    args.flex_bot_type      char {mustBeMember(args.flex_bot_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
    args.flex_bot_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
    args.flex_bot_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
    args.flex_bot_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
    args.flex_bot_kz  (6,1) double {mustBeNumeric} = ones(6,1)*1e8 % Axial Stiffness [N/m]
    args.flex_bot_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % X bending Damping [Nm/(rad/s)]
    args.flex_bot_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Y bending Damping [Nm/(rad/s)]
    args.flex_bot_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Torsionnal Damping [Nm/(rad/s)]
    args.flex_bot_cz  (6,1) double {mustBeNumeric} = ones(6,1)*1e2 % Axial Damping [N/(m/s)]
    %% Top Flexible Joints
    args.flex_top_type      char {mustBeMember(args.flex_top_type,{'2dof', '3dof', '4dof', 'flexible'})} = '4dof'
    args.flex_top_kRx (6,1) double {mustBeNumeric} = ones(6,1)*5 % X bending stiffness [Nm/rad]
    args.flex_top_kRy (6,1) double {mustBeNumeric} = ones(6,1)*5 % Y bending stiffness [Nm/rad]
    args.flex_top_kRz (6,1) double {mustBeNumeric} = ones(6,1)*260 % Torsionnal stiffness [Nm/rad]
    args.flex_top_kz  (6,1) double {mustBeNumeric} = ones(6,1)*1e8 % Axial Stiffness [N/m]
    args.flex_top_cRx (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % X bending Damping [Nm/(rad/s)]
    args.flex_top_cRy (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Y bending Damping [Nm/(rad/s)]
    args.flex_top_cRz (6,1) double {mustBeNumeric} = ones(6,1)*0.1 % Torsionnal Damping [Nm/(rad/s)]
    args.flex_top_cz  (6,1) double {mustBeNumeric} = ones(6,1)*1e2 % Axial Damping [N/(m/s)]
    %% Jacobian - Location of frame {A} and {B}
    args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
    %% Relative Motion Sensor
    args.motion_sensor_type char {mustBeMember(args.motion_sensor_type,{'struts', 'plates'})} = 'struts'
    %% Actuators
    args.actuator_type char {mustBeMember(args.actuator_type,{'2dof', 'flexible frame', 'flexible'})} = 'flexible'
    args.actuator_Ga  (6,1) double {mustBeNumeric} = ones(6,1)*1 % Actuator gain [N/V]
    args.actuator_Gs  (6,1) double {mustBeNumeric} = ones(6,1)*1 % Sensor gain [V/m]
                                                                 % For 2DoF
    args.actuator_k   (6,1) double {mustBeNumeric} = ones(6,1)*0.35e6 % [N/m]
    args.actuator_ke  (6,1) double {mustBeNumeric} = ones(6,1)*1.5e6 % [N/m]
    args.actuator_ka  (6,1) double {mustBeNumeric} = ones(6,1)*43e6 % [N/m]
    args.actuator_c   (6,1) double {mustBeNumeric} = ones(6,1)*3e1 % [N/(m/s)]
    args.actuator_ce  (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
    args.actuator_ca  (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % [N/(m/s)]
    args.actuator_Leq (6,1) double {mustBeNumeric} = ones(6,1)*0.056 % [m]
                                                                     % For Flexible Frame
    args.actuator_ks (6,1) double {mustBeNumeric} = ones(6,1)*235e6 % Stiffness of one stack [N/m]
    args.actuator_cs (6,1) double {mustBeNumeric} = ones(6,1)*1e1 % Stiffness of one stack [N/m]
                                                                  % For Flexible
    args.actuator_xi  (1,1) double {mustBeNumeric} = 0.01 % Damping Ratio
end

Nano Hexapod Object 

nano_hexapod = struct();

Flexible Joints - Bot 

nano_hexapod.flex_bot = struct();

switch args.flex_bot_type
  case '2dof'
    nano_hexapod.flex_bot.type = 1;
  case '3dof'
    nano_hexapod.flex_bot.type = 2;
  case '4dof'
    nano_hexapod.flex_bot.type = 3;
  case 'flexible'
    nano_hexapod.flex_bot.type = 4;
end

nano_hexapod.flex_bot.kRx = args.flex_bot_kRx; % X bending stiffness [Nm/rad]
nano_hexapod.flex_bot.kRy = args.flex_bot_kRy; % Y bending stiffness [Nm/rad]
nano_hexapod.flex_bot.kRz = args.flex_bot_kRz; % Torsionnal stiffness [Nm/rad]
nano_hexapod.flex_bot.kz  = args.flex_bot_kz;  % Axial stiffness [N/m]

nano_hexapod.flex_bot.cRx = args.flex_bot_cRx; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cRy = args.flex_bot_cRy; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cRz = args.flex_bot_cRz; % [Nm/(rad/s)]
nano_hexapod.flex_bot.cz  = args.flex_bot_cz;  %[N/(m/s)]

Flexible Joints - Top 

nano_hexapod.flex_top = struct();

switch args.flex_top_type
  case '2dof'
    nano_hexapod.flex_top.type = 1;
  case '3dof'
    nano_hexapod.flex_top.type = 2;
  case '4dof'
    nano_hexapod.flex_top.type = 3;
  case 'flexible'
    nano_hexapod.flex_top.type = 4;
end

nano_hexapod.flex_top.kRx = args.flex_top_kRx; % X bending stiffness [Nm/rad]
nano_hexapod.flex_top.kRy = args.flex_top_kRy; % Y bending stiffness [Nm/rad]
nano_hexapod.flex_top.kRz = args.flex_top_kRz; % Torsionnal stiffness [Nm/rad]
nano_hexapod.flex_top.kz  = args.flex_top_kz;  % Axial stiffness [N/m]

nano_hexapod.flex_top.cRx = args.flex_top_cRx; % [Nm/(rad/s)]
nano_hexapod.flex_top.cRy = args.flex_top_cRy; % [Nm/(rad/s)]
nano_hexapod.flex_top.cRz = args.flex_top_cRz; % [Nm/(rad/s)]
nano_hexapod.flex_top.cz  = args.flex_top_cz;  %[N/(m/s)]

Relative Motion Sensor 

nano_hexapod.motion_sensor = struct();

switch args.motion_sensor_type
  case 'struts'
    nano_hexapod.motion_sensor.type = 1;
  case 'plates'
    nano_hexapod.motion_sensor.type = 2;
end

Amplified Piezoelectric Actuator 

nano_hexapod.actuator = struct();

switch args.actuator_type
  case '2dof'
    nano_hexapod.actuator.type = 1;
  case 'flexible frame'
    nano_hexapod.actuator.type = 2;
  case 'flexible'
    nano_hexapod.actuator.type = 3;
end
nano_hexapod.actuator.Ga = args.actuator_Ga; % Actuator gain [N/V]
nano_hexapod.actuator.Gs = args.actuator_Gs; % Sensor gain [V/m]

2dof

nano_hexapod.actuator.k  = args.actuator_k;  % [N/m]
nano_hexapod.actuator.ke = args.actuator_ke; % [N/m]
nano_hexapod.actuator.ka = args.actuator_ka; % [N/m]

nano_hexapod.actuator.c  = args.actuator_c;  % [N/(m/s)]
nano_hexapod.actuator.ce = args.actuator_ce; % [N/(m/s)]
nano_hexapod.actuator.ca = args.actuator_ca; % [N/(m/s)]

nano_hexapod.actuator.Leq = args.actuator_Leq; % [m]

Flexible frame and fully flexible

switch args.actuator_type
  case 'flexible frame'
    nano_hexapod.actuator.K = readmatrix('APA300ML_b_mat_K.CSV'); % Stiffness Matrix
    nano_hexapod.actuator.M = readmatrix('APA300ML_b_mat_M.CSV'); % Mass Matrix
    nano_hexapod.actuator.P = extractNodes('APA300ML_b_out_nodes_3D.txt'); % Node coordinates [m]
  case 'flexible'
    nano_hexapod.actuator.K = readmatrix('full_APA300ML_K.CSV'); % Stiffness Matrix
    nano_hexapod.actuator.M = readmatrix('full_APA300ML_M.CSV'); % Mass Matrix
    nano_hexapod.actuator.P = extractNodes('full_APA300ML_out_nodes_3D.txt'); % Node coordiantes [m]
end

nano_hexapod.actuator.xi = args.actuator_xi; % Damping ratio

nano_hexapod.actuator.ks = args.actuator_ks; % Stiffness of one stack [N/m]
nano_hexapod.actuator.cs = args.actuator_cs; % Damping of one stack [N/m]

Geometry 

nano_hexapod.geometry = struct();

Center of joints $a_i$ with respect to {F}:

Fa = [[-86.05,  -74.78, 22.49],
      [ 86.05,  -74.78, 22.49],
      [ 107.79, -37.13, 22.49],
      [ 21.74,  111.91, 22.49],
      [-21.74,  111.91, 22.49],
      [-107.79, -37.13, 22.49]]'*1e-3; % Ai w.r.t. {F} [m]

Center of joints $b_i$ with respect to {M}:

Mb = [[-28.47, -106.25, -22.50],
      [ 28.47, -106.25, -22.50],
      [ 106.25, 28.47,  -22.50],
      [ 77.78,  77.78,  -22.50],
      [-77.78,  77.78,  -22.50],
      [-106.25, 28.47,  -22.50]]'*1e-3; % Bi w.r.t. {M} [m]

Now compute the positions $b_i$ with respect to {F}:

Fb = Mb + [0; 0; 95e-3]; % Bi w.r.t. {F} [m]

The unit vector representing the orientation of the struts can then be computed:

si = Fb - Fa;
si = si./vecnorm(si); % Normalize

Location of encoder measurement points when fixed on the plates:

Fc = [[-29.362, -105.765, 52.605]
      [ 29.362, -105.765, 52.605]
      [ 106.276, 27.454,  52.605]
      [ 76.914,  78.31,   52.605]
      [-76.914,  78.31,   52.605]
      [-106.276, 27.454,  52.605]]'*1e-3; % Meas pos w.r.t. {F}
Mc = Fc - [0; 0; 95e-3]; % Meas pos w.r.t. {M}
nano_hexapod.geometry.Fa = Fa;
nano_hexapod.geometry.Fb = Fb;
nano_hexapod.geometry.Fc = Fc;
nano_hexapod.geometry.Mb = Mb;
nano_hexapod.geometry.Mc = Mc;
nano_hexapod.geometry.si = si;
nano_hexapod.geometry.MO_B = args.MO_B;

Jacobian for Actuators 

Bb = Mb - [0; 0; args.MO_B];

nano_hexapod.geometry.J = [nano_hexapod.geometry.si', cross(Bb, nano_hexapod.geometry.si)'];

Jacobian for Sensors 

switch args.motion_sensor_type
  case 'struts'
    nano_hexapod.geometry.Js = nano_hexapod.geometry.J;
  case 'plates'
    Bc = Mc - [0; 0; args.MO_B];
    nano_hexapod.geometry.Js = [nano_hexapod.geometry.si', cross(Bc, nano_hexapod.geometry.si)'];
end

Save the Structure 

if nargout == 0
  save('./mat/stages.mat', 'nano_hexapod', '-append');
end