Post on 05-Mar-2020
1
1Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτικά Συστήματα Ελέγχου: Κινηματική/Στατική/Δυναμική Ανάλυση και Έλεγχος Ρομποτικών Χειριστών
Κων/νος ΤζαφέσταςΤομέας Σημάτων, Ελέγχου & Ρομποτικής
Σχολή Ηλεκτρ. Μηχ/κών & Μηχ/κών Υπολ., Ε.Μ.Π.
Τηλ.: (210) 772-3687 (Κτήριο Ηλεκτρ., Γραφείο 21.11)E-mail: ktzaf@cs.ntua.grWeb: http://www.softlab.ntua.gr/~ktzaf/
Διατμηματικό Πρόγραμμα Μεταπτυχιακών Σπουδών«ΣΥΣΤΗΜΑΤΑ ΑΥΤΟΜΑΤΙΣΜΟΥ»
2Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Περιεχόμενα Μαθήματος
• Κινηματική Ανάλυση– Ορθή και ανάστροφη κινηματική ανάλυση– Διαφορική κινηματική ανάλυση
• Στατική Ανάλυση Ρομποτικών Χειριστών• Δυναμική Ανάλυση Ρομποτικών Μηχανισμών• Ρομποτικός Έλεγχος
– Γραμμικός / Μη-Γραμμικός Έλεγχος Τροχιάς• Σχεδιασμός Τροχιάς Ρομποτικών Χειριστών• Προγραμματισμός Ρομποτικών Χειριστών
2
3Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Βιβλιογραφία(Εισαγωγή στη Ρομποτική)
• Τζαφέστας, Σπύρος Γ., «Ρομποτική. Τομ. 1: Ανάλυση και έλεγχος» (629.892 ΤΖΑ)
• Δουλγέρη Ζωή, «Ρομποτική. Κινηματική, Δυναμική και Έλεγχος Αρθρωτών Βραχιόνων», Εκδόσεις ΚΡΙΤΙΚΗ 2007.
• Εμίρης Δημήτριος, «Ρομποτική», Εκδόσεις Άνωση, 1999.• Asada, H., Slotine, J.-J., “Robot Analysis and Control,” John Wiley & Sons,
1986.• B. Siciliano et al., “Robotics: modelling, planning and control”, Springer, 2009 • Craig, John J., “Introduction to robotics : mechanics and control,” Addison-
Wesley, 1989. (629.892 CRA)• Yoshikawa, Tsuneo, “Foundations of robotics : analysis and control,” The MIT
Press, 1990. (629.892 YOS)• Schilling, Robert J., “Fundamentals of robotics : analysis and control,” Prentice
Hall, 1990. (629.892 SCH)• K. S. Fu, R. C. Gonzalez, G. S. G. Lee, “Robotics : control, sensing, vision, and
intelligence,” McGraw-Hill, 1987. (629.892 FU)
4Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Βιβλιογραφία (advanced robotics)
• Murray, R.M., Li, Z., and Sastry, S., “A Mathematical Introduction to Robotic Manipulation,” CRC Press, 1994. (629.892 MUR)
• Mason, Matthew, “Mechanics of Robotic Manipulation,” MIT Press, 2001. • Mason, M. and Salisbury, J.K., Jr., “Robot Hands and the Mechanics of
Manipulation,” MIT Press, 1985.• Latombe, Jean-Claude, "Robot motion planning," Kluwer Academic
Publishers, 1991. (629.892)• Meystel, A., "Autonomous mobile robots : vehicles with cognitive control,"
World Scientific, 1991. (629.892 MEY)• Borenstein, Johann, "Navigating mobile robots : systems and techniques,"
Wellesley, MA.: : AK Peters, Ltd., 1996. (629.892). • Sheridan, Thomas B., "Telerobotics, automation, and human supervisory
control," The MIT Press, 1992. (620.46 SHE)
3
5Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τι είναι Ρομπότ? (1/3)
Ετυμολογία του όρου:robota (Τσέχικα): άμισθη/εξαναγκασμένη εργασίαrabu (Σλάβικα): σκλάβος, работать (rabotat’: Ρώσικα): εργασίαarbeit (Γερμανικά): εργασία, ή Erbe (κληρονόμος)
Ρίζα : rob ή rabεπίσης, orb ή orph οrphelin - ορφανός ... serf - σκλαβιάorbh (Ινδο-Ευρωπαϊκή ρίζα): κληρονόμος, κληρονομιά
Πρώτη εμφάνιση της έννοιας: Karel Capek (1921), «RUR: Les robots universels de Rossum",εμφάνιση ενός «Ανδροϊδούς» το οποίο αποκαλείται «robot»...
6Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τι είναι Ρομπότ? (2/3)
Μπορούμε να ορίσουμε ως ρομπότ μια μηχανή που «αισθάνεται», «σκέφτεται» και «επενεργεί» (sense, think, act). Άρα, ένα ρομπότ διαθέτει: • αισθητήρες (sensors), για την απόκτηση πληροφορίας (a) από το εξωτερικό περιβάλλον (exteroceptive), ή (b) σε σχέση με την εσωτερική κατάσταση (proprioceptive)• δυνατότητες επεξεργασίας (processing) αντίληψη, συλλογισμός, λήψη αποφάσεων, σχεδιασμός δράσης (cognition)• επενεργητές (actuators), για την εκτέλεση κάποιας εργασίας στο περιβάλλον (motion, manipulation)
4
7Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τι είναι Ρομπότ?
Τρείς βασικές ιδιότητες ενός ρομπότ:• δυνατότητες επαναπρογραμματισμού (programmability):
a robot is a computer (information/data processing)
• δυνατότητες μηχανικής δράσης (mechanical abilities), εκτέλεση φυσικών εργασιών πάνω στο περιβάλλον (physical, not data processing)
a robot is a machine (mechatronic device)• προσαρμοστικότητα, ευελιξία, πολυσχιδής λειτουργικότητα (adaptability, versatility, flexibility):
adapt to different environment and task requirements
(3/3)
8Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτική – Εισαγωγή (1)• Ρομπότ: «Ευφυείς», «ευέλικτοι», «προσαρμοζόμενοι» μηχανισμοί κίνηση και δράση στο χώρο
• Κατηγορίες Ρομποτικών Συστημάτων:- Βιομηχανικοί (κλασσικοί) ρομποτικοί χειριστές (industrial
robot manipulators)- Επιδέξιοι ρομποτικοί χειριστές (dextrous robots)- Αυτοκινούμενα ρομπότ – ρομπότ προσφοράς υπηρεσιών
(mobile/service robotics)- Μικρο-ρομποτική (micro-robotics)
Τηλε-ρομποτική vs. Ευφυή/αυτόνομα ρομπότ
5
9Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτική – Εισαγωγή (2)• Ρομποτική: «κατακόρυφη» κατάτμηση σε θεματολογικά επιστημονικά πεδία / «οριζόντια» κατάτμηση σε πεδία εφαρμογών
Μηχανική
(ανάλυση
/σχεδίαση)
Ηλεκτρονική
(μικρο
-επεξεργαστές,
Αισθητήρες,
embe
dded
syst
ems e
tc.)
Αυτόματος Έλεγχος
Συστημάτων
Υπολογιστική Νοημοσύνη
Προγραμματισμός Υπολογιστών
Διασύνδεση
ανθρώπου-μηχανής ...
Βιομηχανικές Εφαρμογές(robotized manufacturing etc.)
Προσφορά Υπηρεσιών(service & intervention robots)- mobile robotics (wheeled, legged)- dextrous robotics (medical etc.)- telerobotics - microrobotics
10Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτική και Αυτοματοποιημένα Συστήματα Παραγωγής
StaubliStaubli FanucFanuc
Computer IntegratedComputer IntegratedManufacturing (CIM)Manufacturing (CIM)
Ολοκληρωμένα συστήματα Ολοκληρωμένα συστήματα προγραμματισμού αυτοματοπρογραμματισμού αυτοματο--ποιημένων διαδικασιών ποιημένων διαδικασιών παραγωγήςπαραγωγής
6
11Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Επιδέξιοι Ρομποτικοί ΧειριστέςΡομποτικοί Χειριστές με πλεονέζοντες βαθμούς ελευθερίας(redundant robot manipulators)
DLR lightweight 7dof robot On-line obstacle avoidance(kinematic redundancies)
12Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτικοί Χειριστές με Πλεονάζοντες Βαθμούς Ελευθερίας (1/2)
NASA – Robotics Research – Modular Redundant Robot ModArm
7
13Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτικοί Χειριστές με Πλεονάζοντες Βαθμούς Ελευθερίας (2/2)
NASA – RoboticsResearch - ModArm DLR – KineMedic Redundant Robot
Εφαρμογές στο Διάστημα Ιατρικές Εφαρμογές
(όπου απαιτείται αυξημένη «ικανότητα χειρισμού»)
14Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Συνεργαζόμενοι Ρομποτικοί Χειριστές (1)
ARM –Autonomous Robotic Manipulation Program (DARPA)
8
15Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Dual Arm Robot Manipulator (Dexter/UMass)
Συνεργαζόμενοι Ρομποτικοί Χειριστές (2)
16Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Justin Humanoid Robot with DLR-III arms and DLR-II hands
DLR: German Aerospace Center –Germany's National Research Center for Aeronautics and Space
(Deutsches Zentrum für Luft- und Raumfahrt , DLR)
Συνεργαζόμενοι Ρομποτικοί Χειριστές (3)
9
17Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Επιδέξια (Ανθρωπόμορφα) Ρομποτικά Χέρια(Dexterous Robot Hands) (1/3)
Utah/MIT robot handUtah/MIT robot handJPL/NASA handJPL/NASA hand
ΔεξιότηταΔεξιότητα: : Συνεργασία πολλαπλών βαθμών ελευθερίας για τονΣυνεργασία πολλαπλών βαθμών ελευθερίας για τονέλεγχο σύνθετων/λεπτών εργασιών χειρισμούέλεγχο σύνθετων/λεπτών εργασιών χειρισμού
18Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Επιδέξια Ρομποτικά Χέρια (2/3)
Shadow Robot HandThumb &
fingersLittle finger
Drill Demo
10
19Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Επιδέξια Ρομποτικά Χέρια (3/3)
DLR Hand DLR Hand ΙΙΙΙ
Robonaut Robonaut Humanoid / NASAHumanoid / NASA
20Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Επιδέξια Ρομποτικά Χέρια – Παραδείγματα (2)
DLR Hand DLR Hand ΙΙΙΙ
UUΜΜass Humanoid Robotass Humanoid Robot
11
21Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτικό Χέρι (υπό κατασκευή)
22Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
• Αυτόνομη εκμάθηση δεξιοτήτων– Μη επιβλεπόμενη (αναπτυξιακή) μάθηση (developmental robot learning)
• Εκπαίδευση ρομπότ από άνθρωπο / Αλληλεπίδραση ανθρώπου‐ρομπότ– Μάθηση μέσω παρατήρησης (robot teaching by demonstration, learning by observation / by imitation)
– Τηλερομποτική / Τηλεχειρισμός
Εκμάθηση Ρομποτικών Δεξοτήτων
12
23Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τηλερομποτική – Εισαγωγή
Multi‐disciplinary field:• Robot control – sensors – actuators• Networks – communication link• Human‐machine interaction / human‐robot cooperation• Human factors (perception/action/decision, sensori‐motor behavior)
Remote RobotController
Master ControlStation
CommunicationChannel
24Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τηλερομποτική – Ιστορική αναδρομή
• 1950: mechanical master-slave telemanipulators(Raymond Goertz – Argonne National Labs)
• 1960: electrical telemanipulatorsBilateral servo-control link (CEA/Saclay: nuclear industry)
• 1970’s: telemanipulation technology meets robotics. Creation of the field of telerobotics
• 1980’s: computer-assisted teleoperation (Vertut & Coiffet)
• 1990’s: advanced telerobotics– Shared-autonomy teleoperation control– Predictive displays (Bejczy/JPL) and virtual reality methods– Supervisory control (Sheridan/MIT)
13
25Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Εξέλιξη της Τηλερομποτικής
MechanicalTelemanipulators
Human-Robot Communication / Teleoperation
Robo
t Aut
onom
y
Servo-ControlledMaster-Slave
Computer-AidedTeleoperation
Shared-AutonomyTeleoperation
SupervisoryControl
Intelligent / Autonomous
Robots
Robotics
Telemanipulation
Teler
obotics
26Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτικός Τηλεχειρισμός
Master-slave telemanipulation(άμεσος αμφίδρομος τηλεχειρισμός)
(CEA / France)
Computer-assisted teleoperation(τηλεχειρισμός υποβοηθούμενος από υπολογιστή) (JPL / NASA - USA)
Παραδείγματα συστημάτων:
Τηλερομποτική: Eφαρμογές σε περιβάλλοντα εργασίας (α) μη δομημένα (unstructured) και (β) μη φιλικά (hostile) προς τον άνθρωπο (τηλεχειρισμός ραδιενεργών υλικών, διάστημα, υποβρύχια έρευνα, απενεργοποίηση βομβών κλπ.)
DLR – Γερμανία
LRP – Γαλλία
Potiers Vilette
TUM – Γερμανία
14
27Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τηλερομποτική: Βασικές Έννοιες
• Στόχος: «Τηλεπαρουσία» (Telepresence)– «Διαφάνεια» (transparency) του συστήματος ως προςτην ανατροφοδότηση αισθητηριακής πληροφορίας
– «Παρουσία» (feel of presence) στον απομακρυσμένο χώρο ρομποτικής εργασίας (sensory-physical/action-decision)
• Πρόβλημα: χρονικές καθυστερήσειςστο βρόχο αμφίδρομης επικοινωνίας (time-delays)– Προβλεπτικός Έλεγχος (predictive control)– Προβλεπτικά μοντέλα απεικόνισης και ανάδρασης
(predictive displays) – Μοντέλα Εικονικής Πραγματικότητας
28Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Εφαρμογή – Ρομποτική Χειρουργική
Σύγχρονη Ρομποτική Τεχνολογία ως μέσο
υποβοήθησης / υποκατάστασης του
έργου του χειρουργού, μέσω φυσικής
δράσης (τηλεχειριζόμενης ή αυτόνομης)
στο πραγματικό επεμβατικό πεδίο
15
29Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Χειρουργικά Ρομποτικά Συστήματα
• DaVinci® Surgical Robotic System (1/2)
30Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Χειρουργικά Ρομποτικά Συστήματα
• DaVinci® Surgical Robotic System (2/2)
«Απτικές»Διατάξεις
«Ρομποτικές»Διατάξεις
(master)
(slave)
16
31Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Νανορομποτική Χειρουργική
Surgical console for cellular surgery
Courtesy Prof Jaydev Desai, Drexel Univ, Philadelphia, PA 2005
32Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
«Φιλικές» Ρομποτικές Εφαρμογές
Η Τεχνολογία είναι «αρκετά ώριμη» για ενσωμάτωση σε τόσο «επεμβατικές» εφαρμογές;
Ποιό το «αποδεκτό ρίσκο»;
Η κοινωνία είναι «έτοιμη» να αποδεχτεί τέτοιες σημαντικές μεταβολές στην παροχή υπηρεσιών υγείας;
Περισσότερο «φιλικές» (μη επεμβατικές) εφαρμογές ρομποτικής τεχνολογίας:Ρομπότ βοηθοί / νοσηλευτές Ρομποτικά - Απτικά συστήματα στη χειρουργική εκπαίδευση, άσκηση και πιστοποίηση δεξιοτήτων
17
33Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομπότ Βοηθός / Νοσηλευτής
SATAVA 7 July, 1999DARPA
InTouch Technologies, Inc, Goleta, CA
34Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Χειρουργικοί Προσομοιωτές
Laparoscopic Simulator with tactile feedbackXitact, Lausanne Switzerland
Λαπαρο‐ενδοσκοπικός Προσομοιωτής με ανάδρασης αφής
18
35Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Χειρουργικοί Προσομοιωτές (2)
Συστήματα εικονικής προσομοίωσης ιατρικών(επεμβατικών) πράξεων
Συστήματα Εικονικής Πραγματικότητας (virtual reality) με «ανάδρασηαφής» (haptic display) για προσομοίωση κλινικών πράξεωνΕκπαίδευση και πιστοποίηση κλινικών «δεξιοτήτων»
ΑπτικήΡομποτικήΔιάταξη
Εικονική προσομοίωση
36Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Διεπιστημονική Συνέργεια –The “Bio‐Intelligence” Age
ΒΙΟΛΟΓΙΑ ΦΥΣΙΚΗ
ΠΛΗΡΟΦΟΡΙΚΗ
ΜΕΛΛΟΝ
RoboticsMEMS/Nano
GenomicsBioinformatics
BiosensorsBiomaterialsBiomimetic
Courtesy, Dr. Satava, UW
• Human‐robot interaction for “skill transfer”
robot skill acquisition• Bio‐inspired sensory‐motor control architectures
adaptive / learning robots• Bio‐mimetic robot structures• Bio‐signals (EMG / BCI) in human‐robot communication & telerobotic systems
19
37Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αυτοκινούμενα Ρομπότ• Περιβάλλον Κίνησης (ground, air, sea, underwater, ...)• Μηχανισμοί κίνησης (wheeled, legged, hybrid, etc. ...)• Αισθητήριες διατάξεις (sensors)• Σύνθεση αισθητηριακών δεδομένων (sensor/data fusion)
– Χαρτογράφηση Χώρου (mapping)– Εντοπισμός θέσης (localization)– Συνδυασμένη χαρτογράφηση και εκτίμηση θέσης σε άγνωστο δυναμικό περιβάλλον (SLAM)
• Σχεδιασμός Δρόμου (path planning) • Σχεδιασμός Δράσης (task planning)• Μάθηση Συμπεριφορών …
38Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αυτοκινούμενα Ρομπότ – Αισθητήρες(mobile robots – sensors)
Αισθητήρες Αισθητήρες ΥπερήχωνΥπερήχων
ΣύστημαΣύστημαΌρασηςΌρασης
ΑσύρματοΑσύρματοEthernetEthernet
Αισθητήρες Αισθητήρες ΥπέρυθρωνΥπέρυθρων
Laser Laser Range FinderRange Finder
ΜικρόςΜικρόςGripperGripper
ActiveMedia ActiveMedia RobotsRobots RWI RWI –– IS RoboticsIS Robotics
Video-1
Video-2
20
39Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τροχοφόρα Αυτοκινούμενα Ρομπότ –Σύγχρονα Συστήματα
(in action)
Indoor Outdoor
40Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
UAV – Unmanned Aerial VehiclesΠαράδειγμα Εφαρμογών: Autonomous Helicopter Projects
CM
U P
roje
ct
21
41Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αυτόνομα Υποβρύχια Ρομπότ
NA
SA
–“E
ndur
ance
” rob
ot
Εφαρμογή: pipe inspection
Δίκτυο υποβρύχιων αισθητήρων
AM
OU
R: A
uton
omou
sM
odul
ar U
nder
wat
er R
obot
(Env
ironm
enta
lly N
on-D
istu
rbin
g U
nder
-ice
Rob
otic
Ant
arct
ic E
xplo
rer)
42Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Mars Rovers (Διαστημικά Ρομποτικά Οχήματα)
Rocky I
Sojourner
Rocky IV prototype
Mars “Pathfinder” Mission
22
43Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Mars Rovers (συνέχεια)
Marsokhod concept
(συνεργασίαNASA – ESA)
44Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Mars Rovers – Spirit και Opportunity
Χαρτογράφηση και Σχεδιασμός Δρόμου
anim-part3
23
45Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Βαδίζοντα Ρομπότ – (Ανθρωπόμορφα) Δίποδα με Σύστημα Όρασης (biped walking robots)
JohnnieΠολυτεχνείο Μονάχου
(TUM)
ΣύστημαΣύστημαΚατευθυνόμενηςΚατευθυνόμενηςΣτερεοσκοπικήςΣτερεοσκοπικήςΌρασηςΌρασης
Σχεδιασμός καιΣχεδιασμός καιΈλεγχος της κίνησηςΈλεγχος της κίνησηςτου Ρομπόττου Ρομπότ
Sample movie(Johnnie)
Αισθητήρες ΔύναμηςΑισθητήρες Δύναμης((force/tactile sensors)force/tactile sensors)
46Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Δυναμικός ρομποτικός βηματισμός
3D One-Leg Hopper (1983-1984)
3D Biped (1989-1995)
Quadruped (1984-1987)
ΜΙΤ Legged-Lab. Mark Raibert, Legged Robots that Balance, MIT Press, 1986.
24
47Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
“Biologically-inspired” legged robots
Uniroo (1991-1993)Troody
48Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Multi-Legged Mobile / Walking robots
SONY - Aibo
EdutainementEdutainementResearch Research
Sample movie
Σχεδίαση «εμπνευσμένη» από «φυσικά συστήματα»
Quadruped Robot LittleDog CMU / Boston Dynamics
(e.g. locomotion, gaiting, control)
Genghis 6-legged robotAI lab / MIT
(adaptive behaviors)
25
49Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Βαδίζοντα Ρομπότ - Εφαρμογές Εφαρμογές intervention, service, exploration, rescue, etc.
Μεταφορά «Υλικού» -Επιχειρήσεις διάσωσης
Εξερεύνηση «δύσβατων» περιοχών
Εξάποδο (hexapod) ρομπότ DanteΤετράποδο Ρομπότ BigDog,
CMU / Boston Dynamics
50Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ολοκληρωμένα Κινούμενα Ρομποτικά Συστήματα Υπηρεσιών (Service Robots)
Βαδίζοντα Ανθρωπόμορφα ΡομπότΒαδίζοντα Ανθρωπόμορφα ΡομπότΚινούμενα Ρομπότ με Κινούμενα Ρομπότ με Ενσωματωμένο Ρομποτικό ΒραχίοναΕνσωματωμένο Ρομποτικό Βραχίονα
walkwalk
stepstep
Honda Honda HumanoidHumanoid RobotRobotχειρισμόςχειρισμός συνεργασίασυνεργασία
26
51Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
NAO(France)
Humanoid Robots
Justin / DLR(Germany)
HRP-2 / JAIST(Japan)
Charli (USA)Cognitive Humanoid
Autonomous Robot with Learning Intelligence
REEM-H2(PAL Robotics,Barcelona / Spain)
Asimo(Honda)
52Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Τροχοφόρα αυτοκινούμενα ρομπότ με σύνθετο σύστημα οδήγησης τροχών
27
53Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Εφαρμογές Ολοκληρωμένων Κινούμενων Ρομποτικών Οχημάτων
Εντοπισμός & απενεργοποίηση εκρηκτικών
Ρομπότ «Διάσωσης» (Rescue)
Intervention & Service Robots (Ρομπότ Παρέμβασης και Υπηρεσιών)
Ρομπότ «Εξερεύνησης» σε περιβάλλον «μη φιλικό» προς τον άνθρωπο
54Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
«Υβριδικοί» Μηχανισμοί Κίνησης
NASA's ATHLETE (All-Terrain Hex-Legged Extra-Terrestrial Explorer)
28
55Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Σύνθετοι Μηχανισμοί Κίνησης«Αναρριχόμενα» ρομπότ
(climbing robots)
JPL’
sLE
MU
R ro
bot
«Έρποντα» Ρομπότ (snake robots)
SnakeRescueRobot
Mod-SnakeRobot(CMU)
56Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
About Robot Control (1)Autonomous vs. Human-Intervention (supervisory) robot control
Isaak Asimov’s “laws of robotics”:1. A robot should never harm a human being2. A robot should obey a human being, unless this contradicts the
first law 3. A robot should not harm another robot, unless this contradicts
the first or the second law
Levels of Robot Control:At the lowest level, we want to ensure motors driving robots’ joints or wheels are used in stable configurations (no oscillations)
At the next level, we want to ensure that no collisions occurWe also expect robots to perform other “intelligent” behaviors
29
57Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
About Robot Control (2)
Goal SettingGlobal Planning
NavigationObstacle Avoidance
StabilityAttitude ControlServo Control
Human Input
High-levelControl
Intermediate-Level Control
Low-levelControl
Strategic LevelGlobal PlanningLong-range goals
Tactical LevelLocal Planning
Short-term goals
ActuatorsPlant
FeedbackSensors
58Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
ΕΝΟΤΗΤΑ 1:Κινηματική ΑνάλυσηΡομποτικών Χειριστών
30
59Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Κινηματική Ανάλυση των Ρομπότ
• Προκαταρκτικά Γεωμετρικά Εργαλεία– Μετασχηματισμοί στο χώρο κλπ.
• Ορθή κινηματική ανάλυση ρομπότ (γεωμετρικό μοντέλο)– Μετατοπίσεις αρθρώσεων {qi} Θέση/Προσανατολισμός (x,θ) τελικούστοιχείου δράσης του ρομπότ
• Αντίστροφη κινηματική ανάλυση• Ορθή διαφορική κινηματική ανάλυση (κινηματικό μοντέλο)
– Ιακωβιανή μήτρα J: ταχύτητες αρθώσεων {qi} ταχύτητα (v,ω) τελικού στοιχείου δράσης του ρομπότ
• Αντίστροφη διαφορική κινηματική ανάλυση
60Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Βασικοί Ορισμοί – Αρχές
• Ρομποτικοί βραχίονες (βιομηχανικοί ρομποτικοί χειριστές) (robot manipulators): ανοικτές κινηματικές αλυσίδες
• Κινηματική αλυσίδα (kinematic chain): σύστημα στερεώνσωμάτων που συνδέονται μέσω αρθρώσεων (joints)
• Βαθμοί ελευθερίας (degrees of freedom - DOF):αριθμός ανεξάρτητων μεταβλητών για την περιγραφή της διάταξης(configuration) ενός μηχανισμού στο χώρο
31
61Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Βασικές Ρομποτικές Αρθρώσεις
Σφαιρική άρθρωση(Spherical Joint)3 DOF (Variables - θ1, θ2, θ3)
Περιστροφική άρθρωση(revolute joint)1 βαθμός ελευθερίας(degree of freedom – DOF)(Μεταβλητή : θ ή q)
Γραμμική (πρισματική) άρθρωση(prismatic joint)1 DOF (linear) (Variable - d)
62Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομποτικοί Βραχίονες / Χειριστές:Ανοικτές (σειριακές) κινηματικές αλυσίδες
Ορολογία:Link = σύνδεσμοςJoint = άρθρωσηActuator = κινητήρας (κινητήριο στοιχείο)End-effector = τελικό στοιχείο δράσης
32
63Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράλληλες κινηματικές αλυσίδες
Επίπεδος παράλληλος μηχανισμός
Πλατφόρμα Stewart (6 DOF)
64Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα Ρομποτικού Βραχίονα –Το ρομπότ PUMA 560
1
2
34
5 6
PUMA: Programmable Universal Machine for AssemblyUnimation Inc. 1978 (now Staübli)
The PUMA 560 has SIX (6) revolute jointsA revolute joint has ONE degree of freedom (1 DOF) that is defined by its angle
33
65Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα Αρθρωτού Ρομπότ 6 DOF: Το ρομπότ Τ3
1
23
4
5
6
66Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ρομπότ Adept 1850 Palletizer
34
67Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Κινηματική Δομή Κλασσικών Ρομποτικών Χειριστών: Ταξινόμηση
Αρθρωτό ρομπότ (τύπου PUMA) Ρομπότ τύπου SCARA
Καρτεσιανό ρομπότ Κυλινδρικό ρομπότ Σφαιρικό ρομπότ
68Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Κινηματική Ανάλυση: Προκαταρκτικά Γεωμετρικά Εργαλεία
• Θέση και προσανατολισμός στερεού σώματος
xy
z
xσ
yσ
zσ
O
Oσ
n
o
a
r
Θέση: r = OOσ =rxryrz
Προσανατολισμός: R = [ n, o, a ]
R =nx ox axny oy aynz oz az
Μήτρα προσανατολισμού (ή στροφής) (3 x 3) :
[ n, o, a] : ορθοκανονικό σύστημα αναφοράςμοναδιαία διανύσματα : |n|2 = nx
2 + ny2 + nz
2 = 1, κλπ...κάθετα μεταξύ τους : n · o = 0 , n · a = 0 , o · a = 0
35
69Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Μετασχηματισμοί στο χώρο• Μετασχηματισμοί συντεταγμένων
Έστω pΣ = [pn, po, pa]T οι συντεταγμένες του σημείου P στο σύστημα αναφοράς RΣ
pO = (OP)O = rΣ + (OΣP)O
(OΣP)O = pn · n + po · o + pa · a = ORΣ · pΣ
όπου ORΣ = [n , o , a]x
y
z
O
OΣ
n
o
rΣ
a
Pxσ
yσ
zσ
Άρα: pO = rΣ + ORΣ · pΣ
Μετατόπιση ΟΟΣ (εκφρασμένη στο RO)
Στροφή του RΣ ως προς το RO
⇔ pΣ = -(ORΣ)Τ · rΣ + (ORΣ)Τ · pΟ
70Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Στροφή – Ειδικές Περιπτώσεις
• Περιστροφή γύρω από τον άξονα z (R0 R1)
xy
z ≡ z1
O
n
oa
x1
y1
θz
θz
cos(θz)sin(θz)
nxny nz
n = = cos(θz)sin(θz)
0
oxoy oz
o = = -sin(θz)cos(θz)
0
OR1 = [n , o , a] =cos(θz)sin(θz)
0
-sin(θz)cos(θz)
0
001
= Rz(θz)
θz
• Περιστροφή γύρω από τον άξονα x : Rx(θx) =100
0cos(θx)sin(θx)
0-sin(θx)cos(θx)
cos(θy)0
-sin(θy)
010
sin(θy)0
cos(θy)• Περιστροφή γύρω από τον άξονα y : Ry(θy) =
36
71Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παραμετροποίηση Στροφής
Euler(φ,θ,ψ) = Rz(φ) Rx(θ) Rz(ψ) =
Γωνίες Euler (στροφή ως προς: z x (or y) z)
cφcψ-sφcθsψ -cφsψ-sφcθcψ sφsθsφcψ+cφcθsψ -sφsψ+cφcθcψ -cφsθ
sθsψ sθcψ cθ
Γωνίες κύλισης, ανύψωσης, στροφής,(roll,pitch,yaw)
cφcθ cφsθsψ-sφcψ cφsθcψ+sφsψsφcθ sφsθsψ+cφcψ sφsθcψ-cφsψ-sθ cθsψ cθcψ
R(φ,θ,ψ) = Rz(φ) Ry(θ) Rx(ψ) =
y
z
x
72Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ομογενείς Μετασχηματισμοί
pO = rΣ + ORΣ · pΣ ⇒ PO = OAΣ · PΣ
όπου: PO = , PΣ =
σpxσpyσpz
1
ΟpxΟpyΟpz
1ομογενή διανύσματα συντεταγμένων
και : OAΣ =ORΣ rΣ0 0 0 1 ομογενής μήτρα μετασχηματισμού (4 x 4)
pO pΣ
(OAΣ)-1 = (ORΣ)Τ –(ORΣ)ΤrΣ0 0 0 1
⇔ (αντίστροφη ομογενής μήτρα)
xy
z
O
OΣ
n
o
rΣ
a
Pxσ
yσ
zσ
37
73Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ομογενείς Μετασχηματισμοί (συνέχεια)
Το ομογενές διάνυσμα V(1) = [vn,vo,va,1]T εκφρασμένο στο «τοπικό» σύστημα αναφοράς R1 (n,o,a),
ενώ το διάνυσμα V(0) εκφράζεται ως προς το «κοινό» σύστημα αναφοράς RO-X,Y,Z της βάσης
Η μήτρα περιστροφής και το διάνυσμα μετατόπισης p(0) μπορούν να συνδυαστούν σε μία ομογενή μήτρα μετασχηματισμού, εφόσον εκφράζονται ως προς κοινό σύστημα αναφοράς.
⎥⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢⎢
⎣
⎡
1va
vo
vn
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
zzzz
yyyy
xxxx
1000paonpaonpaon
V(0)= =
V(0) = A · V(1)10
…
v(0) = vn n + vo o + va a + p(0)
vx = vn nx + vo ox + va ax + px
vx
vy
vz
1
10Ax0
y0
z0
O
O1
p(0)
V
= oy1^= az1^
= nx1^
v(1)
v(0)
(1)V
74Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ομογενείς Μήτρες Μετασχηματισμών – Ειδικές Περιπτώσεις (1)
Γραμμική μετατόπιση (μεταφορά) χωρίς στροφή ⎦⎣
P
Y
X
Z
Y
X
Z
o
n
a
on
a
Στροφή χωρίς μεταφορά
Γραμμική
Μετατόπιση
RN
RO ⎥⎥⎥⎥⎤
⎢⎢⎢⎢⎡
=
1000p100p010p001
Αz
y
x
N
O
⎥⎥⎥⎥
⎦
⎤
⎢⎢⎢⎢
⎣
⎡
=
10000aon0aon0aon
zzz
yyy
xxx
Μήτρα στροφής
ΑN
O
38
75Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ομογενείς Μήτρες Μετασχηματισμών – Ειδικές Περιπτώσεις (2)
Rot(x,θx) =
1 0 0 00 cosθx -sinθx 00 sinθx cosθx 00 0 0 1
Rot(y,θy) =
Rot(z,θz) =
cosθy 0 sinθy 00 1 0 0-sinθy 0 cosθy 00 0 0 1
cosθz -sinθz 0 0sinθz cosθz 0 0
0 0 1 00 0 0 1
Tra(x,dx) =
1 0 0 dx0 1 0 00 0 1 00 0 0 1
Tra(y,dy) =
1 0 0 00 1 0 dy0 0 1 00 0 0 1
Tra(z,dz) =
1 0 0 00 1 0 00 0 1 dz0 0 0 1
76Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Διαδοχικοί ομογενείς μετασχηματισμοί
Α : 4x4 ομογενής μήτρα μετασχηματισμούαπό το πλαίσιο i στο πλαίσιο i-1 (i=1,…,n)δηλαδή, n διαδοχικοί μετασχηματισμοί από το πλαίσιο n στο πλαίσιο 0. Τότε :
i-1i
X = A · A · … · A · … · A · X0 01
12
i-1i
n-1n
n
X : ομογενές (4x1) διάνυσμα θέσης στο πλαίσιο 0X : ομογενές (4x1) διάνυσμα θέσης στο πλαίσιο n
n
0
39
77Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ορθή Κινηματική Ανάλυση(Γεωμετρικό μοντέλο ρομποτικού βραχίονα)
78Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Κινηματική Ανάλυση: Εισαγωγή• Ορθή κινηματική ανάλυση
– Μετατοπίσεις αρθρώσεων {qi} Μετατόπισητελικού στοιχείου δράσης (θέση p, προσανατολισμός R)
– Μετασχηματισμός από το χώρο αρθρώσεων στο χώρο δράσης (εργασίας) proprioception
• Αντίστροφη κινηματική ανάλυση– Θέση τελικού στοιχείου δράσης (p , R) {qi}
• Αντίστροφη διαφορική κινηματική ανάλυση– Ταχύτητα τελικού στοιχείου δράσης (v , ω)
{qi} Σχεδιασμός δρόμου ρομπότ
40
79Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ορθή κινηματική ανάλυση:ανοικτές κινηματικές αλυσίδες
Τελικό στοιχείο δράσης
Σύνδεσμος i
zn
Σύνδεσμος nΣύνδεσμος 2
Σύνδεσμος 1
Άρθρωσ
η 2
x0
y0
z0Βάση Σύνδεσμος 0 pn(q) = O0On(q)
ανοικτή κινημ
ατική αλυσίδα
q2
q3
Δοσμένων των μεταβλητών αρθρώσεων {qi, i=1,…,n}Υπολογισμός των :
- Θέση: p n = Γ(q)- Προσανατολισμός: R (q)0
n
Γεωμετρικό μοντέλο : R (q) pn(q)0 0 0 1
0n
A (q) = 0n
O0
...
...Άρθρωση i
Άρθρωση i+1
xn
yn
On
Άρθρωση 1
q1qi
qi+1
x y zR =0n
0n
0n
0n
O1
x1
z1
y1 Oi
συνημίτονα κατεύθυνσης
80Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ορθή κινηματική ανάλυση:ανοικτές κινηματικές αλυσίδες (συνέχεια)
Κινηματική εξίσωση (γεωμετρικό μοντέλο) ρομποτικού βραχίονα:Συνδυασμός των διαδοχικών μετασχηματισμών Α (i=1,…n)(από τη βάση Ο0-x0y0z0 προς τον καρπό Οn-xnynzn) τηςσειριακής κινηματικής αλυσίδας.
i-1i
Α (q) = A (q1) · A (q2) · … · A (qi) · … · A (qn)0n
01
12
i-1i
n-1nT =
y0
z0
O0
O1
x1
z1
y1
x0
Oi-1
xi-1
zi-1
yi-1
… xiyi
zi
…
xn
zn
yn
OiOn
A (qi)i-1i
A (q1)01
T = A (q)0n
41
81Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ορθή κινηματική ανάλυση:Παράδειγμα (1)
12
l1q1
q2
x0
y0
O0
l2y1
yΕxΕ
x1O1
OΕθ
2 βαθμοί ελευθ.2D, επίπεδο
(pΕ)x = l1·cos(q1) + l2·cos(q1 + q2)(pΕ)y = l1·sin(q1) + l2·sin(q1 + q2)θ = q1 + q2
Κινηματική μοντέλο:(2 ανεξ. μεταβλητές: q1 και q2)
Θέση : pΕ = [(pΕ)x , (pΕ)y]Τ
Προσανατολισμός : θ(ως προς q1 και q2)
A (qi) = Rot(z,qi) · Tra(x, li) =
cos(qi) -sin(qi) 0 li cos(qi)sin(qi) cos(qi) 0 li sin(qi)
0 0 1 00 0 0 1
i-1i
T = A = A · A0E
01
12
82Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Ορθή κινηματική ανάλυση:Παράδειγμα (2)
12
3
l1q1
q2
q3
x0
y0
O0
l2
l3
xΕyΕ
y1
y2 x2
x1
OΕ
O1
O2
θ (pΕ)x = l1·c1 + l2·c12 + l3·c123
(pΕ)y = l1·s1 + l2·s12 + l3·s123
θ = q1 + q2 + q3
όπου :c1 = cos(q1)c12 = cos(q1 + q2) c123 = cos(q1 + q2 + q3)s1 = sin(q1)s12 = sin(q1 + q2) s123 = sin(q1 + q2 + q3)
Κινηματική μοντέλο:
3 βαθμοί ελευθ.2D, επίπεδο
42
83Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Μέθοδος Denavit-Hartenberg (1)
• Βασική αρχή (ιδέα): 4 παράμετροι για την περιγραφή της σχετικής τοποθέτησης του πλαισίου (i) ως προς το (i-1):
γωνία α, μετατοπίσεις a, d, και γωνία θ
Άρθρωση i Άρθρωση i+1Άρθρωση i-1
Σύνδεσμος iΣύνδεσμος i-1
Σύνδεσμος i+1Σύνδε
σμος i-2
xi-1
zi-1
Oi-1
xi
ziyi
θi
di
ai
αi
OiΣi
(i = 1, …, n)αi
84Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Μέθοδος Denavit-Hartenberg (2)Άρθρωση i Άρθρωση i+1Άρθρωση i-1
Σύνδεσμος iΣύνδεσμος i-1
Σύνδεσμος i+1Σύνδε
σμος i-2
xi-1
zi-1
Oi-1
xi
ziyi
θi
di
ai
αi
OiΣi
(i = 1, …, n)αi
• θi : γωνία μεταξύ του άξονα xi-1 και της κοινής καθέτου ΣiΟi(στροφή γύρω από τον άξονα zi-1 – άρθρωση i)
• di : η απόσταση Oi-1 και Σi (μετατόπιση κατά μήκος του zi-1 – άρθρωση i)
• ai : το μήκος της κοινής καθέτου ΣiΟi (άρθρωση i ´ άρθρωση i+1)
• αi : γωνία μεταξύ του άξονα zi-1 και zi (στροφή γύρω από τον άξονα xi)
43
85Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Μέθοδος Denavit-Hartenberg (3)Άρθρωση i Άρθρωση i+1Άρθρωση i-1
Σύνδεσμος iΣύνδεσμος i-1
Σύνδεσμος i+1Σύνδε
σμος i-2
xi-1
zi-1
Oi-1
xi
ziyi
θi
di
ai
αi
OiΣi
(i = 1, …, n)αi
• Βήμα 1: περιστροφή του πλαισίου i-1 γύρω από τον άξονα zi-1
κατά γωνία θi• Βήμα 2: μετατόπιση di του πλαισίου i-1 κατά μήκος του άξονα zi-1
• Βήμα 3: μετατόπιση ai (μήκος της κοινής καθέτου) κατά το νέο (στραφέντα) άξονα xi-1 (που τώρα συμπίπτει με τον xi)
• Βήμα 4: περιστροφή γύρω από τον άξονα xi κατά γωνία αi
86Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Μέθοδος Denavit-Hartenberg (4)• Βήμα 3: μετατόπιση ai κατά τον άξονα xi
• Βήμα 4: περιστροφή γύρω από τον άξονα xi κατά γωνία αi
• Βήμα 1: περιστροφή γύρω από τον zi-1 κατά θi
• Βήμα 2: μετατόπιση di κατά μήκος του άξονα zi-1
Α =
1 0 0 ai0 cosαi -sinαi 00 sinαi cosαi 00 0 0 1
Σii
Α =
cosθi -sinθi 0 0sinθi cosθi 0 0
0 0 1 di0 0 0 1
i-1Σi
44
87Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Η Μήτρα Denavit-Hartenberg
cosθi -sinθi cosαi sinθi sinαi ai cosθi
sinθi cosθi cosαi -cosθi sinαi ai sinθi
0 sinαi cosαi di
0 0 0 1
Α = i-1i Α · A = Σi
ii-1Σi
Θέση και προσανατολισμός του πλαισίου i ως προς το i-1:
• qi = θi για περιστροφική άρθρωση• qi = di για πρισματική άρθρωση
ai και αi ορίζονται από τη γεωμετρία του συνδέσμουκαι είναι σταθερές
88Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Μέθοδος Denavit-Hartenberg: Παράδειγμα
x0
y0
z0
x2
a1
q1
x1
z1y1
a2
q2
q3
y2
z2d3
y3
x3
z3
q3d3003q20-90oa22q100a11
θidiαiaiΣύνδε-σμος i
Πίνακας ΠαραμέτρωνDenavit-Hartenberg
45
89Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα 1: Ρομποτικός Βραχίονας 2-R-1-P
x0
y0
z0
O0άρθρωση 1
q1
x3
y3z3O3
q2
άρθρωση 2
x1
z1
O1d3
x2
y2
z2
O2
l0
l1
x0
y0
z0
O0
x1l1
l0
z1
y1O2
x2
y2
z2
x3
y3
z3
O3
d3
q1
q2
1R
2R
3P
O1
Πίνακας ΠαραμέτρωνDenavit-Hartenberg
0d3003q2l1+90o02q1l0-90o01
θidiαiaiΣύνδε-σμος i
Εύρεση κινηματικού μοντέλου
Κινηματική Δομή: Διάγραμμα
90Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα 1 (2-R-1-P) (συνέχεια) (1)
0d3003q2l1+90o02q1l0-90o01
θidiαiaiΣύνδε-σμος i
Πίνακας ΠαραμέτρωνDenavit-Hartenberg cosq1 0 -sinq1 0
sinq1 0 cosq1 00 -1 0 l0
0 0 0 1
A =01
cosq2 0 sinq2 0sinq2 0 -cosq2 0
0 1 0 l1
0 0 0 1
A =12
1 0 0 00 1 0 00 0 1 d3
0 0 0 1
A =23T = A =0
3
c1c2 -s1 c1s2 -l1s1 + d3c1s2s1c2 c1 s1s2 l1c1 + d3s1s2
-s2 0 c2 l0 + d3c2
0 0 0 1
p (q1,q2,d3)03R (q1,q2)
03
46
91Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα 1 (2-R-1-P) (συνέχεια) (2)
• Γεωμετρικό μοντέλο : «Εποπτική» (γεωμετρική) λύση
x
l0
O3
d3
y
z
Oq1
q2
l1
l1
d3xy
d3z
d3xyl1
q1
d3y
x
y
p3z = l0 + d3z
p3y = l1 cosq1 + d3y
p3z
p3y
όπου: d3z = d3 cosq2
p3x = -l1 sinq1 + d3x
όπου: d3y = d3xy sinq1
όπου: d3x = d3xy cosq1
( d3xy = d3 sinq2 )
p3z = l0 + d3c2
p3y = l1 c1 + d3 s2s1
p3x = -l1 s1 + d3 s2c1
Άρα:
O
p (q1,q2,d3)03
92Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα 2: Ρομποτικός Βραχίονας 3-R
3 βαθμοί ελευθ.3D, στο χώρο
άρθρωση
1
άρθρωση 2
άρθρωση 3
Ρομποτικός Βραχίονας 3-R(3 περιστροφικές αρθρώσεις: q1, q2, q3)
x0
y0
z0
O0
q1
q2
q3
l1
l2
l3
q1
Κινηματικό (γεωμετρικό) μοντέλο:
O1
O2
OΕ
z1
y1
x1
z2
x2
y2
x
y
xE
yEzE
x
yz
q1
q2
q3
l1
l2
l3
OΕ
O2
O1
O0
A = Rot(z,q1) · Tra(z,l1)01
A = Rot(y,q2) · Tra(z,l2)12
A = Rot(y,q3) · Tra(z,l3)23
A = A · A · A 03
23
12
01
47
93Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα 2 (3-R) (συνέχεια) (1)
x
yz
q1
q2
q3
l1
l2
l3
OΕ
O2
O1
O0
A (q1) =01
c1 -s1 0 0s1 c1 0 00 0 1 l10 0 0 1
A (q2) =12
c2 0 s2 l2s20 1 0 0
-s2 0 c2 l2c20 0 0 1
A (q3) =23
c3 0 s3 l3s30 1 0 0
-s3 0 c3 l3c30 0 0 1
A (q) =13
c2 0 s2 l2s20 1 0 0
-s2 0 c2 l2c20 0 0 1
A (q) =03
c3 0 s3 l3s30 1 0 0
-s3 0 c3 l3c30 0 0 1
=
c23 0 s23 l2s2 + l3s230 1 0 0
-s23 0 c23 l2c2 + l3c230 0 0 1
c1 -s1 0 0s1 c1 0 00 0 1 l10 0 0 1
c23 0 s23 l2s2 + l3s230 1 0 0
-s23 0 c23 l2c2 + l3c230 0 0 1
…
, ,
94Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Παράδειγμα 2 (3-R) (συνέχεια) (2)
• Γεωμετρικό μοντέλο ρομπότ 3-R :
p3z = l1 + l2 cosq2 + l3 cos(q2+q3)p3y = p3xy sinq1
p3x = p3xy cosq1όπου: p3xy = l2 sinq2 + l3 sin(q2+q3)
p3y
x
l1
O3
y
z
O
q3
p3xy
q1
l2
q2
O2
O1
p3xp3x = (l2 s2 + l3 s23) c1p3x = (l2 s2 + l3 s23) s1p3z = l1 + l2 c2 + l3 c23
p (q1,q2,q3)03
«Εποπτική» (γεωμετρική) λύση:
Αλγεβρικό γινόμενο διαδοχικών μετασχηματισμών:
p (q1,q2,q3) = 03
(l2s2 + l3s23) c1(l2s2 + l3s23) s1l1 + l2c2 + l3c23
A [1:3, 4 ] =03
48
95Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη Κινηματική Ανάλυση Ρομποτικών Χειριστών
96Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη Κινηματική Ανάλυση
• Ορθή κινηματική ανάλυση (γεωμετρικό μοντέλο): κινηματική εξίσωση ρομπότ, δηλ. από τιςμετατοπίσεις qi (i=1,..,n) των n αρθρώσεων εύρεσηθέσης και προσανατολισμού τελικού στοιχείου δράσης
• Αντίστροφη κινηματική ανάλυση: εύρεση των μετατοπίσεων qi (i=1,..,n) των αρθρώσεων που οδηγούντο τελικό στοιχείο δράσης σε επιθυμητή θέση καιπροσανατολισμόΓια την τοποθέτηση του τελικού στοιχείου σε οποιαδήποτεθέση/προσανατολισμό μέσα στο χώρο εργασίας (workspace) απαιτείται το ρομπότ να έχει τουλάχιστον 6 βαθμούς ελευθερίας
49
97Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Ένα απλό παράδειγμα
θ
X
Yd
(px , py)
Δεδομένα: px , py
Εύρεση: [q1, q2] = [θ, d]
Εύρεση θ :
Πιο συγκεκριμένα: arctan 2( )y
x
pp
θ =
arctan( ) ( rad)y
x
pk
pθ π= ± ⋅
Εύρεση d : ( )2 2x yd p p= +
px = d cos(θ)py = d sin(θ)
Σφαιρικός επίπεδος μηχανισμός(planar polar mechanism)
⇒ tan(θ) = py / px
98Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Παράδειγμα 2
12
l1q1
q2
x0
y0
O0
l2y1
yΕxΕ
x1O1
OΕθ
Δεδομένα: l1 , l2 , px , py
Εύρεση: [q1 , q2]
px
py
θ
px = l1 cos(q1) + l2 cos(q1 + q2)py = l1 sin(q1) + l2 sin(q1 + q2)θ = q1 + q2
2 2 2 2 2 21 1 2 12 1 2 1 122 2 2 21 1 2 12 1 2 1 12
( ) ( ) c c 2 c c
s s 2 s sx yp p l l l l
l l l l
⎛ ⎞⎜ ⎟⎝ ⎠
+ = + + +
+ + +
2 2 2 21 2 1 2 1 12 1 12( ) ( ) 2 (c c s s )x yp p l l l l⎛ ⎞
⎜ ⎟⎝ ⎠
+ = + + +
cosq2
2 2 2 21 2
1 22
( ) ( )2arccos x yp p l ll lq
⎛ ⎞⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠
+ − −= ±
(px , py)
50
99Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Παράδειγμα 2 (συνέχεια)
12
l1
q1
q2
x0
y0
O0
l2
yΕ xΕOΕθ
2η λύση
py
px
2 2 2 21 2
1 22
( ) ( )2arccos x yp p l ll lq
⎛ ⎞⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠
+ − −= ±
φd q2
q1
ψ
[px,py]
Νόμος των ημιτόνων στο τρίγωνο Ο0Ο1ΟΕ :O1
2
sin sind lϕ ψ= ⇒ l2 sin(180ο-q2) = d sinψ
⇒ ψ = arcsin(l2s2/d)
tan(q1+ψ) = py/px ⇒ q1 = atan2(py/px) - ψ
όπου: d = sqrt((px)2+(py)2)
Γεωμετρική λύση για το q1
Άρα: , 2 21 2 2
x y
satan2 arcsin(p ) (p )
y xlq p p
⎛ ⎞⎜ ⎟⎛ ⎞ ⎜ ⎟⎜ ⎟
⎝ ⎠ ⎜ ⎟⎜ ⎟⎝ ⎠
= −+
100Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Ρομπότ 6 βαθμών ελευθερίας (5-R-1-P)
x0
y0
z0O0
z1
y1
x1 z2x2
y2
o
aOE
n
-q2
d3 -d3cosq2
d3sinq2
ΣφαιρικόςΚαρπός
Σ
z0l0 y0
z1
y1
x1
z2
y2l1
l2
o
a
d3
z4
Σ≡O3≡O4≡O5z3≡z5
x3≡x4≡x5
O1q50+90o05
0d3003
q2l1+90o02
q6l2006
q40-90o04
q1l0-90o01
θidiαiaiΣύνδε-σμος i
Παράμετροι D-H
51
101Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Ρομπότ 5-R-1-P (συνέχεια) (1)
A (q1) =01
c1 0 -s1 0s1 0 c1 00 -1 0 l00 0 0 1
A (q2) =12
c2 0 s2 0s2 0 -c2 00 1 0 l10 0 0 1
A (d3) =23
1 0 0 00 1 0 00 0 1 d30 0 0 1
A (q4) =34
c4 0 -s4 0s4 0 c4 00 -1 0 00 0 0 1
A (q5) =45
c5 0 s5 0s5 0 -c5 00 1 0 00 0 0 1
A (q6) =56
c6 -s6 0 0s6 c6 0 00 0 1 l20 0 0 1
T = A (q1) · A (q2) · A (d3) · A (q4) · A (q5) · A (q6) 01
12
23
34
45
56
Αντίστροφο κινηματικό πρόβλημα: δοσμένου Τ εύρεση {qi}
102Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Ρομπότ 5-R-1-P (συνέχεια) (2)
Έστω T =
nx ox ax pxny oy ay pynz oz az pz0 0 0 1
Έστω επίσης:(η θέση του σημείου Σ)
p = p*= [px py pz]T0Σ
* * *
px
py
pz
=px - l2ax
py - l2ay
pz - l2az*
*
*
p* = p - l2 a
z0l0 y0
z1
y1
x1
z2
y2l1
l2
o
a
z4
Σ≡O3≡O4≡O5z3≡z5
O1
d3p*
p
52
103Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Ρομπότ 5-R-1-P (συνέχεια) (3)
A (q2,d3) =13
c2 0 s2 d3 s2s2 0 -c2 -d3 c20 1 0 l10 0 0 1
c1 s1 0 00 0 -1 l0
-s1 c1 0 00 0 0 1
=
(A )-1(q1) =01
c1 s1 0 00 0 -1 l0
-s1 c1 0 00 0 0 1
... ... ... px
... ... ... py
... ... ... pz0 0 0 1
*
*
* … … … pxc1+pys1… … … -pz+ l0… … … -pxs1+pyc10 0 0 1
* *
**
*
* * 2 * 2 2* * 1
1 1 1 1 *1
( ) ( ) - s c 2arctan x x yx y
y
p p p ll p p ql p
⎛ ⎞⎜ ⎟⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠
− ± + −= + ⇒ =
+τ = tan(θ/2)sinθ = (2τ)/(1+τ2)cosθ = (1-τ2)/(1+τ2)
(A )-1 · A =01
03
A =13
1 1 * *
2 *0
c sarctan x y
z
p pqp l
⎛ ⎞+⎜ ⎟
⎜ ⎟−⎜ ⎟
⎝ ⎠
= * * 2 * 23 1 1 0( c s ) ( l )zx yd p p p=± + + −
104Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Ρομπότ 5-R-1-P (συνέχεια) (4)
A (q5 ,q6) =46 A (q5) A (q6) =
45
56
c5c6 -c5s6 s5 l2s5s5c6 -s5s6 -c5 -l2c5s6 c6 0 00 0 0 1
Έστω T´ =
n´x o´x a´x p´xn´y o´y a´y p´yn´z o´z a´z p´z0 0 0 1
(A A A )-1 · T = A =01
12
23
36
A (q5 ,q6) = (A )-1 · T´ =46
34
… … a´xc4+a´ys4 …-n´z -o´z -a´z …
-n´xs4+n´yc4 -o´xs4+o´yc4 -a´xs4+a´yc4 …0 0 0 1
A (q4) =34
c4 0 -s4 0s4 0 c4 00 -1 0 00 0 0 1
53
105Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου
Αντίστροφη κινηματική ανάλυση: Ρομπότ 5-R-1-P (συνέχεια) (5)
-a´xs4+a´yc4 = 0
a´xc4+a´ys4 = s5
-a´z = -c5
-n´xs4+n´yc4 = s6
-o´xs4+o´yc4 = c6
q4 = arctan(a´y / a´x)
' 'x y4 4
5 'z
a c a sarctana
q⎛ ⎞⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠
+=
' 'x y4 4
6 ' 'x y4 4
-n s n carctan-o s o c
q⎛ ⎞⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠
+=+