Comment bien calculer directement la cinématique d'un robot delta?

Je vais essayer de mettre sur pied une simple simulation d'un robot delta, et j'aimerais les utiliser avant la cinématique directe (cinématique) pour calculer la préhension de la position dans l'espace en passant 3 angles.

J'ai commencé avec le Trossen Robotics Forum Robot Delta Tutoriel et je peux comprendre la plupart des mathématiques, mais pas tous. Je suis perdu à la dernière partie dans la cinématique inverse, lorsque vous essayez de calculer le point où l'3 de la sphère se croisent. J'ai regardé les coordonnées sphériques en général, mais ne pouvait pas travailler sur les deux angles utilisés pour trouver pour faire pivoter en direction de (E(x,y,z)). Je vois qu'ils sont de la résolution de l'équation d'une sphère, mais c'est là que je me suis perdu.

Un delta robot est un robot parallèle (ce qui est la base et la fin de l'effecteur(tête) toujours rester parallèle). La base et de préhension sont des triangles équilatéraux et les jambes sont (généralement) placé au milieu des côtés du triangle.

Le côté de la base du robot delta est marqué f.
Du côté de l'effecteur du robot delta est marqué e.
La partie supérieure de la jambe est marqué rf et le côté inférieur re.

L'origine(O) est au centre du triangle de base.
Les servo-moteurs sont au milieu de la base du triangle de côtés (F1,F2,F3).
Les articulations sont marqués J1,J2,J3. Le bas des jambes rejoindre la fin effectrices dans les points E1,E2,E3
et E est le centre de l'effecteur triangle.

Je peux facilement calculer les points F1,F2,F3 et J1,J2,J3.
C'est E1,E2,E3, je vais avoir des problèmes avec. D'après les explications,
Je comprends que le point J1 obtient traduire vers l'intérieur un peu (par la moitié de la préhension de la médiane)
à J1 " et il devient le centre d'une sphère avec un rayon re (en bas la longueur de la jambe).
De le faire pour tous les joints de résultat dans les 3 sphères d'intersection dans le même lieu: E(x,y,z). Par résolution de l'équation de la sphère, nous trouvons E(x,y,z).

Il y a aussi une formule explique:

mais c'est là que je me suis perdu. Mes compétences en mathématiques ne sont pas de grands.
Quelqu'un pourrait-il expliquer ces plus facilement,
pour les moins de math savvy de nous ?

J'ai aussi utilisé l'exemple de code fourni (si vous avez un permis de WebGL
navigateur), vous pouvez exécuter ici. Cliquez et faites glisser pour faire pivoter la scène. Pour contrôler les trois angles utilisez q/Q, w/W,e/E pour augmenter/diminuer les angles.

Plein de code:

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

Le problème est que, lors de la visualisation, la partie inférieure des modifications de la longueur (comme vous pouvez le voir dans la version imprimée de message0 et il ne devrait pas, ce qui ajoute encore à ma confusion.

J'ai utilisé le fourni le code C, en Java/Traitement, mais le langage de programmation est moins important.

[Modifier par spektre]

J'ai juste eu à ajouter cette image (pour la didactique des raisons).

• doublé de non-sens n'est pas le meilleur moyen pour saisir la cinématique capacités
• que je comprends de la base avec les moteurs est sur cette image sur le triangle supérieur avion
• et l'outil est sur le bas du triangle d'avion

OriginalL'auteur George Profenza | 2013-08-10