Thesis - RWTH Aachen University

194 C.1 Theoretische Grundlagen der Manipulatorkinematik
Pfadkorrektur
Die Pfadkorrektur bezieht sich auf die Anpassung der Stützstellen des Pfades, so dass die Roboterbewegung
während der Berechnung eines Verhaltens kompensiert wird. Dabei nimmt der neue
Anfangspunkt des Pfades die aktuelle Position des Roboters an und die beiden letzten Stützstellen
bleiben erhalten, um das ursprüngliche Ziel und die Bewegungsrichtung an der Zielposition nicht
zu verfälschen. Alle anderen Stützstellen müssen so geändert werden, dass sich der neue Pfad dem
ursprünglichen Pfad anpasst, je mehr er sich dem Ziel nähert.
Sei Psol = { � P1,sol, . . . , � Pk,sol} der gegebene Pfad, � P1,tat die aktuelle Anfangsposition des Manipulators
und k die Anzahl der Stützstellen des Pfades. Zur Korrektur des Pfades wird der Fehler in der
Anfangsposition mit der Anzahl der noch bevorstehenden Stützstellen k − l − 2 im Verhältnis zu
k − 2, also zu der Anzahl der Stützstellen bis der vorletzten Stützstelle, gewichtet und zur anzupassende
Stützstelle hinzuaddiert. Demnach wird die angepasste Stütztelle � Pl,kor wie folgt bestimmt:
�Pl,kor = � Pl,sol + ( � P1,tat − � k − l − 2
P1,sol)
k − 2
(C.37)
Dadurch wird die Abweichung vom gewünschten Pfad geringer, je mehr Stützstellen der Manipulator
durchläuft. Gleichung C.37 ergibt einen glatten Pfad, der die ursprüngliche Form der Bewegung
erhält.
Trajektorieregler
Aus dem Ausgabepfad eines Verhaltens muss eine entsprechende Trajektorie bestimmt werden, die
mit einem Trajektorieregler abgefahren wird. Da der Manipulator mit Gelenkgeschwindigkeiten angesteuert
wird, müssen zuerst die Stützstellen, falls sie im kartesischen Raum definiert sind, anhand
der inversen Kinematik in Gelenkstellungen umgewandelt werden (siehe Abschnitt C.1.2) 6 . Danach
wird für jedes Gelenk eine kontinuierliche Funktion der Winkelstellung in Abhängigkeit von der
Zeit definiert, die das Gelenk von der aktuellen Stellung in die Winkelstellung der nächsten Stützstelle
überführt. Dabei müssen alle Gelenke die Winkelstellungen der Stützstellen zu derselben Zeit
erreichen, um die angegebene Position im Raum anzunehmen. Der gesamte Programmablauf des
Trajektoriereglers ist in Abbildung C.9 dargestellt.
Als Funktionen für die Winkelstellungen werden in dieser Arbeit kubische Polynome verwendet
(Gleichung C.38), da sie stetig differenzierbar sind und eine kontinuierliche erste und zweite Ableitungen
haben. Dies erzeugt eine glatte Bewegung der Gelenke und hält dadurch die mechanische
Anspannung der Gelenkmotoren gering [Cra89]. Sei θi(t) der Winkelwert von Gelenk i zu Zeitpunkt
t, dann ist die Funktion für die Winkelstellungen:
θi(t) = a0i + a1it + a2it 2 + a3it 3
Aus der Gleichung C.38 ergibt sich ein parabolisches Geschwindigkeitsprofil:
und ein lineare Beschleunigung:
˙θi(t) = a1i + 2a2it + 3a3it 2
(C.38)
(C.39)
¨θi(t) = 2a2i + 6a3it (C.40)
6 Die Ausführung der Trajektorie wird im Gelenkraum geregelt, um somit Singularitäten der Roboter Jacobi Matrix
(Gleichung C.32, die bei einer Regelung im kartesischen Raum vorkommen können, zu umgehen.