Come calcolare correttamente la cinematica diretta per un robot delta?

Question

Sto cercando di mettere insieme una semplice simulazione per un robot delta e mi piacerebbe usare la cinematica in avanti (cinematica diretta) per calcolare la posizione dell'effettore finale nello spazio facendo passare 3 angoli.

Ho iniziato con il Trossen Robotics Forum Delta Robot Tutorial e posso capire la maggior parte della matematica, ma non tutti. Mi sono perso nell'ultima parte della cinematica in avanti, quando provo a calcolare il punto in cui si intersecano le 3 sfere. Ho esaminato le coordinate sferiche in generale, ma non sono riuscito a capire i due angoli usati per trovare la rotazione verso (verso E (x, y, z)). Vedo che stanno risolvendo l'equazione di una sfera, ma è lì che mi perdo.

Un robot delta è un robot parallelo (cioè la base e l'effettore terminale (testa) rimanere sempre parallelo). La base e l'effettore finale sono triangoli equilateri e le gambe sono (tipicamente) poste al centro dei lati del triangolo.

Il lato della base del robot delta è contrassegnato con f. Il lato dell'effettore del robot delta è contrassegnato con e. La parte superiore della gamba è contrassegnata con rf e la parte inferiore re.

L'origine (O) si trova al centro del triangolo di base. I servomotori si trovano al centro dei lati del triangolo di base (F1, F2, F3). I giunti sono contrassegnati J1, J2, J3. La parte inferiore delle gambe unisce l'effettore finale ai punti E1, E2, E3 ed E è il centro del triangolo effettore finale.

Posso calcolare facilmente i punti F1, F2, F3 e J1, J2, J3. Sono E1, E2, E3 con cui ho problemi. Dalle spiegazioni, , capisco che il punto J1 si traduce un po '(metà della mediana dell'effettore finale) da a J1' e diventa il centro di una sfera con raggio re (lunghezza della gamba inferiore). Facendo questo per tutti i giunti si otterranno 3 sfere che si intersecano nello stesso punto: E (x, y, z). Risolvendo l'equazione della sfera troviamo E (x, y, z).

C'è anche una formula spiegato:

ma questo è dove mi perdo. Le mie abilità matematiche non sono grandiose. Qualcuno potrebbe spiegarlo in un modo più semplice, per meno esperti di matematica di noi?

Ho anche utilizzato il codice di esempio fornito che (se si dispone di un browser abilitato WebGL ) è possibile eseguire here. Fare clic e trascinare per ruotare la scena. Per controllare i tre angoli utilizzare q/Q, w/W, e/E per diminuire/aumentare gli angoli.

pieno listato di codice:

//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); 
} 

Il problema è che, quando la visualizzazione, la minore lunghezza di modifiche di parte (come si può vedere nelle message0 stampati e non dovrebbe, il che aggiunge ulteriore alla mia confusione.

Ho usato il codice C fornito in Java/Processing, ma il linguaggio di programmazione è meno importante.

[Edit by Spektre]

ho solo dovuto aggiungere questa immagine (per motivi didattici).

• il dialogo rivestito non è il modo migliore per afferrare i cinematica capacità
• come comprendo la base con i motori è su questa immagine sul piano triangolo superiore
• e lo strumento è il triangolo inferiore aereo

Answer 1

avrei fatto come segue (rappresentazione algebrica soluzione grafica):

0.123,51641 milioni

1. calcolo F1, F2, F3;

2. risolvere sistema

   // 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 
   

   • dove q è la distanza giunto | Ei-E | che è costante

[Note]

non risolvono manualmente

• uso derivano o qualcosa per ottenere soluzione algebrica
• e utilizzare solo valida soluzione
• il suo sistema quadratico quindi ci saranno probabilmente più soluzioni per te deve controllare da quella corretta

Solo una domanda stupida perché non risolvere cinematica inversa

• è più probabile quello che vi serve (se solo non fai una visualizzazione solo)
• ed è anche un po 'più semplice in questo caso

anche quando si utilizza la cinematica appena diretti

• Io non sono del tutto convinto che si dovrebbe guidare tutti i 3 giunti
• molto probabilmente unità solo 2 di loro
• e calcolare il 3.th così la cinematica rimanere in posizione valida

[Edit1]

C'è una semplificazione che appena sembrano me:

1. Ti = tradurre Ji verso l'asse Z da q (parallela piano XY)

2. ora se avete solo bisogno di trovare intersezione di 3 sfere da ti

   • questo punto è E

3. così Ei è ora semplice definizione di E (inverso dalla traduzione Ji)

PS. Spero che tu sappia come calcolare gli angoli quando hai tutti i punti ...