Wie Sie richtig berechnen Sie die direkte Kinematik für einen delta-Roboter?

Ich versuche, die zusammen eine einfache simulation für einen delta-Roboter und ich möchte in der vorwärts-Kinematik (direkte Kinematik) zum berechnen der end-Effektor-position im Raum durch vorbeifahrenden 3 Winkeln.

Habe ich begonnen, mit der Trossen Robotics Forum Delta-Roboter-Lernprogramm und ich kann verstehen, die meisten der Mathematik, aber nicht alle. Ich bin verloren auf dem letzten Teil in der vorwärts-Kinematik, wenn Sie versuchen, zu berechnen, den Punkt, wo die 3 Bereiche überschneiden. Ich habe sphärische Koordinaten im Allgemeinen, aber konnte nicht herausfinden, die zwei Winkel verwendet, um zu drehen in Richtung (E(x,y,z)). Ich sehe, Sie sind die Lösung der Gleichung einer Kugel, aber das ist, wo ich verloren gehen.

Wie Sie richtig berechnen Sie die direkte Kinematik für einen delta-Roboter?

Wie Sie richtig berechnen Sie die direkte Kinematik für einen delta-Roboter?

Wie Sie richtig berechnen Sie die direkte Kinematik für einen delta-Roboter?

Einen delta-Roboter parallel-Roboter (im Sinne der Basis-und der end-Effektor(Kopf) bleiben immer parallel). Die Basis und Endeffektor gleichseitige Dreiecke und die Beine sind (in der Regel), platziert in der Mitte des Dreiecks.

Den Seiten der Basis der delta-Roboter ist gekennzeichnet f.
Die Seite der Effektor von der delta-Roboter ist gekennzeichnet e.
Der Obere Teil des Beines ist gekennzeichnet rf und der unteren Seite re.

Den Ursprung(O) ist in der Mitte der Basis-Dreieck.
Die Servomotoren werden in der Mitte des Basis-Dreiecks (F1,F2,F3).
Die Gelenke sind markiert J1,J2,J3. Die Unterschenkel join the end effector, an den Punkten E1,E2,E3
und E ist der Mittelpunkt des end-Effektor-Dreieck.

Kann ich leicht berechnen, Punkte F1,F2,F3 und J1,J2,J3.
Es ist E1,E2,E3 habe ich Probleme mit. Aus den Erklärungen,
Ich verstehe, dass der Punkt J1 bekommt, übersetzen nach innen ein wenig (um die Hälfte des end-Effektor - median)
mit J1', und es wird ein Zentrum einer Kugel mit radius re (unterschenkellänge).
Tun Sie dies für alle Gelenke Ergebnis in 3 sich überschneidende Sphären an der gleichen Stelle: E(x,y,z). Durch lösen der Kugel-Gleichung finden wir E(x,y,z).

Gibt es auch eine Formel erklärt:

Wie Sie richtig berechnen Sie die direkte Kinematik für einen delta-Roboter?

Wie Sie richtig berechnen Sie die direkte Kinematik für einen delta-Roboter?
aber das ist, wo ich verloren gehen. Meine mathematischen Fähigkeiten sind nicht groß.
Könnte mir bitte jemand erklären diese in einfacher Weise,
für die weniger Mathe-bewanderten von uns ?

Habe ich auch die sample-code zur Verfügung gestellt, die (wenn Sie einen WebGL aktiviert
browser), die Sie ausführen können hier. Klicken und ziehen zum drehen der Szene. Die Steuerung der drei Winkel verwenden Sie q/Q, w/W,e/E zu verringern/erhöhen Winkeln.

Vollständige code-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);
} 

Das problem ist, wenn die Visualisierung, der untere Teil ändert die Länge (wie Sie sehen können, in der gedruckten message0 und es sollte nicht zu, die weiter ergänzt meine Verwirrung.

Habe ich die mitgelieferte C-code in Java/Processing, aber die Programmiersprache ist weniger wichtig.

[Edit by spektre]

Ich nur hatte, um dieses Bild (aus didaktischen Gründen).

  • gefütterter Unsinn ist nicht der beste Weg für die Erfassung der Kinematik Fähigkeiten
  • wie verstehe ich die base mit den Motoren ist auf diesem Bild auf das Obere Dreieck Flugzeug
  • und das tool ist auf dem unteren Dreieck Flugzeug

Wie Sie richtig berechnen Sie die direkte Kinematik für einen delta-Roboter?

Nachdem Sie den Stecker (7) und (8) in (1), erhältst du eine quadratische Gleichung, die Sie einfach brauchen, um es zu lösen mit z=(-b+-sqrt(b^2-4*a*c))/(2*a) wo a ist der Koeffizient der z^2, b von z und c ist der freie Koeffizient ist, dann stecken Sie z in (7) und (8) um x und y. Ich denke, die Länge ändert sich, weil nicht jeder Satz von Winkeln tragfähig ist, D. H. im wirklichen Leben, die Sie nicht ändern können, ein Winkel ohne die anderen beiden entsprechend.
Ich denke, Ihr Kommentar sollte eine Antwort. Es ist besser als Spektre Antwort unten.

InformationsquelleAutor George Profenza | 2013-08-10

Schreibe einen Kommentar