Optimierung Absolutgenauigkeit
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:
| Größe
|
Beispiel 1
|
Beispiel 2
| | --- | --- | --- | | 2x2 |
1 2 4 3
|
1 4 2 3
| | 3x3 | 1 2 3 6 5 4 7 8 9 | 1 6 7 2 5 8 3 4 9 | | 4x4 | 1 2 3 4 8 7 6 5 9 10 11 12 16 15 14 13 | 1 8 9 16 2 7 10 15 3 6 11 14 4 5 12 13
|
Die Funktion teachCameraAlignment hat 7 bzw 4 Übergabeparameter. Zuerst wird die Variante mit 7 Übergabeparameter beschrieben.
teachCameraAlignment(index, horstX, horstY, horstZ, cameraX, cameraY, cameraZ)
| Parameter
|
Beschreibung
| | --- | --- | | index | Nummer des einzulernden Punktes. Der erste Punkt hat index 1, der zweite 2 etc. Wie die Nummerierung der einzelnen Punkte ist, kann der Tabelle oben entnommen werden. | | horstX | Die von horstOS ermittelte X-Position. Kann z.b. mit getCurrentPose().x ermittelt werden. | | horstY | Die von horstOS ermittelte Y-Position. Kann z.b. mit getCurrentPose().y ermittelt werden. | | horstZ | Die von horstOS ermittelte Z-Position. Kann z.b. mit getCurrentPose().z ermittelt werden. | | cameraX | Die x-Position, die von einer Kamera ermittelt wird. | | cameraY | Die y-Position, die von einer Kamera ermittelt wird. | | cameraZ | Die z-Position, die von einer Kamera ermittelt wird. |
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.
| Parameter
|
Beschreibung
| | --- | --- | | index | Nummer des einzulernden Punktes. Der erste Punkt hat index 1, der zweite 2 etc. Wie die Nummerierung der einzelnen Punkte ist, kann der Tabelle oben entnommen werden. | | cameraX | Die x-Position, die von einer Kamera ermittelt wird. | | cameraY | Die y-Position, die von einer Kamera ermittelt wird. | | cameraZ | Die z-Position, die von einer Kamera ermittelt wird. |
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.
| Parameter
|
Beschreibung
| | --- | --- | | cameraX | Die x-Position, die von einer Kamera ermittelt wird. | | cameraY | Die y-Position, die von einer Kamera ermittelt wird. | | cameraZ | Die z-Position, die von einer Kamera ermittelt wird. |
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)Mit dieser Funktion wird der Quader initialisiert, indem die Eckpunkte einzeln geteached werden.
Dazu wird in einer Config-Map die erste Ecke der unteren Ebene des Quaders übergeben - entweder als joint-Konfiguration oder als Pose (xyz + Eulerwinkel). Weitere Parameter sind x, y und z - Koordinate eines 3D-Vektors, der dann den Quader vollständig definiert: die anderen Ecken werden mittels der im Vektor definierten Werte angefahren (relativ von der Startposition/von der ersten Ecke aus gesehen).
Parameter | Beschreibung |
|---|---|
| joints | Joint-Konfiguration der ersten Ecke der unteren Ebene des Quaders |
| oder | |
| xyz+euler | Pose der ersten Ecke der unteren Ebene des Quaders |
| x | x-Wert des Vektors, mit dem die anderen Ecken angefahren werden |
| y | y-Wert des Vektors, mit dem die anderen Ecken angefahren werden |
| z | z-Wert des Vektors, mit dem die anderen Ecken angefahren werden |
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.
Parameter | Beschreibung |
|---|---|
| E1K | [ x, y, z ] Sensor-Koordinaten von Ecke 1 (1. Punkt der unteren Ebene des Quaders) |
| E2K | [ x, y, z ] Sensor-Koordinaten von Ecke 2 (2. Punkt der unteren Ebene des Quaders) |
| E3K | [ x, y, z ] Sensor-Koordinaten von Ecke 3 (3. Punkt der unteren Ebene des Quaders) |
| E4K | [ x, y, z ] Sensor-Koordinaten von Ecke 4 (4. Punkt der unteren Ebene des Quaders) |
| E5K | [ x, y, z ] Sensor-Koordinaten von Ecke 5 (1. Punkt der oberen Ebene des Quaders) |
| E6K | [ x, y, z ] Sensor-Koordinaten von Ecke 6 (2. Punkt der oberen Ebene des Quaders) |
| E7K | [ x, y, z ] Sensor-Koordinaten von Ecke 7 (3. Punkt der oberen Ebene des Quaders) |
| E8K | [ x, y, z ] Sensor-Koordinaten von Ecke 8 (4. Punkt der oberen Ebene des Quaders) |
| E1R | [ x, y, z ] Roboter-Position von Ecke 1 (1. Punkt der unteren Ebene des Quaders) |
| E2R | [ x, y, z ] Roboter-Position von Ecke 2 (2. Punkt der unteren Ebene des Quaders) |
| E3R | [ x, y, z ] Roboter-Position von Ecke 3 (3. Punkt der unteren Ebene des Quaders) |
| E4R | [ x, y, z ] Roboter-Position von Ecke 4 (4. Punkt der unteren Ebene des Quaders) |
| E5R | [ x, y, z ] Roboter-Position von Ecke 5 (1. Punkt der oberen Ebene des Quaders) |
| E6R | [ x, y, z ] Roboter-Position von Ecke 6 (2. Punkt der oberen Ebene des Quaders) |
| E7R | [ x, y, z ] Roboter-Position von Ecke 7 (3. Punkt der oberen Ebene des Quaders) |
| E8R | [ x, y, z ] Roboter-Position von Ecke 8 (4. Punkt der oberen Ebene des Quaders) |
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.
Parameter | Beschreibung |
|---|---|
| index | Index des Quaders, der bei der Korrektur verwendet werden soll |
| x | x-Koordinate der Sensor-Position, die korrigiert werden soll |
| y | y-Koordinate der Sensor-Position, die korrigiert werden soll |
| z | z-Koordinate der 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())