如何正确计算三角洲机器人的正运动学？

Question

如何正确计算三角洲机器人的正运动学？

javamathcomputational-geometryroboticskinematics

8

我正在尝试制作一个简单的三角洲机器人模拟，并希望使用正向运动学（直接运动学）通过传递三个角度来计算末端执行器在空间中的位置。

我从Trossen Robotics Forum Delta Robot Tutorial开始，我可以理解大部分的数学知识，但并非全部。当尝试计算三个球相交处的点时，我迷失了方向。我查看了一般的球面坐标，但无法计算出找到E(x, y, z)旋转的两个角度。我看到他们正在解决一个球的方程，但这就是我迷失方向的地方。

delta robot direct kinematics

一个Delta机器人是一种平行机器人（意味着基座和终端执行器（头部）始终保持平行）。基座和终端执行器都是等边三角形，腿通常放置在三角形边的中间。

Delta机器人基底的边长标记为f。 Delta机器人执行器的边长标记为e。腿的上部标记为rf，下部标记为re。

原点(O)位于基底三角形中心。伺服电机位于基底三角形边的中心位置(F1、F2、F3)。关节标记为J1、J2、J3。下部腿连接到终端执行器的E1、E2、E3点，并且E是终端执行器三角形的中心点。

我可以轻松计算出点 F1、F2、F3 和 J1、J2、J3。但是，我遇到了关于 E1、E2、E3 的问题。根据解释，我知道点 J1 会向内移动一点（由末端执行器中线的一半确定），形成一个以 J1' 为中心、半径为 re（下肢长度）的球体。将所有关节都这样处理后，得到的三个球体会在同一位置相交：E(x,y,z)。通过解方程组，我们可以求出 E(x,y,z)。

还有一个公式的说明：

dk equation 1

dk equation 2 但这就是我迷失的地方。我的数学技能不太好。有没有人能以更简单的方式解释一下，让我们中的数学不那么精通的人也能理解呢？

我还使用了提供的示例代码（如果您的浏览器支持WebGL），您可以在此处运行。单击并拖动以旋转场景。要控制三个角度，请使用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);
}

问题在于，在可视化时，下部分会改变长度（如您所见的打印信息），而实际上它不应该，这进一步增加了我的困惑。

我已经在Java/Processing中使用了提供的C代码，但编程语言并不是最重要的。

[由spektre编辑]

我只是为了教学目的而添加了这张图片。

- 无意义的线条不是掌握运动学能力的最佳方式 - 据我所理解，带有电机的底座位于图像的上三角形平面上 - 工具位于底部三角形平面上

delta robot

- George Profenza

1

将（7）和（8）代入（1）中，您会得到一个二次方程，只需使用 z = (-b±sqrt(b^2-4*a*c))/(2*a) 解决它，其中 a 是 z^2 的系数，b 是 z 的系数，c 是自由系数，然后将 z 代入（7）和（8）以获取 x 和 y。我认为长度会发生改变，因为不是所有角度组合都是可行的，即在现实生活中，不能改变一个角度而不相应地改变其他两个角度。 - pseudoDust

@pseudo 我认为你的评论应该作为答案。它比下面Spektre的回答更好。 - payala

1个回答

网页内容由stack overflow 提供, 点击上面的

可以查看英文原文，
原文链接

- Spektre · Accepted Answer

我会这样做（图形解法的代数表示）：

计算 F1、F2、F3；

求解以下系统

// 球体 J1 到 J3 对应的球面 …… 平行四边形（使用下半部分的 Z 轴）
(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 位于球面上
E1=(x1,y1,z1)
E2=(x2,y2,z2)
E3=(x3,y3,z3)
// Ei 平行于 Fi……必须调整坐标系，使基础三角形与 XY-平面平行
z1=z2
z1=z3
z2=z3
// Ei 和 Ej 之间的距离始终为 q
// 否则将是无效的位置（运动学会卡住甚至破坏）
|E1-E2|=q
|E1-E3|=q
|E2-E3|=q
// midpoint 就是 Ei 的平均值
E=(E1+E2+E3)/3

其中 q 是关节距离 |Ei-E|，其为常数

[注]

不要手动解决它

使用derive或其他方法获得代数解
只使用有效解
它是二次系统，所以可能会有更多的解，因此您必须检查正确的解

只是一个愚蠢的问题，为什么不解决逆运动学

这很可能是你需要的（如果你不只是做可视化）
在这种情况下也更简单

此外，当您仅使用直接运动学时

我并不完全相信您应该驱动所有三个关节
最好只驱动其中两个
并计算第三个，以使运动学保持有效位置

[编辑1]

我发现了一个简化：

将Ji沿Z轴平移q个单位（与XY平面平行），得到Ti
现在如果你只需要从Ti中找到3个球的交点
- 这个点就是E
因此，Ei现在是E的简单平移（与Ji的平移相反）

附注：我希望您知道如何在拥有所有点时计算角度...