Kinematik des Puma 200

Kinematik des Puma 200 1

2 Inhaltsverzeichnis Inhaltsverzeichnis 1 Einführung 3 2 Denavit-Hartenberg-Konfiguration 5 3 Mehrdeutigkeiten 7 4 Direkte Kinematik 10 5 Inverse Kinematik 13 6 Orientierung des Greifers 16 Literatur 18 Inde9

1 Einführung 3 1 Einführung Der Puma 200 ist ein Roboterarm mit sechs Gelenken, von denen die ersten drei dazu dienen, das Werkzeug (in unserem Falle ein Greifer ) an eine vorgeschriebene Position im Raum zu bewegen, und die letzten drei dazu, die Orientierung des Werkzeugs festzulegen. Die Gelenkachsen bezeichnen wir mit L 1,..., L 6. L 4 L 3 Ellbogen Unterarm L 2 L 1 L 5 L 6 Im Folgenden werden kleine, fette Buchstaben verwendet, um Vektoren und Punkte x Ê 3 zu beschreiben. Mit großen Buchstaben werden Matrizen für affine Transformationen der Form y = Ax + b, A SO 3 (Ê), beschrieben. Beim Rechnen ist es in der Regel günstiger, diese Transformationen in erweiterten Koordinaten darzustellen, die wir durch hohle Buchstaben kennzeichnen. Die Gleichung y = Ax + b schreibt sich so als Ý = Ü mit Ý = [ [ y x, Ü = 1] 1] und = [ ] A b 0. 1

4 1 Einführung Für die Kinematik benötigen wir in jedem Gelenk ein lokales Koordinatensystem der Form KS = (o; x, y, z), wobei o der Ursprung des Koordinatensystems ist und die Vektoren x, y und z die normierten, paarweise orthogonalen Richtungsvektoren entlang der enstprechenden Koordinatenachsen sind. Wir setzen dabei voraus, dass alle vorkommenden Koordinatensysteme rechtshändig sind, d.h. det(x, y, z) > 0. Das Vektorprodukt zweier Vektoren x und y ist durch y 1 x 2 y 3 x 3 y 2 x 2 x 3 := y 2 y 3 x 3 y 1 y 3 y 2 x 2 y 1 definiert. Es gilt stets x y = y x und (x y) span(x, y). Wir werden anstelle der Funktion arctan im Folgenden die Funktion atan 1) verwenden: arctan ( ) y x > 0, y Ê x π arctan ( ) y x < 0, y > 0 atan(y, x) = x arctan ( y x) π x < 0, y 0 sign(y) π x = 0, y 0 2 Die Funktion atan hat gegenüber arctan den Vorteil, dass sie den Wert y in das x Intervall [ π, π) und nicht in ( π, π ) abbildet. So wird jedem Paar (y, x) (0, 0) 2 2 eindeutig ein Winkel zugeordnet. 1) Diese Funktion ist in der Programmiersprache C durch atan2 realisiert.

2 Denavit-Hartenberg-Konfiguration 5 2 Denavit-Hartenberg-Konfiguration Die Denavit-Hartenberg-Konfiguration legt fest, in welcher Weise die lokalen Koordinatensysteme der einzelnen Gelenke eines Roboterarmes ineinander transformiert werden bzw. wie diese Koordinatensysteme zu wählen sind. Es bezeichne L i die Drehachse durch das i-te Gelenk und KS i = (o i ; x i, y i, z i ) das durch die Konfiguration festgelegte Koordinatensystem im i-ten Gelenk. Die Lage von KS 1 kann beliebig gewählt werden, solange z 1 auf der Achse L 1 liegt. Die Konfiguration wird wie folgt bestimmt: 1. Windschiefe Achsen: Bestimme ausgehend von KS i den Lotvektor a i von L i auf L i+1. Der Lotfußpunkt von a i auf L i+1 ist der Ursprung o i+1 des neuen Koordinatensystems KS i+1. Die Richtung a i ist die neue x i+1 -Richtung. z i+1 zeigt entlang der Achse L i+1, wobei das Vorzeichen willkürlich gewählt werden kann. Es wird y i+1 = z i x i festgelegt. Damit kann man KS i mit folgender Bewegung in KS i+1 überführen: Drehung um den Winkel ϑ i um die z i -Achse, so dass x i parallel zu x i+1 liegt. Verschiebung um den Vektor d i entlang der z i -Achse, so dass o i in den Lotfußpunkt des Lotes a i geschoben wird. Verschiebung um a i, um den Koordinatenursprung auf o i+1 zu bewegen. Drehung um α i um die x i+1 -Achse, um die z i -Achse auf die z i+1 Achse zu kippen. 2. Sich schneidende Achsen: Es ist a i = 0, so dass die x i+1 -Richtung nicht wie bei windschiefen Geraden bestimmt werden kann. Die übrigen Größen können genauso wie bei windschiefen Geraden bestimmt werden. Setze x i+1 = z i z i+1 z i z i+1. 3. Parallele Achsen: Die Fußpunkte des Lotes a i sind nicht eindeutig bestimmt, alle anderen Größen lassen sich wie bei windschiefen Geraden bestimmen. Bestimme die Fußpunkte des Lotes so, dass die oben genannte Verschiebung d i entlang der z i -Achse zu 0 wird.

6 2 Denavit-Hartenberg-Konfiguration Dies führt zu folgender erweiterter Bewegungsmatrix von KS i zu KS i+1 : cos(ϑ i ) sin(ϑ i ) cos(α i ) sin(ϑ i ) sin(α i ) a i cos(ϑ i ) i = sin(ϑ i ) cos(ϑ i ) cos(α i ) cos(ϑ i ) sin(α i ) a i sin(ϑ i ) 0 sin(α i ) cos(α i ) d i 0 0 0 1 mit a i = a i und d i = d i. Diese Matrix kann auch als Koordinatentransformation von KS i+1 nach KS i aufgefasst werden. Für den Puma 200 erhalten wir somit folgende Denavit-Hartenberg-Konfiguration: Gelenk a i α i d i 1 0 π/2 0 2 203,3 mm 0 0 3 0 π/2 127 mm 4 0 π/2 203,2 mm 5 0 π/2 0 Dabei wurde KS 1 in den Schnittpunkt der Achsen L 1 und L 2 gelegt, die -Achse zeigt in Richtung des Armes in Ruhestellung. Die Orientierungen der z i -Achsen ergeben sich aus der Zeichnung. Die Winkel ϑ i hängen von der aktuellen Stellung des Armes ab, wobei zu beachten ist, dass ϑ i immer relativ zum Koordinatensystem KS i 1 aufzufassen ist. Die Maße sind der Konstruktionszeichnung des Armes zu entnehmen. L 3 z 1 a 2 x 3 x 4 z 3 o 3 o 4 d 3 L 2 o 1,2 x z 2 2 z 4 d 4 x 6 x 5 z 6 o 5,6 L 6 L 1 L5 z 5 L 4

3 Mehrdeutigkeiten 7 3 Mehrdeutigkeiten Sind die Winkel ϑ 1,..., ϑ 6 gegeben, so ist die Stellung des Roboterarmes, also die Transformationsmatrix = 1 2 3 4 5 6 mit i wie in Abschnitt 2, eindeutig bestimmt. Ist umgekehrt die Matrix gegeben, so gibt es in der Regel mehrere Möglichkeiten für die Winkel ϑ i, die zu dieser Transformation führen. Die folgende Abbildung verdeutlicht dies. KS i Arm hoch KS i 1 KS i+1 KS i Arm tief Diese Mehrdeutigkeiten müssen für die inverse Kinematik (Abschnitt 5) aufgelöst werden, da abhängig von der gewählten Einstellung unterschiedliche Berechnungen durchgeführt werden müssen. Beim Puma 200 treten an drei Stellen Mehrdeutigkeiten auf: 1. Der Arm ist links bzw. rechts, wenn der Greifer links bzw. rechts von der z 2 -Achse liegt. 2) y 1 y 1 p x o 6 ϑ 1 p y q x ϑ 1 q y o 1 p y o 1 q y rechts von z z p x o 6 z 2 q x links von z z z 2 Projektion des Armes auf die y 1 -Ebene 2) Diese Festlegung ist genau entgegengesetzt dazu, den Arm als linken Arm bzw. rechten Arm aufzufassen, wie es etwa bei Paul und Zhang [1] getan wird.

8 3 Mehrdeutigkeiten Bezeichnen wir mit p den Verschiebungsvektor von KS 1 zu KS 6, so erhalten wir p aus der letzten Spalte von = (b ij ): p x b 14 p y = b 24 p z b 34 Um zu entscheiden, ob der Arm links oder rechts von z 2 liegt, betrachten wir die Projektion in die y 1 -Ebene. Wir drehen die Projektion des Armes um den Winkel ϑ 1 um die z 1 -Achse zurück, so dass der zurückgedrehte Vektor z 2 auf der negativen y 1 -Achse liegt: [ ] [ ] [ ] qx cos(ϑ1 ) sin(ϑ := 1 ) px sin(ϑ 1 ) cos(ϑ 1 ) q y Dann kann man die Stellung sofort anhand des Vorzeichens von q x ablesen: sign(q x ) = +1 Arm links, sign(q x ) = 1 Arm rechts. Im Falle q x = 0 wird die Stellung per Definition als links bezeichnet. 2. Der Ellbogen steht hoch bzw. tief, d.h. der Koordinatenursprung o 3 befindet sich oberhalb bzw. unterhalb der Verbindungslinie von o 1 zu o 6. Um dies zu entscheiden, drehen wir den im Schritt zuvor bereits um ϑ 1 zurückgedrehten Arm nun um ϑ 2 um die y 1 -Achse und betrachten die Projektion auf die z 1 -Ebene: [vx ] [ ] [ ] cos(ϑ2 ) sin(ϑ := 2 ) qx sin(ϑ 2 ) cos(ϑ 2 ) v z Wir können dann die Stellung des Ellbogens am Vorzeichen von v z ablesen, müssen dabei jedoch die Fälle Arm links und Arm rechts unterscheiden. (a) Der Arm ist links. p y p z q x p z q x z 1 z 1 p z ϑ 2 v z v x ϑ 2 o 1 Ellbogen tief, Arm links v x Ellbogen hoch, Arm links v z Projektion des Armes auf die z 1 -Ebene

3 Mehrdeutigkeiten 9 In diesem Fall ist sign(v z ) = +1 Ellbogen tief, sign(v z ) = 1 Ellbogen hoch. (b) Der Arm ist rechts. z 1 z 1 p z q x o 1 q x v z ϑ 2 ϑ 2 v x Ellbogen hoch, Arm rechts p z v z Ellbogen tief, Arm rechts Projektion des Armes auf die z 1 -Ebene v x In diesem Fall ist sign(v z ) = +1 Ellbogen hoch, sign(v z ) = 1 Ellbogen tief. In beiden Fällen wird der Ellbogen für v z = 0 per Definition als hoch bezeichnet. 3. Der Unterarm ist verdreht bzw. nicht verdreht, d.h. es gibt zwei Möglichkeiten, abhängig von der Drehung um die Gelenkachse L 4 um den Winkel ϑ 4 eine vorgegebene Orientierung des Greifers, also von KS 6, zu realisieren. z 5 z 5 x 5 x 5 x 5 x 6 x 6 ϑ 5 ϑ 5 x 6 ϑ 4 ϑ 4 π z 5 ϑ 6 ϑ 6 π z5 z 6 z 6 z6 KS 6 y y 5 5 KS 6 z 6 x 6 x 5 ϑ 4,ϑ 5,ϑ 6 gegeben ϑ 4, ϑ 5,ϑ 6 ϑ 4 π, ϑ 5,ϑ 6 ϑ 4 π, ϑ 5,ϑ 6 π verdreht nicht verdreht y 5 y 5 Anfangs- und Endzustand in der Zeichnung realisieren die selbe Orientierung. Das Vorzeichen von sin(ϑ 5 ) legt die Verdrehung fest: sign(sin(ϑ 5 )) = +1 Unterarm nicht verdreht, sign(sin(ϑ 5 )) = 1 Unterarm verdreht.

10 4 Direkte Kinematik 4 Direkte Kinematik Die Aufgabe der direkten Kinematik ist es, aus vorgegebenen Winkeln ϑ 1,..., ϑ 6 die Transformationsmatrix von KS 6 nach KS 1 zu bestimmen. Mit den i aus Abschnitt 2 ist = 1 2 3 4 5 6. Desweiteren kann in der direkten Kinematik auch ermittelt werden, welche Konfiguration der Roboterarm bgzl. der in Abschnitt 3 vorgestellten Mehrdeutigkeiten einnimmt. Um effizient zu berechnen, verwenden wir das Verfahren aus dem Artikel von Paul und Zhang [1], das an den Puma 200 und die Denavit-Hartenberg-Konfiguration aus Abschnitt 2 angepasst wurde. Wir definieren dafür zunächst folgende Matrizen: 23 = 2 3 Å 6 = 6 Å 5 = 5 Å 6 = 5 6 Å 4 = 4 Å 5 = 4 5 6 Å 3 = 3 Å 4 = 3 4 5 6 Å 2 = 23 Å 4 = 23 4 5 6 Å 1 = 1 Å 2 = 1 23 4 5 6 = Die Idee ist nun, angefangen bei Å 6, nacheinander die Matrizen Å k zu bestimmen, indem sie mit den jeweils bekannten rechten Seiten dieser Gleichungen gleichgesetzt werden und damit die einzelnen Einträge der Å k zu bestimmen, ohne jedesmal eine komplette Matrizenmultiplikation durchzuführen. Es bezeichne m (k) ij den Eintrag in der i-ten Zeile und j-ten Spalte der Matrix Å k. Der Lesbarkeit wegen schreiben wir c i = cos(ϑ i ), s i = sin(ϑ i ), c 23 = cos(ϑ 2 + ϑ 3 ), s 23 = sin(ϑ 2 + ϑ 3 ). Wir erhalten sofort c 6 s 6 0 0 c 5 c 6 c 5 s 6 s 5 0 Å 6 = s 6 c 6 0 0 0 0 1 0 und Å 5 = s 5 c 6 s 5 s 6 c 5 0 s 6 c 6 0 0. 0 0 0 1 0 0 0 1 Somit haben wir m (5) 11 := c 5c 6 m (5) 12 := c 5s 6 m (5) 21 := s 5c 6 m (5) 22 := s 5 s 6

4 Direkte Kinematik 11 und können Å 4 schreiben als Wir setzen nun Damit ist Å 2 = und wir setzen Schließlich ist Å 4 = 11 c 4 + s 4 s 6 c 4 m (5) 12 + s 4 c 6 c 4 s 5 0 11 s 4 c 4 s 6 s 4 m (5) 12 c 4 c 6 s 4 s 5 0 m (5) 21 m (5) 22 c 5 0. 0 0 0 1 m (5) m (5) m (4) 11 := m (5) 11 c 4 + s 4 s 6 m (4) 12 := c 4 m (5) 12 + s 4 c 6 m (4) 13 := c 4 s 5 m (4) 21 := m (5) 11 s 4 c 4 s 6 m (4) 22 := s 4 m (5) 12 c 4 c 6 m (4) 23 := s 4 s 5. c 23 m (4) 11 + s 23 m (5) 21 c 23 m (4) 12 + s 23 m (5) 22 c 23 m (4) 13 c 5 s 23 c 2 a 2 + s 23 d 4 s 23 m (4) 11 c 23 m (5) 21 s 23 m (4) 12 c 23 m (5) 22 s 23 m (4) 13 + c 5 c 23 s 2 a 2 c 23 d 4 m (4) 21 m (4) 22 m (4) 23 0 0 0 0 1 m (2) 11 := c 23m (4) 11 + s 23m (5) 21 m (2) 12 := c 23m (4) 12 + s 23m (5) 22 m (2) 13 := c 23 m (4) 13 c 5 s 23 m (2) 14 := c 2 a 2 + s 23 d 4 m (2) 21 := s 23 m (4) 11 c 23 m (5) 21 m (2) 22 := s 23 m (4) 12 c 23 m (5) 22 m (2) 23 := s 23 m (4) 13 + c 5 c 23 m (2) 24 := s 2 a 2 c 23 d 4. c 1 m (2) 11 + s 1 m (4) 21 c 1 m (2) 12 + s 1 m (4) 22 c 1 m (2) 13 + s 1 m (4) 23 c 1 m (2) 14 + s 1 d 3 s = Å 1 = 1 m (2) 11 c 1 m (4) 21 s 1 m (2) 12 c 1 m (4) 22 s 1 m (2) 13 c 1 m (4) 23 s 1 m (2) 14 c 1 d 3 m (2) 21 m (2) 22 m (2) 23 m (2). 24 0 0 0 1

12 4 Direkte Kinematik Wir erhalten also die Komponenten von = (b ij ) durch b 12 := c 1 m (2) 12 + s 1 m (4) 22 b 13 := c 1 m (2) 13 + s 1m (4) 23 b 14 := c 1 m (2) 14 + s 1d 3 b 22 := s 1 m (2) 12 c 1m (4) 22 b 23 := s 1 m (2) 13 c 1m (4) 23 b 24 := s 1 m (2) 14 c 1d 3 b 32 := m (2) 22 b 33 := m (2) 23 b 34 := m (2) 24 und da die linke obere 3 3-Teilmatrix von orthogonal sein muss, können wir die erste Spalte von auch durch b 11 b 12 b 13 b 21 := b 22 b 23 und b 41 := 0 b 31 b 32 b 33 erhalten. Dies spart die Berechnung der Zwischenvariablen m (2) 11, m (2) 21, m (4) 11, m (4) 21, m (5) 11 und m (5) 21.

5 Inverse Kinematik 13 5 Inverse Kinematik Die inverse Kinematik soll aus der gegebenen Transformationsmatrix die Winkel ϑ 1,..., ϑ 6 rekonstruieren. Wegen der in Abschnitt 3 dargelegten Mehrdeutigkeiten können mehrere Lösungen für die Winkel ϑ i existieren. Daher muss vorgegeben werden, ob der Arm links oder rechts stehen soll, der Ellbogen hoch oder tief und ob der Unterarm verdreht sein soll oder nicht. Unser Vorgehen für die inverse Kinematik ist im Prinzip eine Umkehrung des Vorgehens bei der direkten Kinematik. Wir definieren die Matrizen Ì 0 = Å 1 = Ì 1 = Å 2 = 1 1 Ì 0 Ì 2 = Å 3 = 1 2 Ì 1 Ì 3 = Å 4 = 1 23 Ì 1 Ì 4 = Å 5 = 1 4 Ì 3 Ì 5 = Å 6 = 1 5 Ì 4 = 6 Es werden wieder nacheinander die von den ϑ i abhängigen linken Seiten aus den jeweils bekannten rechten Seiten ermittelt. Es bezeichne t (k) ij den Eintrag in der i-ten Zeile und j-ten Spalte der Matrix Ì k. Die Rechnungen unterscheiden sich nur in Details von den bei Paul und Zhang [1] erläuterten, so dass wir sie nicht ausführen. Die letzte Spalte von Ì 0 enthält die Verschiebung p von KS 1 nach KS 6. Durch Gleichsetzen mit Å 1 und Umformen erhalten wir { ( arcsin d3 ) ϑ 1 = atan(p y, p x ) + r falls Arm links π arcsin ( ) d 3 r falls Arm rechts mit r = p 2 x + p 2 y. Die Zeichnung erklärt die Fallunterscheidung, vgl. auch die erste Mehrdeutigkeit in Abschnitt 3. y 1 Arm rechts y 1 β 2 d 3 r o 1 ϑ 1 β2 β 1 d 3 r x1 o 1 ϑ1 β 1 Arm links Hier ist β 1 = atan(p y, p x ) und β 2 = arcsin( d 3 r ). Steht der Arm rechts, so ist ϑ 1 = π β 2 +β 1. Steht der Arm links, so ist ϑ 1 = β 1 +β 2. Beachte, dass stets β 2 0 gilt.

14 5 Inverse Kinematik Ist d 3 > r, so ist die vorgegebene Position p durch den Roboterarm nicht erreichbar, da sie näher an o 1 liegt, als der Greifer positioniert werden kann. Vergleichen wir nun Ì 1 mit 1 1 Å 1, so stellen wir fest, dass t (1) 14 = c 1 p x + s 1 p y gerade dem Wert q x aus Abschnitt 3 entspricht. Zusammen mit dem Wert p z = b 34 = t (0) 34 und dem Vorzeichen { +1 Arm links σ = 1 Arm rechts können wir nach ϑ 2 = atan(p z, t (1) 14 ) + σ ) r arccos( 2 +a 2 2 d2 4 2 ra 2 arccos( r 2 +a 2 2 d2 4 2 ra 2 ) Ellbogen hoch Ellbogen tief aufösen, wobei r = q 2 x + p 2 z ist. Die Zeichnung verdeutlicht dies für den Fall, dass der Arm links steht, vgl. die zweite Mehrdeutigkeit in Abschnitt 3. q x p z hoch z 1 r z 1 q x β 1 tief β 1 r p z β 2 ϑ 2 ϑ 2 β 2 o 1 o 1 Hier ist (nach dem Cosinussatz) β 1 = arccos( r2 +a 2 2 d2 4 2 ra 2 ) und β 2 = atan(p z, q x ). Steht der Arm hoch, so ist ϑ 2 = β 2 +β 1 und steht der Arm tief, so ist ϑ 2 = β 2 β 1. Beachte, dass stets β 1 0 gilt. Entsprechende Überlegungen führen auf die Formel für den Fall, dass der Arm rechts steht, und rechtfertigen auch die Wahl des Vorzeichens σ. In der letzte Spalte von Ì 2 = Å 3 = 1 2 Ì 1 stehen die Einträge t (2) 14 = s 3d 4 = c 2 t (1) 14 + s 2p z a 2, t (2) 24 = c 3 d 4 = s 2 t (1) 14 + c 2 p z, was wir nach ϑ 3 = atan ( ) c 2 t (1) 14 + s 2 p z a 2 s 2 t (1) 14 c 2 p z

5 Inverse Kinematik 15 auflösen. Mit ϑ 2 und ϑ 3 sind nun auch s 23 und c 23 bekannt, und mit t (1) 13 = c 1 b 13 + s 1 b 23 bestimmen wir die Einträge t (3) 13 Es ist nun tan(ϑ 5 ) = s 1 5 t(3) 23 s 1 5 t(3) 13 von s 5 verlorenginge, setzen wir { ϑ 4 = und t(3) 23 aus Ì 3 = Å 4 = 1 23 Ì 1 zu t (3) 23 = c 4 s 5 = s 23 b 33 + c 23 t (1) 13, t (3) 13 = s 4 s 5 = s 1 b 13 c 1 b 23.. Da beim Kürzen von s 5 Information über das Vorzeichen atan(t (3) 23, t (3) atan( t (3) 23, t (3) 13 ) falls Unterarm nicht verdreht 13 ) falls Unterarm verdreht und lösen somit die dritte Mehrdeutigkeit aus Abschnitt 3 auf. Diese Gleichung ist singulär für s 5 = 0. Dies ist jedoch genau dann der Fall, wenn die Drehachsen L 4 und L 6 zusammenfallen, so dass wir dann ohne weiteres ϑ 4 = 0 annehmen können, da jede Drehung um L 4 ebensogut durch eine zusätzliche Drehung um L 6 realisiert werden kann. Aus dem Vergleich Ì 4 = 1 4 Ì 3 erhalten wir s 5 = c 4 t (3) 13 + s 4 t (3) 23 c 5 = c 23 b 33 s 23 t (1) 13 und somit ϑ 5 = atan(c 4 t (3) 13 + s 4t (3) 23, c 23b 33 s 23 t (1) 13 ). Schließlich ist Ì 5 = 1 5 Ì 4 und wir erhalten mit und t (4) 12 = c 4(s 23 b 32 + c 23 (c 1 b 12 + s 1 b 22 )) s 4 (c 1 b 22 s 1 b 12 ) t (4) 22 = s 23(c 1 b 12 + s 1 b 22 ) c 23 b 32 t (4) 32 = c 4(c 1 b 22 s 1 b 12 ) + s 4 (s 23 b 32 + c 23 (c 1 b 12 + s 1 b 22 )) s 6 = c 5 t (4) 12 s 5t (4) 22 c 6 = t (4) 32 den Winkel ϑ 6 = atan( c 5 t (4) 12 s 5 t (4) 22, t (4) 32 ).

16 6 Orientierung des Greifers 6 Orientierung des Greifers Will man die Lage des Greifers im Raum angeben, so mag es recht umständlich scheinen, dies durch explizite Angabe der Matrix zu tun. Es ist sicherlich einfacher, stattdessen den Verschiebungsvektor p von o 1 nach o 6 und die Orientierung des Greifers durch seine drei Winkel gegenüber der z 1 -, y 1 - und -Achse (in dieser Reihenfolge) anzugeben. hat die Form = [ ] B p 0, 1 wobei die Drehmatrix B SO 3 (Ê) die Orientierung des Greifers beschreibt und p die Verschiebung ist. B lässt sich als Produkt B = R z (ζ)r y (η)r x (ξ) von drei Rotationen schreiben. Diese Darstellung entspricht ausgehend von KS 1 einer Rotation um die z 1 -Achse um den Winkel ζ gefolgt von einer Rotation um die y 1 -Achse um den Winkel η und schließlich einer Rotation um die -Achse um den Winkel ξ. Die Matrizen müssen in dieser Reihenfolge stehen, denn die Drehung durch R y (η) im bereits durch R z (ζ) gedrehten Koordinatensystem schreibt sich im ursprünglichen KS 1 als R z (ζ)r y (η)r z (ζ) 1. Die Verknüpfung der beiden Drehungen ergibt also in der Darstellung bzgl. des ursprünglichen KS 1 (R z (ζ)r y (η)r z (ζ) 1 )R z (ζ) = R z (ζ)r y (η). Entsprechendes kann man sich für die Darstellung der Drehung R x (ξ) nach vorangegangenen Drehungen um ζ und ξ überlegen. Die Matrizen haben die Gestalt cos(ζ) sin(ζ) 0 cos(η) 0 sin(η) R z (ζ) = sin(ζ) cos(ζ) 0,R y (η) = 0 1 0, 0 0 1 sin(η) 0 cos(η) 1 0 0 R x (ξ) = 0 cos(ξ) sin(ξ) 0 sin(ξ) cos(ξ) und somit ist B = cos(η) cos(ζ) cos(ξ) sin(ζ) cos(ζ) sin(ξ) sin(η) sin(ξ) sin(ζ) cos(ξ) cos(ζ) sin(η) cos(η) sin(ζ) cos(ξ) cos(ζ) sin(ξ) sin(η) sin(ζ) cos(ζ) sin(ξ) cos(ξ) sin(η) sin(ζ). sin(η) cos(η) cos(ξ) cos(η) sin(ξ)

6 Orientierung des Greifers 17 Sind für die inverse Kinematik anstelle der Matrix der Vektor p und die Winkel ζ, η und ξ gegeben, so lässt sich B (und damit auch ) durch diese Darstellung bestimmen. Soll die direkte Kinematik nicht die Matrix, sondern den Vektor p und die Winkel ζ, η und ξ ausgeben, so kann man zuerst die Matrix (und somit auch B) bestimmen, und dann folgende Gleichungen nutzen: b 14 p = b 24, b 34 ζ = atan(sin(ζ), cos(ζ)) = atan(cos(η) sin(ζ), cos(η) cos(ζ)) = atan(b 12, b 11 ), ξ = atan(sin(ξ), cos(ξ)) = atan(cos(η) sin(ξ), cos(η) cos(ξ)) = atan(b 32, b 32 ), und mit b 11 cos(ζ) + b 21 sin(ζ) = cos(η) cos(ζ) 2 + cos(η) sin(ζ) 2 = cos(η) η = atan(sin(η), cos(η)) = atan(b 13, b 11 cos(ζ) + b 21 sin(ζ)).

18 Literatur Literatur [1] Paul, Zhang: Computationally Efficient Kinematics for Manipulators with Spherical Wrists Based on the Homogeneous Transformation Representation, The International Journal of Robotics Research Vol. 5 No. 2, 1986.

Index Armstellung hoch, 8 links, 7 rechts, 7 tief, 8 verdreht, 9 atan, 4 Denavit-Hartenberg-Konfiguration, 5 direkte Kinematik, 10, 17 Ellbogen, 8 erweiterte Koordinaten, 3 Greifer, 3, 16 hoch, 8, 14 inverse Kinematik, 13, 17 Kinematik direkte, 10, 17 inverse, 13, 17 Koordinatensystem, 4 links, 7, 13 lokales Koordinatensystem, 4 Mehrdeutigkeiten, 7, 13 15 rechts, 7, 13 tief, 8, 14 Vektorprodukt, 4 verdreht, 9, 15 19