Jak poprawnie obliczyć kinematykę bezpośrednią dla robota delta?

Question

Próbuję stworzyć prostą symulację dla robota delta i chciałbym użyć kinematyki do przodu (bezpośrednia kinematyka) do obliczenia położenia końcowego efektora w przestrzeni przez podanie 3 kątów.

Zacząłem od Trossen Robotics Forum Delta Robot Tutorial i rozumiem większość matematyki, ale nie wszystkie. Zgubiłem się na ostatniej części w kinematykach do przodu, próbując obliczyć punkt, w którym przecinają się 3 kule. Przyjrzałem się ogólnie współrzędnym sferycznym, ale nie mogłem wyliczyć dwóch kątów używanych do znalezienia, aby obrócić w kierunku (do E (x, y, z)). Widzę, że rozwiązują równanie sfery, ale tu się gubię.

delta robot jest równoległy robot (czyli podstawy i końcówkę (głowy) zawsze pozostania równolegle). Efektem podstawowym i końcowym są trójkąty równoboczne, a nogi są (zazwyczaj) umieszczane w środku boków trójkąta.

Strona podstawy robota delta jest oznaczona f. Strona efektora robota delta jest oznaczona e. Górna część nogi jest oznaczona rf, a dolna strona re.

Początek (O) znajduje się pośrodku trójkąta bazowego. Serwosilniki znajdują się po środku boków trójkąta podstawowego (F1, F2, F3). Połączenia są oznaczone jako J1, J2, J3. Niższe nogi łączą efektor końcowy w punktach E1, E2, E3 , a E jest środkiem trójkąta efektora końcowego.

Potrafię łatwo obliczyć punkty F1, F2, F3 i J1, J2, J3. To E1, E2, E3 Mam problemy z. Z wyjaśnień rozumiem, że punkt J1 zostaje przesunięty nieco do wewnątrz (o połowę mediana efektora końcowego) o do J1 'i staje się środkiem kuli o promieniu re (długość dolnej części nogi). Wykonanie tej czynności dla wszystkich połączeń spowoduje, że 3 kule przecinają się w tym samym miejscu: E (x, y, z). Rozwiązując równanie sfery znajdujemy E (x, y, z).

Istnieje również formuła wyjaśnił:

ale to gdzie ja zgubić. Moje umiejętności matematyczne nie są świetne. Czy ktoś mógłby wyjaśnić te informacje w prostszy sposób, , aby dowiedzieć się więcej na ten temat?

Użyłem również dostarczonego kodu przykładowego, który (jeśli masz przeglądarkę WebGL ), możesz uruchomić here. Kliknij i przeciągnij, aby obrócić scenę. Aby kontrolować trzy kąty, użyj przycisków q/Q, w/W, e/E, aby zmniejszyć/zwiększyć kąty.

Pełna Listing:

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

Problem jest, gdy wizualizacji, dolna część zmiany długości (jak widać w drukowanych message0 i nie powinien, co dodatkowo przyczynia się do mojego zamieszania.

Użyłem dostarczonego kodu C w języku Java/Processing, ale język programowania jest najmniej ważny.

[Edytuj przez spektre]

po prostu musiałem dodać ten obraz (ze względów dydaktycznych).

• wyłożonej nonsens nie jest najlepszym sposobem na uchwycenie kinematyki zdolności
• jak rozumiem bazy z silników jest na tym zdjęciu na górnej trójkątnej płaszczyzny
• a narzędzie jest na dolnej trójkąta samolot

Answer 1

Chciałbym zrobić to w następujący sposób (algebraiczna reprezentacja rozwiązania graficzne):

1. Obliczyć F1, F2, F3;

2. systemowi rozwiązać

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

   • gdzie q jest wspólna odległość | Ei-E | która jest stała

[Uwagi]

Nie rozwiąże go ręcznie

• użycie czerpać lub coś w celu uzyskania roztworu algebraicznych
• i używać tylko ważne rozwiązanie
• jego kwadratowy system, więc najprawdopodobniej będzie więcej rozwiązań, więc trzeba sprawdzić prawidłowość jeden

Just a głupie pytanie dlaczego nie można rozwiązać odwrotne kinematyki

• Jest bardzo prawdopodobne, co trzeba (jeśli tylko nie rób wizualizację tylko)
• a także jest nieco prostsze w tym przypadku

również podczas korzystania tylko bezpośrednie kinematyki

• Nie jestem do końca przekonany, że należy prowadzić wszystkie 3 stawy
• najprawdopodobniej napęd tylko 2 z nich
• i obliczyć 3.p tak kinematyka pozostać w aktualnej pozycji

[Edit1]

nie jest uproszczenie, że tylko wydaje mi:

1. Ti tłumaczyć Ji w kierunku osi Z pod q (równolegle do płaszczyzna XY)

2. teraz, jeśli tylko trzeba znaleźć punkt przecięcia 3 sfer z Ti

   • ten punkt jest E

3. tak Ei jest teraz prosty przekład E (odwrotność z przeliczenia Ji)

PS. Mam nadzieję, że wiesz, jak obliczyć kąty, gdy masz wszystkie punkty ...