Thesis - RWTH Aachen University

190 C.1 Theoretische Grundlagen der Manipulatorkinematik
Abbildung C.8: Unterschiedliche Gelenkstellungen mit derselben kartesischen Lage des Greifers.
Sei die gewünschte kartesische Position und Orientierung des Greifers mit der homogenen Matrix 0 6T ausgedrückt:
⎛
⎞
⎜
0
6T = ⎜
⎝
r11 r12 r13 px
r21 r22 r23 py
r31 r32 r33 pz
0 0 0 1
⎟
⎠
(C.19)
Seien zusätzlich die Denavit-Hartenberg (DH) Parameter in der Tabelle C.1 angegeben und cos(θi) =
ci und sin(θi) = si . Dann ist die algebraische Lösung für das Problem der inversen Kinematik:
�
θ1 = atan2(py, px) − atan2(d3, ± p2 x + p2 y − d2 3) (C.20)
mit
θ2 = θ23 − θ3
�
(C.21)
θ3 = atan2(a3, d4) − atan2(K, ± a2 3 + d2 4 − K2 ) (C.22)
K = p2 x + p2 y + p2 z − a2 2 − a2 3 − d2 3 − d2 4
2a2
θ23 = atan2((−a3 − a2c3)pz −(c1px + s1py)(d4 − a2s3), (a2s3 − d4)pz−(a3 + a2c3)(c1px + s1py))
Falls s5 �= 0 ist:
θ4 = atan2(−r13s1 + r23c1, −r13c1c23 − r23s1c23 + r33s23) (C.23)