Thesis - RWTH Aachen University

C.1 Theoretische Grundlagen der Manipulatorkinematik 191
Mit s5 = 0 befindet sich der Manipulator in einer singulären Gelenkstellung, in der die Achsen 4 und
6 in einer Reihe aufgestellt sind und dieselbe Bewegung des letzten Segmentes des Roboterarmes verursachen.
Diese Situation kann detektiert werden, falls beide Argumente von Gleichung C.23 gegen
Null gehen. Dann kann θ4 frei gewählt werden und θ6 wird entsprechend bestimmt. Meistens behält
θ4 seinen gegenwärtigen Wert.
mit
s5 = −r13(c1c23c4 + s1s4) − r23(s1c23c4 − c1s4) + r33(s23c4)
c5 = r13(−c1s23) + r23(−s1s23) + r33(−c23)
s6 = −r11(c1c23s4 + s1c4) − r21(s1c23s4 − c1c4) + r31(s23s4)
θ5 = atan2(s5, c5) (C.24)
θ6 = atan2(s6, c6) (C.25)
c6 = r11((c1c23c4 + s1s4)c5 − c1s23s5) + r21((s1c23c4 − c1s4)c5 − s1s23s5) − r31(c23c4c5 + c23s5)
Wegen der Plus-Minus Vorzeichen in Gleichungen C.20 und C.22 entstehen vier mögliche Lösungen.
Zusätzlich existieren vier weitere Lösungen, da man das Handgelenk des Manipulators, das aus den
Segmenten 4, 5 und 6 besteht, um 180 0 drehen kann. Für jede der vier Lösungen aus Gleichungen
C.20 bis C.25 entsteht eine weitere alternative Lösung durch:
θ ′ 4 = θ4 + 180 0
θ ′ 5 = −θ5
θ ′ 6 = θ6 + 180 0
(C.26)
Daraus ergeben sich höchstens acht Lösungen, also acht mögliche Gelenkstellungen. Diese werden
überprüft, ob sie eine Kollision der Segmente miteinander produzieren und ob sie eventuell die Grenzwerte
für bestimmte Gelenke überschreiten. Aus den gültigen Gelenkstellungen � θj wird dann die günstigste
zur gegenwärtigen Gelenkposition des Roboterarmes � θ ′ ausgewählt. Bei der Auswahl werden
sowohl die Distanz der Lösungen zur aktuellen Gelenkstellung als auch die Gefahr vor Selbstkollisionen
der Manipulatorsegmente berücksichtigt. Weiterhin dürfen die Lösungen die Gelenke nicht
ausserhalb ihrer Drehungsgrenzen führen. Lösungen, die eine große Bewegung der ersten drei Gelenke
verursachen, erhalten eine schlechtere Bewertung, da diese Gelenke eine größere Massenverschiebung
des Manipulators produzieren und dadurch die Kollisionswahrscheinlichkeit mit Hindernissen
erhöhen.
Diese Kriterien werden in einer Kostenfunktion zusammengefasst. Sei �θj = (θ1j, ..., θ6j) T , j = 1...8,
eine der acht möglichen Lösungen, �θ ′ = (θ ′ 1, ..., θ ′ 6) T die aktuelle Gelenkstellung des Manipulators
und θUi die Obergrenze und θDi die Untergrenze des Drehungswinkels von Gelenk i. Dann ist die
Kostenfunktion:
fGK( �θj, �θ ′ 6�
) = gGKij | θij − θ ′ i | (C.27)
i=1