java - デルタロボットの直接運動学を正しく計算するには?

Question

デルタ ロボットの簡単なシミュレーションをまとめようとしています。フォワード キネマティクス (ダイレクト キネマティクス) を使用して、3 つの角度を渡して空間内のエンド エフェクタの位置を計算したいと考えています。

私はTrossen Robotics Forum の Delta Robot Tutorialから始めましたが、ほとんどの数学は理解できますが、すべてではありません。順運動学の最後の部分で、3 つの球体が交差する点を計算しようとすると、わかりません。私は一般的に球座標を見てきましたが、（E（x、y、z）に向かって）回転するために使用される2つの角度を見つけることができませんでした。球の方程式を解いているように見えますが、ここで迷ってしまいます。

デルタ ロボットはパラレル ロボットです (つまり、ベースとエンド エフェクタ (ヘッド) は常に平行のままです)。ベースとエンド エフェクタは正三角形で、脚は (通常) 三角形の辺の中央に配置されます。

デルタロボットのベース側面には マークが付いていfます。デルタロボットのエフェクタの側面には マークが付いていeます。脚の上部がマークされrf、下側がマークされreます。

原点 (O) は底辺三角形の中心にあります。サーボ モーターは、基本三角形の辺 (F1、F2、F3) の中央にあります。ジョイントには J1、J2、J3 のマークが付いています。下肢はポイント E1、E2、E3 でエンド エフェクタに結合し、E はエンド エフェクタの三角形の中心です。

ポイント F1、F2、F3 および J1、J2、J3 を簡単に計算できます。私が問題を抱えているのはE1、E2、E3です。re説明から、ポイント J1 が少し内側 (エンド エフェクタの中央値の半分) に移動して J1' になり、半径(下肢の長さ)を持つ球の中心になることがわかります。すべてのジョイントに対してこれを行うと、3 つの球が同じ場所で交差することになります: E(x,y,z)。球方程式を解くと、E(x,y,z) が得られます。

説明された式もあります：

しかし、これは私が迷子になるところです。私の数学の能力は素晴らしいものではありません。数学にあまり詳しくない私たちのために、誰かがそれらをより簡単な方法で説明してもらえますか?

提供されているサンプル コードも使用しました (WebGL 対応のブラウザーを使用している場合)ここで実行できます。クリック アンド ドラッグしてシーンを回転させます。3 つの角度を制御するには、q/Q、w/W、e/E を使用して角度を増減します。

完全なコード リスト:

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

問題は、視覚化するときに下部の長さが変化することです (印刷されたメッセージ 0 でわかるように、そうすべきではないため、さらに混乱が生じます。

Java/Processing で提供された C コードを使用しましたが、プログラミング言語はそれほど重要ではありません。

[spektreによる編集]

この写真を追加する必要がありました（教訓的な理由で）。

• 並べられたナンセンスは、運動学の能力を把握するための最良の方法ではありません
• 私が理解しているように、モーターのベースは上の三角形の平面のこの画像にあります
• ツールは下の三角形の平面にあります