Ρομποτικά Συστήματαusers.softlab.ntua.gr/~ktzaf/Courses/robot-kinematics-1a-dpms.pdf4 Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ

1

1Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ. Μάθημα: Ρομποτικά Συστήματα Ελέγχου

Ρομποτικά Συστήματα Ελέγχου: Κινηματική/Στατική/Δυναμική Ανάλυση και Έλεγχος Ρομποτικών Χειριστών

Κων/νος ΤζαφέσταςΤομέας Σημάτων, Ελέγχου & Ρομποτικής

Σχολή Ηλεκτρ. Μηχ/κών & Μηχ/κών Υπολ., Ε.Μ.Π.

Τηλ.: (210) 772-3687 (Κτήριο Ηλεκτρ., Γραφείο 21.11)E-mail: [email protected]: http://www.softlab.ntua.gr/~ktzaf/

Διατμηματικό Πρόγραμμα Μεταπτυχιακών Σπουδών«ΣΥΣΤΗΜΑΤΑ ΑΥΤΟΜΑΤΙΣΜΟΥ»


Περιεχόμενα Μαθήματος

• Κινηματική Ανάλυση– Ορθή και ανάστροφη κινηματική ανάλυση– Διαφορική κινηματική ανάλυση

• Στατική Ανάλυση Ρομποτικών Χειριστών• Δυναμική Ανάλυση Ρομποτικών Μηχανισμών• Ρομποτικός Έλεγχος

– Γραμμικός / Μη-Γραμμικός Έλεγχος Τροχιάς• Σχεδιασμός Τροχιάς Ρομποτικών Χειριστών• Προγραμματισμός Ρομποτικών Χειριστών

2


Βιβλιογραφία(Εισαγωγή στη Ρομποτική)

• Τζαφέστας, Σπύρος Γ., «Ρομποτική. Τομ. 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)


Βιβλιογραφία (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


Τι είναι Ρομπότ? (1/3)

Ετυμολογία του όρου:robota (Τσέχικα): άμισθη/εξαναγκασμένη εργασίαrabu (Σλάβικα): σκλάβος, работать (rabotat’: Ρώσικα): εργασίαarbeit (Γερμανικά): εργασία, ή Erbe (κληρονόμος)

Ρίζα : rob ή rabεπίσης, orb ή orph οrphelin - ορφανός ... serf - σκλαβιάorbh (Ινδο-Ευρωπαϊκή ρίζα): κληρονόμος, κληρονομιά

Πρώτη εμφάνιση της έννοιας: Karel Capek (1921), «RUR: Les robots universels de Rossum",εμφάνιση ενός «Ανδροϊδούς» το οποίο αποκαλείται «robot»...


Τι είναι Ρομπότ? (2/3)

Μπορούμε να ορίσουμε ως ρομπότ μια μηχανή που «αισθάνεται», «σκέφτεται» και «επενεργεί» (sense, think, act). Άρα, ένα ρομπότ διαθέτει: • αισθητήρες (sensors), για την απόκτηση πληροφορίας (a) από το εξωτερικό περιβάλλον (exteroceptive), ή (b) σε σχέση με την εσωτερική κατάσταση (proprioceptive)• δυνατότητες επεξεργασίας (processing) αντίληψη, συλλογισμός, λήψη αποφάσεων, σχεδιασμός δράσης (cognition)• επενεργητές (actuators), για την εκτέλεση κάποιας εργασίας στο περιβάλλον (motion, manipulation)

4


Τι είναι Ρομπότ?

Τρείς βασικές ιδιότητες ενός ρομπότ:• δυνατότητες επαναπρογραμματισμού (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)


Ρομποτική – Εισαγωγή (1)• Ρομπότ: «Ευφυείς», «ευέλικτοι», «προσαρμοζόμενοι» μηχανισμοί κίνηση και δράση στο χώρο

• Κατηγορίες Ρομποτικών Συστημάτων:- Βιομηχανικοί (κλασσικοί) ρομποτικοί χειριστές (industrial

robot manipulators)- Επιδέξιοι ρομποτικοί χειριστές (dextrous robots)- Αυτοκινούμενα ρομπότ – ρομπότ προσφοράς υπηρεσιών

(mobile/service robotics)- Μικρο-ρομποτική (micro-robotics)

Τηλε-ρομποτική vs. Ευφυή/αυτόνομα ρομπότ

5


Ρομποτική – Εισαγωγή (2)• Ρομποτική: «κατακόρυφη» κατάτμηση σε θεματολογικά επιστημονικά πεδία / «οριζόντια» κατάτμηση σε πεδία εφαρμογών

Μηχανική

(ανάλυση

/σχεδίαση)

Ηλεκτρονική

(μικρο

-επεξεργαστές,

Αισθητήρες,

embe

dded

syst

ems e

tc.)

Αυτόματος Έλεγχος

Συστημάτων

Υπολογιστική Νοημοσύνη

Προγραμματισμός Υπολογιστών

Διασύνδεση

ανθρώπου-μηχανής ...

Βιομηχανικές Εφαρμογές(robotized manufacturing etc.)

Προσφορά Υπηρεσιών(service & intervention robots)- mobile robotics (wheeled, legged)- dextrous robotics (medical etc.)- telerobotics - microrobotics


Ρομποτική και Αυτοματοποιημένα Συστήματα Παραγωγής

StaubliStaubli FanucFanuc

Computer IntegratedComputer IntegratedManufacturing (CIM)Manufacturing (CIM)

Ολοκληρωμένα συστήματα Ολοκληρωμένα συστήματα προγραμματισμού αυτοματοπρογραμματισμού αυτοματο--ποιημένων διαδικασιών ποιημένων διαδικασιών παραγωγήςπαραγωγής

6


Επιδέξιοι Ρομποτικοί ΧειριστέςΡομποτικοί Χειριστές με πλεονέζοντες βαθμούς ελευθερίας(redundant robot manipulators)

DLR lightweight 7dof robot On-line obstacle avoidance(kinematic redundancies)


Ρομποτικοί Χειριστές με Πλεονάζοντες Βαθμούς Ελευθερίας (1/2)

NASA – Robotics Research – Modular Redundant Robot ModArm

7


Ρομποτικοί Χειριστές με Πλεονάζοντες Βαθμούς Ελευθερίας (2/2)

NASA – RoboticsResearch - ModArm DLR – KineMedic Redundant Robot

Εφαρμογές στο Διάστημα Ιατρικές Εφαρμογές

(όπου απαιτείται αυξημένη «ικανότητα χειρισμού»)


Συνεργαζόμενοι Ρομποτικοί Χειριστές (1)

ARM –Autonomous Robotic Manipulation Program (DARPA)

8


Dual Arm Robot Manipulator (Dexter/UMass)



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)


9


Επιδέξια (Ανθρωπόμορφα) Ρομποτικά Χέρια(Dexterous Robot Hands) (1/3)

Utah/MIT robot handUtah/MIT robot handJPL/NASA handJPL/NASA hand

ΔεξιότηταΔεξιότητα: : Συνεργασία πολλαπλών βαθμών ελευθερίας για τονΣυνεργασία πολλαπλών βαθμών ελευθερίας για τονέλεγχο σύνθετων/λεπτών εργασιών χειρισμούέλεγχο σύνθετων/λεπτών εργασιών χειρισμού


Επιδέξια Ρομποτικά Χέρια (2/3)

Shadow Robot HandThumb &

fingersLittle finger

Drill Demo

10


Επιδέξια Ρομποτικά Χέρια (3/3)

DLR Hand DLR Hand ΙΙΙΙ

Robonaut Robonaut Humanoid / NASAHumanoid / NASA


Επιδέξια Ρομποτικά Χέρια – Παραδείγματα (2)

DLR Hand DLR Hand ΙΙΙΙ

UUΜΜass Humanoid Robotass Humanoid Robot

11


Ρομποτικό Χέρι (υπό κατασκευή)


• Αυτόνομη εκμάθηση δεξιοτήτων– Μη επιβλεπόμενη (αναπτυξιακή) μάθηση (developmental robot learning)

• Εκπαίδευση ρομπότ από άνθρωπο / Αλληλεπίδραση ανθρώπου‐ρομπότ– Μάθηση μέσω παρατήρησης (robot teaching by demonstration, learning by observation / by imitation)

– Τηλερομποτική / Τηλεχειρισμός

Εκμάθηση Ρομποτικών Δεξοτήτων

12


Τηλερομποτική – Εισαγωγή

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


Τηλερομποτική – Ιστορική αναδρομή

• 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


Εξέλιξη της Τηλερομποτικής

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


Ρομποτικός Τηλεχειρισμός

Master-slave telemanipulation(άμεσος αμφίδρομος τηλεχειρισμός)

(CEA / France)

Computer-assisted teleoperation(τηλεχειρισμός υποβοηθούμενος από υπολογιστή) (JPL / NASA - USA)

Παραδείγματα συστημάτων:

Τηλερομποτική: Eφαρμογές σε περιβάλλοντα εργασίας (α) μη δομημένα (unstructured) και (β) μη φιλικά (hostile) προς τον άνθρωπο (τηλεχειρισμός ραδιενεργών υλικών, διάστημα, υποβρύχια έρευνα, απενεργοποίηση βομβών κλπ.)

DLR – Γερμανία

LRP – Γαλλία

Potiers Vilette

TUM – Γερμανία

14


Τηλερομποτική: Βασικές Έννοιες

• Στόχος: «Τηλεπαρουσία» (Telepresence)– «Διαφάνεια» (transparency) του συστήματος ως προςτην ανατροφοδότηση αισθητηριακής πληροφορίας

– «Παρουσία» (feel of presence) στον απομακρυσμένο χώρο ρομποτικής εργασίας (sensory-physical/action-decision)

• Πρόβλημα: χρονικές καθυστερήσειςστο βρόχο αμφίδρομης επικοινωνίας (time-delays)– Προβλεπτικός Έλεγχος (predictive control)– Προβλεπτικά μοντέλα απεικόνισης και ανάδρασης

(predictive displays) – Μοντέλα Εικονικής Πραγματικότητας


Εφαρμογή – Ρομποτική Χειρουργική

Σύγχρονη Ρομποτική Τεχνολογία ως μέσο

υποβοήθησης / υποκατάστασης του

έργου του χειρουργού, μέσω φυσικής

δράσης (τηλεχειριζόμενης ή αυτόνομης)

στο πραγματικό επεμβατικό πεδίο

15


Χειρουργικά Ρομποτικά Συστήματα

• DaVinci® Surgical Robotic System (1/2)


Χειρουργικά Ρομποτικά Συστήματα

• DaVinci® Surgical Robotic System (2/2)

«Απτικές»Διατάξεις

«Ρομποτικές»Διατάξεις

(master)

(slave)

16


Νανορομποτική Χειρουργική

Surgical console for cellular surgery

Courtesy Prof Jaydev Desai, Drexel Univ, Philadelphia, PA 2005


«Φιλικές» Ρομποτικές Εφαρμογές

Η Τεχνολογία είναι «αρκετά ώριμη» για ενσωμάτωση σε τόσο «επεμβατικές» εφαρμογές;

Ποιό το «αποδεκτό ρίσκο»;

Η κοινωνία είναι «έτοιμη» να αποδεχτεί τέτοιες σημαντικές μεταβολές στην παροχή υπηρεσιών υγείας;

Περισσότερο «φιλικές» (μη επεμβατικές) εφαρμογές ρομποτικής τεχνολογίας:Ρομπότ βοηθοί / νοσηλευτές Ρομποτικά - Απτικά συστήματα στη χειρουργική εκπαίδευση, άσκηση και πιστοποίηση δεξιοτήτων

17


Ρομπότ Βοηθός / Νοσηλευτής

SATAVA 7 July, 1999DARPA

InTouch Technologies, Inc, Goleta, CA


Χειρουργικοί Προσομοιωτές

Laparoscopic Simulator with tactile feedbackXitact, Lausanne Switzerland

Λαπαρο‐ενδοσκοπικός Προσομοιωτής με ανάδρασης αφής

18


Χειρουργικοί Προσομοιωτές (2)

Συστήματα εικονικής προσομοίωσης ιατρικών(επεμβατικών) πράξεων

Συστήματα Εικονικής Πραγματικότητας (virtual reality) με «ανάδρασηαφής» (haptic display) για προσομοίωση κλινικών πράξεωνΕκπαίδευση και πιστοποίηση κλινικών «δεξιοτήτων»

ΑπτικήΡομποτικήΔιάταξη

Εικονική προσομοίωση


Διεπιστημονική Συνέργεια –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


Αυτοκινούμενα Ρομπότ• Περιβάλλον Κίνησης (ground, air, sea, underwater, ...)• Μηχανισμοί κίνησης (wheeled, legged, hybrid, etc. ...)• Αισθητήριες διατάξεις (sensors)• Σύνθεση αισθητηριακών δεδομένων (sensor/data fusion)

– Χαρτογράφηση Χώρου (mapping)– Εντοπισμός θέσης (localization)– Συνδυασμένη χαρτογράφηση και εκτίμηση θέσης σε άγνωστο δυναμικό περιβάλλον (SLAM)

• Σχεδιασμός Δρόμου (path planning) • Σχεδιασμός Δράσης (task planning)• Μάθηση Συμπεριφορών …


Αυτοκινούμενα Ρομπότ – Αισθητήρες(mobile robots – sensors)

Αισθητήρες Αισθητήρες ΥπερήχωνΥπερήχων

ΣύστημαΣύστημαΌρασηςΌρασης

ΑσύρματοΑσύρματοEthernetEthernet

Αισθητήρες Αισθητήρες ΥπέρυθρωνΥπέρυθρων

Laser Laser Range FinderRange Finder

ΜικρόςΜικρόςGripperGripper

ActiveMedia ActiveMedia RobotsRobots RWI RWI –– IS RoboticsIS Robotics

Video-1

Video-2

20


Τροχοφόρα Αυτοκινούμενα Ρομπότ –Σύγχρονα Συστήματα

(in action)

Indoor Outdoor


UAV – Unmanned Aerial VehiclesΠαράδειγμα Εφαρμογών: Autonomous Helicopter Projects

CM

U P

roje

ct

21


Αυτόνομα Υποβρύχια Ρομπότ

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)


Mars Rovers (Διαστημικά Ρομποτικά Οχήματα)

Rocky I

Sojourner

Rocky IV prototype

Mars “Pathfinder” Mission

22


Mars Rovers (συνέχεια)

Marsokhod concept

(συνεργασίαNASA – ESA)


Mars Rovers – Spirit και Opportunity

Χαρτογράφηση και Σχεδιασμός Δρόμου

anim-part3

23


Βαδίζοντα Ρομπότ – (Ανθρωπόμορφα) Δίποδα με Σύστημα Όρασης (biped walking robots)

JohnnieΠολυτεχνείο Μονάχου

(TUM)

ΣύστημαΣύστημαΚατευθυνόμενηςΚατευθυνόμενηςΣτερεοσκοπικήςΣτερεοσκοπικήςΌρασηςΌρασης

Σχεδιασμός καιΣχεδιασμός καιΈλεγχος της κίνησηςΈλεγχος της κίνησηςτου Ρομπόττου Ρομπότ

Sample movie(Johnnie)

Αισθητήρες ΔύναμηςΑισθητήρες Δύναμης((force/tactile sensors)force/tactile sensors)


Δυναμικός ρομποτικός βηματισμός

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


“Biologically-inspired” legged robots

Uniroo (1991-1993)Troody


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


Βαδίζοντα Ρομπότ - Εφαρμογές Εφαρμογές intervention, service, exploration, rescue, etc.

Μεταφορά «Υλικού» -Επιχειρήσεις διάσωσης

Εξερεύνηση «δύσβατων» περιοχών

Εξάποδο (hexapod) ρομπότ DanteΤετράποδο Ρομπότ BigDog,

CMU / Boston Dynamics


Ολοκληρωμένα Κινούμενα Ρομποτικά Συστήματα Υπηρεσιών (Service Robots)

Βαδίζοντα Ανθρωπόμορφα ΡομπότΒαδίζοντα Ανθρωπόμορφα ΡομπότΚινούμενα Ρομπότ με Κινούμενα Ρομπότ με Ενσωματωμένο Ρομποτικό ΒραχίοναΕνσωματωμένο Ρομποτικό Βραχίονα

walkwalk

stepstep

Honda Honda HumanoidHumanoid RobotRobotχειρισμόςχειρισμός συνεργασίασυνεργασία

26


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)


Τροχοφόρα αυτοκινούμενα ρομπότ με σύνθετο σύστημα οδήγησης τροχών

27


Εφαρμογές Ολοκληρωμένων Κινούμενων Ρομποτικών Οχημάτων

Εντοπισμός & απενεργοποίηση εκρηκτικών

Ρομπότ «Διάσωσης» (Rescue)

Intervention & Service Robots (Ρομπότ Παρέμβασης και Υπηρεσιών)

Ρομπότ «Εξερεύνησης» σε περιβάλλον «μη φιλικό» προς τον άνθρωπο


«Υβριδικοί» Μηχανισμοί Κίνησης

NASA's ATHLETE (All-Terrain Hex-Legged Extra-Terrestrial Explorer)

28


Σύνθετοι Μηχανισμοί Κίνησης«Αναρριχόμενα» ρομπότ

(climbing robots)

JPL’

sLE

MU

R ro

bot

«Έρποντα» Ρομπότ (snake robots)

SnakeRescueRobot

Mod-SnakeRobot(CMU)


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


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


ΕΝΟΤΗΤΑ 1:Κινηματική ΑνάλυσηΡομποτικών Χειριστών

30


Κινηματική Ανάλυση των Ρομπότ

• Προκαταρκτικά Γεωμετρικά Εργαλεία– Μετασχηματισμοί στο χώρο κλπ.

• Ορθή κινηματική ανάλυση ρομπότ (γεωμετρικό μοντέλο)– Μετατοπίσεις αρθρώσεων {qi} Θέση/Προσανατολισμός (x,θ) τελικούστοιχείου δράσης του ρομπότ

• Αντίστροφη κινηματική ανάλυση• Ορθή διαφορική κινηματική ανάλυση (κινηματικό μοντέλο)

– Ιακωβιανή μήτρα J: ταχύτητες αρθώσεων {qi} ταχύτητα (v,ω) τελικού στοιχείου δράσης του ρομπότ

• Αντίστροφη διαφορική κινηματική ανάλυση


Βασικοί Ορισμοί – Αρχές

• Ρομποτικοί βραχίονες (βιομηχανικοί ρομποτικοί χειριστές) (robot manipulators): ανοικτές κινηματικές αλυσίδες

• Κινηματική αλυσίδα (kinematic chain): σύστημα στερεώνσωμάτων που συνδέονται μέσω αρθρώσεων (joints)

• Βαθμοί ελευθερίας (degrees of freedom - DOF):αριθμός ανεξάρτητων μεταβλητών για την περιγραφή της διάταξης(configuration) ενός μηχανισμού στο χώρο

31


Βασικές Ρομποτικές Αρθρώσεις

Σφαιρική άρθρωση(Spherical Joint)3 DOF (Variables - θ1, θ2, θ3)

Περιστροφική άρθρωση(revolute joint)1 βαθμός ελευθερίας(degree of freedom – DOF)(Μεταβλητή : θ ή q)

Γραμμική (πρισματική) άρθρωση(prismatic joint)1 DOF (linear) (Variable - d)


Ρομποτικοί Βραχίονες / Χειριστές:Ανοικτές (σειριακές) κινηματικές αλυσίδες

Ορολογία:Link = σύνδεσμοςJoint = άρθρωσηActuator = κινητήρας (κινητήριο στοιχείο)End-effector = τελικό στοιχείο δράσης

32


Παράλληλες κινηματικές αλυσίδες

Επίπεδος παράλληλος μηχανισμός

Πλατφόρμα Stewart (6 DOF)


Παράδειγμα Ρομποτικού Βραχίονα –Το ρομπότ 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


Παράδειγμα Αρθρωτού Ρομπότ 6 DOF: Το ρομπότ Τ3

1

23

4

5

6


Ρομπότ Adept 1850 Palletizer

34


Κινηματική Δομή Κλασσικών Ρομποτικών Χειριστών: Ταξινόμηση

Αρθρωτό ρομπότ (τύπου PUMA) Ρομπότ τύπου SCARA

Καρτεσιανό ρομπότ Κυλινδρικό ρομπότ Σφαιρικό ρομπότ


Κινηματική Ανάλυση: Προκαταρκτικά Γεωμετρικά Εργαλεία

• Θέση και προσανατολισμός στερεού σώματος

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


Μετασχηματισμοί στο χώρο• Μετασχηματισμοί συντεταγμένων

Έστω 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Ο


Στροφή – Ειδικές Περιπτώσεις

• Περιστροφή γύρω από τον άξονα 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


Παραμετροποίηση Στροφής

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


Ομογενείς Μετασχηματισμοί

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


Ομογενείς Μετασχηματισμοί (συνέχεια)

Το ομογενές διάνυσμα 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


Ομογενείς Μήτρες Μετασχηματισμών – Ειδικές Περιπτώσεις (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


Ομογενείς Μήτρες Μετασχηματισμών – Ειδικές Περιπτώσεις (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


Διαδοχικοί ομογενείς μετασχηματισμοί

Α : 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


Ορθή Κινηματική Ανάλυση(Γεωμετρικό μοντέλο ρομποτικού βραχίονα)


Κινηματική Ανάλυση: Εισαγωγή• Ορθή κινηματική ανάλυση

– Μετατοπίσεις αρθρώσεων {qi} Μετατόπισητελικού στοιχείου δράσης (θέση p, προσανατολισμός R)

– Μετασχηματισμός από το χώρο αρθρώσεων στο χώρο δράσης (εργασίας) proprioception

• Αντίστροφη κινηματική ανάλυση– Θέση τελικού στοιχείου δράσης (p , R) {qi}

• Αντίστροφη διαφορική κινηματική ανάλυση– Ταχύτητα τελικού στοιχείου δράσης (v , ω)

{qi} Σχεδιασμός δρόμου ρομπότ

40


Ορθή κινηματική ανάλυση:ανοικτές κινηματικές αλυσίδες

Τελικό στοιχείο δράσης

Σύνδεσμος 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

συνημίτονα κατεύθυνσης


Ορθή κινηματική ανάλυση:ανοικτές κινηματικές αλυσίδες (συνέχεια)

Κινηματική εξίσωση (γεωμετρικό μοντέλο) ρομποτικού βραχίονα:Συνδυασμός των διαδοχικών μετασχηματισμών Α (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


Ορθή κινηματική ανάλυση:Παράδειγμα (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


Ορθή κινηματική ανάλυση:Παράδειγμα (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


Μέθοδος 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


Μέθοδος Denavit-Hartenberg (2)Άρθρωση 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


Μέθοδος Denavit-Hartenberg (3)Άρθρωση 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


Μέθοδος 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


Η Μήτρα 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 ορίζονται από τη γεωμετρία του συνδέσμουκαι είναι σταθερές


Μέθοδος 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


Παράδειγμα 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


Εύρεση κινηματικού μοντέλου

Κινηματική Δομή: Διάγραμμα


Παράδειγμα 1 (2-R-1-P) (συνέχεια) (1)

0d3003q2l1+90o02q1l0-90o01


Πίνακας Παραμέτρων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


Παράδειγμα 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


Παράδειγμα 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


Παράδειγμα 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

…

, ,


Παράδειγμα 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


Αντίστροφη Κινηματική Ανάλυση Ρομποτικών Χειριστών


Αντίστροφη Κινηματική Ανάλυση

• Ορθή κινηματική ανάλυση (γεωμετρικό μοντέλο): κινηματική εξίσωση ρομπότ, δηλ. από τιςμετατοπίσεις qi (i=1,..,n) των n αρθρώσεων εύρεσηθέσης και προσανατολισμού τελικού στοιχείου δράσης

• Αντίστροφη κινηματική ανάλυση: εύρεση των μετατοπίσεων qi (i=1,..,n) των αρθρώσεων που οδηγούντο τελικό στοιχείο δράσης σε επιθυμητή θέση καιπροσανατολισμόΓια την τοποθέτηση του τελικού στοιχείου σε οποιαδήποτεθέση/προσανατολισμό μέσα στο χώρο εργασίας (workspace) απαιτείται το ρομπότ να έχει τουλάχιστον 6 βαθμούς ελευθερίας

49


Αντίστροφη κινηματική ανάλυση: Ένα απλό παράδειγμα

θ

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


Αντίστροφη κινηματική ανάλυση: Παράδειγμα 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


Αντίστροφη κινηματική ανάλυση: Παράδειγμα 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

⎛ ⎞⎜ ⎟⎛ ⎞ ⎜ ⎟⎜ ⎟

⎝ ⎠ ⎜ ⎟⎜ ⎟⎝ ⎠

= −+


Αντίστροφη κινηματική ανάλυση: Ρομπότ 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


Παράμετροι D-H

51


Αντίστροφη κινηματική ανάλυση: Ρομπότ 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}



Έστω 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



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=± + + −



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ý oý aý pýn´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ýs4 …-n´z -o´z -a´z …

-n´xs4+nýc4 -o´xs4+oýc4 -a´xs4+aýc4 …0 0 0 1

A (q4) =34

c4 0 -s4 0s4 0 c4 00 -1 0 00 0 0 1

53



-a´xs4+aýc4 = 0

a´xc4+aýs4 = s5

-a´z = -c5

-n´xs4+nýc4 = s6

-o´xs4+oýc4 = c6

q4 = arctan(aý / 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⎛ ⎞⎜ ⎟⎜ ⎟⎜ ⎟⎝ ⎠

+=+

Documents

Ρομποτικά Συστήματαusers.softlab.ntua.gr/~ktzaf/Courses/robot-kinematics-1a-dpms.pdf4 Εθνικό Μετσόβιο Πολυτεχνείο, Σχολή ΗΜ&ΜΥ