Wie berechne ich direkte Kinematik für einen Delta-Roboter?

Question

Ich versuche eine einfache Simulation für einen Deltaroboter zusammenzustellen und würde die Vorwärtskinematik (direkte Kinematik) verwenden, um die Position des Endeffektors im Raum zu berechnen, indem drei Winkel passiert werden.

Ich habe mit der Trossen Robotics Forum Delta Robot Tutorial gestartet und ich kann die meisten Mathe verstehen, aber nicht alle. Ich bin im letzten Teil der Vorwärtskinematik verloren, wenn ich versuche, den Schnittpunkt der 3 Sphären zu berechnen. Ich habe im Allgemeinen sphärische Koordinaten betrachtet, konnte aber nicht die beiden Winkel herausfinden, die verwendet wurden, um nach (E (x, y, z)) zu rotieren. Ich sehe, dass sie die Gleichung einer Kugel lösen, aber da bin ich verloren.

Ein Delta-Roboter ist ein Parallelroboter (die Basis und den Endeffektor (Kopf) immer parallel bleiben Bedeutung). Die Basis und der Endeffektor sind gleichseitige Dreiecke, und die Beine sind (typischerweise) in der Mitte der Seiten des Dreiecks angeordnet.

Die Seite der Basis des Delta-Roboters ist mit f gekennzeichnet. Die Seite des Effektors des Delta-Roboters ist mit e markiert. Der obere Teil des Beins ist mit rf und die untere Seite mit re gekennzeichnet.

Der Ursprung (O) befindet sich in der Mitte des Basisdreiecks. Die Servomotoren befinden sich in der Mitte der Seiten des Basisdreiecks (F1, F2, F3). Die Gelenke sind mit J1, J2, J3 gekennzeichnet. Die Unterschenkel verbinden den Endeffektor an den Punkten E1, E2, E3 und E ist der Mittelpunkt des Endeffektordreiecks.

Ich kann leicht die Punkte F1, F2, F3 und J1, J2, J3 berechnen. Es ist E1, E2, E3 Ich habe Probleme mit. Aus den Erläuterungen, Ich verstehe, dass Punkt J1 nach innen übersetzt ein bisschen (um den halben Endeffektor Median) zu J1 'und es wird der Mittelpunkt einer Kugel mit Radius re (Unterschenkellänge). Wenn Sie dies für alle Gelenke tun, erhalten Sie 3 Kugeln, die sich an der gleichen Stelle schneiden: E (x, y, z). Durch Lösen der Kugelgleichung finden wir E (x, y, z).

Es gibt auch eine Formel erklärt:

aber das ist, wo ich verloren gehen. Meine mathematischen Fähigkeiten sind nicht großartig. Könnte jemand bitte diese auf eine einfachere Art und Weise erklären, für die weniger Mathematik versierte von uns?

Ich habe auch den mitgelieferten Beispielcode verwendet, der (wenn Sie einen WebGL-fähigen Browser haben) können Sie here ausführen. Klicken und ziehen Sie, um die Szene zu drehen. Zur Steuerung der drei Winkel verwenden Sie q/Q, w/W, e/E, um die Winkel zu verringern/zu erhöhen.

Vollständige Codeauflistung:

//Rhino measurements in cm 
final float e = 21;//end effector side 
final float f = 60.33;//base side 
final float rf = 67.5;//upper leg length - radius of upper sphere 
final float re = 95;//lower leg length - redius of lower sphere (with offset will join in E(x,y,z)) 

final float sqrt3 = sqrt(3.0); 
final float sin120 = sqrt3/2.0; 
final float cos120 = -0.5;   
final float tan60 = sqrt3; 
final float sin30 = 0.5; 
final float tan30 = 1/sqrt3; 
final float a120 = TWO_PI/3; 
final float a60 = TWO_PI/6; 

//bounds 
final float minX = -200; 
final float maxX = 200; 
final float minY = -200; 
final float maxY = 200; 
final float minZ = -200; 
final float maxZ = -10; 
final float maxT = 54; 
final float minT = -21; 

float xp = 0; 
float yp = 0; 
float zp =-45; 
float t1 = 0;//theta 
float t2 = 0; 
float t3 = 0; 

float prevX; 
float prevY; 
float prevZ; 
float prevT1; 
float prevT2; 
float prevT3; 

boolean validPosition; 
//cheap arcball 
PVector offset,cameraRotation = new PVector(),cameraTargetRotation = new PVector(); 

void setup() { 
    size(900,600,P3D); 
} 

void draw() { 
    background(192); 
    pushMatrix(); 
    translate(width * .5,height * .5,300); 
    //rotateY(map(mouseX,0,width,-PI,PI)); 

    if (mousePressed && (mouseX > 300)){ 
    cameraTargetRotation.x += -float(mouseY-pmouseY); 
    cameraTargetRotation.y += float(mouseX-pmouseX); 
    } 
    rotateX(radians(cameraRotation.x -= (cameraRotation.x - cameraTargetRotation.x) * .35)); 
    rotateY(radians(cameraRotation.y -= (cameraRotation.y - cameraTargetRotation.y) * .35)); 

    stroke(0); 
    et(f,color(255)); 
    drawPoint(new PVector(),2,color(255,0,255)); 
    float[] t = new float[]{t1,t2,t3}; 
    for(int i = 0 ; i < 3; i++){ 
    float a = HALF_PI+(radians(120)*i); 
    float r1 = f/1.25 * tan(radians(30)); 
    float r2 = e/1.25 * tan(radians(30)); 
    PVector F = new PVector(cos(a) * r1,sin(a) * r1,0); 
    PVector E = new PVector(cos(a) * r2,sin(a) * r2,0); 
    E.add(xp,yp,zp); 
    //J = F * rxMat 
    PMatrix3D m = new PMatrix3D(); 
    m.translate(F.x,F.y,F.z); 
    m.rotateZ(a); 
    m.rotateY(radians(t[i])); 
    m.translate(rf,0,0); 

    PVector J = new PVector(); 
    m.mult(new PVector(),J); 
    line(F.x,F.y,F.z,J.x,J.y,J.z); 
    line(E.x,E.y,E.z,J.x,J.y,J.z); 
    drawPoint(F,2,color(255,0,0)); 
    drawPoint(J,2,color(255,255,0)); 
    drawPoint(E,2,color(0,255,0)); 
    //println(dist(F.x,F.y,F.z,J.x,J.y,J.z)+"\t"+rf); 
    println(dist(E.x,E.y,E.z,J.x,J.y,J.z)+"\t"+re);//length should not change 
    } 
    pushMatrix(); 
    translate(xp,yp,zp); 
    drawPoint(new PVector(),2,color(0,255,255)); 
    et(e,color(255)); 
    popMatrix(); 
    popMatrix(); 
} 
void drawPoint(PVector p,float s,color c){ 
    pushMatrix(); 
    translate(p.x,p.y,p.z); 
    fill(c); 
    box(s); 
    popMatrix(); 
} 
void et(float r,color c){//draw equilateral triangle, r is radius (median), c is colour 
    pushMatrix(); 
    rotateZ(-HALF_PI); 
    fill(c); 
    beginShape(); 
    for(int i = 0 ; i < 3; i++) 
    vertex(cos(a120*i) * r,sin(a120*i) * r,0); 
    endShape(CLOSE); 
    popMatrix(); 
} 
void keyPressed(){ 
    float amt = 3; 
    if(key == 'q') t1 -= amt; 
    if(key == 'Q') t1 += amt; 
    if(key == 'w') t2 -= amt; 
    if(key == 'W') t2 += amt; 
    if(key == 'e') t3 -= amt; 
    if(key == 'E') t3 += amt; 
    t1 = constrain(t1,minT,maxT); 
    t2 = constrain(t2,minT,maxT); 
    t3 = constrain(t3,minT,maxT); 
    dk(); 
} 

void ik() { 
    if (xp < minX) { xp = minX; } 
    if (xp > maxX) { xp = maxX; } 
    if (yp < minX) { yp = minX; } 
    if (yp > maxX) { yp = maxX; } 
    if (zp < minZ) { zp = minZ; } 
    if (zp > maxZ) { zp = maxZ; } 

    validPosition = true; 
    //set the first angle 
    float theta1 = rotateYZ(xp, yp, zp); 
    if (theta1 != 999) { 
    float theta2 = rotateYZ(xp*cos120 + yp*sin120, yp*cos120-xp*sin120, zp); // rotate coords to +120 deg 
    if (theta2 != 999) { 
     float theta3 = rotateYZ(xp*cos120 - yp*sin120, yp*cos120+xp*sin120, zp); // rotate coords to -120 deg 
     if (theta3 != 999) { 
     //we succeeded - point exists 
     if (theta1 <= maxT && theta2 <= maxT && theta3 <= maxT && theta1 >= minT && theta2 >= minT && theta3 >= minT) { //bounds check 
      t1 = theta1; 
      t2 = theta2; 
      t3 = theta3; 
     } else { 
      validPosition = false; 
     } 

     } else { 
     validPosition = false; 
     } 
    } else { 
     validPosition = false; 
    } 
    } else { 
    validPosition = false; 
    } 

    //uh oh, we failed, revert to our last known good positions 
    if (!validPosition) { 
    xp = prevX; 
    yp = prevY; 
    zp = prevZ; 
    } 

} 

void dk() { 
    validPosition = true; 

    float t = (f-e)*tan30/2; 
    float dtr = PI/(float)180.0; 

    float theta1 = dtr*t1; 
    float theta2 = dtr*t2; 
    float theta3 = dtr*t3; 

    float y1 = -(t + rf*cos(theta1)); 
    float z1 = -rf*sin(theta1); 

    float y2 = (t + rf*cos(theta2))*sin30; 
    float x2 = y2*tan60; 
    float z2 = -rf*sin(theta2); 

    float y3 = (t + rf*cos(theta3))*sin30; 
    float x3 = -y3*tan60; 
    float z3 = -rf*sin(theta3); 

    float dnm = (y2-y1)*x3-(y3-y1)*x2; 

    float w1 = y1*y1 + z1*z1; 
    float w2 = x2*x2 + y2*y2 + z2*z2; 
    float w3 = x3*x3 + y3*y3 + z3*z3; 

    // x = (a1*z + b1)/dnm 
    float a1 = (z2-z1)*(y3-y1)-(z3-z1)*(y2-y1); 
    float b1 = -((w2-w1)*(y3-y1)-(w3-w1)*(y2-y1))/2.0; 

    // y = (a2*z + b2)/dnm; 
    float a2 = -(z2-z1)*x3+(z3-z1)*x2; 
    float b2 = ((w2-w1)*x3 - (w3-w1)*x2)/2.0; 

    // a*z^2 + b*z + c = 0 
    float a = a1*a1 + a2*a2 + dnm*dnm; 
    float b = 2*(a1*b1 + a2*(b2-y1*dnm) - z1*dnm*dnm); 
    float c = (b2-y1*dnm)*(b2-y1*dnm) + b1*b1 + dnm*dnm*(z1*z1 - re*re); 

    // discriminant 
    float d = b*b - (float)4.0*a*c; 
    if (d < 0) { validPosition = false; } 

    zp = -(float)0.5*(b+sqrt(d))/a; 
    xp = (a1*zp + b1)/dnm; 
    yp = (a2*zp + b2)/dnm; 

    if (xp >= minX && xp <= maxX&& yp >= minX && yp <= maxX && zp >= minZ & zp <= maxZ) { //bounds check 
    } else { 
    validPosition = false; 
    } 

    if (!validPosition) {  
    xp = prevX; 
    yp = prevY; 
    zp = prevZ; 
    t1 = prevT1; 
    t2 = prevT2; 
    t3 = prevT3; 
    } 

} 

void storePrev() { 
    prevX = xp; 
    prevY = yp; 
    prevZ = zp; 
    prevT1 = t1; 
    prevT2 = t2; 
    prevT3 = t3; 
} 

float rotateYZ(float x0, float y0, float z0) { 
    float y1 = -0.5 * 0.57735 * f; // f/2 * tg 30 
    y0 -= 0.5 * 0.57735 * e; // shift center to edge 
    // z = a + b*y 
    float a = (x0*x0 + y0*y0 + z0*z0 +rf*rf - re*re - y1*y1)/(2*z0); 
    float b = (y1-y0)/z0; 
    // discriminant 
    float d = -(a+b*y1)*(a+b*y1)+rf*(b*b*rf+rf); 
    if (d < 0) return 999; // non-existing point 
    float yj = (y1 - a*b - sqrt(d))/(b*b + 1); // choosing outer point 
    float zj = a + b*yj; 
    return 180.0*atan(-zj/(y1 - yj))/PI + ((yj>y1)?180.0:0.0); 
} 

Das Problem ist, wenn die Visualisierung, der untere Teil Änderungen Länge (wie Sie in den gedruckten message0 sehen und es sollte nicht, was meine Verwirrung fügt weiter hinzu.

Ich habe den mitgelieferten C-Code in Java/Processing verwendet, aber die Programmiersprache ist am wenigsten wichtig.

[Bearbeiten von Spektre]

Ich hatte gerade dieses Bild hinzufügen (aus didaktischen Gründen).

• der ausgekleideten nonsense ist nicht der beste Weg, um die Kinematik Fähigkeiten zum Greifen
• wie ich die Basis mit den Motoren verstehe, wird auf diesem Bild auf der oberen Dreiecksebene
• und das Werkzeug wird auf dem unteren Dreieck
Ebene

Answer 1

ich würde es tun, wie (algebraische Darstellung von Grafik-Lösung) folgt:

1. berechnen F1, F2, F3;

2. löst System

   // spheres from Ji to Ei ... parallelograms (use lower Z half sphere) 
   (x1-J1.x)^2 + (y1-J1.y)^2 +(z1-J1.z)^2 = re^2 
   (x2-J2.x)^2 + (y2-J2.y)^2 +(z2-J2.z)^2 = re^2 
   (x3-J3.x)^2 + (y3-J3.y)^2 +(z3-J3.z)^2 = re^2 
   // Ei lies on the sphere 
   E1=(x1,y1,z1) 
   E2=(x2,y2,z2) 
   E3=(x3,y3,z3) 
   // Ei is parallel to Fi ... coordinate system must be adjusted 
   // so base triangles are parallel with XY-plane 
   z1=z2 
   z1=z3 
   z2=z3 
   // distance between any Ei Ej must be always q 
   // else it is invalid position (kinematics get stuck or even damage) 
   |E1-E2|=q 
   |E1-E3|=q 
   |E2-E3|=q 
   // midpoint is just average of Ei 
   E=(E1+E2+E3)/3 
   

   • wobei q der gemeinsame Abstand | Ei-E | die konstant

[Notizen]

manuell

• Verwendung ableiten oder etwas zu erhalten algebraische Lösung
• und verwenden Sie nur gültige Lösung nicht lösen Sie
• sein quadratisches System, so wird es wahrscheinlich mehr Lösungen für Sie haben für die richtige

nur eine dumme Frage zu prüfen, warum Sie inverse Kinematik

• es sehr wahrscheinlich, was Sie (wenn Sie nicht nur eine Visualisierung tun nur) nicht lösen müssen
• und ist auch ein bisschen einfacher in diesem Fall

auch wenn Sie nur direkte Kinematik

verwenden
• Ich bin nicht ganz überzeugt, dass Sie alle drei Gelenke
• höchstwahrscheinlich Laufwerk nur 2 von ihnen
• und berechnen die 3 fahren sollte.th, so bleiben die Kinematik in gültige Position

[Edit1]

Es eine Vereinfachung ist, die gerade erscheinen mir:

1. Ti = translate Ji in Richtung der Z-Achse durch Q (parallel zu XY-Ebene)

2. jetzt, wenn Sie müssen nur Kreuzung finden von 3 Kugeln aus Ti

   • dieser Punkt E

3. so Ei ist jetzt einfache Übersetzung von E (inverse Übersetzung von der Ji)

PS. Ich hoffe, dass Sie wissen, wie man Winkel berechnet, wenn Sie alle Punkte haben ...