Winkel B und C berechnen

    Hallo Leute,


    ich bin noch recht neu in der Roboterprogrammierung und bin derzeit nach vielem Lesen an einen Punkt gekommen, bei dem ich etwas Hilfe benötige.


    Folgendes Szenario. Am Flansch meines Roboters befindet sich ein Stiftförmiges Taste-Tool und es gibt 2 Punkte im Raum. Das Tool soll nun derart positioniert werden, dass der TCP des Tools im ersten Punkt liegt und die Z-Achse des Tools (Stoßrichtung) durch den zweiten Punkt verläuft. Dabei soll die Drehung der Z-Achse, also Winkel A, des Tools unverändert bleiben (sonst würde es ja auch unendlich viele Möglichkeiten geben).
    Mir ist nun nicht ganz klar, wie ich die Winkel B und C berechnen kann.


    Als Beispiel habe ich folgende Positionen:

    Code
    Position 1:
X920.604126,Y -728.745239,Z 817.446411,A -115.381302,B -16.6707706,C 155.427002


Zielposition auf die das Tool "zeigen" soll
X 917.087891,Y -729.155396,Z 811.407471
    • Schritt für Schritt zum Roboterprofi!

    mal grob angedacht: Du könntest den Frame von Position 1 gedanklich und rechnerisch als Basiskoordinatensystem ansehen und die Positionsdaten von Punkt 2 darin verorten (durch Frameverknüpfung mit dem invertierten Punkt 1, siehe http://www.roboterforum.de/rob…tieren/msg51453/#msg51453). Denn wenn Dein TCP an Punkt 1 angekommen ist, liegt er ja genau so wie die Position und Orientierung. Punkt 2 (XYZ) bildet dann eine Abweichung zum "Nullpunkt". Jetzt kannst Du zwei rechtwinklige Dreiecke bilden: die eine Kathete ist jeweils die Z-Differenz (in der ursprünglichen Stoßrichtung). Die andere ist die Abweichung in X oder in Y. Mit dem Cotangens dann auf die Winkel, und die sollten, in einen Korrekturframe eingetragen ([x 0, y 0, z 0, a 0, b Winkel_um_y, c Winkel_um_x]) und auf den Ursprungspunkt draufgerechnet (Punkt1:Korrekturframe) die neue Position/Orientierung ergeben.
    Oder so ähnlich.


    Grüße,
    Michael

    Morgen Zusammen


    Genauso würde ich es auch machen - oder so ähnlich :zwink:


    Gruß


    Schybulla

    Weil der Klügere nach gibt - regieren die Dummen die Welt

    hm also so ganz klar ist mir noch nicht wie das funktioniert. Was liefert INV_POS denn z.b. genau?
    Könntest du nochmal genauer deine Vorgehensweise erklären? Wäre nett.

    Der Frame der Position 1 besteht aus einer Position und einer Richtung. Durch Invertieren bekommt man das Gegenstück, also den Frame, den man bräuchte, um von der aktuellen Position und der eigenen Richtung aus den Nullpunkt zu finden. Wenn Du z. B. die Position1 aus Deinem Beispiel invertierst, bekommst Du {X -487.2102, Y 746.3136, Z 1119.1256, A -118.05, B 29.18, C -176.23}.


    Jetzt kann man durch eine Frameverknüpfung mit Hilfe dieses Frames die ursprüngliche Verschiebung rausrechnen. Wir nehmen also den Punkt 2 (im Beispiel {X 917.0879, Y -729.1554, Z 811.4075, A 0.00, B 0.00, C 0.00} und rechnen die Verschiebung von Position1 raus.
    Abweichung = {X 917.0879, Y -729.1554, Z 811.4075, A 0.00, B 0.00, C 0.00} : {X -487.2102, Y 746.3136, Z 1119.1256, A -118.05, B 29.18, C -176.23}
    Als Ergebnis kommt raus: {X 0.0699, Y 0.0998, Z 7.0001, A -118.05, B 29.18, C -176.23}.
    (Man muss aufpassen - durch Rundungsfehler wird das Ergebnis nicht besser. Wenn ich Pos1 mit Pos2 im FAMOS direkt vergleiche, komme ich auf {X 0.0665, Y 0.0994, Z 6.9990, A -118.05, B 29.18, C -176.23} ... wir weichen im Tausendstel-Bereich deutlich ab.)
    Dieser Frame ist die Beschreibung der Position 2, wenn Position 1 das BASE wäre.


    Jetzt kommt die Ausrichtung. In Deinem Beispiel war die Abweichung schon sehr gering, auf 7mm Abstand hatte es 7 Hundertstel in X und 1 Zehntel in Y. Die Abweichung (!) in X (!) macht die Drehung um Y (!), also B (Winkelfunktion); die Abweichung in Y macht die Drehung um X, also C. Im Beispiel komme ich auf ungefähr 0.5721° in B und -0.8168° in C. Hier wird es durch Rundungsfehler langsam seltsam. Wenn ich direkt ausrichte, dann komme ich auf {X 0.0000, Y 0.0000, Z 0.0000, A -0.01, B 0.54, C -0.81}, wobei mir FAMOS den A jetzt selbständig gedreht hat um das fast Hundertstel Grad. Kannste ignorieren, sieht also so schlecht nicht nicht aus.
    Obiger Frame ist jetzt die Korrektur, die anzuwenden wäre, relativ zum Tool, um Position1 auf Position2 auszurichten. Müsste also mit
    Endposition = Position1 : Korrekturframe erledigt sein.


    Einleuchtender geht das, wenn man sich das Problem visualisieren kann mit virtuellen oder realen Dreibeinen.


    Grüße,
    Michael

    Super vielen Dank für die Erklärung. So konnte ich es gut nachvollziehen.
    Ich habe es nun auch Programmiert, nur bekomme ich mit den gleichen Zahlen für den Winkel C einen anderen Wert heraus. Und zwar +0.81681, also Vorzeichen verdreht.
    Bist du sicher, dass dein Wert stimmt?


    Meine Berechnungsfunktion sieht nun so aus:


    Wird noch etwas optimiert! ;)

    Zitat von Fahrrad


    sicher?


    Ich bin mir sicher, niemals sicher zu sein.


    Tatsache ist aber, wenn ich mir das visualisiere, liegt ja Position2 im komplett positiven Sektor, betrachtet von Position1 aus. Wenn ich also in Richtung Y gucke (grün), muss ich im Uhrzeigersinn drehen, um die Z-Achse (blau, Stoßkante der Flächen) mehr zum Zielpunkt zu drehen.
    Gucke ich dagegen in X-Richtung (rot), muss ich linksrum drehen, um das zu tun.
    (Die weißen Strichskes haben hier keine Bedeutung.)


    Die passenden Vorzeichen zu züchten überlasse ich Dir :denk:.


    Grüße,
    Michael

    Ok muss ich mir nochmal genau überlegen.


    Nochmal eine kurze Frage, mit welcher Software hast du denn das ganze offline berechnet? Also das INV_POS() und den Frame-Operator und mit was hast du das PNG erstellt?
    Bin wie gesagt neu in der ganzen Sache.

    Ich habe vor einiger Zeit eine Anlage programmiert, bei der das Werkzeug (Plasmaschneider) auf einer Kegelfläche entlang fahren musste. Letztendlich ging es auch dort darum, aus einer gewünschten Orientierung des Werkzeugs im Raum A, B und C abzuleiten. A kann ich vorgeben, B und C ergeben sich dann.


    Das ist aber ohne KUKA Funktionen gelöst, sondern rein durch hergeleitete Mathematik.
    Sollte noch Interesse bestehen, kann ich die Vorgehensweise mal posten.
    Ist keine Hexerei, mit den 3 Rotationsmatrizen und Gleichungsauflösungen kommt man dann relativ schnell ans Ziel.

    Programmiersklave, Danke werde ich mir direkt mal anschauen.


    Grubba
    sehr sehr gerne, würde mich interessieren, denn ich werde Teile davon auch selbst in C++ realisieren müssen.

    Ok, dann mal mein Ansatz:


    Es sei ein Punkt mit den Koordinaten 0.0,0.0,1.0. Das Koordinatensystem ist so, das die Z-Achse nach oben zeigt.


    Nun dreht man diesen Punkt (oder Einsvektor) um die drei Winkel A, B und C.


    Dazu die Reihenfolge so wählen, dass die Drehung um das ortsfeste Koordinatensystem erfolgt.


    Dazu gibts einen schönen Beitrag:
    http://www.roboterforum.de/rob…ierung/msg31321/#msg31321



    Wiki sorgt für die Matrizen:
    http://de.wikipedia.org/wiki/Rotationsmatrix


    Nach dem Anwenden der drei Matrizen auf den Punkt erhält man die neuen Koordinaten in Abhängigkeit von A,B und C.


    Umstellen und Auflösen der Gleichungen nach A,B und C ermöglicht uns dann, einfach die Position des Punktes anzugeben und als Ergebnis die Winkel zu erhalten. Da es für einen Punkt ja eigentlich unendlich viele Lösungen gibt, muss man einen Winkel vorgeben, z.B. A.


    Ich hoffe, es ist verständlich... :huh:

    Einmal editiert, zuletzt von Grubba ()

    Das mit dem Code-Ausschnitt ist so eine Sache...


    Das Umstellen und Auflösen ist eine Sache von Bleistift und Papier.
    Wenn du das Ergebnis hast, kannst du die Gleichungen mit den Standard Winkelfunktionen einfach in KRL eingeben (Oder C++ oder was auch immer).


    Um mal den Start zu vereinfachen:


    1. Schritt - Drehung Punkt um C


    (1 0 0 ) (0) ( 0 )
    (0 Cos Alpha -Sin Alpha ) (0) = (-Sin Alpha)
    (0 Sin Alpha Cos Alpha ) (1) (Cos Alpha)


    2. Schritt
    Dieses Ergebnis mit der Rotationsmatrix für Drehung um B multiplizieren


    3. Schritt
    Dann dieses Ergebnis mit der Rotationsmatrix für Drehung um A multiplizieren


    Als Ergebnis gibts für jede Koordinate eine Gleichung. Diese dann nach B und C auflösen/umstellen, so dass B und C Funktionen von X,Y,Z und A werden.


    Wichtig ist noch folgendes:
    Zur Vereinfachung der Rechnung ist der ursprüngliche Vektor (oder Punkt) für die Berechnung auf 0,0,1 gelegt. Wenn du nachher X,Y und Z vorgibst, muss die Länge dieses Vektors auch 1 sein.

    So also das Ausmultiplizieren ging ja noch, allerdings ist mir nicht klar, wie ich die Gleichungen auflösen kann, da ich jeweils sinus und cosinus in einer Gleichung habe.



    Du hast ja in dein Gleichungssystem als Vektor (0,0,1) eingesetzt, ich brauch ja aber den allgemeinen Fall, sprich X,Y und Z. Daher ist mir nicht ganz klar, was du mit deinem letzten Satz gemeint hast.

    Ich komme auf:


    (C= Cos, S=Sin, A,B,C=Drehwinkel)


    x = CA x SB x CC + SA x SC
    y = SA x SB x CC - CA x SC
    z = CB x CC


    Alle Winkelfunktionen mit A betrachte ich als Konstante, da A von mir vorgegeben wird und sich ja B und C ergeben sollen. So sehen die Ausdrücke dann nicht mehr ganz so lang aus.


    Diese drei Gleichungen lassen sich nun so auflösen, dass du für B und C Gleichungen erhältst, die als Parameter nur X, Y und Z benötigen.


    Das aufstellen der drei Gleichungen habe ich mit dem Einheitsvektor gemacht, weil sich dadurch das Gleichungssystem vereinfacht.


    Wenn du anschließend wieder X,Y und Z vorgibst, muss der resultierende Vektor auch ein Einsvektor sein. Also z.B. x=y=z=0.577


    Wenn du garnicht vorankommst, schnapp ich mir noch mal meine Schmierzettel und häng mal das Ergebnis an.

    Wäre klasse, wenn du es anhängen könntest.
    Deine drei Gleichungen erhalte ich ebenfalls sobald ich 0,0,1 als Vektor einsetze, jedoch bekomme ich es nicht wirklich aufgelöst. Habe immer am Ende Sinus UND! Cosinus übrig.
    Vor allem versteh ich das aber mit dem Einsvektor nicht. Nimm mal die Punkte aus meinem Ausgangspost:
    Deine drei Gleichungen erhalte ich ebenfalls sobald ich 0,0,1 als Vektor einsetze.


    Position 1:
    X920.604126,Y -728.745239,Z 817.446411,A -115.381302,B -16.6707706,C 155.427002


    Zielposition auf die das Tool "zeigen" soll
    X 917.087891,Y -729.155396,Z 811.407471



    Da ändern sich doch alle Koordinaten und nicht nur die Z Koordinate oder versteh ich jetzt etwas völlig falsch.

    Zitat


    jedoch bekomme ich es nicht wirklich aufgelöst. Habe immer am Ende Sinus UND! Cosinus übrig.


    Das stimmt. Aber auflösen kann man es aber ja trotzdem...


    Jetzt noch einmal zum Prinzip, vielleicht ist es ja doch nicht, was du brauchen kannst:


    Ich definiere einen Punkt, das ist in meinem Fall der Einsvektor 0,0,1. Dann drehe ich diesen Punkt um die drei Achsen mit den Rotationsmatrixen, wie es auch der Roboter machen würde. Dann erhalte ich als Ergebnis die neuen Koordinaten des nun gedrehten Punktes.


    Fragestellung für mich war damals:
    Ich habe einen Punkt im Raum, um welche Werte muss ich die Achsen drehen, damit mein Tool in Richtung dieses Punktes zeigt? Also das Ganze rückwärts rechnen, so erhalte ich durch Vorgabe des Punktes die Achswinkel.


    Da ich die Gleichungen mit dem Einsvektor 0,0,1 aufgestellt habe, muss ich für die Rückrechnung auch einen Einsvektor nehmen, ansonsten passt es nicht.
    Wenn du also z.B. möchtest, dass dein Werkzeug auf 777,0,0 zeigt, musst du 1,0,0 vorgeben. So änderst du die Richtung nicht, und hast trotzdem einen Einsvektor. Der Punkt, durch den dein Werkzeug zeigen soll, ist immer auf 0,0,0 bezogen. Wenn dein Werkzeug parallel zur X-Achse orientiert werden soll, muss du als Zielpunkt 1,0,0 vorgeben, EGAL wo dein TCP gerade liegt.
    In deinem Fall musst du also noch aus deiner TCP-Position und Zielposition den Richtungsvektor ermitteln. Also wie in der Schule Endpunkt-Anfangspunkt. Dann auf Einsvektor normieren und fertig. :D


    Vielleicht kann ichs auch so beschreiben:


    Dein Robi hält einen Bleistift. Jetzt soll dein Bleistift in eine beliebige Richtung zeigen. Diese Richtung definierst du durch den Einsvektor. Die Gleichung ermittelt dann für dich die Achswinkel B und C, nach Vorgabe von A. Die Position des TCP ist dabei also völlig egal. Die Richtung in Form des Einsvektors musst du aber vorher noch selbst ermitteln.

    Ok ich glaube mir wurde es klarer. Danke für deine Erklärung.
    Jetzt müssen dann nur noch meine Gleichungen stimmen und damit habe ich mir wirklich schwer getan.


    Ich bekomme raus:


    Code
    c = asin( ( sa*x - ca*y ) );
b = atan( ( ca*x + sa*y ) / z );


    Kannst du das bestätigen?
    Wobei der Arcussinus ja problematisch ist, da er ja mehrdeutig ist.

Erstelle ein Benutzerkonto oder melde dich an um zu kommentieren

Du musst ein Benutzerkonto haben um einen Kommentar hinterlassen zu können

Benutzerkonto erstellen
Neues Benutzerkonto für unsere Community erstellen. Geht einfach!
Neues Benutzerkonto erstellen
Anmelden
Du hast bereits ein Benutzerkonto? Melde dich hier an.
Jetzt anmelden