Dieser Artikel beschreibt, wie im textuellen Programmieren die Abweichung einer von einem externen Sensor (wie z.B. einer Kamera) ermittelten Position zur tats�chlichen Position des Roboters abgemildert werden kann.
Dies trifft vor allem auf den Horst600 zu. Wenn nach einer Kalibrierung mit einer Kamera ein Punkt angefahren werden soll, liegt die Abweichung des Roboters von der zu erreichenden Position bei bis zu 1,5mm. Dies ist bei manchen Anwendungen zu viel. Diese Ungenauigkeiten existieren aus mechanischen Gr�nden, k�nnen durch die Software aber abgemildert werden.
Hierzu wurden zwei unterschiedliche Funktionsweisen implementiert.
1 CameraAlignment
Es gibt zwei Funktionen, die ben�tigt werden, um die Abweichung der Absolutgenauigkeit abzumildern. Dies sind teachCameraAlignment und getCorrectedDestination.
1.1 teachCameraAlignment
Zuerst muss die an einigen Positionen ermittelt werden, wie gro� die Abweichung sind. Hierzu m�ssen mehrere Punkte angegeben werden. Dabei m�ssen sowohl die Position mitgegeben werden, die der Roboter hat und die Position, die die Kamera ermittelt hat.
Es muss eine quadratische Anzahl an Punkten eingelernt werden. Dabei k�nnen zwischen 2x2 und 10x10 Punkte eingelernt werden. Je mehr Punkte eingelernt werden, desto besser ist die Abmilderung.
Die Punkte m�ssen jeweils S-F�rmig eingelernt werden und die sich bildendenden (inneren) Vierecke m�ssen m�glichst rechtwinklig sein.
Hier ein paar Beispiele, wie die Punkte eingelernt werden m�ssen:
Die Funktion teachCameraAlignment hat 7 bzw 4 �bergabeparameter. Zuerst wird die Variante mit 7 �bergabeparameter beschrieben.
teachCameraAlignment(index, horstX, horstY, horstZ, cameraX, cameraY, cameraZ)
1.1.2. teachCameraAlignment(index, cameraX, cameraY, cameraZ)
Diese Methode ist identisch zu der obigen. Es fehlen nur die Werte f�r horst. Diese werden automatisch von der aktuellen Roboterposition ermittelt.
1.2 getCorrectedDestination
Wenn man �ber teachCameraAlignment eine quadratische Anzahl Punkte eingelernt hat, kann man diese nun nutzen, um die Abweichung abzumildern.
Dazu gibt es die Funktion getCorrectedDestination. Hier werden als �bergabeparameter die von der Kamera ermitteltete Position eines Gegenstands erwartet.
Der R�ckgabewert besteht aus einem Objekt, welches eine Position in x, y und z enth�lt. Dies ist die Position, welche horst anfahren muss, um den von der Kamera detektierten Punkt anzufahren. Auf die einzelnen Koordinaten kann mit getX(), getY() und getZ() zugegriffen werden.
1.3. Beispielprogramm
//hier ein Beispiel wie die Funktionalit�t in einem 2x2 Gitter funktioniert
//die punkte muessen im oder gegen den uhrzeigersinn eingelernt werden, also z.b.
// 1 2 oder 1 4
// 4 3 2 3
//Falls man 3x3 Punkte einlernt, so muss S-F�rmig eingelernt werden,
//also z.b.
// 1 2 3 1 6 7
// 6 5 4 oder 2 5 8
// 7 8 9 3 4 9
//TODO zu Gegenstand in Ecke 1 fahren
moveJoint(0.25,0.0,0.225, 0,0, 1, 0);
var corner1Horst = getCurrentPose();
//TODO Position des Gegenstands mit kamera ermitteln; hier vergeb ich einfach 3 zufaellige werte
var corner1Camera = { x: 0.26, y: 0.0, z:0.225};
teachCameraAlignment(1, corner1Horst.x , corner1Horst.y, corner1Horst.z,
corner1Camera.x, corner1Camera.y, corner1Camera.z);
//TODO zu Gegenstand in Ecke 2 fahren
moveJoint(0.30,0.0,0.225, 0,0, 1, 0);
var corner2Horst = getCurrentPose();
//TODO Position des Gegenstands mit kamera ermitteln; hier vergeb ich einfach 3 zufaellige werte
var corner2Camera = { x: 0.31, y: 0.0, z:0.225};
teachCameraAlignment(2, corner2Horst.x , corner2Horst.y, corner2Horst.z,
corner2Camera.x, corner2Camera.y, corner2Camera.z);
//TODO zu Gegenstand in Ecke 3 fahren
moveJoint(0.30,0.1,0.225, 0,0, 1, 0);
var corner3Horst = getCurrentPose();
//TODO Position des Gegenstands mit kamera ermitteln; hier vergeb ich einfach 3 zufaellige werte
var corner3Camera = { x: 0.31, y: 0.11, z:0.225};
teachCameraAlignment(3, corner3Horst.x , corner3Horst.y, corner3Horst.z,
corner3Camera.x, corner3Camera.y, corner3Camera.z);
//TODO zu Gegenstand in Ecke 2 fahren
moveJoint(0.25,0.1,0.225, 0,0, 1, 0);
var corner4Horst = getCurrentPose();
//TODO Position des Gegenstands mit kamera ermitteln; hier vergeb ich einfach 3 zufaellige werte
var corner4Camera = { x: 0.26, y: 0.11, z:0.25};
teachCameraAlignment(4, corner4Horst.x , corner4Horst.y, corner4Horst.z,
corner4Camera.x, corner4Camera.y, corner4Camera.z);
//nun sind die vier eckpunkte eingelernt und unser algorithmus kann verwendet werden
//TODO kamera einen Gegenstand irgendwo erkennen lassen; hier zufaellige werte
var destination = { x: 0.27, y: 0.05, z:0.25, q0: 0, q1: 0, q2: 1, q3: 0};
//von der software genaueren punkt ausrechnen lassen
var correctedDestination = getCorrectedDestination(destination.x, destination.y, destination.z);
//zum gegenstand fahren und greifen
moveJoint(correctedDestination.getX(), correctedDestination.getY(), correctedDestination.getZ(),
destination.q0, destination.q1, destination.q2, destination.q3);
2. CalibratedQuader
Mithilfe der im Folgenden beschriebenen Funktionen ist es m�glich, den in der Einleitung beschriebenen Fehler in einem 3D-Raum, der hier als Quader definiert wird, zu minimieren. Zuerst wird der Quader definiert, dann kann die korrigierte Position innerhalb dieses Quaders ermittelt werden.
Der Quader kann auf zweierlei Art und Weise definiert werden. Entweder �ber Teachen der einzelnen Eckpunkte (teachCalibratedQuader) oder mit �bergabe einer Config-Map, die alle ben�tigten Informationen enth�lt (initializeCalibratedQuader).
2.1. teachCalibratedQuader
int teachCalibratedQuader(Map<String, Object> configMap, double x, double y, double z)
Bei Ausf�hren der Funktion erscheint erst eine entsprechende Informations-Meldung. Danach f�hrt der Roboter zuerst den Mittelpunkt des Quaders an. Anschlie�end f�hrt der Roboter die erste Ecke des Quaders an, stoppt dort und das Programm fragt per User-Eingabe den Wert der Sensor-Koordinaten an dieser Position ab. Nach Eingabe der Daten wird wieder der Mittelpunkt des Quaders angefahren. Der Vorgang wiederholt sich, bis alle Ecken des Quaders abgefahren sind.
Die Funktion gibt den Index des initialisierten Quaders zur�ck. Es ist m�glich, mehrere Quader zu definieren. Beim Korrigieren der Position mit getXYZinQuader ist dieser Index zu verwenden.
3.2. initializeCalibratedQuader
int initializeCalibratedQuader(Map<String, Object> configMap)
Mit dieser Funktion wird der Quader initialisiert, indem in einer Config-Map jeweils die Koordinaten, die der Sensor erfasst hat sowie die zugeh�rige Roboter-Position �bergeben werden.
Die Funktion gibt den Index des initialisierten Quaders zur�ck. Es ist m�glich, mehrere Quader zu definieren. Beim Korrigieren der Position mit getXYZinQuader ist dieser Index zu verwenden.
3.3. getXYZinQuader
Vector3D getXYZinQuader(int index, double x, double y, double z)
Mit dieser Funktion kann die korrigierte Position, die der Roboter in einem vorher definierten Quader anfahren soll. Parameter sind der Index des jeweiligen Quaders sowie die ermittelte Sensor-Position, die korrigiert werden soll.
Die Funktion gibt die korrigierte (fehler-minimierte) Position zur�ck, die der Roboter anfahren soll, um die vom Sensor ermittelte Position m�glichst genau zu erreichen. Auf die entsprechenden Eintr�ge kann mit getX(), getY() und getZ() zugegriffen werden.
2.4. Beispielprogramm
teachCalibratedQuader({"joints": [10.0, 10.0, 10.0, 10.0, 10.0, 10.0]}, 0.1, 0.1, 0.1)
teachCalibratedQuader({"xyz+euler": [ 0.3612, -0.0112, 0.3913, 148.186, 54.163, 154.569]},0.05, 0.1, 0.2)
var index;
index=initializeCalibratedQuader({
"E1K" : [ 0, 0, 0],
"E2K" : [ 0, 1, 0],
"E3K" : [ 1, 1, 0],
"E4K" : [ 1, 0, 0],
"E5K" : [ 0, 0, 1],
"E6K" : [ 0, 1, 1],
"E7K" : [ 1, 1, 1],
"E8K" : [ 1, 0, 1],
"E1R" : [ 0, 0.01, 0.01],
"E2R" : [ 0, 1.01, 0.01],
"E3R" : [ 0.99, 1, 0],
"E4R" : [ 0.99, 0, 0],
"E5R" : [ 0, 0.01, 1.01],
"E6R" : [ 0, 1.01, 1.01],
"E7R" : [ 0.99, 1, 1],
"E8R" : [ 0.99, 0, 1]
});
show_info(index);
var test=getXYZinQuader(index,0.5,0.5,0.5);
sleep(10000);
show_info("x="+test.getX()+" y="+test.getY()+" z="+test.getZ())