4679 lines
182 KiB
Org Mode
4679 lines
182 KiB
Org Mode
#+TITLE: Multi-Body Model - Micro Station
|
|
:DRAWER:
|
|
#+LANGUAGE: en
|
|
#+EMAIL: dehaeze.thomas@gmail.com
|
|
#+AUTHOR: Dehaeze Thomas
|
|
|
|
#+HTML_LINK_HOME: ../index.html
|
|
#+HTML_LINK_UP: ../index.html
|
|
|
|
#+HTML_HEAD: <link rel="stylesheet" type="text/css" href="https://research.tdehaeze.xyz/css/style.css"/>
|
|
#+HTML_HEAD: <script type="text/javascript" src="https://research.tdehaeze.xyz/js/script.js"></script>
|
|
|
|
#+BIND: org-latex-image-default-option "scale=1"
|
|
#+BIND: org-latex-image-default-width ""
|
|
|
|
#+LaTeX_CLASS: scrreprt
|
|
#+LaTeX_CLASS_OPTIONS: [a4paper, 10pt, DIV=12, parskip=full, bibliography=totoc]
|
|
#+LATEX_HEADER: \input{preamble.tex}
|
|
#+LATEX_HEADER_EXTRA: \input{preamble_extra.tex}
|
|
#+LATEX_HEADER_EXTRA: \bibliography{simscape-micro-station.bib}
|
|
|
|
#+BIND: org-latex-bib-compiler "biber"
|
|
|
|
#+PROPERTY: header-args:matlab :session *MATLAB*
|
|
#+PROPERTY: header-args:matlab+ :comments org
|
|
#+PROPERTY: header-args:matlab+ :exports none
|
|
#+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
|
|
#+PROPERTY: header-args:matlab+ :tangle no
|
|
|
|
#+PROPERTY: header-args:latex :headers '("\\usepackage{tikz}" "\\usepackage{import}" "\\import{$HOME/Cloud/tikz/org/}{config.tex}")
|
|
#+PROPERTY: header-args:latex+ :imagemagick t :fit yes
|
|
#+PROPERTY: header-args:latex+ :iminoptions -scale 100% -density 150
|
|
#+PROPERTY: header-args:latex+ :imoutoptions -quality 100
|
|
#+PROPERTY: header-args:latex+ :results file raw replace
|
|
#+PROPERTY: header-args:latex+ :buffer no
|
|
#+PROPERTY: header-args:latex+ :tangle no
|
|
#+PROPERTY: header-args:latex+ :eval no-export
|
|
#+PROPERTY: header-args:latex+ :exports results
|
|
#+PROPERTY: header-args:latex+ :mkdirp yes
|
|
#+PROPERTY: header-args:latex+ :output-dir figs
|
|
#+PROPERTY: header-args:latex+ :post pdf2svg(file=*this*, ext="png")
|
|
:END:
|
|
|
|
#+latex: \clearpage
|
|
|
|
* Build :noexport:
|
|
#+NAME: startblock
|
|
#+BEGIN_SRC emacs-lisp :results none :tangle no
|
|
(add-to-list 'org-latex-classes
|
|
'("scrreprt"
|
|
"\\documentclass{scrreprt}"
|
|
("\\chapter{%s}" . "\\chapter*{%s}")
|
|
("\\section{%s}" . "\\section*{%s}")
|
|
("\\subsection{%s}" . "\\subsection*{%s}")
|
|
("\\paragraph{%s}" . "\\paragraph*{%s}")
|
|
))
|
|
|
|
|
|
;; Remove automatic org heading labels
|
|
(defun my-latex-filter-removeOrgAutoLabels (text backend info)
|
|
"Org-mode automatically generates labels for headings despite explicit use of `#+LABEL`. This filter forcibly removes all automatically generated org-labels in headings."
|
|
(when (org-export-derived-backend-p backend 'latex)
|
|
(replace-regexp-in-string "\\\\label{sec:org[a-f0-9]+}\n" "" text)))
|
|
(add-to-list 'org-export-filter-headline-functions
|
|
'my-latex-filter-removeOrgAutoLabels)
|
|
|
|
;; Remove all org comments in the output LaTeX file
|
|
(defun delete-org-comments (backend)
|
|
(loop for comment in (reverse (org-element-map (org-element-parse-buffer)
|
|
'comment 'identity))
|
|
do
|
|
(setf (buffer-substring (org-element-property :begin comment)
|
|
(org-element-property :end comment))
|
|
"")))
|
|
(add-hook 'org-export-before-processing-hook 'delete-org-comments)
|
|
|
|
;; Use no package by default
|
|
(setq org-latex-packages-alist nil)
|
|
(setq org-latex-default-packages-alist nil)
|
|
|
|
;; Do not include the subtitle inside the title
|
|
(setq org-latex-subtitle-separate t)
|
|
(setq org-latex-subtitle-format "\\subtitle{%s}")
|
|
|
|
(setq org-export-before-parsing-hook '(org-ref-glossary-before-parsing
|
|
org-ref-acronyms-before-parsing))
|
|
#+END_SRC
|
|
|
|
* Notes :noexport:
|
|
** Notes
|
|
Prefix is =ustation=
|
|
|
|
From modal analysis: validation of the multi-body model.
|
|
*Goals*:
|
|
- *Modeling of the micro-station*: Kinematics + Dynamics + Disturbances
|
|
- Kinematics of each stage
|
|
- Modeling: solid bodies + joints. Show what is used for each stage
|
|
- Correlation with the dynamical measurements
|
|
- Inclusion of disturbances (correlation with measurements)
|
|
*Notes*:
|
|
- Do not talk about Nano-Hexapod yet
|
|
- Do not talk about external metrology
|
|
- Therefore, do not talk about computation of the sample position / error
|
|
|
|
** DONE [#B] Put colors for each different stage
|
|
CLOSED: [2024-10-30 Wed 14:07]
|
|
|
|
** DONE [#B] Delete Gravity compensation Stage
|
|
CLOSED: [2024-10-30 Wed 13:51]
|
|
|
|
** DONE [#B] Maybe make a simpler Simscape model for this report
|
|
CLOSED: [2024-10-30 Wed 13:51]
|
|
|
|
** DONE [#A] Open the Simscape model and verify it all works
|
|
CLOSED: [2024-10-30 Wed 13:33] SCHEDULED: <2024-10-30 Wed>
|
|
|
|
#+begin_src matlab
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
|
|
initializeGround( 'type', 'rigid');
|
|
initializeGranite( 'type', 'flexible');
|
|
initializeTy( 'type', 'flexible');
|
|
initializeRy( 'type', 'flexible');
|
|
initializeRz( 'type', 'flexible');
|
|
initializeMicroHexapod('type', 'flexible');
|
|
|
|
initializeReferences();
|
|
initializeDisturbances('enable', false);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
|
|
% initializeAxisc( 'type', 'flexible');
|
|
|
|
% initializeMirror( 'type', 'none');
|
|
% initializeNanoHexapod( 'type', 'none');
|
|
% initializeSample( 'type', 'none');
|
|
|
|
initializeController( 'type', 'open-loop');
|
|
|
|
#+end_src
|
|
|
|
** DONE [#B] Just keep smallest number of variants for each stage
|
|
CLOSED: [2024-10-30 Wed 16:15]
|
|
|
|
- [ ] none
|
|
- [ ] rigid
|
|
- [ ] flexible
|
|
- [X] Init => Removed
|
|
- [X] modal analysis => Removed
|
|
|
|
** DONE [#A] Verify that we get "correct" compliance
|
|
CLOSED: [2024-10-30 Wed 21:53] SCHEDULED: <2024-10-30 Wed>
|
|
|
|
- [ ] Find the compliance measurements
|
|
- The one from modal analysis
|
|
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/2018-01-12 - Marc]] : Compliance measurement in X,Y,Z with geophone, marble not glued
|
|
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/2018-10-12 - Marc]]: same but in the hutch with glued marble
|
|
- *08/2020*: [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/micro-station-compliance/index.org::+TITLE: Compliance Measurement of the Micro Station]]
|
|
- [ ] See if it matches somehow the current model
|
|
- [ ] If not, see if model parameters can be tuned to have better match
|
|
For instance from values here: file:/home/thomas/Cloud/meetings/esrf-meetings/2018-04-24-Simscape-Model/2018-04-24-Simscape-Model.pdf
|
|
|
|
** DONE [#B] Make sure to well model the micro-hexapod
|
|
CLOSED: [2024-10-31 Thu 10:36] SCHEDULED: <2024-10-31 Thu>
|
|
|
|
- [ ] Find total mass (should be 37 kg)
|
|
- [ ] Find mass of top platform, bottom platform, struts
|
|
- [ ] Estimation of the strut stiffness from the manufacturer stiffness?
|
|
X,Y: 5N/um
|
|
Z: 50N/um
|
|
From the geometry, compute the strut stiffness: should be 10N/um (currently configured at 20N/um)
|
|
|
|
** DONE [#A] Import all relevant report to this file
|
|
CLOSED: [2024-11-05 Tue 22:56]
|
|
|
|
Based on:
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/kinematics.org][kinematics]]: compute sample's motion from each stage motion
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/simscape_subsystems.org][simscape_subsystems]]: general presentation of the micro-station. Used model: solid body + joints. Presentation of each stage.
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/documents/work-package-1/work-package-1.org::*Specification of requirements][Specification of requirements]]
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/identification.org][identification]]: comparison of measurements and simscape model (not so good?)
|
|
- [X] file:/home/thomas/Cloud/meetings/esrf-meetings/2018-04-24-Simscape-Model/2018-04-24-Simscape-Model.pdf
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/experiments.org][experiments]]: simulation of experiments with the Simscape model
|
|
- [X] Disturbances: Similar to what was done for the uniaxial model (the same?): [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/disturbances.org::+TITLE: Identification of the disturbances]]
|
|
- [-] Measurement of disturbances / things that will have to be corrected using the NASS:
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-control-system/index.org]]
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/disturbance-sr-rz/index.org]]
|
|
Already discussed in the uniaxial report
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/ground-motion/index.org]]
|
|
Already discussed in the uniaxial report
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-spindle/index.org]]
|
|
Can be nice to have all errors of the Spindle (angular and linear)
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-to-dynamic/index.org]]
|
|
Similarly, errors of the translation stage are computed
|
|
- [X] Check [[file:~/Cloud/work-projects/ID31-NASS/specifications/id-31-spindle-meas][this directory]]
|
|
Nice pictures if necessary
|
|
- [X] Check [[file:~/Cloud/work-projects/ID31-NASS/specifications/stage-by-stage][this directory]]
|
|
|
|
** DONE [#B] Make good "init" for the Simscape model
|
|
CLOSED: [2024-11-05 Tue 22:57]
|
|
|
|
In model properties => Callbacks => Init Fct
|
|
There are some loading of .mat files.
|
|
Is it the best option?
|
|
|
|
** DONE [#C] Check the effect of each stage on the compliance
|
|
CLOSED: [2024-10-31 Thu 10:37]
|
|
|
|
- [X] Put =rigid= mode one by one to see the effect
|
|
- Mode at 20Hz is Rx,Ry mode of the granite
|
|
- Mode at 60Hz in vertical is granite + Ty
|
|
- Main modes are due to micro-hexapod
|
|
|
|
** DONE [#C] Add two perturbations: =Frz_x= and =Frz_y=
|
|
CLOSED: [2024-11-05 Tue 21:44]
|
|
Maybe I can estimate them from the measurements that was made on the spindle?
|
|
|
|
** DONE [#B] Verify Kinematics
|
|
CLOSED: [2024-11-05 Tue 08:55]
|
|
|
|
- [ ] Move all stages
|
|
- [ ] From kinematics, estimate pose of micro-hexapod top platform
|
|
- [ ] Compare with 6DoF metrology
|
|
|
|
** DONE [#C] Determine which Degree-Of-Freedom to keep and which to constrain
|
|
CLOSED: [2024-10-31 Thu 10:38]
|
|
|
|
For instance, for now the granite can not rotate, but in reality, the modes may be linked to the granite's rotation.
|
|
By constraining more DoF, the simulation will be faster and the obtain state space will have a lower order.
|
|
|
|
=> *All are 6dof*
|
|
|
|
** DONE [#B] Compute eigenvalues of the model to see if we have similar frequencies than the modal analysis?
|
|
CLOSED: [2024-10-31 Thu 10:38]
|
|
|
|
Yes, it is pretty good!
|
|
|
|
** DONE [#B] Describe sample's position from all stage 6DoF motion
|
|
CLOSED: [2024-11-05 Tue 13:30]
|
|
|
|
** CANC [#B] Maybe speak about Screw axis?
|
|
CLOSED: [2024-11-05 Tue 13:30]
|
|
|
|
- State "CANC" from "TODO" [2024-11-05 Tue 13:30]
|
|
Can be computed from rotation matrix.
|
|
It seems it is used for the computation of the errors
|
|
|
|
** DONE [#C] Rework disturbance measurements
|
|
CLOSED: [2024-11-05 Tue 21:44]
|
|
|
|
Model vibrations induced by the scanning of stages.
|
|
Therefore, we don't model vibrations of the Ry stage or Micro-Hexapod.
|
|
*The goal of this section is to make this =dist_psd.mat= file containing the description of all disturbances*
|
|
|
|
- *Ground motion* already measured (similar in 3 directions?)
|
|
- *Spindle*: already measured but only in vertical direction
|
|
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-spindle/index.org]]
|
|
Here it is in the *PEL* laboratory, without the micro-station.
|
|
This is not too problematic.
|
|
- [X] Picture of the measurement setup
|
|
- [X] Plot measurement errors (X/Y, Z, Rx/Ry)
|
|
- [X] Extract PSD of disturbance forces (Probably X, Y and Z)
|
|
- *Ty stage*: new measurement
|
|
- [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-measurements/static-to-dynamic/index.org]]
|
|
- [ ] Picture of the measurement setup
|
|
- [ ] Plot measurement
|
|
- [ ] Extract PSD of vertical disturbance force
|
|
|
|
Say which disturbances are considered:
|
|
- Ground motions: x, y and z
|
|
- Ty: x and z
|
|
- Rz: z, x and y ?
|
|
|
|
** DONE [#C] Make a comparison of the measured vibrations of the micro-station with the vibrations of the simscape model of the micro-station
|
|
CLOSED: [2024-11-06 Wed 12:28]
|
|
Do we have a correlation? At least in the frequency domain?
|
|
|
|
Procedure:
|
|
- [ ] Take the time domain measurement of the vibrations due to spindle or translation stage
|
|
- [ ] Take the estimated PSD of the estimated disturbance force
|
|
- [ ] Simulate the Simscape model without the nano-hexapod (same conditions as when measuring the disturbances) and add only the considered disturbance
|
|
- [ ] Save the position errors due to the disturbance
|
|
- [ ] Compare with the measured vibrations
|
|
|
|
|
|
** DONE [#A] Update the disturbances PSD signals
|
|
CLOSED: [2024-11-06 Wed 12:28]
|
|
|
|
[[*=initializeDisturbances=: Initialize Disturbances][=initializeDisturbances=: Initialize Disturbances]]
|
|
|
|
- [X] It is suppose in this script that all disturbances have the same frequency vectors, and therefore the same time vector...
|
|
It does not anymore
|
|
- [X] See how to deal with that
|
|
|
|
Be able to pass custom =.mat= files (one mat file per disturbance)?
|
|
|
|
- [ ] Ground motion, X, Y and Z
|
|
- [ ] Ty stage, X and Z
|
|
- [ ] Rz stage, X, Y and Z
|
|
- Maybe say that we remove the excentricity (by circle fit: show it in the figure)
|
|
- Then the rest is modeled by stochastic disturbance
|
|
|
|
** DONE [#C] Add picture of measured ground motion
|
|
CLOSED: [2024-11-06 Wed 16:29]
|
|
** DONE [#C] Add screenshot of Simscape model
|
|
CLOSED: [2024-11-06 Wed 16:29]
|
|
** DONE [#B] I have no measurement of horizontal ground motion :@marc:
|
|
CLOSED: [2024-11-06 Wed 16:29]
|
|
|
|
- State "DONE" from "WAIT" [2024-11-06 Wed 16:29]
|
|
- Wait for Marc reply
|
|
|
|
** TODO [#C] Add glossary
|
|
|
|
PoI | Point of interest
|
|
|
|
* Introduction :ignore:
|
|
|
|
From the start of this work, it became increasingly clear that an accurate micro-station model was necessary.
|
|
|
|
First, during the uniaxial study, it became clear that the micro-station dynamics affects the nano-hexapod dynamics.
|
|
Then, using the 3-DoF rotating model, it was discovered that the rotation of the nano-hexapod induces gyroscopic effects that affect the system dynamics and should therefore be modeled.
|
|
Finally, a modal analysis of the micro-station showed how complex the dynamics of the station is.
|
|
The modal analysis also confirm that each stage behaves as a rigid body in the frequency range of interest.
|
|
Therefore, a multi-body model is a good candidate to accurately represent the micro-station dynamics.
|
|
|
|
In this report, the development of such a multi-body model is presented.
|
|
|
|
First, each stage of the micro-station is described.
|
|
The kinematics of the micro-station (i.e. how the motion of the stages are combined) is presented in Section ref:sec:ustation_kinematics.
|
|
|
|
Then, the multi-body model is presented and tuned to match the measured dynamics of the micro-station (Section ref:sec:ustation_modeling).
|
|
|
|
Disturbances affecting the positioning accuracy also need to be modeled properly.
|
|
To do so, the effects of these disturbances were first measured experimental and then injected into the multi-body model (Section ref:sec:ustation_disturbances).
|
|
|
|
To validate the accuracy of the micro-station model, "real world" experiments are simulated and compared with measurements in Section ref:sec:ustation_experiments.
|
|
|
|
# #+name: tab:ustation_section_matlab_code
|
|
# #+caption: Report sections and corresponding Matlab files
|
|
# #+attr_latex: :environment tabularx :width 0.6\linewidth :align lX
|
|
# #+attr_latex: :center t :booktabs t
|
|
# | *Sections* | *Matlab File* |
|
|
# |---------------------------------------+-----------------------------|
|
|
# | Section ref:sec:ustation_kinematics | =ustation_1_kinematics.m= |
|
|
# | Section ref:sec:ustation_modeling | =ustation_2_modeling.m= |
|
|
# | Section ref:sec:ustation_disturbances | =ustation_3_disturbances.m= |
|
|
# | Section ref:sec:ustation_experiments | =ustation_4_experiments.m= |
|
|
|
|
* Micro-Station Kinematics
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/ustation_1_kinematics.m
|
|
:END:
|
|
<<sec:ustation_kinematics>>
|
|
** Introduction :ignore:
|
|
|
|
The micro-station consists of 4 stacked positioning stages (Figure ref:fig:ustation_cad_view).
|
|
From bottom to top, the stacked stages are the translation stage $D_y$, the tilt stage $R_y$, the rotation stage (Spindle) $R_z$ and the positioning hexapod.
|
|
Such a stacked architecture allows high mobility, but the overall stiffness is reduced, and the dynamics is very complex. complex dynamics.
|
|
|
|
#+name: fig:ustation_cad_view
|
|
#+caption: CAD view of the micro-station with the translation stage (in blue), tilt stage (in red), rotation stage (in yellow) and positioning hexapod (in purple). On top of these four stages, a solid part (shown in green) will be replaced by the stabilization stage.
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/ustation_cad_view.png]]
|
|
|
|
There are different ways of modeling the stage dynamics in a multi-body model.
|
|
The one chosen in this work consists of modeling each stage by two solid bodies connected by one 6-DoF joint.
|
|
The stiffness and damping properties of the joint
|
|
s can be tuned separately for each DoF.
|
|
|
|
The "controlled" DoF of each stage (for instance the $D_y$ direction for the translation stage) is modeled as infinitely rigid (i.e. its motion is imposed by a "setpoint") while the other DoFs have limited stiffness to model the different micro-station modes.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% ustation_1_kinematics.m
|
|
#+end_src
|
|
|
|
#+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 :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Motion Stages
|
|
<<ssec:ustation_stages>>
|
|
|
|
**** Translation Stage
|
|
|
|
The translation stage is used to position and scan the sample laterally with respect to the X-ray beam.
|
|
|
|
A linear motor was first used to enable fast and accurate scans.
|
|
It was later replaced with a stepper motor and lead-screw, as the feedback control used for the linear motor was unreliable[fn:12].
|
|
An optical linear encoder is used to measure the stage motion and for controlling the position.
|
|
|
|
Four cylindrical bearings[fn:4] are used to guide the motion (i.e. minimize the parasitic motions) and have high stiffness.
|
|
|
|
**** Tilt Stage
|
|
|
|
The tilt stage is guided by four linear motion guides[fn:1] which are placed such that the center of rotation coincide with the X-ray beam.
|
|
Each linear guide is very stiff in radial directions such that the only DoF with low stiffness is in $R_y$.
|
|
|
|
This stage is mainly used in /reflectivity/ experiments where the sample $R_y$ angle is scanned.
|
|
This stage can also be used to tilt the rotation axis of the Spindle.
|
|
|
|
To precisely control the $R_y$ angle, a stepper motor and two optical encoders are used in a PID feedback loop.
|
|
|
|
#+attr_latex: :options [b]{0.48\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:ustation_ty_stage
|
|
#+caption: Translation Stage
|
|
#+attr_latex: :scale 1 :float nil
|
|
[[file:figs/ustation_ty_stage.png]]
|
|
#+end_minipage
|
|
\hfill
|
|
#+attr_latex: :options [b]{0.48\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:ustation_ry_stage
|
|
#+caption: Tilt Stage
|
|
#+attr_latex: :scale 1 :float nil
|
|
[[file:figs/ustation_ry_stage.png]]
|
|
#+end_minipage
|
|
|
|
**** Spindle
|
|
|
|
Then, a rotation stage is used for tomography experiments.
|
|
It is composed of an air bearing spindle[fn:2], whose angular position is controlled with a 3 phase synchronous motor based on the reading of 4 optical encoders.
|
|
|
|
Additional rotary unions and slip-rings are used to be able to pass electrical signals, fluids and gazes through the rotation stage.
|
|
|
|
**** Micro-Hexapod
|
|
|
|
Finally, a Stewart platform[fn:3] is used to position the sample.
|
|
It includes a DC motor and an optical linear encoders in each of the six struts.
|
|
|
|
This stage is used to position the point of interest of the sample with respect to the spindle rotation axis.
|
|
It can also be used to precisely position the PoI vertically with respect to the x-ray.
|
|
|
|
#+attr_latex: :options [t]{0.49\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:ustation_rz_stage
|
|
#+caption: Rotation Stage (Spindle)
|
|
#+attr_latex: :scale 1 :float nil
|
|
[[file:figs/ustation_rz_stage.png]]
|
|
#+end_minipage
|
|
\hfill
|
|
#+attr_latex: :options [t]{0.49\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:ustation_hexapod_stage
|
|
#+caption: Micro Hexapod
|
|
#+attr_latex: :scale 1 :float nil
|
|
[[file:figs/ustation_hexapod_stage.png]]
|
|
#+end_minipage
|
|
|
|
** Mathematical description of a rigid body motion
|
|
<<ssec:ustation_motion_description>>
|
|
**** Introduction :ignore:
|
|
|
|
In this section, mathematical tools[fn:6] that are used to describe the motion of positioning stages are introduced.
|
|
|
|
First, the tools to describe the pose of a solid body (i.e. it's position and orientation) are introduced.
|
|
The motion induced by a positioning stage is described by transformation matrices.
|
|
Finally, the motions of all stacked stages are combined, and the sample's motion is computed from each stage motion.
|
|
|
|
**** Spatial motion representation
|
|
|
|
The /pose/ of a solid body relative to a specific frame can be described by six independent parameters.
|
|
Three parameters are typically used to describe its position, and three other parameters describe its orientation.
|
|
|
|
The /position/ of a point $P$ with respect to a frame $\{A\}$ can be described by a $3 \times 1$ position vector eqref:eq:ustation_position.
|
|
The name of the frame is usually added as a leading superscript: ${}^AP$ which reads as vector $P$ in frame $\{A\}$.
|
|
|
|
\begin{equation}\label{eq:ustation_position}
|
|
{}^AP = \begin{bmatrix} P_x\\ P_y\\ P_z \end{bmatrix}
|
|
\end{equation}
|
|
|
|
A pure translation of a solid body (i.e., of a frame $\{B\}$ attached to the solid body) can be described by the position ${}^AP_{O_B}$ as shown in Figure ref:fig:ustation_translation.
|
|
|
|
#+name: fig:ustation_transformation_schematics
|
|
#+caption: Rigid body motion representation. (\subref{fig:ustation_translation}) pure translation. (\subref{fig:ustation_rotation}) pure rotation. (\subref{fig:ustation_transformation}) combined rotation and translation.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_translation}Pure translation}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.8
|
|
[[file:figs/ustation_translation.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_rotation}Pure rotation}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.8
|
|
[[file:figs/ustation_rotation.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_transformation}General transformation}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.8
|
|
[[file:figs/ustation_transformation.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
The /orientation/ of a rigid body is the same at all its points (by definition).
|
|
Hence, the orientation of a rigid body can be viewed as that of a moving frame attached to the rigid body.
|
|
It can be represented in several different ways: the rotation matrix, the screw axis representation, and the Euler angles are common descriptions.
|
|
|
|
The rotation matrix ${}^A\bm{R}_B$ is a $3 \times 3$ matrix containing the Cartesian unit vectors of frame $\{\bm{B}\}$ represented in frame $\{\bm{A}\}$ eqref:eq:ustation_rotation_matrix.
|
|
|
|
\begin{equation}\label{eq:ustation_rotation_matrix}
|
|
{}^A\bm{R}_B = \left[ {}^A\hat{\bm{x}}_B | {}^A\hat{\bm{y}}_B | {}^A\hat{\bm{z}}_B \right] = \begin{bmatrix}
|
|
u_{x} & v_{x} & z_{x} \\
|
|
u_{y} & v_{y} & z_{y} \\
|
|
u_{z} & v_{z} & z_{z}
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
Consider a pure rotation of a rigid body ($\{\bm{A}\}$ and $\{\bm{B}\}$ are coincident at their origins, as shown in Figure ref:fig:ustation_rotation).
|
|
The rotation matrix can be used to express the coordinates of a point $P$ in a fixed frame $\{A\}$ (i.e. ${}^AP$) from its coordinate in the moving frame $\{B\}$ using Equation eqref:eq:ustation_rotation.
|
|
|
|
\begin{equation} \label{eq:ustation_rotation}
|
|
{}^AP = {}^A\bm{R}_B {}^BP
|
|
\end{equation}
|
|
|
|
|
|
For rotations along $x$, $y$ or $z$ axis, the formulas of the corresponding rotation matrices are given in Equation eqref:eq:ustation_rotation_matrices_xyz.
|
|
|
|
\begin{subequations}\label{eq:ustation_rotation_matrices_xyz}
|
|
\begin{align}
|
|
\bm{R}_x(\theta_x) &= \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos(\theta_x) & -\sin(\theta_x) \\ 0 & \sin(\theta_x) & \cos(\theta_x) \end{bmatrix} \\
|
|
\bm{R}_y(\theta_y) &= \begin{bmatrix} \cos(\theta_y) & 0 & \sin(\theta_y) \\ 0 & 1 & 0 \\ -\sin(\theta_y) & 0 & \cos(\theta_y) \end{bmatrix} \\
|
|
\bm{R}_z(\theta_z) &= \begin{bmatrix} \cos(\theta_z) & -\sin(\theta_z) & 0 \\ \sin(\theta_z) & \cos(\theta_x) & 0 \\ 0 & 0 & 1 \end{bmatrix}
|
|
\end{align}
|
|
\end{subequations}
|
|
|
|
Sometimes, it is useful to express a rotation as a combination of three rotations described by $\bm{R}_x$, $\bm{R}_y$ and $\bm{R}_z$.
|
|
The order of rotation is very important[fn:5], therefore, in this study, rotations are expressed as three successive rotations about the coordinate axes of the moving frame eqref:eq:ustation_rotation_combination.
|
|
|
|
\begin{equation}\label{eq:ustation_rotation_combination}
|
|
{}^A\bm{R}_B(\alpha, \beta, \gamma) = \bm{R}_u(\alpha) \bm{R}_v(\beta) \bm{R}_c(\gamma)
|
|
\end{equation}
|
|
|
|
Such rotation can be parameterized by three Euler angles $(\alpha,\ \beta,\ \gamma)$, which can be computed from a given rotation matrix using equations eqref:eq:ustation_euler_angles.
|
|
|
|
\begin{subequations}\label{eq:ustation_euler_angles}
|
|
\begin{align}
|
|
\alpha &= \text{atan2}(-R_{23}/\cos(\beta),\ R_{33}/\cos(\beta)) \\
|
|
\beta &= \text{atan2}( R_{13},\ \sqrt{R_{11}^2 + R_{12}^2}) \\
|
|
\gamma &= \text{atan2}(-R_{12}/\cos(\beta),\ R_{11}/\cos(\beta))
|
|
\end{align}
|
|
\end{subequations}
|
|
|
|
**** Motion of a Rigid Body
|
|
|
|
Since the relative positions of a rigid body with respect to a moving frame $\{B\}$ attached to it are fixed for all time, it is sufficient to know the position of the origin of the frame $O_B$ and the orientation of the frame $\{B\}$ with respect to the fixed frame $\{A\}$, to represent the position of any point $P$ in the space.
|
|
|
|
Therefore, the pose of a rigid body can be fully determined by:
|
|
1. The position vector of point $O_B$ with respect to frame $\{A\}$ which is denoted ${}^AP_{O_B}$
|
|
2. The orientation of the rigid body, or the moving frame $\{B\}$ attached to it with respect to the fixed frame $\{A\}$, that is represented by ${}^A\bm{R}_B$.
|
|
|
|
The position of any point $P$ of the rigid body with respect to the fixed frame $\{\bm{A}\}$, which is denoted ${}^A\bm{P}$ may be determined thanks to the /Chasles' theorem/, which states that if the pose of a rigid body $\{{}^A\bm{R}_B, {}^AP_{O_B}\}$ is given, then the position of any point $P$ of this rigid body with respect to $\{\bm{A}\}$ is given by Equation eqref:eq:ustation_chasles_therorem.
|
|
|
|
\begin{equation} \label{eq:ustation_chasles_therorem}
|
|
{}^AP = {}^A\bm{R}_B {}^BP + {}^AP_{O_B}
|
|
\end{equation}
|
|
|
|
While equation eqref:eq:ustation_chasles_therorem can describe the motion of a rigid body, it can be written in a more convenient way using $4 \times 4$ homogeneous transformation matrices and $4 \times 1$ homogeneous coordinates.
|
|
The homogeneous transformation matrix is composed of the rotation matrix ${}^A\bm{R}_B$ representing the orientation and the position vector ${}^AP_{O_B}$ representing the translation.
|
|
It is partitioned as shown in Equation eqref:eq:ustation_homogeneous_transformation_parts.
|
|
|
|
\begin{equation}\label{eq:ustation_homogeneous_transformation_parts}
|
|
{}^A\bm{T}_B =
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & \\
|
|
& {}^A\bm{R}_B & & {}^AP_{O_B} \\
|
|
& & & \cr
|
|
\hline
|
|
0 & 0 & 0 & 1
|
|
\end{array} \right]
|
|
\end{equation}
|
|
|
|
Then, ${}^AP$ can be computed from ${}^BP$ and the homogeneous transformation matrix using eqref:eq:ustation_homogeneous_transformation.
|
|
|
|
\begin{equation}\label{eq:ustation_homogeneous_transformation}
|
|
\left[ \begin{array}{c} \\ {}^AP \\ \cr \hline 1 \end{array} \right]
|
|
=
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & \\
|
|
& {}^A\bm{R}_B & & {}^AP_{O_B} \\
|
|
& & & \cr
|
|
\hline
|
|
0 & 0 & 0 & 1
|
|
\end{array} \right]
|
|
\left[ \begin{array}{c} \\ {}^BP \\ \cr \hline 1 \end{array} \right] \quad \Rightarrow \quad {}^AP = {}^A\bm{R}_B {}^BP + {}^AP_{O_B}
|
|
\end{equation}
|
|
|
|
One key advantage of homogeneous transformation is that it can easily be generalized for consecutive transformations.
|
|
Let us consider the motion of a rigid body described at three locations (Figure ref:fig:ustation_combined_transformation).
|
|
Frame $\{A\}$ represents the initial location, frame $\{B\}$ is an intermediate location, and frame $\{C\}$ represents the rigid body at its final location.
|
|
|
|
#+name: fig:ustation_combined_transformation
|
|
#+caption: Motion of a rigid body represented at three locations by frame $\{A\}$, $\{B\}$ and $\{C\}$
|
|
[[file:figs/ustation_combined_transformation.png]]
|
|
|
|
Furthermore, suppose the position vector of a point $P$ of the rigid body is given in the final location, that is ${}^CP$ is given, and the position of this point is to be found in the fixed frame $\{A\}$, that is ${}^AP$.
|
|
Since the locations of the rigid body are known relative to each other, ${}^CP$ can be transformed to ${}^BP$ using ${}^B\bm{T}_C$ using ${}^BP = {}^B\bm{T}_C {}^CP$.
|
|
Similarly, ${}^BP$ can be transformed into ${}^AP$ using ${}^AP = {}^A\bm{T}_B {}^BP$.
|
|
|
|
Combining the two relations, Equation eqref:eq:ustation_consecutive_transformations is obtained.
|
|
This shows that combining multiple transformations is equivalent as to compute $4 \times 4$ matrix multiplications.
|
|
|
|
\begin{equation}\label{eq:ustation_consecutive_transformations}
|
|
{}^AP = \underbrace{{}^A\bm{T}_B {}^B\bm{T}_C}_{{}^A\bm{T}_C} {}^CP
|
|
\end{equation}
|
|
|
|
Another key advantage of homogeneous transformation is the easy inverse transformation, which can be computed using Equation eqref:eq:ustation_inverse_homogeneous_transformation.
|
|
|
|
\begin{equation}\label{eq:ustation_inverse_homogeneous_transformation}
|
|
{}^B\bm{T}_A = {}^A\bm{T}_B^{-1} =
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & \\
|
|
& {}^A\bm{R}_B^T & & -{}^A \bm{R}_B^T {}^AP_{O_B} \\
|
|
& & & \cr
|
|
\hline
|
|
0 & 0 & 0 & 1 \\
|
|
\end{array} \right]
|
|
\end{equation}
|
|
|
|
** Micro-Station Kinematics
|
|
<<ssec:ustation_kinematics>>
|
|
|
|
Each stage is described by two frames; one is attached to the fixed platform $\{A\}$ while the other is fixed to the mobile platform $\{B\}$.
|
|
At "rest" position, the two have the same pose and coincide with the point of interest ($O_A = O_B$).
|
|
An example of the tilt stage is shown in Figure ref:fig:ustation_stage_motion.
|
|
The mobile frame of the translation stage is equal to the fixed frame of the tilt stage: $\{B_{D_y}\} = \{A_{R_y}\}$.
|
|
Similarly, the mobile frame of the tilt stage is equal to the fixed frame of the spindle: $\{B_{R_y}\} = \{A_{R_z}\}$.
|
|
|
|
#+name: fig:ustation_stage_motion
|
|
#+caption: Example of the motion induced by the tilt-stage $R_y$. "Rest" position in shown in blue while a arbitrary position in shown in red. Parasitic motions are here magnified for clarity.
|
|
[[file:figs/ustation_stage_motion.png]]
|
|
|
|
The motion induced by a positioning stage can be described by a homogeneous transformation matrix from frame $\{A\}$ to frame $\{B\}$ as explain in Section ref:ssec:ustation_kinematics.
|
|
As any motion stage induces parasitic motion in all 6 DoF, the transformation matrix representing its induced motion can be written as in eqref:eq:ustation_translation_stage_errors.
|
|
|
|
\begin{equation}\label{eq:ustation_translation_stage_errors}
|
|
{}^A\bm{T}_B(D_x, D_y, D_z, \theta_x, \theta_y, \theta_z) =
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & D_x \\
|
|
& \bm{R}_x(\theta_x) \bm{R}_y(\theta_y) \bm{R}_z(\theta_z) & & D_y \\
|
|
& & & D_z \cr
|
|
\hline
|
|
0 & 0 & 0 & 1
|
|
\end{array} \right]
|
|
\end{equation}
|
|
|
|
The homogeneous transformation matrix corresponding to the micro-station $\bm{T}_{\mu\text{-station}}$ is simply equal to the matrix multiplication of the homogeneous transformation matrices of the individual stages as shown in Equation eqref:eq:ustation_transformation_station.
|
|
|
|
\begin{equation}\label{eq:ustation_transformation_station}
|
|
\bm{T}_{\mu\text{-station}} = \bm{T}_{D_y} \cdot \bm{T}_{R_y} \cdot \bm{T}_{R_z} \cdot \bm{T}_{\mu\text{-hexapod}}
|
|
\end{equation}
|
|
|
|
$\bm{T}_{\mu\text{-station}}$ represents the pose of the sample (supposed to be rigidly fixed on top of the positioning-hexapod) with respect to the granite.
|
|
|
|
If the transformation matrices of the individual stages are each representing a perfect motion (i.e. the stages are supposed to have no parasitic motion), $\bm{T}_{\mu\text{-station}}$ then represent the pose setpoint of the sample with respect to the granite.
|
|
The transformation matrices for the translation stage, tilt stage, spindle, and positioning hexapod can be written as shown in Equation eqref:eq:ustation_transformation_matrices_stages.
|
|
|
|
\begin{equation}\label{eq:ustation_transformation_matrices_stages}
|
|
\begin{align}
|
|
\bm{T}_{D_y} &= \begin{bmatrix}
|
|
1 & 0 & 0 & 0 \\
|
|
0 & 1 & 0 & D_y \\
|
|
0 & 0 & 1 & 0 \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix} \quad
|
|
\bm{T}_{\mu\text{-hexapod}} =
|
|
\left[ \begin{array}{ccc|c}
|
|
& & & D_{\mu x} \\
|
|
& \bm{R}_x(\theta_{\mu x}) \bm{R}_y(\theta_{\mu y}) \bm{R}_{z}(\theta_{\mu z}) & & D_{\mu y} \\
|
|
& & & D_{\mu z} \cr
|
|
\hline
|
|
0 & 0 & 0 & 1
|
|
\end{array} \right] \\
|
|
\bm{T}_{R_z} &= \begin{bmatrix}
|
|
\cos(\theta_z) & -\sin(\theta_z) & 0 & 0 \\
|
|
\sin(\theta_z) & \cos(\theta_z) & 0 & 0 \\
|
|
0 & 0 & 1 & 0 \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix} \quad
|
|
\bm{T}_{R_y} = \begin{bmatrix}
|
|
\cos(\theta_y) & 0 & \sin(\theta_y) & 0 \\
|
|
0 & 1 & 0 & 0 \\
|
|
-\sin(\theta_y) & 0 & \cos(\theta_y) & 0 \\
|
|
0 & 0 & 0 & 1
|
|
\end{bmatrix}
|
|
\end{align}
|
|
\end{equation}
|
|
|
|
#+begin_src matlab
|
|
%% Stage setpoints
|
|
Dy = 1e-3; % Translation Stage [m]
|
|
Ry = 3*pi/180; % Tilt Stage [rad]
|
|
Rz = 180*pi/180; % Spindle [rad]
|
|
|
|
%% Stage individual Homogeneous transformations
|
|
% Translation Stage
|
|
Rty = [1 0 0 0;
|
|
0 1 0 Dy;
|
|
0 0 1 0;
|
|
0 0 0 1];
|
|
|
|
% Tilt Stage - Pure rotating aligned with Ob
|
|
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
|
0 1 0 0;
|
|
-sin(Ry) 0 cos(Ry) 0;
|
|
0 0 0 1];
|
|
|
|
% Spindle - Rotation along the Z axis
|
|
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
|
sin(Rz) cos(Rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
% Micro-Station homogeneous transformation
|
|
Ttot = Rty*Rry*Rrz;
|
|
|
|
%% Compute translations and rotations (Euler angles) induced by the micro-station
|
|
ustation_dx = Ttot(1,4);
|
|
ustation_dy = Ttot(2,4);
|
|
ustation_dz = Ttot(3,4);
|
|
ustation_ry = atan2( Ttot(1, 3), sqrt(Ttot(1, 1)^2 + Ttot(1, 2)^2));
|
|
ustation_rx = atan2(-Ttot(2, 3)/cos(ustation_ry), Ttot(3, 3)/cos(ustation_ry));
|
|
ustation_rz = atan2(-Ttot(1, 2)/cos(ustation_ry), Ttot(1, 1)/cos(ustation_ry));
|
|
|
|
%% Verification using the Simscape model
|
|
% All stages are initialized as rigid bodies to avoid any guiding error
|
|
initializeGround( 'type', 'rigid');
|
|
initializeGranite( 'type', 'rigid');
|
|
initializeTy( 'type', 'rigid');
|
|
initializeRy( 'type', 'rigid');
|
|
initializeRz( 'type', 'rigid');
|
|
initializeMicroHexapod('type', 'rigid');
|
|
|
|
initializeLoggingConfiguration('log', 'all');
|
|
|
|
initializeReferences('Dy_amplitude', Dy, ...
|
|
'Ry_amplitude', Ry, ...
|
|
'Rz_amplitude', Rz);
|
|
|
|
initializeDisturbances('enable', false);
|
|
|
|
set_param(conf_simulink, 'StopTime', '0.5');
|
|
|
|
% Simulation is performed
|
|
sim(mdl);
|
|
|
|
% Sample's motion is computed from "external metrology"
|
|
T_sim = [simout.y.R.Data(:,:,end), [simout.y.x.Data(end); simout.y.y.Data(end); simout.y.z.Data(end)]; [0,0,0,1]];
|
|
sim_dx = T_sim(1,4);
|
|
sim_dy = T_sim(2,4);
|
|
sim_dz = T_sim(3,4);
|
|
sim_ry = atan2( T_sim(1, 3), sqrt(T_sim(1, 1)^2 + T_sim(1, 2)^2));
|
|
sim_rx = atan2(-T_sim(2, 3)/cos(sim_ry), T_sim(3, 3)/cos(sim_ry));
|
|
sim_rz = atan2(-T_sim(1, 2)/cos(sim_ry), T_sim(1, 1)/cos(sim_ry));
|
|
#+end_src
|
|
|
|
* Micro-Station Dynamics
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/ustation_2_modeling.m
|
|
:END:
|
|
<<sec:ustation_modeling>>
|
|
** Introduction :ignore:
|
|
|
|
In this section, the multi-body model of the micro-station is presented.
|
|
Such model consists of several rigid bodies connected by springs and dampers.
|
|
The inertia of the solid bodies and the stiffness properties of the guiding mechanisms were first estimated based on the CAD model and data-sheets (Section ref:ssec:ustation_model_simscape).
|
|
|
|
The obtained dynamics is then compared with the modal analysis performed on the micro-station (Section ref:ssec:ustation_model_comp_dynamics).
|
|
|
|
# TODO - Add reference to uniaxial model
|
|
As the dynamics of the nano-hexapod is impacted by the micro-station compliance, the most important dynamical characteristic that should be well modeled is the overall compliance of the micro-station.
|
|
To do so, the 6-DoF compliance of the micro-station is measured and then compared with the 6-DoF compliance extracted from the multi-body model (Section ref:ssec:ustation_model_compliance).
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% ustation_2_modeling.m
|
|
#+end_src
|
|
|
|
#+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 :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Multi-Body Model
|
|
<<ssec:ustation_model_simscape>>
|
|
|
|
By performing a modal analysis of the micro-station, it was verified that in the frequency range of interest, each stage behaved as a rigid body.
|
|
This confirms that a multi-body model can be used to properly model the micro-station.
|
|
|
|
A multi-body model consists of several solid bodies connected by joints.
|
|
Each solid body can be represented by its inertia properties (most of the time computed automatically from the 3D model and material density).
|
|
Joints are used to impose kinematic constraints between solid bodies and to specify dynamical properties (i.e. spring stiffness and damping coefficient).
|
|
External forces can be used to model disturbances, and "sensors" can be used to measure the relative pose between two defined frames.
|
|
|
|
#+name: fig:ustation_simscape_stage_example
|
|
#+caption: Example of a stage (here the tilt-stage) represented in the multi-body model software (Simscape). It is composed of two solid bodies connected by a 6-DoF joint. One joint DoF (here the tilt angle) can be imposed, the other DoFs are represented by springs and dampers. Additional disturbances forces for all DoF can be included
|
|
[[file:figs/ustation_simscape_stage_example.png]]
|
|
|
|
Therefore, the micro-station is modeled by several solid bodies connected by joints.
|
|
A typical stage (here the tilt-stage) is modeled as shown in Figure ref:fig:ustation_simscape_stage_example where two solid bodies (the fixed part and the mobile part) are connected by a 6-DoF joint.
|
|
One DoF of the 6-DoF joint is "imposed" by a setpoint (i.e. modeled as infinitely stiff), while the other 5 are each modeled by a spring and damper.
|
|
Additional forces can be used to model disturbances induced by the stage motion.
|
|
The obtained 3D representation of the multi-body model is shown in Figure ref:fig:ustation_simscape_model.
|
|
|
|
#+name: fig:ustation_simscape_model
|
|
#+caption: 3D view of the micro-station multi-body model
|
|
#+attr_latex: :width 0.8\linewidth
|
|
[[file:figs/ustation_simscape_model.jpg]]
|
|
|
|
The ground is modeled by a solid body connected to the "world frame" through a joint only allowing 3 translations.
|
|
The granite was then connected to the ground using a 6-DoF joint.
|
|
The translation stage is connected to the granite by a 6-DoF joint, but the $D_y$ motion is imposed.
|
|
Similarly, the tilt-stage and the spindle are connected to the stage below using a 6-DoF joint, with 1 imposed DoF each time.
|
|
Finally, the positioning hexapod has 6-DoF.
|
|
|
|
The total number of "free" degrees of freedom is 27, so the model has 54 states.
|
|
The springs and dampers values were first estimated from the joint/stage specifications and were later fined-tuned based on the measurements.
|
|
The spring values are summarized in Table ref:tab:ustation_6dof_stiffness_values.
|
|
|
|
#+name: tab:ustation_6dof_stiffness_values
|
|
#+caption: Summary of the stage stiffnesses. The contrained degrees-of-freedom are indicated by "-". The frames in which the 6-DoF joints are defined are indicated in figures found in Section ref:ssec:ustation_stages
|
|
#+attr_latex: :environment tabularx :width \linewidth :align Xcccccc
|
|
#+attr_latex: :center t :booktabs t
|
|
| *Stage* | $D_x$ | $D_y$ | $D_z$ | $R_x$ | $R_y$ | $R_z$ |
|
|
|-------------+-----------------+-----------------+-----------------+-------------------------+------------------------+-------------------------|
|
|
| Granite | $5\,kN/\mu m$ | $5\,kN/\mu m$ | $5\,kN/\mu m$ | $25\,Nm/\mu\text{rad}$ | $25\,Nm/\mu\text{rad}$ | $10\,Nm/\mu\text{rad}$ |
|
|
| Translation | $200\,N/\mu m$ | - | $200\,N/\mu m$ | $60\,Nm/\mu\text{rad}$ | $90\,Nm/\mu\text{rad}$ | $60\,Nm/\mu\text{rad}$ |
|
|
| Tilt | $380\,N/\mu m$ | $400\,N/\mu m$ | $380\,N/\mu m$ | $120\,Nm/\mu\text{rad}$ | - | $120\,Nm/\mu\text{rad}$ |
|
|
| Spindle | $700\,N/\mu m$ | $700\,N/\mu m$ | $2\,kN/\mu m$ | $10\,Nm/\mu\text{rad}$ | $10\,Nm/\mu\text{rad}$ | - |
|
|
| Hexapod | $10\,N/\mu m$ | $10\,N/\mu m$ | $100\,N/\mu m$ | $1.5\,Nm/rad$ | $1.5\,Nm/rad$ | $0.27\,Nm/rad$ |
|
|
|
|
** Comparison with the measured dynamics
|
|
<<ssec:ustation_model_comp_dynamics>>
|
|
|
|
The dynamics of the micro-station was measured by placing accelerometers on each stage and by impacting the translation stage with an instrumented hammer in three directions.
|
|
The obtained FRFs were then projected at the CoM of each stage.
|
|
|
|
To gain a first insight into the accuracy of the obtained model, the FRFs from the hammer impacts to the acceleration of each stage were extracted from the multi-body model and compared with the measurements in Figure ref:fig:ustation_comp_com_response.
|
|
|
|
Even though there is some similarity between the model and the measurements (similar overall shapes and amplitudes), it is clear that the multi-body model does not accurately represent the complex micro-station dynamics.
|
|
Tuning the numerous model parameters to better match the measurements is a highly non-linear optimization problem that is difficult to solve in practice.
|
|
|
|
#+begin_src matlab
|
|
%% Indentify the model dynamics from the 3 hammer imapcts
|
|
% To the motion of each solid body at their CoM
|
|
|
|
% All stages are initialized
|
|
initializeGround( 'type', 'rigid');
|
|
initializeGranite( 'type', 'flexible');
|
|
initializeTy( 'type', 'flexible');
|
|
initializeRy( 'type', 'flexible');
|
|
initializeRz( 'type', 'flexible');
|
|
initializeMicroHexapod('type', 'flexible');
|
|
|
|
initializeLoggingConfiguration('log', 'none');
|
|
|
|
initializeReferences();
|
|
initializeDisturbances('enable', false);
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/F_hammer'], 1, 'openinput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Granite/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Translation Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Tilt Stage/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Spindle/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/accelerometer'], 1, 'openoutput'); io_i = io_i + 1;
|
|
|
|
% Run the linearization
|
|
G_ms = linearize(mdl, io, 0);
|
|
G_ms.InputName = {'Fx', 'Fy', 'Fz'};
|
|
G_ms.OutputName = {'gtop_x', 'gtop_y', 'gtop_z', 'gtop_rx', 'gtop_ry', 'gtop_rz', ...
|
|
'ty_x', 'ty_y', 'ty_z', 'ty_rx', 'ty_ry', 'ty_rz', ...
|
|
'ry_x', 'ry_y', 'ry_z', 'ry_rx', 'ry_ry', 'ry_rz', ...
|
|
'rz_x', 'rz_y', 'rz_z', 'rz_rx', 'rz_ry', 'rz_rz', ...
|
|
'hexa_x', 'hexa_y', 'hexa_z', 'hexa_rx', 'hexa_ry', 'hexa_rz'};
|
|
|
|
% Load estimated FRF at CoM
|
|
load('ustation_frf_matrix.mat', 'freqs');
|
|
load('ustation_frf_com.mat', 'frfs_CoM');
|
|
|
|
% Initialization of some variables to the figures
|
|
dirs = {'x', 'y', 'z', 'rx', 'ry', 'rz'};
|
|
stages = {'gbot', 'gtop', 'ty', 'ry', 'rz', 'hexa'};
|
|
f = logspace(log10(10), log10(500), 1000);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Spindle - X response
|
|
n_stg = 5; % Measured Stage
|
|
n_dir = 1; % Measured DoF: x, y, z, Rx, Ry, Rz
|
|
n_exc = 1; % Hammer impact: x, y, z
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
|
text(50, 2e-2,{'$D_x/F_x$ - Spindle'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([10, 200]);
|
|
ylim([1e-6, 1e-1])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_comp_com_response_rz_x.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Hexapod - Y response
|
|
n_stg = 6; % Measured Stage
|
|
n_dir = 2; % Measured DoF: x, y, z, Rx, Ry, Rz
|
|
n_exc = 2; % Hammer impact: x, y, z
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
|
text(50, 2e-2,{'$D_y/F_y$ - Hexapod'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([10, 200]);
|
|
ylim([1e-6, 1e-1])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_comp_com_response_hexa_y.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Tilt stage - Z response
|
|
n_stg = 4; % Measured Stage
|
|
n_dir = 3; % Measured DoF: x, y, z, Rx, Ry, Rz
|
|
n_exc = 3; % Hammer impact: x, y, z
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(frfs_CoM(6*(n_stg-1) + n_dir, n_exc, :))), 'DisplayName', 'Measurement');
|
|
plot(f, abs(squeeze(freqresp(G_ms([stages{n_stg}, '_', dirs{n_dir}], ['F', dirs{n_exc}]), f, 'Hz'))), 'DisplayName', 'Model');
|
|
text(50, 2e-2,{'$D_z/F_z$ - Tilt'},'VerticalAlignment','bottom','HorizontalAlignment','center')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [$m/s^2/N$]');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([10, 200]);
|
|
ylim([1e-6, 1e-1])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_comp_com_response_ry_z.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_comp_com_response
|
|
#+caption: FRFs between the hammer impacts on the translation stage and the measured stage acceleration expressed at its CoM. Comparison of the measured and extracted FRFs from the multi-body model. Different directions are computed for different stages.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_comp_com_response_rz_x}Spindle, $x$ response}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_comp_com_response_rz_x.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_comp_com_response_hexa_y}Hexapod, $y$ response}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_comp_com_response_hexa_y.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_comp_com_response_ry_z}Tilt, $z$ response}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_comp_com_response_ry_z.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Micro-station compliance
|
|
<<ssec:ustation_model_compliance>>
|
|
|
|
As discussed in the previous section, the dynamics of the micro-station is complex, and tuning the multi-body model parameters to obtain a perfect match is difficult.
|
|
|
|
When considering the NASS, the most important dynamical characteristics of the micro-station is its compliance, as it can affect the plant dynamics.
|
|
Therefore, the adopted strategy is to accurately model the micro-station compliance.
|
|
|
|
The micro-station compliance was experimentally measured using the setup illustrated in Figure ref:fig:ustation_compliance_meas.
|
|
Four 3-axis accelerometers were fixed to the micro-hexapod top platform.
|
|
The micro-hexapod top platform was impacted at 10 different points.
|
|
For each impact position, 10 impacts were performed to average and improve the data quality.
|
|
|
|
#+name: fig:ustation_compliance_meas
|
|
#+caption: Schematic of the measurement setup used to estimate the compliance of the micro-station. The top platform of the positioning hexapod is shown with four 3-axis accelerometers (shown in red) are on top. 10 hammer impacts are performed at different locations (shown in blue).
|
|
[[file:figs/ustation_compliance_meas.png]]
|
|
|
|
To convert the 12 acceleration signals $a_{\mathcal{L}} = [a_{1x}\ a_{1y}\ a_{1z}\ a_{2x}\ \dots\ a_{4z}]$ to the acceleration expressed in the frame $\{\mathcal{X}\}$ $a_{\mathcal{X}} = [a_{dx}\ a_{dy}\ a_{dz}\ a_{rx}\ a_{ry}\ a_{rz}]$, a Jacobian matrix $\bm{J}_a$ is written based on the positions and orientations of the accelerometers eqref:eq:ustation_compliance_acc_jacobian.
|
|
|
|
\begin{equation}\label{eq:ustation_compliance_acc_jacobian}
|
|
\bm{J}_a = \begin{bmatrix}
|
|
1 & 0 & 0 & 0 & 0 &-d \\
|
|
0 & 1 & 0 & 0 & 0 & 0 \\
|
|
0 & 0 & 1 & d & 0 & 0 \\
|
|
1 & 0 & 0 & 0 & 0 & 0 \\
|
|
0 & 1 & 0 & 0 & 0 &-d \\
|
|
0 & 0 & 1 & 0 & d & 0 \\
|
|
1 & 0 & 0 & 0 & 0 & d \\
|
|
0 & 1 & 0 & 0 & 0 & 0 \\
|
|
0 & 0 & 1 &-d & 0 & 0 \\
|
|
1 & 0 & 0 & 0 & 0 & 0 \\
|
|
0 & 1 & 0 & 0 & 0 & d \\
|
|
0 & 0 & 1 & 0 &-d & 0
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
Then, the acceleration in the cartesian frame can be computed using eqref:eq:ustation_compute_cart_acc.
|
|
|
|
\begin{equation}\label{eq:ustation_compute_cart_acc}
|
|
a_{\mathcal{X}} = \bm{J}_a^\dagger \cdot a_{\mathcal{L}}
|
|
\end{equation}
|
|
|
|
Similar to what is done for the accelerometers, a Jacobian matrix $\bm{J}_F$ is computed eqref:eq:ustation_compliance_force_jacobian and used to convert the individual hammer forces $F_{\mathcal{L}}$ to force and torques $F_{\mathcal{X}}$ applied at the center of the micro-hexapod top plate (defined by frame $\{\mathcal{X}\}$ in Figure ref:fig:ustation_compliance_meas).
|
|
|
|
\begin{equation}\label{eq:ustation_compliance_force_jacobian}
|
|
\bm{J}_F = \begin{bmatrix}
|
|
0 & -1 & 0 & 0 & 0 & 0\\
|
|
0 & 0 & -1 & -d & 0 & 0\\
|
|
1 & 0 & 0 & 0 & 0 & 0\\
|
|
0 & 0 & -1 & 0 & -d & 0\\
|
|
0 & 1 & 0 & 0 & 0 & 0\\
|
|
0 & 0 & -1 & d & 0 & 0\\
|
|
-1 & 0 & 0 & 0 & 0 & 0\\
|
|
0 & 0 & -1 & 0 & d & 0\\
|
|
-1 & 0 & 0 & 0 & 0 & -d\\
|
|
-1 & 0 & 0 & 0 & 0 & d
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
The equivalent forces and torques applied at center of $\{\mathcal{X}\}$ are then computed using eqref:eq:ustation_compute_cart_force.
|
|
|
|
\begin{equation}\label{eq:ustation_compute_cart_force}
|
|
F_{\mathcal{X}} = \bm{J}_F^t \cdot F_{\mathcal{L}}
|
|
\end{equation}
|
|
|
|
Using the two Jacobian matrices, the FRF from the 10 hammer impacts to the 12 accelerometer outputs can be converted to the FRF from 6 forces/torques applied at the origin of frame $\{\mathcal{X}\}$ to the 6 linear/angular accelerations of the top platform expressed with respect to $\{\mathcal{X}\}$.
|
|
These FRFs were then used for comparison with the multi-body model.
|
|
|
|
#+begin_src matlab
|
|
% Positions and orientation of accelerometers
|
|
% | *Num* | *Position* | *Orientation* | *Sensibility* | *Channels* |
|
|
% |-------+------------+---------------+---------------+------------|
|
|
% | 1 | [0, +A, 0] | [x, y, z] | 1V/g | 1-3 |
|
|
% | 2 | [-B, 0, 0] | [x, y, z] | 1V/g | 4-6 |
|
|
% | 3 | [0, -A, 0] | [x, y, z] | 0.1V/g | 7-9 |
|
|
% | 4 | [+B, 0, 0] | [x, y, z] | 1V/g | 10-12 |
|
|
%
|
|
|
|
% Measuerment channels
|
|
%
|
|
% | Acc Number | Dir | Channel Number |
|
|
% |------------+-----+----------------|
|
|
% | 1 | x | 1 |
|
|
% | 1 | y | 2 |
|
|
% | 1 | z | 3 |
|
|
% | 2 | x | 4 |
|
|
% | 2 | y | 5 |
|
|
% | 2 | z | 6 |
|
|
% | 3 | x | 7 |
|
|
% | 3 | y | 8 |
|
|
% | 3 | z | 9 |
|
|
% | 4 | x | 10 |
|
|
% | 4 | y | 11 |
|
|
% | 4 | z | 12 |
|
|
% | Hammer | | 13 |
|
|
|
|
% 10 measurements corresponding to 10 different Hammer impact locations
|
|
% | *Num* | *Direction* | *Position* | Accelerometer position | Jacobian number |
|
|
% |-------+-------------+------------+------------------------+-----------------|
|
|
% | 1 | -Y | [0, +A, 0] | 1 | -2 |
|
|
% | 2 | -Z | [0, +A, 0] | 1 | -3 |
|
|
% | 3 | X | [-B, 0, 0] | 2 | 4 |
|
|
% | 4 | -Z | [-B, 0, 0] | 2 | -6 |
|
|
% | 5 | Y | [0, -A, 0] | 3 | 8 |
|
|
% | 6 | -Z | [0, -A, 0] | 3 | -9 |
|
|
% | 7 | -X | [+B, 0, 0] | 4 | -10 |
|
|
% | 8 | -Z | [+B, 0, 0] | 4 | -12 |
|
|
% | 9 | -X | [0, -A, 0] | 3 | -7 |
|
|
% | 10 | -X | [0, +A, 0] | 1 | -1 |
|
|
|
|
|
|
%% Jacobian for accelerometers
|
|
% aX = Ja . aL
|
|
d = 0.14; % [m]
|
|
Ja = [1 0 0 0 0 -d;
|
|
0 1 0 0 0 0;
|
|
0 0 1 d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 1 0 0 0 -d;
|
|
0 0 1 0 d 0;
|
|
1 0 0 0 0 d;
|
|
0 1 0 0 0 0;
|
|
0 0 1 -d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 1 0 0 0 d;
|
|
0 0 1 0 -d 0];
|
|
|
|
Ja_inv = pinv(Ja);
|
|
|
|
%% Jacobian for hammer impacts
|
|
% F = Jf' tau
|
|
Jf = [0 -1 0 0 0 0;
|
|
0 0 -1 -d 0 0;
|
|
1 0 0 0 0 0;
|
|
0 0 -1 0 -d 0;
|
|
0 1 0 0 0 0;
|
|
0 0 -1 d 0 0;
|
|
-1 0 0 0 0 0;
|
|
0 0 -1 0 d 0;
|
|
-1 0 0 0 0 -d;
|
|
-1 0 0 0 0 d]';
|
|
Jf_inv = pinv(Jf);
|
|
|
|
%% Load raw measurement data
|
|
% "Track1" to "Track12" are the 12 accelerometers
|
|
% "Track13" is the instrumented hammer force sensor
|
|
data = [
|
|
load('ustation_compliance_hammer_1.mat'), ...
|
|
load('ustation_compliance_hammer_2.mat'), ...
|
|
load('ustation_compliance_hammer_3.mat'), ...
|
|
load('ustation_compliance_hammer_4.mat'), ...
|
|
load('ustation_compliance_hammer_5.mat'), ...
|
|
load('ustation_compliance_hammer_6.mat'), ...
|
|
load('ustation_compliance_hammer_7.mat'), ...
|
|
load('ustation_compliance_hammer_8.mat'), ...
|
|
load('ustation_compliance_hammer_9.mat'), ...
|
|
load('ustation_compliance_hammer_10.mat')];
|
|
|
|
%% Prepare the FRF computation
|
|
Ts = 1e-3; % Sampling Time [s]
|
|
Nfft = floor(1/Ts); % Number of points for the FFT computation
|
|
win = ones(Nfft, 1); % Rectangular window
|
|
[~, f] = tfestimate(data(1).Track13, data(1).Track1, win, [], Nfft, 1/Ts);
|
|
|
|
% Samples taken before and after the hammer "impact"
|
|
pre_n = floor(0.1/Ts);
|
|
post_n = Nfft - pre_n - 1;
|
|
|
|
%% Compute the FRFs for the 10 hammer impact locations.
|
|
% The FRF from hammer force the 12 accelerometers are computed
|
|
% for each of the 10 hammer impacts and averaged
|
|
G_raw = zeros(12,10,length(f));
|
|
|
|
for i = 1:10 % For each impact location
|
|
% Find the 10 impacts
|
|
[~, impacts_i] = find(diff(data(i).Track13 > 50)==1);
|
|
% Only keep the first 10 impacts if there are more than 10 impacts
|
|
if length(impacts_i)>10
|
|
impacts_i(11:end) = [];
|
|
end
|
|
|
|
% Average the FRF for the 10 impacts
|
|
for impact_i = impacts_i
|
|
i_start = impact_i - pre_n;
|
|
i_end = impact_i + post_n;
|
|
data_in = data(i).Track13(i_start:i_end); % [N]
|
|
% Remove hammer DC offset
|
|
data_in = data_in - mean(data_in(end-pre_n:end));
|
|
% Combine all outputs [m/s^2]
|
|
data_out = [data(i).Track1( i_start:i_end); ...
|
|
data(i).Track2( i_start:i_end); ...
|
|
data(i).Track3( i_start:i_end); ...
|
|
data(i).Track4( i_start:i_end); ...
|
|
data(i).Track5( i_start:i_end); ...
|
|
data(i).Track6( i_start:i_end); ...
|
|
data(i).Track7( i_start:i_end); ...
|
|
data(i).Track8( i_start:i_end); ...
|
|
data(i).Track9( i_start:i_end); ...
|
|
data(i).Track10(i_start:i_end); ...
|
|
data(i).Track11(i_start:i_end); ...
|
|
data(i).Track12(i_start:i_end)];
|
|
|
|
[frf, ~] = tfestimate(data_in, data_out', win, [], Nfft, 1/Ts);
|
|
G_raw(:,i,:) = squeeze(G_raw(:,i,:)) + 0.1*frf'./(-(2*pi*f').^2);
|
|
end
|
|
end
|
|
|
|
%% Compute transfer function in cartesian frame using Jacobian matrices
|
|
% FRF_cartesian = inv(Ja) * FRF * inv(Jf)
|
|
FRF_cartesian = pagemtimes(Ja_inv, pagemtimes(G_raw, Jf_inv));
|
|
#+end_src
|
|
|
|
The compliance of the micro-station multi-body model was extracted by computing the transfer function from forces/torques applied on the hexapod's top platform to the "absolute" motion of the top platform.
|
|
These results are compared with the measurements in Figure ref:fig:ustation_frf_compliance_model.
|
|
Considering the complexity of the micro-station compliance dynamics, the model compliance matches sufficiently well for the current application.
|
|
|
|
#+begin_src matlab
|
|
%% Identification of the compliance of the micro-station
|
|
|
|
% Initialize simulation with default parameters (flexible elements)
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
|
|
initializeReferences();
|
|
initializeSimscapeConfiguration();
|
|
initializeLoggingConfiguration();
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Fm'], 1, 'openinput'); io_i = io_i + 1; % Direct Forces/Torques applied on the micro-hexapod top platform
|
|
io(io_i) = linio([mdl, '/Micro-Station/Micro Hexapod/Flexible/Dm'], 1, 'output'); io_i = io_i + 1; % Absolute displacement of the top platform
|
|
|
|
% Run the linearization
|
|
Gm = linearize(mdl, io, 0);
|
|
Gm.InputName = {'Fmx', 'Fmy', 'Fmz', 'Mmx', 'Mmy', 'Mmz'};
|
|
Gm.OutputName = {'Dx', 'Dy', 'Dz', 'Drx', 'Dry', 'Drz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
|
|
figure;
|
|
hold on;
|
|
plot(f, abs(squeeze(FRF_cartesian(1,1,:))), '-', 'color', [colors(1,:), 0.5], 'DisplayName', '$D_x/F_x$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(2,2,:))), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$D_y/F_y$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(3,3,:))), '-', 'color', [colors(3,:), 0.5], 'DisplayName', '$D_z/F_z$ - Measured')
|
|
plot(f, abs(squeeze(freqresp(Gm(1,1), f, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$D_x/F_x$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(2,2), f, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$D_y/F_y$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(3,3), f, 'Hz'))), '--', 'color', colors(3,:), 'DisplayName', '$D_z/F_z$ - Model')
|
|
hold off;
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [m/N]');
|
|
xlim([20, 500]); ylim([2e-10, 1e-6]);
|
|
xticks([20, 50, 100, 200, 500])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_frf_compliance_xyz_model.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Extracted FRF of the compliance of the micro-station in the Cartesian frame from the Simscape model
|
|
figure;
|
|
hold on;
|
|
plot(f, abs(squeeze(FRF_cartesian(4,4,:))), '-', 'color', [colors(1,:), 0.5], 'DisplayName', '$R_x/M_x$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(5,5,:))), '-', 'color', [colors(2,:), 0.5], 'DisplayName', '$R_y/M_y$ - Measured')
|
|
plot(f, abs(squeeze(FRF_cartesian(6,6,:))), '-', 'color', [colors(3,:), 0.5], 'DisplayName', '$R_z/M_z$ - Measured')
|
|
plot(f, abs(squeeze(freqresp(Gm(4,4), f, 'Hz'))), '--', 'color', colors(1,:), 'DisplayName', '$R_x/M_x$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(5,5), f, 'Hz'))), '--', 'color', colors(2,:), 'DisplayName', '$R_y/M_y$ - Model')
|
|
plot(f, abs(squeeze(freqresp(Gm(6,6), f, 'Hz'))), '--', 'color', colors(3,:), 'DisplayName', '$R_z/M_z$ - Model')
|
|
hold off;
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 2);
|
|
leg.ItemTokenSize(1) = 15;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Magnitude [rad/Nm]');
|
|
xlim([20, 500]); ylim([2e-7, 1e-4]);
|
|
xticks([20, 50, 100, 200, 500])
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_frf_compliance_Rxyz_model.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_frf_compliance_model
|
|
#+caption: Compliance of the micro-station expressed in frame $\{\mathcal{X}\}$. The measured FRFs are display by solid lines, while the FRFs extracted from the multi-body models are shown by dashed lines. Both translation terms (\subref{fig:ustation_frf_compliance_xyz_model}) and rotational terms (\subref{fig:ustation_frf_compliance_Rxyz_model}) are displayed.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_frf_compliance_xyz_model}Compliance in translation}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/ustation_frf_compliance_xyz_model.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_frf_compliance_Rxyz_model}Compliance in rotation}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/ustation_frf_compliance_Rxyz_model.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
* Estimation of Disturbances
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/ustation_3_disturbances.m
|
|
:END:
|
|
<<sec:ustation_disturbances>>
|
|
** Introduction :ignore:
|
|
|
|
The goal of this section is to obtain a realistic representation of disturbances affecting the micro-station.
|
|
These disturbance sources are then used during time domain simulations to accurately model the micro-station behavior.
|
|
The focus on stochastic disturbances because, in principle, it is possible to calibrate the repeatable part of disturbances.
|
|
Such disturbances include ground motions and vibrations induces by scanning the translation stage and the spindle.
|
|
|
|
In the multi-body model, stage vibrations are modeled as internal forces applied in the stage joint.
|
|
In practice, disturbance forces cannot be directly measured.
|
|
Instead, the vibrations of the micro-station's top platform induced by the disturbances were measured (Section ref:ssec:ustation_disturbances_meas).
|
|
|
|
To estimate the equivalent disturbance force that induces such vibration, the transfer functions from disturbance sources (i.e. forces applied in the stages' joint) to the displacements of the micro-station's top platform with respect to the granite are extracted from the multi-body model (Section ref:ssec:ustation_disturbances_sensitivity).
|
|
Finally, the obtained disturbance sources are compared in Section ref:ssec:ustation_disturbances_results.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% ustation_3_disturbances.m
|
|
#+end_src
|
|
|
|
#+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 :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Disturbance measurements
|
|
<<ssec:ustation_disturbances_meas>>
|
|
**** Introduction :ignore:
|
|
In this section, ground motion is directly measured using geophones.
|
|
Vibrations induced by scanning the translation stage and the spindle are also measured using dedicated setups.
|
|
|
|
The tilt stage and the micro-hexapod also have positioning errors; however, they are not modeled here because these two stages are only used for pre-positioning and not for scanning.
|
|
Therefore, from a control perspective, they are not important.
|
|
|
|
**** Ground Motion
|
|
|
|
The ground motion was measured by using a sensitive 3-axis geophone[fn:11] placed on the ground.
|
|
The generated voltages were recorded with a high resolution DAC, and converted to displacement using the Geophone sensitivity transfer function.
|
|
The obtained ground motion displacement is shown in Figure ref:fig:ustation_ground_disturbance.
|
|
|
|
#+begin_src matlab
|
|
%% Compute Floor Motion Spectral Density
|
|
% Load floor motion data
|
|
% velocity in [m/s] is measured in X, Y and Z directions
|
|
load('ustation_ground_motion.mat', 'Ts', 'Fs', 'vel_x', 'vel_y', 'vel_z', 't');
|
|
|
|
% Estimate ground displacement from measured velocity
|
|
% This is done by integrating the motion
|
|
gm_x = lsim(1/(s+0.1*2*pi), vel_x, t);
|
|
gm_y = lsim(1/(s+0.1*2*pi), vel_y, t);
|
|
gm_z = lsim(1/(s+0.1*2*pi), vel_z, t);
|
|
|
|
Nfft = floor(2/Ts);
|
|
win = hanning(Nfft);
|
|
Noverlap = floor(Nfft/2);
|
|
|
|
[pxx_gm_vx, f_gm] = pwelch(vel_x, win, Noverlap, Nfft, 1/Ts);
|
|
[pxx_gm_vy, ~] = pwelch(vel_y, win, Noverlap, Nfft, 1/Ts);
|
|
[pxx_gm_vz, ~] = pwelch(vel_z, win, Noverlap, Nfft, 1/Ts);
|
|
|
|
% Convert PSD in velocity to PSD in displacement
|
|
pxx_gm_x = pxx_gm_vx./((2*pi*f_gm).^2);
|
|
pxx_gm_y = pxx_gm_vy./((2*pi*f_gm).^2);
|
|
pxx_gm_z = pxx_gm_vz./((2*pi*f_gm).^2);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measured ground motion
|
|
figure;
|
|
hold on;
|
|
plot(t, 1e6*gm_x, 'DisplayName', '$D_{xf}$')
|
|
plot(t, 1e6*gm_y, 'DisplayName', '$D_{yf}$')
|
|
plot(t, 1e6*gm_z, 'DisplayName', '$D_{zf}$')
|
|
hold off;
|
|
xlabel('Time [s]');
|
|
ylabel('Ground motion [$\mu$m]')
|
|
xlim([0, 5]); ylim([-2, 2])
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_ground_disturbance.pdf', 'width', 'half', 'height', 450);
|
|
#+end_src
|
|
|
|
#+attr_latex: :options [b]{0.54\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:ustation_ground_disturbance
|
|
#+caption: Measured ground motion
|
|
#+attr_latex: :scale 1 :float nil
|
|
[[file:figs/ustation_ground_disturbance.png]]
|
|
#+end_minipage
|
|
\hfill
|
|
#+attr_latex: :options [b]{0.44\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:ustation_geophone_picture
|
|
#+caption: (3D) L-4C geophone
|
|
#+attr_latex: :width 0.92\linewidth :float nil
|
|
[[file:figs/ustation_geophone_picture.jpg]]
|
|
#+end_minipage
|
|
|
|
**** Ty Stage
|
|
|
|
To measure the positioning errors of the translation stage, the setup shown in Figure ref:fig:ustation_errors_ty_setup is used.
|
|
A special optical element (called a "straightness interferometer"[fn:9]) is fixed on top of the micro-station, while a laser source[fn:10] and a straightness reflector are fixed on the ground.
|
|
A similar setup was used to measure the horizontal deviation (i.e. in the $x$ direction), as well as the pitch and yaw errors of the translation stage.
|
|
|
|
#+name: fig:ustation_errors_ty_setup
|
|
#+caption: Experimental setup to measure the flatness (vertical deviation) of the translation stage
|
|
[[file:figs/ustation_errors_ty_setup.png]]
|
|
|
|
Six scans were performed between $-4.5\,mm$ and $4.5\,mm$.
|
|
The results for each individual scan are shown in Figure ref:fig:ustation_errors_dy_vertical.
|
|
The measurement axis may not be perfectly aligned with the translation stage axis; this, a linear fit is removed from the measurement.
|
|
The remaining vertical displacement is shown in Figure ref:fig:ustation_errors_dy_vertical_remove_mean.
|
|
A vertical error of $\pm300\,nm$ induced by the translation stage is expected.
|
|
Similar result is obtained for the $x$ lateral direction.
|
|
|
|
#+begin_src matlab
|
|
%% Ty errors
|
|
% Setpoint is in [mm]
|
|
% Vertical error is in [um]
|
|
ty_errors = load('ustation_errors_ty.mat');
|
|
|
|
% Compute best straight line to remove it from data
|
|
average_error = mean(ty_errors.ty_z')';
|
|
straight_line = average_error - detrend(average_error, 1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measurement of the linear (vertical) deviation of the Translation stage
|
|
figure;
|
|
hold on;
|
|
plot(ty_errors.setpoint, ty_errors.ty_z(:,1), '-', 'color', colors(1,:), 'DisplayName', 'Raw data')
|
|
plot(ty_errors.setpoint, ty_errors.ty_z(:,2:end), '-', 'color', colors(1,:), 'HandleVisibility', 'off')
|
|
plot(ty_errors.setpoint, straight_line, '--', 'color', colors(2,:), 'DisplayName', 'Linear fit')
|
|
hold off;
|
|
xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
|
|
xlim([-5, 5]); ylim([-8, 8]);
|
|
legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
xticks([-5:1:5]); yticks([-8:2:8]);
|
|
exportFig('figs/ustation_errors_dy_vertical.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measurement of the linear (vertical) deviation of the Translation stage - Remove best linear fit
|
|
figure;
|
|
plot(ty_errors.setpoint, ty_errors.ty_z - straight_line, 'k-')
|
|
xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
|
|
xlim([-5, 5]); ylim([-0.4, 0.4]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
xticks([-5:1:5]); yticks([-0.4:0.1:0.4]);
|
|
exportFig('figs/ustation_errors_dy_vertical_remove_mean.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_errors_dy
|
|
#+caption: Measurement of the linear (vertical) deviation of the Translation stage (\subref{fig:ustation_errors_dy_vertical}). A linear fit is then removed from the data (\subref{fig:ustation_errors_dy_vertical_remove_mean}).
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_errors_dy_vertical}Measured vertical error}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/ustation_errors_dy_vertical.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_errors_dy_vertical_remove_mean}Error after removing linear fit}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/ustation_errors_dy_vertical_remove_mean.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
#+begin_src matlab
|
|
%% Convert the data to frequency domain
|
|
% Suppose a certain constant velocity scan
|
|
delta_ty = (ty_errors.setpoint(end) - ty_errors.setpoint(1))/(length(ty_errors.setpoint) - 1); % [mm]
|
|
ty_vel = 8*1.125; % [mm/s]
|
|
Ts = delta_ty/ty_vel;
|
|
Fs = 1/Ts;
|
|
|
|
% Frequency Analysis
|
|
Nfft = floor(length(ty_errors.setpoint)); % Number of frequency points
|
|
win = hanning(Nfft); % Windowing
|
|
Noverlap = floor(Nfft/2); % Overlap for frequency analysis
|
|
|
|
% Comnpute the power spectral density
|
|
[pxx_dy_dz, f_dy] = pwelch(1e-6*detrend(ty_errors.ty_z, 1), win, Noverlap, Nfft, Fs);
|
|
pxx_dy_dz = mean(pxx_dy_dz')';
|
|
|
|
% Having the PSD of the lateral error equal to the PSD of the vertical error
|
|
% is a reasonable assumption (and verified in practice)
|
|
pxx_dy_dx = pxx_dy_dz;
|
|
#+end_src
|
|
|
|
**** Spindle
|
|
|
|
To measure the positioning errors induced by the Spindle, a "Spindle error analyzer"[fn:7] is used as shown in Figure ref:fig:ustation_rz_meas_lion_setup.
|
|
A specific target is fixed on top of the micro-station, which consists of two sphere with 1 inch diameter precisely aligned with the spindle rotation axis.
|
|
Five capacitive sensors[fn:8] are pointing at the two spheres, as shown in Figure ref:fig:ustation_rz_meas_lion_zoom.
|
|
From the 5 measured displacements $[d_1,\,d_2,\,d_3,\,d_4,\,d_5]$, the translations and rotations $[D_x,\,D_y,\,D_z,\,R_x,\,R_y]$ of the target can be estimated.
|
|
|
|
#+name: fig:ustation_rz_meas_lion_setup
|
|
#+caption: Experimental setup used to estimate the errors induced by the Spindle rotation (\subref{fig:ustation_rz_meas_lion}). The motion of the two reference spheres is measured using 5 capacitive sensors (\subref{fig:ustation_rz_meas_lion_zoom})
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_rz_meas_lion}Micro-station and 5-DoF metrology}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_rz_meas_lion.jpg]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_rz_meas_lion_zoom}Zoom on the metrology system}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_rz_meas_lion_zoom.jpg]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
A measurement was performed during a constant rotational velocity of the spindle of 60rpm and during 10 turns.
|
|
The obtained results are shown in Figure ref:fig:ustation_errors_spindle.
|
|
A large fraction of the radial (Figure ref:fig:ustation_errors_spindle_radial) and tilt (Figure ref:fig:ustation_errors_spindle_tilt) errors is linked to the fact that the two spheres are not perfectly aligned with the rotation axis of the Spindle.
|
|
This is displayed by the dashed circle.
|
|
After removing the best circular fit from the data, the vibrations induced by the Spindle may be viewed as stochastic disturbances.
|
|
However, some misalignment between the "point-of-interest" of the sample and the rotation axis will be considered because the alignment is not perfect in practice.
|
|
The vertical motion induced by scanning the spindle is in the order of $\pm 30\,nm$ (Figure ref:fig:ustation_errors_spindle_axial).
|
|
|
|
#+begin_src matlab
|
|
%% Spindle Errors
|
|
% Errors are already converted to x,y,z,Rx,Ry
|
|
% Units are in [m] and [rad]
|
|
spindle_errors = load('ustation_errors_spindle.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measured radial errors of the Spindle
|
|
figure;
|
|
hold on;
|
|
plot(1e6*spindle_errors.Dx(1:100:end), 1e6*spindle_errors.Dy(1:100:end), 'DisplayName', 'Raw data')
|
|
% Compute best fitting circle
|
|
[x0, y0, R] = circlefit(spindle_errors.Dx, spindle_errors.Dy);
|
|
theta = linspace(0, 2*pi, 500); % Angle to plot the circle [rad]
|
|
plot(1e6*(x0 + R*cos(theta)), 1e6*(y0 + R*sin(theta)), '--', 'DisplayName', 'Best Fit')
|
|
hold off;
|
|
xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
|
|
axis equal
|
|
xlim([-1, 1]); ylim([-1, 1]);
|
|
xticks([-1, -0.5, 0, 0.5, 1]);
|
|
yticks([-1, -0.5, 0, 0.5, 1]);
|
|
leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_errors_spindle_radial.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measured axial errors of the Spindle
|
|
figure;
|
|
plot(spindle_errors.deg(1:100:end)/360, 1e9*spindle_errors.Dz(1:100:end))
|
|
xlabel('Rotation [turn]'); ylabel('Z displacement [nm]');
|
|
axis square
|
|
xlim([0,2]); ylim([-40, 40]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_errors_spindle_axial.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measured tilt errors of the Spindle
|
|
figure;
|
|
plot(1e6*spindle_errors.Rx(1:100:end), 1e6*spindle_errors.Ry(1:100:end))
|
|
xlabel('X angle [$\mu$rad]'); ylabel('Y angle [$\mu$rad]');
|
|
axis equal
|
|
xlim([-35, 35]); ylim([-35, 35]);
|
|
xticks([-30, -15, 0, 15, 30]);
|
|
yticks([-30, -15, 0, 15, 30]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_errors_spindle_tilt.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_errors_spindle
|
|
#+caption: Measurement of the radial (\subref{fig:ustation_errors_spindle_radial}), axial (\subref{fig:ustation_errors_spindle_axial}) and tilt (\subref{fig:ustation_errors_spindle_tilt}) Spindle errors during a 60rpm spindle rotation. The circular best fit is shown by the dashed circle. It represents the misalignment of the spheres with the rotation axis.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_errors_spindle_radial}Radial errors}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_errors_spindle_radial.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_errors_spindle_axial}Axial error}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_errors_spindle_axial.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_errors_spindle_tilt}Tilt errors}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_errors_spindle_tilt.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
#+begin_src matlab
|
|
%% Remove the circular fit from the data
|
|
[x0, y0, R] = circlefit(spindle_errors.Dx, spindle_errors.Dy);
|
|
|
|
% Search the best angular match
|
|
fun = @(theta)rms((spindle_errors.Dx - (x0 + R*cos(pi/180*spindle_errors.deg+theta(1)))).^2 + (spindle_errors.Dy - (y0 - R*sin(pi/180*spindle_errors.deg+theta(1)))).^2);
|
|
x0 = [0];
|
|
delta_theta = fminsearch(fun, 0);
|
|
|
|
% Compute the remaining error after removing the best circular fit
|
|
spindle_errors.Dx_err = spindle_errors.Dx - (x0 + R*cos(pi/180*spindle_errors.deg+delta_theta));
|
|
spindle_errors.Dy_err = spindle_errors.Dy - (y0 - R*sin(pi/180*spindle_errors.deg+delta_theta));
|
|
|
|
%% Convert the data to frequency domain
|
|
Ts = (spindle_errors.t(end) - spindle_errors.t(1))/(length(spindle_errors.t)-1); % [s]
|
|
Fs = 1/Ts; % [Hz]
|
|
|
|
% Frequency Analysis
|
|
Nfft = floor(2.0*Fs); % Number of frequency points
|
|
win = hanning(Nfft); % Windowing
|
|
Noverlap = floor(Nfft/2); % Overlap for frequency analysis
|
|
|
|
% Comnpute the power spectral density
|
|
[pxx_rz_dz, f_rz] = pwelch(spindle_errors.Dz, win, Noverlap, Nfft, Fs);
|
|
[pxx_rz_dx, ~ ] = pwelch(spindle_errors.Dx_err, win, Noverlap, Nfft, Fs);
|
|
[pxx_rz_dy, ~ ] = pwelch(spindle_errors.Dy_err, win, Noverlap, Nfft, Fs);
|
|
#+end_src
|
|
|
|
** Sensitivity to disturbances
|
|
<<ssec:ustation_disturbances_sensitivity>>
|
|
|
|
To compute the disturbance source (i.e. forces) that induced the measured vibrations in Section ref:ssec:ustation_disturbances_meas, the transfer function from the disturbance sources to the stage vibration (i.e. the "sensitivity to disturbances") needs to be estimated.
|
|
This is achieved using the multi-body model presented in Section ref:sec:ustation_modeling.
|
|
The obtained transfer functions are shown in Figure ref:fig:ustation_model_sensitivity.
|
|
|
|
#+begin_src matlab
|
|
%% Extract sensitivity to disturbances from the Simscape model
|
|
% Initialize stages
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
|
|
initializeDisturbances('enable', false);
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration();
|
|
|
|
% Configure inputs and outputs
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Disturbances/Dwx'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
|
|
io(io_i) = linio([mdl, '/Disturbances/Dwy'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
|
|
io(io_i) = linio([mdl, '/Disturbances/Dwz'], 1, 'openinput'); io_i = io_i + 1; % Vertical Ground Motion
|
|
io(io_i) = linio([mdl, '/Disturbances/Fdy_x'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Ty
|
|
io(io_i) = linio([mdl, '/Disturbances/Fdy_z'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Ty
|
|
io(io_i) = linio([mdl, '/Disturbances/Frz_x'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
|
|
io(io_i) = linio([mdl, '/Disturbances/Frz_y'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
|
|
io(io_i) = linio([mdl, '/Disturbances/Frz_z'], 1, 'openinput'); io_i = io_i + 1; % Parasitic force Rz
|
|
io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/x'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
|
|
io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/y'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
|
|
io(io_i) = linio([mdl, '/Micro-Station/metrology_6dof/z'], 1, 'openoutput'); io_i = io_i + 1; % Relative motion - Hexapod/Granite
|
|
|
|
% Run the linearization
|
|
Gd = linearize(mdl, io, 0);
|
|
Gd.InputName = {'Dwx', 'Dwy', 'Dwz', 'Fdy_x', 'Fdy_z', 'Frz_x', 'Frz_y', 'Frz_z'};
|
|
Gd.OutputName = {'Dx', 'Dy', 'Dz'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
% The identified dynamics are then saved for further use.
|
|
save('matlab/mat/ustation_disturbance_sensitivity.mat', 'Gd')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
% The identified dynamics are then saved for further use.
|
|
save('./mat/ustation_disturbance_sensitivity.mat', 'Gd')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Sensitivity to Ground motion
|
|
freqs = logspace(log10(10), log10(2e2), 1000);
|
|
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Dwx'), freqs, 'Hz'))), 'DisplayName', '$D_x/D_{xf}$');
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dy', 'Dwy'), freqs, 'Hz'))), 'DisplayName', '$D_y/D_{yf}$');
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Dwz'), freqs, 'Hz'))), 'DisplayName', '$D_z/D_{zf}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/m]'); xlabel('Frequency [Hz]');
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_model_sensitivity_ground_motion.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Sensitivity to Translation stage disturbance forces
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Fdy_x'), freqs, 'Hz'))), 'color', colors(1,:), 'DisplayName', '$D_x/F_{xD_y}$');
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Fdy_z'), freqs, 'Hz'))), 'color', colors(3,:), 'DisplayName', '$D_z/F_{zD_y}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
ylim([1e-10, 1e-7]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_model_sensitivity_ty.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Sensitivity to Spindle disturbance forces
|
|
figure;
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dx', 'Frz_x'), freqs, 'Hz'))), 'DisplayName', '$D_x/F_{xR_z}$');
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dy', 'Frz_y'), freqs, 'Hz'))), 'DisplayName', '$D_y/F_{yR_z}$');
|
|
plot(freqs, abs(squeeze(freqresp(Gd('Dz', 'Frz_z'), freqs, 'Hz'))), 'DisplayName', '$D_z/F_{zR_z}$');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); xlabel('Frequency [Hz]');
|
|
ylim([1e-10, 1e-7]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_model_sensitivity_rz.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_model_sensitivity
|
|
#+caption: Extracted transfer functions from disturbances to relative motion between the micro-station's top platform and the granite. The considered disturbances are the ground motion (\subref{fig:ustation_model_sensitivity_ground_motion}), the translation stage vibrations (\subref{fig:ustation_model_sensitivity_ty}), and the spindle vibrations (\subref{fig:ustation_model_sensitivity_rz}).
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_model_sensitivity_ground_motion}Ground motion}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_model_sensitivity_ground_motion.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_model_sensitivity_ty}Translation stage}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_model_sensitivity_ty.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_model_sensitivity_rz}Spindle}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_model_sensitivity_rz.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Obtained disturbance sources
|
|
<<ssec:ustation_disturbances_results>>
|
|
|
|
From the measured effect of disturbances in Section ref:ssec:ustation_disturbances_meas and the sensitivity to disturbances extracted from the multi-body model in Section ref:ssec:ustation_disturbances_sensitivity, the power spectral density of the disturbance sources (i.e. forces applied in the stage's joint) can be estimated.
|
|
The obtained power spectral density of the disturbances are shown in Figure ref:fig:ustation_dist_sources.
|
|
|
|
#+begin_src matlab
|
|
%% Compute the PSD of the equivalent disturbance sources
|
|
pxx_rz_fx = pxx_rz_dx./abs(squeeze(freqresp(Gd('Dx', 'Frz_x'), f_rz, 'Hz'))).^2;
|
|
pxx_rz_fy = pxx_rz_dy./abs(squeeze(freqresp(Gd('Dy', 'Frz_y'), f_rz, 'Hz'))).^2;
|
|
pxx_rz_fz = pxx_rz_dz./abs(squeeze(freqresp(Gd('Dz', 'Frz_z'), f_rz, 'Hz'))).^2;
|
|
|
|
pxx_dy_fx = pxx_dy_dx./abs(squeeze(freqresp(Gd('Dx', 'Fdy_x'), f_dy, 'Hz'))).^2;
|
|
pxx_dy_fz = pxx_dy_dz./abs(squeeze(freqresp(Gd('Dz', 'Fdy_z'), f_dy, 'Hz'))).^2;
|
|
|
|
%% Save the PSD of the disturbance sources for further used
|
|
% in the Simscape model
|
|
|
|
% Ground motion
|
|
min_f = 1; max_f = 100;
|
|
gm_dist.f = f_gm(f_gm < max_f & f_gm > min_f);
|
|
gm_dist.pxx_x = pxx_gm_x(f_gm < max_f & f_gm > min_f);
|
|
gm_dist.pxx_y = pxx_gm_y(f_gm < max_f & f_gm > min_f);
|
|
gm_dist.pxx_z = pxx_gm_z(f_gm < max_f & f_gm > min_f);
|
|
|
|
% Translation stage
|
|
min_f = 0.5; max_f = 500;
|
|
dy_dist.f = f_dy(f_dy < max_f & f_dy > min_f);
|
|
dy_dist.pxx_fx = pxx_dy_fx(f_dy < max_f & f_dy > min_f);
|
|
dy_dist.pxx_fz = pxx_dy_fz(f_dy < max_f & f_dy > min_f);
|
|
|
|
% Spindle
|
|
min_f = 1; max_f = 500;
|
|
rz_dist.f = f_rz(f_rz < max_f & f_rz > min_f);
|
|
rz_dist.pxx_fx = pxx_rz_fx(f_rz < max_f & f_rz > min_f);
|
|
rz_dist.pxx_fy = pxx_rz_fy(f_rz < max_f & f_rz > min_f);
|
|
rz_dist.pxx_fz = pxx_rz_fz(f_rz < max_f & f_rz > min_f);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :tangle no
|
|
% The identified dynamics are then saved for further use.
|
|
save('matlab/mat/ustation_disturbance_psd.mat', 'rz_dist', 'dy_dist', 'gm_dist')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no
|
|
% The identified dynamics are then saved for further use.
|
|
save('./mat/ustation_disturbance_psd.mat', 'rz_dist', 'dy_dist', 'gm_dist')
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Ground Motion
|
|
figure;
|
|
hold on;
|
|
plot(f_gm, sqrt(pxx_gm_x), 'DisplayName', '$D_{wx}$');
|
|
plot(f_gm, sqrt(pxx_gm_y), 'DisplayName', '$D_{wy}$');
|
|
plot(f_gm, sqrt(pxx_gm_z), 'DisplayName', '$D_{wz}$');
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{m}{\sqrt{Hz}}\right]$')
|
|
xlim([1, 200]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_dist_source_ground_motion.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
hold on;
|
|
plot(f_dy, sqrt(pxx_dy_fx), 'DisplayName', '$F_{xD_y}$');
|
|
plot(f_dy, sqrt(pxx_dy_fz), 'DisplayName', '$F_{zD_y}$');
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
|
|
xlim([1, 200]); ylim([1e-3, 1e3]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_dist_source_translation_stage.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
hold on;
|
|
plot(f_rz, sqrt(pxx_rz_fx), 'DisplayName', '$F_{xR_z}$');
|
|
plot(f_rz, sqrt(pxx_rz_fy), 'DisplayName', '$F_{yR_z}$');
|
|
plot(f_rz, sqrt(pxx_rz_fz), 'DisplayName', '$F_{zR_z}$');
|
|
set(gca, 'xscale', 'log'); set(gca, 'yscale', 'log');
|
|
xlabel('Frequency [Hz]'); ylabel('Spectral Density $\left[\frac{N}{\sqrt{Hz}}\right]$')
|
|
xlim([1, 200]); ylim([1e-3, 1e3]);
|
|
leg = legend('location', 'northeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_dist_source_spindle.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_dist_sources
|
|
#+caption: Measured spectral density of the micro-station disturbance sources. Ground motion (\subref{fig:ustation_dist_source_ground_motion}), translation stage (\subref{fig:ustation_dist_source_translation_stage}) and spindle (\subref{fig:ustation_dist_source_spindle}).
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_ground_motion}Ground Motion}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_dist_source_ground_motion.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_translation_stage}Translation Stage}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_dist_source_translation_stage.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_spindle}Spindle}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_dist_source_spindle.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
The disturbances are characterized by their power spectral densities, as shown in Figure ref:fig:ustation_dist_sources.
|
|
However, to perform time domain simulations, disturbances must be represented by a time domain signal.
|
|
To generate stochastic time-domain signals with a specific power spectral densities, the discrete inverse Fourier transform is used, as explained in [[cite:&preumont94_random_vibrat_spect_analy chap. 12.11]].
|
|
Examples of the obtained time-domain disturbance signals are shown in Figure ref:fig:ustation_dist_sources_time.
|
|
|
|
#+begin_src matlab
|
|
%% Compute time domain disturbance signals
|
|
initializeDisturbances();
|
|
load('nass_model_disturbances.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
hold on;
|
|
plot(Rz.t, Rz.x, 'DisplayName', '$F_{xR_z}$');
|
|
plot(Rz.t, Rz.y, 'DisplayName', '$F_{yR_z}$');
|
|
plot(Rz.t, Rz.z, 'DisplayName', '$F_{zR_z}$');
|
|
xlabel('Time [s]'); ylabel('Amplitude [N]')
|
|
xlim([0, 1]); ylim([-100, 100]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_dist_source_spindle_time.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
hold on;
|
|
plot(Dy.t, Dy.x, 'DisplayName', '$F_{xD_y}$');
|
|
plot(Dy.t, Dy.z, 'DisplayName', '$F_{zD_y}$');
|
|
xlabel('Time [s]'); ylabel('Amplitude [N]')
|
|
xlim([0, 1]); ylim([-60, 60])
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_dist_source_translation_stage_time.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
hold on;
|
|
plot(Dw.t, 1e6*Dw.x, 'DisplayName', '$D_{xf}$');
|
|
plot(Dw.t, 1e6*Dw.y, 'DisplayName', '$D_{yf}$');
|
|
plot(Dw.t, 1e6*Dw.z, 'DisplayName', '$D_{zf}$');
|
|
xlabel('Time [s]'); ylabel('Amplitude [$\mu$m]')
|
|
xlim([0, 1]); ylim([-0.6, 0.6])
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_dist_source_ground_motion_time.pdf', 'width', 'third', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_dist_sources_time
|
|
#+caption: Generated time domain disturbance signals. Ground motion (\subref{fig:ustation_dist_source_ground_motion_time}), translation stage (\subref{fig:ustation_dist_source_translation_stage_time}) and spindle (\subref{fig:ustation_dist_source_spindle_time}).
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_ground_motion_time}Ground Motion}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_dist_source_ground_motion_time.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_translation_stage_time}Translation Stage}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_dist_source_translation_stage_time.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_dist_source_spindle_time}Spindle}
|
|
#+attr_latex: :options {0.33\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.9\linewidth
|
|
[[file:figs/ustation_dist_source_spindle_time.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
* Simulation of Scientific Experiments
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/ustation_4_experiments.m
|
|
:END:
|
|
<<sec:ustation_experiments>>
|
|
** Introduction :ignore:
|
|
|
|
To fully validate the micro-station multi-body model, two time-domain simulations corresponding to typical use cases were performed.
|
|
|
|
First, a tomography experiment (i.e. a constant Spindle rotation) was performed and was compared with experimental measurements (Section ref:sec:ustation_experiments_tomography).
|
|
Second, a constant velocity scans with the translation stage was performed and also compared with the experimental data (Section ref:sec:ustation_experiments_ty_scans).
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+begin_src matlab
|
|
%% ustation_4_experiments.m
|
|
#+end_src
|
|
|
|
#+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 :tangle no :noweb yes
|
|
<<m-init-path>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :eval no :noweb yes
|
|
<<m-init-path-tangle>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-simscape>>
|
|
#+end_src
|
|
|
|
#+begin_src matlab :noweb yes
|
|
<<m-init-other>>
|
|
#+end_src
|
|
|
|
** Tomography Experiment
|
|
<<sec:ustation_experiments_tomography>>
|
|
|
|
To simulate a tomography experiment, the setpoint of the Spindle is configured to perform a constant rotation with a rotational velocity of 60rpm.
|
|
Both ground motion and spindle vibration disturbances were simulated based on what was computed in Section ref:sec:ustation_disturbances.
|
|
A radial offset of $\approx 1\,\mu m$ between the "point-of-interest" and the spindle's rotation axis is introduced to represent what is experimentally observed.
|
|
During the 10 second simulation (i.e. 10 spindle turns), the position of the "point-of-interest" with respect to the granite was recorded.
|
|
Results are shown in Figure ref:fig:ustation_errors_model_spindle.
|
|
A good correlation with the measurements is observed both for radial errors (Figure ref:fig:ustation_errors_model_spindle_radial) and axial errors (Figure ref:fig:ustation_errors_model_spindle_axial).
|
|
|
|
#+begin_src matlab
|
|
%% Tomography experiment
|
|
% Sample is not centered with the rotation axis
|
|
% This is done by offsetfing the micro-hexapod by 0.9um
|
|
P_micro_hexapod = [0.9e-6; 0; 0]; % [m]
|
|
|
|
set_param(conf_simulink, 'StopTime', '10');
|
|
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod('AP', P_micro_hexapod);
|
|
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration('log', 'all');
|
|
|
|
initializeDisturbances(...
|
|
'Dw_x', true, ... % Ground Motion - X direction
|
|
'Dw_y', true, ... % Ground Motion - Y direction
|
|
'Dw_z', true, ... % Ground Motion - Z direction
|
|
'Fdy_x', false, ... % Translation Stage - X direction
|
|
'Fdy_z', false, ... % Translation Stage - Z direction
|
|
'Frz_x', true, ... % Spindle - X direction
|
|
'Frz_y', true, ... % Spindle - Y direction
|
|
'Frz_z', true); % Spindle - Z direction
|
|
|
|
initializeReferences(...
|
|
'Rz_type', 'rotating', ...
|
|
'Rz_period', 1, ...
|
|
'Dh_pos', [P_micro_hexapod; 0; 0; 0]);
|
|
|
|
sim(mdl);
|
|
exp_tomography = simout;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Compare with the measurements
|
|
spindle_errors = load('ustation_errors_spindle.mat');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measured radial errors of the Spindle
|
|
figure;
|
|
hold on;
|
|
plot(1e6*spindle_errors.Dx(1:100:end), 1e6*spindle_errors.Dy(1:100:end), 'DisplayName', 'Measurements')
|
|
plot(1e6*exp_tomography.y.x.Data, 1e6*exp_tomography.y.y.Data, 'DisplayName', 'Simulation')
|
|
hold off;
|
|
xlabel('X displacement [$\mu$m]'); ylabel('Y displacement [$\mu$m]');
|
|
axis equal
|
|
xlim([-1, 1]); ylim([-1, 1]);
|
|
xticks([-1, -0.5, 0, 0.5, 1]);
|
|
yticks([-1, -0.5, 0, 0.5, 1]);
|
|
leg = legend('location', 'none', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_errors_model_spindle_radial.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Measured axial errors of the Spindle
|
|
figure;
|
|
hold on;
|
|
plot(spindle_errors.deg(1:100:end)/360, detrend(1e9*spindle_errors.Dz(1:100:end), 0), 'DisplayName', 'Measurements')
|
|
plot(exp_tomography.y.z.Time, detrend(1e9*exp_tomography.y.z.Data, 0), 'DisplayName', 'Simulation')
|
|
hold off;
|
|
xlabel('Rotation [turn]'); ylabel('Z displacement [nm]');
|
|
axis square
|
|
xlim([0,2]); ylim([-40, 40]);
|
|
leg = legend('location', 'southwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/ustation_errors_model_spindle_axial.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_errors_model_spindle
|
|
#+caption: Simulation results for a tomography experiment at constant velocity of 60rpm. The comparison is made with measurements for both radial (\subref{fig:ustation_errors_model_spindle_radial}) and axial errors (\subref{fig:ustation_errors_model_spindle_axial}).
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_errors_model_spindle_radial}Radial error}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/ustation_errors_model_spindle_radial.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:ustation_errors_model_spindle_axial}Axial error}
|
|
#+attr_latex: :options {0.49\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/ustation_errors_model_spindle_axial.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Raster Scans with the translation stage
|
|
<<sec:ustation_experiments_ty_scans>>
|
|
|
|
A second experiment was performed in which the translation stage was scanned at constant velocity.
|
|
The translation stage setpoint is configured to have a "triangular" shape with stroke of $\pm 4.5\, mm$.
|
|
Both ground motion and translation stage vibrations were included in the simulation.
|
|
Similar to what was performed for the tomography simulation, the PoI position with respect to the granite was recorded and compared with the experimental measurements in Figure ref:fig:ustation_errors_model_dy_vertical.
|
|
A similar error amplitude was observed, thus indicating that the multi-body model with the included disturbances accurately represented the micro-station behavior in typical scientific experiments.
|
|
|
|
#+begin_src matlab
|
|
%% Translation stage latteral scans
|
|
set_param(conf_simulink, 'StopTime', '2');
|
|
|
|
initializeGround();
|
|
initializeGranite();
|
|
initializeTy();
|
|
initializeRy();
|
|
initializeRz();
|
|
initializeMicroHexapod();
|
|
|
|
initializeSimscapeConfiguration('gravity', false);
|
|
initializeLoggingConfiguration('log', 'all');
|
|
|
|
initializeDisturbances(...
|
|
'Dw_x', true, ... % Ground Motion - X direction
|
|
'Dw_y', true, ... % Ground Motion - Y direction
|
|
'Dw_z', true, ... % Ground Motion - Z direction
|
|
'Fdy_x', true, ... % Translation Stage - X direction
|
|
'Fdy_z', true, ... % Translation Stage - Z direction
|
|
'Frz_x', false, ... % Spindle - X direction
|
|
'Frz_y', false, ... % Spindle - Y direction
|
|
'Frz_z', false); % Spindle - Z direction
|
|
|
|
initializeReferences(...
|
|
'Dy_type', 'triangular', ...
|
|
'Dy_amplitude', 4.5e-3, ...
|
|
'Dy_period', 2);
|
|
|
|
sim(mdl);
|
|
exp_latteral_scans = simout;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
% Load the experimentally measured errors
|
|
ty_errors = load('ustation_errors_ty.mat');
|
|
|
|
% Compute best straight line to remove it from data
|
|
average_error = mean(ty_errors.ty_z')';
|
|
straight_line = average_error - detrend(average_error, 1);
|
|
|
|
% Only plot data for the scan from -4.5mm to 4.5mm
|
|
dy_setpoint = 1e3*exp_latteral_scans.y.y.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5);
|
|
dz_error = detrend(1e6*exp_latteral_scans.y.z.Data(exp_latteral_scans.y.y.time > 0.5 & exp_latteral_scans.y.y.time < 1.5), 0);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
figure;
|
|
hold on;
|
|
plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,1) - straight_line, 0), 'color', colors(1,:), 'DisplayName', 'Measurement')
|
|
% plot(ty_errors.setpoint, detrend(ty_errors.ty_z(:,[4,6]) - straight_line, 0), 'color', colors(1,:), 'HandleVisibility', 'off')
|
|
plot(dy_setpoint, dz_error, 'color', colors(2,:), 'DisplayName', 'Simulation')
|
|
hold off;
|
|
xlabel('$D_y$ position [mm]'); ylabel('Vertical error [$\mu$m]');
|
|
xlim([-5, 5]); ylim([-0.4, 0.4]);
|
|
leg = legend('location', 'southeast', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
xticks([-5:1:5]); yticks([-0.4:0.1:0.4]);
|
|
exportFig('figs/ustation_errors_model_dy_vertical.pdf', 'width', 'half', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:ustation_errors_model_dy_vertical
|
|
#+caption: Vertical errors during a constant-velocity scan of the translation stage. Comparison of the measurements and simulated errors.
|
|
#+RESULTS:
|
|
[[file:figs/ustation_errors_model_dy_vertical.png]]
|
|
|
|
* Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
<<sec:uniaxial_conclusion>>
|
|
|
|
In this study, a multi-body model of the micro-station was developed.
|
|
It was difficult to match the measured dynamics obtained from the modal analysis of the micro-station.
|
|
However, the most important dynamical characteristic to be modeled is the compliance, as it affects the dynamics of the NASS.
|
|
After tuning the model parameters, a good match with the measured compliance was obtained (Figure ref:fig:ustation_frf_compliance_model).
|
|
|
|
The disturbances affecting the sample position should also be well modeled.
|
|
After experimentally estimating the disturbances (Section ref:sec:ustation_disturbances), the multi-body model was finally validated by performing a tomography simulation (Figure ref:fig:ustation_errors_model_spindle) as well as a simulation in which the translation stage was scanned (Figure ref:fig:ustation_errors_model_dy_vertical).
|
|
|
|
* Bibliography :ignore:
|
|
#+latex: \printbibliography[heading=bibintoc,title={Bibliography}]
|
|
|
|
* Helping Functions :noexport:
|
|
** Initialize Path
|
|
#+NAME: m-init-path
|
|
#+BEGIN_SRC matlab
|
|
addpath('./matlab/'); % Path for scripts
|
|
|
|
%% Path for functions, data and scripts
|
|
addpath('./matlab/mat/'); % Path for Computed FRF
|
|
addpath('./matlab/src/'); % Path for functions
|
|
addpath('./matlab/STEPS/'); % Path for STEPS
|
|
addpath('./matlab/subsystems/'); % Path for Subsystems Simulink files
|
|
#+END_SRC
|
|
|
|
#+NAME: m-init-path-tangle
|
|
#+BEGIN_SRC matlab
|
|
%% Path for functions, data and scripts
|
|
addpath('./mat/'); % Path for Data
|
|
addpath('./src/'); % Path for functions
|
|
addpath('./STEPS/'); % Path for STEPS
|
|
addpath('./subsystems/'); % Path for Subsystems Simulink files
|
|
#+END_SRC
|
|
|
|
** Initialize Simscape Model
|
|
#+NAME: m-init-simscape
|
|
#+begin_src matlab
|
|
% Simulink Model name
|
|
mdl = 'ustation_simscape';
|
|
|
|
load('nass_model_conf_simulink.mat');
|
|
#+end_src
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(log10(10), log10(2e3), 1000);
|
|
#+END_SRC
|
|
|
|
* Matlab Functions :noexport:
|
|
** =initializeSimscapeConfiguration=: Simscape Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeSimscapeConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
#+begin_src matlab
|
|
function [] = initializeSimscapeConfiguration(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
args.gravity logical {mustBeNumericOrLogical} = true
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
#+begin_src matlab
|
|
conf_simscape = struct();
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
#+begin_src matlab
|
|
if args.gravity
|
|
conf_simscape.type = 1;
|
|
else
|
|
conf_simscape.type = 2;
|
|
end
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_conf_simscape.mat', 'file')
|
|
save('mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
|
|
else
|
|
save('mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_conf_simscape.mat', 'file')
|
|
save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeLoggingConfiguration=: Logging Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeLoggingConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
#+begin_src matlab
|
|
function [] = initializeLoggingConfiguration(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
args.log char {mustBeMember(args.log,{'none', 'all', 'forces'})} = 'none'
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
|
end
|
|
#+end_src
|
|
|
|
*** Structure initialization
|
|
#+begin_src matlab
|
|
conf_log = struct();
|
|
#+end_src
|
|
|
|
*** Add Type
|
|
#+begin_src matlab
|
|
switch args.log
|
|
case 'none'
|
|
conf_log.type = 0;
|
|
case 'all'
|
|
conf_log.type = 1;
|
|
case 'forces'
|
|
conf_log.type = 2;
|
|
end
|
|
#+end_src
|
|
|
|
*** Sampling Time
|
|
#+begin_src matlab
|
|
conf_log.Ts = args.Ts;
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_conf_log.mat', 'file')
|
|
save('mat/nass_model_conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('mat/nass_model_conf_log.mat', 'conf_log');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_conf_log.mat', 'file')
|
|
save('matlab/mat/nass_model_conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_conf_log.mat', 'conf_log');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeReferences=: Generate Reference Signals
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeReferences.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function Declaration and Documentation
|
|
#+begin_src matlab
|
|
function [ref] = initializeReferences(args)
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
% Sampling Frequency [s]
|
|
args.Ts (1,1) double {mustBeNumeric, mustBePositive} = 1e-3
|
|
% Maximum simulation time [s]
|
|
args.Tmax (1,1) double {mustBeNumeric, mustBePositive} = 100
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
|
args.Dy_type char {mustBeMember(args.Dy_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
|
% Amplitude of the displacement [m]
|
|
args.Dy_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the displacement [s]
|
|
args.Dy_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% Either "constant" / "triangular" / "sinusoidal"
|
|
args.Ry_type char {mustBeMember(args.Ry_type,{'constant', 'triangular', 'sinusoidal'})} = 'constant'
|
|
% Amplitude [rad]
|
|
args.Ry_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the displacement [s]
|
|
args.Ry_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% Either "constant" / "rotating"
|
|
args.Rz_type char {mustBeMember(args.Rz_type,{'constant', 'rotating', 'rotating-not-filtered'})} = 'constant'
|
|
% Initial angle [rad]
|
|
args.Rz_amplitude (1,1) double {mustBeNumeric} = 0
|
|
% Period of the rotating [s]
|
|
args.Rz_period (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
% For now, only constant is implemented
|
|
args.Dh_type char {mustBeMember(args.Dh_type,{'constant'})} = 'constant'
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform (Pitch-Roll-Yaw Euler angles)
|
|
args.Dh_pos (6,1) double {mustBeNumeric} = zeros(6, 1), ...
|
|
% For now, only constant is implemented
|
|
args.Rm_type char {mustBeMember(args.Rm_type,{'constant'})} = 'constant'
|
|
% Initial position of the two masses
|
|
args.Rm_pos (2,1) double {mustBeNumeric} = [0; pi]
|
|
% For now, only constant is implemented
|
|
args.Dn_type char {mustBeMember(args.Dn_type,{'constant'})} = 'constant'
|
|
% Initial position [m,m,m,rad,rad,rad] of the top platform
|
|
args.Dn_pos (6,1) double {mustBeNumeric} = zeros(6,1)
|
|
end
|
|
#+end_src
|
|
|
|
|
|
*** Initialize Parameters
|
|
#+begin_src matlab
|
|
%% Set Sampling Time
|
|
Ts = args.Ts;
|
|
Tmax = args.Tmax;
|
|
|
|
%% Low Pass Filter to filter out the references
|
|
s = zpk('s');
|
|
w0 = 2*pi*10;
|
|
xi = 1;
|
|
H_lpf = 1/(1 + 2*xi/w0*s + s^2/w0^2);
|
|
#+end_src
|
|
|
|
*** Translation Stage
|
|
#+begin_src matlab
|
|
%% Translation stage - Dy
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Dy = zeros(length(t), 1);
|
|
Dyd = zeros(length(t), 1);
|
|
Dydd = zeros(length(t), 1);
|
|
switch args.Dy_type
|
|
case 'constant'
|
|
Dy(:) = args.Dy_amplitude;
|
|
Dyd(:) = 0;
|
|
Dydd(:) = 0;
|
|
case 'triangular'
|
|
% This is done to unsure that we start with no displacement
|
|
Dy_raw = args.Dy_amplitude*sawtooth(2*pi*t/args.Dy_period,1/2);
|
|
i0 = find(t>=args.Dy_period/4,1);
|
|
Dy(1:end-i0+1) = Dy_raw(i0:end);
|
|
Dy(end-i0+2:end) = Dy_raw(end); % we fix the last value
|
|
|
|
% The signal is filtered out
|
|
Dy = lsim(H_lpf, Dy, t);
|
|
Dyd = lsim(H_lpf*s, Dy, t);
|
|
Dydd = lsim(H_lpf*s^2, Dy, t);
|
|
case 'sinusoidal'
|
|
Dy(:) = args.Dy_amplitude*sin(2*pi/args.Dy_period*t);
|
|
Dyd = args.Dy_amplitude*2*pi/args.Dy_period*cos(2*pi/args.Dy_period*t);
|
|
Dydd = -args.Dy_amplitude*(2*pi/args.Dy_period)^2*sin(2*pi/args.Dy_period*t);
|
|
otherwise
|
|
warning('Dy_type is not set correctly');
|
|
end
|
|
|
|
Dy = struct('time', t, 'signals', struct('values', Dy), 'deriv', Dyd, 'dderiv', Dydd);
|
|
#+end_src
|
|
|
|
*** Tilt Stage
|
|
#+begin_src matlab
|
|
%% Tilt Stage - Ry
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Ry = zeros(length(t), 1);
|
|
Ryd = zeros(length(t), 1);
|
|
Rydd = zeros(length(t), 1);
|
|
|
|
switch args.Ry_type
|
|
case 'constant'
|
|
Ry(:) = args.Ry_amplitude;
|
|
Ryd(:) = 0;
|
|
Rydd(:) = 0;
|
|
case 'triangular'
|
|
Ry_raw = args.Ry_amplitude*sawtooth(2*pi*t/args.Ry_period,1/2);
|
|
i0 = find(t>=args.Ry_period/4,1);
|
|
Ry(1:end-i0+1) = Ry_raw(i0:end);
|
|
Ry(end-i0+2:end) = Ry_raw(end); % we fix the last value
|
|
|
|
% The signal is filtered out
|
|
Ry = lsim(H_lpf, Ry, t);
|
|
Ryd = lsim(H_lpf*s, Ry, t);
|
|
Rydd = lsim(H_lpf*s^2, Ry, t);
|
|
case 'sinusoidal'
|
|
Ry(:) = args.Ry_amplitude*sin(2*pi/args.Ry_period*t);
|
|
|
|
Ryd = args.Ry_amplitude*2*pi/args.Ry_period*cos(2*pi/args.Ry_period*t);
|
|
Rydd = -args.Ry_amplitude*(2*pi/args.Ry_period)^2*sin(2*pi/args.Ry_period*t);
|
|
otherwise
|
|
warning('Ry_type is not set correctly');
|
|
end
|
|
|
|
Ry = struct('time', t, 'signals', struct('values', Ry), 'deriv', Ryd, 'dderiv', Rydd);
|
|
#+end_src
|
|
|
|
*** Spindle
|
|
#+begin_src matlab
|
|
%% Spindle - Rz
|
|
t = 0:Ts:Tmax; % Time Vector [s]
|
|
Rz = zeros(length(t), 1);
|
|
Rzd = zeros(length(t), 1);
|
|
Rzdd = zeros(length(t), 1);
|
|
|
|
switch args.Rz_type
|
|
case 'constant'
|
|
Rz(:) = args.Rz_amplitude;
|
|
Rzd(:) = 0;
|
|
Rzdd(:) = 0;
|
|
case 'rotating-not-filtered'
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
|
|
|
% The signal is filtered out
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
|
Rzd(:) = 2*pi/args.Rz_period;
|
|
Rzdd(:) = 0;
|
|
|
|
% We add the angle offset
|
|
Rz = Rz + args.Rz_amplitude;
|
|
|
|
case 'rotating'
|
|
Rz(:) = 2*pi/args.Rz_period*t;
|
|
|
|
% The signal is filtered out
|
|
Rz = lsim(H_lpf, Rz, t);
|
|
Rzd = lsim(H_lpf*s, Rz, t);
|
|
Rzdd = lsim(H_lpf*s^2, Rz, t);
|
|
|
|
% We add the angle offset
|
|
Rz = Rz + args.Rz_amplitude;
|
|
otherwise
|
|
warning('Rz_type is not set correctly');
|
|
end
|
|
|
|
Rz = struct('time', t, 'signals', struct('values', Rz), 'deriv', Rzd, 'dderiv', Rzdd);
|
|
#+end_src
|
|
|
|
*** Micro Hexapod
|
|
#+begin_src matlab
|
|
%% Micro-Hexapod
|
|
t = [0, Ts];
|
|
Dh = zeros(length(t), 6);
|
|
Dhl = zeros(length(t), 6);
|
|
|
|
switch args.Dh_type
|
|
case 'constant'
|
|
Dh = [args.Dh_pos, args.Dh_pos];
|
|
|
|
load('nass_model_stages.mat', 'micro_hexapod');
|
|
|
|
AP = [args.Dh_pos(1) ; args.Dh_pos(2) ; args.Dh_pos(3)];
|
|
|
|
tx = args.Dh_pos(4);
|
|
ty = args.Dh_pos(5);
|
|
tz = args.Dh_pos(6);
|
|
|
|
ARB = [cos(tz) -sin(tz) 0;
|
|
sin(tz) cos(tz) 0;
|
|
0 0 1]*...
|
|
[ cos(ty) 0 sin(ty);
|
|
0 1 0;
|
|
-sin(ty) 0 cos(ty)]*...
|
|
[1 0 0;
|
|
0 cos(tx) -sin(tx);
|
|
0 sin(tx) cos(tx)];
|
|
|
|
[~, Dhl] = inverseKinematics(micro_hexapod, 'AP', AP, 'ARB', ARB);
|
|
Dhl = [Dhl, Dhl];
|
|
otherwise
|
|
warning('Dh_type is not set correctly');
|
|
end
|
|
|
|
Dh = struct('time', t, 'signals', struct('values', Dh));
|
|
Dhl = struct('time', t, 'signals', struct('values', Dhl));
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_references.mat', 'file')
|
|
save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
|
else
|
|
save('mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_references.mat', 'file')
|
|
save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_references.mat', 'Dy', 'Ry', 'Rz', 'Dh', 'Dhl', 'args', 'Ts');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeDisturbances=: Initialize Disturbances
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeDisturbances.m
|
|
:header-args:matlab+: :comments none :mkdirp yes
|
|
:header-args:matlab+: :eval no :results none
|
|
:END:
|
|
|
|
*** Function Declaration and Documentation
|
|
#+begin_src matlab
|
|
function [] = initializeDisturbances(args)
|
|
% initializeDisturbances - Initialize the disturbances
|
|
%
|
|
% Syntax: [] = initializeDisturbances(args)
|
|
%
|
|
% Inputs:
|
|
% - args -
|
|
|
|
#+end_src
|
|
|
|
*** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
% Global parameter to enable or disable the disturbances
|
|
args.enable logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - X direction
|
|
args.Dw_x logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - Y direction
|
|
args.Dw_y logical {mustBeNumericOrLogical} = true
|
|
% Ground Motion - Z direction
|
|
args.Dw_z logical {mustBeNumericOrLogical} = true
|
|
% Translation Stage - X direction
|
|
args.Fdy_x logical {mustBeNumericOrLogical} = true
|
|
% Translation Stage - Z direction
|
|
args.Fdy_z logical {mustBeNumericOrLogical} = true
|
|
% Spindle - X direction
|
|
args.Frz_x logical {mustBeNumericOrLogical} = true
|
|
% Spindle - Y direction
|
|
args.Frz_y logical {mustBeNumericOrLogical} = true
|
|
% Spindle - Z direction
|
|
args.Frz_z logical {mustBeNumericOrLogical} = true
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
% Initialization of random numbers
|
|
rng("shuffle");
|
|
#+end_src
|
|
|
|
*** Ground Motion
|
|
#+begin_src matlab
|
|
%% Ground Motion
|
|
if args.enable
|
|
% Load the PSD of disturbance
|
|
load('ustation_disturbance_psd.mat', 'gm_dist')
|
|
|
|
% Frequency Data
|
|
Dw.f = gm_dist.f;
|
|
Dw.psd_x = gm_dist.pxx_x;
|
|
Dw.psd_y = gm_dist.pxx_y;
|
|
Dw.psd_z = gm_dist.pxx_z;
|
|
|
|
% Time data
|
|
Fs = 2*Dw.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(Dw.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
Dw.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
|
|
% ASD representation of the ground motion
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(Dw.psd_x(i)/T0);
|
|
end
|
|
|
|
if args.Dw_x
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dw.x = N/sqrt(2)*ifft(Cx); % Ground Motion - x direction [m]
|
|
else
|
|
Dw.x = zeros(length(Dw.t), 1);
|
|
end
|
|
|
|
if args.Dw_y
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dw.y = N/sqrt(2)*ifft(Cx); % Ground Motion - y direction [m]
|
|
else
|
|
Dw.y = zeros(length(Dw.t), 1);
|
|
end
|
|
|
|
if args.Dw_y
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dw.z = N/sqrt(2)*ifft(Cx); % Ground Motion - z direction [m]
|
|
else
|
|
Dw.z = zeros(length(Dw.t), 1);
|
|
end
|
|
|
|
else
|
|
Dw.t = [0,1]; % Time Vector [s]
|
|
Dw.x = [0,0]; % Ground Motion - X [m]
|
|
Dw.y = [0,0]; % Ground Motion - Y [m]
|
|
Dw.z = [0,0]; % Ground Motion - Z [m]
|
|
end
|
|
#+end_src
|
|
|
|
*** Translation stage
|
|
#+begin_src matlab
|
|
%% Translation stage
|
|
if args.enable
|
|
% Load the PSD of disturbance
|
|
load('ustation_disturbance_psd.mat', 'dy_dist')
|
|
|
|
% Frequency Data
|
|
Dy.f = dy_dist.f;
|
|
Dy.psd_x = dy_dist.pxx_fx;
|
|
Dy.psd_z = dy_dist.pxx_fz;
|
|
|
|
% Time data
|
|
Fs = 2*Dy.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(Dy.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
Dy.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
|
|
% ASD representation of the disturbance voice
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(Dy.psd_x(i)/T0);
|
|
end
|
|
|
|
if args.Fdy_x
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dy.x = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - X direction [N]
|
|
else
|
|
Dy.x = zeros(length(Dy.t), 1);
|
|
end
|
|
|
|
if args.Fdy_z
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Dy.z = N/sqrt(2)*ifft(Cx); % Translation stage disturbances - Z direction [N]
|
|
else
|
|
Dy.z = zeros(length(Dy.t), 1);
|
|
end
|
|
|
|
else
|
|
Dy.t = [0,1]; % Time Vector [s]
|
|
Dy.x = [0,0]; % Translation Stage disturbances - X [N]
|
|
Dy.z = [0,0]; % Translation Stage disturbances - Z [N]
|
|
end
|
|
#+end_src
|
|
|
|
*** Spindle
|
|
#+begin_src matlab
|
|
%% Spindle
|
|
if args.enable
|
|
% Load the PSD of disturbance
|
|
load('ustation_disturbance_psd.mat', 'rz_dist')
|
|
|
|
% Frequency Data
|
|
Rz.f = rz_dist.f;
|
|
Rz.psd_x = rz_dist.pxx_fx;
|
|
Rz.psd_y = rz_dist.pxx_fy;
|
|
Rz.psd_z = rz_dist.pxx_fz;
|
|
|
|
% Time data
|
|
Fs = 2*Rz.f(end); % Sampling Frequency of data is twice the maximum frequency of the PSD vector [Hz]
|
|
N = 2*length(Rz.f); % Number of Samples match the one of the wanted PSD
|
|
T0 = N/Fs; % Signal Duration [s]
|
|
Rz.t = linspace(0, T0, N+1)'; % Time Vector [s]
|
|
|
|
% ASD representation of the disturbance voice
|
|
C = zeros(N/2,1);
|
|
for i = 1:N/2
|
|
C(i) = sqrt(Rz.psd_x(i)/T0);
|
|
end
|
|
|
|
if args.Frz_x
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Rz.x = N/sqrt(2)*ifft(Cx); % spindle disturbances - X direction [N]
|
|
else
|
|
Rz.x = zeros(length(Rz.t), 1);
|
|
end
|
|
|
|
if args.Frz_y
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Rz.y = N/sqrt(2)*ifft(Cx); % spindle disturbances - Y direction [N]
|
|
else
|
|
Rz.y = zeros(length(Rz.t), 1);
|
|
end
|
|
|
|
if args.Frz_z
|
|
theta = 2*pi*rand(N/2,1); % Generate random phase [rad]
|
|
Cx = [0 ; C.*complex(cos(theta),sin(theta))];
|
|
Cx = [Cx; flipud(conj(Cx(2:end)))];;
|
|
Rz.z = N/sqrt(2)*ifft(Cx); % spindle disturbances - Z direction [N]
|
|
else
|
|
Rz.z = zeros(length(Rz.t), 1);
|
|
end
|
|
|
|
else
|
|
Rz.t = [0,1]; % Time Vector [s]
|
|
Rz.x = [0,0]; % Spindle disturbances - X [N]
|
|
Rz.y = [0,0]; % Spindle disturbances - X [N]
|
|
Rz.z = [0,0]; % Spindle disturbances - Z [N]
|
|
end
|
|
#+end_src
|
|
|
|
*** Direct Forces
|
|
#+begin_src matlab
|
|
u = zeros(100, 6);
|
|
Fd = u;
|
|
#+end_src
|
|
|
|
*** Set initial value to zero
|
|
#+begin_src matlab
|
|
Dw.x = Dw.x - Dw.x(1);
|
|
Dw.y = Dw.y - Dw.y(1);
|
|
Dw.z = Dw.z - Dw.z(1);
|
|
|
|
Dy.x = Dy.x - Dy.x(1);
|
|
Dy.z = Dy.z - Dy.z(1);
|
|
|
|
Rz.x = Rz.x - Rz.x(1);
|
|
Rz.y = Rz.y - Rz.y(1);
|
|
Rz.z = Rz.z - Rz.z(1);
|
|
#+end_src
|
|
|
|
*** Save the Structure
|
|
#+begin_src matlab
|
|
if exist('./mat', 'dir')
|
|
save('mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
|
|
elseif exist('./matlab', 'dir')
|
|
save('matlab/mat/nass_model_disturbances.mat', 'Dw', 'Dy', 'Rz', 'Fd', 'args');
|
|
end
|
|
#+end_src
|
|
|
|
** =describeMicroStationSetup=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/describeMicroStationSetup.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
*** Function description
|
|
#+begin_src matlab
|
|
function [] = describeMicroStationSetup()
|
|
% describeMicroStationSetup -
|
|
%
|
|
% Syntax: [] = describeMicroStationSetup()
|
|
%
|
|
% Inputs:
|
|
% - -
|
|
%
|
|
% Outputs:
|
|
% - -
|
|
#+end_src
|
|
|
|
*** Simscape Configuration
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_conf_simscape.mat', 'conf_simscape');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Simscape Configuration:\n');
|
|
|
|
if conf_simscape.type == 1
|
|
fprintf('- Gravity is included\n');
|
|
else
|
|
fprintf('- Gravity is not included\n');
|
|
end
|
|
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
*** Disturbances
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_disturbances.mat', 'args');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Disturbances:\n');
|
|
if ~args.enable
|
|
fprintf('- No disturbance is included\n');
|
|
else
|
|
if args.Dwx && args.Dwy && args.Dwz
|
|
fprintf('- Ground motion\n');
|
|
end
|
|
if args.Fdy_x && args.Fdy_z
|
|
fprintf('- Vibrations of the Translation Stage\n');
|
|
end
|
|
if args.Frz_z
|
|
fprintf('- Vibrations of the Spindle\n');
|
|
end
|
|
end
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
*** References
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_references.mat', 'args');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Reference Tracking:\n');
|
|
fprintf('- Translation Stage:\n');
|
|
switch args.Dy_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Dy = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
case 'triangular'
|
|
fprintf(' - Triangular Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
|
case 'sinusoidal'
|
|
fprintf(' - Sinusoidal Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Dy_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Dy_period);
|
|
end
|
|
|
|
fprintf('- Tilt Stage:\n');
|
|
switch args.Ry_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Ry = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
case 'triangular'
|
|
fprintf(' - Triangular Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
|
case 'sinusoidal'
|
|
fprintf(' - Sinusoidal Path\n');
|
|
fprintf(' - Amplitude = %.0f [mm]\n', args.Ry_amplitude*1e3);
|
|
fprintf(' - Period = %.0f [s]\n', args.Ry_period);
|
|
end
|
|
|
|
fprintf('- Spindle:\n');
|
|
switch args.Rz_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Rz = %.0f [deg]\n', 180/pi*args.Rz_amplitude);
|
|
case { 'rotating', 'rotating-not-filtered' }
|
|
fprintf(' - Rotating\n');
|
|
fprintf(' - Speed = %.0f [rpm]\n', 60/args.Rz_period);
|
|
end
|
|
|
|
|
|
fprintf('- Micro Hexapod:\n');
|
|
switch args.Dh_type
|
|
case 'constant'
|
|
fprintf(' - Constant Position\n');
|
|
fprintf(' - Dh = %.0f, %.0f, %.0f [mm]\n', args.Dh_pos(1), args.Dh_pos(2), args.Dh_pos(3));
|
|
fprintf(' - Rh = %.0f, %.0f, %.0f [deg]\n', args.Dh_pos(4), args.Dh_pos(5), args.Dh_pos(6));
|
|
end
|
|
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
*** Micro-Station
|
|
#+begin_src matlab
|
|
load('./mat/nass_model_stages.mat', 'ground', 'granite', 'ty', 'ry', 'rz', 'micro_hexapod', 'axisc');
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
fprintf('Micro Station:\n');
|
|
|
|
if granite.type == 1 && ...
|
|
ty.type == 1 && ...
|
|
ry.type == 1 && ...
|
|
rz.type == 1 && ...
|
|
micro_hexapod.type == 1;
|
|
fprintf('- All stages are rigid\n');
|
|
elseif granite.type == 2 && ...
|
|
ty.type == 2 && ...
|
|
ry.type == 2 && ...
|
|
rz.type == 2 && ...
|
|
micro_hexapod.type == 2;
|
|
fprintf('- All stages are flexible\n');
|
|
else
|
|
if granite.type == 1 || granite.type == 4
|
|
fprintf('- Granite is rigid\n');
|
|
else
|
|
fprintf('- Granite is flexible\n');
|
|
end
|
|
if ty.type == 1 || ty.type == 4
|
|
fprintf('- Translation Stage is rigid\n');
|
|
else
|
|
fprintf('- Translation Stage is flexible\n');
|
|
end
|
|
if ry.type == 1 || ry.type == 4
|
|
fprintf('- Tilt Stage is rigid\n');
|
|
else
|
|
fprintf('- Tilt Stage is flexible\n');
|
|
end
|
|
if rz.type == 1 || rz.type == 4
|
|
fprintf('- Spindle is rigid\n');
|
|
else
|
|
fprintf('- Spindle is flexible\n');
|
|
end
|
|
if micro_hexapod.type == 1 || micro_hexapod.type == 4
|
|
fprintf('- Micro Hexapod is rigid\n');
|
|
else
|
|
fprintf('- Micro Hexapod is flexible\n');
|
|
end
|
|
|
|
end
|
|
|
|
fprintf('\n');
|
|
#+end_src
|
|
|
|
** =computeReferencePose=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/computeReferencePose.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
function [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
|
% computeReferencePose - Compute the homogeneous transformation matrix corresponding to the wanted pose of the sample
|
|
%
|
|
% Syntax: [WTr] = computeReferencePose(Dy, Ry, Rz, Dh, Dn)
|
|
%
|
|
% Inputs:
|
|
% - Dy - Reference of the Translation Stage [m]
|
|
% - Ry - Reference of the Tilt Stage [rad]
|
|
% - Rz - Reference of the Spindle [rad]
|
|
% - Dh - Reference of the Micro Hexapod (Pitch, Roll, Yaw angles) [m, m, m, rad, rad, rad]
|
|
% - Dn - Reference of the Nano Hexapod [m, m, m, rad, rad, rad]
|
|
%
|
|
% Outputs:
|
|
% - WTr -
|
|
|
|
%% Translation Stage
|
|
Rty = [1 0 0 0;
|
|
0 1 0 Dy;
|
|
0 0 1 0;
|
|
0 0 0 1];
|
|
|
|
%% Tilt Stage - Pure rotating aligned with Ob
|
|
Rry = [ cos(Ry) 0 sin(Ry) 0;
|
|
0 1 0 0;
|
|
-sin(Ry) 0 cos(Ry) 0;
|
|
0 0 0 1];
|
|
|
|
%% Spindle - Rotation along the Z axis
|
|
Rrz = [cos(Rz) -sin(Rz) 0 0 ;
|
|
sin(Rz) cos(Rz) 0 0 ;
|
|
0 0 1 0 ;
|
|
0 0 0 1 ];
|
|
|
|
|
|
%% Micro-Hexapod
|
|
Rhx = [1 0 0;
|
|
0 cos(Dh(4)) -sin(Dh(4));
|
|
0 sin(Dh(4)) cos(Dh(4))];
|
|
|
|
Rhy = [ cos(Dh(5)) 0 sin(Dh(5));
|
|
0 1 0;
|
|
-sin(Dh(5)) 0 cos(Dh(5))];
|
|
|
|
Rhz = [cos(Dh(6)) -sin(Dh(6)) 0;
|
|
sin(Dh(6)) cos(Dh(6)) 0;
|
|
0 0 1];
|
|
|
|
Rh = [1 0 0 Dh(1) ;
|
|
0 1 0 Dh(2) ;
|
|
0 0 1 Dh(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rh(1:3, 1:3) = Rhz*Rhy*Rhx;
|
|
|
|
%% Nano-Hexapod
|
|
Rnx = [1 0 0;
|
|
0 cos(Dn(4)) -sin(Dn(4));
|
|
0 sin(Dn(4)) cos(Dn(4))];
|
|
|
|
Rny = [ cos(Dn(5)) 0 sin(Dn(5));
|
|
0 1 0;
|
|
-sin(Dn(5)) 0 cos(Dn(5))];
|
|
|
|
Rnz = [cos(Dn(6)) -sin(Dn(6)) 0;
|
|
sin(Dn(6)) cos(Dn(6)) 0;
|
|
0 0 1];
|
|
|
|
Rn = [1 0 0 Dn(1) ;
|
|
0 1 0 Dn(2) ;
|
|
0 0 1 Dn(3) ;
|
|
0 0 0 1 ];
|
|
|
|
Rn(1:3, 1:3) = Rnz*Rny*Rnx;
|
|
|
|
%% Total Homogeneous transformation
|
|
WTr = Rty*Rry*Rrz*Rh*Rn;
|
|
end
|
|
#+end_src
|
|
|
|
** =circlefit=
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/circlefit.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
#+begin_src matlab
|
|
function [xc,yc,R,a] = circlefit(x,y)
|
|
%
|
|
% [xc yx R] = circfit(x,y)
|
|
%
|
|
% fits a circle in x,y plane in a more accurate
|
|
% (less prone to ill condition )
|
|
% procedure than circfit2 but using more memory
|
|
% x,y are column vector where (x(i),y(i)) is a measured point
|
|
%
|
|
% result is center point (yc,xc) and radius R
|
|
% an optional output is the vector of coeficient a
|
|
% describing the circle's equation
|
|
%
|
|
% x^2+y^2+a(1)*x+a(2)*y+a(3)=0
|
|
%
|
|
% By: Izhak bucher 25/oct /1991,
|
|
x=x(:); y=y(:);
|
|
a=[x y ones(size(x))]\[-(x.^2+y.^2)];
|
|
xc = -.5*a(1);
|
|
yc = -.5*a(2);
|
|
R = sqrt((a(1)^2+a(2)^2)/4-a(3));
|
|
#+end_src
|
|
|
|
** Initialize Micro-Station Stages
|
|
*** =initializeGround=: Ground
|
|
#+begin_src matlab :tangle matlab/src/initializeGround.m :comments none :mkdirp yes :eval no
|
|
function [ground] = initializeGround(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid'})} = 'rigid'
|
|
args.rot_point (3,1) double {mustBeNumeric} = zeros(3,1) % Rotation point for the ground motion [m]
|
|
end
|
|
|
|
ground = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
ground.type = 0;
|
|
case 'rigid'
|
|
ground.type = 1;
|
|
end
|
|
|
|
ground.shape = [2, 2, 0.5]; % [m]
|
|
ground.density = 2800; % [kg/m3]
|
|
|
|
ground.rot_point = args.rot_point;
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'ground', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'ground');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'ground', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'ground');
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeGranite=: Granite
|
|
#+begin_src matlab :tangle matlab/src/initializeGranite.m :comments none :mkdirp yes :eval no
|
|
function [granite] = initializeGranite(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'rigid', 'flexible', 'none'})} = 'flexible'
|
|
args.density (1,1) double {mustBeNumeric, mustBeNonnegative} = 2800 % Density [kg/m3]
|
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = [5e9; 5e9; 5e9; 2.5e7; 2.5e7; 1e7] % [N/m]
|
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = [4.0e5; 1.1e5; 9.0e5; 2e4; 2e4; 1e4] % [N/(m/s)]
|
|
args.x0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the X direction [m]
|
|
args.y0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Y direction [m]
|
|
args.z0 (1,1) double {mustBeNumeric} = 0 % Rest position of the Joint in the Z direction [m]
|
|
args.sample_pos (1,1) double {mustBeNumeric} = 0.775 % Height of the measurment point [m]
|
|
end
|
|
|
|
granite = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
granite.type = 0;
|
|
case 'rigid'
|
|
granite.type = 1;
|
|
case 'flexible'
|
|
granite.type = 2;
|
|
end
|
|
|
|
granite.density = args.density; % [kg/m3]
|
|
granite.STEP = 'granite.STEP';
|
|
|
|
% Z-offset for the initial position of the sample with respect to the granite top surface.
|
|
granite.sample_pos = args.sample_pos; % [m]
|
|
|
|
granite.K = args.K; % [N/m]
|
|
granite.C = args.C; % [N/(m/s)]
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'granite', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'granite');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'granite', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'granite');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeTy=: Translation Stage
|
|
#+begin_src matlab :tangle matlab/src/initializeTy.m :comments none :mkdirp yes :eval no
|
|
function [ty] = initializeTy(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
|
end
|
|
|
|
ty = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
ty.type = 0;
|
|
case 'rigid'
|
|
ty.type = 1;
|
|
case 'flexible'
|
|
ty.type = 2;
|
|
end
|
|
|
|
% Ty Granite frame
|
|
ty.granite_frame.density = 7800; % [kg/m3] => 43kg
|
|
ty.granite_frame.STEP = 'Ty_Granite_Frame.STEP';
|
|
|
|
% Guide Translation Ty
|
|
ty.guide.density = 7800; % [kg/m3] => 76kg
|
|
ty.guide.STEP = 'Ty_Guide.STEP';
|
|
|
|
% Ty - Guide_Translation12
|
|
ty.guide12.density = 7800; % [kg/m3]
|
|
ty.guide12.STEP = 'Ty_Guide_12.STEP';
|
|
|
|
% Ty - Guide_Translation11
|
|
ty.guide11.density = 7800; % [kg/m3]
|
|
ty.guide11.STEP = 'Ty_Guide_11.STEP';
|
|
|
|
% Ty - Guide_Translation22
|
|
ty.guide22.density = 7800; % [kg/m3]
|
|
ty.guide22.STEP = 'Ty_Guide_22.STEP';
|
|
|
|
% Ty - Guide_Translation21
|
|
ty.guide21.density = 7800; % [kg/m3]
|
|
ty.guide21.STEP = 'Ty_Guide_21.STEP';
|
|
|
|
% Ty - Plateau translation
|
|
ty.frame.density = 7800; % [kg/m3]
|
|
ty.frame.STEP = 'Ty_Stage.STEP';
|
|
|
|
% Ty Stator Part
|
|
ty.stator.density = 5400; % [kg/m3]
|
|
ty.stator.STEP = 'Ty_Motor_Stator.STEP';
|
|
|
|
% Ty Rotor Part
|
|
ty.rotor.density = 5400; % [kg/m3]
|
|
ty.rotor.STEP = 'Ty_Motor_Rotor.STEP';
|
|
|
|
ty.K = [2e8; 1e8; 2e8; 6e7; 9e7; 6e7]; % [N/m, N*m/rad]
|
|
ty.C = [8e4; 5e4; 8e4; 2e4; 3e4; 1e4]; % [N/(m/s), N*m/(rad/s)]
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'ty', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'ty');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'ty', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'ty');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeRy=: Tilt Stage
|
|
#+begin_src matlab :tangle matlab/src/initializeRy.m :comments none :mkdirp yes :eval no
|
|
function [ry] = initializeRy(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
|
args.Ry_init (1,1) double {mustBeNumeric} = 0
|
|
end
|
|
|
|
ry = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
ry.type = 0;
|
|
case 'rigid'
|
|
ry.type = 1;
|
|
case 'flexible'
|
|
ry.type = 2;
|
|
end
|
|
|
|
% Ry - Guide for the tilt stage
|
|
ry.guide.density = 7800; % [kg/m3]
|
|
ry.guide.STEP = 'Tilt_Guide.STEP';
|
|
|
|
% Ry - Rotor of the motor
|
|
ry.rotor.density = 2400; % [kg/m3]
|
|
ry.rotor.STEP = 'Tilt_Motor_Axis.STEP';
|
|
|
|
% Ry - Motor
|
|
ry.motor.density = 3200; % [kg/m3]
|
|
ry.motor.STEP = 'Tilt_Motor.STEP';
|
|
|
|
% Ry - Plateau Tilt
|
|
ry.stage.density = 7800; % [kg/m3]
|
|
ry.stage.STEP = 'Tilt_Stage.STEP';
|
|
|
|
% Z-Offset so that the center of rotation matches the sample center;
|
|
ry.z_offset = 0.58178; % [m]
|
|
|
|
ry.Ry_init = args.Ry_init; % [rad]
|
|
|
|
ry.K = [3.8e8; 4e8; 3.8e8; 1.2e8; 6e4; 1.2e8];
|
|
ry.C = [1e5; 1e5; 1e5; 3e4; 1e3; 3e4];
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'ry', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'ry');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'ry', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'ry');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeRz=: Spindle
|
|
#+begin_src matlab :tangle matlab/src/initializeRz.m :comments none :mkdirp yes :eval no
|
|
function [rz] = initializeRz(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
|
end
|
|
|
|
rz = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
rz.type = 0;
|
|
case 'rigid'
|
|
rz.type = 1;
|
|
case 'flexible'
|
|
rz.type = 2;
|
|
end
|
|
|
|
% Spindle - Slip Ring
|
|
rz.slipring.density = 7800; % [kg/m3]
|
|
rz.slipring.STEP = 'Spindle_Slip_Ring.STEP';
|
|
|
|
% Spindle - Rotor
|
|
rz.rotor.density = 7800; % [kg/m3]
|
|
rz.rotor.STEP = 'Spindle_Rotor.STEP';
|
|
|
|
% Spindle - Stator
|
|
rz.stator.density = 7800; % [kg/m3]
|
|
rz.stator.STEP = 'Spindle_Stator.STEP';
|
|
|
|
rz.K = [7e8; 7e8; 2e9; 1e7; 1e7; 1e7];
|
|
rz.C = [4e4; 4e4; 7e4; 1e4; 1e4; 1e4];
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'rz', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'rz');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'rz', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'rz');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeMicroHexapod=: Micro Hexapod
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeMicroHexapod.m :comments none :mkdirp yes :eval no
|
|
function [micro_hexapod] = initializeMicroHexapod(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'rigid', 'flexible'})} = 'flexible'
|
|
% initializeFramesPositions
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 350e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 270e-3
|
|
% generateGeneralConfiguration
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 175.5e-3
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180)
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 45e-3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 118e-3
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180)
|
|
% initializeStrutDynamics
|
|
args.Ki (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e7*ones(6,1)
|
|
args.Ci (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.4e3*ones(6,1)
|
|
% initializeCylindricalPlatforms
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 207.5e-3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 10
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 26e-3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3
|
|
% initializeCylindricalStruts
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 25e-3
|
|
% inverseKinematics
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
stewart = initializeStewartPlatform();
|
|
|
|
stewart = initializeFramesPositions(stewart, ...
|
|
'H', args.H, ...
|
|
'MO_B', args.MO_B);
|
|
|
|
stewart = generateGeneralConfiguration(stewart, ...
|
|
'FH', args.FH, ...
|
|
'FR', args.FR, ...
|
|
'FTh', args.FTh, ...
|
|
'MH', args.MH, ...
|
|
'MR', args.MR, ...
|
|
'MTh', args.MTh);
|
|
|
|
stewart = computeJointsPose(stewart);
|
|
|
|
stewart = initializeStrutDynamics(stewart, ...
|
|
'K', args.Ki, ...
|
|
'C', args.Ci);
|
|
|
|
stewart = initializeJointDynamics(stewart, ...
|
|
'type_F', 'universal_p', ...
|
|
'type_M', 'spherical_p');
|
|
|
|
stewart = initializeCylindricalPlatforms(stewart, ...
|
|
'Fpm', args.Fpm, ...
|
|
'Fph', args.Fph, ...
|
|
'Fpr', args.Fpr, ...
|
|
'Mpm', args.Mpm, ...
|
|
'Mph', args.Mph, ...
|
|
'Mpr', args.Mpr);
|
|
|
|
stewart = initializeCylindricalStruts(stewart, ...
|
|
'Fsm', args.Fsm, ...
|
|
'Fsh', args.Fsh, ...
|
|
'Fsr', args.Fsr, ...
|
|
'Msm', args.Msm, ...
|
|
'Msh', args.Msh, ...
|
|
'Msr', args.Msr);
|
|
|
|
stewart = computeJacobian(stewart);
|
|
|
|
stewart = initializeStewartPose(stewart, ...
|
|
'AP', args.AP, ...
|
|
'ARB', args.ARB);
|
|
|
|
stewart = initializeInertialSensor(stewart, 'type', 'none');
|
|
|
|
switch args.type
|
|
case 'none'
|
|
stewart.type = 0;
|
|
case 'rigid'
|
|
stewart.type = 1;
|
|
case 'flexible'
|
|
stewart.type = 2;
|
|
end
|
|
|
|
micro_hexapod = stewart;
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nass_model_stages.mat', 'file')
|
|
save('mat/nass_model_stages.mat', 'micro_hexapod', '-append');
|
|
else
|
|
save('mat/nass_model_stages.mat', 'micro_hexapod');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nass_model_stages.mat', 'file')
|
|
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod', '-append');
|
|
else
|
|
save('matlab/mat/nass_model_stages.mat', 'micro_hexapod');
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** Stewart platform
|
|
*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeStewartPlatform.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Documentation
|
|
|
|
#+name: fig:stewart-frames-position
|
|
#+caption: Definition of the position of the frames
|
|
[[file:figs/stewart-frames-position.png]]
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeStewartPlatform()
|
|
% initializeStewartPlatform - Initialize the stewart structure
|
|
%
|
|
% Syntax: [stewart] = initializeStewartPlatform(args)
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following sub-structures:
|
|
% - platform_F -
|
|
% - platform_M -
|
|
% - joints_F -
|
|
% - joints_M -
|
|
% - struts_F -
|
|
% - struts_M -
|
|
% - actuators -
|
|
% - geometry -
|
|
% - properties -
|
|
#+end_src
|
|
|
|
**** Initialize the Stewart structure
|
|
#+begin_src matlab
|
|
stewart = struct();
|
|
stewart.platform_F = struct();
|
|
stewart.platform_M = struct();
|
|
stewart.joints_F = struct();
|
|
stewart.joints_M = struct();
|
|
stewart.struts_F = struct();
|
|
stewart.struts_M = struct();
|
|
stewart.actuators = struct();
|
|
stewart.sensors = struct();
|
|
stewart.sensors.inertial = struct();
|
|
stewart.sensors.force = struct();
|
|
stewart.sensors.relative = struct();
|
|
stewart.geometry = struct();
|
|
stewart.kinematics = struct();
|
|
#+end_src
|
|
|
|
*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeFramesPositions.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Documentation
|
|
|
|
#+name: fig:stewart-frames-position
|
|
#+caption: Definition of the position of the frames
|
|
[[file:figs/stewart-frames-position.png]]
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeFramesPositions(stewart, args)
|
|
% initializeFramesPositions - Initialize the positions of frames {A}, {B}, {F} and {M}
|
|
%
|
|
% Syntax: [stewart] = initializeFramesPositions(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - args - Can have the following fields:
|
|
% - H [1x1] - Total Height of the Stewart Platform (height from {F} to {M}) [m]
|
|
% - MO_B [1x1] - Height of the frame {B} with respect to {M} [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following fields:
|
|
% - geometry.H [1x1] - Total Height of the Stewart Platform [m]
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F} [m]
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M} [m]
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F} [m]
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
|
end
|
|
#+end_src
|
|
|
|
**** Compute the position of each frame
|
|
#+begin_src matlab
|
|
H = args.H; % Total Height of the Stewart Platform [m]
|
|
|
|
FO_M = [0; 0; H]; % Position of {M} with respect to {F} [m]
|
|
|
|
MO_B = [0; 0; args.MO_B]; % Position of {B} with respect to {M} [m]
|
|
|
|
FO_A = MO_B + FO_M; % Position of {A} with respect to {F} [m]
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.geometry.H = H;
|
|
stewart.geometry.FO_M = FO_M;
|
|
stewart.platform_M.MO_B = MO_B;
|
|
stewart.platform_F.FO_A = FO_A;
|
|
#+end_src
|
|
|
|
*** =generateGeneralConfiguration=: Generate a Very General Configuration
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/generateGeneralConfiguration.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Documentation
|
|
|
|
#+begin_src latex :file stewart_bottom_plate.pdf :tangle no
|
|
\begin{tikzpicture}
|
|
% Internal and external limit
|
|
\draw[fill=white!80!black] (0, 0) circle [radius=3];
|
|
% Circle where the joints are located
|
|
\draw[dashed] (0, 0) circle [radius=2.5];
|
|
|
|
% Bullets for the positions of the joints
|
|
\node[] (J1) at ( 80:2.5){$\bullet$};
|
|
\node[] (J2) at (100:2.5){$\bullet$};
|
|
\node[] (J3) at (200:2.5){$\bullet$};
|
|
\node[] (J4) at (220:2.5){$\bullet$};
|
|
\node[] (J5) at (320:2.5){$\bullet$};
|
|
\node[] (J6) at (340:2.5){$\bullet$};
|
|
|
|
% Name of the points
|
|
\node[above right] at (J1) {$a_{1}$};
|
|
\node[above left] at (J2) {$a_{2}$};
|
|
\node[above left] at (J3) {$a_{3}$};
|
|
\node[right ] at (J4) {$a_{4}$};
|
|
\node[left ] at (J5) {$a_{5}$};
|
|
\node[above right] at (J6) {$a_{6}$};
|
|
|
|
% First 2 angles
|
|
\draw[dashed, ->] (0:1) arc [start angle=0, end angle=80, radius=1] node[below right]{$\theta_{1}$};
|
|
\draw[dashed, ->] (0:1.5) arc [start angle=0, end angle=100, radius=1.5] node[left ]{$\theta_{2}$};
|
|
|
|
% Division of 360 degrees by 3
|
|
\draw[dashed] (0, 0) -- ( 80:3.2);
|
|
\draw[dashed] (0, 0) -- (100:3.2);
|
|
\draw[dashed] (0, 0) -- (200:3.2);
|
|
\draw[dashed] (0, 0) -- (220:3.2);
|
|
\draw[dashed] (0, 0) -- (320:3.2);
|
|
\draw[dashed] (0, 0) -- (340:3.2);
|
|
|
|
% Radius for the position of the joints
|
|
\draw[<->] (0, 0) --node[near end, above]{$R$} (180:2.5);
|
|
|
|
\draw[->] (0, 0) -- ++(3.4, 0) node[above]{$x$};
|
|
\draw[->] (0, 0) -- ++(0, 3.4) node[left]{$y$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = generateGeneralConfiguration(stewart, args)
|
|
% generateGeneralConfiguration - Generate a Very General Configuration
|
|
%
|
|
% Syntax: [stewart] = generateGeneralConfiguration(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - args - Can have the following fields:
|
|
% - FH [1x1] - Height of the position of the fixed joints with respect to the frame {F} [m]
|
|
% - FR [1x1] - Radius of the position of the fixed joints in the X-Y [m]
|
|
% - FTh [6x1] - Angles of the fixed joints in the X-Y plane with respect to the X axis [rad]
|
|
% - MH [1x1] - Height of the position of the mobile joints with respect to the frame {M} [m]
|
|
% - FR [1x1] - Radius of the position of the mobile joints in the X-Y [m]
|
|
% - MTh [6x1] - Angles of the mobile joints in the X-Y plane with respect to the X axis [rad]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 115e-3;
|
|
args.FTh (6,1) double {mustBeNumeric} = [-10, 10, 120-10, 120+10, 240-10, 240+10]*(pi/180);
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 90e-3;
|
|
args.MTh (6,1) double {mustBeNumeric} = [-60+10, 60-10, 60+10, 180-10, 180+10, -60-10]*(pi/180);
|
|
end
|
|
#+end_src
|
|
|
|
**** Compute the pose
|
|
#+begin_src matlab
|
|
Fa = zeros(3,6);
|
|
Mb = zeros(3,6);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
for i = 1:6
|
|
Fa(:,i) = [args.FR*cos(args.FTh(i)); args.FR*sin(args.FTh(i)); args.FH];
|
|
Mb(:,i) = [args.MR*cos(args.MTh(i)); args.MR*sin(args.MTh(i)); -args.MH];
|
|
end
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
#+end_src
|
|
|
|
*** =computeJointsPose=: Compute the Pose of the Joints
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/computeJointsPose.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Documentation
|
|
|
|
#+name: fig:stewart-struts
|
|
#+caption: Position and orientation of the struts
|
|
[[file:figs/stewart-struts.png]]
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = computeJointsPose(stewart)
|
|
% computeJointsPose -
|
|
%
|
|
% Syntax: [stewart] = computeJointsPose(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - platform_F.Fa [3x6] - Its i'th column is the position vector of joint ai with respect to {F}
|
|
% - platform_M.Mb [3x6] - Its i'th column is the position vector of joint bi with respect to {M}
|
|
% - platform_F.FO_A [3x1] - Position of {A} with respect to {F}
|
|
% - platform_M.MO_B [3x1] - Position of {B} with respect to {M}
|
|
% - geometry.FO_M [3x1] - Position of {M} with respect to {F}
|
|
%
|
|
% Outputs:
|
|
% - stewart - A structure with the following added fields
|
|
% - geometry.Aa [3x6] - The i'th column is the position of ai with respect to {A}
|
|
% - geometry.Ab [3x6] - The i'th column is the position of bi with respect to {A}
|
|
% - geometry.Ba [3x6] - The i'th column is the position of ai with respect to {B}
|
|
% - geometry.Bb [3x6] - The i'th column is the position of bi with respect to {B}
|
|
% - geometry.l [6x1] - The i'th element is the initial length of strut i
|
|
% - geometry.As [3x6] - The i'th column is the unit vector of strut i expressed in {A}
|
|
% - geometry.Bs [3x6] - The i'th column is the unit vector of strut i expressed in {B}
|
|
% - struts_F.l [6x1] - Length of the Fixed part of the i'th strut
|
|
% - struts_M.l [6x1] - Length of the Mobile part of the i'th strut
|
|
% - platform_F.FRa [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the bottom of the i'th strut from {F}
|
|
% - platform_M.MRb [3x3x6] - The i'th 3x3 array is the rotation matrix to orientate the top of the i'th strut from {M}
|
|
#+end_src
|
|
|
|
**** Check the =stewart= structure elements
|
|
#+begin_src matlab
|
|
assert(isfield(stewart.platform_F, 'Fa'), 'stewart.platform_F should have attribute Fa')
|
|
Fa = stewart.platform_F.Fa;
|
|
|
|
assert(isfield(stewart.platform_M, 'Mb'), 'stewart.platform_M should have attribute Mb')
|
|
Mb = stewart.platform_M.Mb;
|
|
|
|
assert(isfield(stewart.platform_F, 'FO_A'), 'stewart.platform_F should have attribute FO_A')
|
|
FO_A = stewart.platform_F.FO_A;
|
|
|
|
assert(isfield(stewart.platform_M, 'MO_B'), 'stewart.platform_M should have attribute MO_B')
|
|
MO_B = stewart.platform_M.MO_B;
|
|
|
|
assert(isfield(stewart.geometry, 'FO_M'), 'stewart.geometry should have attribute FO_M')
|
|
FO_M = stewart.geometry.FO_M;
|
|
#+end_src
|
|
|
|
**** Compute the position of the Joints
|
|
#+begin_src matlab
|
|
Aa = Fa - repmat(FO_A, [1, 6]);
|
|
Bb = Mb - repmat(MO_B, [1, 6]);
|
|
|
|
Ab = Bb - repmat(-MO_B-FO_M+FO_A, [1, 6]);
|
|
Ba = Aa - repmat( MO_B+FO_M-FO_A, [1, 6]);
|
|
#+end_src
|
|
|
|
**** Compute the strut length and orientation
|
|
#+begin_src matlab
|
|
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
|
|
|
l = vecnorm(Ab - Aa)';
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
|
#+end_src
|
|
|
|
**** Compute the orientation of the Joints
|
|
#+begin_src matlab
|
|
FRa = zeros(3,3,6);
|
|
MRb = zeros(3,3,6);
|
|
|
|
for i = 1:6
|
|
FRa(:,:,i) = [cross([0;1;0], As(:,i)) , cross(As(:,i), cross([0;1;0], As(:,i))) , As(:,i)];
|
|
FRa(:,:,i) = FRa(:,:,i)./vecnorm(FRa(:,:,i));
|
|
|
|
MRb(:,:,i) = [cross([0;1;0], Bs(:,i)) , cross(Bs(:,i), cross([0;1;0], Bs(:,i))) , Bs(:,i)];
|
|
MRb(:,:,i) = MRb(:,:,i)./vecnorm(MRb(:,:,i));
|
|
end
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.geometry.Aa = Aa;
|
|
stewart.geometry.Ab = Ab;
|
|
stewart.geometry.Ba = Ba;
|
|
stewart.geometry.Bb = Bb;
|
|
stewart.geometry.As = As;
|
|
stewart.geometry.Bs = Bs;
|
|
stewart.geometry.l = l;
|
|
|
|
stewart.struts_F.l = l/2;
|
|
stewart.struts_M.l = l/2;
|
|
|
|
stewart.platform_F.FRa = FRa;
|
|
stewart.platform_M.MRb = MRb;
|
|
#+end_src
|
|
|
|
*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeCylindricalPlatforms.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeCylindricalPlatforms(stewart, args)
|
|
% initializeCylindricalPlatforms - Initialize the geometry of the Fixed and Mobile Platforms
|
|
%
|
|
% Syntax: [stewart] = initializeCylindricalPlatforms(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - Fpm [1x1] - Fixed Platform Mass [kg]
|
|
% - Fph [1x1] - Fixed Platform Height [m]
|
|
% - Fpr [1x1] - Fixed Platform Radius [m]
|
|
% - Mpm [1x1] - Mobile Platform Mass [kg]
|
|
% - Mph [1x1] - Mobile Platform Height [m]
|
|
% - Mpr [1x1] - Mobile Platform Radius [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - platform_F [struct] - structure with the following fields:
|
|
% - type = 1
|
|
% - M [1x1] - Fixed Platform Mass [kg]
|
|
% - I [3x3] - Fixed Platform Inertia matrix [kg*m^2]
|
|
% - H [1x1] - Fixed Platform Height [m]
|
|
% - R [1x1] - Fixed Platform Radius [m]
|
|
% - platform_M [struct] - structure with the following fields:
|
|
% - M [1x1] - Mobile Platform Mass [kg]
|
|
% - I [3x3] - Mobile Platform Inertia matrix [kg*m^2]
|
|
% - H [1x1] - Mobile Platform Height [m]
|
|
% - R [1x1] - Mobile Platform Radius [m]
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 125e-3
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 1
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 100e-3
|
|
end
|
|
#+end_src
|
|
|
|
**** Compute the Inertia matrices of platforms
|
|
#+begin_src matlab
|
|
I_F = diag([1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
|
1/12*args.Fpm * (3*args.Fpr^2 + args.Fph^2), ...
|
|
1/2 *args.Fpm * args.Fpr^2]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
I_M = diag([1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
|
1/12*args.Mpm * (3*args.Mpr^2 + args.Mph^2), ...
|
|
1/2 *args.Mpm * args.Mpr^2]);
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.platform_F.type = 1;
|
|
|
|
stewart.platform_F.I = I_F;
|
|
stewart.platform_F.M = args.Fpm;
|
|
stewart.platform_F.R = args.Fpr;
|
|
stewart.platform_F.H = args.Fph;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
stewart.platform_M.type = 1;
|
|
|
|
stewart.platform_M.I = I_M;
|
|
stewart.platform_M.M = args.Mpm;
|
|
stewart.platform_M.R = args.Mpr;
|
|
stewart.platform_M.H = args.Mph;
|
|
#+end_src
|
|
|
|
*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeCylindricalStruts.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeCylindricalStruts(stewart, args)
|
|
% initializeCylindricalStruts - Define the mass and moment of inertia of cylindrical struts
|
|
%
|
|
% Syntax: [stewart] = initializeCylindricalStruts(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - Fsm [1x1] - Mass of the Fixed part of the struts [kg]
|
|
% - Fsh [1x1] - Height of cylinder for the Fixed part of the struts [m]
|
|
% - Fsr [1x1] - Radius of cylinder for the Fixed part of the struts [m]
|
|
% - Msm [1x1] - Mass of the Mobile part of the struts [kg]
|
|
% - Msh [1x1] - Height of cylinder for the Mobile part of the struts [m]
|
|
% - Msr [1x1] - Radius of cylinder for the Mobile part of the struts [m]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - struts_F [struct] - structure with the following fields:
|
|
% - M [6x1] - Mass of the Fixed part of the struts [kg]
|
|
% - I [3x3x6] - Moment of Inertia for the Fixed part of the struts [kg*m^2]
|
|
% - H [6x1] - Height of cylinder for the Fixed part of the struts [m]
|
|
% - R [6x1] - Radius of cylinder for the Fixed part of the struts [m]
|
|
% - struts_M [struct] - structure with the following fields:
|
|
% - M [6x1] - Mass of the Mobile part of the struts [kg]
|
|
% - I [3x3x6] - Moment of Inertia for the Mobile part of the struts [kg*m^2]
|
|
% - H [6x1] - Height of cylinder for the Mobile part of the struts [m]
|
|
% - R [6x1] - Radius of cylinder for the Mobile part of the struts [m]
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 0.1
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 50e-3
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3
|
|
end
|
|
#+end_src
|
|
|
|
**** Compute the properties of the cylindrical struts
|
|
|
|
#+begin_src matlab
|
|
Fsm = ones(6,1).*args.Fsm;
|
|
Fsh = ones(6,1).*args.Fsh;
|
|
Fsr = ones(6,1).*args.Fsr;
|
|
|
|
Msm = ones(6,1).*args.Msm;
|
|
Msh = ones(6,1).*args.Msh;
|
|
Msr = ones(6,1).*args.Msr;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
I_F = zeros(3, 3, 6); % Inertia of the "fixed" part of the strut
|
|
I_M = zeros(3, 3, 6); % Inertia of the "mobile" part of the strut
|
|
|
|
for i = 1:6
|
|
I_F(:,:,i) = diag([1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
|
1/12 * Fsm(i) * (3*Fsr(i)^2 + Fsh(i)^2), ...
|
|
1/2 * Fsm(i) * Fsr(i)^2]);
|
|
|
|
I_M(:,:,i) = diag([1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
|
1/12 * Msm(i) * (3*Msr(i)^2 + Msh(i)^2), ...
|
|
1/2 * Msm(i) * Msr(i)^2]);
|
|
end
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.struts_M.type = 1;
|
|
|
|
stewart.struts_M.I = I_M;
|
|
stewart.struts_M.M = Msm;
|
|
stewart.struts_M.R = Msr;
|
|
stewart.struts_M.H = Msh;
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
stewart.struts_F.type = 1;
|
|
|
|
stewart.struts_F.I = I_F;
|
|
stewart.struts_F.M = Fsm;
|
|
stewart.struts_F.R = Fsr;
|
|
stewart.struts_F.H = Fsh;
|
|
#+end_src
|
|
|
|
*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeStrutDynamics.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Documentation
|
|
|
|
#+name: fig:piezoelectric_stack
|
|
#+attr_html: :width 500px
|
|
#+caption: Example of a piezoelectric stach actuator (PI)
|
|
[[file:figs/piezoelectric_stack.jpg]]
|
|
|
|
A simplistic model of such amplified actuator is shown in Figure ref:fig:actuator_model_simple where:
|
|
- $K$ represent the vertical stiffness of the actuator
|
|
- $C$ represent the vertical damping of the actuator
|
|
- $F$ represents the force applied by the actuator
|
|
- $F_{m}$ represents the total measured force
|
|
- $v_{m}$ represents the absolute velocity of the top part of the actuator
|
|
- $d_{m}$ represents the total relative displacement of the actuator
|
|
|
|
#+begin_src latex :file actuator_model_simple.pdf :tangle no
|
|
\begin{tikzpicture}
|
|
\draw (-1, 0) -- (1, 0);
|
|
|
|
% Spring, Damper, and Actuator
|
|
\draw[spring] (-1, 0) -- (-1, 1.5) node[midway, left=0.1]{$K$};
|
|
\draw[damper] ( 0, 0) -- ( 0, 1.5) node[midway, left=0.2]{$C$};
|
|
\draw[actuator] ( 1, 0) -- ( 1, 1.5) node[midway, left=0.1](F){$F$};
|
|
|
|
\node[forcesensor={2}{0.2}] (fsens) at (0, 1.5){};
|
|
|
|
\node[left] at (fsens.west) {$F_{m}$};
|
|
|
|
\draw[dashed] (1, 0) -- ++(0.4, 0);
|
|
\draw[dashed] (1, 1.7) -- ++(0.4, 0);
|
|
|
|
\draw[->] (0, 1.7)node[]{$\bullet$} -- ++(0, 0.5) node[right]{$v_{m}$};
|
|
|
|
\draw[<->] (1.4, 0) -- ++(0, 1.7) node[midway, right]{$d_{m}$};
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:actuator_model_simple
|
|
#+caption: Simple model of an Actuator
|
|
#+RESULTS:
|
|
[[file:figs/actuator_model_simple.png]]
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeStrutDynamics(stewart, args)
|
|
% initializeStrutDynamics - Add Stiffness and Damping properties of each strut
|
|
%
|
|
% Syntax: [stewart] = initializeStrutDynamics(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - K [6x1] - Stiffness of each strut [N/m]
|
|
% - C [6x1] - Damping of each strut [N/(m/s)]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - actuators.type = 1
|
|
% - actuators.K [6x1] - Stiffness of each strut [N/m]
|
|
% - actuators.C [6x1] - Damping of each strut [N/(m/s)]
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{'classical', 'amplified'})} = 'classical'
|
|
args.K (6,1) double {mustBeNumeric, mustBeNonnegative} = 20e6*ones(6,1)
|
|
args.C (6,1) double {mustBeNumeric, mustBeNonnegative} = 2e1*ones(6,1)
|
|
args.k1 (6,1) double {mustBeNumeric} = 1e6*ones(6,1)
|
|
args.ke (6,1) double {mustBeNumeric} = 5e6*ones(6,1)
|
|
args.ka (6,1) double {mustBeNumeric} = 60e6*ones(6,1)
|
|
args.c1 (6,1) double {mustBeNumeric} = 10*ones(6,1)
|
|
args.F_gain (6,1) double {mustBeNumeric} = 1*ones(6,1)
|
|
args.me (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
|
args.ma (6,1) double {mustBeNumeric} = 0.01*ones(6,1)
|
|
end
|
|
#+end_src
|
|
|
|
**** Add Stiffness and Damping properties of each strut
|
|
#+begin_src matlab
|
|
if strcmp(args.type, 'classical')
|
|
stewart.actuators.type = 1;
|
|
elseif strcmp(args.type, 'amplified')
|
|
stewart.actuators.type = 2;
|
|
end
|
|
|
|
stewart.actuators.K = args.K;
|
|
stewart.actuators.C = args.C;
|
|
|
|
stewart.actuators.k1 = args.k1;
|
|
stewart.actuators.c1 = args.c1;
|
|
|
|
stewart.actuators.ka = args.ka;
|
|
stewart.actuators.ke = args.ke;
|
|
|
|
stewart.actuators.F_gain = args.F_gain;
|
|
|
|
stewart.actuators.ma = args.ma;
|
|
stewart.actuators.me = args.me;
|
|
#+end_src
|
|
|
|
*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeJointDynamics.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeJointDynamics(stewart, args)
|
|
% initializeJointDynamics - Add Stiffness and Damping properties for the spherical joints
|
|
%
|
|
% Syntax: [stewart] = initializeJointDynamics(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - type_F - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
|
% - type_M - 'universal', 'spherical', 'universal_p', 'spherical_p'
|
|
% - Kf_M [6x1] - Bending (Rx, Ry) Stiffness for each top joints [(N.m)/rad]
|
|
% - Kt_M [6x1] - Torsion (Rz) Stiffness for each top joints [(N.m)/rad]
|
|
% - Cf_M [6x1] - Bending (Rx, Ry) Damping of each top joint [(N.m)/(rad/s)]
|
|
% - Ct_M [6x1] - Torsion (Rz) Damping of each top joint [(N.m)/(rad/s)]
|
|
% - Kf_F [6x1] - Bending (Rx, Ry) Stiffness for each bottom joints [(N.m)/rad]
|
|
% - Kt_F [6x1] - Torsion (Rz) Stiffness for each bottom joints [(N.m)/rad]
|
|
% - Cf_F [6x1] - Bending (Rx, Ry) Damping of each bottom joint [(N.m)/(rad/s)]
|
|
% - Cf_F [6x1] - Torsion (Rz) Damping of each bottom joint [(N.m)/(rad/s)]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - stewart.joints_F and stewart.joints_M:
|
|
% - type - 1 (universal), 2 (spherical), 3 (universal perfect), 4 (spherical perfect)
|
|
% - Kx, Ky, Kz [6x1] - Translation (Tx, Ty, Tz) Stiffness [N/m]
|
|
% - Kf [6x1] - Flexion (Rx, Ry) Stiffness [(N.m)/rad]
|
|
% - Kt [6x1] - Torsion (Rz) Stiffness [(N.m)/rad]
|
|
% - Cx, Cy, Cz [6x1] - Translation (Rx, Ry) Damping [N/(m/s)]
|
|
% - Cf [6x1] - Flexion (Rx, Ry) Damping [(N.m)/(rad/s)]
|
|
% - Cb [6x1] - Torsion (Rz) Damping [(N.m)/(rad/s)]
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.type_F char {mustBeMember(args.type_F,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'universal'
|
|
args.type_M char {mustBeMember(args.type_M,{'universal', 'spherical', 'universal_p', 'spherical_p', 'universal_3dof', 'spherical_3dof', 'flexible'})} = 'spherical'
|
|
args.Kf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
|
args.Cf_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
|
args.Ct_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Kf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 33*ones(6,1)
|
|
args.Cf_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-4*ones(6,1)
|
|
args.Kt_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 236*ones(6,1)
|
|
args.Ct_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e-3*ones(6,1)
|
|
args.Ka_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
|
args.Ca_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Kr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
|
args.Cr_F (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Ka_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.2e8*ones(6,1)
|
|
args.Ca_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.Kr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1.1e7*ones(6,1)
|
|
args.Cr_M (6,1) double {mustBeNumeric, mustBeNonnegative} = 1e1*ones(6,1)
|
|
args.K_M double {mustBeNumeric} = zeros(6,6)
|
|
args.M_M double {mustBeNumeric} = zeros(6,6)
|
|
args.n_xyz_M double {mustBeNumeric} = zeros(2,3)
|
|
args.xi_M double {mustBeNumeric} = 0.1
|
|
args.step_file_M char {} = ''
|
|
args.K_F double {mustBeNumeric} = zeros(6,6)
|
|
args.M_F double {mustBeNumeric} = zeros(6,6)
|
|
args.n_xyz_F double {mustBeNumeric} = zeros(2,3)
|
|
args.xi_F double {mustBeNumeric} = 0.1
|
|
args.step_file_F char {} = ''
|
|
end
|
|
#+end_src
|
|
|
|
**** Add Actuator Type
|
|
#+begin_src matlab
|
|
switch args.type_F
|
|
case 'universal'
|
|
stewart.joints_F.type = 1;
|
|
case 'spherical'
|
|
stewart.joints_F.type = 2;
|
|
case 'universal_p'
|
|
stewart.joints_F.type = 3;
|
|
case 'spherical_p'
|
|
stewart.joints_F.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_F.type = 5;
|
|
case 'universal_3dof'
|
|
stewart.joints_F.type = 6;
|
|
case 'spherical_3dof'
|
|
stewart.joints_F.type = 7;
|
|
end
|
|
|
|
switch args.type_M
|
|
case 'universal'
|
|
stewart.joints_M.type = 1;
|
|
case 'spherical'
|
|
stewart.joints_M.type = 2;
|
|
case 'universal_p'
|
|
stewart.joints_M.type = 3;
|
|
case 'spherical_p'
|
|
stewart.joints_M.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_M.type = 5;
|
|
case 'universal_3dof'
|
|
stewart.joints_M.type = 6;
|
|
case 'spherical_3dof'
|
|
stewart.joints_M.type = 7;
|
|
end
|
|
#+end_src
|
|
|
|
**** Add Stiffness and Damping in Translation of each strut
|
|
Axial and Radial (shear) Stiffness
|
|
#+begin_src matlab
|
|
stewart.joints_M.Ka = args.Ka_M;
|
|
stewart.joints_M.Kr = args.Kr_M;
|
|
|
|
stewart.joints_F.Ka = args.Ka_F;
|
|
stewart.joints_F.Kr = args.Kr_F;
|
|
#+end_src
|
|
|
|
Translation Damping
|
|
#+begin_src matlab
|
|
stewart.joints_M.Ca = args.Ca_M;
|
|
stewart.joints_M.Cr = args.Cr_M;
|
|
|
|
stewart.joints_F.Ca = args.Ca_F;
|
|
stewart.joints_F.Cr = args.Cr_F;
|
|
#+end_src
|
|
|
|
**** Add Stiffness and Damping in Rotation of each strut
|
|
Rotational Stiffness
|
|
#+begin_src matlab
|
|
stewart.joints_M.Kf = args.Kf_M;
|
|
stewart.joints_M.Kt = args.Kt_M;
|
|
|
|
stewart.joints_F.Kf = args.Kf_F;
|
|
stewart.joints_F.Kt = args.Kt_F;
|
|
#+end_src
|
|
|
|
Rotational Damping
|
|
#+begin_src matlab
|
|
stewart.joints_M.Cf = args.Cf_M;
|
|
stewart.joints_M.Ct = args.Ct_M;
|
|
|
|
stewart.joints_F.Cf = args.Cf_F;
|
|
stewart.joints_F.Ct = args.Ct_F;
|
|
#+end_src
|
|
|
|
**** Stiffness and Mass matrices for flexible joint
|
|
|
|
#+begin_src matlab
|
|
stewart.joints_F.M = args.M_F;
|
|
stewart.joints_F.K = args.K_F;
|
|
stewart.joints_F.n_xyz = args.n_xyz_F;
|
|
stewart.joints_F.xi = args.xi_F;
|
|
stewart.joints_F.xi = args.xi_F;
|
|
stewart.joints_F.step_file = args.step_file_F;
|
|
|
|
stewart.joints_M.M = args.M_M;
|
|
stewart.joints_M.K = args.K_M;
|
|
stewart.joints_M.n_xyz = args.n_xyz_M;
|
|
stewart.joints_M.xi = args.xi_M;
|
|
stewart.joints_M.step_file = args.step_file_M;
|
|
#+end_src
|
|
|
|
*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeStewartPose.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeStewartPose(stewart, args)
|
|
% initializeStewartPose - Determine the initial stroke in each leg to have the wanted pose
|
|
% It uses the inverse kinematic
|
|
%
|
|
% Syntax: [stewart] = initializeStewartPose(stewart, args)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - Aa [3x6] - The positions ai expressed in {A}
|
|
% - Bb [3x6] - The positions bi expressed in {B}
|
|
% - args - Can have the following fields:
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - actuators.Leq [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
#+end_src
|
|
|
|
**** Use the Inverse Kinematic function
|
|
#+begin_src matlab
|
|
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.actuators.Leq = dLi;
|
|
#+end_src
|
|
|
|
*** =computeJacobian=: Compute the Jacobian Matrix
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/computeJacobian.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = computeJacobian(stewart)
|
|
% computeJacobian -
|
|
%
|
|
% Syntax: [stewart] = computeJacobian(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - With at least the following fields:
|
|
% - geometry.As [3x6] - The 6 unit vectors for each strut expressed in {A}
|
|
% - geometry.Ab [3x6] - The 6 position of the joints bi expressed in {A}
|
|
% - actuators.K [6x1] - Total stiffness of the actuators
|
|
%
|
|
% Outputs:
|
|
% - stewart - With the 3 added field:
|
|
% - kinematics.J [6x6] - The Jacobian Matrix
|
|
% - kinematics.K [6x6] - The Stiffness Matrix
|
|
% - kinematics.C [6x6] - The Compliance Matrix
|
|
#+end_src
|
|
|
|
**** Check the =stewart= structure elements
|
|
#+begin_src matlab
|
|
assert(isfield(stewart.geometry, 'As'), 'stewart.geometry should have attribute As')
|
|
As = stewart.geometry.As;
|
|
|
|
assert(isfield(stewart.geometry, 'Ab'), 'stewart.geometry should have attribute Ab')
|
|
Ab = stewart.geometry.Ab;
|
|
|
|
assert(isfield(stewart.actuators, 'K'), 'stewart.actuators should have attribute K')
|
|
Ki = stewart.actuators.K;
|
|
#+end_src
|
|
|
|
|
|
**** Compute Jacobian Matrix
|
|
#+begin_src matlab
|
|
J = [As' , cross(Ab, As)'];
|
|
#+end_src
|
|
|
|
**** Compute Stiffness Matrix
|
|
#+begin_src matlab
|
|
K = J'*diag(Ki)*J;
|
|
#+end_src
|
|
|
|
**** Compute Compliance Matrix
|
|
#+begin_src matlab
|
|
C = inv(K);
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.kinematics.J = J;
|
|
stewart.kinematics.K = K;
|
|
stewart.kinematics.C = C;
|
|
#+end_src
|
|
|
|
*** =initializeInertialSensor=: Initialize the inertial sensor in each strut
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/initializeInertialSensor.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Geophone - Working Principle
|
|
From the schematic of the Z-axis geophone shown in Figure ref:fig:z_axis_geophone, we can write the transfer function from the support velocity $\dot{w}$ to the relative velocity of the inertial mass $\dot{d}$:
|
|
\[ \frac{\dot{d}}{\dot{w}} = \frac{-\frac{s^2}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
|
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
|
|
|
#+name: fig:z_axis_geophone
|
|
#+caption: Schematic of a Z-Axis geophone
|
|
[[file:figs/inertial_sensor.png]]
|
|
|
|
We see that at frequencies above $\omega_0$:
|
|
\[ \frac{\dot{d}}{\dot{w}} \approx -1 \]
|
|
|
|
And thus, the measurement of the relative velocity of the mass with respect to its support gives the absolute velocity of the support.
|
|
|
|
We generally want to have the smallest resonant frequency $\omega_0$ to measure low frequency absolute velocity, however there is a trade-off between $\omega_0$ and the mass of the inertial mass.
|
|
|
|
**** Accelerometer - Working Principle
|
|
From the schematic of the Z-axis accelerometer shown in Figure ref:fig:z_axis_accelerometer, we can write the transfer function from the support acceleration $\ddot{w}$ to the relative position of the inertial mass $d$:
|
|
\[ \frac{d}{\ddot{w}} = \frac{-\frac{1}{{\omega_0}^2}}{\frac{s^2}{{\omega_0}^2} + 2 \xi \frac{s}{\omega_0} + 1} \]
|
|
with:
|
|
- $\omega_0 = \sqrt{\frac{k}{m}}$
|
|
- $\xi = \frac{1}{2} \sqrt{\frac{m}{k}}$
|
|
|
|
#+name: fig:z_axis_accelerometer
|
|
#+caption: Schematic of a Z-Axis geophone
|
|
[[file:figs/inertial_sensor.png]]
|
|
|
|
We see that at frequencies below $\omega_0$:
|
|
\[ \frac{d}{\ddot{w}} \approx -\frac{1}{{\omega_0}^2} \]
|
|
|
|
And thus, the measurement of the relative displacement of the mass with respect to its support gives the absolute acceleration of the support.
|
|
|
|
Note that there is trade-off between:
|
|
- the highest measurable acceleration $\omega_0$
|
|
- the sensitivity of the accelerometer which is equal to $-\frac{1}{{\omega_0}^2}$
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [stewart] = initializeInertialSensor(stewart, args)
|
|
% initializeInertialSensor - Initialize the inertial sensor in each strut
|
|
%
|
|
% Syntax: [stewart] = initializeInertialSensor(args)
|
|
%
|
|
% Inputs:
|
|
% - args - Structure with the following fields:
|
|
% - type - 'geophone', 'accelerometer', 'none'
|
|
% - mass [1x1] - Weight of the inertial mass [kg]
|
|
% - freq [1x1] - Cutoff frequency [Hz]
|
|
%
|
|
% Outputs:
|
|
% - stewart - updated Stewart structure with the added fields:
|
|
% - stewart.sensors.inertial
|
|
% - type - 1 (geophone), 2 (accelerometer), 3 (none)
|
|
% - K [1x1] - Stiffness [N/m]
|
|
% - C [1x1] - Damping [N/(m/s)]
|
|
% - M [1x1] - Inertial Mass [kg]
|
|
% - G [1x1] - Gain
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{'geophone', 'accelerometer', 'none'})} = 'none'
|
|
args.mass (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e-2
|
|
args.freq (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e3
|
|
end
|
|
#+end_src
|
|
|
|
**** Compute the properties of the sensor
|
|
#+begin_src matlab
|
|
sensor = struct();
|
|
|
|
switch args.type
|
|
case 'geophone'
|
|
sensor.type = 1;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
|
case 'accelerometer'
|
|
sensor.type = 2;
|
|
|
|
sensor.M = args.mass;
|
|
sensor.K = sensor.M * (2*pi*args.freq)^2;
|
|
sensor.C = 2*sqrt(sensor.M * sensor.K);
|
|
sensor.G = -sensor.K/sensor.M;
|
|
case 'none'
|
|
sensor.type = 3;
|
|
end
|
|
#+end_src
|
|
|
|
**** Populate the =stewart= structure
|
|
#+begin_src matlab
|
|
stewart.sensors.inertial = sensor;
|
|
#+end_src
|
|
|
|
|
|
|
|
*** =inverseKinematics=: Compute Inverse Kinematics
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/inverseKinematics.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
|
|
**** Theory
|
|
For inverse kinematic analysis, it is assumed that the position ${}^A\bm{P}$ and orientation of the moving platform ${}^A\bm{R}_B$ are given and the problem is to obtain the joint variables, namely, $\bm{L} = [l_1, l_2, \dots, l_6]^T$.
|
|
|
|
From the geometry of the manipulator, the loop closure for each limb, $i = 1, 2, \dots, 6$ can be written as
|
|
\begin{align*}
|
|
l_i {}^A\hat{\bm{s}}_i &= {}^A\bm{A} + {}^A\bm{b}_i - {}^A\bm{a}_i \\
|
|
&= {}^A\bm{A} + {}^A\bm{R}_b {}^B\bm{b}_i - {}^A\bm{a}_i
|
|
\end{align*}
|
|
|
|
To obtain the length of each actuator and eliminate $\hat{\bm{s}}_i$, it is sufficient to dot multiply each side by itself:
|
|
\begin{equation}
|
|
l_i^2 \left[ {}^A\hat{\bm{s}}_i^T {}^A\hat{\bm{s}}_i \right] = \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]^T \left[ {}^A\bm{P} + {}^A\bm{R}_B {}^B\bm{b}_i - {}^A\bm{a}_i \right]
|
|
\end{equation}
|
|
|
|
Hence, for $i = 1, 2, \dots, 6$, each limb length can be uniquely determined by:
|
|
\begin{equation}
|
|
l_i = \sqrt{{}^A\bm{P}^T {}^A\bm{P} + {}^B\bm{b}_i^T {}^B\bm{b}_i + {}^A\bm{a}_i^T {}^A\bm{a}_i - 2 {}^A\bm{P}^T {}^A\bm{a}_i + 2 {}^A\bm{P}^T \left[{}^A\bm{R}_B {}^B\bm{b}_i\right] - 2 \left[{}^A\bm{R}_B {}^B\bm{b}_i\right]^T {}^A\bm{a}_i}
|
|
\end{equation}
|
|
|
|
If the position and orientation of the moving platform lie in the feasible workspace of the manipulator, one unique solution to the limb length is determined by the above equation.
|
|
Otherwise, when the limbs' lengths derived yield complex numbers, then the position or orientation of the moving platform is not reachable.
|
|
|
|
**** Function description
|
|
#+begin_src matlab
|
|
function [Li, dLi] = inverseKinematics(stewart, args)
|
|
% inverseKinematics - Compute the needed length of each strut to have the wanted position and orientation of {B} with respect to {A}
|
|
%
|
|
% Syntax: [stewart] = inverseKinematics(stewart)
|
|
%
|
|
% Inputs:
|
|
% - stewart - A structure with the following fields
|
|
% - geometry.Aa [3x6] - The positions ai expressed in {A}
|
|
% - geometry.Bb [3x6] - The positions bi expressed in {B}
|
|
% - geometry.l [6x1] - Length of each strut
|
|
% - args - Can have the following fields:
|
|
% - AP [3x1] - The wanted position of {B} with respect to {A}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
%
|
|
% Outputs:
|
|
% - Li [6x1] - The 6 needed length of the struts in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
% - dLi [6x1] - The 6 needed displacement of the struts from the initial position in [m] to have the wanted pose of {B} w.r.t. {A}
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
#+end_src
|
|
|
|
**** Check the =stewart= structure elements
|
|
#+begin_src matlab
|
|
assert(isfield(stewart.geometry, 'Aa'), 'stewart.geometry should have attribute Aa')
|
|
Aa = stewart.geometry.Aa;
|
|
|
|
assert(isfield(stewart.geometry, 'Bb'), 'stewart.geometry should have attribute Bb')
|
|
Bb = stewart.geometry.Bb;
|
|
|
|
assert(isfield(stewart.geometry, 'l'), 'stewart.geometry should have attribute l')
|
|
l = stewart.geometry.l;
|
|
#+end_src
|
|
|
|
|
|
**** Compute
|
|
#+begin_src matlab
|
|
Li = sqrt(args.AP'*args.AP + diag(Bb'*Bb) + diag(Aa'*Aa) - (2*args.AP'*Aa)' + (2*args.AP'*(args.ARB*Bb))' - diag(2*(args.ARB*Bb)'*Aa));
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
dLi = Li-l;
|
|
#+end_src
|
|
|
|
* Footnotes
|
|
|
|
[fn:12]It was probably caused by rust of the linear guides along its stroke.
|
|
[fn:11]A 3-Axis L4C geophone manufactured Sercel was used.
|
|
[fn:10]Laser source is manufactured by Agilent (5519b).
|
|
[fn:9]The special optics (straightness interferometer and reflector) are manufactured by Agilent (10774A).
|
|
[fn:8]C8 capacitive sensors and CPL290 capacitive driver electronics from Lion Precision.
|
|
[fn:7]The Spindle Error Analyzer is made by Lion Precision.
|
|
[fn:6]The tools presented here are largely taken from [[cite:&taghirad13_paral]].
|
|
[fn:5]Rotations are non commutative in 3D.
|
|
[fn:4]Ball cage (N501) and guide bush (N550) from Mahr are used.
|
|
[fn:3]Modified Zonda Hexapod by Symetrie.
|
|
[fn:2]Made by LAB Motion Systems.
|
|
[fn:1]HCR 35 A C1, from THK.
|