Thesis - RWTH Aachen University

4.3 Pfadplanung im lokalen Manipulationsraum 103
entfernt. Alle restliche Punkte, die näher zu Mi liegen als kD2, verbindet man mit Mi über eine Kante.
Danach wird einer der verbundenen Punkte ausgewählt und der Vorgang wird wiederholt, bis alle
Punkte bearbeitet sind. Die Punkte, die nicht entfernt wurden, bilden dann die Knoten, ihre Verbindungen
miteinander die Kanten des Graphen.
Anschließend findet die Wegesuche im Graphen statt. Der berechnete Weg muss möglichst kurz sein;
gleichzeitig muss der Greifer genügend Abstand zu Hindernissen bewahren, damit die Ausweichmöglichkeit
des Roboters in der Nähe von Hindernissen nicht eingeschränkt wird. Der Roboterarm sollte
enge Passagen möglichst vermeiden solange zwar längere dafür aber sichere Wege vorhanden sind.
Deswegen werden die Kanten des Graphen gewichtet. Die Gewichtung einer Kante gij hängt sowohl
von der geometrischen Entfernung der zugehörigen Punkte Mi und Mj zueinander, als auch von ihrer
Distanz zum nächsten Hindernis ab.
�
gij =
((xi − xj) 2 + (yi − yj) 2 + (zi − zj) 2 ) wi + wj
2
= | MB �pi − MB �pj |
�
( wi + wj
) (4.34)
2
wobei W �pl = (xl, yl, zl) T die Position von Ml im Suchraum ist und wl ein Maß der Hindernisentfernung:
wl = max(1, kDF (1 − dOMl )) (4.35)
Dabei ist kDF eine Konstante und dOMl die Distanz von Punkt Ml von der nächsten Hindernisumrandung,
die nach dem Verfahren in Abschnitt 4.2.3 berechnet wird. Durch Gleichung 4.34 werden
Kanten in der Nähe von Hindernissen schlechter bewertet, um geometrisch längere Wege in größerer
Entfernung von Hindernissen zu bevorzugen.
Der kürzeste Weg im Graph wird nach dem Algorithmus von Dijkstra [Dij59] ermittelt. Der Graph
wird dazu in einen Baum umgewandelt, in dem jeder Knoten über den kürzesten Weg mit dem Ziel
verbunden ist. Daher kann auf Neuberechnung des Graphen verzichtet werden, wenn ein Weg von
einer neuen Anfangsposition zur selben Zielposition berechnet werden soll.
Die Qualität des ermittelten Pfades ist von der Genauigkeit der Hindernislokalisierung mit der Bordkamera
abhängig. Ist der Lokalisierungsfehler gering, dann führt der ermittelte Pfad kollisionsfrei zur
Zielposition. Je ungenauer jedoch die Lokalisierung, desto wahrscheinlicher werden Kollisionen mit
Hindernissen. Da beim Pfadplanungsverhalten keine bildgestützte Regelung des Greifers beim Erreichen
der Zielposition stattfindet, verursacht eine fehlerbehaftete Objektlokalisierung einen Misserfolg
beim Greifen. Verlässt weiterhin der Manipulator den vom Planungsverhalten ermittelten Pfad,
dann können nur die reaktiven Hindernisvermeidungsverhalten Kollisionen mit Objekten verhindern.
Deshalb sind die reaktive Verhalten notwendig, um anhand der aktuellen Kameradaten den vom Pfadplanungsverhalten
vorgeschlagenen Pfad anzupassen bzw. zu korrigieren.
4.3.3 Überprüfung der Stützstellen des Pfades
Nachdem ein Pfad für den Greifer gefunden ist, muss man anhand der inversen Kinematik überprüfen,
ob die kartesischen Stützstellen des Pfades vom Manipulator erreichbar sind, d.h ob die Gelenkstellungen
es dem Greifer erlauben, die berechnete Position anzunehmen 27 . Die Stützstelle wird als
unerreichbar deklariert, falls die inverse Kinematik keine Lösung liefert, oder die Lösung zu Selbstkollisionen
der Manipulatorsegmente führt. Das Verhalten platziert ein virtuelles Hindernis an der
27 Gleichungen C.20-C.25 im Anhang C.1.