3565 lines
157 KiB
Org Mode
3565 lines
157 KiB
Org Mode
#+TITLE: Simscape Model - Nano Hexapod
|
|
: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-nano-hexapod.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 =nhexa=
|
|
|
|
Based on:
|
|
- [ ] Stewart platform presentation: [[file:~/Cloud/meetings/group-meetings-me/2020-01-27-Stewart-Platform-Simscape/2020-01-27-Stewart-Platform-Simscape.org]]
|
|
- [ ] Add some sections from here: [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/index.org]]
|
|
For instance:
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/stewart-architecture.org][stewart architecture]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/stewart_platform.org::+TITLE: Stewart Platform - Simscape Model]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org][kinematic study]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/identification.org][stewart platform - dynamics]]
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/cubic-configuration.org][cubic configuration]]
|
|
- [ ] Look at the [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][NASS 2020 report]]
|
|
Sections 5.1, 5.4
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/amplified_piezoelectric_stack.org][amplified_piezoelectric_stack]] (Just use 2DoF here)
|
|
- [ ] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/nano_hexapod.org][nano_hexapod]] (it seems this report is already after the detailed design phase: yes but some parts could be interesting)
|
|
- [ ] Should the study of effect of flexible joints be included here?
|
|
- [X] file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-vibration-isolation.org
|
|
|
|
Questions:
|
|
- [ ] The APA model should maybe not be used here, same for the nice top and bottom plates. Here the detailed design is not yet performed
|
|
|
|
** DONE [#A] Copy relevant parts of reports
|
|
CLOSED: [2025-02-06 Thu 15:27]
|
|
|
|
- [X] Stewart platform presentation: [[file:~/Cloud/meetings/group-meetings-me/2020-01-27-Stewart-Platform-Simscape/2020-01-27-Stewart-Platform-Simscape.org]]
|
|
- [X] Add some sections from here: [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/index.org]]
|
|
For instance:
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/stewart-architecture.org][stewart architecture]]
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/stewart_platform.org::+TITLE: Stewart Platform - Simscape Model]]
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/kinematic-study.org][kinematic study]]
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/identification.org]]
|
|
Effect of joints stiffnesses
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/cubic-configuration.org][cubic configuration]]
|
|
Not relevant here: in chapter 2
|
|
- [X] Look at the [[file:~/Cloud/work-projects/ID31-NASS/documents/state-of-thesis-2020/index.org][NASS 2020 report]]
|
|
Sections 5.1, 5.4
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/amplified_piezoelectric_stack.org][amplified_piezoelectric_stack]] (Just use 2DoF here)
|
|
- [X] [[file:~/Cloud/work-projects/ID31-NASS/matlab/nass-simscape/org/nano_hexapod.org][nano_hexapod]] (it seems this report is already after the detailed design phase: yes but some parts could be interesting)
|
|
*Will also be used in Chapter 2*
|
|
- [X] Should the study of effect of flexible joints be included here?
|
|
*No, considered perfect and then optimized in chapter 2*
|
|
- [X] file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/control-vibration-isolation.org
|
|
|
|
** DONE [#A] Make a nice outline
|
|
CLOSED: [2025-02-05 Wed 17:45]
|
|
|
|
*Introduction*
|
|
- Choice of architecture to do 5DoF control
|
|
- Stewart platform
|
|
- Need to model the active vibration platform
|
|
- Control
|
|
|
|
*1 - Active Vibration Platforms*:
|
|
Introduction:
|
|
Maybe no sections, just a review discussing several aspect of the platforms.
|
|
1. Review of active vibration platforms (focused on Synchrotron applications)
|
|
2. Serial and Parallel Architecture: advantages and disadvantages of both
|
|
3. Which architecture => Parallel manipulator? Why *Stewart platform*?
|
|
|
|
*2 - The Stewart Platform*:
|
|
Introduction: some history about Stewart platform and why it is so used
|
|
1. Architecture (plates, struts, joints)
|
|
2. Kinematics and Jacobian
|
|
4. Static Analysis
|
|
5. Dynamic Analysis: very complex => multi-body model
|
|
For instance, compute the plant for massless struts and perfect joints (will be compared with Simscape model).
|
|
But say that if we want to model more complex cases, it becomes impractical (cite papers).
|
|
|
|
*3 - Multi-Body model of the Stewart platform*:
|
|
Introduction: Complex dynamics => analytical formulas can be complex => Choose to study the dynamics using a multi-body model
|
|
1. Model definition: (Matlab Toolbox), frames, inertias of parts, stiffnesses, struts, etc...
|
|
2. Joints: perfect 2dof/3dof (+ mass-less)
|
|
3. Actuators: APA + Encoder (mass-less)
|
|
4. Nano-Hexapod: definition of each part + Plant with defined inputs/outputs (force sensor, relative displacement sensor, etc...)
|
|
Compare with analytical formulas (see number of states)
|
|
|
|
*4 - Control of the Stewart Platform*:
|
|
Introduction: MIMO control => much more complex than SISO control because of interaction. Possible to ignore interaction when good decoupling (important to have tools to study interaction)
|
|
1. Centralized and Decentralized Control
|
|
2. Decoupling Control / Choice of control space file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org
|
|
Estimate coupling: RGA
|
|
- Jacobian matrices, CoK, CoM, control in the frame of the struts, ...
|
|
- Discussion of cubic architecture (quick, as it is going to be in detailed in chapter 2)
|
|
- SVD, Modal, ...
|
|
3. Active Damping: decentralized IFF
|
|
Guaranteed stability?
|
|
For decentralized control: "MIMO root locus"
|
|
How to optimize the added damping to all modes?
|
|
4. HAC-LAC
|
|
Stability of closed-loop: Nyquist (main advantage: possible to do with experimental FRF)
|
|
|
|
*Conclusion*:
|
|
- Configurable Stewart platform model
|
|
- Will be included in the multi-body model of the micro-station => nass multi body model
|
|
|
|
** DONE [#A] Location of this report in the complete thesis
|
|
CLOSED: [2025-02-05 Wed 16:04]
|
|
|
|
*Before the report* (assumptions):
|
|
- Uniaxial model: no stiff actuator, HAC-LAC strategy
|
|
- Rotating model:
|
|
Soft actuators are problematic due to gyroscopic effects
|
|
Use moderately stiff (1um/N).
|
|
IFF can be applied with APA architecture
|
|
- Model of Micro-station is ready
|
|
|
|
*In this report*:
|
|
- Goal: build a flexible (i.e. configurable) multi-body model of a Stewart platform that will be used in the next section to perform dynamical analysis and simulate experiments with the complete NASS
|
|
- Here, I propose to work with "perfect" stewart platforms:
|
|
- almost mass-less struts
|
|
- joints with zero stiffness in free DoFs (i.e. 2-DoF and 3-DoF joints)
|
|
- Presentation of Stewart platforms (Literature review about stewart platforms will be done in chapter 2)
|
|
- Presentation of the Simscape model
|
|
|
|
*After the report* (NASS-Simscape):
|
|
- nano-hexapod on top of micro-station
|
|
- control is performed
|
|
- simulations => validation of the concept
|
|
|
|
** ANSW [#A] Should I talk about APA here?
|
|
CLOSED: [2025-02-11 Tue 23:00]
|
|
|
|
- It seems APA is not mentioned in chapter 1
|
|
- Could be nice to only talk about APA in *chapter 2* as in chapter 1 we don't care too much about actual implementation/design
|
|
|
|
- [ ] Should the NASS be validated using 1 DoF actuators?
|
|
How to deal with IFF?
|
|
- *Add parallel stiffness*
|
|
- High pass filter
|
|
|
|
** DONE [#A] Check all figures
|
|
CLOSED: [2025-02-12 Wed 10:30]
|
|
|
|
- [ ] Captions
|
|
- [ ] Legend, units, etc...
|
|
|
|
** DONE [#A] Make sure the Simulink file for the Stewart platform is working well
|
|
CLOSED: [2025-02-12 Wed 10:30] SCHEDULED: <2025-02-10 Mon>
|
|
|
|
It should be the exact model reference that will be included in the NASS model (referenced subsystem).
|
|
|
|
- [X] Check what was already done for the toolbox
|
|
- [X] Same parameters for the APA as in previous model (1N/um ?)
|
|
*kn = 1N/um*
|
|
nano hexapod mass: *15kg*
|
|
cn = 2*0.01*sqrt((ms + mn)*kn)
|
|
=> depends on the sample's mass: between 30 and 60, *cn = 50N/(m/s) seems reasonable*
|
|
*real mass of the top platform is 5kg*
|
|
- [X] Use similar strategy that for the NASS simulation (these .mat files, etc.)
|
|
- [X] Similar interfaces: {F}, {M},
|
|
- inputs: 6 actuator inputs
|
|
- output 1: 6 encoders
|
|
- output 2: 6 force sensors
|
|
- [X] joints configurable with
|
|
- [X] 2dof
|
|
- [X] 3dof
|
|
- [X] 4dof
|
|
- [X] flexible => will be added for chapter 2
|
|
- [X] actuator:
|
|
- [X] 1dof
|
|
- [X] 2dof (APA)
|
|
- [X] FEM => will be added for chapter 2
|
|
- [X] plates: cylindrical or .STEP
|
|
Only cylindrical for now
|
|
- [X] Add payload:
|
|
- size: height, diameter/radius
|
|
- Weight
|
|
- [X] Control configuration
|
|
- [X] Log configuration
|
|
- [X] *Do I want to be able to change each individual parameter value of each strut => no*
|
|
|
|
** TODO [#A] For simplicity, maybe not talk at all about parallel stiffness with the force sensor
|
|
This could be the topic of the NASS section.
|
|
|
|
** DONE [#B] Check all notations
|
|
CLOSED: [2025-02-12 Wed 10:42]
|
|
|
|
- [ ] Make sure they are all defined in correct order
|
|
- [ ] Make sure all vectors and matrices are bold
|
|
|
|
| | uniaxial model | rotating model | HERE | Experimental |
|
|
|----------------------+----------------+----------------+-----------------------------------------------------+-----------------------------------------------------|
|
|
| Strut force | $f$ | $F_u$, $F_v$ | $\bm{f}$, $f_i$ | plant input $\bm{u}$, stack voltages $\bm{V}_a$ |
|
|
| Strut displacement | $d\mathcal{L}$ | $d_u$, $d_v$ | $\bm{\mathcal{L}}$, $l_i$ | |
|
|
| Error in strut frame | | | $\bm{\epsilon\mathcal{L}}$, $\epsilon\mathcal{L}_i$ | $\bm{\epsilon\mathcal{L}}$, $\epsilon\mathcal{L}_i$ |
|
|
| Encoder | - | | | $\bm{d}_e$ |
|
|
| Force Sensor | $f_n$ | $f_u$, $f_v$ | $\bm{f}_n$ | voltages: $\bm{V}_s$ |
|
|
| Sample motion | $d$ | | $\bm{\mathcal{X}}$ | $D_x$, ..., $R_y$ |
|
|
| Damped plant input | $u^\prime$ | | $\bm{f}^\prime$ | $\mathcal{u}^\prime$ |
|
|
|
|
Conclusion:
|
|
- [X] Actuator forces: $\bm{\tau}$ => $\bm{f}$ and $f_i$
|
|
- [X] Strut displacement: $d_L$ => $\bm{\mathcal{L}}$ and $l_i$
|
|
- [X] Force sensor: $f_m$ => $\bm{f}_n$ and $f_{ni}$
|
|
|
|
** DONE [#B] Remove all un-used control architecture
|
|
CLOSED: [2025-02-12 Wed 10:42]
|
|
|
|
- [ ] Make sure HAC-IFF works as explained in the document
|
|
|
|
** TODO [#C] Mention the Toolbox (maybe make a DOI for that)
|
|
** DONE [#C] Better understand principle of virtual work
|
|
CLOSED: [2025-02-10 Mon 15:51]
|
|
|
|
[[*Static Forces][Static Forces]]
|
|
|
|
Better understand this: https://en.wikipedia.org/wiki/Virtual_work
|
|
|
|
Also add link or explanation for this equation.
|
|
|
|
** DONE [#A] Should I include the effect of rotation somewhere?
|
|
CLOSED: [2025-02-11 Tue 23:00]
|
|
Similar to what was done with the 3DoF model?
|
|
|
|
=> *no, it will be done in the NASS chapter*
|
|
|
|
** DONE [#B] Define the geometry for the simplified nano-hexapod
|
|
CLOSED: [2025-02-06 Thu 18:56]
|
|
|
|
- [X] Micro-Hexapod radius: 150mm
|
|
- [X] Top plate radius: 150mm
|
|
- [X] Total height should match height of the nano-hexapod => 95mm
|
|
- [X] Location of joints: 20mm above/below bottom/top surfaces
|
|
- [X] Joints on a radius of 120mm and 110mm at the top
|
|
- [X] Angles of the joints:
|
|
- Bottom: +/- 10 degrees (50 deg offset)
|
|
- Top: +/-15 degrees (45 deg offset)
|
|
- [X] Check order of the struts to match the (final) nano-hexapod model
|
|
- Bottom:
|
|
- 190, 290, 310, 50, 70, 170
|
|
- top: 255, 285, 15, 45, 135, 165
|
|
|
|
** DONE [#C] First time in the report that we speak about MIMO control ? Or maybe next section!
|
|
CLOSED: [2025-02-06 Thu 16:01]
|
|
|
|
Maybe should introduce:
|
|
- "MIMO" Root locus
|
|
- "MIMO" Nyquist plot / characteristic loci
|
|
|
|
Or should this be in annexes?
|
|
|
|
Maybe say that in this phd-thesis, the focus is not on the control.
|
|
I tried multiple architectures (complementary filters, etc.), but the focus is not on that.
|
|
|
|
** ANSW [#C] Cubic architecture should be the topic here or in the detailed design?
|
|
CLOSED: [2025-02-06 Thu 16:01]
|
|
|
|
I suppose that it should be in the detailed design phase.
|
|
(Review about Stewart platform design should be made in Chapter two.)
|
|
|
|
Here, just use simple control architecture for general validation (and not optimization).
|
|
|
|
** ANSW [#C] Should I make a review of control strategies?
|
|
CLOSED: [2025-02-06 Thu 16:01]
|
|
|
|
Yes it seems to good location for review related to control.
|
|
|
|
Jacobian matrix.
|
|
Control is the frame of the struts, in the cartesian frame (CoM, CoK), modal control, etc...
|
|
|
|
[[file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org][file:~/Cloud/research/matlab/decoupling-strategies/svd-control.org]]
|
|
|
|
** DONE [#C] Compare simscape =linearize= and analytical formula
|
|
CLOSED: [2025-02-06 Thu 16:01]
|
|
|
|
- [X] OK for $\omega=0$ (using just the Stiffness matrix)
|
|
- [ ] Should add the mass matrix and compare for all frequencies
|
|
|
|
The analytical dynamic model is taken from cite:taghirad13_paral
|
|
|
|
** DONE [#C] Output the cubic configuration with clear display of the cube and center of the cube
|
|
CLOSED: [2025-02-06 Thu 16:02]
|
|
|
|
[[file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/cubic-configuration.org][cubic configuration]]
|
|
|
|
*No, this will be in Chapter 2*
|
|
|
|
** CANC [#C] Maybe make an appendix to present the developed toolbox?
|
|
CLOSED: [2025-02-06 Thu 16:02]
|
|
|
|
- State "CANC" from "TODO" [2025-02-06 Thu 16:02]
|
|
* Introduction :ignore:
|
|
|
|
Now that the multi-body model of the micro-station has been developed and validated using dynamical measurements, a model of the active vibration platform can be integrated.
|
|
|
|
First, the mechanical architecture of the active platform needs to be carefully chosen.
|
|
In Section ref:sec:nhexa_platform_review, a quick review of active vibration platforms is performed.
|
|
|
|
The chosen architecture is the Stewart platform, which is presented in Section ref:sec:nhexa_stewart_platform.
|
|
It is a parallel manipulator that require the use of specific tools to study its kinematics.
|
|
|
|
However, to study the dynamics of the Stewart platform, the use of analytical equations is very complex.
|
|
Instead, a multi-body model of the Stewart platform is developed (Section ref:sec:nhexa_model), that can then be easily integrated on top of the micro-station's model.
|
|
|
|
From a control point of view, the Stewart platform is a MIMO system with complex dynamics.
|
|
To control such system, it requires several tools to study interaction (Section ref:sec:nhexa_control).
|
|
|
|
* TODO Active Vibration Platforms
|
|
<<sec:nhexa_platform_review>>
|
|
** Introduction :ignore:
|
|
|
|
*Goals*:
|
|
- Quick review of active vibration platforms (5 or 6DoF) similar to NASS
|
|
- Explain why Stewart platform architecture is chosen
|
|
|
|
- Wanted controlled DOF: Y, Z, Ry
|
|
- But because of continuous rotation (key specificity): X,Y,Z,Rx,Ry in the frame of the active platform
|
|
|
|
- Literature review? (*maybe more suited for chapter 2*)
|
|
- file:~/Cloud/work-projects/ID31-NASS/matlab/stewart-simscape/org/bibliography.org
|
|
- Talk about flexible joint? Maybe not so much as it should be topic of second chapter.
|
|
Just say that we must of flexible joints that can be defined as 3 to 6DoF joints, and it will be optimize in chapter 2.
|
|
- [[cite:&taghirad13_paral]]
|
|
|
|
- For some systems, just XYZ control (stack stages), example: holler
|
|
- For other systems, Stewart platform (ID16a), piezo based
|
|
- Examples of Stewart platforms for general vibration control, some with Piezo, other with Voice coil. IFF, ...
|
|
Show different geometry configuration
|
|
- DCM: tripod?
|
|
|
|
** Active vibration control of sample stages
|
|
|
|
[[file:~/Cloud/work-projects/ID31-NASS/phd-thesis-chapters/A0-nass-introduction/nass-introduction.org::*Review of stages with online metrology for Synchrotrons][Review of stages with online metrology for Synchrotrons]]
|
|
|
|
- [ ] Talk about external metrology?
|
|
Maybe not the topic here.
|
|
- [ ] Talk about control architecture?
|
|
- [ ] Comparison with the micro-station / NASS
|
|
|
|
** Serial and Parallel Manipulators
|
|
|
|
*Goal*:
|
|
- Explain why a parallel manipulator is here preferred
|
|
- Compact, 6DoF, higher control bandwidth, linear, simpler
|
|
|
|
- Show some example of serial and parallel manipulators
|
|
|
|
- A review of Stewart platform will be given in Chapter related to the detailed design of the Nano-Hexapod
|
|
|
|
|
|
#+name: tab:nhexa_serial_vs_parallel
|
|
#+caption: Advantages and Disadvantages of both serial and parallel robots
|
|
#+attr_latex: :environment tabularx :width \linewidth :align lXX
|
|
#+attr_latex: :center t :booktabs t :float t
|
|
| | *Serial Robots* | *Parallel Robots* |
|
|
|--------------------+-----------------+-------------------|
|
|
| Advantages | Large Workspace | High Stiffness |
|
|
| Disadvantages | Low Stiffness | Small Workspace |
|
|
| Kinematic Struture | Open | Closed-loop |
|
|
|
|
* The Stewart platform
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/nhexa_1_stewart_platform.m
|
|
:END:
|
|
<<sec:nhexa_stewart_platform>>
|
|
** Introduction :ignore:
|
|
|
|
The Stewart platform, first introduced by Stewart in 1965 [[cite:&stewart65_platf_with_six_degrees_freed]] for flight simulation applications, represents a significant milestone in parallel manipulator design.
|
|
This mechanical architecture has evolved far beyond its original purpose, finding applications across diverse fields from precision positioning systems to robotic surgery.
|
|
The fundamental design consists of two platforms connected by six adjustable struts in parallel, creating a fully parallel manipulator capable of six degrees of freedom motion.
|
|
|
|
Unlike serial manipulators where errors worsen through the kinematic chain, parallel architectures distribute loads across multiple actuators, leading to enhanced mechanical stiffness and improved positioning accuracy.
|
|
This parallel configuration also results in superior dynamic performance, as the actuators directly contribute to the platform's motion without intermediate linkages.
|
|
These characteristics of Stewart platforms have made them particularly valuable in applications requiring high precision and stiffness.
|
|
|
|
For the NASS application, the Stewart platform architecture presents three key advantages.
|
|
First, as a fully parallel manipulator, all motion errors of the micro-station can be compensated through the coordinated action of the six actuators.
|
|
Second, its compact design compared to serial manipulators makes it ideal for integration on top micro-station where only $95\,mm$ of height is available.
|
|
Third, the good dynamical properties should enable high bandwidth positioning control.
|
|
|
|
While Stewart platforms excel in precision and stiffness, they typically exhibit a relatively limited workspace compared to serial manipulators.
|
|
However, this limitation is not significant for the NASS application, as the required motion range corresponds to the positioning errors of the micro-station which are in the order of $10\,\mu m$.
|
|
|
|
This section provides a comprehensive analysis of the Stewart platform's properties, focusing on aspects crucial for precision positioning applications.
|
|
The analysis encompasses the platform's kinematic relationships (Section ref:ssec:nhexa_stewart_platform_kinematics), the use of the Jacobian matrix (Section ref:ssec:nhexa_stewart_platform_jacobian), static behavior (Section ref:ssec:nhexa_stewart_platform_static), and dynamic characteristics (Section ref:ssec:nhexa_stewart_platform_dynamics).
|
|
These theoretical foundations form the basis for subsequent design decisions and control strategies, which will be elaborated in later sections.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+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
|
|
|
|
** Mechanical Architecture
|
|
<<ssec:nhexa_stewart_platform_architecture>>
|
|
|
|
The Stewart platform consists of two rigid platforms connected by six struts arranged in parallel (Figure ref:fig:nhexa_stewart_architecture).
|
|
Each strut incorporates an active prismatic joint that enables controlled length variation, with its ends attached to the fixed and mobile platforms through joints.
|
|
The typical configuration consists of a universal joint at one end and a spherical joint at the other, providing the necessary degrees of freedom[fn:1].
|
|
|
|
#+name: fig:nhexa_stewart_architecture
|
|
#+caption: Schematical representation of the Stewart platform architecture.
|
|
[[file:figs/nhexa_stewart_architecture.png]]
|
|
|
|
To facilitate rigorous analysis of the Stewart platform, four reference frames are defined:
|
|
- The fixed base frame $\{F\}$, located at the center of the base platform's bottom surface, serves as the mounting reference for the support structure.
|
|
- The mobile frame $\{M\}$, situated at the center of the top platform's upper surface, provides a reference for payload mounting.
|
|
- The point-of-interest frame $\{A\}$, fixed to the base but positioned at the workspace center.
|
|
- The moving point-of-interest frame $\{B\}$, attached to the mobile platform and coincident with frame $\{A\}$ in the home position.
|
|
|
|
Frames $\{F\}$ and $\{M\}$ serve primarily to define the joint locations.
|
|
On the other hand, frames $\{A\}$ and $\{B\}$ are used to describe the relative motion of the two platforms through the position vector ${}^A\bm{P}_B$ of frame $\{B\}$ expressed in frame $\{A\}$ and the rotation matrix ${}^A\bm{R}_B$ expressing the orientation of $\{B\}$ with respect to $\{A\}$.
|
|
For the nano-hexapod, frames $\{A\}$ and $\{B\}$ are chosen to be located at the theoretical focus point of the X-ray light which is $150\,mm$ above the top platform, i.e. above $\{M\}$.
|
|
|
|
Location of the joints and orientation and length of the struts are crucial for subsequent kinematic, static, and dynamic analyses of the Stewart platform.
|
|
The center of rotation for the joint fixed to the base is noted $\bm{a}_i$, while $\bm{b}_i$ is used for the top platform joints.
|
|
The struts orientation are represented by the unit vectors $\hat{\bm{s}}_i$ and their lengths by the scalars $l_i$.
|
|
This is summarized in Figure ref:fig:nhexa_stewart_notations.
|
|
|
|
#+name: fig:nhexa_stewart_notations
|
|
#+caption: Frame and key notations for the Stewart platform
|
|
[[file:figs/nhexa_stewart_notations.png]]
|
|
|
|
** Kinematic Analysis
|
|
<<ssec:nhexa_stewart_platform_kinematics>>
|
|
**** Introduction :ignore:
|
|
|
|
The kinematic analysis of the Stewart platform involves understanding the geometric relationships between the platform position/orientation and the actuator lengths, without considering the forces involved.
|
|
|
|
**** Loop Closure
|
|
|
|
The foundation of the kinematic analysis lies in the geometric constraints imposed by each strut, which can be expressed through loop closure equations.
|
|
For each strut $i$ (illustrated in Figure ref:fig:nhexa_stewart_loop_closure), the loop closure equation eqref:eq:nhexa_loop_closure can be written.
|
|
|
|
\begin{equation}\label{eq:nhexa_loop_closure}
|
|
{}^A\bm{P}_B = {}^A\bm{a}_i + l_i{}^A\hat{\bm{s}}_i - \underbrace{{}^B\bm{b}_i}_{{}^A\bm{R}_B {}^B\bm{b}_i} \quad \text{for } i=1 \text{ to } 6
|
|
\end{equation}
|
|
|
|
Such equation links the pose variables ${}^A\bm{P}$ and ${}^A\bm{R}_B$, the position vectors describing the known geometry of the base and of the moving platform, $\bm{a}_i$ and $\bm{b}_i$, and the strut vector $l_i {}^A\hat{\bm{s}}_i$:
|
|
|
|
#+name: fig:nhexa_stewart_loop_closure
|
|
#+caption: Notations to compute the kinematic loop closure
|
|
[[file:figs/nhexa_stewart_loop_closure.png]]
|
|
|
|
**** Inverse Kinematics
|
|
|
|
The inverse kinematic problem involves determining the required strut lengths $\bm{\mathcal{L}} = \left[ l_1, l_2, \ldots, l_6 \right]^T$ for a desired platform pose $\bm{\mathcal{X}}$ (i.e. position ${}^A\bm{P}$ and orientation ${}^A\bm{R}_B$).
|
|
This problem can be solved analytically using the loop closure equations eqref:eq:nhexa_loop_closure.
|
|
The obtain strut lengths are given by eqref:eq:nhexa_inverse_kinematics.
|
|
|
|
\begin{equation}\label{eq:nhexa_inverse_kinematics}
|
|
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 platform lie in the feasible workspace, the solution is unique.
|
|
Otherwise, the solution gives complex numbers.
|
|
|
|
**** Forward Kinematics
|
|
|
|
The forward kinematic problem seeks to determine the platform pose $\bm{\mathcal{X}}$ given a set of strut lengths $\bm{\mathcal{L}}$.
|
|
Unlike the inverse kinematics, this presents a significant challenge as it requires solving a system of nonlinear equations.
|
|
While various numerical methods exist for solving this problem, they can be computationally intensive and may not guarantee convergence to the correct solution.
|
|
|
|
For the nano-hexapod application, where displacements are typically small, an approximate solution based on linearization around the operating point provides a practical alternative.
|
|
This approximation, developed in subsequent sections through the Jacobian matrix analysis, proves particularly useful for real-time control applications.
|
|
|
|
# TODO - Add references
|
|
|
|
** The Jacobian Matrix
|
|
<<ssec:nhexa_stewart_platform_jacobian>>
|
|
**** Introduction :ignore:
|
|
|
|
The Jacobian matrix plays a central role in analyzing the Stewart platform's behavior, providing a linear mapping between platform and actuator velocities.
|
|
While the previously derived kinematic relationships are essential for position analysis, the Jacobian enables velocity analysis and forms the foundation for both static and dynamic studies.
|
|
|
|
**** Jacobian Computation - Velocity Loop Closure
|
|
|
|
As was shown in Section ref:ssec:nhexa_stewart_platform_kinematics, the strut lengths $\bm{\mathcal{L}}$ and the platform pose $\bm{\mathcal{X}}$ are related through a system of nonlinear algebraic equations representing the kinematic constraints imposed by the struts.
|
|
|
|
By taking the time derivative of the position loop close eqref:eq:nhexa_loop_closure, the /velocity loop closure/ is obtained eqref:eq:nhexa_loop_closure_velocity.
|
|
|
|
\begin{equation}\label{eq:nhexa_loop_closure_velocity}
|
|
{}^A\bm{v}_p + {}^A \dot{\bm{R}}_B {}^B\bm{b}_i + {}^A\bm{R}_B \underbrace{{}^B\dot{\bm{b}_i}}_{=0} = \dot{l}_i {}^A\hat{\bm{s}}_i + l_i {}^A\dot{\hat{\bm{s}}}_i + \underbrace{{}^A\dot{\bm{a}}_i}_{=0}
|
|
\end{equation}
|
|
|
|
Moreover, we have:
|
|
- ${}^A\dot{\bm{R}}_B {}^B\bm{b}_i = {}^A\bm{\omega} \times {}^A\bm{R}_B {}^B\bm{b}_i = {}^A\bm{\omega} \times {}^A\bm{b}_i$ in which ${}^A\bm{\omega}$ denotes the angular velocity of the moving platform expressed in the fixed frame $\{\bm{A}\}$.
|
|
- $l_i {}^A\dot{\hat{\bm{s}}}_i = l_i \left( {}^A\bm{\omega}_i \times \hat{\bm{s}}_i \right)$ in which ${}^A\bm{\omega}_i$ is the angular velocity of strut $i$ express in fixed frame $\{\bm{A}\}$.
|
|
|
|
By multiplying both sides by ${}^A\hat{\bm{s}}_i$, eqref:eq:nhexa_loop_closure_velocity_bis is obtained.
|
|
|
|
\begin{equation}\label{eq:nhexa_loop_closure_velocity_bis}
|
|
{}^A\hat{\bm{s}}_i {}^A\bm{v}_p + \underbrace{{}^A\hat{\bm{s}}_i ({}^A\bm{\omega} \times {}^A\bm{b}_i)}_{=({}^A\bm{b}_i \times {}^A\hat{\bm{s}}_i) {}^A\bm{\omega}} = \dot{l}_i + \underbrace{{}^A\hat{s}_i l_i \left( {}^A\bm{\omega}_i \times {}^A\hat{\bm{s}}_i \right)}_{=0}
|
|
\end{equation}
|
|
|
|
Equation eqref:eq:nhexa_loop_closure_velocity_bis can be rearranged in a matrix form to obtain eqref:eq:nhexa_jacobian_velocities, with $\dot{\bm{\mathcal{L}}} = [ \dot{l}_1 \ \dots \ \dot{l}_6 ]^T$ the vector of strut velocities, and $\dot{\bm{\mathcal{X}}} = [{}^A\bm{v}_p ,\ {}^A\bm{\omega}]^T$ the vector of platform velocity and angular velocity.
|
|
|
|
\begin{equation}\label{eq:nhexa_jacobian_velocities}
|
|
\boxed{\dot{\bm{\mathcal{L}}} = \bm{J} \dot{\bm{\mathcal{X}}}}
|
|
\end{equation}
|
|
|
|
The matrix $\bm{J}$ is called the Jacobian matrix, and is defined by eqref:eq:nhexa_jacobian, with:
|
|
- ${}^A\hat{\bm{s}}_i$ the orientation of the struts expressed in $\{A\}$
|
|
- ${}^A\bm{b}_i$ the position of the joints with respect to $O_B$ and express in $\{A\}$
|
|
|
|
\begin{equation}\label{eq:nhexa_jacobian}
|
|
\bm{J} = \begin{bmatrix}
|
|
{{}^A\hat{\bm{s}}_1}^T & ({}^A\bm{b}_1 \times {}^A\hat{\bm{s}}_1)^T \\
|
|
{{}^A\hat{\bm{s}}_2}^T & ({}^A\bm{b}_2 \times {}^A\hat{\bm{s}}_2)^T \\
|
|
{{}^A\hat{\bm{s}}_3}^T & ({}^A\bm{b}_3 \times {}^A\hat{\bm{s}}_3)^T \\
|
|
{{}^A\hat{\bm{s}}_4}^T & ({}^A\bm{b}_4 \times {}^A\hat{\bm{s}}_4)^T \\
|
|
{{}^A\hat{\bm{s}}_5}^T & ({}^A\bm{b}_5 \times {}^A\hat{\bm{s}}_5)^T \\
|
|
{{}^A\hat{\bm{s}}_6}^T & ({}^A\bm{b}_6 \times {}^A\hat{\bm{s}}_6)^T
|
|
\end{bmatrix}
|
|
\end{equation}
|
|
|
|
This Jacobian matrix $\bm{J}$ therefore links the rate of change of strut length to the velocity and angular velocity of the top platform with respect to the fixed base through a set of linear equations.
|
|
However, $\bm{J}$ needs to be recomputed for every Stewart platform pose as it depends on the actual pose of of the manipulator.
|
|
|
|
**** Approximate solution of the Forward and Inverse Kinematic problems
|
|
|
|
For small displacements $\delta \bm{\mathcal{X}} = [\delta x, \delta y, \delta z, \delta \theta_x, \delta \theta_y, \delta \theta_z ]^T$ around an operating point $\bm{\mathcal{X}}_0$ (for which the Jacobian was computed), the associated joint displacement $\delta\bm{\mathcal{L}} = [\delta l_1,\,\delta l_2,\,\delta l_3,\,\delta l_4,\,\delta l_5,\,\delta l_6]^T$ can be computed using the Jacobian (approximate solution of the inverse kinematic problem):
|
|
\begin{equation}\label{eq:nhexa_inverse_kinematics_approximate}
|
|
\boxed{\delta\bm{\mathcal{L}} = \bm{J} \delta\bm{\mathcal{X}}}
|
|
\end{equation}
|
|
|
|
Similarly, for small joint displacements $\delta\bm{\mathcal{L}}$, it is possible to find the induced small displacement of the mobile platform (approximate solution of the forward kinematic problem):
|
|
\begin{equation}\label{eq:nhexa_forward_kinematics_approximate}
|
|
\boxed{\delta\bm{\mathcal{X}} = \bm{J}^{-1} \delta\bm{\mathcal{L}}}
|
|
\end{equation}
|
|
|
|
These two relations solve the forward and inverse kinematic problems for small displacement in a /approximate/ way.
|
|
As the inverse kinematic can be easily solved exactly this is not much useful, however, as the forward kinematic problem is difficult to solve, this approximation can be very useful for small displacements.
|
|
|
|
**** Range validity of the approximate inverse kinematics
|
|
|
|
The accuracy of the Jacobian-based forward kinematics solution was estimated through a systematic error analysis.
|
|
For a series of platform positions along the $x\text{-axis}$, the exact strut lengths are computed using the analytical inverse kinematics equation eqref:eq:nhexa_inverse_kinematics.
|
|
These strut lengths are then used with the Jacobian to estimate the platform pose, from which the error between the estimated and true poses can be calculated.
|
|
|
|
The estimation errors in the $x$, $y$, and $z$ directions are shown in Figure ref:fig:nhexa_forward_kinematics_approximate_errors.
|
|
The results demonstrate that for displacements up to approximately $1\,\%$ of the hexapod's size (which corresponds to $100\,\mu m$ as the size of the Stewart platform is here $\approx 100\,mm$), the Jacobian approximation provides excellent accuracy.
|
|
|
|
This finding has particular significance for the Nano-hexapod application.
|
|
Since the maximum required stroke ($\approx 100\,\mu m$) is three orders of magnitude smaller than the stewart platform size ($\approx 100\,mm$), the Jacobian matrix can be considered constant throughout the workspace.
|
|
It can be computed once at the rest position and used for both forward and inverse kinematics with high accuracy.
|
|
|
|
#+begin_src matlab
|
|
%% Estimate the errors associated with approximate forward kinematics using the Jacobian matrix
|
|
stewart = initializeSimplifiedNanoHexapod('H', 100e-3, 'MO_B', 0);
|
|
|
|
Xrs = logspace(-6, -1, 100); % Wanted X translation of the mobile platform [m]
|
|
|
|
% Compute the strut exact length for each X-position
|
|
Ls_exact = zeros(6, length(Xrs));
|
|
for i = 1:length(Xrs)
|
|
[~, L_exact(:, i)] = inverseKinematics(stewart, 'AP', [Xrs(i); 0; 0]);
|
|
end
|
|
|
|
% From the strut length, compute the stewart pose using the Jacobian matrix
|
|
Xrs_approx = zeros(6, length(Xrs));
|
|
for i = 1:length(Xrs)
|
|
Xrs_approx(:, i) = inv(stewart.geometry.J)*L_exact(:, i);
|
|
end
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Errors associated with the use of the Jacobian matrix to solve the forward kinematic problem
|
|
figure;
|
|
hold on;
|
|
plot(1e6*Xrs, 1e9*abs(Xrs - Xrs_approx(1, :)), 'DisplayName', '$\epsilon_x$');
|
|
plot(1e6*Xrs, 1e9*abs(Xrs_approx(2, :)), 'DisplayName', '$\epsilon_y$');
|
|
plot(1e6*Xrs, 1e9*abs(Xrs_approx(3, :)), 'DisplayName', '$\epsilon_z$');
|
|
plot(1e6*Xrs, 1e6*Xrs, 'k--', 'DisplayName', '$0.1\%$ error');
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
xlim([1, 1e4]); ylim([1, 1e4]);
|
|
xlabel('$D_x$ motion');
|
|
ylabel('Kinematic Errors');
|
|
xticks([1, 10, 100, 1000, 10000]);
|
|
yticks([1, 10, 100, 1000, 10000]);
|
|
xticklabels({'$1\mu m$', '$10\mu m$', '$100\mu m$', '$1mm$', '$10mm$'});
|
|
yticklabels({'$1nm$', '$10nm$', '$100nm$', '$1\mu m$', '$10\mu m$'});
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nhexa_forward_kinematics_approximate_errors.pdf', 'width', 'wide', 'height', 'normal');
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_forward_kinematics_approximate_errors
|
|
#+caption: Errors associated with the use of the Jacobian matrix to solve the forward kinematic problem. A Stewart platform with an height of $100\,mm$ was used to perform this analysis
|
|
#+RESULTS:
|
|
[[file:figs/nhexa_forward_kinematics_approximate_errors.png]]
|
|
|
|
**** Static Forces
|
|
|
|
The static force analysis of the Stewart platform can be elegantly performed using the principle of virtual work.
|
|
This principle states that, for a system in static equilibrium, the total virtual work of all forces acting on the system must be zero for any virtual displacement compatible with the system's constraints.
|
|
|
|
Let $\bm{f} = [f_1, f_2, \cdots, f_6]^T$ represent the vector of actuator forces applied in each strut, and $\bm{\mathcal{F}} = [\bm{F}, \bm{n}]^T$ denote the external wrench (combined force $\bm{F}$ and torque $\bm{n}$) acting on the mobile platform at point $\bm{O}_B$.
|
|
The virtual work $\delta W$ consists of two contributions:
|
|
- The work performed by the actuator forces through virtual strut displacements $\delta \bm{\mathcal{L}}$: $\bm{f}^T \delta \bm{\mathcal{L}}$
|
|
- The work performed by the external wrench through virtual platform displacements $\delta \bm{\mathcal{X}}$: $-\bm{\mathcal{F}}^T \delta \bm{\mathcal{X}}$
|
|
|
|
The principle of virtual work can thus be expressed as:
|
|
\begin{equation}
|
|
\delta W = \bm{f}^T \delta \bm{\mathcal{L}} - \bm{\mathcal{F}}^T \delta \bm{\mathcal{X}} = 0
|
|
\end{equation}
|
|
|
|
Using the Jacobian relationship that links virtual displacements eqref:eq:nhexa_inverse_kinematics_approximate, this equation becomes:
|
|
\begin{equation}
|
|
\left( \bm{f}^T \bm{J} - \bm{\mathcal{F}}^T \right) \delta \bm{\mathcal{X}} = 0
|
|
\end{equation}
|
|
|
|
Since this equation must hold for any virtual displacement $\delta \bm{\mathcal{X}}$, the following force mapping relationships can be derived:
|
|
|
|
\begin{equation}\label{eq:nhexa_jacobian_forces}
|
|
\bm{f}^T \bm{J} - \bm{\mathcal{F}}^T = 0 \quad \Rightarrow \quad \boxed{\bm{\mathcal{F}} = \bm{J}^T \bm{f}} \quad \text{and} \quad \boxed{\bm{f} = \bm{J}^{-T} \bm{\mathcal{F}}}
|
|
\end{equation}
|
|
|
|
These equations establish that the transpose of the Jacobian matrix maps actuator forces to platform forces and torques, while its inverse transpose maps platform forces and torques to required actuator forces.
|
|
|
|
** Static Analysis
|
|
<<ssec:nhexa_stewart_platform_static>>
|
|
|
|
The static stiffness characteristics of the Stewart platform play a crucial role in its performance, particularly for precision positioning applications.
|
|
These characteristics are fundamentally determined by both the actuator properties and the platform geometry.
|
|
|
|
Starting from the individual actuators, the relationship between applied force $f_i$ and resulting displacement $\delta l_i$ for each strut $i$ is characterized by its stiffness $k_i$:
|
|
\begin{equation}
|
|
f_i = k_i \delta l_i, \quad i = 1,\ \dots,\ 6
|
|
\end{equation}
|
|
|
|
These individual relationships can be combined into a matrix form using the diagonal stiffness matrix $\bm{\mathcal{K}}$:
|
|
\begin{equation}
|
|
\bm{f} = \bm{\mathcal{K}} \cdot \delta \bm{\mathcal{L}}, \quad \bm{\mathcal{K}} = \text{diag}\left[ k_1,\ \dots,\ k_6 \right]
|
|
\end{equation}
|
|
|
|
By applying the force mapping relationships eqref:eq:nhexa_jacobian_forces derived in the previous section and the Jacobian relationship for small displacements eqref:eq:nhexa_forward_kinematics_approximate, the relationship between applied wrench $\bm{\mathcal{F}}$ and resulting platform displacement $\delta \bm{\mathcal{X}}$ is obtained eqref:eq:nhexa_stiffness_matrix.
|
|
|
|
\begin{equation}\label{eq:nhexa_stiffness_matrix}
|
|
\bm{\mathcal{F}} = \underbrace{\bm{J}^T \bm{\mathcal{K}} \bm{J}}_{\bm{K}} \cdot \delta \bm{\mathcal{X}}
|
|
\end{equation}
|
|
where $\bm{K} = \bm{J}^T \bm{\mathcal{K}} \bm{J}$ is identified as the platform stiffness matrix.
|
|
|
|
The inverse relationship is given by the compliance matrix $\bm{C}$:
|
|
|
|
\begin{equation}
|
|
\delta \bm{\mathcal{X}} = \underbrace{(\bm{J}^T \bm{\mathcal{K}} \bm{J})^{-1}}_{\bm{C}} \bm{\mathcal{F}}
|
|
\end{equation}
|
|
|
|
These relationships reveal that the overall platform stiffness and compliance characteristics are determined by two factors:
|
|
- The individual actuator stiffnesses represented by $\bm{\mathcal{K}}$
|
|
- The geometric configuration embodied in the Jacobian matrix $\bm{J}$
|
|
|
|
This geometric dependency means that the platform's stiffness varies throughout its workspace, as the Jacobian matrix changes with the platform's position and orientation.
|
|
For the NASS application, where the workspace is relatively small compared to the platform dimensions, these variations can be considered minimal.
|
|
However, the initial geometric configuration significantly impacts the overall stiffness characteristics.
|
|
The relationship between maximum stroke and stiffness presents another important design consideration.
|
|
As both parameters are influenced by the geometric configuration, their optimization involves inherent trade-offs that must be carefully balanced based on application requirements.
|
|
The optimization of this configuration to achieve desired stiffness properties while having enough stroke will be addressed during the detailed design phase.
|
|
|
|
** Dynamic Analysis
|
|
<<ssec:nhexa_stewart_platform_dynamics>>
|
|
|
|
The dynamic behavior of a Stewart platform can be analyzed through various approaches, depending on the desired level of model fidelity.
|
|
For initial analysis, we consider a simplified model with the following assumptions:
|
|
- Massless struts
|
|
- Ideal joints without friction or compliance
|
|
- Rigid platform and base
|
|
|
|
Under these assumptions, the system dynamics can be expressed in the Cartesian space as:
|
|
\begin{equation}
|
|
\bm{M} s^2 \bm{\mathcal{X}} = \Sigma \bm{\mathcal{F}}
|
|
\end{equation}
|
|
|
|
where $\bm{M}$ represents the platform mass matrix, $\bm{\mathcal{X}}$ the platform pose, and $\Sigma \bm{\mathcal{F}}$ the sum of forces acting on the platform.
|
|
|
|
The primary forces acting on the system are actuator forces $\bm{f}$, elastic forces due to strut stiffness $-\bm{\mathcal{K}} \bm{\mathcal{L}}$ and damping forces in the struts $\bm{\mathcal{C}} \dot{\bm{\mathcal{L}}}$.
|
|
|
|
\begin{equation}
|
|
\Sigma \bm{\mathcal{F}} = \bm{J}^T (\bm{f} - \bm{\mathcal{K}} \bm{\mathcal{L}} - s \bm{\mathcal{C}} \bm{\mathcal{L}}), \quad \bm{\mathcal{K}} = \text{diag}(k_1\,\dots\,k_6),\ \bm{\mathcal{C}} = \text{diag}(c_1\,\dots\,c_6)
|
|
\end{equation}
|
|
|
|
Combining these forces and using eqref:eq:nhexa_forward_kinematics_approximate yields the complete dynamic equation eqref:eq:nhexa_dynamical_equations.
|
|
|
|
\begin{equation}\label{eq:nhexa_dynamical_equations}
|
|
\bm{M} s^2 \bm{\mathcal{X}} = \bm{\mathcal{F}} - \bm{J}^T \bm{\mathcal{K}} \bm{J} \bm{\mathcal{X}} - \bm{J}^T \bm{\mathcal{C}} \bm{J} s \bm{\mathcal{X}}
|
|
\end{equation}
|
|
|
|
The transfer function matrix in the Cartesian frame becomes eqref:eq:nhexa_transfer_function_cart.
|
|
|
|
\begin{equation}\label{eq:nhexa_transfer_function_cart}
|
|
\frac{{\mathcal{X}}}{\bm{\mathcal{F}}}(s) = ( \bm{M} s^2 + \bm{J}^{T} \bm{\mathcal{C}} \bm{J} s + \bm{J}^{T} \bm{\mathcal{K}} \bm{J} )^{-1}
|
|
\end{equation}
|
|
|
|
Through coordinate transformation using the Jacobian matrix, the dynamics in the actuator space is obtained eqref:eq:nhexa_transfer_function_struts.
|
|
|
|
\begin{equation}\label{eq:nhexa_transfer_function_struts}
|
|
\frac{\bm{\mathcal{L}}}{\bm{f}}(s) = ( \bm{J}^{-T} \bm{M} \bm{J}^{-1} s^2 + \bm{\mathcal{C}} + \bm{\mathcal{K}} )^{-1}
|
|
\end{equation}
|
|
|
|
While this simplified model provides useful insights, real Stewart platforms exhibit more complex behaviors.
|
|
Several factors significantly increase model complexity:
|
|
- Strut dynamics, including mass distribution and internal resonances
|
|
- Joint compliance and friction effects
|
|
- Supporting structure dynamics and payload dynamics, which are both very critical for NASS
|
|
|
|
# TODO - Add citations [[cite:&mcinroy00_desig_contr_flexur_joint_hexap;&mcinroy02_model_desig_flexur_joint_stewar]]
|
|
|
|
These additional effects make analytical modeling impractical for complete system analysis.
|
|
|
|
** Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
The fundamental characteristics of the Stewart platform have been analyzed in this chapter.
|
|
Essential kinematic relationships were developed through loop closure equations, from which both exact and approximate solutions for the inverse and forward kinematic problems were derived.
|
|
The Jacobian matrix was established as a central mathematical tool, through which crucial insights into velocity relationships, static force transmission, and dynamic behavior of the platform were obtained.
|
|
|
|
For the NASS application, where displacements are typically limited to the micrometer range, the accuracy of linearized models using a constant Jacobian matrix has been demonstrated, by which both analysis and control can be significantly simplified.
|
|
However, additional complexities such as strut masses, joint compliance, and supporting structure dynamics must be considered in the full dynamic behavior.
|
|
This will be performed in the next section using a multi-body model.
|
|
|
|
All these characteristics (maneuverability, stiffness, dynamics, etc.) are fundamentally determined by the platform's geometry.
|
|
While a reasonable geometric configuration will be used to validate the NASS during this conceptual phase, the optimization of these geometric parameters will be explored during the detailed design phase.
|
|
|
|
* Multi-Body Model
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/nhexa_2_model.m
|
|
:END:
|
|
<<sec:nhexa_model>>
|
|
** Introduction :ignore:
|
|
|
|
The dynamic modeling of Stewart platforms has traditionally relied on analytical approaches.
|
|
However, these analytical models become increasingly complex when the full dynamic behavior of struts and joints must be captured.
|
|
To overcome these limitations, a flexible multi-body approach has been developed that can be readily integrated into the broader NASS system model.
|
|
Through this multi-body modeling approach, each component model (including joints, actuators, and sensors) can be progressively refined.
|
|
|
|
The analysis is structured in three parts.
|
|
First, the multi-body model is developed, wherein detailed geometric parameters, inertial properties, and actuator characteristics are established (Section ref:ssec:nhexa_model_def).
|
|
The model is then validated through comparison with analytical equations in a simplified configuration (Section ref:ssec:nhexa_model_validation).
|
|
Finally, the validated model is employed to analyze the nano-hexapod dynamics, from which insights for the control system design are derived (Section ref:ssec:nhexa_model_dynamics).
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+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
|
|
|
|
** Model Definition
|
|
<<ssec:nhexa_model_def>>
|
|
**** Geometry
|
|
|
|
The Stewart platform's geometry is defined by two principal coordinate frames (Figure ref:fig:nhexa_stewart_model_def): a fixed base frame $\{F\}$ and a moving platform frame $\{M\}$.
|
|
The joints connecting the actuators to these frames are located at positions ${}^F\bm{a}_i$ and ${}^M\bm{b}_i$ respectively.
|
|
The point of interest, denoted by frame $\{A\}$, is situated $150\,mm$ above the moving platform frame $\{M\}$.
|
|
|
|
The geometric parameters of the nano-hexapod are summarized in Table ref:tab:nhexa_stewart_model_geometry.
|
|
These parameters define the positions of all connection points in their respective coordinate frames.
|
|
From these parameters, key kinematic properties can be derived: the strut orientations $\hat{\bm{s}}_i$, strut lengths $l_i$, and the system's Jacobian matrix $\bm{J}$.
|
|
|
|
#+attr_latex: :options [b]{0.6\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:nhexa_stewart_model_def
|
|
#+caption: Geometry of the stewart platform
|
|
#+attr_latex: :float nil :scale 1
|
|
[[file:figs/nhexa_stewart_model_def.png]]
|
|
#+end_minipage
|
|
\hfill
|
|
#+attr_latex: :options [b]{0.38\linewidth}
|
|
#+begin_minipage
|
|
#+begin_scriptsize
|
|
#+latex: \centering
|
|
#+attr_latex: :environment tabularx :width 0.75\linewidth :placement [b] :align Xrrr
|
|
#+attr_latex: :booktabs t :float nil :center nil
|
|
| | $\bm{x}$ | $\bm{y}$ | $\bm{z}$ |
|
|
|-----------+----------+----------+----------|
|
|
| ${}^M\bm{O}_B$ | $0$ | $0$ | $150$ |
|
|
| ${}^F\bm{O}_M$ | $0$ | $0$ | $95$ |
|
|
| ${}^F\bm{a}_1$ | $-92$ | $-77$ | $20$ |
|
|
| ${}^F\bm{a}_2$ | $92$ | $-77$ | $20$ |
|
|
| ${}^F\bm{a}_3$ | $113$ | $-41$ | $20$ |
|
|
| ${}^F\bm{a}_4$ | $21$ | $118$ | $20$ |
|
|
| ${}^F\bm{a}_5$ | $-21$ | $118$ | $20$ |
|
|
| ${}^F\bm{a}_6$ | $-113$ | $-41$ | $20$ |
|
|
| ${}^M\bm{b}_1$ | $-28$ | $-106$ | $-20$ |
|
|
| ${}^M\bm{b}_2$ | $28$ | $-106$ | $-20$ |
|
|
| ${}^M\bm{b}_3$ | $106$ | $28$ | $-20$ |
|
|
| ${}^M\bm{b}_4$ | $78$ | $78$ | $-20$ |
|
|
| ${}^M\bm{b}_5$ | $-78$ | $78$ | $-20$ |
|
|
| ${}^M\bm{b}_6$ | $-106$ | $28$ | $-20$ |
|
|
#+latex: \captionof{table}{\label{tab:nhexa_stewart_model_geometry}Parameter values in [mm]}
|
|
#+end_scriptsize
|
|
#+end_minipage
|
|
|
|
**** Inertia of Plates
|
|
|
|
The fixed base and moving platform are modeled as solid cylindrical bodies.
|
|
The base platform is characterized by a radius of $120\,mm$ and thickness of $15\,mm$, matching the dimensions of the micro-hexapod's top platform.
|
|
The moving platform is similarly modeled with a radius of $110\,mm$ and thickness of $15\,mm$.
|
|
Both platforms are assigned a mass of $5\,kg$.
|
|
|
|
**** Joints
|
|
|
|
The platform's joints play a crucial role in its dynamic behavior.
|
|
At both the upper and lower connection points, various degrees of freedom can be modeled, including universal joints, spherical joints, and configurations with additional axial and lateral stiffness components.
|
|
For each degree of freedom, stiffness characteristics can be incorporated into the model.
|
|
|
|
In the conceptual design phase, a simplified joint configuration is employed: the bottom joints are modeled as two-degree-of-freedom universal joints, while the top joints are represented as three-degree-of-freedom spherical joints.
|
|
These joints are considered massless and exhibit no stiffness along their degrees of freedom.
|
|
|
|
**** Actuators
|
|
|
|
The actuator model comprises several key elements (Figure ref:fig:nhexa_actuator_model).
|
|
At its core, each actuator is modeled as a prismatic joint with internal stiffness $k_a$ and damping $c_a$, driven by a force source $f$.
|
|
Similarly to what was found using the rotating 3-DoF model, a parallel stiffness $k_p$ is added in parallel with the force sensor to ensure stability when considering spindle rotation effects.
|
|
|
|
Each actuator is equipped with two sensors: a force sensor providing measurements $f_n$ and a relative motion sensor measuring strut length $l_i$.
|
|
The actuator parameters used in the conceptual phase are presented in Table ref:tab:nhexa_actuator_parameters.
|
|
|
|
This modular approach to actuator modeling allows for future refinements as the design evolves, enabling the incorporation of additional dynamic effects or sensor characteristics as needed.
|
|
|
|
#+attr_latex: :options [b]{0.6\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:nhexa_actuator_model
|
|
#+caption: Model of the nano-hexapod actuators
|
|
#+attr_latex: :float nil :scale 1
|
|
[[file:figs/nhexa_actuator_model.png]]
|
|
#+end_minipage
|
|
\hfill
|
|
#+attr_latex: :options [b]{0.38\linewidth}
|
|
#+begin_minipage
|
|
#+begin_scriptsize
|
|
#+latex: \centering
|
|
#+attr_latex: :environment tabularx :width 0.4\linewidth :placement [b] :align Xl
|
|
#+attr_latex: :booktabs t :float nil :center nil
|
|
| | Value |
|
|
|-------+-----------------|
|
|
| $k_a$ | $1\,N/\mu m$ |
|
|
| $c_a$ | $50\,N/(m/s)$ |
|
|
| $k_p$ | $0.05\,N/\mu m$ |
|
|
#+latex: \captionof{table}{\label{tab:nhexa_actuator_parameters}Actuator parameters}
|
|
#+end_scriptsize
|
|
#+end_minipage
|
|
|
|
** Validation of the multi-body model
|
|
<<ssec:nhexa_model_validation>>
|
|
|
|
The developed multi-body model of the Stewart platform is represented schematically in Figure ref:fig:nhexa_stewart_model_input_outputs, highlighting the key inputs and outputs: actuator forces $\bm{f}$, force sensor measurements $\bm{f}_n$, and relative displacement measurements $\bm{\mathcal{L}}$.
|
|
The frames $\{F\}$ and $\{M\}$ serve as interfaces for integration with other elements in the multi-body system.
|
|
A three-dimensional visualization of the model is presented in Figure ref:fig:nhexa_simscape_screenshot.
|
|
|
|
#+attr_latex: :options [b]{0.6\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:nhexa_stewart_model_input_outputs
|
|
#+caption: Nano-Hexapod plant with inputs and outputs. Frames $\{F\}$ and $\{M\}$ can be connected to other elements in the multi-body models.
|
|
#+attr_latex: :scale 1 :float nil
|
|
[[file:figs/nhexa_stewart_model_input_outputs.png]]
|
|
#+end_minipage
|
|
\hfill
|
|
#+attr_latex: :options [b]{0.35\linewidth}
|
|
#+begin_minipage
|
|
#+name: fig:nhexa_simscape_screenshot
|
|
#+caption: 3D representation of the multi-body model
|
|
#+attr_latex: :width 0.90\linewidth :float nil
|
|
[[file:figs/nhexa_simscape_screenshot.jpg]]
|
|
#+end_minipage
|
|
|
|
The validation of the multi-body model is performed using the simplest Stewart platform configuration, enabling direct comparison with the analytical transfer functions derived in Section ref:ssec:nhexa_stewart_platform_dynamics.
|
|
This configuration consists of massless universal joints at the base, massless spherical joints at the top platform, and massless struts with stiffness $k_a = 1\,\text{N}/\mu\text{m}$ and damping $c_a = 10\,\text{N}/({\text{m}/\text{s}})$.
|
|
The geometric parameters remain as specified in Table ref:tab:nhexa_actuator_parameters.
|
|
|
|
While the moving platform itself is considered massless, a $10\,\text{kg}$ cylindrical payload is mounted on top with a radius of $r = 110\,mm$ and a height $h = 300\,mm$.
|
|
|
|
For the analytical model, the stiffness, damping and mass matrices are defined in eqref:eq:nhexa_analytical_matrices.
|
|
|
|
\begin{subequations}\label{eq:nhexa_analytical_matrices}
|
|
\begin{align}
|
|
\bm{\mathcal{K}} &= \text{diag}(k_a,\ k_a,\ k_a,\ k_a,\ k_a,\ k_a) \\
|
|
\bm{\mathcal{C}} &= \text{diag}(c_a,\ c_a,\ c_a,\ c_a,\ c_a,\ c_a) \\
|
|
\bm{M} &= \text{diag}\left(m,\ m,\ m,\ \frac{1}{12}m(3r^2 + h^2),\ \frac{1}{12}m(3r^2 + h^2),\ \frac{1}{2}mr^2\right)
|
|
\end{align}
|
|
\end{subequations}
|
|
|
|
The transfer functions from actuator forces to strut displacements are computed using these matrices according to equation eqref:eq:nhexa_transfer_function_struts.
|
|
These analytical transfer functions are then compared with those extracted from the multi-body model.
|
|
The multi-body model yields a state-space representation with 12 states, corresponding to the six degrees of freedom of the moving platform.
|
|
|
|
Figure ref:fig:nhexa_comp_multi_body_analytical presents a comparison between the analytical and multi-body transfer functions, specifically showing the response from the first actuator force to all six strut displacements.
|
|
The close agreement between both approaches across the frequency spectrum validates the multi-body model's accuracy in capturing the system's dynamic behavior.
|
|
|
|
#+begin_src matlab
|
|
%% Plant using Analytical Equations
|
|
% Stewart platform definition
|
|
k = 1e6; % Actuator stiffness [N/m]
|
|
c = 1e1; % Actuator damping [N/(m/s)]
|
|
|
|
stewart = initializeSimplifiedNanoHexapod(...
|
|
'Mpm', 1e-3, ...
|
|
'actuator_type', '1dof', ...
|
|
'actuator_k', k, ...
|
|
'actuator_kp', 0, ...
|
|
'actuator_c', c ...
|
|
);
|
|
|
|
% Payload: Cylinder
|
|
h = 300e-3; % Height of the cylinder [m]
|
|
r = 110e-3; % Radius of the cylinder [m]
|
|
m = 10; % Mass of the payload [kg]
|
|
initializeSample('type', 'cylindrical', 'm', m, 'H', h, 'R', r);
|
|
|
|
% Mass Matrix
|
|
M = zeros(6,6);
|
|
M(1,1) = m;
|
|
M(2,2) = m;
|
|
M(3,3) = m;
|
|
M(4,4) = 1/12*m*(3*r^2 + h^2);
|
|
M(5,5) = 1/12*m*(3*r^2 + h^2);
|
|
M(6,6) = 1/2*m*r^2;
|
|
|
|
% Stiffness and Damping matrices
|
|
K = k*eye(6);
|
|
C = c*eye(6);
|
|
|
|
% Compute plant in the frame of the struts
|
|
G_analytical = inv(ss(inv(stewart.geometry.J')*M*inv(stewart.geometry.J)*s^2 + C*s + K));
|
|
|
|
% Compare with Simscape model
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Encoders [m]
|
|
|
|
G_simscape = linearize(mdl, io);
|
|
G_simscape.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_simscape.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Comparison of the analytical transfer functions and the multi-body model
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_simscape(i,1), freqs, 'Hz'))), 'color', [colors(i,:), 0.5], ...
|
|
'DisplayName', sprintf('$l_%i/f_1$ - Multi-Body', i))
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_analytical(i,1), freqs, 'Hz'))), '--', 'color', [colors(i,:)], ...
|
|
'DisplayName', sprintf('$l_%i/f_1$ - Analytical', i))
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-4]);
|
|
leg = legend('location', 'northwest', 'FontSize', 6, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_simscape(i,1), freqs, 'Hz'))), 'color', [colors(i,:),0.5]);
|
|
end
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_analytical(i,1), freqs, 'Hz'))), '--', 'color', colors(i,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file replace
|
|
exportFig('figs/nhexa_comp_multi_body_analytical.pdf', 'width', 'wide', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_comp_multi_body_analytical
|
|
#+caption: Comparison of the analytical transfer functions and the multi-body model
|
|
#+RESULTS:
|
|
[[file:figs/nhexa_comp_multi_body_analytical.png]]
|
|
|
|
** Nano Hexapod Dynamics
|
|
<<ssec:nhexa_model_dynamics>>
|
|
|
|
Following the validation of the multi-body model, a detailed analysis of the nano-hexapod dynamics has been performed.
|
|
The model parameters are set according to the specifications outlined in Section ref:ssec:nhexa_model_def, with a payload mass of $10\,kg$.
|
|
Transfer functions from actuator forces $\bm{f}$ to both strut displacements $\bm{\mathcal{L}}$ and force measurements $\bm{f}_n$ are derived from the multi-body model.
|
|
|
|
The transfer functions relating actuator forces to strut displacements are presented in Figure ref:fig:nhexa_multi_body_plant_dL.
|
|
Due to the system's symmetrical design and identical strut configurations, all diagonal terms (transfer functions from force $f_i$ to displacement $l_i$ of the same strut) exhibit identical behavior.
|
|
While the system possesses six degrees of freedom, only four distinct resonance frequencies are observed in the frequency response.
|
|
This reduction from six to four observable modes is attributed to the system's symmetry, where two pairs of resonances occur at identical frequencies.
|
|
|
|
The system's behavior can be characterized in three frequency regions.
|
|
At low frequencies, well below the first resonance, the plant demonstrates good decoupling between actuators, with the response dominated by the strut stiffness: $\bm{G}(j\omega) \xrightarrow[\omega \to 0]{} \bm{\mathcal{K}}^{-1}$.
|
|
In the mid-frequency range, the system exhibits coupled dynamics through its resonant modes, reflecting the complex interactions between the platform's degrees of freedom.
|
|
At high frequencies, above the highest resonance, the response is governed by the payload's inertia mapped to the strut coordinates: $\bm{G}(j\omega) \xrightarrow[\omega \to \infty]{} \bm{J} \bm{M}^{-T} \bm{J}^T \frac{-1}{\omega^2}$
|
|
|
|
The force sensor transfer functions, shown in Figure ref:fig:nhexa_multi_body_plant_fm, display characteristics typical of collocated actuator-sensor pairs.
|
|
Each actuator's transfer function to its associated force sensor exhibits alternating complex conjugate poles and zeros.
|
|
The inclusion of parallel stiffness introduces an additional complex conjugate zero at low frequency, a feature previously observed in the three-degree-of-freedom rotating model.
|
|
|
|
#+begin_src matlab
|
|
%% Multi-Body model of the Nano-Hexapod
|
|
% Initialize 1DoF
|
|
initializeSimplifiedNanoHexapod('flex_type_F', '2dof', 'flex_type_M', '3dof', 'actuator_type', '1dof');
|
|
initializeSample('type', 'cylindrical', 'm', 10, 'H', 300e-3);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'dL'); io_i = io_i + 1; % Encoders [m]
|
|
io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N]
|
|
|
|
% With no payload
|
|
G = linearize(mdl, io);
|
|
G.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
|
'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% Multi-Body model of the Nano-Hexapod without parallel stiffness
|
|
% Initialize 1DoF
|
|
initializeSimplifiedNanoHexapod('flex_type_F', '2dof', 'flex_type_M', '3dof', 'actuator_type', '1dof', 'actuator_kp', 0);
|
|
|
|
% With no payload
|
|
G_no_kp = linearize(mdl, io);
|
|
G_no_kp.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_no_kp.OutputName = {'dL1', 'dL2', 'dL3', 'dL4', 'dL5', 'dL6', ...
|
|
'fn1', 'fn2', 'fn3', 'fn4', 'fn5', 'fn6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Transfer function from actuator force inputs to displacement of each strut
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$l_i/f_i$')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$l_i/f_j$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-4]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(i,i), freqs, 'Hz'))), 'color', [colors(1,:),0.5]);
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_multi_body_plant_dL.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Transfer function from actuator force inputs to force sensor in each strut
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G(6+i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(7,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$')
|
|
plot(freqs, abs(squeeze(freqresp(G_no_kp(7,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$f_{ni}/f_i$ (no $k_p$)')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G(6+i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_no_kp(6+i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G(7,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$f_{ni}/f_j$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [N/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-4, 1e2]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G(6+i,i), freqs, 'Hz'))), 'color', colors(1,:));
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_no_kp(6+i,i), freqs, 'Hz'))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_multi_body_plant_fm.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_multi_body_plant
|
|
#+caption: Bode plot of the transfer functions computed from the nano-hexapod multi-body model
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_multi_body_plant_dL}$\bm{f}$ to $\bm{\mathcal{L}}$}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/nhexa_multi_body_plant_dL.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_multi_body_plant_fm}$\bm{f}$ to $\bm{f}_{n}$}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/nhexa_multi_body_plant_fm.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
The multi-body modeling approach presented in this section provides a comprehensive framework for analyzing the dynamics of the nano-hexapod system.
|
|
Through comparison with analytical solutions in a simplified configuration, the model's accuracy has been validated, demonstrating its ability to capture the essential dynamic behavior of the Stewart platform.
|
|
|
|
A key advantage of this modeling approach lies in its flexibility for future refinements.
|
|
While the current implementation employs idealized joints for the conceptual design phase, the framework readily accommodates the incorporation of joint stiffness and other non-ideal effects.
|
|
The joint stiffness, known to impact the performance of decentralized IFF control strategy [[cite:&preumont07_six_axis_singl_stage_activ]], can be studied as the design evolved and will be optimized during the detail design phase.
|
|
The validated multi-body model will serve as a valuable tool for predicting system behavior and evaluating control performance throughout the design process.
|
|
|
|
* Control of Stewart Platforms
|
|
:PROPERTIES:
|
|
:HEADER-ARGS:matlab+: :tangle matlab/nhexa_3_control.m
|
|
:END:
|
|
<<sec:nhexa_control>>
|
|
** Introduction :ignore:
|
|
|
|
The control of Stewart platforms presents distinct challenges compared to the uniaxial model due to their multi-input multi-output nature.
|
|
While the uniaxial model demonstrated the effectiveness of the HAC-LAC strategy, its extension to Stewart platforms requires careful consideration discussed in this section.
|
|
|
|
First, the distinction between centralized and decentralized control approaches is discussed in Section ref:ssec:nhexa_control_centralized_decentralized.
|
|
The impact of the control space selection - either Cartesian or strut space - is then analyzed in Section ref:ssec:nhexa_control_space, highlighting the trade-offs between direction-specific tuning and implementation simplicity.
|
|
|
|
Building upon these analyses, a decentralized active damping strategy using Integral Force Feedback is developed in Section ref:ssec:nhexa_control_iff, followed by the implementation of a centralized High Authority Control for positioning in Section ref:ssec:nhexa_control_hac_lac.
|
|
This architecture, while simple, will be used to demonstrate the feasibility of the NASS concept and will provide a foundation for more sophisticated control strategies to be developed during the detailed design phase.
|
|
|
|
** Matlab Init :noexport:ignore:
|
|
#+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
|
|
|
|
** Centralized and Decentralized Control
|
|
<<ssec:nhexa_control_centralized_decentralized>>
|
|
|
|
In the control of MIMO systems and more specifically of Stewart platforms, a fundamental architectural decision lies in the choice between centralized and decentralized control strategies.
|
|
|
|
In decentralized control, each actuator operates based on feedback from its associated sensor only, creating independent control loops as illustrated in Figure ref:fig:nhexa_stewart_decentralized_control.
|
|
While mechanical coupling between the struts exists, the control decisions are made locally, with each controller processing information from a single sensor-actuator pair.
|
|
This approach offers simplicity in implementation and reduced computational requirements.
|
|
|
|
Conversely, centralized control utilizes information from all sensors to determine the control action for each actuator.
|
|
This strategy potentially enables better performance by explicitly accounting for the mechanical coupling between the struts, though at the cost of increased complexity in both design and implementation.
|
|
|
|
The choice between these approaches depends significantly on the degree of interaction between the different control channels, but also on the available sensors and actuators.
|
|
For instance, when using external metrology systems that measure the platform's global position, centralized control becomes necessary as each sensor measurement depends on all actuator inputs.
|
|
|
|
In the context of the nano-hexapod, two distinct control strategies will be examined during the conceptual phase:
|
|
- Decentralized Integral Force Feedback (IFF), which utilizes collocated force sensors to implement independent control loops for each strut (Section ref:ssec:nhexa_control_iff)
|
|
- High-Authority Control (HAC), which employs a centralized approach to achieve precise positioning based on external metrology measurements (Section ref:ssec:nhexa_control_hac_lac)
|
|
|
|
#+name: fig:nhexa_stewart_decentralized_control
|
|
#+caption: Decentralized control strategy using the encoders. The two controllers for the struts on the back are not shown for simplicity.
|
|
[[file:figs/nhexa_stewart_decentralized_control.png]]
|
|
|
|
** Choice of the Control Space
|
|
<<ssec:nhexa_control_space>>
|
|
|
|
When controlling a Stewart platform using external metrology that measures the pose of frame $\{B\}$ with respect to $\{A\}$, denoted as $\bm{\mathcal{X}}$, the control architecture can be implemented in either Cartesian space or strut space.
|
|
This choice impacts both the control design and the obtained performance.
|
|
|
|
**** Control in the Strut space
|
|
|
|
In this approach, illustrated in Figure ref:fig:nhexa_control_strut, the control is performed in the space of the struts.
|
|
The Jacobian matrix is used to solve the inverse kinematics in real-time, mapping position errors from Cartesian space $\bm{\epsilon}_{\mathcal{X}}$ to strut space $\bm{\epsilon}_{\mathcal{L}}$.
|
|
A diagonal controller then processes these strut-space errors to generate force commands for each actuator.
|
|
|
|
The main advantage of this approach emerges from the plant characteristics in strut space, as shown in Figure ref:fig:nhexa_plant_frame_struts.
|
|
The diagonal terms of the plant (transfer functions from force to displacement of the same strut, as measured by the external metrology) are identical due to the system's symmetry.
|
|
This simplifies the control design as only one controller needs to be tuned.
|
|
Furthermore, at low frequencies, the plant exhibits good decoupling between struts, allowing for effective independent control of each axis.
|
|
|
|
#+begin_src latex :file nhexa_control_strut.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{2.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
|
|
|
|
\node[block={2.0cm}{2.0cm}, left=0.8 of inputF] (K) {\begin{matrix}K_1 & & 0 \\ & \ddots & \\ 0 & & K_6\end{matrix}};
|
|
\node[block, left=0.8 of K] (J) {$\bm{J}$};
|
|
\node[addb={+}{}{}{}{-}, left=0.8 of J] (subr) {};
|
|
% \node[block, align=center, left=0.6 of subr] (J) {Inverse\\Kinematics};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputX) -- ++(0.8, 0);
|
|
\draw[->] ($(outputX) + (0.3, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -1.2) -| (subr.south);
|
|
\draw[->] (subr.east) -- node[midway, above]{$\bm{\epsilon}_{\mathcal{X}}$} (J.west);
|
|
\draw[->] (J.east) -- node[midway, above]{$\bm{\epsilon}_{\mathcal{L}}$} (K.west);
|
|
\draw[->] (K.east) -- node[midway, above]{$\bm{f}$} (inputF);
|
|
|
|
\draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-0.8, 0);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+begin_src latex :file nhexa_control_cartesian.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{2.0cm}] (P) {Plant};
|
|
\coordinate[] (inputF) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputX) at ($(P.south east)!0.5!(P.north east)$);
|
|
|
|
\node[block, left=0.8 of inputF] (J) {$\bm{J}^{-T}$};
|
|
\node[block={2.0cm}{2.0cm}, left=0.8 of J] (K) {\begin{matrix}K_{D_x} & & 0 \\ & \ddots & \\ 0 & & K_{R_z}\end{matrix}};
|
|
\node[addb={+}{}{}{}{-}, left=0.8 of K] (subr) {};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputX) -- ++(0.8, 0);
|
|
\draw[->] ($(outputX) + (0.3, 0)$)node[branch]{} node[above]{$\bm{\mathcal{X}}$} -- ++(0, -1.2) -| (subr.south);
|
|
|
|
\draw[->] (subr.east) -- node[midway, above]{$\bm{\epsilon}_{\mathcal{X}}$} (K.west);
|
|
\draw[->] (K.east) -- node[midway, above]{$\bm{\mathcal{F}}$} (J.west);
|
|
\draw[->] (J.east) -- node[midway, above]{$\bm{f}$} (inputF.west);
|
|
\draw[<-] (subr.west)node[above left]{$\bm{r}_{\mathcal{X}}$} -- ++(-0.8, 0);
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_control_frame
|
|
#+caption: Two control strategies
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_control_strut}Control in the frame of the struts. $\bm{J}$ is used to project errors in the frame of the struts}
|
|
#+attr_latex: :options {0.98\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/nhexa_control_strut.png]]
|
|
#+end_subfigure
|
|
|
|
\bigskip
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_control_cartesian}Control in the Cartesian frame. $\bm{J}^{-T}$ is used to project force and torques on each strut}
|
|
#+attr_latex: :options {0.98\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 1
|
|
[[file:figs/nhexa_control_cartesian.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
**** Control in Cartesian Space
|
|
|
|
Alternatively, control can be implemented directly in Cartesian space, as shown in Figure ref:fig:nhexa_control_cartesian.
|
|
Here, the controller processes Cartesian errors $\bm{\epsilon}_{\mathcal{X}}$ to generate forces and torques $\bm{\mathcal{F}}$, which are then mapped to actuator forces through the transpose of the inverse Jacobian matrix.
|
|
|
|
The plant behavior in Cartesian space, illustrated in Figure ref:fig:nhexa_plant_frame_cartesian, reveals interesting characteristics.
|
|
Some degrees of freedom, particularly the vertical translation and rotation about the vertical axis, exhibit simpler second-order dynamics.
|
|
A key advantage of this approach is that control performance can be individually tuned for each direction.
|
|
This is particularly valuable when performance requirements differ between degrees of freedom - for instance, when higher positioning accuracy is required vertically than horizontally, or when certain rotational degrees of freedom can tolerate larger errors than others.
|
|
|
|
However, significant coupling exists between certain degrees of freedom, particularly between rotations and translations (e.g., $\epsilon_{R_x}/\mathcal{F}_y$ or $\epsilon_{D_y}/\bm\mathcal{M}_x$).
|
|
|
|
For the conceptual validation of the nano-hexapod, control in the strut space has been selected due to its simpler implementation and the beneficial decoupling properties observed at low frequencies.
|
|
More sophisticated control strategies will be explored during the detailed design phase.
|
|
|
|
#+begin_src matlab
|
|
%% Identify plant from actuator forces to external metrology
|
|
stewart = initializeSimplifiedNanoHexapod();
|
|
initializeSample('type', 'cylindrical', 'm', 10, 'H', 300e-3);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External Metrology [m, rad]
|
|
|
|
% With no payload
|
|
G = linearize(mdl, io);
|
|
G.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
|
|
%% Plant in the Cartesian Frame
|
|
G_cart = G*inv(stewart.geometry.J');
|
|
G_cart.InputName = {'Fx', 'Fy', 'Fz', 'Mx', 'My', 'Mz'};
|
|
|
|
%% Plant in the frame of the struts
|
|
G_struts = stewart.geometry.J*G;
|
|
G_struts.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Bode plot of the plant projected in the frame of the struts
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i$')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(1,2), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_j$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-4]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', [colors(1,:),0.5]);
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_plant_frame_struts.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Bode plot of the plant projected in the Cartesian frame
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(i,j), freqs, 'Hz'))), 'color', [0, 0, 0, 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$\epsilon_{D_x}/\mathcal{F}_x$ [m/N]')
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(2,2), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$\epsilon_{D_y}/\mathcal{F}_y$ [m/N]')
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(3,3), freqs, 'Hz'))), 'color', colors(3,:), ...
|
|
'DisplayName', '$\epsilon_{D_z}/\mathcal{F}_z$ [m/N]')
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(4,4), freqs, 'Hz'))), 'color', colors(4,:), ...
|
|
'DisplayName', '$\epsilon_{R_x}/\mathcal{M}_x$ [rad/Nm]')
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(5,5), freqs, 'Hz'))), 'color', colors(5,:), ...
|
|
'DisplayName', '$\epsilon_{R_y}/\mathcal{M}_y$ [rad/Nm]')
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(6,6), freqs, 'Hz'))), 'color', colors(6,:), ...
|
|
'DisplayName', '$\epsilon_{R_z}/\mathcal{M}_z$ [rad/Nm]')
|
|
plot(freqs, abs(squeeze(freqresp(G_cart(1,5), freqs, 'Hz'))), 'color', [0, 0, 0, 0.5], ...
|
|
'DisplayName', 'Coupling')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 4e-3]);
|
|
leg = legend('location', 'southwest', 'FontSize', 7, 'NumColumns', 3);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_cart(i,i), freqs, 'Hz'))), 'color', colors(i,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_plant_frame_cartesian.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_plant_frame
|
|
#+caption: Bode plot of the transfer functions computed from the nano-hexapod multi-body model
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_plant_frame_struts}Plant in the frame of the struts}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/nhexa_plant_frame_struts.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_plant_frame_cartesian}Plant in the Cartesian Frame}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width \linewidth
|
|
[[file:figs/nhexa_plant_frame_cartesian.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Active Damping with Decentralized IFF
|
|
<<ssec:nhexa_control_iff>>
|
|
|
|
The decentralized Integral Force Feedback (IFF) control strategy is implemented using independent control loops for each strut, similarly to what is shown in Figure ref:fig:nhexa_stewart_decentralized_control, but using force sensors instead of relative motion sensors.
|
|
|
|
The corresponding block diagram of the control loop is shown in Figure ref:fig:nhexa_decentralized_iff_schematic, in which the controller $\bm{K}_{\text{IFF}}(s)$ is a diagonal matrix where each diagonal element is a pure integrator eqref:eq:nhexa_kiff.
|
|
|
|
#+begin_src latex :file nhexa_decentralized_iff_schematic.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{2.0cm}] (P) {Plant};
|
|
\coordinate[] (input) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputH) at ($(P.south east)!0.2!(P.north east)$);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);
|
|
|
|
\node[block, above=0.2 of P] (Klac) {$\bm{K}_\text{IFF}$};
|
|
\node[addb, left=0.8 of input] (addF) {};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputL) -- ++(0.7, 0) coordinate(eastlac) |- (Klac.east);
|
|
\node[above right] at (outputL){$\bm{f}_n$};
|
|
\draw[->] (Klac.west) -| (addF.north);
|
|
\draw[->] (addF.east) -- (input) node[above left]{$\bm{f}$};
|
|
|
|
\draw[->] (outputH) -- ++(1.7, 0) node[above left]{$\bm{\mathcal{L}}$};
|
|
\draw[<-] (addF.west) -- ++(-0.8, 0) node[above right]{$\bm{f}^{\prime}$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(Klac.north-|eastlac) (addF.west|-P.south)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Pi) {};
|
|
\node[anchor={north west}] at (Pi.north west){\footnotesize{Damped Plant}};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_decentralized_iff_schematic
|
|
#+caption: Schematic of the implemented decentralized IFF controller. The damped plant has a new inputs $\bm{f}^{\prime}$
|
|
#+RESULTS:
|
|
[[file:figs/nhexa_decentralized_iff_schematic.png]]
|
|
|
|
|
|
\begin{equation}\label{eq:nhexa_kiff}
|
|
\bm{K}_{\text{IFF}}(s) = g \cdot \begin{bmatrix}
|
|
K_{\text{IFF}}(s) & & 0 \\
|
|
& \ddots & \\
|
|
0 & & K_{\text{IFF}}(s)
|
|
\end{bmatrix}, \quad K_{\text{IFF}}(s) = \frac{1}{s}
|
|
\end{equation}
|
|
|
|
|
|
In this section, the stiffness in parallel with the force sensor has been omitted since the Stewart platform is not subjected to rotation.
|
|
The effect of this parallel stiffness will be examined in the next section when the platform is integrated into the complete NASS system.
|
|
|
|
The Root Locus analysis, shown in Figure ref:fig:nhexa_decentralized_iff_root_locus, reveals the evolution of the closed-loop poles as the controller gain $g$ varies from $0$ to $\infty$.
|
|
A key characteristic of force feedback control with collocated sensor-actuator pairs is observed: all closed-loop poles are bounded to the left-half plane, indicating guaranteed stability [[cite:&preumont08_trans_zeros_struc_contr_with]].
|
|
This property is particularly valuable as the coupling is very large around resonance frequencies, enabling control of modes that would be difficult to include within the bandwidth using position feedback alone.
|
|
|
|
The bode plot of an individual loop gain (i.e. the loop gain of $K_{\text{IFF}}(s) \cdot \frac{f_{ni}}{f_i}(s)$), presented in Figure ref:fig:nhexa_decentralized_iff_loop_gain, exhibits the typical characteristics of integral force feedback of having a phase bounded between $-90^o$ and $+90^o$.
|
|
The loop-gain is high around the resonance frequencies, indicating that the decentralized IFF provides significant control authority over these modes.
|
|
This high gain, combined with the bounded phase, enables effective damping of the resonant modes while maintaining stability.
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant
|
|
stewart = initializeSimplifiedNanoHexapod('actuator_kp', 0); % Ignoring parallel stiffness for now
|
|
initializeSample('type', 'cylindrical', 'm', 10, 'H', 300e-3);
|
|
initializeLoggingConfiguration('log', 'none');
|
|
initializeController('type', 'open-loop');
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'openinput'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/plant'], 2, 'openoutput', [], 'fn'); io_i = io_i + 1; % Force Sensors [N]
|
|
|
|
% With no payload
|
|
G_iff = linearize(mdl, io);
|
|
G_iff.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_iff.OutputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab
|
|
%% IFF Controller Design
|
|
Kiff = -500/s * ... % Gain
|
|
eye(6); % Diagonal 6x6 controller (i.e. decentralized)
|
|
|
|
Kiff.InputName = {'fm1', 'fm2', 'fm3', 'fm4', 'fm5', 'fm6'};
|
|
Kiff.OutputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Root Locus plot of the Decentralized IFF Control
|
|
gains = logspace(-2, 1, 200);
|
|
|
|
figure;
|
|
tiledlayout(1, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
nexttile();
|
|
hold on;
|
|
plot(real(pole(G_iff)), imag(pole(G_iff)), 'x', 'color', colors(1,:), ...
|
|
'DisplayName', '$g = 0$');
|
|
plot(real(tzero(G_iff)), imag(tzero(G_iff)), 'o', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
|
|
for g = gains
|
|
clpoles = pole(feedback(G_iff, g*Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), '.', 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
|
|
% Optimal gain
|
|
clpoles = pole(feedback(G_iff, Kiff, +1));
|
|
plot(real(clpoles), imag(clpoles), 'kx', ...
|
|
'DisplayName', '$g_{opt}$');
|
|
hold off;
|
|
axis equal;
|
|
xlim([-600, 50]); ylim([-50, 600]);
|
|
xticks([-600:100:0]);
|
|
yticks([0:100:600]);
|
|
set(gca, 'XTickLabel',[]); set(gca, 'YTickLabel',[]);
|
|
xlabel('Real part'); ylabel('Imaginary part');
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_decentralized_iff_root_locus.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Loop gain for the Decentralized IFF
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(-G_iff(1,1)*Kiff(1,1), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-2, 1e2]);
|
|
% leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
% leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(-G_iff(1,1)*Kiff(1,1), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_decentralized_iff_loop_gain.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_decentralized_iff_results
|
|
#+caption: Decentralized IFF
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_iff_loop_gain}Loop Gain: bode plot of $K_{\text{IFF}}(s) \frac{f_{n1}}{f_1}(s)$}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.85
|
|
[[file:figs/nhexa_decentralized_iff_loop_gain.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_iff_root_locus}Root Locus}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.85
|
|
[[file:figs/nhexa_decentralized_iff_root_locus.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** MIMO High-Authority Control - Low-Authority Control
|
|
<<ssec:nhexa_control_hac_lac>>
|
|
|
|
The design of the High Authority Control positioning loop is now examined.
|
|
The complete HAC-IFF control architecture is illustrated in Figure ref:fig:nhexa_hac_iff_schematic, where the reference signal $\bm{r}_{\mathcal{X}}$ represents the desired pose, and $\bm{\mathcal{X}}$ is the measured pose by the external metrology system.
|
|
|
|
Following the analysis from Section ref:ssec:nhexa_control_space, the control is implemented in the strut space.
|
|
The Jacobian matrix $\bm{J}^{-1}$ performs real-time approximate inverse kinematics to map position errors from Cartesian space $\bm{\epsilon}_{\mathcal{X}}$ to strut space $\bm{\epsilon}_{\mathcal{L}}$.
|
|
A diagonal High Authority Controller $\bm{K}_{\text{HAC}}$ then processes these errors in the frame of the struts.
|
|
|
|
#+begin_src latex :file nhexa_hac_iff_schematic.pdf
|
|
\begin{tikzpicture}
|
|
% Blocs
|
|
\node[block={2.0cm}{2.0cm}] (P) {Plant};
|
|
\coordinate[] (input) at ($(P.south west)!0.5!(P.north west)$);
|
|
\coordinate[] (outputH) at ($(P.south east)!0.2!(P.north east)$);
|
|
\coordinate[] (outputL) at ($(P.south east)!0.8!(P.north east)$);
|
|
|
|
\node[block, above=0.2 of P] (Klac) {$\bm{K}_\text{IFF}$};
|
|
\node[addb, left=0.8 of input] (addF) {};
|
|
|
|
\node[block, left=0.8 of addF] (Khac) {$\bm{K}_\text{HAC}$};
|
|
\node[block, left=0.8 of Khac] (inverseK) {$\bm{J}^{-1}$};
|
|
|
|
\node[addb={+}{}{}{}{-}, left=0.8 of inverseK] (subL) {};
|
|
|
|
% Connections and labels
|
|
\draw[->] (outputL) -- ++(0.7, 0) coordinate(eastlac) |- (Klac.east);
|
|
\node[above right] at (outputL){$\bm{f}_n$};
|
|
\draw[->] (Klac.west) -| (addF.north);
|
|
\draw[->] (addF.east) -- (input) node[above left]{$\bm{f}$};
|
|
|
|
\draw[->] (outputH) -- ++(1.7, 0) node[above left]{$\bm{\mathcal{X}}$};
|
|
\draw[->] (Khac.east) node[above right]{$\bm{f}^{\prime}$} -- (addF.west);
|
|
|
|
\draw[->] ($(outputH) + (1.2, 0)$)node[branch]{} |- ($(subL.south)+(0, -1.2)$) -- (subL.south);
|
|
\draw[->] (subL.east) -- (inverseK.west) node[above left]{$\bm{\epsilon}_\mathcal{X}$};
|
|
\draw[->] (inverseK.east) -- (Khac.west) node[above left]{$\bm{\epsilon}_\mathcal{L}$};
|
|
|
|
\draw[<-] (subL.west) -- ++(-0.8, 0) node[above right]{$\bm{r}_\mathcal{X}$};
|
|
|
|
\begin{scope}[on background layer]
|
|
\node[fit={(Klac.north-|eastlac) (addF.west|-P.south)}, fill=black!20!white, draw, dashed, inner sep=8pt] (Pi) {};
|
|
\node[anchor={north west}] at (Pi.north west){\footnotesize{Damped Plant}};
|
|
\end{scope}
|
|
\end{tikzpicture}
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_hac_iff_schematic
|
|
#+caption: HAC-IFF control architecture with the High Authority Controller being implemented in the frame of the struts
|
|
#+RESULTS:
|
|
[[file:figs/nhexa_hac_iff_schematic.png]]
|
|
|
|
The effect of decentralized IFF on the plant dynamics can be observed by comparing two sets of transfer functions.
|
|
Figure ref:fig:nhexa_decentralized_hac_iff_plant_undamped shows the original transfer functions from actuator forces $\bm{f}$ to strut errors $\bm{\epsilon}_{\mathcal{L}}$, characterized by pronounced resonant peaks.
|
|
When decentralized IFF is implemented, the transfer functions from modified inputs $\bm{f}^{\prime}$ to strut errors $\bm{\epsilon}_{\mathcal{L}}$, shown in Figure ref:fig:nhexa_decentralized_hac_iff_plant_damped, exhibit significantly attenuated resonances.
|
|
This damping of structural resonances serves two purposes: it reduces vibrations in the vicinity of resonances and simplifies the design of the high authority controller by providing a simpler plant dynamics.
|
|
|
|
#+begin_src matlab
|
|
%% Identify the IFF Plant
|
|
initializeController('type', 'iff');
|
|
|
|
% Input/Output definition
|
|
clear io; io_i = 1;
|
|
io(io_i) = linio([mdl, '/Controller'], 1, 'input'); io_i = io_i + 1; % Actuator Inputs [N]
|
|
io(io_i) = linio([mdl, '/plant'], 1, 'openoutput'); io_i = io_i + 1; % External Metrology [m,rad]
|
|
|
|
% With no payload
|
|
G_hac = linearize(mdl, io);
|
|
G_hac.InputName = {'f1', 'f2', 'f3', 'f4', 'f5', 'f6'};
|
|
G_hac.OutputName = {'Dx', 'Dy', 'Dz', 'Rx', 'Ry', 'Rz'};
|
|
|
|
%% Plant in the frame of the struts
|
|
G_hac_struts = stewart.geometry.J*G_hac;
|
|
G_hac_struts.OutputName = {'D1', 'D2', 'D3', 'D4', 'D5', 'D6'};
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Bode plot of the plant projected in the frame of the struts
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(i,j), freqs, 'Hz'))), 'color', [0,0,0,0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(1,1), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i$')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', colors(1,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(1,2), freqs, 'Hz'))), 'color', [0,0,0,0.1], ...
|
|
'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_j$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-4]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', colors(1,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_decentralized_hac_iff_plant_undamped.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Bode plot of the plant projected in the frame of the struts
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'Compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
for i = 1:5
|
|
for j = i+1:6
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_struts(i,j), freqs, 'Hz'))), 'color', [0,0,0,0.1], ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(1,1), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i$')
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_struts(1,1), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_i^\prime$')
|
|
for i = 2:6
|
|
plot(freqs, abs(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.2], ...
|
|
'HandleVisibility', 'off');
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_struts(i,i), freqs, 'Hz'))), 'color', colors(2,:), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_struts(1,2), freqs, 'Hz'))), 'color', [0,0,0,0.1], ...
|
|
'DisplayName', '$-\epsilon_{\mathcal{L}i}/f_j^\prime$')
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Amplitude [m/N]'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-9, 1e-4]);
|
|
leg = legend('location', 'northwest', 'FontSize', 8, 'NumColumns', 1);
|
|
leg.ItemTokenSize(1) = 15;
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_struts(i,i), freqs, 'Hz'))), 'color', [colors(1,:), 0.2]);
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_struts(i,i), freqs, 'Hz'))), 'color', colors(2,:));
|
|
end
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
ylabel('Phase [deg]'); xlabel('Frequency [Hz]');
|
|
ylim([-180, 180]);
|
|
yticks([-180, -90, 0, 90, 180]);
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([freqs(1), freqs(end)]);
|
|
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_decentralized_hac_iff_plant_damped.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_decentralized_hac_iff_plant
|
|
#+caption: Plant in the frame of the strut for the High Authority Controller.
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_hac_iff_plant_undamped}Undamped plant in the frame of the struts}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nhexa_decentralized_hac_iff_plant_undamped.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_hac_iff_plant_damped}Damped plant with Decentralized IFF}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :width 0.95\linewidth
|
|
[[file:figs/nhexa_decentralized_hac_iff_plant_damped.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
Building upon the damped plant dynamics shown in Figure ref:fig:nhexa_decentralized_hac_iff_plant_damped, a high authority controller is designed with the structure given in eqref:eq:nhexa_khac.
|
|
The controller combines three elements: an integrator providing high gain at low frequencies, a lead compensator improving stability margins, and a low-pass filter for robustness to unmodeled high-frequency dynamics.
|
|
The loop gain of an individual control channel is shown in Figure ref:fig:nhexa_decentralized_hac_iff_loop_gain.
|
|
|
|
\begin{equation}\label{eq:nhexa_khac}
|
|
\bm{K}_{\text{HAC}}(s) = \begin{bmatrix}
|
|
K_{\text{HAC}}(s) & & 0 \\
|
|
& \ddots & \\
|
|
0 & & K_{\text{HAC}}(s)
|
|
\end{bmatrix}, \quad K_{\text{HAC}}(s) = g_0 \cdot \underbrace{\frac{\omega_c}{s}}_{\text{int}} \cdot \underbrace{\frac{1}{\sqrt{\alpha}}\frac{1 + \frac{s}{\omega_c/\sqrt{\alpha}}}{1 + \frac{s}{\omega_c\sqrt{\alpha}}}}_{\text{lead}} \cdot \underbrace{\frac{1}{1 + \frac{s}{\omega_0}}}_{\text{LPF}}
|
|
\end{equation}
|
|
|
|
The stability of the MIMO feedback loop is analyzed through the /characteristic loci/ method.
|
|
Such characteristic loci, shown in Figure ref:fig:nhexa_decentralized_hac_iff_root_locus, represent the eigenvalues of the loop gain matrix $\bm{G}(j\omega)\bm{K}(j\omega)$ plotted in the complex plane as frequency varies from $0$ to $\infty$.
|
|
For MIMO systems, this method generalizes the classical Nyquist stability criterion: with the open-loop system being stable, the closed-loop system is stable if none of the characteristic loci encircle the -1 point [[cite:&skogestad07_multiv_feedb_contr]].
|
|
As seen in Figure ref:fig:nhexa_decentralized_hac_iff_root_locus, all loci remain to the right of the -1 point, confirming the stability of the closed-loop system. Additionally, the distance of the loci from the -1 point provides information about stability margins for the coupled system.
|
|
|
|
#+begin_src matlab :exports none
|
|
%% High Authority Controller - Mid Stiffness Nano-Hexapod
|
|
% Wanted crossover
|
|
wc = 2*pi*20; % [rad/s]
|
|
|
|
% Integrator
|
|
H_int = wc/s;
|
|
|
|
% Lead to increase phase margin
|
|
a = 2; % Amount of phase lead / width of the phase lead / high frequency gain
|
|
H_lead = 1/sqrt(a)*(1 + s/(wc/sqrt(a)))/(1 + s/(wc*sqrt(a)));
|
|
|
|
% Low Pass filter to increase robustness
|
|
H_lpf = 1/(1 + s/2/pi/200);
|
|
|
|
% Gain to have unitary crossover at 5Hz
|
|
H_gain = 1./abs(evalfr(G_hac_struts(1, 1), 1j*wc));
|
|
|
|
% Decentralized HAC
|
|
Khac = H_gain * ... % Gain
|
|
H_int * ... % Integrator
|
|
H_lpf * ... % Low Pass filter
|
|
eye(6); % 6x6 Diagonal
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none
|
|
%% Plot of the eigenvalues of L in the complex plane
|
|
Ldet = zeros(6, length(freqs));
|
|
Lmimo = squeeze(freqresp(G_hac_struts*Khac, freqs, 'Hz'));
|
|
for i_f = 2:length(freqs)
|
|
Ldet(:, i_f) = eig(squeeze(Lmimo(:,:,i_f)));
|
|
end
|
|
|
|
figure;
|
|
hold on;
|
|
for i = 1:6
|
|
plot(real(squeeze(Ldet(i,:))), imag(squeeze(Ldet(i,:))), ...
|
|
'.', 'color', colors(1, :), ...
|
|
'HandleVisibility', 'off');
|
|
plot(real(squeeze(Ldet(i,:))), -imag(squeeze(Ldet(i,:))), ...
|
|
'.', 'color', colors(1, :), ...
|
|
'HandleVisibility', 'off');
|
|
end
|
|
plot(-1, 0, 'kx', 'HandleVisibility', 'off');
|
|
hold off;
|
|
set(gca, 'XScale', 'lin'); set(gca, 'YScale', 'lin');
|
|
xlabel('Real Part'); ylabel('Imaginary Part');
|
|
axis square
|
|
xlim([-1.8, 0.2]); ylim([-1, 1]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_decentralized_hac_iff_root_locus.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :exports none :results none
|
|
%% Loop gain for the Decentralized HAC_IFF
|
|
figure;
|
|
tiledlayout(3, 1, 'TileSpacing', 'compact', 'Padding', 'None');
|
|
|
|
ax1 = nexttile([2,1]);
|
|
hold on;
|
|
plot(freqs, abs(squeeze(freqresp(G_hac_struts(1,1)*Khac(1,1), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'log');
|
|
ylabel('Loop Gain'); set(gca, 'XTickLabel',[]);
|
|
ylim([1e-2, 1e2]);
|
|
|
|
ax2 = nexttile;
|
|
hold on;
|
|
plot(freqs, 180/pi*angle(squeeze(freqresp(G_hac_struts(1,1)*Khac(1,1), freqs, 'Hz'))));
|
|
hold off;
|
|
set(gca, 'XScale', 'log'); set(gca, 'YScale', 'lin');
|
|
xlabel('Frequency [Hz]'); ylabel('Phase [deg]');
|
|
hold off;
|
|
yticks(-360:90:360);
|
|
ylim([-180, 180])
|
|
|
|
linkaxes([ax1,ax2],'x');
|
|
xlim([1, 1e3]);
|
|
#+end_src
|
|
|
|
#+begin_src matlab :tangle no :exports results :results file none
|
|
exportFig('figs/nhexa_decentralized_hac_iff_loop_gain.pdf', 'width', 'half', 'height', 600);
|
|
#+end_src
|
|
|
|
#+name: fig:nhexa_decentralized_hac_iff_results
|
|
#+caption: Decentralized HAC-IFF
|
|
#+attr_latex: :options [htbp]
|
|
#+begin_figure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_hac_iff_loop_gain}Loop Gain}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.85
|
|
[[file:figs/nhexa_decentralized_hac_iff_loop_gain.png]]
|
|
#+end_subfigure
|
|
#+attr_latex: :caption \subcaption{\label{fig:nhexa_decentralized_hac_iff_root_locus}Characteristic Loci}
|
|
#+attr_latex: :options {0.48\textwidth}
|
|
#+begin_subfigure
|
|
#+attr_latex: :scale 0.85
|
|
[[file:figs/nhexa_decentralized_hac_iff_root_locus.png]]
|
|
#+end_subfigure
|
|
#+end_figure
|
|
|
|
** Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
The control architecture developed for the uniaxial and the rotating models has been adapted for the Stewart platform.
|
|
|
|
Two fundamental choices were first addressed: the selection between centralized and decentralized approaches, and the choice of control space.
|
|
While control in Cartesian space enables direction-specific performance tuning, the implementation in strut space was selected for the conceptual design phase due to two key advantages: good decoupling at low frequencies and identical diagonal terms in the plant transfer functions, allowing a single controller design to be replicated across all struts.
|
|
|
|
The HAC-LAC strategy was then implemented.
|
|
The inner loop implements decentralized Integral Force Feedback for active damping.
|
|
The collocated nature of the force sensors ensures stability despite strong coupling between struts at resonance frequencies, enabling effective damping of structural modes.
|
|
The outer loop implements High Authority Control, enabling precise positioning of the platform.
|
|
|
|
This control architecture will then be used for the conceptual validation of the NASS.
|
|
More sophisticated control strategies will be investigated during the detailed design phase
|
|
|
|
* Conclusion
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
<<sec:nhexa_conclusion>>
|
|
|
|
- Configurable Stewart platform model
|
|
- Will be included in the multi-body model of the micro-station => nass multi body model
|
|
- Control: complex problem, try to use simplest architecture
|
|
|
|
* 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/subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
%% Data directory
|
|
data_dir = './matlab/mat/'
|
|
#+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('./subsystems/'); % Path for Subsystems Simulink files
|
|
|
|
%% Data directory
|
|
data_dir = './mat/';
|
|
#+END_SRC
|
|
|
|
** Initialize Simscape Model
|
|
#+NAME: m-init-simscape
|
|
#+begin_src matlab
|
|
% Simulink Model name
|
|
mdl = 'nano_hexapod_model';
|
|
#+end_src
|
|
|
|
** Initialize other elements
|
|
#+NAME: m-init-other
|
|
#+BEGIN_SRC matlab
|
|
%% Colors for the figures
|
|
colors = colororder;
|
|
|
|
%% Frequency Vector
|
|
freqs = logspace(0, 3, 1000);
|
|
#+END_SRC
|
|
|
|
* Matlab Functions :noexport:
|
|
** =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/nano_hexapod_model_conf_log.mat', 'file')
|
|
save('mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('mat/nano_hexapod_model_conf_log.mat', 'conf_log');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nano_hexapod_model_conf_log.mat', 'file')
|
|
save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log', '-append');
|
|
else
|
|
save('matlab/mat/nano_hexapod_model_conf_log.mat', 'conf_log');
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeController=: Initialize Controller
|
|
#+begin_src matlab :tangle matlab/src/initializeController.m :comments none :mkdirp yes :eval no
|
|
function [] = initializeController(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'open-loop', 'iff'})} = 'open-loop'
|
|
end
|
|
|
|
controller = struct();
|
|
|
|
switch args.type
|
|
case 'open-loop'
|
|
controller.type = 1;
|
|
controller.name = 'Open-Loop';
|
|
case 'iff'
|
|
controller.type = 2;
|
|
controller.name = 'Decentralized Integral Force Feedback';
|
|
end
|
|
|
|
if exist('./mat', 'dir')
|
|
save('mat/nano_hexapod_model_controller.mat', 'controller');
|
|
elseif exist('./matlab', 'dir')
|
|
save('matlab/mat/nano_hexapod_model_controller.mat', 'controller');
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
** =initializeSample=: Sample
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeSample.m :comments none :mkdirp yes :eval no
|
|
function [sample] = initializeSample(args)
|
|
|
|
arguments
|
|
args.type char {mustBeMember(args.type,{'none', 'cylindrical'})} = 'none'
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 200e-3 % Height [m]
|
|
args.R (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius [m]
|
|
args.m (1,1) double {mustBeNumeric, mustBePositive} = 1 % Mass [kg]
|
|
end
|
|
|
|
sample = struct();
|
|
|
|
switch args.type
|
|
case 'none'
|
|
sample.type = 0;
|
|
sample.m = 0;
|
|
case 'cylindrical'
|
|
sample.type = 1;
|
|
|
|
sample.H = args.H;
|
|
sample.R = args.R;
|
|
sample.m = args.m;
|
|
end
|
|
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nano_hexapod.mat', 'file')
|
|
save('mat/nano_hexapod.mat', 'sample', '-append');
|
|
else
|
|
save('mat/nano_hexapod.mat', 'sample');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nano_hexapod.mat', 'file')
|
|
save('matlab/mat/nano_hexapod.mat', 'sample', '-append');
|
|
else
|
|
save('matlab/mat/nano_hexapod.mat', 'sample');
|
|
end
|
|
end
|
|
|
|
end
|
|
#+end_src
|
|
|
|
** Stewart platform
|
|
*** =initializeSimplifiedNanoHexapod=: Nano Hexapod
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeSimplifiedNanoHexapod.m :comments none :mkdirp yes :eval no
|
|
function [nano_hexapod] = initializeSimplifiedNanoHexapod(args)
|
|
|
|
arguments
|
|
%% initializeFramesPositions
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 95e-3 % Height of the nano-hexapod [m]
|
|
args.MO_B (1,1) double {mustBeNumeric} = 150e-3 % Height of {B} w.r.t. {M} [m]
|
|
%% generateGeneralConfiguration
|
|
args.FH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of fixed joints [m]
|
|
args.FR (1,1) double {mustBeNumeric, mustBePositive} = 120e-3 % Radius of fixed joints [m]
|
|
args.FTh (6,1) double {mustBeNumeric} = [220, 320, 340, 80, 100, 200]*(pi/180) % Angles of fixed joints [rad]
|
|
args.MH (1,1) double {mustBeNumeric, mustBePositive} = 15e-3 % Height of mobile joints [m]
|
|
args.MR (1,1) double {mustBeNumeric, mustBePositive} = 110e-3 % Radius of mobile joints [m]
|
|
args.MTh (6,1) double {mustBeNumeric} = [255, 285, 15, 45, 135, 165]*(pi/180) % Angles of fixed joints [rad]
|
|
%% Actuators
|
|
args.actuator_type char {mustBeMember(args.actuator_type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
|
args.actuator_k (1,1) double {mustBeNumeric, mustBePositive} = 1e6
|
|
args.actuator_kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e4
|
|
args.actuator_ke (1,1) double {mustBeNumeric, mustBePositive} = 4952605
|
|
args.actuator_ka (1,1) double {mustBeNumeric, mustBePositive} = 2476302
|
|
args.actuator_c (1,1) double {mustBeNumeric, mustBePositive} = 50
|
|
args.actuator_cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.actuator_ce (1,1) double {mustBeNumeric, mustBePositive} = 100
|
|
args.actuator_ca (1,1) double {mustBeNumeric, mustBePositive} = 50
|
|
%% initializeCylindricalPlatforms
|
|
args.Fpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the fixed plate [kg]
|
|
args.Fph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the fixed plate [m]
|
|
args.Fpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the fixed plate [m]
|
|
args.Mpm (1,1) double {mustBeNumeric, mustBePositive} = 5 % Mass of the mobile plate [kg]
|
|
args.Mph (1,1) double {mustBeNumeric, mustBePositive} = 10e-3 % Thickness of the mobile plate [m]
|
|
args.Mpr (1,1) double {mustBeNumeric, mustBePositive} = 150e-3 % Radius of the mobile plate [m]
|
|
%% initializeCylindricalStruts
|
|
args.Fsm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the fixed part of the strut [kg]
|
|
args.Fsh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the fixed part of the struts [m]
|
|
args.Fsr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
|
args.Msm (1,1) double {mustBeNumeric, mustBePositive} = 1e-3 % Mass of the mobile part of the strut [kg]
|
|
args.Msh (1,1) double {mustBeNumeric, mustBePositive} = 60e-3 % Length of the mobile part of the struts [m]
|
|
args.Msr (1,1) double {mustBeNumeric, mustBePositive} = 5e-3 % Radius of the fixed part of the struts [m]
|
|
%% Bottom and Top Flexible Joints
|
|
args.flex_type_F char {mustBeMember(args.flex_type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
|
args.flex_type_M char {mustBeMember(args.flex_type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
|
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
%% 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, ...
|
|
'type', args.actuator_type, ...
|
|
'k', args.actuator_k, ...
|
|
'kp', args.actuator_kp, ...
|
|
'ke', args.actuator_ke, ...
|
|
'ka', args.actuator_ka, ...
|
|
'c', args.actuator_c, ...
|
|
'cp', args.actuator_cp, ...
|
|
'ce', args.actuator_ce, ...
|
|
'ca', args.actuator_ca);
|
|
|
|
stewart = initializeJointDynamics(stewart, ...
|
|
'type_F', args.flex_type_F, ...
|
|
'type_M', args.flex_type_M, ...
|
|
'Kf_M', args.Kf_M, ...
|
|
'Cf_M', args.Cf_M, ...
|
|
'Kt_M', args.Kt_M, ...
|
|
'Ct_M', args.Ct_M, ...
|
|
'Kf_F', args.Kf_F, ...
|
|
'Cf_F', args.Cf_F, ...
|
|
'Kt_F', args.Kt_F, ...
|
|
'Ct_F', args.Ct_F, ...
|
|
'Ka_F', args.Ka_F, ...
|
|
'Ca_F', args.Ca_F, ...
|
|
'Kr_F', args.Kr_F, ...
|
|
'Cr_F', args.Cr_F, ...
|
|
'Ka_M', args.Ka_M, ...
|
|
'Ca_M', args.Ca_M, ...
|
|
'Kr_M', args.Kr_M, ...
|
|
'Cr_M', args.Cr_M);
|
|
|
|
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);
|
|
|
|
nano_hexapod = stewart;
|
|
if exist('./mat', 'dir')
|
|
if exist('./mat/nano_hexapod.mat', 'file')
|
|
save('mat/nano_hexapod.mat', 'nano_hexapod', '-append');
|
|
else
|
|
save('mat/nano_hexapod.mat', 'nano_hexapod');
|
|
end
|
|
elseif exist('./matlab', 'dir')
|
|
if exist('./matlab/mat/nano_hexapod.mat', 'file')
|
|
save('matlab/mat/nano_hexapod.mat', 'nano_hexapod', '-append');
|
|
else
|
|
save('matlab/mat/nano_hexapod.mat', 'nano_hexapod');
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeStewartPlatform=: Initialize the Stewart Platform structure
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeStewartPlatform.m :comments none :mkdirp yes :eval no
|
|
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 -
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeFramesPositions=: Initialize the positions of frames {A}, {B}, {F} and {M}
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeFramesPositions.m :comments none :mkdirp yes :eval no
|
|
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]
|
|
|
|
arguments
|
|
stewart
|
|
args.H (1,1) double {mustBeNumeric, mustBePositive} = 90e-3
|
|
args.MO_B (1,1) double {mustBeNumeric} = 50e-3
|
|
end
|
|
|
|
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]
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =generateGeneralConfiguration=: Generate a Very General Configuration
|
|
|
|
#+begin_src matlab :tangle matlab/src/generateGeneralConfiguration.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
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
|
|
|
|
Fa = zeros(3,6);
|
|
Mb = zeros(3,6);
|
|
|
|
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
|
|
|
|
stewart.platform_F.Fa = Fa;
|
|
stewart.platform_M.Mb = Mb;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =computeJointsPose=: Compute the Pose of the Joints
|
|
|
|
#+begin_src matlab :tangle matlab/src/computeJointsPose.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
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;
|
|
|
|
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]);
|
|
|
|
As = (Ab - Aa)./vecnorm(Ab - Aa); % As_i is the i'th vector of As
|
|
|
|
l = vecnorm(Ab - Aa)';
|
|
|
|
Bs = (Bb - Ba)./vecnorm(Bb - Ba);
|
|
|
|
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
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeCylindricalPlatforms=: Initialize the geometry of the Fixed and Mobile Platforms
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeCylindricalPlatforms.m :comments none :mkdirp yes :eval no
|
|
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]
|
|
|
|
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
|
|
|
|
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]);
|
|
|
|
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]);
|
|
|
|
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;
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeCylindricalStruts=: Define the inertia of cylindrical struts
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeCylindricalStruts.m :comments none :mkdirp yes :eval no
|
|
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]
|
|
|
|
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
|
|
|
|
stewart.struts_M.type = 1;
|
|
|
|
stewart.struts_M.M = args.Msm;
|
|
stewart.struts_M.R = args.Msr;
|
|
stewart.struts_M.H = args.Msh;
|
|
|
|
stewart.struts_F.type = 1;
|
|
|
|
stewart.struts_F.M = args.Fsm;
|
|
stewart.struts_F.R = args.Fsr;
|
|
stewart.struts_F.H = args.Fsh;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeStrutDynamics=: Add Stiffness and Damping properties of each strut
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeStrutDynamics.m :comments none :mkdirp yes :eval no
|
|
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)]
|
|
|
|
arguments
|
|
stewart
|
|
args.type char {mustBeMember(args.type,{'1dof', '2dof', 'flexible'})} = '1dof'
|
|
args.k (1,1) double {mustBeNumeric, mustBeNonnegative} = 20e6
|
|
args.kp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.ke (1,1) double {mustBeNumeric, mustBeNonnegative} = 5e6
|
|
args.ka (1,1) double {mustBeNumeric, mustBeNonnegative} = 60e6
|
|
args.c (1,1) double {mustBeNumeric, mustBeNonnegative} = 2e1
|
|
args.cp (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.ce (1,1) double {mustBeNumeric, mustBeNonnegative} = 1e6
|
|
args.ca (1,1) double {mustBeNumeric, mustBeNonnegative} = 10
|
|
|
|
args.F_gain (1,1) double {mustBeNumeric} = 1
|
|
args.me (1,1) double {mustBeNumeric} = 0.01
|
|
args.ma (1,1) double {mustBeNumeric} = 0.01
|
|
end
|
|
|
|
if strcmp(args.type, '1dof')
|
|
stewart.actuators.type = 1;
|
|
elseif strcmp(args.type, '2dof')
|
|
stewart.actuators.type = 2;
|
|
elseif strcmp(args.type, 'flexible')
|
|
stewart.actuators.type = 3;
|
|
end
|
|
|
|
stewart.actuators.k = args.k;
|
|
stewart.actuators.c = args.c;
|
|
|
|
% Parallel stiffness
|
|
stewart.actuators.kp = args.kp;
|
|
stewart.actuators.cp = args.cp;
|
|
|
|
stewart.actuators.ka = args.ka;
|
|
stewart.actuators.ca = args.ca;
|
|
|
|
stewart.actuators.ke = args.ke;
|
|
stewart.actuators.ce = args.ce;
|
|
|
|
stewart.actuators.F_gain = args.F_gain;
|
|
|
|
stewart.actuators.ma = args.ma;
|
|
stewart.actuators.me = args.me;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =initializeJointDynamics=: Add Stiffness and Damping properties for spherical joints
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeJointDynamics.m :comments none :mkdirp yes :eval no
|
|
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)]
|
|
|
|
arguments
|
|
stewart
|
|
args.type_F char {mustBeMember(args.type_F,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '2dof'
|
|
args.type_M char {mustBeMember(args.type_M,{'2dof', '3dof', '4dof', '6dof', 'flexible'})} = '3dof'
|
|
args.Kf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cf_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kt_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ct_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_F (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ka_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Ca_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Kr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
args.Cr_M (1,1) double {mustBeNumeric, mustBeNonnegative} = 0
|
|
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
|
|
|
|
switch args.type_F
|
|
case '2dof'
|
|
stewart.joints_F.type = 1;
|
|
case '3dof'
|
|
stewart.joints_F.type = 2;
|
|
case '4dof'
|
|
stewart.joints_F.type = 3;
|
|
case '6dof'
|
|
stewart.joints_F.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_F.type = 5;
|
|
otherwise
|
|
error("joints_F are not correctly defined")
|
|
end
|
|
|
|
switch args.type_M
|
|
case '2dof'
|
|
stewart.joints_M.type = 1;
|
|
case '3dof'
|
|
stewart.joints_M.type = 2;
|
|
case '4dof'
|
|
stewart.joints_M.type = 3;
|
|
case '6dof'
|
|
stewart.joints_M.type = 4;
|
|
case 'flexible'
|
|
stewart.joints_M.type = 5;
|
|
otherwise
|
|
error("joints_M are not correctly defined")
|
|
end
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
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
|
|
#+end_src
|
|
|
|
*** =initializeStewartPose=: Determine the initial stroke in each leg to have the wanted pose
|
|
|
|
#+begin_src matlab :tangle matlab/src/initializeStewartPose.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
[Li, dLi] = inverseKinematics(stewart, 'AP', args.AP, 'ARB', args.ARB);
|
|
|
|
stewart.actuators.Leq = dLi;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =computeJacobian=: Compute the Jacobian Matrix
|
|
|
|
#+begin_src matlab :tangle matlab/src/computeJacobian.m :comments none :mkdirp yes :eval no
|
|
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:
|
|
% - geometry.J [6x6] - The Jacobian Matrix
|
|
% - geometry.K [6x6] - The Stiffness Matrix
|
|
% - geometry.C [6x6] - The Compliance Matrix
|
|
|
|
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;
|
|
|
|
J = [As' , cross(Ab, As)'];
|
|
|
|
K = J'*diag(Ki)*J;
|
|
|
|
C = inv(K);
|
|
|
|
stewart.geometry.J = J;
|
|
stewart.geometry.K = K;
|
|
stewart.geometry.C = C;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =inverseKinematics=: Compute Inverse Kinematics
|
|
|
|
#+begin_src matlab :tangle matlab/src/inverseKinematics.m :comments none :mkdirp yes :eval no
|
|
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}
|
|
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
end
|
|
|
|
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;
|
|
|
|
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));
|
|
|
|
dLi = Li-l;
|
|
|
|
end
|
|
#+end_src
|
|
|
|
*** =displayArchitecture=: 3D plot of the Stewart platform architecture
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/displayArchitecture.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:displayArchitecture>>
|
|
|
|
This Matlab function is accessible [[file:../src/displayArchitecture.m][here]].
|
|
|
|
**** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = displayArchitecture(stewart, args)
|
|
% displayArchitecture - 3D plot of the Stewart platform architecture
|
|
%
|
|
% Syntax: [] = displayArchitecture(args)
|
|
%
|
|
% Inputs:
|
|
% - stewart
|
|
% - args - Structure with 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}
|
|
% - ARB [3x3] - The rotation matrix that gives the wanted orientation of {B} with respect to {A}
|
|
% - F_color [color] - Color used for the Fixed elements
|
|
% - M_color [color] - Color used for the Mobile elements
|
|
% - L_color [color] - Color used for the Legs elements
|
|
% - frames [true/false] - Display the Frames
|
|
% - legs [true/false] - Display the Legs
|
|
% - joints [true/false] - Display the Joints
|
|
% - labels [true/false] - Display the Labels
|
|
% - platforms [true/false] - Display the Platforms
|
|
% - views ['all', 'xy', 'yz', 'xz', 'default'] -
|
|
%
|
|
% Outputs:
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
args.AP (3,1) double {mustBeNumeric} = zeros(3,1)
|
|
args.ARB (3,3) double {mustBeNumeric} = eye(3)
|
|
args.F_color = [0 0.4470 0.7410]
|
|
args.M_color = [0.8500 0.3250 0.0980]
|
|
args.L_color = [0 0 0]
|
|
args.frames logical {mustBeNumericOrLogical} = true
|
|
args.legs logical {mustBeNumericOrLogical} = true
|
|
args.joints logical {mustBeNumericOrLogical} = true
|
|
args.labels logical {mustBeNumericOrLogical} = true
|
|
args.platforms logical {mustBeNumericOrLogical} = true
|
|
args.views char {mustBeMember(args.views,{'all', 'xy', 'xz', 'yz', 'default'})} = 'default'
|
|
end
|
|
#+end_src
|
|
|
|
**** Check the =stewart= structure elements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
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, 'H'), 'stewart.geometry should have attribute H')
|
|
H = stewart.geometry.H;
|
|
|
|
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;
|
|
#+end_src
|
|
|
|
|
|
**** Figure Creation, Frames and Homogeneous transformations
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
The reference frame of the 3d plot corresponds to the frame $\{F\}$.
|
|
#+begin_src matlab
|
|
if ~strcmp(args.views, 'all')
|
|
figure;
|
|
else
|
|
f = figure('visible', 'off');
|
|
end
|
|
|
|
hold on;
|
|
#+end_src
|
|
|
|
We first compute homogeneous matrices that will be useful to position elements on the figure where the reference frame is $\{F\}$.
|
|
#+begin_src matlab
|
|
FTa = [eye(3), FO_A; ...
|
|
zeros(1,3), 1];
|
|
ATb = [args.ARB, args.AP; ...
|
|
zeros(1,3), 1];
|
|
BTm = [eye(3), -MO_B; ...
|
|
zeros(1,3), 1];
|
|
|
|
FTm = FTa*ATb*BTm;
|
|
#+end_src
|
|
|
|
Let's define a parameter that define the length of the unit vectors used to display the frames.
|
|
#+begin_src matlab
|
|
d_unit_vector = H/4;
|
|
#+end_src
|
|
|
|
Let's define a parameter used to position the labels with respect to the center of the element.
|
|
#+begin_src matlab
|
|
d_label = H/20;
|
|
#+end_src
|
|
|
|
**** Fixed Base elements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
Let's first plot the frame $\{F\}$.
|
|
#+begin_src matlab
|
|
Ff = [0, 0, 0];
|
|
if args.frames
|
|
quiver3(Ff(1)*ones(1,3), Ff(2)*ones(1,3), Ff(3)*ones(1,3), ...
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
|
|
|
if args.labels
|
|
text(Ff(1) + d_label, ...
|
|
Ff(2) + d_label, ...
|
|
Ff(3) + d_label, '$\{F\}$', 'Color', args.F_color);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
Now plot the frame $\{A\}$ fixed to the Base.
|
|
#+begin_src matlab
|
|
if args.frames
|
|
quiver3(FO_A(1)*ones(1,3), FO_A(2)*ones(1,3), FO_A(3)*ones(1,3), ...
|
|
[d_unit_vector 0 0], [0 d_unit_vector 0], [0 0 d_unit_vector], '-', 'Color', args.F_color)
|
|
|
|
if args.labels
|
|
text(FO_A(1) + d_label, ...
|
|
FO_A(2) + d_label, ...
|
|
FO_A(3) + d_label, '$\{A\}$', 'Color', args.F_color);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
Let's then plot the circle corresponding to the shape of the Fixed base.
|
|
#+begin_src matlab
|
|
if args.platforms && stewart.platform_F.type == 1
|
|
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
|
v = null([0; 0; 1]'); % Two vectors that are perpendicular to the circle normal
|
|
center = [0; 0; 0]; % Center of the circle
|
|
radius = stewart.platform_F.R; % Radius of the circle [m]
|
|
|
|
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
|
|
|
plot3(points(1,:), ...
|
|
points(2,:), ...
|
|
points(3,:), '-', 'Color', args.F_color);
|
|
end
|
|
#+end_src
|
|
|
|
Let's now plot the position and labels of the Fixed Joints
|
|
#+begin_src matlab
|
|
if args.joints
|
|
scatter3(Fa(1,:), ...
|
|
Fa(2,:), ...
|
|
Fa(3,:), 'MarkerEdgeColor', args.F_color);
|
|
if args.labels
|
|
for i = 1:size(Fa,2)
|
|
text(Fa(1,i) + d_label, ...
|
|
Fa(2,i), ...
|
|
Fa(3,i), sprintf('$a_{%i}$', i), 'Color', args.F_color);
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
**** Mobile Platform elements
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
|
|
Plot the frame $\{M\}$.
|
|
#+begin_src matlab
|
|
Fm = FTm*[0; 0; 0; 1]; % Get the position of frame {M} w.r.t. {F}
|
|
|
|
if args.frames
|
|
FM_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
|
quiver3(Fm(1)*ones(1,3), Fm(2)*ones(1,3), Fm(3)*ones(1,3), ...
|
|
FM_uv(1,1:3), FM_uv(2,1:3), FM_uv(3,1:3), '-', 'Color', args.M_color)
|
|
|
|
if args.labels
|
|
text(Fm(1) + d_label, ...
|
|
Fm(2) + d_label, ...
|
|
Fm(3) + d_label, '$\{M\}$', 'Color', args.M_color);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
Plot the frame $\{B\}$.
|
|
#+begin_src matlab
|
|
FB = FO_A + args.AP;
|
|
|
|
if args.frames
|
|
FB_uv = FTm*[d_unit_vector*eye(3); zeros(1,3)]; % Rotated Unit vectors
|
|
quiver3(FB(1)*ones(1,3), FB(2)*ones(1,3), FB(3)*ones(1,3), ...
|
|
FB_uv(1,1:3), FB_uv(2,1:3), FB_uv(3,1:3), '-', 'Color', args.M_color)
|
|
|
|
if args.labels
|
|
text(FB(1) - d_label, ...
|
|
FB(2) + d_label, ...
|
|
FB(3) + d_label, '$\{B\}$', 'Color', args.M_color);
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
Let's then plot the circle corresponding to the shape of the Mobile platform.
|
|
#+begin_src matlab
|
|
if args.platforms && stewart.platform_M.type == 1
|
|
theta = [0:0.01:2*pi+0.01]; % Angles [rad]
|
|
v = null((FTm(1:3,1:3)*[0;0;1])'); % Two vectors that are perpendicular to the circle normal
|
|
center = Fm(1:3); % Center of the circle
|
|
radius = stewart.platform_M.R; % Radius of the circle [m]
|
|
|
|
points = center*ones(1, length(theta)) + radius*(v(:,1)*cos(theta) + v(:,2)*sin(theta));
|
|
|
|
plot3(points(1,:), ...
|
|
points(2,:), ...
|
|
points(3,:), '-', 'Color', args.M_color);
|
|
end
|
|
#+end_src
|
|
|
|
Plot the position and labels of the rotation joints fixed to the mobile platform.
|
|
#+begin_src matlab
|
|
if args.joints
|
|
Fb = FTm*[Mb;ones(1,6)];
|
|
|
|
scatter3(Fb(1,:), ...
|
|
Fb(2,:), ...
|
|
Fb(3,:), 'MarkerEdgeColor', args.M_color);
|
|
|
|
if args.labels
|
|
for i = 1:size(Fb,2)
|
|
text(Fb(1,i) + d_label, ...
|
|
Fb(2,i), ...
|
|
Fb(3,i), sprintf('$b_{%i}$', i), 'Color', args.M_color);
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
**** Legs
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
Plot the legs connecting the joints of the fixed base to the joints of the mobile platform.
|
|
#+begin_src matlab
|
|
if args.legs
|
|
for i = 1:6
|
|
plot3([Fa(1,i), Fb(1,i)], ...
|
|
[Fa(2,i), Fb(2,i)], ...
|
|
[Fa(3,i), Fb(3,i)], '-', 'Color', args.L_color);
|
|
|
|
if args.labels
|
|
text((Fa(1,i)+Fb(1,i))/2 + d_label, ...
|
|
(Fa(2,i)+Fb(2,i))/2, ...
|
|
(Fa(3,i)+Fb(3,i))/2, sprintf('$%i$', i), 'Color', args.L_color);
|
|
end
|
|
end
|
|
end
|
|
#+end_src
|
|
|
|
**** Figure parameters
|
|
#+begin_src matlab
|
|
switch args.views
|
|
case 'default'
|
|
view([1 -0.6 0.4]);
|
|
case 'xy'
|
|
view([0 0 1]);
|
|
case 'xz'
|
|
view([0 -1 0]);
|
|
case 'yz'
|
|
view([1 0 0]);
|
|
end
|
|
axis equal;
|
|
axis off;
|
|
#+end_src
|
|
|
|
**** Subplots
|
|
#+begin_src matlab
|
|
if strcmp(args.views, 'all')
|
|
hAx = findobj('type', 'axes');
|
|
|
|
figure;
|
|
s1 = subplot(2,2,1);
|
|
copyobj(get(hAx(1), 'Children'), s1);
|
|
view([0 0 1]);
|
|
axis equal;
|
|
axis off;
|
|
title('Top')
|
|
|
|
s2 = subplot(2,2,2);
|
|
copyobj(get(hAx(1), 'Children'), s2);
|
|
view([1 -0.6 0.4]);
|
|
axis equal;
|
|
axis off;
|
|
|
|
s3 = subplot(2,2,3);
|
|
copyobj(get(hAx(1), 'Children'), s3);
|
|
view([1 0 0]);
|
|
axis equal;
|
|
axis off;
|
|
title('Front')
|
|
|
|
s4 = subplot(2,2,4);
|
|
copyobj(get(hAx(1), 'Children'), s4);
|
|
view([0 -1 0]);
|
|
axis equal;
|
|
axis off;
|
|
title('Side')
|
|
|
|
close(f);
|
|
end
|
|
#+end_src
|
|
|
|
|
|
*** =describeStewartPlatform=: Display some text describing the current defined Stewart Platform
|
|
:PROPERTIES:
|
|
:header-args:matlab+: :tangle matlab/src/describeStewartPlatform.m
|
|
:header-args:matlab+: :comments none :mkdirp yes :eval no
|
|
:END:
|
|
<<sec:describeStewartPlatform>>
|
|
|
|
This Matlab function is accessible [[file:../src/describeStewartPlatform.m][here]].
|
|
|
|
**** Function description
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
function [] = describeStewartPlatform(stewart)
|
|
% describeStewartPlatform - Display some text describing the current defined Stewart Platform
|
|
%
|
|
% Syntax: [] = describeStewartPlatform(args)
|
|
%
|
|
% Inputs:
|
|
% - stewart
|
|
%
|
|
% Outputs:
|
|
#+end_src
|
|
|
|
**** Optional Parameters
|
|
:PROPERTIES:
|
|
:UNNUMBERED: t
|
|
:END:
|
|
#+begin_src matlab
|
|
arguments
|
|
stewart
|
|
end
|
|
#+end_src
|
|
|
|
**** Geometry
|
|
#+begin_src matlab
|
|
fprintf('GEOMETRY:\n')
|
|
fprintf('- The height between the fixed based and the top platform is %.3g [mm].\n', 1e3*stewart.geometry.H)
|
|
|
|
if stewart.platform_M.MO_B(3) > 0
|
|
fprintf('- Frame {A} is located %.3g [mm] above the top platform.\n', 1e3*stewart.platform_M.MO_B(3))
|
|
else
|
|
fprintf('- Frame {A} is located %.3g [mm] below the top platform.\n', - 1e3*stewart.platform_M.MO_B(3))
|
|
end
|
|
|
|
fprintf('- The initial length of the struts are:\n')
|
|
fprintf('\t %.3g, %.3g, %.3g, %.3g, %.3g, %.3g [mm]\n', 1e3*stewart.geometry.l)
|
|
fprintf('\n')
|
|
#+end_src
|
|
|
|
**** Actuators
|
|
#+begin_src matlab
|
|
fprintf('ACTUATORS:\n')
|
|
if stewart.actuators.type == 1
|
|
fprintf('- The actuators are modelled as 1DoF.\n')
|
|
fprintf('- The Stiffness and Damping of each actuators is:\n')
|
|
fprintf('\t k = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.k(1), stewart.actuators.c(1))
|
|
if stewart.actuators.kp > 0
|
|
fprintf('\t Added parallel stiffness: kp = %.0e [N/m] \t c = %.0e [N/(m/s)]\n', stewart.actuators.kp(1))
|
|
end
|
|
elseif stewart.actuators.type == 2
|
|
fprintf('- The actuators are modelled as 2DoF (APA).\n')
|
|
fprintf('- The vertical stiffness and damping contribution of the piezoelectric stack is:\n')
|
|
fprintf('\t ka = %.0e [N/m] \t ca = %.0e [N/(m/s)]\n', stewart.actuators.ka(1), stewart.actuators.ca(1))
|
|
fprintf('- Vertical stiffness when the piezoelectric stack is removed is:\n')
|
|
fprintf('\t kr = %.0e [N/m] \t cr = %.0e [N/(m/s)]\n', stewart.actuators.kr(1), stewart.actuators.cr(1))
|
|
elseif stewart.actuators.type == 3
|
|
fprintf('- The actuators are modelled with a flexible element (FEM).\n')
|
|
end
|
|
fprintf('\n')
|
|
#+end_src
|
|
|
|
**** Joints
|
|
#+begin_src matlab
|
|
fprintf('JOINTS:\n')
|
|
#+end_src
|
|
|
|
Type of the joints on the fixed base.
|
|
#+begin_src matlab
|
|
switch stewart.joints_F.type
|
|
case 1
|
|
fprintf('- The joints on the fixed based are universal joints (2DoF)\n')
|
|
case 2
|
|
fprintf('- The joints on the fixed based are spherical joints (3DoF)\n')
|
|
end
|
|
#+end_src
|
|
|
|
Type of the joints on the mobile platform.
|
|
#+begin_src matlab
|
|
switch stewart.joints_M.type
|
|
case 1
|
|
fprintf('- The joints on the mobile based are universal joints (2DoF)\n')
|
|
case 2
|
|
fprintf('- The joints on the mobile based are spherical joints (3DoF)\n')
|
|
end
|
|
#+end_src
|
|
|
|
Position of the fixed joints
|
|
#+begin_src matlab
|
|
fprintf('- The position of the joints on the fixed based with respect to {F} are (in [mm]):\n')
|
|
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_F.Fa)
|
|
#+end_src
|
|
|
|
Position of the mobile joints
|
|
#+begin_src matlab
|
|
fprintf('- The position of the joints on the mobile based with respect to {M} are (in [mm]):\n')
|
|
fprintf('\t % .3g \t % .3g \t % .3g\n', 1e3*stewart.platform_M.Mb)
|
|
fprintf('\n')
|
|
#+end_src
|
|
|
|
**** Kinematics
|
|
#+begin_src matlab
|
|
fprintf('KINEMATICS:\n')
|
|
|
|
if isfield(stewart.kinematics, 'K')
|
|
fprintf('- The Stiffness matrix K is (in [N/m]):\n')
|
|
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.K)
|
|
end
|
|
|
|
if isfield(stewart.kinematics, 'C')
|
|
fprintf('- The Damping matrix C is (in [m/N]):\n')
|
|
fprintf('\t % .0e \t % .0e \t % .0e \t % .0e \t % .0e \t % .0e\n', stewart.kinematics.C)
|
|
end
|
|
#+end_src
|
|
|
|
* Footnotes
|
|
|
|
[fn:1]Different architecture exists, typically referred as "6-SPS" (Spherical, Prismatic, Spherical) or "6-UPS" (Universal, Prismatic, Spherical)
|