Gliederung. Gliederung (cont.) Inverse Kinematik von Manipulatoren

Transkript

1 - Gliederung Jianwei Zhang Fakultät für Mathematik, Informatik und Naturwissenschaften Technische Aspekte Multimodaler Systeme 26. April 2011 Allgemeine Informationen Einführung Koordinaten eines Manipulators Kinematik-Gleichungen Analytische Lösbarkeit eines Manipulators Beispiel 1: ein planarer dreigelenkiger Manipulator Algebraische Lösung des PUMA 560 Die Lösung für RPY-Winkel Geometrische Lösung des PUMA 560 Eine Programmierumgebung für Roboter unter UNIX: RCCL Differentielle Bewegungen mit homogenen Transformationen Jacobi-Matrix eines Manipulators J. Zhang 91 J. Zhang 92 Gliederung (cont.) Aufgabenbeschreibung Robotergrammierung auf drei Ebenen Trajektoriegenerierung Trajektoriengenerierung Einführung in RCCL Dynamik Roboterregelung Programmierung auf Aufgabenebene und Bahnplanung Programmierung auf Aufgabenebene und Bahnplanung Programmierung auf Aufgabenebene und Bahnplanung Architekturen sensorbasierter intelligenter Systeme Aus- und Rückblick Die Problematik: Robotermanipulatoren werden meistens im Gelenkwinkelraum gesteuert. Zu handhabende Objekte werden aber meistens im Weltkoordinatensystem dargestellt. Um einen bestimmten Tool-Frame T bezüglich des Welt-Frames zu erreichen, werden die Gelenkwinkel θ(t) = (θ 1 (t), θ 2 (t),..., θ n (t)) T in folgenden zwei Schritten berechnet: 1. Berechne T 6 = Z 1 BGE 1 ; 2. Berechne θ 1, θ 2,..., θ n aus T 6. : inverse Kinematik, sogar wichtiger als direkte Kinematik J. Zhang 93 J. Zhang 94

2 Lösbarkeit am Beispiel PUMA 560 Lösbarkeit am Beispiel PUMA 560 wobei n x s x a x p x T 6 = T T = n y s y a y p y n z s z a z p z n x = C 1 [C 23 (C 4 C 5 C 6 S 4 S 6 ) S 23 S 5 C 6 ] S 1 (S 4 C 5 C 6 + C 4 S 6 ) (2) n y = S 1 [C 23 (C 4 C 5 C 6 S 4 S 6 S 23 S 5 S 6 ] + C 1 (S 4 C 5 C 6 + C 4 S 6 ) (3) n z = S 23 [C 4 C 5 C 6 S 4 S 6 ] C 23 S 5 C 6 (4) S x =... (5) S y =... (6) S z =... (7) a x =... (8) a y =... (9) a z =... (10) J. Zhang 95 J. Zhang 96 Lösbarkeit am Beispiel PUMA 560 Anmerkung p x = C 1 [d 6 (C 23 C 4 S 5 + S 23 C 5 ) + S 23 d 4 + a 3 C 23 + a 2 C 2 ] S 1 (d 6 S 4 S 5 + d 2 ) (11) p y = S 1 [d 6 (C 23 C 4 S 5 + S 23 C 5 ) + S 23 d 4 + s 3 C 23 + a 2 C 2 ] + C 1 (d 6 S 4 S 5 + d 2 ) (12) p z = d 6 (C 23 C 5 S 23 C 4 S 5 ) + C 23 d 4 a 3 S 23 a 2 S 2 (13) Nicht lineare Gleichungen Existenz der Lösungen: Arbeitsraum ( workspace ): das Volumen des Raums wo der End-Effektor des Manipulators erreichen kann. dextrous workspace reachable workspace Mehrere Gelenkstellungen, die zur gleichen Effektorstellung führen Z.B. für PUMA 560: Mehrdeutigkeiten der Lösung θ 1, θ 2, θ 3 zu gegebenen p. Für jede Lösung θ4, θ 5, θ 6, eine alternative Lösung ist: J. Zhang 97 J. Zhang 98

3 Anmerkung Lösungsmethoden θ 4 = θ θ 5 = θ 5 θ 6 = θ Lösungsstrategien: geschlossene Lösungen vs. numerische Lösungen Geschlossene Form: algebraische Lösung + : korrekte Lösungen aus Gleichungen : geometrisch nicht anschaulich geometrische Lösung + : Fallunterscheidung der Roboter-Konfigurationen : Robotertypen-spezifisch Numerische Form: iterative Verfahren + : Verfahren übertragbar : rechenintensiv, nicht garantierte Konvergenz bei Sondernfällen J. Zhang 99 J. Zhang Analytische Lösbarkeit eines Manipulators Lösungsmethoden Analytische Lösbarkeit eines Manipulators Die inverse Kinematik aller Systeme mit 6 translatorischen und rotatorischen Freiheitsgraden in einer einfachen seriellen Kette ist numerisch lösbar. Unter bestimmten Voraussetzungen an die Armgeometrie (ausreichende Bedingungen) existieren die geschlossenen Lösungen: Entweder: 3 aufeinanderfolgende Achsen schneiden sich in einem Punkt Oder: 3 aufeinanderfolgende Achsen sind parallel Es ist wichtig, einen Manipulator so zu entwerfen, daß eine Lösung in der geschlossenen Form existiert. So sind fast alle Industrieroboter entworfen. Beispiel PUMA: Die Achsen 4, 5 und 6 schneiden sich. J. Zhang 101 J. Zhang 102

4 - Beispiel 1: ein planarer dreigelenkiger Manipulator Beispiel 1: ein planarer dreigelenkiger Manipulator - Beispiel 1: ein planarer dreigelenkiger Manipulator Beispiel 1: ein planarer dreigelenkiger Manipulator Gelenk α i 1 a i 1 d i θ i θ l 1 0 θ l 2 0 θ 3 C 123 S l 1 C 1 + l 2 C 12 0 T 3 = S 123 C l 1 S 1 + l 2 S J. Zhang 103 J. Zhang Beispiel 1: ein planarer dreigelenkiger Manipulator - Beispiel 1: ein planarer dreigelenkiger Manipulator Die algebraische Lösung des Beispiel 1 - I Die algebraische Lösung des Beispiel 1 - II Spezifikation des End-Effektors: (x, y, φ). Für einen solchen Vektor gilt es dann: C φ S φ 0 x 0 T 3 = S φ C φ 0 y Wir bekommen vier Gleichungen: C φ = C 123 (14) S φ = S 123 (15) x = l 1 C 1 + l 2 C 12 (16) y = l 1 S 1 + l 2 S 12 (17) J. Zhang 105 (Ableitung) Eine spezielle Funktion atan2 wird definiert: 0 für x = 0, y = 0 π/2 für x = 0, y > 0 3 π/2 für x = 0, y < 0 θ = atan2(y, x) = atan(y, x) für +x and +y 2π atan(y, x) für +x und -y π atan(y, x) für -x und +y π + atan(y, x) f r -x und -y Die Lösungen: θ 2 = atan2(s 2, C 2 ) J. Zhang 106

5 - Beispiel 1: ein planarer dreigelenkiger Manipulator Die algebraische Lösung des Beispiel 1 - II wobei S 2 = ± 1 C2 2, und C 2 = x2 +y 2 l1 2 l2 2 2l 1 l 2. θ 1 = atan2(y, x) atan2(k 2, k 1 ) wobei k 1 = l 1 + l 2 C 2 und k 2 = l 2 S 2. θ 3 kann aus der folgenden Gleichung berechnet werden: θ 1 + θ 2 + θ 3 = atan2(s φ, C φ ) = φ - Beispiel 1: ein planarer dreigelenkiger Manipulator Die geometrische Lösung des Beispiel 1 - I Berechne θ 2 über Gesetz des Cosinus : Die Lösung: wobei gilt: Für θ 1, θ 2, θ 3 gilt: x 2 + y 2 = l l 2 2 2l 1 l 2 cos(180 + θ 2 ) θ 2 = ±cos 1 x 2 + y 2 l 2 1 l 2 2 2l 1 l 2 θ 1 = β ± ψ β = atan2(y, x), cos ψ = x 2 + y 2 l 2 1 l 2 2 2l 1 x 2 + y 2 θ 1 + θ 2 + θ 3 = φ J. Zhang 107 J. Zhang Beispiel 1: ein planarer dreigelenkiger Manipulator Algebraische Lösung mit Hilfe v. Konvertierung in Polynomen - Beispiel 1: ein planarer dreigelenkiger Manipulator Algebraische Lösung mit Hilfe v. Konvertierung in Polynomen Um transzendentale Gleichungen in polynomische Gleichungen zu konvertieren, können die folgenden Substitutionen verwendet werden: u = tan θ 2 cos θ = 1 u2 1 + u 2 sin θ = 2u 1 + u 2 Beispiel: Wir haben eine transzendentale Gleichung: Nach der Konvertierung: Die Lösung von u: Dann: a cos θ + b sin θ = c a(1 u 2 ) + 2bu = c(1 + u 2 ) u = b ± b 2 a 2 c 2 a + c θ = 2tan 1 ( b ± b 2 a 2 c 2 ) a + c J. Zhang 109 J. Zhang 110

6 - Algebraische Lösung des PUMA Algebraische Lösung des PUMA 560 Algebraische Lösung des PUMA I Algebraische Lösung des PUMA II Berechnung von θ 1, θ 2, θ 3 : Die ersten drei Gelenkwinkel θ 1, θ 2, θ 3 bestimmen die Position des Manipulator-Endpunktes (p x, p y, p z ) T (im Fall d 6 = 0). Daraus ergibt sich: p x = C 1 [S 23 d 4 + a 3 C 23 + a 2 C 2 ] S 1 d 2 (18) p y = S 1 [S 23 d 4 + a 3 C 23 + a 2 C 2 ] + C 1 d 2 (19) p z = C 23 d 4 a 3 S 23 a 2 S 2 (20) p y p θ 1 = tan 1 x 2 + py 2 d2 2 p xd 2 ( ) p x px 2 + py 2 d2 2 + p y d 2 wobei θ 3 = tan 1 ( A 3 A B2 3 D2 3 + B 3D 3 ) B 3 A B2 3 D2 3 + A 3D 3 A 3 = 2a 2 a 3 B 3 = 2a 2 d 4 D 3 = p 2 x + p 2 y + p 2 z a 2 2 a 2 3 d 2 2 d 2 4 J. Zhang 111 J. Zhang Algebraische Lösung des PUMA Die Lösung für RPY-Winkel Algebraische Lösung des PUMA II Die Lösung für RPY-Winkel und (RPY: Roll, Pitch, Yaw) wobei B 2 p θ 2 = tan 1 x 2 + py 2 d2 2 + A 2p z ( ) A 2 px 2 + py 2 d2 2 + B 2p z A 2 = d 4 C 3 a 3 S 3 B 2 = a 3 C 3 d 4 S 3 a 2 Wir lösen die folgende Gleichung: wobei T = R z,φ R y,θ R x,ψ R 1 z,φ T = R y,θr x,ψ f 11 (n) f 11 (o) f 11 (a) 0 C θ SθSψ SθC ψ 0 f 12 (n) f 12 (o) f 12 (a) 0 f 13 (n) f 13 (o) f 13 (a) 0 = 0 Cψ Sψ 0 Sθ CθSψ CθCψ J. Zhang 113 J. Zhang 114

7 - Die Lösung für RPY-Winkel - Die Lösung für RPY-Winkel Die Lösung für RPY-Winkel Die Lösung für RPY-Winkel und f 11 = Cφx + Sφy f 12 = Sψx + Cφy f 13 = z Die Gleichung mit f 12 (n) führt: : Cθ = Cφn x + Sφn y θ = atan2( n z, Cφn x + Sφa y ) Die Gleichungen mit dem Element 2,3 und 2,2 sind jeweils: Sφn x + Cφn y = 0 Sψ = Sφa x + Cφa y : φ = atan2(n y, n x ) und φ = φ Die Gleichungen mit dem Element 1,3 und 1,1 sind jeweils: : Cψ = Sφo x + Cφo y ψ = atan2(sφa x Cφa y, Sφo x + Cφo y ) Sθ = n z J. Zhang 115 J. Zhang Geometrische Lösung des PUMA Geometrische Lösung des PUMA 560 Geometrische Lösung des PUMA I Geometrische Lösung des PUMA I Definition von verschiedenen Arm-Konfigurationen: Für Schulter: Für Ellbogen: Für Handgelenk: RIGHT-arm, LEFT-arm ABOVE-arm, BELOW-arm WRIST-DOWN, WRIST-UP Darauf basierend werden die folgenden Anzeige-Variablen definiert: { +1 RIGHT-arm ARM = 1 LEFT-arm { +1 ABOVE-arm ELBOW = 1 BELOW-arm { +1 WRIST-DOWN WRIST = 1 WRIST-UP Die gesamte Lösung der inversen Kinematik wird über Analyse solcher Arm-Konfiguration erzielt. J. Zhang 117 J. Zhang 118

8 - Geometrische Lösung des PUMA Geometrische Lösung des PUMA 560 Probleme bei der Entwicklung von Steuerungssoftware Bisher war die Entwicklung von Steuerungssoftware an einem bestimmten Robotertyp gekoppelt. Jede Maschine hatte damit ihre eigene spezielle Software, die genau die Fähigkeit dieser Baureihe ausnutzte. Erweiterbarkeit und Portierbarkeit war damit eine langwierige und schwierige Aufgabe. Probleme bei der Entwicklung von Steuerungssoftware Es steht der Bedarf, eine allgemeine Steuerungssoftware zu entwickeln, die folgende Ziele erfüllen soll: Steuerungsmöglichkeiten für Low-Level Eigenschaften der Hardware; größtmögliche Portierbarkeit auf die verschiedenen Plattformen; eine möglichst große Mächtigkeit zur flexiblen und schnellen Erstellung von Anwendungen in derselben Sprache, in der die Steuerungssoftware geschrieben wurde. J. Zhang 119 J. Zhang Eine Programmierumgebung für Roboter unter UNIX: RCCL - Eine Programmierumgebung für Roboter unter UNIX: RCCL Eine Programmierumgebung für Roboter unter UNIX: RCCL Die Systemarchitektur RCCL: Robot Control C Library J. Zhang 121 J. Zhang 122

9 - Eine Programmierumgebung für Roboter unter UNIX: RCCL - Eine Programmierumgebung für Roboter unter UNIX: RCCL Steuerung von mehreren Robotern Beschreibung von Bewegungen mit Hilfe der Positionsgleichungen J. Zhang 123 J. Zhang Eine Programmierumgebung für Roboter unter UNIX: RCCL - Eine Programmierumgebung für Roboter unter UNIX: RCCL Beschreibung von Bewegungen mit Hilfe der Positionsgleichungen Ein Beispiel - Programm # include <rccl.h> # include " manex.560. h" main () { TRSF_PTR p, t; /* #1 */ POS_PTR pos ; /* #2 */ MANIP * mnp ; /* #3 */ JNTS rcclpark ; /* #4 */ char * robotname ; /* #5 */ rcclsetoptions ( RCCL_ERROR_EXIT ); /* #6 */ robotname = getdefaultrobot (); /* #7 */ if (! getrobotposition ( rcclpark.v, " rcclpark ", robotname )) { printf ( position rcclpark not defined for robot \n ); exit ( -1); } /* #8 */ t = alloctransxyz ("T", UNDEF, , 0.0, 75.0); p = alloctransrot ("P", UNDEF, P_X, P_Y, P_Z, xunit, 180.0); pos = makeposition (" pos ", T6, EQ, p, t, NULL ); /* #9 */ J. Zhang 125 J. Zhang 126

10 - Eine Programmierumgebung für Roboter unter UNIX: RCCL - Eine Programmierumgebung für Roboter unter UNIX: RCCL Ein Beispiel - Programm Ein Beispiel - Roboterbewegungen mnp = rcclcreate ( robotname, 0); /* #10 */ rcclstart (); movej (mnp, & rcclpark ); /* #11 */ setmod (mnp, c ); /* #12 */ move (mnp, pos ); /* #13 */ stop (mnp, ); movej (mnp, & rcclpark ); /* #14 */ stop (mnp, ); } waitforcompleted ( mnp ); /* #15 */ rcclrelease ( YES ); /* #16 */ J. Zhang 127 J. Zhang 128